Merge Old RCAC Implementation to Newest PX4 Version

.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_FW
 [submodule "src/drivers/uavcan/libuavcan"]
 	path = src/drivers/uavcan/libuavcan
 	url = https://github.com/PX4/libuavcan.git
@@ -66,3 +66,7 @@
 [submodule "src/drivers/uavcannode_gps_demo/libcanard"]
 	path = src/drivers/uavcannode_gps_demo/libcanard
 	url = https://github.com/UAVCAN/libcanard
+[submodule "src/lib/mathlib/math/RCAC"]
+	path = src/lib/mathlib/math/RCAC
+	url = git@github.com:joonghle/RCAC_cpp_light.git
+	branch = RCAC_v2_simple

.vscode/settings.json

@@ -138,5 +138,16 @@
         "workbench.settings.enableNaturalLanguageSearch": false,
         "yaml.schemas": {
                 "${workspaceFolder}/validation/module_schema.yaml": "${workspaceFolder}/src/modules/*/module.yaml"
-        }
+        },
+        "editor.fontSize": 16,
+        "python.autoComplete.extraPaths": [
+                "/home/spjohn/work/cat_mav/devel_isolated/mavros/lib/python3/dist-packages",
+                "/home/spjohn/work/cat_mav/devel_isolated/mavros_msgs/lib/python3/dist-packages",
+                "/opt/ros/noetic/lib/python3/dist-packages"
+        ],
+        "python.analysis.extraPaths": [
+                "/home/spjohn/work/cat_mav/devel_isolated/mavros/lib/python3/dist-packages",
+                "/home/spjohn/work/cat_mav/devel_isolated/mavros_msgs/lib/python3/dist-packages",
+                "/opt/ros/noetic/lib/python3/dist-packages"
+        ]
 }

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,10 @@ 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
+mavlink stream -r 2 -s RCAC_FW_ROLL -u $udp_gcs_port_local
+mavlink stream -r 2 -s RCAC_FW_PITCH -u $udp_gcs_port_local
+mavlink stream -r 2 -s RCAC_FW_YAW -u $udp_gcs_port_local

ROMFS/px4fmu_common/init.d/rcS

@@ -612,3 +612,24 @@ 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/ttyACM0 -s RCAC_FW_ROLL -r 2
+mavlink stream -d /dev/ttyS1 -s RCAC_FW_ROLL -r 2
+mavlink stream -d /dev/ttyS2 -s RCAC_FW_ROLL -r 2
+mavlink stream -d /dev/ttyACM0 -s RCAC_FW_PITCH -r 2
+mavlink stream -d /dev/ttyS1 -s RCAC_FW_PITCH -r 2
+mavlink stream -d /dev/ttyS2 -s RCAC_FW_PITCH -r 2
+mavlink stream -d /dev/ttyACM0 -s RCAC_FW_YAW -r 2
+mavlink stream -d /dev/ttyS1 -s RCAC_FW_YAW -r 2
+mavlink stream -d /dev/ttyS2 -s RCAC_FW_YAW -r 2
+mavlink stream -d /dev/ttyS1 -s ODOMETRY -r 10
+mavlink stream -d /dev/ttyS2 -s LOCAL_POSITION_NED -10

ROMFS/px4fmu_common/mixers/AETRFG.main.mix

@@ -23,6 +23,7 @@ differences between the servos must be made mechanically.  To obtain the correct
 motion using a Y  cable, the servos can be positioned reversed from one another.
 
 M: 1
+O:      -10000  -10000      0  -10000  10000
 S: 0 0   10000   10000      0  -10000  10000
 
 CH2: Elevator mixer

msg/CMakeLists.txt

@@ -107,6 +107,12 @@ set(msg_files
 	qshell_retval.msg
 	radio_status.msg
 	rate_ctrl_status.msg
+	rcac_att_variables.msg
+	rcac_fw_pitch.msg
+	rcac_fw_roll.msg
+	rcac_fw_yaw.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,16 @@
+# 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 rcac_att_ru			# Ru for Attitude Controller
+float32 rcac_att_n			# N Filter Coefficient for 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_fw_pitch.msg

@@ -0,0 +1,34 @@
+uint64 timestamp			# time since system start (microseconds)
+
+uint64 ii_rate				# iteration step of the FW RCAC Pitch Rate Controller
+bool switch_rate 			# FW RCAC Pitch Rate Controller switch
+float32 alpha_pid_rate			# Gain that multiplies the rate PID gains
+float32 rcac_z_rate 			# Performance variable for the FW RCAC Pitch Rate Controller
+float32 rcac_dzz_rate			# Deadzoned of performance variable
+float32 rcac_u_rate 			# Control generated by the FW RCAC Pitch Rate Controller
+float32 rcac_nnf_rate 			# N Filter Coefficient of RCAC Pitch Rate Controller
+float32[2] rcac_theta_rate 		# P gains for the FW RCAC Pitch Rate Controller
+
+float32[2] px4_theta_rate		# P gains for the PX4 FW Pitch Rate Controller
+
+float32 p11_rate			# P(1,1) of the FW RCAC Pitch Rate Controller
+float32 p12_rate			# P(1,2) of the FW RCAC Pitch Rate Controller
+float32 p21_rate			# P(2,1) of the FW RCAC Pitch Rate Controller
+float32 p22_rate			# P(2,2) of the FW RCAC Pitch Rate Controller
+
+uint64 ii_att				# iteration step of the FW RCAC Pitch Att Controller
+bool switch_att 			# FW RCAC Pitch Att Controller switch
+float32 alpha_pid_att			# Gain that multiplies the Pitch att PID gains
+float32 rcac_z_att 			# Performance variable for the FW RCAC Pitch Att Controller
+float32 rcac_u_att 			# Control generated by the FW RCAC Pitch Att Controller
+float32 rcac_nnf_att			# N Filter Coefficient of RCAC Pitch Att Controller
+float32 rcac_theta_att 			# P gains for the FW RCAC Pitch Att Controller
+
+float32 px4_theta_att			# P gains for the PX4 FW Pitch Att Controller
+
+float32 p11_att				# P(1,1) of the FW RCAC Pitch Att Controller
+
+uint8 rate_dz_type			# Rate dz type
+float32 rate_dz_s1			# Rate dz s1 parameter
+float32 rate_dz_s2			# Rate dz s2 parameter
+float32 rate_dz_alf			# Rate dz alpha parameter

msg/rcac_fw_roll.msg

@@ -0,0 +1,34 @@
+uint64 timestamp			# time since system start (microseconds)
+
+uint64 ii_rate				# iteration step of the FW RCAC Roll Rate Controller
+bool switch_rate 			# FW RCAC Roll Rate Controller switch
+float32 alpha_pid_rate			# Gain that multiplies the Roll Rate PID gains
+float32 rcac_z_rate 			# Performance variable for the FW RCAC Roll Rate Controller
+float32 rcac_dzz_rate			# Deadzoned of performance variable
+float32 rcac_u_rate 			# Control generated by the FW RCAC Roll Rate Controller
+float32 rcac_nnf_rate 			# N Filter Coefficient of RCAC Roll Rate Controller
+float32[2] rcac_theta_rate 		# P gains for the FW RCAC Roll Rate Controller
+
+float32[2] px4_theta_rate		# P gains for the PX4 FW Roll Rate Controller
+
+float32 p11_rate			# P(1,1) of the FW RCAC Roll Rate Controller
+float32 p12_rate			# P(1,2) of the FW RCAC Roll Rate Controller
+float32 p21_rate			# P(2,1) of the FW RCAC Roll Rate Controller
+float32 p22_rate			# P(2,2) of the FW RCAC Roll Rate Controller
+
+uint64 ii_att				# iteration step of the FW RCAC Roll Att Controller
+bool switch_att 			# FW RCAC Roll Att Controller switch
+float32 alpha_pid_att			# Gain that multiplies the Roll att PID gains
+float32 rcac_z_att 			# Performance variable for the FW RCAC Roll Att Controller
+float32 rcac_u_att 			# Control generated by the FW RCAC Roll Att Controller
+float32 rcac_nnf_att			# N Filter Coefficient of RCAC Roll Att Controller
+float32 rcac_theta_att 			# P gains for the FW RCAC Roll Att Controller
+
+float32 px4_theta_att			# P gains for the PX4 FW Roll Att Controller
+
+float32 p11_att				# P(1,1) of the FW RCAC Roll Att Controller
+
+uint8 rate_dz_type			# Rate dz type
+float32 rate_dz_s1			# Rate dz s1 parameter
+float32 rate_dz_s2			# Rate dz s2 parameter
+float32 rate_dz_alf			# Rate dz alpha parameter

msg/rcac_fw_yaw.msg

@@ -0,0 +1,19 @@
+uint64 timestamp			# time since system start (microseconds)
+
+uint64 ii_rate				# iteration step of the FW RCAC Yaw Rate Controller
+bool switch_rate 			# FW RCAC Yaw Rate Controller switch
+float32 alpha_pid_rate			# Gain that multiplies the Yaw Rate PID gains
+float32 rcac_z_rate 			# Performance variable for the FW RCAC Yaw Rate Controller
+float32 rcac_dzz_rate			# Deadzoned of performance variable
+float32 rcac_u_rate 			# Control generated by the FW RCAC Yaw Rate Controller
+float32 rcac_nnf_rate			# N Filter Coefficient of RCAC Pitch Rate Controller
+float32[2] rcac_theta_rate 		# P gains for the FW RCAC Yaw Rate Controller
+
+float32[2] px4_theta_rate		# P gains for the PX4 FW Yaw Rate Controller
+
+float32 p11_rate			# P(1,1) of the FW RCAC Yaw Rate Controller
+
+uint8 rate_dz_type			# Rate dz type
+float32 rate_dz_s1			# Rate dz s1 parameter
+float32 rate_dz_s2			# Rate dz s2 parameter
+float32 rate_dz_alf			# Rate dz alpha parameter

msg/rcac_pos_vel_variables.msg

@@ -0,0 +1,32 @@
+# Fused local position in NED.
+
+uint64 timestamp			# time since system start (microseconds)
+
+float32 pid_factor			# PID scaling
+float32 rcac_master_sw			# Master RCAC switch
+float32 beta_mot_fr			# Gain that limits the QC FR motor PWM signal
+bool    beta_mot_fr_sw			# Switch for gain that limits the QC FR motor PWM signal
+
+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
+float32 rcac_pos_ru			# Ru for Position Controller
+float32 rcac_pos_n			# N Filter Coefficient for 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 rcac_vel_ru			# Ru for Velocity Controller
+float32 rcac_vel_n			# N Filter Coefficient for 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,17 @@
+# 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 rcac_rate_ru			# Ru for Rate Controller
+float32 rcac_rate_n			# N Filter Coefficient for 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

platforms/common/px4_work_queue/WorkQueueManager.cpp

@@ -271,7 +271,7 @@ WorkQueueManagerRun(int, char **)
 			// On posix system , the desired stacksize round to the nearest multiplier of the system pagesize
 			// It is a requirement of the  pthread_attr_setstacksize* function
 			const unsigned int page_size = sysconf(_SC_PAGESIZE);
-			const size_t stacksize_adj = math::max(PTHREAD_STACK_MIN, PX4_STACK_ADJUSTED(wq->stacksize));
+			const size_t stacksize_adj = math::max(static_cast<long>(PTHREAD_STACK_MIN), static_cast<long>(PX4_STACK_ADJUSTED(wq->stacksize)));
 			const size_t stacksize = (stacksize_adj + page_size - (stacksize_adj % page_size));
 #endif
 			int ret_setstacksize = pthread_attr_setstacksize(&attr, stacksize);

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,9 @@
+## Quadcopter initial position
+#export PX4_HOME_LAT=42.29454
+#export PX4_HOME_LON=-83.710547
+#export PX4_HOME_ALT=274.0
+# FW initial position
+export PX4_HOME_LAT=42.298612
+export PX4_HOME_LON=-83.842467
+export PX4_HOME_ALT=271.2
+make px4_sitl gazebo_plane_catapult

run_gazebo_plane_catapult_testfield_new.sh

@@ -0,0 +1,10 @@
+# LL on the right (Comment accordingly!!)
+#export PX4_HOME_LAT=42.298612 
+#export PX4_HOME_LON=-83.842467
+
+# LL on the left (Comment accordingly!!)
+export PX4_HOME_LAT=42.298502
+export PX4_HOME_LON=-83.843980
+export PX4_HOME_ALT=271.2
+
+make px4_sitl gazebo_plane_catapult

run_jmavsim.sh

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

run_jmavsim_valgrind.sh

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

src/lib/mathlib/CMakeLists.txt

@@ -35,6 +35,8 @@ px4_add_library(mathlib
 	math/test/test.cpp
 	math/filter/LowPassFilter2p.cpp
 	math/filter/LowPassFilter2pVector3f.cpp
+	math/filter/ContDiffDeadzone.cpp
+	math/RCAC/src/RCAC.cpp
 )
 
 px4_add_unit_gtest(SRC math/filter/MedianFilterTest.cpp)