diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1060_rover b/ROMFS/px4fmu_common/init.d-posix/airframes/1060_rover index dcb0500f09..ccf86b9f8a 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1060_rover +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1060_rover @@ -8,6 +8,7 @@ if [ $AUTOCNF = yes ] then param set GND_L1_DIST 5 + param set GND_L1_PERIOD 10 param set GND_SP_CTRL_MODE 1 param set GND_SPEED_D 0.001 param set GND_SPEED_I 3 @@ -16,7 +17,6 @@ then param set GND_SPEED_THR_SC 1 param set GND_SPEED_TRIM 4 param set GND_THR_CRUISE 0.3 - param set GND_THR_IDLE 0 param set GND_THR_MAX 0.5 param set GND_THR_MIN 0 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1061_r1_rover b/ROMFS/px4fmu_common/init.d-posix/airframes/1061_r1_rover index 1d3a436cc2..ddd04393a1 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1061_r1_rover +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1061_r1_rover @@ -17,7 +17,6 @@ then param set GND_SPEED_THR_SC 1 param set GND_SPEED_TRIM 4 param set GND_THR_CRUISE 0.3 - param set GND_THR_IDLE 0 param set GND_THR_MAX 0.5 param set GND_THR_MIN 0 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1062_tf-r1 b/ROMFS/px4fmu_common/init.d-posix/airframes/1062_tf-r1 index 80d70249f4..43743a2be9 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1062_tf-r1 +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1062_tf-r1 @@ -22,7 +22,6 @@ then param set GND_SPEED_TRIM 15 param set GND_SPEED_MAX 25 param set GND_THR_CRUISE 0.3 - param set GND_THR_IDLE 0 param set GND_THR_MAX 0.5 param set GND_THR_MIN 0 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1070_boat b/ROMFS/px4fmu_common/init.d-posix/airframes/1070_boat index cac17b7f86..a8c1b52f74 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1070_boat +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1070_boat @@ -17,7 +17,6 @@ then param set GND_SPEED_THR_SC 1 param set GND_SPEED_TRIM 1 param set GND_THR_CRUISE 0.85 - param set GND_THR_IDLE 0 param set GND_THR_MAX 1 param set GND_THR_MIN 0 diff --git a/ROMFS/px4fmu_common/init.d/airframes/50000_generic_ground_vehicle b/ROMFS/px4fmu_common/init.d/airframes/50000_generic_ground_vehicle index f21f131435..2e40df0ff3 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/50000_generic_ground_vehicle +++ b/ROMFS/px4fmu_common/init.d/airframes/50000_generic_ground_vehicle @@ -27,21 +27,18 @@ param set-default FW_AIRSPD_MAX 3 param set-default FW_AIRSPD_MIN 0 param set-default FW_AIRSPD_TRIM 1 -param set-default GND_L1_DIST 10 +# Settings for a typical wheelbase 0f 0.3m +param set-default GND_L1_DIST 1 +param set-default GND_L1_PERIOD 5 param set-default GND_SP_CTRL_MODE 1 param set-default GND_SPEED_P 0.25 param set-default GND_SPEED_I 3 param set-default GND_SPEED_D 0.001 param set-default GND_SPEED_IMAX 0.125 param set-default GND_SPEED_THR_SC 1 -param set-default GND_THR_IDLE 0 param set-default GND_THR_CRUISE 0.3 param set-default GND_THR_MAX 0.5 param set-default GND_THR_MIN 0 -param set-default GND_WR_P 2 -param set-default GND_WR_I 0.9674 -param set-default GND_WR_IMAX 0.1 -param set-default GND_WR_D 1.2 param set-default MIS_LTRMIN_ALT 0.01 param set-default MIS_TAKEOFF_ALT 0.01 diff --git a/ROMFS/px4fmu_common/init.d/airframes/50003_aion_robotics_r1_rover b/ROMFS/px4fmu_common/init.d/airframes/50003_aion_robotics_r1_rover index 3000f92354..c36e815517 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/50003_aion_robotics_r1_rover +++ b/ROMFS/px4fmu_common/init.d/airframes/50003_aion_robotics_r1_rover @@ -35,7 +35,6 @@ param set-default FW_AIRSPD_MAX 3 param set-default GND_SP_CTRL_MODE 1 param set-default GND_L1_DIST 5 param set-default GND_L1_PERIOD 3 -param set-default GND_THR_IDLE 0 param set-default GND_THR_CRUISE 0.7 param set-default GND_THR_MAX 0.5 diff --git a/ROMFS/px4fmu_common/init.d/airframes/50004_nxpcup_car_dfrobot_gpx b/ROMFS/px4fmu_common/init.d/airframes/50004_nxpcup_car_dfrobot_gpx index 85c4b9dfd7..73fdffb03c 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/50004_nxpcup_car_dfrobot_gpx +++ b/ROMFS/px4fmu_common/init.d/airframes/50004_nxpcup_car_dfrobot_gpx @@ -32,7 +32,6 @@ param set-default FW_AIRSPD_MIN 0 param set-default FW_AIRSPD_TRIM 1 param set-default FW_AIRSPD_MAX 3 -param set-default GND_THR_IDLE 0 param set-default GND_THR_CRUISE 0.3 param set-default GND_THR_MAX 0.5 diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface index 61a096dc5d..be788effa1 100644 --- a/ROMFS/px4fmu_common/init.d/rc.interface +++ b/ROMFS/px4fmu_common/init.d/rc.interface @@ -67,22 +67,6 @@ fi # if [ $OUTPUT_MODE != none ] then - if [ $OUTPUT_MODE = mkblctrl ] - then - if [ $MKBLCTRL_MODE = x ] - then - set MKBLCTRL_ARG "-mkmode x" - fi - if [ $MKBLCTRL_MODE = + ] - then - set MKBLCTRL_ARG "-mkmode +" - fi - - if ! mkblctrl $MKBLCTRL_ARG - then - tune_control play error - fi - fi if [ $OUTPUT_MODE = hil -o $OUTPUT_MODE = sim ] then @@ -153,12 +137,7 @@ then fi fi - if [ $OUTPUT_MODE = mkblctrl ] - then - set OUTPUT_DEV /dev/mkblctrl0 - else - set OUTPUT_DEV /dev/pwm_output0 - fi + set OUTPUT_DEV /dev/pwm_output0 if [ $OUTPUT_MODE = uavcan_esc ] then diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 50d42d4dab..6c2d616505 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -37,8 +37,6 @@ set MIXER none set MIXER_AUX none set MIXER_FILE none set MIXER_EXTRA none -set MK_MODE none -set MKBLCTRL_ARG "" set OUTPUT_MODE none set PARAM_FILE /fs/microsd/params set PWM_OUT none @@ -591,8 +589,6 @@ unset MAV_TYPE unset MIXER unset MIXER_AUX unset MIXER_FILE -unset MK_MODE -unset MKBLCTRL_ARG unset OUTPUT_MODE unset PARAM_DEFAULTS_VER unset PARAM_FILE diff --git a/ROMFS/px4fmu_common/mixers-sitl/vectored6dof_sitl.main.mix b/ROMFS/px4fmu_common/mixers-sitl/vectored6dof_sitl.main.mix index 6a1f1f63f9..1b27ef1640 100644 --- a/ROMFS/px4fmu_common/mixers-sitl/vectored6dof_sitl.main.mix +++ b/ROMFS/px4fmu_common/mixers-sitl/vectored6dof_sitl.main.mix @@ -1,32 +1,40 @@ # Motor 1 -M: 2 +M: 3 S: 0 2 -4000 -4000 0 -4000 +4000 S: 0 3 +4000 +4000 0 -4000 +4000 +S: 0 4 -4000 -4000 0 -4000 +4000 # Motor 2 -M: 2 +M: 3 S: 0 2 +4000 +4000 0 -4000 +4000 S: 0 3 +4000 +4000 0 -4000 +4000 +S: 0 4 +4000 +4000 0 -4000 +4000 # Motor 3 -M: 2 +M: 3 S: 0 2 -4000 -4000 0 -4000 +4000 S: 0 3 +4000 +4000 0 -4000 +4000 +S: 0 4 +4000 +4000 0 -4000 +4000 # Motor 4 -M: 2 +M: 3 S: 0 2 +4000 +4000 0 -4000 +4000 S: 0 3 +4000 +4000 0 -4000 +4000 +S: 0 4 -4000 -4000 0 -4000 +4000 # Motor 5 -M: 2 +M: 3 S: 0 0 -4000 -4000 0 -4000 +4000 S: 0 1 +4000 +4000 0 -4000 +4000 +S: 0 5 -4000 -4000 0 -4000 +4000 # Motor 6 -M: 2 +M: 3 S: 0 0 -4000 -4000 0 -4000 +4000 S: 0 1 -4000 -4000 0 -4000 +4000 +S: 0 5 +4000 +4000 0 -4000 +4000 # Motor 7 -M: 2 +M: 3 S: 0 0 +4000 +4000 0 -4000 +4000 S: 0 1 +4000 +4000 0 -4000 +4000 +S: 0 5 +4000 +4000 0 -4000 +4000 # Motor 8 -M: 2 +M: 3 S: 0 0 +4000 +4000 0 -4000 +4000 S: 0 1 -4000 -4000 0 -4000 +4000 +S: 0 5 -4000 -4000 0 -4000 +4000 diff --git a/ROMFS/px4fmu_common/mixers/rover_diff_and_servo.main.mix b/ROMFS/px4fmu_common/mixers/rover_diff_and_servo.main.mix index c6c8dcbe92..3f707dd21a 100644 --- a/ROMFS/px4fmu_common/mixers/rover_diff_and_servo.main.mix +++ b/ROMFS/px4fmu_common/mixers/rover_diff_and_servo.main.mix @@ -13,25 +13,28 @@ Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 (ro See the README for more information on the scaler format. -Output 0 +Output 1 - Empty ----------------------------------------- Z: -Steering mixer using roll on output 1 +Output 2 - Steering mixer using yaw ------------------------------------------ M: 1 +O: 10000 10000 0 -10000 10000 5000 S: 0 2 10000 10000 0 -10000 10000 -Output 2 +Output 3 - Left row of wheels using yaw and throttle (1s rise time) ------------------------------------------ M: 2 +O: 10000 10000 0 -10000 10000 10000 S: 0 2 -500 -500 0 0 10000 S: 0 3 10000 10000 0 -10000 10000 -Output 3 +Output 4 - Right row of wheels using yaw and throttle (1s rise time) ------------------------------------------ M: 2 +O: 10000 10000 0 -10000 10000 10000 S: 0 2 500 500 0 0 10000 S: 0 3 10000 10000 0 -10000 10000 diff --git a/ROMFS/px4fmu_common/mixers/rover_generic.main.mix b/ROMFS/px4fmu_common/mixers/rover_generic.main.mix index 9605ee3edd..fee98ac1de 100644 --- a/ROMFS/px4fmu_common/mixers/rover_generic.main.mix +++ b/ROMFS/px4fmu_common/mixers/rover_generic.main.mix @@ -13,23 +13,25 @@ Inputs to the mixer come from channel group 0 (vehicle attitude), channels 2 (ya See the README for more information on the scaler format. -Output 0 +Output 1: Empty --------------------------------------- Z: -Steering mixer using roll on output 1 +Output 2: Steering mixer using yaw, with 0.5s rise time --------------------------------------- M: 1 +O: 10000 10000 0 -10000 10000 5000 S: 0 2 10000 10000 0 -10000 10000 -Output 2 +Output 3: Empty --------------------------------------- This mixer is empty. Z: -Output 3 +Output 4: Throttle with 2s rise time --------------------------------------- M: 1 +O: 10000 10000 0 -10000 10000 20000 S: 0 3 10000 10000 0 -10000 10000 diff --git a/ROMFS/px4fmu_common/mixers/vectored6dof.main.mix b/ROMFS/px4fmu_common/mixers/vectored6dof.main.mix index 6a1f1f63f9..0ef3b6501c 100644 --- a/ROMFS/px4fmu_common/mixers/vectored6dof.main.mix +++ b/ROMFS/px4fmu_common/mixers/vectored6dof.main.mix @@ -1,32 +1,40 @@ # Motor 1 -M: 2 +M: 3 S: 0 2 -4000 -4000 0 -4000 +4000 S: 0 3 +4000 +4000 0 -4000 +4000 +S: 0 4 +4000 +4000 0 -4000 +4000 # Motor 2 -M: 2 +M: 3 S: 0 2 +4000 +4000 0 -4000 +4000 S: 0 3 +4000 +4000 0 -4000 +4000 +S: 0 4 -4000 -4000 0 -4000 +4000 # Motor 3 -M: 2 +M: 3 S: 0 2 -4000 -4000 0 -4000 +4000 S: 0 3 +4000 +4000 0 -4000 +4000 +S: 0 4 -4000 -4000 0 -4000 +4000 # Motor 4 -M: 2 +M: 3 S: 0 2 +4000 +4000 0 -4000 +4000 S: 0 3 +4000 +4000 0 -4000 +4000 +S: 0 4 +4000 +4000 0 -4000 +4000 # Motor 5 -M: 2 +M: 3 S: 0 0 -4000 -4000 0 -4000 +4000 S: 0 1 +4000 +4000 0 -4000 +4000 +S: 0 5 +4000 +4000 0 -4000 +4000 # Motor 6 -M: 2 +M: 3 S: 0 0 -4000 -4000 0 -4000 +4000 S: 0 1 -4000 -4000 0 -4000 +4000 +S: 0 5 -4000 -4000 0 -4000 +4000 # Motor 7 -M: 2 +M: 3 S: 0 0 +4000 +4000 0 -4000 +4000 S: 0 1 +4000 +4000 0 -4000 +4000 +S: 0 5 -4000 -4000 0 -4000 +4000 # Motor 8 -M: 2 +M: 3 S: 0 0 +4000 +4000 0 -4000 +4000 S: 0 1 -4000 -4000 0 -4000 +4000 +S: 0 5 +4000 +4000 0 -4000 +4000 diff --git a/boards/airmind/mindpx-v2/default.cmake b/boards/airmind/mindpx-v2/default.cmake index 05b4da96de..507f621ee8 100644 --- a/boards/airmind/mindpx-v2/default.cmake +++ b/boards/airmind/mindpx-v2/default.cmake @@ -35,7 +35,6 @@ px4_add_board( lights/rgbled #lights/rgbled_ncp5623c magnetometer # all available magnetometer drivers - mkblctrl optical_flow # all available optical flow drivers #osd pca9685 diff --git a/boards/av/x-v1/default.cmake b/boards/av/x-v1/default.cmake index f65c54bd4d..9d9e681e5d 100644 --- a/boards/av/x-v1/default.cmake +++ b/boards/av/x-v1/default.cmake @@ -34,7 +34,6 @@ px4_add_board( #lights/rgbled #lights/rgbled_ncp5623c magnetometer # all available magnetometer drivers - mkblctrl optical_flow # all available optical flow drivers #osd pca9685 diff --git a/boards/cuav/nora/default.cmake b/boards/cuav/nora/default.cmake index 188f93cf64..c5688aeb9a 100644 --- a/boards/cuav/nora/default.cmake +++ b/boards/cuav/nora/default.cmake @@ -39,7 +39,6 @@ px4_add_board( lights/rgbled_ncp5623c lights/rgbled_pwm magnetometer # all available magnetometer drivers - mkblctrl optical_flow # all available optical flow drivers #osd pca9685 diff --git a/boards/cuav/x7pro/default.cmake b/boards/cuav/x7pro/default.cmake index ad721b553a..1e011dcc1e 100644 --- a/boards/cuav/x7pro/default.cmake +++ b/boards/cuav/x7pro/default.cmake @@ -30,7 +30,7 @@ px4_add_board( gps heater #imu # all available imu drivers - imu/adis16448 + imu/analog_devices/adis16448 imu/adis16477 imu/adis16497 imu/bosch/bmi088 @@ -42,7 +42,6 @@ px4_add_board( lights/rgbled_ncp5623c lights/rgbled_pwm magnetometer # all available magnetometer drivers - mkblctrl optical_flow # all available optical flow drivers #osd pca9685 diff --git a/boards/cubepilot/cubeorange/console.cmake b/boards/cubepilot/cubeorange/console.cmake index 4f52a483f0..043e4ce6cd 100644 --- a/boards/cubepilot/cubeorange/console.cmake +++ b/boards/cubepilot/cubeorange/console.cmake @@ -30,7 +30,7 @@ px4_add_board( gps #heater #imu # all available imu drivers - imu/adis16448 + imu/analog_devices/adis16448 imu/adis16477 imu/adis16497 imu/invensense/icm20602 @@ -41,7 +41,6 @@ px4_add_board( lights/rgbled lights/rgbled_ncp5623c magnetometer # all available magnetometer drivers - mkblctrl optical_flow # all available optical flow drivers #osd pca9685 diff --git a/boards/cubepilot/cubeorange/default.cmake b/boards/cubepilot/cubeorange/default.cmake index 42fafe571f..0ba9f31077 100644 --- a/boards/cubepilot/cubeorange/default.cmake +++ b/boards/cubepilot/cubeorange/default.cmake @@ -30,7 +30,7 @@ px4_add_board( gps #heater #imu # all available imu drivers - imu/adis16448 + imu/analog_devices/adis16448 imu/adis16477 imu/adis16497 imu/invensense/icm20602 @@ -41,7 +41,6 @@ px4_add_board( lights/rgbled lights/rgbled_ncp5623c magnetometer # all available magnetometer drivers - mkblctrl optical_flow # all available optical flow drivers #osd pca9685 diff --git a/boards/cubepilot/cubeyellow/console.cmake b/boards/cubepilot/cubeyellow/console.cmake index 173b7b9edd..2f2201d4d3 100644 --- a/boards/cubepilot/cubeyellow/console.cmake +++ b/boards/cubepilot/cubeyellow/console.cmake @@ -29,7 +29,7 @@ px4_add_board( gps #heater #imu # all available imu drivers - imu/adis16448 + imu/analog_devices/adis16448 imu/adis16477 imu/adis16497 imu/invensense/icm20602 @@ -40,7 +40,6 @@ px4_add_board( lights/rgbled lights/rgbled_ncp5623c magnetometer # all available magnetometer drivers - mkblctrl optical_flow # all available optical flow drivers #osd pca9685 diff --git a/boards/cubepilot/cubeyellow/default.cmake b/boards/cubepilot/cubeyellow/default.cmake index f06a255ac6..4b3a5d67b3 100644 --- a/boards/cubepilot/cubeyellow/default.cmake +++ b/boards/cubepilot/cubeyellow/default.cmake @@ -29,7 +29,7 @@ px4_add_board( gps #heater #imu # all available imu drivers - imu/adis16448 + imu/analog_devices/adis16448 imu/adis16477 imu/adis16497 imu/invensense/icm20602 @@ -40,7 +40,6 @@ px4_add_board( lights/rgbled lights/rgbled_ncp5623c magnetometer # all available magnetometer drivers - mkblctrl optical_flow # all available optical flow drivers #osd pca9685 diff --git a/boards/holybro/durandal-v1/default.cmake b/boards/holybro/durandal-v1/default.cmake index 7cc7c16d38..82c153a347 100644 --- a/boards/holybro/durandal-v1/default.cmake +++ b/boards/holybro/durandal-v1/default.cmake @@ -29,7 +29,7 @@ px4_add_board( gps heater #imu # all available imu drivers - imu/adis16448 + imu/analog_devices/adis16448 imu/adis16477 imu/adis16497 imu/bosch/bmi088 @@ -39,7 +39,6 @@ px4_add_board( lights/rgbled lights/rgbled_ncp5623c magnetometer # all available magnetometer drivers - mkblctrl optical_flow # all available optical flow drivers #osd pca9685 diff --git a/boards/holybro/pix32v5/default.cmake b/boards/holybro/pix32v5/default.cmake index 2654472a25..20f8115817 100644 --- a/boards/holybro/pix32v5/default.cmake +++ b/boards/holybro/pix32v5/default.cmake @@ -28,7 +28,7 @@ px4_add_board( gps #heater #imu # all available imu drivers - imu/adis16448 + imu/analog_devices/adis16448 imu/adis16477 imu/adis16497 imu/bosch/bmi055 @@ -41,7 +41,6 @@ px4_add_board( lights/rgbled_ncp5623c lights/rgbled_pwm magnetometer # all available magnetometer drivers - mkblctrl optical_flow # all available optical flow drivers #osd pca9685 diff --git a/boards/modalai/fc-v1/default.cmake b/boards/modalai/fc-v1/default.cmake index bc08640995..54bfb8901f 100644 --- a/boards/modalai/fc-v1/default.cmake +++ b/boards/modalai/fc-v1/default.cmake @@ -32,7 +32,6 @@ px4_add_board( lights/rgbled lights/rgbled_ncp5623c magnetometer # all available magnetometer drivers - mkblctrl #optical_flow # all available optical flow drivers #osd pca9685 diff --git a/boards/mro/ctrl-zero-f7-oem/default.cmake b/boards/mro/ctrl-zero-f7-oem/default.cmake index a3bcb9c124..887020703c 100644 --- a/boards/mro/ctrl-zero-f7-oem/default.cmake +++ b/boards/mro/ctrl-zero-f7-oem/default.cmake @@ -37,7 +37,6 @@ px4_add_board( lights/rgbled lights/rgbled_ncp5623c magnetometer # all available magnetometer drivers - mkblctrl #optical_flow # all available optical flow drivers #osd pca9685 diff --git a/boards/mro/ctrl-zero-f7/default.cmake b/boards/mro/ctrl-zero-f7/default.cmake index 25209d9672..169d907c6a 100644 --- a/boards/mro/ctrl-zero-f7/default.cmake +++ b/boards/mro/ctrl-zero-f7/default.cmake @@ -37,7 +37,6 @@ px4_add_board( lights/rgbled lights/rgbled_ncp5623c magnetometer # all available magnetometer drivers - mkblctrl #optical_flow # all available optical flow drivers #osd pca9685 diff --git a/boards/mro/pixracerpro/default.cmake b/boards/mro/pixracerpro/default.cmake index af008a3dfe..0bd854ff26 100644 --- a/boards/mro/pixracerpro/default.cmake +++ b/boards/mro/pixracerpro/default.cmake @@ -37,7 +37,6 @@ px4_add_board( lights/rgbled lights/rgbled_ncp5623c magnetometer # all available magnetometer drivers - mkblctrl optical_flow # all available optical flow drivers #osd pca9685 diff --git a/boards/mro/x21-777/default.cmake b/boards/mro/x21-777/default.cmake index 38649c6f90..3f091aa63e 100644 --- a/boards/mro/x21-777/default.cmake +++ b/boards/mro/x21-777/default.cmake @@ -37,7 +37,6 @@ px4_add_board( lights/rgbled lights/rgbled_ncp5623c magnetometer # all available magnetometer drivers - mkblctrl #optical_flow # all available optical flow drivers #osd pca9685 diff --git a/boards/mro/x21/default.cmake b/boards/mro/x21/default.cmake index 283ca92b68..baf3ff0123 100644 --- a/boards/mro/x21/default.cmake +++ b/boards/mro/x21/default.cmake @@ -35,7 +35,6 @@ px4_add_board( lights/rgbled #lights/rgbled_ncp5623c magnetometer # all available magnetometer drivers - mkblctrl #optical_flow # all available optical flow drivers optical_flow/px4flow #osd diff --git a/boards/nxp/fmuk66-e/default.cmake b/boards/nxp/fmuk66-e/default.cmake index 40a617bfa9..51cbe7180b 100644 --- a/boards/nxp/fmuk66-e/default.cmake +++ b/boards/nxp/fmuk66-e/default.cmake @@ -35,7 +35,6 @@ px4_add_board( lights/rgbled_ncp5623c lights/rgbled_pwm magnetometer # all available magnetometer drivers - mkblctrl #optical_flow # all available optical flow drivers optical_flow/px4flow #osd diff --git a/boards/nxp/fmuk66-e/socketcan.cmake b/boards/nxp/fmuk66-e/socketcan.cmake index 3ef9be2606..db21a3a0b7 100644 --- a/boards/nxp/fmuk66-e/socketcan.cmake +++ b/boards/nxp/fmuk66-e/socketcan.cmake @@ -34,7 +34,6 @@ px4_add_board( lights/rgbled_ncp5623c lights/rgbled_pwm magnetometer # all available magnetometer drivers - mkblctrl #optical_flow # all available optical flow drivers optical_flow/px4flow #osd diff --git a/boards/nxp/fmuk66-v3/default.cmake b/boards/nxp/fmuk66-v3/default.cmake index 81b403fb6b..dccf51ad58 100644 --- a/boards/nxp/fmuk66-v3/default.cmake +++ b/boards/nxp/fmuk66-v3/default.cmake @@ -35,7 +35,6 @@ px4_add_board( lights/rgbled_ncp5623c lights/rgbled_pwm magnetometer # all available magnetometer drivers - mkblctrl #optical_flow # all available optical flow drivers optical_flow/px4flow #osd diff --git a/boards/nxp/fmuk66-v3/rtps.cmake b/boards/nxp/fmuk66-v3/rtps.cmake index f00cee07e7..ebf7f7beeb 100644 --- a/boards/nxp/fmuk66-v3/rtps.cmake +++ b/boards/nxp/fmuk66-v3/rtps.cmake @@ -34,7 +34,6 @@ px4_add_board( lights/rgbled_ncp5623c lights/rgbled_pwm magnetometer # all available magnetometer drivers - mkblctrl #optical_flow # all available optical flow drivers optical_flow/px4flow #osd diff --git a/boards/nxp/fmuk66-v3/socketcan.cmake b/boards/nxp/fmuk66-v3/socketcan.cmake index d8cd18d5a6..0aee173d69 100644 --- a/boards/nxp/fmuk66-v3/socketcan.cmake +++ b/boards/nxp/fmuk66-v3/socketcan.cmake @@ -34,7 +34,6 @@ px4_add_board( lights/rgbled_ncp5623c lights/rgbled_pwm magnetometer # all available magnetometer drivers - mkblctrl #optical_flow # all available optical flow drivers optical_flow/px4flow #osd diff --git a/boards/nxp/fmurt1062-v1/default.cmake b/boards/nxp/fmurt1062-v1/default.cmake index fa4f14052f..23dc2d9989 100644 --- a/boards/nxp/fmurt1062-v1/default.cmake +++ b/boards/nxp/fmurt1062-v1/default.cmake @@ -24,7 +24,7 @@ px4_add_board( distance_sensor # all available distance sensor drivers # dshot not ported gps - #imu/adis16448 + #imu/analog_devices/adis16448 #imu/adis16477 #imu/adis16497 #imu # all available imu drivers diff --git a/boards/omnibus/f4sd/default.cmake b/boards/omnibus/f4sd/default.cmake index a0a5d06d84..676c7c6cf8 100644 --- a/boards/omnibus/f4sd/default.cmake +++ b/boards/omnibus/f4sd/default.cmake @@ -28,7 +28,6 @@ px4_add_board( lights/rgbled #magnetometer # all available magnetometer drivers magnetometer/hmc5883 - #mkblctrl optical_flow/px4flow osd #pca9685 diff --git a/boards/px4/fmu-v2/default.cmake b/boards/px4/fmu-v2/default.cmake index 84c983da4b..f65075961f 100644 --- a/boards/px4/fmu-v2/default.cmake +++ b/boards/px4/fmu-v2/default.cmake @@ -34,7 +34,7 @@ px4_add_board( gps #heater #imu # all available imu drivers - #imu/adis16448 + #imu/analog_devices/adis16448 #imu/adis16477 #imu/adis16497 imu/l3gd20 @@ -50,7 +50,6 @@ px4_add_board( lights/rgbled #magnetometer # all available magnetometer drivers magnetometer/hmc5883 - #mkblctrl #optical_flow # all available optical flow drivers #optical_flow/px4flow #osd diff --git a/boards/px4/fmu-v2/lpe.cmake b/boards/px4/fmu-v2/lpe.cmake index f0540820ef..a21c45816d 100644 --- a/boards/px4/fmu-v2/lpe.cmake +++ b/boards/px4/fmu-v2/lpe.cmake @@ -32,7 +32,7 @@ px4_add_board( distance_sensor # all available distance sensor drivers gps #heater - #imu/adis16448 + #imu/analog_devices/adis16448 #imu # all available imu drivers imu/l3gd20 imu/lsm303d @@ -43,7 +43,6 @@ px4_add_board( lights/rgbled #magnetometer # all available magnetometer drivers magnetometer/hmc5883 - #mkblctrl #optical_flow # all available optical flow drivers optical_flow/px4flow #pca9685 diff --git a/boards/px4/fmu-v2/test.cmake b/boards/px4/fmu-v2/test.cmake index 088b207ae7..732f116d41 100644 --- a/boards/px4/fmu-v2/test.cmake +++ b/boards/px4/fmu-v2/test.cmake @@ -33,7 +33,7 @@ px4_add_board( gps #heater #imu # all available imu drivers - #imu/adis16448 + #imu/analog_devices/adis16448 #imu/adis16477 #imu/adis16497 imu/l3gd20 @@ -46,7 +46,6 @@ px4_add_board( lights/rgbled #magnetometer # all available magnetometer drivers magnetometer/hmc5883 - #mkblctrl #optical_flow # all available optical flow drivers optical_flow/px4flow #osd diff --git a/boards/px4/fmu-v3/ctrlalloc.cmake b/boards/px4/fmu-v3/ctrlalloc.cmake index 00e745cec0..726900aaf6 100644 --- a/boards/px4/fmu-v3/ctrlalloc.cmake +++ b/boards/px4/fmu-v3/ctrlalloc.cmake @@ -31,7 +31,7 @@ px4_add_board( gps #heater #imu # all available imu drivers - imu/adis16448 + imu/analog_devices/adis16448 imu/adis16477 imu/adis16497 imu/l3gd20 @@ -45,7 +45,6 @@ px4_add_board( lights/rgbled lights/rgbled_ncp5623c magnetometer # all available magnetometer drivers - mkblctrl #optical_flow # all available optical flow drivers optical_flow/px4flow #osd diff --git a/boards/px4/fmu-v3/default.cmake b/boards/px4/fmu-v3/default.cmake index 14cf1fcf1e..6891046e53 100644 --- a/boards/px4/fmu-v3/default.cmake +++ b/boards/px4/fmu-v3/default.cmake @@ -31,7 +31,7 @@ px4_add_board( gps #heater #imu # all available imu drivers - imu/adis16448 + imu/analog_devices/adis16448 imu/adis16477 imu/adis16497 imu/l3gd20 @@ -45,7 +45,6 @@ px4_add_board( lights/rgbled lights/rgbled_ncp5623c magnetometer # all available magnetometer drivers - mkblctrl #optical_flow # all available optical flow drivers optical_flow/px4flow #osd diff --git a/boards/px4/fmu-v3/rtps.cmake b/boards/px4/fmu-v3/rtps.cmake index 736cf3c7df..cc2ad9ee8e 100644 --- a/boards/px4/fmu-v3/rtps.cmake +++ b/boards/px4/fmu-v3/rtps.cmake @@ -30,7 +30,7 @@ px4_add_board( gps #heater #imu # all available imu drivers - imu/adis16448 + imu/analog_devices/adis16448 imu/adis16477 imu/adis16497 imu/l3gd20 @@ -44,7 +44,6 @@ px4_add_board( lights/rgbled lights/rgbled_ncp5623c magnetometer # all available magnetometer drivers - mkblctrl #optical_flow # all available optical flow drivers optical_flow/px4flow #osd diff --git a/boards/px4/fmu-v3/stackcheck.cmake b/boards/px4/fmu-v3/stackcheck.cmake index b647895b5e..cba93b5f94 100644 --- a/boards/px4/fmu-v3/stackcheck.cmake +++ b/boards/px4/fmu-v3/stackcheck.cmake @@ -30,7 +30,7 @@ px4_add_board( distance_sensor # all available distance sensor drivers gps #heater - imu/adis16448 + imu/analog_devices/adis16448 #imu # all available imu drivers imu/l3gd20 imu/lsm303d @@ -42,7 +42,6 @@ px4_add_board( lights/rgbled lights/rgbled_ncp5623c magnetometer # all available magnetometer drivers - mkblctrl #optical_flow # all available optical flow drivers optical_flow/px4flow pca9685 diff --git a/boards/px4/fmu-v4/cannode.cmake b/boards/px4/fmu-v4/cannode.cmake index cb54af827d..fb922141b6 100644 --- a/boards/px4/fmu-v4/cannode.cmake +++ b/boards/px4/fmu-v4/cannode.cmake @@ -42,7 +42,7 @@ px4_add_board( #dshot gps #imu # all available imu drivers - #imu/adis16448 + #imu/analog_devices/adis16448 #imu/adis16477 #imu/adis16497 imu/invensense/icm20602 diff --git a/boards/px4/fmu-v4/ctrlalloc.cmake b/boards/px4/fmu-v4/ctrlalloc.cmake index 9970fa75d6..d94052facf 100644 --- a/boards/px4/fmu-v4/ctrlalloc.cmake +++ b/boards/px4/fmu-v4/ctrlalloc.cmake @@ -29,7 +29,7 @@ px4_add_board( gps heater #imu # all available imu drivers - imu/adis16448 + imu/analog_devices/adis16448 imu/adis16477 imu/adis16497 imu/invensense/icm20602 @@ -42,7 +42,6 @@ px4_add_board( lights/rgbled lights/rgbled_ncp5623c magnetometer # all available magnetometer drivers - mkblctrl optical_flow # all available optical flow drivers #osd pca9685 diff --git a/boards/px4/fmu-v4/default.cmake b/boards/px4/fmu-v4/default.cmake index 3d657b650b..6c88eb2c8e 100644 --- a/boards/px4/fmu-v4/default.cmake +++ b/boards/px4/fmu-v4/default.cmake @@ -29,7 +29,7 @@ px4_add_board( gps heater #imu # all available imu drivers - imu/adis16448 + imu/analog_devices/adis16448 imu/adis16477 imu/adis16497 imu/invensense/icm20602 @@ -42,7 +42,6 @@ px4_add_board( lights/rgbled lights/rgbled_ncp5623c magnetometer # all available magnetometer drivers - mkblctrl optical_flow # all available optical flow drivers #osd pca9685 @@ -90,6 +89,7 @@ px4_add_board( sih temperature_compensation uuv_att_control + uuv_pos_control vmount vtol_att_control SYSTEMCMDS diff --git a/boards/px4/fmu-v4/optimized.cmake b/boards/px4/fmu-v4/optimized.cmake index 1dba592281..05bb456997 100644 --- a/boards/px4/fmu-v4/optimized.cmake +++ b/boards/px4/fmu-v4/optimized.cmake @@ -30,7 +30,7 @@ px4_add_board( gps #heater #imu # all available imu drivers - #imu/adis16448 + #imu/analog_devices/adis16448 #imu/adis16477 #imu/adis16497 imu/invensense/icm20602 diff --git a/boards/px4/fmu-v4/rtps.cmake b/boards/px4/fmu-v4/rtps.cmake index a7f1793216..d87230acab 100644 --- a/boards/px4/fmu-v4/rtps.cmake +++ b/boards/px4/fmu-v4/rtps.cmake @@ -26,7 +26,7 @@ px4_add_board( gps heater #imu # all available imu drivers - imu/adis16448 + imu/analog_devices/adis16448 imu/adis16477 imu/adis16497 imu/invensense/icm20602 @@ -39,7 +39,6 @@ px4_add_board( lights/rgbled lights/rgbled_ncp5623c magnetometer # all available magnetometer drivers - mkblctrl optical_flow # all available optical flow drivers #osd pca9685 @@ -85,6 +84,7 @@ px4_add_board( sih temperature_compensation uuv_att_control + uuv_pos_control vmount vtol_att_control SYSTEMCMDS diff --git a/boards/px4/fmu-v4/stackcheck.cmake b/boards/px4/fmu-v4/stackcheck.cmake index 610e7231f6..45e7577208 100644 --- a/boards/px4/fmu-v4/stackcheck.cmake +++ b/boards/px4/fmu-v4/stackcheck.cmake @@ -26,7 +26,7 @@ px4_add_board( gps heater #imu # all available imu drivers - #imu/adis16448 + #imu/analog_devices/adis16448 #imu/adis16477 #imu/adis16497 imu/invensense/icm20602 @@ -39,7 +39,6 @@ px4_add_board( lights/rgbled lights/rgbled_ncp5623c magnetometer # all available magnetometer drivers - mkblctrl optical_flow # all available optical flow drivers #osd pca9685 diff --git a/boards/px4/fmu-v4/uavcanv1.cmake b/boards/px4/fmu-v4/uavcanv1.cmake index 9dfdd33164..b4a9b29283 100644 --- a/boards/px4/fmu-v4/uavcanv1.cmake +++ b/boards/px4/fmu-v4/uavcanv1.cmake @@ -28,7 +28,7 @@ px4_add_board( gps heater #imu # all available imu drivers - imu/adis16448 + imu/analog_devices/adis16448 imu/adis16477 imu/adis16497 imu/invensense/icm20602 @@ -41,7 +41,6 @@ px4_add_board( lights/rgbled lights/rgbled_ncp5623c magnetometer # all available magnetometer drivers - mkblctrl optical_flow # all available optical flow drivers #osd pca9685 diff --git a/boards/px4/fmu-v4pro/default.cmake b/boards/px4/fmu-v4pro/default.cmake index 3ef740388c..3592562acd 100644 --- a/boards/px4/fmu-v4pro/default.cmake +++ b/boards/px4/fmu-v4pro/default.cmake @@ -38,7 +38,6 @@ px4_add_board( lights/rgbled lights/rgbled_ncp5623c magnetometer # all available magnetometer drivers - mkblctrl #optical_flow # all available optical flow drivers optical_flow/px4flow #osd @@ -87,6 +86,7 @@ px4_add_board( sih temperature_compensation uuv_att_control + uuv_pos_control vmount vtol_att_control SYSTEMCMDS diff --git a/boards/px4/fmu-v4pro/rtps.cmake b/boards/px4/fmu-v4pro/rtps.cmake index 537f1fece4..8167947868 100644 --- a/boards/px4/fmu-v4pro/rtps.cmake +++ b/boards/px4/fmu-v4pro/rtps.cmake @@ -37,7 +37,6 @@ px4_add_board( lights/rgbled lights/rgbled_ncp5623c magnetometer # all available magnetometer drivers - mkblctrl #optical_flow # all available optical flow drivers optical_flow/px4flow #osd @@ -84,6 +83,7 @@ px4_add_board( sih temperature_compensation uuv_att_control + uuv_pos_control vmount vtol_att_control SYSTEMCMDS diff --git a/boards/px4/fmu-v5/critmonitor.cmake b/boards/px4/fmu-v5/critmonitor.cmake index 49fe32447b..48d0368bbd 100644 --- a/boards/px4/fmu-v5/critmonitor.cmake +++ b/boards/px4/fmu-v5/critmonitor.cmake @@ -27,7 +27,7 @@ px4_add_board( gps #heater #imu # all available imu drivers - imu/adis16448 + imu/analog_devices/adis16448 imu/adis16477 imu/adis16497 imu/bosch/bmi055 @@ -40,7 +40,6 @@ px4_add_board( lights/rgbled_ncp5623c lights/rgbled_pwm magnetometer # all available magnetometer drivers - mkblctrl optical_flow # all available optical flow drivers #osd pca9685 diff --git a/boards/px4/fmu-v5/ctrlalloc.cmake b/boards/px4/fmu-v5/ctrlalloc.cmake index 285bb2b18a..0adf5cff0d 100644 --- a/boards/px4/fmu-v5/ctrlalloc.cmake +++ b/boards/px4/fmu-v5/ctrlalloc.cmake @@ -28,7 +28,7 @@ px4_add_board( gps #heater #imu # all available imu drivers - imu/adis16448 + imu/analog_devices/adis16448 imu/adis16477 imu/adis16497 imu/bosch/bmi055 @@ -41,7 +41,6 @@ px4_add_board( lights/rgbled_ncp5623c lights/rgbled_pwm magnetometer # all available magnetometer drivers - mkblctrl optical_flow # all available optical flow drivers #osd pca9685 @@ -94,6 +93,7 @@ px4_add_board( sih temperature_compensation uuv_att_control + uuv_pos_control vmount vtol_att_control SYSTEMCMDS diff --git a/boards/px4/fmu-v5/debug.cmake b/boards/px4/fmu-v5/debug.cmake index 9edd9ca693..e157e46b59 100644 --- a/boards/px4/fmu-v5/debug.cmake +++ b/boards/px4/fmu-v5/debug.cmake @@ -29,7 +29,7 @@ px4_add_board( gps #heater #imu # all available imu drivers - #imu/adis16448 + #imu/analog_devices/adis16448 #imu/adis16477 #imu/adis16497 #imu/bosch/bmi055 @@ -42,7 +42,6 @@ px4_add_board( lights/rgbled_ncp5623c lights/rgbled_pwm magnetometer # all available magnetometer drivers - #mkblctrl optical_flow # all available optical flow drivers #osd #pca9685 diff --git a/boards/px4/fmu-v5/default.cmake b/boards/px4/fmu-v5/default.cmake index 8c9f8519be..8eedc7b875 100644 --- a/boards/px4/fmu-v5/default.cmake +++ b/boards/px4/fmu-v5/default.cmake @@ -28,7 +28,7 @@ px4_add_board( gps #heater #imu # all available imu drivers - imu/adis16448 + imu/analog_devices/adis16448 imu/adis16477 imu/adis16497 imu/bosch/bmi055 @@ -41,7 +41,6 @@ px4_add_board( lights/rgbled_ncp5623c lights/rgbled_pwm magnetometer # all available magnetometer drivers - mkblctrl optical_flow # all available optical flow drivers #osd pca9685 @@ -92,6 +91,7 @@ px4_add_board( sih temperature_compensation uuv_att_control + uuv_pos_control vmount vtol_att_control SYSTEMCMDS diff --git a/boards/px4/fmu-v5/fixedwing.cmake b/boards/px4/fmu-v5/fixedwing.cmake index f7b35b887e..9ff4e3286d 100644 --- a/boards/px4/fmu-v5/fixedwing.cmake +++ b/boards/px4/fmu-v5/fixedwing.cmake @@ -23,7 +23,7 @@ px4_add_board( differential_pressure # all available differential pressure drivers distance_sensor # all available distance sensor drivers gps - imu/adis16448 + imu/analog_devices/adis16448 imu/adis16477 imu/adis16497 #imu # all available imu drivers diff --git a/boards/px4/fmu-v5/irqmonitor.cmake b/boards/px4/fmu-v5/irqmonitor.cmake index 7c566f9b54..84f51e730a 100644 --- a/boards/px4/fmu-v5/irqmonitor.cmake +++ b/boards/px4/fmu-v5/irqmonitor.cmake @@ -27,7 +27,7 @@ px4_add_board( gps #heater #imu # all available imu drivers - imu/adis16448 + imu/analog_devices/adis16448 imu/adis16477 imu/adis16497 imu/bosch/bmi055 @@ -40,7 +40,6 @@ px4_add_board( lights/rgbled_ncp5623c lights/rgbled_pwm magnetometer # all available magnetometer drivers - mkblctrl optical_flow # all available optical flow drivers #osd pca9685 diff --git a/boards/px4/fmu-v5/multicopter.cmake b/boards/px4/fmu-v5/multicopter.cmake index 607e1a74c6..f966510aa2 100644 --- a/boards/px4/fmu-v5/multicopter.cmake +++ b/boards/px4/fmu-v5/multicopter.cmake @@ -24,7 +24,7 @@ px4_add_board( distance_sensor # all available distance sensor drivers dshot gps - imu/adis16448 + imu/analog_devices/adis16448 imu/adis16477 imu/adis16497 #imu # all available imu drivers diff --git a/boards/px4/fmu-v5/rover.cmake b/boards/px4/fmu-v5/rover.cmake index e4d1b62528..94d03e80e9 100644 --- a/boards/px4/fmu-v5/rover.cmake +++ b/boards/px4/fmu-v5/rover.cmake @@ -22,7 +22,7 @@ px4_add_board( camera_trigger distance_sensor # all available distance sensor drivers gps - imu/adis16448 + imu/analog_devices/adis16448 imu/adis16477 imu/adis16497 imu/bosch/bmi055 @@ -33,7 +33,6 @@ px4_add_board( lights/rgbled_ncp5623c lights/rgbled_pwm magnetometer # all available magnetometer drivers - mkblctrl optical_flow # all available optical flow drivers pca9685 pwm_input diff --git a/boards/px4/fmu-v5/rtps.cmake b/boards/px4/fmu-v5/rtps.cmake index beb4a7ef7b..615857d1b2 100644 --- a/boards/px4/fmu-v5/rtps.cmake +++ b/boards/px4/fmu-v5/rtps.cmake @@ -27,7 +27,7 @@ px4_add_board( gps #heater #imu # all available imu drivers - imu/adis16448 + imu/analog_devices/adis16448 imu/adis16477 imu/adis16497 imu/bosch/bmi055 @@ -40,7 +40,6 @@ px4_add_board( lights/rgbled_ncp5623c lights/rgbled_pwm magnetometer # all available magnetometer drivers - mkblctrl optical_flow # all available optical flow drivers #osd pca9685 diff --git a/boards/px4/fmu-v5/stackcheck.cmake b/boards/px4/fmu-v5/stackcheck.cmake index b7a4635baf..d497763de4 100644 --- a/boards/px4/fmu-v5/stackcheck.cmake +++ b/boards/px4/fmu-v5/stackcheck.cmake @@ -29,7 +29,7 @@ px4_add_board( gps #heater #imu # all available imu drivers - #imu/adis16448 + #imu/analog_devices/adis16448 #imu/adis16477 #imu/adis16497 #imu/bosch/bmi055 @@ -42,7 +42,6 @@ px4_add_board( #lights/rgbled_ncp5623c lights/rgbled_pwm magnetometer # all available magnetometer drivers - #mkblctrl optical_flow # all available optical flow drivers #osd #pca9685 diff --git a/boards/px4/fmu-v5/uavcanv1.cmake b/boards/px4/fmu-v5/uavcanv1.cmake index f209bfd0b3..b75b5353ff 100644 --- a/boards/px4/fmu-v5/uavcanv1.cmake +++ b/boards/px4/fmu-v5/uavcanv1.cmake @@ -28,7 +28,7 @@ px4_add_board( gps #heater #imu # all available imu drivers - imu/adis16448 + imu/analog_devices/adis16448 imu/adis16477 imu/adis16497 imu/bosch/bmi055 @@ -41,7 +41,6 @@ px4_add_board( lights/rgbled_ncp5623c lights/rgbled_pwm magnetometer # all available magnetometer drivers - mkblctrl optical_flow # all available optical flow drivers #osd pca9685 diff --git a/boards/px4/fmu-v5x/base_phy_DP83848C.cmake b/boards/px4/fmu-v5x/base_phy_DP83848C.cmake index 50a8c31665..e9bdbce90e 100644 --- a/boards/px4/fmu-v5x/base_phy_DP83848C.cmake +++ b/boards/px4/fmu-v5x/base_phy_DP83848C.cmake @@ -28,7 +28,7 @@ px4_add_board( gps #heater #imu # all available imu drivers - imu/adis16448 + imu/analog_devices/adis16448 imu/adis16477 imu/adis16497 imu/bosch/bmi088 @@ -39,7 +39,6 @@ px4_add_board( lights/rgbled lights/rgbled_ncp5623c magnetometer # all available magnetometer drivers - mkblctrl optical_flow # all available optical flow drivers #osd pca9685 diff --git a/boards/px4/fmu-v5x/default.cmake b/boards/px4/fmu-v5x/default.cmake index 4fa24a2f5e..d760d664df 100644 --- a/boards/px4/fmu-v5x/default.cmake +++ b/boards/px4/fmu-v5x/default.cmake @@ -29,7 +29,7 @@ px4_add_board( gps #heater #imu # all available imu drivers - imu/adis16448 + imu/analog_devices/adis16448 imu/adis16477 imu/adis16497 imu/bosch/bmi088 @@ -40,7 +40,6 @@ px4_add_board( lights/rgbled lights/rgbled_ncp5623c magnetometer # all available magnetometer drivers - mkblctrl optical_flow # all available optical flow drivers #osd pca9685 diff --git a/boards/px4/fmu-v5x/init/rc.board_sensors b/boards/px4/fmu-v5x/init/rc.board_sensors index 8449a80dc9..633784ecb8 100644 --- a/boards/px4/fmu-v5x/init/rc.board_sensors +++ b/boards/px4/fmu-v5x/init/rc.board_sensors @@ -8,22 +8,52 @@ board_adc start ina226 -X -b 1 -t 1 -k start ina226 -X -b 2 -t 2 -k start -# Internal SPI BMI088 -bmi088 -A -R 4 -s start -bmi088 -G -R 4 -s start +if ver hwtypecmp V5X90 V5X91 V5Xa0 V5Xa1 +then + #SKYNODE base fmu board orientation -# Internal SPI bus ICM42688p -icm42688p -R 6 -s start + # Internal SPI BMI088 + bmi088 -A -R 2 -s start + bmi088 -G -R 2 -s start -# Internal SPI bus ICM-20602 (hard-mounted) -icm20602 -R 10 -s start + # Internal SPI bus ICM42688p + icm42688p -R 4 -s start -# Internal magnetometer on I2c -bmm150 -I start + # Internal SPI bus ICM-20602 (hard-mounted) + icm20602 -R 8 -s start + + # Internal magnetometer on I2c + bmm150 -I -R 6 start + +else + #FMUv5Xbase board orientation + + # Internal SPI BMI088 + bmi088 -A -R 4 -s start + bmi088 -G -R 4 -s start + + # Internal SPI bus ICM42688p + icm42688p -R 6 -s start + + # Internal SPI bus ICM-20602 (hard-mounted) + icm20602 -R 10 -s start + + # Internal magnetometer on I2c + bmm150 -I start + +fi # Possible internal Baro -bmp388 -I -a 0x77 start -bmp388 -I start -# Baro on I2C3 -ms5611 -X start +# Disable startup of internal baros if param is set to false +if param compare SENS_INT_BARO_EN 1 +then + bmp388 -I -a 0x77 start + if ver hwtypecmp V5X91 V5Xa1 + then + bmp388 -X -b 2 start + else + bmp388 -I start + fi + +fi diff --git a/boards/px4/fmu-v5x/nuttx-config/scripts/script.ld b/boards/px4/fmu-v5x/nuttx-config/scripts/script.ld index 297385ddcb..8d5a78da6b 100644 --- a/boards/px4/fmu-v5x/nuttx-config/scripts/script.ld +++ b/boards/px4/fmu-v5x/nuttx-config/scripts/script.ld @@ -127,6 +127,14 @@ SECTIONS _einit = ABSOLUTE(.); } > FLASH_AXIM + /* + * Construction data for parameters. + */ + __param ALIGN(4): { + __param_start = ABSOLUTE(.); + KEEP(*(__param*)) + __param_end = ABSOLUTE(.); + } > FLASH_AXIM .ARM.extab : { *(.ARM.extab*) diff --git a/boards/px4/fmu-v5x/src/board_config.h b/boards/px4/fmu-v5x/src/board_config.h index 61c62097d7..64bcd8e4fb 100644 --- a/boards/px4/fmu-v5x/src/board_config.h +++ b/boards/px4/fmu-v5x/src/board_config.h @@ -434,6 +434,7 @@ #define BOARD_NUM_IO_TIMERS 5 + #define PX4_I2C_BUS_MTD 4,5 __BEGIN_DECLS diff --git a/boards/px4/fmu-v5x/src/init.cpp b/boards/px4/fmu-v5x/src/init.cpp index fee67d3d80..395684f690 100644 --- a/boards/px4/fmu-v5x/src/init.cpp +++ b/boards/px4/fmu-v5x/src/init.cpp @@ -286,5 +286,31 @@ __EXPORT int board_app_initialize(uintptr_t arg) px4_platform_configure(); + int hw_version = board_get_hw_version(); + + if (hw_version == 0x9 || hw_version == 0xa) { + static MCP23009 mcp23009{3, 0x25}; + + // No USB + if (hw_version == 0x9) { + // < P8 + ret = mcp23009.init(0xf0, 0xf0, 0x0f); + // >= P8 + //ret = mcp23009.init(0xf1, 0xf0, 0x0f); + } + + if (hw_version == 0xa) { + // < P6 + //ret = mcp23009.init(0xf0, 0xf0, 0x0f); + // >= P6 + ret = mcp23009.init(0xf1, 0xf0, 0x0f); + } + + if (ret != OK) { + led_on(LED_RED); + return ret; + } + } + return OK; } diff --git a/boards/px4/fmu-v6u/default.cmake b/boards/px4/fmu-v6u/default.cmake index 4f48780cbb..4c839c807c 100644 --- a/boards/px4/fmu-v6u/default.cmake +++ b/boards/px4/fmu-v6u/default.cmake @@ -28,7 +28,7 @@ px4_add_board( gps heater #imu # all available imu drivers - imu/adis16448 + imu/analog_devices/adis16448 imu/adis16477 imu/adis16497 imu/bosch/bmi088 @@ -41,7 +41,6 @@ px4_add_board( lights/rgbled lights/rgbled_ncp5623c magnetometer # all available magnetometer drivers - mkblctrl optical_flow # all available optical flow drivers #osd pca9685 diff --git a/boards/px4/fmu-v6u/stackcheck.cmake b/boards/px4/fmu-v6u/stackcheck.cmake index 1136f8cca0..4c839c807c 100644 --- a/boards/px4/fmu-v6u/stackcheck.cmake +++ b/boards/px4/fmu-v6u/stackcheck.cmake @@ -1,7 +1,7 @@ px4_add_board( PLATFORM nuttx - VENDOR px4 + VENDOR px4 MODEL fmu-v6u LABEL default TOOLCHAIN arm-none-eabi @@ -28,7 +28,7 @@ px4_add_board( gps heater #imu # all available imu drivers - imu/adis16448 + imu/analog_devices/adis16448 imu/adis16477 imu/adis16497 imu/bosch/bmi088 @@ -41,7 +41,6 @@ px4_add_board( lights/rgbled lights/rgbled_ncp5623c magnetometer # all available magnetometer drivers - mkblctrl optical_flow # all available optical flow drivers #osd pca9685 diff --git a/boards/px4/fmu-v6x/default.cmake b/boards/px4/fmu-v6x/default.cmake index 2f0068b40e..0444cfb5d6 100644 --- a/boards/px4/fmu-v6x/default.cmake +++ b/boards/px4/fmu-v6x/default.cmake @@ -29,7 +29,7 @@ px4_add_board( gps heater #imu # all available imu drivers - imu/adis16448 + imu/analog_devices/adis16448 imu/adis16477 imu/adis16497 imu/bosch/bmi088 @@ -39,7 +39,6 @@ px4_add_board( lights/rgbled lights/rgbled_ncp5623c magnetometer # all available magnetometer drivers - mkblctrl optical_flow # all available optical flow drivers #osd pca9685 diff --git a/boards/px4/fmu-v6x/stackcheck.cmake b/boards/px4/fmu-v6x/stackcheck.cmake index 2f0068b40e..0444cfb5d6 100644 --- a/boards/px4/fmu-v6x/stackcheck.cmake +++ b/boards/px4/fmu-v6x/stackcheck.cmake @@ -29,7 +29,7 @@ px4_add_board( gps heater #imu # all available imu drivers - imu/adis16448 + imu/analog_devices/adis16448 imu/adis16477 imu/adis16497 imu/bosch/bmi088 @@ -39,7 +39,6 @@ px4_add_board( lights/rgbled lights/rgbled_ncp5623c magnetometer # all available magnetometer drivers - mkblctrl optical_flow # all available optical flow drivers #osd pca9685 diff --git a/boards/px4/sitl/ctrlalloc.cmake b/boards/px4/sitl/ctrlalloc.cmake index 9d30f52c6c..61c126f8fc 100644 --- a/boards/px4/sitl/ctrlalloc.cmake +++ b/boards/px4/sitl/ctrlalloc.cmake @@ -57,6 +57,7 @@ px4_add_board( simulator temperature_compensation uuv_att_control + uuv_pos_control vmount vtol_att_control SYSTEMCMDS diff --git a/boards/px4/sitl/default.cmake b/boards/px4/sitl/default.cmake index 7ac353f20d..cccb9d9409 100644 --- a/boards/px4/sitl/default.cmake +++ b/boards/px4/sitl/default.cmake @@ -55,6 +55,7 @@ px4_add_board( simulator temperature_compensation uuv_att_control + uuv_pos_control vmount vtol_att_control SYSTEMCMDS diff --git a/boards/px4/sitl/nolockstep.cmake b/boards/px4/sitl/nolockstep.cmake index a8ddbbf837..f797304951 100644 --- a/boards/px4/sitl/nolockstep.cmake +++ b/boards/px4/sitl/nolockstep.cmake @@ -55,6 +55,7 @@ px4_add_board( simulator temperature_compensation uuv_att_control + uuv_pos_control vmount vtol_att_control SYSTEMCMDS diff --git a/boards/px4/sitl/rtps.cmake b/boards/px4/sitl/rtps.cmake index 879dd9941b..cba4e8adfc 100644 --- a/boards/px4/sitl/rtps.cmake +++ b/boards/px4/sitl/rtps.cmake @@ -55,6 +55,7 @@ px4_add_board( simulator temperature_compensation uuv_att_control + uuv_pos_control vmount vtol_att_control SYSTEMCMDS diff --git a/boards/px4/sitl/test.cmake b/boards/px4/sitl/test.cmake index c70de702c6..4c101974a4 100644 --- a/boards/px4/sitl/test.cmake +++ b/boards/px4/sitl/test.cmake @@ -54,6 +54,7 @@ px4_add_board( simulator temperature_compensation uuv_att_control + uuv_pos_control vmount vtol_att_control SYSTEMCMDS diff --git a/boards/spracing/h7extreme/default.cmake b/boards/spracing/h7extreme/default.cmake index dfa30b3047..9c7f0ada3a 100644 --- a/boards/spracing/h7extreme/default.cmake +++ b/boards/spracing/h7extreme/default.cmake @@ -24,7 +24,7 @@ px4_add_board( gps #heater #imu # all available imu drivers - #imu/adis16448 + #imu/analog_devices/adis16448 #imu/adis16477 #imu/adis16497 #imu/bmi088 @@ -36,7 +36,6 @@ px4_add_board( lights/rgbled lights/rgbled_ncp5623c magnetometer # all available magnetometer drivers - #mkblctrl optical_flow # all available optical flow drivers osd #pca9685 diff --git a/src/drivers/drv_io_heater.h b/src/drivers/drv_io_heater.h index f946042a58..65146e391b 100644 --- a/src/drivers/drv_io_heater.h +++ b/src/drivers/drv_io_heater.h @@ -47,11 +47,11 @@ * ioctl() definitions */ -#define IO_HEATER_DEVICE_PATH "/dev/px4io" +#define IO_HEATER_DEVICE_PATH "/dev/px4io" -#define _IO_HEATER_BASE (0x2e00) +#define _IO_HEATER_BASE (0x2e00) -#define PX4IO_HEATER_CONTROL _PX4_IOC(_IO_HEATER_BASE, 0) +#define PX4IO_HEATER_CONTROL _PX4_IOC(_IO_HEATER_BASE, 0) /* ... to IOX_SET_VALUE + 8 */ diff --git a/src/drivers/heater/heater.cpp b/src/drivers/heater/heater.cpp index 910fc4aac7..f73cbfca08 100644 --- a/src/drivers/heater/heater.cpp +++ b/src/drivers/heater/heater.cpp @@ -74,13 +74,11 @@ Heater::Heater() : #endif - // Initialize heater to off state heater_enable(); } Heater::~Heater() { - // Reset heater to off state heater_disable(); #ifdef HEATER_PX4IO @@ -88,46 +86,6 @@ Heater::~Heater() #endif } -void Heater::heater_enable() -{ -#ifdef HEATER_PX4IO - px4_ioctl(_io_fd, PX4IO_HEATER_CONTROL, HEATER_MODE_OFF); -#endif -#ifdef HEATER_GPIO - px4_arch_configgpio(GPIO_HEATER_OUTPUT); -#endif -} - -void Heater::heater_disable() -{ -#ifdef HEATER_PX4IO - px4_ioctl(_io_fd, PX4IO_HEATER_CONTROL, HEATER_MODE_DISABLED); -#endif -#ifdef HEATER_GPIO - px4_arch_configgpio(GPIO_HEATER_OUTPUT); -#endif -} - -void Heater::heater_on() -{ -#ifdef HEATER_PX4IO - px4_ioctl(_io_fd, PX4IO_HEATER_CONTROL, HEATER_MODE_ON); -#endif -#ifdef HEATER_GPIO - px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, 1); -#endif -} - -void Heater::heater_off() -{ -#ifdef HEATER_PX4IO - px4_ioctl(_io_fd, PX4IO_HEATER_CONTROL, HEATER_MODE_OFF); -#endif -#ifdef HEATER_GPIO - px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, 0); -#endif -} - int Heater::custom_command(int argc, char *argv[]) { // Check if the driver is running. @@ -139,56 +97,51 @@ int Heater::custom_command(int argc, char *argv[]) return print_usage("Unrecognized command."); } -void Heater::Run() +uint32_t Heater::get_sensor_id() { - if (should_exit()) { - exit_and_cleanup(); - return; - } + return _sensor_accel.device_id; +} - if (_heater_on) { - // Turn the heater off. - heater_off(); - _heater_on = false; +void Heater::heater_disable() +{ + // Reset heater to off state. +#ifdef HEATER_PX4IO + px4_ioctl(_io_fd, PX4IO_HEATER_CONTROL, HEATER_MODE_DISABLED); +#endif +#ifdef HEATER_GPIO + px4_arch_configgpio(GPIO_HEATER_OUTPUT); +#endif +} - } else { - update_params(false); +void Heater::heater_enable() +{ + // Initialize heater to off state. +#ifdef HEATER_PX4IO + px4_ioctl(_io_fd, PX4IO_HEATER_CONTROL, HEATER_MODE_OFF); +#endif +#ifdef HEATER_GPIO + px4_arch_configgpio(GPIO_HEATER_OUTPUT); +#endif +} - _sensor_accel_sub.update(&_sensor_accel); +void Heater::heater_off() +{ +#ifdef HEATER_PX4IO + px4_ioctl(_io_fd, PX4IO_HEATER_CONTROL, HEATER_MODE_OFF); +#endif +#ifdef HEATER_GPIO + px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, 0); +#endif +} - // Obtain the current IMU sensor temperature. - _sensor_temperature = _sensor_accel.temperature; - - // Calculate the temperature delta between the setpoint and reported temperature. - float temperature_delta = _param_sens_imu_temp.get() - _sensor_temperature; - - // Modulate the heater time on with a feedforward/PI controller. - _proportional_value = temperature_delta * _param_sens_imu_temp_p.get(); - _integrator_value += temperature_delta * _param_sens_imu_temp_i.get(); - - // Constrain the integrator value to no more than 25% of the duty cycle. - _integrator_value = math::constrain(_integrator_value, -0.25f, 0.25f); - - // Calculate the duty cycle. This is a value between 0 and 1. - float duty = _proportional_value + _integrator_value; - - _controller_time_on_usec = (int)(duty * (float)_controller_period_usec); - - // Constrain the heater time within the allowable duty cycle. - _controller_time_on_usec = math::constrain(_controller_time_on_usec, 0, _controller_period_usec); - - // Turn the heater on. - _heater_on = true; - heater_on(); - } - - // Schedule the next cycle. - if (_heater_on) { - ScheduleDelayed(_controller_time_on_usec); - - } else { - ScheduleDelayed(_controller_period_usec - _controller_time_on_usec); - } +void Heater::heater_on() +{ +#ifdef HEATER_PX4IO + px4_ioctl(_io_fd, PX4IO_HEATER_CONTROL, HEATER_MODE_ON); +#endif +#ifdef HEATER_GPIO + px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, 1); +#endif } void Heater::initialize_topics() @@ -196,16 +149,21 @@ void Heater::initialize_topics() // Get the total number of accelerometer instances. uint8_t number_of_imus = orb_group_count(ORB_ID(sensor_accel)); - // Check each instance for the correct ID. + // Get the total number of accelerometer instances and check each instance for the correct ID. for (uint8_t x = 0; x < number_of_imus; x++) { - _sensor_accel_sub = uORB::Subscription{ORB_ID(sensor_accel), x}; + _sensor_accel.device_id = 0; - if (!_sensor_accel_sub.advertised()) { - continue; + while (_sensor_accel.device_id == 0) { + _sensor_accel_sub = uORB::Subscription{ORB_ID(sensor_accel), x}; + + if (!_sensor_accel_sub.advertised()) { + px4_usleep(100); + continue; + } + + _sensor_accel_sub.copy(&_sensor_accel); } - _sensor_accel_sub.copy(&_sensor_accel); - // If the correct ID is found, exit the for-loop with _sensor_accel_sub pointing to the correct instance. if (_sensor_accel.device_id == (uint32_t)_param_sens_temp_id.get()) { break; @@ -221,55 +179,22 @@ void Heater::initialize_topics() int Heater::print_status() { - PX4_INFO("Sensor ID: %d - Temperature: %3.3fC, Setpoint: %3.2fC, Heater State: %s", + float _feedforward_value = _param_sens_imu_temp_ff.get(); + + PX4_INFO("Sensor ID: %d,\tSetpoint: %3.2fC,\t Sensor Temperature: %3.2fC,\tDuty Cycle (usec): %d", _sensor_accel.device_id, - (double)_sensor_temperature, - (double)_param_sens_imu_temp.get(), - _heater_on ? "On" : "Off"); + static_cast(_param_sens_imu_temp.get()), + static_cast(_sensor_accel.temperature), + _controller_period_usec); + PX4_INFO("Feed Forward control effort: %3.2f%%,\tProportional control effort: %3.2f%%,\tIntegrator control effort: %3.3f%%,\t Heater cycle: %3.2f%%", + static_cast(_feedforward_value * 100), + static_cast(_proportional_value * 100), + static_cast(_integrator_value * 100), + static_cast(static_cast(_controller_time_on_usec) / static_cast(_controller_period_usec) * 100)); return PX4_OK; } -int Heater::start() -{ - update_params(true); - initialize_topics(); - - ScheduleNow(); - - return PX4_OK; -} - -int Heater::task_spawn(int argc, char *argv[]) -{ - Heater *heater = new Heater(); - - if (!heater) { - PX4_ERR("driver allocation failed"); - return PX4_ERROR; - } - - _object.store(heater); - _task_id = task_id_is_work_queue; - - heater->start(); - - return 0; -} - -void Heater::update_params(const bool force) -{ - // check for parameter updates - if (_parameter_update_sub.updated() || force) { - // clear update - parameter_update_s pupdate; - _parameter_update_sub.copy(&pupdate); - - // update parameters from storage - ModuleParams::updateParams(); - } -} - int Heater::print_usage(const char *reason) { if (reason) { @@ -291,6 +216,97 @@ This task can be started at boot from the startup scripts by setting SENS_EN_THE return 0; } +void Heater::Run() +{ + if (should_exit()) { + exit_and_cleanup(); + return; + } + + if (_heater_on) { + // Turn the heater off. + heater_off(); + _heater_on = false; + + } else { + update_params(false); + + _sensor_accel_sub.update(&_sensor_accel); + + float temperature_delta {0.f}; + + // Update the current IMU sensor temperature if valid. + if (!isnan(_sensor_accel.temperature)) { + temperature_delta = _param_sens_imu_temp.get() - _sensor_accel.temperature; + } + + _proportional_value = temperature_delta * _param_sens_imu_temp_p.get(); + _integrator_value += temperature_delta * _param_sens_imu_temp_i.get(); + + if (fabs(_param_sens_imu_temp_i.get()) <= 0.0001) { + _integrator_value = 0.f; + } + + // Guard against integrator wind up. + _integrator_value = math::constrain(_integrator_value, -0.25f, 0.25f); + + _controller_time_on_usec = static_cast((_param_sens_imu_temp_ff.get() + _proportional_value + + _integrator_value) * static_cast(_controller_period_usec)); + + _controller_time_on_usec = math::constrain(_controller_time_on_usec, 0, _controller_period_usec); + + _heater_on = true; + heater_on(); + } + + // Schedule the next cycle. + if (_heater_on) { + ScheduleDelayed(_controller_time_on_usec); + + } else { + ScheduleDelayed(_controller_period_usec - _controller_time_on_usec); + } +} + +int Heater::start() +{ + update_params(true); + initialize_topics(); + + // Allow sufficient time for all additional sensors and processes to start. + ScheduleDelayed(100000); + return PX4_OK; +} + +int Heater::task_spawn(int argc, char *argv[]) +{ + Heater *heater = new Heater(); + + if (!heater) { + PX4_ERR("driver allocation failed"); + return PX4_ERROR; + } + + _object.store(heater); + _task_id = task_id_is_work_queue; + + heater->start(); + return 0; +} + +void Heater::update_params(const bool force) +{ + // check for parameter updates + if (_parameter_update_sub.updated() || force) { + // clear update + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); + + // update parameters from storage + ModuleParams::updateParams(); + } +} + /** * Main entry point for the heater driver module */ diff --git a/src/drivers/heater/heater.h b/src/drivers/heater/heater.h index 4e72a58b89..126287c452 100644 --- a/src/drivers/heater/heater.h +++ b/src/drivers/heater/heater.h @@ -104,6 +104,9 @@ public: */ int controller_period(char *argv[]); + /** @brief Returns the id of the target sensor. */ + uint32_t get_sensor_id(); + /** * @brief Sets and/or reports the heater controller integrator gain value. * @param argv Pointer to the input argument array. @@ -118,12 +121,6 @@ public: */ float proportional(char *argv[]); - /** - * @brief Reports the heater target sensor. - * @return Returns the id of the target sensor - */ - uint32_t sensor_id(); - /** * @brief Initiates the heater driver work queue, starts a new background task, * and fails if it is already running. @@ -146,18 +143,12 @@ public: protected: - /** - * @brief Called once to initialize uORB topics. - */ + /** @brief Called once to initialize uORB topics. */ void initialize_topics(); private: - /** - * @brief Calculates the heater element on/off time, carries out - * closed loop feedback and feedforward temperature control, - * and schedules the next cycle. - */ + /** @brief Calculates the heater element on/off time and schedules the next cycle. */ void Run() override; /** @@ -166,27 +157,19 @@ private: */ void update_params(const bool force = false); - /** - * @brief Enables / configures the heater (either by GPIO or PX4IO) - */ + /** Enables / configures the heater (either by GPIO or PX4IO). */ void heater_enable(); - /** - * @brief Disnables the heater (either by GPIO or PX4IO) - */ + /** Disnables the heater (either by GPIO or PX4IO). */ void heater_disable(); - /** - * @brief Turns the heater on (either by GPIO or PX4IO) - */ + /** Turns the heater on (either by GPIO or PX4IO). */ void heater_on(); - /** - * @brief Turns the heater off (either by GPIO or PX4IO) - */ + /** Turns the heater off (either by GPIO or PX4IO). */ void heater_off(); - /** Work queue struct for the RTOS scheduler. */ + /** Work queue struct for the scheduler. */ static struct work_s _work; /** File descriptor for PX4IO for heater ioctl's */ @@ -194,28 +177,24 @@ private: int _io_fd; #endif - int _controller_period_usec = CONTROLLER_PERIOD_DEFAULT; - - int _controller_time_on_usec = 0; - bool _heater_on = false; - float _integrator_value = 0.0f; + int _controller_period_usec = CONTROLLER_PERIOD_DEFAULT; + int _controller_time_on_usec = 0; + + float _integrator_value = 0.0f; + float _proportional_value = 0.0f; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; - float _proportional_value = 0.0f; - uORB::Subscription _sensor_accel_sub{ORB_ID(sensor_accel)}; sensor_accel_s _sensor_accel{}; - float _sensor_temperature = 0.0f; - - /** @note Declare local parameters using defined parameters. */ DEFINE_PARAMETERS( + (ParamFloat) _param_sens_imu_temp_ff, (ParamFloat) _param_sens_imu_temp_i, (ParamFloat) _param_sens_imu_temp_p, - (ParamInt) _param_sens_temp_id, - (ParamFloat) _param_sens_imu_temp + (ParamInt) _param_sens_temp_id, + (ParamFloat) _param_sens_imu_temp ) }; diff --git a/src/drivers/heater/heater_params.c b/src/drivers/heater/heater_params.c index 8dfb5256f4..151f9f3eaf 100644 --- a/src/drivers/heater/heater_params.c +++ b/src/drivers/heater/heater_params.c @@ -60,6 +60,18 @@ PARAM_DEFINE_INT32(SENS_TEMP_ID, 0); */ PARAM_DEFINE_FLOAT(SENS_IMU_TEMP, 55.0f); +/** + * IMU heater controller feedforward value. + * + * @category system + * @group Sensors + * @unit % + * @min 0 + * @max 1.0 + * @decimal 3 + */ +PARAM_DEFINE_FLOAT(SENS_IMU_TEMP_FF, 0.05f); + /** * IMU heater controller integrator gain value. * diff --git a/src/drivers/imu/CMakeLists.txt b/src/drivers/imu/CMakeLists.txt index 54947f6dd5..ef867fd353 100644 --- a/src/drivers/imu/CMakeLists.txt +++ b/src/drivers/imu/CMakeLists.txt @@ -31,9 +31,9 @@ # ############################################################################ -add_subdirectory(adis16448) add_subdirectory(adis16477) add_subdirectory(adis16497) +add_subdirectory(analog_devices) add_subdirectory(bma180) add_subdirectory(bmi055) add_subdirectory(bmi088) diff --git a/src/drivers/imu/adis16448/ADIS16448.cpp b/src/drivers/imu/adis16448/ADIS16448.cpp deleted file mode 100644 index 7897ace6d1..0000000000 --- a/src/drivers/imu/adis16448/ADIS16448.cpp +++ /dev/null @@ -1,514 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2018-2019 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * 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 ADIS16448.cpp - */ - -#include "ADIS16448.h" - -ADIS16448::ADIS16448(I2CSPIBusOption bus_option, int bus, int32_t device, enum Rotation rotation, int bus_frequency, - spi_mode_e spi_mode) : - SPI(DRV_IMU_DEVTYPE_ADIS16448, MODULE_NAME, bus, device, spi_mode, bus_frequency), - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), - _px4_accel(get_device_id(), rotation), - _px4_baro(get_device_id()), - _px4_gyro(get_device_id(), rotation), - _px4_mag(get_device_id(), rotation) -{ - _px4_accel.set_scale(ADIS16448_ACCEL_SENSITIVITY); - _px4_gyro.set_scale(ADIS16448_GYRO_INITIAL_SENSITIVITY); - _px4_mag.set_scale(ADIS16448_MAG_SENSITIVITY); - - _px4_mag.set_external(external()); -} - -ADIS16448::~ADIS16448() -{ - // Delete the perf counter. - perf_free(_perf_read); - perf_free(_perf_transfer); - perf_free(_perf_bad_transfer); - perf_free(_perf_crc_bad); -} - -int -ADIS16448::init() -{ - // Do SPI init (and probe) first. - int ret = SPI::init(); - - if (ret != PX4_OK) { - DEVICE_DEBUG("SPI setup failed %d", ret); - - // If probe/setup failed, return result. - return ret; - } - - ret = measure(); - - if (ret != PX4_OK) { - PX4_ERR("measure failed"); - return PX4_ERROR; - } - - start(); - - return OK; -} - -bool ADIS16448::reset() -{ - // Software reset - write_reg16(ADIS16448_GLOB_CMD, 1 << 7); // GLOB_CMD bit 7 Software reset - - // Reset Recovery Time 90 ms - usleep(90000); - - if (!self_test()) { - return false; - } - - // Factory calibration restore - //write_reg16(ADIS16448_GLOB_CMD, 1 << 1); // GLOB_CMD bit 1 Factory calibration restore - - // include the CRC-16 code in burst read output sequence - write_reg16(ADIS16448_MSC_CTRL, 1 << 4); - - // Set digital FIR filter tap. - //if (!set_dlpf_filter(BITS_FIR_NO_TAP_CFG)) { - // return PX4_ERROR; - //} - - // Set IMU sample rate. - if (!set_sample_rate(_sample_rate)) { - return false; - } - - // Set gyroscope scale to default value. - //if (!set_gyro_dyn_range(GYRO_INITIAL_SENSITIVITY)) { - // return false; - //} - - // Settling time. - usleep(100000); - - return true; -} - -bool ADIS16448::self_test() -{ - bool ret = true; - - // start internal self test routine - write_reg16(ADIS16448_MSC_CTRL, 0x04); // MSC_CTRL bit 10 Internal self test (cleared upon completion) - - // Automatic Self-Test Time 45 ms - usleep(45000); - - // check test status (ADIS16448_DIAG_STAT) - const uint16_t status = read_reg16(ADIS16448_DIAG_STAT); - - const bool self_test_error = (status & (1 << 5)); // 5: Self-test diagnostic error flag - - if (self_test_error) { - //PX4_ERR("self test failed DIAG_STAT: 0x%04X", status); - - // Magnetometer - const bool mag_fail = (status & (1 << 0)); // 0: Magnetometer functional test - - if (mag_fail) { - // tolerate mag test failure (likely due to surrounding magnetic field) - PX4_WARN("Magnetometer functional test fail"); - } - - // Barometer - const bool baro_fail = (status & (1 << 1)); // 1: Barometer functional test - - if (baro_fail) { - PX4_ERR("Barometer functional test test fail"); - ret = false; - } - - // Gyroscope - const bool gyro_x_fail = (status & (1 << 10)); // 10: X-axis gyroscope self-test failure - const bool gyro_y_fail = (status & (1 << 11)); // 11: Y-axis gyroscope self-test failure - const bool gyro_z_fail = (status & (1 << 12)); // 12: Z-axis gyroscope self-test failure - - if (gyro_x_fail || gyro_y_fail || gyro_z_fail) { - PX4_ERR("gyroscope self-test failure"); - ret = false; - } - - // Accelerometer - const bool accel_x_fail = (status & (1 << 13)); // 13: X-axis accelerometer self-test failure - const bool accel_y_fail = (status & (1 << 14)); // 14: Y-axis accelerometer self-test failure - const bool accel_z_fail = (status & (1 << 15)); // 15: Z-axis accelerometer self-test failure - - if (accel_x_fail || accel_y_fail || accel_z_fail) { - PX4_ERR("accelerometer self-test failure"); - ret = false; - } - } - - return ret; -} - -int -ADIS16448::probe() -{ - bool reset_success = reset(); - - // Retry 5 time to get the ADIS16448 PRODUCT ID number. - for (size_t i = 0; i < 5; i++) { - // Read product ID. - _product_ID = read_reg16(ADIS16448_PRODUCT_ID); - - if (_product_ID == ADIS16448_Product) { - break; - } - - reset_success = reset(); - } - - if (!reset_success) { - DEVICE_DEBUG("unable to successfully reset"); - return PX4_ERROR; - } - - // Recognize product serial number. - uint16_t serial_number = (read_reg16(ADIS16334_SERIAL_NUMBER) & 0xfff); - - // Verify product ID. - switch (_product_ID) { - case ADIS16448_Product: - DEVICE_DEBUG("ADIS16448 is detected ID: 0x%02x, Serial: 0x%02x", _product_ID, serial_number); - modify_reg16(ADIS16448_GPIO_CTRL, 0x0200, 0x0002); // Turn on ADIS16448 adaptor board led. - return OK; - } - - DEVICE_DEBUG("unexpected ID 0x%02x", _product_ID); - - return -EIO; -} - -bool -ADIS16448::set_sample_rate(uint16_t desired_sample_rate_hz) -{ - uint16_t smpl_prd = 0; - - if (desired_sample_rate_hz <= 51) { - smpl_prd = BITS_SMPL_PRD_16_TAP_CFG; - - } else if (desired_sample_rate_hz <= 102) { - smpl_prd = BITS_SMPL_PRD_8_TAP_CFG; - - } else if (desired_sample_rate_hz <= 204) { - smpl_prd = BITS_SMPL_PRD_4_TAP_CFG; - - } else if (desired_sample_rate_hz <= 409) { - smpl_prd = BITS_SMPL_PRD_2_TAP_CFG; - - } else { - smpl_prd = BITS_SMPL_PRD_NO_TAP_CFG; - } - - modify_reg16(ADIS16448_SMPL_PRD, 0x1F00, smpl_prd); - - if ((read_reg16(ADIS16448_SMPL_PRD) & 0x1F00) != smpl_prd) { - PX4_ERR("failed to set IMU sample rate"); - - return false; - } - - return true; -} - -bool -ADIS16448::set_dlpf_filter(uint16_t desired_filter_tap) -{ - // Set the DLPF FIR filter tap. This affects both accelerometer and gyroscope. - modify_reg16(ADIS16448_SENS_AVG, 0x0007, desired_filter_tap); - - // Verify data write on the IMU. - if ((read_reg16(ADIS16448_SENS_AVG) & 0x0007) != desired_filter_tap) { - PX4_ERR("failed to set IMU filter"); - - return false; - } - - return true; -} - -bool -ADIS16448::set_gyro_dyn_range(uint16_t desired_gyro_dyn_range) -{ - uint16_t gyro_range_selection = 0; - - if (desired_gyro_dyn_range <= 250) { - gyro_range_selection = BITS_GYRO_DYN_RANGE_250_CFG; - - } else if (desired_gyro_dyn_range <= 500) { - gyro_range_selection = BITS_GYRO_DYN_RANGE_500_CFG; - - } else { - gyro_range_selection = BITS_GYRO_DYN_RANGE_1000_CFG; - } - - modify_reg16(ADIS16448_SENS_AVG, 0x0700, gyro_range_selection); - - // Verify data write on the IMU. - if ((read_reg16(ADIS16448_SENS_AVG) & 0x0700) != gyro_range_selection) { - PX4_ERR("failed to set gyro range"); - return false; - - } else { - _px4_gyro.set_scale(((gyro_range_selection >> 8) / 100.0f) * M_PI_F / 180.0f); - } - - return true; -} - -void -ADIS16448::print_status() -{ - I2CSPIDriverBase::print_status(); - perf_print_counter(_perf_read); - perf_print_counter(_perf_transfer); - perf_print_counter(_perf_bad_transfer); - perf_print_counter(_perf_crc_bad); -} - -void -ADIS16448::modify_reg16(unsigned reg, uint16_t clearbits, uint16_t setbits) -{ - uint16_t val = read_reg16(reg); - val &= ~clearbits; - val |= setbits; - write_reg16(reg, val); -} - -uint16_t -ADIS16448::read_reg16(unsigned reg) -{ - uint16_t cmd[1]; - - cmd[0] = ((reg | DIR_READ) << 8) & 0xff00; - - transferhword(cmd, nullptr, 1); - usleep(T_STALL); - transferhword(nullptr, cmd, 1); - usleep(T_STALL); - - return cmd[0]; -} - -void -ADIS16448::write_reg16(unsigned reg, uint16_t value) -{ - uint16_t cmd[2]; - - cmd[0] = ((reg | DIR_WRITE) << 8) | (0x00ff & value); - cmd[1] = (((reg + 0x1) | DIR_WRITE) << 8) | ((0xff00 & value) >> 8); - - transferhword(cmd, nullptr, 1); - usleep(T_STALL); - transferhword(cmd + 1, nullptr, 1); - usleep(T_STALL); -} - -void -ADIS16448::start() -{ - // Start polling at the specified interval - ScheduleOnInterval((1_s / _sample_rate), 10000); -} - -// computes the CCITT CRC16 on the data received from a burst read -static uint16_t ComputeCRC16(uint16_t burstData[13]) -{ - uint16_t crc = 0xFFFF; // Holds the CRC value - - unsigned int data; // Holds the lower/Upper byte for CRC computation - static constexpr unsigned int POLY = 0x1021; // Divisor used during CRC computation - - // Compute CRC on burst data starting from XGYRO_OUT and ending with TEMP_OUT. - // Start with the lower byte and then the upper byte of each word. - // i.e. Compute XGYRO_OUT_LSB CRC first and then compute XGYRO_OUT_MSB CRC. - for (int i = 1; i < 12; i++) { - unsigned int upperByte = (burstData[i] >> 8) & 0xFF; - unsigned int lowerByte = (burstData[i] & 0xFF); - data = lowerByte; // Compute lower byte CRC first - - for (int ii = 0; ii < 8; ii++, data >>= 1) { - if ((crc & 0x0001) ^ (data & 0x0001)) { - crc = (crc >> 1) ^ POLY; - - } else { - crc >>= 1; - } - } - - data = upperByte; // Compute upper byte of CRC - - for (int ii = 0; ii < 8; ii++, data >>= 1) { - if ((crc & 0x0001) ^ (data & 0x0001)) { - crc = (crc >> 1) ^ POLY; - - } else { - crc >>= 1; - } - } - } - - crc = ~crc; // Compute complement of CRC - data = crc; - crc = (crc << 8) | (data >> 8 & 0xFF); // Perform byte swap prior to returning CRC - - return crc; -} - -/** - * convert 12 bit integer format to int16. - */ -static int16_t -convert12BitToINT16(uint16_t word) -{ - int16_t outputbuffer = 0; - - if ((word >> 11) & 0x1) { - outputbuffer = (word & 0xfff) | 0xf000; - - } else { - outputbuffer = (word & 0x0fff); - } - - return (outputbuffer); -} - -int -ADIS16448::measure() -{ - // Start measuring. - perf_begin(_perf_read); - - // Fetch the full set of measurements from the ADIS16448 in one pass (burst read). -#pragma pack(push, 1) // Ensure proper memory alignment. - struct Report { - uint16_t cmd; - - uint16_t DIAG_STAT; - - int16_t XGYRO_OUT; - int16_t YGYRO_OUT; - int16_t ZGYRO_OUT; - - int16_t XACCL_OUT; - int16_t YACCL_OUT; - int16_t ZACCL_OUT; - - int16_t XMAGN_OUT; - int16_t YMAGN_OUT; - int16_t ZMAGN_OUT; - - uint16_t BARO_OUT; - - uint16_t TEMP_OUT; - - uint16_t CRC16; - } report{}; -#pragma pack(pop) - - report.cmd = ((ADIS16448_GLOB_CMD | DIR_READ) << 8) & 0xff00; - - const hrt_abstime timestamp_sample = hrt_absolute_time(); - - perf_begin(_perf_transfer); - - if (OK != transferhword((uint16_t *)&report, ((uint16_t *)&report), sizeof(report) / sizeof(int16_t))) { - perf_end(_perf_transfer); - perf_end(_perf_read); - - perf_count(_perf_bad_transfer); - - return -EIO; - } - - perf_end(_perf_transfer); - - // checksum - if (report.CRC16 != ComputeCRC16((uint16_t *)&report.DIAG_STAT)) { - perf_count(_perf_crc_bad); - perf_end(_perf_read); - return -EIO; - } - - // error count - const uint64_t error_count = perf_event_count(_perf_bad_transfer) + perf_event_count(_perf_crc_bad); - - // temperature - const float temperature = (convert12BitToINT16(report.TEMP_OUT) * 0.07386f) + 31.0f; // 0.07386°C/LSB, 31°C = 0x000 - - _px4_accel.set_error_count(error_count); - _px4_accel.set_temperature(temperature); - _px4_accel.update(timestamp_sample, report.XACCL_OUT, report.YACCL_OUT, report.ZACCL_OUT); - - _px4_gyro.set_error_count(error_count); - _px4_gyro.set_temperature(temperature); - _px4_gyro.update(timestamp_sample, report.XGYRO_OUT, report.YGYRO_OUT, report.ZGYRO_OUT); - - // DIAG_STAT bit 7: New data, xMAGN_OUT/BARO_OUT - const bool baro_mag_update = (report.DIAG_STAT & (1 << 7)); - - if (baro_mag_update) { - _px4_mag.set_error_count(error_count); - _px4_mag.set_temperature(temperature); - _px4_mag.update(timestamp_sample, report.XMAGN_OUT, report.YMAGN_OUT, report.ZMAGN_OUT); - - _px4_baro.set_error_count(error_count); - _px4_baro.set_temperature(temperature); - _px4_baro.update(timestamp_sample, report.BARO_OUT * ADIS16448_BARO_SENSITIVITY); - } - - // Stop measuring. - perf_end(_perf_read); - - return OK; -} - -void -ADIS16448::RunImpl() -{ - // Make another measurement. - measure(); -} diff --git a/src/drivers/imu/adis16448/ADIS16448.h b/src/drivers/imu/adis16448/ADIS16448.h deleted file mode 100644 index 0b0b167996..0000000000 --- a/src/drivers/imu/adis16448/ADIS16448.h +++ /dev/null @@ -1,206 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2018-2019 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * 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 adis16448.cpp - * - * Driver for the Analog device ADIS16448 connected via SPI. - * - * @author Amir Melzer - * @author Andrew Tridgell - * @author Pat Hickey - * @author Lorenz Meier - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -using namespace time_literals; - -#define DIR_READ 0x00 -#define DIR_WRITE 0x80 - -#define ADIS16448_GPIO_CTRL 0x32 /* Auxiliary digital input/output control */ -#define ADIS16448_MSC_CTRL 0x34 /* Miscellaneous control */ -#define ADIS16448_SMPL_PRD 0x36 /* Internal sample period (rate) control */ -#define ADIS16448_SENS_AVG 0x38 /* Dynamic range and digital filter control */ -#define ADIS16448_DIAG_STAT 0x3C /* System status */ -#define ADIS16448_GLOB_CMD 0x3E /* System command */ -#define ADIS16448_PRODUCT_ID 0x56 /* Product identifier */ -#define ADIS16334_SERIAL_NUMBER 0x58 /* Serial number, lot specific */ - -#define ADIS16448_Product 0x4040/* Product ID Description for ADIS16448 */ - -#define BITS_SMPL_PRD_NO_TAP_CFG (0<<8) -#define BITS_SMPL_PRD_2_TAP_CFG (1<<8) -#define BITS_SMPL_PRD_4_TAP_CFG (2<<8) -#define BITS_SMPL_PRD_8_TAP_CFG (3<<8) -#define BITS_SMPL_PRD_16_TAP_CFG (4<<8) - -#define BITS_GYRO_DYN_RANGE_1000_CFG (4<<8) -#define BITS_GYRO_DYN_RANGE_500_CFG (2<<8) -#define BITS_GYRO_DYN_RANGE_250_CFG (1<<8) - -#define BITS_FIR_NO_TAP_CFG (0<<0) -#define BITS_FIR_2_TAP_CFG (1<<0) -#define BITS_FIR_4_TAP_CFG (2<<0) -#define BITS_FIR_8_TAP_CFG (3<<0) -#define BITS_FIR_16_TAP_CFG (4<<0) -#define BITS_FIR_32_TAP_CFG (5<<0) -#define BITS_FIR_64_TAP_CFG (6<<0) -#define BITS_FIR_128_TAP_CFG (7<<0) - -#define T_STALL 9 - -static constexpr float ADIS16448_ACCEL_SENSITIVITY{1.0f / 1200.0f * CONSTANTS_ONE_G}; // 1200 LSB/g -static constexpr float ADIS16448_GYRO_INITIAL_SENSITIVITY{math::radians(1.0 / 25.0)}; // 25 LSB/°/sec -static constexpr float ADIS16448_BARO_SENSITIVITY{0.02f}; // 20 microbar per LSB, -static constexpr float ADIS16448_MAG_SENSITIVITY{1.0 / 7.0 / 1000.0}; // 7 LSB/mgauss - - -static constexpr float ADIS16448_ACCEL_GYRO_UPDATE_RATE{819.2}; // accel and gryo max update 819.2 samples per second -static constexpr float ADIS16448_MAG_BARO_UPDATE_RATE{51.2}; // xMAGN_OUT and BARO_OUT registers update at 51.2 samples per second - -class ADIS16448 : public device::SPI, public I2CSPIDriver -{ -public: - ADIS16448(I2CSPIBusOption bus_option, int bus, int32_t device, enum Rotation rotation, int bus_frequency, - spi_mode_e spi_mode); - virtual ~ADIS16448(); - - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); - static void print_usage(); - - int init() override; - - void print_status() override; - - void RunImpl(); -protected: - int probe() override; - -private: - - enum class - Register : uint8_t { - GPIO_CTRL = 0x32, // Auxiliary digital input/output control - MSC_CTRL = 0x34, // Miscellaneous control - SMPL_PRD = 0x36, // Internal sample period (rate) control - SENS_AVG = 0x38, // Dynamic range and digital filter control - - DIAG_STAT = 0x3C, // System status - GLOB_CMD = 0x3E, // System command - - PRODUCT_ID = 0x56, // Product identifier - SERIAL_NUMBER = 0x58, // Serial number, lot specific - }; - - /** - * Fetch measurements from the sensor and update the report buffers. - */ - int measure(); - - /** - * Modify a register in the ADIS16448 - * Bits are cleared before bits are set. - * - * @param reg The register to modify. - * @param clearbits Bits in the register to clear. - * @param setbits Bits in the register to set. - */ - void modify_reg16(unsigned reg, uint16_t clearbits, uint16_t setbits); - - /** - * Resets the chip and measurements ranges - */ - bool reset(); - - bool self_test(); - - /** - * Read a register from the ADIS16448 - * @arg reg The register to read. - * @return Returns the register value. - */ - uint16_t read_reg16(unsigned reg); - - /** - * Write a register in the ADIS16448 - * @param reg The register to write. - * @param value The new value to write. - */ - void write_reg16(unsigned reg, uint16_t value); - - /** - * Set low pass filter frequency. - */ - bool set_dlpf_filter(uint16_t frequency_hz); - - /** - * Set the gyroscope dynamic range. - */ - bool set_gyro_dyn_range(uint16_t desired_gyro_dyn_range); - - /** - * Set sample rate (approximate) - 1kHz to 5Hz. - */ - bool set_sample_rate(uint16_t desired_sample_rate_hz); - - /** - * Start automatic measurement. - */ - void start(); - - PX4Accelerometer _px4_accel; - PX4Barometer _px4_baro; - PX4Gyroscope _px4_gyro; - PX4Magnetometer _px4_mag; - - uint16_t _product_ID{0}; // Product ID code. - - static constexpr float _sample_rate{ADIS16448_ACCEL_GYRO_UPDATE_RATE}; - - perf_counter_t _perf_read{perf_counter_t(perf_alloc(PC_ELAPSED, "ADIS16448: read"))}; - perf_counter_t _perf_transfer{perf_counter_t(perf_alloc(PC_ELAPSED, "ADIS16448: transfer"))}; - perf_counter_t _perf_bad_transfer{perf_counter_t(perf_alloc(PC_COUNT, "ADIS16448: bad transfers"))}; - perf_counter_t _perf_crc_bad{perf_counter_t(perf_alloc(PC_COUNT, "ADIS16448: CRC16 bad"))}; - -}; diff --git a/src/drivers/imu/analog_devices/CMakeLists.txt b/src/drivers/imu/analog_devices/CMakeLists.txt new file mode 100644 index 0000000000..cef30903c9 --- /dev/null +++ b/src/drivers/imu/analog_devices/CMakeLists.txt @@ -0,0 +1,34 @@ +############################################################################ +# +# Copyright (c) 2021 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +add_subdirectory(adis16448) diff --git a/src/drivers/imu/analog_devices/adis16448/ADIS16448.cpp b/src/drivers/imu/analog_devices/adis16448/ADIS16448.cpp new file mode 100644 index 0000000000..0f88db4963 --- /dev/null +++ b/src/drivers/imu/analog_devices/adis16448/ADIS16448.cpp @@ -0,0 +1,594 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "ADIS16448.hpp" + +using namespace time_literals; + +// computes the CCITT CRC16 on the data received from a burst read +static uint16_t ComputeCRC16(uint16_t burstData[13]) +{ + uint16_t crc = 0xFFFF; // Holds the CRC value + + unsigned int data; // Holds the lower/Upper byte for CRC computation + static constexpr unsigned int POLY = 0x1021; // Divisor used during CRC computation + + // Compute CRC on burst data starting from XGYRO_OUT and ending with TEMP_OUT. + // Start with the lower byte and then the upper byte of each word. + // i.e. Compute XGYRO_OUT_LSB CRC first and then compute XGYRO_OUT_MSB CRC. + for (int i = 1; i < 12; i++) { + unsigned int upperByte = (burstData[i] >> 8) & 0xFF; + unsigned int lowerByte = (burstData[i] & 0xFF); + data = lowerByte; // Compute lower byte CRC first + + for (int ii = 0; ii < 8; ii++, data >>= 1) { + if ((crc & 0x0001) ^ (data & 0x0001)) { + crc = (crc >> 1) ^ POLY; + + } else { + crc >>= 1; + } + } + + data = upperByte; // Compute upper byte of CRC + + for (int ii = 0; ii < 8; ii++, data >>= 1) { + if ((crc & 0x0001) ^ (data & 0x0001)) { + crc = (crc >> 1) ^ POLY; + + } else { + crc >>= 1; + } + } + } + + crc = ~crc; // Compute complement of CRC + data = crc; + crc = (crc << 8) | (data >> 8 & 0xFF); // Perform byte swap prior to returning CRC + + return crc; +} + +// convert 12 bit integer format to int16. +static int16_t convert12BitToINT16(uint16_t word) +{ + int16_t output = 0; + + if ((word >> 11) & 0x1) { + // sign extend + output = (word & 0xFFF) | 0xF000; + + } else { + output = (word & 0x0FFF); + } + + return output; +} + +ADIS16448::ADIS16448(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, + spi_drdy_gpio_t drdy_gpio) : + SPI(DRV_IMU_DEVTYPE_ADIS16448, MODULE_NAME, bus, device, SPIDEV_MODE3, bus_frequency), + I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), + _drdy_gpio(drdy_gpio), // TODO: DRDY disabled + _px4_accel(get_device_id(), rotation), + _px4_baro(get_device_id()), + _px4_gyro(get_device_id(), rotation), + _px4_mag(get_device_id(), rotation) +{ +} + +ADIS16448::~ADIS16448() +{ + perf_free(_reset_perf); + perf_free(_perf_crc_bad); + perf_free(_bad_register_perf); + perf_free(_bad_transfer_perf); +} + +int ADIS16448::init() +{ + int ret = SPI::init(); + + if (ret != PX4_OK) { + DEVICE_DEBUG("SPI::init failed (%i)", ret); + return ret; + } + + return Reset() ? 0 : -1; +} + +bool ADIS16448::Reset() +{ + _state = STATE::RESET; + DataReadyInterruptDisable(); + ScheduleClear(); + ScheduleNow(); + return true; +} + +void ADIS16448::exit_and_cleanup() +{ + DataReadyInterruptDisable(); + I2CSPIDriverBase::exit_and_cleanup(); +} + +void ADIS16448::print_status() +{ + I2CSPIDriverBase::print_status(); + + perf_print_counter(_reset_perf); + perf_print_counter(_perf_crc_bad); + perf_print_counter(_bad_register_perf); + perf_print_counter(_bad_transfer_perf); +} + +int ADIS16448::probe() +{ + // Power-On Start-Up Time 205 ms + if (hrt_absolute_time() < 205_ms) { + PX4_WARN("Power-On Start-Up Time is 205 ms"); + } + + const uint16_t PROD_ID = RegisterRead(Register::PROD_ID); + + if (PROD_ID != Product_identification) { + DEVICE_DEBUG("unexpected PROD_ID 0x%02x", PROD_ID); + return PX4_ERROR; + } + + const uint16_t SERIAL_NUM = RegisterRead(Register::SERIAL_NUM); + const uint16_t LOT_ID1 = RegisterRead(Register::LOT_ID1); + const uint16_t LOT_ID2 = RegisterRead(Register::LOT_ID2); + + PX4_INFO("Serial Number: 0x%02x, Lot ID1: 0x%02x ID2: 0x%02x", SERIAL_NUM, LOT_ID1, LOT_ID2); + + // Only enable CRC-16 for verified lots (HACK to support older ADIS16448AMLZ with no explicit detection) + if (LOT_ID1 == 0x1824) { + _check_crc = true; + } + + return PX4_OK; +} + +void ADIS16448::RunImpl() +{ + const hrt_abstime now = hrt_absolute_time(); + + switch (_state) { + case STATE::RESET: + perf_count(_reset_perf); + // GLOB_CMD: software reset + RegisterWrite(Register::GLOB_CMD, GLOB_CMD_BIT::Software_reset); + _reset_timestamp = now; + _failure_count = 0; + _state = STATE::WAIT_FOR_RESET; + ScheduleDelayed(90_ms); // Reset Recovery Time 90 ms + break; + + case STATE::WAIT_FOR_RESET: + + if (_self_test_passed) { + if ((RegisterRead(Register::PROD_ID) == Product_identification)) { + // if reset succeeded then configure + _state = STATE::CONFIGURE; + ScheduleNow(); + + } else { + // RESET not complete + if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + PX4_DEBUG("Reset failed, retrying"); + _state = STATE::RESET; + ScheduleDelayed(100_ms); + + } else { + PX4_DEBUG("Reset not complete, check again in 10 ms"); + ScheduleDelayed(10_ms); + } + } + + } else { + RegisterWrite(Register::MSC_CTRL, MSC_CTRL_BIT::Internal_self_test); + _state = STATE::SELF_TEST_CHECK; + ScheduleDelayed(45_ms); // Automatic Self-Test Time 45 ms + } + + break; + + case STATE::SELF_TEST_CHECK: { + const uint16_t DIAG_STAT = RegisterRead(Register::DIAG_STAT); + + if (DIAG_STAT & DIAG_STAT_BIT::Self_test_diagnostic_error_flag) { + PX4_ERR("self test failed"); + + // Magnetometer + if (DIAG_STAT & DIAG_STAT_BIT::Magnetometer_functional_test) { + // tolerate mag test failure (likely due to surrounding magnetic field) + PX4_ERR("Magnetometer functional test fail"); + } + + // Barometer + if (DIAG_STAT & DIAG_STAT_BIT::Barometer_functional_test) { + PX4_ERR("Barometer functional test test fail"); + } + + // Gyroscope + const bool gyro_x_fail = DIAG_STAT & DIAG_STAT_BIT::X_axis_gyroscope_self_test_failure; + const bool gyro_y_fail = DIAG_STAT & DIAG_STAT_BIT::Y_axis_gyroscope_self_test_failure; + const bool gyro_z_fail = DIAG_STAT & DIAG_STAT_BIT::Z_axis_gyroscope_self_test_failure; + + if (gyro_x_fail || gyro_y_fail || gyro_z_fail) { + PX4_ERR("gyroscope self-test failure"); + } + + // Accelerometer + const bool accel_x_fail = DIAG_STAT & DIAG_STAT_BIT::X_axis_accelerometer_self_test_failure; + const bool accel_y_fail = DIAG_STAT & DIAG_STAT_BIT::Y_axis_accelerometer_self_test_failure; + const bool accel_z_fail = DIAG_STAT & DIAG_STAT_BIT::Z_axis_accelerometer_self_test_failure; + + if (accel_x_fail || accel_y_fail || accel_z_fail) { + PX4_ERR("accelerometer self-test failure"); + } + + _self_test_passed = false; + _state = STATE::RESET; + ScheduleDelayed(1000_ms); + + } else { + PX4_DEBUG("self test passed"); + _self_test_passed = true; + _state = STATE::RESET; + ScheduleNow(); + } + } + + break; + + case STATE::CONFIGURE: + if (Configure()) { + // if configure succeeded then start reading + _state = STATE::READ; + + if (DataReadyInterruptConfigure()) { + _data_ready_interrupt_enabled = true; + + // backup schedule as a watchdog timeout + ScheduleDelayed(100_ms); + + } else { + _data_ready_interrupt_enabled = false; + ScheduleOnInterval(SAMPLE_INTERVAL_US, SAMPLE_INTERVAL_US); + } + + } else { + // CONFIGURE not complete + if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + PX4_DEBUG("Configure failed, resetting"); + _state = STATE::RESET; + + } else { + PX4_DEBUG("Configure failed, retrying"); + } + + ScheduleDelayed(100_ms); + } + + break; + + case STATE::READ: { + if (_data_ready_interrupt_enabled) { + // push backup schedule back + ScheduleDelayed(SAMPLE_INTERVAL_US * 2); + } + + bool success = false; + + struct BurstRead { + uint16_t cmd; + uint16_t DIAG_STAT; + int16_t XGYRO_OUT; + int16_t YGYRO_OUT; + int16_t ZGYRO_OUT; + int16_t XACCL_OUT; + int16_t YACCL_OUT; + int16_t ZACCL_OUT; + int16_t XMAGN_OUT; + int16_t YMAGN_OUT; + int16_t ZMAGN_OUT; + uint16_t BARO_OUT; + uint16_t TEMP_OUT; + uint16_t CRC16; + } buffer{}; + + // ADIS16448 burst report should be 224 bits + static_assert(sizeof(BurstRead) == (224 / 8), "ADIS16448 report not 224 bits"); + + buffer.cmd = static_cast(Register::GLOB_CMD) << 8; + set_frequency(SPI_SPEED_BURST); + + if (transferhword((uint16_t *)&buffer, (uint16_t *)&buffer, sizeof(buffer) / sizeof(uint16_t)) == PX4_OK) { + + bool publish_data = true; + + // checksum + if (_check_crc) { + if (buffer.CRC16 != ComputeCRC16((uint16_t *)&buffer.DIAG_STAT)) { + perf_count(_perf_crc_bad); + publish_data = false; + } + } + + if (buffer.DIAG_STAT == DIAG_STAT_BIT::SPI_communication_failure) { + perf_count(_bad_transfer_perf); + publish_data = false; + } + + if (publish_data) { + + const uint32_t error_count = perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf); + _px4_accel.set_error_count(error_count); + _px4_gyro.set_error_count(error_count); + + // temperature 0.07386°C/LSB, 31°C = 0x000 + const float temperature = (convert12BitToINT16(buffer.TEMP_OUT) * 0.07386f) + 31.f; + + _px4_accel.set_temperature(temperature); + _px4_gyro.set_temperature(temperature); + + // sensor's frame is +x forward, +y left, +z up + // flip y & z to publish right handed with z down (x forward, y right, z down) + const int16_t accel_x = buffer.XACCL_OUT; + const int16_t accel_y = (buffer.YACCL_OUT == INT16_MIN) ? INT16_MAX : -buffer.YACCL_OUT; + const int16_t accel_z = (buffer.ZACCL_OUT == INT16_MIN) ? INT16_MAX : -buffer.ZACCL_OUT; + + const int16_t gyro_x = buffer.XGYRO_OUT; + const int16_t gyro_y = (buffer.YGYRO_OUT == INT16_MIN) ? INT16_MAX : -buffer.YGYRO_OUT; + const int16_t gyro_z = (buffer.ZGYRO_OUT == INT16_MIN) ? INT16_MAX : -buffer.ZGYRO_OUT; + + _px4_accel.update(now, accel_x, accel_y, accel_z); + _px4_gyro.update(now, gyro_x, gyro_y, gyro_z); + + // DIAG_STAT bit 7: New data, xMAGN_OUT/BARO_OUT + if (buffer.DIAG_STAT & DIAG_STAT_BIT::New_data_xMAGN_OUT_BARO_OUT) { + _px4_mag.set_error_count(error_count); + _px4_mag.set_temperature(temperature); + + const int16_t mag_x = buffer.XMAGN_OUT; + const int16_t mag_y = (buffer.YMAGN_OUT == INT16_MIN) ? INT16_MAX : -buffer.YMAGN_OUT; + const int16_t mag_z = (buffer.ZMAGN_OUT == INT16_MIN) ? INT16_MAX : -buffer.ZMAGN_OUT; + _px4_mag.update(now, mag_x, mag_y, mag_z); + + _px4_baro.set_error_count(error_count); + _px4_baro.set_temperature(temperature); + + float pressure_pa = buffer.BARO_OUT * 0.02f; // 20 μbar per LSB + _px4_baro.update(now, pressure_pa); + } + + success = true; + + if (_failure_count > 0) { + _failure_count--; + } + } + + } else { + perf_count(_bad_transfer_perf); + } + + if (!success) { + _failure_count++; + + // full reset if things are failing consistently + if (_failure_count > 10) { + Reset(); + return; + } + } + + if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) { + // check configuration registers periodically or immediately following any failure + if (RegisterCheck(_register_cfg[_checked_register])) { + _last_config_check_timestamp = now; + _checked_register = (_checked_register + 1) % size_register_cfg; + + } else { + // register check failed, force reset + perf_count(_bad_register_perf); + Reset(); + } + } + } + + break; + } +} + +bool ADIS16448::Configure() +{ + // first set and clear all configured register bits + for (const auto ®_cfg : _register_cfg) { + RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits); + } + + // now check that all are configured + bool success = true; + + for (const auto ®_cfg : _register_cfg) { + if (!RegisterCheck(reg_cfg)) { + success = false; + } + } + + _px4_accel.set_scale(0.833f * 1e-3f * CONSTANTS_ONE_G); // 0.833 mg/LSB + _px4_gyro.set_scale(math::radians(0.04f)); // 0.04 °/sec/LSB + _px4_mag.set_scale(142.9f * 1e-6f); // μgauss/LSB + + _px4_accel.set_range(18.f * CONSTANTS_ONE_G); + _px4_gyro.set_range(math::radians(1000.f)); + + _px4_mag.set_external(external()); + + return success; +} + +int ADIS16448::DataReadyInterruptCallback(int irq, void *context, void *arg) +{ + static_cast(arg)->DataReady(); + return 0; +} + +void ADIS16448::DataReady() +{ + ScheduleNow(); +} + +bool ADIS16448::DataReadyInterruptConfigure() +{ + if (_drdy_gpio == 0) { + return false; + } + + // check if DIO1 is connected to data ready + { + // DIO1 output set low + RegisterWrite(Register::GPIO_CTRL, GPIO_CTRL_BIT::GPIO1_DIRECTION); + bool write0_valid = (px4_arch_gpioread(_drdy_gpio) == 1); + + // DIO1 output set high + RegisterWrite(Register::GPIO_CTRL, GPIO_CTRL_BIT::GPIO1_DATA_LEVEL | GPIO_CTRL_BIT::GPIO1_DIRECTION); + bool write1_valid = (px4_arch_gpioread(_drdy_gpio) == 0); + + // DIO1 output set low again + RegisterWrite(Register::GPIO_CTRL, GPIO_CTRL_BIT::GPIO1_DIRECTION); + bool write2_valid = (px4_arch_gpioread(_drdy_gpio) == 1); + + if (write0_valid && write1_valid && write2_valid) { + PX4_INFO("DIO1 DRDY valid"); + // Setup data ready on falling edge + return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &DataReadyInterruptCallback, this) == 0; + + } else { + PX4_DEBUG("DIO1 DRDY invalid"); + } + } + + // check if DIO2 is connected to data ready + { + // DIO2 output set low + RegisterWrite(Register::GPIO_CTRL, GPIO_CTRL_BIT::GPIO2_DIRECTION); + bool write0_valid = (px4_arch_gpioread(_drdy_gpio) == 1); + + // DIO2 output set high + RegisterWrite(Register::GPIO_CTRL, GPIO_CTRL_BIT::GPIO2_DATA_LEVEL | GPIO_CTRL_BIT::GPIO2_DIRECTION); + bool write1_valid = (px4_arch_gpioread(_drdy_gpio) == 0); + + // DIO2 output set low again + RegisterWrite(Register::GPIO_CTRL, GPIO_CTRL_BIT::GPIO2_DIRECTION); + bool write2_valid = (px4_arch_gpioread(_drdy_gpio) == 1); + + if (write0_valid && write1_valid && write2_valid) { + PX4_INFO("DIO2 DRDY valid"); + + } else { + PX4_DEBUG("DIO2 DRDY invalid"); + } + } + + return false; +} + +bool ADIS16448::DataReadyInterruptDisable() +{ + if (_drdy_gpio == 0) { + return false; + } + + return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0; +} + +bool ADIS16448::RegisterCheck(const register_config_t ®_cfg) +{ + bool success = true; + + const uint16_t reg_value = RegisterRead(reg_cfg.reg); + + if (reg_cfg.set_bits && ((reg_value & reg_cfg.set_bits) != reg_cfg.set_bits)) { + PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits); + success = false; + } + + if (reg_cfg.clear_bits && ((reg_value & reg_cfg.clear_bits) != 0)) { + PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits); + success = false; + } + + return success; +} + +uint16_t ADIS16448::RegisterRead(Register reg) +{ + set_frequency(SPI_SPEED); + + uint16_t cmd[1]; + cmd[0] = (static_cast(reg) << 8); + + transferhword(cmd, nullptr, 1); + usleep(SPI_STALL_PERIOD); + transferhword(nullptr, cmd, 1); + + return cmd[0]; +} + +void ADIS16448::RegisterWrite(Register reg, uint16_t value) +{ + set_frequency(SPI_SPEED); + + uint16_t cmd[2]; + cmd[0] = (((static_cast(reg)) | DIR_WRITE) << 8) | ((0x00FF & value)); + cmd[1] = (((static_cast(reg) + 1) | DIR_WRITE) << 8) | ((0xFF00 & value) >> 8); + + transferhword(cmd, nullptr, 1); + usleep(SPI_STALL_PERIOD); + transferhword(cmd + 1, nullptr, 1); +} + +void ADIS16448::RegisterSetAndClearBits(Register reg, uint16_t setbits, uint16_t clearbits) +{ + const uint16_t orig_val = RegisterRead(reg); + + uint16_t val = (orig_val & ~clearbits) | setbits; + + if (orig_val != val) { + RegisterWrite(reg, val); + } +} diff --git a/src/drivers/imu/analog_devices/adis16448/ADIS16448.hpp b/src/drivers/imu/analog_devices/adis16448/ADIS16448.hpp new file mode 100644 index 0000000000..4987fef32b --- /dev/null +++ b/src/drivers/imu/analog_devices/adis16448/ADIS16448.hpp @@ -0,0 +1,138 @@ +/**************************************************************************** + * + * Copyright (c) 2021 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 ADIS16448.hpp + * + * Driver for the Analog Devices ADIS16448 connected via SPI. + * + */ + +#pragma once + +#include "Analog_Devices_ADIS16448_registers.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace Analog_Devices_ADIS16448; + +class ADIS16448 : public device::SPI, public I2CSPIDriver +{ +public: + ADIS16448(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, + spi_drdy_gpio_t drdy_gpio); + ~ADIS16448() override; + + static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance); + static void print_usage(); + + void RunImpl(); + + int init() override; + void print_status() override; + +private: + void exit_and_cleanup() override; + + struct register_config_t { + Register reg; + uint16_t set_bits{0}; + uint16_t clear_bits{0}; + }; + + int probe() override; + + bool Reset(); + + bool Configure(); + + static int DataReadyInterruptCallback(int irq, void *context, void *arg); + void DataReady(); + bool DataReadyInterruptConfigure(); + bool DataReadyInterruptDisable(); + + bool RegisterCheck(const register_config_t ®_cfg); + + uint16_t RegisterRead(Register reg); + void RegisterWrite(Register reg, uint16_t value); + void RegisterSetAndClearBits(Register reg, uint16_t setbits, uint16_t clearbits); + + const spi_drdy_gpio_t _drdy_gpio; + + PX4Accelerometer _px4_accel; + PX4Barometer _px4_baro; + PX4Gyroscope _px4_gyro; + PX4Magnetometer _px4_mag; + + perf_counter_t _reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": reset")}; + perf_counter_t _perf_crc_bad{perf_counter_t(perf_alloc(PC_COUNT, MODULE_NAME": CRC16 bad"))}; + perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")}; + perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")}; + + hrt_abstime _reset_timestamp{0}; + hrt_abstime _last_config_check_timestamp{0}; + int _failure_count{0}; + + bool _check_crc{false}; // CRC-16 not supported on earlier models (eg ADIS16448AMLZ) + bool _data_ready_interrupt_enabled{false}; + + bool _self_test_passed{false}; + + enum class STATE : uint8_t { + RESET, + WAIT_FOR_RESET, + SELF_TEST_CHECK, + CONFIGURE, + READ, + } _state{STATE::RESET}; + + uint8_t _checked_register{0}; + static constexpr uint8_t size_register_cfg{4}; + register_config_t _register_cfg[size_register_cfg] { + // Register | Set bits, Clear bits + { Register::MSC_CTRL, MSC_CTRL_BIT::CRC16_for_burst, 0 }, + { Register::SMPL_PRD, SMPL_PRD_BIT::internal_sampling_clock, SMPL_PRD_BIT::decimation_rate }, + { Register::SENS_AVG, SENS_AVG_BIT::Measurement_range_1000_set, SENS_AVG_BIT::Measurement_range_1000_clear | SENS_AVG_BIT::Filter_Size_Variable_B }, + { Register::GPIO_CTRL, GPIO_CTRL_BIT::GPIO2_DIRECTION | GPIO_CTRL_BIT::GPIO1_DIRECTION, 0}, + }; +}; diff --git a/src/drivers/imu/analog_devices/adis16448/Analog_Devices_ADIS16448_registers.hpp b/src/drivers/imu/analog_devices/adis16448/Analog_Devices_ADIS16448_registers.hpp new file mode 100644 index 0000000000..a3d221d36c --- /dev/null +++ b/src/drivers/imu/analog_devices/adis16448/Analog_Devices_ADIS16448_registers.hpp @@ -0,0 +1,161 @@ +/**************************************************************************** + * + * Copyright (c) 2021 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 Analog_Devices_ADIS16448_registers.hpp + * + * Analog Devices ADIS16448 registers. + * + */ + +#pragma once + +#include + +// TODO: move to a central header +static constexpr uint16_t Bit0 = (1 << 0); +static constexpr uint16_t Bit1 = (1 << 1); +static constexpr uint16_t Bit2 = (1 << 2); +static constexpr uint16_t Bit3 = (1 << 3); +static constexpr uint16_t Bit4 = (1 << 4); +static constexpr uint16_t Bit5 = (1 << 5); +static constexpr uint16_t Bit6 = (1 << 6); +static constexpr uint16_t Bit7 = (1 << 7); +static constexpr uint16_t Bit8 = (1 << 8); +static constexpr uint16_t Bit9 = (1 << 9); +static constexpr uint16_t Bit10 = (1 << 10); +static constexpr uint16_t Bit11 = (1 << 11); +static constexpr uint16_t Bit12 = (1 << 12); +static constexpr uint16_t Bit13 = (1 << 13); +static constexpr uint16_t Bit14 = (1 << 14); +static constexpr uint16_t Bit15 = (1 << 15); + +namespace Analog_Devices_ADIS16448 +{ +static constexpr uint32_t SPI_SPEED = 2 * 1000 * 1000; // 2 MHz SPI serial interface +static constexpr uint32_t SPI_SPEED_BURST = 1 * 1000 * 1000; // 1 MHz SPI serial interface for burst read + +static constexpr uint32_t SPI_STALL_PERIOD = 9; // 9 us Stall period between data + +static constexpr uint16_t DIR_WRITE = 0x80; + +static constexpr uint16_t Product_identification = 0x4040; + +static constexpr uint32_t SAMPLE_INTERVAL_US = (1e6f / 819.2f); // ~819.2 Hz + + +enum class Register : uint16_t { + GPIO_CTRL = 0x32, // Auxiliary digital input/output control + + MSC_CTRL = 0x34, // Miscellaneous control + SMPL_PRD = 0x36, // Internal sample period (rate) control + SENS_AVG = 0x38, // Dynamic range and digital filter control + + DIAG_STAT = 0x3C, // System status + GLOB_CMD = 0x3E, // System command + + LOT_ID1 = 0x52, // Lot identification number + LOT_ID2 = 0x54, // Lot identification number + PROD_ID = 0x56, // Product identifier + SERIAL_NUM = 0x58, // Lot-specific serial number +}; + +// MSC_CTRL +enum MSC_CTRL_BIT : uint16_t { + Checksum_memory_test = Bit11, // Checksum memory test (cleared upon completion) + Internal_self_test = Bit10, // Internal self test (cleared upon completion) + // Not used = Bit5 + CRC16_for_burst = Bit4, // include the CRC-16 code in burst read output sequence + // Not used = Bit3 + Data_ready_enable = Bit2, + Data_ready_polarity = Bit1, // 1 = active high when data is valid + Data_ready_line_select = Bit0, // Data ready line select 1 = DIO2, 0 = DIO1 +}; + +// DIAG_STAT +enum DIAG_STAT_BIT : uint16_t { + Z_axis_accelerometer_self_test_failure = Bit15, // 1 = fail, 0 = pass + Y_axis_accelerometer_self_test_failure = Bit14, // 1 = fail, 0 = pass + X_axis_accelerometer_self_test_failure = Bit13, // 1 = fail, 0 = pass + Z_axis_gyroscope_self_test_failure = Bit12, // 1 = fail, 0 = pass + Y_axis_gyroscope_self_test_failure = Bit11, // 1 = fail, 0 = pass + X_axis_gyroscope_self_test_failure = Bit10, // 1 = fail, 0 = pass + + New_data_xMAGN_OUT_BARO_OUT = Bit7, // New data, xMAGN_OUT/BARO_OUT + Flash_test_checksum_flag = Bit6, // 1 = fail, 0 = pass + Self_test_diagnostic_error_flag = Bit5, // 1 = fail, 0 = pass + + SPI_communication_failure = Bit3, // 1 = fail, 0 = pass + + Barometer_functional_test = Bit1, // 1 = fail, 0 = pass + Magnetometer_functional_test = Bit0, // 1 = fail, 0 = pass +}; + +// GLOB_CMD +enum GLOB_CMD_BIT : uint16_t { + Software_reset = Bit7, +}; + +// SMPL_PRD +enum SMPL_PRD_BIT : uint16_t { + // [12:8] D, decimation rate setting, binomial, + decimation_rate = Bit12 | Bit11 | Bit10 | Bit9, // disable + + internal_sampling_clock = Bit0, // 1 = internal sampling clock, 819.2 SPS +}; + +// SENS_AVG +enum SENS_AVG_BIT : uint16_t { + // [10:8] Measurement range (sensitivity) selection + Measurement_range_1000_set = Bit10, // 100 = ±1000°/sec (default condition) + Measurement_range_1000_clear = Bit9 | Bit8, + + // [2:0] Filter Size Variable B + Filter_Size_Variable_B = Bit2 | Bit1 | Bit0, // disable + +}; + +// GPIO_CTRL +enum GPIO_CTRL_BIT : uint16_t { + GPIO4_DATA_LEVEL = Bit11, // General-Purpose I/O Line 1 (DIO1) data level + GPIO3_DATA_LEVEL = Bit10, // General-Purpose I/O Line 1 (DIO1) data level + GPIO2_DATA_LEVEL = Bit9, // General-Purpose I/O Line 1 (DIO1) data level + GPIO1_DATA_LEVEL = Bit8, // General-Purpose I/O Line 1 (DIO1) data level + + GPIO4_DIRECTION = Bit3, // General-Purpose I/O Line 4 (DIO4) direction control 1 = output, 0 = input + GPIO3_DIRECTION = Bit2, // General-Purpose I/O Line 3 (DIO3) direction control 1 = output, 0 = input + GPIO2_DIRECTION = Bit1, // General-Purpose I/O Line 2 (DIO2) direction control 1 = output, 0 = input + GPIO1_DIRECTION = Bit0, // General-Purpose I/O Line 1 (DIO1) direction control 1 = output, 0 = input +}; + +} // namespace Analog_Devices_ADIS16448 diff --git a/src/drivers/imu/adis16448/CMakeLists.txt b/src/drivers/imu/analog_devices/adis16448/CMakeLists.txt similarity index 89% rename from src/drivers/imu/adis16448/CMakeLists.txt rename to src/drivers/imu/analog_devices/adis16448/CMakeLists.txt index 4f4463fb03..ed488387ef 100644 --- a/src/drivers/imu/adis16448/CMakeLists.txt +++ b/src/drivers/imu/analog_devices/adis16448/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2019 PX4 Development Team. All rights reserved. +# Copyright (c) 2021 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 @@ -30,18 +30,22 @@ # POSSIBILITY OF SUCH DAMAGE. # ############################################################################ + px4_add_module( - MODULE drivers__imu__adis16448 + MODULE drivers__imu__analog_devices__adis16448 MAIN adis16448 COMPILE_FLAGS - -Wno-cast-align # TODO: fix and enable + ${MAX_CUSTOM_OPT_LEVEL} + #-DDEBUG_BUILD SRCS ADIS16448.cpp + ADIS16448.hpp adis16448_main.cpp + Analog_Devices_ADIS16448_registers.hpp DEPENDS - px4_work_queue drivers_accelerometer drivers_barometer drivers_gyroscope drivers_magnetometer + px4_work_queue ) diff --git a/src/drivers/imu/adis16448/adis16448_main.cpp b/src/drivers/imu/analog_devices/adis16448/adis16448_main.cpp similarity index 91% rename from src/drivers/imu/adis16448/adis16448_main.cpp rename to src/drivers/imu/analog_devices/adis16448/adis16448_main.cpp index fbf3cdfc66..b04c9d6403 100644 --- a/src/drivers/imu/adis16448/adis16448_main.cpp +++ b/src/drivers/imu/analog_devices/adis16448/adis16448_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2018-2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2021 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -31,13 +31,12 @@ * ****************************************************************************/ -#include "ADIS16448.h" +#include "ADIS16448.hpp" #include #include -void -ADIS16448::print_usage() +void ADIS16448::print_usage() { PRINT_MODULE_USAGE_NAME("adis16448", "driver"); PRINT_MODULE_USAGE_SUBCATEGORY("imu"); @@ -51,7 +50,7 @@ I2CSPIDriverBase *ADIS16448::instantiate(const BusCLIArguments &cli, const BusIn int runtime_instance) { ADIS16448 *instance = new ADIS16448(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.rotation, - cli.bus_frequency, cli.spi_mode); + cli.bus_frequency, iterator.DRDYGPIO()); if (!instance) { PX4_ERR("alloc failed"); @@ -71,7 +70,7 @@ extern "C" int adis16448_main(int argc, char *argv[]) int ch; using ThisDriver = ADIS16448; BusCLIArguments cli{false, true}; - cli.default_spi_frequency = 1000000; + cli.default_spi_frequency = SPI_SPEED; while ((ch = cli.getopt(argc, argv, "R:")) != EOF) { switch (ch) { @@ -92,13 +91,11 @@ extern "C" int adis16448_main(int argc, char *argv[]) if (!strcmp(verb, "start")) { return ThisDriver::module_start(cli, iterator); - } - if (!strcmp(verb, "stop")) { + } else if (!strcmp(verb, "stop")) { return ThisDriver::module_stop(iterator); - } - if (!strcmp(verb, "status")) { + } else if (!strcmp(verb, "status")) { return ThisDriver::module_status(iterator); } diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp deleted file mode 100644 index 52e5b65cfc..0000000000 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ /dev/null @@ -1,1397 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012-2015 PX4 Development Team. All rights reserved. - * Author: Marco Bauer - * - * 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 mkblctrl.cpp - * - * Driver/configurator for the Mikrokopter BL-Ctrl. - * - * @author Marco Bauer - * - */ - -#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 - -#define I2C_BUS_SPEED 100000 -#define UPDATE_RATE 200 -#define MAX_MOTORS 8 -#define BLCTRL_BASE_ADDR 0x29 -#define BLCTRL_OLD 0 -#define BLCTRL_NEW 1 -#define BLCTRL_MIN_VALUE -0.920F -#define MOTOR_STATE_PRESENT_MASK 0x80 -#define MOTOR_STATE_ERROR_MASK 0x7F -#define MOTOR_SPINUP_COUNTER 30 -#define MOTOR_LOCATE_DELAY 10000000 -#define ESC_UORB_PUBLISH_DELAY 500000 - -#define CONTROL_INPUT_DROP_LIMIT_MS 20 -#define RC_MIN_VALUE 1010 -#define RC_MAX_VALUE 2100 - - -struct MotorData_t { - unsigned int Version; // the version of the BL (0 = old) - unsigned int SetPoint; // written by attitude controller - unsigned int SetPointLowerBits; // for higher Resolution of new BLs - float SetPoint_PX4; // Values from PX4 - unsigned int State; // 7 bit for I2C error counter, highest bit indicates if motor is present - unsigned int ReadMode; // select data to read - unsigned short RawPwmValue; // length of PWM pulse - // the following bytes must be exactly in that order! - unsigned int Current; // in 0.1 A steps, read back from BL - unsigned int MaxPWM; // read back from BL is less than 255 if BL is in current limit - unsigned int Temperature; // old BL-Ctrl will return a 255 here, the new version the temp. in - unsigned int RoundCount; -}; - - - - -class MK : public device::I2C -{ -public: - enum MappingMode { - MAPPING_MK = 0, - MAPPING_PX4, - }; - - enum FrameType { - FRAME_PLUS = 0, - FRAME_X, - }; - - MK(int bus, const char *_device_path); - ~MK(); - - virtual int ioctl(file *filp, int cmd, unsigned long arg); - virtual int init(unsigned motors); - virtual ssize_t write(file *filp, const char *buffer, size_t len); - - int set_motor_count(unsigned count); - int set_motor_test(bool motortest); - int set_overrideSecurityChecks(bool overrideSecurityChecks); - void set_px4mode(int px4mode); - void set_frametype(int frametype); - unsigned int mk_check_for_blctrl(unsigned int count, bool showOutput, bool initI2C); - void set_rc_min_value(unsigned value); - void set_rc_max_value(unsigned value); - -private: - static const unsigned _max_actuators = MAX_MOTORS; - static const bool showDebug = false; - - int _update_rate; - int _task; - int _t_actuators; - int _t_actuator_armed; - unsigned int _motor; - int _px4mode; - int _frametype; - char _device[20]; - orb_advert_t _t_outputs; - orb_advert_t _t_esc_status; - uORB::Publication _tune_control_pub{ORB_ID(tune_control)}; - unsigned int _num_outputs; - bool _primary_pwm_device; - bool _motortest; - bool _overrideSecurityChecks; - volatile bool _task_should_exit; - bool _armed; - unsigned long debugCounter; - MixerGroup *_mixers; - bool _indicate_esc; - unsigned _rc_min_value; - unsigned _rc_max_value; - param_t _param_indicate_esc; - actuator_controls_s _controls; - MotorData_t Motor[MAX_MOTORS]; - - static int task_main_trampoline(int argc, char *argv[]); - int task_main(); - - static int control_callback(uintptr_t handle, - uint8_t control_group, - uint8_t control_index, - float &input); - - int pwm_ioctl(file *filp, int cmd, unsigned long arg); - int mk_servo_arm(bool status); - int mk_servo_set(unsigned int chan, short val); - int mk_servo_test(unsigned int chan); - int mk_servo_locate(); - short scaling(float val, float inMin, float inMax, float outMin, float outMax); - void play_beep(int count); - -}; - - - - -const int blctrlAddr_quad_plus[] = { 2, 2, -2, -2, 0, 0, 0, 0 }; // Addresstranslator for Quad + configuration -const int blctrlAddr_hexa_plus[] = { 0, 2, 2, -2, 1, -3, 0, 0 }; // Addresstranslator for Hexa + configuration -const int blctrlAddr_octo_plus[] = { 0, 3, -1, 0, 3, 0, 0, -5 }; // Addresstranslator for Octo + configuration - -const int blctrlAddr_quad_x[] = { 2, 2, -2, -2, 0, 0, 0, 0 }; // Addresstranslator for Quad X configuration -const int blctrlAddr_hexa_x[] = { 2, 4, -2, 0, -3, -1, 0, 0 }; // Addresstranslator for Hexa X configuration -const int blctrlAddr_octo_x[] = { 1, 4, 0, 1, -4, 1, 1, -4 }; // Addresstranslator for Octo X configuration - -const int blctrlAddr_px4[] = { 0, 0, 0, 0, 0, 0, 0, 0}; // Native PX4 order - nothing to translate - -int addrTranslator[] = {0, 0, 0, 0, 0, 0, 0, 0}; // work copy - -namespace -{ - -MK *g_mk; - -} // namespace - -MK::MK(int bus, const char *_device_path) : - I2C(0, "mkblctrl", bus, 0, I2C_BUS_SPEED), - _update_rate(UPDATE_RATE), - _task(-1), - _t_actuators(-1), - _t_actuator_armed(-1), - _motor(-1), - _px4mode(MAPPING_MK), - _frametype(FRAME_PLUS), - _t_outputs(0), - _t_esc_status(0), - _num_outputs(0), - _primary_pwm_device(false), - _motortest(false), - _overrideSecurityChecks(false), - _task_should_exit(false), - _armed(false), - _mixers(nullptr), - _indicate_esc(false), - _rc_min_value(RC_MIN_VALUE), - _rc_max_value(RC_MAX_VALUE) -{ - strncpy(_device, _device_path, sizeof(_device)); - /* enforce null termination */ - _device[sizeof(_device) - 1] = '\0'; -} - -MK::~MK() -{ - if (_task != -1) { - /* tell the task we want it to go away */ - _task_should_exit = true; - - unsigned i = 10; - - do { - /* wait 50ms - it should wake every 100ms or so worst-case */ - usleep(50000); - - /* if we have given up, kill it */ - if (--i == 0) { - task_delete(_task); - break; - } - - } while (_task != -1); - } - - /* clean up the alternate device node */ - if (_primary_pwm_device) { - unregister_driver(_device); - } - - g_mk = nullptr; -} - -int -MK::init(unsigned motors) -{ - _param_indicate_esc = param_find("MKBLCTRL_TEST"); - - _num_outputs = motors; - debugCounter = 0; - int ret; - ASSERT(_task == -1); - - ret = I2C::init(); - - if (ret != OK) { - warnx("I2C init failed"); - return ret; - } - - usleep(500000); - - if (sizeof(_device) > 0) { - ret = register_driver(_device, &fops, 0666, (void *)this); - - if (ret == OK) { - DEVICE_LOG("creating alternate output device"); - _primary_pwm_device = true; - } - - } - - /* start the IO interface task */ - _task = px4_task_spawn_cmd("mkblctrl", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 20, - 1500, - (main_t)&MK::task_main_trampoline, - nullptr); - - - if (_task < 0) { - DEVICE_DEBUG("task start failed: %d", errno); - return -errno; - } - - return OK; -} - -int -MK::task_main_trampoline(int argc, char *argv[]) -{ - return g_mk->task_main(); -} - -void -MK::set_px4mode(int px4mode) -{ - _px4mode = px4mode; -} - -void -MK::set_frametype(int frametype) -{ - _frametype = frametype; -} - -void -MK::set_rc_min_value(unsigned value) -{ - _rc_min_value = value; - PX4_INFO("rc_min = %i", _rc_min_value); -} - -void -MK::set_rc_max_value(unsigned value) -{ - _rc_max_value = value; - PX4_INFO("rc_max = %i", _rc_max_value); -} - -int -MK::set_motor_count(unsigned count) -{ - if (count > 0) { - - _num_outputs = count; - - if (_px4mode == MAPPING_MK) { - if (_frametype == FRAME_PLUS) { - PX4_INFO("Mikrokopter ESC addressing. Frame: +"); - - } else if (_frametype == FRAME_X) { - PX4_INFO("Mikrokopter ESC addressing. Frame: X"); - } - - if (_num_outputs == 4) { - if (_frametype == FRAME_PLUS) { - memcpy(&addrTranslator, &blctrlAddr_quad_plus, sizeof(blctrlAddr_quad_plus)); - - } else if (_frametype == FRAME_X) { - memcpy(&addrTranslator, &blctrlAddr_quad_x, sizeof(blctrlAddr_quad_x)); - } - - } else if (_num_outputs == 6) { - if (_frametype == FRAME_PLUS) { - memcpy(&addrTranslator, &blctrlAddr_hexa_plus, sizeof(blctrlAddr_hexa_plus)); - - } else if (_frametype == FRAME_X) { - memcpy(&addrTranslator, &blctrlAddr_hexa_x, sizeof(blctrlAddr_hexa_x)); - } - - } else if (_num_outputs == 8) { - if (_frametype == FRAME_PLUS) { - memcpy(&addrTranslator, &blctrlAddr_octo_plus, sizeof(blctrlAddr_octo_plus)); - - } else if (_frametype == FRAME_X) { - memcpy(&addrTranslator, &blctrlAddr_octo_x, sizeof(blctrlAddr_octo_x)); - } - } - - } else { - PX4_INFO("PX4 ESC addressing."); - memcpy(&addrTranslator, &blctrlAddr_px4, sizeof(blctrlAddr_px4)); - } - - if (_num_outputs == 4) { - PX4_INFO("4 ESCs = Quadrocopter"); - - } else if (_num_outputs == 6) { - PX4_INFO("6 ESCs = Hexacopter"); - - } else if (_num_outputs == 8) { - PX4_INFO("8 ESCs = Octocopter"); - } - - return OK; - - } else { - return -1; - } - -} - -int -MK::set_motor_test(bool motortest) -{ - _motortest = motortest; - return OK; -} - -int -MK::set_overrideSecurityChecks(bool overrideSecurityChecks) -{ - _overrideSecurityChecks = overrideSecurityChecks; - return OK; -} - -short -MK::scaling(float val, float inMin, float inMax, float outMin, float outMax) -{ - short retVal = 0; - - retVal = (val - inMin) / (inMax - inMin) * (outMax - outMin) + outMin; - - if (retVal < outMin) { - retVal = outMin; - - } else if (retVal > outMax) { - retVal = outMax; - } - - return retVal; -} - -void -MK::play_beep(int count) -{ - tune_control_s tune{}; - tune.tune_id = static_cast(TuneID::SINGLE_BEEP); - - for (int i = 0; i < count; i++) { - tune.timestamp = hrt_absolute_time(); - _tune_control_pub.publish(tune); - usleep(300000); - } -} - -int -MK::task_main() -{ - int32_t param_mkblctrl_test = 0; - /* - * Subscribe to the appropriate PWM output topic based on whether we are the - * primary PWM output or not. - */ - _t_actuators = orb_subscribe(ORB_ID(actuator_controls_0)); - orb_set_interval(_t_actuators, int(1000 / _update_rate)); /* set the topic update rate (200Hz)*/ - - /* - * Subscribe to actuator_armed topic. - */ - _t_actuator_armed = orb_subscribe(ORB_ID(actuator_armed)); - orb_set_interval(_t_actuator_armed, 200); /* 5Hz update rate */ - - /* - * advertise the mixed control outputs. - */ - actuator_outputs_s outputs; - memset(&outputs, 0, sizeof(outputs)); - int dummy; - _t_outputs = orb_advertise_multi(ORB_ID(actuator_outputs), &outputs, &dummy); - - /* - * advertise the blctrl status. - */ - esc_status_s esc; - memset(&esc, 0, sizeof(esc)); - _t_esc_status = orb_advertise(ORB_ID(esc_status), &esc); - - pollfd fds[2]; - fds[0].fd = _t_actuators; - fds[0].events = POLLIN; - fds[1].fd = _t_actuator_armed; - fds[1].events = POLLIN; - - up_pwm_servo_set_rate(_update_rate); /* unnecessary ? */ - - DEVICE_LOG("starting"); - - /* loop until killed */ - while (!_task_should_exit) { - - param_get(_param_indicate_esc, ¶m_mkblctrl_test); - - if (param_mkblctrl_test > 0) { - _indicate_esc = true; - - } else { - _indicate_esc = false; - } - - /* waiting for data */ - int ret = ::poll(&fds[0], 2, CONTROL_INPUT_DROP_LIMIT_MS); - - /* this would be bad... */ - if (ret < 0) { - DEVICE_LOG("poll error %d", errno); - usleep(1000000); - continue; - } - - /* do we have a control update? */ - if (fds[0].revents & POLLIN) { - - bool changed = false; - orb_check(_t_actuators, &changed); - - if (changed) { - - /* get controls - must always do this to avoid spinning */ - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls); - - /* can we mix? */ - if (_mixers != nullptr) { - - /* do mixing */ - outputs.noutputs = _mixers->mix(&outputs.output[0], _num_outputs); - outputs.timestamp = hrt_absolute_time(); - - /* iterate actuators */ - for (unsigned int i = 0; i < _num_outputs; i++) { - /* last resort: catch NaN, INF and out-of-band errors */ - if (i < outputs.noutputs && - PX4_ISFINITE(outputs.output[i]) && - outputs.output[i] >= -1.0f && - outputs.output[i] <= 1.0f) { - /* scale for PWM output 900 - 2100us */ - /* nothing to do here */ - } else { - /* - * Value is NaN, INF or out of band - set to the minimum value. - * This will be clearly visible on the servo status and will limit the risk of accidentally - * spinning motors. It would be deadly in flight. - */ - if (outputs.output[i] < -1.0f) { - outputs.output[i] = -1.0f; - - } else if (outputs.output[i] > 1.0f) { - outputs.output[i] = 1.0f; - - } else { - outputs.output[i] = -1.0f; - } - } - - if (!_overrideSecurityChecks) { - /* don't go under BLCTRL_MIN_VALUE */ - if (outputs.output[i] < BLCTRL_MIN_VALUE) { - outputs.output[i] = BLCTRL_MIN_VALUE; - } - } - - /* output to BLCtrl's */ - if (_motortest != true && _indicate_esc != true) { - Motor[i].SetPoint_PX4 = outputs.output[i]; - mk_servo_set(i, scaling(outputs.output[i], -1.0f, 1.0f, 0, - 2047)); // scale the output to 0 - 2047 and sent to output routine - } - } - } - } - } - - /* how about an arming update? */ - if (fds[1].revents & POLLIN) { - actuator_armed_s aa; - - /* get new value */ - orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &aa); - - /* update PWM servo armed status if armed and not locked down */ - mk_servo_arm(aa.armed && !aa.lockdown); - } - - /* - * Only update esc topic every half second. - */ - - if (hrt_absolute_time() - esc.timestamp > ESC_UORB_PUBLISH_DELAY) { - esc.counter++; - esc.timestamp = hrt_absolute_time(); - esc.esc_count = (uint8_t) _num_outputs; - esc.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_I2C; - esc.esc_online_flags = (1 << esc.esc_count) - 1; - esc.esc_armed_flags = (1 << esc.esc_count) - 1; - - for (unsigned int i = 0; i < _num_outputs; i++) { - esc.esc[i].esc_address = (uint8_t) BLCTRL_BASE_ADDR + i; - esc.esc[i].esc_voltage = 0.0F; - esc.esc[i].esc_current = static_cast(Motor[i].Current) * 0.1F; - esc.esc[i].esc_rpm = (uint16_t) 0; - - esc.esc[i].esc_temperature = static_cast(Motor[i].Temperature); - esc.esc[i].esc_state = (uint8_t) Motor[i].State; - esc.esc[i].esc_errorcount = (uint16_t) 0; - - // if motortest is requested - do it... (deprecated in future) - if (_motortest == true) { - mk_servo_test(i); - } - - // if esc locate is requested - if (_indicate_esc == true) { - mk_servo_locate(); - } - } - - orb_publish(ORB_ID(esc_status), _t_esc_status, &esc); - - } - - } - - ::close(_t_actuators); - ::close(_t_actuator_armed); - - - /* make sure servos are off */ - up_pwm_servo_deinit(); - - DEVICE_LOG("stopping"); - - /* note - someone else is responsible for restoring the GPIO config */ - - /* tell the dtor that we are exiting */ - _task = -1; - return 0; -} - - -int -MK::mk_servo_arm(bool status) -{ - _armed = status; - return 0; -} - - -unsigned int -MK::mk_check_for_blctrl(unsigned int count, bool showOutput, bool initI2C) -{ - if (initI2C) { - I2C::init(); - } - - _retries = 50; - uint8_t foundMotorCount = 0; - - for (unsigned i = 0; i < MAX_MOTORS; i++) { - Motor[i].Version = 0; - Motor[i].SetPoint = 0; - Motor[i].SetPointLowerBits = 0; - Motor[i].State = 0; - Motor[i].ReadMode = 0; - Motor[i].RawPwmValue = 0; - Motor[i].Current = 0; - Motor[i].MaxPWM = 0; - Motor[i].Temperature = 0; - Motor[i].RoundCount = 0; - } - - uint8_t msg = 0; - uint8_t result[3]; - - for (unsigned i = 0; i < count; i++) { - result[0] = 0; - result[1] = 0; - result[2] = 0; - - set_device_address(BLCTRL_BASE_ADDR + i); - - if (OK == transfer(&msg, 1, &result[0], 3)) { - Motor[i].Current = result[0]; - Motor[i].MaxPWM = result[1]; - Motor[i].Temperature = result[2]; - Motor[i].State |= MOTOR_STATE_PRESENT_MASK; // set present bit; - foundMotorCount++; - - if ((Motor[i].MaxPWM & 252) == 248) { - Motor[i].Version = BLCTRL_NEW; - - } else { - Motor[i].Version = BLCTRL_OLD; - } - } - } - - if (showOutput) { - PX4_INFO("MotorsFound: %i", foundMotorCount); - - for (unsigned i = 0; i < foundMotorCount; i++) { - PX4_INFO("blctrl[%i] : found=%i\tversion=%i\tcurrent=%i\tmaxpwm=%i\ttemperature=%i", i, - Motor[i].State, Motor[i].Version, Motor[i].Current, Motor[i].MaxPWM, Motor[i].Temperature); - } - - - if (!_overrideSecurityChecks) { - if (foundMotorCount != 4 && foundMotorCount != 6 && foundMotorCount != 8) { - _task_should_exit = true; - } - } - } - - return foundMotorCount; -} - -int -MK::mk_servo_set(unsigned int chan, short val) -{ - short tmpVal = 0; - _retries = 0; - uint8_t result[3] = { 0, 0, 0 }; - uint8_t msg[2] = { 0, 0 }; - uint8_t bytesToSendBL2 = 2; - - tmpVal = val; - - if (tmpVal > 2047) { - tmpVal = 2047; - - } else if (tmpVal < 0) { - tmpVal = 0; - } - - Motor[chan].SetPoint = (uint8_t)(tmpVal >> 3) & 0xff; - Motor[chan].SetPointLowerBits = ((uint8_t)tmpVal % 8) & 0x07; - - if (_armed == false) { - Motor[chan].SetPoint = 0; - Motor[chan].SetPointLowerBits = 0; - } - - //if(Motor[chan].State & MOTOR_STATE_PRESENT_MASK) { - set_device_address(BLCTRL_BASE_ADDR + (chan + addrTranslator[chan])); - - if (Motor[chan].Version == BLCTRL_OLD) { - /* - * Old BL-Ctrl 8Bit served. Version < 2.0 - */ - msg[0] = Motor[chan].SetPoint; - - if (Motor[chan].RoundCount >= 16) { - // on each 16th cyle we read out the status messages from the blctrl - if (OK == transfer(&msg[0], 1, &result[0], 2)) { - Motor[chan].Current = result[0]; - Motor[chan].MaxPWM = result[1]; - Motor[chan].Temperature = 255; - - } else { - if ((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) { Motor[chan].State++; } // error - } - - Motor[chan].RoundCount = 0; - - } else { - if (OK != transfer(&msg[0], 1, nullptr, 0)) { - if ((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) { Motor[chan].State++; } // error - } - } - - } else { - /* - * New BL-Ctrl 11Bit served. Version >= 2.0 - */ - msg[0] = Motor[chan].SetPoint; - msg[1] = Motor[chan].SetPointLowerBits; - - if (Motor[chan].SetPointLowerBits == 0) { - bytesToSendBL2 = 1; // if setpoint lower bits are zero, we send only the higher bits - this saves time - } - - if (Motor[chan].RoundCount >= 16) { - // on each 16th cyle we read out the status messages from the blctrl - if (OK == transfer(&msg[0], bytesToSendBL2, &result[0], 3)) { - Motor[chan].Current = result[0]; - Motor[chan].MaxPWM = result[1]; - Motor[chan].Temperature = result[2]; - - } else { - if ((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) { Motor[chan].State++; } // error - } - - Motor[chan].RoundCount = 0; - - } else { - if (OK != transfer(&msg[0], bytesToSendBL2, nullptr, 0)) { - if ((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) { Motor[chan].State++; } // error - } - } - - } - - Motor[chan].RoundCount++; - //} - - if (showDebug == true) { - debugCounter++; - - if (debugCounter == 2000) { - debugCounter = 0; - - for (unsigned int i = 0; i < _num_outputs; i++) { - if (Motor[i].State & MOTOR_STATE_PRESENT_MASK) { - PX4_INFO("#%i:\tVer: %i\tVal: %i\tCurr: %i\tMaxPWM: %i\tTemp: %i\tState: %i", i, Motor[i].Version, - Motor[i].SetPoint, Motor[i].Current, Motor[i].MaxPWM, Motor[i].Temperature, Motor[i].State); - } - } - - PX4_INFO("\n"); - } - } - - return 0; -} - - -int -MK::mk_servo_test(unsigned int chan) -{ - int ret = 0; - float tmpVal = 0; - float val = -1; - _retries = 0; - uint8_t msg[2] = { 0, 0 }; - - if (debugCounter >= MOTOR_SPINUP_COUNTER) { - debugCounter = 0; - _motor++; - - if (_motor < _num_outputs) { - PX4_INFO("Motortest - #%i:\tspinup", _motor); - } - - if (_motor >= _num_outputs) { - _motor = -1; - _motortest = false; - PX4_INFO("Motortest finished..."); - } - } - - debugCounter++; - - if (_motor == chan) { - val = BLCTRL_MIN_VALUE; - - } else { - val = -1; - } - - tmpVal = (511 + (511 * val)); - - if (tmpVal > 1024) { - tmpVal = 1024; - } - - Motor[chan].SetPoint = (uint8_t)(tmpVal / 4); - - if (_motor != chan) { - Motor[chan].SetPoint = 0; - Motor[chan].SetPointLowerBits = 0; - } - - if (Motor[chan].Version == BLCTRL_OLD) { - msg[0] = Motor[chan].SetPoint; - - } else { - msg[0] = Motor[chan].SetPoint; - msg[1] = Motor[chan].SetPointLowerBits; - } - - set_device_address(BLCTRL_BASE_ADDR + (chan + addrTranslator[chan])); - - if (Motor[chan].Version == BLCTRL_OLD) { - ret = transfer(&msg[0], 1, nullptr, 0); - - } else { - ret = transfer(&msg[0], 2, nullptr, 0); - } - - return ret; -} - - -int -MK::mk_servo_locate() -{ - int ret = 0; - static unsigned int chan = 0; - static uint64_t last_timestamp = 0; - _retries = 0; - uint8_t msg[2] = { 0, 0 }; - - - if (hrt_absolute_time() - last_timestamp > MOTOR_LOCATE_DELAY) { - last_timestamp = hrt_absolute_time(); - - set_device_address(BLCTRL_BASE_ADDR + (chan + addrTranslator[chan])); - chan++; - - if (chan <= _num_outputs) { - PX4_INFO("ESC Locate - #%i:\tgreen", chan); - play_beep(chan); - } - - if (chan > _num_outputs) { - chan = 0; - } - } - - // do i2c transfer to selected esc - ret = transfer(&msg[0], 1, nullptr, 0); - - return ret; -} - - -int -MK::control_callback(uintptr_t handle, - uint8_t control_group, - uint8_t control_index, - float &input) -{ - const actuator_controls_s *controls = (actuator_controls_s *)handle; - - input = controls->control[control_index]; - return 0; -} - -int -MK::ioctl(file *filp, int cmd, unsigned long arg) -{ - int ret; - - ret = pwm_ioctl(filp, cmd, arg); - - /* if nobody wants it, let CDev have it */ - if (ret == -ENOTTY) { - ret = CDev::ioctl(filp, cmd, arg); - } - - return ret; -} - -int -MK::pwm_ioctl(file *filp, int cmd, unsigned long arg) -{ - int ret = OK; - - lock(); - - switch (cmd) { - case PWM_SERVO_ARM: - mk_servo_arm(true); - break; - - case PWM_SERVO_SET_ARM_OK: - case PWM_SERVO_CLEAR_ARM_OK: - // these are no-ops, as no safety switch - break; - - case PWM_SERVO_DISARM: - mk_servo_arm(false); - break; - - case PWM_SERVO_SET_UPDATE_RATE: - ret = OK; - break; - - case PWM_SERVO_GET_UPDATE_RATE: - *(uint32_t *)arg = _update_rate; - break; - - case PWM_SERVO_SET_SELECT_UPDATE_RATE: - ret = OK; - break; - - - case PWM_SERVO_SET(0) ... PWM_SERVO_SET(_max_actuators - 1): - if (arg < 2150) { - Motor[cmd - PWM_SERVO_SET(0)].RawPwmValue = (unsigned short)arg; - mk_servo_set(cmd - PWM_SERVO_SET(0), scaling(arg, _rc_min_value, _rc_max_value, 0, 2047)); - - } else { - ret = -EINVAL; - } - - break; - - case PWM_SERVO_GET(0) ... PWM_SERVO_GET(_max_actuators - 1): - /* copy the current output value from the channel */ - *(servo_position_t *)arg = Motor[cmd - PWM_SERVO_GET(0)].RawPwmValue; - - break; - - case PWM_SERVO_GET_RATEGROUP(0): - case PWM_SERVO_GET_RATEGROUP(1): - case PWM_SERVO_GET_RATEGROUP(2): - case PWM_SERVO_GET_RATEGROUP(3): - //*(uint32_t *)arg = up_pwm_servo_get_rate_group(cmd - PWM_SERVO_GET_RATEGROUP(0)); - break; - - case PWM_SERVO_GET_COUNT: - *(unsigned *)arg = _num_outputs; - break; - - case MIXERIOCRESET: - if (_mixers != nullptr) { - delete _mixers; - _mixers = nullptr; - } - - break; - - case MIXERIOCLOADBUF: { - const char *buf = (const char *)arg; - unsigned buflen = strlen(buf); - - if (_mixers == nullptr) { - _mixers = new MixerGroup(); - } - - if (_mixers == nullptr) { - ret = -ENOMEM; - - } else { - - ret = _mixers->load_from_buf(control_callback, (uintptr_t)&_controls, buf, buflen); - - if (ret != 0) { - DEVICE_DEBUG("mixer load failed with %d", ret); - delete _mixers; - _mixers = nullptr; - ret = -EINVAL; - } - } - - break; - } - - case PWM_SERVO_SET_MIN_PWM: { - struct pwm_output_values *pwm = (struct pwm_output_values *)arg; - - if (pwm->channel_count > _max_actuators) - /* fail with error */ - { - return -E2BIG; - } - - set_rc_min_value((unsigned)pwm->values[0]); - ret = OK; - break; - } - - case PWM_SERVO_GET_MIN_PWM: - ret = OK; - break; - - case PWM_SERVO_SET_MAX_PWM: { - struct pwm_output_values *pwm = (struct pwm_output_values *)arg; - - if (pwm->channel_count > _max_actuators) - /* fail with error */ - { - return -E2BIG; - } - - set_rc_max_value((unsigned)pwm->values[0]); - ret = OK; - break; - } - - case PWM_SERVO_GET_MAX_PWM: - ret = OK; - break; - - - default: - ret = -ENOTTY; - break; - } - - unlock(); - - return ret; -} - -/* - this implements PWM output via a write() method, for compatibility - with px4io - */ -ssize_t -MK::write(file *filp, const char *buffer, size_t len) -{ - unsigned count = len / 2; - uint16_t values[8]; - - if (count > _num_outputs) { - // we only have 8 I2C outputs in the driver - count = _num_outputs; - } - - // allow for misaligned values - memcpy(values, buffer, count * 2); - - for (uint8_t i = 0; i < count; i++) { - Motor[i].RawPwmValue = (unsigned short)values[i]; - mk_servo_set(i, scaling(values[i], _rc_min_value, _rc_max_value, 0, 2047)); - } - - return count * 2; -} - - -namespace -{ - -enum MappingMode { - MAPPING_MK = 0, - MAPPING_PX4, -}; - -enum FrameType { - FRAME_PLUS = 0, - FRAME_X, -}; - - -int -mk_new_mode(int motorcount, bool motortest, int px4mode, int frametype, bool overrideSecurityChecks, unsigned rcmin, - unsigned rcmax) -{ - int shouldStop = 0; - - /* set rc min pulse value */ - g_mk->set_rc_min_value(rcmin); - - /* set rc max pulse value */ - g_mk->set_rc_max_value(rcmax); - - /* native PX4 addressing) */ - g_mk->set_px4mode(px4mode); - - /* set frametype (geometry) */ - g_mk->set_frametype(frametype); - - /* motortest if enabled */ - g_mk->set_motor_test(motortest); - - /* ovveride security checks if enabled */ - g_mk->set_overrideSecurityChecks(overrideSecurityChecks); - - /* count used motors */ - do { - if (g_mk->mk_check_for_blctrl(8, false, false) != 0) { - shouldStop = 4; - - } else { - shouldStop++; - } - - sleep(1); - } while (shouldStop < 3); - - g_mk->set_motor_count(g_mk->mk_check_for_blctrl(8, true, false)); - - return OK; -} - -int -mk_start(unsigned motors, const char *device_path) -{ - int ret; - - // try i2c3 first - g_mk = new MK(3, device_path); - - if (!g_mk) { - return -ENOMEM; - } - - if (OK == g_mk->init(motors)) { - warnx("[mkblctrl] scanning i2c3...\n"); - ret = g_mk->mk_check_for_blctrl(8, false, false); - - if (ret > 0) { - return OK; - } - } - - delete g_mk; - g_mk = nullptr; - - // fallback to bus 1 - g_mk = new MK(1, device_path); - - if (!g_mk) { - return -ENOMEM; - } - - if (OK == g_mk->init(motors)) { - warnx("[mkblctrl] scanning i2c1...\n"); - ret = g_mk->mk_check_for_blctrl(8, false, false); - - if (ret > 0) { - return OK; - } - } - - delete g_mk; - g_mk = nullptr; - - return -ENXIO; -} - - -} // namespace - -extern "C" __EXPORT int mkblctrl_main(int argc, char *argv[]); - -int -mkblctrl_main(int argc, char *argv[]) -{ - int motorcount = 8; - int px4mode = MAPPING_PX4; - int frametype = FRAME_PLUS; // + plus is default - bool motortest = false; - bool overrideSecurityChecks = false; - bool showHelp = false; - bool newMode = true; - const char *devicepath = ""; - unsigned rc_min_value = RC_MIN_VALUE; - unsigned rc_max_value = RC_MAX_VALUE; - char *ep; - - /* - * optional parameters - */ - for (int i = 1; i < argc; i++) { - - /* look for the optional frame parameter */ - if (strcmp(argv[i], "-mkmode") == 0 || strcmp(argv[i], "--mkmode") == 0) { - if (argc > i + 1) { - if (strcmp(argv[i + 1], "+") == 0 || strcmp(argv[i + 1], "x") == 0 || strcmp(argv[i + 1], "X") == 0) { - px4mode = MAPPING_MK; - //newMode = true; - - if (strcmp(argv[i + 1], "+") == 0) { - frametype = FRAME_PLUS; - - } else { - frametype = FRAME_X; - } - - } else { - errx(1, "only + or x for frametype supported !"); - } - - } else { - errx(1, "missing argument for mkmode (-mkmode)"); - return 1; - } - } - - /* look for the optional test parameter */ - if (strcmp(argv[i], "-t") == 0) { - motortest = true; - //newMode = true; - } - - /* look for the optional -h --help parameter */ - if (strcmp(argv[i], "-h") == 0 || strcmp(argv[i], "--help") == 0) { - showHelp = true; - } - - /* look for the optional --override-security-checks parameter */ - if (strcmp(argv[i], "--override-security-checks") == 0) { - overrideSecurityChecks = true; - //newMode = true; - } - - /* look for the optional device parameter */ - if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) { - if (argc > i + 1) { - devicepath = argv[i + 1]; - //newMode = true; - - } else { - errx(1, "missing the devicename (-d)"); - return 1; - } - } - - /* look for the optional -rc_min parameter */ - if (strcmp(argv[i], "-rc_min") == 0) { - if (argc > i + 1) { - rc_min_value = strtoul(argv[i + 1], &ep, 0); - - if (*ep != '\0') { - errx(1, "bad pwm val (-rc_min)"); - return 1; - } - - } else { - errx(1, "missing value (-rc_min)"); - return 1; - } - } - - /* look for the optional -rc_max parameter */ - if (strcmp(argv[i], "-rc_max") == 0) { - if (argc > i + 1) { - rc_max_value = strtoul(argv[i + 1], &ep, 0); - - if (*ep != '\0') { - errx(1, "bad pwm val (-rc_max)"); - return 1; - } - - } else { - errx(1, "missing value (-rc_max)"); - return 1; - } - } - - - } - - if (showHelp) { - PX4_INFO("mkblctrl: help:"); - PX4_INFO(" [-mkmode {+/x}] [-b i2c_bus_number] [-d devicename] [--override-security-checks] [-h / --help]"); - PX4_INFO("\t -mkmode {+/x} \t\t Type of frame, if Mikrokopter motor order is used."); - PX4_INFO("\t -d {devicepath & name}\t\t Create alternate pwm device."); - PX4_INFO("\t --override-security-checks \t\t Disable all security checks (arming and number of ESCs). Used to test single Motors etc. (DANGER !!!)"); - PX4_INFO("\t -rcmin {pwn-value}\t\t Set RC_MIN Value."); - PX4_INFO("\t -rcmax {pwn-value}\t\t Set RC_MAX Value."); - PX4_INFO("\n"); - PX4_INFO("Motortest:"); - PX4_INFO("First you have to start mkblctrl, the you can enter Motortest Mode with:"); - PX4_INFO("mkblctrl -t"); - PX4_INFO("This will spin up once every motor in order of motoraddress. (DANGER !!!)"); - exit(1); - } - - - if (!motortest) { - if (g_mk == nullptr) { - if (mk_start(motorcount, devicepath) != OK) { - errx(1, "failed to start the MK-BLCtrl driver"); - } - - /* parameter set ? */ - if (newMode) { - /* switch parameter */ - return mk_new_mode(motorcount, motortest, px4mode, frametype, overrideSecurityChecks, rc_min_value, rc_max_value); - } - - exit(0); - - } else { - errx(1, "MK-BLCtrl driver already running"); - } - - } else { - if (g_mk == nullptr) { - errx(1, "MK-BLCtrl driver not running. You have to start it first."); - - } else { - g_mk->set_motor_test(motortest); - exit(0); - - } - } -} diff --git a/src/drivers/protocol_splitter/protocol_splitter.cpp b/src/drivers/protocol_splitter/protocol_splitter.cpp index 64b58afd20..ed25867f6e 100644 --- a/src/drivers/protocol_splitter/protocol_splitter.cpp +++ b/src/drivers/protocol_splitter/protocol_splitter.cpp @@ -55,6 +55,10 @@ class ReadBuffer; extern "C" __EXPORT int protocol_splitter_main(int argc, char *argv[]); + +const char *Sp2HeaderMagic = "SP2"; +const int Sp2HeaderSize = 8; + struct StaticData { Mavlink2Dev *mavlink2; RtpsDev *rtps; @@ -73,7 +77,8 @@ class ReadBuffer { public: int read(int fd); - void move(void *dest, size_t pos, size_t n); + void copy(void *dest, size_t pos, size_t n); + void remove(size_t pos, size_t n); uint8_t buffer[512] = {}; size_t buf_size = 0; @@ -106,12 +111,21 @@ int ReadBuffer::read(int fd) return r; } -void ReadBuffer::move(void *dest, size_t pos, size_t n) +void ReadBuffer::copy(void *dest, size_t pos, size_t n) +{ + ASSERT(pos < buf_size); + ASSERT(pos + n <= buf_size); + + if (dest) { + memmove(dest, buffer + pos, n); // send desired data + } +} + +void ReadBuffer::remove(size_t pos, size_t n) { ASSERT(pos < buf_size); ASSERT(pos + n <= buf_size); - memmove(dest, buffer + pos, n); // send desired data memmove(buffer + pos, buffer + (pos + n), sizeof(buffer) - pos - n); buf_size -= n; } @@ -131,6 +145,19 @@ public: protected: + /* + struct Sp2Header { + char magic[3]; + uint8_t type; + uint16_t payload_len; + uint16_t reserved (align) + } + */ + + enum MessageType {Mavlink = 0, Rtps}; + + uint8_t _header[8] = {}; + virtual pollevent_t poll_state(struct file *filp); @@ -246,12 +273,15 @@ Mavlink2Dev::Mavlink2Dev(ReadBuffer *read_buffer) : DevCommon("/dev/mavlink") , _read_buffer{read_buffer} { + memcpy(_header, Sp2HeaderMagic, 3); + _header[3] = MessageType::Mavlink; + memset(&_header[4], 0, 4); } ssize_t Mavlink2Dev::read(struct file *filp, char *buffer, size_t buflen) { int i, ret; - uint16_t packet_len = 0; + uint16_t packet_len, payload_len; /* last reading was partial (i.e., buffer didn't fit whole message), * so now we'll just send remaining bytes */ @@ -286,36 +316,28 @@ ssize_t Mavlink2Dev::read(struct file *filp, char *buffer, size_t buflen) ret = 0; - if (_read_buffer->buf_size < 3) { + if (_read_buffer->buf_size < Sp2HeaderSize) { goto end; } // Search for a mavlink packet on buffer to send it i = 0; - while ((unsigned)i < (_read_buffer->buf_size - 3) - && _read_buffer->buffer[i] != 253 - && _read_buffer->buffer[i] != 254) { + while ((unsigned)i < (_read_buffer->buf_size - Sp2HeaderSize) && + (_read_buffer->buffer[i] != 'S' + || _read_buffer->buffer[i + 1] != 'P' + || _read_buffer->buffer[i + 2] != '2' + || _read_buffer->buffer[i + 3] != (uint8_t) MessageType::Mavlink)) { i++; } - // We need at least the first three bytes to get packet len - if ((unsigned)i >= _read_buffer->buf_size - 3) { + // We need at least the first six bytes to get packet len + if ((unsigned)i >= _read_buffer->buf_size - Sp2HeaderSize) { goto end; } - if (_read_buffer->buffer[i] == 253) { - uint8_t payload_len = _read_buffer->buffer[i + 1]; - uint8_t incompat_flags = _read_buffer->buffer[i + 2]; - packet_len = payload_len + 12; - - if (incompat_flags & 0x1) { //signing - packet_len += 13; - } - - } else { - packet_len = _read_buffer->buffer[i + 1] + 8; - } + payload_len = ((uint16_t)_read_buffer->buffer[i + 4] << 8) | _read_buffer->buffer[i + 5]; + packet_len = payload_len + Sp2HeaderSize; // packet is bigger than what we've read, better luck next time if ((unsigned)i + packet_len > _read_buffer->buf_size) { @@ -324,16 +346,19 @@ ssize_t Mavlink2Dev::read(struct file *filp, char *buffer, size_t buflen) /* if buffer doesn't fit message, send what's possible and copy remaining * data into a temporary buffer on this class */ - if (packet_len > buflen) { - _read_buffer->move(buffer, i, buflen); - _read_buffer->move(_partial_buffer, i, packet_len - buflen); - _remaining_partial = packet_len - buflen; + if (payload_len > buflen) { + _read_buffer->copy(buffer, i + Sp2HeaderSize, buflen); + _read_buffer->copy(_partial_buffer, i + Sp2HeaderSize + buflen, payload_len - buflen); + _read_buffer->remove(i, packet_len); + _remaining_partial = payload_len - buflen; ret = buflen; goto end; } - _read_buffer->move(buffer, i, packet_len); - ret = packet_len; + _read_buffer->copy(buffer, i + Sp2HeaderSize, payload_len); + _read_buffer->remove(i, packet_len); + + ret = payload_len; end: unlock(Read); @@ -392,6 +417,9 @@ ssize_t Mavlink2Dev::write(struct file *filp, const char *buffer, size_t buflen) ret = -1; } else { + _header[4] = (uint8_t)((buflen >> 8) & 0xff); + _header[5] = (uint8_t)(buflen & 0xff); + ::write(_fd, _header, 8); ret = ::write(_fd, buffer, buflen); } @@ -426,6 +454,10 @@ RtpsDev::RtpsDev(ReadBuffer *read_buffer) : DevCommon("/dev/rtps") , _read_buffer{read_buffer} { + memcpy(_header, Sp2HeaderMagic, 3); + _header[3] = MessageType::Rtps; + memset(&_header[4], 0, 4); + } ssize_t RtpsDev::read(struct file *filp, char *buffer, size_t buflen) @@ -446,24 +478,28 @@ ssize_t RtpsDev::read(struct file *filp, char *buffer, size_t buflen) ret = 0; - if (_read_buffer->buf_size < HEADER_SIZE) { - goto end; // starting ">>>" + topic + seq + lenhigh + lenlow + crchigh + crclow + if (_read_buffer->buf_size < Sp2HeaderSize) { + goto end; } // Search for a rtps packet on buffer to send it i = 0; - while ((unsigned)i < (_read_buffer->buf_size - HEADER_SIZE) && (memcmp(_read_buffer->buffer + i, ">>>", 3) != 0)) { + while ((unsigned)i < (_read_buffer->buf_size - Sp2HeaderSize) && + (_read_buffer->buffer[i] != 'S' + || _read_buffer->buffer[i + 1] != 'P' + || _read_buffer->buffer[i + 2] != '2' + || _read_buffer->buffer[i + 3] != (uint8_t) MessageType::Rtps)) { i++; } // We need at least the first six bytes to get packet len - if ((unsigned)i >= _read_buffer->buf_size - HEADER_SIZE) { + if ((unsigned)i >= _read_buffer->buf_size - Sp2HeaderSize) { goto end; } - payload_len = ((uint16_t)_read_buffer->buffer[i + 5] << 8) | _read_buffer->buffer[i + 6]; - packet_len = payload_len + HEADER_SIZE; + payload_len = ((uint16_t)_read_buffer->buffer[i + 4] << 8) | _read_buffer->buffer[i + 5]; + packet_len = payload_len + Sp2HeaderSize; // packet is bigger than what we've read, better luck next time if ((unsigned)i + packet_len > _read_buffer->buf_size) { @@ -476,8 +512,9 @@ ssize_t RtpsDev::read(struct file *filp, char *buffer, size_t buflen) goto end; } - _read_buffer->move(buffer, i, packet_len); - ret = packet_len; + _read_buffer->copy(buffer, i + Sp2HeaderSize, payload_len); + _read_buffer->remove(i, packet_len); + ret = payload_len; end: unlock(Read); @@ -524,6 +561,9 @@ ssize_t RtpsDev::write(struct file *filp, const char *buffer, size_t buflen) ret = -1; } else { + _header[4] = (uint8_t)((buflen >> 8) & 0xff); + _header[5] = (uint8_t)(buflen & 0xff); + ::write(_fd, _header, 8); ret = ::write(_fd, buffer, buflen); } diff --git a/src/lib/circuit_breaker/circuit_breaker_params.c b/src/lib/circuit_breaker/circuit_breaker_params.c index f6d808104c..de34a6a01f 100644 --- a/src/lib/circuit_breaker/circuit_breaker_params.c +++ b/src/lib/circuit_breaker/circuit_breaker_params.c @@ -84,7 +84,7 @@ PARAM_DEFINE_INT32(CBRK_RATE_CTRL, 0); * @category Developer * @group Circuit Breaker */ -PARAM_DEFINE_INT32(CBRK_IO_SAFETY, 0); +PARAM_DEFINE_INT32(CBRK_IO_SAFETY, 22027); /** * Circuit breaker for airspeed sensor diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 6c65d9113c..69cf6e66f1 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -382,57 +382,113 @@ bool Commander::shutdown_if_allowed() hrt_elapsed_time(&_boot_timestamp), arm_disarm_reason_t::SHUTDOWN); } -transition_result_t -Commander::arm_disarm(bool arm, bool run_preflight_checks, arm_disarm_reason_t calling_reason) +static constexpr const char *arm_disarm_reason_str(arm_disarm_reason_t calling_reason) { - transition_result_t arming_res = TRANSITION_NOT_CHANGED; + switch (calling_reason) { + case arm_disarm_reason_t::TRANSITION_TO_STANDBY: return ""; - // Transition the armed state. By passing _mavlink_log_pub to arming_state_transition it will - // output appropriate error messages if the state cannot transition. - arming_res = arming_state_transition(&_status, - _safety, - arm ? vehicle_status_s::ARMING_STATE_ARMED : vehicle_status_s::ARMING_STATE_STANDBY, - &_armed, - run_preflight_checks, - &_mavlink_log_pub, - &_status_flags, - _arm_requirements, - hrt_elapsed_time(&_boot_timestamp), calling_reason); + case arm_disarm_reason_t::RC_STICK: return "RC"; - if (arming_res == TRANSITION_CHANGED) { - const char *reason = ""; + case arm_disarm_reason_t::RC_SWITCH: return "RC (switch)"; - switch (calling_reason) { - case arm_disarm_reason_t::TRANSITION_TO_STANDBY: reason = ""; break; + case arm_disarm_reason_t::COMMAND_INTERNAL: return "internal command"; - case arm_disarm_reason_t::RC_STICK: reason = "RC"; break; + case arm_disarm_reason_t::COMMAND_EXTERNAL: return "external command"; - case arm_disarm_reason_t::RC_SWITCH: reason = "RC (switch)"; break; + case arm_disarm_reason_t::MISSION_START: return "mission start"; - case arm_disarm_reason_t::COMMAND_INTERNAL: reason = "internal command"; break; + case arm_disarm_reason_t::SAFETY_BUTTON: return "safety button"; - case arm_disarm_reason_t::COMMAND_EXTERNAL: reason = "external command"; break; + case arm_disarm_reason_t::AUTO_DISARM_LAND: return "landing"; - case arm_disarm_reason_t::MISSION_START: reason = "mission start"; break; + case arm_disarm_reason_t::AUTO_DISARM_PREFLIGHT: return "auto preflight disarming"; - case arm_disarm_reason_t::SAFETY_BUTTON: reason = "safety button"; break; + case arm_disarm_reason_t::KILL_SWITCH: return "kill-switch"; - case arm_disarm_reason_t::AUTO_DISARM_LAND: reason = "landing"; break; + case arm_disarm_reason_t::LOCKDOWN: return "lockdown"; - case arm_disarm_reason_t::AUTO_DISARM_PREFLIGHT: reason = "auto preflight disarming"; break; + case arm_disarm_reason_t::FAILURE_DETECTOR: return "failure detector"; - case arm_disarm_reason_t::KILL_SWITCH: reason = "kill-switch"; break; + case arm_disarm_reason_t::SHUTDOWN: return "shutdown request"; - case arm_disarm_reason_t::LOCKDOWN: reason = "lockdown"; break; + case arm_disarm_reason_t::UNIT_TEST: return "unit tests"; + } - case arm_disarm_reason_t::FAILURE_DETECTOR: reason = "failure detector"; break; + return ""; +}; - case arm_disarm_reason_t::SHUTDOWN: reason = "shutdown request"; break; +transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_preflight_checks) +{ + // allow a grace period for re-arming: preflight checks don't need to pass during that time, for example for accidential in-air disarming + if (_param_com_rearm_grace.get() && (hrt_elapsed_time(&_last_disarmed_timestamp) < 5_s)) { + run_preflight_checks = false; + } - case arm_disarm_reason_t::UNIT_TEST: reason = "unit tests"; break; + if (run_preflight_checks) { + if (_vehicle_control_mode.flag_control_manual_enabled) { + const bool throttle_above_low = (_manual_control_setpoint.z > 0.1f); + const bool throttle_above_center = (_manual_control_setpoint.z > 0.6f); + + if (_vehicle_control_mode.flag_control_climb_rate_enabled && throttle_above_center) { + mavlink_log_critical(&_mavlink_log_pub, "Arming denied! Throttle not centered"); + tune_negative(true); + return TRANSITION_DENIED; + + } + + if (!_vehicle_control_mode.flag_control_climb_rate_enabled && throttle_above_low) { + mavlink_log_critical(&_mavlink_log_pub, "Arming denied! Throttle not zero"); + tune_negative(true); + return TRANSITION_DENIED; + } } - mavlink_log_info(&_mavlink_log_pub, "%s by %s", arm ? "Armed" : "Disarmed", reason); + if ((_param_geofence_action.get() == geofence_result_s::GF_ACTION_RTL) + && !_status_flags.condition_home_position_valid) { + mavlink_log_critical(&_mavlink_log_pub, "Arming denied! Geofence RTL requires valid home"); + tune_negative(true); + return TRANSITION_DENIED; + } + } + + transition_result_t arming_res = arming_state_transition(&_status, + _safety, + vehicle_status_s::ARMING_STATE_ARMED, + &_armed, + run_preflight_checks, + &_mavlink_log_pub, + &_status_flags, + _arm_requirements, + hrt_elapsed_time(&_boot_timestamp), calling_reason); + + if (arming_res == TRANSITION_CHANGED) { + mavlink_log_info(&_mavlink_log_pub, "Armed by %s", arm_disarm_reason_str(calling_reason)); + + _status_changed = true; + + } else if (arming_res == TRANSITION_DENIED) { + tune_negative(true); + } + + return arming_res; +} + +transition_result_t Commander::disarm(arm_disarm_reason_t calling_reason) +{ + transition_result_t arming_res = arming_state_transition(&_status, + _safety, + vehicle_status_s::ARMING_STATE_STANDBY, + &_armed, + false, + &_mavlink_log_pub, + &_status_flags, + _arm_requirements, + hrt_elapsed_time(&_boot_timestamp), calling_reason); + + if (arming_res == TRANSITION_CHANGED) { + mavlink_log_info(&_mavlink_log_pub, "Disarmed by %s", arm_disarm_reason_str(calling_reason)); + + _status_changed = true; } else if (arming_res == TRANSITION_DENIED) { tune_negative(true); @@ -651,17 +707,18 @@ Commander::handle_command(const vehicle_command_s &cmd) // Adhere to MAVLink specs, but base on knowledge that these fundamentally encode ints // for logic state parameters - if (static_cast(cmd.param1 + 0.5f) != 0 && static_cast(cmd.param1 + 0.5f) != 1) { + const int param1_arm = static_cast(roundf(cmd.param1)); + + if (param1_arm != 0 && param1_arm != 1) { mavlink_log_critical(&_mavlink_log_pub, "Unsupported ARM_DISARM param: %.3f", (double)cmd.param1); } else { + const bool cmd_arms = (param1_arm == 1); - bool cmd_arms = (static_cast(cmd.param1 + 0.5f) == 1); + // Arm is forced (checks skipped) when param2 is set to a magic number. + const bool forced = (static_cast(roundf(cmd.param2)) == 21196); - // Arm/disarm is enforced when param2 is set to a magic number. - const bool enforce = (static_cast(roundf(cmd.param2)) == 21196); - - if (!enforce) { + if (!forced) { if (!(_land_detector.landed || _land_detector.maybe_landed) && !is_ground_rover(&_status)) { if (cmd_arms) { if (_armed.armed) { @@ -685,41 +742,27 @@ Commander::handle_command(const vehicle_command_s &cmd) if (cmd.source_system == _status.system_id && cmd.source_component == _status.component_id && cmd_from_io && cmd_arms) { _status.arming_state = vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE; - - } else { - // Refuse to arm if preflight checks have failed - if (_status.hil_state != vehicle_status_s::HIL_STATE_ON - && !_status_flags.condition_system_sensors_initialized) { - mavlink_log_critical(&_mavlink_log_pub, "Arming denied! Preflight checks have failed"); - cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED; - break; - } - - const bool throttle_above_low = (_manual_control_setpoint.z > 0.1f); - const bool throttle_above_center = (_manual_control_setpoint.z > 0.6f); - - if (cmd_arms && throttle_above_center && - (_status.nav_state == vehicle_status_s::NAVIGATION_STATE_POSCTL || - _status.nav_state == vehicle_status_s::NAVIGATION_STATE_ALTCTL)) { - mavlink_log_critical(&_mavlink_log_pub, "Arming denied! Throttle not centered"); - cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED; - break; - } - - if (cmd_arms && throttle_above_low && - (_status.nav_state == vehicle_status_s::NAVIGATION_STATE_MANUAL || - _status.nav_state == vehicle_status_s::NAVIGATION_STATE_ACRO || - _status.nav_state == vehicle_status_s::NAVIGATION_STATE_STAB || - _status.nav_state == vehicle_status_s::NAVIGATION_STATE_RATTITUDE)) { - mavlink_log_critical(&_mavlink_log_pub, "Arming denied! Throttle not zero"); - cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED; - break; - } } } - transition_result_t arming_res = arm_disarm(cmd_arms, !enforce, - (cmd.from_external ? arm_disarm_reason_t::COMMAND_EXTERNAL : arm_disarm_reason_t::COMMAND_INTERNAL)); + transition_result_t arming_res = TRANSITION_DENIED; + + if (cmd_arms) { + if (cmd.from_external) { + arming_res = arm(arm_disarm_reason_t::COMMAND_EXTERNAL); + + } else { + arming_res = arm(arm_disarm_reason_t::COMMAND_INTERNAL, !forced); + } + + } else { + if (cmd.from_external) { + arming_res = disarm(arm_disarm_reason_t::COMMAND_EXTERNAL); + + } else { + arming_res = disarm(arm_disarm_reason_t::COMMAND_INTERNAL); + } + } if (arming_res == TRANSITION_DENIED) { cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; @@ -931,7 +974,7 @@ Commander::handle_command(const vehicle_command_s &cmd) // switch to AUTO_MISSION and ARM if ((TRANSITION_DENIED != main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_MISSION, _status_flags, &_internal_state)) - && (TRANSITION_DENIED != arm_disarm(true, true, arm_disarm_reason_t::MISSION_START))) { + && (TRANSITION_DENIED != arm(arm_disarm_reason_t::MISSION_START))) { cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; @@ -1528,10 +1571,8 @@ Commander::run() while (!should_exit()) { - transition_result_t arming_ret = TRANSITION_NOT_CHANGED; - /* update parameters */ - bool params_updated = _parameter_update_sub.updated(); + const bool params_updated = _parameter_update_sub.updated(); if (params_updated || param_init_forced) { // clear update @@ -1754,9 +1795,7 @@ Commander::run() } if (safety_disarm_allowed) { - if (TRANSITION_CHANGED == arm_disarm(false, true, arm_disarm_reason_t::SAFETY_BUTTON)) { - _status_changed = true; - } + disarm(arm_disarm_reason_t::SAFETY_BUTTON); } } @@ -1848,8 +1887,12 @@ Commander::run() } if (_auto_disarm_landed.get_state()) { - arm_disarm(false, true, - (_have_taken_off_since_arming ? arm_disarm_reason_t::AUTO_DISARM_LAND : arm_disarm_reason_t::AUTO_DISARM_PREFLIGHT)); + if (_have_taken_off_since_arming) { + disarm(arm_disarm_reason_t::AUTO_DISARM_LAND); + + } else { + disarm(arm_disarm_reason_t::AUTO_DISARM_PREFLIGHT); + } } } @@ -1866,12 +1909,11 @@ Commander::run() if (_auto_disarm_killed.get_state()) { if (_armed.manual_lockdown) { - arm_disarm(false, true, arm_disarm_reason_t::KILL_SWITCH); + disarm(arm_disarm_reason_t::KILL_SWITCH); } else { - arm_disarm(false, true, arm_disarm_reason_t::LOCKDOWN); + disarm(arm_disarm_reason_t::LOCKDOWN); } - } } else { @@ -1895,15 +1937,10 @@ Commander::run() /* If in INIT state, try to proceed to STANDBY state */ if (!_status_flags.condition_calibration_enabled && _status.arming_state == vehicle_status_s::ARMING_STATE_INIT) { - arming_ret = arming_state_transition(&_status, _safety, vehicle_status_s::ARMING_STATE_STANDBY, &_armed, - true /* fRunPreArmChecks */, &_mavlink_log_pub, &_status_flags, - _arm_requirements, hrt_elapsed_time(&_boot_timestamp), - arm_disarm_reason_t::TRANSITION_TO_STANDBY); - - if (arming_ret == TRANSITION_DENIED) { - /* do not complain if not allowed into standby */ - arming_ret = TRANSITION_NOT_CHANGED; - } + arming_state_transition(&_status, _safety, vehicle_status_s::ARMING_STATE_STANDBY, &_armed, + true /* fRunPreArmChecks */, &_mavlink_log_pub, &_status_flags, + _arm_requirements, hrt_elapsed_time(&_boot_timestamp), + arm_disarm_reason_t::TRANSITION_TO_STANDBY); } /* start mission result check */ @@ -2053,35 +2090,29 @@ Commander::run() _geofence_violated_prev = false; } - // abort auto mode or geofence reaction if sticks are moved significantly - // but only if not in a low battery handling action - const bool is_rotary_wing = _status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; + // abort autonomous mode and switch to position mode if sticks are moved significantly + if ((_param_rc_override.get() != 0) && (_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING)) { - const bool override_auto_mode = - (_param_rc_override.get() & OVERRIDE_AUTO_MODE_BIT) && - (_internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_TAKEOFF || - _internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LAND || - _internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_RTL || - _internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_MISSION || - _internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LOITER || - _internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET || - _internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_PRECLAND); + const bool override_auto_mode = (_param_rc_override.get() & OVERRIDE_AUTO_MODE_BIT) + && _vehicle_control_mode.flag_control_auto_enabled; - const bool override_offboard_mode = - (_param_rc_override.get() & OVERRIDE_OFFBOARD_MODE_BIT) && - _internal_state.main_state == commander_state_s::MAIN_STATE_OFFBOARD; + const bool override_offboard_mode = (_param_rc_override.get() & OVERRIDE_OFFBOARD_MODE_BIT) + && _vehicle_control_mode.flag_control_offboard_enabled; - if ((override_auto_mode || override_offboard_mode) && is_rotary_wing - && !in_low_battery_failsafe && !_geofence_warning_action_on) { - const float minimum_stick_deflection = 0.01f * _param_com_rc_stick_ov.get(); + if ((override_auto_mode || override_offboard_mode) && !in_low_battery_failsafe && !_geofence_warning_action_on) { + const float minimum_stick_deflection = 0.01f * _param_com_rc_stick_ov.get(); - // transition to previous state if sticks are touched - if (!_status.rc_signal_lost && - ((fabsf(_manual_control_setpoint.x) > minimum_stick_deflection) || - (fabsf(_manual_control_setpoint.y) > minimum_stick_deflection))) { - // revert to position control in any case - main_state_transition(_status, commander_state_s::MAIN_STATE_POSCTL, _status_flags, &_internal_state); - mavlink_log_info(&_mavlink_log_pub, "Pilot took over control using sticks"); + if (!_status.rc_signal_lost && + ((fabsf(_manual_control_setpoint.x) > minimum_stick_deflection) || + (fabsf(_manual_control_setpoint.y) > minimum_stick_deflection))) { + + if (main_state_transition(_status, commander_state_s::MAIN_STATE_POSCTL, _status_flags, + &_internal_state) == TRANSITION_CHANGED) { + tune_positive(true); + mavlink_log_info(&_mavlink_log_pub, "Pilot took over control using sticks"); + _status_changed = true; + } + } } } @@ -2149,18 +2180,19 @@ Commander::run() (_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING || _land_detector.landed) && (stick_in_lower_left || arm_button_pressed || arm_switch_to_disarm_transition)) { - const bool manual_thrust_mode = _internal_state.main_state == commander_state_s::MAIN_STATE_MANUAL - || _internal_state.main_state == commander_state_s::MAIN_STATE_ACRO - || _internal_state.main_state == commander_state_s::MAIN_STATE_STAB - || _internal_state.main_state == commander_state_s::MAIN_STATE_RATTITUDE; + const bool manual_thrust_mode = _vehicle_control_mode.flag_control_manual_enabled + && !_vehicle_control_mode.flag_control_climb_rate_enabled; + const bool rc_wants_disarm = (_stick_off_counter == rc_arm_hyst && _stick_on_counter < rc_arm_hyst) || arm_switch_to_disarm_transition; if (rc_wants_disarm && (_land_detector.landed || manual_thrust_mode)) { - arming_ret = arming_state_transition(&_status, _safety, vehicle_status_s::ARMING_STATE_STANDBY, &_armed, - true /* fRunPreArmChecks */, - &_mavlink_log_pub, &_status_flags, _arm_requirements, hrt_elapsed_time(&_boot_timestamp), - (arm_switch_to_disarm_transition ? arm_disarm_reason_t::RC_SWITCH : arm_disarm_reason_t::RC_STICK)); + if (arm_switch_to_disarm_transition) { + disarm(arm_disarm_reason_t::RC_SWITCH); + + } else { + disarm(arm_disarm_reason_t::RC_STICK); + } } _stick_off_counter++; @@ -2197,29 +2229,16 @@ Commander::run() * for being in manual mode only applies to manual arming actions. * the system can be armed in auto if armed via the GCS. */ - if ((_internal_state.main_state != commander_state_s::MAIN_STATE_MANUAL) - && (_internal_state.main_state != commander_state_s::MAIN_STATE_ACRO) - && (_internal_state.main_state != commander_state_s::MAIN_STATE_STAB) - && (_internal_state.main_state != commander_state_s::MAIN_STATE_ALTCTL) - && (_internal_state.main_state != commander_state_s::MAIN_STATE_POSCTL) - && (_internal_state.main_state != commander_state_s::MAIN_STATE_RATTITUDE) - ) { - print_reject_arm("Not arming: Switch to a manual mode first"); + if (!_vehicle_control_mode.flag_control_manual_enabled) { + mavlink_log_critical(&_mavlink_log_pub, "Arming denied! Switch to a manual mode first"); + tune_negative(true); - } else if (!_status_flags.condition_home_position_valid && - (_param_geofence_action.get() == geofence_result_s::GF_ACTION_RTL)) { + } else { + if (arm_switch_to_arm_transition) { + arm(arm_disarm_reason_t::RC_SWITCH); - print_reject_arm("Not arming: Geofence RTL requires valid home"); - - } else if (_status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) { - arming_ret = arming_state_transition(&_status, _safety, vehicle_status_s::ARMING_STATE_ARMED, &_armed, - !in_rearming_grace_period /* fRunPreArmChecks */, - &_mavlink_log_pub, &_status_flags, _arm_requirements, hrt_elapsed_time(&_boot_timestamp), - (arm_switch_to_arm_transition ? arm_disarm_reason_t::RC_SWITCH : arm_disarm_reason_t::RC_STICK)); - - if (arming_ret != TRANSITION_CHANGED) { - px4_usleep(100000); - print_reject_arm("Not arming: Preflight checks failed"); + } else { + arm(arm_disarm_reason_t::RC_STICK); } } } @@ -2234,54 +2253,34 @@ Commander::run() _last_manual_control_switches_arm_switch = _manual_control_switches.arm_switch; - if (arming_ret == TRANSITION_DENIED) { - /* - * the arming transition can be denied to a number of reasons: - * - pre-flight check failed (sensors not ok or not calibrated) - * - safety not disabled - * - system not in manual mode - */ - tune_negative(true); - } - if (_manual_control_switches_sub.update(&_manual_control_switches) || safety_updated) { // handle landing gear switch if configured and in a manual mode - if ((_manual_control_switches.gear_switch != manual_control_switches_s::SWITCH_POS_NONE) && + if ((_vehicle_control_mode.flag_control_manual_enabled) && + (_manual_control_switches.gear_switch != manual_control_switches_s::SWITCH_POS_NONE) && (_last_manual_control_switches.gear_switch != manual_control_switches_s::SWITCH_POS_NONE) && (_manual_control_switches.gear_switch != _last_manual_control_switches.gear_switch)) { - // TODO: replace with vehicle_control_mode manual - if (_status.nav_state == vehicle_status_s::NAVIGATION_STATE_MANUAL || - _status.nav_state == vehicle_status_s::NAVIGATION_STATE_ACRO || - _status.nav_state == vehicle_status_s::NAVIGATION_STATE_RATTITUDE || - _status.nav_state == vehicle_status_s::NAVIGATION_STATE_STAB || - _status.nav_state == vehicle_status_s::NAVIGATION_STATE_ALTCTL || - _status.nav_state == vehicle_status_s::NAVIGATION_STATE_POSCTL || - _status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD || - _status.nav_state == vehicle_status_s::NAVIGATION_STATE_ORBIT) { + // Only switch the landing gear up if the user switched from gear down to gear up. + int8_t gear = landing_gear_s::GEAR_KEEP; - // Only switch the landing gear up if the user switched from gear down to gear up. - int8_t gear = landing_gear_s::GEAR_KEEP; + if (_manual_control_switches.gear_switch == manual_control_switches_s::SWITCH_POS_OFF) { + gear = landing_gear_s::GEAR_DOWN; - if (_manual_control_switches.gear_switch == manual_control_switches_s::SWITCH_POS_OFF) { - gear = landing_gear_s::GEAR_DOWN; + } else if (_manual_control_switches.gear_switch == manual_control_switches_s::SWITCH_POS_ON) { + // gear up ignored unless flying + if (!_land_detector.landed && !_land_detector.maybe_landed) { + gear = landing_gear_s::GEAR_UP; - } else if (_manual_control_switches.gear_switch == manual_control_switches_s::SWITCH_POS_ON) { - // gear up ignored unless flying - if (!_land_detector.landed && !_land_detector.maybe_landed) { - gear = landing_gear_s::GEAR_UP; - - } else { - mavlink_log_critical(&_mavlink_log_pub, "Landed, unable to retract landing gear") - } + } else { + mavlink_log_critical(&_mavlink_log_pub, "Landed, unable to retract landing gear") } + } - if (gear != landing_gear_s::GEAR_KEEP) { - landing_gear_s landing_gear{}; - landing_gear.landing_gear = gear; - landing_gear.timestamp = hrt_absolute_time(); - _landing_gear_pub.publish(landing_gear); - } + if (gear != landing_gear_s::GEAR_KEEP) { + landing_gear_s landing_gear{}; + landing_gear.landing_gear = gear; + landing_gear.timestamp = hrt_absolute_time(); + _landing_gear_pub.publish(landing_gear); } } @@ -2440,7 +2439,7 @@ Commander::run() if (_status.failure_detector_status & vehicle_status_s::FAILURE_ARM_ESC) { // 500ms is the PWM spoolup time. Within this timeframe controllers are not affecting actuator_outputs if (hrt_elapsed_time(&_status.armed_time) < 500_ms) { - arm_disarm(false, true, arm_disarm_reason_t::FAILURE_DETECTOR); + disarm(arm_disarm_reason_t::FAILURE_DETECTOR); mavlink_log_critical(&_mavlink_log_pub, "ESCs did not respond to arm request"); } } @@ -2530,8 +2529,6 @@ Commander::run() _have_taken_off_since_arming = false; } - _was_armed = _armed.armed; - /* now set navigation state according to failsafe and main state */ bool nav_state_changed = set_nav_state(&_status, &_armed, @@ -2611,9 +2608,6 @@ Commander::run() _internal_state.timestamp = hrt_absolute_time(); _commander_state_pub.publish(_internal_state); - /* publish vehicle_status_flags */ - _status_flags.timestamp = hrt_absolute_time(); - // Evaluate current prearm status if (!_armed.armed && !_status_flags.condition_calibration_enabled) { bool preflight_check_res = PreFlightCheck::preflightCheck(nullptr, _status, _status_flags, false, true, 30_s); @@ -2627,6 +2621,8 @@ Commander::run() && prearm_check_res), _status); } + /* publish vehicle_status_flags */ + _status_flags.timestamp = hrt_absolute_time(); _vehicle_status_flags_pub.publish(_status_flags); } @@ -2718,6 +2714,8 @@ Commander::run() _last_condition_local_position_valid = _status_flags.condition_local_position_valid; _last_condition_global_position_valid = _status_flags.condition_global_position_valid; + _was_armed = _armed.armed; + arm_auth_update(now, params_updated || param_init_forced); px4_indicate_external_reset_lockout(LockoutComponent::Commander, _armed.armed); @@ -3360,56 +3358,55 @@ Commander::check_posvel_validity(const bool data_valid, const float data_accurac void Commander::update_control_mode() { - vehicle_control_mode_s control_mode{}; - - control_mode.timestamp = hrt_absolute_time(); + _vehicle_control_mode = {}; /* set vehicle_control_mode according to set_navigation_state */ - control_mode.flag_armed = _armed.armed; - control_mode.flag_external_manual_override_ok = (_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING - && !_status.is_vtol); + _vehicle_control_mode.flag_armed = _armed.armed; + + _vehicle_control_mode.flag_external_manual_override_ok = + (_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING && !_status.is_vtol); switch (_status.nav_state) { case vehicle_status_s::NAVIGATION_STATE_MANUAL: - control_mode.flag_control_manual_enabled = true; - control_mode.flag_control_rates_enabled = stabilization_required(); - control_mode.flag_control_attitude_enabled = stabilization_required(); + _vehicle_control_mode.flag_control_manual_enabled = true; + _vehicle_control_mode.flag_control_rates_enabled = stabilization_required(); + _vehicle_control_mode.flag_control_attitude_enabled = stabilization_required(); break; case vehicle_status_s::NAVIGATION_STATE_STAB: - control_mode.flag_control_manual_enabled = true; - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = true; + _vehicle_control_mode.flag_control_manual_enabled = true; + _vehicle_control_mode.flag_control_rates_enabled = true; + _vehicle_control_mode.flag_control_attitude_enabled = true; break; case vehicle_status_s::NAVIGATION_STATE_RATTITUDE: - control_mode.flag_control_manual_enabled = true; - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = true; - control_mode.flag_control_rattitude_enabled = true; + _vehicle_control_mode.flag_control_manual_enabled = true; + _vehicle_control_mode.flag_control_rates_enabled = true; + _vehicle_control_mode.flag_control_attitude_enabled = true; + _vehicle_control_mode.flag_control_rattitude_enabled = true; break; case vehicle_status_s::NAVIGATION_STATE_ALTCTL: - control_mode.flag_control_manual_enabled = true; - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = true; - control_mode.flag_control_altitude_enabled = true; - control_mode.flag_control_climb_rate_enabled = true; + _vehicle_control_mode.flag_control_manual_enabled = true; + _vehicle_control_mode.flag_control_rates_enabled = true; + _vehicle_control_mode.flag_control_attitude_enabled = true; + _vehicle_control_mode.flag_control_altitude_enabled = true; + _vehicle_control_mode.flag_control_climb_rate_enabled = true; break; case vehicle_status_s::NAVIGATION_STATE_POSCTL: - control_mode.flag_control_manual_enabled = true; - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = true; - control_mode.flag_control_altitude_enabled = true; - control_mode.flag_control_climb_rate_enabled = true; - control_mode.flag_control_position_enabled = !_status.in_transition_mode; - control_mode.flag_control_velocity_enabled = !_status.in_transition_mode; + _vehicle_control_mode.flag_control_manual_enabled = true; + _vehicle_control_mode.flag_control_rates_enabled = true; + _vehicle_control_mode.flag_control_attitude_enabled = true; + _vehicle_control_mode.flag_control_altitude_enabled = true; + _vehicle_control_mode.flag_control_climb_rate_enabled = true; + _vehicle_control_mode.flag_control_position_enabled = !_status.in_transition_mode; + _vehicle_control_mode.flag_control_velocity_enabled = !_status.in_transition_mode; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: /* override is not ok for the RTL and recovery mode */ - control_mode.flag_external_manual_override_ok = false; + _vehicle_control_mode.flag_external_manual_override_ok = false; /* fallthrough */ case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET: @@ -3419,109 +3416,109 @@ Commander::update_control_mode() case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER: case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF: - control_mode.flag_control_auto_enabled = true; - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = true; - control_mode.flag_control_altitude_enabled = true; - control_mode.flag_control_climb_rate_enabled = true; - control_mode.flag_control_position_enabled = !_status.in_transition_mode; - control_mode.flag_control_velocity_enabled = !_status.in_transition_mode; + _vehicle_control_mode.flag_control_auto_enabled = true; + _vehicle_control_mode.flag_control_rates_enabled = true; + _vehicle_control_mode.flag_control_attitude_enabled = true; + _vehicle_control_mode.flag_control_altitude_enabled = true; + _vehicle_control_mode.flag_control_climb_rate_enabled = true; + _vehicle_control_mode.flag_control_position_enabled = !_status.in_transition_mode; + _vehicle_control_mode.flag_control_velocity_enabled = !_status.in_transition_mode; break; case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL: - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = true; - control_mode.flag_control_climb_rate_enabled = true; + _vehicle_control_mode.flag_control_rates_enabled = true; + _vehicle_control_mode.flag_control_attitude_enabled = true; + _vehicle_control_mode.flag_control_climb_rate_enabled = true; break; case vehicle_status_s::NAVIGATION_STATE_ACRO: - control_mode.flag_control_manual_enabled = true; - control_mode.flag_control_rates_enabled = true; + _vehicle_control_mode.flag_control_manual_enabled = true; + _vehicle_control_mode.flag_control_rates_enabled = true; break; case vehicle_status_s::NAVIGATION_STATE_DESCEND: - control_mode.flag_control_auto_enabled = false; - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = true; - control_mode.flag_control_climb_rate_enabled = true; + _vehicle_control_mode.flag_control_auto_enabled = false; + _vehicle_control_mode.flag_control_rates_enabled = true; + _vehicle_control_mode.flag_control_attitude_enabled = true; + _vehicle_control_mode.flag_control_climb_rate_enabled = true; break; case vehicle_status_s::NAVIGATION_STATE_TERMINATION: /* disable all controllers on termination */ - control_mode.flag_control_termination_enabled = true; + _vehicle_control_mode.flag_control_termination_enabled = true; break; case vehicle_status_s::NAVIGATION_STATE_OFFBOARD: { + _vehicle_control_mode.flag_control_offboard_enabled = true; - const offboard_control_mode_s &offboard_control_mode = _offboard_control_mode_sub.get(); + const offboard_control_mode_s &offboard = _offboard_control_mode_sub.get(); - control_mode.flag_control_offboard_enabled = true; + if (!offboard.ignore_acceleration_force) { + // OFFBOARD acceleration + _vehicle_control_mode.flag_control_acceleration_enabled = true; + _vehicle_control_mode.flag_control_attitude_enabled = true; + _vehicle_control_mode.flag_control_rates_enabled = true; - /* - * The control flags depend on what is ignored according to the offboard control mode topic - * Inner loop flags (e.g. attitude) also depend on outer loop ignore flags (e.g. position) - */ - control_mode.flag_control_rates_enabled = - !offboard_control_mode.ignore_bodyrate_x || - !offboard_control_mode.ignore_bodyrate_y || - !offboard_control_mode.ignore_bodyrate_z || - !offboard_control_mode.ignore_attitude || - !offboard_control_mode.ignore_position || - !offboard_control_mode.ignore_velocity || - !offboard_control_mode.ignore_acceleration_force; + } else if (!offboard.ignore_position) { + // OFFBOARD position + _vehicle_control_mode.flag_control_position_enabled = true; + _vehicle_control_mode.flag_control_velocity_enabled = true; + _vehicle_control_mode.flag_control_altitude_enabled = true; + _vehicle_control_mode.flag_control_climb_rate_enabled = true; + _vehicle_control_mode.flag_control_attitude_enabled = true; + _vehicle_control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = !offboard_control_mode.ignore_attitude || - !offboard_control_mode.ignore_position || - !offboard_control_mode.ignore_velocity || - !offboard_control_mode.ignore_acceleration_force; + } else if (!offboard.ignore_velocity) { + // OFFBOARD velocity + _vehicle_control_mode.flag_control_velocity_enabled = true; + _vehicle_control_mode.flag_control_altitude_enabled = true; + _vehicle_control_mode.flag_control_climb_rate_enabled = true; + _vehicle_control_mode.flag_control_attitude_enabled = true; + _vehicle_control_mode.flag_control_rates_enabled = true; + + } else if (!offboard.ignore_attitude) { + // OFFBOARD attitude + _vehicle_control_mode.flag_control_attitude_enabled = true; + _vehicle_control_mode.flag_control_rates_enabled = true; + + } else if (!offboard.ignore_bodyrate_x || !offboard.ignore_bodyrate_y || !offboard.ignore_bodyrate_z) { + // OFFBOARD rate + _vehicle_control_mode.flag_control_rates_enabled = true; + } // TO-DO: Add support for other modes than yawrate control - control_mode.flag_control_yawrate_override_enabled = - offboard_control_mode.ignore_bodyrate_x && - offboard_control_mode.ignore_bodyrate_y && - !offboard_control_mode.ignore_bodyrate_z && - !offboard_control_mode.ignore_attitude; - - control_mode.flag_control_rattitude_enabled = false; - - control_mode.flag_control_acceleration_enabled = !offboard_control_mode.ignore_acceleration_force && - !_status.in_transition_mode; - - control_mode.flag_control_velocity_enabled = (!offboard_control_mode.ignore_velocity || - !offboard_control_mode.ignore_position) && !_status.in_transition_mode && - !control_mode.flag_control_acceleration_enabled; - - control_mode.flag_control_climb_rate_enabled = (!offboard_control_mode.ignore_velocity || - !offboard_control_mode.ignore_position); - - control_mode.flag_control_position_enabled = !offboard_control_mode.ignore_position && !_status.in_transition_mode && - !control_mode.flag_control_acceleration_enabled; - - control_mode.flag_control_altitude_enabled = (!offboard_control_mode.ignore_velocity || - !offboard_control_mode.ignore_position) && !control_mode.flag_control_acceleration_enabled; + _vehicle_control_mode.flag_control_yawrate_override_enabled = offboard.ignore_bodyrate_x && offboard.ignore_bodyrate_y + && !offboard.ignore_bodyrate_z && !offboard.ignore_attitude; + // VTOL transition override + if (_status.in_transition_mode) { + _vehicle_control_mode.flag_control_acceleration_enabled = false; + _vehicle_control_mode.flag_control_velocity_enabled = false; + _vehicle_control_mode.flag_control_position_enabled = false; + } } break; case vehicle_status_s::NAVIGATION_STATE_ORBIT: - control_mode.flag_control_manual_enabled = false; - control_mode.flag_control_auto_enabled = false; - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = true; - control_mode.flag_control_rattitude_enabled = false; - control_mode.flag_control_altitude_enabled = true; - control_mode.flag_control_climb_rate_enabled = true; - control_mode.flag_control_position_enabled = !_status.in_transition_mode; - control_mode.flag_control_velocity_enabled = !_status.in_transition_mode; - control_mode.flag_control_acceleration_enabled = false; - control_mode.flag_control_termination_enabled = false; + _vehicle_control_mode.flag_control_manual_enabled = false; + _vehicle_control_mode.flag_control_auto_enabled = false; + _vehicle_control_mode.flag_control_rates_enabled = true; + _vehicle_control_mode.flag_control_attitude_enabled = true; + _vehicle_control_mode.flag_control_rattitude_enabled = false; + _vehicle_control_mode.flag_control_altitude_enabled = true; + _vehicle_control_mode.flag_control_climb_rate_enabled = true; + _vehicle_control_mode.flag_control_position_enabled = !_status.in_transition_mode; + _vehicle_control_mode.flag_control_velocity_enabled = !_status.in_transition_mode; + _vehicle_control_mode.flag_control_acceleration_enabled = false; + _vehicle_control_mode.flag_control_termination_enabled = false; break; default: break; } - _control_mode_pub.publish(control_mode); + _vehicle_control_mode.timestamp = hrt_absolute_time(); + _control_mode_pub.publish(_vehicle_control_mode); } bool @@ -3549,18 +3546,6 @@ Commander::print_reject_mode(const char *msg) } } -void -Commander::print_reject_arm(const char *msg) -{ - const hrt_abstime t = hrt_absolute_time(); - - if (t - _last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) { - _last_print_mode_reject_time = t; - mavlink_log_critical(&_mavlink_log_pub, "%s", msg); - tune_negative(true); - } -} - void Commander::answer_command(const vehicle_command_s &cmd, uint8_t result) { switch (result) { @@ -3815,13 +3800,9 @@ void Commander::avoidance_check() const bool sensor_oa_present = cp_healthy || _status_flags.avoidance_system_required || cp_enabled; - const bool auto_mode = _internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_MISSION - || _internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LOITER - || _internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_RTL - || _internal_state.main_state == commander_state_s::MAIN_STATE_OFFBOARD - || _internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_TAKEOFF - || _internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LAND; - const bool pos_ctl_mode = _internal_state.main_state == commander_state_s::MAIN_STATE_POSCTL; + const bool auto_mode = _vehicle_control_mode.flag_control_auto_enabled; + const bool pos_ctl_mode = (_vehicle_control_mode.flag_control_manual_enabled + && _vehicle_control_mode.flag_control_position_enabled); const bool sensor_oa_enabled = ((auto_mode && _status_flags.avoidance_system_required) || (pos_ctl_mode && cp_enabled)); const bool sensor_oa_healthy = ((auto_mode && _status_flags.avoidance_system_valid) || (pos_ctl_mode && cp_healthy)); diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 9c08bf5485..4574f1c741 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -124,7 +124,8 @@ public: private: void answer_command(const vehicle_command_s &cmd, uint8_t result); - transition_result_t arm_disarm(bool arm, bool run_preflight_checks, arm_disarm_reason_t calling_reason); + transition_result_t arm(arm_disarm_reason_t calling_reason, bool run_preflight_checks = true); + transition_result_t disarm(arm_disarm_reason_t calling_reason); void battery_status_check(); @@ -153,7 +154,6 @@ private: void offboard_control_update(); - void print_reject_arm(const char *msg); void print_reject_mode(const char *msg); void reset_posvel_validity(); @@ -391,16 +391,19 @@ private: main_state_t _main_state_pre_offboard{commander_state_s::MAIN_STATE_MANUAL}; - actuator_armed_s _armed{}; - commander_state_s _internal_state{}; cpuload_s _cpuload{}; geofence_result_s _geofence_result{}; vehicle_land_detected_s _land_detector{}; safety_s _safety{}; - vehicle_status_s _status{}; - vehicle_status_flags_s _status_flags{}; vtol_vehicle_status_s _vtol_status{}; + // commander publications + actuator_armed_s _armed{}; + commander_state_s _internal_state{}; + vehicle_control_mode_s _vehicle_control_mode{}; + vehicle_status_s _status{}; + vehicle_status_flags_s _status_flags{}; + WorkerThread _worker_thread; // Subscriptions diff --git a/src/modules/ekf2/EKF2Selector.cpp b/src/modules/ekf2/EKF2Selector.cpp index 2447019b4e..c79616bd0e 100644 --- a/src/modules/ekf2/EKF2Selector.cpp +++ b/src/modules/ekf2/EKF2Selector.cpp @@ -73,8 +73,8 @@ bool EKF2Selector::SelectInstance(uint8_t ekf_instance) // update sensor_selection immediately sensor_selection_s sensor_selection{}; - sensor_selection.accel_device_id = _instance[ekf_instance].status.accel_device_id; - sensor_selection.gyro_device_id = _instance[ekf_instance].status.gyro_device_id; + sensor_selection.accel_device_id = _instance[ekf_instance].accel_device_id; + sensor_selection.gyro_device_id = _instance[ekf_instance].gyro_device_id; sensor_selection.timestamp = hrt_absolute_time(); _sensor_selection_pub.publish(sensor_selection); @@ -84,8 +84,24 @@ bool EKF2Selector::SelectInstance(uint8_t ekf_instance) _instance[_selected_instance].estimator_status_sub.unregisterCallback(); if (!_instance[_selected_instance].healthy) { - PX4_WARN("primary EKF changed %d (%s) -> %d", _selected_instance, - _instance[_selected_instance].filter_fault ? "filter fault" : "unhealthy", ekf_instance); + const char *reason = nullptr; + + if (_instance[_selected_instance].filter_fault) { + reason = "filter fault"; + + } else if (_instance[_selected_instance].timeout) { + reason = "timeout"; + + } else if (_gyro_fault_detected) { + reason = "gyro fault"; + + } else if (_accel_fault_detected) { + reason = "accel fault"; + } + + if (reason) { + PX4_WARN("primary EKF changed %d (%s) -> %d", _selected_instance, reason, ekf_instance); + } } } @@ -220,13 +236,28 @@ bool EKF2Selector::UpdateErrorScores() bool updated = false; bool primary_updated = false; + // default estimator timeout + hrt_abstime status_timeout = 50_ms; + + if (hrt_elapsed_time(&_attitude_last.timestamp) > FILTER_UPDATE_PERIOD) { + // much lower timeout if current primary estimator attitude isn't publishing + status_timeout = 2 * FILTER_UPDATE_PERIOD; + } + // calculate individual error scores for (uint8_t i = 0; i < EKF2_MAX_INSTANCES; i++) { const bool prev_healthy = _instance[i].healthy; - const estimator_status_s &status = _instance[i].status; + estimator_status_s status; - if (_instance[i].estimator_status_sub.update(&_instance[i].status)) { + if (_instance[i].estimator_status_sub.update(&status)) { + + _instance[i].timestamp_sample_last = status.timestamp_sample; + + _instance[i].accel_device_id = status.accel_device_id; + _instance[i].gyro_device_id = status.gyro_device_id; + _instance[i].baro_device_id = status.baro_device_id; + _instance[i].mag_device_id = status.mag_device_id; if ((i + 1) > _available_instances) { _available_instances = i + 1; @@ -250,22 +281,24 @@ bool EKF2Selector::UpdateErrorScores() _instance[i].combined_test_ratio = combined_test_ratio; _instance[i].healthy = tilt_align && yaw_align && (status.filter_fault_flags == 0); _instance[i].filter_fault = (status.filter_fault_flags != 0); + _instance[i].timeout = false; if (!PX4_ISFINITE(_instance[i].relative_test_ratio)) { _instance[i].relative_test_ratio = 0; } - } else if (hrt_elapsed_time(&status.timestamp) > (FILTER_UPDATE_PERIOD * 2)) { + } else if (hrt_elapsed_time(&_instance[i].timestamp_sample_last) > status_timeout) { _instance[i].healthy = false; + _instance[i].timeout = true; } // if the gyro used by the EKF is faulty, declare the EKF unhealthy without delay - if (_gyro_fault_detected && (faulty_gyro_id != 0) && (status.gyro_device_id == faulty_gyro_id)) { + if (_gyro_fault_detected && (faulty_gyro_id != 0) && (_instance[i].gyro_device_id == faulty_gyro_id)) { _instance[i].healthy = false; } // if the accelerometer used by the EKF is faulty, declare the EKF unhealthy without delay - if (_accel_fault_detected && (faulty_accel_id != 0) && (status.accel_device_id == faulty_accel_id)) { + if (_accel_fault_detected && (faulty_accel_id != 0) && (_instance[i].accel_device_id == faulty_accel_id)) { _instance[i].healthy = false; } @@ -327,6 +360,8 @@ void EKF2Selector::PublishVehicleAttitude(bool reset) attitude.timestamp = hrt_absolute_time(); _vehicle_attitude_pub.publish(attitude); + + _instance[_selected_instance].timestamp_sample_last = attitude.timestamp_sample; } } @@ -391,7 +426,7 @@ void EKF2Selector::PublishVehicleLocalPosition(bool reset) _local_position_last = local_position; // publish estimator's local position for system (vehicle_local_position) unless it's stale - if (local_position.timestamp >= _instance[_selected_instance].status.timestamp_sample) { + if (local_position.timestamp_sample >= _instance[_selected_instance].timestamp_sample_last) { // republish with total reset count and current timestamp local_position.xy_reset_counter = _xy_reset_counter; local_position.z_reset_counter = _z_reset_counter; @@ -451,7 +486,7 @@ void EKF2Selector::PublishVehicleGlobalPosition(bool reset) _global_position_last = global_position; // publish estimator's global position for system (vehicle_global_position) unless it's stale - if (global_position.timestamp >= _instance[_selected_instance].status.timestamp_sample) { + if (global_position.timestamp_sample >= _instance[_selected_instance].timestamp_sample_last) { // republish with total reset count and current timestamp global_position.lat_lon_reset_counter = _lat_lon_reset_counter; global_position.alt_reset_counter = _alt_reset_counter; @@ -484,8 +519,8 @@ void EKF2Selector::Run() // if no valid instance then force select first instance with valid IMU if (_selected_instance == INVALID_INSTANCE) { for (uint8_t i = 0; i < EKF2_MAX_INSTANCES; i++) { - if ((_instance[i].status.accel_device_id != 0) - && (_instance[i].status.gyro_device_id != 0)) { + if ((_instance[i].accel_device_id != 0) + && (_instance[i].gyro_device_id != 0)) { if (SelectInstance(i)) { break; @@ -541,7 +576,7 @@ void EKF2Selector::Run() best_test_ratio = test_ratio; // also check next best available ekf using a different IMU - if (_instance[i].status.accel_device_id != _instance[_selected_instance].status.accel_device_id) { + if (_instance[i].accel_device_id != _instance[_selected_instance].accel_device_id) { best_ekf_different_imu = i; } } @@ -574,10 +609,10 @@ void EKF2Selector::Run() selector_status.instances_available = _available_instances; selector_status.instance_changed_count = _instance_changed_count; selector_status.last_instance_change = _last_instance_change; - selector_status.accel_device_id = _instance[_selected_instance].status.accel_device_id; - selector_status.baro_device_id = _instance[_selected_instance].status.baro_device_id; - selector_status.gyro_device_id = _instance[_selected_instance].status.gyro_device_id; - selector_status.mag_device_id = _instance[_selected_instance].status.mag_device_id; + selector_status.accel_device_id = _instance[_selected_instance].accel_device_id; + selector_status.baro_device_id = _instance[_selected_instance].baro_device_id; + selector_status.gyro_device_id = _instance[_selected_instance].gyro_device_id; + selector_status.mag_device_id = _instance[_selected_instance].mag_device_id; selector_status.gyro_fault_detected = _gyro_fault_detected; selector_status.accel_fault_detected = _accel_fault_detected; @@ -621,7 +656,7 @@ void EKF2Selector::Run() vehicle_odometry_s vehicle_odometry; if (_instance[_selected_instance].estimator_odometry_sub.update(&vehicle_odometry)) { - if (vehicle_odometry.timestamp >= _instance[_selected_instance].status.timestamp_sample) { + if (vehicle_odometry.timestamp_sample >= _instance[_selected_instance].timestamp_sample_last) { vehicle_odometry.timestamp = hrt_absolute_time(); _vehicle_odometry_pub.publish(vehicle_odometry); } @@ -641,8 +676,7 @@ void EKF2Selector::PrintStatus() const EstimatorInstance &inst = _instance[i]; PX4_INFO("%d: ACC: %d, GYRO: %d, MAG: %d, %s, test ratio: %.7f (%.5f) %s", - inst.instance, inst.status.accel_device_id, inst.status.gyro_device_id, - inst.status.mag_device_id, + inst.instance, inst.accel_device_id, inst.gyro_device_id, inst.mag_device_id, inst.healthy ? "healthy" : "unhealthy", (double)inst.combined_test_ratio, (double)inst.relative_test_ratio, (_selected_instance == i) ? "*" : ""); diff --git a/src/modules/ekf2/EKF2Selector.hpp b/src/modules/ekf2/EKF2Selector.hpp index a98908e7a8..38cce2859e 100644 --- a/src/modules/ekf2/EKF2Selector.hpp +++ b/src/modules/ekf2/EKF2Selector.hpp @@ -104,7 +104,12 @@ private: uORB::Subscription estimator_global_position_sub; uORB::Subscription estimator_odometry_sub; - estimator_status_s status{}; + uint64_t timestamp_sample_last{0}; + + uint32_t accel_device_id{0}; + uint32_t gyro_device_id{0}; + uint32_t baro_device_id{0}; + uint32_t mag_device_id{0}; hrt_abstime time_last_selected{0}; @@ -113,6 +118,7 @@ private: bool healthy{false}; bool filter_fault{false}; + bool timeout{false}; const uint8_t instance; }; diff --git a/src/modules/flight_mode_manager/tasks/Utility/Sticks.cpp b/src/modules/flight_mode_manager/tasks/Utility/Sticks.cpp index a271db5f18..a58baf926a 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/Sticks.cpp +++ b/src/modules/flight_mode_manager/tasks/Utility/Sticks.cpp @@ -53,7 +53,8 @@ bool Sticks::checkAndSetStickInputs() // Linear scale _positions(0) = manual_control_setpoint.x; // NED x, pitch [-1,1] _positions(1) = manual_control_setpoint.y; // NED y, roll [-1,1] - _positions(2) = -(manual_control_setpoint.z - 0.5f) * 2.f; // NED z, thrust resacaled from [0,1] to [-1,1] + _positions(2) = -(math::constrain(manual_control_setpoint.z, 0.0f, + 1.0f) - 0.5f) * 2.f; // NED z, thrust resacaled from [0,1] to [-1,1] _positions(3) = manual_control_setpoint.r; // yaw [-1,1] // Exponential scale diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index ce72df0583..74c4ddc3af 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -165,7 +165,7 @@ FixedwingAttitudeControl::vehicle_manual_poll() -radians(_param_fw_man_p_max.get()), radians(_param_fw_man_p_max.get())); _att_sp.yaw_body = 0.0f; - _att_sp.thrust_body[0] = _manual_control_setpoint.z; + _att_sp.thrust_body[0] = math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f); Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body)); q.copyTo(_att_sp.q_d); @@ -182,7 +182,7 @@ FixedwingAttitudeControl::vehicle_manual_poll() _rates_sp.roll = _manual_control_setpoint.y * radians(_param_fw_acro_x_max.get()); _rates_sp.pitch = -_manual_control_setpoint.x * radians(_param_fw_acro_y_max.get()); _rates_sp.yaw = _manual_control_setpoint.r * radians(_param_fw_acro_z_max.get()); - _rates_sp.thrust_body[0] = _manual_control_setpoint.z; + _rates_sp.thrust_body[0] = math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f); _rate_sp_pub.publish(_rates_sp); @@ -194,7 +194,7 @@ FixedwingAttitudeControl::vehicle_manual_poll() -_manual_control_setpoint.x * _param_fw_man_p_sc.get() + _param_trim_pitch.get(); _actuators.control[actuator_controls_s::INDEX_YAW] = _manual_control_setpoint.r * _param_fw_man_y_sc.get() + _param_trim_yaw.get(); - _actuators.control[actuator_controls_s::INDEX_THROTTLE] = _manual_control_setpoint.z; + _actuators.control[actuator_controls_s::INDEX_THROTTLE] = math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f); } } } diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 2eed783cbc..26216d7e27 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -233,14 +233,14 @@ FixedwingPositionControl::manual_control_setpoint_poll() _manual_control_setpoint_sub.update(&_manual_control_setpoint); _manual_control_setpoint_altitude = _manual_control_setpoint.x; - _manual_control_setpoint_airspeed = _manual_control_setpoint.z; + _manual_control_setpoint_airspeed = math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f); if (_param_fw_posctl_inv_st.get()) { /* Alternate stick allocation (similar concept as for multirotor systems: * demanding up/down with the throttle stick, and move faster/break with the pitch one. */ - _manual_control_setpoint_altitude = -(_manual_control_setpoint.z * 2.f - 1.f); - _manual_control_setpoint_airspeed = _manual_control_setpoint.x / 2.f + 0.5f; + _manual_control_setpoint_altitude = -(math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f) * 2.f - 1.f); + _manual_control_setpoint_airspeed = math::constrain(_manual_control_setpoint.x, 0.0f, 1.0f) / 2.f + 0.5f; } } diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index c1a7327047..684d3c4c64 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -127,7 +127,8 @@ MulticopterAttitudeControl::generate_attitude_setpoint(const Quatf &q, float dt, if (reset_yaw_sp) { _man_yaw_sp = yaw; - } else if (_manual_control_setpoint.z > 0.05f || _param_mc_airmode.get() == (int32_t)Mixer::Airmode::roll_pitch_yaw) { + } else if (math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f) > 0.05f + || _param_mc_airmode.get() == (int32_t)Mixer::Airmode::roll_pitch_yaw) { const float yaw_rate = math::radians(_param_mpc_man_y_max.get()); attitude_setpoint.yaw_sp_move_rate = _manual_control_setpoint.r * yaw_rate; @@ -210,7 +211,7 @@ MulticopterAttitudeControl::generate_attitude_setpoint(const Quatf &q, float dt, Quatf q_sp = Eulerf(attitude_setpoint.roll_body, attitude_setpoint.pitch_body, attitude_setpoint.yaw_body); q_sp.copyTo(attitude_setpoint.q_d); - attitude_setpoint.thrust_body[2] = -throttle_curve(_manual_control_setpoint.z); + attitude_setpoint.thrust_body[2] = -throttle_curve(math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f)); attitude_setpoint.timestamp = hrt_absolute_time(); _vehicle_attitude_setpoint_pub.publish(attitude_setpoint); diff --git a/src/modules/mc_rate_control/MulticopterRateControl.cpp b/src/modules/mc_rate_control/MulticopterRateControl.cpp index dc97d17dbb..44ea046597 100644 --- a/src/modules/mc_rate_control/MulticopterRateControl.cpp +++ b/src/modules/mc_rate_control/MulticopterRateControl.cpp @@ -192,7 +192,7 @@ MulticopterRateControl::Run() math::superexpo(_manual_control_setpoint.r, _param_mc_acro_expo_y.get(), _param_mc_acro_supexpoy.get())}; _rates_sp = man_rate_sp.emult(_acro_rate_max); - _thrust_sp = _manual_control_setpoint.z; + _thrust_sp = math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f); // publish rate setpoint vehicle_rates_setpoint_s v_rates_sp{}; diff --git a/src/modules/micrortps_bridge/micrortps_client/CMakeLists.txt b/src/modules/micrortps_bridge/micrortps_client/CMakeLists.txt index cbba329896..8890b08a7a 100644 --- a/src/modules/micrortps_bridge/micrortps_client/CMakeLists.txt +++ b/src/modules/micrortps_bridge/micrortps_client/CMakeLists.txt @@ -82,7 +82,7 @@ if (NOT "${config_rtps_send_topics}" STREQUAL "" OR NOT "${config_rtps_receive_t --agent-outdir ${micrortps_bridge_path}/micrortps_agent/src --client-outdir ${micrortps_bridge_path}/micrortps_client --idl-dir ${micrortps_bridge_path}/micrortps_agent/idl - >micrortps_bridge.log >/dev/null + >micrortps_bridge.log 2>&1 || cat micrortps_bridge.log DEPENDS ${send_topic_files} ${receive_topic_files} COMMENT "Generating RTPS topic bridge" ) diff --git a/src/modules/navigator/gpsfailure.cpp b/src/modules/navigator/gpsfailure.cpp index 6a38bcae87..a475ae5dfd 100644 --- a/src/modules/navigator/gpsfailure.cpp +++ b/src/modules/navigator/gpsfailure.cpp @@ -153,7 +153,7 @@ GpsFailure::advance_gpsf() switch (_gpsf_state) { case GPSF_STATE_NONE: _gpsf_state = GPSF_STATE_LOITER; - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Global position failure: fixed bank loiter"); + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Global position failure: loitering"); break; case GPSF_STATE_LOITER: diff --git a/src/modules/rc_update/rc_update.cpp b/src/modules/rc_update/rc_update.cpp index e2a167c659..ea0b665e5f 100644 --- a/src/modules/rc_update/rc_update.cpp +++ b/src/modules/rc_update/rc_update.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2016-2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2016-2021 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 @@ -587,7 +587,7 @@ void RCUpdate::UpdateManualSetpoint(const hrt_abstime ×tamp_sample) manual_control_setpoint.y = get_rc_value(rc_channels_s::FUNCTION_ROLL, -1.f, 1.f); manual_control_setpoint.x = get_rc_value(rc_channels_s::FUNCTION_PITCH, -1.f, 1.f); manual_control_setpoint.r = get_rc_value(rc_channels_s::FUNCTION_YAW, -1.f, 1.f); - manual_control_setpoint.z = get_rc_value(rc_channels_s::FUNCTION_THROTTLE, 0.f, 1.f); + manual_control_setpoint.z = get_rc_value(rc_channels_s::FUNCTION_THROTTLE, -1.f, 1.f); manual_control_setpoint.flaps = get_rc_value(rc_channels_s::FUNCTION_FLAPS, -1.f, 1.f); manual_control_setpoint.aux1 = get_rc_value(rc_channels_s::FUNCTION_AUX_1, -1.f, 1.f); manual_control_setpoint.aux2 = get_rc_value(rc_channels_s::FUNCTION_AUX_2, -1.f, 1.f); diff --git a/src/modules/rover_pos_control/RoverPositionControl.cpp b/src/modules/rover_pos_control/RoverPositionControl.cpp index 2b592da5fc..a5cdda49ff 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.cpp +++ b/src/modules/rover_pos_control/RoverPositionControl.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2017 PX4 Development Team. All rights reserved. + * Copyright (c) 2017, 2021 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 @@ -58,8 +58,9 @@ extern "C" __EXPORT int rover_pos_control_main(int argc, char *argv[]); RoverPositionControl::RoverPositionControl() : ModuleParams(nullptr), + WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers), /* performance counters */ - _loop_perf(perf_alloc(PC_ELAPSED, "rover position control")) // TODO : do we even need these perf counters + _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")) // TODO : do we even need these perf counters { } @@ -68,6 +69,17 @@ RoverPositionControl::~RoverPositionControl() perf_free(_loop_perf); } +bool +RoverPositionControl::init() +{ + if (!_vehicle_attitude_sub.registerCallback()) { + PX4_ERR("vehicle attitude callback registration failed!"); + return false; + } + + return true; +} + void RoverPositionControl::parameters_update(bool force) { // check for parameter updates @@ -96,55 +108,82 @@ void RoverPositionControl::parameters_update(bool force) void RoverPositionControl::vehicle_control_mode_poll() { - bool updated; - orb_check(_control_mode_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); + if (_control_mode_sub.updated()) { + _control_mode_sub.copy(&_control_mode); } } void RoverPositionControl::manual_control_setpoint_poll() { - bool manual_updated; - orb_check(_manual_control_setpoint_sub, &manual_updated); + if (_control_mode.flag_control_manual_enabled) { + if (_manual_control_setpoint_sub.copy(&_manual_control_setpoint)) { + float dt = math::constrain(hrt_elapsed_time(&_manual_setpoint_last_called) * 1e-6f, 0.0002f, 0.04f); - if (manual_updated) { - orb_copy(ORB_ID(manual_control_setpoint), _manual_control_setpoint_sub, &_manual_control_setpoint); + if (!_control_mode.flag_control_climb_rate_enabled && + !_control_mode.flag_control_offboard_enabled) { + + if (_control_mode.flag_control_attitude_enabled) { + // STABILIZED mode generate the attitude setpoint from manual user inputs + _att_sp.roll_body = 0.0; + _att_sp.pitch_body = 0.0; + + /* reset yaw setpoint to current position if needed */ + if (_reset_yaw_sp) { + const float vehicle_yaw = Eulerf(Quatf(_vehicle_att.q)).psi(); + _manual_yaw_sp = vehicle_yaw; + _reset_yaw_sp = false; + + } else { + const float yaw_rate = math::radians(_param_gnd_man_y_max.get()); + _att_sp.yaw_sp_move_rate = _manual_control_setpoint.y * yaw_rate; + _manual_yaw_sp = wrap_pi(_manual_yaw_sp + _att_sp.yaw_sp_move_rate * dt); + } + + _att_sp.yaw_body = _manual_yaw_sp; + _att_sp.thrust_body[0] = _manual_control_setpoint.z; + + Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body)); + q.copyTo(_att_sp.q_d); + + _att_sp.timestamp = hrt_absolute_time(); + + + _attitude_sp_pub.publish(_att_sp); + + } else { + _act_controls.control[actuator_controls_s::INDEX_ROLL] = 0.0f; // Nominally roll: _manual_control_setpoint.y; + _act_controls.control[actuator_controls_s::INDEX_PITCH] = 0.0f; // Nominally pitch: -_manual_control_setpoint.x; + // Set heading from the manual roll input channel + _act_controls.control[actuator_controls_s::INDEX_YAW] = + _manual_control_setpoint.y; // Nominally yaw: _manual_control_setpoint.r; + // Set throttle from the manual throttle channel + _act_controls.control[actuator_controls_s::INDEX_THROTTLE] = _manual_control_setpoint.z; + _reset_yaw_sp = true; + } + + } else { + _reset_yaw_sp = true; + } + + _manual_setpoint_last_called = hrt_absolute_time(); + } } } void RoverPositionControl::position_setpoint_triplet_poll() { - bool pos_sp_triplet_updated; - orb_check(_pos_sp_triplet_sub, &pos_sp_triplet_updated); - - if (pos_sp_triplet_updated) { - orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet); + if (_pos_sp_triplet_sub.updated()) { + _pos_sp_triplet_sub.copy(&_pos_sp_triplet); } } void RoverPositionControl::attitude_setpoint_poll() { - bool att_sp_updated; - orb_check(_att_sp_sub, &att_sp_updated); - - if (att_sp_updated) { - orb_copy(ORB_ID(vehicle_attitude_setpoint), _att_sp_sub, &_att_sp); - } -} - -void -RoverPositionControl::vehicle_attitude_poll() -{ - bool att_updated; - orb_check(_vehicle_attitude_sub, &att_updated); - - if (att_updated) { - orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &_vehicle_att); + if (_att_sp_sub.updated()) { + _att_sp_sub.copy(&_att_sp); } } @@ -347,72 +386,27 @@ RoverPositionControl::control_attitude(const vehicle_attitude_s &att, const vehi } void -RoverPositionControl::run() +RoverPositionControl::Run() { - _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); - _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - _local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); - _manual_control_setpoint_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - _pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); - _att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); - - _vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); - - /* rate limit control mode updates to 5Hz */ - orb_set_interval(_control_mode_sub, 200); - - /* rate limit position updates to 50 Hz */ - orb_set_interval(_global_pos_sub, 20); - orb_set_interval(_local_pos_sub, 20); - parameters_update(true); - /* wakeup source(s) */ - px4_pollfd_struct_t fds[5]; - - /* Setup of loop */ - fds[0].fd = _global_pos_sub; - fds[0].events = POLLIN; - fds[1].fd = _manual_control_setpoint_sub; - fds[1].events = POLLIN; - fds[2].fd = _sensor_combined_sub; - fds[2].events = POLLIN; - fds[3].fd = _vehicle_attitude_sub; // Poll attitude - fds[3].events = POLLIN; - fds[4].fd = _local_pos_sub; // Added local position as source of position - fds[4].events = POLLIN; - - while (!should_exit()) { - - /* wait for up to 500ms for data */ - int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 500); - - /* this is undesirable but not much we can do - might want to flag unhappy status */ - if (pret < 0) { - warn("poll error %d, %d", pret, errno); - continue; - } + if (_vehicle_attitude_sub.update(&_vehicle_att)) { /* check vehicle control mode for changes to publication state */ vehicle_control_mode_poll(); attitude_setpoint_poll(); - //manual_control_setpoint_poll(); + manual_control_setpoint_poll(); _vehicle_acceleration_sub.update(); /* update parameters from storage */ parameters_update(); - bool manual_mode = _control_mode.flag_control_manual_enabled; - /* only run controller if position changed */ - if (fds[0].revents & POLLIN || fds[4].revents & POLLIN) { - perf_begin(_loop_perf); + if (_local_pos_sub.update(&_local_pos)) { /* load local copies */ - orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos); - orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos); + _global_pos_sub.update(&_global_pos); position_setpoint_triplet_poll(); @@ -436,7 +430,7 @@ RoverPositionControl::run() matrix::Vector2d current_position(_global_pos.lat, _global_pos.lon); matrix::Vector3f current_velocity(_local_pos.vx, _local_pos.vy, _local_pos.vz); - if (!manual_mode && _control_mode.flag_control_position_enabled) { + if (!_control_mode.flag_control_manual_enabled && _control_mode.flag_control_position_enabled) { if (control_position(current_position, ground_speed, _pos_sp_triplet)) { @@ -466,109 +460,54 @@ RoverPositionControl::run() } - } else if (!manual_mode && _control_mode.flag_control_velocity_enabled) { + } else if (!_control_mode.flag_control_manual_enabled && _control_mode.flag_control_velocity_enabled) { control_velocity(current_velocity, _pos_sp_triplet); } - - - perf_end(_loop_perf); } - if (fds[3].revents & POLLIN) { - - vehicle_attitude_poll(); - - if (!manual_mode && _control_mode.flag_control_attitude_enabled - && !_control_mode.flag_control_position_enabled - && !_control_mode.flag_control_velocity_enabled) { - - control_attitude(_vehicle_att, _att_sp); - - } + // Respond to an attitude update and run the attitude controller if enabled + if (_control_mode.flag_control_attitude_enabled + && !_control_mode.flag_control_position_enabled + && !_control_mode.flag_control_velocity_enabled) { + control_attitude(_vehicle_att, _att_sp); } - if (fds[1].revents & POLLIN) { - - // This should be copied even if not in manual mode. Otherwise, the poll(...) call will keep - // returning immediately and this loop will eat up resources. - orb_copy(ORB_ID(manual_control_setpoint), _manual_control_setpoint_sub, &_manual_control_setpoint); - - if (manual_mode) { - /* manual/direct control */ - //PX4_INFO("Manual mode!"); - _act_controls.control[actuator_controls_s::INDEX_ROLL] = _manual_control_setpoint.y; - _act_controls.control[actuator_controls_s::INDEX_PITCH] = -_manual_control_setpoint.x; - _act_controls.control[actuator_controls_s::INDEX_YAW] = _manual_control_setpoint.r; //TODO: Readd yaw scale param - _act_controls.control[actuator_controls_s::INDEX_THROTTLE] = _manual_control_setpoint.z; - } - } - - if (fds[2].revents & POLLIN) { - - orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined); - - //orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &_vehicle_att); + /* Only publish if any of the proper modes are enabled */ + if (_control_mode.flag_control_velocity_enabled || + _control_mode.flag_control_attitude_enabled || + _control_mode.flag_control_position_enabled || + _control_mode.flag_control_manual_enabled) { + // timestamp and publish controls _act_controls.timestamp = hrt_absolute_time(); - - /* Only publish if any of the proper modes are enabled */ - if (_control_mode.flag_control_velocity_enabled || - _control_mode.flag_control_attitude_enabled || - _control_mode.flag_control_position_enabled || - manual_mode) { - /* publish the actuator controls */ - _actuator_controls_pub.publish(_act_controls); - } + _actuator_controls_pub.publish(_act_controls); } - } - - orb_unsubscribe(_control_mode_sub); - orb_unsubscribe(_global_pos_sub); - orb_unsubscribe(_local_pos_sub); - orb_unsubscribe(_manual_control_setpoint_sub); - orb_unsubscribe(_pos_sp_triplet_sub); - orb_unsubscribe(_vehicle_attitude_sub); - orb_unsubscribe(_sensor_combined_sub); - - warnx("exiting.\n"); } int RoverPositionControl::task_spawn(int argc, char *argv[]) { - /* start the task */ - _task_id = px4_task_spawn_cmd("rover_pos_ctrl", - SCHED_DEFAULT, - SCHED_PRIORITY_POSITION_CONTROL, - 1700, - (px4_main_t)&RoverPositionControl::run_trampoline, - nullptr); - - if (_task_id < 0) { - warn("task start failed"); - return -errno; - } - - return OK; -} - -RoverPositionControl *RoverPositionControl::instantiate(int argc, char *argv[]) -{ - - if (argc > 0) { - PX4_WARN("Command 'start' takes no arguments."); - return nullptr; - } - RoverPositionControl *instance = new RoverPositionControl(); - if (instance == nullptr) { - PX4_ERR("Failed to instantiate RoverPositionControl object"); + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; + + if (instance->init()) { + return PX4_OK; + } + + } else { + PX4_ERR("alloc failed"); } - return instance; + delete instance; + _object.store(nullptr); + _task_id = -1; + + return PX4_ERROR; } int RoverPositionControl::custom_command(int argc, char *argv[]) diff --git a/src/modules/rover_pos_control/RoverPositionControl.hpp b/src/modules/rover_pos_control/RoverPositionControl.hpp index f503d367f2..e80c53f99d 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.hpp +++ b/src/modules/rover_pos_control/RoverPositionControl.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2017 PX4 Development Team. All rights reserved. + * Copyright (c) 2017, 2021 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 @@ -55,14 +55,15 @@ #include #include #include +#include #include +#include #include #include #include #include #include #include -#include #include #include #include @@ -75,7 +76,7 @@ using matrix::Dcmf; using namespace time_literals; -class RoverPositionControl : public ModuleBase, public ModuleParams +class RoverPositionControl final : public ModuleBase, public ModuleParams, public px4::WorkItem { public: RoverPositionControl(); @@ -87,31 +88,30 @@ public: static int task_spawn(int argc, char *argv[]); /** @see ModuleBase */ - static RoverPositionControl *instantiate(int argc, char *argv[]); - static int custom_command(int argc, char *argv[]); /** @see ModuleBase */ static int print_usage(const char *reason = nullptr); - /** @see ModuleBase::run() */ - void run() override; + bool init(); private: + void Run() override; + uORB::SubscriptionCallbackWorkItem _vehicle_attitude_sub{this, ORB_ID(vehicle_attitude)}; + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + + uORB::Publication _attitude_sp_pub{ORB_ID(vehicle_attitude_setpoint)}; uORB::Publication _pos_ctrl_status_pub{ORB_ID(position_controller_status)}; /**< navigation capabilities publication */ uORB::Publication _actuator_controls_pub{ORB_ID(actuator_controls_0)}; /**< actuator controls publication */ - int _control_mode_sub{-1}; /**< control mode subscription */ - int _global_pos_sub{-1}; - int _local_pos_sub{-1}; - int _manual_control_setpoint_sub{-1}; /**< notification of manual control updates */ - int _pos_sp_triplet_sub{-1}; - int _att_sp_sub{-1}; - int _vehicle_attitude_sub{-1}; - int _sensor_combined_sub{-1}; - - uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + uORB::Subscription _control_mode_sub{ORB_ID(vehicle_control_mode)}; /**< control mode subscription */ + uORB::Subscription _global_pos_sub{ORB_ID(vehicle_global_position)}; + uORB::Subscription _local_pos_sub{ORB_ID(vehicle_local_position)}; + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; /**< notification of manual control updates */ + uORB::Subscription _pos_sp_triplet_sub{ORB_ID(position_setpoint_triplet)}; + uORB::Subscription _att_sp_sub{ORB_ID(vehicle_attitude_setpoint)}; manual_control_setpoint_s _manual_control_setpoint{}; /**< r/c channel data */ position_setpoint_triplet_s _pos_sp_triplet{}; /**< triplet of mission items */ @@ -121,13 +121,13 @@ private: vehicle_local_position_s _local_pos{}; /**< global vehicle position */ actuator_controls_s _act_controls{}; /**< direct control of actuators */ vehicle_attitude_s _vehicle_att{}; - sensor_combined_s _sensor_combined{}; uORB::SubscriptionData _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)}; perf_counter_t _loop_perf; /**< loop performance counter */ hrt_abstime _control_position_last_called{0}; /**) _param_l1_period, (ParamFloat) _param_l1_damping, @@ -173,6 +176,7 @@ private: (ParamFloat) _param_wheel_base, (ParamFloat) _param_max_turn_angle, + (ParamFloat) _param_gnd_man_y_max, (ParamFloat) _param_nav_loiter_rad /**< loiter radius for Rover */ ) @@ -181,11 +185,11 @@ private: */ void parameters_update(bool force = false); - void manual_control_setpoint_poll(); void position_setpoint_triplet_poll(); void attitude_setpoint_poll(); void vehicle_control_mode_poll(); void vehicle_attitude_poll(); + void manual_control_setpoint_poll(); /** * Control position. diff --git a/src/modules/rover_pos_control/rover_pos_control_params.c b/src/modules/rover_pos_control/rover_pos_control_params.c index cc3b639b12..b3120f50a7 100644 --- a/src/modules/rover_pos_control/rover_pos_control_params.c +++ b/src/modules/rover_pos_control/rover_pos_control_params.c @@ -49,36 +49,50 @@ */ /** - * L1 distance - * - * This is the waypoint radius + * Distance from front axle to rear axle * + * A value of 0.31 is typical for 1/10 RC cars. * * @unit m * @min 0.0 - * @max 100.0 + * @decimal 3 + * @increment 0.01 + * @group Rover Position Control + */ +PARAM_DEFINE_FLOAT(GND_WHEEL_BASE, 0.31f); + +/** + * L1 distance + * + * This is the distance at which the next waypoint is activated. This should be set + * to about 2-4x of GND_WHEEL_BASE and not smaller than one meter (due to GPS accuracy). + * + * + * @unit m + * @min 1.0 + * @max 50.0 * @decimal 1 * @increment 0.1 * @group Rover Position Control */ -PARAM_DEFINE_FLOAT(GND_L1_DIST, 5.0f); +PARAM_DEFINE_FLOAT(GND_L1_DIST, 1.0f); /** * L1 period * * This is the L1 distance and defines the tracking * point ahead of the rover it's following. - * Using values around 2-5 for a traxxas stampede. Shorten + * Use values around 2-5m for a 0.3m wheel base. Tuning instructions: Shorten * slowly during tuning until response is sharp without oscillation. * * @unit m - * @min 0.0 + * @min 0.5 * @max 50.0 * @decimal 1 * @increment 0.5 * @group Rover Position Control */ -PARAM_DEFINE_FLOAT(GND_L1_PERIOD, 10.0f); +PARAM_DEFINE_FLOAT(GND_L1_PERIOD, 5.0f); /** * L1 damping @@ -138,21 +152,6 @@ PARAM_DEFINE_FLOAT(GND_THR_MAX, 0.3f); */ PARAM_DEFINE_FLOAT(GND_THR_MIN, 0.0f); -/** - * Idle throttle - * - * This is the minimum throttle while on the ground, it should be 0 for a rover - * - * - * @unit norm - * @min 0.0 - * @max 0.4 - * @decimal 2 - * @increment 0.01 - * @group Rover Position Control - */ -PARAM_DEFINE_FLOAT(GND_THR_IDLE, 0.0f); - /** * Control mode for speed * @@ -261,18 +260,6 @@ PARAM_DEFINE_FLOAT(GND_SPEED_TRIM, 3.0f); */ PARAM_DEFINE_FLOAT(GND_SPEED_MAX, 10.0f); -/** - * Distance from front axle to rear axle - * - * - * @unit m - * @min 0.0 - * @decimal 3 - * @increment 0.01 - * @group Rover Position Control - */ -PARAM_DEFINE_FLOAT(GND_WHEEL_BASE, 2.0f); - /** * Maximum turn angle for Ackerman steering. * At a control output of 0, the steering wheels are at 0 radians. @@ -286,3 +273,14 @@ PARAM_DEFINE_FLOAT(GND_WHEEL_BASE, 2.0f); * @group Rover Position Control */ PARAM_DEFINE_FLOAT(GND_MAX_ANG, 0.7854f); + +/** + * Max manual yaw rate + * + * @unit deg/s + * @min 0.0 + * @max 400 + * @decimal 1 + * @group Rover Position Control + */ +PARAM_DEFINE_FLOAT(GND_MAN_Y_MAX, 150.0f); diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 843cd6e5c4..60778f73ff 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -221,3 +221,15 @@ PARAM_DEFINE_INT32(SENS_EXT_I2C_PRB, 1); * @group Sensors */ PARAM_DEFINE_INT32(SENS_IMU_MODE, 1); + +/** + * Enable internal barometers + * + * For systems with an external barometer, this should be set to false to make sure that the external is used. + * + * @boolean + * @reboot_required true + * @category system + * @group Sensors + */ +PARAM_DEFINE_INT32(SENS_INT_BARO_EN, 1); diff --git a/src/drivers/mkblctrl/CMakeLists.txt b/src/modules/uuv_pos_control/CMakeLists.txt similarity index 91% rename from src/drivers/mkblctrl/CMakeLists.txt rename to src/modules/uuv_pos_control/CMakeLists.txt index 27cf0f6f81..f97dd0ea42 100644 --- a/src/drivers/mkblctrl/CMakeLists.txt +++ b/src/modules/uuv_pos_control/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# Copyright (c) 2020 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -31,12 +31,10 @@ # ############################################################################ px4_add_module( - MODULE drivers__mkblctrl - MAIN mkblctrl + MODULE modules__uuv_pos_control + MAIN uuv_pos_control + STACK_MAIN 1200 COMPILE_FLAGS SRCS - mkblctrl.cpp - DEPENDS - mixer + uuv_pos_control.cpp ) - diff --git a/src/modules/uuv_pos_control/uuv_pos_control.cpp b/src/modules/uuv_pos_control/uuv_pos_control.cpp new file mode 100644 index 0000000000..afa18d8e9f --- /dev/null +++ b/src/modules/uuv_pos_control/uuv_pos_control.cpp @@ -0,0 +1,291 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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. + * + ****************************************************************************/ + +/** + * + * This module is a modification of the hippocampus control module and is designed for the + * BlueROV2. + * + * All the acknowledgments and credits for the fw wing app are reported in those files. + * + * @author Tim Hansen + * @author Daniel Duecker + */ + +#include "uuv_pos_control.hpp" + + + +/** + * UUV pos_controller app start / stop handling function + * + * @ingroup apps + */ +extern "C" __EXPORT int uuv_pos_control_main(int argc, char *argv[]); + + +UUVPOSControl::UUVPOSControl(): + ModuleParams(nullptr), + WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers), + /* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")) +{ +} + +UUVPOSControl::~UUVPOSControl() +{ + perf_free(_loop_perf); +} + +bool UUVPOSControl::init() +{ + if (!_vehicle_local_position_sub.registerCallback()) { + PX4_ERR("vehicle_pos callback registration failed!"); + return false; + } + + return true; +} + +void UUVPOSControl::parameters_update(bool force) +{ + // check for parameter updates + if (_parameter_update_sub.updated() || force) { + // clear update + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); + + // update parameters from storage + updateParams(); + } +} + +void UUVPOSControl::publish_attitude_setpoint(const float thrust_x, const float thrust_y, const float thrust_z, + const float roll_des, const float pitch_des, const float yaw_des) +{ + //watch if inputs are not to high + vehicle_attitude_setpoint_s vehicle_attitude_setpoint = {}; + vehicle_attitude_setpoint.timestamp = hrt_absolute_time(); + + vehicle_attitude_setpoint.roll_body = roll_des; + vehicle_attitude_setpoint.pitch_body = pitch_des; + vehicle_attitude_setpoint.yaw_body = yaw_des; + + vehicle_attitude_setpoint.thrust_body[0] = thrust_x; + vehicle_attitude_setpoint.thrust_body[1] = thrust_y; + vehicle_attitude_setpoint.thrust_body[2] = thrust_z; + + + _att_sp_pub.publish(vehicle_attitude_setpoint); +} + +void UUVPOSControl::pose_controller_6dof(const float x_pos_des, const float y_pos_des, const float z_pos_des, + const float roll_des, const float pitch_des, const float yaw_des, + vehicle_attitude_s &vehicle_attitude, vehicle_local_position_s &vlocal_pos) +{ + //get current rotation of vehicle + Quatf q_att(vehicle_attitude.q); + Vector3f pos_des = Vector3f(x_pos_des, y_pos_des, z_pos_des); + + Vector3f p_control_output = Vector3f(_param_pose_gain_x.get() * (pos_des(0) - vlocal_pos.x) - _param_pose_gain_d_x.get() + * vlocal_pos.vx, + _param_pose_gain_y.get() * (pos_des(1) - vlocal_pos.y) - _param_pose_gain_d_y.get() * vlocal_pos.vy, + _param_pose_gain_z.get() * (pos_des(2) - vlocal_pos.z) - _param_pose_gain_d_z.get() * vlocal_pos.vz); + + Vector3f rotated_input = q_att.conjugate_inversed(p_control_output);//rotate the coord.sys (from global to body) + + publish_attitude_setpoint(rotated_input(0), + rotated_input(1), + rotated_input(2), + roll_des, pitch_des, yaw_des); + +} + +void UUVPOSControl::stabilization_controller_6dof(const float x_pos_des, const float y_pos_des, const float z_pos_des, + const float roll_des, const float pitch_des, const float yaw_des, + vehicle_attitude_s &vehicle_attitude, vehicle_local_position_s &vlocal_pos) +{ + //get current rotation of vehicle + Quatf q_att(vehicle_attitude.q); + Vector3f pos_des = Vector3f(0, 0, z_pos_des); + + Vector3f p_control_output = Vector3f(0, + 0, + _param_pose_gain_z.get() * (pos_des(2) - vlocal_pos.z)); + //potential d controller missing + Vector3f rotated_input = q_att.conjugate_inversed(p_control_output);//rotate the coord.sys (from global to body) + + publish_attitude_setpoint(rotated_input(0) + x_pos_des, rotated_input(1) + y_pos_des, rotated_input(2), + roll_des, pitch_des, yaw_des); + +} + +void UUVPOSControl::Run() +{ + if (should_exit()) { + _vehicle_local_position_sub.unregisterCallback(); + exit_and_cleanup(); + return; + } + + perf_begin(_loop_perf); + + /* check vehicle control mode for changes to publication state */ + _vcontrol_mode_sub.update(&_vcontrol_mode); + + + /* update parameters from storage */ + parameters_update(); + + //vehicle_attitude_s attitude; + vehicle_local_position_s vlocal_pos; + + /* only run controller if local_pos changed */ + if (_vehicle_local_position_sub.update(&vlocal_pos)) { + + /* Run geometric attitude controllers if NOT manual mode*/ + if (!_vcontrol_mode.flag_control_manual_enabled + && _vcontrol_mode.flag_control_attitude_enabled + && _vcontrol_mode.flag_control_rates_enabled) { + + + _vehicle_attitude_sub.update(&_vehicle_attitude);//get current vehicle attitude + _pos_sp_triplet_sub.update(&_pos_setpoint); + + float roll_des, pitch_des, yaw_des, x_pos_des, y_pos_des, z_pos_des; + + + roll_des = 0; + pitch_des = 0; + yaw_des = _pos_setpoint.current.yaw; + + x_pos_des = _pos_setpoint.current.x; + y_pos_des = _pos_setpoint.current.y; + z_pos_des = _pos_setpoint.current.z; + + + //stabilization controller(keep pos and hold depth + angle) vs position controller(global + yaw) + if (_param_stabilization.get() == 0) { + pose_controller_6dof(x_pos_des, y_pos_des, z_pos_des, + roll_des, pitch_des, yaw_des, _vehicle_attitude, vlocal_pos); + + } else { + stabilization_controller_6dof(x_pos_des, y_pos_des, z_pos_des, + roll_des, pitch_des, yaw_des, _vehicle_attitude, vlocal_pos); + } + + + + } + } + + /* Manual Control mode (e.g. gamepad,...) - raw feedthrough no assistance */ + if (_manual_control_setpoint_sub.update(&_manual_control_setpoint)) { + // This should be copied even if not in manual mode. Otherwise, the poll(...) call will keep + // returning immediately and this loop will eat up resources. + if (_vcontrol_mode.flag_control_manual_enabled && !_vcontrol_mode.flag_control_rates_enabled) { + /* manual/direct control */ + } + + } + + /* Only publish if any of the proper modes are enabled */ + if (_vcontrol_mode.flag_control_manual_enabled || + _vcontrol_mode.flag_control_attitude_enabled) { + } + + perf_end(_loop_perf); +} + +int UUVPOSControl::task_spawn(int argc, char *argv[]) +{ + UUVPOSControl *instance = new UUVPOSControl(); + + 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 UUVPOSControl::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + + +int UUVPOSControl::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +Controls the attitude of an unmanned underwater vehicle (UUV). +Publishes `actuator_controls_0` messages at a constant 250Hz. +### Implementation +Currently, this implementation supports only a few modes: + * Full manual: Roll, pitch, yaw, and throttle controls are passed directly through to the actuators + * Auto mission: The uuv runs missions +### Examples +CLI usage example: +$ uuv_pos_control start +$ uuv_pos_control status +$ uuv_pos_control stop +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("uuv_pos_control", "controller"); + PRINT_MODULE_USAGE_COMMAND("start") + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +int uuv_pos_control_main(int argc, char *argv[]) +{ + return UUVPOSControl::main(argc, argv); +} diff --git a/src/modules/uuv_pos_control/uuv_pos_control.hpp b/src/modules/uuv_pos_control/uuv_pos_control.hpp new file mode 100644 index 0000000000..b3e89c8129 --- /dev/null +++ b/src/modules/uuv_pos_control/uuv_pos_control.hpp @@ -0,0 +1,152 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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. + * + ****************************************************************************/ + +/** + * + * This module is a modification of the hippocampus control module and is designed for the + * BlueROV2. + * + * All the acknowledgments and credits for the fw wing app are reported in those files. + * + * @author Tim Hansen + * @author Daniel Duecker + */ + +#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 + +using matrix::Eulerf; +using matrix::Quatf; +using matrix::Matrix3f; +using matrix::Vector3f; +using matrix::Dcmf; + +using uORB::SubscriptionData; + +using namespace time_literals; + +class UUVPOSControl: public ModuleBase, public ModuleParams, public px4::WorkItem +{ +public: + UUVPOSControl(); + ~UUVPOSControl(); + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + bool init(); + +private: + uORB::Publication _att_sp_pub{ORB_ID(vehicle_attitude_setpoint)}; + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; /**< current vehicle attitude */ + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; /**< notification of manual control updates */ + uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle status subscription */ + uORB::Subscription _pos_sp_triplet_sub{ORB_ID(position_setpoint_triplet)}; /**< position setpoint */ + + uORB::SubscriptionCallbackWorkItem _vehicle_local_position_sub{this, ORB_ID(vehicle_local_position)}; + + + //actuator_controls_s _actuators {}; /**< actuator control inputs */ + manual_control_setpoint_s _manual_control_setpoint {}; /**< r/c channel data */ + vehicle_attitude_s _vehicle_attitude {}; /**< vehicle attitude */ + position_setpoint_triplet_s _pos_setpoint {}; /**< vehicle position setpoint */ + vehicle_control_mode_s _vcontrol_mode {}; /**< vehicle control mode */ + + perf_counter_t _loop_perf; /**< loop performance counter */ + + DEFINE_PARAMETERS( + (ParamFloat) _param_pose_gain_x, + (ParamFloat) _param_pose_gain_y, + (ParamFloat) _param_pose_gain_z, + (ParamFloat) _param_pose_gain_d_x, + (ParamFloat) _param_pose_gain_d_y, + (ParamFloat) _param_pose_gain_d_z, + + (ParamInt) _param_input_mode, + (ParamInt) _param_stabilization, + (ParamInt) _param_skip_ctrl + ) + + void Run() override; + /** + * Update our local parameter cache. + */ + void parameters_update(bool force = false); + + /** + * Control Attitude + */ + void publish_attitude_setpoint(const float thrust_x, const float thrust_y, const float thrust_z, + const float roll_des, const float pitch_des, const float yaw_des); + void pose_controller_6dof(const float x_pos_des, const float y_pos_des, const float z_pos_des, + const float roll_des, const float pitch_des, const float yaw_des, + vehicle_attitude_s &vehicle_attitude, vehicle_local_position_s &vlocal_pos); + void stabilization_controller_6dof(const float x_pos_des, const float y_pos_des, const float z_pos_des, + const float roll_des, const float pitch_des, const float yaw_des, + vehicle_attitude_s &vehicle_attitude, vehicle_local_position_s &vlocal_pos); +}; diff --git a/src/drivers/mkblctrl/mkblctrl_params.c b/src/modules/uuv_pos_control/uuv_pos_control_params.c similarity index 51% rename from src/drivers/mkblctrl/mkblctrl_params.c rename to src/modules/uuv_pos_control/uuv_pos_control_params.c index 7586d00f27..d3532edd43 100644 --- a/src/drivers/mkblctrl/mkblctrl_params.c +++ b/src/modules/uuv_pos_control/uuv_pos_control_params.c @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. - * Author: Marco Bauer + * Copyright (c) 2013-2020 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 @@ -18,7 +17,7 @@ * 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 + * 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, @@ -33,21 +32,66 @@ ****************************************************************************/ /** - * @file mkblctrl_params.c + * @file uuv_pos_control_params.c * - * Parameters defined by the mkblctrl driver. + * Parameters defined by the position control task for unmanned underwater vehicles (UUVs) * - * @author Marco Bauer + * This is a modification of the fixed wing/ground rover params and it is designed for ground rovers. + * It has been developed starting from the fw module, simplified and improved with dedicated items. + * + * All the ackowledgments and credits for the fw wing/rover app are reported in those files. + * + * @author Tim Hansen + * @author Daniel Duecker */ -#include -#include +/* + * Controller parameters, accessible via MAVLink + * + */ +/** + * Gain of P controller X + * + * @group UUV Position Control + */ +PARAM_DEFINE_FLOAT(UUV_GAIN_X_P, 1.0f); +/** + * Gain of P controller Y + * + * @group UUV Position Control + */ +PARAM_DEFINE_FLOAT(UUV_GAIN_Y_P, 1.0f); +/** + * Gain of P controller Z + * + * @group UUV Position Control + */ +PARAM_DEFINE_FLOAT(UUV_GAIN_Z_P, 1.0f); /** - * Test mode (Identify) of MKBLCTRL Driver + * Gain of D controller X * - * @boolean - * @group MKBLCTRL Testmode + * @group UUV Position Control */ -PARAM_DEFINE_INT32(MKBLCTRL_TEST, 0); +PARAM_DEFINE_FLOAT(UUV_GAIN_X_D, 0.2f); +/** + * Gain of D controller Y + * + * @group UUV Position Control + */ +PARAM_DEFINE_FLOAT(UUV_GAIN_Y_D, 0.2f); +/** + * Gain of D controller Z + * + * @group UUV Position Control + */ +PARAM_DEFINE_FLOAT(UUV_GAIN_Z_D, 0.2f); +/** + * Stabilization mode(1) or Position Control(0) + * + * @value 0 Position Control + * @value 1 Stabilization Mode + * @group UUV Position Control + */ +PARAM_DEFINE_INT32(UUV_STAB_MODE, 1);