Add rcac

.gitmodules

@@ -1,7 +1,7 @@
 [submodule "mavlink/include/mavlink/v2.0"]
 	path = mavlink/include/mavlink/v2.0
-	url = https://github.com/mavlink/c_library_v2.git
-	branch = master
+	url = https://github.com/JAParedes/c_library_v2.git
+	branch = mavlink_rcac
 [submodule "src/drivers/uavcan/libuavcan"]
 	path = src/drivers/uavcan/libuavcan
 	url = https://github.com/PX4/libuavcan.git

.vscode/c_cpp_properties.json

@@ -3,7 +3,10 @@
         {
             "name": "PX4",
             "includePath": [
-                "${workspaceFolder}/**"
+                "${workspaceFolder}/**",
+                "${workspaceFolder}/src/lib",
+                "${workspaceFolder}/src",
+                "${workspaceFolder}/src/lib"
             ],
             "defines": [],
             "macFrameworkPath": [],

ROMFS/px4fmu_common/init.d-posix/rcS

@@ -242,6 +242,9 @@ if [ -e etc/init.d-posix/rc.mavlink_override ]
 then
 	echo "Running non-default mavlink config rc.mavlink_override"
 	sh etc/init.d-posix/rc.mavlink_override
+	#mavlink stream -r 2 -s RCAC_ATT_VARIABLES -u $udp_gcs_port_local
+	#mavlink stream -r 2 -s RCAC_RATE_VARIABLES -u $udp_gcs_port_local
+	#mavlink stream -r 2 -s RCAC_POS_VEL_VARIABLES -u $udp_gcs_port_local
 else
 	# GCS link
 	mavlink start -x -u $udp_gcs_port_local -r 4000000
@@ -254,9 +257,15 @@ else
 	mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u $udp_gcs_port_local
 	mavlink stream -r 20 -s RC_CHANNELS -u $udp_gcs_port_local
 	mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u $udp_gcs_port_local
+	#mavlink stream -r 2 -s RCAC_ATT_VARIABLES -u $udp_gcs_port_local
+	#mavlink stream -r 2 -s RCAC_RATE_VARIABLES -u $udp_gcs_port_local
+	#mavlink stream -r 2 -s RCAC_POS_VEL_VARIABLES -u $udp_gcs_port_local
+
+
 
 	# API/Offboard link
 	mavlink start -x -u $udp_offboard_port_local -r 4000000 -m onboard -o $udp_offboard_port_remote
+	mavlink stream -r 50 -s LOCAL_POSITION_NED -u $udp_offboard_port_local
 
 	# Onboard link to camera
 	mavlink start -x -u $udp_onboard_payload_port_local -r 4000 -f -m onboard -o $udp_onboard_payload_port_remote
@@ -276,3 +285,7 @@ fi
 
 mavlink boot_complete
 replay trystart
+
+mavlink stream -r 2 -s RCAC_ATT_VARIABLES -u $udp_gcs_port_local
+mavlink stream -r 2 -s RCAC_RATE_VARIABLES -u $udp_gcs_port_local
+mavlink stream -r 2 -s RCAC_POS_VEL_VARIABLES -u $udp_gcs_port_local

ROMFS/px4fmu_common/init.d/rcS

@@ -612,3 +612,15 @@ unset VEHICLE_TYPE
 # Boot is complete, inform MAVLink app(s) that the system is now fully up and running.
 #
 mavlink boot_complete
+
+mavlink stream -d /dev/ttyACM0 -s RCAC_ATT_VARIABLES -r 2
+mavlink stream -d /dev/ttyS1 -s RCAC_ATT_VARIABLES -r 2
+mavlink stream -d /dev/ttyS2 -s RCAC_ATT_VARIABLES -r 2
+mavlink stream -d /dev/ttyACM0 -s RCAC_RATE_VARIABLES -r 2
+mavlink stream -d /dev/ttyS1 -s RCAC_RATE_VARIABLES -r 2
+mavlink stream -d /dev/ttyS2 -s RCAC_RATE_VARIABLES -r 2
+mavlink stream -d /dev/ttyACM0 -s RCAC_POS_VEL_VARIABLES -r 2
+mavlink stream -d /dev/ttyS1 -s RCAC_POS_VEL_VARIABLES -r 2
+mavlink stream -d /dev/ttyS2 -s RCAC_POS_VEL_VARIABLES -r 2
+mavlink stream -d /dev/ttyS1 -s ODOMETRY -r 10
+mavlink stream -d /dev/ttyS2 -s LOCAL_POSITION_NED -10

msg/CMakeLists.txt

@@ -107,6 +107,9 @@ set(msg_files
 	qshell_retval.msg
 	radio_status.msg
 	rate_ctrl_status.msg
+	rcac_att_variables.msg
+	rcac_pos_vel_variables.msg
+	rcac_rate_variables.msg
 	rc_channels.msg
 	rc_parameter_map.msg
 	rpm.msg

msg/rcac_att_variables.msg

@@ -0,0 +1,14 @@
+# Fused local position in NED.
+
+uint64 timestamp			# time since system start (microseconds)
+
+uint64 ii_att				# iteration step of the RCAC Attitude Controller
+bool switch_att				# RCAC Attitude Controller switch
+float32 alpha_pid_att 			# Gain that multiplies the attitude PID gains
+float32[3] rcac_att_z 			# Performance variable for the Attitude Controller
+float32[3] rcac_att_u 			# Control generated by the Attitude Controller
+float32[3] rcac_att_theta 		# P gains for the Attitude Controller
+
+float32[3] px4_att_theta 		# P gains for the PX4 Attitude Controller
+
+float32 p11_att				# P(1,1) of the RCAC Attitude Controller

msg/rcac_pos_vel_variables.msg

@@ -0,0 +1,26 @@
+# Fused local position in NED.
+
+uint64 timestamp			# time since system start (microseconds)
+
+float32 pid_factor			# PID scaling
+float32 rcac_master_sw			# Master RCAC switch
+
+uint64 ii_pos				# iteration step of the RCAC Position Controller
+bool switch_pos				# RCAC Position Controller switch
+float32 alpha_pid_pos 			# Gain that multiplies the position PID gains
+float32[3] rcac_pos_z 			# Performance variable for the Position Controller
+float32[3] rcac_pos_u 			# Control generated by the Position Controller
+float32[3] rcac_pos_theta 		# P gains for the Position Controller
+
+uint64 ii_vel				# iteration step of the RCAC Velocity Controller
+bool switch_vel				# RCAC Velocity Controller switch
+float32 alpha_pid_vel 			# Gain that multiplies the velocity PID gains
+float32[3] rcac_vel_z 			# Performance variable for the Velocity Controller
+float32[3] rcac_vel_u 			# Control generated by the Velocity Controller
+float32[9] rcac_vel_theta 		# PID gains for the Velocity Controller
+
+float32[9] px4_ol_theta 		# P and PID gains for the PX4 outer loop Controller
+					# First 3 are P, next 3 are PID for XY, last 3 are PID for Z
+
+float32 p11_pos				# P(1,1) of the RCAC Position Controller
+float32 p11_velx 			# P(1,1) of the RCAC Velocity Controller

msg/rcac_rate_variables.msg

@@ -0,0 +1,14 @@
+# Fused local position in NED.
+
+uint64 timestamp			# time since system start (microseconds)
+
+uint64 ii_rate				# iteration step of the RCAC Rate Controller
+bool switch_rate			# RCAC Rate Controller switch
+float32 alpha_pid_rate 			# Gain that multiplies the rate PID gains
+float32[3] rcac_rate_z 			# Performance variable for the Rate Controller
+float32[3] rcac_rate_u 			# Control generated by the Rate Controller
+float32[12] rcac_rate_theta 		# PID+FF gains for the Rate Controller
+
+float32[12] px4_rate_theta 		# P gains for the PX4 Rate Controller
+
+float32 p11_ratex 			# P(1,1) of the RCAC Rate Controller

repo_update.sh

@@ -0,0 +1,4 @@
+make distclean
+git submodule sync
+git submodule update --init --recursive
+git submodule update --remote --recursive mavlink

run_gazebo.sh

@@ -0,0 +1,4 @@
+export PX4_HOME_LAT=42.29454
+export PX4_HOME_LON=-83.710547
+export PX4_HOME_ALT=274.0
+make px4_sitl gazebo

run_jmavsim.sh

@@ -0,0 +1,4 @@
+export PX4_HOME_LAT=42.29454
+export PX4_HOME_LON=-83.710547
+export PX4_HOME_ALT=274.0
+make px4_sitl_default jmavsim

src/lib/mathlib/CMakeLists.txt

@@ -35,8 +35,12 @@ px4_add_library(mathlib
 	math/test/test.cpp
 	math/filter/LowPassFilter2p.cpp
 	math/filter/LowPassFilter2pVector3f.cpp
+	math/RCAC.cpp
+	math/RCAC.h
 )
 
 px4_add_unit_gtest(SRC math/filter/MedianFilterTest.cpp)
 px4_add_unit_gtest(SRC math/filter/NotchFilterTest.cpp)
 px4_add_unit_gtest(SRC math/FunctionsTest.cpp)
+px4_add_unit_gtest(math/RCAC.cpp)
+px4_add_unit_gtest(math/RCAC.h)

src/lib/mathlib/math/RCAC.cpp

@@ -0,0 +1,132 @@
+#include "RCAC.h"
+#include <iostream>
+//#include <uORB/topics/rcac_pos_vel_states.h>
+#include <drivers/drv_hrt.h>
+
+// using namespace std;
+
+RCAC::RCAC(float P0_val, float lambda_val, float N_nf_val){
+
+    P0 = P0_val;
+    lambda = lambda_val;
+    N_nf = N_nf_val;
+
+    // Initialize interal RCAC v ariables
+    P = eye<float, 3>() * P0;
+    theta.setZero();
+    filtNu.setZero();
+    filtNu(0,nf-1)=N_nf;
+    u_km1 = 0;
+    z_km1 = 0;
+
+    Phi_k.setZero();
+
+    ubar.setZero();
+    Phibar.setZero();
+
+    Phi_filt.setZero();
+
+    one_matrix = eye<float, 1>();
+}
+
+void RCAC::init_test(){
+    std::cout << "init success!" << std::endl;
+}
+
+void RCAC::set_RCAC_data(float z_km1_val, float u_km1_val)
+{
+    z_km1 = z_km1_val;
+    u_km1 = u_km1_val;
+}
+
+void RCAC::buildRegressor(float z, float z_int, float z_diff)
+{
+    Phi_k(0, 0) = z;
+    Phi_k(0, 1) = z_int;
+    Phi_k(0, 2) = z_diff;
+}
+
+void RCAC::filter_data()
+{
+    for (int ii = nf - 1; ii > 0; ii--)
+    {
+        ubar(ii, 0) = ubar(ii - 1, 0);
+    }
+    ubar(0, 0) = u_km1;
+
+    for (int ii = nf; ii > 0; ii--)
+    {
+        for (int jj = 0; jj < 3; jj++)
+        {
+            Phibar(ii, jj) = Phibar(ii - 1, jj);
+        }
+    }
+    for (int jj = 0; jj < 3; jj++)
+    {
+        Phibar(0, jj) = Phi_k(0, jj);
+    }
+
+    z_filt = z_km1;
+
+    dummy = filtNu * ubar;
+    u_filt = dummy(0, 0);
+
+    for (int ii = 0; ii < nf; ii++)
+    {
+        for (int jj = 0; jj < 3; jj++)
+        {
+            PhibarBlock(ii, jj) = Phibar(ii + 1, jj);
+        }
+    }
+    Phi_filt = filtNu * PhibarBlock;
+}
+
+void RCAC::update_theta()
+{
+    if (kk > 3)
+    {
+        dummy = Phi_filt * P * Phi_filt.transpose();
+        Gamma = lambda + dummy(0, 0);
+        P = P - P * Phi_filt.transpose() * 1 / Gamma * Phi_filt * P;
+        P = P / lambda;
+
+        theta = theta - P * Phi_filt.transpose() * (z_filt * one_matrix + Phi_filt * theta - u_filt);
+    }
+    //std::cout << kk << "\t" << z_filt << "\t" << u_filt << "\t" << Phi_filt(0 ,0) << "\t" << theta(0 ,0) << std::endl;
+}
+
+float RCAC::compute_uk(float z, float z_int, float z_diff, float u)
+{
+    set_RCAC_data(z, u);
+    buildRegressor(z, z_int, z_diff);
+    filter_data();
+    update_theta();
+    dummy = Phi_k * theta;
+    u_k = dummy(0, 0);
+    kk = kk + 1;
+    return u_k;
+}
+/*
+void RCAC::populate_states(int index)
+{
+    _rcac_pos_vel_states.timestamp = hrt_absolute_time();
+    _rcac_pos_vel_states.id_xyz = index;
+
+    //Add an if else statement depending on if position on velocity controller component
+    _rcac_pos_vel_states.u_pos[index] = get_rcac_uk();
+    _rcac_pos_vel_states.theta_pos[index] = get_rcac_theta();
+    _rcac_pos_vel_states.z_pos[index] = u_km1;
+    //_rcac_pos_vel_states_pub.publish(_rcac_pos_vel_states);
+    std::cout << "publisher values "<< _rcac_pos_vel_states.u << std::endl;
+}
+
+void RCAC::publish_states(){
+    _rcac_pos_vel_states_pub.publish(_rcac_pos_vel_states);
+}
+*/
+// [ 0 0 0]
+
+//pos_mes
+//vel_mes
+// if (1)
+// pos_mes

src/lib/mathlib/math/RCAC.h

@@ -0,0 +1,72 @@
+#pragma once
+
+#include <matrix/matrix/math.hpp>
+#include <uORB/Publication.hpp>
+//#include <uORB/topics/rcac_pos_vel_states.h>
+
+using namespace matrix;
+// using namespace std;
+
+/**
+ * The parent RCAC class. This class handles all the low level computation of RCAC
+ * such as the filtering, coefficient updates, and keeping track of the regressors.
+ *
+ * The notation here follows the JGCD 2019 implementation
+ */
+class RCAC
+{
+    float P0;
+    float lambda;
+    float N_nf;
+/*
+private:
+    uORB::Publication<rcac_pos_vel_states_s>    _rcac_pos_vel_states_pub{ORB_ID(rcac_pos_vel_states)};
+    rcac_pos_vel_states_s _rcac_pos_vel_states{};
+    //uORB::Publication<rcac_pos_vel_variables_s>     _rcac_pos_vel_variables_pub{ORB_ID(rcac_pos_vel_variables)};
+*/
+
+public:
+    RCAC(float P0_val, float lambda_val, float N_nf_val);
+    ///void init_RCAC(float, float, float);
+
+    int   getkk() {return kk;};
+    float get_rcac_uk() {return u_k;};
+    float get_rcac_theta(int i) {return theta(i,0);}
+    float get_rcac_P(int i, int j){return P(i, j);};
+    float get_rcac_Phi(int i) {return Phi_k(i,0);}
+    float get_rcac_ukm1() {return u_km1;};
+
+    void set_RCAC_data(float, float);
+    void buildRegressor(float zkm1, float zkm1_int, float zkm1_diff);
+    void filter_data();
+    void update_theta();
+    float compute_uk(float z, float z_int, float z_diff, float u);
+    void init_test();
+    void publish_states(int index);
+
+protected:
+    const int nf = 2;
+    matrix::Matrix<float, 1, 2> filtNu;         // 1st order filter. Gf = filtNu(0) + filtNu(1)/q
+                                                // In most cases, filtNu(0) = 0 and filtNu(1) = +-1
+
+    //RCAC internal variables
+    matrix::Matrix<float, 3, 3> P;
+    matrix::Matrix<float, 3, 1> theta;          // Kp = theta(0,0)
+                                                // Ki = theta(1,0)
+                                                // Kd = theta(2,0)
+
+    float u_k, u_km1, u_filt;
+    float z_km1, z_filt;
+    matrix::Matrix<float, 1, 3> Phi_k, Phi_filt;
+
+    matrix::Matrix<float, 2, 1> ubar;        // Size nf by 1
+    matrix::Matrix<float, 3, 3> Phibar;      // Size nf+1 by 1
+    matrix::Matrix<float, 2, 3> PhibarBlock; // Size nf by 1
+
+    float Gamma;
+    float Idty_lz;
+    matrix::Matrix<float, 1, 1> one_matrix;
+    matrix::Matrix<float, 1, 1> dummy;
+
+    int kk = 0;
+};

src/modules/logger/logged_topics.cpp

@@ -111,6 +111,11 @@ void LoggedTopics::add_default_topics()
 	add_topic("vehicle_status");
 	add_topic("vehicle_status_flags");
 	add_topic("vtol_vehicle_status", 200);
+	add_topic("rcac_pos_vel_variables", 100);
+	add_topic("rcac_att_variables", 100);
+	add_topic("rcac_rate_variables", 100);
+
+
 
 	// Control allocation topics
 	add_topic("vehicle_angular_acceleration_setpoint", 20);