diff --git a/.gitmodules b/.gitmodules index 8338c4fa2d4c..d83d1d5710c5 100644 --- a/.gitmodules +++ b/.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 diff --git a/.vscode/settings.json b/.vscode/settings.json index 5ded130f740e..5dbca8892cbb 100644 --- a/.vscode/settings.json +++ b/.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" + ] } diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index edf49e203a5b..4a6464d6e9df 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/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 diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 50d42d4dab45..394b3c0d7a8d 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/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 diff --git a/ROMFS/px4fmu_common/mixers/AETRFG.main.mix b/ROMFS/px4fmu_common/mixers/AETRFG.main.mix index 4ebf75fdd007..6528a163c9f9 100644 --- a/ROMFS/px4fmu_common/mixers/AETRFG.main.mix +++ b/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 diff --git a/mavlink/include/mavlink/v2.0 b/mavlink/include/mavlink/v2.0 index 076158a97560..001d0f93b2b9 160000 --- a/mavlink/include/mavlink/v2.0 +++ b/mavlink/include/mavlink/v2.0 @@ -1 +1 @@ -Subproject commit 076158a9756064c78817a425f67991acba95b8ec +Subproject commit 001d0f93b2b9647617ceb618ac0f44ef2d1d5e0a diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 64cfd389d4cd..a9bda2e01c83 100644 --- a/msg/CMakeLists.txt +++ b/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 diff --git a/msg/rcac_att_variables.msg b/msg/rcac_att_variables.msg new file mode 100644 index 000000000000..b44a66f5a156 --- /dev/null +++ b/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 diff --git a/msg/rcac_fw_pitch.msg b/msg/rcac_fw_pitch.msg new file mode 100644 index 000000000000..a6188fc6f0b2 --- /dev/null +++ b/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 diff --git a/msg/rcac_fw_roll.msg b/msg/rcac_fw_roll.msg new file mode 100644 index 000000000000..bb438fa41ec4 --- /dev/null +++ b/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 diff --git a/msg/rcac_fw_yaw.msg b/msg/rcac_fw_yaw.msg new file mode 100644 index 000000000000..5becab826ad9 --- /dev/null +++ b/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 diff --git a/msg/rcac_pos_vel_variables.msg b/msg/rcac_pos_vel_variables.msg new file mode 100644 index 000000000000..795c22c914ff --- /dev/null +++ b/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 diff --git a/msg/rcac_rate_variables.msg b/msg/rcac_rate_variables.msg new file mode 100644 index 000000000000..e2a2f5324d18 --- /dev/null +++ b/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 diff --git a/platforms/common/px4_work_queue/WorkQueueManager.cpp b/platforms/common/px4_work_queue/WorkQueueManager.cpp index 7486e0bee4ff..02ddfd5e723c 100644 --- a/platforms/common/px4_work_queue/WorkQueueManager.cpp +++ b/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(PTHREAD_STACK_MIN), static_cast(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); diff --git a/repo_update.sh b/repo_update.sh new file mode 100755 index 000000000000..be33a81b7619 --- /dev/null +++ b/repo_update.sh @@ -0,0 +1,4 @@ +make distclean +git submodule sync +git submodule update --init --recursive +git submodule update --remote --recursive mavlink diff --git a/run_gazebo.sh b/run_gazebo.sh new file mode 100755 index 000000000000..ef09159f955a --- /dev/null +++ b/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 \ No newline at end of file diff --git a/run_gazebo_plane_catapult_testfield_new.sh b/run_gazebo_plane_catapult_testfield_new.sh new file mode 100755 index 000000000000..25b3a9d4f66d --- /dev/null +++ b/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 diff --git a/run_jmavsim.sh b/run_jmavsim.sh new file mode 100755 index 000000000000..00aa1f0275a9 --- /dev/null +++ b/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 diff --git a/run_jmavsim_valgrind.sh b/run_jmavsim_valgrind.sh new file mode 100755 index 000000000000..d332738358ac --- /dev/null +++ b/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 diff --git a/src/lib/mathlib/CMakeLists.txt b/src/lib/mathlib/CMakeLists.txt index 629a6a5cca59..a561b3734e1e 100644 --- a/src/lib/mathlib/CMakeLists.txt +++ b/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) diff --git a/src/lib/mathlib/math/RCAC b/src/lib/mathlib/math/RCAC new file mode 160000 index 000000000000..67a5c245d6ea --- /dev/null +++ b/src/lib/mathlib/math/RCAC @@ -0,0 +1 @@ +Subproject commit 67a5c245d6ea3593c1cb4540b4d3b41a071ab38b diff --git a/src/lib/mathlib/math/filter/ContDiffDeadzone.cpp b/src/lib/mathlib/math/filter/ContDiffDeadzone.cpp new file mode 100644 index 000000000000..99bebf1cd45d --- /dev/null +++ b/src/lib/mathlib/math/filter/ContDiffDeadzone.cpp @@ -0,0 +1,63 @@ +// Continuous Differentiable Deadzone Implementation for RCAC + +#include "ContDiffDeadzone.hpp" +#include +//#include + +namespace math +{ + +float ContDiffDeadzone::apply(float sample) +{ + + if (_method == 1) { + // Type 1 Continuous Differntiate Deadzone + if (sample < -_m1_s2) { + return (_m1_s2 - _s1)*(_m1_s2 - _s1)*(-_alf*(_m1_s2 - _s1) + 3.0f*_alf*(sample + _m1_s2)); + } + else if (sample >= -_m1_s2 && sample < -_s1) { + return _alf * (sample + _s1) * (sample + _s1) * (sample + _s1); + } + else if (sample > _m1_s2) { + return (_m1_s2 - _s1)*(_m1_s2 - _s1)*(_alf*(_m1_s2 - _s1) + 3.0f*_alf*(sample - _m1_s2)); + } + else if (sample > _s1 && sample <= _m1_s2) { + return _alf * (sample - _s1) * (sample - _s1) * (sample - _s1); + } + else { + return 0.0; + } + } + else if (_method == 2) { + // / Type 2 Continuous Differntiate Deadzone + if (sample < -_s2) { + return sample; + } + else if (sample >= -_s2 && sample < -_s1) { + return sample*(sample*(_m2_cl[3]*sample + _m2_cl[2]) + _m2_cl[1]) + _m2_cl[0]; + } + else if (sample > _s1 && sample <= _s2) { + return sample*(sample*(_m2_ch[3]*sample + _m2_ch[2]) + _m2_ch[1]) + _m2_ch[0]; + } + else if (sample > _s2) { + return sample; + } + else { + return 0.0; + } + + } + else if (_method == 3) { + // Simple deadzone + if (sample <= -_s1 || sample >= _s1) { + return sample; + } + else { + return 0.0; + } + } + else return sample; +} + + +} // namespace math diff --git a/src/lib/mathlib/math/filter/ContDiffDeadzone.hpp b/src/lib/mathlib/math/filter/ContDiffDeadzone.hpp new file mode 100644 index 000000000000..af3b5d3f839a --- /dev/null +++ b/src/lib/mathlib/math/filter/ContDiffDeadzone.hpp @@ -0,0 +1,73 @@ +// @file ContDiffDeadzone.hpp +// @brief A class to implement a continuously differentiable deadzone for RCAC +/// Author: Joonghyun Lee + +#pragma once + +#include +#include + +namespace math +{ +class __EXPORT ContDiffDeadzone +{ +public: + + ContDiffDeadzone() = default; + + /** + * Add a new raw value to the filter + * + * @return retrieve the filtered result + */ + float apply(float sample); + + /** + * Set params for deadzone implementations + * + * @return Nil + */ + void set_param(unsigned char method, float s1, float alf, float s2) + { + if (s1 < 0.0f) _s1 = 0.0; + else _s1 = s1; + + if (s2 < _s1) _s2 = _s1; + else _s2 = s2; + + _alf = alf; + _method = method; + + // Continuous differentiable deadzone Method 1 + _m1_s2 = _s1 + sqrtf(1.0f / (3.0f * _alf)); + + // Continuous differentiable deadzone Method 2 + float m2_d1 = _s1*(_s1*(_s1*_s1 + _s2*(-4.0f*_s1 + _s2*(3.0f*_s2 + 3.0f))) +_s2*_s2*_s2*(-6.0f*_s2 + 2.0f)) + _s2*_s2*_s2*_s2*(3.0f*_s2 - 2.0f); + float m2_d2 = _s1*(-_s1 + 2.0f*_s2) + _s2*_s2*(-3.0f*_s2 + 2.0f); + + _m2_cl[0] = -_s1*_s1*_s2/m2_d2 - _s2*(_s1*_s1*(_s1*_s1 + _s2*(-4.0f*_s1 + 3.0f*_s2*_s2)))/m2_d1; + _m2_cl[1] = 6.0f*_s2*_s1*_s2*(_s1 - s2*s2)/m2_d1 - _s1*(_s1 + 2.0f*_s2)/m2_d2; + _m2_cl[2] = 3.0f*_s2*(_s1*_s1 - _s2*_s2*_s2)/m2_d1 - (2.0f*_s1 + _s2)/m2_d2; + _m2_cl[3] = 2.0f*_s2/(_s1*(_s1*(_s1 - 3.0f*_s2) + 3.0f*_s2*_s2*_s2) + _s2*_s2*_s2*(-3.0f*_s2 + 2.0f)) - 1.0f/m2_d2; + + _m2_ch[0] = -_m2_cl[0]; + _m2_ch[1] = _m2_cl[1]; + _m2_ch[2] = -_m2_cl[2]; + _m2_ch[3] = _m2_cl[3]; + } + +protected: + + float _s1 = 0.01; + float _s2 = 20.0; + float _alf = 20.0; + + unsigned char _method = 0; + + float _m1_s2 = 0.0; + + float _m2_cl[4]; + float _m2_ch[4]; +}; + +} // namespace math diff --git a/src/lib/mathlib/mathlib.h b/src/lib/mathlib/mathlib.h index 488f5b29308c..891bd04380f1 100644 --- a/src/lib/mathlib/mathlib.h +++ b/src/lib/mathlib/mathlib.h @@ -45,5 +45,6 @@ #include "math/Functions.hpp" #include "math/SearchMin.hpp" #include "math/TrajMath.hpp" +#include "math/RCAC/src/RCAC.h" #endif diff --git a/src/lib/mixer_module/mixer_module.cpp b/src/lib/mixer_module/mixer_module.cpp index e1eff51dc36b..408c55c52990 100644 --- a/src/lib/mixer_module/mixer_module.cpp +++ b/src/lib/mixer_module/mixer_module.cpp @@ -37,6 +37,7 @@ #include #include +// #include using namespace time_literals; @@ -115,6 +116,7 @@ void MixingOutput::updateParams() _mixers->set_thrust_factor(_param_thr_mdl_fac.get()); _mixers->set_airmode((Mixer::Airmode)_param_mc_airmode.get()); } + } bool MixingOutput::updateSubscriptions(bool allow_wq_switch, bool limit_callbacks_to_primary) @@ -439,17 +441,91 @@ bool MixingOutput::update() return true; } +void +MixingOutput::publish_rcac_pos_vel_variables() +{ + rcac_pos_vel_variables_s _rcac_pos_vel_variables{}; // spjohn /**< RCAC variables */ + _rcac_pos_vel_variables_sub.update(&_rcac_pos_vel_variables); + + _rcac_pos_vel_variables.timestamp = hrt_absolute_time(); + _rcac_pos_vel_variables.beta_mot_fr = beta_val; + _rcac_pos_vel_variables.beta_mot_fr_sw = beta_switch_ON; + + _rcac_pos_vel_variables_pub.publish(_rcac_pos_vel_variables); +} + +void +MixingOutput::set_beta_switch(float beta_switch) +{ + beta_switch_ON = 0; + if (beta_switch > 0.0f) + { + beta_switch_ON = 1; + } + beta_val = _param_beta_mot_fr.get(); + + publish_rcac_pos_vel_variables(); +} + void MixingOutput::setAndPublishActuatorOutputs(unsigned num_outputs, actuator_outputs_s &actuator_outputs) { + _rc_channels_sub.update(&_rc_channels_switch); + + // RC channel 16 (SC) : ON ~ 2000 ms = 1.0f + // OFF ~ 1000 ms = -1.0f + float beta_switch = _rc_channels_switch.channels[16]; + + // SITL 1 + beta_switch = 1.0f; // comment out this line to use RC input + if (beta_switch > 0.0f) + { + // NOTE: For HITL set BETA_MOT_FR_SW = 1.0 in QGC before flying + set_beta_switch(_param_beta_mot_fr_sw.get()); + } + else + { + set_beta_switch(beta_switch); + } + + actuator_outputs.noutputs = num_outputs; for (size_t i = 0; i < num_outputs; ++i) { actuator_outputs.output[i] = _current_output_value[i]; } + + // beta is used to scale the upper-limit on the PWM signal sent to the FR (front right) motor + // and is meant to mimic partial actuator failure + // NOTE: 900.0f is the constant PWM value ouput to all QC actuators while landed + if ((beta_switch_ON) && (actuator_outputs.output[0] > 900.0f)) + { + float pwm_upp_lim = _max_value[0] * beta_val; //beta_motor_FR; + // std::cout << "\nbeta_motor_FR = " << beta_motor_FR; + // std::cout << "\npwm_max = " << pwm_upp_lim; + // std::cout << "\nactuator output 0 (before) = " << actuator_outputs.output[0]; + actuator_outputs.output[0] = (int)math::constrain(actuator_outputs.output[0], (float)_min_value[0], pwm_upp_lim); + // std::cout << "\nactuator output 0 (after) = " << actuator_outputs.output[0]; + // actuator_outputs.output[0] = (int)(beta_motor_FR*pwm_max); + // std::cout << "\nactuator outputs (after) = [" << actuator_outputs.output[0] << ", " << actuator_outputs.output[1] << ", " << actuator_outputs.output[2] << ", " << actuator_outputs.output[3] << "]\n\n"; + } + actuator_outputs.timestamp = hrt_absolute_time(); _outputs_pub.publish(actuator_outputs); + + // if ((beta_motor_FR_sw) && (actuator_outputs.output[0] > 900.0f)) + // { + // // PWM bounds (jmavsim): landed = 900, active = [1000, 2000] + // // minimum tolerable constraint on PWM upper limit = 1520 + // // scale factors < 0.76 constrain the FR PWM upper limit to <= 1520 + // float scale_factor_min = 0.76f; + // float pwm_upp_lim = _max_value[0] * scale_factor_min; + // actuator_outputs.output[0] = (int)math::constrain(actuator_outputs.output[0], (float)_min_value[0], pwm_upp_lim); + + // std::cout << "\nactuator outputs = [" << actuator_outputs.output[0] << ", " << actuator_outputs.output[1] << ", " << actuator_outputs.output[2] << ", " << actuator_outputs.output[3] << "]\n"; + // } + // std::cout << "\n\nmax = " << _max_value[0] << ", min = " << _min_value[0] << "\n\n"; } void diff --git a/src/lib/mixer_module/mixer_module.hpp b/src/lib/mixer_module/mixer_module.hpp index 834e9f4b16ee..378a3ca3ba4a 100644 --- a/src/lib/mixer_module/mixer_module.hpp +++ b/src/lib/mixer_module/mixer_module.hpp @@ -38,6 +38,7 @@ #include #include #include +#include #include #include #include @@ -51,6 +52,8 @@ #include #include #include +#include // spjohn +#include // spjohn /** * @class OutputModuleInterface @@ -59,6 +62,9 @@ class OutputModuleInterface : public px4::ScheduledWorkItem, public ModuleParams { public: + + + static constexpr int MAX_ACTUATORS = PWM_OUTPUT_MAX_CHANNELS; OutputModuleInterface(const char *name, const px4::wq_config_t &config) @@ -90,6 +96,8 @@ class OutputModuleInterface : public px4::ScheduledWorkItem, public ModuleParams class MixingOutput : public ModuleParams { public: + // float beta_mot_FR = 1.0f; + static constexpr int MAX_ACTUATORS = OutputModuleInterface::MAX_ACTUATORS; enum class SchedulingPolicy { @@ -177,6 +185,10 @@ class MixingOutput : public ModuleParams int reorderedMotorIndex(int index) const; void setIgnoreLockdown(bool ignore_lockdown) { _ignore_lockdown = ignore_lockdown; } + // void setBetaMotorFR(){beta_mot_FR = _param_beta_mot_fr.get();}; + + bool beta_switch_ON = 0; + float beta_val = 1.0f; protected: void updateParams() override; @@ -197,6 +209,7 @@ class MixingOutput : public ModuleParams void publishMixerStatus(const actuator_outputs_s &actuator_outputs); void updateLatencyPerfCounter(const actuator_outputs_s &actuator_outputs); + static int controlCallback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input); enum class MotorOrdering : int32_t { @@ -238,10 +251,15 @@ class MixingOutput : public ModuleParams uORB::Subscription _armed_sub{ORB_ID(actuator_armed)}; uORB::SubscriptionCallbackWorkItem _control_subs[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; + uORB::Subscription _rc_channels_sub{ORB_ID(rc_channels)}; // spjohn /**< Switch from the RC channel */ + // uORB::Subscription _rcac_pos_vel_variables_sub{ORB_ID(rcac_pos_vel_variables)}; uORB::PublicationMulti _outputs_pub{ORB_ID(actuator_outputs)}; uORB::PublicationMulti _to_mixer_status{ORB_ID(multirotor_motor_limits)}; ///< mixer status flags + uORB::Subscription _rcac_pos_vel_variables_sub{ORB_ID(rcac_pos_vel_variables)}; // spjohn /**< RCAC variables log */ + uORB::Publication _rcac_pos_vel_variables_pub{ORB_ID(rcac_pos_vel_variables)}; // spjohn /**< RCAC variables log */ + actuator_controls_s _controls[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS] {}; actuator_armed_s _armed{}; @@ -274,11 +292,29 @@ class MixingOutput : public ModuleParams perf_counter_t _control_latency_perf; + + rc_channels_s _rc_channels_switch{}; // spjohn /**< Switch from the RC channel */ + + + DEFINE_PARAMETERS( (ParamInt) _param_mc_airmode, ///< multicopter air-mode (ParamFloat) _param_mot_slew_max, (ParamFloat) _param_thr_mdl_fac, ///< thrust to motor control signal modelling factor - (ParamInt) _param_mot_ordering + (ParamInt) _param_mot_ordering, + (ParamFloat) _param_beta_mot_fr, + (ParamFloat) _param_beta_mot_fr_sw + + ); + + /** + * Publish RCAC position and velocity variables for logging and mavlink send commands + */ + void publish_rcac_pos_vel_variables(); - ) + /** + * Publish RCAC position and velocity variables for logging and mavlink send commands + * @param beta_switch Denotes the state of the switch that enables beta. + */ + void set_beta_switch(float beta_switch); }; diff --git a/src/lib/mixer_module/motor_params.c b/src/lib/mixer_module/motor_params.c index 94d246f74b26..9aab1921dd7a 100644 --- a/src/lib/mixer_module/motor_params.c +++ b/src/lib/mixer_module/motor_params.c @@ -69,3 +69,24 @@ PARAM_DEFINE_FLOAT(MOT_SLEW_MAX, 0.0f); * @group PWM Outputs */ PARAM_DEFINE_FLOAT(THR_MDL_FAC, 0.0f); + +/** + * Value used to scale the FR propeller PWM output + * + * @min 0.0 + * @max 1.0 + * @group PWM Outputs + */ +PARAM_DEFINE_FLOAT(BETA_MOT_FR, 1.0f); + +/** + * Switch for beta PWM scaling factor used to degrade front-right motor + * + * ONLY supported for Quads. + * + * @value 0 Disabled + * @value 1 Active + * + * @group Mixer Output + */ +PARAM_DEFINE_FLOAT(BETA_MOT_FR_SW, -1.0f); diff --git a/src/lib/mixer_module/params.c b/src/lib/mixer_module/params.c index 040c77b74b1d..549577255288 100644 --- a/src/lib/mixer_module/params.c +++ b/src/lib/mixer_module/params.c @@ -33,3 +33,5 @@ PARAM_DEFINE_INT32(MC_AIRMODE, 0); * @group Mixer Output */ PARAM_DEFINE_INT32(MOT_ORDERING, 0); + + diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index dac00f755f4d..faa82f9a7de7 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -92,6 +92,23 @@ FixedwingAttitudeControl::parameters_update() _pitch_ctrl.set_k_ff(_param_fw_pr_ff.get()); _pitch_ctrl.set_integrator_max(_param_fw_pr_imax.get()); + /* pitch RCAC parameters */ + _pitch_ctrl._rcac_rate_params_io.set_N_nf(_param_fw_rcac_pitch_rate_nnf.get()); + _pitch_ctrl._rcac_rate_params_io.set_P0(_param_fw_rcac_pitch_rate_p0.get()); + _pitch_ctrl._rcac_rate_params_io.set_alpha(_param_fw_rcac_pitch_rate_alpha.get()); + _pitch_ctrl._rcac_rate_params_io.set_Ru(_param_fw_rcac_pitch_rate_ru.get()); + _pitch_ctrl._rcac_rate_params_io.set_RCAC_EN(_param_fw_rcac_pitch_rate_sw.get()); + + _pitch_ctrl.init_rcac_att(); + + _pitch_ctrl._rcac_att_params_io.set_N_nf(_param_fw_rcac_pitch_att_nnf.get()); + _pitch_ctrl._rcac_att_params_io.set_P0(_param_fw_rcac_pitch_att_p0.get()); + _pitch_ctrl._rcac_att_params_io.set_alpha(_param_fw_rcac_pitch_att_alpha.get()); + _pitch_ctrl._rcac_att_params_io.set_Ru(_param_fw_rcac_pitch_att_ru.get()); + _pitch_ctrl._rcac_att_params_io.set_RCAC_EN(_param_fw_rcac_pitch_att_sw.get()); + + _pitch_ctrl.init_rcac_rate(); + /* roll control parameters */ _roll_ctrl.set_time_constant(_param_fw_r_tc.get()); _roll_ctrl.set_k_p(_param_fw_rr_p.get()); @@ -99,12 +116,38 @@ FixedwingAttitudeControl::parameters_update() _roll_ctrl.set_k_ff(_param_fw_rr_ff.get()); _roll_ctrl.set_integrator_max(_param_fw_rr_imax.get()); + /* roll RCAC parameters */ + _roll_ctrl._rcac_rate_params_io.set_N_nf(_param_fw_rcac_roll_rate_nnf.get()); + _roll_ctrl._rcac_rate_params_io.set_P0(_param_fw_rcac_roll_rate_p0.get()); + _roll_ctrl._rcac_rate_params_io.set_alpha(_param_fw_rcac_roll_rate_alpha.get()); + _roll_ctrl._rcac_rate_params_io.set_Ru(_param_fw_rcac_roll_rate_ru.get()); + _roll_ctrl._rcac_rate_params_io.set_RCAC_EN(_param_fw_rcac_roll_rate_sw.get()); + + _roll_ctrl.init_rcac_att(); + + _roll_ctrl._rcac_att_params_io.set_N_nf(_param_fw_rcac_roll_att_nnf.get()); + _roll_ctrl._rcac_att_params_io.set_P0(_param_fw_rcac_roll_att_p0.get()); + _roll_ctrl._rcac_att_params_io.set_alpha(_param_fw_rcac_roll_att_alpha.get()); + _roll_ctrl._rcac_att_params_io.set_Ru(_param_fw_rcac_roll_att_ru.get()); + _roll_ctrl._rcac_att_params_io.set_RCAC_EN(_param_fw_rcac_roll_att_sw.get()); + + _roll_ctrl.init_rcac_rate(); + /* yaw control parameters */ _yaw_ctrl.set_k_p(_param_fw_yr_p.get()); _yaw_ctrl.set_k_i(_param_fw_yr_i.get()); _yaw_ctrl.set_k_ff(_param_fw_yr_ff.get()); _yaw_ctrl.set_integrator_max(_param_fw_yr_imax.get()); + /* roll RCAC parameters */ + _yaw_ctrl._rcac_rate_params_io.set_N_nf(_param_fw_rcac_yaw_rate_nnf.get()); + _yaw_ctrl._rcac_rate_params_io.set_P0(_param_fw_rcac_yaw_rate_p0.get()); + _yaw_ctrl._rcac_rate_params_io.set_alpha(_param_fw_rcac_yaw_rate_alpha.get()); + _yaw_ctrl._rcac_rate_params_io.set_Ru(_param_fw_rcac_yaw_rate_ru.get()); + _yaw_ctrl._rcac_rate_params_io.set_RCAC_EN(_param_fw_rcac_yaw_rate_sw.get()); + + _yaw_ctrl.init_rcac_rate(); + /* wheel control parameters */ _wheel_ctrl.set_k_p(_param_fw_wr_p.get()); _wheel_ctrl.set_k_i(_param_fw_wr_i.get()); @@ -112,6 +155,16 @@ FixedwingAttitudeControl::parameters_update() _wheel_ctrl.set_integrator_max(_param_fw_wr_imax.get()); _wheel_ctrl.set_max_rate(radians(_param_fw_w_rmax.get())); + /* Rate rcac deadzone parameters for roll/pitch/yaw */ + _pitch_ctrl.set_dz_params(_param_fw_rate_dz_mode.get(), _param_fw_rate_dz_s1.get(), + _param_fw_rate_dz_alf.get(), _param_fw_rate_dz_s2.get()); + _roll_ctrl.set_dz_params(_param_fw_rate_dz_mode.get(), _param_fw_rate_dz_s1.get(), + _param_fw_rate_dz_alf.get(), _param_fw_rate_dz_s2.get()); + _yaw_ctrl.set_dz_params(_param_fw_rate_dz_mode.get(), _param_fw_rate_dz_s1.get(), + _param_fw_rate_dz_alf.get(), _param_fw_rate_dz_s2.get()); + + //TODO: Reinitialize RCAC + return PX4_OK; } @@ -143,6 +196,22 @@ FixedwingAttitudeControl::vehicle_manual_poll() // Always copy the new manual setpoint, even if it wasn't updated, to fill the _actuators with valid values if (_manual_control_setpoint_sub.copy(&_manual_control_setpoint)) { + // PID and RCAC Switch + #ifdef __PX4_NUTTX + _rc_channels_sub.update(&_rc_channels_switch); + bool RCAC_HW_EN = _rc_channels_switch.channels[14] > 0; + bool PID_override = _rc_channels_switch.channels[13] > 0; + #else + bool RCAC_HW_EN = true; + bool PID_override = false; + #endif /* __PX4_NUTTX */ + + set_RCAC_MASTER_EN(RCAC_HW_EN); + + set_RCAC_landed(_landed); + + set_PID_OVERRIDE(PID_override); + // Check if we are in rattitude mode and the pilot is above the threshold on pitch if (_vcontrol_mode.flag_control_rattitude_enabled) { if (fabsf(_manual_control_setpoint.y) > _param_fw_ratt_th.get() @@ -424,6 +493,14 @@ void FixedwingAttitudeControl::Run() _pitch_ctrl.reset_integrator(); _yaw_ctrl.reset_integrator(); _wheel_ctrl.reset_integrator(); + + // RCAC Resetter when Aircraft is On Grounds + //TODO: Check Logic - Not sure if this is necessary or hurts performance in FW. + _roll_ctrl.init_rcac_att(); + _roll_ctrl.init_rcac_rate(); + _pitch_ctrl.init_rcac_att(); + _pitch_ctrl.init_rcac_rate(); + _yaw_ctrl.init_rcac_rate(); } /* Prepare data for attitude controllers */ @@ -649,6 +726,10 @@ void FixedwingAttitudeControl::Run() _vcontrol_mode.flag_control_manual_enabled) { _actuators_0_pub.publish(_actuators); } + + publish_rcac_roll_variables(); + publish_rcac_pitch_variables(); + publish_rcac_yaw_variables(); } perf_end(_loop_perf); @@ -780,3 +861,136 @@ extern "C" __EXPORT int fw_att_control_main(int argc, char *argv[]) { return FixedwingAttitudeControl::main(argc, argv); } + +/* RCAC Roll Controller Publisher Function */ +void FixedwingAttitudeControl::publish_rcac_roll_variables() +{ + // PX4_INFO("Entered FW Att Control RCAC Publisher"); + _rcac_fw_roll.timestamp = hrt_absolute_time(); + _rcac_fw_roll.ii_rate = _roll_ctrl._rcac_rate_public_io.get_kk(); + _rcac_fw_roll.alpha_pid_rate = _roll_ctrl._rcac_rate_public_io.get_alpha(); + _rcac_fw_roll.p11_rate = _roll_ctrl._rcac_rate_public_io.get_Pij(0,0); + _rcac_fw_roll.p12_rate = _roll_ctrl._rcac_rate_public_io.get_Pij(0,1); + _rcac_fw_roll.p21_rate = _roll_ctrl._rcac_rate_public_io.get_Pij(1,0); + _rcac_fw_roll.p22_rate = _roll_ctrl._rcac_rate_public_io.get_Pij(1,1); + _rcac_fw_roll.rcac_z_rate = _roll_ctrl.get_rate_error(); + _rcac_fw_roll.rcac_dzz_rate = _roll_ctrl.get_rate_dzz(); + _rcac_fw_roll.rcac_u_rate = _roll_ctrl._rcac_rate_public_io.get_uk(); + _rcac_fw_roll.rcac_nnf_rate = _roll_ctrl._rcac_rate_params_io.get_N_nf(); + _rcac_fw_roll.switch_rate = _roll_ctrl._rcac_rate_public_io.get_RCAC_status();; + _rcac_fw_roll.rate_dz_alf = _param_fw_rate_dz_alf.get(); + _rcac_fw_roll.rate_dz_s1 = _param_fw_rate_dz_s1.get(); + _rcac_fw_roll.rate_dz_s2 = _param_fw_rate_dz_s2.get(); + _rcac_fw_roll.rate_dz_type = _param_fw_rate_dz_mode.get(); + + _rcac_fw_roll.ii_att = _roll_ctrl._rcac_att_public_io.get_kk(); + _rcac_fw_roll.alpha_pid_att = _roll_ctrl._rcac_att_public_io.get_alpha(); + _rcac_fw_roll.p11_att = _roll_ctrl._rcac_att_public_io.get_P11(); + _rcac_fw_roll.rcac_z_att = _roll_ctrl._rcac_att_public_io.get_zk(); + _rcac_fw_roll.rcac_u_att = _roll_ctrl._rcac_att_public_io.get_uk(); + _rcac_fw_roll.rcac_nnf_att = _roll_ctrl._rcac_att_params_io.get_N_nf(); + _rcac_fw_roll.switch_att = _roll_ctrl._rcac_att_public_io.get_RCAC_status();; + + for (int i = 0; i < RCAC_RATE_L_THETA; ++i) { + _rcac_fw_roll.rcac_theta_rate[i] = _roll_ctrl._rcac_rate_public_io.get_theta(i); + _rcac_fw_roll.px4_theta_rate[i] = _roll_ctrl.get_PX4_theta(i); + } + + _rcac_fw_roll.px4_theta_att = 1 / _roll_ctrl.get_tc(); + _rcac_fw_roll.rcac_theta_att = _roll_ctrl._rcac_att_public_io.get_theta(0); + + _rcac_fw_roll_pub.publish(_rcac_fw_roll); +} + +/* RCAC Pitch Controller Publisher Function */ +void FixedwingAttitudeControl::publish_rcac_pitch_variables() +{ + // PX4_INFO("Entered FW Att Control RCAC Publisher"); + _rcac_fw_pitch.timestamp = hrt_absolute_time(); + _rcac_fw_pitch.ii_rate = _pitch_ctrl._rcac_rate_public_io.get_kk(); + _rcac_fw_pitch.alpha_pid_rate = _pitch_ctrl._rcac_rate_public_io.get_alpha(); + _rcac_fw_pitch.p11_rate = _pitch_ctrl._rcac_rate_public_io.get_Pij(0,0); + _rcac_fw_pitch.p12_rate = _pitch_ctrl._rcac_rate_public_io.get_Pij(0,1); + _rcac_fw_pitch.p21_rate = _pitch_ctrl._rcac_rate_public_io.get_Pij(1,0); + _rcac_fw_pitch.p22_rate = _pitch_ctrl._rcac_rate_public_io.get_Pij(1,1); + _rcac_fw_pitch.rcac_z_rate = _pitch_ctrl.get_rate_error(); + _rcac_fw_pitch.rcac_dzz_rate = _pitch_ctrl.get_rate_dzz(); + _rcac_fw_pitch.rcac_u_rate = _pitch_ctrl._rcac_rate_public_io.get_uk(); + _rcac_fw_pitch.rcac_nnf_rate = _pitch_ctrl._rcac_rate_params_io.get_N_nf(); + _rcac_fw_pitch.switch_rate = _pitch_ctrl._rcac_rate_public_io.get_RCAC_status(); + _rcac_fw_pitch.rate_dz_alf = _param_fw_rate_dz_alf.get(); + _rcac_fw_pitch.rate_dz_s1 = _param_fw_rate_dz_s1.get(); + _rcac_fw_pitch.rate_dz_s2 = _param_fw_rate_dz_s2.get(); + _rcac_fw_pitch.rate_dz_type = _param_fw_rate_dz_mode.get(); + + _rcac_fw_pitch.ii_att = _pitch_ctrl._rcac_att_public_io.get_kk(); + _rcac_fw_pitch.alpha_pid_att = _pitch_ctrl._rcac_att_public_io.get_alpha(); + _rcac_fw_pitch.p11_att = _pitch_ctrl._rcac_att_public_io.get_P11(); + _rcac_fw_pitch.rcac_z_att = _pitch_ctrl._rcac_att_public_io.get_zk(); + _rcac_fw_pitch.rcac_u_att = _pitch_ctrl._rcac_att_public_io.get_uk(); + _rcac_fw_pitch.rcac_nnf_att = _pitch_ctrl._rcac_att_params_io.get_N_nf(); + _rcac_fw_pitch.switch_att = _pitch_ctrl._rcac_att_public_io.get_RCAC_status(); + + for (int i = 0; i < RCAC_RATE_L_THETA; ++i) { + _rcac_fw_pitch.rcac_theta_rate[i] = _pitch_ctrl._rcac_rate_public_io.get_theta(i); + _rcac_fw_pitch.px4_theta_rate[i] = _pitch_ctrl.get_PX4_theta(i); + } + + _rcac_fw_pitch.px4_theta_att = 1 / _pitch_ctrl.get_tc(); + _rcac_fw_pitch.rcac_theta_att = _pitch_ctrl._rcac_att_public_io.get_theta(0); + + _rcac_fw_pitch_pub.publish(_rcac_fw_pitch); +} + +/* RCAC Yaw Controller Publisher Function */ +void FixedwingAttitudeControl::publish_rcac_yaw_variables() +{ + // PX4_INFO("Entered FW Att Control RCAC Publisher"); + _rcac_fw_yaw.timestamp = hrt_absolute_time(); + _rcac_fw_yaw.ii_rate = _yaw_ctrl._rcac_rate_public_io.get_kk(); + _rcac_fw_yaw.alpha_pid_rate = _yaw_ctrl._rcac_rate_public_io.get_alpha(); + _rcac_fw_yaw.p11_rate = _yaw_ctrl._rcac_rate_public_io.get_P11(); + _rcac_fw_yaw.rcac_z_rate = _yaw_ctrl.get_rate_error(); + _rcac_fw_yaw.rcac_dzz_rate = _yaw_ctrl.get_rate_dzz(); + _rcac_fw_yaw.rcac_u_rate = _yaw_ctrl._rcac_rate_public_io.get_uk(); + _rcac_fw_yaw.rcac_nnf_rate = _yaw_ctrl._rcac_rate_params_io.get_N_nf(); + _rcac_fw_yaw.switch_rate = _yaw_ctrl._rcac_rate_public_io.get_RCAC_status(); + _rcac_fw_yaw.rate_dz_alf = _param_fw_rate_dz_alf.get(); + _rcac_fw_yaw.rate_dz_s1 = _param_fw_rate_dz_s1.get(); + _rcac_fw_yaw.rate_dz_s2 = _param_fw_rate_dz_s2.get(); + _rcac_fw_yaw.rate_dz_type = _param_fw_rate_dz_mode.get(); + + + for (int i = 0; i < RCAC_RATE_L_THETA; ++i) { + _rcac_fw_yaw.rcac_theta_rate[i] = _yaw_ctrl._rcac_rate_public_io.get_theta(i); + _rcac_fw_yaw.px4_theta_rate[i] = _yaw_ctrl.get_PX4_theta(i); + } + + _rcac_fw_yaw_pub.publish(_rcac_fw_yaw); +} + +void FixedwingAttitudeControl::set_RCAC_MASTER_EN(bool MASTER_EN_in) +{ + _roll_ctrl._rcac_att_public_io.set_MASTER_EN(MASTER_EN_in); + _roll_ctrl._rcac_rate_public_io.set_MASTER_EN(MASTER_EN_in); + _pitch_ctrl._rcac_att_public_io.set_MASTER_EN(MASTER_EN_in); + _pitch_ctrl._rcac_rate_public_io.set_MASTER_EN(MASTER_EN_in); + _yaw_ctrl._rcac_rate_public_io.set_MASTER_EN(MASTER_EN_in); +} + +void FixedwingAttitudeControl::set_PID_OVERRIDE(bool PID_OVERRIDE_in) +{ + _roll_ctrl._rcac_att_public_io.set_PID_OVERRIDE(PID_OVERRIDE_in); + _roll_ctrl._rcac_rate_public_io.set_PID_OVERRIDE(PID_OVERRIDE_in); + _pitch_ctrl._rcac_att_public_io.set_PID_OVERRIDE(PID_OVERRIDE_in); + _pitch_ctrl._rcac_rate_public_io.set_PID_OVERRIDE(PID_OVERRIDE_in); + _yaw_ctrl._rcac_rate_public_io.set_PID_OVERRIDE(PID_OVERRIDE_in); +} + +void FixedwingAttitudeControl::set_RCAC_landed(bool landed_in) { + _roll_ctrl._rcac_att_public_io.set_landed(landed_in); + _roll_ctrl._rcac_rate_public_io.set_landed(landed_in); + _pitch_ctrl._rcac_att_public_io.set_landed(landed_in); + _pitch_ctrl._rcac_rate_public_io.set_landed(landed_in); + _yaw_ctrl._rcac_rate_public_io.set_landed(landed_in); +} diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp index 3a22a0bc5875..e6b5ec998d03 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp @@ -56,6 +56,7 @@ #include #include #include +#include #include #include #include @@ -66,6 +67,10 @@ #include #include #include +#include +#include +#include +#include using matrix::Eulerf; using matrix::Quatf; @@ -108,6 +113,8 @@ class FixedwingAttitudeControl final : public ModuleBase*/ uORB::SubscriptionData _airspeed_validated_sub{ORB_ID(airspeed_validated)}; @@ -115,6 +122,9 @@ class FixedwingAttitudeControl final : public ModuleBase _attitude_sp_pub; uORB::Publication _rate_sp_pub{ORB_ID(vehicle_rates_setpoint)}; uORB::PublicationMulti _rate_ctrl_status_pub{ORB_ID(rate_ctrl_status)}; + uORB::Publication _rcac_fw_roll_pub{ORB_ID(rcac_fw_roll)}; + uORB::Publication _rcac_fw_pitch_pub{ORB_ID(rcac_fw_pitch)}; + uORB::Publication _rcac_fw_yaw_pub{ORB_ID(rcac_fw_yaw)}; actuator_controls_s _actuators {}; /**< actuator control inputs */ manual_control_setpoint_s _manual_control_setpoint {}; /**< r/c channel data */ @@ -123,6 +133,11 @@ class FixedwingAttitudeControl final : public ModuleBase) _param_trim_pitch, (ParamFloat) _param_trim_roll, - (ParamFloat) _param_trim_yaw + (ParamFloat) _param_trim_yaw, + + (ParamFloat) _param_fw_rcac_roll_rate_p0, + (ParamFloat) _param_fw_rcac_roll_rate_ru, + (ParamFloat) _param_fw_rcac_roll_rate_alpha, + (ParamFloat) _param_fw_rcac_roll_rate_nnf, + (ParamBool ) _param_fw_rcac_roll_rate_sw, + + (ParamFloat) _param_fw_rcac_roll_att_p0, + (ParamFloat) _param_fw_rcac_roll_att_ru, + (ParamFloat) _param_fw_rcac_roll_att_alpha, + (ParamFloat) _param_fw_rcac_roll_att_nnf, + (ParamBool ) _param_fw_rcac_roll_att_sw, + + (ParamFloat) _param_fw_rcac_pitch_rate_p0, + (ParamFloat) _param_fw_rcac_pitch_rate_ru, + (ParamFloat) _param_fw_rcac_pitch_rate_alpha, + (ParamFloat) _param_fw_rcac_pitch_rate_nnf, + (ParamBool ) _param_fw_rcac_pitch_rate_sw, + + (ParamFloat) _param_fw_rcac_pitch_att_p0, + (ParamFloat) _param_fw_rcac_pitch_att_ru, + (ParamFloat) _param_fw_rcac_pitch_att_alpha, + (ParamFloat) _param_fw_rcac_pitch_att_nnf, + (ParamBool ) _param_fw_rcac_pitch_att_sw, + + (ParamFloat) _param_fw_rcac_yaw_rate_p0, + (ParamFloat) _param_fw_rcac_yaw_rate_ru, + (ParamFloat) _param_fw_rcac_yaw_rate_alpha, + (ParamFloat) _param_fw_rcac_yaw_rate_nnf, + (ParamBool ) _param_fw_rcac_yaw_rate_sw, + + (ParamInt) _param_fw_rate_dz_mode, + (ParamFloat) _param_fw_rate_dz_s1, + (ParamFloat) _param_fw_rate_dz_s2, + (ParamFloat) _param_fw_rate_dz_alf ) ECL_RollController _roll_ctrl; @@ -232,4 +282,13 @@ class FixedwingAttitudeControl final : public ModuleBase -#include + +//using namespace math; ECL_Controller::ECL_Controller() : _last_run(0), @@ -65,6 +66,9 @@ ECL_Controller::ECL_Controller() : _rate_setpoint(0.0f), _bodyrate_setpoint(0.0f) { + _rcac_rate_params.initParams.RBlock_EN = true; + _rcac_rate_public_io = RCAC_Public_IO(&_rcac_rate); + _rcac_rate_params_io = RCACParams_IO(&_rcac_rate_params); } void ECL_Controller::reset_integrator() @@ -109,6 +113,11 @@ void ECL_Controller::set_bodyrate_setpoint(float rate) _bodyrate_setpoint = math::constrain(rate, -_max_rate, _max_rate); } +void ECL_Controller::set_dz_params(unsigned char type, float s1, float alf, float s2) +{ + _ContDiffDeadzone.set_param(type, s1, alf, s2); +} + float ECL_Controller::get_rate_error() { return _rate_error; @@ -118,7 +127,7 @@ float ECL_Controller::get_desired_rate() { return _rate_setpoint; } - +float get_PX4_theta(); float ECL_Controller::get_desired_bodyrate() { return _bodyrate_setpoint; @@ -129,6 +138,32 @@ float ECL_Controller::get_integrator() return _integrator; } +float ECL_Controller::get_PX4_theta(size_t idx) +{ + switch(idx) { + + case 0: + return _k_p; + break; + case 1: + return _k_i; + break; + default: + // Not supposed to happen + return FLT_MAX; + } +} + +float ECL_Controller::get_tc() +{ + return _tc; +} + +float ECL_Controller::get_rate_dzz() +{ + return _z_k_rate_dz; +} + float ECL_Controller::constrain_airspeed(float airspeed, float minspeed, float maxspeed) { float airspeed_result = airspeed; @@ -143,3 +178,8 @@ float ECL_Controller::constrain_airspeed(float airspeed, float minspeed, float m return airspeed_result; } + +void ECL_Controller::init_rcac_rate() +{ + _rcac_rate = RCAC(_rcac_rate_params, _rcac_rate.get_MASTER_EN(), _integrator_max); +} diff --git a/src/modules/fw_att_control/ecl_controller.h b/src/modules/fw_att_control/ecl_controller.h index 37fd64e9f7b4..2eebfad0324c 100644 --- a/src/modules/fw_att_control/ecl_controller.h +++ b/src/modules/fw_att_control/ecl_controller.h @@ -48,9 +48,18 @@ #pragma once +#define RCAC_RATE_L_THETA 2 // P-I Control RCAC +#define RCAC_RATE_L_RBLOCK 2 + +#define RCAC_ATT_L_THETA 1 // P Control RCAC +#define RCAC_ATT_L_RBLOCK 2 + #include +#include #include +#include + struct ECL_ControlData { float roll; float pitch; @@ -88,17 +97,28 @@ class ECL_Controller void set_k_p(float k_p); void set_k_i(float k_i); void set_k_ff(float k_ff); - void set_integrator_max(float max); + virtual void set_integrator_max(float max); //Set to Virtual for RCAC Integrator Max void set_max_rate(float max_rate); void set_bodyrate_setpoint(float rate); + void set_dz_params(unsigned char type, float s1, float alf, float s2); /* Getters */ float get_rate_error(); float get_desired_rate(); float get_desired_bodyrate(); float get_integrator(); + float get_PX4_theta(size_t idx); + float get_tc(); + float get_rate_dzz(); - void reset_integrator(); + virtual void reset_integrator(); //Set to Virtual for RCAC Integrator Reset + + /* RCAC Rate Public Classes with Getter/Setters */ + RCAC_Public_IO _rcac_rate_public_io; + RCACParams_IO _rcac_rate_params_io; + + /* RCAC initializer */ + void init_rcac_rate(); protected: uint64_t _last_run; @@ -114,4 +134,30 @@ class ECL_Controller float _rate_setpoint; float _bodyrate_setpoint; float constrain_airspeed(float airspeed, float minspeed, float maxspeed); + + //RCAC implementation for Rate Controller. + RCAC _rcac_rate; + RCACParams _rcac_rate_params; + + float z_k_rate, z_km1_rate, u_k_rate, u_km1_rate; + + //Continuous Deadzone for rate controller + math::ContDiffDeadzone _ContDiffDeadzone; + + float _dz_s1; + float _dz_alf; + float _dz_s2; + unsigned char _dz_type; + float _z_k_rate_dz; + + + // bool _rcac_rate_SW; + // bool _rcac_rate_Rblock_SW; + // int _rcac_rate_e_fun; + // float _rcac_rate_P0; + // float _rcac_rate_Rz; + // float _rcac_rate_Ru; + // float _rcac_rate_lambda; + // float _rcac_rate_N; + // float _rate_pid_alpha; }; diff --git a/src/modules/fw_att_control/ecl_pitch_controller.cpp b/src/modules/fw_att_control/ecl_pitch_controller.cpp index 47890efaf1e9..afada87337d2 100644 --- a/src/modules/fw_att_control/ecl_pitch_controller.cpp +++ b/src/modules/fw_att_control/ecl_pitch_controller.cpp @@ -41,7 +41,14 @@ #include "ecl_pitch_controller.h" #include #include -#include +// #include + +ECL_PitchController::ECL_PitchController() : ECL_Controller() +{ + _rcac_att_params.initParams.RBlock_EN = true; + _rcac_att_public_io = RCAC_Public_IO(&_rcac_att); + _rcac_att_params_io = RCACParams_IO(&_rcac_att_params); +} float ECL_PitchController::control_attitude(const float dt, const ECL_ControlData &ctl_data) { @@ -57,9 +64,21 @@ float ECL_PitchController::control_attitude(const float dt, const ECL_ControlDat /* Calculate the error */ float pitch_error = ctl_data.pitch_setpoint - ctl_data.pitch; + if ( (_rcac_att.get_RCAC_EN() && _rcac_att.getkk() == 0) || + (!_rcac_att.get_RCAC_EN() && _rcac_att.getkk() != 0) ) + { + init_rcac_att(); + } + /* Apply P controller: rate setpoint from current error and time constant */ _rate_setpoint = pitch_error / _tc; + matrix::Matrix Phi_att; + Phi_att(0, 0) = pitch_error; + u_k_att = _rcac_att.compute_uk(_rate_error, Phi_att, _rcac_att.get_rcac_uk()); + + _rate_setpoint = _rcac_att.get_alpha() * _rate_setpoint + u_k_att; + return _rate_setpoint; } @@ -81,10 +100,23 @@ float ECL_PitchController::control_bodyrate(const float dt, const ECL_ControlDat /* Calculate body angular rate error */ _rate_error = _bodyrate_setpoint - ctl_data.body_y_rate; + // Cyinyong: deadzone for rate error + _z_k_rate_dz = _ContDiffDeadzone.apply(_rate_error); + + //TODO: Possibly not need this if RCAC_EN is modified directly instead of changing kk + /* Initialize RCAC on first iteration and rcac switched on. */ + if ( (_rcac_rate.get_RCAC_EN() && _rcac_rate.getkk() == 0) || + (!_rcac_rate.get_RCAC_EN() && _rcac_rate.getkk() != 0) ) + { + init_rcac_rate(); + } + if (!ctl_data.lock_integrator && _k_i > 0.0f) { /* Integral term scales with 1/IAS^2 */ float id = _rate_error * dt * ctl_data.scaler * ctl_data.scaler; + // Cyinyong: Created a dz integral variable for rcac + float id_rcac = _z_k_rate_dz * ctl_data.scaler * ctl_data.scaler; /* * anti-windup: do not allow integrator to increase if actuator is at limit @@ -100,6 +132,11 @@ float ECL_PitchController::control_bodyrate(const float dt, const ECL_ControlDat /* add and constrain */ _integrator = math::constrain(_integrator + id * _k_i, -_integrator_max, _integrator_max); + + /* RCAC lib integrator (No gain with integral) */ + // if (_rcac_rate_params.tuneParams.RCAC_EN) + // Cyinyong: edit for id to include dz and remove *dt for rcac integration + _rcac_rate.update_integral(id_rcac, dt); } /* Apply PI rate controller and store non-limited output */ @@ -108,6 +145,14 @@ float ECL_PitchController::control_bodyrate(const float dt, const ECL_ControlDat _rate_error * _k_p * ctl_data.scaler * ctl_data.scaler + _integrator; + matrix::Matrix Phi_rate; + // Cyinyong: add in dz for z_k_rate + Phi_rate(0, 0) = _z_k_rate_dz; //_rate_error; + Phi_rate(0, 1) = _rcac_rate.get_rcac_integral(); + u_k_rate = _rcac_rate.compute_uk(_z_k_rate_dz, Phi_rate, _rcac_rate.get_rcac_uk()); + + _last_output = _last_output * _rcac_rate.get_alpha() + u_k_rate; + return math::constrain(_last_output, -1.0f, 1.0f); } @@ -121,3 +166,35 @@ float ECL_PitchController::control_euler_rate(const float dt, const ECL_ControlD return control_bodyrate(dt, ctl_data); } + +// matrix::Vector ECL_PitchController::get_PX4_theta() +// { +// //TODO: Update this function if FF is being used with RCAC Later +// matrix::Vector PX4_theta; +// PX4_theta(0) = _k_p; +// PX4_theta(1) = _k_i; +// return PX4_theta; +// } + +void ECL_PitchController::reset_integrator() +{ + _integrator = 0.0f; + _rcac_rate.reset_integral(); +} + +void ECL_PitchController::init_rcac_att() +{ + _rcac_att = RCAC(_rcac_att_params, _rcac_att.get_MASTER_EN()); +} + +//TODO: Double check if this function is necessary +void ECL_PitchController::set_integrator_max(float max) +{ + _integrator_max = max; + // _rcac_rate.set_lim_int(max); +} + +// void ECL_PitchController::reset_RCAC_kk() +// { +// _rcac_rate.reset_kk(); +// } diff --git a/src/modules/fw_att_control/ecl_pitch_controller.h b/src/modules/fw_att_control/ecl_pitch_controller.h index 06498cfee7bb..ee85bcbf1dce 100644 --- a/src/modules/fw_att_control/ecl_pitch_controller.h +++ b/src/modules/fw_att_control/ecl_pitch_controller.h @@ -49,6 +49,10 @@ #ifndef ECL_PITCH_CONTROLLER_H #define ECL_PITCH_CONTROLLER_H +// #define RCAC_PITCH_L_THETA 2 // P-I Control RCAC +// #define RCAC_PITCH_L_RBLOCK 2 + + #include #include "ecl_controller.h" @@ -57,12 +61,14 @@ class ECL_PitchController : public ECL_Controller { public: - ECL_PitchController() = default; + ECL_PitchController(); ~ECL_PitchController() = default; float control_attitude(const float dt, const ECL_ControlData &ctl_data) override; float control_euler_rate(const float dt, const ECL_ControlData &ctl_data) override; float control_bodyrate(const float dt, const ECL_ControlData &ctl_data) override; + void reset_integrator() override; + void set_integrator_max(float max) override; /* Additional Setters */ void set_max_rate_pos(float max_rate_pos) @@ -85,9 +91,92 @@ class ECL_PitchController : _roll_ff = roll_ff; } + + /* PX4 Gains For Logging */ + // matrix::Vector get_PX4_theta(); + + /* Attitude Control RCAC Declarations */ + + + RCAC_Public_IO _rcac_att_public_io; + RCACParams_IO _rcac_att_params_io; + + /* Initialize Att RCAC Variables */ + void init_rcac_att(); + + // void reset_RCAC_kk(); + + /* RCAC Getter Functions - For Publishing */ + // int get_RCAC_pitch_ii() {return _rcac_pitch.getkk();} + // float get_RCAC_pitch_Ru() {return rcac_pitch_Ru;} + // float get_RCAC_pitch_Rz() {return rcac_pitch_Rz;} + // float get_RCAC_pitch_P0() {return rcac_pitch_P0;} + // float get_RCAC_pitch_P11() {return _rcac_pitch.get_rcac_P(0, 0);} + // float get_RCAC_pitch_alpha() {return alpha_PID_pitch;} + // float get_RCAC_pitch_uk() {return _rcac_pitch.get_rcac_uk();} + // bool get_RCAC_pitch_switch() {return RCAC_pitch_SW;} + // matrix::Vector get_RCAC_theta(); + + /* RCAC Setter Functions - For Alteration in QGC */ + // void set_RCAC_pitch_Ru(float pitch_Ru_in) + // { + // // if (pitch_Ru_in != rcac_pitch_Ru) + // PX4_INFO("[Pitch RCAC Param Update] Ru: %6.4f", (double)pitch_Ru_in); + // rcac_pitch_Ru = pitch_Ru_in; + // } + + // void set_RCAC_pitch_Rz(float pitch_Rz_in) + // { + // // if (rcac_pitch_Rz != pitch_Rz_in) + // PX4_INFO("[Pitch RCAC] Rz Update: %6.4f", (double)pitch_Rz_in); + // rcac_pitch_Rz = pitch_Rz_in; + // } + + // void set_RCAC_pitch_P0(float pitch_P0_in) + // { + // // if (rcac_pitch_P0 != pitch_P0_in) + // PX4_INFO("[Pitch RCAC Param Update] P0: %6.4f", (double)pitch_P0_in); + // rcac_pitch_P0 = pitch_P0_in; + // } + + // void set_RCAC_pitch_alpha(float pitch_alpha_in) + // { + // // if (alpha_PID_pitch != pitch_alpha_in) + // PX4_INFO("[Pitch RCAC Param Update] Alpha: %6.4f", (double)pitch_alpha_in); + // alpha_PID_pitch = pitch_alpha_in; + // } + + // void set_RCAC_pitch_SW(bool pitch_SW_in) + // { + // // if (RCAC_pitch_SW != pitch_SW_in) + // PX4_INFO("[Pitch RCAC Param Update] RCAC Switch: %s", pitch_SW_in ? "true" : "false"); + // RCAC_pitch_SW = pitch_SW_in; + // } + protected: float _max_rate_neg{0.0f}; float _roll_ff{0.0f}; + +private: + + // Jun 24, 2021: New RCAC Variables + + RCAC _rcac_att; + RCACParams _rcac_att_params; + + float z_k_att, z_km1_att, u_k_att, u_km1_att; + + // bool RCAC_pitch_SW = false; + // bool RCAC_pitch_Rblock_SW = true; + + // matrix::Matrix rcac_pitch_Rblock; + // int rcac_pitch_e_fun = 0; + // float rcac_pitch_P0 = 0.0011f;// + // float rcac_pitch_Rz = 1.0; + // float rcac_pitch_Ru = 1.0;// + // float rcac_pitch_lambda = 1.0; + // float rcac_pitch_N = -1.0;// + // float alpha_PID_pitch = 1.0f;// }; #endif // ECL_PITCH_CONTROLLER_H diff --git a/src/modules/fw_att_control/ecl_roll_controller.cpp b/src/modules/fw_att_control/ecl_roll_controller.cpp index 9bc44b76aa5f..ff55725f341c 100644 --- a/src/modules/fw_att_control/ecl_roll_controller.cpp +++ b/src/modules/fw_att_control/ecl_roll_controller.cpp @@ -41,7 +41,15 @@ #include "ecl_roll_controller.h" #include #include -#include + +//TODO: Need to do params update based on QGC + +ECL_RollController::ECL_RollController() : ECL_Controller() +{ + _rcac_att_params.initParams.RBlock_EN = true; + _rcac_att_public_io = RCAC_Public_IO(&_rcac_att); + _rcac_att_params_io = RCACParams_IO(&_rcac_att_params); +} float ECL_RollController::control_attitude(const float dt, const ECL_ControlData &ctl_data) { @@ -55,9 +63,21 @@ float ECL_RollController::control_attitude(const float dt, const ECL_ControlData /* Calculate the error */ float roll_error = ctl_data.roll_setpoint - ctl_data.roll; + if ( (_rcac_att.get_RCAC_EN() && _rcac_att.getkk() == 0) || + (!_rcac_att.get_RCAC_EN() && _rcac_att.getkk() != 0) ) + { + init_rcac_att(); + } + /* Apply P controller: rate setpoint from current error and time constant */ _rate_setpoint = roll_error / _tc; + matrix::Matrix Phi_att; + Phi_att(0, 0) = roll_error; + u_k_att = _rcac_att.compute_uk(_rate_error, Phi_att, _rcac_att.get_rcac_uk()); + + _rate_setpoint = _rcac_att.get_alpha() * _rate_setpoint + u_k_att; + return _rate_setpoint; } @@ -78,10 +98,23 @@ float ECL_RollController::control_bodyrate(const float dt, const ECL_ControlData /* Calculate body angular rate error */ _rate_error = _bodyrate_setpoint - ctl_data.body_x_rate; + // Cyinyong: deadzone for rate error + _z_k_rate_dz = _ContDiffDeadzone.apply(_rate_error); + + /* Initialize RCAC on first iteration and roll switched on. */ + if ( (_rcac_rate.get_RCAC_EN() && _rcac_rate.getkk() == 0) || + (!_rcac_rate.get_RCAC_EN() && _rcac_rate.getkk() != 0) ) + { + init_rcac_rate(); + } + + if (!ctl_data.lock_integrator && _k_i > 0.0f) { /* Integral term scales with 1/IAS^2 */ float id = _rate_error * dt * ctl_data.scaler * ctl_data.scaler; + // Cyinyong: Created a dz integral variable for rcac + float id_rcac = _z_k_rate_dz * ctl_data.scaler * ctl_data.scaler; /* * anti-windup: do not allow integrator to increase if actuator is at limit @@ -97,6 +130,11 @@ float ECL_RollController::control_bodyrate(const float dt, const ECL_ControlData /* add and constrain */ _integrator = math::constrain(_integrator + id * _k_i, -_integrator_max, _integrator_max); + + /* RCAC lib integrator (No gain with integral) */ + // if (_rcac_rate_params.tuneParams.RCAC_EN) + // Cyinyong: edit for id to include dz and remove *dt for rcac integration + _rcac_rate.update_integral(id_rcac, dt); } /* Apply PI rate controller and store non-limited output */ @@ -105,6 +143,14 @@ float ECL_RollController::control_bodyrate(const float dt, const ECL_ControlData _rate_error * _k_p * ctl_data.scaler * ctl_data.scaler + _integrator; + matrix::Matrix Phi_rate; + // Cyinyong: add in dz for z_k_rate + Phi_rate(0, 0) = _z_k_rate_dz; //_rate_error; + Phi_rate(0, 1) = _rcac_rate.get_rcac_integral(); + u_k_rate = _rcac_rate.compute_uk(_z_k_rate_dz, Phi_rate, _rcac_rate.get_rcac_uk()); + + _last_output = _last_output * _rcac_rate.get_alpha() + u_k_rate; + return math::constrain(_last_output, -1.0f, 1.0f); } @@ -117,3 +163,60 @@ float ECL_RollController::control_euler_rate(const float dt, const ECL_ControlDa return control_bodyrate(dt, ctl_data); } + +void ECL_RollController::init_rcac_att() +{ + _rcac_att = RCAC(_rcac_att_params, _rcac_att.get_MASTER_EN()); +} + +// void ECL_RollController::init_RCAC_roll() +// { +// if (RCAC_roll_Rblock_SW) +// { +// rcac_roll_Rblock(0,0) = rcac_roll_Rz; +// rcac_roll_Rblock(1,1) = rcac_roll_Ru; +// _rcac_roll = RCAC (rcac_roll_P0, rcac_roll_lambda, rcac_roll_Rblock, rcac_roll_N, rcac_roll_e_fun, _integrator_max); +// } + +// else +// { +// _rcac_roll = RCAC (rcac_roll_P0, rcac_roll_lambda, rcac_roll_N, rcac_roll_e_fun, _integrator_max); +// } +// } + +// matrix::Vector ECL_RollController::get_RCAC_theta() +// { +// matrix::Vector RCAC_theta; + +// for (size_t i = 0; i < 2; ++i) +// { +// RCAC_theta(i) = _rcac_roll.get_rcac_theta(i); +// } +// return RCAC_theta; +// } + +// matrix::Vector ECL_RollController::get_PX4_theta() +// { +// //TODO: Update this function if FF is being used with RCAC Later +// matrix::Vector PX4_theta; +// PX4_theta(0) = _k_p; +// PX4_theta(1) = _k_i; +// return PX4_theta; +// } + +void ECL_RollController::reset_integrator() +{ + _integrator = 0.0f; + _rcac_rate.reset_integral(); +} + +void ECL_RollController::set_integrator_max(float max) +{ + _integrator_max = max; + // _rcac_rate.set_lim_int(max); +} + +// void ECL_RollController::reset_RCAC_kk() +// { +// _rcac_roll.reset_kk(); +// } diff --git a/src/modules/fw_att_control/ecl_roll_controller.h b/src/modules/fw_att_control/ecl_roll_controller.h index f51d349abff3..9e7d21b711aa 100644 --- a/src/modules/fw_att_control/ecl_roll_controller.h +++ b/src/modules/fw_att_control/ecl_roll_controller.h @@ -49,18 +49,108 @@ #ifndef ECL_ROLL_CONTROLLER_H #define ECL_ROLL_CONTROLLER_H +// #define RCAC_ROLL_L_THETA 2 // P-I Control RCAC +// #define RCAC_ROLL_L_RBLOCK 2 + #include "ecl_controller.h" +#include class ECL_RollController : public ECL_Controller { public: - ECL_RollController() = default; + ECL_RollController(); // = default; ~ECL_RollController() = default; float control_attitude(const float dt, const ECL_ControlData &ctl_data) override; float control_euler_rate(const float dt, const ECL_ControlData &ctl_data) override; float control_bodyrate(const float dt, const ECL_ControlData &ctl_data) override; + void reset_integrator() override; + void set_integrator_max(float max) override; + + /* PX4 Gains For Logging */ + // matrix::Vector get_PX4_theta(); + + /* Attitude Control RCAC Declarations */ + + RCAC_Public_IO _rcac_att_public_io; + RCACParams_IO _rcac_att_params_io; + + /* Initialize RCAC Variables */ + void init_rcac_att(); + + // void reset_RCAC_kk(); + + /* RCAC Getter Functions - For Publishing */ + // int get_RCAC_roll_ii() {return _rcac_roll.getkk();} + // float get_RCAC_roll_Ru() {return rcac_roll_Ru;} + // float get_RCAC_roll_Rz() {return rcac_roll_Rz;} + // float get_RCAC_roll_P0() {return rcac_roll_P0;} + // float get_RCAC_roll_P11() {return _rcac_roll.get_rcac_P(0, 0);} + // float get_RCAC_roll_alpha() {return alpha_PID_roll;} + // float get_RCAC_roll_uk() {return _rcac_roll.get_rcac_uk();} + // bool get_RCAC_roll_switch() {return RCAC_roll_SW;} + // matrix::Vector get_RCAC_theta(); + + /* RCAC Setter Functions - For Alteration in QGC */ + // void set_RCAC_roll_Ru(float roll_Ru_in) + // { + // // if (roll_Ru_in != rcac_roll_Ru) + // PX4_INFO("[Roll RCAC Param Update] Ru: %6.4f", (double)roll_Ru_in); + // rcac_roll_Ru = roll_Ru_in; + // } + + // void set_RCAC_roll_Rz(float roll_Rz_in) + // { + // // if (rcac_roll_Rz != roll_Rz_in) + // PX4_INFO("[Roll RCAC] Rz Update: %6.4f", (double)roll_Rz_in); + // rcac_roll_Rz = roll_Rz_in; + // } + + // void set_RCAC_roll_P0(float roll_P0_in) + // { + // // if (rcac_roll_P0 != roll_P0_in) + // PX4_INFO("[Roll RCAC Param Update] P0: %6.4f", (double)roll_P0_in); + // rcac_roll_P0 = roll_P0_in; + // } + + // void set_RCAC_roll_alpha(float roll_alpha_in) + // { + // // if (alpha_PID_roll != roll_alpha_in) + // PX4_INFO("[Roll RCAC Param Update] Alpha: %6.4f", (double)roll_alpha_in); + // alpha_PID_roll = roll_alpha_in; + // } + + // void set_RCAC_roll_SW(bool roll_SW_in) + // { + // // if (RCAC_roll_SW != roll_SW_in) + // PX4_INFO("[Roll RCAC Param Update] RCAC Switch: %s", roll_SW_in ? "true" : "false"); + // RCAC_roll_SW = roll_SW_in; + // } + +private: + + // Jun 24, 2021: New RCAC Variables + // matrix::Matrix rcac_roll_Rblock; + + // RCAC _rcac_roll; + // float z_k_roll, z_km1_roll, u_k_roll, u_km1_roll; + + // bool RCAC_roll_SW = false; + // bool RCAC_roll_Rblock_SW = true; + + // int rcac_roll_e_fun = 0; + // float rcac_roll_P0 = 0.0011f; + // float rcac_roll_Rz = 1.0; + // float rcac_roll_Ru = 1.0; + // float rcac_roll_lambda = 1.0; + // float rcac_roll_N = -1.0; + // float alpha_PID_roll = 1.0f; + + RCAC _rcac_att; + RCACParams _rcac_att_params; + + float z_k_att, z_km1_att, u_k_att, u_km1_att; }; #endif // ECL_ROLL_CONTROLLER_H diff --git a/src/modules/fw_att_control/ecl_yaw_controller.cpp b/src/modules/fw_att_control/ecl_yaw_controller.cpp index f848866abd83..3e4d6185b8e0 100644 --- a/src/modules/fw_att_control/ecl_yaw_controller.cpp +++ b/src/modules/fw_att_control/ecl_yaw_controller.cpp @@ -112,10 +112,22 @@ float ECL_YawController::control_bodyrate(const float dt, const ECL_ControlData /* Calculate body angular rate error */ _rate_error = _bodyrate_setpoint - ctl_data.body_z_rate; + // Cyinyong: deadzone for rate error + _z_k_rate_dz = _ContDiffDeadzone.apply(_rate_error); + + /* Initialize RCAC on first iteration and roll switched on. */ + if ( (_rcac_rate.get_RCAC_EN() && _rcac_rate.getkk() == 0) || + (!_rcac_rate.get_RCAC_EN() && _rcac_rate.getkk() != 0) ) + { + init_rcac_rate(); + } + if (!ctl_data.lock_integrator && _k_i > 0.0f) { /* Integral term scales with 1/IAS^2 */ float id = _rate_error * dt * ctl_data.scaler * ctl_data.scaler; + // Cyinyong: Created a dz integral variable for rcac + float id_rcac = _z_k_rate_dz * ctl_data.scaler * ctl_data.scaler; /* * anti-windup: do not allow integrator to increase if actuator is at limit @@ -131,6 +143,11 @@ float ECL_YawController::control_bodyrate(const float dt, const ECL_ControlData /* add and constrain */ _integrator = math::constrain(_integrator + id * _k_i, -_integrator_max, _integrator_max); + + /* RCAC lib integrator (No gain with integral) */ + if (_rcac_rate_params.tuneParams.RCAC_EN) + // Cyinyong: edit for id to include dz and remove *dt for rcac integration + _rcac_rate.update_integral(id_rcac, dt); } /* Apply PI rate controller and store non-limited output */ @@ -139,6 +156,14 @@ float ECL_YawController::control_bodyrate(const float dt, const ECL_ControlData _rate_error * _k_p * ctl_data.scaler * ctl_data.scaler + _integrator; + matrix::Matrix Phi_rate; + Phi_rate(0, 0) = _z_k_rate_dz; //_rate_error; + Phi_rate(0, 1) = _rcac_rate.get_rcac_integral(); + u_k_rate = _rcac_rate.compute_uk(_z_k_rate_dz, Phi_rate, _rcac_rate.get_rcac_uk()); + + _last_output = _last_output * _rcac_rate.get_alpha() + u_k_rate; + + return math::constrain(_last_output, -1.0f, 1.0f); } diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index 9646d63d9aab..45f4973698be 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -765,3 +765,323 @@ PARAM_DEFINE_FLOAT(FW_DTRIM_R_FLPS, 0.0f); * @increment 0.01 */ PARAM_DEFINE_FLOAT(FW_DTRIM_P_FLPS, 0.0f); + +/** + * Initial P0 Value for Roll RCAC + * + * This P0 value is used to adjust sensitivity of RCAC in Roll Axis. + * + * @group RCAC FW Params + * @min 0.0 + * @max 10.0 + * @decimal 6 + */ +PARAM_DEFINE_FLOAT(FW_ROL_RATE_P0, 0.0f); + +/** + * Initial N_nf Value for Roll RCAC + * + * This N_nf value is used to adjust sensitivity of RCAC in Roll Axis. + * + * @group RCAC FW Params + * @min 0.0 + * @max 1000.0 + * @decimal 6 + */ +PARAM_DEFINE_FLOAT(FW_ROL_RATE_NNF, 0.0f); + + +/** + * Ru value for Roll RCAC + * + * Ru is one of the parameters for adjusting RCAC performance in Roll Axis. + * + * @group RCAC FW Params + * @min -10.0 + * @max 10.0 + * @decimal 2 + * @increment 0.01 + */ +PARAM_DEFINE_FLOAT(FW_ROL_RATE_RU, 1.0f); + +/** + * Alpha value for scaling Roll PID output + * + * Can change this value to degrade controller performance in Roll Axis + * + * @group RCAC FW Params + * @min -10.0 + * @max 10.0 + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(FW_ROL_RATE_ALF, 1.0f); + +/** + * Switch for enabling/disabling RCAC in the FW Roll Axis + * + * @boolean + * @group RCAC FW Params + */ +PARAM_DEFINE_INT32(FW_ROL_RATE_SW, 0); + +/** + * Initial P0 Value for Roll RCAC + * + * This P0 value is used to adjust sensitivity of RCAC in Roll Axis. + * + * @group RCAC FW Params + * @min 0.0 + * @max 10.0 + * @decimal 6 + */ +PARAM_DEFINE_FLOAT(FW_ROL_ATT_P0, 0.0f); + +/** + * Initial N_nf Value for Roll RCAC + * + * This N_nf value is used to adjust sensitivity of RCAC in Roll Axis. + * + * @group RCAC FW Params + * @min 0.0 + * @max 1000.0 + * @decimal 6 + */ +PARAM_DEFINE_FLOAT(FW_ROL_ATT_NNF, 0.0f); + + +/** + * Ru value for Roll RCAC + * + * Ru is one of the parameters for adjusting RCAC performance in Roll Axis. + * + * @group RCAC FW Params + * @min -10.0 + * @max 10.0 + * @decimal 2 + * @increment 0.01 + */ +PARAM_DEFINE_FLOAT(FW_ROL_ATT_RU, 1.0f); + +/** + * Alpha value for scaling Roll PID output + * + * Can change this value to degrade controller performance in Roll Axis + * + * @group RCAC FW Params + * @min -10.0 + * @max 10.0 + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(FW_ROL_ATT_ALF, 1.0f); + +/** + * Switch for enabling/disabling RCAC in the FW Roll Axis + * + * @boolean + * @group RCAC FW Params + */ +PARAM_DEFINE_INT32(FW_ROL_ATT_SW, 0); + + + +/** + * Initial P0 Value for Pitch RCAC + * + * This P0 value is used to adjust sensitivity of RCAC in the Pitch Axis. + * + * @group RCAC FW Params + * @min 0.0 + * @max 10.0 + * @decimal 6 + */ +PARAM_DEFINE_FLOAT(FW_PTCH_RATE_P0, 0.0f); + +/** + * Initial N_nf Value for Pitch RCAC + * + * This N_nf value is used to adjust sensitivity of RCAC in Roll Axis. + * + * @group RCAC FW Params + * @min 0.0 + * @max 1000.0 + * @decimal 6 + */ +PARAM_DEFINE_FLOAT(FW_PTCH_RATE_NNF, 0.0f); + +/** + * Ru value for Pitch RCAC + * + * Ru is one of the parameters for adjusting RCAC performance in the Pitch Axis. + * + * @group RCAC FW Params + * @min -10.0 + * @max 10.0 + * @decimal 2 + * @increment 0.01 + */ +PARAM_DEFINE_FLOAT(FW_PTCH_RATE_RU, 1.0f); + +/** + * Alpha value for scaling Pitch PID output + * + * Can change this value to degrade controller performance in the Pitch Axis. + * + * @group RCAC FW Params + * @min -10.0 + * @max 10.0 + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(FW_PTCH_RATE_ALF, 1.0f); + +/** + * Switch for enabling/disabling RCAC in the FW Pitch Axis + * + * @boolean + * @group RCAC FW Params + */ +PARAM_DEFINE_INT32(FW_PTCH_RATE_SW, 0); + +/** + * Initial P0 Value for Pitch RCAC + * + * This P0 value is used to adjust sensitivity of RCAC in the Pitch Axis. + * + * @group RCAC FW Params + * @min 0.0 + * @max 10.0 + * @decimal 6 + */ +PARAM_DEFINE_FLOAT(FW_PTCH_ATT_P0, 0.0f); + +/** + * Initial N_nf Value for Pitch RCAC + * + * This N_nf value is used to adjust sensitivity of RCAC in Roll Axis. + * + * @group RCAC FW Params + * @min 0.0 + * @max 1000.0 + * @decimal 6 + */ +PARAM_DEFINE_FLOAT(FW_PTCH_ATT_NNF, 0.0f); + +/** + * Ru value for Pitch RCAC + * + * Ru is one of the parameters for adjusting RCAC performance in the Pitch Axis. + * + * @group RCAC FW Params + * @min -10.0 + * @max 10.0 + * @decimal 2 + * @increment 0.01 + */ +PARAM_DEFINE_FLOAT(FW_PTCH_ATT_RU, 1.0f); + +/** + * Alpha value for scaling Pitch PID output + * + * Can change this value to degrade controller performance in the Pitch Axis. + * + * @group RCAC FW Params + * @min -10.0 + * @max 10.0 + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(FW_PTCH_ATT_ALF, 1.0f); + +/** + * Switch for enabling/disabling RCAC in the FW Pitch Axis + * + * @boolean + * @group RCAC FW Params + */ +PARAM_DEFINE_INT32(FW_PTCH_ATT_SW, 0); + +/** + * Initial P0 Value for Pitch RCAC + * + * This P0 value is used to adjust sensitivity of RCAC in the Yaw Axis. + * + * @group RCAC FW Params + * @min 0.0 + * @max 10.0 + * @decimal 6 + */ +PARAM_DEFINE_FLOAT(FW_YAW_RATE_P0, 0.0f); + +/** + * Initial N_nf Value for Pitch RCAC + * + * This N_nf value is used to adjust sensitivity of RCAC in Yaw Axis. + * + * @group RCAC FW Params + * @min 0.0 + * @max 1000.0 + * @decimal 6 + */ +PARAM_DEFINE_FLOAT(FW_YAW_RATE_NNF, 0.0f); + +/** + * Ru value for Pitch RCAC + * + * Ru is one of the parameters for adjusting RCAC performance in the Yaw Axis. + * + * @group RCAC FW Params + * @min -10.0 + * @max 10.0 + * @decimal 2 + * @increment 0.01 + */ +PARAM_DEFINE_FLOAT(FW_YAW_RATE_RU, 1.0f); + +/** + * Alpha value for scaling Pitch PID output + * + * Can change this value to degrade controller performance in the Yaw Axis. + * + * @group RCAC FW Params + * @min -10.0 + * @max 10.0 + * @decimal 2 + */ +PARAM_DEFINE_FLOAT(FW_YAW_RATE_ALF, 1.0f); + +/** + * Switch for enabling/disabling RCAC in the FW Yaw Axis + * + * @boolean + * @group RCAC FW Params + */ +PARAM_DEFINE_INT32(FW_YAW_RATE_SW, 0); + +/** + * Switch for toggling of ContDiffDz modes for RCAC in the FW Pitch Rate Axis + * + * Mode 0: no dz, Mode 1: Type 1 ContDiffDz, Mode 2: Type 2 ContDiffDz, Mode 3: Simple Dz + * + * @group RCAC FW Params + */ +PARAM_DEFINE_INT32(FW_RATE_DZ_MODE, 0); + +/** + * Deadzone bounds with zero action s1 + * + * @group RCAC FW Params + */ +PARAM_DEFINE_FLOAT(FW_RATE_DZ_S1, 0.0f); + +/** + * Deadzone bounds with differentiating effect s2 for type 2 + * + * @group RCAC FW Params + */ +PARAM_DEFINE_FLOAT(FW_RATE_DZ_S2, 0.0f); + +/** + * Slope of deadzone for type 1 + * + * @group RCAC FW Params + */ +PARAM_DEFINE_FLOAT(FW_RATE_DZ_A, 0.0f); + diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 38ccef0e7574..e6992384a3f6 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -111,6 +111,14 @@ 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); + add_topic("rcac_fw_roll", 100); + add_topic("rcac_fw_pitch", 100); + add_topic("rcac_fw_yaw", 100); + + // Control allocation topics add_topic("vehicle_angular_acceleration_setpoint", 20); diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index a8fbf7454f24..418de6afeb0a 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1600,6 +1600,12 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("POSITION_TARGET_LOCAL_NED", 1.5f); configure_stream_local("RAW_RPM", 2.0f); configure_stream_local("RC_CHANNELS", 5.0f); + configure_stream_local("RCAC_ATT_VARIABLES", 2.0f); + configure_stream_local("RCAC_RATE_VARIABLES", 2.0f); + configure_stream_local("RCAC_POS_VEL_VARIABLES", 2.0f); + configure_stream_local("RCAC_FW_ROLL", 2.0f); + configure_stream_local("RCAC_FW_PITCH", 2.0f); + configure_stream_local("RCAC_FW_YAW", 2.0f); configure_stream_local("SERVO_OUTPUT_RAW_0", 1.0f); configure_stream_local("SYS_STATUS", 1.0f); configure_stream_local("UTM_GLOBAL_POSITION", 0.5f); @@ -1655,6 +1661,12 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("POSITION_TARGET_LOCAL_NED", 10.0f); configure_stream_local("RAW_RPM", 5.0f); configure_stream_local("RC_CHANNELS", 20.0f); + configure_stream_local("RCAC_ATT_VARIABLES", 2.0f); + configure_stream_local("RCAC_RATE_VARIABLES", 2.0f); + configure_stream_local("RCAC_POS_VEL_VARIABLES", 2.0f); + configure_stream_local("RCAC_FW_ROLL", 2.0f); + configure_stream_local("RCAC_FW_PITCH", 2.0f); + configure_stream_local("RCAC_FW_YAW", 2.0f); configure_stream_local("SERVO_OUTPUT_RAW_0", 10.0f); configure_stream_local("SYS_STATUS", 5.0f); configure_stream_local("SYSTEM_TIME", 1.0f); @@ -1708,6 +1720,12 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("POSITION_TARGET_GLOBAL_INT", 1.5f); configure_stream_local("POSITION_TARGET_LOCAL_NED", 1.5f); configure_stream_local("RC_CHANNELS", 5.0f); + configure_stream_local("RCAC_ATT_VARIABLES", 2.0f); + configure_stream_local("RCAC_RATE_VARIABLES", 2.0f); + configure_stream_local("RCAC_POS_VEL_VARIABLES", 2.0f); + configure_stream_local("RCAC_FW_ROLL", 2.0f); + configure_stream_local("RCAC_FW_PITCH", 2.0f); + configure_stream_local("RCAC_FW_YAW", 2.0f); configure_stream_local("SERVO_OUTPUT_RAW_0", 1.0f); configure_stream_local("SYS_STATUS", 5.0f); configure_stream_local("TRAJECTORY_REPRESENTATION_WAYPOINTS", 5.0f); @@ -1737,6 +1755,12 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("GPS_RAW_INT", 1.0f); configure_stream_local("HOME_POSITION", 0.5f); configure_stream_local("RC_CHANNELS", 5.0f); + configure_stream_local("RCAC_ATT_VARIABLES", 2.0f); + configure_stream_local("RCAC_RATE_VARIABLES", 2.0f); + configure_stream_local("RCAC_POS_VEL_VARIABLES", 2.0f); + configure_stream_local("RCAC_FW_ROLL", 2.0f); + configure_stream_local("RCAC_FW_PITCH", 2.0f); + configure_stream_local("RCAC_FW_YAW", 2.0f); configure_stream_local("SERVO_OUTPUT_RAW_0", 1.0f); configure_stream_local("SYS_STATUS", 5.0f); configure_stream_local("SYSTEM_TIME", 1.0f); @@ -1788,6 +1812,12 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("POSITION_TARGET_GLOBAL_INT", 10.0f); configure_stream_local("RAW_RPM", 5.0f); configure_stream_local("RC_CHANNELS", 10.0f); + configure_stream_local("RCAC_ATT_VARIABLES", 2.0f); + configure_stream_local("RCAC_RATE_VARIABLES", 2.0f); + configure_stream_local("RCAC_POS_VEL_VARIABLES", 2.0f); + configure_stream_local("RCAC_FW_ROLL", 2.0f); + configure_stream_local("RCAC_FW_PITCH", 2.0f); + configure_stream_local("RCAC_FW_YAW", 2.0f); configure_stream_local("SCALED_IMU", 25.0f); configure_stream_local("SCALED_IMU2", 25.0f); configure_stream_local("SCALED_IMU3", 25.0f); @@ -1825,6 +1855,12 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("RC_CHANNELS", 0.5f); configure_stream_local("SYS_STATUS", 0.1f); configure_stream_local("VFR_HUD", 1.0f); + configure_stream_local("RCAC_ATT_VARIABLES", 2.0f); + configure_stream_local("RCAC_RATE_VARIABLES", 2.0f); + configure_stream_local("RCAC_POS_VEL_VARIABLES", 2.0f); + configure_stream_local("RCAC_FW_ROLL", 2.0f); + configure_stream_local("RCAC_FW_PITCH", 2.0f); + configure_stream_local("RCAC_FW_YAW", 2.0f); #if !defined(CONSTRAINED_FLASH) configure_stream_local("LINK_NODE_STATUS", 1.0f); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 170b83868fb0..ed0365462d43 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -52,6 +52,13 @@ #include #include #include +#include +#include +#include +#include +#include +#include +#include #include #include @@ -71,6 +78,12 @@ #include #include #include +#include +#include +#include +#include +#include +#include #include #include #include @@ -2224,8 +2237,10 @@ class MavlinkStreamOdometry : public MavlinkStream odom_updated = _vodom_sub.update(&odom); // set the frame_id according to the local frame of the data + //Juan: Take into account for MOCAP frame if (odom.local_frame == vehicle_odometry_s::LOCAL_FRAME_NED) { - msg.frame_id = MAV_FRAME_LOCAL_NED; + //msg.frame_id = MAV_FRAME_LOCAL_NED; + msg.frame_id = MAV_FRAME_LOCAL_FRD; } else { msg.frame_id = MAV_FRAME_LOCAL_FRD; @@ -2237,7 +2252,8 @@ class MavlinkStreamOdometry : public MavlinkStream } else { odom_updated = _odom_sub.update(&odom); - msg.frame_id = MAV_FRAME_LOCAL_NED; + //msg.frame_id = MAV_FRAME_LOCAL_NED; + msg.frame_id = MAV_FRAME_LOCAL_FRD; // source: PX4 estimator msg.estimator_type = MAV_ESTIMATOR_TYPE_AUTOPILOT; @@ -3005,6 +3021,437 @@ class MavlinkStreamCameraCapture : public MavlinkStream } }; +// Juan: Added classes for RCAC messages streaming +class MavlinkStreamRcacAttVariables : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamRcacAttVariables::get_name_static(); + } + static constexpr const char *get_name_static() + { + return "RCAC_ATT_VARIABLES"; + } + static constexpr uint16_t get_id_static() + { + return MAVLINK_MSG_ID_RCAC_ATT_VARIABLES; + } + uint16_t get_id() + { + return get_id_static(); + } + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamRcacAttVariables(mavlink); + } + unsigned get_size() + { + return MAVLINK_MSG_ID_RCAC_ATT_VARIABLES_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + + //MavlinkOrbSubscription *_sub; + //uint64_t _rcac_arv_time; + uORB::Subscription _rcac_att_sub{ORB_ID(rcac_att_variables)}; + + /* do not allow top copying this class */ + MavlinkStreamRcacAttVariables(MavlinkStreamRcacAttVariables &); + MavlinkStreamRcacAttVariables& operator = (const MavlinkStreamRcacAttVariables &); + +protected: + /*explicit MavlinkStreamRcacAttVariables(Mavlink *mavlink) : MavlinkStream(mavlink), + _sub(_mavlink->add_orb_subscription(ORB_ID(rcac_att_variables))), + _rcac_arv_time(0) + {}*/ + explicit MavlinkStreamRcacAttVariables(Mavlink *mavlink) : MavlinkStream(mavlink) + {} + + //bool send(const hrt_abstime t) + bool send() override + { + //struct rcac_att_variables_struct_s _rcac_att_variables; + struct rcac_att_variables_s _rcac_att_variables; + //rcac_att_variables_s _rcac_att_variables; + + if (_rcac_att_sub.update(&_rcac_att_variables)) { + mavlink_rcac_att_variables_t _msg_rcac_att_variables; + + _msg_rcac_att_variables.timestamp = _rcac_att_variables.timestamp; + _msg_rcac_att_variables.switch_att = _rcac_att_variables.switch_att; + _msg_rcac_att_variables.alpha_pid_att = _rcac_att_variables.alpha_pid_att; + _msg_rcac_att_variables.p11_att = _rcac_att_variables.p11_att; + _msg_rcac_att_variables.rcac_att_theta[0] = _rcac_att_variables.rcac_att_theta[0]; + _msg_rcac_att_variables.rcac_att_theta[1] = _rcac_att_variables.rcac_att_theta[1]; + _msg_rcac_att_variables.rcac_att_theta[2] = _rcac_att_variables.rcac_att_theta[2]; + + mavlink_msg_rcac_att_variables_send_struct(_mavlink->get_channel(), &_msg_rcac_att_variables); + + return true; + } + + return false; + } +}; +/******************************************/ +class MavlinkStreamRcacRateVariables : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamRcacRateVariables::get_name_static(); + } + static constexpr const char *get_name_static() + { + return "RCAC_RATE_VARIABLES"; + } + static constexpr uint16_t get_id_static() + { + return MAVLINK_MSG_ID_RCAC_RATE_VARIABLES; + } + uint16_t get_id() + { + return get_id_static(); + } + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamRcacRateVariables(mavlink); + } + unsigned get_size() + { + return MAVLINK_MSG_ID_RCAC_RATE_VARIABLES_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + + //MavlinkOrbSubscription *_sub; + //uint64_t _rcac_arv_time; + uORB::Subscription _rcac_rate_sub{ORB_ID(rcac_rate_variables)}; + + /* do not allow top copying this class */ + MavlinkStreamRcacRateVariables(MavlinkStreamRcacRateVariables &); + MavlinkStreamRcacRateVariables& operator = (const MavlinkStreamRcacRateVariables &); + +protected: + /*explicit MavlinkStreamRcacRateVariables(Mavlink *mavlink) : MavlinkStream(mavlink), + _sub(_mavlink->add_orb_subscription(ORB_ID(rcac_rate_variables))), + _rcac_arv_time(0) + {}*/ + explicit MavlinkStreamRcacRateVariables(Mavlink *mavlink) : MavlinkStream(mavlink) + {} + + //bool send(const hrt_abstime t) + bool send() override + { + //struct rcac_rate_variables_struct_s _rcac_rate_variables; + struct rcac_rate_variables_s _rcac_rate_variables; + //rcac_rate_variables_s _rcac_rate_variables; + + if (_rcac_rate_sub.update(&_rcac_rate_variables)) { + mavlink_rcac_rate_variables_t _msg_rcac_rate_variables; + + _msg_rcac_rate_variables.timestamp = _rcac_rate_variables.timestamp; + _msg_rcac_rate_variables.switch_rate = _rcac_rate_variables.switch_rate; + _msg_rcac_rate_variables.alpha_pid_rate = _rcac_rate_variables.alpha_pid_rate; + _msg_rcac_rate_variables.p11_ratex = _rcac_rate_variables.p11_ratex; + _msg_rcac_rate_variables.rcac_rate_theta[0] = _rcac_rate_variables.rcac_rate_theta[0]; + _msg_rcac_rate_variables.rcac_rate_theta[1] = _rcac_rate_variables.rcac_rate_theta[1]; + _msg_rcac_rate_variables.rcac_rate_theta[2] = _rcac_rate_variables.rcac_rate_theta[2]; + _msg_rcac_rate_variables.rcac_rate_theta[3] = _rcac_rate_variables.rcac_rate_theta[3]; + _msg_rcac_rate_variables.rcac_rate_theta[4] = _rcac_rate_variables.rcac_rate_theta[4]; + _msg_rcac_rate_variables.rcac_rate_theta[5] = _rcac_rate_variables.rcac_rate_theta[5]; + _msg_rcac_rate_variables.rcac_rate_theta[6] = _rcac_rate_variables.rcac_rate_theta[6]; + _msg_rcac_rate_variables.rcac_rate_theta[7] = _rcac_rate_variables.rcac_rate_theta[7]; + _msg_rcac_rate_variables.rcac_rate_theta[8] = _rcac_rate_variables.rcac_rate_theta[8]; + _msg_rcac_rate_variables.rcac_rate_theta[9] = _rcac_rate_variables.rcac_rate_theta[9]; + _msg_rcac_rate_variables.rcac_rate_theta[10] = _rcac_rate_variables.rcac_rate_theta[10]; + _msg_rcac_rate_variables.rcac_rate_theta[11] = _rcac_rate_variables.rcac_rate_theta[11]; + + mavlink_msg_rcac_rate_variables_send_struct(_mavlink->get_channel(), &_msg_rcac_rate_variables); + + return true; + } + + return false; + } +}; +/******************************************/ +class MavlinkStreamRcacPosVelVariables : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamRcacPosVelVariables::get_name_static(); + } + static constexpr const char *get_name_static() + { + return "RCAC_POS_VEL_VARIABLES"; + } + static constexpr uint16_t get_id_static() + { + return MAVLINK_MSG_ID_RCAC_POS_VEL_VARIABLES; + } + uint16_t get_id() + { + return get_id_static(); + } + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamRcacPosVelVariables(mavlink); + } + unsigned get_size() + { + return MAVLINK_MSG_ID_RCAC_POS_VEL_VARIABLES_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + /*MavlinkOrbSubscription *_sub; + uint64_t _rcac_pvv_time;*/ + uORB::Subscription _rcac_pos_vel_sub{ORB_ID(rcac_pos_vel_variables)}; + + /* do not allow top copying this class */ + MavlinkStreamRcacPosVelVariables(MavlinkStreamRcacPosVelVariables &); + MavlinkStreamRcacPosVelVariables& operator = (const MavlinkStreamRcacPosVelVariables &); + +protected: + /*explicit MavlinkStreamRcacPosVelVariables(Mavlink *mavlink) : MavlinkStream(mavlink), + _sub(_mavlink->add_orb_subscription(ORB_ID(rcac_pos_vel_variables))), + _rcac_pvv_time(0) + {}*/ + explicit MavlinkStreamRcacPosVelVariables(Mavlink *mavlink) : MavlinkStream(mavlink) + {} + + //bool send(const hrt_abstime t) + bool send() override + { + //struct rcac_pos_vel_variables_struct_s _rcac_pos_vel_variables; + struct rcac_pos_vel_variables_s _rcac_pos_vel_variables; + //rcac_pos_vel_variables_s _rcac_pos_vel_variables; + + /*if (_sub->update(&_rcac_pvv_time, &_rcac_pos_vel_variables)) {*/ + if (_rcac_pos_vel_sub.update(&_rcac_pos_vel_variables)) { + mavlink_rcac_pos_vel_variables_t _msg_rcac_pos_vel_variables; + + _msg_rcac_pos_vel_variables.timestamp = _rcac_pos_vel_variables.timestamp; + _msg_rcac_pos_vel_variables.pid_factor = _rcac_pos_vel_variables.pid_factor; + _msg_rcac_pos_vel_variables.rcac_master_sw = _rcac_pos_vel_variables.rcac_master_sw; + _msg_rcac_pos_vel_variables.switch_pos = _rcac_pos_vel_variables.switch_pos; + _msg_rcac_pos_vel_variables.switch_vel = _rcac_pos_vel_variables.switch_vel; + _msg_rcac_pos_vel_variables.alpha_pid_pos = _rcac_pos_vel_variables.alpha_pid_pos; + _msg_rcac_pos_vel_variables.alpha_pid_vel = _rcac_pos_vel_variables.alpha_pid_vel; + _msg_rcac_pos_vel_variables.p11_pos = _rcac_pos_vel_variables.p11_pos; + _msg_rcac_pos_vel_variables.p11_velx = _rcac_pos_vel_variables.p11_velx; + _msg_rcac_pos_vel_variables.rcac_pos_theta[0] = _rcac_pos_vel_variables.rcac_pos_theta[0]; + _msg_rcac_pos_vel_variables.rcac_pos_theta[1] = _rcac_pos_vel_variables.rcac_pos_theta[1]; + _msg_rcac_pos_vel_variables.rcac_pos_theta[2] = _rcac_pos_vel_variables.rcac_pos_theta[2]; + _msg_rcac_pos_vel_variables.rcac_vel_theta[0] = _rcac_pos_vel_variables.rcac_vel_theta[0]; + _msg_rcac_pos_vel_variables.rcac_vel_theta[1] = _rcac_pos_vel_variables.rcac_vel_theta[1]; + _msg_rcac_pos_vel_variables.rcac_vel_theta[2] = _rcac_pos_vel_variables.rcac_vel_theta[2]; + _msg_rcac_pos_vel_variables.rcac_vel_theta[3] = _rcac_pos_vel_variables.rcac_vel_theta[3]; + _msg_rcac_pos_vel_variables.rcac_vel_theta[4] = _rcac_pos_vel_variables.rcac_vel_theta[4]; + _msg_rcac_pos_vel_variables.rcac_vel_theta[5] = _rcac_pos_vel_variables.rcac_vel_theta[5]; + _msg_rcac_pos_vel_variables.rcac_vel_theta[6] = _rcac_pos_vel_variables.rcac_vel_theta[6]; + _msg_rcac_pos_vel_variables.rcac_vel_theta[7] = _rcac_pos_vel_variables.rcac_vel_theta[7]; + _msg_rcac_pos_vel_variables.rcac_vel_theta[8] = _rcac_pos_vel_variables.rcac_vel_theta[8]; + mavlink_msg_rcac_pos_vel_variables_send_struct(_mavlink->get_channel(), &_msg_rcac_pos_vel_variables); + return true; + } + + return false; + } +}; +/******************************************/ +class MavlinkStreamRcacFwRoll : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamRcacFwRoll::get_name_static(); + } + static constexpr const char *get_name_static() + { + return "RCAC_FW_ROLL"; + } + static constexpr uint16_t get_id_static() + { + return MAVLINK_MSG_ID_RCAC_FW_ROLL; + } + uint16_t get_id() + { + return get_id_static(); + } + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamRcacFwRoll(mavlink); + } + unsigned get_size() + { + return MAVLINK_MSG_ID_RCAC_FW_ROLL_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + uORB::Subscription _rcac_fw_roll_sub{ORB_ID(rcac_fw_roll)}; + + /* do not allow top copying this class */ + MavlinkStreamRcacFwRoll(MavlinkStreamRcacFwRoll &); + MavlinkStreamRcacFwRoll& operator = (const MavlinkStreamRcacFwRoll &); + +protected: + explicit MavlinkStreamRcacFwRoll(Mavlink *mavlink) : MavlinkStream(mavlink) + {} + + bool send() override + { + struct rcac_fw_roll_s _rcac_fw_roll; + + if (_rcac_fw_roll_sub.update(&_rcac_fw_roll)) { + mavlink_rcac_fw_roll_t _msg_rcac_fw_roll; + + _msg_rcac_fw_roll.timestamp = _rcac_fw_roll.timestamp; + _msg_rcac_fw_roll.switch_att = _rcac_fw_roll.switch_att; + _msg_rcac_fw_roll.switch_rate = _rcac_fw_roll.switch_rate; + _msg_rcac_fw_roll.alpha_pid_att = _rcac_fw_roll.alpha_pid_att; + _msg_rcac_fw_roll.alpha_pid_rate = _rcac_fw_roll.alpha_pid_rate; + _msg_rcac_fw_roll.p11_att = _rcac_fw_roll.p11_att; + _msg_rcac_fw_roll.p11_rate = _rcac_fw_roll.p11_rate; + _msg_rcac_fw_roll.rcac_theta_att = _rcac_fw_roll.rcac_theta_att; + _msg_rcac_fw_roll.rcac_theta_rate[0] = _rcac_fw_roll.rcac_theta_rate[0]; + _msg_rcac_fw_roll.rcac_theta_rate[1] = _rcac_fw_roll.rcac_theta_rate[1]; + mavlink_msg_rcac_fw_roll_send_struct(_mavlink->get_channel(), &_msg_rcac_fw_roll); + return true; + } + + return false; + } +}; +/******************************************/ +class MavlinkStreamRcacFwPitch : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamRcacFwPitch::get_name_static(); + } + static constexpr const char *get_name_static() + { + return "RCAC_FW_PITCH"; + } + static constexpr uint16_t get_id_static() + { + return MAVLINK_MSG_ID_RCAC_FW_PITCH; + } + uint16_t get_id() + { + return get_id_static(); + } + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamRcacFwPitch(mavlink); + } + unsigned get_size() + { + return MAVLINK_MSG_ID_RCAC_FW_PITCH_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + uORB::Subscription _rcac_fw_pitch_sub{ORB_ID(rcac_fw_pitch)}; + + /* do not allow top copying this class */ + MavlinkStreamRcacFwPitch(MavlinkStreamRcacFwPitch &); + MavlinkStreamRcacFwPitch& operator = (const MavlinkStreamRcacFwPitch &); + +protected: + explicit MavlinkStreamRcacFwPitch(Mavlink *mavlink) : MavlinkStream(mavlink) + {} + + bool send() override + { + struct rcac_fw_pitch_s _rcac_fw_pitch; + + if (_rcac_fw_pitch_sub.update(&_rcac_fw_pitch)) { + mavlink_rcac_fw_pitch_t _msg_rcac_fw_pitch; + + _msg_rcac_fw_pitch.timestamp = _rcac_fw_pitch.timestamp; + _msg_rcac_fw_pitch.switch_att = _rcac_fw_pitch.switch_att; + _msg_rcac_fw_pitch.switch_rate = _rcac_fw_pitch.switch_rate; + _msg_rcac_fw_pitch.alpha_pid_att = _rcac_fw_pitch.alpha_pid_att; + _msg_rcac_fw_pitch.alpha_pid_rate = _rcac_fw_pitch.alpha_pid_rate; + _msg_rcac_fw_pitch.p11_att = _rcac_fw_pitch.p11_att; + _msg_rcac_fw_pitch.p11_rate = _rcac_fw_pitch.p11_rate; + _msg_rcac_fw_pitch.rcac_theta_att = _rcac_fw_pitch.rcac_theta_att; + _msg_rcac_fw_pitch.rcac_theta_rate[0] = _rcac_fw_pitch.rcac_theta_rate[0]; + _msg_rcac_fw_pitch.rcac_theta_rate[1] = _rcac_fw_pitch.rcac_theta_rate[1]; + mavlink_msg_rcac_fw_pitch_send_struct(_mavlink->get_channel(), &_msg_rcac_fw_pitch); + return true; + } + + return false; + } +}; +/******************************************/ +/******************************************/ +class MavlinkStreamRcacFwYaw : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamRcacFwYaw::get_name_static(); + } + static constexpr const char *get_name_static() + { + return "RCAC_FW_YAW"; + } + static constexpr uint16_t get_id_static() + { + return MAVLINK_MSG_ID_RCAC_FW_YAW; + } + uint16_t get_id() + { + return get_id_static(); + } + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamRcacFwYaw(mavlink); + } + unsigned get_size() + { + return MAVLINK_MSG_ID_RCAC_FW_YAW_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + uORB::Subscription _rcac_fw_yaw_sub{ORB_ID(rcac_fw_yaw)}; + + /* do not allow top copying this class */ + MavlinkStreamRcacFwYaw(MavlinkStreamRcacFwYaw &); + MavlinkStreamRcacFwYaw& operator = (const MavlinkStreamRcacFwYaw &); + +protected: + explicit MavlinkStreamRcacFwYaw(Mavlink *mavlink) : MavlinkStream(mavlink) + {} + + bool send() override + { + struct rcac_fw_yaw_s _rcac_fw_yaw; + + if (_rcac_fw_yaw_sub.update(&_rcac_fw_yaw)) { + mavlink_rcac_fw_yaw_t _msg_rcac_fw_yaw; + + _msg_rcac_fw_yaw.timestamp = _rcac_fw_yaw.timestamp; + _msg_rcac_fw_yaw.switch_rate = _rcac_fw_yaw.switch_rate; + _msg_rcac_fw_yaw.alpha_pid_rate = _rcac_fw_yaw.alpha_pid_rate; + _msg_rcac_fw_yaw.p11_rate = _rcac_fw_yaw.p11_rate; + _msg_rcac_fw_yaw.rcac_theta_rate[0] = _rcac_fw_yaw.rcac_theta_rate[0]; + _msg_rcac_fw_yaw.rcac_theta_rate[1] = _rcac_fw_yaw.rcac_theta_rate[1]; + mavlink_msg_rcac_fw_yaw_send_struct(_mavlink->get_channel(), &_msg_rcac_fw_yaw); + return true; + } + + return false; + } +}; + + static const StreamListItem streams_list[] = { create_stream_list_item(), #if defined(STATUSTEXT_HPP) @@ -3093,6 +3540,12 @@ static const StreamListItem streams_list[] = { create_stream_list_item(), create_stream_list_item(), create_stream_list_item(), + create_stream_list_item(), + create_stream_list_item(), + create_stream_list_item(), + create_stream_list_item(), + create_stream_list_item(), + create_stream_list_item(), #if defined(DISTANCE_SENSOR_HPP) create_stream_list_item(), #endif // DISTANCE_SENSOR_HPP diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 56a63bac3b43..3182a601dc02 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1405,7 +1405,7 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg) } else { odometry.local_frame = vehicle_odometry_s::LOCAL_FRAME_FRD; } - + // Juan: Keep this in mind when trying MOCAP out. if ((odom.estimator_type == MAV_ESTIMATOR_TYPE_VISION) || (odom.estimator_type == MAV_ESTIMATOR_TYPE_VIO) || (odom.estimator_type == MAV_ESTIMATOR_TYPE_UNKNOWN)) { diff --git a/src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp b/src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp index de0ba91b05c0..bcb7e2555643 100644 --- a/src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp +++ b/src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp @@ -36,11 +36,19 @@ */ #include - #include +// #include using namespace matrix; +AttitudeControl::AttitudeControl() { + _rcacParams_att.initParams.RBlock_EN = true; + for (size_t i = 0; i < 3; ++i) { + _rcac_att_public_io(i) = RCAC_Public_IO(&_rcac_att(i)); + } + _rcac_att_params_io = RCACParams_IO(&_rcacParams_att); +} + void AttitudeControl::setProportionalGain(const matrix::Vector3f &proportional_gain, const float yaw_weight) { _proportional_gain = proportional_gain; @@ -52,7 +60,8 @@ void AttitudeControl::setProportionalGain(const matrix::Vector3f &proportional_g } } -matrix::Vector3f AttitudeControl::update(const Quatf &q) const +//matrix::Vector3f AttitudeControl::update(const Quatf &q) const +matrix::Vector3f AttitudeControl::update(const Quatf &q, const bool landed) { Quatf qd = _attitude_setpoint_q; @@ -85,18 +94,39 @@ matrix::Vector3f AttitudeControl::update(const Quatf &q) const // using sin(alpha/2) scaled rotation axis as attitude error (see quaternion definition by axis angle) // also taking care of the antipodal unit quaternion ambiguity + //const Vector3f eq = 2.f * qe.canonical().imag(); const Vector3f eq = 2.f * qe.canonical().imag(); // calculate angular rates setpoint matrix::Vector3f rate_setpoint = eq.emult(_proportional_gain); - // Feed forward the yaw setpoint rate. - // yawspeed_setpoint is the feed forward commanded rotation around the world z-axis, - // but we need to apply it in the body frame (because _rates_sp is expressed in the body frame). - // Therefore we infer the world z-axis (expressed in the body frame) by taking the last column of R.transposed (== q.inversed) - // and multiply it by the yaw setpoint rate (yawspeed_setpoint). - // This yields a vector representing the commanded rotatation around the world z-axis expressed in the body frame - // such that it can be added to the rates setpoint. + //// rate_setpoint = alpha_PID*rate_setpoint; + //// rate_setpoint = alpha_PID_att*rate_setpoint; + + //this->z_k_Pq_R.setZero(); + z_k_Pq_R = eq; + + + if ( (_rcac_att(0).get_RCAC_EN() && _rcac_att(0).getkk() == 0) || + (!_rcac_att(0).get_RCAC_EN() && _rcac_att(0).getkk() != 0) ) + { + init_RCAC_att(); + } + + // Landed Check Removed after Landed check is now moved to mc_att_control_main. + for (size_t i = 0; i <= 2; ++i) + { + matrix::Matrix Phi_att; + Phi_att(0, 0) = z_k_Pq_R(i); + u_k_Pq_R(i) = _rcac_att(i).compute_uk(z_k_Pq_R(i), Phi_att, u_km1_Pq_R(i)); + } + + u_km1_Pq_R = u_k_Pq_R; + + + + rate_setpoint = _rcacParams_att.tuneParams.alpha_PID * rate_setpoint + u_k_Pq_R; + rate_setpoint += q.inversed().dcm_z() * _yawspeed_setpoint; // limit rates @@ -104,5 +134,8 @@ matrix::Vector3f AttitudeControl::update(const Quatf &q) const rate_setpoint(i) = math::constrain(rate_setpoint(i), -_rate_limit(i), _rate_limit(i)); } + u_km1_Pq_R = u_k_Pq_R; + return rate_setpoint; + } diff --git a/src/modules/mc_att_control/AttitudeControl/AttitudeControl.hpp b/src/modules/mc_att_control/AttitudeControl/AttitudeControl.hpp index 70980e2b3531..ba1792bf176a 100644 --- a/src/modules/mc_att_control/AttitudeControl/AttitudeControl.hpp +++ b/src/modules/mc_att_control/AttitudeControl/AttitudeControl.hpp @@ -50,11 +50,16 @@ #include #include +#include +#include + +#define RCAC_ATT_L_THETA 1 +#define RCAC_ATT_L_RBLOCK 2 class AttitudeControl { public: - AttitudeControl() = default; + AttitudeControl(); ~AttitudeControl() = default; /** @@ -89,7 +94,33 @@ class AttitudeControl * @param q estimation of the current vehicle attitude unit quaternion * @return [rad/s] body frame 3D angular rate setpoint vector to be executed by the rate controller */ - matrix::Vector3f update(const matrix::Quatf &q) const; + //matrix::Vector3f update(const matrix::Quatf &q) const; + matrix::Vector3f update(const matrix::Quatf &q, const bool landed); + + /** + * Get the + * @see _proportional_gain + * @return PX4 PID gains for the attitude controller + */ + float get_PX4_att_theta(size_t idx) + { + return _proportional_gain(idx); + } + + /** + * Set RCAC variables. + * @see all RCAC variables + */ + void init_RCAC_att() + { + for (size_t i = 0; i < 3; ++i) { + _rcac_att(i) = RCAC(_rcacParams_att, _rcac_att(i).get_MASTER_EN()); + } + } + + /* RCAC Pos/Vel Public Classes with Getter/Setters */ + matrix::Vector, 3> _rcac_att_public_io; + RCACParams_IO _rcac_att_params_io; private: matrix::Vector3f _proportional_gain; @@ -98,4 +129,11 @@ class AttitudeControl matrix::Quatf _attitude_setpoint_q; ///< latest known attitude setpoint e.g. from position control float _yawspeed_setpoint{0.f}; ///< latest known yawspeed feed-forward setpoint + + matrix::Vector3f z_k_Pq_R, z_km1_Pq_R, u_k_Pq_R, u_km1_Pq_R; + + // New RCAC_Class_Variables + matrix::Vector, 3> _rcac_att; + RCACParams _rcacParams_att; + }; diff --git a/src/modules/mc_att_control/mc_att_control.hpp b/src/modules/mc_att_control/mc_att_control.hpp index 41b4e3a95eea..73c414a2ea2b 100644 --- a/src/modules/mc_att_control/mc_att_control.hpp +++ b/src/modules/mc_att_control/mc_att_control.hpp @@ -53,9 +53,11 @@ #include #include #include +#include #include #include +#include #include using namespace time_literals; @@ -91,6 +93,8 @@ class MulticopterAttitudeControl : public ModuleBase */ void parameters_updated(); + void publish_rcac_att_variables(); + float throttle_curve(float throttle_stick_input); /** @@ -108,14 +112,21 @@ class MulticopterAttitudeControl : public ModuleBase uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; /**< manual control setpoint subscription */ uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */ uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */ +uORB::Subscription _rc_channels_sub{ORB_ID(rc_channels)}; /**< RC switch>*/ uORB::SubscriptionCallbackWorkItem _vehicle_attitude_sub{this, ORB_ID(vehicle_attitude)}; uORB::Publication _v_rates_sp_pub{ORB_ID(vehicle_rates_setpoint)}; /**< rate setpoint publication */ uORB::Publication _vehicle_attitude_setpoint_pub; + uORB::Publication _rcac_att_variables_pub{ORB_ID(rcac_att_variables)}; /**< RCAC attitude variables log */ + + + struct manual_control_setpoint_s _manual_control_setpoint {}; /**< manual control setpoint */ struct vehicle_control_mode_s _v_control_mode {}; /**< vehicle control mode */ + struct rc_channels_s _rc_channels_switch{}; /**< Switch from the RC channel */ + rcac_att_variables_s _rcac_att_variables{}; /**< RCAC variables */ perf_counter_t _loop_perf; /**< loop duration performance counter */ @@ -129,6 +140,7 @@ class MulticopterAttitudeControl : public ModuleBase hrt_abstime _last_run{0}; bool _landed{true}; + bool _maybe_landed{true}; bool _reset_yaw_sp{true}; bool _vehicle_type_rotary_wing{true}; bool _vtol{false}; @@ -160,7 +172,14 @@ class MulticopterAttitudeControl : public ModuleBase (ParamInt) _param_mpc_thr_curve, /**< throttle curve behavior */ (ParamInt) _param_mc_airmode, - (ParamFloat) _param_mc_man_tilt_tau - ) + (ParamFloat) _param_mc_man_tilt_tau, + (ParamFloat) _param_mpc_rcac_att_sw, + (ParamFloat) _param_mpc_att_alpha, + (ParamFloat) _param_mpc_rcac_att_P0, + (ParamFloat) _param_mpc_rcac_att_Ru, + (ParamFloat) _param_mpc_rcac_att_N + + + ); }; diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index c1a732704731..c0831ddb498d 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -98,6 +98,15 @@ MulticopterAttitudeControl::parameters_updated() radians(_param_mc_yawrate_max.get()))); _man_tilt_max = math::radians(_param_mpc_man_tilt_max.get()); + + //Set P0 for RCAC attitude and Rate controller + _attitude_control._rcac_att_params_io.set_P0(_param_mpc_rcac_att_P0.get()); + _attitude_control._rcac_att_params_io.set_Ru(_param_mpc_rcac_att_Ru.get()); + _attitude_control._rcac_att_params_io.set_RCAC_EN(_param_mpc_rcac_att_sw.get() > 0.0f); + _attitude_control._rcac_att_params_io.set_N_nf(_param_mpc_rcac_att_N.get()); + _attitude_control.init_RCAC_att(); + PX4_INFO("Att Control P0:\t%8.6f", (double)_param_mpc_rcac_att_P0.get()); + } float @@ -216,6 +225,30 @@ MulticopterAttitudeControl::generate_attitude_setpoint(const Quatf &q, float dt, _vehicle_attitude_setpoint_pub.publish(attitude_setpoint); } +void +MulticopterAttitudeControl::publish_rcac_att_variables() +{ + _rcac_att_variables.timestamp = hrt_absolute_time(); + _rcac_att_variables.ii_att = _attitude_control._rcac_att_public_io(0).get_kk(); + _rcac_att_variables.switch_att = _attitude_control._rcac_att_params_io.get_switch(); // _rcac_att_public_io(0).get_RCAC_EN(); + + _rcac_att_variables.alpha_pid_att = _attitude_control._rcac_att_params_io.get_alpha(); + + _rcac_att_variables.p11_att = _attitude_control._rcac_att_public_io(0).get_P11(); + + _rcac_att_variables.rcac_att_ru = _attitude_control._rcac_att_params_io.get_Ru(); + _rcac_att_variables.rcac_att_n = _attitude_control._rcac_att_params_io.get_N_nf(); + + for (int i = 0; i <= 2; i++) { + _rcac_att_variables.rcac_att_z[i] = _attitude_control._rcac_att_public_io(i).get_zk(); + _rcac_att_variables.rcac_att_u[i] = _attitude_control._rcac_att_public_io(i).get_uk(); + _rcac_att_variables.rcac_att_theta[i] = _attitude_control._rcac_att_public_io(i).get_theta(0); + + _rcac_att_variables.px4_att_theta[i] = _attitude_control.get_PX4_att_theta(i); + } + _rcac_att_variables_pub.publish(_rcac_att_variables); +} + void MulticopterAttitudeControl::Run() { @@ -260,6 +293,33 @@ MulticopterAttitudeControl::Run() _quat_reset_counter = v_att.quat_reset_counter; } + //RCtopic + // Ankit: RCAC switches and the PID scaling factor controlled by the RC transmitter + _rc_channels_sub.update(&_rc_channels_switch); + + #ifdef __PX4_NUTTX + float PID_scale_f = _rc_channels_switch.channels[13]; + float RCAC_switch = _rc_channels_switch.channels[14]; + #else + float PID_scale_f = -1.0f; + float RCAC_switch = 1.0f; + #endif /* __PX4_NUTTX */ + + // Make Float RC inputs to bools + // Joon: Changed these from if statements to boolean logic + bool RCAC_HW_EN = (RCAC_switch > 0.0f); + bool ALPHA_HW_EN = (PID_scale_f < 0.0f); + + // bool RCAC_SW_fromParams = _param_mpc_rcac_att_sw.get() > 0.0f; + + bool RCAC_ATT_MASTER_EN = RCAC_HW_EN && !_landed; // && RCAC_SW_fromParams + + for (size_t i = 0; i < 3; ++i) { + _attitude_control._rcac_att_public_io(i).set_MASTER_EN(RCAC_ATT_MASTER_EN); + } + + _attitude_control._rcac_att_params_io.set_alpha(ALPHA_HW_EN ? _param_mpc_att_alpha.get() : 1.0f); + // Guard against too small (< 0.2ms) and too large (> 20ms) dt's. const float dt = math::constrain(((v_att.timestamp - _last_run) * 1e-6f), 0.0002f, 0.02f); _last_run = v_att.timestamp; @@ -268,11 +328,14 @@ MulticopterAttitudeControl::Run() _manual_control_setpoint_sub.update(&_manual_control_setpoint); _v_control_mode_sub.update(&_v_control_mode); + if (_vehicle_land_detected_sub.updated()) { vehicle_land_detected_s vehicle_land_detected; if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)) { _landed = vehicle_land_detected.landed; + _maybe_landed = vehicle_land_detected.maybe_landed; + // _attitude_control. } } @@ -322,7 +385,9 @@ MulticopterAttitudeControl::Run() _man_y_input_filter.reset(0.f); } - Vector3f rates_sp = _attitude_control.update(q); + //Juan: Made changes to controller inputs + const bool landed = _maybe_landed || _landed; + Vector3f rates_sp = _attitude_control.update(q, landed); if (_v_control_mode.flag_control_yawrate_override_enabled) { /* Yaw rate override enabled, overwrite the yaw setpoint */ @@ -343,6 +408,7 @@ MulticopterAttitudeControl::Run() v_rates_sp.timestamp = hrt_absolute_time(); _v_rates_sp_pub.publish(v_rates_sp); + publish_rcac_att_variables(); } // reset yaw setpoint during transitions, tailsitter.cpp generates @@ -355,6 +421,7 @@ MulticopterAttitudeControl::Run() perf_end(_loop_perf); } + int MulticopterAttitudeControl::task_spawn(int argc, char *argv[]) { bool vtol = false; diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c index 6ac8fd1479b4..834e74b7447a 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -175,3 +175,55 @@ PARAM_DEFINE_FLOAT(MC_RATT_TH, 0.8f); * @group Multicopter Position Control */ PARAM_DEFINE_FLOAT(MC_MAN_TILT_TAU, 0.0f); + +/** + * RCAC Attitude switches. + * + * Specifies RCAC state. + * + * @min -10 + * @max 10 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MPC_RCAC_ATT_SW, -1.0f); + +/** + * PID Attitude gain. + * + * Specifies gain applied to the PID attitude gains. + * + * @min -10 + * @max 10 + * @group Multicopter RCAC Control + */ +PARAM_DEFINE_FLOAT(MPC_ATT_ALPHA, 1.0f); + +/** + * P0 for the Attitude controller + * + * @min 0.0 + * @max 10.0 + * @decimal 8 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MPC_RCAC_ATT_P0, 0.1f); + +/** + * Ru for the Attitude controller + * + * @min 0.0 + * @max 10.0 + * @decimal 8 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MPC_RCAC_ATT_RU, 1.0f); + +/** + * N Filter Coef for the Attitude controller + * + * @min 0.0 + * @max 10.0 + * @decimal 8 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MPC_RCAC_ATT_N, 1.0f); \ No newline at end of file diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.cpp b/src/modules/mc_pos_control/MulticopterPositionControl.cpp index d2c4f6b47ee8..08664b2146fc 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.cpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.cpp @@ -70,7 +70,6 @@ bool MulticopterPositionControl::init() // limit to every other vehicle_local_position update (50 Hz) _local_pos_sub.set_interval_us(20_ms); - _time_stamp_last_loop = hrt_absolute_time(); return true; @@ -138,6 +137,32 @@ int MulticopterPositionControl::parameters_update(bool force) // initialize vectors from params and enforce constraints _param_mpc_tko_speed.set(math::min(_param_mpc_tko_speed.get(), _param_mpc_z_vel_max_up.get())); _param_mpc_land_speed.set(math::min(_param_mpc_land_speed.get(), _param_mpc_z_vel_max_dn.get())); + + if (_vehicle_land_detected_sub.updated()) { + vehicle_land_detected_s vehicle_land_detected; + + if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)) { + _landed = vehicle_land_detected.landed; + _maybe_landed = vehicle_land_detected.maybe_landed; + } + } + const bool landed = _maybe_landed || _landed; + + // Only allow Parameter Updates on ground. + if (landed) + { + _control._rcac_pos_params_io.set_P0(_param_mpc_rcac_pos_P0.get()); + _control._rcac_vel_params_io.set_P0(_param_mpc_rcac_vel_P0.get()); + _control._rcac_pos_params_io.set_Ru(_param_mpc_rcac_pos_Ru.get()); + _control._rcac_vel_params_io.set_Ru(_param_mpc_rcac_vel_Ru.get()); + _control._rcac_pos_params_io.set_N_nf(_param_mpc_rcac_pos_N.get()); + _control._rcac_vel_params_io.set_N_nf(_param_mpc_rcac_vel_N.get()); + _control._rcac_pos_params_io.set_RCAC_EN(_param_mpc_rcac_pos_sw.get() > 0.0f); + _control._rcac_vel_params_io.set_RCAC_EN(_param_mpc_rcac_vel_sw.get() > 0.0f); + + _control.init_RCAC_pos(); + _control.init_RCAC_vel(); + } } return OK; @@ -156,6 +181,7 @@ void MulticopterPositionControl::poll_subscriptions() } } } + _rc_channels_sub.update(&_rc_channels_switch); } void MulticopterPositionControl::set_vehicle_states(const float &vel_sp_z) @@ -213,8 +239,63 @@ void MulticopterPositionControl::set_vehicle_states(const float &vel_sp_z) } } +void MulticopterPositionControl::publish_rcac_pos_vel_variables(float pid_scale, float rcac_switch) +{ + _rcac_pos_vel_variables_sub.update(&_rcac_pos_vel_variables); + _rcac_pos_vel_variables.timestamp = hrt_absolute_time(); + // _rcac_pos_vel_variables.rcac_alpha[0] = _rc_channels_switch.channels[13]; + // _rcac_pos_vel_variables.rcac_alpha[1] = _rc_channels_switch.channels[14]; + // _rcac_pos_vel_variables.rcac_alpha[0] = PID_scale_f; + // _rcac_pos_vel_variables.rcac_alpha[1] = RCAC_switch; + // TODO: make pid_scale and rcac_switch into bools + _rcac_pos_vel_variables.pid_factor = pid_scale; + _rcac_pos_vel_variables.rcac_master_sw = rcac_switch; + _rcac_pos_vel_variables.ii_pos = _control._rcac_pos_public_io(0).get_kk(); + _rcac_pos_vel_variables.ii_vel = _control._rcac_vel_public_io(0).get_kk(); + _rcac_pos_vel_variables.switch_pos = _control._rcac_pos_params_io.get_switch(); + _rcac_pos_vel_variables.switch_vel = _control._rcac_vel_params_io.get_switch(); + _rcac_pos_vel_variables.alpha_pid_pos = _control._rcac_pos_params_io.get_alpha(); + _rcac_pos_vel_variables.alpha_pid_vel = _control._rcac_vel_params_io.get_alpha(); + _rcac_pos_vel_variables.p11_pos = _control._rcac_pos_public_io(0).get_P11(); + _rcac_pos_vel_variables.p11_velx =_control._rcac_vel_public_io(0).get_P11(); + _rcac_pos_vel_variables.rcac_pos_ru = _control._rcac_pos_params_io.get_Ru(); + _rcac_pos_vel_variables.rcac_vel_ru = _control._rcac_vel_params_io.get_Ru(); + _rcac_pos_vel_variables.rcac_pos_n = _control._rcac_pos_params_io.get_N_nf(); + _rcac_pos_vel_variables.rcac_vel_n = _control._rcac_vel_params_io.get_N_nf(); + + for (int i = 0; i <= 2; i++) { + _rcac_pos_vel_variables.rcac_pos_z[i] = _control._rcac_pos_public_io(i).get_zk(); + _rcac_pos_vel_variables.rcac_pos_u[i] = _control._rcac_pos_public_io(i).get_uk(); + _rcac_pos_vel_variables.rcac_pos_theta[i] = _control._rcac_pos_public_io(i).get_theta(i); + + _rcac_pos_vel_variables.rcac_vel_z[i] = _control._rcac_vel_public_io(i).get_zk(); + _rcac_pos_vel_variables.rcac_vel_u[i] = _control._rcac_vel_public_io(i).get_uk(); + } + for (int i = 0; i < 3; ++i) { + + for (int j = 0; j < RCAC_POS_L_THETA; ++j) { + size_t velThetaIdx = 3 * i + j; + _rcac_pos_vel_variables.rcac_vel_theta[velThetaIdx] = _control._rcac_vel_public_io(i).get_theta(j); + } + // TODO: Make this 6 Terms instead of 9 + // Theta only contains P/I Terms, hence adding 0 to every third term to fit 9 terms. + _rcac_pos_vel_variables.rcac_vel_theta[3 * i + 2] = 0; + + } + + for (int i = 0; i <= 8; i++) { + + _rcac_pos_vel_variables.px4_ol_theta[i] = _control.get_PX4_ol_theta()(i); + + } + _rcac_pos_vel_variables_pub.publish(_rcac_pos_vel_variables); + +} + void MulticopterPositionControl::Run() { + + if (should_exit()) { _local_pos_sub.unregisterCallback(); exit_and_cleanup(); @@ -226,6 +307,33 @@ void MulticopterPositionControl::Run() if (_local_pos_sub.update(&_local_pos)) { poll_subscriptions(); + + #ifdef __PX4_NUTTX + float PID_scale_f = _rc_channels_switch.channels[13]; + float RCAC_switch = _rc_channels_switch.channels[14]; + #else + float PID_scale_f = -1.0f; + float RCAC_switch = 1.0f; + #endif /* __PX4_NUTTX */ + + bool RCAC_HW_EN = (RCAC_switch > 0.0f); + bool ALPHA_HW_EN = (PID_scale_f < 0.0f); + + // bool RCAC_VEL_SW_fromParams = _param_mpc_rcac_vel_sw.get() > 0.0f; + // bool RCAC_POS_SW_fromParams = _param_mpc_rcac_pos_sw.get() > 0.0f; + + bool RCAC_POS_MASTER_EN = RCAC_HW_EN && !_landed; // && RCAC_POS_SW_fromParams + bool RCAC_VEL_MASTER_EN = RCAC_HW_EN && !_landed; // && RCAC_VEL_SW_fromParams + + // Refresh Parameters to update RCAC_EN if it has changed. + for (size_t i = 0; i < 3; ++i) { + _control._rcac_pos_public_io(i).set_MASTER_EN(RCAC_POS_MASTER_EN); + _control._rcac_vel_public_io(i).set_MASTER_EN(RCAC_VEL_MASTER_EN); + } + + _control._rcac_pos_params_io.set_alpha(ALPHA_HW_EN ? _param_mpc_pos_alpha.get() : 1.0f); + _control._rcac_vel_params_io.set_alpha(ALPHA_HW_EN ? _param_mpc_vel_alpha.get() : 1.0f); + parameters_update(false); const hrt_abstime time_stamp_now = _local_pos.timestamp; @@ -240,6 +348,16 @@ void MulticopterPositionControl::Run() vehicle_local_position_setpoint_s setpoint; + if (_vehicle_land_detected_sub.updated()) { + vehicle_land_detected_s vehicle_land_detected; + + if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)) { + _landed = vehicle_land_detected.landed; + _maybe_landed = vehicle_land_detected.maybe_landed; + } + } + const bool landed = _maybe_landed || _landed; + // check if any task is active if (_trajectory_setpoint_sub.update(&setpoint)) { _control.setInputSetpoint(setpoint); @@ -256,11 +374,12 @@ void MulticopterPositionControl::Run() if (constraints.reset_integral) { _control.resetIntegral(); + //_control.init_RCAC_pos_vel(); } } // Run position control - if (_control.update(dt)) { + if (_control.update(dt, landed)) { _failsafe_land_hysteresis.set_state_and_update(false, time_stamp_now); } else { @@ -277,7 +396,7 @@ void MulticopterPositionControl::Run() constraints = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, false, {}}; _control.setConstraints(constraints); - _control.update(dt); + _control.update(dt, landed); } // Publish internal position control setpoints @@ -298,12 +417,16 @@ void MulticopterPositionControl::Run() _control.getAttitudeSetpoint(attitude_setpoint); _vehicle_attitude_setpoint_pub.publish(attitude_setpoint); + publish_rcac_pos_vel_variables(PID_scale_f, RCAC_switch); + } else { // reset the numerical derivatives to not generate d term spikes when coming from non-position controlled operation _vel_x_deriv.reset(); _vel_y_deriv.reset(); _vel_z_deriv.reset(); + publish_rcac_pos_vel_variables(PID_scale_f, RCAC_switch); } + publish_rcac_pos_vel_variables(PID_scale_f, RCAC_switch); } perf_end(_cycle_perf); diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.hpp b/src/modules/mc_pos_control/MulticopterPositionControl.hpp index 4a3ebba52178..46396952019d 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.hpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.hpp @@ -60,6 +60,10 @@ #include #include #include +#include +#include + +#include #include "PositionControl/PositionControl.hpp" @@ -91,21 +95,31 @@ class MulticopterPositionControl : public ModuleBase uORB::Publication _local_pos_sp_pub{ORB_ID(vehicle_local_position_setpoint)}; /**< vehicle local position setpoint publication */ + uORB::Publication _rcac_pos_vel_variables_pub{ORB_ID(rcac_pos_vel_variables)}; /**< RCAC variables log */ + + uORB::SubscriptionCallbackWorkItem _local_pos_sub{this, ORB_ID(vehicle_local_position)}; /**< vehicle local position */ uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + uORB::Subscription _rcac_pos_vel_variables_sub{ORB_ID(rcac_pos_vel_variables)}; // spjohn /**< RCAC variables log */ uORB::Subscription _control_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle control mode subscription */ uORB::Subscription _hover_thrust_estimate_sub{ORB_ID(hover_thrust_estimate)}; uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)}; uORB::Subscription _vehicle_constraints_sub{ORB_ID(vehicle_constraints)}; + uORB::Subscription _rc_channels_sub{ORB_ID(rc_channels)}; /**< Switch from the RC channel */ + uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */ hrt_abstime _time_stamp_last_loop{0}; /**< time stamp of last loop iteration */ + bool _landed{true}; + bool _maybe_landed{true}; int _task_failure_count{0}; /**< counter for task failures */ vehicle_control_mode_s _control_mode{}; /**< vehicle control mode */ vehicle_local_position_s _local_pos{}; /**< vehicle local position */ + rcac_pos_vel_variables_s _rcac_pos_vel_variables{}; /**< RCAC variables */ + rc_channels_s _rc_channels_switch{}; /**< Switch from the RC channel */ DEFINE_PARAMETERS( // Position Control @@ -123,6 +137,16 @@ class MulticopterPositionControl : public ModuleBase (ParamFloat) _param_mpc_tiltmax_air, (ParamFloat) _param_mpc_thr_hover, (ParamBool) _param_mpc_use_hte, + (ParamFloat) _param_mpc_rcac_pos_sw, + (ParamFloat) _param_mpc_rcac_vel_sw, + (ParamFloat) _param_mpc_pos_alpha, + (ParamFloat) _param_mpc_vel_alpha, + (ParamFloat) _param_mpc_rcac_pos_P0, + (ParamFloat) _param_mpc_rcac_vel_P0, + (ParamFloat) _param_mpc_rcac_pos_Ru, + (ParamFloat) _param_mpc_rcac_vel_Ru, + (ParamFloat) _param_mpc_rcac_pos_N, + (ParamFloat) _param_mpc_rcac_vel_N, // Takeoff / Land (ParamFloat) _param_mpc_tko_speed, @@ -149,6 +173,8 @@ class MulticopterPositionControl : public ModuleBase bool _in_failsafe = false; /**< true if failsafe was entered within current cycle */ + bool _rcac_logging = true; /**< True if logging the aircraft state variable */ //TODO: MAV integration + bool _hover_thrust_initialized{false}; /** Timeout in us for trajectory data to get considered invalid */ @@ -164,6 +190,13 @@ class MulticopterPositionControl : public ModuleBase perf_counter_t _cycle_perf; + /** + * Publish RCAC position and velocity variables for logging and mavlink send commands + * @param pid_scale Denotes the gain by which regular PID commands are scaled. + * @param rcac_switch Denotes the state of the swicth taht enables RCAC. + */ + void publish_rcac_pos_vel_variables(float pid_scale, float rcac_switch); + /** * Update our local parameter cache. * Parameter update can be forced when argument is true. diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp index 8cdafc284793..e1f2d6dc060e 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp @@ -38,12 +38,28 @@ #include "PositionControl.hpp" #include "ControlMath.hpp" #include -#include #include #include +// #include using namespace matrix; +PositionControl::PositionControl() +{ + _rcacParams_pos.initParams.RBlock_EN = true; + _rcacParams_vel.initParams.RBlock_EN = true; + _rcacParams_vel.initParams.errorNormMode = 5; + + for (int i = 0; i < 3; ++i) { + _rcac_pos_public_io(i) = RCAC_Public_IO(&_rcac_pos(i)); + _rcac_vel_public_io(i) = RCAC_Public_IO(&_rcac_vel(i)); + } + + _rcac_pos_params_io = RCACParams_IO(&_rcacParams_pos); + _rcac_vel_params_io = RCACParams_IO(&_rcacParams_vel); + //TODO: See if Zero Initialization of Vectors are needed. +} + void PositionControl::setVelocityGains(const Vector3f &P, const Vector3f &I, const Vector3f &D) { _gain_vel_p = P; @@ -79,7 +95,7 @@ void PositionControl::setState(const PositionControlStates &states) _vel_dot = states.acceleration; } -void PositionControl::setInputSetpoint(const vehicle_local_position_setpoint_s &setpoint) +void PositionControl::setInputSetpoint(vehicle_local_position_setpoint_s setpoint) { _pos_sp = Vector3f(setpoint.x, setpoint.y, setpoint.z); _vel_sp = Vector3f(setpoint.vx, setpoint.vy, setpoint.vz); @@ -109,15 +125,16 @@ void PositionControl::setConstraints(const vehicle_constraints_s &constraints) // ignore _constraints.speed_xy TODO: remove it completely as soon as no task uses it anymore to avoid confusion } -bool PositionControl::update(const float dt) +bool PositionControl::update(const float dt, const bool landed) { // x and y input setpoints always have to come in pairs const bool valid = (PX4_ISFINITE(_pos_sp(0)) == PX4_ISFINITE(_pos_sp(1))) && (PX4_ISFINITE(_vel_sp(0)) == PX4_ISFINITE(_vel_sp(1))) && (PX4_ISFINITE(_acc_sp(0)) == PX4_ISFINITE(_acc_sp(1))); - _positionControl(); - _velocityControl(dt); + + _positionControl(landed); + _velocityControl(dt, landed); _yawspeed_sp = PX4_ISFINITE(_yawspeed_sp) ? _yawspeed_sp : 0.f; _yaw_sp = PX4_ISFINITE(_yaw_sp) ? _yaw_sp : _yaw; // TODO: better way to disable yaw control @@ -125,10 +142,48 @@ bool PositionControl::update(const float dt) return valid && _updateSuccessful(); } -void PositionControl::_positionControl() +void PositionControl::_positionControl(const bool landed) { // P-position controller Vector3f vel_sp_position = (_pos_sp - _pos).emult(_gain_pos_p); + + z_k_pos = _pos_sp - _pos; + + u_k_pos.setZero(); + + // if (_rcacParams_pos.tuneParams.RCAC_EN) + // if (_rcac_pos(0).getkk() == 0) + // { + // init_RCAC_pos(); + // u_km1_pos = u_k_pos; + // } + + if ( (_rcac_pos(0).get_RCAC_EN() && _rcac_pos(0).getkk() == 0) || + (!_rcac_pos(0).get_RCAC_EN() && _rcac_pos(0).getkk() != 0) ) + { + init_RCAC_pos(); + } + + for (int i = 0; i <= 2; i++) + { if (((i<2) || ((i==2)&&(_pos_sp(2)<0))) && (!isnan(_pos_sp(0)))) + { + matrix::Matrix Phi_pos; + Phi_pos(0, 0) = z_k_pos(i); + + u_k_pos(i) = _rcac_pos(i).compute_uk(z_k_pos(i), Phi_pos, u_km1_pos(i)); + } + } + u_km1_pos = u_k_pos; + + + + + + vel_sp_position = _rcacParams_pos.tuneParams.alpha_PID*vel_sp_position + u_k_pos; + // std::cout << "\nvel_sp_pos = [" << vel_sp_position(0) << ", " << vel_sp_position(1) << ", " << vel_sp_position(2) << "\n"; + // std::cout << "\nvel_sp_pos(3) = FF = " << vel_sp_position(3) << "\n"; + // PX4_INFO("vel_sp_ff = \t%8.6f \t%8.6f \t%8.6f", (double)vel_sp_position(0), (double)vel_sp_position(1), (double)vel_sp_position(2)); + // Position and feed-forward velocity setpoints or position states being NAN results in them not having an influence ControlMath::addIfNotNanVector3f(_vel_sp, vel_sp_position); // make sure there are no NAN elements for further reference while constraining @@ -141,12 +196,42 @@ void PositionControl::_positionControl() _vel_sp(2) = math::constrain(_vel_sp(2), -_constraints.speed_up, _constraints.speed_down); } -void PositionControl::_velocityControl(const float dt) +void PositionControl::_velocityControl(const float dt, const bool landed) { // PID velocity control Vector3f vel_error = _vel_sp - _vel; Vector3f acc_sp_velocity = vel_error.emult(_gain_vel_p) + _vel_int - _vel_dot.emult(_gain_vel_d); + z_k_vel = _vel_sp - _vel; + + u_k_vel.setZero(); + + if ( (_rcac_vel(0).get_RCAC_EN() && _rcac_vel(0).getkk() == 0) || + (!_rcac_vel(0).get_RCAC_EN() && _rcac_vel(0).getkk() != 0) ) + { + init_RCAC_vel(); + } + + for (int i = 0; i <= 2; i++) + { + if (!isnan(_pos_sp(0))) + { + matrix::Matrix Phi_vel; + Phi_vel(0, 0) = z_k_vel(i); + Phi_vel(0, 1) = _vel_int(i); + Phi_vel(0, 2) = _vel_dot(i) * 0; + + u_k_vel(i) = _rcac_vel(i).compute_uk(z_k_vel(i), Phi_vel, u_km1_vel(i)); + } + } + + u_km1_vel = u_k_vel; + + // std::cout << "\nacc_sp_velocity = [" << acc_sp_velocity(0) << ", " << acc_sp_velocity(1) << ", " << acc_sp_velocity(2) << "]\n"; + + + acc_sp_velocity = _rcacParams_vel.tuneParams.alpha_PID*acc_sp_velocity + u_k_vel; + // No control input from setpoints or corresponding states which are NAN ControlMath::addIfNotNanVector3f(_acc_sp, acc_sp_velocity); @@ -175,6 +260,8 @@ void PositionControl::_velocityControl(const float dt) const Vector2f thrust_sp_xy(_thr_sp); const float thrust_sp_xy_norm = thrust_sp_xy.norm(); + + if (thrust_sp_xy_norm > thrust_max_xy) { _thr_sp.xy() = thrust_sp_xy / thrust_sp_xy_norm * thrust_max_xy; } @@ -187,7 +274,13 @@ void PositionControl::_velocityControl(const float dt) // Make sure integral doesn't get NAN ControlMath::setZeroIfNanVector3f(vel_error); - // Update integral part of velocity control + // Update integral of Velocity Control - RCAC Implementaiton + for (int i = 0; i < 3; ++i) { + //RCAC lib method + _rcac_vel(i).update_integral(vel_error(i), dt); + } + + // Update integral part of velocity control - PX4 Implementation _vel_int += vel_error.emult(_gain_vel_i) * dt; // limit thrust integral @@ -248,3 +341,47 @@ void PositionControl::getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_ ControlMath::thrustToAttitude(_thr_sp, _yaw_sp, attitude_setpoint); attitude_setpoint.yaw_sp_move_rate = _yawspeed_sp; } + +const matrix::Vector3f PositionControl::get_PX4_pos_theta() +{ + matrix::Vector3f PX4_theta{}; + PX4_theta(0) = _gain_pos_p(0); + PX4_theta(1) = _gain_pos_p(1); + PX4_theta(2) = _gain_pos_p(2); + return PX4_theta; +} + +const matrix::Vector PositionControl::get_PX4_ol_theta() +{ + matrix::Vector PX4_theta{}; + PX4_theta(0) = _gain_pos_p(0); + PX4_theta(1) = _gain_pos_p(1); + PX4_theta(2) = _gain_pos_p(2); + + PX4_theta(3) = _gain_vel_p(0); + PX4_theta(4) = _gain_vel_i(0); + PX4_theta(5) = _gain_vel_d(0); + + PX4_theta(6) = _gain_vel_p(2); + PX4_theta(7) = _gain_vel_i(2); + PX4_theta(8) = _gain_vel_d(2); + + return PX4_theta; +} + +void PositionControl::init_RCAC_pos() +{ + for (int i = 0; i < 3; ++i) + { + _rcac_pos(i) = RCAC(_rcacParams_pos, _rcac_pos(i).get_MASTER_EN()); + } +} + +void PositionControl::init_RCAC_vel() +{ + for (int i = 0; i < 2; ++i) + { + _rcac_vel(i) = RCAC(_rcacParams_vel, _rcac_vel(i).get_MASTER_EN()); + } + _rcac_vel(2) = RCAC(_rcacParams_vel, _rcac_vel(2).get_MASTER_EN(), CONSTANTS_ONE_G); +} diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.hpp b/src/modules/mc_pos_control/PositionControl/PositionControl.hpp index 4986c93a86f8..37f163149cc7 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.hpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.hpp @@ -3,7 +3,7 @@ * Copyright (c) 2018 - 2019 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions + * moication, are permitted provided that the following conditions * are met: * * 1. Redistributions of source code must retain the above copyright @@ -40,10 +40,18 @@ #pragma once #include +#include #include #include #include +#define RCAC_POS_L_THETA 1 +#define RCAC_POS_L_RBLOCK 2 + +#define RCAC_VEL_L_THETA 3 +#define RCAC_VEL_L_RBLOCK 2 +// #include "vector" + struct PositionControlStates { matrix::Vector3f position; matrix::Vector3f velocity; @@ -75,7 +83,7 @@ class PositionControl { public: - PositionControl() = default; + PositionControl(); ~PositionControl() = default; /** @@ -137,7 +145,7 @@ class PositionControl * Note: NAN value means no feed forward/leave state uncontrolled if there's no higher order setpoint. * @param setpoint a vehicle_local_position_setpoint_s structure */ - void setInputSetpoint(const vehicle_local_position_setpoint_s &setpoint); + void setInputSetpoint(vehicle_local_position_setpoint_s setpoint); /** * Pass constraints that are stricter than the global limits @@ -155,13 +163,18 @@ class PositionControl * @param dt time in seconds since last iteration * @return true if update succeeded and output setpoint is executable, false if not */ - bool update(const float dt); + bool update(const float dt, const bool landed); /** * Set the integral term in xy to 0. * @see _vel_int */ - void resetIntegral() { _vel_int.setZero(); } + void resetIntegral() + { + _vel_int.setZero(); + for (int i = 0; i < 3; ++i) + _rcac_vel(i).reset_integral(); + } /** * Get the controllers output local position setpoint @@ -179,11 +192,40 @@ class PositionControl */ void getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_setpoint) const; + /* RCAC Pos/Vel Public Classes with Getter/Setters */ + matrix::Vector, 3> _rcac_pos_public_io; + RCACParams_IO _rcac_pos_params_io; + + + matrix::Vector, 3> _rcac_vel_public_io; + RCACParams_IO _rcac_vel_params_io; + + /** + * Get the + * @see Pos P gains + * @return The P gains + */ + const matrix::Vector3f get_PX4_pos_theta(); + + /** + * Get the + * @see PID gains + * @return PX4's PID gains in the outer loop + */ + const matrix::Vector get_PX4_ol_theta(); + + /** + * Reset RCAC variables + * @see _thr_int + */ + void init_RCAC_pos(); + void init_RCAC_vel(); + private: bool _updateSuccessful(); - void _positionControl(); ///< Position proportional control - void _velocityControl(const float dt); ///< Velocity PID control + void _positionControl(const bool landed); ///< Position proportional control + void _velocityControl(const float dt, const bool landed); ///< Velocity PID control void _accelerationControl(); ///< Acceleration setpoint processing // Gains @@ -218,4 +260,22 @@ class PositionControl matrix::Vector3f _thr_sp; /**< desired thrust */ float _yaw_sp{}; /**< desired heading */ float _yawspeed_sp{}; /** desired yaw-speed */ + + + + // int pos_start = 0; // spjohn - used for testing + // int vel_start = 0; + + // RCAC -- Position Controller + matrix::Vector, 3> _rcac_pos; + RCACParams _rcacParams_pos; + + matrix::Vector3f z_k_pos, u_k_pos, u_km1_pos; + + // RCAC -- Velocity Controller + matrix::Vector, 3> _rcac_vel; + RCACParams _rcacParams_vel; + + matrix::Vector3f z_k_vel, u_k_vel, u_km1_vel, Pv_intg; + }; diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index 332490345aba..8687ae0f531b 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -789,3 +789,108 @@ PARAM_DEFINE_FLOAT(MPC_SPOOLUP_TIME, 1.0f); * @group Mission */ PARAM_DEFINE_INT32(MPC_YAW_MODE, 0); + +/** + * RCAC Position switches. + * + * Specifies RCAC state. + * + * @min -10 + * @max 10 + * @group Multicopter RCAC Control + */ +PARAM_DEFINE_FLOAT(MPC_RCAC_POS_SW, -1.0f); + +/** + * RCAC Velocity switches. + * + * Specifies RCAC state. + * + * @min -10 + * @max 10 + * @group Multicopter RCAC Control + */ +PARAM_DEFINE_FLOAT(MPC_RCAC_VEL_SW, -1.0f); + +/** + * PID Position gain. + * + * Specifies gain applied to the PID position gains. + * + * @min -10 + * @max 10 + * @group Multicopter RCAC Control + */ +PARAM_DEFINE_FLOAT(MPC_POS_ALPHA, 1.0f); + +/** + * PID Velocity gain. + * + * Specifies gain applied to the PID velocity gains. + * + * @min -10 + * @max 10 + * @group Multicopter RCAC Control + */ +PARAM_DEFINE_FLOAT(MPC_VEL_ALPHA, 1.0f); + +/** + * P0 for the position controller + * + * @min 0.0 + * @max 10.0 + * @decimal 8 + * @group Multicopter RCAC Control + */ +PARAM_DEFINE_FLOAT(MPC_RCAC_POS_P0, 0.005f); + +/** + * P0 for the velocity controller + * + * @min 0.0 + * @max 10.0 + * @decimal 8 + * @group Multicopter RCAC Control + */ +PARAM_DEFINE_FLOAT(MPC_RCAC_VEL_P0, 0.001f); + +/** + * Ru for the position controller + * + * @min 0.0 + * @max 10.0 + * @decimal 8 + * @group Multicopter RCAC Control + */ +PARAM_DEFINE_FLOAT(MPC_RCAC_POS_RU, 0.005f); + +/** + * Ru for the velocity controller + * + * @min 0.0 + * @max 10.0 + * @decimal 8 + * @group Multicopter RCAC Control + */ +PARAM_DEFINE_FLOAT(MPC_RCAC_VEL_RU, 1.0f); + +/** + * N for the position controller + * + * @min 0.0 + * @max 10.0 + * @decimal 8 + * @group Multicopter RCAC Control + */ +PARAM_DEFINE_FLOAT(MPC_RCAC_POS_N, 0.005f); + +/** + * N for the velocity controller + * + * @min 0.0 + * @max 10.0 + * @decimal 8 + * @group Multicopter RCAC Control + */ +PARAM_DEFINE_FLOAT(MPC_RCAC_VEL_N, 1.0f); + diff --git a/src/modules/mc_rate_control/MulticopterRateControl.cpp b/src/modules/mc_rate_control/MulticopterRateControl.cpp index dc97d17dbb48..9e487498069e 100644 --- a/src/modules/mc_rate_control/MulticopterRateControl.cpp +++ b/src/modules/mc_rate_control/MulticopterRateControl.cpp @@ -94,6 +94,44 @@ MulticopterRateControl::parameters_updated() radians(_param_mc_acro_y_max.get())); _actuators_0_circuit_breaker_enabled = circuit_breaker_enabled_by_val(_param_cbrk_rate_ctrl.get(), CBRK_RATE_CTRL_KEY); + + + //Set P0 for RCAC attitude and Rate controller + + _rate_control._rcac_rate_params_io.set_P0(_param_mpc_rcac_rate_P0.get()); + _rate_control._rcac_rate_params_io.set_Ru(_param_mpc_rcac_rate_Ru.get()); + _rate_control._rcac_rate_params_io.set_RCAC_EN(_param_mpc_rcac_rate_sw.get() > 0.0f); + _rate_control._rcac_rate_params_io.set_N_nf(_param_mpc_rcac_rate_N.get()); + // _rate_control.init_RCAC_rate(); + PX4_INFO("Rate Control P0:\t%8.6f", (double)_param_mpc_rcac_rate_P0.get()); + +} + +void +MulticopterRateControl::publish_rcac_rate_variables() +{ + _rcac_rate_variables.timestamp = hrt_absolute_time(); + _rcac_rate_variables.ii_rate = _rate_control._rcac_rate_public_io(0).get_kk(); + _rcac_rate_variables.switch_rate = _rate_control._rcac_rate_params_io.get_switch(); //_rcac_rate_public_io(0).get_RCAC_EN(); + + _rcac_rate_variables.alpha_pid_rate = _rate_control._rcac_rate_params_io.get_alpha(); + + _rcac_rate_variables.p11_ratex = _rate_control._rcac_rate_public_io(0).get_P11(); + + _rcac_rate_variables.rcac_rate_ru = _rate_control._rcac_rate_params_io.get_Ru(); + _rcac_rate_variables.rcac_rate_n = _rate_control._rcac_rate_params_io.get_N_nf(); + + for (int i = 0; i <= 2; i++) { + + _rcac_rate_variables.rcac_rate_z[i] = _rate_control._rcac_rate_public_io(i).get_zk(); + _rcac_rate_variables.rcac_rate_u[i] = _rate_control._rcac_rate_public_io(i).get_uk(); + + } + for (int i = 0; i <= 11; i++) { + _rcac_rate_variables.rcac_rate_theta[i] = _rate_control.get_RCAC_rate_theta()(i,0); + _rcac_rate_variables.px4_rate_theta[i] = _rate_control.get_PX4_rate_theta()(i,0); + } + _rcac_rate_variables_pub.publish(_rcac_rate_variables); } void @@ -135,6 +173,30 @@ MulticopterRateControl::Run() const Vector3f angular_accel{v_angular_acceleration.xyz}; const Vector3f rates{angular_velocity.xyz}; + //RCtopic + // Ankit: RCAC switches and the PID scaling factor controlled by the RC transmitter + _rc_channels_sub.update(&_rc_channels_switch); + + #ifdef __PX4_NUTTX + float PID_scale_f = _rc_channels_switch.channels[13]; + float RCAC_switch = _rc_channels_switch.channels[14]; + #else + float PID_scale_f = -1.0f; + float RCAC_switch = 1.0f; + #endif /* __PX4_NUTTX */ + + // Joon: Changed these from if statements to boolean logic + bool RCAC_HW_EN = (RCAC_switch > 0.0f); + bool ALPHA_HW_EN = (PID_scale_f < 0.0f); + + bool RCAC_RATE_MASTER_EN = RCAC_HW_EN && !_landed; + + for (size_t i = 0; i < 3; ++i) { + _rate_control._rcac_rate_public_io(i).set_MASTER_EN(RCAC_RATE_MASTER_EN); + } + + _rate_control._rcac_rate_params_io.set_alpha(ALPHA_HW_EN ? _param_mpc_rate_alpha.get() : 1.0f); + /* check for updates in other topics */ _v_control_mode_sub.update(&_v_control_mode); @@ -145,6 +207,9 @@ MulticopterRateControl::Run() _landed = vehicle_land_detected.landed; _maybe_landed = vehicle_land_detected.maybe_landed; } + + if (_landed) + _rate_control.init_RCAC_rate(); } _vehicle_status_sub.update(&_vehicle_status); @@ -276,6 +341,7 @@ MulticopterRateControl::Run() actuators.timestamp = hrt_absolute_time(); _actuators_0_pub.publish(actuators); + publish_rcac_rate_variables(); } else if (_v_control_mode.flag_control_termination_enabled) { if (!_vehicle_status.is_vtol) { @@ -283,6 +349,7 @@ MulticopterRateControl::Run() actuator_controls_s actuators{}; actuators.timestamp = hrt_absolute_time(); _actuators_0_pub.publish(actuators); + publish_rcac_rate_variables(); } } } diff --git a/src/modules/mc_rate_control/MulticopterRateControl.hpp b/src/modules/mc_rate_control/MulticopterRateControl.hpp index 68168480136f..88f4b21e94d3 100644 --- a/src/modules/mc_rate_control/MulticopterRateControl.hpp +++ b/src/modules/mc_rate_control/MulticopterRateControl.hpp @@ -59,6 +59,8 @@ #include #include #include +#include +#include using namespace time_literals; @@ -87,6 +89,8 @@ class MulticopterRateControl : public ModuleBase, public */ void parameters_updated(); + void publish_rcac_rate_variables(); + RateControl _rate_control; ///< class for rate control calculations uORB::Subscription _battery_status_sub{ORB_ID(battery_status)}; @@ -98,6 +102,7 @@ class MulticopterRateControl : public ModuleBase, public uORB::Subscription _vehicle_angular_acceleration_sub{ORB_ID(vehicle_angular_acceleration)}; uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _rc_channels_sub{ORB_ID(rc_channels)}; /**< RC switch>*/ uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; @@ -107,10 +112,14 @@ class MulticopterRateControl : public ModuleBase, public uORB::PublicationMulti _controller_status_pub{ORB_ID(rate_ctrl_status)}; /**< controller status publication */ uORB::Publication _landing_gear_pub{ORB_ID(landing_gear)}; uORB::Publication _v_rates_sp_pub{ORB_ID(vehicle_rates_setpoint)}; /**< rate setpoint publication */ + uORB::Publication _rcac_rate_variables_pub{ORB_ID(rcac_rate_variables)}; /**< RCAC variables log */ + manual_control_setpoint_s _manual_control_setpoint{}; vehicle_control_mode_s _v_control_mode{}; vehicle_status_s _vehicle_status{}; + rc_channels_s _rc_channels_switch{}; /**< Switch from the RC channel */ + rcac_rate_variables_s _rcac_rate_variables{}; bool _actuators_0_circuit_breaker_enabled{false}; /**< circuit breaker to suppress output */ bool _landed{true}; @@ -164,8 +173,14 @@ class MulticopterRateControl : public ModuleBase, public (ParamBool) _param_mc_bat_scale_en, - (ParamInt) _param_cbrk_rate_ctrl - ) + (ParamInt) _param_cbrk_rate_ctrl, + + (ParamFloat) _param_mpc_rcac_rate_sw, + (ParamFloat) _param_mpc_rate_alpha, + (ParamFloat) _param_mpc_rcac_rate_P0, + (ParamFloat) _param_mpc_rcac_rate_Ru, + (ParamFloat) _param_mpc_rcac_rate_N + ); matrix::Vector3f _acro_rate_max; /**< max attitude rates in acro mode */ diff --git a/src/modules/mc_rate_control/RateControl/RateControl.cpp b/src/modules/mc_rate_control/RateControl/RateControl.cpp index 37a252550619..218ae8222c08 100644 --- a/src/modules/mc_rate_control/RateControl/RateControl.cpp +++ b/src/modules/mc_rate_control/RateControl/RateControl.cpp @@ -37,9 +37,21 @@ #include #include +// #include using namespace matrix; +RateControl::RateControl() { + // Set InitParams Here + _rcacParams_rate.initParams.RBlock_EN = true; + _rcacParams_rate.initParams.errorNormMode = 5; + + for (size_t i = 0; i < 3; ++i) { + _rcac_rate_public_io(i) = RCAC_Public_IO(&_rcac_rate(i)); + } + _rcac_rate_params_io = RCACParams_IO(&_rcacParams_rate); +} + void RateControl::setGains(const Vector3f &P, const Vector3f &I, const Vector3f &D) { _gain_p = P; @@ -64,13 +76,36 @@ Vector3f RateControl::update(const Vector3f &rate, const Vector3f &rate_sp, cons Vector3f rate_error = rate_sp - rate; // PID control with feed forward - const Vector3f torque = _gain_p.emult(rate_error) + _rate_int - _gain_d.emult(angular_accel) + _gain_ff.emult(rate_sp); + //const Vector3f torque = _gain_p.emult(rate_error) + _rate_int - _gain_d.emult(angular_accel) + _gain_ff.emult(rate_sp); + Vector3f torque = _gain_p.emult(rate_error) + _rate_int - _gain_d.emult(angular_accel) + _gain_ff.emult(rate_sp); + + //PX4_INFO("Rate Controller:\t%8.4f", (double)dt); + z_k_rate = rate_error; + + u_k_rate.setZero(); - // update integral only if we are not landed - if (!landed) { + if (!landed) updateIntegral(rate_error, dt); + + if ( (_rcac_rate(0).get_RCAC_EN() && _rcac_rate(0).getkk() == 0) || + (!_rcac_rate(0).get_RCAC_EN() && _rcac_rate(0).getkk() != 0) ) + { + init_RCAC_rate(); + } + + // Landed Check Removed after Landed check is now moved to mc_att_control_main. + for (size_t i = 0; i <= 2; ++i) + { + matrix::Matrix Phi_rate; + Phi_rate(0, 0) = z_k_rate(i); + Phi_rate(0, 1) = _rcac_rate(i).get_rcac_integral(); //_rate_int(i); + Phi_rate(0, 2) = 0 * angular_accel(i); + u_k_rate(i) = _rcac_rate(i).compute_uk(z_k_rate(i), Phi_rate, u_km1_rate(i)); } + torque = _rcacParams_rate.tuneParams.alpha_PID * torque + u_k_rate; + u_km1_rate = u_k_rate; + return torque; } @@ -97,12 +132,16 @@ void RateControl::updateIntegral(Vector3f &rate_error, const float dt) i_factor = math::max(0.0f, 1.f - i_factor * i_factor); // Perform the integration using a first order method - float rate_i = _rate_int(i) + i_factor * _gain_i(i) * rate_error(i) * dt; + // float rate_i = _rate_int(i) + i_factor * _gain_i(i) * rate_error(i) * dt; + float rate_i = _rate_int(i) + i_factor * rate_error(i) * dt; // do not propagate the result if out of range or invalid if (PX4_ISFINITE(rate_i)) { _rate_int(i) = math::constrain(rate_i, -_lim_int(i), _lim_int(i)); } + + // Update RCAC Class Integral + _rcac_rate(i).update_integral(i_factor * rate_error(i), dt); } } diff --git a/src/modules/mc_rate_control/RateControl/RateControl.hpp b/src/modules/mc_rate_control/RateControl/RateControl.hpp index 6f0783a34953..982d13ef037e 100644 --- a/src/modules/mc_rate_control/RateControl/RateControl.hpp +++ b/src/modules/mc_rate_control/RateControl/RateControl.hpp @@ -45,10 +45,13 @@ #include #include +#define RCAC_RATE_L_THETA 3 +#define RCAC_RATE_L_RBLOCK 2 + class RateControl { public: - RateControl() = default; + RateControl(); ~RateControl() = default; /** @@ -92,7 +95,12 @@ class RateControl * Set the integral term to 0 to prevent windup * @see _rate_int */ - void resetIntegral() { _rate_int.zero(); } + void resetIntegral() + { + _rate_int.zero(); + for (int i = 0; i < 3; ++i) + _rcac_rate(i).reset_integral(); + } /** * Get status message of controller for logging/debugging @@ -100,6 +108,69 @@ class RateControl */ void getRateControlStatus(rate_ctrl_status_s &rate_ctrl_status); + /** + * Get the + * @see theta_k_Pr_R + * @return The theta variable computed by RCAC in the PID+FF controller + */ + const matrix::Matrix get_RCAC_rate_theta() + { + matrix::Matrix RCAC_theta; + + RCAC_theta.setZero(); + + for (size_t i = 0; i <= 2; ++i) + { + RCAC_theta(i,0) = _rcac_rate(0).get_rcac_theta(i); + RCAC_theta(i+4,0) = _rcac_rate(1).get_rcac_theta(i); + RCAC_theta(i+8,0) = _rcac_rate(2).get_rcac_theta(i); + } + + return RCAC_theta; + } + + /** + * Get the + * @see gains + * @return PX4 PID gains for the rate controller + */ + const matrix::Matrix get_PX4_rate_theta() + { + matrix::Matrix PX4_rate_theta; + + PX4_rate_theta.setZero(); + + PX4_rate_theta(0,0) = _gain_p(0); + PX4_rate_theta(1,0) = _gain_i(0); + PX4_rate_theta(2,0) = _gain_d(0); + PX4_rate_theta(3,0) = _gain_ff(0); + PX4_rate_theta(4,0) = _gain_p(1); + PX4_rate_theta(5,0) = _gain_i(1); + PX4_rate_theta(6,0) = _gain_d(1); + PX4_rate_theta(7,0) = _gain_ff(1); + PX4_rate_theta(8,0) = _gain_p(2); + PX4_rate_theta(9,0) = _gain_i(2); + PX4_rate_theta(10,0) = _gain_d(2); + PX4_rate_theta(11,0) = _gain_ff(2); + + return PX4_rate_theta; + } + + /** + * Set RCAC variables. + * @see all RCAC variables + */ + void init_RCAC_rate() + { + for (size_t i = 0; i < 3; ++i) { + _rcac_rate(i) = RCAC(_rcacParams_rate, _rcac_rate(i).get_MASTER_EN(), _lim_int(i)); + } + } + + /* RCAC Pos/Vel Public Classes with Getter/Setters */ + matrix::Vector, 3> _rcac_rate_public_io; + RCACParams_IO _rcac_rate_params_io; + private: void updateIntegral(matrix::Vector3f &rate_error, const float dt); @@ -115,4 +186,10 @@ class RateControl bool _mixer_saturation_positive[3] {}; bool _mixer_saturation_negative[3] {}; + + matrix::Vector3f z_k_rate, z_km1_rate, u_k_rate, u_km1_rate; + + // New RCAC_Class_Variables + matrix::Vector, 3> _rcac_rate; + RCACParams _rcacParams_rate; }; diff --git a/src/modules/mc_rate_control/mc_rate_control_params.c b/src/modules/mc_rate_control/mc_rate_control_params.c index df6f98e1cdb6..878cb94b4455 100644 --- a/src/modules/mc_rate_control/mc_rate_control_params.c +++ b/src/modules/mc_rate_control/mc_rate_control_params.c @@ -395,3 +395,55 @@ PARAM_DEFINE_FLOAT(MC_ACRO_SUPEXPOY, 0.7f); * @group Multicopter Rate Control */ PARAM_DEFINE_INT32(MC_BAT_SCALE_EN, 0); + +/** + * RCAC Rate switches. + * + * Specifies RCAC state. + * + * @min -10 + * @max 10 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MPC_RCAC_RATE_SW, -1.0f); + +/** + * PID Rate gain. + * + * Specifies gain applied to the PID rate gains. + * + * @min -10 + * @max 10 + * @group Multicopter RCAC Control + */ +PARAM_DEFINE_FLOAT(MPC_RATE_ALPHA, 1.0f); + +/** + * P0 for the Rate controller + * + * @min 0.0 + * @max 10.0 + * @decimal 8 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MPC_RCAC_RATE_P0, 0.000001f); + +/** + * Ru for the Rate controller + * + * @min 0.0 + * @max 10.0 + * @decimal 8 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MPC_RCAC_RATE_RU, 1.0f); + +/** + * N_nf for the Rate controller + * + * @min 0.0 + * @max 10.0 + * @decimal 8 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MPC_RCAC_RATE_N, 1.0f); \ No newline at end of file diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 43e35efdedf7..862fd0bf9c51 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -371,7 +371,13 @@ mixer_tick() && !(r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN)) { /* update the servo outputs. */ for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) { - up_pwm_servo_set(i, r_page_servos[i]); + if ((i==0) && (r_page_actuators[6]> 4000)){ //Flaps or Gear here, SR + up_pwm_servo_set(i, r_page_servo_disarmed[i]); + isr_debug(5,"check"); + } + else { + up_pwm_servo_set(i, r_page_servos[i]); + } } /* set S.BUS1 or S.BUS2 outputs */