fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 05:04:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #10 from ethz-asl/pr-upstream-merge

Browse Source 
16/02/2021 Upstream Merge
This commit is contained in:
JaeyoungLim 2021-02-23 09:43:53 +01:00 committed by GitHub
commit 472e191fef
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
115 changed files with 2438 additions and 3104 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ROMFS/px4fmu_common/init.d-posix/airframes/1060_rover
View File

@ -8,6 +8,7 @@
if [ $AUTOCNF = yes ]
then
param set GND_L1_DIST 5
param set GND_L1_PERIOD 10
param set GND_SP_CTRL_MODE 1
param set GND_SPEED_D 0.001
param set GND_SPEED_I 3
@ -16,7 +17,6 @@ then
param set GND_SPEED_THR_SC 1
param set GND_SPEED_TRIM 4
param set GND_THR_CRUISE 0.3
param set GND_THR_IDLE 0
param set GND_THR_MAX 0.5
param set GND_THR_MIN 0

1
ROMFS/px4fmu_common/init.d-posix/airframes/1061_r1_rover
View File

@ -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

1
ROMFS/px4fmu_common/init.d-posix/airframes/1062_tf-r1
View File

@ -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

1
ROMFS/px4fmu_common/init.d-posix/airframes/1070_boat
View File

@ -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

9
ROMFS/px4fmu_common/init.d/airframes/50000_generic_ground_vehicle
View File

@ -27,21 +27,18 @@ param set-default FW_AIRSPD_MAX 3
param set-default FW_AIRSPD_MIN 0
param set-default FW_AIRSPD_TRIM 1
param set-default GND_L1_DIST 10
# Settings for a typical wheelbase 0f 0.3m
param set-default GND_L1_DIST 1
param set-default GND_L1_PERIOD 5
param set-default GND_SP_CTRL_MODE 1
param set-default GND_SPEED_P 0.25
param set-default GND_SPEED_I 3
param set-default GND_SPEED_D 0.001
param set-default GND_SPEED_IMAX 0.125
param set-default GND_SPEED_THR_SC 1
param set-default GND_THR_IDLE 0
param set-default GND_THR_CRUISE 0.3
param set-default GND_THR_MAX 0.5
param set-default GND_THR_MIN 0
param set-default GND_WR_P 2
param set-default GND_WR_I 0.9674
param set-default GND_WR_IMAX 0.1
param set-default GND_WR_D 1.2
param set-default MIS_LTRMIN_ALT 0.01
param set-default MIS_TAKEOFF_ALT 0.01

1
ROMFS/px4fmu_common/init.d/airframes/50003_aion_robotics_r1_rover
View File

@ -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

1
ROMFS/px4fmu_common/init.d/airframes/50004_nxpcup_car_dfrobot_gpx
View File

@ -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

23
ROMFS/px4fmu_common/init.d/rc.interface
View File

@ -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

4
ROMFS/px4fmu_common/init.d/rcS
View File

@ -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

24
ROMFS/px4fmu_common/mixers-sitl/vectored6dof_sitl.main.mix
View File

@ -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

11
ROMFS/px4fmu_common/mixers/rover_diff_and_servo.main.mix
View File

@ -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

10
ROMFS/px4fmu_common/mixers/rover_generic.main.mix
View File

@ -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

24
ROMFS/px4fmu_common/mixers/vectored6dof.main.mix
View File

@ -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

1
boards/airmind/mindpx-v2/default.cmake
View File

@ -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

1
boards/av/x-v1/default.cmake
View File

@ -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

1
boards/cuav/nora/default.cmake
View File

@ -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

3
boards/cuav/x7pro/default.cmake
View File

@ -30,7 +30,7 @@ px4_add_board(
gps
heater
#imu # all available imu drivers
imu/adis16448
imu/analog_devices/adis16448
imu/adis16477
imu/adis16497
imu/bosch/bmi088
@ -42,7 +42,6 @@ px4_add_board(
lights/rgbled_ncp5623c
lights/rgbled_pwm
magnetometer # all available magnetometer drivers
mkblctrl
optical_flow # all available optical flow drivers
#osd
pca9685

3
boards/cubepilot/cubeorange/console.cmake
View File

@ -30,7 +30,7 @@ px4_add_board(
gps
#heater
#imu # all available imu drivers
imu/adis16448
imu/analog_devices/adis16448
imu/adis16477
imu/adis16497
imu/invensense/icm20602
@ -41,7 +41,6 @@ px4_add_board(
lights/rgbled
lights/rgbled_ncp5623c
magnetometer # all available magnetometer drivers
mkblctrl
optical_flow # all available optical flow drivers
#osd
pca9685

3
boards/cubepilot/cubeorange/default.cmake
View File

@ -30,7 +30,7 @@ px4_add_board(
gps
#heater
#imu # all available imu drivers
imu/adis16448
imu/analog_devices/adis16448
imu/adis16477
imu/adis16497
imu/invensense/icm20602
@ -41,7 +41,6 @@ px4_add_board(
lights/rgbled
lights/rgbled_ncp5623c
magnetometer # all available magnetometer drivers
mkblctrl
optical_flow # all available optical flow drivers
#osd
pca9685

3
boards/cubepilot/cubeyellow/console.cmake
View File

@ -29,7 +29,7 @@ px4_add_board(
gps
#heater
#imu # all available imu drivers
imu/adis16448
imu/analog_devices/adis16448
imu/adis16477
imu/adis16497
imu/invensense/icm20602
@ -40,7 +40,6 @@ px4_add_board(
lights/rgbled
lights/rgbled_ncp5623c
magnetometer # all available magnetometer drivers
mkblctrl
optical_flow # all available optical flow drivers
#osd
pca9685

3
boards/cubepilot/cubeyellow/default.cmake
View File

@ -29,7 +29,7 @@ px4_add_board(
gps
#heater
#imu # all available imu drivers
imu/adis16448
imu/analog_devices/adis16448
imu/adis16477
imu/adis16497
imu/invensense/icm20602
@ -40,7 +40,6 @@ px4_add_board(
lights/rgbled
lights/rgbled_ncp5623c
magnetometer # all available magnetometer drivers
mkblctrl
optical_flow # all available optical flow drivers
#osd
pca9685

3
boards/holybro/durandal-v1/default.cmake
View File

@ -29,7 +29,7 @@ px4_add_board(
gps
heater
#imu # all available imu drivers
imu/adis16448
imu/analog_devices/adis16448
imu/adis16477
imu/adis16497
imu/bosch/bmi088
@ -39,7 +39,6 @@ px4_add_board(
lights/rgbled
lights/rgbled_ncp5623c
magnetometer # all available magnetometer drivers
mkblctrl
optical_flow # all available optical flow drivers
#osd
pca9685

3
boards/holybro/pix32v5/default.cmake
View File

@ -28,7 +28,7 @@ px4_add_board(
gps
#heater
#imu # all available imu drivers
imu/adis16448
imu/analog_devices/adis16448
imu/adis16477
imu/adis16497
imu/bosch/bmi055
@ -41,7 +41,6 @@ px4_add_board(
lights/rgbled_ncp5623c
lights/rgbled_pwm
magnetometer # all available magnetometer drivers
mkblctrl
optical_flow # all available optical flow drivers
#osd
pca9685

1
boards/modalai/fc-v1/default.cmake
View File

@ -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

1
boards/mro/ctrl-zero-f7-oem/default.cmake
View File

@ -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

1
boards/mro/ctrl-zero-f7/default.cmake
View File

@ -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

1
boards/mro/pixracerpro/default.cmake
View File

@ -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

1
boards/mro/x21-777/default.cmake
View File

@ -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

1
boards/mro/x21/default.cmake
View File

@ -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

1
boards/nxp/fmuk66-e/default.cmake
View File

@ -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

1
boards/nxp/fmuk66-e/socketcan.cmake
View File

@ -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

1
boards/nxp/fmuk66-v3/default.cmake
View File

@ -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

1
boards/nxp/fmuk66-v3/rtps.cmake
View File

@ -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

1
boards/nxp/fmuk66-v3/socketcan.cmake
View File

@ -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

2
boards/nxp/fmurt1062-v1/default.cmake
View File

@ -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

1
boards/omnibus/f4sd/default.cmake
View File

@ -28,7 +28,6 @@ px4_add_board(
lights/rgbled
#magnetometer # all available magnetometer drivers
magnetometer/hmc5883
#mkblctrl
optical_flow/px4flow
osd
#pca9685

3
boards/px4/fmu-v2/default.cmake
View File

@ -34,7 +34,7 @@ px4_add_board(
gps
#heater
#imu # all available imu drivers
#imu/adis16448
#imu/analog_devices/adis16448
#imu/adis16477
#imu/adis16497
imu/l3gd20
@ -50,7 +50,6 @@ px4_add_board(
lights/rgbled
#magnetometer # all available magnetometer drivers
magnetometer/hmc5883
#mkblctrl
#optical_flow # all available optical flow drivers
#optical_flow/px4flow
#osd

3
boards/px4/fmu-v2/lpe.cmake
View File

@ -32,7 +32,7 @@ px4_add_board(
distance_sensor # all available distance sensor drivers
gps
#heater
#imu/adis16448
#imu/analog_devices/adis16448
#imu # all available imu drivers
imu/l3gd20
imu/lsm303d
@ -43,7 +43,6 @@ px4_add_board(
lights/rgbled
#magnetometer # all available magnetometer drivers
magnetometer/hmc5883
#mkblctrl
#optical_flow # all available optical flow drivers
optical_flow/px4flow
#pca9685

3
boards/px4/fmu-v2/test.cmake
View File

@ -33,7 +33,7 @@ px4_add_board(
gps
#heater
#imu # all available imu drivers
#imu/adis16448
#imu/analog_devices/adis16448
#imu/adis16477
#imu/adis16497
imu/l3gd20
@ -46,7 +46,6 @@ px4_add_board(
lights/rgbled
#magnetometer # all available magnetometer drivers
magnetometer/hmc5883
#mkblctrl
#optical_flow # all available optical flow drivers
optical_flow/px4flow
#osd

3
boards/px4/fmu-v3/ctrlalloc.cmake
View File

@ -31,7 +31,7 @@ px4_add_board(
gps
#heater
#imu # all available imu drivers
imu/adis16448
imu/analog_devices/adis16448
imu/adis16477
imu/adis16497
imu/l3gd20
@ -45,7 +45,6 @@ px4_add_board(
lights/rgbled
lights/rgbled_ncp5623c
magnetometer # all available magnetometer drivers
mkblctrl
#optical_flow # all available optical flow drivers
optical_flow/px4flow
#osd

3
boards/px4/fmu-v3/default.cmake
View File

@ -31,7 +31,7 @@ px4_add_board(
gps
#heater
#imu # all available imu drivers
imu/adis16448
imu/analog_devices/adis16448
imu/adis16477
imu/adis16497
imu/l3gd20
@ -45,7 +45,6 @@ px4_add_board(
lights/rgbled
lights/rgbled_ncp5623c
magnetometer # all available magnetometer drivers
mkblctrl
#optical_flow # all available optical flow drivers
optical_flow/px4flow
#osd

3
boards/px4/fmu-v3/rtps.cmake
View File

@ -30,7 +30,7 @@ px4_add_board(
gps
#heater
#imu # all available imu drivers
imu/adis16448
imu/analog_devices/adis16448
imu/adis16477
imu/adis16497
imu/l3gd20
@ -44,7 +44,6 @@ px4_add_board(
lights/rgbled
lights/rgbled_ncp5623c
magnetometer # all available magnetometer drivers
mkblctrl
#optical_flow # all available optical flow drivers
optical_flow/px4flow
#osd

3
boards/px4/fmu-v3/stackcheck.cmake
View File

@ -30,7 +30,7 @@ px4_add_board(
distance_sensor # all available distance sensor drivers
gps
#heater
imu/adis16448
imu/analog_devices/adis16448
#imu # all available imu drivers
imu/l3gd20
imu/lsm303d
@ -42,7 +42,6 @@ px4_add_board(
lights/rgbled
lights/rgbled_ncp5623c
magnetometer # all available magnetometer drivers
mkblctrl
#optical_flow # all available optical flow drivers
optical_flow/px4flow
pca9685

2
boards/px4/fmu-v4/cannode.cmake
View File

@ -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

3
boards/px4/fmu-v4/ctrlalloc.cmake
View File

@ -29,7 +29,7 @@ px4_add_board(
gps
heater
#imu # all available imu drivers
imu/adis16448
imu/analog_devices/adis16448
imu/adis16477
imu/adis16497
imu/invensense/icm20602
@ -42,7 +42,6 @@ px4_add_board(
lights/rgbled
lights/rgbled_ncp5623c
magnetometer # all available magnetometer drivers
mkblctrl
optical_flow # all available optical flow drivers
#osd
pca9685

4
boards/px4/fmu-v4/default.cmake
View File

@ -29,7 +29,7 @@ px4_add_board(
gps
heater
#imu # all available imu drivers
imu/adis16448
imu/analog_devices/adis16448
imu/adis16477
imu/adis16497
imu/invensense/icm20602
@ -42,7 +42,6 @@ px4_add_board(
lights/rgbled
lights/rgbled_ncp5623c
magnetometer # all available magnetometer drivers
mkblctrl
optical_flow # all available optical flow drivers
#osd
pca9685
@ -90,6 +89,7 @@ px4_add_board(
sih
temperature_compensation
uuv_att_control
uuv_pos_control
vmount
vtol_att_control
SYSTEMCMDS

2
boards/px4/fmu-v4/optimized.cmake
View File

@ -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

4
boards/px4/fmu-v4/rtps.cmake
View File

@ -26,7 +26,7 @@ px4_add_board(
gps
heater
#imu # all available imu drivers
imu/adis16448
imu/analog_devices/adis16448
imu/adis16477
imu/adis16497
imu/invensense/icm20602
@ -39,7 +39,6 @@ px4_add_board(
lights/rgbled
lights/rgbled_ncp5623c
magnetometer # all available magnetometer drivers
mkblctrl
optical_flow # all available optical flow drivers
#osd
pca9685
@ -85,6 +84,7 @@ px4_add_board(
sih
temperature_compensation
uuv_att_control
uuv_pos_control
vmount
vtol_att_control
SYSTEMCMDS

3
boards/px4/fmu-v4/stackcheck.cmake
View File

@ -26,7 +26,7 @@ px4_add_board(
gps
heater
#imu # all available imu drivers
#imu/adis16448
#imu/analog_devices/adis16448
#imu/adis16477
#imu/adis16497
imu/invensense/icm20602
@ -39,7 +39,6 @@ px4_add_board(
lights/rgbled
lights/rgbled_ncp5623c
magnetometer # all available magnetometer drivers
mkblctrl
optical_flow # all available optical flow drivers
#osd
pca9685

3
boards/px4/fmu-v4/uavcanv1.cmake
View File

@ -28,7 +28,7 @@ px4_add_board(
gps
heater
#imu # all available imu drivers
imu/adis16448
imu/analog_devices/adis16448
imu/adis16477
imu/adis16497
imu/invensense/icm20602
@ -41,7 +41,6 @@ px4_add_board(
lights/rgbled
lights/rgbled_ncp5623c
magnetometer # all available magnetometer drivers
mkblctrl
optical_flow # all available optical flow drivers
#osd
pca9685

2
boards/px4/fmu-v4pro/default.cmake
View File

@ -38,7 +38,6 @@ px4_add_board(
lights/rgbled
lights/rgbled_ncp5623c
magnetometer # all available magnetometer drivers
mkblctrl
#optical_flow # all available optical flow drivers
optical_flow/px4flow
#osd
@ -87,6 +86,7 @@ px4_add_board(
sih
temperature_compensation
uuv_att_control
uuv_pos_control
vmount
vtol_att_control
SYSTEMCMDS

2
boards/px4/fmu-v4pro/rtps.cmake
View File

@ -37,7 +37,6 @@ px4_add_board(
lights/rgbled
lights/rgbled_ncp5623c
magnetometer # all available magnetometer drivers
mkblctrl
#optical_flow # all available optical flow drivers
optical_flow/px4flow
#osd
@ -84,6 +83,7 @@ px4_add_board(
sih
temperature_compensation
uuv_att_control
uuv_pos_control
vmount
vtol_att_control
SYSTEMCMDS

3
boards/px4/fmu-v5/critmonitor.cmake
View File

@ -27,7 +27,7 @@ px4_add_board(
gps
#heater
#imu # all available imu drivers
imu/adis16448
imu/analog_devices/adis16448
imu/adis16477
imu/adis16497
imu/bosch/bmi055
@ -40,7 +40,6 @@ px4_add_board(
lights/rgbled_ncp5623c
lights/rgbled_pwm
magnetometer # all available magnetometer drivers
mkblctrl
optical_flow # all available optical flow drivers
#osd
pca9685

4
boards/px4/fmu-v5/ctrlalloc.cmake
View File

@ -28,7 +28,7 @@ px4_add_board(
gps
#heater
#imu # all available imu drivers
imu/adis16448
imu/analog_devices/adis16448
imu/adis16477
imu/adis16497
imu/bosch/bmi055
@ -41,7 +41,6 @@ px4_add_board(
lights/rgbled_ncp5623c
lights/rgbled_pwm
magnetometer # all available magnetometer drivers
mkblctrl
optical_flow # all available optical flow drivers
#osd
pca9685
@ -94,6 +93,7 @@ px4_add_board(
sih
temperature_compensation
uuv_att_control
uuv_pos_control
vmount
vtol_att_control
SYSTEMCMDS

3
boards/px4/fmu-v5/debug.cmake
View File

@ -29,7 +29,7 @@ px4_add_board(
gps
#heater
#imu # all available imu drivers
#imu/adis16448
#imu/analog_devices/adis16448
#imu/adis16477
#imu/adis16497
#imu/bosch/bmi055
@ -42,7 +42,6 @@ px4_add_board(
lights/rgbled_ncp5623c
lights/rgbled_pwm
magnetometer # all available magnetometer drivers
#mkblctrl
optical_flow # all available optical flow drivers
#osd
#pca9685

4
boards/px4/fmu-v5/default.cmake
View File

@ -28,7 +28,7 @@ px4_add_board(
gps
#heater
#imu # all available imu drivers
imu/adis16448
imu/analog_devices/adis16448
imu/adis16477
imu/adis16497
imu/bosch/bmi055
@ -41,7 +41,6 @@ px4_add_board(
lights/rgbled_ncp5623c
lights/rgbled_pwm
magnetometer # all available magnetometer drivers
mkblctrl
optical_flow # all available optical flow drivers
#osd
pca9685
@ -92,6 +91,7 @@ px4_add_board(
sih
temperature_compensation
uuv_att_control
uuv_pos_control
vmount
vtol_att_control
SYSTEMCMDS

2
boards/px4/fmu-v5/fixedwing.cmake
View File

@ -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

3
boards/px4/fmu-v5/irqmonitor.cmake
View File

@ -27,7 +27,7 @@ px4_add_board(
gps
#heater
#imu # all available imu drivers
imu/adis16448
imu/analog_devices/adis16448
imu/adis16477
imu/adis16497
imu/bosch/bmi055
@ -40,7 +40,6 @@ px4_add_board(
lights/rgbled_ncp5623c
lights/rgbled_pwm
magnetometer # all available magnetometer drivers
mkblctrl
optical_flow # all available optical flow drivers
#osd
pca9685

2
boards/px4/fmu-v5/multicopter.cmake
View File

@ -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

3
boards/px4/fmu-v5/rover.cmake
View File

@ -22,7 +22,7 @@ px4_add_board(
camera_trigger
distance_sensor # all available distance sensor drivers
gps
imu/adis16448
imu/analog_devices/adis16448
imu/adis16477
imu/adis16497
imu/bosch/bmi055
@ -33,7 +33,6 @@ px4_add_board(
lights/rgbled_ncp5623c
lights/rgbled_pwm
magnetometer # all available magnetometer drivers
mkblctrl
optical_flow # all available optical flow drivers
pca9685
pwm_input

3
boards/px4/fmu-v5/rtps.cmake
View File

@ -27,7 +27,7 @@ px4_add_board(
gps
#heater
#imu # all available imu drivers
imu/adis16448
imu/analog_devices/adis16448
imu/adis16477
imu/adis16497
imu/bosch/bmi055
@ -40,7 +40,6 @@ px4_add_board(
lights/rgbled_ncp5623c
lights/rgbled_pwm
magnetometer # all available magnetometer drivers
mkblctrl
optical_flow # all available optical flow drivers
#osd
pca9685

3
boards/px4/fmu-v5/stackcheck.cmake
View File

@ -29,7 +29,7 @@ px4_add_board(
gps
#heater
#imu # all available imu drivers
#imu/adis16448
#imu/analog_devices/adis16448
#imu/adis16477
#imu/adis16497
#imu/bosch/bmi055
@ -42,7 +42,6 @@ px4_add_board(
#lights/rgbled_ncp5623c
lights/rgbled_pwm
magnetometer # all available magnetometer drivers
#mkblctrl
optical_flow # all available optical flow drivers
#osd
#pca9685

3
boards/px4/fmu-v5/uavcanv1.cmake
View File

@ -28,7 +28,7 @@ px4_add_board(
gps
#heater
#imu # all available imu drivers
imu/adis16448
imu/analog_devices/adis16448
imu/adis16477
imu/adis16497
imu/bosch/bmi055
@ -41,7 +41,6 @@ px4_add_board(
lights/rgbled_ncp5623c
lights/rgbled_pwm
magnetometer # all available magnetometer drivers
mkblctrl
optical_flow # all available optical flow drivers
#osd
pca9685

3
boards/px4/fmu-v5x/base_phy_DP83848C.cmake
View File

@ -28,7 +28,7 @@ px4_add_board(
gps
#heater
#imu # all available imu drivers
imu/adis16448
imu/analog_devices/adis16448
imu/adis16477
imu/adis16497
imu/bosch/bmi088
@ -39,7 +39,6 @@ px4_add_board(
lights/rgbled
lights/rgbled_ncp5623c
magnetometer # all available magnetometer drivers
mkblctrl
optical_flow # all available optical flow drivers
#osd
pca9685

3
boards/px4/fmu-v5x/default.cmake
View File

@ -29,7 +29,7 @@ px4_add_board(
gps
#heater
#imu # all available imu drivers
imu/adis16448
imu/analog_devices/adis16448
imu/adis16477
imu/adis16497
imu/bosch/bmi088
@ -40,7 +40,6 @@ px4_add_board(
lights/rgbled
lights/rgbled_ncp5623c
magnetometer # all available magnetometer drivers
mkblctrl
optical_flow # all available optical flow drivers
#osd
pca9685

56
boards/px4/fmu-v5x/init/rc.board_sensors
View File

@ -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

8
boards/px4/fmu-v5x/nuttx-config/scripts/script.ld
View File

@ -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*)

1
boards/px4/fmu-v5x/src/board_config.h
View File

@ -434,6 +434,7 @@
#define BOARD_NUM_IO_TIMERS 5
#define PX4_I2C_BUS_MTD 4,5
__BEGIN_DECLS

26
boards/px4/fmu-v5x/src/init.cpp
View File

@ -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;
}

3
boards/px4/fmu-v6u/default.cmake
View File

@ -28,7 +28,7 @@ px4_add_board(
gps
heater
#imu # all available imu drivers
imu/adis16448
imu/analog_devices/adis16448
imu/adis16477
imu/adis16497
imu/bosch/bmi088
@ -41,7 +41,6 @@ px4_add_board(
lights/rgbled
lights/rgbled_ncp5623c
magnetometer # all available magnetometer drivers
mkblctrl
optical_flow # all available optical flow drivers
#osd
pca9685

5
boards/px4/fmu-v6u/stackcheck.cmake
View File

@ -1,7 +1,7 @@
px4_add_board(
PLATFORM nuttx
VENDOR px4
VENDOR px4
MODEL fmu-v6u
LABEL default
TOOLCHAIN arm-none-eabi
@ -28,7 +28,7 @@ px4_add_board(
gps
heater
#imu # all available imu drivers
imu/adis16448
imu/analog_devices/adis16448
imu/adis16477
imu/adis16497
imu/bosch/bmi088
@ -41,7 +41,6 @@ px4_add_board(
lights/rgbled
lights/rgbled_ncp5623c
magnetometer # all available magnetometer drivers
mkblctrl
optical_flow # all available optical flow drivers
#osd
pca9685

3
boards/px4/fmu-v6x/default.cmake
View File

@ -29,7 +29,7 @@ px4_add_board(
gps
heater
#imu # all available imu drivers
imu/adis16448
imu/analog_devices/adis16448
imu/adis16477
imu/adis16497
imu/bosch/bmi088
@ -39,7 +39,6 @@ px4_add_board(
lights/rgbled
lights/rgbled_ncp5623c
magnetometer # all available magnetometer drivers
mkblctrl
optical_flow # all available optical flow drivers
#osd
pca9685

3
boards/px4/fmu-v6x/stackcheck.cmake
View File

@ -29,7 +29,7 @@ px4_add_board(
gps
heater
#imu # all available imu drivers
imu/adis16448
imu/analog_devices/adis16448
imu/adis16477
imu/adis16497
imu/bosch/bmi088
@ -39,7 +39,6 @@ px4_add_board(
lights/rgbled
lights/rgbled_ncp5623c
magnetometer # all available magnetometer drivers
mkblctrl
optical_flow # all available optical flow drivers
#osd
pca9685

1
boards/px4/sitl/ctrlalloc.cmake
View File

@ -57,6 +57,7 @@ px4_add_board(
simulator
temperature_compensation
uuv_att_control
uuv_pos_control
vmount
vtol_att_control
SYSTEMCMDS

1
boards/px4/sitl/default.cmake
View File

@ -55,6 +55,7 @@ px4_add_board(
simulator
temperature_compensation
uuv_att_control
uuv_pos_control
vmount
vtol_att_control
SYSTEMCMDS

1
boards/px4/sitl/nolockstep.cmake
View File

@ -55,6 +55,7 @@ px4_add_board(
simulator
temperature_compensation
uuv_att_control
uuv_pos_control
vmount
vtol_att_control
SYSTEMCMDS

1
boards/px4/sitl/rtps.cmake
View File

@ -55,6 +55,7 @@ px4_add_board(
simulator
temperature_compensation
uuv_att_control
uuv_pos_control
vmount
vtol_att_control
SYSTEMCMDS

1
boards/px4/sitl/test.cmake
View File

@ -54,6 +54,7 @@ px4_add_board(
simulator
temperature_compensation
uuv_att_control
uuv_pos_control
vmount
vtol_att_control
SYSTEMCMDS

3
boards/spracing/h7extreme/default.cmake
View File

@ -24,7 +24,7 @@ px4_add_board(
gps
#heater
#imu # all available imu drivers
#imu/adis16448
#imu/analog_devices/adis16448
#imu/adis16477
#imu/adis16497
#imu/bmi088
@ -36,7 +36,6 @@ px4_add_board(
lights/rgbled
lights/rgbled_ncp5623c
magnetometer # all available magnetometer drivers
#mkblctrl
optical_flow # all available optical flow drivers
osd
#pca9685

6
src/drivers/drv_io_heater.h
View File

@ -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 */

290
src/drivers/heater/heater.cpp
View File

@ -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<double>(_param_sens_imu_temp.get()),
static_cast<double>(_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<double>(_feedforward_value * 100),
static_cast<double>(_proportional_value * 100),
static_cast<double>(_integrator_value * 100),
static_cast<double>(static_cast<float>(_controller_time_on_usec) / static_cast<float>(_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<int>((_param_sens_imu_temp_ff.get() + _proportional_value +
_integrator_value) * static_cast<float>(_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
*/

57
src/drivers/heater/heater.h
View File

@ -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<px4::params::SENS_IMU_TEMP_FF>) _param_sens_imu_temp_ff,
(ParamFloat<px4::params::SENS_IMU_TEMP_I>) _param_sens_imu_temp_i,
(ParamFloat<px4::params::SENS_IMU_TEMP_P>) _param_sens_imu_temp_p,
(ParamInt<px4::params::SENS_TEMP_ID>) _param_sens_temp_id,
(ParamFloat<px4::params::SENS_IMU_TEMP>) _param_sens_imu_temp
(ParamInt<px4::params::SENS_TEMP_ID>) _param_sens_temp_id,
(ParamFloat<px4::params::SENS_IMU_TEMP>) _param_sens_imu_temp
)
};

12
src/drivers/heater/heater_params.c
View File

@ -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.
*

2
src/drivers/imu/CMakeLists.txt
View File

@ -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)

514
src/drivers/imu/adis16448/ADIS16448.cpp
View File

@ -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();
}

206
src/drivers/imu/adis16448/ADIS16448.h
View File

@ -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 <lm@inf.ethz.ch>
*/
#include <drivers/device/spi.h>
#include <ecl/geo/geo.h>
#include <lib/conversion/rotation.h>
#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
#include <lib/drivers/barometer/PX4Barometer.hpp>
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
#include <perf/perf_counter.h>
#include <px4_platform_common/i2c_spi_buses.h>
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<ADIS16448>
{
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"))};
};

34
src/drivers/imu/analog_devices/CMakeLists.txt Normal file
View File

@ -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)

594
src/drivers/imu/analog_devices/adis16448/ADIS16448.cpp Normal file
View File

@ -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<uint16_t>(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 &reg_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 &reg_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<ADIS16448 *>(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 &reg_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<uint16_t>(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<uint16_t>(reg)) | DIR_WRITE) << 8) | ((0x00FF & value));
cmd[1] = (((static_cast<uint16_t>(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);
}
}

138
src/drivers/imu/analog_devices/adis16448/ADIS16448.hpp Normal file
View File

@ -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 <drivers/drv_hrt.h>
#include <lib/drivers/device/spi.h>
#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
#include <lib/drivers/barometer/PX4Barometer.hpp>
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
#include <lib/ecl/geo/geo.h>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/atomic.h>
#include <px4_platform_common/i2c_spi_buses.h>
using namespace Analog_Devices_ADIS16448;
class ADIS16448 : public device::SPI, public I2CSPIDriver<ADIS16448>
{
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 &reg_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},
};
};

161
src/drivers/imu/analog_devices/adis16448/Analog_Devices_ADIS16448_registers.hpp Normal file
View File

@ -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 <cstdint>
// 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

12
src/drivers/imu/adis16448/CMakeLists.txt → src/drivers/imu/analog_devices/adis16448/CMakeLists.txt
View File

@ -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
)

17
src/drivers/imu/adis16448/adis16448_main.cpp → src/drivers/imu/analog_devices/adis16448/adis16448_main.cpp
View File

@ -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 <px4_platform_common/getopt.h>
#include <px4_platform_common/module.h>
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);
}

1397
src/drivers/mkblctrl/mkblctrl.cpp
View File

File diff suppressed because it is too large Load Diff

112
src/drivers/protocol_splitter/protocol_splitter.cpp
View File

@ -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);
}

2
src/lib/circuit_breaker/circuit_breaker_params.c
View File

@ -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

595
src/modules/commander/Commander.cpp
View File

@ -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<int>(cmd.param1 + 0.5f) != 0 && static_cast<int>(cmd.param1 + 0.5f) != 1) {
const int param1_arm = static_cast<int>(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<int>(cmd.param1 + 0.5f) == 1);
// Arm is forced (checks skipped) when param2 is set to a magic number.
const bool forced = (static_cast<int>(roundf(cmd.param2)) == 21196);
// Arm/disarm is enforced when param2 is set to a magic number.
const bool enforce = (static_cast<int>(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));

15
src/modules/commander/Commander.hpp
View File

@ -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

76
src/modules/ekf2/EKF2Selector.cpp
View File

@ -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) ? "*" : "");

8
src/modules/ekf2/EKF2Selector.hpp
View File

@ -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;
};

3
src/modules/flight_mode_manager/tasks/Utility/Sticks.cpp
View File

@ -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

Some files were not shown because too many files have changed in this diff Show More