From fb2a1996218d8cc1052813b2f78d23e875d9cc27 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sat, 13 Feb 2021 21:33:24 -0500 Subject: [PATCH 01/26] delete unmaintaned mkblctrl driver --- ROMFS/px4fmu_common/init.d/rc.interface | 23 +- ROMFS/px4fmu_common/init.d/rcS | 4 - boards/airmind/mindpx-v2/default.cmake | 1 - boards/av/x-v1/default.cmake | 1 - boards/cuav/nora/default.cmake | 1 - boards/cuav/x7pro/default.cmake | 1 - boards/cubepilot/cubeorange/console.cmake | 1 - boards/cubepilot/cubeorange/default.cmake | 1 - boards/cubepilot/cubeyellow/console.cmake | 1 - boards/cubepilot/cubeyellow/default.cmake | 1 - boards/holybro/durandal-v1/default.cmake | 1 - boards/holybro/pix32v5/default.cmake | 1 - boards/modalai/fc-v1/default.cmake | 1 - boards/mro/ctrl-zero-f7-oem/default.cmake | 1 - boards/mro/ctrl-zero-f7/default.cmake | 1 - boards/mro/pixracerpro/default.cmake | 1 - boards/mro/x21-777/default.cmake | 1 - boards/mro/x21/default.cmake | 1 - boards/nxp/fmuk66-e/default.cmake | 1 - boards/nxp/fmuk66-e/socketcan.cmake | 1 - boards/nxp/fmuk66-v3/default.cmake | 1 - boards/nxp/fmuk66-v3/rtps.cmake | 1 - boards/nxp/fmuk66-v3/socketcan.cmake | 1 - boards/omnibus/f4sd/default.cmake | 1 - boards/px4/fmu-v2/default.cmake | 1 - boards/px4/fmu-v2/lpe.cmake | 1 - boards/px4/fmu-v2/test.cmake | 1 - boards/px4/fmu-v3/ctrlalloc.cmake | 1 - boards/px4/fmu-v3/default.cmake | 1 - boards/px4/fmu-v3/rtps.cmake | 1 - boards/px4/fmu-v3/stackcheck.cmake | 1 - boards/px4/fmu-v4/ctrlalloc.cmake | 1 - boards/px4/fmu-v4/default.cmake | 1 - boards/px4/fmu-v4/rtps.cmake | 1 - boards/px4/fmu-v4/stackcheck.cmake | 1 - boards/px4/fmu-v4/uavcanv1.cmake | 1 - boards/px4/fmu-v4pro/default.cmake | 1 - boards/px4/fmu-v4pro/rtps.cmake | 1 - boards/px4/fmu-v5/critmonitor.cmake | 1 - boards/px4/fmu-v5/ctrlalloc.cmake | 1 - boards/px4/fmu-v5/debug.cmake | 1 - boards/px4/fmu-v5/default.cmake | 1 - boards/px4/fmu-v5/irqmonitor.cmake | 1 - boards/px4/fmu-v5/rover.cmake | 1 - boards/px4/fmu-v5/rtps.cmake | 1 - boards/px4/fmu-v5/stackcheck.cmake | 1 - boards/px4/fmu-v5/uavcanv1.cmake | 1 - boards/px4/fmu-v5x/base_phy_DP83848C.cmake | 1 - boards/px4/fmu-v5x/default.cmake | 1 - boards/px4/fmu-v6u/default.cmake | 1 - boards/px4/fmu-v6u/stackcheck.cmake | 3 +- boards/px4/fmu-v6x/default.cmake | 1 - boards/px4/fmu-v6x/stackcheck.cmake | 1 - boards/spracing/h7extreme/default.cmake | 1 - src/drivers/mkblctrl/CMakeLists.txt | 42 - src/drivers/mkblctrl/mkblctrl.cpp | 1397 -------------------- src/drivers/mkblctrl/mkblctrl_params.c | 53 - 57 files changed, 2 insertions(+), 1571 deletions(-) delete mode 100644 src/drivers/mkblctrl/CMakeLists.txt delete mode 100644 src/drivers/mkblctrl/mkblctrl.cpp delete mode 100644 src/drivers/mkblctrl/mkblctrl_params.c 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/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..bf4a25a36b 100644 --- a/boards/cuav/x7pro/default.cmake +++ b/boards/cuav/x7pro/default.cmake @@ -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..77f016be3e 100644 --- a/boards/cubepilot/cubeorange/console.cmake +++ b/boards/cubepilot/cubeorange/console.cmake @@ -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..b4353de6ab 100644 --- a/boards/cubepilot/cubeorange/default.cmake +++ b/boards/cubepilot/cubeorange/default.cmake @@ -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..2dc0be9875 100644 --- a/boards/cubepilot/cubeyellow/console.cmake +++ b/boards/cubepilot/cubeyellow/console.cmake @@ -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..f5697ce4a9 100644 --- a/boards/cubepilot/cubeyellow/default.cmake +++ b/boards/cubepilot/cubeyellow/default.cmake @@ -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..af56dae993 100644 --- a/boards/holybro/durandal-v1/default.cmake +++ b/boards/holybro/durandal-v1/default.cmake @@ -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..1c85d25df5 100644 --- a/boards/holybro/pix32v5/default.cmake +++ b/boards/holybro/pix32v5/default.cmake @@ -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/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..5957115394 100644 --- a/boards/px4/fmu-v2/default.cmake +++ b/boards/px4/fmu-v2/default.cmake @@ -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..1757a4912c 100644 --- a/boards/px4/fmu-v2/lpe.cmake +++ b/boards/px4/fmu-v2/lpe.cmake @@ -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..aab53f5cd4 100644 --- a/boards/px4/fmu-v2/test.cmake +++ b/boards/px4/fmu-v2/test.cmake @@ -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..e17094c16d 100644 --- a/boards/px4/fmu-v3/ctrlalloc.cmake +++ b/boards/px4/fmu-v3/ctrlalloc.cmake @@ -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..2ade27c004 100644 --- a/boards/px4/fmu-v3/default.cmake +++ b/boards/px4/fmu-v3/default.cmake @@ -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..012a8a6fb4 100644 --- a/boards/px4/fmu-v3/rtps.cmake +++ b/boards/px4/fmu-v3/rtps.cmake @@ -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..0dcc05bee9 100644 --- a/boards/px4/fmu-v3/stackcheck.cmake +++ b/boards/px4/fmu-v3/stackcheck.cmake @@ -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/ctrlalloc.cmake b/boards/px4/fmu-v4/ctrlalloc.cmake index 9970fa75d6..df65099c1e 100644 --- a/boards/px4/fmu-v4/ctrlalloc.cmake +++ b/boards/px4/fmu-v4/ctrlalloc.cmake @@ -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..b65c7ee71d 100644 --- a/boards/px4/fmu-v4/default.cmake +++ b/boards/px4/fmu-v4/default.cmake @@ -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/rtps.cmake b/boards/px4/fmu-v4/rtps.cmake index a7f1793216..20a50e3bf5 100644 --- a/boards/px4/fmu-v4/rtps.cmake +++ b/boards/px4/fmu-v4/rtps.cmake @@ -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/stackcheck.cmake b/boards/px4/fmu-v4/stackcheck.cmake index 610e7231f6..cbdb8569cd 100644 --- a/boards/px4/fmu-v4/stackcheck.cmake +++ b/boards/px4/fmu-v4/stackcheck.cmake @@ -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..b689e945d7 100644 --- a/boards/px4/fmu-v4/uavcanv1.cmake +++ b/boards/px4/fmu-v4/uavcanv1.cmake @@ -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..0905161880 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 diff --git a/boards/px4/fmu-v4pro/rtps.cmake b/boards/px4/fmu-v4pro/rtps.cmake index 537f1fece4..4bc52191d7 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 diff --git a/boards/px4/fmu-v5/critmonitor.cmake b/boards/px4/fmu-v5/critmonitor.cmake index 49fe32447b..a490448a66 100644 --- a/boards/px4/fmu-v5/critmonitor.cmake +++ b/boards/px4/fmu-v5/critmonitor.cmake @@ -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..7f76304e67 100644 --- a/boards/px4/fmu-v5/ctrlalloc.cmake +++ b/boards/px4/fmu-v5/ctrlalloc.cmake @@ -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-v5/debug.cmake b/boards/px4/fmu-v5/debug.cmake index 9edd9ca693..f7f1a79584 100644 --- a/boards/px4/fmu-v5/debug.cmake +++ b/boards/px4/fmu-v5/debug.cmake @@ -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..4cb7672727 100644 --- a/boards/px4/fmu-v5/default.cmake +++ b/boards/px4/fmu-v5/default.cmake @@ -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-v5/irqmonitor.cmake b/boards/px4/fmu-v5/irqmonitor.cmake index 7c566f9b54..89c5d85f78 100644 --- a/boards/px4/fmu-v5/irqmonitor.cmake +++ b/boards/px4/fmu-v5/irqmonitor.cmake @@ -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/rover.cmake b/boards/px4/fmu-v5/rover.cmake index e4d1b62528..63909681e5 100644 --- a/boards/px4/fmu-v5/rover.cmake +++ b/boards/px4/fmu-v5/rover.cmake @@ -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..e3df35d28e 100644 --- a/boards/px4/fmu-v5/rtps.cmake +++ b/boards/px4/fmu-v5/rtps.cmake @@ -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..8fa71424c2 100644 --- a/boards/px4/fmu-v5/stackcheck.cmake +++ b/boards/px4/fmu-v5/stackcheck.cmake @@ -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..56aaf8ed3e 100644 --- a/boards/px4/fmu-v5/uavcanv1.cmake +++ b/boards/px4/fmu-v5/uavcanv1.cmake @@ -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..ca8280efe0 100644 --- a/boards/px4/fmu-v5x/base_phy_DP83848C.cmake +++ b/boards/px4/fmu-v5x/base_phy_DP83848C.cmake @@ -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..0e57ae8611 100644 --- a/boards/px4/fmu-v5x/default.cmake +++ b/boards/px4/fmu-v5x/default.cmake @@ -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-v6u/default.cmake b/boards/px4/fmu-v6u/default.cmake index 4f48780cbb..c04dcd5d8a 100644 --- a/boards/px4/fmu-v6u/default.cmake +++ b/boards/px4/fmu-v6u/default.cmake @@ -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..c04dcd5d8a 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 @@ -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..0ba7936de3 100644 --- a/boards/px4/fmu-v6x/default.cmake +++ b/boards/px4/fmu-v6x/default.cmake @@ -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..0ba7936de3 100644 --- a/boards/px4/fmu-v6x/stackcheck.cmake +++ b/boards/px4/fmu-v6x/stackcheck.cmake @@ -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/spracing/h7extreme/default.cmake b/boards/spracing/h7extreme/default.cmake index dfa30b3047..26c97d62b4 100644 --- a/boards/spracing/h7extreme/default.cmake +++ b/boards/spracing/h7extreme/default.cmake @@ -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/mkblctrl/CMakeLists.txt b/src/drivers/mkblctrl/CMakeLists.txt deleted file mode 100644 index 27cf0f6f81..0000000000 --- a/src/drivers/mkblctrl/CMakeLists.txt +++ /dev/null @@ -1,42 +0,0 @@ -############################################################################ -# -# Copyright (c) 2015 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ -px4_add_module( - MODULE drivers__mkblctrl - MAIN mkblctrl - COMPILE_FLAGS - SRCS - mkblctrl.cpp - DEPENDS - mixer - ) - 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/mkblctrl/mkblctrl_params.c b/src/drivers/mkblctrl/mkblctrl_params.c deleted file mode 100644 index 7586d00f27..0000000000 --- a/src/drivers/mkblctrl/mkblctrl_params.c +++ /dev/null @@ -1,53 +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_params.c - * - * Parameters defined by the mkblctrl driver. - * - * @author Marco Bauer - */ - -#include -#include - -/** - * Test mode (Identify) of MKBLCTRL Driver - * - * @boolean - * @group MKBLCTRL Testmode - */ -PARAM_DEFINE_INT32(MKBLCTRL_TEST, 0); - From 9d20dea3b866192ac16638c7e753bf3a58e7a5b9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 12 Feb 2021 22:35:56 +0100 Subject: [PATCH 02/26] Rover: remove unused parameter --- .../init.d-posix/airframes/1060_rover | 1 - .../init.d-posix/airframes/1061_r1_rover | 1 - .../init.d-posix/airframes/1062_tf-r1 | 1 - .../init.d-posix/airframes/1070_boat | 1 - .../init.d/airframes/50000_generic_ground_vehicle | 1 - .../init.d/airframes/50003_aion_robotics_r1_rover | 1 - .../init.d/airframes/50004_nxpcup_car_dfrobot_gpx | 1 - .../rover_pos_control/rover_pos_control_params.c | 15 --------------- 8 files changed, 22 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1060_rover b/ROMFS/px4fmu_common/init.d-posix/airframes/1060_rover index dcb0500f09..d2400bec18 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1060_rover +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1060_rover @@ -16,7 +16,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..e6c09ecbd2 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/50000_generic_ground_vehicle +++ b/ROMFS/px4fmu_common/init.d/airframes/50000_generic_ground_vehicle @@ -34,7 +34,6 @@ 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 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/src/modules/rover_pos_control/rover_pos_control_params.c b/src/modules/rover_pos_control/rover_pos_control_params.c index cc3b639b12..e168732ef5 100644 --- a/src/modules/rover_pos_control/rover_pos_control_params.c +++ b/src/modules/rover_pos_control/rover_pos_control_params.c @@ -138,21 +138,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 * From 8a448718b97bbbcf0d6be820ade2fc660cab4395 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 13 Feb 2021 19:01:20 +0100 Subject: [PATCH 03/26] RC update: Do not constrain throttle If the user calibrated to negative throttle, enable them to use it. --- src/modules/rc_update/rc_update.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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); From 0d3676e5b0dfdb90d6b2f772a0bc3e9cb3c5daa1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 13 Feb 2021 20:32:33 +0100 Subject: [PATCH 04/26] Safety switch: Default to safety off 90% of all real-world vehicle configs default to this and it is something that users stumble over if they configure a new system. There are valid cases where this would not be desired - for these it can be still switched off. --- src/lib/circuit_breaker/circuit_breaker_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 From 6d489a4b4da35a638cdd50c85e27ed24b4401b35 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 13 Feb 2021 20:33:40 +0100 Subject: [PATCH 05/26] FW att control: Robustify for throttle scaling PX4 can support negative (reverse) throttle and the fixed wing controller is not expecting this input range. This hardens it against it. --- src/modules/fw_att_control/FixedwingAttitudeControl.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) 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); } } } From 548e070b911054780c1fdab3beb6a6310504b28b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 13 Feb 2021 20:34:02 +0100 Subject: [PATCH 06/26] FW pos control: Robustify for throttle scaling PX4 can support negative (reverse) throttle and the fixed wing controller is not expecting this input range. This hardens it against it. --- src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) 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; } } From 15219fbfe50d3786d236ff53bbb65b75a352f052 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 13 Feb 2021 20:34:44 +0100 Subject: [PATCH 07/26] MC att control: Robustify for throttle scaling PX4 can support negative (reverse) throttle and the multicopter controller is not expecting this input range. This hardens it against it. --- src/modules/mc_att_control/mc_att_control_main.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) 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); From 3135e4d31a91b7f09195decef238c86dc77dc0d4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 13 Feb 2021 20:37:27 +0100 Subject: [PATCH 08/26] SITL: Set correct L1 period --- ROMFS/px4fmu_common/init.d-posix/airframes/1060_rover | 1 + 1 file changed, 1 insertion(+) diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1060_rover b/ROMFS/px4fmu_common/init.d-posix/airframes/1060_rover index d2400bec18..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 From bb0b4db0284872467b887f1dfafe555b4395c53a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 13 Feb 2021 20:37:56 +0100 Subject: [PATCH 09/26] ROMFS: Better defaults for rover --- .../init.d/airframes/50000_generic_ground_vehicle | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) 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 e6c09ecbd2..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,7 +27,9 @@ 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 @@ -37,10 +39,6 @@ param set-default GND_SPEED_THR_SC 1 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 From ec2cf702760e812654c07701e0771e0d00401ce7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 13 Feb 2021 20:39:40 +0100 Subject: [PATCH 10/26] Rover position controller: Modernize implementation This commit moves the steering output from yaw to the roll channel to better reflect the lateral control input / reaction mapping. It also removes old unused parameters and modernizes the mainloop to remove unnecessary polling. --- .../mixers/rover_diff_and_servo.main.mix | 11 ++- .../mixers/rover_generic.main.mix | 10 ++- .../RoverPositionControl.cpp | 74 ++++++------------- .../RoverPositionControl.hpp | 4 - .../rover_pos_control_params.c | 42 ++++++----- 5 files changed, 59 insertions(+), 82 deletions(-) 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/src/modules/rover_pos_control/RoverPositionControl.cpp b/src/modules/rover_pos_control/RoverPositionControl.cpp index 2b592da5fc..20592f15b8 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.cpp +++ b/src/modules/rover_pos_control/RoverPositionControl.cpp @@ -104,17 +104,6 @@ RoverPositionControl::vehicle_control_mode_poll() } } -void -RoverPositionControl::manual_control_setpoint_poll() -{ - bool manual_updated; - orb_check(_manual_control_setpoint_sub, &manual_updated); - - if (manual_updated) { - orb_copy(ORB_ID(manual_control_setpoint), _manual_control_setpoint_sub, &_manual_control_setpoint); - } -} - void RoverPositionControl::position_setpoint_triplet_poll() { @@ -357,7 +346,6 @@ RoverPositionControl::run() _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); @@ -369,19 +357,15 @@ RoverPositionControl::run() parameters_update(true); /* wakeup source(s) */ - px4_pollfd_struct_t fds[5]; + px4_pollfd_struct_t fds[3]; /* Setup of loop */ fds[0].fd = _global_pos_sub; fds[0].events = POLLIN; - fds[1].fd = _manual_control_setpoint_sub; + fds[1].fd = _local_pos_sub; // Added local position as source of position fds[1].events = POLLIN; - fds[2].fd = _sensor_combined_sub; + fds[2].fd = _vehicle_attitude_sub; // Poll attitude 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()) { @@ -397,7 +381,6 @@ RoverPositionControl::run() /* check vehicle control mode for changes to publication state */ vehicle_control_mode_poll(); attitude_setpoint_poll(); - //manual_control_setpoint_poll(); _vehicle_acceleration_sub.update(); @@ -406,8 +389,8 @@ RoverPositionControl::run() bool manual_mode = _control_mode.flag_control_manual_enabled; - /* only run controller if position changed */ - if (fds[0].revents & POLLIN || fds[4].revents & POLLIN) { + // Respond to a position update + if (fds[0].revents & POLLIN || fds[1].revents & POLLIN) { perf_begin(_loop_perf); /* load local copies */ @@ -476,7 +459,8 @@ RoverPositionControl::run() perf_end(_loop_perf); } - if (fds[3].revents & POLLIN) { + // Respond to an attitude update and run the attitude controller if enabled + if (fds[2].revents & POLLIN) { vehicle_attitude_poll(); @@ -490,37 +474,28 @@ RoverPositionControl::run() } - 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. + // Manual pass-through if no other control mode is enabled + if (manual_mode) { + /* manual/direct control */ 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; - } + _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; } - 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); + // publish the controller output + if (_control_mode.flag_control_velocity_enabled || + _control_mode.flag_control_attitude_enabled || + _control_mode.flag_control_position_enabled || + manual_mode) { + // 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); } } @@ -531,7 +506,6 @@ RoverPositionControl::run() 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"); } diff --git a/src/modules/rover_pos_control/RoverPositionControl.hpp b/src/modules/rover_pos_control/RoverPositionControl.hpp index f503d367f2..241f272287 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.hpp +++ b/src/modules/rover_pos_control/RoverPositionControl.hpp @@ -62,7 +62,6 @@ #include #include #include -#include #include #include #include @@ -109,7 +108,6 @@ private: 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}; @@ -121,7 +119,6 @@ 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)}; @@ -181,7 +178,6 @@ 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(); 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 e168732ef5..bdc82044d8 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 @@ -246,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. From 66d86aae2eabed7723423d4fb950b8e9de4c302d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 13 Feb 2021 20:40:37 +0100 Subject: [PATCH 11/26] Flight mode manager: Protect for full throttle scaling PX4 supports -1 to 1 as input and this module was not protected against the input range. --- src/modules/flight_mode_manager/tasks/Utility/Sticks.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) 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 From cc11fb281854a1f9154cac9d8258262e60898c99 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 13 Feb 2021 20:41:28 +0100 Subject: [PATCH 12/26] Multicopter rate controller: Protect for full throttle scaling PX4 supports -1 to 1 as input and this module was not protected against the input range. --- src/modules/mc_rate_control/MulticopterRateControl.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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{}; From 8d78b8a01d2226432050008ffd7a8031ca6a6596 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 13 Feb 2021 22:12:07 +0100 Subject: [PATCH 13/26] Change wording in GPS failure handling This matches better the different platforms that are using this functionality. --- src/modules/navigator/gpsfailure.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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: From 3c12573e93ad5fa84ae746c098fbf38c9f59ab4b Mon Sep 17 00:00:00 2001 From: Jaeyoung-Lim Date: Sat, 13 Feb 2021 10:59:59 +0100 Subject: [PATCH 14/26] Use uORB::Subscription for rover_pos_control Modernize rover position control --- .../RoverPositionControl.cpp | 175 ++++++------------ .../RoverPositionControl.hpp | 32 ++-- 2 files changed, 69 insertions(+), 138 deletions(-) diff --git a/src/modules/rover_pos_control/RoverPositionControl.cpp b/src/modules/rover_pos_control/RoverPositionControl.cpp index 20592f15b8..8076f75b94 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,44 +108,32 @@ void RoverPositionControl::parameters_update(bool force) void RoverPositionControl::vehicle_control_mode_poll() { - bool updated; - orb_check(_control_mode_sub, &updated); + if (_control_mode_sub.updated()) { + _control_mode_sub.copy(&_control_mode); + } +} - if (updated) { - orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); +void +RoverPositionControl::manual_control_setpoint_poll() +{ + if (_manual_control_setpoint_sub.updated()) { + _manual_control_setpoint_sub.copy(&_manual_control_setpoint); } } 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); } } @@ -336,51 +336,16 @@ 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)); - - /* 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[3]; - - /* Setup of loop */ - fds[0].fd = _global_pos_sub; - fds[0].events = POLLIN; - fds[1].fd = _local_pos_sub; // Added local position as source of position - fds[1].events = POLLIN; - fds[2].fd = _vehicle_attitude_sub; // Poll attitude - fds[2].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(); _vehicle_acceleration_sub.update(); @@ -389,13 +354,11 @@ RoverPositionControl::run() bool manual_mode = _control_mode.flag_control_manual_enabled; - // Respond to a position update - if (fds[0].revents & POLLIN || fds[1].revents & POLLIN) { - perf_begin(_loop_perf); + /* only run controller if position changed */ + 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(); @@ -454,31 +417,19 @@ RoverPositionControl::run() control_velocity(current_velocity, _pos_sp_triplet); } - - - perf_end(_loop_perf); } // Respond to an attitude update and run the attitude controller if enabled - if (fds[2].revents & POLLIN) { + if (!manual_mode && _control_mode.flag_control_attitude_enabled + && !_control_mode.flag_control_position_enabled + && !_control_mode.flag_control_velocity_enabled) { - 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); - - } + control_attitude(_vehicle_att, _att_sp); } // Manual pass-through if no other control mode is enabled if (manual_mode) { - /* manual/direct control */ - orb_copy(ORB_ID(manual_control_setpoint), _manual_control_setpoint_sub, &_manual_control_setpoint); - _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 @@ -497,52 +448,30 @@ RoverPositionControl::run() _act_controls.timestamp = hrt_absolute_time(); _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); - - 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 241f272287..0d853add1b 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,7 +55,9 @@ #include #include #include +#include #include +#include #include #include #include @@ -74,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(); @@ -86,30 +88,29 @@ 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 _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}; - - 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 */ @@ -182,6 +183,7 @@ private: void attitude_setpoint_poll(); void vehicle_control_mode_poll(); void vehicle_attitude_poll(); + void manual_control_setpoint_poll(); /** * Control position. From fdd9b3ea513300cf7160c41653abaf9edc321b58 Mon Sep 17 00:00:00 2001 From: Jaeyoung-Lim Date: Sat, 13 Feb 2021 15:09:53 +0100 Subject: [PATCH 15/26] Add support for stabilized flight mode for rovers This commit adds support for stabilized flight mode for rovers, which enables the rover tracking a fixed heading that is set with a manual input --- .../RoverPositionControl.cpp | 78 ++++++++++++++----- .../RoverPositionControl.hpp | 6 ++ .../rover_pos_control_params.c | 11 +++ 3 files changed, 74 insertions(+), 21 deletions(-) diff --git a/src/modules/rover_pos_control/RoverPositionControl.cpp b/src/modules/rover_pos_control/RoverPositionControl.cpp index 8076f75b94..eaa3e82b1d 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.cpp +++ b/src/modules/rover_pos_control/RoverPositionControl.cpp @@ -116,8 +116,58 @@ RoverPositionControl::vehicle_control_mode_poll() void RoverPositionControl::manual_control_setpoint_poll() { - if (_manual_control_setpoint_sub.updated()) { - _manual_control_setpoint_sub.copy(&_manual_control_setpoint); + 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 (!_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.r * 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(); + } } } @@ -352,8 +402,6 @@ RoverPositionControl::Run() /* update parameters from storage */ parameters_update(); - bool manual_mode = _control_mode.flag_control_manual_enabled; - /* only run controller if position changed */ if (_local_pos_sub.update(&_local_pos)) { @@ -382,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)) { @@ -412,7 +460,7 @@ 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); @@ -420,30 +468,18 @@ RoverPositionControl::Run() } // Respond to an attitude update and run the attitude controller if enabled - if (!manual_mode && _control_mode.flag_control_attitude_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); } - // Manual pass-through if no other control mode is enabled - if (manual_mode) { - _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; - } - - // publish the controller output + /* 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) { + _control_mode.flag_control_manual_enabled) { // timestamp and publish controls _act_controls.timestamp = hrt_absolute_time(); _actuator_controls_pub.publish(_act_controls); diff --git a/src/modules/rover_pos_control/RoverPositionControl.hpp b/src/modules/rover_pos_control/RoverPositionControl.hpp index 0d853add1b..e80c53f99d 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.hpp +++ b/src/modules/rover_pos_control/RoverPositionControl.hpp @@ -102,6 +102,7 @@ private: 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 */ @@ -126,6 +127,7 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ hrt_abstime _control_position_last_called{0}; /**) _param_l1_period, (ParamFloat) _param_l1_damping, @@ -171,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 */ ) 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 bdc82044d8..b3120f50a7 100644 --- a/src/modules/rover_pos_control/rover_pos_control_params.c +++ b/src/modules/rover_pos_control/rover_pos_control_params.c @@ -273,3 +273,14 @@ PARAM_DEFINE_FLOAT(GND_SPEED_MAX, 10.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); From 2be482c5aef67b11875d19492d343d94901d7c8f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 13 Feb 2021 23:22:19 +0100 Subject: [PATCH 16/26] Sensors: New parameter for baro priority This allows to give external barometers priority if they are present. --- src/modules/sensors/sensor_params.c | 12 ++++++++++++ 1 file changed, 12 insertions(+) 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); From 4c9b2c65b5e9e9433a0ee5ba06a2c4bb2a6d2e27 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 13 Feb 2021 23:23:12 +0100 Subject: [PATCH 17/26] Support v5X build variants This adds support for the different implementation variants of the v5X standard. --- boards/px4/fmu-v5x/init/rc.board_sensors | 56 ++++++++++++++----- .../fmu-v5x/nuttx-config/scripts/script.ld | 8 +++ boards/px4/fmu-v5x/src/board_config.h | 1 + boards/px4/fmu-v5x/src/init.cpp | 26 +++++++++ 4 files changed, 78 insertions(+), 13 deletions(-) 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; } From 4c5d2363d480222a3f1999e755284b4751a2fc09 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 14 Feb 2021 11:14:52 +0100 Subject: [PATCH 18/26] Rover control: Use roll sticks It is more intuitive to use the roll stick as lateral control input given that drones defined this category, plus this is how consumer car / rover radio controls have been working already. --- src/modules/rover_pos_control/RoverPositionControl.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/rover_pos_control/RoverPositionControl.cpp b/src/modules/rover_pos_control/RoverPositionControl.cpp index eaa3e82b1d..a5cdda49ff 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.cpp +++ b/src/modules/rover_pos_control/RoverPositionControl.cpp @@ -136,7 +136,7 @@ RoverPositionControl::manual_control_setpoint_poll() } else { const float yaw_rate = math::radians(_param_gnd_man_y_max.get()); - _att_sp.yaw_sp_move_rate = _manual_control_setpoint.r * yaw_rate; + _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); } From 40a452dcd2f470541d923ff8a9556fc9a40916ca Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sat, 13 Feb 2021 18:40:54 -0500 Subject: [PATCH 19/26] ekf2: selector improve timeout handling and reporting - relax estimator_status timeout unless attitude hasn't published recently --- src/modules/ekf2/EKF2Selector.cpp | 76 ++++++++++++++++++++++--------- src/modules/ekf2/EKF2Selector.hpp | 8 +++- 2 files changed, 62 insertions(+), 22 deletions(-) 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; }; From 37f78537c3299df0766ef62a3cd185fcd70ed229 Mon Sep 17 00:00:00 2001 From: Jari Nippula Date: Wed, 3 Feb 2021 13:28:19 +0200 Subject: [PATCH 20/26] protocol-splitter: Additional protocol layer added --- .../protocol_splitter/protocol_splitter.cpp | 112 ++++++++++++------ 1 file changed, 76 insertions(+), 36 deletions(-) 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); } From 0618f048f20290b45780c8fe1e33ac8d913eeb8a Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 15 Feb 2021 12:03:21 -0500 Subject: [PATCH 21/26] commander: use control mode flags and cleanup arm_disarm - keep `vehicle_control_mode` last state in commander and use appropriate flags in place of various main_state and nav_state checks - consolidate scattered arming requirements in `arm_disarm()` - there were a number of different requirements from arming via RC vs Mavlink that don't make any sense - if geofence enabled require valid home before arming - throttle requirements for manual modes - remove unnecessary mavlink feedback that differs between arming interfaces (mavlink vs RC) - let the preflight/prearm checks respond directly in most cases Co-authored-by: Matthias Grob --- src/modules/commander/Commander.cpp | 595 ++++++++++++++-------------- src/modules/commander/Commander.hpp | 15 +- 2 files changed, 297 insertions(+), 313 deletions(-) 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 From 540e4f9464f4aeaa14accc88715be5c870c4081e Mon Sep 17 00:00:00 2001 From: Tim Date: Mon, 15 Feb 2021 18:40:28 +0100 Subject: [PATCH 22/26] Uuv position control extension (#16688) * Commit for the Integration of a position controller for the a Underwater vehicle. This module is an extension of the uuv_att_control to control an Underwater vehicle to any position, given by the SET_POSITION_TARGET_LOCAL_NED which includes x y z yaw. Since the position control is designed for a 6DOF Robot, the roll and pitch angle are controlled to be 0. Additionally there is a stabilization control, which holds the robot at a defined depth, and not move in any direction. In general the idea is to have this position module to control the position of the uuv. The position module reseives the desired position of the uuv and sends appropriate attitude setpoints to the uuv_attitude_control module. Additionally the mixer file is adapted, to include the 6 different inputs(x y z roll pitch yaw). * Commit for the Integration of a position controller for the a Underwater vehicle. This module is an extension of the uuv_att_control to control an Underwater vehicle to any position, given by the SET_POSITION_TARGET_LOCAL_NED which includes x y z yaw. Since the position control is designed for a 6DOF Robot, the roll and pitch angle are controlled to be 0. Additionally there is a stabilization control, which holds the robot at a defined depth, and not move in any direction. In general the idea is to have this position module to control the position of the uuv. The position module receives the desired position of the uuv and sends appropriate attitude setpoints to the uuv_attitude_control module. Additionally the mixer file is adapted, to include the 6 different inputs(x y z roll pitch yaw). Currently not solved/missing: - Problem with gazebo model(propeller moving chaotically). - Mixer correct gazebo vs real life (has to be tested in the future) - correct integration in uuv.apps (when choose which module) - very basic controller chosen (could be improved a lot in the future) * Remove error caused by unused variables and a different build error * added better description of the parameter. Additionally the group is changed. * added better description of the parameter. Additionally the group is changed. Fixed bug about parameter * Added EOF to the files. * Removed parameter for direct position control for safety reasons. * small bugfix --- .../mixers-sitl/vectored6dof_sitl.main.mix | 24 +- .../mixers/vectored6dof.main.mix | 24 +- boards/px4/fmu-v4/default.cmake | 1 + boards/px4/fmu-v4/rtps.cmake | 1 + boards/px4/fmu-v4pro/default.cmake | 1 + boards/px4/fmu-v4pro/rtps.cmake | 1 + boards/px4/fmu-v5/ctrlalloc.cmake | 1 + boards/px4/fmu-v5/default.cmake | 1 + boards/px4/sitl/ctrlalloc.cmake | 1 + boards/px4/sitl/default.cmake | 1 + boards/px4/sitl/nolockstep.cmake | 1 + boards/px4/sitl/rtps.cmake | 1 + boards/px4/sitl/test.cmake | 1 + src/modules/uuv_pos_control/CMakeLists.txt | 40 +++ .../uuv_pos_control/uuv_pos_control.cpp | 291 ++++++++++++++++++ .../uuv_pos_control/uuv_pos_control.hpp | 152 +++++++++ .../uuv_pos_control/uuv_pos_control_params.c | 97 ++++++ 17 files changed, 623 insertions(+), 16 deletions(-) create mode 100644 src/modules/uuv_pos_control/CMakeLists.txt create mode 100644 src/modules/uuv_pos_control/uuv_pos_control.cpp create mode 100644 src/modules/uuv_pos_control/uuv_pos_control.hpp create mode 100644 src/modules/uuv_pos_control/uuv_pos_control_params.c 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/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/px4/fmu-v4/default.cmake b/boards/px4/fmu-v4/default.cmake index b65c7ee71d..65495b3e1c 100644 --- a/boards/px4/fmu-v4/default.cmake +++ b/boards/px4/fmu-v4/default.cmake @@ -89,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/rtps.cmake b/boards/px4/fmu-v4/rtps.cmake index 20a50e3bf5..eac3a3fb82 100644 --- a/boards/px4/fmu-v4/rtps.cmake +++ b/boards/px4/fmu-v4/rtps.cmake @@ -84,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-v4pro/default.cmake b/boards/px4/fmu-v4pro/default.cmake index 0905161880..3592562acd 100644 --- a/boards/px4/fmu-v4pro/default.cmake +++ b/boards/px4/fmu-v4pro/default.cmake @@ -86,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 4bc52191d7..8167947868 100644 --- a/boards/px4/fmu-v4pro/rtps.cmake +++ b/boards/px4/fmu-v4pro/rtps.cmake @@ -83,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/ctrlalloc.cmake b/boards/px4/fmu-v5/ctrlalloc.cmake index 7f76304e67..cd3f1ec6a7 100644 --- a/boards/px4/fmu-v5/ctrlalloc.cmake +++ b/boards/px4/fmu-v5/ctrlalloc.cmake @@ -93,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/default.cmake b/boards/px4/fmu-v5/default.cmake index 4cb7672727..3ad3f7fe8e 100644 --- a/boards/px4/fmu-v5/default.cmake +++ b/boards/px4/fmu-v5/default.cmake @@ -91,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/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/src/modules/uuv_pos_control/CMakeLists.txt b/src/modules/uuv_pos_control/CMakeLists.txt new file mode 100644 index 0000000000..f97dd0ea42 --- /dev/null +++ b/src/modules/uuv_pos_control/CMakeLists.txt @@ -0,0 +1,40 @@ +############################################################################ +# +# 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. +# +############################################################################ +px4_add_module( + MODULE modules__uuv_pos_control + MAIN uuv_pos_control + STACK_MAIN 1200 + COMPILE_FLAGS + SRCS + 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/modules/uuv_pos_control/uuv_pos_control_params.c b/src/modules/uuv_pos_control/uuv_pos_control_params.c new file mode 100644 index 0000000000..d3532edd43 --- /dev/null +++ b/src/modules/uuv_pos_control/uuv_pos_control_params.c @@ -0,0 +1,97 @@ +/**************************************************************************** + * + * 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 + * 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 uuv_pos_control_params.c + * + * Parameters defined by the position control task for unmanned underwater vehicles (UUVs) + * + * 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 + */ + +/* + * 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); + +/** + * Gain of D controller X + * + * @group UUV Position Control + */ +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); From 2ad1cb24ccc9889084dc6d7082106aa77abc1630 Mon Sep 17 00:00:00 2001 From: CAI Dongcai Date: Mon, 12 Oct 2020 23:43:40 +0800 Subject: [PATCH 23/26] fix empty log file "micrortps_bridge.log" --- src/modules/micrortps_bridge/micrortps_client/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/micrortps_bridge/micrortps_client/CMakeLists.txt b/src/modules/micrortps_bridge/micrortps_client/CMakeLists.txt index cbba329896..401e4f2d66 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 DEPENDS ${send_topic_files} ${receive_topic_files} COMMENT "Generating RTPS topic bridge" ) From 15392f8e538defb4566c86ed95843145579f2e62 Mon Sep 17 00:00:00 2001 From: Ramon Roche Date: Mon, 15 Feb 2021 11:02:21 -0800 Subject: [PATCH 24/26] silent success and log failures Co-authored-by: Nuno Marques --- src/modules/micrortps_bridge/micrortps_client/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/micrortps_bridge/micrortps_client/CMakeLists.txt b/src/modules/micrortps_bridge/micrortps_client/CMakeLists.txt index 401e4f2d66..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 + >micrortps_bridge.log 2>&1 || cat micrortps_bridge.log DEPENDS ${send_topic_files} ${receive_topic_files} COMMENT "Generating RTPS topic bridge" ) From 59b31e3c5b3eb3f2a87ded131e12f0c0596e0815 Mon Sep 17 00:00:00 2001 From: mcsauder Date: Mon, 15 Feb 2021 17:39:05 -0700 Subject: [PATCH 25/26] Minor cleanup/error checking, static_casts, print_status() additions, and formatting in the heater driver. --- src/drivers/drv_io_heater.h | 6 +- src/drivers/heater/heater.cpp | 290 +++++++++++++++-------------- src/drivers/heater/heater.h | 57 ++---- src/drivers/heater/heater_params.c | 12 ++ 4 files changed, 186 insertions(+), 179 deletions(-) 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. * From 44df0fb7a25e8387812399458a18fadb24a7a03f Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 14 Feb 2021 16:23:35 -0500 Subject: [PATCH 26/26] Analog Devices ADIS16448 rewrite - new IMU driver structure with state machine (no sleeps in bus thread) - verify all configured registers and trigger reset on failure - detect if DIO1 or DIO2 are actually connected for data ready interrupt usage - don't use CRC-16 on burst transfers except for verified lots --- boards/cuav/x7pro/default.cmake | 2 +- boards/cubepilot/cubeorange/console.cmake | 2 +- boards/cubepilot/cubeorange/default.cmake | 2 +- boards/cubepilot/cubeyellow/console.cmake | 2 +- boards/cubepilot/cubeyellow/default.cmake | 2 +- boards/holybro/durandal-v1/default.cmake | 2 +- boards/holybro/pix32v5/default.cmake | 2 +- boards/nxp/fmurt1062-v1/default.cmake | 2 +- boards/px4/fmu-v2/default.cmake | 2 +- boards/px4/fmu-v2/lpe.cmake | 2 +- boards/px4/fmu-v2/test.cmake | 2 +- boards/px4/fmu-v3/ctrlalloc.cmake | 2 +- boards/px4/fmu-v3/default.cmake | 2 +- boards/px4/fmu-v3/rtps.cmake | 2 +- boards/px4/fmu-v3/stackcheck.cmake | 2 +- boards/px4/fmu-v4/cannode.cmake | 2 +- boards/px4/fmu-v4/ctrlalloc.cmake | 2 +- boards/px4/fmu-v4/default.cmake | 2 +- boards/px4/fmu-v4/optimized.cmake | 2 +- boards/px4/fmu-v4/rtps.cmake | 2 +- boards/px4/fmu-v4/stackcheck.cmake | 2 +- boards/px4/fmu-v4/uavcanv1.cmake | 2 +- boards/px4/fmu-v5/critmonitor.cmake | 2 +- boards/px4/fmu-v5/ctrlalloc.cmake | 2 +- boards/px4/fmu-v5/debug.cmake | 2 +- boards/px4/fmu-v5/default.cmake | 2 +- boards/px4/fmu-v5/fixedwing.cmake | 2 +- boards/px4/fmu-v5/irqmonitor.cmake | 2 +- boards/px4/fmu-v5/multicopter.cmake | 2 +- boards/px4/fmu-v5/rover.cmake | 2 +- boards/px4/fmu-v5/rtps.cmake | 2 +- boards/px4/fmu-v5/stackcheck.cmake | 2 +- boards/px4/fmu-v5/uavcanv1.cmake | 2 +- boards/px4/fmu-v5x/base_phy_DP83848C.cmake | 2 +- boards/px4/fmu-v5x/default.cmake | 2 +- boards/px4/fmu-v6u/default.cmake | 2 +- boards/px4/fmu-v6u/stackcheck.cmake | 2 +- boards/px4/fmu-v6x/default.cmake | 2 +- boards/px4/fmu-v6x/stackcheck.cmake | 2 +- boards/spracing/h7extreme/default.cmake | 2 +- src/drivers/imu/CMakeLists.txt | 2 +- src/drivers/imu/adis16448/ADIS16448.cpp | 514 --------------- src/drivers/imu/adis16448/ADIS16448.h | 206 ------ src/drivers/imu/analog_devices/CMakeLists.txt | 34 + .../analog_devices/adis16448/ADIS16448.cpp | 594 ++++++++++++++++++ .../analog_devices/adis16448/ADIS16448.hpp | 138 ++++ .../Analog_Devices_ADIS16448_registers.hpp | 161 +++++ .../adis16448/CMakeLists.txt | 12 +- .../adis16448/adis16448_main.cpp | 17 +- 49 files changed, 983 insertions(+), 775 deletions(-) delete mode 100644 src/drivers/imu/adis16448/ADIS16448.cpp delete mode 100644 src/drivers/imu/adis16448/ADIS16448.h create mode 100644 src/drivers/imu/analog_devices/CMakeLists.txt create mode 100644 src/drivers/imu/analog_devices/adis16448/ADIS16448.cpp create mode 100644 src/drivers/imu/analog_devices/adis16448/ADIS16448.hpp create mode 100644 src/drivers/imu/analog_devices/adis16448/Analog_Devices_ADIS16448_registers.hpp rename src/drivers/imu/{ => analog_devices}/adis16448/CMakeLists.txt (89%) rename src/drivers/imu/{ => analog_devices}/adis16448/adis16448_main.cpp (91%) diff --git a/boards/cuav/x7pro/default.cmake b/boards/cuav/x7pro/default.cmake index bf4a25a36b..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 diff --git a/boards/cubepilot/cubeorange/console.cmake b/boards/cubepilot/cubeorange/console.cmake index 77f016be3e..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 diff --git a/boards/cubepilot/cubeorange/default.cmake b/boards/cubepilot/cubeorange/default.cmake index b4353de6ab..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 diff --git a/boards/cubepilot/cubeyellow/console.cmake b/boards/cubepilot/cubeyellow/console.cmake index 2dc0be9875..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 diff --git a/boards/cubepilot/cubeyellow/default.cmake b/boards/cubepilot/cubeyellow/default.cmake index f5697ce4a9..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 diff --git a/boards/holybro/durandal-v1/default.cmake b/boards/holybro/durandal-v1/default.cmake index af56dae993..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 diff --git a/boards/holybro/pix32v5/default.cmake b/boards/holybro/pix32v5/default.cmake index 1c85d25df5..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 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/px4/fmu-v2/default.cmake b/boards/px4/fmu-v2/default.cmake index 5957115394..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 diff --git a/boards/px4/fmu-v2/lpe.cmake b/boards/px4/fmu-v2/lpe.cmake index 1757a4912c..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 diff --git a/boards/px4/fmu-v2/test.cmake b/boards/px4/fmu-v2/test.cmake index aab53f5cd4..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 diff --git a/boards/px4/fmu-v3/ctrlalloc.cmake b/boards/px4/fmu-v3/ctrlalloc.cmake index e17094c16d..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 diff --git a/boards/px4/fmu-v3/default.cmake b/boards/px4/fmu-v3/default.cmake index 2ade27c004..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 diff --git a/boards/px4/fmu-v3/rtps.cmake b/boards/px4/fmu-v3/rtps.cmake index 012a8a6fb4..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 diff --git a/boards/px4/fmu-v3/stackcheck.cmake b/boards/px4/fmu-v3/stackcheck.cmake index 0dcc05bee9..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 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 df65099c1e..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 diff --git a/boards/px4/fmu-v4/default.cmake b/boards/px4/fmu-v4/default.cmake index 65495b3e1c..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 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 eac3a3fb82..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 diff --git a/boards/px4/fmu-v4/stackcheck.cmake b/boards/px4/fmu-v4/stackcheck.cmake index cbdb8569cd..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 diff --git a/boards/px4/fmu-v4/uavcanv1.cmake b/boards/px4/fmu-v4/uavcanv1.cmake index b689e945d7..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 diff --git a/boards/px4/fmu-v5/critmonitor.cmake b/boards/px4/fmu-v5/critmonitor.cmake index a490448a66..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 diff --git a/boards/px4/fmu-v5/ctrlalloc.cmake b/boards/px4/fmu-v5/ctrlalloc.cmake index cd3f1ec6a7..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 diff --git a/boards/px4/fmu-v5/debug.cmake b/boards/px4/fmu-v5/debug.cmake index f7f1a79584..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 diff --git a/boards/px4/fmu-v5/default.cmake b/boards/px4/fmu-v5/default.cmake index 3ad3f7fe8e..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 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 89c5d85f78..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 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 63909681e5..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 diff --git a/boards/px4/fmu-v5/rtps.cmake b/boards/px4/fmu-v5/rtps.cmake index e3df35d28e..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 diff --git a/boards/px4/fmu-v5/stackcheck.cmake b/boards/px4/fmu-v5/stackcheck.cmake index 8fa71424c2..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 diff --git a/boards/px4/fmu-v5/uavcanv1.cmake b/boards/px4/fmu-v5/uavcanv1.cmake index 56aaf8ed3e..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 diff --git a/boards/px4/fmu-v5x/base_phy_DP83848C.cmake b/boards/px4/fmu-v5x/base_phy_DP83848C.cmake index ca8280efe0..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 diff --git a/boards/px4/fmu-v5x/default.cmake b/boards/px4/fmu-v5x/default.cmake index 0e57ae8611..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 diff --git a/boards/px4/fmu-v6u/default.cmake b/boards/px4/fmu-v6u/default.cmake index c04dcd5d8a..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 diff --git a/boards/px4/fmu-v6u/stackcheck.cmake b/boards/px4/fmu-v6u/stackcheck.cmake index c04dcd5d8a..4c839c807c 100644 --- a/boards/px4/fmu-v6u/stackcheck.cmake +++ b/boards/px4/fmu-v6u/stackcheck.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 diff --git a/boards/px4/fmu-v6x/default.cmake b/boards/px4/fmu-v6x/default.cmake index 0ba7936de3..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 diff --git a/boards/px4/fmu-v6x/stackcheck.cmake b/boards/px4/fmu-v6x/stackcheck.cmake index 0ba7936de3..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 diff --git a/boards/spracing/h7extreme/default.cmake b/boards/spracing/h7extreme/default.cmake index 26c97d62b4..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 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); }