diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index 39b35ea893..4edc31eb41 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/ROMFS/px4fmu_common/init.d-posix/rcS @@ -234,6 +234,7 @@ replay tryapplyparams simulator start -c $simulator_tcp_port tone_alarm start gpssim start +rc_update start sensors start commander start navigator start diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index c54dc8605c..6a84c735f5 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -310,6 +310,12 @@ else tune_control play -t 2 fi + # + # RC update (map raw RC input to calibrate manual control) + # start before commander + # + rc_update start + # # Sensors System (start before Commander so Preflight checks are properly run). # Commander needs to be this early for in-air-restarts. diff --git a/boards/aerotenna/ocpoc/default.cmake b/boards/aerotenna/ocpoc/default.cmake index 316f373bc7..edbb876a47 100644 --- a/boards/aerotenna/ocpoc/default.cmake +++ b/boards/aerotenna/ocpoc/default.cmake @@ -50,6 +50,7 @@ px4_add_board( mc_rate_control #micrortps_bridge navigator + rc_update rover_pos_control sensors sih diff --git a/boards/airmind/mindpx-v2/default.cmake b/boards/airmind/mindpx-v2/default.cmake index 8240cf076f..4a6ca480f7 100644 --- a/boards/airmind/mindpx-v2/default.cmake +++ b/boards/airmind/mindpx-v2/default.cmake @@ -72,6 +72,7 @@ px4_add_board( mc_pos_control mc_rate_control navigator + rc_update rover_pos_control sensors sih diff --git a/boards/atlflight/eagle/default.cmake b/boards/atlflight/eagle/default.cmake index acc31558f2..0d7a07802d 100644 --- a/boards/atlflight/eagle/default.cmake +++ b/boards/atlflight/eagle/default.cmake @@ -83,6 +83,7 @@ px4_add_board( mc_rate_control mc_pos_control navigator + rc_update sensors #sih simulator diff --git a/boards/atlflight/eagle/qurt-default.cmake b/boards/atlflight/eagle/qurt-default.cmake index 0c4f77749c..d09f544c84 100644 --- a/boards/atlflight/eagle/qurt-default.cmake +++ b/boards/atlflight/eagle/qurt-default.cmake @@ -76,6 +76,7 @@ px4_add_board( mc_att_control mc_rate_control mc_pos_control + rc_update sensors #sih vmount diff --git a/boards/atlflight/excelsior/default.cmake b/boards/atlflight/excelsior/default.cmake index a6e6671c0d..ccb1ae9a8b 100644 --- a/boards/atlflight/excelsior/default.cmake +++ b/boards/atlflight/excelsior/default.cmake @@ -83,6 +83,7 @@ px4_add_board( mc_rate_control mc_pos_control navigator + rc_update sensors sih simulator diff --git a/boards/atlflight/excelsior/qurt-default.cmake b/boards/atlflight/excelsior/qurt-default.cmake index ebfa9a57a5..1017478c03 100644 --- a/boards/atlflight/excelsior/qurt-default.cmake +++ b/boards/atlflight/excelsior/qurt-default.cmake @@ -75,6 +75,7 @@ px4_add_board( mc_att_control mc_rate_control mc_pos_control + rc_update sensors sih vmount diff --git a/boards/auav/x21/default.cmake b/boards/auav/x21/default.cmake index 660d883360..910d0f3947 100644 --- a/boards/auav/x21/default.cmake +++ b/boards/auav/x21/default.cmake @@ -72,6 +72,7 @@ px4_add_board( mc_pos_control mc_rate_control navigator + rc_update rover_pos_control sensors sih diff --git a/boards/av/x-v1/default.cmake b/boards/av/x-v1/default.cmake index 9ea3482e1f..b48da09c4a 100644 --- a/boards/av/x-v1/default.cmake +++ b/boards/av/x-v1/default.cmake @@ -72,6 +72,7 @@ px4_add_board( mc_pos_control mc_rate_control navigator + rc_update rover_pos_control sensors sih diff --git a/boards/beaglebone/blue/cross.cmake b/boards/beaglebone/blue/cross.cmake index ee261b4f1a..e9c6cdc7df 100644 --- a/boards/beaglebone/blue/cross.cmake +++ b/boards/beaglebone/blue/cross.cmake @@ -47,6 +47,7 @@ px4_add_board( mc_pos_control mc_rate_control navigator + rc_update rover_pos_control sensors sih diff --git a/boards/beaglebone/blue/native.cmake b/boards/beaglebone/blue/native.cmake index 0ab0543065..1d1c12469c 100644 --- a/boards/beaglebone/blue/native.cmake +++ b/boards/beaglebone/blue/native.cmake @@ -45,6 +45,7 @@ px4_add_board( mc_pos_control mc_rate_control navigator + rc_update rover_pos_control sensors sih diff --git a/boards/bitcraze/crazyflie/default.cmake b/boards/bitcraze/crazyflie/default.cmake index b6cffa653d..5260d52cc3 100644 --- a/boards/bitcraze/crazyflie/default.cmake +++ b/boards/bitcraze/crazyflie/default.cmake @@ -30,6 +30,7 @@ px4_add_board( mc_pos_control mc_rate_control navigator + rc_update sensors SYSTEMCMDS bl_update diff --git a/boards/emlid/navio2/cross.cmake b/boards/emlid/navio2/cross.cmake index f64c14c090..7a17ac8b24 100644 --- a/boards/emlid/navio2/cross.cmake +++ b/boards/emlid/navio2/cross.cmake @@ -51,6 +51,7 @@ px4_add_board( mc_pos_control mc_rate_control navigator + rc_update rover_pos_control sensors sih diff --git a/boards/emlid/navio2/native.cmake b/boards/emlid/navio2/native.cmake index ab25617ec0..8c226b4db6 100644 --- a/boards/emlid/navio2/native.cmake +++ b/boards/emlid/navio2/native.cmake @@ -49,6 +49,7 @@ px4_add_board( mc_pos_control mc_rate_control navigator + rc_update rover_pos_control sensors sih diff --git a/boards/holybro/durandal-v1/default.cmake b/boards/holybro/durandal-v1/default.cmake index 333b420d8d..a48428c6f4 100644 --- a/boards/holybro/durandal-v1/default.cmake +++ b/boards/holybro/durandal-v1/default.cmake @@ -77,6 +77,7 @@ px4_add_board( mc_pos_control mc_rate_control navigator + rc_update rover_pos_control sensors sih diff --git a/boards/holybro/kakutef7/default.cmake b/boards/holybro/kakutef7/default.cmake index 280c2b4be4..a02b4ea631 100644 --- a/boards/holybro/kakutef7/default.cmake +++ b/boards/holybro/kakutef7/default.cmake @@ -44,6 +44,7 @@ px4_add_board( mc_pos_control mc_rate_control navigator + rc_update sensors SYSTEMCMDS bl_update diff --git a/boards/intel/aerofc-v1/default.cmake b/boards/intel/aerofc-v1/default.cmake index b18df6146b..832507cdf4 100644 --- a/boards/intel/aerofc-v1/default.cmake +++ b/boards/intel/aerofc-v1/default.cmake @@ -52,6 +52,7 @@ px4_add_board( mc_rate_control #micrortps_bridge navigator + rc_update #rover_pos_control sensors #sih diff --git a/boards/intel/aerofc-v1/rtps.cmake b/boards/intel/aerofc-v1/rtps.cmake index 4269403e19..0780b2bfe9 100644 --- a/boards/intel/aerofc-v1/rtps.cmake +++ b/boards/intel/aerofc-v1/rtps.cmake @@ -52,6 +52,7 @@ px4_add_board( mc_rate_control micrortps_bridge navigator + rc_update #rover_pos_control sensors #sih diff --git a/boards/modalai/fc-v1/default.cmake b/boards/modalai/fc-v1/default.cmake index 73529136ab..dc2bc006c0 100644 --- a/boards/modalai/fc-v1/default.cmake +++ b/boards/modalai/fc-v1/default.cmake @@ -70,6 +70,7 @@ px4_add_board( mc_pos_control mc_rate_control navigator + rc_update rover_pos_control sensors sih diff --git a/boards/mro/ctrl-zero-f7/default.cmake b/boards/mro/ctrl-zero-f7/default.cmake index 6ff20a1166..59a1617c68 100644 --- a/boards/mro/ctrl-zero-f7/default.cmake +++ b/boards/mro/ctrl-zero-f7/default.cmake @@ -76,6 +76,7 @@ px4_add_board( mc_rate_control #micrortps_bridge navigator + rc_update rover_pos_control sensors sih diff --git a/boards/nxp/fmuk66-v3/default.cmake b/boards/nxp/fmuk66-v3/default.cmake index 53ed59b14d..fbef96bd34 100644 --- a/boards/nxp/fmuk66-v3/default.cmake +++ b/boards/nxp/fmuk66-v3/default.cmake @@ -73,6 +73,7 @@ px4_add_board( mc_pos_control mc_rate_control navigator + rc_update rover_pos_control sensors sih diff --git a/boards/omnibus/f4sd/default.cmake b/boards/omnibus/f4sd/default.cmake index dc5c0b6528..caee09e50b 100644 --- a/boards/omnibus/f4sd/default.cmake +++ b/boards/omnibus/f4sd/default.cmake @@ -60,6 +60,7 @@ px4_add_board( mc_pos_control mc_rate_control navigator + rc_update #rover_pos_control sensors #sih diff --git a/boards/parrot/bebop/default.cmake b/boards/parrot/bebop/default.cmake index a03b47bdb7..7e5094a626 100644 --- a/boards/parrot/bebop/default.cmake +++ b/boards/parrot/bebop/default.cmake @@ -39,6 +39,7 @@ px4_add_board( mc_rate_control mc_pos_control navigator + rc_update sensors sih #vtol_att_control diff --git a/boards/px4/fmu-v2/default.cmake b/boards/px4/fmu-v2/default.cmake index 0a6e5104fc..40b855f970 100644 --- a/boards/px4/fmu-v2/default.cmake +++ b/boards/px4/fmu-v2/default.cmake @@ -48,7 +48,7 @@ px4_add_board( magnetometer/hmc5883 #mkblctrl #optical_flow # all available optical flow drivers - optical_flow/px4flow + #optical_flow/px4flow #osd #pca9685 #protocol_splitter @@ -83,6 +83,7 @@ px4_add_board( mc_pos_control mc_rate_control navigator + rc_update #rover_pos_control sensors #sih diff --git a/boards/px4/fmu-v2/fixedwing.cmake b/boards/px4/fmu-v2/fixedwing.cmake index 669166f0ad..622caf636d 100644 --- a/boards/px4/fmu-v2/fixedwing.cmake +++ b/boards/px4/fmu-v2/fixedwing.cmake @@ -54,6 +54,7 @@ px4_add_board( logger mavlink navigator + rc_update sensors vmount SYSTEMCMDS diff --git a/boards/px4/fmu-v2/lpe.cmake b/boards/px4/fmu-v2/lpe.cmake index a946d6bdd7..00c99f961b 100644 --- a/boards/px4/fmu-v2/lpe.cmake +++ b/boards/px4/fmu-v2/lpe.cmake @@ -79,6 +79,7 @@ px4_add_board( mc_pos_control navigator battery_status + rc_update sensors vmount #vtol_att_control diff --git a/boards/px4/fmu-v2/multicopter.cmake b/boards/px4/fmu-v2/multicopter.cmake index 15b1e08e10..68f941780a 100644 --- a/boards/px4/fmu-v2/multicopter.cmake +++ b/boards/px4/fmu-v2/multicopter.cmake @@ -52,6 +52,7 @@ px4_add_board( mc_pos_control mc_rate_control navigator + rc_update sensors vmount SYSTEMCMDS diff --git a/boards/px4/fmu-v2/rover.cmake b/boards/px4/fmu-v2/rover.cmake index e1f2d34c23..bf0d856274 100644 --- a/boards/px4/fmu-v2/rover.cmake +++ b/boards/px4/fmu-v2/rover.cmake @@ -48,6 +48,7 @@ px4_add_board( mavlink navigator battery_status + rc_update sensors vmount diff --git a/boards/px4/fmu-v2/test.cmake b/boards/px4/fmu-v2/test.cmake index 77bd9f1e37..8fdf968648 100644 --- a/boards/px4/fmu-v2/test.cmake +++ b/boards/px4/fmu-v2/test.cmake @@ -82,6 +82,7 @@ px4_add_board( mc_pos_control mc_rate_control navigator + rc_update #rover_pos_control sensors #sih diff --git a/boards/px4/fmu-v3/default.cmake b/boards/px4/fmu-v3/default.cmake index c418f1dc81..2563230fdc 100644 --- a/boards/px4/fmu-v3/default.cmake +++ b/boards/px4/fmu-v3/default.cmake @@ -82,6 +82,7 @@ px4_add_board( mc_rate_control #micrortps_bridge navigator + rc_update rover_pos_control sensors sih diff --git a/boards/px4/fmu-v3/rtps.cmake b/boards/px4/fmu-v3/rtps.cmake index 99f31ea36e..c66529573b 100644 --- a/boards/px4/fmu-v3/rtps.cmake +++ b/boards/px4/fmu-v3/rtps.cmake @@ -82,6 +82,7 @@ px4_add_board( mc_rate_control micrortps_bridge navigator + rc_update rover_pos_control sensors sih diff --git a/boards/px4/fmu-v3/stackcheck.cmake b/boards/px4/fmu-v3/stackcheck.cmake index c4e5c211c5..b5c0730e33 100644 --- a/boards/px4/fmu-v3/stackcheck.cmake +++ b/boards/px4/fmu-v3/stackcheck.cmake @@ -78,6 +78,7 @@ px4_add_board( mc_pos_control navigator battery_status + rc_update sensors sih vmount diff --git a/boards/px4/fmu-v4/default.cmake b/boards/px4/fmu-v4/default.cmake index 4c689ac502..1550c0dc0a 100644 --- a/boards/px4/fmu-v4/default.cmake +++ b/boards/px4/fmu-v4/default.cmake @@ -73,6 +73,7 @@ px4_add_board( mc_rate_control #micrortps_bridge navigator + rc_update rover_pos_control sensors sih diff --git a/boards/px4/fmu-v4/rtps.cmake b/boards/px4/fmu-v4/rtps.cmake index 5b40eae0f0..3c4185725e 100644 --- a/boards/px4/fmu-v4/rtps.cmake +++ b/boards/px4/fmu-v4/rtps.cmake @@ -73,6 +73,7 @@ px4_add_board( mc_rate_control micrortps_bridge navigator + rc_update rover_pos_control sensors sih diff --git a/boards/px4/fmu-v4/stackcheck.cmake b/boards/px4/fmu-v4/stackcheck.cmake index 6f92c6962e..5d52be2eee 100644 --- a/boards/px4/fmu-v4/stackcheck.cmake +++ b/boards/px4/fmu-v4/stackcheck.cmake @@ -73,6 +73,7 @@ px4_add_board( mc_pos_control mc_rate_control navigator + rc_update rover_pos_control sensors sih diff --git a/boards/px4/fmu-v4pro/default.cmake b/boards/px4/fmu-v4pro/default.cmake index 1023760e65..0c58cec706 100644 --- a/boards/px4/fmu-v4pro/default.cmake +++ b/boards/px4/fmu-v4pro/default.cmake @@ -75,6 +75,7 @@ px4_add_board( mc_rate_control #micrortps_bridge navigator + rc_update rover_pos_control sensors sih diff --git a/boards/px4/fmu-v4pro/rtps.cmake b/boards/px4/fmu-v4pro/rtps.cmake index 3f2b0dd575..fedb273a0b 100644 --- a/boards/px4/fmu-v4pro/rtps.cmake +++ b/boards/px4/fmu-v4pro/rtps.cmake @@ -74,6 +74,7 @@ px4_add_board( mc_rate_control micrortps_bridge navigator + rc_update rover_pos_control sensors sih diff --git a/boards/px4/fmu-v5/critmonitor.cmake b/boards/px4/fmu-v5/critmonitor.cmake index 0aa1e7f99f..d5586251fd 100644 --- a/boards/px4/fmu-v5/critmonitor.cmake +++ b/boards/px4/fmu-v5/critmonitor.cmake @@ -78,6 +78,7 @@ px4_add_board( mc_pos_control navigator battery_status + rc_update sensors sih vmount diff --git a/boards/px4/fmu-v5/default.cmake b/boards/px4/fmu-v5/default.cmake index 4223a43536..a7ae3e9513 100644 --- a/boards/px4/fmu-v5/default.cmake +++ b/boards/px4/fmu-v5/default.cmake @@ -78,6 +78,7 @@ px4_add_board( mc_rate_control #micrortps_bridge navigator + rc_update rover_pos_control sensors sih diff --git a/boards/px4/fmu-v5/fixedwing.cmake b/boards/px4/fmu-v5/fixedwing.cmake index acee4338c9..ed2da4aead 100644 --- a/boards/px4/fmu-v5/fixedwing.cmake +++ b/boards/px4/fmu-v5/fixedwing.cmake @@ -57,6 +57,7 @@ px4_add_board( logger mavlink navigator + rc_update sensors vmount SYSTEMCMDS diff --git a/boards/px4/fmu-v5/irqmonitor.cmake b/boards/px4/fmu-v5/irqmonitor.cmake index a8789d4172..c283d679c2 100644 --- a/boards/px4/fmu-v5/irqmonitor.cmake +++ b/boards/px4/fmu-v5/irqmonitor.cmake @@ -78,6 +78,7 @@ px4_add_board( mc_pos_control navigator battery_status + rc_update sensors sih vmount diff --git a/boards/px4/fmu-v5/multicopter.cmake b/boards/px4/fmu-v5/multicopter.cmake index 0be3f4b9c7..f537a57a61 100644 --- a/boards/px4/fmu-v5/multicopter.cmake +++ b/boards/px4/fmu-v5/multicopter.cmake @@ -66,6 +66,7 @@ px4_add_board( mc_pos_control mc_rate_control navigator + rc_update sensors sih vmount diff --git a/boards/px4/fmu-v5/rover.cmake b/boards/px4/fmu-v5/rover.cmake index 7a59033412..25514919c3 100644 --- a/boards/px4/fmu-v5/rover.cmake +++ b/boards/px4/fmu-v5/rover.cmake @@ -56,6 +56,7 @@ px4_add_board( logger mavlink navigator + rc_update rover_pos_control sensors vmount diff --git a/boards/px4/fmu-v5/rtps.cmake b/boards/px4/fmu-v5/rtps.cmake index 3d07ee3571..81ecf32dce 100644 --- a/boards/px4/fmu-v5/rtps.cmake +++ b/boards/px4/fmu-v5/rtps.cmake @@ -78,6 +78,7 @@ px4_add_board( mc_rate_control micrortps_bridge navigator + rc_update rover_pos_control sensors sih diff --git a/boards/px4/fmu-v5/stackcheck.cmake b/boards/px4/fmu-v5/stackcheck.cmake index de3a4967a0..6dd460174d 100644 --- a/boards/px4/fmu-v5/stackcheck.cmake +++ b/boards/px4/fmu-v5/stackcheck.cmake @@ -77,6 +77,7 @@ px4_add_board( mc_pos_control mc_rate_control navigator + rc_update rover_pos_control sensors sih diff --git a/boards/px4/fmu-v5x/default.cmake b/boards/px4/fmu-v5x/default.cmake index 76766f234f..850d4a8e46 100644 --- a/boards/px4/fmu-v5x/default.cmake +++ b/boards/px4/fmu-v5x/default.cmake @@ -77,6 +77,7 @@ px4_add_board( mc_pos_control mc_rate_control navigator + rc_update rover_pos_control sensors sih diff --git a/boards/px4/raspberrypi/cross.cmake b/boards/px4/raspberrypi/cross.cmake index 89b82e928d..e5d0efdedb 100644 --- a/boards/px4/raspberrypi/cross.cmake +++ b/boards/px4/raspberrypi/cross.cmake @@ -45,6 +45,7 @@ px4_add_board( mc_pos_control navigator battery_status + rc_update sensors sih vmount diff --git a/boards/px4/raspberrypi/native.cmake b/boards/px4/raspberrypi/native.cmake index b1985ed1ab..6f4985313a 100644 --- a/boards/px4/raspberrypi/native.cmake +++ b/boards/px4/raspberrypi/native.cmake @@ -43,6 +43,7 @@ px4_add_board( mc_pos_control navigator battery_status + rc_update sensors sih #simulator diff --git a/boards/px4/sitl/default.cmake b/boards/px4/sitl/default.cmake index 6ddef271d3..666654726e 100644 --- a/boards/px4/sitl/default.cmake +++ b/boards/px4/sitl/default.cmake @@ -38,6 +38,7 @@ px4_add_board( mc_pos_control mc_rate_control navigator + rc_update replay rover_pos_control sensors diff --git a/boards/px4/sitl/rtps.cmake b/boards/px4/sitl/rtps.cmake index 3c392d3a1d..6a02e9e75e 100644 --- a/boards/px4/sitl/rtps.cmake +++ b/boards/px4/sitl/rtps.cmake @@ -41,6 +41,7 @@ px4_add_board( micrortps_bridge navigator replay + rc_update rover_pos_control sensors #sih diff --git a/boards/px4/sitl/test.cmake b/boards/px4/sitl/test.cmake index 2c1570bfd8..589f0c88fd 100644 --- a/boards/px4/sitl/test.cmake +++ b/boards/px4/sitl/test.cmake @@ -41,6 +41,7 @@ px4_add_board( mc_pos_control navigator replay + rc_update sensors simulator vmount diff --git a/boards/uvify/core/default.cmake b/boards/uvify/core/default.cmake index 1e3834b605..aac6a27ffa 100644 --- a/boards/uvify/core/default.cmake +++ b/boards/uvify/core/default.cmake @@ -54,6 +54,7 @@ px4_add_board( mc_pos_control mc_rate_control navigator + rc_update sensors sih vmount diff --git a/platforms/qurt/src/px4/common/commands_hil.c b/platforms/qurt/src/px4/common/commands_hil.c index c2aa5953c4..682707d08b 100644 --- a/platforms/qurt/src/px4/common/commands_hil.c +++ b/platforms/qurt/src/px4/common/commands_hil.c @@ -49,6 +49,7 @@ const char *get_commands() "param set CAL_MAG0_ID 196608\n" // "rgbled start\n" // "tone_alarm start\n" + "rc_update start\n" "commander start --hil\n" "sensors start\n" "ekf2 start\n" diff --git a/posix-configs/SITL/init/test/test_shutdown b/posix-configs/SITL/init/test/test_shutdown index 91e15dd70e..2f6d527a0c 100644 --- a/posix-configs/SITL/init/test/test_shutdown +++ b/posix-configs/SITL/init/test/test_shutdown @@ -19,6 +19,7 @@ tone_alarm start gpssim start pwm_out_sim start +rc_update start sensors start commander start land_detector start vtol @@ -50,6 +51,7 @@ logger status pwm_out_sim status sensors status commander status +rc_update stop land_detector status navigator status ekf2 status diff --git a/posix-configs/bbblue/px4.config b/posix-configs/bbblue/px4.config index f46cf8ae1f..ee37c105a6 100644 --- a/posix-configs/bbblue/px4.config +++ b/posix-configs/bbblue/px4.config @@ -62,6 +62,7 @@ gps start -d /dev/ttyS2 -s -p ubx bbblue_adc start bbblue_adc test +rc_update start sensors start commander start navigator start diff --git a/posix-configs/bbblue/px4_fw.config b/posix-configs/bbblue/px4_fw.config index 2cd8bfdbd1..3dc7d8bc48 100644 --- a/posix-configs/bbblue/px4_fw.config +++ b/posix-configs/bbblue/px4_fw.config @@ -62,6 +62,7 @@ gps start -d /dev/ttyS2 -s -p ubx bbblue_adc start bbblue_adc test +rc_update start sensors start commander start navigator start diff --git a/posix-configs/bebop/px4.config b/posix-configs/bebop/px4.config index 7e856a21fa..da56c51282 100644 --- a/posix-configs/bebop/px4.config +++ b/posix-configs/bebop/px4.config @@ -43,6 +43,7 @@ df_ak8963_wrapper start -R 32 #df_bebop_rangefinder_wrapper start gps start -d /dev/ttyPA1 bebop_flow start +rc_update start sensors start commander start ekf2 start diff --git a/posix-configs/eagle/200qx/px4.config b/posix-configs/eagle/200qx/px4.config index 36461a02b6..186b5b1fcd 100644 --- a/posix-configs/eagle/200qx/px4.config +++ b/posix-configs/eagle/200qx/px4.config @@ -27,6 +27,7 @@ sleep 1 df_mpu9250_wrapper start df_bmp280_wrapper start gps start -d /dev/tty-4 +rc_update start sensors start commander start ekf2 start diff --git a/posix-configs/eagle/210qc/px4.config b/posix-configs/eagle/210qc/px4.config index 36461a02b6..186b5b1fcd 100644 --- a/posix-configs/eagle/210qc/px4.config +++ b/posix-configs/eagle/210qc/px4.config @@ -27,6 +27,7 @@ sleep 1 df_mpu9250_wrapper start df_bmp280_wrapper start gps start -d /dev/tty-4 +rc_update start sensors start commander start ekf2 start diff --git a/posix-configs/eagle/flight/px4.config b/posix-configs/eagle/flight/px4.config index e26b326766..b5dff8b0d3 100644 --- a/posix-configs/eagle/flight/px4.config +++ b/posix-configs/eagle/flight/px4.config @@ -12,6 +12,7 @@ df_trone_wrapper start #df_ltc2946_wrapper start # (currently not working on all boards...) #df_isl29501_wrapper start sleep 1 +rc_update start sensors start commander start ekf2 start diff --git a/posix-configs/eagle/hil/px4.config b/posix-configs/eagle/hil/px4.config index 14e9b4d443..c3a723552d 100644 --- a/posix-configs/eagle/hil/px4.config +++ b/posix-configs/eagle/hil/px4.config @@ -38,6 +38,7 @@ param set ATT_W_ACC 0.0002 param set ATT_W_MAG 0.002 param set ATT_W_GYRO_BIAS 0.05 sleep 1 +rc_update start commander start -hil sensors start ekf2 start diff --git a/posix-configs/eagle/init/rcS b/posix-configs/eagle/init/rcS index f419407283..f6ea2b320e 100644 --- a/posix-configs/eagle/init/rcS +++ b/posix-configs/eagle/init/rcS @@ -176,6 +176,7 @@ fi qshell df_mpu9250_wrapper start qshell df_bmp280_wrapper start +qshell rc_update start qshell sensors start if param compare SYS_MC_EST_GROUP 1 diff --git a/posix-configs/excelsior/px4.config b/posix-configs/excelsior/px4.config index fb8cef21ad..02401b1344 100644 --- a/posix-configs/excelsior/px4.config +++ b/posix-configs/excelsior/px4.config @@ -77,6 +77,7 @@ param set UART_ESC_MOTOR4 3 sleep 1 df_mpu9250_wrapper start df_bmp280_wrapper start +rc_update start sensors start commander start ekf2 start diff --git a/posix-configs/ocpoc/px4.config b/posix-configs/ocpoc/px4.config index fcc6ff09ef..b707dd63df 100644 --- a/posix-configs/ocpoc/px4.config +++ b/posix-configs/ocpoc/px4.config @@ -21,6 +21,7 @@ df_ms5611_wrapper start rgbled start -b 1 ocpoc_adc start gps start -d /dev/ttyS3 -s +rc_update start sensors start commander start navigator start diff --git a/posix-configs/rpi/px4.config b/posix-configs/rpi/px4.config index c15ea63e40..86003af76d 100644 --- a/posix-configs/rpi/px4.config +++ b/posix-configs/rpi/px4.config @@ -24,6 +24,7 @@ df_ms5611_wrapper start navio_rgbled start navio_adc start gps start -d /dev/spidev0.0 -i spi -p ubx +rc_update start sensors start commander start navigator start diff --git a/posix-configs/rpi/px4_fw.config b/posix-configs/rpi/px4_fw.config index 7e89775d2a..d6a14d471a 100644 --- a/posix-configs/rpi/px4_fw.config +++ b/posix-configs/rpi/px4_fw.config @@ -24,6 +24,7 @@ navio_rgbled start navio_adc start gps start -d /dev/spidev0.0 -i spi -p ubx ms4525_airspeed start +rc_update start sensors start commander start navigator start diff --git a/posix-configs/rpi/px4_hil.config b/posix-configs/rpi/px4_hil.config index 6d1b7def9f..5a554a8313 100644 --- a/posix-configs/rpi/px4_hil.config +++ b/posix-configs/rpi/px4_hil.config @@ -13,6 +13,7 @@ param set SYS_AUTOSTART 1001 param set MAV_BROADCAST 1 param set MAV_TYPE 2 dataman start +rc_update start sensors start -hil commander start -hil navigator start diff --git a/posix-configs/rpi/px4_no_shield.config b/posix-configs/rpi/px4_no_shield.config index de57d45b1c..4afb0ae78b 100644 --- a/posix-configs/rpi/px4_no_shield.config +++ b/posix-configs/rpi/px4_no_shield.config @@ -17,6 +17,7 @@ param set MAV_TYPE 2 df_mpu9250_wrapper start -R 10 df_ms5611_wrapper start gps start -d /dev/ttyACM0 -i uart -p ubx +rc_update start sensors start commander start ekf2 start diff --git a/posix-configs/rpi/px4_test.config b/posix-configs/rpi/px4_test.config index a1bd4dcdfa..6aabc6f2e2 100644 --- a/posix-configs/rpi/px4_test.config +++ b/posix-configs/rpi/px4_test.config @@ -24,6 +24,7 @@ df_ms5611_wrapper start navio_rgbled start navio_adc start gps start -d /dev/spidev0.0 -i spi -p ubx +rc_update start sensors start commander start navigator start diff --git a/src/modules/rc_update/CMakeLists.txt b/src/modules/rc_update/CMakeLists.txt new file mode 100644 index 0000000000..db99402d10 --- /dev/null +++ b/src/modules/rc_update/CMakeLists.txt @@ -0,0 +1,42 @@ +############################################################################ +# +# 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 +# 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. +# +############################################################################ + +px4_add_module( + MODULE modules__rc_update + MAIN rc_update + SRCS + rc_update.cpp + parameters.cpp + DEPENDS + mathlib + ) diff --git a/src/modules/rc_update/parameters.cpp b/src/modules/rc_update/parameters.cpp new file mode 100644 index 0000000000..9750204e7f --- /dev/null +++ b/src/modules/rc_update/parameters.cpp @@ -0,0 +1,325 @@ +/**************************************************************************** + * + * Copyright (c) 2016 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 parameters.cpp + * + * @author Beat Kueng + */ + +#include "parameters.h" + +namespace RCUpdate +{ + +void initialize_parameter_handles(ParameterHandles ¶meter_handles) +{ + /* basic r/c parameters */ + for (unsigned i = 0; i < RC_MAX_CHAN_COUNT; i++) { + char nbuf[16]; + + /* min values */ + sprintf(nbuf, "RC%d_MIN", i + 1); + parameter_handles.min[i] = param_find(nbuf); + + /* trim values */ + sprintf(nbuf, "RC%d_TRIM", i + 1); + parameter_handles.trim[i] = param_find(nbuf); + + /* max values */ + sprintf(nbuf, "RC%d_MAX", i + 1); + parameter_handles.max[i] = param_find(nbuf); + + /* channel reverse */ + sprintf(nbuf, "RC%d_REV", i + 1); + parameter_handles.rev[i] = param_find(nbuf); + + /* channel deadzone */ + sprintf(nbuf, "RC%d_DZ", i + 1); + parameter_handles.dz[i] = param_find(nbuf); + + } + + /* mandatory input switched, mapped to channels 1-4 per default */ + parameter_handles.rc_map_roll = param_find("RC_MAP_ROLL"); + parameter_handles.rc_map_pitch = param_find("RC_MAP_PITCH"); + parameter_handles.rc_map_yaw = param_find("RC_MAP_YAW"); + parameter_handles.rc_map_throttle = param_find("RC_MAP_THROTTLE"); + parameter_handles.rc_map_failsafe = param_find("RC_MAP_FAILSAFE"); + + /* mandatory mode switches, mapped to channel 5 and 6 per default */ + parameter_handles.rc_map_mode_sw = param_find("RC_MAP_MODE_SW"); + parameter_handles.rc_map_return_sw = param_find("RC_MAP_RETURN_SW"); + + parameter_handles.rc_map_flaps = param_find("RC_MAP_FLAPS"); + + /* optional mode switches, not mapped per default */ + parameter_handles.rc_map_rattitude_sw = param_find("RC_MAP_RATT_SW"); + parameter_handles.rc_map_posctl_sw = param_find("RC_MAP_POSCTL_SW"); + parameter_handles.rc_map_loiter_sw = param_find("RC_MAP_LOITER_SW"); + parameter_handles.rc_map_acro_sw = param_find("RC_MAP_ACRO_SW"); + parameter_handles.rc_map_offboard_sw = param_find("RC_MAP_OFFB_SW"); + parameter_handles.rc_map_kill_sw = param_find("RC_MAP_KILL_SW"); + parameter_handles.rc_map_arm_sw = param_find("RC_MAP_ARM_SW"); + parameter_handles.rc_map_trans_sw = param_find("RC_MAP_TRANS_SW"); + parameter_handles.rc_map_gear_sw = param_find("RC_MAP_GEAR_SW"); + parameter_handles.rc_map_stab_sw = param_find("RC_MAP_STAB_SW"); + parameter_handles.rc_map_man_sw = param_find("RC_MAP_MAN_SW"); + + parameter_handles.rc_map_aux1 = param_find("RC_MAP_AUX1"); + parameter_handles.rc_map_aux2 = param_find("RC_MAP_AUX2"); + parameter_handles.rc_map_aux3 = param_find("RC_MAP_AUX3"); + parameter_handles.rc_map_aux4 = param_find("RC_MAP_AUX4"); + parameter_handles.rc_map_aux5 = param_find("RC_MAP_AUX5"); + parameter_handles.rc_map_aux6 = param_find("RC_MAP_AUX6"); + + /* RC to parameter mapping for changing parameters with RC */ + for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) { + char name[rc_parameter_map_s::PARAM_ID_LEN]; + snprintf(name, rc_parameter_map_s::PARAM_ID_LEN, "RC_MAP_PARAM%d", + i + 1); // shifted by 1 because param name starts at 1 + parameter_handles.rc_map_param[i] = param_find(name); + } + + parameter_handles.rc_map_flightmode = param_find("RC_MAP_FLTMODE"); + + /* RC thresholds */ + parameter_handles.rc_fails_thr = param_find("RC_FAILS_THR"); + parameter_handles.rc_assist_th = param_find("RC_ASSIST_TH"); + parameter_handles.rc_auto_th = param_find("RC_AUTO_TH"); + parameter_handles.rc_rattitude_th = param_find("RC_RATT_TH"); + parameter_handles.rc_posctl_th = param_find("RC_POSCTL_TH"); + parameter_handles.rc_return_th = param_find("RC_RETURN_TH"); + parameter_handles.rc_loiter_th = param_find("RC_LOITER_TH"); + parameter_handles.rc_acro_th = param_find("RC_ACRO_TH"); + parameter_handles.rc_offboard_th = param_find("RC_OFFB_TH"); + parameter_handles.rc_killswitch_th = param_find("RC_KILLSWITCH_TH"); + parameter_handles.rc_armswitch_th = param_find("RC_ARMSWITCH_TH"); + parameter_handles.rc_trans_th = param_find("RC_TRANS_TH"); + parameter_handles.rc_gear_th = param_find("RC_GEAR_TH"); + parameter_handles.rc_stab_th = param_find("RC_STAB_TH"); + parameter_handles.rc_man_th = param_find("RC_MAN_TH"); + + /* RC low pass filter configuration */ + parameter_handles.rc_flt_smp_rate = param_find("RC_FLT_SMP_RATE"); + parameter_handles.rc_flt_cutoff = param_find("RC_FLT_CUTOFF"); + + // These are parameters for which QGroundControl always expects to be returned in a list request. + // We do a param_find here to force them into the list. + (void)param_find("RC_CHAN_CNT"); +} + +int update_parameters(const ParameterHandles ¶meter_handles, Parameters ¶meters) +{ + bool rc_valid = true; + float tmpScaleFactor = 0.0f; + float tmpRevFactor = 0.0f; + int ret = PX4_OK; + + /* rc values */ + for (unsigned int i = 0; i < RC_MAX_CHAN_COUNT; i++) { + + param_get(parameter_handles.min[i], &(parameters.min[i])); + param_get(parameter_handles.trim[i], &(parameters.trim[i])); + param_get(parameter_handles.max[i], &(parameters.max[i])); + param_get(parameter_handles.rev[i], &(parameters.rev[i])); + param_get(parameter_handles.dz[i], &(parameters.dz[i])); + + tmpScaleFactor = (1.0f / ((parameters.max[i] - parameters.min[i]) / 2.0f) * parameters.rev[i]); + tmpRevFactor = tmpScaleFactor * parameters.rev[i]; + + /* handle blowup in the scaling factor calculation */ + if (!PX4_ISFINITE(tmpScaleFactor) || + (tmpRevFactor < 0.000001f) || + (tmpRevFactor > 0.2f)) { + PX4_WARN("RC chan %u not sane, scaling: %8.6f, rev: %d", i, (double)tmpScaleFactor, (int)(parameters.rev[i])); + /* scaling factors do not make sense, lock them down */ + parameters.scaling_factor[i] = 0.0f; + rc_valid = false; + + } else { + parameters.scaling_factor[i] = tmpScaleFactor; + } + } + + /* handle wrong values */ + if (!rc_valid) { + PX4_ERR("WARNING WARNING WARNING\n\nRC CALIBRATION NOT SANE!\n\n"); + } + + const char *paramerr = "FAIL PARM LOAD"; + + /* channel mapping */ + if (param_get(parameter_handles.rc_map_roll, &(parameters.rc_map_roll)) != OK) { + PX4_WARN("%s", paramerr); + } + + if (param_get(parameter_handles.rc_map_pitch, &(parameters.rc_map_pitch)) != OK) { + PX4_WARN("%s", paramerr); + } + + if (param_get(parameter_handles.rc_map_yaw, &(parameters.rc_map_yaw)) != OK) { + PX4_WARN("%s", paramerr); + } + + if (param_get(parameter_handles.rc_map_throttle, &(parameters.rc_map_throttle)) != OK) { + PX4_WARN("%s", paramerr); + } + + if (param_get(parameter_handles.rc_map_failsafe, &(parameters.rc_map_failsafe)) != OK) { + PX4_WARN("%s", paramerr); + } + + if (param_get(parameter_handles.rc_map_mode_sw, &(parameters.rc_map_mode_sw)) != OK) { + PX4_WARN("%s", paramerr); + } + + if (param_get(parameter_handles.rc_map_return_sw, &(parameters.rc_map_return_sw)) != OK) { + PX4_WARN("%s", paramerr); + } + + if (param_get(parameter_handles.rc_map_rattitude_sw, &(parameters.rc_map_rattitude_sw)) != OK) { + PX4_WARN("%s", paramerr); + } + + if (param_get(parameter_handles.rc_map_posctl_sw, &(parameters.rc_map_posctl_sw)) != OK) { + PX4_WARN("%s", paramerr); + } + + if (param_get(parameter_handles.rc_map_loiter_sw, &(parameters.rc_map_loiter_sw)) != OK) { + PX4_WARN("%s", paramerr); + } + + if (param_get(parameter_handles.rc_map_acro_sw, &(parameters.rc_map_acro_sw)) != OK) { + PX4_WARN("%s", paramerr); + } + + if (param_get(parameter_handles.rc_map_offboard_sw, &(parameters.rc_map_offboard_sw)) != OK) { + PX4_WARN("%s", paramerr); + } + + if (param_get(parameter_handles.rc_map_kill_sw, &(parameters.rc_map_kill_sw)) != OK) { + PX4_WARN("%s", paramerr); + } + + if (param_get(parameter_handles.rc_map_arm_sw, &(parameters.rc_map_arm_sw)) != OK) { + PX4_WARN("%s", paramerr); + } + + if (param_get(parameter_handles.rc_map_trans_sw, &(parameters.rc_map_trans_sw)) != OK) { + PX4_WARN("%s", paramerr); + } + + if (param_get(parameter_handles.rc_map_flaps, &(parameters.rc_map_flaps)) != OK) { + PX4_WARN("%s", paramerr); + } + + if (param_get(parameter_handles.rc_map_gear_sw, &(parameters.rc_map_gear_sw)) != OK) { + PX4_WARN("%s", paramerr); + } + + if (param_get(parameter_handles.rc_map_stab_sw, &(parameters.rc_map_stab_sw)) != OK) { + PX4_WARN("%s", paramerr); + } + + if (param_get(parameter_handles.rc_map_man_sw, &(parameters.rc_map_man_sw)) != OK) { + PX4_WARN("%s", paramerr); + } + + param_get(parameter_handles.rc_map_aux1, &(parameters.rc_map_aux1)); + param_get(parameter_handles.rc_map_aux2, &(parameters.rc_map_aux2)); + param_get(parameter_handles.rc_map_aux3, &(parameters.rc_map_aux3)); + param_get(parameter_handles.rc_map_aux4, &(parameters.rc_map_aux4)); + param_get(parameter_handles.rc_map_aux5, &(parameters.rc_map_aux5)); + param_get(parameter_handles.rc_map_aux6, &(parameters.rc_map_aux6)); + + for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) { + param_get(parameter_handles.rc_map_param[i], &(parameters.rc_map_param[i])); + } + + param_get(parameter_handles.rc_map_flightmode, &(parameters.rc_map_flightmode)); + + param_get(parameter_handles.rc_fails_thr, &(parameters.rc_fails_thr)); + param_get(parameter_handles.rc_assist_th, &(parameters.rc_assist_th)); + parameters.rc_assist_inv = (parameters.rc_assist_th < 0); + parameters.rc_assist_th = fabsf(parameters.rc_assist_th); + param_get(parameter_handles.rc_auto_th, &(parameters.rc_auto_th)); + parameters.rc_auto_inv = (parameters.rc_auto_th < 0); + parameters.rc_auto_th = fabsf(parameters.rc_auto_th); + param_get(parameter_handles.rc_rattitude_th, &(parameters.rc_rattitude_th)); + parameters.rc_rattitude_inv = (parameters.rc_rattitude_th < 0); + parameters.rc_rattitude_th = fabsf(parameters.rc_rattitude_th); + param_get(parameter_handles.rc_posctl_th, &(parameters.rc_posctl_th)); + parameters.rc_posctl_inv = (parameters.rc_posctl_th < 0); + parameters.rc_posctl_th = fabsf(parameters.rc_posctl_th); + param_get(parameter_handles.rc_return_th, &(parameters.rc_return_th)); + parameters.rc_return_inv = (parameters.rc_return_th < 0); + parameters.rc_return_th = fabsf(parameters.rc_return_th); + param_get(parameter_handles.rc_loiter_th, &(parameters.rc_loiter_th)); + parameters.rc_loiter_inv = (parameters.rc_loiter_th < 0); + parameters.rc_loiter_th = fabsf(parameters.rc_loiter_th); + param_get(parameter_handles.rc_acro_th, &(parameters.rc_acro_th)); + parameters.rc_acro_inv = (parameters.rc_acro_th < 0); + parameters.rc_acro_th = fabsf(parameters.rc_acro_th); + param_get(parameter_handles.rc_offboard_th, &(parameters.rc_offboard_th)); + parameters.rc_offboard_inv = (parameters.rc_offboard_th < 0); + parameters.rc_offboard_th = fabsf(parameters.rc_offboard_th); + param_get(parameter_handles.rc_killswitch_th, &(parameters.rc_killswitch_th)); + parameters.rc_killswitch_inv = (parameters.rc_killswitch_th < 0); + parameters.rc_killswitch_th = fabsf(parameters.rc_killswitch_th); + param_get(parameter_handles.rc_armswitch_th, &(parameters.rc_armswitch_th)); + parameters.rc_armswitch_inv = (parameters.rc_armswitch_th < 0); + parameters.rc_armswitch_th = fabsf(parameters.rc_armswitch_th); + param_get(parameter_handles.rc_trans_th, &(parameters.rc_trans_th)); + parameters.rc_trans_inv = (parameters.rc_trans_th < 0); + parameters.rc_trans_th = fabsf(parameters.rc_trans_th); + param_get(parameter_handles.rc_gear_th, &(parameters.rc_gear_th)); + parameters.rc_gear_inv = (parameters.rc_gear_th < 0); + parameters.rc_gear_th = fabsf(parameters.rc_gear_th); + param_get(parameter_handles.rc_stab_th, &(parameters.rc_stab_th)); + parameters.rc_stab_inv = (parameters.rc_stab_th < 0); + parameters.rc_stab_th = fabsf(parameters.rc_stab_th); + param_get(parameter_handles.rc_man_th, &(parameters.rc_man_th)); + parameters.rc_man_inv = (parameters.rc_man_th < 0); + parameters.rc_man_th = fabsf(parameters.rc_man_th); + + param_get(parameter_handles.rc_flt_smp_rate, &(parameters.rc_flt_smp_rate)); + parameters.rc_flt_smp_rate = math::max(1.0f, parameters.rc_flt_smp_rate); + param_get(parameter_handles.rc_flt_cutoff, &(parameters.rc_flt_cutoff)); + /* make sure the filter is in its stable region -> fc < fs/2 */ + parameters.rc_flt_cutoff = math::min(parameters.rc_flt_cutoff, (parameters.rc_flt_smp_rate / 2) - 1.f); + + return ret; +} + +} /* namespace RCUpdate */ diff --git a/src/modules/rc_update/parameters.h b/src/modules/rc_update/parameters.h new file mode 100644 index 0000000000..ff1718206d --- /dev/null +++ b/src/modules/rc_update/parameters.h @@ -0,0 +1,207 @@ +/**************************************************************************** + * + * Copyright (c) 2016 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 + +/** + * @file parameters.h + * + * defines the list of parameters that are used within the RCUpdate module + * + * @author Beat Kueng + */ +#include +#include + +#include +#include + +#include +#include +namespace RCUpdate +{ + +static constexpr unsigned RC_MAX_CHAN_COUNT = + input_rc_s::RC_INPUT_MAX_CHANNELS; /**< maximum number of r/c channels we handle */ + +struct Parameters { + float min[RC_MAX_CHAN_COUNT]; + float trim[RC_MAX_CHAN_COUNT]; + float max[RC_MAX_CHAN_COUNT]; + float rev[RC_MAX_CHAN_COUNT]; + float dz[RC_MAX_CHAN_COUNT]; + float scaling_factor[RC_MAX_CHAN_COUNT]; + + int32_t rc_map_roll; + int32_t rc_map_pitch; + int32_t rc_map_yaw; + int32_t rc_map_throttle; + int32_t rc_map_failsafe; + + int32_t rc_map_mode_sw; + int32_t rc_map_return_sw; + int32_t rc_map_rattitude_sw; + int32_t rc_map_posctl_sw; + int32_t rc_map_loiter_sw; + int32_t rc_map_acro_sw; + int32_t rc_map_offboard_sw; + int32_t rc_map_kill_sw; + int32_t rc_map_arm_sw; + int32_t rc_map_trans_sw; + int32_t rc_map_gear_sw; + int32_t rc_map_stab_sw; + int32_t rc_map_man_sw; + int32_t rc_map_flaps; + + int32_t rc_map_aux1; + int32_t rc_map_aux2; + int32_t rc_map_aux3; + int32_t rc_map_aux4; + int32_t rc_map_aux5; + int32_t rc_map_aux6; + + int32_t rc_map_param[rc_parameter_map_s::RC_PARAM_MAP_NCHAN]; + + int32_t rc_map_flightmode; + + int32_t rc_fails_thr; + float rc_assist_th; + float rc_auto_th; + float rc_rattitude_th; + float rc_posctl_th; + float rc_return_th; + float rc_loiter_th; + float rc_acro_th; + float rc_offboard_th; + float rc_killswitch_th; + float rc_armswitch_th; + float rc_trans_th; + float rc_gear_th; + float rc_stab_th; + float rc_man_th; + + bool rc_assist_inv; + bool rc_auto_inv; + bool rc_rattitude_inv; + bool rc_posctl_inv; + bool rc_return_inv; + bool rc_loiter_inv; + bool rc_acro_inv; + bool rc_offboard_inv; + bool rc_killswitch_inv; + bool rc_armswitch_inv; + bool rc_trans_inv; + bool rc_gear_inv; + bool rc_stab_inv; + bool rc_man_inv; + + float rc_flt_smp_rate; + float rc_flt_cutoff; +}; + +struct ParameterHandles { + param_t min[RC_MAX_CHAN_COUNT]; + param_t trim[RC_MAX_CHAN_COUNT]; + param_t max[RC_MAX_CHAN_COUNT]; + param_t rev[RC_MAX_CHAN_COUNT]; + param_t dz[RC_MAX_CHAN_COUNT]; + + param_t rc_map_roll; + param_t rc_map_pitch; + param_t rc_map_yaw; + param_t rc_map_throttle; + param_t rc_map_failsafe; + + param_t rc_map_mode_sw; + param_t rc_map_return_sw; + param_t rc_map_rattitude_sw; + param_t rc_map_posctl_sw; + param_t rc_map_loiter_sw; + param_t rc_map_acro_sw; + param_t rc_map_offboard_sw; + param_t rc_map_kill_sw; + param_t rc_map_arm_sw; + param_t rc_map_trans_sw; + param_t rc_map_gear_sw; + param_t rc_map_flaps; + param_t rc_map_stab_sw; + param_t rc_map_man_sw; + + param_t rc_map_aux1; + param_t rc_map_aux2; + param_t rc_map_aux3; + param_t rc_map_aux4; + param_t rc_map_aux5; + param_t rc_map_aux6; + + param_t rc_map_param[rc_parameter_map_s::RC_PARAM_MAP_NCHAN]; + param_t rc_param[rc_parameter_map_s::RC_PARAM_MAP_NCHAN]; /**< param handles for the parameters which are bound + to a RC channel, equivalent float values in the + _parameters struct are not existing + because these parameters are never read. */ + + param_t rc_map_flightmode; + + param_t rc_fails_thr; + param_t rc_assist_th; + param_t rc_auto_th; + param_t rc_rattitude_th; + param_t rc_posctl_th; + param_t rc_return_th; + param_t rc_loiter_th; + param_t rc_acro_th; + param_t rc_offboard_th; + param_t rc_killswitch_th; + param_t rc_armswitch_th; + param_t rc_trans_th; + param_t rc_gear_th; + param_t rc_stab_th; + param_t rc_man_th; + + param_t rc_flt_smp_rate; + param_t rc_flt_cutoff; +}; + +/** + * initialize ParameterHandles struct + */ +void initialize_parameter_handles(ParameterHandles ¶meter_handles); + + +/** + * Read out the parameters using the handles into the parameters struct. + * @return 0 on success, <0 on error + */ +int update_parameters(const ParameterHandles ¶meter_handles, Parameters ¶meters); + +} /* namespace RCUpdate */ diff --git a/src/modules/sensors/rc_params.c b/src/modules/rc_update/rc_params.c similarity index 100% rename from src/modules/sensors/rc_params.c rename to src/modules/rc_update/rc_params.c diff --git a/src/modules/sensors/rc_update.cpp b/src/modules/rc_update/rc_update.cpp similarity index 83% rename from src/modules/sensors/rc_update.cpp rename to src/modules/rc_update/rc_update.cpp index bb8824a8eb..f278b5aeed 100644 --- a/src/modules/sensors/rc_update.cpp +++ b/src/modules/rc_update/rc_update.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2016 PX4 Development Team. All rights reserved. + * Copyright (c) 2016-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 @@ -39,18 +39,53 @@ #include "rc_update.h" -using namespace sensors; +#include "parameters.h" -RCUpdate::RCUpdate(const Parameters ¶meters) - : _parameters(parameters), - _filter_roll(50.0f, 10.f), /* get replaced by parameter */ - _filter_pitch(50.0f, 10.f), - _filter_yaw(50.0f, 10.f), - _filter_throttle(50.0f, 10.f) +namespace RCUpdate { + +RCUpdate::RCUpdate() : + ModuleParams(nullptr), + WorkItem(MODULE_NAME, px4::wq_configurations::hp_default), + _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME)), + _filter_roll(50.0f, 10.f), /* get replaced by parameter */ + _filter_pitch(50.0f, 10.f), + _filter_yaw(50.0f, 10.f), + _filter_throttle(50.0f, 10.f) +{ + initialize_parameter_handles(_parameter_handles); + rc_parameter_map_poll(true /* forced */); + + parameters_updated(); } -void RCUpdate::update_rc_functions() +RCUpdate::~RCUpdate() +{ + perf_free(_loop_perf); +} + +bool +RCUpdate::init() +{ + if (!_input_rc_sub.registerCallback()) { + PX4_ERR("input_rc callback registration failed!"); + return false; + } + + return true; +} + +void +RCUpdate::parameters_updated() +{ + /* read the parameter values into _parameters */ + update_parameters(_parameter_handles, _parameters); + + update_rc_functions(); +} + +void +RCUpdate::update_rc_functions() { /* update RC function mappings */ _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_THROTTLE] = _parameters.rc_map_throttle - 1; @@ -97,9 +132,9 @@ void RCUpdate::update_rc_functions() } void -RCUpdate::rc_parameter_map_poll(ParameterHandles ¶meter_handles, bool forced) +RCUpdate::rc_parameter_map_poll(bool forced) { - if (_rc_parameter_map_sub.updated()) { + if (_rc_parameter_map_sub.updated() || forced) { _rc_parameter_map_sub.copy(&_rc_parameter_map); /* update parameter handles to which the RC channels are mapped */ @@ -113,10 +148,10 @@ RCUpdate::rc_parameter_map_poll(ParameterHandles ¶meter_handles, bool forced /* Set the handle by index if the index is set, otherwise use the id */ if (_rc_parameter_map.param_index[i] >= 0) { - parameter_handles.rc_param[i] = param_for_used_index((unsigned)_rc_parameter_map.param_index[i]); + _parameter_handles.rc_param[i] = param_for_used_index((unsigned)_rc_parameter_map.param_index[i]); } else { - parameter_handles.rc_param[i] = param_find(&_rc_parameter_map.param_id[i * (rc_parameter_map_s::PARAM_ID_LEN + 1)]); + _parameter_handles.rc_param[i] = param_find(&_rc_parameter_map.param_id[i * (rc_parameter_map_s::PARAM_ID_LEN + 1)]); } } @@ -188,7 +223,7 @@ RCUpdate::get_rc_sw2pos_position(uint8_t func, float on_th, bool on_inv) } void -RCUpdate::set_params_from_rc(const ParameterHandles ¶meter_handles) +RCUpdate::set_params_from_rc() { for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) { if (_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) { @@ -207,21 +242,42 @@ RCUpdate::set_params_from_rc(const ParameterHandles ¶meter_handles) float param_val = math::constrain( _rc_parameter_map.value0[i] + _rc_parameter_map.scale[i] * rc_val, _rc_parameter_map.value_min[i], _rc_parameter_map.value_max[i]); - param_set(parameter_handles.rc_param[i], ¶m_val); + param_set(_parameter_handles.rc_param[i], ¶m_val); } } } void -RCUpdate::rc_poll(const ParameterHandles ¶meter_handles) +RCUpdate::Run() { - if (_rc_sub.updated()) { - /* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */ - input_rc_s rc_input{}; - _rc_sub.copy(&rc_input); + if (should_exit()) { + _input_rc_sub.unregisterCallback(); + exit_and_cleanup(); + return; + } + + perf_begin(_loop_perf); + + // 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(); + } + + rc_parameter_map_poll(); + + /* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */ + input_rc_s rc_input; + + if (_input_rc_sub.copy(&rc_input)) { /* detect RC signal loss */ - bool signal_lost; + bool signal_lost = true; /* check flags and require at least four channels to consider the signal valid */ if (rc_input.rc_lost || rc_input.rc_failsafe || rc_input.channel_count < 4) { @@ -422,9 +478,83 @@ RCUpdate::rc_poll(const ParameterHandles ¶meter_handles) /* Update parameters from RC Channels (tuning with RC) if activated */ if (hrt_elapsed_time(&_last_rc_to_param_map_time) > 1e6) { - set_params_from_rc(parameter_handles); + set_params_from_rc(); _last_rc_to_param_map_time = hrt_absolute_time(); } } } + + perf_end(_loop_perf); +} + +int +RCUpdate::task_spawn(int argc, char *argv[]) +{ + RCUpdate *instance = new RCUpdate(); + + 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 +RCUpdate::print_status() +{ + PX4_INFO("Running"); + perf_print_counter(_loop_perf); + + return 0; +} + +int +RCUpdate::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int +RCUpdate::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +The rc_update module handles RC channel mapping: read the raw input channels (`input_rc`), +then apply the calibration, map the RC channels to the configured channels & mode switches, +low-pass filter, and then publish as `rc_channels` and `manual_control_setpoint`. + +### Implementation +To reduce control latency, the module is scheduled on input_rc publications. + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("rc_update", "system"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +} // namespace RCUpdate + +extern "C" __EXPORT int rc_update_main(int argc, char *argv[]) +{ + return RCUpdate::RCUpdate::main(argc, argv); } diff --git a/src/modules/sensors/rc_update.h b/src/modules/rc_update/rc_update.h similarity index 70% rename from src/modules/sensors/rc_update.h rename to src/modules/rc_update/rc_update.h index 5f805919d8..2feb9a0065 100644 --- a/src/modules/sensors/rc_update.h +++ b/src/modules/rc_update/rc_update.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2016 PX4 Development Team. All rights reserved. + * Copyright (c) 2016-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 @@ -41,18 +41,27 @@ #include "parameters.h" +#include +#include +#include +#include +#include +#include #include -#include -#include +#include +#include +#include #include #include #include +#include #include #include #include #include +#include -namespace sensors +namespace RCUpdate { /** @@ -60,31 +69,43 @@ namespace sensors * * Handling of RC updates */ -class RCUpdate +class RCUpdate : public ModuleBase, public ModuleParams, public px4::WorkItem { public: - /** - * @param parameters parameter values. These do not have to be initialized when constructing this object. - * Only when calling init(), they have to be initialized. - */ - RCUpdate(const Parameters ¶meters); + RCUpdate(); + ~RCUpdate() override; + + /** @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); + + void Run() override; + bool init(); + + /** @see ModuleBase::print_status() */ + int print_status() override; + +private: /** * Check for changes in rc_parameter_map */ - void rc_parameter_map_poll(ParameterHandles ¶meter_handles, bool forced = false); + void rc_parameter_map_poll(bool forced = false); /** * update the RC functions. Call this when the parameters change. */ - void update_rc_functions(); + void update_rc_functions(); /** - * Gather and publish RC input data. + * Update our local parameter cache. */ - void rc_poll(const ParameterHandles ¶meter_handles); - -private: + void parameters_updated(); /** * Get and limit value for specified RC function. Returns NAN if not mapped. @@ -103,10 +124,16 @@ private: * * @param */ - void set_params_from_rc(const ParameterHandles ¶meter_handles); + void set_params_from_rc(); + perf_counter_t _loop_perf; /**< loop performance counter */ - uORB::Subscription _rc_sub{ORB_ID(input_rc)}; /**< raw rc channels data subscription */ + Parameters _parameters{}; /**< local copies of interesting parameters */ + ParameterHandles _parameter_handles{}; /**< handles for interesting parameters */ + + uORB::SubscriptionCallbackWorkItem _input_rc_sub{this, ORB_ID(input_rc)}; + + uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; /**< notification of parameter updates */ uORB::Subscription _rc_parameter_map_sub{ORB_ID(rc_parameter_map)}; /**< rc parameter map subscription */ uORB::Publication _rc_pub{ORB_ID(rc_channels)}; /**< raw r/c control topic */ @@ -119,8 +146,6 @@ private: rc_parameter_map_s _rc_parameter_map {}; float _param_rc_values[rc_parameter_map_s::RC_PARAM_MAP_NCHAN] {}; /**< parameter values for RC control */ - const Parameters &_parameters; - hrt_abstime _last_rc_to_param_map_time = 0; math::LowPassFilter2p _filter_roll; /**< filters for the main 4 stick inputs */ @@ -132,4 +157,4 @@ private: -} /* namespace sensors */ +} /* namespace RCUpdate */ diff --git a/src/modules/sensors/CMakeLists.txt b/src/modules/sensors/CMakeLists.txt index 0a5b6620f1..177f7e38e0 100644 --- a/src/modules/sensors/CMakeLists.txt +++ b/src/modules/sensors/CMakeLists.txt @@ -40,7 +40,6 @@ px4_add_module( PRIORITY "SCHED_PRIORITY_MAX-5" SRCS voted_sensors_update.cpp - rc_update.cpp sensors.cpp parameters.cpp temperature_compensation.cpp diff --git a/src/modules/sensors/parameters.cpp b/src/modules/sensors/parameters.cpp index c6783717ad..4d32966bb9 100644 --- a/src/modules/sensors/parameters.cpp +++ b/src/modules/sensors/parameters.cpp @@ -44,96 +44,6 @@ namespace sensors void initialize_parameter_handles(ParameterHandles ¶meter_handles) { - /* basic r/c parameters */ - for (unsigned i = 0; i < RC_MAX_CHAN_COUNT; i++) { - char nbuf[16]; - - /* min values */ - sprintf(nbuf, "RC%d_MIN", i + 1); - parameter_handles.min[i] = param_find(nbuf); - - /* trim values */ - sprintf(nbuf, "RC%d_TRIM", i + 1); - parameter_handles.trim[i] = param_find(nbuf); - - /* max values */ - sprintf(nbuf, "RC%d_MAX", i + 1); - parameter_handles.max[i] = param_find(nbuf); - - /* channel reverse */ - sprintf(nbuf, "RC%d_REV", i + 1); - parameter_handles.rev[i] = param_find(nbuf); - - /* channel deadzone */ - sprintf(nbuf, "RC%d_DZ", i + 1); - parameter_handles.dz[i] = param_find(nbuf); - - } - - /* mandatory input switched, mapped to channels 1-4 per default */ - parameter_handles.rc_map_roll = param_find("RC_MAP_ROLL"); - parameter_handles.rc_map_pitch = param_find("RC_MAP_PITCH"); - parameter_handles.rc_map_yaw = param_find("RC_MAP_YAW"); - parameter_handles.rc_map_throttle = param_find("RC_MAP_THROTTLE"); - parameter_handles.rc_map_failsafe = param_find("RC_MAP_FAILSAFE"); - - /* mandatory mode switches, mapped to channel 5 and 6 per default */ - parameter_handles.rc_map_mode_sw = param_find("RC_MAP_MODE_SW"); - parameter_handles.rc_map_return_sw = param_find("RC_MAP_RETURN_SW"); - - parameter_handles.rc_map_flaps = param_find("RC_MAP_FLAPS"); - - /* optional mode switches, not mapped per default */ - parameter_handles.rc_map_rattitude_sw = param_find("RC_MAP_RATT_SW"); - parameter_handles.rc_map_posctl_sw = param_find("RC_MAP_POSCTL_SW"); - parameter_handles.rc_map_loiter_sw = param_find("RC_MAP_LOITER_SW"); - parameter_handles.rc_map_acro_sw = param_find("RC_MAP_ACRO_SW"); - parameter_handles.rc_map_offboard_sw = param_find("RC_MAP_OFFB_SW"); - parameter_handles.rc_map_kill_sw = param_find("RC_MAP_KILL_SW"); - parameter_handles.rc_map_arm_sw = param_find("RC_MAP_ARM_SW"); - parameter_handles.rc_map_trans_sw = param_find("RC_MAP_TRANS_SW"); - parameter_handles.rc_map_gear_sw = param_find("RC_MAP_GEAR_SW"); - parameter_handles.rc_map_stab_sw = param_find("RC_MAP_STAB_SW"); - parameter_handles.rc_map_man_sw = param_find("RC_MAP_MAN_SW"); - - parameter_handles.rc_map_aux1 = param_find("RC_MAP_AUX1"); - parameter_handles.rc_map_aux2 = param_find("RC_MAP_AUX2"); - parameter_handles.rc_map_aux3 = param_find("RC_MAP_AUX3"); - parameter_handles.rc_map_aux4 = param_find("RC_MAP_AUX4"); - parameter_handles.rc_map_aux5 = param_find("RC_MAP_AUX5"); - parameter_handles.rc_map_aux6 = param_find("RC_MAP_AUX6"); - - /* RC to parameter mapping for changing parameters with RC */ - for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) { - char name[rc_parameter_map_s::PARAM_ID_LEN]; - snprintf(name, rc_parameter_map_s::PARAM_ID_LEN, "RC_MAP_PARAM%d", - i + 1); // shifted by 1 because param name starts at 1 - parameter_handles.rc_map_param[i] = param_find(name); - } - - parameter_handles.rc_map_flightmode = param_find("RC_MAP_FLTMODE"); - - /* RC thresholds */ - parameter_handles.rc_fails_thr = param_find("RC_FAILS_THR"); - parameter_handles.rc_assist_th = param_find("RC_ASSIST_TH"); - parameter_handles.rc_auto_th = param_find("RC_AUTO_TH"); - parameter_handles.rc_rattitude_th = param_find("RC_RATT_TH"); - parameter_handles.rc_posctl_th = param_find("RC_POSCTL_TH"); - parameter_handles.rc_return_th = param_find("RC_RETURN_TH"); - parameter_handles.rc_loiter_th = param_find("RC_LOITER_TH"); - parameter_handles.rc_acro_th = param_find("RC_ACRO_TH"); - parameter_handles.rc_offboard_th = param_find("RC_OFFB_TH"); - parameter_handles.rc_killswitch_th = param_find("RC_KILLSWITCH_TH"); - parameter_handles.rc_armswitch_th = param_find("RC_ARMSWITCH_TH"); - parameter_handles.rc_trans_th = param_find("RC_TRANS_TH"); - parameter_handles.rc_gear_th = param_find("RC_GEAR_TH"); - parameter_handles.rc_stab_th = param_find("RC_STAB_TH"); - parameter_handles.rc_man_th = param_find("RC_MAN_TH"); - - /* RC low pass filter configuration */ - parameter_handles.rc_flt_smp_rate = param_find("RC_FLT_SMP_RATE"); - parameter_handles.rc_flt_cutoff = param_find("RC_FLT_CUTOFF"); - /* Differential pressure offset */ parameter_handles.diff_pres_offset_pa = param_find("SENS_DPRES_OFF"); #ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL @@ -155,10 +65,6 @@ void initialize_parameter_handles(ParameterHandles ¶meter_handles) parameter_handles.air_tube_length = param_find("CAL_AIR_TUBELEN"); parameter_handles.air_tube_diameter_mm = param_find("CAL_AIR_TUBED_MM"); - // These are parameters for which QGroundControl always expects to be returned in a list request. - // We do a param_find here to force them into the list. - (void)param_find("RC_CHAN_CNT"); - (void)param_find("BAT_V_DIV"); (void)param_find("BAT_A_PER_V"); @@ -187,186 +93,8 @@ void initialize_parameter_handles(ParameterHandles ¶meter_handles) (void)param_find("SYS_CAL_TMIN"); } -int update_parameters(const ParameterHandles ¶meter_handles, Parameters ¶meters) +void update_parameters(const ParameterHandles ¶meter_handles, Parameters ¶meters) { - bool rc_valid = true; - float tmpScaleFactor = 0.0f; - float tmpRevFactor = 0.0f; - int ret = PX4_OK; - - /* rc values */ - for (unsigned int i = 0; i < RC_MAX_CHAN_COUNT; i++) { - - param_get(parameter_handles.min[i], &(parameters.min[i])); - param_get(parameter_handles.trim[i], &(parameters.trim[i])); - param_get(parameter_handles.max[i], &(parameters.max[i])); - param_get(parameter_handles.rev[i], &(parameters.rev[i])); - param_get(parameter_handles.dz[i], &(parameters.dz[i])); - - tmpScaleFactor = (1.0f / ((parameters.max[i] - parameters.min[i]) / 2.0f) * parameters.rev[i]); - tmpRevFactor = tmpScaleFactor * parameters.rev[i]; - - /* handle blowup in the scaling factor calculation */ - if (!PX4_ISFINITE(tmpScaleFactor) || - (tmpRevFactor < 0.000001f) || - (tmpRevFactor > 0.2f)) { - PX4_WARN("RC chan %u not sane, scaling: %8.6f, rev: %d", i, (double)tmpScaleFactor, (int)(parameters.rev[i])); - /* scaling factors do not make sense, lock them down */ - parameters.scaling_factor[i] = 0.0f; - rc_valid = false; - - } else { - parameters.scaling_factor[i] = tmpScaleFactor; - } - } - - /* handle wrong values */ - if (!rc_valid) { - PX4_ERR("WARNING WARNING WARNING\n\nRC CALIBRATION NOT SANE!\n\n"); - } - - const char *paramerr = "FAIL PARM LOAD"; - - /* channel mapping */ - if (param_get(parameter_handles.rc_map_roll, &(parameters.rc_map_roll)) != OK) { - PX4_WARN("%s", paramerr); - } - - if (param_get(parameter_handles.rc_map_pitch, &(parameters.rc_map_pitch)) != OK) { - PX4_WARN("%s", paramerr); - } - - if (param_get(parameter_handles.rc_map_yaw, &(parameters.rc_map_yaw)) != OK) { - PX4_WARN("%s", paramerr); - } - - if (param_get(parameter_handles.rc_map_throttle, &(parameters.rc_map_throttle)) != OK) { - PX4_WARN("%s", paramerr); - } - - if (param_get(parameter_handles.rc_map_failsafe, &(parameters.rc_map_failsafe)) != OK) { - PX4_WARN("%s", paramerr); - } - - if (param_get(parameter_handles.rc_map_mode_sw, &(parameters.rc_map_mode_sw)) != OK) { - PX4_WARN("%s", paramerr); - } - - if (param_get(parameter_handles.rc_map_return_sw, &(parameters.rc_map_return_sw)) != OK) { - PX4_WARN("%s", paramerr); - } - - if (param_get(parameter_handles.rc_map_rattitude_sw, &(parameters.rc_map_rattitude_sw)) != OK) { - PX4_WARN("%s", paramerr); - } - - if (param_get(parameter_handles.rc_map_posctl_sw, &(parameters.rc_map_posctl_sw)) != OK) { - PX4_WARN("%s", paramerr); - } - - if (param_get(parameter_handles.rc_map_loiter_sw, &(parameters.rc_map_loiter_sw)) != OK) { - PX4_WARN("%s", paramerr); - } - - if (param_get(parameter_handles.rc_map_acro_sw, &(parameters.rc_map_acro_sw)) != OK) { - PX4_WARN("%s", paramerr); - } - - if (param_get(parameter_handles.rc_map_offboard_sw, &(parameters.rc_map_offboard_sw)) != OK) { - PX4_WARN("%s", paramerr); - } - - if (param_get(parameter_handles.rc_map_kill_sw, &(parameters.rc_map_kill_sw)) != OK) { - PX4_WARN("%s", paramerr); - } - - if (param_get(parameter_handles.rc_map_arm_sw, &(parameters.rc_map_arm_sw)) != OK) { - PX4_WARN("%s", paramerr); - } - - if (param_get(parameter_handles.rc_map_trans_sw, &(parameters.rc_map_trans_sw)) != OK) { - PX4_WARN("%s", paramerr); - } - - if (param_get(parameter_handles.rc_map_flaps, &(parameters.rc_map_flaps)) != OK) { - PX4_WARN("%s", paramerr); - } - - if (param_get(parameter_handles.rc_map_gear_sw, &(parameters.rc_map_gear_sw)) != OK) { - PX4_WARN("%s", paramerr); - } - - if (param_get(parameter_handles.rc_map_stab_sw, &(parameters.rc_map_stab_sw)) != OK) { - PX4_WARN("%s", paramerr); - } - - if (param_get(parameter_handles.rc_map_man_sw, &(parameters.rc_map_man_sw)) != OK) { - PX4_WARN("%s", paramerr); - } - - param_get(parameter_handles.rc_map_aux1, &(parameters.rc_map_aux1)); - param_get(parameter_handles.rc_map_aux2, &(parameters.rc_map_aux2)); - param_get(parameter_handles.rc_map_aux3, &(parameters.rc_map_aux3)); - param_get(parameter_handles.rc_map_aux4, &(parameters.rc_map_aux4)); - param_get(parameter_handles.rc_map_aux5, &(parameters.rc_map_aux5)); - param_get(parameter_handles.rc_map_aux6, &(parameters.rc_map_aux6)); - - for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) { - param_get(parameter_handles.rc_map_param[i], &(parameters.rc_map_param[i])); - } - - param_get(parameter_handles.rc_map_flightmode, &(parameters.rc_map_flightmode)); - - param_get(parameter_handles.rc_fails_thr, &(parameters.rc_fails_thr)); - param_get(parameter_handles.rc_assist_th, &(parameters.rc_assist_th)); - parameters.rc_assist_inv = (parameters.rc_assist_th < 0); - parameters.rc_assist_th = fabsf(parameters.rc_assist_th); - param_get(parameter_handles.rc_auto_th, &(parameters.rc_auto_th)); - parameters.rc_auto_inv = (parameters.rc_auto_th < 0); - parameters.rc_auto_th = fabsf(parameters.rc_auto_th); - param_get(parameter_handles.rc_rattitude_th, &(parameters.rc_rattitude_th)); - parameters.rc_rattitude_inv = (parameters.rc_rattitude_th < 0); - parameters.rc_rattitude_th = fabsf(parameters.rc_rattitude_th); - param_get(parameter_handles.rc_posctl_th, &(parameters.rc_posctl_th)); - parameters.rc_posctl_inv = (parameters.rc_posctl_th < 0); - parameters.rc_posctl_th = fabsf(parameters.rc_posctl_th); - param_get(parameter_handles.rc_return_th, &(parameters.rc_return_th)); - parameters.rc_return_inv = (parameters.rc_return_th < 0); - parameters.rc_return_th = fabsf(parameters.rc_return_th); - param_get(parameter_handles.rc_loiter_th, &(parameters.rc_loiter_th)); - parameters.rc_loiter_inv = (parameters.rc_loiter_th < 0); - parameters.rc_loiter_th = fabsf(parameters.rc_loiter_th); - param_get(parameter_handles.rc_acro_th, &(parameters.rc_acro_th)); - parameters.rc_acro_inv = (parameters.rc_acro_th < 0); - parameters.rc_acro_th = fabsf(parameters.rc_acro_th); - param_get(parameter_handles.rc_offboard_th, &(parameters.rc_offboard_th)); - parameters.rc_offboard_inv = (parameters.rc_offboard_th < 0); - parameters.rc_offboard_th = fabsf(parameters.rc_offboard_th); - param_get(parameter_handles.rc_killswitch_th, &(parameters.rc_killswitch_th)); - parameters.rc_killswitch_inv = (parameters.rc_killswitch_th < 0); - parameters.rc_killswitch_th = fabsf(parameters.rc_killswitch_th); - param_get(parameter_handles.rc_armswitch_th, &(parameters.rc_armswitch_th)); - parameters.rc_armswitch_inv = (parameters.rc_armswitch_th < 0); - parameters.rc_armswitch_th = fabsf(parameters.rc_armswitch_th); - param_get(parameter_handles.rc_trans_th, &(parameters.rc_trans_th)); - parameters.rc_trans_inv = (parameters.rc_trans_th < 0); - parameters.rc_trans_th = fabsf(parameters.rc_trans_th); - param_get(parameter_handles.rc_gear_th, &(parameters.rc_gear_th)); - parameters.rc_gear_inv = (parameters.rc_gear_th < 0); - parameters.rc_gear_th = fabsf(parameters.rc_gear_th); - param_get(parameter_handles.rc_stab_th, &(parameters.rc_stab_th)); - parameters.rc_stab_inv = (parameters.rc_stab_th < 0); - parameters.rc_stab_th = fabsf(parameters.rc_stab_th); - param_get(parameter_handles.rc_man_th, &(parameters.rc_man_th)); - parameters.rc_man_inv = (parameters.rc_man_th < 0); - parameters.rc_man_th = fabsf(parameters.rc_man_th); - - param_get(parameter_handles.rc_flt_smp_rate, &(parameters.rc_flt_smp_rate)); - parameters.rc_flt_smp_rate = math::max(1.0f, parameters.rc_flt_smp_rate); - param_get(parameter_handles.rc_flt_cutoff, &(parameters.rc_flt_cutoff)); - /* make sure the filter is in its stable region -> fc < fs/2 */ - parameters.rc_flt_cutoff = math::min(parameters.rc_flt_cutoff, (parameters.rc_flt_smp_rate / 2) - 1.f); - /* Airspeed offset */ param_get(parameter_handles.diff_pres_offset_pa, &(parameters.diff_pres_offset_pa)); #ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL @@ -384,8 +112,6 @@ int update_parameters(const ParameterHandles ¶meter_handles, Parameters &par param_get(parameter_handles.air_cmodel, ¶meters.air_cmodel); param_get(parameter_handles.air_tube_length, ¶meters.air_tube_length); param_get(parameter_handles.air_tube_diameter_mm, ¶meters.air_tube_diameter_mm); - - return ret; } } /* namespace sensors */ diff --git a/src/modules/sensors/parameters.h b/src/modules/sensors/parameters.h index 6c7cd44aaf..eb821cd93e 100644 --- a/src/modules/sensors/parameters.h +++ b/src/modules/sensors/parameters.h @@ -40,29 +40,13 @@ * * @author Beat Kueng */ -#include -#include -#include -#include - -#include -#include +#include namespace sensors { -static const unsigned RC_MAX_CHAN_COUNT = - input_rc_s::RC_INPUT_MAX_CHANNELS; /**< maximum number of r/c channels we handle */ - struct Parameters { - float min[RC_MAX_CHAN_COUNT]; - float trim[RC_MAX_CHAN_COUNT]; - float max[RC_MAX_CHAN_COUNT]; - float rev[RC_MAX_CHAN_COUNT]; - float dz[RC_MAX_CHAN_COUNT]; - float scaling_factor[RC_MAX_CHAN_COUNT]; - float diff_pres_offset_pa; #ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL float diff_pres_analog_scale; @@ -72,72 +56,6 @@ struct Parameters { float board_offset[3]; - int32_t rc_map_roll; - int32_t rc_map_pitch; - int32_t rc_map_yaw; - int32_t rc_map_throttle; - int32_t rc_map_failsafe; - - int32_t rc_map_mode_sw; - int32_t rc_map_return_sw; - int32_t rc_map_rattitude_sw; - int32_t rc_map_posctl_sw; - int32_t rc_map_loiter_sw; - int32_t rc_map_acro_sw; - int32_t rc_map_offboard_sw; - int32_t rc_map_kill_sw; - int32_t rc_map_arm_sw; - int32_t rc_map_trans_sw; - int32_t rc_map_gear_sw; - int32_t rc_map_stab_sw; - int32_t rc_map_man_sw; - int32_t rc_map_flaps; - - int32_t rc_map_aux1; - int32_t rc_map_aux2; - int32_t rc_map_aux3; - int32_t rc_map_aux4; - int32_t rc_map_aux5; - int32_t rc_map_aux6; - - int32_t rc_map_param[rc_parameter_map_s::RC_PARAM_MAP_NCHAN]; - - int32_t rc_map_flightmode; - - int32_t rc_fails_thr; - float rc_assist_th; - float rc_auto_th; - float rc_rattitude_th; - float rc_posctl_th; - float rc_return_th; - float rc_loiter_th; - float rc_acro_th; - float rc_offboard_th; - float rc_killswitch_th; - float rc_armswitch_th; - float rc_trans_th; - float rc_gear_th; - float rc_stab_th; - float rc_man_th; - - bool rc_assist_inv; - bool rc_auto_inv; - bool rc_rattitude_inv; - bool rc_posctl_inv; - bool rc_return_inv; - bool rc_loiter_inv; - bool rc_acro_inv; - bool rc_offboard_inv; - bool rc_killswitch_inv; - bool rc_armswitch_inv; - bool rc_trans_inv; - bool rc_gear_inv; - bool rc_stab_inv; - bool rc_man_inv; - - float rc_flt_smp_rate; - float rc_flt_cutoff; - float baro_qnh; int32_t air_cmodel; @@ -146,72 +64,11 @@ struct Parameters { }; struct ParameterHandles { - param_t min[RC_MAX_CHAN_COUNT]; - param_t trim[RC_MAX_CHAN_COUNT]; - param_t max[RC_MAX_CHAN_COUNT]; - param_t rev[RC_MAX_CHAN_COUNT]; - param_t dz[RC_MAX_CHAN_COUNT]; - param_t diff_pres_offset_pa; #ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL param_t diff_pres_analog_scale; #endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */ - param_t rc_map_roll; - param_t rc_map_pitch; - param_t rc_map_yaw; - param_t rc_map_throttle; - param_t rc_map_failsafe; - - param_t rc_map_mode_sw; - param_t rc_map_return_sw; - param_t rc_map_rattitude_sw; - param_t rc_map_posctl_sw; - param_t rc_map_loiter_sw; - param_t rc_map_acro_sw; - param_t rc_map_offboard_sw; - param_t rc_map_kill_sw; - param_t rc_map_arm_sw; - param_t rc_map_trans_sw; - param_t rc_map_gear_sw; - param_t rc_map_flaps; - param_t rc_map_stab_sw; - param_t rc_map_man_sw; - - param_t rc_map_aux1; - param_t rc_map_aux2; - param_t rc_map_aux3; - param_t rc_map_aux4; - param_t rc_map_aux5; - param_t rc_map_aux6; - - param_t rc_map_param[rc_parameter_map_s::RC_PARAM_MAP_NCHAN]; - param_t rc_param[rc_parameter_map_s::RC_PARAM_MAP_NCHAN]; /**< param handles for the parameters which are bound - to a RC channel, equivalent float values in the - _parameters struct are not existing - because these parameters are never read. */ - - param_t rc_map_flightmode; - - param_t rc_fails_thr; - param_t rc_assist_th; - param_t rc_auto_th; - param_t rc_rattitude_th; - param_t rc_posctl_th; - param_t rc_return_th; - param_t rc_loiter_th; - param_t rc_acro_th; - param_t rc_offboard_th; - param_t rc_killswitch_th; - param_t rc_armswitch_th; - param_t rc_trans_th; - param_t rc_gear_th; - param_t rc_stab_th; - param_t rc_man_th; - - param_t rc_flt_smp_rate; - param_t rc_flt_cutoff; - param_t board_rotation; param_t board_offset[3]; @@ -234,6 +91,6 @@ void initialize_parameter_handles(ParameterHandles ¶meter_handles); * Read out the parameters using the handles into the parameters struct. * @return 0 on success, <0 on error */ -int update_parameters(const ParameterHandles ¶meter_handles, Parameters ¶meters); +void update_parameters(const ParameterHandles ¶meter_handles, Parameters ¶meters); } /* namespace sensors */ diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index fccd8359eb..105d0f5f36 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -41,53 +41,36 @@ * @author Beat Küng */ -#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 - -#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 #include +#include #include #include #include "parameters.h" -#include "rc_update.h" #include "voted_sensors_update.h" #include "vehicle_acceleration/VehicleAcceleration.hpp" @@ -102,15 +85,6 @@ using namespace time_literals; * subtract 5 degrees in an attempt to account for the electrical upheating of the PCB */ #define PCB_TEMP_ESTIMATE_DEG 5.0f -#define STICK_ON_OFF_LIMIT 0.75f - -/** - * Sensor app start / stop handling function - * - * @ingroup apps - */ -extern "C" __EXPORT int sensors_main(int argc, char *argv[]); - class Sensors : public ModuleBase, public ModuleParams { public: @@ -166,7 +140,6 @@ private: Parameters _parameters{}; /**< local copies of interesting parameters */ ParameterHandles _parameter_handles{}; /**< handles for interesting parameters */ - RCUpdate _rc_update; VotedSensorsUpdate _voted_sensors_update; @@ -211,7 +184,6 @@ Sensors::Sensors(bool hil_enabled) : ModuleParams(nullptr), _hil_enabled(hil_enabled), _loop_perf(perf_alloc(PC_ELAPSED, "sensors")), - _rc_update(_parameters), _voted_sensors_update(_parameters, hil_enabled) { initialize_parameter_handles(_parameter_handles); @@ -237,16 +209,11 @@ Sensors::parameters_update() } /* read the parameter values into _parameters */ - int ret = update_parameters(_parameter_handles, _parameters); + update_parameters(_parameter_handles, _parameters); - if (ret) { - return ret; - } - - _rc_update.update_rc_functions(); _voted_sensors_update.parametersUpdate(); - return ret; + return PX4_OK; } int @@ -438,8 +405,6 @@ Sensors::run() diff_pres_poll(airdata); - _rc_update.rc_parameter_map_poll(_parameter_handles, true /* forced */); - /* wakeup source */ px4_pollfd_struct_t poll_fds = {}; poll_fds.events = POLLIN; @@ -532,14 +497,8 @@ Sensors::run() /* check parameters for updates */ parameter_update_poll(); - - /* check rc parameter map for updates */ - _rc_update.rc_parameter_map_poll(_parameter_handles); } - /* Look for new r/c input data */ - _rc_update.rc_poll(_parameter_handles); - perf_end(_loop_perf); } @@ -599,9 +558,6 @@ The provided functionality includes: If there are multiple of the same type, do voting and failover handling. Then apply the board rotation and temperature calibration (if enabled). And finally publish the data; one of the topics is `sensor_combined`, used by many parts of the system. -- Do RC channel mapping: read the raw input channels (`input_rc`), then apply the calibration, map the RC channels - to the configured channels & mode switches, low-pass filter, and then publish as `rc_channels` and - `manual_control_setpoint`. - Make sure the sensor drivers get the updated calibration parameters (scale & offset) when the parameters change or on startup. The sensor drivers use the ioctl interface for parameter updates. For this to work properly, the sensor drivers must already be running when `sensors` is started. @@ -653,7 +609,7 @@ Sensors *Sensors::instantiate(int argc, char *argv[]) return new Sensors(hil_enabled); } -int sensors_main(int argc, char *argv[]) +extern "C" __EXPORT int sensors_main(int argc, char *argv[]) { return Sensors::main(argc, argv); }