From a5d24645d894c4b58d2d0c42e2203e4cb640ba67 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 21 Oct 2019 14:29:25 -0400 Subject: [PATCH] WIP: sensors split rc_update into new standalone module --- ROMFS/px4fmu_common/init.d-posix/rcS | 1 + ROMFS/px4fmu_common/init.d/rcS | 6 + boards/aerotenna/ocpoc/ubuntu.cmake | 1 + boards/airmind/mindpx-v2/default.cmake | 1 + boards/atlflight/eagle/default.cmake | 1 + boards/atlflight/eagle/qurt-default.cmake | 1 + boards/atlflight/excelsior/default.cmake | 1 + boards/atlflight/excelsior/qurt-default.cmake | 1 + boards/auav/x21/default.cmake | 1 + boards/av/x-v1/default.cmake | 1 + boards/beaglebone/blue/cross.cmake | 1 + boards/beaglebone/blue/native.cmake | 1 + boards/bitcraze/crazyflie/default.cmake | 1 + boards/emlid/navio2/cross.cmake | 1 + boards/emlid/navio2/native.cmake | 1 + boards/holybro/kakutef7/default.cmake | 1 + boards/intel/aerofc-v1/default.cmake | 1 + boards/intel/aerofc-v1/rtps.cmake | 1 + boards/modalai/fc-v1/default.cmake | 1 + boards/mro/ctrl-zero-f7/default.cmake | 1 + boards/nxp/fmuk66-v3/default.cmake | 1 + boards/omnibus/f4sd/default.cmake | 1 + boards/parrot/bebop/default.cmake | 1 + boards/px4/fmu-v2/default.cmake | 7 +- boards/px4/fmu-v2/fixedwing.cmake | 7 +- boards/px4/fmu-v2/lpe.cmake | 1 + boards/px4/fmu-v2/multicopter.cmake | 1 + boards/px4/fmu-v2/rover.cmake | 1 + boards/px4/fmu-v2/test.cmake | 1 + boards/px4/fmu-v3/default.cmake | 9 +- boards/px4/fmu-v3/rtps.cmake | 1 + boards/px4/fmu-v3/stackcheck.cmake | 1 + boards/px4/fmu-v4/default.cmake | 1 + boards/px4/fmu-v4/rtps.cmake | 1 + boards/px4/fmu-v4/stackcheck.cmake | 1 + boards/px4/fmu-v4pro/default.cmake | 1 + boards/px4/fmu-v4pro/rtps.cmake | 1 + boards/px4/fmu-v5/critmonitor.cmake | 1 + boards/px4/fmu-v5/default.cmake | 1 + boards/px4/fmu-v5/fixedwing.cmake | 1 + boards/px4/fmu-v5/irqmonitor.cmake | 1 + boards/px4/fmu-v5/multicopter.cmake | 1 + boards/px4/fmu-v5/rover.cmake | 1 + boards/px4/fmu-v5/rtps.cmake | 1 + boards/px4/fmu-v5/stackcheck.cmake | 1 + boards/px4/fmu-v5x/default.cmake | 1 + boards/px4/raspberrypi/cross.cmake | 1 + boards/px4/raspberrypi/native.cmake | 1 + boards/px4/sitl/default.cmake | 1 + boards/px4/sitl/rtps.cmake | 1 + boards/px4/sitl/test.cmake | 1 + boards/uvify/core/default.cmake | 1 + src/modules/rc_update/CMakeLists.txt | 42 +++ src/modules/rc_update/parameters.cpp | 325 ++++++++++++++++++ src/modules/rc_update/parameters.h | 207 +++++++++++ .../{sensors => rc_update}/rc_params.c | 0 .../{sensors => rc_update}/rc_update.cpp | 174 ++++++++-- .../{sensors => rc_update}/rc_update.h | 67 ++-- src/modules/sensors/CMakeLists.txt | 1 - src/modules/sensors/parameters.cpp | 276 +-------------- src/modules/sensors/parameters.h | 148 +------- src/modules/sensors/sensors.cpp | 76 ++-- 62 files changed, 863 insertions(+), 530 deletions(-) create mode 100644 src/modules/rc_update/CMakeLists.txt create mode 100644 src/modules/rc_update/parameters.cpp create mode 100644 src/modules/rc_update/parameters.h rename src/modules/{sensors => rc_update}/rc_params.c (100%) rename src/modules/{sensors => rc_update}/rc_update.cpp (83%) rename src/modules/{sensors => rc_update}/rc_update.h (71%) diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index 17f00d9e2441..bc9fa9100121 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/ROMFS/px4fmu_common/init.d-posix/rcS @@ -209,6 +209,7 @@ simulator start -c $simulator_tcp_port tone_alarm start gpssim start sensors start +rc_update start commander start navigator start diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 8dc71cffd3ac..44f561034c7a 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/ubuntu.cmake b/boards/aerotenna/ocpoc/ubuntu.cmake index f5a9b2bf2f36..f1954b259627 100644 --- a/boards/aerotenna/ocpoc/ubuntu.cmake +++ b/boards/aerotenna/ocpoc/ubuntu.cmake @@ -48,6 +48,7 @@ px4_add_board( mc_pos_control navigator battery_status + rc_update sensors sih #simulator diff --git a/boards/airmind/mindpx-v2/default.cmake b/boards/airmind/mindpx-v2/default.cmake index 2707f24cc147..eacc05a1caf7 100644 --- a/boards/airmind/mindpx-v2/default.cmake +++ b/boards/airmind/mindpx-v2/default.cmake @@ -66,6 +66,7 @@ px4_add_board( mc_pos_control navigator battery_status + rc_update sensors sih vmount diff --git a/boards/atlflight/eagle/default.cmake b/boards/atlflight/eagle/default.cmake index 62b7d6fd5556..c0a984bbf083 100644 --- a/boards/atlflight/eagle/default.cmake +++ b/boards/atlflight/eagle/default.cmake @@ -82,6 +82,7 @@ px4_add_board( mc_att_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 1b9ca0f58529..d7e410036e4e 100644 --- a/boards/atlflight/eagle/qurt-default.cmake +++ b/boards/atlflight/eagle/qurt-default.cmake @@ -72,6 +72,7 @@ px4_add_board( local_position_estimator mc_att_control mc_pos_control + rc_update sensors #sih vmount diff --git a/boards/atlflight/excelsior/default.cmake b/boards/atlflight/excelsior/default.cmake index 7fc6cce92328..f9c333fa8325 100644 --- a/boards/atlflight/excelsior/default.cmake +++ b/boards/atlflight/excelsior/default.cmake @@ -82,6 +82,7 @@ px4_add_board( mc_att_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 acbd5099b993..bed6c282a392 100644 --- a/boards/atlflight/excelsior/qurt-default.cmake +++ b/boards/atlflight/excelsior/qurt-default.cmake @@ -72,6 +72,7 @@ px4_add_board( local_position_estimator mc_att_control mc_pos_control + rc_update sensors sih vmount diff --git a/boards/auav/x21/default.cmake b/boards/auav/x21/default.cmake index 23f44146e30b..e244378eb53e 100644 --- a/boards/auav/x21/default.cmake +++ b/boards/auav/x21/default.cmake @@ -71,6 +71,7 @@ px4_add_board( mc_pos_control navigator battery_status + rc_update sensors sih vmount diff --git a/boards/av/x-v1/default.cmake b/boards/av/x-v1/default.cmake index 2ee43312d0d1..a934d3efa6b8 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 navigator battery_status + rc_update sensors sih vmount diff --git a/boards/beaglebone/blue/cross.cmake b/boards/beaglebone/blue/cross.cmake index ae19a81487de..2fca47ca9370 100644 --- a/boards/beaglebone/blue/cross.cmake +++ b/boards/beaglebone/blue/cross.cmake @@ -47,6 +47,7 @@ px4_add_board( mc_pos_control navigator battery_status + rc_update sensors sih vmount diff --git a/boards/beaglebone/blue/native.cmake b/boards/beaglebone/blue/native.cmake index 978de4866077..1e3d6409b498 100644 --- a/boards/beaglebone/blue/native.cmake +++ b/boards/beaglebone/blue/native.cmake @@ -45,6 +45,7 @@ px4_add_board( mc_pos_control navigator battery_status + rc_update sensors sih vmount diff --git a/boards/bitcraze/crazyflie/default.cmake b/boards/bitcraze/crazyflie/default.cmake index c96239134e37..b4a25b17cdd3 100644 --- a/boards/bitcraze/crazyflie/default.cmake +++ b/boards/bitcraze/crazyflie/default.cmake @@ -34,6 +34,7 @@ px4_add_board( mc_att_control mc_pos_control navigator + rc_update sensors sih #vtol_att_control diff --git a/boards/emlid/navio2/cross.cmake b/boards/emlid/navio2/cross.cmake index a72074b59e78..7d76e165219f 100644 --- a/boards/emlid/navio2/cross.cmake +++ b/boards/emlid/navio2/cross.cmake @@ -51,6 +51,7 @@ px4_add_board( mc_pos_control navigator battery_status + rc_update sensors sih #simulator diff --git a/boards/emlid/navio2/native.cmake b/boards/emlid/navio2/native.cmake index d001994211f8..b5e71ce720c8 100644 --- a/boards/emlid/navio2/native.cmake +++ b/boards/emlid/navio2/native.cmake @@ -49,6 +49,7 @@ px4_add_board( mc_pos_control navigator battery_status + rc_update sensors sih #simulator diff --git a/boards/holybro/kakutef7/default.cmake b/boards/holybro/kakutef7/default.cmake index 98cd405c4e56..84d870dc4467 100644 --- a/boards/holybro/kakutef7/default.cmake +++ b/boards/holybro/kakutef7/default.cmake @@ -45,6 +45,7 @@ px4_add_board( mc_pos_control navigator battery_status + rc_update sensors SYSTEMCMDS diff --git a/boards/intel/aerofc-v1/default.cmake b/boards/intel/aerofc-v1/default.cmake index 574d2981c3d4..6b285ea51ed1 100644 --- a/boards/intel/aerofc-v1/default.cmake +++ b/boards/intel/aerofc-v1/default.cmake @@ -51,6 +51,7 @@ px4_add_board( mc_pos_control navigator battery_status + rc_update sensors #sih vmount diff --git a/boards/intel/aerofc-v1/rtps.cmake b/boards/intel/aerofc-v1/rtps.cmake index f4aa2f4fe3b3..7129c3b87c1c 100644 --- a/boards/intel/aerofc-v1/rtps.cmake +++ b/boards/intel/aerofc-v1/rtps.cmake @@ -55,6 +55,7 @@ px4_add_board( micrortps_bridge navigator battery_status + rc_update sensors sih vmount diff --git a/boards/modalai/fc-v1/default.cmake b/boards/modalai/fc-v1/default.cmake index a9e6b887336d..253dbb799b2f 100644 --- a/boards/modalai/fc-v1/default.cmake +++ b/boards/modalai/fc-v1/default.cmake @@ -72,6 +72,7 @@ px4_add_board( mc_pos_control navigator rover_pos_control + rc_update sensors sih vmount diff --git a/boards/mro/ctrl-zero-f7/default.cmake b/boards/mro/ctrl-zero-f7/default.cmake index 473b53ba66c3..ab4db1fcd028 100644 --- a/boards/mro/ctrl-zero-f7/default.cmake +++ b/boards/mro/ctrl-zero-f7/default.cmake @@ -74,6 +74,7 @@ px4_add_board( mc_pos_control navigator battery_status + rc_update sensors sih vmount diff --git a/boards/nxp/fmuk66-v3/default.cmake b/boards/nxp/fmuk66-v3/default.cmake index b6901eb73196..a1596a536b88 100644 --- a/boards/nxp/fmuk66-v3/default.cmake +++ b/boards/nxp/fmuk66-v3/default.cmake @@ -70,6 +70,7 @@ px4_add_board( mc_pos_control navigator battery_status + rc_update sensors sih vmount diff --git a/boards/omnibus/f4sd/default.cmake b/boards/omnibus/f4sd/default.cmake index 002f95eab666..49b8ca3b7029 100644 --- a/boards/omnibus/f4sd/default.cmake +++ b/boards/omnibus/f4sd/default.cmake @@ -62,6 +62,7 @@ px4_add_board( mc_pos_control navigator battery_status + rc_update sensors sih #vmount diff --git a/boards/parrot/bebop/default.cmake b/boards/parrot/bebop/default.cmake index bd4f9646c9a9..ba8d40617b6f 100644 --- a/boards/parrot/bebop/default.cmake +++ b/boards/parrot/bebop/default.cmake @@ -38,6 +38,7 @@ px4_add_board( mc_att_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 c6dfa2bb2185..f33387275f80 100644 --- a/boards/px4/fmu-v2/default.cmake +++ b/boards/px4/fmu-v2/default.cmake @@ -62,7 +62,9 @@ px4_add_board( #uavcan MODULES + #airspeed_selector #attitude_estimator_q + battery_status camera_feedback commander dataman @@ -70,7 +72,6 @@ px4_add_board( events fw_att_control fw_pos_control_l1 - #rover_pos_control land_detector #landing_target_estimator load_mon @@ -80,11 +81,11 @@ px4_add_board( mc_att_control mc_pos_control navigator - battery_status + rc_update + #rover_pos_control sensors vmount vtol_att_control - #airspeed_selector SYSTEMCMDS bl_update diff --git a/boards/px4/fmu-v2/fixedwing.cmake b/boards/px4/fmu-v2/fixedwing.cmake index c90b5b88e8eb..a657dc3f5795 100644 --- a/boards/px4/fmu-v2/fixedwing.cmake +++ b/boards/px4/fmu-v2/fixedwing.cmake @@ -21,7 +21,7 @@ px4_add_board( adc #barometer # all available barometer drivers barometer/ms5611 - batt_smbus + #batt_smbus camera_capture camera_trigger differential_pressure # all available differential pressure drivers @@ -42,6 +42,8 @@ px4_add_board( #uavcan MODULES + airspeed_selector + battery_status camera_feedback commander dataman @@ -54,10 +56,9 @@ px4_add_board( logger mavlink navigator - battery_status + rc_update sensors vmount - airspeed_selector SYSTEMCMDS bl_update diff --git a/boards/px4/fmu-v2/lpe.cmake b/boards/px4/fmu-v2/lpe.cmake index 48ead8e6ee9d..df3773209ec8 100644 --- a/boards/px4/fmu-v2/lpe.cmake +++ b/boards/px4/fmu-v2/lpe.cmake @@ -77,6 +77,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 1d0eb6cc5e52..d0406bb0753b 100644 --- a/boards/px4/fmu-v2/multicopter.cmake +++ b/boards/px4/fmu-v2/multicopter.cmake @@ -55,6 +55,7 @@ px4_add_board( mc_pos_control navigator battery_status + rc_update sensors vmount diff --git a/boards/px4/fmu-v2/rover.cmake b/boards/px4/fmu-v2/rover.cmake index 1fa9c5dcf958..dbcc970f10b7 100644 --- a/boards/px4/fmu-v2/rover.cmake +++ b/boards/px4/fmu-v2/rover.cmake @@ -47,6 +47,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 a3de52cf9bfd..73b6eaa584de 100644 --- a/boards/px4/fmu-v2/test.cmake +++ b/boards/px4/fmu-v2/test.cmake @@ -77,6 +77,7 @@ px4_add_board( mc_pos_control navigator battery_status + rc_update sensors vmount vtol_att_control diff --git a/boards/px4/fmu-v3/default.cmake b/boards/px4/fmu-v3/default.cmake index e1d3b55586f3..8f84d11f148f 100644 --- a/boards/px4/fmu-v3/default.cmake +++ b/boards/px4/fmu-v3/default.cmake @@ -32,11 +32,11 @@ px4_add_board( #heater imu/adis16448 #imu # all available imu drivers + imu/icm20948 imu/l3gd20 imu/lsm303d imu/mpu6000 imu/mpu9250 - imu/icm20948 irlock lights/blinkm lights/rgbled @@ -61,7 +61,9 @@ px4_add_board( uavcan MODULES + airspeed_selector attitude_estimator_q + battery_status camera_feedback commander dataman @@ -69,7 +71,6 @@ px4_add_board( events fw_att_control fw_pos_control_l1 - rover_pos_control land_detector landing_target_estimator load_mon @@ -79,12 +80,12 @@ px4_add_board( mc_att_control mc_pos_control navigator - battery_status + rc_update + rover_pos_control sensors sih vmount vtol_att_control - airspeed_selector SYSTEMCMDS bl_update diff --git a/boards/px4/fmu-v3/rtps.cmake b/boards/px4/fmu-v3/rtps.cmake index 41bf5184327b..56b59f031e76 100644 --- a/boards/px4/fmu-v3/rtps.cmake +++ b/boards/px4/fmu-v3/rtps.cmake @@ -79,6 +79,7 @@ px4_add_board( micrortps_bridge navigator battery_status + rc_update sensors sih vmount diff --git a/boards/px4/fmu-v3/stackcheck.cmake b/boards/px4/fmu-v3/stackcheck.cmake index 2da5d1279602..5b550989b9f4 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 cfdcc98e5243..8d41dd0e24d4 100644 --- a/boards/px4/fmu-v4/default.cmake +++ b/boards/px4/fmu-v4/default.cmake @@ -67,6 +67,7 @@ px4_add_board( mc_pos_control navigator rover_pos_control + rc_update sensors sih vmount diff --git a/boards/px4/fmu-v4/rtps.cmake b/boards/px4/fmu-v4/rtps.cmake index 074a670b1844..a42d9d7c0ac4 100644 --- a/boards/px4/fmu-v4/rtps.cmake +++ b/boards/px4/fmu-v4/rtps.cmake @@ -68,6 +68,7 @@ px4_add_board( micrortps_bridge navigator battery_status + rc_update sensors sih vmount diff --git a/boards/px4/fmu-v4/stackcheck.cmake b/boards/px4/fmu-v4/stackcheck.cmake index 4090796eeb8c..bb0cec232a8b 100644 --- a/boards/px4/fmu-v4/stackcheck.cmake +++ b/boards/px4/fmu-v4/stackcheck.cmake @@ -65,6 +65,7 @@ px4_add_board( mc_pos_control navigator battery_status + rc_update sensors sih vmount diff --git a/boards/px4/fmu-v4pro/default.cmake b/boards/px4/fmu-v4pro/default.cmake index 70c3ba5c54fb..081a933a8733 100644 --- a/boards/px4/fmu-v4pro/default.cmake +++ b/boards/px4/fmu-v4pro/default.cmake @@ -77,6 +77,7 @@ px4_add_board( mc_pos_control navigator battery_status + rc_update sensors sih vmount diff --git a/boards/px4/fmu-v4pro/rtps.cmake b/boards/px4/fmu-v4pro/rtps.cmake index 12240d8827c7..a356038bf31a 100644 --- a/boards/px4/fmu-v4pro/rtps.cmake +++ b/boards/px4/fmu-v4pro/rtps.cmake @@ -77,6 +77,7 @@ px4_add_board( micrortps_bridge navigator battery_status + rc_update sensors sih vmount diff --git a/boards/px4/fmu-v5/critmonitor.cmake b/boards/px4/fmu-v5/critmonitor.cmake index 2230bd1683b3..bf3acd71f175 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 ba965890f6ca..c05311d13b73 100644 --- a/boards/px4/fmu-v5/default.cmake +++ b/boards/px4/fmu-v5/default.cmake @@ -79,6 +79,7 @@ px4_add_board( mc_pos_control navigator battery_status + rc_update sensors sih vmount diff --git a/boards/px4/fmu-v5/fixedwing.cmake b/boards/px4/fmu-v5/fixedwing.cmake index 57bf1989193a..b7b9ec72802f 100644 --- a/boards/px4/fmu-v5/fixedwing.cmake +++ b/boards/px4/fmu-v5/fixedwing.cmake @@ -56,6 +56,7 @@ px4_add_board( mavlink navigator battery_status + rc_update sensors vmount airspeed_selector diff --git a/boards/px4/fmu-v5/irqmonitor.cmake b/boards/px4/fmu-v5/irqmonitor.cmake index d4e36c90abb0..b60577fe597e 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 96c9d0b61f77..b1ea89757cf5 100644 --- a/boards/px4/fmu-v5/multicopter.cmake +++ b/boards/px4/fmu-v5/multicopter.cmake @@ -63,6 +63,7 @@ px4_add_board( mc_pos_control navigator battery_status + rc_update sensors sih vmount diff --git a/boards/px4/fmu-v5/rover.cmake b/boards/px4/fmu-v5/rover.cmake index 8d5524cb5909..b3ca5a9bf568 100644 --- a/boards/px4/fmu-v5/rover.cmake +++ b/boards/px4/fmu-v5/rover.cmake @@ -58,6 +58,7 @@ px4_add_board( mavlink navigator battery_status + rc_update sensors vmount diff --git a/boards/px4/fmu-v5/rtps.cmake b/boards/px4/fmu-v5/rtps.cmake index 0c7c515dcf7c..67e8a279ea42 100644 --- a/boards/px4/fmu-v5/rtps.cmake +++ b/boards/px4/fmu-v5/rtps.cmake @@ -77,6 +77,7 @@ px4_add_board( micrortps_bridge navigator battery_status + rc_update sensors sih vmount diff --git a/boards/px4/fmu-v5/stackcheck.cmake b/boards/px4/fmu-v5/stackcheck.cmake index 04613c0cba23..e4b77e928505 100644 --- a/boards/px4/fmu-v5/stackcheck.cmake +++ b/boards/px4/fmu-v5/stackcheck.cmake @@ -77,6 +77,7 @@ px4_add_board( #micrortps_bridge navigator battery_status + rc_update sensors sih vmount diff --git a/boards/px4/fmu-v5x/default.cmake b/boards/px4/fmu-v5x/default.cmake index ca7251d85586..4931571d855b 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 navigator rover_pos_control + rc_update sensors sih vmount diff --git a/boards/px4/raspberrypi/cross.cmake b/boards/px4/raspberrypi/cross.cmake index 339da358c2fe..993bff1637cd 100644 --- a/boards/px4/raspberrypi/cross.cmake +++ b/boards/px4/raspberrypi/cross.cmake @@ -44,6 +44,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 4c0bff6d7e51..6dbb7e160f4b 100644 --- a/boards/px4/raspberrypi/native.cmake +++ b/boards/px4/raspberrypi/native.cmake @@ -42,6 +42,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 8cade4ed946f..59a744bad4c0 100644 --- a/boards/px4/sitl/default.cmake +++ b/boards/px4/sitl/default.cmake @@ -40,6 +40,7 @@ px4_add_board( mc_pos_control navigator replay + rc_update sensors simulator vmount diff --git a/boards/px4/sitl/rtps.cmake b/boards/px4/sitl/rtps.cmake index 5a676e40aaf6..9ebe99dcd915 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 sensors simulator vmount diff --git a/boards/px4/sitl/test.cmake b/boards/px4/sitl/test.cmake index 737717daf685..08ba056b629c 100644 --- a/boards/px4/sitl/test.cmake +++ b/boards/px4/sitl/test.cmake @@ -40,6 +40,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 b17507af9046..819c2e484291 100644 --- a/boards/uvify/core/default.cmake +++ b/boards/uvify/core/default.cmake @@ -71,6 +71,7 @@ px4_add_board( mc_pos_control navigator battery_status + rc_update sensors sih vmount diff --git a/src/modules/rc_update/CMakeLists.txt b/src/modules/rc_update/CMakeLists.txt new file mode 100644 index 000000000000..db99402d108f --- /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 000000000000..9750204e7fc3 --- /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 000000000000..016a1be8708a --- /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 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]; + + 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 bb8824a8ebbb..f278b5aeedb7 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(); +} + +RCUpdate::~RCUpdate() +{ + perf_free(_loop_perf); } -void RCUpdate::update_rc_functions() +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 71% rename from src/modules/sensors/rc_update.h rename to src/modules/rc_update/rc_update.h index 5f805919d814..5af2e1ef730f 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 @@ class RCUpdate * * @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 @@ class RCUpdate 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 @@ class RCUpdate -} /* namespace sensors */ +} /* namespace RCUpdate */ diff --git a/src/modules/sensors/CMakeLists.txt b/src/modules/sensors/CMakeLists.txt index 0a5b6620f1fe..177f7e38e04b 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 9df49a7304d3..688ee98453b7 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("CAL_ACC0_ID"); (void)param_find("CAL_GYRO0_ID"); @@ -184,186 +90,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 @@ -381,8 +109,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 1cfc92f51c5e..08b6a6c3bc24 100644 --- a/src/modules/sensors/parameters.h +++ b/src/modules/sensors/parameters.h @@ -40,29 +40,12 @@ * * @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 +55,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 +63,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 +90,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 5efcf28592df..e3b90d748862 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -41,16 +41,6 @@ * @author Beat Küng */ -#include - -#include -#include -#include -#include -#include -#include -#include - #include #include #include @@ -62,32 +52,35 @@ #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 +95,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 +150,6 @@ class Sensors : public ModuleBase, public ModuleParams Parameters _parameters{}; /**< local copies of interesting parameters */ ParameterHandles _parameter_handles{}; /**< handles for interesting parameters */ - RCUpdate _rc_update; VotedSensorsUpdate _voted_sensors_update; @@ -211,7 +194,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 +219,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 +415,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 +507,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 +568,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 +619,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); }