diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps index 5c652e6a64..e26050bed1 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -48,6 +48,11 @@ fi # End Estimator Group Selection # ############################################################################### +# +# Start Multicopter Rate Controller. +# +mc_rate_control start + # # Start Multicopter Attitude Controller. # diff --git a/ROMFS/px4fmu_common/init.d/rc.vtol_apps b/ROMFS/px4fmu_common/init.d/rc.vtol_apps index 0861bdca56..28c734e860 100644 --- a/ROMFS/px4fmu_common/init.d/rc.vtol_apps +++ b/ROMFS/px4fmu_common/init.d/rc.vtol_apps @@ -17,6 +17,7 @@ ekf2 start vtol_att_control start +mc_rate_control start mc_att_control start mc_pos_control start fw_att_control start diff --git a/Tools/uorb_graph/create.py b/Tools/uorb_graph/create.py index 2f50e76224..cc9a80ffc0 100755 --- a/Tools/uorb_graph/create.py +++ b/Tools/uorb_graph/create.py @@ -243,7 +243,7 @@ class Graph(object): ('mc_pos_control', r'mc_pos_control_main\.cpp$', r'\b_attitude_setpoint_id=([^,)]+)', r'^_attitude_setpoint_id$'), - ('mc_att_control', r'mc_att_control_main\.cpp$', r'\b_actuators_id=([^,)]+)', r'^_actuators_id$'), + ('mc_rate_control', r'MulticopterRateControl\.cpp$', r'\b_actuators_id=([^,)]+)', r'^_actuators_id$'), ('mc_att_control', r'mc_att_control_main\.cpp$', r'\_attitude_sp_id=([^,)]+)', r'^_attitude_sp_id$'), ('fw_att_control', r'FixedwingAttitudeControl\.cpp$', r'\b_actuators_id=([^,)]+)', r'^_actuators_id$'), diff --git a/boards/aerotenna/ocpoc/ubuntu.cmake b/boards/aerotenna/ocpoc/ubuntu.cmake index 6a37b27096..c784bc0311 100644 --- a/boards/aerotenna/ocpoc/ubuntu.cmake +++ b/boards/aerotenna/ocpoc/ubuntu.cmake @@ -47,6 +47,7 @@ px4_add_board( logger mavlink mc_att_control + mc_rate_control mc_pos_control navigator battery_status diff --git a/boards/airmind/mindpx-v2/default.cmake b/boards/airmind/mindpx-v2/default.cmake index 2707f24cc1..bdbcb18188 100644 --- a/boards/airmind/mindpx-v2/default.cmake +++ b/boards/airmind/mindpx-v2/default.cmake @@ -63,6 +63,7 @@ px4_add_board( logger mavlink mc_att_control + mc_rate_control mc_pos_control navigator battery_status diff --git a/boards/atlflight/eagle/default.cmake b/boards/atlflight/eagle/default.cmake index 62b7d6fd55..acc31558f2 100644 --- a/boards/atlflight/eagle/default.cmake +++ b/boards/atlflight/eagle/default.cmake @@ -80,6 +80,7 @@ px4_add_board( logger mavlink mc_att_control + mc_rate_control mc_pos_control navigator sensors diff --git a/boards/atlflight/eagle/qurt-default.cmake b/boards/atlflight/eagle/qurt-default.cmake index 449fcbeed5..0c4f77749c 100644 --- a/boards/atlflight/eagle/qurt-default.cmake +++ b/boards/atlflight/eagle/qurt-default.cmake @@ -74,6 +74,7 @@ px4_add_board( landing_target_estimator local_position_estimator mc_att_control + mc_rate_control mc_pos_control sensors #sih diff --git a/boards/atlflight/excelsior/default.cmake b/boards/atlflight/excelsior/default.cmake index 7fc6cce923..a6e6671c0d 100644 --- a/boards/atlflight/excelsior/default.cmake +++ b/boards/atlflight/excelsior/default.cmake @@ -80,6 +80,7 @@ px4_add_board( logger mavlink mc_att_control + mc_rate_control mc_pos_control navigator sensors diff --git a/boards/atlflight/excelsior/qurt-default.cmake b/boards/atlflight/excelsior/qurt-default.cmake index 9f0da0588e..ebfa9a57a5 100644 --- a/boards/atlflight/excelsior/qurt-default.cmake +++ b/boards/atlflight/excelsior/qurt-default.cmake @@ -73,6 +73,7 @@ px4_add_board( landing_target_estimator local_position_estimator mc_att_control + mc_rate_control mc_pos_control sensors sih diff --git a/boards/auav/x21/default.cmake b/boards/auav/x21/default.cmake index 23f44146e3..6df1fd5494 100644 --- a/boards/auav/x21/default.cmake +++ b/boards/auav/x21/default.cmake @@ -68,6 +68,7 @@ px4_add_board( logger mavlink mc_att_control + mc_rate_control mc_pos_control navigator battery_status diff --git a/boards/av/x-v1/default.cmake b/boards/av/x-v1/default.cmake index 2ee43312d0..3f0e7e78c3 100644 --- a/boards/av/x-v1/default.cmake +++ b/boards/av/x-v1/default.cmake @@ -69,6 +69,7 @@ px4_add_board( logger mavlink mc_att_control + mc_rate_control mc_pos_control navigator battery_status diff --git a/boards/beaglebone/blue/cross.cmake b/boards/beaglebone/blue/cross.cmake index 70b78f2b30..88a1ae5719 100644 --- a/boards/beaglebone/blue/cross.cmake +++ b/boards/beaglebone/blue/cross.cmake @@ -46,6 +46,7 @@ px4_add_board( logger mavlink mc_att_control + mc_rate_control mc_pos_control navigator battery_status diff --git a/boards/beaglebone/blue/native.cmake b/boards/beaglebone/blue/native.cmake index d2f767b8f8..3e2771e331 100644 --- a/boards/beaglebone/blue/native.cmake +++ b/boards/beaglebone/blue/native.cmake @@ -44,6 +44,7 @@ px4_add_board( logger mavlink mc_att_control + mc_rate_control mc_pos_control navigator battery_status diff --git a/boards/bitcraze/crazyflie/default.cmake b/boards/bitcraze/crazyflie/default.cmake index c96239134e..8cea363b3b 100644 --- a/boards/bitcraze/crazyflie/default.cmake +++ b/boards/bitcraze/crazyflie/default.cmake @@ -32,6 +32,7 @@ px4_add_board( logger mavlink mc_att_control + mc_rate_control mc_pos_control navigator sensors diff --git a/boards/emlid/navio2/cross.cmake b/boards/emlid/navio2/cross.cmake index 4cda5b8f68..88e857915c 100644 --- a/boards/emlid/navio2/cross.cmake +++ b/boards/emlid/navio2/cross.cmake @@ -51,6 +51,7 @@ px4_add_board( logger mavlink mc_att_control + mc_rate_control mc_pos_control navigator battery_status diff --git a/boards/emlid/navio2/native.cmake b/boards/emlid/navio2/native.cmake index c63b5b8b30..08e8647740 100644 --- a/boards/emlid/navio2/native.cmake +++ b/boards/emlid/navio2/native.cmake @@ -49,6 +49,7 @@ px4_add_board( logger mavlink mc_att_control + mc_rate_control mc_pos_control navigator battery_status diff --git a/boards/holybro/kakutef7/default.cmake b/boards/holybro/kakutef7/default.cmake index 98cd405c4e..700991aa3c 100644 --- a/boards/holybro/kakutef7/default.cmake +++ b/boards/holybro/kakutef7/default.cmake @@ -42,6 +42,7 @@ px4_add_board( logger mavlink mc_att_control + mc_rate_control mc_pos_control navigator battery_status diff --git a/boards/intel/aerofc-v1/default.cmake b/boards/intel/aerofc-v1/default.cmake index 574d2981c3..e03361657a 100644 --- a/boards/intel/aerofc-v1/default.cmake +++ b/boards/intel/aerofc-v1/default.cmake @@ -48,6 +48,7 @@ px4_add_board( logger mavlink mc_att_control + mc_rate_control mc_pos_control navigator battery_status diff --git a/boards/intel/aerofc-v1/rtps.cmake b/boards/intel/aerofc-v1/rtps.cmake index f4aa2f4fe3..91a1a6ff92 100644 --- a/boards/intel/aerofc-v1/rtps.cmake +++ b/boards/intel/aerofc-v1/rtps.cmake @@ -51,6 +51,7 @@ px4_add_board( logger mavlink mc_att_control + mc_rate_control mc_pos_control micrortps_bridge navigator diff --git a/boards/modalai/fc-v1/default.cmake b/boards/modalai/fc-v1/default.cmake index 8a3698285a..69fc2d7130 100644 --- a/boards/modalai/fc-v1/default.cmake +++ b/boards/modalai/fc-v1/default.cmake @@ -71,6 +71,7 @@ px4_add_board( mavlink mc_att_control mc_pos_control + mc_rate_control navigator rover_pos_control sensors diff --git a/boards/mro/ctrl-zero-f7/default.cmake b/boards/mro/ctrl-zero-f7/default.cmake index 473b53ba66..13963e3e0e 100644 --- a/boards/mro/ctrl-zero-f7/default.cmake +++ b/boards/mro/ctrl-zero-f7/default.cmake @@ -72,6 +72,7 @@ px4_add_board( mavlink mc_att_control mc_pos_control + mc_rate_control navigator battery_status sensors diff --git a/boards/nxp/fmuk66-v3/default.cmake b/boards/nxp/fmuk66-v3/default.cmake index b6901eb731..b61b4c1685 100644 --- a/boards/nxp/fmuk66-v3/default.cmake +++ b/boards/nxp/fmuk66-v3/default.cmake @@ -67,6 +67,7 @@ px4_add_board( logger mavlink mc_att_control + mc_rate_control mc_pos_control navigator battery_status diff --git a/boards/omnibus/f4sd/default.cmake b/boards/omnibus/f4sd/default.cmake index 002f95eab6..8cb4bba015 100644 --- a/boards/omnibus/f4sd/default.cmake +++ b/boards/omnibus/f4sd/default.cmake @@ -59,6 +59,7 @@ px4_add_board( logger mavlink mc_att_control + mc_rate_control mc_pos_control navigator battery_status diff --git a/boards/parrot/bebop/default.cmake b/boards/parrot/bebop/default.cmake index bd4f9646c9..a03b47bdb7 100644 --- a/boards/parrot/bebop/default.cmake +++ b/boards/parrot/bebop/default.cmake @@ -36,6 +36,7 @@ px4_add_board( logger mavlink mc_att_control + mc_rate_control mc_pos_control navigator sensors diff --git a/boards/px4/fmu-v2/default.cmake b/boards/px4/fmu-v2/default.cmake index ce11a39c3a..88672e31c6 100644 --- a/boards/px4/fmu-v2/default.cmake +++ b/boards/px4/fmu-v2/default.cmake @@ -78,6 +78,7 @@ px4_add_board( logger mavlink mc_att_control + mc_rate_control mc_pos_control navigator battery_status diff --git a/boards/px4/fmu-v2/lpe.cmake b/boards/px4/fmu-v2/lpe.cmake index 257a6fb71b..a946d6bdd7 100644 --- a/boards/px4/fmu-v2/lpe.cmake +++ b/boards/px4/fmu-v2/lpe.cmake @@ -75,6 +75,7 @@ px4_add_board( logger mavlink mc_att_control + mc_rate_control mc_pos_control navigator battery_status diff --git a/boards/px4/fmu-v2/multicopter.cmake b/boards/px4/fmu-v2/multicopter.cmake index 004e0b3221..31318ecd5c 100644 --- a/boards/px4/fmu-v2/multicopter.cmake +++ b/boards/px4/fmu-v2/multicopter.cmake @@ -33,7 +33,7 @@ px4_add_board( irlock lights/rgbled magnetometer/hmc5883 - optical_flow/px4flow + #optical_flow/px4flow px4fmu px4io tone_alarm @@ -52,6 +52,7 @@ px4_add_board( logger mavlink mc_att_control + mc_rate_control mc_pos_control navigator battery_status @@ -81,6 +82,6 @@ px4_add_board( tune_control usb_connected ver - work_queue + #work_queue ) diff --git a/boards/px4/fmu-v2/test.cmake b/boards/px4/fmu-v2/test.cmake index a539b78819..69c5dade1a 100644 --- a/boards/px4/fmu-v2/test.cmake +++ b/boards/px4/fmu-v2/test.cmake @@ -75,6 +75,7 @@ px4_add_board( logger mavlink mc_att_control + mc_rate_control mc_pos_control navigator battery_status diff --git a/boards/px4/fmu-v3/default.cmake b/boards/px4/fmu-v3/default.cmake index e1d3b55586..4fb71aaa08 100644 --- a/boards/px4/fmu-v3/default.cmake +++ b/boards/px4/fmu-v3/default.cmake @@ -77,6 +77,7 @@ px4_add_board( logger mavlink mc_att_control + mc_rate_control mc_pos_control navigator battery_status diff --git a/boards/px4/fmu-v3/rtps.cmake b/boards/px4/fmu-v3/rtps.cmake index 41bf518432..d26fdee2d8 100644 --- a/boards/px4/fmu-v3/rtps.cmake +++ b/boards/px4/fmu-v3/rtps.cmake @@ -75,6 +75,7 @@ px4_add_board( logger mavlink mc_att_control + mc_rate_control mc_pos_control micrortps_bridge navigator diff --git a/boards/px4/fmu-v3/stackcheck.cmake b/boards/px4/fmu-v3/stackcheck.cmake index 2da5d12796..582920b5bb 100644 --- a/boards/px4/fmu-v3/stackcheck.cmake +++ b/boards/px4/fmu-v3/stackcheck.cmake @@ -75,6 +75,7 @@ px4_add_board( logger mavlink mc_att_control + mc_rate_control mc_pos_control navigator battery_status diff --git a/boards/px4/fmu-v4/default.cmake b/boards/px4/fmu-v4/default.cmake index d2e2fc39aa..620a37b897 100644 --- a/boards/px4/fmu-v4/default.cmake +++ b/boards/px4/fmu-v4/default.cmake @@ -66,6 +66,7 @@ px4_add_board( logger mavlink mc_att_control + mc_rate_control mc_pos_control navigator rover_pos_control diff --git a/boards/px4/fmu-v4/rtps.cmake b/boards/px4/fmu-v4/rtps.cmake index 9ffea58c15..8363a5214c 100644 --- a/boards/px4/fmu-v4/rtps.cmake +++ b/boards/px4/fmu-v4/rtps.cmake @@ -67,6 +67,7 @@ px4_add_board( logger mavlink mc_att_control + mc_rate_control mc_pos_control micrortps_bridge navigator diff --git a/boards/px4/fmu-v4/stackcheck.cmake b/boards/px4/fmu-v4/stackcheck.cmake index 8c63dfd358..8f2eaa354f 100644 --- a/boards/px4/fmu-v4/stackcheck.cmake +++ b/boards/px4/fmu-v4/stackcheck.cmake @@ -66,6 +66,7 @@ px4_add_board( logger mavlink mc_att_control + mc_rate_control mc_pos_control navigator rover_pos_control diff --git a/boards/px4/fmu-v4pro/default.cmake b/boards/px4/fmu-v4pro/default.cmake index bd4c86569a..52db7a1d8c 100644 --- a/boards/px4/fmu-v4pro/default.cmake +++ b/boards/px4/fmu-v4pro/default.cmake @@ -71,6 +71,7 @@ px4_add_board( logger mavlink mc_att_control + mc_rate_control mc_pos_control navigator battery_status diff --git a/boards/px4/fmu-v4pro/rtps.cmake b/boards/px4/fmu-v4pro/rtps.cmake index 12240d8827..026bf881a3 100644 --- a/boards/px4/fmu-v4pro/rtps.cmake +++ b/boards/px4/fmu-v4pro/rtps.cmake @@ -73,6 +73,7 @@ px4_add_board( logger mavlink mc_att_control + mc_rate_control mc_pos_control micrortps_bridge navigator diff --git a/boards/px4/fmu-v5/critmonitor.cmake b/boards/px4/fmu-v5/critmonitor.cmake index 2230bd1683..e6ebf414fb 100644 --- a/boards/px4/fmu-v5/critmonitor.cmake +++ b/boards/px4/fmu-v5/critmonitor.cmake @@ -75,6 +75,7 @@ px4_add_board( logger mavlink mc_att_control + mc_rate_control mc_pos_control navigator battery_status diff --git a/boards/px4/fmu-v5/default.cmake b/boards/px4/fmu-v5/default.cmake index ba965890f6..9625a42553 100644 --- a/boards/px4/fmu-v5/default.cmake +++ b/boards/px4/fmu-v5/default.cmake @@ -76,6 +76,7 @@ px4_add_board( logger mavlink mc_att_control + mc_rate_control mc_pos_control navigator battery_status diff --git a/boards/px4/fmu-v5/irqmonitor.cmake b/boards/px4/fmu-v5/irqmonitor.cmake index d4e36c90ab..e26ccbc641 100644 --- a/boards/px4/fmu-v5/irqmonitor.cmake +++ b/boards/px4/fmu-v5/irqmonitor.cmake @@ -75,6 +75,7 @@ px4_add_board( logger mavlink mc_att_control + mc_rate_control mc_pos_control navigator battery_status diff --git a/boards/px4/fmu-v5/multicopter.cmake b/boards/px4/fmu-v5/multicopter.cmake index 96c9d0b61f..cfbd7048e8 100644 --- a/boards/px4/fmu-v5/multicopter.cmake +++ b/boards/px4/fmu-v5/multicopter.cmake @@ -60,6 +60,7 @@ px4_add_board( logger mavlink mc_att_control + mc_rate_control mc_pos_control navigator battery_status diff --git a/boards/px4/fmu-v5/rtps.cmake b/boards/px4/fmu-v5/rtps.cmake index 0c7c515dcf..87ff47c758 100644 --- a/boards/px4/fmu-v5/rtps.cmake +++ b/boards/px4/fmu-v5/rtps.cmake @@ -73,6 +73,7 @@ px4_add_board( logger mavlink mc_att_control + mc_rate_control mc_pos_control micrortps_bridge navigator diff --git a/boards/px4/fmu-v5/stackcheck.cmake b/boards/px4/fmu-v5/stackcheck.cmake index 69a602e2a4..a42f57288a 100644 --- a/boards/px4/fmu-v5/stackcheck.cmake +++ b/boards/px4/fmu-v5/stackcheck.cmake @@ -21,8 +21,8 @@ px4_add_board( adc barometer/ms5611 #batt_smbus - camera_capture - camera_trigger + #camera_capture + #camera_trigger differential_pressure # all available differential pressure drivers distance_sensor # all available distance sensor drivers gps @@ -42,6 +42,7 @@ px4_add_board( mkblctrl optical_flow # all available optical flow drivers #pca9685 + #power_monitor/ina226 #protocol_splitter pwm_input pwm_out_sim @@ -73,6 +74,7 @@ px4_add_board( logger mavlink mc_att_control + mc_rate_control mc_pos_control #micrortps_bridge navigator diff --git a/boards/px4/fmu-v5x/default.cmake b/boards/px4/fmu-v5x/default.cmake index ca7251d855..eddedd50d0 100644 --- a/boards/px4/fmu-v5x/default.cmake +++ b/boards/px4/fmu-v5x/default.cmake @@ -74,6 +74,7 @@ px4_add_board( logger mavlink mc_att_control + mc_rate_control mc_pos_control navigator rover_pos_control diff --git a/boards/px4/raspberrypi/cross.cmake b/boards/px4/raspberrypi/cross.cmake index 339da358c2..89b82e928d 100644 --- a/boards/px4/raspberrypi/cross.cmake +++ b/boards/px4/raspberrypi/cross.cmake @@ -41,6 +41,7 @@ px4_add_board( logger mavlink mc_att_control + mc_rate_control mc_pos_control navigator battery_status diff --git a/boards/px4/raspberrypi/native.cmake b/boards/px4/raspberrypi/native.cmake index 4c0bff6d7e..b1985ed1ab 100644 --- a/boards/px4/raspberrypi/native.cmake +++ b/boards/px4/raspberrypi/native.cmake @@ -39,6 +39,7 @@ px4_add_board( logger mavlink mc_att_control + mc_rate_control mc_pos_control navigator battery_status diff --git a/boards/px4/sitl/default.cmake b/boards/px4/sitl/default.cmake index 8cade4ed94..b5912ec74e 100644 --- a/boards/px4/sitl/default.cmake +++ b/boards/px4/sitl/default.cmake @@ -37,6 +37,7 @@ px4_add_board( logger mavlink mc_att_control + mc_rate_control mc_pos_control navigator replay diff --git a/boards/px4/sitl/rtps.cmake b/boards/px4/sitl/rtps.cmake index 5a676e40aa..389c76215b 100644 --- a/boards/px4/sitl/rtps.cmake +++ b/boards/px4/sitl/rtps.cmake @@ -37,6 +37,7 @@ px4_add_board( logger mavlink mc_att_control + mc_rate_control mc_pos_control micrortps_bridge navigator diff --git a/boards/px4/sitl/test.cmake b/boards/px4/sitl/test.cmake index 737717daf6..2c1570bfd8 100644 --- a/boards/px4/sitl/test.cmake +++ b/boards/px4/sitl/test.cmake @@ -37,6 +37,7 @@ px4_add_board( logger mavlink mc_att_control + mc_rate_control mc_pos_control navigator replay diff --git a/boards/uvify/core/default.cmake b/boards/uvify/core/default.cmake index b9de8876a8..6e36477534 100644 --- a/boards/uvify/core/default.cmake +++ b/boards/uvify/core/default.cmake @@ -55,6 +55,7 @@ px4_add_board( mavlink mc_att_control mc_pos_control + mc_rate_control navigator sensors sih diff --git a/platforms/qurt/src/px4/common/commands_hil.c b/platforms/qurt/src/px4/common/commands_hil.c index ebc7688244..c2aa5953c4 100644 --- a/platforms/qurt/src/px4/common/commands_hil.c +++ b/platforms/qurt/src/px4/common/commands_hil.c @@ -54,6 +54,7 @@ const char *get_commands() "ekf2 start\n" "mc_pos_control start\n" "mc_att_control start\n" + "mc_rate_control start\n" "sleep 1\n" "pwm_out_sim start\n" "param set RC1_MAX 2015\n" diff --git a/posix-configs/SITL/init/test/test_shutdown b/posix-configs/SITL/init/test/test_shutdown index 6c742afd07..91e15dd70e 100644 --- a/posix-configs/SITL/init/test/test_shutdown +++ b/posix-configs/SITL/init/test/test_shutdown @@ -27,6 +27,7 @@ ekf2 start vtol_att_control start mc_pos_control start mc_att_control start +mc_rate_control start fw_pos_control_l1 start fw_att_control start airspeed_selector start @@ -54,6 +55,7 @@ navigator status ekf2 status mc_pos_control status mc_att_control status +mc_rate_control status fw_pos_control_l1 status fw_att_control status airspeed_selector status @@ -64,6 +66,7 @@ uorb status echo "Stopping all modules" logger stop pwm_out_sim stop +mc_rate_control stop mc_att_control stop fw_att_control stop mc_pos_control stop diff --git a/posix-configs/bbblue/px4.config b/posix-configs/bbblue/px4.config index e40b2dcea3..f46cf8ae1f 100644 --- a/posix-configs/bbblue/px4.config +++ b/posix-configs/bbblue/px4.config @@ -22,10 +22,10 @@ param set SYS_AUTOSTART 4011 # Note that the setting here applies to all PWM channels. # param set PWM_MIN 1120 # param set PWM_MAX 1920 -# Not using DJI 430 LITE ESC anymore due to its hiccups: +# Not using DJI 430 LITE ESC anymore due to its hiccups: # each random motor stop would cause a scary flip in the fly # Replacing with 4 BLHeli32 (Wraith32 V2) ESCs solved the main problem in BBBlue porting - + # Broadcast heartbeats on local network. This allows a ground control station # to automatically find the drone on the local network. param set MAV_BROADCAST 1 @@ -70,10 +70,11 @@ land_detector start multicopter mc_pos_control start mc_att_control start +mc_rate_control start #fw_att_control start #fw_pos_control_l1 start -mavlink start -n SoftAp -x -u 14556 -r 1000000 +mavlink start -n SoftAp -x -u 14556 -r 1000000 # -n name of wifi interface: SoftAp, wlan or your custom interface, # e.g., -n SoftAp . The default is wlan diff --git a/posix-configs/bbblue/px4_fw.config b/posix-configs/bbblue/px4_fw.config index 414749688a..2cd8bfdbd1 100644 --- a/posix-configs/bbblue/px4_fw.config +++ b/posix-configs/bbblue/px4_fw.config @@ -25,7 +25,7 @@ param set SYS_AUTOSTART 4011 # Not using DJI 430 LITE ESC anymore due to its hiccups: # each random motor stop would cause a scary flip in the fly # Replacing with 4 BLHeli32 (Wraith32 V2) ESCs solved the main problem in BBBlue porting - + # Broadcast heartbeats on local network. This allows a ground control station # to automatically find the drone on the local network. param set MAV_BROADCAST 1 @@ -36,7 +36,7 @@ param set MAV_TYPE 2 # Three possible main power battery sources for BBBlue: # 1. onboard 2S LiPo battery connector, which is connect to ADC channel 6 # 2. onboard 9-18V DC Jack, which is connect to ADC channel 5. This is the board default. -# 3. other power source (e.g., LiPo battery more than 4S/18V). +# 3. other power source (e.g., LiPo battery more than 4S/18V). # Scale the voltage to below 1.8V and connect it to ADC channel # 0, 1, 2 or 3. param set BAT_ADC_CHANNEL 5 @@ -70,11 +70,12 @@ ekf2 start #mc_pos_control start #mc_att_control start +#mc_rate_control start fw_att_control start fw_pos_control_l1 start -mavlink start -n SoftAp -x -u 14556 -r 1000000 -# -n name of wifi interface: SoftAp, wlan or your custom interface, +mavlink start -n SoftAp -x -u 14556 -r 1000000 +# -n name of wifi interface: SoftAp, wlan or your custom interface, # e.g., -n wlan . The default on BBBlue is SoftAp sleep 1 diff --git a/posix-configs/bebop/px4.config b/posix-configs/bebop/px4.config index 8201f78e9a..7e856a21fa 100644 --- a/posix-configs/bebop/px4.config +++ b/posix-configs/bebop/px4.config @@ -51,6 +51,7 @@ navigator start land_detector start multicopter mc_pos_control start mc_att_control start +mc_rate_control start sleep 1 mavlink start -x -u 14556 -r 20000 sleep 1 diff --git a/posix-configs/eagle/200qx/px4.config b/posix-configs/eagle/200qx/px4.config index 48286eec03..36461a02b6 100644 --- a/posix-configs/eagle/200qx/px4.config +++ b/posix-configs/eagle/200qx/px4.config @@ -33,6 +33,7 @@ ekf2 start land_detector start multicopter mc_pos_control start mc_att_control start +mc_rate_control start uart_esc start -D /dev/tty-2 spektrum_rc start -d /dev/tty-1 sleep 1 diff --git a/posix-configs/eagle/210qc/px4.config b/posix-configs/eagle/210qc/px4.config index 48286eec03..36461a02b6 100644 --- a/posix-configs/eagle/210qc/px4.config +++ b/posix-configs/eagle/210qc/px4.config @@ -33,6 +33,7 @@ ekf2 start land_detector start multicopter mc_pos_control start mc_att_control start +mc_rate_control start uart_esc start -D /dev/tty-2 spektrum_rc start -d /dev/tty-1 sleep 1 diff --git a/posix-configs/eagle/flight/px4.config b/posix-configs/eagle/flight/px4.config index ca7624b986..e26b326766 100644 --- a/posix-configs/eagle/flight/px4.config +++ b/posix-configs/eagle/flight/px4.config @@ -18,5 +18,6 @@ ekf2 start land_detector start multicopter mc_pos_control start mc_att_control start +mc_rate_control start snapdragon_pwm_out start spektrum_rc start diff --git a/posix-configs/eagle/hil/px4.config b/posix-configs/eagle/hil/px4.config index 3adb2664b9..14e9b4d443 100644 --- a/posix-configs/eagle/hil/px4.config +++ b/posix-configs/eagle/hil/px4.config @@ -43,6 +43,7 @@ sensors start ekf2 start mc_pos_control start mc_att_control start +mc_rate_control start land_detector start multicopter sleep 1 pwm_out_sim start diff --git a/posix-configs/eagle/init/rcS b/posix-configs/eagle/init/rcS index 220712416f..f419407283 100644 --- a/posix-configs/eagle/init/rcS +++ b/posix-configs/eagle/init/rcS @@ -197,6 +197,7 @@ qshell commander start qshell land_detector start multicopter qshell mc_pos_control start qshell mc_att_control start +qshell mc_rate_control start logger start -b 200 -t diff --git a/posix-configs/excelsior/px4.config b/posix-configs/excelsior/px4.config index bb00bd1997..fb8cef21ad 100644 --- a/posix-configs/excelsior/px4.config +++ b/posix-configs/excelsior/px4.config @@ -83,6 +83,7 @@ ekf2 start land_detector start multicopter mc_pos_control start mc_att_control start +mc_rate_control start uart_esc start -D /dev/tty-1 spektrum_rc start -d /dev/tty-101 sleep 1 diff --git a/posix-configs/ocpoc/px4.config b/posix-configs/ocpoc/px4.config index c843c468a7..fcc6ff09ef 100644 --- a/posix-configs/ocpoc/px4.config +++ b/posix-configs/ocpoc/px4.config @@ -29,6 +29,7 @@ ekf2 start land_detector start multicopter mc_pos_control start mc_att_control start +mc_rate_control start mavlink start -x -d /dev/ttyPS1 mavlink stream -d /dev/ttyPS1 -s HIGHRES_IMU -r 50 diff --git a/posix-configs/rpi/px4.config b/posix-configs/rpi/px4.config index d189b292f0..c15ea63e40 100644 --- a/posix-configs/rpi/px4.config +++ b/posix-configs/rpi/px4.config @@ -31,6 +31,7 @@ ekf2 start land_detector start multicopter mc_pos_control start mc_att_control start +mc_rate_control start mavlink start -x -u 14556 -r 1000000 mavlink stream -u 14556 -s HIGHRES_IMU -r 50 mavlink stream -u 14556 -s ATTITUDE -r 50 diff --git a/posix-configs/rpi/px4_hil.config b/posix-configs/rpi/px4_hil.config index 1a0a21e70d..6d1b7def9f 100644 --- a/posix-configs/rpi/px4_hil.config +++ b/posix-configs/rpi/px4_hil.config @@ -20,6 +20,7 @@ ekf2 start land_detector start multicopter mc_pos_control start mc_att_control start +mc_rate_control start mavlink start -x -u 14577 -r 1000000 navio_sysfs_rc_in start pwm_out_sim start diff --git a/posix-configs/rpi/px4_no_shield.config b/posix-configs/rpi/px4_no_shield.config index 2afb474c37..de57d45b1c 100644 --- a/posix-configs/rpi/px4_no_shield.config +++ b/posix-configs/rpi/px4_no_shield.config @@ -23,6 +23,7 @@ ekf2 start land_detector start multicopter mc_pos_control start mc_att_control start +mc_rate_control start mavlink start -x -u 14556 -r 1000000 sleep 1 mavlink stream -u 14556 -s HIGHRES_IMU -r 50 diff --git a/src/examples/hwtest/hwtest.c b/src/examples/hwtest/hwtest.c index 6c7da863db..5972a2b64d 100644 --- a/src/examples/hwtest/hwtest.c +++ b/src/examples/hwtest/hwtest.c @@ -54,7 +54,7 @@ int ex_hwtest_main(int argc, char *argv[]) { warnx("DO NOT FORGET TO STOP THE DEFAULT CONTROL APPS!"); warnx("(run ,)"); - warnx("( and)"); + warnx("( and)"); warnx("( to do so)"); warnx("usage: http://px4.io/dev/examples/write_output"); diff --git a/src/modules/mc_att_control/CMakeLists.txt b/src/modules/mc_att_control/CMakeLists.txt index ffd12a2450..89952573e4 100644 --- a/src/modules/mc_att_control/CMakeLists.txt +++ b/src/modules/mc_att_control/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# Copyright (c) 2015-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 @@ -32,20 +32,15 @@ ############################################################################ add_subdirectory(AttitudeControl) -add_subdirectory(RateControl) px4_add_module( MODULE modules__mc_att_control MAIN mc_att_control - STACK_MAX 3500 COMPILE_FLAGS SRCS mc_att_control_main.cpp DEPENDS - circuit_breaker - conversion mathlib AttitudeControl - RateControl px4_work_queue ) diff --git a/src/modules/mc_att_control/mc_att_control.hpp b/src/modules/mc_att_control/mc_att_control.hpp index 5d6770b335..69bfa5654c 100644 --- a/src/modules/mc_att_control/mc_att_control.hpp +++ b/src/modules/mc_att_control/mc_att_control.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-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 @@ -31,6 +31,8 @@ * ****************************************************************************/ +#pragma once + #include #include #include @@ -41,27 +43,19 @@ #include #include #include -#include #include #include -#include -#include #include -#include #include -#include -#include #include #include #include #include #include #include -#include #include #include -#include /** * Multicopter attitude control app start / stop handling function @@ -99,17 +93,9 @@ private: */ void parameters_updated(); - /** - * Check for parameter update and handle it. - */ - void parameter_update_poll(); - bool vehicle_attitude_poll(); - void vehicle_motor_limits_poll(); void vehicle_status_poll(); - void publish_actuator_controls(); void publish_rates_setpoint(); - void publish_rate_controller_status(); float throttle_curve(float throttle_stick_input); @@ -118,130 +104,60 @@ private: */ void generate_attitude_setpoint(float dt, bool reset_yaw_sp); - /** - * Get the landing gear state based on the manual control switch position - * @return vehicle_attitude_setpoint_s::LANDING_GEAR_UP or vehicle_attitude_setpoint_s::LANDING_GEAR_DOWN - */ - float get_landing_gear_state(); - - /** * Attitude controller. */ void control_attitude(); - /** - * Attitude rates controller. - */ - void control_attitude_rates(float dt, const matrix::Vector3f &rates); - AttitudeControl _attitude_control; ///< class for attitude control calculations - RateControl _rate_control; ///< class for rate control calculations - uORB::Subscription _v_att_sub{ORB_ID(vehicle_attitude)}; /**< vehicle attitude subscription */ uORB::Subscription _v_att_sp_sub{ORB_ID(vehicle_attitude_setpoint)}; /**< vehicle attitude setpoint subscription */ uORB::Subscription _v_rates_sp_sub{ORB_ID(vehicle_rates_setpoint)}; /**< vehicle rates setpoint subscription */ uORB::Subscription _v_control_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle control mode subscription */ - uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; /**< parameter updates subscription */ + uORB::Subscription _params_sub{ORB_ID(parameter_update)}; /**< parameter updates subscription */ uORB::Subscription _manual_control_sp_sub{ORB_ID(manual_control_setpoint)}; /**< manual control setpoint subscription */ uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */ - uORB::Subscription _motor_limits_sub{ORB_ID(multirotor_motor_limits)}; /**< motor limits subscription */ - uORB::Subscription _battery_status_sub{ORB_ID(battery_status)}; /**< battery status subscription */ uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */ - uORB::Subscription _landing_gear_sub{ORB_ID(landing_gear)}; - uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)}; + uORB::SubscriptionCallbackWorkItem _vehicle_attitude_sub{this, ORB_ID(vehicle_attitude)}; - uORB::PublicationMulti _controller_status_pub{ORB_ID(rate_ctrl_status), ORB_PRIO_DEFAULT}; /**< 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 */ - orb_advert_t _actuators_0_pub{nullptr}; /**< attitude actuator controls publication */ orb_advert_t _vehicle_attitude_setpoint_pub{nullptr}; - orb_id_t _actuators_id{nullptr}; /**< pointer to correct actuator controls0 uORB metadata structure */ orb_id_t _attitude_sp_id{nullptr}; /**< pointer to correct attitude setpoint uORB metadata structure */ - bool _actuators_0_circuit_breaker_enabled{false}; /**< circuit breaker to suppress output */ - struct vehicle_attitude_s _v_att {}; /**< vehicle attitude */ struct vehicle_attitude_setpoint_s _v_att_sp {}; /**< vehicle attitude setpoint */ struct vehicle_rates_setpoint_s _v_rates_sp {}; /**< vehicle rates setpoint */ struct manual_control_setpoint_s _manual_control_sp {}; /**< manual control setpoint */ struct vehicle_control_mode_s _v_control_mode {}; /**< vehicle control mode */ - struct actuator_controls_s _actuators {}; /**< actuator controls */ struct vehicle_status_s _vehicle_status {}; /**< vehicle status */ - struct battery_status_s _battery_status {}; /**< battery status */ struct vehicle_land_detected_s _vehicle_land_detected {}; - struct landing_gear_s _landing_gear {}; - MultirotorMixer::saturation_status _saturation_status{}; - - perf_counter_t _loop_perf; /**< loop performance counter */ - - static constexpr const float initial_update_rate_hz = 250.f; /**< loop update rate used for initialization */ - float _loop_update_rate_hz{initial_update_rate_hz}; /**< current rate-controller loop update rate in [Hz] */ + perf_counter_t _loop_perf; /**< loop duration performance counter */ matrix::Vector3f _rates_sp; /**< angular rates setpoint */ - matrix::Vector3f _att_control; /**< attitude control vector */ - float _thrust_sp{0.0f}; /**< thrust setpoint */ - float _man_yaw_sp{0.f}; /**< current yaw setpoint in manual mode */ - bool _gear_state_initialized{false}; /**< true if the gear state has been initialized */ - hrt_abstime _task_start{hrt_absolute_time()}; hrt_abstime _last_run{0}; - float _dt_accumulator{0.0f}; - int _loop_counter{0}; bool _reset_yaw_sp{true}; - float _attitude_dt{0.0f}; DEFINE_PARAMETERS( (ParamFloat) _param_mc_roll_p, - (ParamFloat) _param_mc_rollrate_p, - (ParamFloat) _param_mc_rollrate_i, - (ParamFloat) _param_mc_rr_int_lim, - (ParamFloat) _param_mc_rollrate_d, - (ParamFloat) _param_mc_rollrate_ff, - (ParamFloat) _param_mc_rollrate_k, - (ParamFloat) _param_mc_pitch_p, - (ParamFloat) _param_mc_pitchrate_p, - (ParamFloat) _param_mc_pitchrate_i, - (ParamFloat) _param_mc_pr_int_lim, - (ParamFloat) _param_mc_pitchrate_d, - (ParamFloat) _param_mc_pitchrate_ff, - (ParamFloat) _param_mc_pitchrate_k, - (ParamFloat) _param_mc_yaw_p, - (ParamFloat) _param_mc_yawrate_p, - (ParamFloat) _param_mc_yawrate_i, - (ParamFloat) _param_mc_yr_int_lim, - (ParamFloat) _param_mc_yawrate_d, - (ParamFloat) _param_mc_yawrate_ff, - (ParamFloat) _param_mc_yawrate_k, - - (ParamFloat) _param_mc_dterm_cutoff, /**< Cutoff frequency for the D-term filter */ (ParamFloat) _param_mc_rollrate_max, (ParamFloat) _param_mc_pitchrate_max, (ParamFloat) _param_mc_yawrate_max, + (ParamFloat) _param_mpc_man_y_max, /**< scaling factor from stick to yaw rate */ - (ParamFloat) _param_mc_acro_r_max, - (ParamFloat) _param_mc_acro_p_max, - (ParamFloat) _param_mc_acro_y_max, - (ParamFloat) _param_mc_acro_expo, /**< expo stick curve shape (roll & pitch) */ - (ParamFloat) _param_mc_acro_expo_y, /**< expo stick curve shape (yaw) */ - (ParamFloat) _param_mc_acro_supexpo, /**< superexpo stick curve shape (roll & pitch) */ - (ParamFloat) _param_mc_acro_supexpoy, /**< superexpo stick curve shape (yaw) */ - (ParamFloat) _param_mc_ratt_th, - (ParamBool) _param_mc_bat_scale_en, - /* Stabilized mode params */ (ParamFloat) _param_mpc_man_tilt_max, /**< maximum tilt allowed for manual flight */ (ParamFloat) _param_mpc_manthr_min, /**< minimum throttle for stabilized */ @@ -250,15 +166,11 @@ private: _param_mpc_thr_hover, /**< throttle at which vehicle is at hover equilibrium */ (ParamInt) _param_mpc_thr_curve, /**< throttle curve behavior */ - (ParamInt) _param_mc_airmode, - - (ParamInt) _param_cbrk_rate_ctrl - + (ParamInt) _param_mc_airmode ) bool _is_tailsitter{false}; - matrix::Vector3f _acro_rate_max; /**< max attitude rates in acro mode */ float _man_tilt_max; /**< maximum tilt allowed for manual flight [rad] */ }; 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 c4e6c6dc96..c6f03ea7b5 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2018 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 @@ -45,10 +45,7 @@ #include "mc_att_control.hpp" -#include #include -#include -#include #include #include @@ -56,8 +53,8 @@ using namespace matrix; MulticopterAttitudeControl::MulticopterAttitudeControl() : ModuleParams(nullptr), - WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl), - _loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")) + WorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl), + _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")) { _vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; @@ -65,10 +62,6 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _v_att.q[0] = 1.f; _v_att_sp.q_d[0] = 1.f; - _rates_sp.zero(); - _thrust_sp = 0.0f; - _att_control.zero(); - parameters_updated(); } @@ -80,8 +73,8 @@ MulticopterAttitudeControl::~MulticopterAttitudeControl() bool MulticopterAttitudeControl::init() { - if (!_vehicle_angular_velocity_sub.registerCallback()) { - PX4_ERR("vehicle_angular_velocity callback registration failed!"); + if (!_vehicle_attitude_sub.registerCallback()) { + PX4_ERR("vehicle_attitude callback registration failed!"); return false; } @@ -94,47 +87,12 @@ MulticopterAttitudeControl::parameters_updated() // Store some of the parameters in a more convenient way & precompute often-used values _attitude_control.setProportionalGain(Vector3f(_param_mc_roll_p.get(), _param_mc_pitch_p.get(), _param_mc_yaw_p.get())); - // rate control parameters - // The controller gain K is used to convert the parallel (P + I/s + sD) form - // to the ideal (K * [1 + 1/sTi + sTd]) form - Vector3f rate_k = Vector3f(_param_mc_rollrate_k.get(), _param_mc_pitchrate_k.get(), _param_mc_yawrate_k.get()); - _rate_control.setGains( - rate_k.emult(Vector3f(_param_mc_rollrate_p.get(), _param_mc_pitchrate_p.get(), _param_mc_yawrate_p.get())), - rate_k.emult(Vector3f(_param_mc_rollrate_i.get(), _param_mc_pitchrate_i.get(), _param_mc_yawrate_i.get())), - rate_k.emult(Vector3f(_param_mc_rollrate_d.get(), _param_mc_pitchrate_d.get(), _param_mc_yawrate_d.get()))); - _rate_control.setIntegratorLimit( - Vector3f(_param_mc_rr_int_lim.get(), _param_mc_pr_int_lim.get(), _param_mc_yr_int_lim.get())); - _rate_control.setDTermCutoff(_loop_update_rate_hz, _param_mc_dterm_cutoff.get(), false); - _rate_control.setFeedForwardGain( - Vector3f(_param_mc_rollrate_ff.get(), _param_mc_pitchrate_ff.get(), _param_mc_yawrate_ff.get())); - // angular rate limits using math::radians; _attitude_control.setRateLimit(Vector3f(radians(_param_mc_rollrate_max.get()), radians(_param_mc_pitchrate_max.get()), radians(_param_mc_yawrate_max.get()))); - // manual rate control acro mode rate limits - _acro_rate_max = Vector3f(radians(_param_mc_acro_r_max.get()), radians(_param_mc_acro_p_max.get()), - radians(_param_mc_acro_y_max.get())); - _man_tilt_max = math::radians(_param_mpc_man_tilt_max.get()); - - _actuators_0_circuit_breaker_enabled = circuit_breaker_enabled_by_val(_param_cbrk_rate_ctrl.get(), CBRK_RATE_CTRL_KEY); -} - -void -MulticopterAttitudeControl::parameter_update_poll() -{ - // check for parameter updates - if (_parameter_update_sub.updated()) { - // clear update - parameter_update_s pupdate; - _parameter_update_sub.copy(&pupdate); - - // update parameters from storage - updateParams(); - parameters_updated(); - } } void @@ -143,9 +101,8 @@ MulticopterAttitudeControl::vehicle_status_poll() /* check if there is new status information */ if (_vehicle_status_sub.update(&_vehicle_status)) { /* set correct uORB ID, depending on if vehicle is VTOL or not */ - if (_actuators_id == nullptr) { + if (_attitude_sp_id == nullptr) { if (_vehicle_status.is_vtol) { - _actuators_id = ORB_ID(actuator_controls_virtual_mc); _attitude_sp_id = ORB_ID(mc_virtual_attitude_setpoint); int32_t vt_type = -1; @@ -155,43 +112,12 @@ MulticopterAttitudeControl::vehicle_status_poll() } } else { - _actuators_id = ORB_ID(actuator_controls_0); _attitude_sp_id = ORB_ID(vehicle_attitude_setpoint); } } } } -void -MulticopterAttitudeControl::vehicle_motor_limits_poll() -{ - /* check if there is a new message */ - multirotor_motor_limits_s motor_limits{}; - - if (_motor_limits_sub.update(&motor_limits)) { - _saturation_status.value = motor_limits.saturation_status; - } -} - -bool -MulticopterAttitudeControl::vehicle_attitude_poll() -{ - /* check if there is a new message */ - const uint8_t prev_quat_reset_counter = _v_att.quat_reset_counter; - - if (_v_att_sub.update(&_v_att)) { - // Check for a heading reset - if (prev_quat_reset_counter != _v_att.quat_reset_counter) { - // we only extract the heading change from the delta quaternion - _man_yaw_sp += Eulerf(Quatf(_v_att.delta_q_reset)).psi(); - } - - return true; - } - - return false; -} - float MulticopterAttitudeControl::throttle_curve(float throttle_stick_input) { @@ -214,30 +140,6 @@ MulticopterAttitudeControl::throttle_curve(float throttle_stick_input) } } -float -MulticopterAttitudeControl::get_landing_gear_state() -{ - // Only switch the landing gear up if we are not landed and if - // the user switched from gear down to gear up. - // If the user had the switch in the gear up position and took off ignore it - // until he toggles the switch to avoid retracting the gear immediately on takeoff. - if (_vehicle_land_detected.landed) { - _gear_state_initialized = false; - } - - float landing_gear = landing_gear_s::GEAR_DOWN; // default to down - - if (_manual_control_sp.gear_switch == manual_control_setpoint_s::SWITCH_POS_ON && _gear_state_initialized) { - landing_gear = landing_gear_s::GEAR_UP; - - } else if (_manual_control_sp.gear_switch == manual_control_setpoint_s::SWITCH_POS_OFF) { - // Switching the gear off does put it into a safe defined state - _gear_state_initialized = true; - } - - return landing_gear; -} - void MulticopterAttitudeControl::generate_attitude_setpoint(float dt, bool reset_yaw_sp) { @@ -334,16 +236,12 @@ MulticopterAttitudeControl::generate_attitude_setpoint(float dt, bool reset_yaw_ if (_attitude_sp_id != nullptr) { orb_publish_auto(_attitude_sp_id, &_vehicle_attitude_setpoint_pub, &attitude_setpoint, nullptr, ORB_PRIO_DEFAULT); } - - _landing_gear.landing_gear = get_landing_gear_state(); - _landing_gear.timestamp = hrt_absolute_time(); - _landing_gear_pub.publish(_landing_gear); } /** * Attitude controller. * Input: 'vehicle_attitude_setpoint' topics (depending on mode) - * Output: '_rates_sp' vector, '_thrust_sp' + * Output: '_rates_sp' vector */ void MulticopterAttitudeControl::control_attitude() @@ -356,124 +254,72 @@ MulticopterAttitudeControl::control_attitude() Vector3f().copyTo(_v_att_sp.thrust_body); } - // physical thrust axis is the negative of body z axis - _thrust_sp = -_v_att_sp.thrust_body[2]; - _rates_sp = _attitude_control.update(Quatf(_v_att.q), Quatf(_v_att_sp.q_d), _v_att_sp.yaw_sp_move_rate); } -/* - * Attitude rates controller. - * Input: '_rates_sp' vector, '_thrust_sp' - * Output: '_att_control' vector - */ -void -MulticopterAttitudeControl::control_attitude_rates(float dt, const Vector3f &rates) -{ - // reset integral if disarmed - if (!_v_control_mode.flag_armed || _vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - _rate_control.resetIntegral(); - } - - const bool landed = _vehicle_land_detected.maybe_landed || _vehicle_land_detected.landed; - _rate_control.setSaturationStatus(_saturation_status); - _att_control = _rate_control.update(rates, _rates_sp, dt, landed); -} - void MulticopterAttitudeControl::publish_rates_setpoint() { - _v_rates_sp.roll = _rates_sp(0); - _v_rates_sp.pitch = _rates_sp(1); - _v_rates_sp.yaw = _rates_sp(2); - _v_rates_sp.thrust_body[0] = 0.0f; - _v_rates_sp.thrust_body[1] = 0.0f; - _v_rates_sp.thrust_body[2] = -_thrust_sp; - _v_rates_sp.timestamp = hrt_absolute_time(); + vehicle_rates_setpoint_s v_rates_sp{}; - _v_rates_sp_pub.publish(_v_rates_sp); -} + v_rates_sp.roll = _rates_sp(0); + v_rates_sp.pitch = _rates_sp(1); + v_rates_sp.yaw = _rates_sp(2); + v_rates_sp.thrust_body[0] = _v_att_sp.thrust_body[0]; + v_rates_sp.thrust_body[1] = _v_att_sp.thrust_body[1]; + v_rates_sp.thrust_body[2] = _v_att_sp.thrust_body[2]; + v_rates_sp.timestamp = hrt_absolute_time(); -void -MulticopterAttitudeControl::publish_rate_controller_status() -{ - rate_ctrl_status_s rate_ctrl_status = {}; - rate_ctrl_status.timestamp = hrt_absolute_time(); - _rate_control.getRateControlStatus(rate_ctrl_status); - _controller_status_pub.publish(rate_ctrl_status); -} - -void -MulticopterAttitudeControl::publish_actuator_controls() -{ - _actuators.control[0] = (PX4_ISFINITE(_att_control(0))) ? _att_control(0) : 0.0f; - _actuators.control[1] = (PX4_ISFINITE(_att_control(1))) ? _att_control(1) : 0.0f; - _actuators.control[2] = (PX4_ISFINITE(_att_control(2))) ? _att_control(2) : 0.0f; - _actuators.control[3] = (PX4_ISFINITE(_thrust_sp)) ? _thrust_sp : 0.0f; - _actuators.control[7] = (float)_landing_gear.landing_gear; - // note: _actuators.timestamp_sample is set in MulticopterAttitudeControl::Run() - _actuators.timestamp = hrt_absolute_time(); - - /* scale effort by battery status */ - if (_param_mc_bat_scale_en.get() && _battery_status.scale > 0.0f) { - for (int i = 0; i < 4; i++) { - _actuators.control[i] *= _battery_status.scale; - } - } - - if (!_actuators_0_circuit_breaker_enabled) { - orb_publish_auto(_actuators_id, &_actuators_0_pub, &_actuators, nullptr, ORB_PRIO_DEFAULT); - } + _v_rates_sp_pub.publish(v_rates_sp); } void MulticopterAttitudeControl::Run() { if (should_exit()) { - _vehicle_angular_velocity_sub.unregisterCallback(); + _vehicle_attitude_sub.unregisterCallback(); exit_and_cleanup(); return; } perf_begin(_loop_perf); - /* run controller on gyro changes */ - vehicle_angular_velocity_s angular_velocity; + // Check if parameters have changed + if (_params_sub.updated()) { + // clear update + parameter_update_s param_update; + _params_sub.copy(¶m_update); + + updateParams(); + parameters_updated(); + } + + // run controller on attitude updates + const uint8_t prev_quat_reset_counter = _v_att.quat_reset_counter; + + if (_vehicle_attitude_sub.update(&_v_att)) { + + // Check for a heading reset + if (prev_quat_reset_counter != _v_att.quat_reset_counter) { + // we only extract the heading change from the delta quaternion + _man_yaw_sp += Eulerf(Quatf(_v_att.delta_q_reset)).psi(); + } - if (_vehicle_angular_velocity_sub.update(&angular_velocity)) { const hrt_abstime now = hrt_absolute_time(); // Guard against too small (< 0.2ms) and too large (> 20ms) dt's. const float dt = math::constrain(((now - _last_run) / 1e6f), 0.0002f, 0.02f); _last_run = now; - const Vector3f rates{angular_velocity.xyz}; - - _actuators.timestamp_sample = angular_velocity.timestamp_sample; - - /* run the rate controller immediately after a gyro update */ - if (_v_control_mode.flag_control_rates_enabled) { - control_attitude_rates(dt, rates); - - publish_actuator_controls(); - publish_rate_controller_status(); - } - /* check for updates in other topics */ + _manual_control_sp_sub.update(&_manual_control_sp); _v_control_mode_sub.update(&_v_control_mode); - _battery_status_sub.update(&_battery_status); _vehicle_land_detected_sub.update(&_vehicle_land_detected); - _landing_gear_sub.update(&_landing_gear); vehicle_status_poll(); - vehicle_motor_limits_poll(); - const bool manual_control_updated = _manual_control_sp_sub.update(&_manual_control_sp); - const bool attitude_updated = vehicle_attitude_poll(); - - _attitude_dt += dt; /* Check if we are in rattitude mode and the pilot is above the threshold on pitch - * or roll (yaw can rotate 360 in normal att control). If both are true don't - * even bother running the attitude controllers */ + * or roll (yaw can rotate 360 in normal att control). If both are true don't + * even bother running the attitude controllers */ if (_v_control_mode.flag_control_rattitude_enabled) { _v_control_mode.flag_control_attitude_enabled = fabsf(_manual_control_sp.y) <= _param_mc_ratt_th.get() && @@ -490,90 +336,35 @@ MulticopterAttitudeControl::Run() bool run_att_ctrl = _v_control_mode.flag_control_attitude_enabled && (is_hovering || is_tailsitter_transition); - if (run_att_ctrl) { - if (attitude_updated) { - // Generate the attitude setpoint from stick inputs if we are in Manual/Stabilized mode - if (_v_control_mode.flag_control_manual_enabled && - !_v_control_mode.flag_control_altitude_enabled && - !_v_control_mode.flag_control_velocity_enabled && - !_v_control_mode.flag_control_position_enabled) { - generate_attitude_setpoint(_attitude_dt, _reset_yaw_sp); - attitude_setpoint_generated = true; - } + // Generate the attitude setpoint from stick inputs if we are in Manual/Stabilized mode + if (_v_control_mode.flag_control_manual_enabled && + !_v_control_mode.flag_control_altitude_enabled && + !_v_control_mode.flag_control_velocity_enabled && + !_v_control_mode.flag_control_position_enabled) { - control_attitude(); - - if (_v_control_mode.flag_control_yawrate_override_enabled) { - /* Yaw rate override enabled, overwrite the yaw setpoint */ - _v_rates_sp_sub.update(&_v_rates_sp); - const auto yawrate_reference = _v_rates_sp.yaw; - _rates_sp(2) = yawrate_reference; - } - - publish_rates_setpoint(); + generate_attitude_setpoint(dt, _reset_yaw_sp); + attitude_setpoint_generated = true; } - } else { - /* attitude controller disabled, poll rates setpoint topic */ - if (_v_control_mode.flag_control_manual_enabled && is_hovering) { - if (manual_control_updated) { - /* manual rates control - ACRO mode */ - Vector3f man_rate_sp( - math::superexpo(_manual_control_sp.y, _param_mc_acro_expo.get(), _param_mc_acro_supexpo.get()), - math::superexpo(-_manual_control_sp.x, _param_mc_acro_expo.get(), _param_mc_acro_supexpo.get()), - math::superexpo(_manual_control_sp.r, _param_mc_acro_expo_y.get(), _param_mc_acro_supexpoy.get())); - _rates_sp = man_rate_sp.emult(_acro_rate_max); - _thrust_sp = _manual_control_sp.z; - publish_rates_setpoint(); - } + control_attitude(); - } else { - /* attitude controller disabled, poll rates setpoint topic */ - if (_v_rates_sp_sub.update(&_v_rates_sp)) { - _rates_sp(0) = _v_rates_sp.roll; - _rates_sp(1) = _v_rates_sp.pitch; - _rates_sp(2) = _v_rates_sp.yaw; - _thrust_sp = -_v_rates_sp.thrust_body[2]; - } + if (_v_control_mode.flag_control_yawrate_override_enabled) { + /* Yaw rate override enabled, overwrite the yaw setpoint */ + _v_rates_sp_sub.update(&_v_rates_sp); + const auto yawrate_reference = _v_rates_sp.yaw; + _rates_sp(2) = yawrate_reference; } + + publish_rates_setpoint(); } - if (_v_control_mode.flag_control_termination_enabled) { - if (!_vehicle_status.is_vtol) { - _rates_sp.zero(); - _rate_control.resetIntegral(); - _thrust_sp = 0.0f; - _att_control.zero(); - publish_actuator_controls(); - } - } + // reset yaw setpoint during transitions, tailsitter.cpp generates + // attitude setpoint for the transition + _reset_yaw_sp = (!attitude_setpoint_generated && !_v_control_mode.flag_control_rattitude_enabled) || + _vehicle_land_detected.landed || + (_vehicle_status.is_vtol && _vehicle_status.in_transition_mode); - if (attitude_updated) { - // reset yaw setpoint during transitions, tailsitter.cpp generates - // attitude setpoint for the transition - _reset_yaw_sp = (!attitude_setpoint_generated && !_v_control_mode.flag_control_rattitude_enabled) || - _vehicle_land_detected.landed || - (_vehicle_status.is_vtol && _vehicle_status.in_transition_mode); - - _attitude_dt = 0.f; - } - - /* calculate loop update rate while disarmed or at least a few times (updating the filter is expensive) */ - if (!_v_control_mode.flag_armed || (now - _task_start) < 3300000) { - _dt_accumulator += dt; - ++_loop_counter; - - if (_dt_accumulator > 1.f) { - const float loop_update_rate = (float)_loop_counter / _dt_accumulator; - _loop_update_rate_hz = _loop_update_rate_hz * 0.5f + loop_update_rate * 0.5f; - _dt_accumulator = 0; - _loop_counter = 0; - _rate_control.setDTermCutoff(_loop_update_rate_hz, _param_mc_dterm_cutoff.get(), true); - } - } - - parameter_update_poll(); } perf_end(_loop_perf); @@ -608,8 +399,6 @@ int MulticopterAttitudeControl::print_status() perf_print_counter(_loop_perf); - print_message(_actuators); - return 0; } @@ -627,11 +416,10 @@ int MulticopterAttitudeControl::print_usage(const char *reason) PRINT_MODULE_DESCRIPTION( R"DESCR_STR( ### Description -This implements the multicopter attitude and rate controller. It takes attitude -setpoints (`vehicle_attitude_setpoint`) or rate setpoints (in acro mode -via `manual_control_setpoint` topic) as inputs and outputs actuator control messages. +This implements the multicopter attitude controller. It takes attitude +setpoints (`vehicle_attitude_setpoint`) as inputs and outputs a rate setpoint. -The controller has two loops: a P loop for angular error and a PID loop for angular rate error. +The controller has a P loop for angular error Publication documenting the implemented Quaternion Attitude Control: Nonlinear Quadrocopter Attitude Control (2013) @@ -640,9 +428,6 @@ Institute for Dynamic Systems and Control (IDSC), ETH Zurich https://www.research-collection.ethz.ch/bitstream/handle/20.500.11850/154099/eth-7387-01.pdf -### Implementation -To reduce control latency, the module directly polls on the gyro topic published by the IMU driver. - )DESCR_STR"); PRINT_MODULE_USAGE_NAME("mc_att_control", "controller"); 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 2d0a2d7669..ce18646183 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -53,87 +53,6 @@ */ PARAM_DEFINE_FLOAT(MC_ROLL_P, 6.5f); -/** - * Roll rate P gain - * - * Roll rate proportional gain, i.e. control output for angular speed error 1 rad/s. - * - * @min 0.0 - * @max 0.5 - * @decimal 3 - * @increment 0.01 - * @group Multicopter Attitude Control - */ -PARAM_DEFINE_FLOAT(MC_ROLLRATE_P, 0.15f); - -/** - * Roll rate I gain - * - * Roll rate integral gain. Can be set to compensate static thrust difference or gravity center offset. - * - * @min 0.0 - * @decimal 3 - * @increment 0.01 - * @group Multicopter Attitude Control - */ -PARAM_DEFINE_FLOAT(MC_ROLLRATE_I, 0.2f); - -/** - * Roll rate integrator limit - * - * Roll rate integrator limit. Can be set to increase the amount of integrator available to counteract disturbances or reduced to improve settling time after large roll moment trim changes. - * - * @min 0.0 - * @decimal 2 - * @increment 0.01 - * @group Multicopter Attitude Control - */ -PARAM_DEFINE_FLOAT(MC_RR_INT_LIM, 0.30f); - -/** - * Roll rate D gain - * - * Roll rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again. - * - * @min 0.0 - * @max 0.01 - * @decimal 4 - * @increment 0.0005 - * @group Multicopter Attitude Control - */ -PARAM_DEFINE_FLOAT(MC_ROLLRATE_D, 0.003f); - -/** - * Roll rate feedforward - * - * Improves tracking performance. - * - * @min 0.0 - * @decimal 4 - * @group Multicopter Attitude Control - */ -PARAM_DEFINE_FLOAT(MC_ROLLRATE_FF, 0.0f); - -/** - * Roll rate controller gain - * - * Global gain of the controller. - * - * This gain scales the P, I and D terms of the controller: - * output = MC_ROLLRATE_K * (MC_ROLLRATE_P * error - * + MC_ROLLRATE_I * error_integral - * + MC_ROLLRATE_D * error_derivative) - * Set MC_ROLLRATE_P=1 to implement a PID in the ideal form. - * Set MC_ROLLRATE_K=1 to implement a PID in the parallel form. - * - * @min 0.0 - * @max 5.0 - * @decimal 4 - * @increment 0.0005 - * @group Multicopter Attitude Control - */ -PARAM_DEFINE_FLOAT(MC_ROLLRATE_K, 1.0f); - /** * Pitch P gain * @@ -148,86 +67,6 @@ PARAM_DEFINE_FLOAT(MC_ROLLRATE_K, 1.0f); */ PARAM_DEFINE_FLOAT(MC_PITCH_P, 6.5f); -/** - * Pitch rate P gain - * - * Pitch rate proportional gain, i.e. control output for angular speed error 1 rad/s. - * - * @min 0.0 - * @max 0.6 - * @decimal 3 - * @increment 0.01 - * @group Multicopter Attitude Control - */ -PARAM_DEFINE_FLOAT(MC_PITCHRATE_P, 0.15f); - -/** - * Pitch rate I gain - * - * Pitch rate integral gain. Can be set to compensate static thrust difference or gravity center offset. - * - * @min 0.0 - * @decimal 3 - * @increment 0.01 - * @group Multicopter Attitude Control - */ -PARAM_DEFINE_FLOAT(MC_PITCHRATE_I, 0.2f); - -/** - * Pitch rate integrator limit - * - * Pitch rate integrator limit. Can be set to increase the amount of integrator available to counteract disturbances or reduced to improve settling time after large pitch moment trim changes. - * - * @min 0.0 - * @decimal 2 - * @increment 0.01 - * @group Multicopter Attitude Control - */ -PARAM_DEFINE_FLOAT(MC_PR_INT_LIM, 0.30f); - -/** - * Pitch rate D gain - * - * Pitch rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again. - * - * @min 0.0 - * @decimal 4 - * @increment 0.0005 - * @group Multicopter Attitude Control - */ -PARAM_DEFINE_FLOAT(MC_PITCHRATE_D, 0.003f); - -/** - * Pitch rate feedforward - * - * Improves tracking performance. - * - * @min 0.0 - * @decimal 4 - * @group Multicopter Attitude Control - */ -PARAM_DEFINE_FLOAT(MC_PITCHRATE_FF, 0.0f); - -/** - * Pitch rate controller gain - * - * Global gain of the controller. - * - * This gain scales the P, I and D terms of the controller: - * output = MC_PITCHRATE_K * (MC_PITCHRATE_P * error - * + MC_PITCHRATE_I * error_integral - * + MC_PITCHRATE_D * error_derivative) - * Set MC_PITCHRATE_P=1 to implement a PID in the ideal form. - * Set MC_PITCHRATE_K=1 to implement a PID in the parallel form. - * - * @min 0.0 - * @max 5.0 - * @decimal 4 - * @increment 0.0005 - * @group Multicopter Attitude Control - */ -PARAM_DEFINE_FLOAT(MC_PITCHRATE_K, 1.0f); - /** * Yaw P gain * @@ -242,87 +81,6 @@ PARAM_DEFINE_FLOAT(MC_PITCHRATE_K, 1.0f); */ PARAM_DEFINE_FLOAT(MC_YAW_P, 2.8f); -/** - * Yaw rate P gain - * - * Yaw rate proportional gain, i.e. control output for angular speed error 1 rad/s. - * - * @min 0.0 - * @max 0.6 - * @decimal 2 - * @increment 0.01 - * @group Multicopter Attitude Control - */ -PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.2f); - -/** - * Yaw rate I gain - * - * Yaw rate integral gain. Can be set to compensate static thrust difference or gravity center offset. - * - * @min 0.0 - * @decimal 2 - * @increment 0.01 - * @group Multicopter Attitude Control - */ -PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.1f); - -/** - * Yaw rate integrator limit - * - * Yaw rate integrator limit. Can be set to increase the amount of integrator available to counteract disturbances or reduced to improve settling time after large yaw moment trim changes. - * - * @min 0.0 - * @decimal 2 - * @increment 0.01 - * @group Multicopter Attitude Control - */ -PARAM_DEFINE_FLOAT(MC_YR_INT_LIM, 0.30f); - -/** - * Yaw rate D gain - * - * Yaw rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again. - * - * @min 0.0 - * @decimal 2 - * @increment 0.01 - * @group Multicopter Attitude Control - */ -PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); - -/** - * Yaw rate feedforward - * - * Improves tracking performance. - * - * @min 0.0 - * @decimal 4 - * @increment 0.01 - * @group Multicopter Attitude Control - */ -PARAM_DEFINE_FLOAT(MC_YAWRATE_FF, 0.0f); - -/** - * Yaw rate controller gain - * - * Global gain of the controller. - * - * This gain scales the P, I and D terms of the controller: - * output = MC_YAWRATE_K * (MC_YAWRATE_P * error - * + MC_YAWRATE_I * error_integral - * + MC_YAWRATE_D * error_derivative) - * Set MC_YAWRATE_P=1 to implement a PID in the ideal form. - * Set MC_YAWRATE_K=1 to implement a PID in the parallel form. - * - * @min 0.0 - * @max 5.0 - * @decimal 4 - * @increment 0.0005 - * @group Multicopter Attitude Control - */ -PARAM_DEFINE_FLOAT(MC_YAWRATE_K, 1.0f); - /** * Max roll rate * @@ -373,107 +131,6 @@ PARAM_DEFINE_FLOAT(MC_PITCHRATE_MAX, 220.0f); */ PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX, 200.0f); -/** - * Max acro roll rate - * default: 2 turns per second - * - * @unit deg/s - * @min 0.0 - * @max 1800.0 - * @decimal 1 - * @increment 5 - * @group Multicopter Attitude Control - */ -PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX, 720.0f); - -/** - * Max acro pitch rate - * default: 2 turns per second - * - * @unit deg/s - * @min 0.0 - * @max 1800.0 - * @decimal 1 - * @increment 5 - * @group Multicopter Attitude Control - */ -PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX, 720.0f); - -/** - * Max acro yaw rate - * default 1.5 turns per second - * - * @unit deg/s - * @min 0.0 - * @max 1800.0 - * @decimal 1 - * @increment 5 - * @group Multicopter Attitude Control - */ -PARAM_DEFINE_FLOAT(MC_ACRO_Y_MAX, 540.0f); - -/** - * Acro mode Expo factor for Roll and Pitch. - * - * Exponential factor for tuning the input curve shape. - * - * 0 Purely linear input curve - * 1 Purely cubic input curve - * - * @min 0 - * @max 1 - * @decimal 2 - * @group Multicopter Attitude Control - */ -PARAM_DEFINE_FLOAT(MC_ACRO_EXPO, 0.69f); - -/** - * Acro mode Expo factor for Yaw. - * - * Exponential factor for tuning the input curve shape. - * - * 0 Purely linear input curve - * 1 Purely cubic input curve - * - * @min 0 - * @max 1 - * @decimal 2 - * @group Multicopter Attitude Control - */ -PARAM_DEFINE_FLOAT(MC_ACRO_EXPO_Y, 0.69f); - -/** - * Acro mode SuperExpo factor for Roll and Pitch. - * - * SuperExpo factor for refining the input curve shape tuned using MC_ACRO_EXPO. - * - * 0 Pure Expo function - * 0.7 resonable shape enhancement for intuitive stick feel - * 0.95 very strong bent input curve only near maxima have effect - * - * @min 0 - * @max 0.95 - * @decimal 2 - * @group Multicopter Attitude Control - */ -PARAM_DEFINE_FLOAT(MC_ACRO_SUPEXPO, 0.7f); - -/** - * Acro mode SuperExpo factor for Yaw. - * - * SuperExpo factor for refining the input curve shape tuned using MC_ACRO_EXPO_Y. - * - * 0 Pure Expo function - * 0.7 resonable shape enhancement for intuitive stick feel - * 0.95 very strong bent input curve only near maxima have effect - * - * @min 0 - * @max 0.95 - * @decimal 2 - * @group Multicopter Attitude Control - */ -PARAM_DEFINE_FLOAT(MC_ACRO_SUPEXPOY, 0.7f); - /** * Threshold for Rattitude mode * @@ -487,35 +144,3 @@ PARAM_DEFINE_FLOAT(MC_ACRO_SUPEXPOY, 0.7f); * @group Multicopter Attitude Control */ PARAM_DEFINE_FLOAT(MC_RATT_TH, 0.8f); - -/** - * Battery power level scaler - * - * This compensates for voltage drop of the battery over time by attempting to - * normalize performance across the operating range of the battery. The copter - * should constantly behave as if it was fully charged with reduced max acceleration - * at lower battery percentages. i.e. if hover is at 0.5 throttle at 100% battery, - * it will still be 0.5 at 60% battery. - * - * @boolean - * @group Multicopter Attitude Control - */ -PARAM_DEFINE_INT32(MC_BAT_SCALE_EN, 0); - -/** - * Cutoff frequency for the low pass filter on the D-term in the rate controller - * - * The D-term uses the derivative of the rate and thus is the most susceptible to noise. - * Therefore, using a D-term filter allows to decrease the driver-level filtering, which - * leads to reduced control latency and permits to increase the P gains. - * A value of 0 disables the filter. - * - * @unit Hz - * @min 0 - * @max 1000 - * @decimal 0 - * @increment 10 - * @group Multicopter Attitude Control - */ -PARAM_DEFINE_FLOAT(MC_DTERM_CUTOFF, 0.f); - diff --git a/src/modules/mc_rate_control/CMakeLists.txt b/src/modules/mc_rate_control/CMakeLists.txt new file mode 100644 index 0000000000..f2d2bdb04f --- /dev/null +++ b/src/modules/mc_rate_control/CMakeLists.txt @@ -0,0 +1,48 @@ +############################################################################ +# +# Copyright (c) 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 +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +add_subdirectory(RateControl) + +px4_add_module( + MODULE modules__mc_rate_control + MAIN mc_rate_control + COMPILE_FLAGS + SRCS + MulticopterRateControl.cpp + MulticopterRateControl.hpp + DEPENDS + circuit_breaker + mathlib + RateControl + px4_work_queue + ) diff --git a/src/modules/mc_rate_control/MulticopterRateControl.cpp b/src/modules/mc_rate_control/MulticopterRateControl.cpp new file mode 100644 index 0000000000..1d1e1b5d08 --- /dev/null +++ b/src/modules/mc_rate_control/MulticopterRateControl.cpp @@ -0,0 +1,412 @@ +/**************************************************************************** + * + * Copyright (c) 2013-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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "MulticopterRateControl.hpp" + +#include +#include +#include +#include + +using namespace matrix; +using namespace time_literals; +using math::radians; + +MulticopterRateControl::MulticopterRateControl() : + ModuleParams(nullptr), + WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl), + _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")) +{ + _vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; + + parameters_updated(); +} + +MulticopterRateControl::~MulticopterRateControl() +{ + perf_free(_loop_perf); +} + +bool +MulticopterRateControl::init() +{ + if (!_vehicle_angular_velocity_sub.registerCallback()) { + PX4_ERR("vehicle_angular_velocity callback registration failed!"); + return false; + } + + return true; +} + +void +MulticopterRateControl::parameters_updated() +{ + // rate control parameters + // The controller gain K is used to convert the parallel (P + I/s + sD) form + // to the ideal (K * [1 + 1/sTi + sTd]) form + const Vector3f rate_k = Vector3f(_param_mc_rollrate_k.get(), _param_mc_pitchrate_k.get(), _param_mc_yawrate_k.get()); + + _rate_control.setGains( + rate_k.emult(Vector3f(_param_mc_rollrate_p.get(), _param_mc_pitchrate_p.get(), _param_mc_yawrate_p.get())), + rate_k.emult(Vector3f(_param_mc_rollrate_i.get(), _param_mc_pitchrate_i.get(), _param_mc_yawrate_i.get())), + rate_k.emult(Vector3f(_param_mc_rollrate_d.get(), _param_mc_pitchrate_d.get(), _param_mc_yawrate_d.get()))); + + _rate_control.setIntegratorLimit( + Vector3f(_param_mc_rr_int_lim.get(), _param_mc_pr_int_lim.get(), _param_mc_yr_int_lim.get())); + + _rate_control.setDTermCutoff(_loop_update_rate_hz, _param_mc_dterm_cutoff.get(), false); + + _rate_control.setFeedForwardGain( + Vector3f(_param_mc_rollrate_ff.get(), _param_mc_pitchrate_ff.get(), _param_mc_yawrate_ff.get())); + + + // manual rate control acro mode rate limits + _acro_rate_max = Vector3f(radians(_param_mc_acro_r_max.get()), radians(_param_mc_acro_p_max.get()), + 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); +} + +void +MulticopterRateControl::vehicle_status_poll() +{ + /* check if there is new status information */ + if (_vehicle_status_sub.update(&_vehicle_status)) { + /* set correct uORB ID, depending on if vehicle is VTOL or not */ + if (_actuators_id == nullptr) { + if (_vehicle_status.is_vtol) { + _actuators_id = ORB_ID(actuator_controls_virtual_mc); + + } else { + _actuators_id = ORB_ID(actuator_controls_0); + } + } + } +} + +float +MulticopterRateControl::get_landing_gear_state() +{ + // Only switch the landing gear up if we are not landed and if + // the user switched from gear down to gear up. + // If the user had the switch in the gear up position and took off ignore it + // until he toggles the switch to avoid retracting the gear immediately on takeoff. + if (_landed) { + _gear_state_initialized = false; + } + + float landing_gear = landing_gear_s::GEAR_DOWN; // default to down + + if (_manual_control_sp.gear_switch == manual_control_setpoint_s::SWITCH_POS_ON && _gear_state_initialized) { + landing_gear = landing_gear_s::GEAR_UP; + + } else if (_manual_control_sp.gear_switch == manual_control_setpoint_s::SWITCH_POS_OFF) { + // Switching the gear off does put it into a safe defined state + _gear_state_initialized = true; + } + + return landing_gear; +} + +void +MulticopterRateControl::Run() +{ + if (should_exit()) { + _vehicle_angular_velocity_sub.unregisterCallback(); + exit_and_cleanup(); + return; + } + + perf_begin(_loop_perf); + + // Check if parameters have changed + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s param_update; + _parameter_update_sub.copy(¶m_update); + + updateParams(); + parameters_updated(); + } + + /* run controller on gyro changes */ + vehicle_angular_velocity_s angular_velocity; + + if (_vehicle_angular_velocity_sub.update(&angular_velocity)) { + const hrt_abstime now = hrt_absolute_time(); + + // Guard against too small (< 0.2ms) and too large (> 20ms) dt's. + const float dt = math::constrain(((now - _last_run) / 1e6f), 0.0002f, 0.02f); + _last_run = now; + + const Vector3f rates{angular_velocity.xyz}; + + /* check for updates in other topics */ + _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; + } + } + + vehicle_status_poll(); + + const bool manual_control_updated = _manual_control_sp_sub.update(&_manual_control_sp); + + // generate the rate setpoint from sticks? + bool manual_rate_sp = false; + + if (_v_control_mode.flag_control_manual_enabled && + !_v_control_mode.flag_control_altitude_enabled && + !_v_control_mode.flag_control_velocity_enabled && + !_v_control_mode.flag_control_position_enabled) { + + // landing gear controlled from stick inputs if we are in Manual/Stabilized mode + // limit landing gear update rate to 50 Hz + if (hrt_elapsed_time(&_landing_gear.timestamp) > 20_ms) { + _landing_gear.landing_gear = get_landing_gear_state(); + _landing_gear.timestamp = hrt_absolute_time(); + _landing_gear_pub.publish(_landing_gear); + } + + if (!_v_control_mode.flag_control_attitude_enabled) { + manual_rate_sp = true; + } + + // Check if we are in rattitude mode and the pilot is within the center threshold on pitch and roll + // if true then use published rate setpoint, otherwise generate from manual_control_setpoint (like acro) + if (_v_control_mode.flag_control_rattitude_enabled) { + manual_rate_sp = + (fabsf(_manual_control_sp.y) > _param_mc_ratt_th.get()) || + (fabsf(_manual_control_sp.x) > _param_mc_ratt_th.get()); + } + + } else { + _landing_gear_sub.update(&_landing_gear); + } + + if (manual_rate_sp) { + if (manual_control_updated) { + + // manual rates control - ACRO mode + const Vector3f man_rate_sp{ + math::superexpo(_manual_control_sp.y, _param_mc_acro_expo.get(), _param_mc_acro_supexpo.get()), + math::superexpo(-_manual_control_sp.x, _param_mc_acro_expo.get(), _param_mc_acro_supexpo.get()), + math::superexpo(_manual_control_sp.r, _param_mc_acro_expo_y.get(), _param_mc_acro_supexpoy.get())}; + + _rates_sp = man_rate_sp.emult(_acro_rate_max); + _thrust_sp = _manual_control_sp.z; + + // publish rate setpoint + vehicle_rates_setpoint_s v_rates_sp{}; + v_rates_sp.roll = _rates_sp(0); + v_rates_sp.pitch = _rates_sp(1); + v_rates_sp.yaw = _rates_sp(2); + v_rates_sp.thrust_body[0] = 0.0f; + v_rates_sp.thrust_body[1] = 0.0f; + v_rates_sp.thrust_body[2] = -_thrust_sp; + v_rates_sp.timestamp = hrt_absolute_time(); + + _v_rates_sp_pub.publish(v_rates_sp); + } + + } else { + // use rates setpoint topic + vehicle_rates_setpoint_s v_rates_sp; + + if (_v_rates_sp_sub.update(&v_rates_sp)) { + _rates_sp(0) = v_rates_sp.roll; + _rates_sp(1) = v_rates_sp.pitch; + _rates_sp(2) = v_rates_sp.yaw; + _thrust_sp = -v_rates_sp.thrust_body[2]; + } + } + + // calculate loop update rate while disarmed or at least a few times (updating the filter is expensive) + if (!_v_control_mode.flag_armed || (now - _task_start) < 3300000) { + _dt_accumulator += dt; + ++_loop_counter; + + if (_dt_accumulator > 1.0f) { + const float loop_update_rate = (float)_loop_counter / _dt_accumulator; + _loop_update_rate_hz = _loop_update_rate_hz * 0.5f + loop_update_rate * 0.5f; + _dt_accumulator = 0; + _loop_counter = 0; + _rate_control.setDTermCutoff(_loop_update_rate_hz, _param_mc_dterm_cutoff.get(), true); + } + } + + // run the rate controller + if (_v_control_mode.flag_control_rates_enabled && !_actuators_0_circuit_breaker_enabled) { + + // reset integral if disarmed + if (!_v_control_mode.flag_armed || _vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + _rate_control.resetIntegral(); + } + + // update saturation status from mixer feedback + if (_motor_limits_sub.updated()) { + multirotor_motor_limits_s motor_limits; + + if (_motor_limits_sub.copy(&motor_limits)) { + MultirotorMixer::saturation_status saturation_status; + saturation_status.value = motor_limits.saturation_status; + + _rate_control.setSaturationStatus(saturation_status); + } + } + + // run rate controller + const Vector3f att_control = _rate_control.update(rates, _rates_sp, dt, _maybe_landed || _landed); + + // publish rate controller status + rate_ctrl_status_s rate_ctrl_status{}; + _rate_control.getRateControlStatus(rate_ctrl_status); + rate_ctrl_status.timestamp = hrt_absolute_time(); + _controller_status_pub.publish(rate_ctrl_status); + + // publish actuator controls + actuator_controls_s actuators{}; + actuators.control[actuator_controls_s::INDEX_ROLL] = PX4_ISFINITE(att_control(0)) ? att_control(0) : 0.0f; + actuators.control[actuator_controls_s::INDEX_PITCH] = PX4_ISFINITE(att_control(1)) ? att_control(1) : 0.0f; + actuators.control[actuator_controls_s::INDEX_YAW] = PX4_ISFINITE(att_control(2)) ? att_control(2) : 0.0f; + actuators.control[actuator_controls_s::INDEX_THROTTLE] = PX4_ISFINITE(_thrust_sp) ? _thrust_sp : 0.0f; + actuators.control[actuator_controls_s::INDEX_LANDING_GEAR] = (float)_landing_gear.landing_gear; + actuators.timestamp_sample = angular_velocity.timestamp_sample; + + // scale effort by battery status if enabled + if (_param_mc_bat_scale_en.get()) { + if (_battery_status_sub.updated()) { + battery_status_s battery_status; + + if (_battery_status_sub.copy(&battery_status)) { + _battery_status_scale = battery_status.scale; + } + } + + if (_battery_status_scale > 0.0f) { + for (int i = 0; i < 4; i++) { + actuators.control[i] *= _battery_status_scale; + } + } + } + + actuators.timestamp = hrt_absolute_time(); + orb_publish_auto(_actuators_id, &_actuators_0_pub, &actuators, nullptr, ORB_PRIO_DEFAULT); + + } else if (_v_control_mode.flag_control_termination_enabled) { + if (!_vehicle_status.is_vtol) { + // publish actuator controls + actuator_controls_s actuators{}; + actuators.timestamp = hrt_absolute_time(); + orb_publish_auto(_actuators_id, &_actuators_0_pub, &actuators, nullptr, ORB_PRIO_DEFAULT); + } + } + } + + perf_end(_loop_perf); +} + +int MulticopterRateControl::task_spawn(int argc, char *argv[]) +{ + MulticopterRateControl *instance = new MulticopterRateControl(); + + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; + + if (instance->init()) { + return PX4_OK; + } + + } else { + PX4_ERR("alloc failed"); + } + + delete instance; + _object.store(nullptr); + _task_id = -1; + + return PX4_ERROR; +} + +int MulticopterRateControl::print_status() +{ + PX4_INFO("Running"); + + perf_print_counter(_loop_perf); + + return 0; +} + +int MulticopterRateControl::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int MulticopterRateControl::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +This implements the multicopter rate controller. It takes rate setpoints (in acro mode +via `manual_control_setpoint` topic) as inputs and outputs actuator control messages. + +The controller has a PID loop for angular rate error. + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME(MODULE_NAME, "controller"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +/** + * Multicopter rate control app start / stop handling function + */ +extern "C" __EXPORT int mc_rate_control_main(int argc, char *argv[]); + +int mc_rate_control_main(int argc, char *argv[]) +{ + return MulticopterRateControl::main(argc, argv); +} diff --git a/src/modules/mc_rate_control/MulticopterRateControl.hpp b/src/modules/mc_rate_control/MulticopterRateControl.hpp new file mode 100644 index 0000000000..76e0848388 --- /dev/null +++ b/src/modules/mc_rate_control/MulticopterRateControl.hpp @@ -0,0 +1,194 @@ +/**************************************************************************** + * + * Copyright (c) 2013-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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class MulticopterRateControl : public ModuleBase, public ModuleParams, public px4::WorkItem +{ +public: + MulticopterRateControl(); + + virtual ~MulticopterRateControl(); + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + /** @see ModuleBase::print_status() */ + int print_status() override; + + void Run() override; + + bool init(); + +private: + + /** + * initialize some vectors/matrices from parameters + */ + void parameters_updated(); + + void vehicle_status_poll(); + + /** + * Get the landing gear state based on the manual control switch position + * @return vehicle_attitude_setpoint_s::LANDING_GEAR_UP or vehicle_attitude_setpoint_s::LANDING_GEAR_DOWN + */ + float get_landing_gear_state(); + + RateControl _rate_control; ///< class for rate control calculations + + uORB::Subscription _v_rates_sp_sub{ORB_ID(vehicle_rates_setpoint)}; /**< vehicle rates setpoint subscription */ + uORB::Subscription _v_control_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle control mode subscription */ + uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; /**< parameter updates subscription */ + uORB::Subscription _manual_control_sp_sub{ORB_ID(manual_control_setpoint)}; /**< manual control setpoint subscription */ + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */ + uORB::Subscription _motor_limits_sub{ORB_ID(multirotor_motor_limits)}; /**< motor limits subscription */ + uORB::Subscription _battery_status_sub{ORB_ID(battery_status)}; /**< battery status subscription */ + uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */ + uORB::Subscription _landing_gear_sub{ORB_ID(landing_gear)}; + + uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)}; + + uORB::PublicationMulti _controller_status_pub{ORB_ID(rate_ctrl_status), ORB_PRIO_DEFAULT}; /**< 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 */ + + orb_advert_t _actuators_0_pub{nullptr}; /**< attitude actuator controls publication */ + orb_id_t _actuators_id{nullptr}; /**< pointer to correct actuator controls0 uORB metadata structure */ + + landing_gear_s _landing_gear{}; + manual_control_setpoint_s _manual_control_sp{}; + vehicle_control_mode_s _v_control_mode{}; + vehicle_status_s _vehicle_status{}; + + bool _actuators_0_circuit_breaker_enabled{false}; /**< circuit breaker to suppress output */ + bool _landed{true}; + bool _maybe_landed{true}; + + float _battery_status_scale{0.0f}; + + perf_counter_t _loop_perf; /**< loop duration performance counter */ + + static constexpr const float initial_update_rate_hz = 1000.f; /**< loop update rate used for initialization */ + float _loop_update_rate_hz{initial_update_rate_hz}; /**< current rate-controller loop update rate in [Hz] */ + + matrix::Vector3f _rates_sp; /**< angular rates setpoint */ + + float _thrust_sp{0.0f}; /**< thrust setpoint */ + + bool _gear_state_initialized{false}; /**< true if the gear state has been initialized */ + + hrt_abstime _task_start{hrt_absolute_time()}; + hrt_abstime _last_run{0}; + float _dt_accumulator{0.0f}; + int _loop_counter{0}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_mc_rollrate_p, + (ParamFloat) _param_mc_rollrate_i, + (ParamFloat) _param_mc_rr_int_lim, + (ParamFloat) _param_mc_rollrate_d, + (ParamFloat) _param_mc_rollrate_ff, + (ParamFloat) _param_mc_rollrate_k, + + (ParamFloat) _param_mc_pitchrate_p, + (ParamFloat) _param_mc_pitchrate_i, + (ParamFloat) _param_mc_pr_int_lim, + (ParamFloat) _param_mc_pitchrate_d, + (ParamFloat) _param_mc_pitchrate_ff, + (ParamFloat) _param_mc_pitchrate_k, + + (ParamFloat) _param_mc_yawrate_p, + (ParamFloat) _param_mc_yawrate_i, + (ParamFloat) _param_mc_yr_int_lim, + (ParamFloat) _param_mc_yawrate_d, + (ParamFloat) _param_mc_yawrate_ff, + (ParamFloat) _param_mc_yawrate_k, + + (ParamFloat) _param_mc_dterm_cutoff, /**< Cutoff frequency for the D-term filter */ + + (ParamFloat) _param_mpc_man_y_max, /**< scaling factor from stick to yaw rate */ + + (ParamFloat) _param_mc_acro_r_max, + (ParamFloat) _param_mc_acro_p_max, + (ParamFloat) _param_mc_acro_y_max, + (ParamFloat) _param_mc_acro_expo, /**< expo stick curve shape (roll & pitch) */ + (ParamFloat) _param_mc_acro_expo_y, /**< expo stick curve shape (yaw) */ + (ParamFloat) _param_mc_acro_supexpo, /**< superexpo stick curve shape (roll & pitch) */ + (ParamFloat) _param_mc_acro_supexpoy, /**< superexpo stick curve shape (yaw) */ + + (ParamFloat) _param_mc_ratt_th, + + (ParamBool) _param_mc_bat_scale_en, + + (ParamInt) _param_cbrk_rate_ctrl + ) + + matrix::Vector3f _acro_rate_max; /**< max attitude rates in acro mode */ + +}; + diff --git a/src/modules/mc_att_control/RateControl/CMakeLists.txt b/src/modules/mc_rate_control/RateControl/CMakeLists.txt similarity index 100% rename from src/modules/mc_att_control/RateControl/CMakeLists.txt rename to src/modules/mc_rate_control/RateControl/CMakeLists.txt diff --git a/src/modules/mc_att_control/RateControl/RateControl.cpp b/src/modules/mc_rate_control/RateControl/RateControl.cpp similarity index 96% rename from src/modules/mc_att_control/RateControl/RateControl.cpp rename to src/modules/mc_rate_control/RateControl/RateControl.cpp index 83605049e0..830a5bf43a 100644 --- a/src/modules/mc_att_control/RateControl/RateControl.cpp +++ b/src/modules/mc_rate_control/RateControl/RateControl.cpp @@ -72,7 +72,7 @@ Vector3f RateControl::update(const Vector3f &rate, const Vector3f &rate_sp, cons Vector3f rate_error = rate_sp - rate; // prepare D-term based on low-pass filtered rates - Vector3f rate_filtered(_lp_filters_d.apply(rate)); + const Vector3f rate_filtered(_lp_filters_d.apply(rate)); Vector3f rate_d; if (dt > FLT_EPSILON) { @@ -80,7 +80,7 @@ Vector3f RateControl::update(const Vector3f &rate, const Vector3f &rate_sp, cons } // PID control with feed forward - Vector3f torque = _gain_p.emult(rate_error) + _rate_int - _gain_d.emult(rate_d) + _gain_ff.emult(rate_sp); + const Vector3f torque = _gain_p.emult(rate_error) + _rate_int - _gain_d.emult(rate_d) + _gain_ff.emult(rate_sp); _rate_prev = rate; _rate_prev_filtered = rate_filtered; diff --git a/src/modules/mc_att_control/RateControl/RateControl.hpp b/src/modules/mc_rate_control/RateControl/RateControl.hpp similarity index 100% rename from src/modules/mc_att_control/RateControl/RateControl.hpp rename to src/modules/mc_rate_control/RateControl/RateControl.hpp diff --git a/src/modules/mc_att_control/RateControl/RateControlTest.cpp b/src/modules/mc_rate_control/RateControl/RateControlTest.cpp similarity index 100% rename from src/modules/mc_att_control/RateControl/RateControlTest.cpp rename to src/modules/mc_rate_control/RateControl/RateControlTest.cpp diff --git a/src/modules/mc_rate_control/mc_rate_control_params.c b/src/modules/mc_rate_control/mc_rate_control_params.c new file mode 100644 index 0000000000..80bf264889 --- /dev/null +++ b/src/modules/mc_rate_control/mc_rate_control_params.c @@ -0,0 +1,415 @@ +/**************************************************************************** + * + * Copyright (c) 2013-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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mc_rate_control_params.c + * Parameters for multicopter attitude controller. + * + * @author Lorenz Meier + * @author Anton Babushkin + */ + +/** + * Roll rate P gain + * + * Roll rate proportional gain, i.e. control output for angular speed error 1 rad/s. + * + * @min 0.0 + * @max 0.5 + * @decimal 3 + * @increment 0.01 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_ROLLRATE_P, 0.15f); + +/** + * Roll rate I gain + * + * Roll rate integral gain. Can be set to compensate static thrust difference or gravity center offset. + * + * @min 0.0 + * @decimal 3 + * @increment 0.01 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_ROLLRATE_I, 0.2f); + +/** + * Roll rate integrator limit + * + * Roll rate integrator limit. Can be set to increase the amount of integrator available to counteract disturbances or reduced to improve settling time after large roll moment trim changes. + * + * @min 0.0 + * @decimal 2 + * @increment 0.01 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_RR_INT_LIM, 0.30f); + +/** + * Roll rate D gain + * + * Roll rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again. + * + * @min 0.0 + * @max 0.01 + * @decimal 4 + * @increment 0.0005 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_ROLLRATE_D, 0.003f); + +/** + * Roll rate feedforward + * + * Improves tracking performance. + * + * @min 0.0 + * @decimal 4 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_ROLLRATE_FF, 0.0f); + +/** + * Roll rate controller gain + * + * Global gain of the controller. + * + * This gain scales the P, I and D terms of the controller: + * output = MC_ROLLRATE_K * (MC_ROLLRATE_P * error + * + MC_ROLLRATE_I * error_integral + * + MC_ROLLRATE_D * error_derivative) + * Set MC_ROLLRATE_P=1 to implement a PID in the ideal form. + * Set MC_ROLLRATE_K=1 to implement a PID in the parallel form. + * + * @min 0.0 + * @max 5.0 + * @decimal 4 + * @increment 0.0005 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_ROLLRATE_K, 1.0f); + +/** + * Pitch rate P gain + * + * Pitch rate proportional gain, i.e. control output for angular speed error 1 rad/s. + * + * @min 0.0 + * @max 0.6 + * @decimal 3 + * @increment 0.01 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_PITCHRATE_P, 0.15f); + +/** + * Pitch rate I gain + * + * Pitch rate integral gain. Can be set to compensate static thrust difference or gravity center offset. + * + * @min 0.0 + * @decimal 3 + * @increment 0.01 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_PITCHRATE_I, 0.2f); + +/** + * Pitch rate integrator limit + * + * Pitch rate integrator limit. Can be set to increase the amount of integrator available to counteract disturbances or reduced to improve settling time after large pitch moment trim changes. + * + * @min 0.0 + * @decimal 2 + * @increment 0.01 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_PR_INT_LIM, 0.30f); + +/** + * Pitch rate D gain + * + * Pitch rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again. + * + * @min 0.0 + * @decimal 4 + * @increment 0.0005 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_PITCHRATE_D, 0.003f); + +/** + * Pitch rate feedforward + * + * Improves tracking performance. + * + * @min 0.0 + * @decimal 4 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_PITCHRATE_FF, 0.0f); + +/** + * Pitch rate controller gain + * + * Global gain of the controller. + * + * This gain scales the P, I and D terms of the controller: + * output = MC_PITCHRATE_K * (MC_PITCHRATE_P * error + * + MC_PITCHRATE_I * error_integral + * + MC_PITCHRATE_D * error_derivative) + * Set MC_PITCHRATE_P=1 to implement a PID in the ideal form. + * Set MC_PITCHRATE_K=1 to implement a PID in the parallel form. + * + * @min 0.0 + * @max 5.0 + * @decimal 4 + * @increment 0.0005 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_PITCHRATE_K, 1.0f); + +/** + * Yaw rate P gain + * + * Yaw rate proportional gain, i.e. control output for angular speed error 1 rad/s. + * + * @min 0.0 + * @max 0.6 + * @decimal 2 + * @increment 0.01 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.2f); + +/** + * Yaw rate I gain + * + * Yaw rate integral gain. Can be set to compensate static thrust difference or gravity center offset. + * + * @min 0.0 + * @decimal 2 + * @increment 0.01 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.1f); + +/** + * Yaw rate integrator limit + * + * Yaw rate integrator limit. Can be set to increase the amount of integrator available to counteract disturbances or reduced to improve settling time after large yaw moment trim changes. + * + * @min 0.0 + * @decimal 2 + * @increment 0.01 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_YR_INT_LIM, 0.30f); + +/** + * Yaw rate D gain + * + * Yaw rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again. + * + * @min 0.0 + * @decimal 2 + * @increment 0.01 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); + +/** + * Yaw rate feedforward + * + * Improves tracking performance. + * + * @min 0.0 + * @decimal 4 + * @increment 0.01 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_YAWRATE_FF, 0.0f); + +/** + * Yaw rate controller gain + * + * Global gain of the controller. + * + * This gain scales the P, I and D terms of the controller: + * output = MC_YAWRATE_K * (MC_YAWRATE_P * error + * + MC_YAWRATE_I * error_integral + * + MC_YAWRATE_D * error_derivative) + * Set MC_YAWRATE_P=1 to implement a PID in the ideal form. + * Set MC_YAWRATE_K=1 to implement a PID in the parallel form. + * + * @min 0.0 + * @max 5.0 + * @decimal 4 + * @increment 0.0005 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_YAWRATE_K, 1.0f); + +/** + * Max acro roll rate + * default: 2 turns per second + * + * @unit deg/s + * @min 0.0 + * @max 1800.0 + * @decimal 1 + * @increment 5 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX, 720.0f); + +/** + * Max acro pitch rate + * default: 2 turns per second + * + * @unit deg/s + * @min 0.0 + * @max 1800.0 + * @decimal 1 + * @increment 5 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX, 720.0f); + +/** + * Max acro yaw rate + * default 1.5 turns per second + * + * @unit deg/s + * @min 0.0 + * @max 1800.0 + * @decimal 1 + * @increment 5 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_ACRO_Y_MAX, 540.0f); + +/** + * Acro mode Expo factor for Roll and Pitch. + * + * Exponential factor for tuning the input curve shape. + * + * 0 Purely linear input curve + * 1 Purely cubic input curve + * + * @min 0 + * @max 1 + * @decimal 2 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_ACRO_EXPO, 0.69f); + +/** + * Acro mode Expo factor for Yaw. + * + * Exponential factor for tuning the input curve shape. + * + * 0 Purely linear input curve + * 1 Purely cubic input curve + * + * @min 0 + * @max 1 + * @decimal 2 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_ACRO_EXPO_Y, 0.69f); + +/** + * Acro mode SuperExpo factor for Roll and Pitch. + * + * SuperExpo factor for refining the input curve shape tuned using MC_ACRO_EXPO. + * + * 0 Pure Expo function + * 0.7 resonable shape enhancement for intuitive stick feel + * 0.95 very strong bent input curve only near maxima have effect + * + * @min 0 + * @max 0.95 + * @decimal 2 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_ACRO_SUPEXPO, 0.7f); + +/** + * Acro mode SuperExpo factor for Yaw. + * + * SuperExpo factor for refining the input curve shape tuned using MC_ACRO_EXPO_Y. + * + * 0 Pure Expo function + * 0.7 resonable shape enhancement for intuitive stick feel + * 0.95 very strong bent input curve only near maxima have effect + * + * @min 0 + * @max 0.95 + * @decimal 2 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_ACRO_SUPEXPOY, 0.7f); + +/** + * Battery power level scaler + * + * This compensates for voltage drop of the battery over time by attempting to + * normalize performance across the operating range of the battery. The copter + * should constantly behave as if it was fully charged with reduced max acceleration + * at lower battery percentages. i.e. if hover is at 0.5 throttle at 100% battery, + * it will still be 0.5 at 60% battery. + * + * @boolean + * @group Multicopter Rate Control + */ +PARAM_DEFINE_INT32(MC_BAT_SCALE_EN, 0); + +/** + * Cutoff frequency for the low pass filter on the D-term in the rate controller + * + * The D-term uses the derivative of the rate and thus is the most susceptible to noise. + * Therefore, using a D-term filter allows to decrease the driver-level filtering, which + * leads to reduced control latency and permits to increase the P gains. + * A value of 0 disables the filter. + * + * @unit Hz + * @min 0 + * @max 1000 + * @decimal 0 + * @increment 10 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_DTERM_CUTOFF, 0.f); + diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index 5b743c0c59..56dfc31737 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -183,7 +183,7 @@ protected: struct vtol_vehicle_status_s *_vtol_vehicle_status; struct actuator_controls_s *_actuators_out_0; //actuator controls going to the mc mixer struct actuator_controls_s *_actuators_out_1; //actuator controls going to the fw mixer (used for elevons) - struct actuator_controls_s *_actuators_mc_in; //actuator controls from mc_att_control + struct actuator_controls_s *_actuators_mc_in; //actuator controls from mc_rate_control struct actuator_controls_s *_actuators_fw_in; //actuator controls from fw_att_control struct vehicle_local_position_s *_local_pos; struct vehicle_local_position_setpoint_s *_local_pos_sp; diff --git a/src/systemcmds/esc_calib/esc_calib.c b/src/systemcmds/esc_calib/esc_calib.c index d905bc2f02..2890e64ce3 100644 --- a/src/systemcmds/esc_calib/esc_calib.c +++ b/src/systemcmds/esc_calib/esc_calib.c @@ -73,7 +73,7 @@ usage(const char *reason) "\n" "Calibration procedure (running the command will guide you through it):\n" "- Remove props, power off the ESC's\n" - "- Stop attitude controllers: mc_att_control stop, fw_att_control stop\n" + "- Stop attitude and rate controllers: mc_rate_control stop, fw_att_control stop\n" "- Make sure safety is off\n" "- Run this command\n" ); @@ -218,7 +218,7 @@ esc_calib_main(int argc, char *argv[]) if (orb_updated) { PX4_ERR("ABORTING! Attitude control still active. Please ensure to shut down all controllers:\n" - "\tmc_att_control stop\n" + "\tmc_rate_control stop\n" "\tfw_att_control stop\n"); return 1; } diff --git a/src/systemcmds/motor_ramp/motor_ramp.cpp b/src/systemcmds/motor_ramp/motor_ramp.cpp index 331d0e56fd..7f2b9b5333 100644 --- a/src/systemcmds/motor_ramp/motor_ramp.cpp +++ b/src/systemcmds/motor_ramp/motor_ramp.cpp @@ -120,7 +120,7 @@ usage(const char *reason) Application to test motor ramp up. Before starting, make sure to stop any running attitude controller: -$ mc_att_control stop +$ mc_rate_control stop $ fw_att_control stop When starting, a background task is started, runs for several seconds (as specified), then exits. @@ -327,7 +327,7 @@ int prepare(int fd, unsigned long *max_channels) if (orb_updated) { PX4_ERR("ABORTING! Attitude control still active. Please ensure to shut down all controllers:\n" - "\tmc_att_control stop\n" + "\tmc_rate_control stop\n" "\tfw_att_control stop\n"); return 1; }