forked from Archive/PX4-Autopilot
Compare commits
108 Commits
Author | SHA1 | Date |
---|---|---|
Silvan Fuhrer | b8c541dd72 | |
Beniamino Pozzan | 2d3238dfbe | |
Vincent Poon | ad36b9add7 | |
Julian Oes | 13cb8ce4a8 | |
Julian Oes | b4f425db56 | |
Beat Küng | e961b29c56 | |
Silvan Fuhrer | 6e65478959 | |
Silvan Fuhrer | 2f5aa92596 | |
Silvan Fuhrer | 080e16f0a6 | |
Thomas Stastny | 27e6a0d227 | |
Thomas Stastny | ff8663b0b1 | |
Thomas Stastny | 3e68e806b8 | |
Thomas Stastny | 39351acbcb | |
Daniel Agar | 2c9b739814 | |
Daniel Agar | 5e8ee98bf9 | |
Daniel Agar | d98f3554da | |
Thomas Stastny | 1f30b2168d | |
Silvan Fuhrer | 17177b3948 | |
Silvan Fuhrer | ccef260f10 | |
Thomas Stastny | 3c9c5f7184 | |
Thomas Stastny | 7316d50555 | |
Thomas Stastny | 97112a7d20 | |
Thomas Stastny | cd9fd12e91 | |
Thomas Stastny | a3f1f0e846 | |
Thomas Stastny | 37015c3dbe | |
Thomas Stastny | 7160da741d | |
Thomas Stastny | db48b04281 | |
Thomas Stastny | da8dcc4942 | |
Thomas Stastny | 7c2c288c09 | |
Thomas Stastny | 30870e50c0 | |
Julian Oes | 28b018f9bd | |
Julian Oes | 3896df3e72 | |
Julian Oes | 56fcabf0b1 | |
Julian Oes | 7b60737f9a | |
Julian Oes | 440165809a | |
Peter van der Perk | d9fd67be3b | |
alexklimaj | 3cf205c4a6 | |
alexklimaj | d54e488288 | |
Matthias Grob | 70081652fe | |
Daniel Agar | 61bfa4fdc5 | |
David Sidrane | 3c94a447d6 | |
Christian Rauch | 44805b9e62 | |
Christian Rauch | c5fc5c83d2 | |
Christian Rauch | ac1a157740 | |
Christian Rauch | 8af893348a | |
Christian Rauch | 251e24bccb | |
Christian Rauch | fc80b09c84 | |
Daniel Agar | 5359fe59e5 | |
Daniel Agar | 24603589d2 | |
Beat Küng | 4b70105583 | |
bresch | 61036599db | |
bresch | eb23779c57 | |
bresch | a07b8c08ab | |
alexklimaj | 36ec1fa811 | |
alexklimaj | 694501b782 | |
Eric Katzfey | 7dd8e3f614 | |
Eric Katzfey | fe335344e7 | |
mjturi-mai | 343c7fcd52 | |
Loic Fernau | 66df5c1bd1 | |
Ramon Roche | 090f929659 | |
alexklimaj | 89b238f094 | |
Matthias Grob | ecd1e9f730 | |
Junwoo Hwang | cca59843cc | |
Matthias Grob | 1806a7cec8 | |
Matthias Grob | 856b2e6178 | |
Matthias Grob | e0e5b38c0e | |
Matthias Grob | d0d113405a | |
Matthias Grob | 4ff03e4a06 | |
Matthias Grob | 8eb34ce8ce | |
Matthias Grob | 62077908a3 | |
Matthias Grob | cb2cc65eff | |
Matthias Grob | 4fc3f07cb1 | |
Matthias Grob | 9b092d6d35 | |
Vincent Poon | 21fb22d581 | |
Julian Oes | 3c1bcbdee6 | |
Julian Oes | d9b1a695b5 | |
Silvan Fuhrer | 8838ebd77d | |
Silvan Fuhrer | 7e0f0516a5 | |
Silvan Fuhrer | 5b3d19944c | |
Silvan Fuhrer | f2e706d7c4 | |
Julian Oes | 5a5849d61a | |
JaeyoungLim | 682190a21f | |
Alex Klimaj | e0a2e0b223 | |
Silvan Fuhrer | d9585bf3d3 | |
Matthias Grob | 2374937388 | |
Matthias Grob | ce8c4094a8 | |
Matthias Grob | 11436f4109 | |
Matthias Grob | db89bd5b5e | |
Matthias Grob | ae678e8e2f | |
Matthias Grob | 53bcb8789f | |
Matthias Grob | afa56d21c5 | |
Matthias Grob | 8cb7a67819 | |
Matthias Grob | c89f0776f6 | |
Julian Oes | b21e7e6c87 | |
Matthias Grob | ccb46a2152 | |
Matthias Grob | f34fbdf0d3 | |
Junwoo Hwang | 7cbf720d26 | |
Ramon Roche | 29a3abb817 | |
Silvan Fuhrer | 4e4ba40289 | |
Silvan Fuhrer | d5cbf5df01 | |
Silvan Fuhrer | 0d7933beac | |
Silvan Fuhrer | 19029526d0 | |
Silvan Fuhrer | b0712ef7a0 | |
Silvan Fuhrer | 71293b4943 | |
Silvan Fuhrer | fb1491de33 | |
Silvan Fuhrer | 487b9ca7ef | |
Roman Bapst | 5aa5fc2da5 | |
Silvan Fuhrer | 4b7da42a6e |
|
@ -40,6 +40,8 @@ pipeline {
|
|||
"ark_can-flow_default",
|
||||
"ark_can-gps_canbootloader",
|
||||
"ark_can-gps_default",
|
||||
"ark_cannode_canbootloader",
|
||||
"ark_cannode_default",
|
||||
"ark_can-rtk-gps_canbootloader",
|
||||
"ark_can-rtk-gps_default",
|
||||
"ark_fmu-v6x_bootloader",
|
||||
|
@ -53,8 +55,10 @@ pipeline {
|
|||
"cuav_nora_default",
|
||||
"cuav_x7pro_default",
|
||||
"cubepilot_cubeorange_default",
|
||||
"cubepilot_cubeorangeplus_default",
|
||||
"cubepilot_cubeyellow_default",
|
||||
"diatone_mamba-f405-mk2_default",
|
||||
"flywoo_gn-f405_default",
|
||||
"freefly_can-rtk-gps_canbootloader",
|
||||
"freefly_can-rtk-gps_default",
|
||||
"holybro_can-gps-v1_canbootloader",
|
||||
|
@ -72,7 +76,7 @@ pipeline {
|
|||
"matek_h743_default",
|
||||
"modalai_fc-v1_default",
|
||||
"modalai_fc-v2_default",
|
||||
"modalai_voxl2-io_default",
|
||||
"mro_ctrl-zero-classic_default",
|
||||
"mro_ctrl-zero-f7_default",
|
||||
"mro_ctrl-zero-f7-oem_default",
|
||||
"mro_ctrl-zero-h7-oem_default",
|
||||
|
@ -85,6 +89,7 @@ pipeline {
|
|||
"nxp_fmuk66-v3_default",
|
||||
"nxp_fmuk66-v3_socketcan",
|
||||
"nxp_fmurt1062-v1_default",
|
||||
"nxp_mr-canhubk3_default",
|
||||
"nxp_ucans32k146_canbootloader",
|
||||
"nxp_ucans32k146_default",
|
||||
"omnibus_f4sd_default",
|
||||
|
|
|
@ -23,7 +23,7 @@ For release notes:
|
|||
```
|
||||
Feature/Bugfix XYZ
|
||||
New parameter: XYZ_Z
|
||||
Documentation: Need to clarfiy page ... / done, read docs.px4.io/...
|
||||
Documentation: Need to clarify page ... / done, read docs.px4.io/...
|
||||
```
|
||||
|
||||
### Alternatives
|
||||
|
|
|
@ -21,7 +21,7 @@
|
|||
[submodule "platforms/nuttx/NuttX/nuttx"]
|
||||
path = platforms/nuttx/NuttX/nuttx
|
||||
url = https://github.com/PX4/NuttX.git
|
||||
branch = px4_firmware_nuttx-10.3.0+
|
||||
branch = px4_firmware_nuttx-10.3.0+-v1.14
|
||||
[submodule "platforms/nuttx/NuttX/apps"]
|
||||
path = platforms/nuttx/NuttX/apps
|
||||
url = https://github.com/PX4/NuttX-apps.git
|
||||
|
|
|
@ -17,6 +17,7 @@ param set-default SENS_EN_GPSSIM 1
|
|||
param set-default SENS_EN_BAROSIM 1
|
||||
param set-default SENS_EN_MAGSIM 1
|
||||
|
||||
param set-default VT_B_TRANS_DUR 5
|
||||
param set-default VT_ELEV_MC_LOCK 0
|
||||
param set-default VT_TYPE 0
|
||||
param set-default VT_FW_DIFTHR_EN 1
|
||||
|
|
|
@ -7,6 +7,7 @@
|
|||
|
||||
param set-default FW_LAUN_DETCN_ON 1
|
||||
param set-default FW_THR_IDLE 0.1 # needs to be running before throw as that's how gazebo detects arming
|
||||
param set-default FW_LAUN_AC_THLD 10
|
||||
|
||||
param set-default FW_LND_ANG 8
|
||||
|
||||
|
|
|
@ -48,35 +48,32 @@ param set-default PWM_MAIN_REV 96 # invert both elevons
|
|||
param set-default EKF2_MULTI_IMU 0
|
||||
param set-default SENS_IMU_MODE 1
|
||||
|
||||
param set-default NPFG_PERIOD 12
|
||||
param set-default FW_PR_I 0.2
|
||||
param set-default FW_PR_P 0.2
|
||||
param set-default FW_P_TC 0.6
|
||||
|
||||
param set-default FW_PR_FF 0.1
|
||||
param set-default FW_PSP_OFF 2
|
||||
param set-default FW_P_LIM_MIN -15
|
||||
param set-default FW_RR_P 0.2
|
||||
param set-default FW_THR_TRIM 0.33
|
||||
param set-default FW_THR_MAX 0.6
|
||||
param set-default FW_RR_FF 0.1
|
||||
param set-default FW_RR_I 0.2
|
||||
param set-default FW_RR_P 0.3
|
||||
param set-default FW_THR_TRIM 0.35
|
||||
param set-default FW_THR_MAX 0.8
|
||||
param set-default FW_THR_MIN 0.05
|
||||
param set-default FW_T_ALT_TC 2
|
||||
param set-default FW_T_CLMB_MAX 8
|
||||
param set-default FW_T_SINK_MAX 2.7
|
||||
param set-default FW_T_SINK_MIN 2.2
|
||||
param set-default FW_T_TAS_TC 2
|
||||
param set-default FW_T_CLMB_MAX 6
|
||||
param set-default FW_T_HRATE_FF 0.5
|
||||
param set-default FW_T_SINK_MAX 3
|
||||
param set-default FW_T_SINK_MIN 1.6
|
||||
|
||||
param set-default MC_AIRMODE 1
|
||||
param set-default MC_ROLL_P 3
|
||||
param set-default MC_PITCH_P 3
|
||||
param set-default MC_ROLLRATE_P 0.3
|
||||
param set-default MC_PITCHRATE_P 0.3
|
||||
|
||||
param set-default MPC_XY_P 0.8
|
||||
param set-default MPC_XY_VEL_P_ACC 3
|
||||
param set-default MPC_XY_VEL_I_ACC 4
|
||||
param set-default MPC_XY_VEL_D_ACC 0.1
|
||||
|
||||
param set-default NAV_ACC_RAD 5
|
||||
|
||||
param set-default VT_ARSP_TRANS 10
|
||||
param set-default VT_B_TRANS_DUR 5
|
||||
param set-default VT_FW_DIFTHR_EN 1
|
||||
param set-default VT_FW_DIFTHR_S_Y 0.5
|
||||
param set-default VT_FW_DIFTHR_S_Y 1
|
||||
param set-default VT_F_TRANS_DUR 1.5
|
||||
param set-default VT_F_TRANS_THR 0.7
|
||||
param set-default VT_TYPE 0
|
||||
|
||||
param set-default WV_EN 0
|
||||
|
|
|
@ -0,0 +1,74 @@
|
|||
#!/bin/sh
|
||||
#
|
||||
# @name Quadrotor + Tailsitter
|
||||
#
|
||||
# @type VTOL Quad Tailsitter
|
||||
#
|
||||
|
||||
. ${R}etc/init.d/rc.vtol_defaults
|
||||
|
||||
param set-default MAV_TYPE 20
|
||||
|
||||
param set-default CA_AIRFRAME 4
|
||||
|
||||
param set-default CA_ROTOR_COUNT 4
|
||||
param set-default CA_ROTOR0_PX 0.15
|
||||
param set-default CA_ROTOR0_PY 0.23
|
||||
param set-default CA_ROTOR0_KM 0.05
|
||||
param set-default CA_ROTOR1_PX -0.15
|
||||
param set-default CA_ROTOR1_PY -0.23
|
||||
param set-default CA_ROTOR1_KM 0.05
|
||||
param set-default CA_ROTOR2_PX 0.15
|
||||
param set-default CA_ROTOR2_PY -0.23
|
||||
param set-default CA_ROTOR2_KM -0.05
|
||||
param set-default CA_ROTOR3_PX -0.15
|
||||
param set-default CA_ROTOR3_PY 0.23
|
||||
param set-default CA_ROTOR3_KM -0.05
|
||||
|
||||
param set-default CA_SV_CS_COUNT 0
|
||||
|
||||
param set-default PWM_MAIN_FUNC1 101
|
||||
param set-default PWM_MAIN_FUNC2 102
|
||||
param set-default PWM_MAIN_FUNC3 103
|
||||
param set-default PWM_MAIN_FUNC4 104
|
||||
param set-default PWM_MAIN_FUNC5 0
|
||||
|
||||
parm set-default FD_FAIL_R 70
|
||||
|
||||
param set-default FW_P_TC 0.6
|
||||
|
||||
param set-default FW_PR_I 0.3
|
||||
param set-default FW_PR_P 0.5
|
||||
param set-default FW_PSP_OFF 2
|
||||
param set-default FW_RR_FF 0.1
|
||||
param set-default FW_RR_I 0.1
|
||||
param set-default FW_RR_P 0.2
|
||||
param set-default FW_YR_FF 0 # make yaw rate controller very weak, only keep default P
|
||||
param set-default FW_YR_I 0
|
||||
param set-default FW_THR_TRIM 0.35
|
||||
param set-default FW_THR_MAX 0.8
|
||||
param set-default FW_THR_MIN 0.05
|
||||
param set-default FW_T_CLMB_MAX 6
|
||||
param set-default FW_T_HRATE_FF 0.5
|
||||
param set-default FW_T_SINK_MAX 3
|
||||
param set-default FW_T_SINK_MIN 1.6
|
||||
param set-default FW_AIRSPD_STALL 10
|
||||
param set-default FW_AIRSPD_MIN 14
|
||||
param set-default FW_AIRSPD_TRIM 18
|
||||
param set-default FW_AIRSPD_MAX 22
|
||||
|
||||
param set-default MC_AIRMODE 2
|
||||
param set-default MAN_ARM_GESTURE 0 # required for yaw airmode
|
||||
param set-default MC_ROLL_P 3
|
||||
param set-default MC_PITCH_P 3
|
||||
param set-default MC_ROLLRATE_P 0.3
|
||||
param set-default MC_PITCHRATE_P 0.3
|
||||
|
||||
param set-default VT_ARSP_TRANS 15
|
||||
param set-default VT_B_TRANS_DUR 5
|
||||
param set-default VT_FW_DIFTHR_EN 7
|
||||
param set-default VT_FW_DIFTHR_S_Y 1
|
||||
param set-default VT_F_TRANS_DUR 1.5
|
||||
param set-default VT_TYPE 0
|
||||
|
||||
param set-default WV_EN 0
|
|
@ -60,6 +60,7 @@ px4_add_romfs_files(
|
|||
1042_gazebo-classic_tiltrotor
|
||||
1043_gazebo-classic_standard_vtol_drop
|
||||
1044_gazebo-classic_plane_lidar
|
||||
1045_gazebo-classic_quadtailsitter
|
||||
1060_gazebo-classic_rover
|
||||
1061_gazebo-classic_r1_rover
|
||||
1062_flightgear_tf-r1
|
||||
|
|
|
@ -20,6 +20,7 @@
|
|||
param set-default EKF2_FUSE_BETA 0 # side slip fusion is currently not supported for tailsitters
|
||||
|
||||
param set UAVCAN_ENABLE 0
|
||||
param set-default VT_B_TRANS_DUR 5
|
||||
param set-default VT_ELEV_MC_LOCK 0
|
||||
param set-default VT_MOT_COUNT 2
|
||||
param set-default VT_TYPE 0
|
||||
|
|
|
@ -31,3 +31,4 @@ param set-default CA_SV_CS1_TYPE 6
|
|||
param set-default MAV_TYPE 19
|
||||
param set-default VT_TYPE 0
|
||||
param set-default VT_ELEV_MC_LOCK 0
|
||||
param set-default VT_B_TRANS_DUR 5
|
||||
|
|
|
@ -34,9 +34,7 @@ param set-default CA_ROTOR3_PX -0.25
|
|||
param set-default CA_ROTOR3_PY 0.25
|
||||
param set-default CA_ROTOR3_KM -0.05
|
||||
|
||||
param set-default PWM_AUX_FUNC1 101
|
||||
param set-default PWM_AUX_FUNC2 102
|
||||
param set-default PWM_AUX_FUNC3 103
|
||||
param set-default PWM_AUX_FUNC4 104
|
||||
param set-default PWM_AUX_TIM0 -4
|
||||
|
||||
param set-default PWM_MAIN_FUNC1 101
|
||||
param set-default PWM_MAIN_FUNC2 102
|
||||
param set-default PWM_MAIN_FUNC3 103
|
||||
param set-default PWM_MAIN_FUNC4 104
|
||||
|
|
|
@ -254,6 +254,8 @@ else
|
|||
#
|
||||
rgbled start -X -q
|
||||
rgbled_ncp5623c start -X -q
|
||||
rgbled_lp5562 start -X -q
|
||||
rgbled_is31fl3195 start -X -q
|
||||
|
||||
#
|
||||
# Override parameters from user configuration file.
|
||||
|
|
|
@ -0,0 +1,115 @@
|
|||
#!/bin/bash
|
||||
# Flash PX4 to a device running AuterionOS in the local network
|
||||
if [ "$1" == "-h" ] || [ "$1" == "--help" ] || [ $# -lt 2 ]; then
|
||||
echo "Usage: $0 -f <firmware.px4|.elf> [-c <configuration_dir>] -d <IP/Device> [-u <user>] [-p <ssh_port>] [--revert]"
|
||||
exit 1
|
||||
fi
|
||||
|
||||
ssh_port=22
|
||||
ssh_user=root
|
||||
|
||||
while getopts ":f:c:d:p:u:r" opt; do
|
||||
case ${opt} in
|
||||
f )
|
||||
if [ -n "$OPTARG" ]; then
|
||||
firmware_file="$OPTARG"
|
||||
else
|
||||
echo "ERROR: -f requires a non-empty option argument."
|
||||
exit 1
|
||||
fi
|
||||
;;
|
||||
c )
|
||||
if [ -f "$OPTARG/rc.autostart" ]; then
|
||||
config_dir="$OPTARG"
|
||||
else
|
||||
echo "ERROR: -c configuration directory is empty or does not contain a valid rc.autostart"
|
||||
exit 1
|
||||
fi
|
||||
;;
|
||||
d )
|
||||
if [ "$OPTARG" ]; then
|
||||
device="$OPTARG"
|
||||
else
|
||||
echo "ERROR: -d requires a non-empty option argument."
|
||||
exit 1
|
||||
fi
|
||||
;;
|
||||
p )
|
||||
if [[ "$OPTARG" =~ ^[0-9]+$ ]]; then
|
||||
ssh_port="$OPTARG"
|
||||
else
|
||||
echo "ERROR: -p ssh_port must be a number."
|
||||
exit 1
|
||||
fi
|
||||
;;
|
||||
u )
|
||||
if [ "$OPTARG" ]; then
|
||||
ssh_user="$OPTARG"
|
||||
else
|
||||
echo "ERROR: -u requires a non-empty option argument."
|
||||
exit 1
|
||||
fi
|
||||
;;
|
||||
r )
|
||||
revert=true
|
||||
;;
|
||||
esac
|
||||
done
|
||||
|
||||
if [ -z "$device" ]; then
|
||||
echo "Error: missing device"
|
||||
exit 1
|
||||
fi
|
||||
|
||||
target_dir=/shared_container_dir/fmu
|
||||
target_file_name="update-dev.tar"
|
||||
|
||||
if [ "$revert" == true ]; then
|
||||
# revert to the release version which was originally deployed
|
||||
cmd="cp $target_dir/update.tar $target_dir/$target_file_name"
|
||||
ssh -t -p $ssh_port $ssh_user@$device "$cmd"
|
||||
else
|
||||
# create custom update-dev.tar
|
||||
tmp_dir="$(mktemp -d)"
|
||||
config_path=""
|
||||
firmware_path=""
|
||||
|
||||
if [ -d "$config_dir" ]; then
|
||||
cp -r "$config_dir" "$tmp_dir/config"
|
||||
config_path=config
|
||||
fi
|
||||
|
||||
if [ -f "$firmware_file" ]; then
|
||||
extension="${firmware_file##*.}"
|
||||
cp "$firmware_file" "$tmp_dir/firmware.$extension"
|
||||
if [ "$extension" == "elf" ]; then
|
||||
# ensure the file is stripped to reduce file size
|
||||
arm-none-eabi-strip "$tmp_dir/firmware.$extension"
|
||||
fi
|
||||
firmware_path="firmware.$extension"
|
||||
fi
|
||||
|
||||
pushd "$tmp_dir" &>/dev/null
|
||||
|
||||
if [ -z $firmware_path ] && [ -z $config_path ]; then
|
||||
exit 1
|
||||
fi
|
||||
|
||||
tar_name="tar"
|
||||
|
||||
if [ -x "$(command -v gtar)" ]; then
|
||||
# check if gnu-tar is installed on macOS and use that instead
|
||||
tar_name="gtar"
|
||||
fi
|
||||
|
||||
$tar_name -C "$tmp_dir" --sort=name --owner=root:0 --group=root:0 --mtime='2019-01-01 00:00:00' -cvf $target_file_name $firmware_path $config_path
|
||||
|
||||
# send it to the target to start flashing
|
||||
scp -P $ssh_port "$target_file_name" $ssh_user@"$device":$target_dir
|
||||
popd &>/dev/null
|
||||
rm -rf "$tmp_dir"
|
||||
fi
|
||||
|
||||
# grab status output for flashing progress
|
||||
cmd="tail --follow=name $target_dir/update_status 2>/dev/null || true"
|
||||
ssh -t -p $ssh_port $ssh_user@$device "$cmd"
|
|
@ -0,0 +1,51 @@
|
|||
#!/usr/bin/env bash
|
||||
|
||||
DIR="$(dirname $(readlink -f $0))"
|
||||
DEFAULT_AUTOPILOT_HOST=10.41.1.1
|
||||
DEFAULT_AUTOPILOT_PORT=33333
|
||||
DEFAULT_AUTOPILOT_USER=auterion
|
||||
|
||||
for i in "$@"
|
||||
do
|
||||
case $i in
|
||||
--file=*)
|
||||
PX4_BINARY_FILE="${i#*=}"
|
||||
;;
|
||||
--default-ip=*)
|
||||
DEFAULT_AUTOPILOT_HOST="${i#*=}"
|
||||
;;
|
||||
--default-port=*)
|
||||
DEFAULT_AUTOPILOT_PORT="${i#*=}"
|
||||
;;
|
||||
--default-user=*)
|
||||
DEFAULT_AUTOPILOT_USER="${i#*=}"
|
||||
;;
|
||||
--revert)
|
||||
REVERT_AUTOPILOT_ARGUMENT=-r
|
||||
;;
|
||||
--wifi)
|
||||
DEFAULT_AUTOPILOT_HOST=10.41.0.1
|
||||
;;
|
||||
*)
|
||||
# unknown option
|
||||
;;
|
||||
esac
|
||||
done
|
||||
|
||||
# allow these to be overridden
|
||||
[ -z "$AUTOPILOT_HOST" ] && AUTOPILOT_HOST=$DEFAULT_AUTOPILOT_HOST
|
||||
[ -z "$AUTOPILOT_PORT" ] && AUTOPILOT_PORT=$DEFAULT_AUTOPILOT_PORT
|
||||
[ -z "$AUTOPILOT_USER" ] && AUTOPILOT_USER=$DEFAULT_AUTOPILOT_USER
|
||||
|
||||
ARGUMENTS=()
|
||||
ARGUMENTS+=(-d "$AUTOPILOT_HOST")
|
||||
ARGUMENTS+=(-p "$AUTOPILOT_PORT")
|
||||
ARGUMENTS+=(-u "$AUTOPILOT_USER")
|
||||
ARGUMENTS+=(${PX4_BINARY_FILE:+-f "$PX4_BINARY_FILE"})
|
||||
ARGUMENTS+=($REVERT_AUTOPILOT_ARGUMENT)
|
||||
|
||||
echo "Flashing $AUTOPILOT_HOST ..."
|
||||
|
||||
"$DIR"/remote_update_fmu.sh "${ARGUMENTS[@]}"
|
||||
|
||||
exit 0
|
|
@ -20,6 +20,7 @@ CONFIG_DRIVERS_HEATER=y
|
|||
CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16507=y
|
||||
CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
|
||||
CONFIG_DRIVERS_IMU_INVENSENSE_IIM42652=y
|
||||
CONFIG_DRIVERS_IMU_INVENSENSE_IIM42653=y
|
||||
CONFIG_COMMON_LIGHT=y
|
||||
CONFIG_COMMON_MAGNETOMETER=y
|
||||
CONFIG_COMMON_OPTICAL_FLOW=y
|
||||
|
|
|
@ -17,12 +17,21 @@ param set-default SENS_EN_INA228 0
|
|||
param set-default SENS_EN_INA226 1
|
||||
|
||||
param set-default SENS_EN_THERMAL 1
|
||||
param set-default SENS_TEMP_ID 2818058
|
||||
param set-default SENS_IMU_TEMP 10.0
|
||||
#param set-default SENS_IMU_TEMP_FF 0.0
|
||||
#param set-default SENS_IMU_TEMP_I 0.025
|
||||
#param set-default SENS_IMU_TEMP_P 1.0
|
||||
|
||||
if ver hwtypecmp ARKV6X000000 ARKV6X001000 ARKV6X002000 ARKV6X003000 ARKV6X004000 ARKV6X005000 ARKV6X006000 ARKV6X007000
|
||||
then
|
||||
param set-default SENS_TEMP_ID 2818058
|
||||
fi
|
||||
|
||||
if ver hwtypecmp ARKV6X000001 ARKV6X001001 ARKV6X002001 ARKV6X003001 ARKV6X004001 ARKV6X005001 ARKV6X006001 ARKV6X007001
|
||||
then
|
||||
param set-default SENS_TEMP_ID 3014666
|
||||
fi
|
||||
|
||||
if ver hwtypecmp ARKV6X001000 ARKV6X001001 ARKV6X001002 ARKV6X001003 ARKV6X001004 ARKV6X001005 ARKV6X001006 ARKV6X001007
|
||||
then
|
||||
param set-default SYS_USE_IO 0
|
||||
|
|
|
@ -47,14 +47,29 @@ then
|
|||
fi
|
||||
fi
|
||||
|
||||
# Internal SPI bus IIM42652 with SPIX measured frequency of 32.051kHz
|
||||
iim42652 -R 3 -s -b 1 -C 32051 start
|
||||
if ver hwtypecmp ARKV6X000000 ARKV6X001000 ARKV6X002000 ARKV6X003000 ARKV6X004000 ARKV6X005000 ARKV6X006000 ARKV6X007000
|
||||
then
|
||||
# Internal SPI bus IIM42652 with SPIX measured frequency of 32.051kHz
|
||||
iim42652 -R 3 -s -b 1 -C 32051 start
|
||||
|
||||
# Internal SPI bus ICM42688p with SPIX measured frequency of 32.051kHz
|
||||
icm42688p -R 9 -s -b 2 -C 32051 start
|
||||
# Internal SPI bus ICM42688p with SPIX measured frequency of 32.051kHz
|
||||
icm42688p -R 9 -s -b 2 -C 32051 start
|
||||
|
||||
# Internal SPI bus ICM42688p with SPIX measured frequency of 32.051kHz
|
||||
icm42688p -R 6 -s -b 3 -C 32051 start
|
||||
# Internal SPI bus ICM42688p with SPIX measured frequency of 32.051kHz
|
||||
icm42688p -R 6 -s -b 3 -C 32051 start
|
||||
fi
|
||||
|
||||
if ver hwtypecmp ARKV6X000001 ARKV6X001001 ARKV6X002001 ARKV6X003001 ARKV6X004001 ARKV6X005001 ARKV6X006001 ARKV6X007001
|
||||
then
|
||||
# Internal SPI bus IIM42653 with SPIX measured frequency of 32.051kHz
|
||||
iim42653 -R 3 -s -b 1 -C 32051 start
|
||||
|
||||
# Internal SPI bus IIM42653 with SPIX measured frequency of 32.051kHz
|
||||
iim42653 -R 9 -s -b 2 -C 32051 start
|
||||
|
||||
# Internal SPI bus IIM42653 with SPIX measured frequency of 32.051kHz
|
||||
iim42653 -R 6 -s -b 3 -C 32051 start
|
||||
fi
|
||||
|
||||
# Internal magnetometer on I2C
|
||||
bmm150 -I start
|
||||
|
|
|
@ -211,23 +211,18 @@
|
|||
#define GPIO_HW_VER_SENSE /* PH3 */ GPIO_ADC3_INP14
|
||||
#define HW_INFO_INIT_PREFIX "ARKV6X"
|
||||
|
||||
#define BOARD_NUM_SPI_CFG_HW_VERSIONS 2 // Rev 0 and Rev 3,4 Sensor sets
|
||||
#define BOARD_NUM_SPI_CFG_HW_VERSIONS 8 // Rev 0 and Rev 1
|
||||
// Base/FMUM
|
||||
#define ARKV6X00 HW_VER_REV(0x0,0x0) // ARKV6X, Rev 0
|
||||
#define ARKV6X01 HW_VER_REV(0x0,0x1) // ARKV6X, BMI388 I2C2 Rev 1
|
||||
#define ARKV6X03 HW_VER_REV(0x0,0x3) // ARKV6X, Sensor Set Rev 3
|
||||
#define ARKV6X04 HW_VER_REV(0x0,0x4) // ARKV6X, Sensor Set Rev 4
|
||||
#define ARKV6X10 HW_VER_REV(0x1,0x0) // NO PX4IO, Rev 0
|
||||
#define ARKV6X13 HW_VER_REV(0x1,0x3) // NO PX4IO, Sensor Set Rev 3
|
||||
#define ARKV6X14 HW_VER_REV(0x1,0x4) // NO PX4IO, Sensor Set Rev 4
|
||||
//#define ARKV6X40 HW_VER_REV(0x4,0x0) // ARKV6X, HB CM4 base Rev 0 // never shipped
|
||||
//#define ARKV6X41 HW_VER_REV(0x4,0x1) // ARKV6X, BMI388 I2C2 HB CM4 base Rev 1 // never shipped
|
||||
#define ARKV6X43 HW_VER_REV(0x4,0x3) // ARKV6X, Sensor Set HB CM4 base Rev 3
|
||||
#define ARKV6X44 HW_VER_REV(0x4,0x4) // ARKV6X, Sensor Set HB CM4 base Rev 4
|
||||
#define ARKV6X50 HW_VER_REV(0x5,0x0) // ARKV6X, ARKV6X Rev 0 with HB Mini Rev 5
|
||||
//#define ARKV6X51 HW_VER_REV(0x5,0x1) // ARKV6X, BMI388 I2C2 HB Mini Rev 1 // never shipped
|
||||
#define ARKV6X53 HW_VER_REV(0x5,0x3) // ARKV6X, Sensor Set HB Mini Rev 3
|
||||
#define ARKV6X54 HW_VER_REV(0x5,0x4) // ARKV6X, Sensor Set HB Mini Rev 4
|
||||
#define ARKV6X00 HW_VER_REV(0x0,0x0) // ARKV6X, Sensor Set Rev 0
|
||||
#define ARKV6X01 HW_VER_REV(0x0,0x1) // ARKV6X, Sensor Set Rev 1
|
||||
//#define ARKV6X03 HW_VER_REV(0x0,0x3) // ARKV6X, Sensor Set Rev 3
|
||||
//#define ARKV6X04 HW_VER_REV(0x0,0x4) // ARKV6X, Sensor Set Rev 4
|
||||
#define ARKV6X10 HW_VER_REV(0x1,0x0) // NO PX4IO, Sensor Set Rev 0
|
||||
#define ARKV6X11 HW_VER_REV(0x1,0x1) // NO PX4IO, Sensor Set Rev 1
|
||||
#define ARKV6X40 HW_VER_REV(0x4,0x0) // ARKV6X, Sensor Set Rev 0 HB CM4 base Rev 3
|
||||
#define ARKV6X41 HW_VER_REV(0x4,0x1) // ARKV6X, Sensor Set Rev 1 HB CM4 base Rev 4
|
||||
#define ARKV6X50 HW_VER_REV(0x5,0x0) // ARKV6X, Sensor Set Rev 0 HB Mini Rev 5
|
||||
#define ARKV6X51 HW_VER_REV(0x5,0x1) // ARKV6X, Sensor Set Rev 1 HB Mini Rev 1 // never shipped
|
||||
|
||||
#define UAVCAN_NUM_IFACES_RUNTIME 1
|
||||
|
||||
|
|
|
@ -160,21 +160,14 @@ static const px4_hw_mft_item_t hw_mft_list_v0650[] = {
|
|||
|
||||
static px4_hw_mft_list_entry_t mft_lists[] = {
|
||||
// ver_rev
|
||||
{ARKV6X00, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)},
|
||||
{ARKV6X01, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2
|
||||
{ARKV6X03, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2, Sensor Set 3
|
||||
//{ARKV6X40, hw_mft_list_v0640, arraySize(hw_mft_list_v0640)}, // HB CM4 base // never shipped
|
||||
//{ARKV6X41, hw_mft_list_v0640, arraySize(hw_mft_list_v0640)}, // BMP388 moved to I2C2 HB CM4 base // never shipped
|
||||
{ARKV6X43, hw_mft_list_v0640, arraySize(hw_mft_list_v0640)}, // BMP388 moved to I2C2, HB CM4 base Sensor Set 3
|
||||
{ARKV6X44, hw_mft_list_v0640, arraySize(hw_mft_list_v0640)}, // BMP388 moved to I2C2, HB CM4 base Sensor Set 4
|
||||
{ARKV6X50, hw_mft_list_v0650, arraySize(hw_mft_list_v0650)}, // ARKV6X Rev 0 with HB Mini Rev 5
|
||||
//{ARKV6X51, hw_mft_list_v0650, arraySize(hw_mft_list_v0650)}, // BMP388 moved to I2C2 HB Mini // never shipped
|
||||
{ARKV6X53, hw_mft_list_v0650, arraySize(hw_mft_list_v0650)}, // BMP388 moved to I2C2, HB Mini Sensor Set 3
|
||||
{ARKV6X54, hw_mft_list_v0650, arraySize(hw_mft_list_v0650)}, // BMP388 moved to I2C2, HB Mini Sensor Set 4
|
||||
{ARKV6X10, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // No PX4IO
|
||||
{ARKV6X13, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // No PX4IO BMP388 moved to I2C2, Sensor Set 3
|
||||
{ARKV6X04, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2, Sensor Set 4
|
||||
{ARKV6X14, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // No PX4IO BMP388 moved to I2C2, Sensor Set 4
|
||||
{ARKV6X00, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // ARKV6X, Sensor Set Rev 0
|
||||
{ARKV6X01, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // ARKV6X, Sensor Set Rev 1
|
||||
{ARKV6X10, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // NO PX4IO, Sensor Set Rev 0
|
||||
{ARKV6X11, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // NO PX4IO, Sensor Set Rev 1
|
||||
{ARKV6X40, hw_mft_list_v0640, arraySize(hw_mft_list_v0640)}, // ARKV6X, Sensor Set Rev 0 HB CM4 base Rev 3
|
||||
{ARKV6X41, hw_mft_list_v0640, arraySize(hw_mft_list_v0640)}, // ARKV6X, Sensor Set Rev 1 HB CM4 base Rev 4
|
||||
{ARKV6X50, hw_mft_list_v0650, arraySize(hw_mft_list_v0650)}, // ARKV6X, Sensor Set Rev 0 HB Mini Rev 5
|
||||
{ARKV6X51, hw_mft_list_v0650, arraySize(hw_mft_list_v0650)}, // ARKV6X, Sensor Set Rev 1 HB Mini Rev 1 // never shipped
|
||||
};
|
||||
|
||||
/************************************************************************************
|
||||
|
|
|
@ -59,7 +59,30 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION
|
|||
}),
|
||||
}),
|
||||
|
||||
initSPIHWVersion(ARKV6X01, { // Placeholder
|
||||
initSPIHWVersion(ARKV6X01, {
|
||||
initSPIBus(SPI::Bus::SPI1, {
|
||||
initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}),
|
||||
}, {GPIO::PortI, GPIO::Pin11}),
|
||||
initSPIBus(SPI::Bus::SPI2, {
|
||||
initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}),
|
||||
}, {GPIO::PortF, GPIO::Pin4}),
|
||||
initSPIBus(SPI::Bus::SPI3, {
|
||||
initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin6}),
|
||||
}, {GPIO::PortE, GPIO::Pin7}),
|
||||
// initSPIBus(SPI::Bus::SPI4, {
|
||||
// // no devices
|
||||
// TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h
|
||||
// }, {GPIO::PortG, GPIO::Pin8}),
|
||||
initSPIBus(SPI::Bus::SPI5, {
|
||||
initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7})
|
||||
}),
|
||||
initSPIBusExternal(SPI::Bus::SPI6, {
|
||||
initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}),
|
||||
initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}),
|
||||
}),
|
||||
}),
|
||||
|
||||
initSPIHWVersion(ARKV6X10, {
|
||||
initSPIBus(SPI::Bus::SPI1, {
|
||||
initSPIDevice(DRV_IMU_DEVTYPE_IIM42652, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}),
|
||||
}, {GPIO::PortI, GPIO::Pin11}),
|
||||
|
@ -81,6 +104,121 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION
|
|||
initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}),
|
||||
}),
|
||||
}),
|
||||
|
||||
initSPIHWVersion(ARKV6X11, {
|
||||
initSPIBus(SPI::Bus::SPI1, {
|
||||
initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}),
|
||||
}, {GPIO::PortI, GPIO::Pin11}),
|
||||
initSPIBus(SPI::Bus::SPI2, {
|
||||
initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}),
|
||||
}, {GPIO::PortF, GPIO::Pin4}),
|
||||
initSPIBus(SPI::Bus::SPI3, {
|
||||
initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin6}),
|
||||
}, {GPIO::PortE, GPIO::Pin7}),
|
||||
// initSPIBus(SPI::Bus::SPI4, {
|
||||
// // no devices
|
||||
// TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h
|
||||
// }, {GPIO::PortG, GPIO::Pin8}),
|
||||
initSPIBus(SPI::Bus::SPI5, {
|
||||
initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7})
|
||||
}),
|
||||
initSPIBusExternal(SPI::Bus::SPI6, {
|
||||
initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}),
|
||||
initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}),
|
||||
}),
|
||||
}),
|
||||
|
||||
initSPIHWVersion(ARKV6X40, {
|
||||
initSPIBus(SPI::Bus::SPI1, {
|
||||
initSPIDevice(DRV_IMU_DEVTYPE_IIM42652, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}),
|
||||
}, {GPIO::PortI, GPIO::Pin11}),
|
||||
initSPIBus(SPI::Bus::SPI2, {
|
||||
initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}),
|
||||
}, {GPIO::PortF, GPIO::Pin4}),
|
||||
initSPIBus(SPI::Bus::SPI3, {
|
||||
initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin6}),
|
||||
}, {GPIO::PortE, GPIO::Pin7}),
|
||||
// initSPIBus(SPI::Bus::SPI4, {
|
||||
// // no devices
|
||||
// TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h
|
||||
// }, {GPIO::PortG, GPIO::Pin8}),
|
||||
initSPIBus(SPI::Bus::SPI5, {
|
||||
initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7})
|
||||
}),
|
||||
initSPIBusExternal(SPI::Bus::SPI6, {
|
||||
initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}),
|
||||
initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}),
|
||||
}),
|
||||
}),
|
||||
|
||||
initSPIHWVersion(ARKV6X41, {
|
||||
initSPIBus(SPI::Bus::SPI1, {
|
||||
initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}),
|
||||
}, {GPIO::PortI, GPIO::Pin11}),
|
||||
initSPIBus(SPI::Bus::SPI2, {
|
||||
initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}),
|
||||
}, {GPIO::PortF, GPIO::Pin4}),
|
||||
initSPIBus(SPI::Bus::SPI3, {
|
||||
initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin6}),
|
||||
}, {GPIO::PortE, GPIO::Pin7}),
|
||||
// initSPIBus(SPI::Bus::SPI4, {
|
||||
// // no devices
|
||||
// TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h
|
||||
// }, {GPIO::PortG, GPIO::Pin8}),
|
||||
initSPIBus(SPI::Bus::SPI5, {
|
||||
initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7})
|
||||
}),
|
||||
initSPIBusExternal(SPI::Bus::SPI6, {
|
||||
initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}),
|
||||
initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}),
|
||||
}),
|
||||
}),
|
||||
|
||||
initSPIHWVersion(ARKV6X50, {
|
||||
initSPIBus(SPI::Bus::SPI1, {
|
||||
initSPIDevice(DRV_IMU_DEVTYPE_IIM42652, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}),
|
||||
}, {GPIO::PortI, GPIO::Pin11}),
|
||||
initSPIBus(SPI::Bus::SPI2, {
|
||||
initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}),
|
||||
}, {GPIO::PortF, GPIO::Pin4}),
|
||||
initSPIBus(SPI::Bus::SPI3, {
|
||||
initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin6}),
|
||||
}, {GPIO::PortE, GPIO::Pin7}),
|
||||
// initSPIBus(SPI::Bus::SPI4, {
|
||||
// // no devices
|
||||
// TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h
|
||||
// }, {GPIO::PortG, GPIO::Pin8}),
|
||||
initSPIBus(SPI::Bus::SPI5, {
|
||||
initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7})
|
||||
}),
|
||||
initSPIBusExternal(SPI::Bus::SPI6, {
|
||||
initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}),
|
||||
initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}),
|
||||
}),
|
||||
}),
|
||||
|
||||
initSPIHWVersion(ARKV6X51, {
|
||||
initSPIBus(SPI::Bus::SPI1, {
|
||||
initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}),
|
||||
}, {GPIO::PortI, GPIO::Pin11}),
|
||||
initSPIBus(SPI::Bus::SPI2, {
|
||||
initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}),
|
||||
}, {GPIO::PortF, GPIO::Pin4}),
|
||||
initSPIBus(SPI::Bus::SPI3, {
|
||||
initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin6}),
|
||||
}, {GPIO::PortE, GPIO::Pin7}),
|
||||
// initSPIBus(SPI::Bus::SPI4, {
|
||||
// // no devices
|
||||
// TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h
|
||||
// }, {GPIO::PortG, GPIO::Pin8}),
|
||||
initSPIBus(SPI::Bus::SPI5, {
|
||||
initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7})
|
||||
}),
|
||||
initSPIBusExternal(SPI::Bus::SPI6, {
|
||||
initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}),
|
||||
initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}),
|
||||
}),
|
||||
}),
|
||||
};
|
||||
|
||||
static constexpr bool unused = validateSPIConfig(px4_spi_buses_all_hw);
|
||||
|
|
|
@ -140,15 +140,7 @@
|
|||
|
||||
#define PX4_PWM_ALTERNATE_RANGES
|
||||
#define PWM_LOWEST_MIN 0
|
||||
#define PWM_MOTOR_OFF 0
|
||||
#define PWM_SERVO_STOP 0
|
||||
#define PWM_DEFAULT_MIN 20
|
||||
#define PWM_HIGHEST_MIN 0
|
||||
#define PWM_HIGHEST_MAX 255
|
||||
#define PWM_DEFAULT_MAX 255
|
||||
#define PWM_LOWEST_MAX 255
|
||||
#define PWM_DEFAULT_TRIM 1500
|
||||
|
||||
|
||||
/* High-resolution timer */
|
||||
#define HRT_TIMER 8 /* use timer8 for the HRT */
|
||||
|
|
|
@ -141,15 +141,7 @@
|
|||
|
||||
#define PX4_PWM_ALTERNATE_RANGES
|
||||
#define PWM_LOWEST_MIN 0
|
||||
#define PWM_MOTOR_OFF 0
|
||||
#define PWM_SERVO_STOP 0
|
||||
#define PWM_DEFAULT_MIN 20
|
||||
#define PWM_HIGHEST_MIN 0
|
||||
#define PWM_HIGHEST_MAX 255
|
||||
#define PWM_DEFAULT_MAX 255
|
||||
#define PWM_LOWEST_MAX 255
|
||||
#define PWM_DEFAULT_TRIM 1500
|
||||
|
||||
|
||||
/* High-resolution timer */
|
||||
#define HRT_TIMER 8 /* use timer8 for the HRT */
|
||||
|
|
|
@ -8,16 +8,22 @@ board_adc start
|
|||
# 1. Isolated {ICM42688p, ICM20948(with mag)}, body-fixed {ICM20649}
|
||||
# 2. Isolated {ICM42688p, ICM42688p}, body-fixed {ICM20649, ICM45686, AK09918}
|
||||
# 3. Isolated {ICM42688p, ICM42688p}, body-fixed {ICM45686, AK09918}
|
||||
# 4. Isolated {ICM45686, ICM45686}, body-fixed {ICM45686, AK09918}
|
||||
|
||||
# SPI4 is isolated, SPI1 is body-fixed
|
||||
|
||||
# SPI4, isolated
|
||||
ms5611 -s -b 4 start
|
||||
|
||||
icm42688p -s -b 4 -R 10 start -c 15
|
||||
if ! icm20948 -s -b 4 -R 10 -M -q start
|
||||
if icm42688p -s -b 4 -R 10 -q start -c 15
|
||||
then
|
||||
icm42688p -s -b 4 -R 6 start -c 13
|
||||
if ! icm20948 -s -b 4 -R 10 -M -q start
|
||||
then
|
||||
icm42688p -s -b 4 -R 6 start -c 13
|
||||
fi
|
||||
else
|
||||
icm45686 -s -b 4 -R 10 start -c 15
|
||||
icm45686 -s -b 4 -R 6 start -c 13
|
||||
fi
|
||||
|
||||
# SPI1, body-fixed
|
||||
|
|
|
@ -49,7 +49,9 @@ constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = {
|
|||
initSPIBus(SPI::Bus::SPI4, {
|
||||
initSPIDevice(DRV_IMU_DEVTYPE_ICM20948, SPI::CS{GPIO::PortE, GPIO::Pin4}), // MPU_EXT_CS
|
||||
initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortC, GPIO::Pin15}), // ACCEL_EXT_CS
|
||||
initSPIDevice(DRV_IMU_DEVTYPE_ICM45686, SPI::CS{GPIO::PortC, GPIO::Pin15}), // ACCEL_EXT_CS
|
||||
initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortC, GPIO::Pin13}), // GYRO_EXT_CS
|
||||
initSPIDevice(DRV_IMU_DEVTYPE_ICM45686, SPI::CS{GPIO::PortC, GPIO::Pin13}), // GYRO_EXT_CS
|
||||
initSPIDevice(DRV_BARO_DEVTYPE_MS5611, SPI::CS{GPIO::PortC, GPIO::Pin14}), // BARO_EXT_CS
|
||||
}),
|
||||
};
|
||||
|
|
|
@ -19,6 +19,7 @@ CONFIG_DRIVERS_IMU_BOSCH_BMI088=y
|
|||
CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y
|
||||
CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
|
||||
CONFIG_DRIVERS_IRLOCK=y
|
||||
CONFIG_DRIVERS_UWB_UWB_SR150=y
|
||||
CONFIG_COMMON_LIGHT=y
|
||||
CONFIG_DRIVERS_LIGHTS_RGBLED_PWM=y
|
||||
CONFIG_COMMON_MAGNETOMETER=y
|
||||
|
|
|
@ -22,7 +22,7 @@ CONFIG_DRIVERS_PWM_OUT=y
|
|||
CONFIG_DRIVERS_RC_INPUT=y
|
||||
CONFIG_DRIVERS_SAFETY_BUTTON=y
|
||||
CONFIG_DRIVERS_TONE_ALARM=y
|
||||
CONFIG_COMMON_UWB=y
|
||||
CONFIG_DRIVERS_UWB_UWB_SR150=y
|
||||
CONFIG_MODULES_BATTERY_STATUS=y
|
||||
CONFIG_MODULES_CAMERA_FEEDBACK=y
|
||||
CONFIG_MODULES_COMMANDER=y
|
||||
|
|
|
@ -1,20 +1,25 @@
|
|||
# CONFIG_BOARD_ROMFSROOT is not set
|
||||
CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS1"
|
||||
CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS4"
|
||||
CONFIG_BOARD_SERIAL_RC="/dev/ttyS5"
|
||||
CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS2"
|
||||
CONFIG_BOARD_SERIAL_TEL4="/dev/ttyS3"
|
||||
CONFIG_BOARD_UAVCAN_INTERFACES=1
|
||||
CONFIG_COMMON_LIGHT=y
|
||||
CONFIG_CYPHAL_BMS_SUBSCRIBER=y
|
||||
CONFIG_DRIVERS_ADC_BOARD_ADC=y
|
||||
CONFIG_DRIVERS_CYPHAL=y
|
||||
CONFIG_DRIVERS_GPS=y
|
||||
CONFIG_DRIVERS_IRLOCK=y
|
||||
CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y
|
||||
CONFIG_DRIVERS_MAGNETOMETER_LIS3MDL=y
|
||||
CONFIG_DRIVERS_RC_INPUT=y
|
||||
CONFIG_DRIVERS_SAFETY_BUTTON=y
|
||||
CONFIG_DRIVERS_UAVCAN=y
|
||||
CONFIG_EXAMPLES_FAKE_GPS=y
|
||||
CONFIG_MODULES_AIRSPEED_SELECTOR=y
|
||||
CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
|
||||
CONFIG_MODULES_BATTERY_STATUS=y
|
||||
CONFIG_MODULES_CAMERA_FEEDBACK=y
|
||||
CONFIG_MODULES_COMMANDER=y
|
||||
CONFIG_MODULES_ESC_BATTERY=y
|
||||
|
|
|
@ -17,6 +17,17 @@ param set-default MAV_1_UDP_PRT 14550
|
|||
param set-default SENS_EXT_I2C_PRB 0
|
||||
param set-default CYPHAL_ENABLE 0
|
||||
|
||||
if ver hwtypecmp MR-CANHUBK3-ADAP
|
||||
then
|
||||
param set-default GPS_1_CONFIG 202
|
||||
param set-default RC_PORT_CONFIG 104
|
||||
param set-default SENS_INT_BARO_EN 0
|
||||
param set-default SYS_HAS_BARO 0
|
||||
# MR-CANHUBK3-ADAP voltage divider
|
||||
param set-default BAT1_V_DIV 13.158
|
||||
safety_button start
|
||||
fi
|
||||
|
||||
if param greater -s UAVCAN_ENABLE 0
|
||||
then
|
||||
ifup can0
|
||||
|
|
|
@ -2,22 +2,31 @@
|
|||
#
|
||||
# NXP MR-CANHUBK3 specific board sensors init
|
||||
#------------------------------------------------------------------------------
|
||||
if ver hwtypecmp MR-CANHUBK3-ADAP
|
||||
then
|
||||
icm42688p -c 2 -b 3 -R 0 -S -f 15000 start
|
||||
# Internal magnetometer on I2c on ADAP
|
||||
bmm150 -X -a 18 start
|
||||
ist8310 -X -b 1 -R 10 start
|
||||
# ADC for voltage input sensing
|
||||
board_adc start
|
||||
|
||||
#board_adc start FIXME no ADC drivers
|
||||
# External SPI bus ICM20649
|
||||
icm20649 -b 4 -S -R 10 start
|
||||
|
||||
#FMUv5Xbase board orientation
|
||||
# External SPI bus ICM42688p
|
||||
icm42688p -c 1 -b 3 -R 10 -S -f 15000 start
|
||||
else
|
||||
bmm150 -X start
|
||||
# External compass on GPS1/I2C1 (the 3rd external bus): standard Holybro Pixhawk 4 or CUAV V5 GPS/compass puck (with lights, safety button, and buzzer)
|
||||
ist8310 -X -b 2 -R 10 start
|
||||
|
||||
# Internal SPI bus ICM20649
|
||||
icm20649 -s -R 6 start
|
||||
# External SPI bus ICM20649
|
||||
icm20649 -b 4 -S -R 6 start
|
||||
|
||||
# Internal SPI bus ICM42688p
|
||||
icm42688p -R 6 -s start
|
||||
|
||||
# Internal magnetometer on I2c
|
||||
bmm150 -I start
|
||||
|
||||
# External compass on GPS1/I2C1 (the 3rd external bus): standard Holybro Pixhawk 4 or CUAV V5 GPS/compass puck (with lights, safety button, and buzzer)
|
||||
ist8310 -X -b 2 -R 10 start
|
||||
# External SPI bus ICM42688p
|
||||
icm42688p -c 1 -b 3 -R 6 -S -f 15000 start
|
||||
fi
|
||||
|
||||
# External compass on GPS1/I2C1 (the 3rd external bus): Drotek RTK GPS with LIS3MDL Compass
|
||||
lis3mdl -X -b 2 -R 2 start
|
||||
|
@ -25,5 +34,5 @@ lis3mdl -X -b 2 -R 2 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
|
||||
bmp388 -X -a 0x77 start
|
||||
fi
|
||||
|
|
|
@ -152,6 +152,18 @@
|
|||
#define PIN_LPUART1_RX (PIN_LPUART1_RX_3 | PIN_INPUT_PULLUP) /* PTC6 */
|
||||
#define PIN_LPUART1_TX PIN_LPUART1_TX_3 /* PTC7 */
|
||||
|
||||
|
||||
/* LPUART4 /dev/ttyS3 P8B 3X7 Pin 3 Single wire RC UART */
|
||||
|
||||
#define PIN_LPUART4_RX PIN_LPUART4_TX_3 /* Dummy since it's Single Wire TX-only */
|
||||
#define PIN_LPUART4_TX PIN_LPUART4_TX_3 /* PTE11 */
|
||||
|
||||
|
||||
/* LPUART7 /dev/ttyS4 P8B 3X7 Pin 3 and Pin 8 */
|
||||
|
||||
#define PIN_LPUART7_RX (PIN_LPUART7_RX_3 | PIN_INPUT_PULLUP) /* PTE0 */
|
||||
#define PIN_LPUART7_TX PIN_LPUART7_TX_3 /* PTE1 */
|
||||
|
||||
/* LPUART9 P24 UART connector */
|
||||
|
||||
#define PIN_LPUART9_RX (PIN_LPUART9_RX_1 | PIN_INPUT_PULLUP) /* PTB2 */
|
||||
|
@ -209,7 +221,8 @@
|
|||
#define PIN_LPSPI4_PCS0 PIN_LPSPI4_PCS0_1 /* PTB8 */
|
||||
#define PIN_LPSPI4_PCS3 PIN_LPSPI4_PCS3_1 /* PTA16 */
|
||||
|
||||
#define PIN_LPSPI4_PCS (PIN_PTA16 | GPIO_LOWDRIVE | GPIO_OUTPUT_ONE) /* PTA16 */
|
||||
#define PIN_LPSPI4_CS_P26 (PIN_PTA16 | GPIO_LOWDRIVE | GPIO_OUTPUT_ONE) /* PTA16 */
|
||||
#define PIN_LPSPI4_CS_P8B (PIN_PTB8 | GPIO_LOWDRIVE | GPIO_OUTPUT_ONE) /* PTB8 */
|
||||
|
||||
/* LPSPI5 P26 external IMU connector */
|
||||
|
||||
|
|
|
@ -245,6 +245,8 @@ CONFIG_S32K3XX_LPUART13=y
|
|||
CONFIG_S32K3XX_LPUART14=y
|
||||
CONFIG_S32K3XX_LPUART1=y
|
||||
CONFIG_S32K3XX_LPUART2=y
|
||||
CONFIG_S32K3XX_LPUART4=y
|
||||
CONFIG_S32K3XX_LPUART7=y
|
||||
CONFIG_S32K3XX_LPUART9=y
|
||||
CONFIG_S32K3XX_LPUART_INVERT=y
|
||||
CONFIG_S32K3XX_LPUART_SINGLEWIRE=y
|
||||
|
|
|
@ -38,6 +38,7 @@ if("${PX4_BOARD_LABEL}" STREQUAL "canbootloader")
|
|||
clockconfig.c
|
||||
periphclocks.c
|
||||
timer_config.cpp
|
||||
hw_rev_ver_canhubk3.c
|
||||
)
|
||||
|
||||
target_link_libraries(drivers_board
|
||||
|
@ -65,6 +66,8 @@ else()
|
|||
spi.cpp
|
||||
timer_config.cpp
|
||||
s32k3xx_userleds.c
|
||||
hw_rev_ver_canhubk3.c
|
||||
manifest.c
|
||||
)
|
||||
|
||||
target_link_libraries(drivers_board
|
||||
|
|
|
@ -88,7 +88,7 @@ __BEGIN_DECLS
|
|||
#define DIRECT_PWM_OUTPUT_CHANNELS 8
|
||||
|
||||
#define RC_SERIAL_PORT "/dev/ttyS5"
|
||||
#define RC_SERIAL_SINGLEWIRE
|
||||
#define RC_SERIAL_SINGLEWIRE_FORCE
|
||||
#define RC_SERIAL_INVERT_RX_ONLY
|
||||
|
||||
#define BOARD_ENABLE_CONSOLE_BUFFER
|
||||
|
@ -110,6 +110,40 @@ __BEGIN_DECLS
|
|||
/* Reboot and ulog we store on a wear-level filesystem */
|
||||
#define HARDFAULT_REBOOT_PATH "/mnt/progmem/reboot"
|
||||
|
||||
/* To detect MR-CANHUBK3-ADAP board */
|
||||
#define BOARD_HAS_HW_VERSIONING 1
|
||||
#define CANHUBK3_ADAP_DETECT (PIN_PTA12 | GPIO_INPUT | GPIO_PULLUP)
|
||||
|
||||
|
||||
/*
|
||||
* ADC channels
|
||||
*
|
||||
* These are the channel numbers of the ADCs of the microcontroller that can be used by the Px4
|
||||
* Firmware in the adc driver. ADC1 has 32 channels, with some a/b selection overlap
|
||||
* in the AD4-AD7 range on the same ADC.
|
||||
*
|
||||
* Only ADC1 is used
|
||||
* Bits 31:0 are ADC1 channels 31:0
|
||||
*/
|
||||
|
||||
#define ADC1_CH(c) (((c) & 0x1f)) /* Define ADC number Channel number */
|
||||
|
||||
/* ADC defines to be used in sensors.cpp to read from a particular channel */
|
||||
|
||||
#define ADC_BATTERY_VOLTAGE_CHANNEL ADC1_CH(0) /* BAT_VSENS 85 PTB4 ADC1_SE10 */
|
||||
#define ADC_BATTERY_CURRENT_CHANNEL ADC1_CH(1) /* Non-existant but needed for compilation */
|
||||
|
||||
|
||||
/* Mask use to initialize the ADC driver */
|
||||
|
||||
#define ADC_CHANNELS ((1 << ADC_BATTERY_VOLTAGE_CHANNEL))
|
||||
|
||||
/* Safety Switch
|
||||
* TBD
|
||||
*/
|
||||
#define GPIO_LED_SAFETY (PIN_PTE26 | GPIO_OUTPUT | GPIO_OUTPUT_ZERO)
|
||||
#define GPIO_BTN_SAFETY (PIN_PTA11 | GPIO_INPUT | GPIO_PULLDOWN)
|
||||
|
||||
/****************************************************************************
|
||||
* Public Data
|
||||
****************************************************************************/
|
||||
|
|
|
@ -0,0 +1,142 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2023 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 hw_rev_ver_canhubk3.c
|
||||
* CANHUBK3 Hardware Revision and Version ID API
|
||||
*/
|
||||
#include <drivers/drv_adc.h>
|
||||
#include <px4_arch/adc.h>
|
||||
#include <px4_platform_common/micro_hal.h>
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#include <px4_platform/board_determine_hw_info.h>
|
||||
#include <stdio.h>
|
||||
#include <board_config.h>
|
||||
|
||||
#include <systemlib/px4_macros.h>
|
||||
|
||||
#if defined(BOARD_HAS_HW_VERSIONING)
|
||||
|
||||
|
||||
#define HW_INFO_SIZE HW_INFO_VER_DIGITS + HW_INFO_REV_DIGITS
|
||||
|
||||
|
||||
/****************************************************************************
|
||||
* Private Data
|
||||
****************************************************************************/
|
||||
static int is_adap_connected = 0;
|
||||
|
||||
/****************************************************************************
|
||||
* Public Functions
|
||||
****************************************************************************/
|
||||
/************************************************************************************
|
||||
* Name: board_get_hw_type
|
||||
*
|
||||
* Description:
|
||||
* Optional returns a 0 terminated string defining the HW type.
|
||||
*
|
||||
* Input Parameters:
|
||||
* None
|
||||
*
|
||||
* Returned Value:
|
||||
* a 0 terminated string defining the HW type. This my be a 0 length string ""
|
||||
*
|
||||
************************************************************************************/
|
||||
|
||||
__EXPORT const char *board_get_hw_type_name()
|
||||
{
|
||||
if (is_adap_connected) {
|
||||
return (const char *)"MR-CANHUBK3-ADAP";
|
||||
|
||||
} else {
|
||||
return (const char *)"MR-CANHUBK344";
|
||||
}
|
||||
}
|
||||
|
||||
/************************************************************************************
|
||||
* Name: board_get_hw_version
|
||||
*
|
||||
* Description:
|
||||
* Optional returns a integer HW version
|
||||
*
|
||||
* Input Parameters:
|
||||
* None
|
||||
*
|
||||
* Returned Value:
|
||||
* An integer value of this boards hardware version.
|
||||
* A value of -1 is the default for boards not supporting the BOARD_HAS_VERSIONING API.
|
||||
* A value of 0 is the default for boards supporting the API but not having version.
|
||||
*
|
||||
************************************************************************************/
|
||||
|
||||
__EXPORT int board_get_hw_version()
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
/************************************************************************************
|
||||
* Name: board_get_hw_revision
|
||||
*
|
||||
* Description:
|
||||
* Optional returns a integer HW revision
|
||||
*
|
||||
* Input Parameters:
|
||||
* None
|
||||
*
|
||||
* Returned Value:
|
||||
* An integer value of this boards hardware revision.
|
||||
* A value of -1 is the default for boards not supporting the BOARD_HAS_VERSIONING API.
|
||||
* A value of 0 is the default for boards supporting the API but not having revision.
|
||||
*
|
||||
************************************************************************************/
|
||||
|
||||
__EXPORT int board_get_hw_revision()
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
/************************************************************************************
|
||||
* Name: board_determine_hw_info
|
||||
*
|
||||
* Description:
|
||||
* Uses GPIO to detect MR-CANHUBK3-ADAP
|
||||
*
|
||||
************************************************************************************/
|
||||
|
||||
int board_determine_hw_info()
|
||||
{
|
||||
s32k3xx_pinconfig(CANHUBK3_ADAP_DETECT);
|
||||
is_adap_connected = !s32k3xx_gpioread(CANHUBK3_ADAP_DETECT);
|
||||
return 0;
|
||||
}
|
||||
#endif
|
|
@ -34,6 +34,6 @@
|
|||
#include <px4_arch/i2c_hw_description.h>
|
||||
|
||||
constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = {
|
||||
initI2CBusInternal(PX4_BUS_NUMBER_TO_PX4(0)),
|
||||
initI2CBusExternal(PX4_BUS_NUMBER_TO_PX4(1)),
|
||||
initI2CBusExternal(PX4_BUS_NUMBER_TO_PX4(0)),
|
||||
initI2CBusInternal(PX4_BUS_NUMBER_TO_PX4(1)),
|
||||
};
|
||||
|
|
|
@ -44,6 +44,7 @@
|
|||
#include "board_config.h"
|
||||
|
||||
#include <px4_platform_common/init.h>
|
||||
#include <px4_platform/board_determine_hw_info.h>
|
||||
|
||||
#if defined(CONFIG_S32K3XX_LPSPI1) && defined(CONFIG_MMCSD)
|
||||
#include <nuttx/mmcsd.h>
|
||||
|
@ -96,6 +97,8 @@ __EXPORT int board_app_initialize(uintptr_t arg)
|
|||
|
||||
int rv;
|
||||
|
||||
board_determine_hw_info();
|
||||
|
||||
|
||||
#if defined(CONFIG_S32K3XX_LPSPI1) && defined(CONFIG_MMCSD)
|
||||
/* LPSPI1 *****************************************************************/
|
||||
|
|
|
@ -0,0 +1,79 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2023 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 manifest.c
|
||||
*
|
||||
* This module supplies the interface to the manifest of hardware that is
|
||||
* optional and dependent on the HW REV and HW VER IDs
|
||||
*
|
||||
* The manifest allows the system to know whether a hardware option
|
||||
* say for example the PX4IO is an no-pop option vs it is broken.
|
||||
*
|
||||
*/
|
||||
|
||||
/****************************************************************************
|
||||
* Included Files
|
||||
****************************************************************************/
|
||||
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#include <stdbool.h>
|
||||
#include <syslog.h>
|
||||
|
||||
#include "systemlib/px4_macros.h"
|
||||
#include "px4_log.h"
|
||||
|
||||
/****************************************************************************
|
||||
* Pre-Processor Definitions
|
||||
****************************************************************************/
|
||||
|
||||
|
||||
/************************************************************************************
|
||||
* Name: board_query_manifest
|
||||
*
|
||||
* Description:
|
||||
* Optional returns manifest item.
|
||||
*
|
||||
* Input Parameters:
|
||||
* manifest_id - the ID for the manifest item to retrieve
|
||||
*
|
||||
* Returned Value:
|
||||
* 0 - item is not in manifest => assume legacy operations
|
||||
* pointer to a manifest item
|
||||
*
|
||||
************************************************************************************/
|
||||
|
||||
__EXPORT px4_hw_mft_item board_query_manifest(px4_hw_mft_item_id_t id)
|
||||
{
|
||||
return px4_hw_mft_unsupported;
|
||||
}
|
|
@ -107,6 +107,9 @@ int s32k3xx_bringup(void)
|
|||
s32k3xx_spidev_initialize();
|
||||
#endif
|
||||
|
||||
s32k3xx_pinconfig(GPIO_LED_SAFETY);
|
||||
s32k3xx_pinconfig(GPIO_BTN_SAFETY);
|
||||
|
||||
#ifdef CONFIG_INPUT_BUTTONS
|
||||
/* Register the BUTTON driver */
|
||||
|
||||
|
|
|
@ -168,6 +168,22 @@ const struct peripheral_clock_config_s g_peripheral_clockconfig0[] = {
|
|||
.clkgate = true,
|
||||
#else
|
||||
.clkgate = false,
|
||||
#endif
|
||||
},
|
||||
{
|
||||
.clkname = LPUART4_CLK,
|
||||
#ifdef CONFIG_S32K3XX_LPUART4
|
||||
.clkgate = true,
|
||||
#else
|
||||
.clkgate = false,
|
||||
#endif
|
||||
},
|
||||
{
|
||||
.clkname = LPUART7_CLK,
|
||||
#ifdef CONFIG_S32K3XX_LPUART7
|
||||
.clkgate = true,
|
||||
#else
|
||||
.clkgate = false,
|
||||
#endif
|
||||
},
|
||||
{
|
||||
|
@ -258,6 +274,10 @@ const struct peripheral_clock_config_s g_peripheral_clockconfig0[] = {
|
|||
.clkname = EMIOS0_CLK,
|
||||
.clkgate = true,
|
||||
},
|
||||
{
|
||||
.clkname = ADC2_CLK,
|
||||
.clkgate = true,
|
||||
}
|
||||
};
|
||||
|
||||
unsigned int const num_of_peripheral_clocks_0 =
|
||||
|
|
|
@ -70,11 +70,12 @@ constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = {
|
|||
initSPIBus(SPI::Bus::SPI3, { // SPI3 is ignored only used for FS26 by a NuttX driver
|
||||
initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortD, GPIO::Pin17})
|
||||
}),
|
||||
initSPIBus(SPI::Bus::SPI4, {
|
||||
initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortA, GPIO::Pin16}, SPI::DRDY{PIN_WKPU20})
|
||||
initSPIBusExternal(SPI::Bus::SPI4, {
|
||||
initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin16}, SPI::DRDY{PIN_WKPU20}),
|
||||
initSPIConfigExternal(SPI::CS{GPIO::PortB, GPIO::Pin8}, SPI::DRDY{PIN_WKPU56})
|
||||
}),
|
||||
initSPIBus(SPI::Bus::SPI5, {
|
||||
initSPIDevice(DRV_IMU_DEVTYPE_ICM20649, SPI::CS{GPIO::PortA, GPIO::Pin14}, SPI::DRDY{PIN_WKPU4})
|
||||
initSPIBusExternal(SPI::Bus::SPI5, {
|
||||
initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin14}, SPI::DRDY{PIN_WKPU4})
|
||||
}),
|
||||
};
|
||||
|
||||
|
@ -337,7 +338,14 @@ void s32k3xx_lpspi4select(FAR struct spi_dev_s *dev, uint32_t devid,
|
|||
spiinfo("devid: %" PRId32 ", CS: %s\n", devid,
|
||||
selected ? "assert" : "de-assert");
|
||||
|
||||
s32k3xx_gpiowrite(PIN_LPSPI4_PCS, !selected);
|
||||
devid = ((devid) & 0xF);
|
||||
|
||||
if (devid == 0) {
|
||||
s32k3xx_gpiowrite(PIN_LPSPI4_CS_P26, !selected);
|
||||
|
||||
} else if (devid == 1) {
|
||||
s32k3xx_gpiowrite(PIN_LPSPI4_CS_P8B, !selected);
|
||||
}
|
||||
}
|
||||
|
||||
uint8_t s32k3xx_lpspi4status(FAR struct spi_dev_s *dev, uint32_t devid)
|
||||
|
|
|
@ -13,6 +13,7 @@ CONFIG_COMMON_MAGNETOMETER=y
|
|||
CONFIG_DRIVERS_PWM_OUT=y
|
||||
CONFIG_BOARD_UAVCAN_INTERFACES=1
|
||||
CONFIG_DRIVERS_UAVCANNODE=y
|
||||
CONFIG_DRIVERS_UWB_UWB_SR150=y
|
||||
CONFIG_UAVCANNODE_ARMING_STATUS=y
|
||||
CONFIG_UAVCANNODE_BEEP_COMMAND=y
|
||||
CONFIG_UAVCANNODE_ESC_RAW_COMMAND=y
|
||||
|
|
|
@ -10,3 +10,7 @@ CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=n
|
|||
CONFIG_BOARD_TESTING=y
|
||||
CONFIG_DRIVERS_TEST_PPM=y
|
||||
CONFIG_SYSTEMCMDS_MICROBENCH=y
|
||||
CONFIG_MODULES_FW_ATT_CONTROL=n
|
||||
CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n
|
||||
CONFIG_MODULES_FW_POS_CONTROL=n
|
||||
CONFIG_MODULES_FW_RATE_CONTROL=n
|
||||
|
|
|
@ -0,0 +1,49 @@
|
|||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2020 PX4 Development Team. All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
# are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# 2. Redistributions in binary form must reproduce the above copyright
|
||||
# notice, this list of conditions and the following disclaimer in
|
||||
# the documentation and/or other materials provided with the
|
||||
# distribution.
|
||||
# 3. Neither the name PX4 nor the names of its contributors may be
|
||||
# used to endorse or promote products derived from this software
|
||||
# without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
|
||||
set(PX4_FW_NAME ${PX4_BINARY_DIR}/${PX4_BOARD_VENDOR}_${PX4_BOARD_MODEL}_${PX4_BOARD_LABEL}.px4)
|
||||
|
||||
add_custom_target(upload_skynode_usb
|
||||
COMMAND ${PX4_SOURCE_DIR}/Tools/auterion/upload_skynode.sh --file=${PX4_FW_NAME}
|
||||
DEPENDS ${PX4_FW_NAME}
|
||||
COMMENT "Uploading PX4"
|
||||
USES_TERMINAL
|
||||
)
|
||||
|
||||
add_custom_target(upload_skynode_wifi
|
||||
COMMAND ${PX4_SOURCE_DIR}/Tools/auterion/upload_skynode.sh --file=${PX4_FW_NAME} --wifi
|
||||
DEPENDS ${PX4_FW_NAME}
|
||||
COMMENT "Uploading PX4"
|
||||
USES_TERMINAL
|
||||
)
|
|
@ -0,0 +1,49 @@
|
|||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2023 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.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
|
||||
set(PX4_FW_NAME ${PX4_BINARY_DIR}/${PX4_BOARD_VENDOR}_${PX4_BOARD_MODEL}_${PX4_BOARD_LABEL}.px4)
|
||||
|
||||
add_custom_target(upload_skynode_usb
|
||||
COMMAND ${PX4_SOURCE_DIR}/Tools/auterion/upload_skynode.sh --file=${PX4_FW_NAME}
|
||||
DEPENDS ${PX4_FW_NAME}
|
||||
COMMENT "Uploading PX4"
|
||||
USES_TERMINAL
|
||||
)
|
||||
|
||||
add_custom_target(upload_skynode_wifi
|
||||
COMMAND ${PX4_SOURCE_DIR}/Tools/auterion/upload_skynode.sh --file=${PX4_FW_NAME} --wifi
|
||||
DEPENDS ${PX4_FW_NAME}
|
||||
COMMENT "Uploading PX4"
|
||||
USES_TERMINAL
|
||||
)
|
|
@ -7,6 +7,7 @@ CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS6"
|
|||
CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS4"
|
||||
CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS1"
|
||||
CONFIG_BOARD_SERIAL_EXT2="/dev/ttyS3"
|
||||
CONFIG_DRIVERS_ADC_ADS1115=y
|
||||
CONFIG_DRIVERS_ADC_BOARD_ADC=y
|
||||
CONFIG_DRIVERS_BAROMETER_BMP388=y
|
||||
CONFIG_DRIVERS_BAROMETER_INVENSENSE_ICP201XX=y
|
||||
|
@ -41,6 +42,7 @@ CONFIG_DRIVERS_TONE_ALARM=y
|
|||
CONFIG_DRIVERS_UAVCAN=y
|
||||
CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2
|
||||
CONFIG_MODULES_AIRSPEED_SELECTOR=y
|
||||
CONFIG_MODULES_BATTERY_STATUS=y
|
||||
CONFIG_MODULES_CAMERA_FEEDBACK=y
|
||||
CONFIG_MODULES_COMMANDER=y
|
||||
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
||||
|
|
|
@ -102,13 +102,8 @@ if ver hwtypecmp V6X002001
|
|||
then
|
||||
rm3100 -I -b 4 start
|
||||
else
|
||||
if ver hwtypecmp V6X009010 V6X010010
|
||||
then
|
||||
# Internal magnetometer on I2C
|
||||
bmm150 -I -R 0 start
|
||||
else
|
||||
bmm150 -I -R 6 start
|
||||
fi
|
||||
# Internal magnetometer on I2C
|
||||
bmm150 -I -R 0 start
|
||||
fi
|
||||
|
||||
# External compass on GPS1/I2C1 (the 3rd external bus): standard Holybro Pixhawk 4 or CUAV V5 GPS/compass puck (with lights, safety button, and buzzer)
|
||||
|
|
|
@ -38,7 +38,7 @@ else()
|
|||
endif()
|
||||
|
||||
add_custom_target(upload
|
||||
COMMAND rsync -arh --progress
|
||||
COMMAND rsync -arh --progress -e "ssh -o StrictHostKeyChecking=no"
|
||||
${CMAKE_RUNTIME_OUTPUT_DIRECTORY} ${PX4_SOURCE_DIR}/posix-configs/rpi/*.config ${PX4_BINARY_DIR}/etc # source
|
||||
pi@${AUTOPILOT_HOST}:/home/pi/px4 # destination
|
||||
DEPENDS px4
|
||||
|
|
|
@ -4,15 +4,15 @@ CONFIG_BOARD_TOOLCHAIN="arm-linux-gnueabihf"
|
|||
CONFIG_BOARD_ARCHITECTURE="cortex-a53"
|
||||
CONFIG_BOARD_TESTING=y
|
||||
CONFIG_DRIVERS_ADC_ADS1115=y
|
||||
CONFIG_DRIVERS_BAROMETER_MS5611=y
|
||||
CONFIG_COMMON_BAROMETERS=y
|
||||
CONFIG_DRIVERS_BATT_SMBUS=y
|
||||
CONFIG_DRIVERS_CAMERA_TRIGGER=y
|
||||
CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
|
||||
CONFIG_COMMON_DISTANCE_SENSOR=y
|
||||
CONFIG_DRIVERS_GPS=y
|
||||
CONFIG_COMMON_IMU=y
|
||||
CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y
|
||||
CONFIG_DRIVERS_IMU_INVENSENSE_MPU9250=y
|
||||
CONFIG_DRIVERS_MAGNETOMETER_HMC5883=y
|
||||
CONFIG_COMMON_MAGNETOMETER=y
|
||||
CONFIG_DRIVERS_PCA9685_PWM_OUT=y
|
||||
CONFIG_DRIVERS_RC_INPUT=y
|
||||
CONFIG_DRIVERS_RPI_RC_IN=y
|
||||
|
|
|
@ -174,6 +174,7 @@ set(msg_files
|
|||
SensorSelection.msg
|
||||
SensorsStatus.msg
|
||||
SensorsStatusImu.msg
|
||||
SensorUwb.msg
|
||||
SystemPower.msg
|
||||
TakeoffStatus.msg
|
||||
TaskStackInfo.msg
|
||||
|
@ -190,8 +191,6 @@ set(msg_files
|
|||
UavcanParameterValue.msg
|
||||
UlogStream.msg
|
||||
UlogStreamAck.msg
|
||||
UwbDistance.msg
|
||||
UwbGrid.msg
|
||||
VehicleAcceleration.msg
|
||||
VehicleAirData.msg
|
||||
VehicleAngularAccelerationSetpoint.msg
|
||||
|
|
|
@ -0,0 +1,34 @@
|
|||
# UWB distance contains the distance information measured by an ultra-wideband positioning system,
|
||||
# such as Pozyx or NXP Rddrone.
|
||||
|
||||
uint64 timestamp # time since system start (microseconds)
|
||||
|
||||
uint32 sessionid # UWB SessionID
|
||||
uint32 time_offset # Time between Ranging Rounds in ms
|
||||
uint32 counter # Number of Ranges since last Start of Ranging
|
||||
uint16 mac # MAC adress of Initiator (controller)
|
||||
|
||||
uint16 mac_dest # MAC adress of Responder (Controlee)
|
||||
uint16 status # status feedback #
|
||||
uint8 nlos # None line of site condition y/n
|
||||
float32 distance # distance in m to the UWB receiver
|
||||
|
||||
|
||||
#Angle of arrival, Angle in Degree -60..+60; FOV in both axis is 120 degrees
|
||||
float32 aoa_azimuth_dev # Angle of arrival of first incomming RX msg
|
||||
float32 aoa_elevation_dev # Angle of arrival of first incomming RX msg
|
||||
float32 aoa_azimuth_resp # Angle of arrival of first incomming RX msg at the responder
|
||||
float32 aoa_elevation_resp # Angle of arrival of first incomming RX msg at the responder
|
||||
|
||||
# Figure of merit for the angle measurements
|
||||
uint8 aoa_azimuth_fom # AOA Azimuth FOM
|
||||
uint8 aoa_elevation_fom # AOA Elevation FOM
|
||||
uint8 aoa_dest_azimuth_fom # AOA Azimuth FOM
|
||||
uint8 aoa_dest_elevation_fom # AOA Elevation FOM
|
||||
|
||||
# Initiator physical configuration
|
||||
uint8 orientation # Direction the sensor faces from MAV_SENSOR_ORIENTATION enum
|
||||
# Standard configuration is Antennas facing down and azimuth aligened in forward direction
|
||||
float32 offset_x # UWB initiator offset in X axis (NED drone frame)
|
||||
float32 offset_y # UWB initiator offset in Y axis (NED drone frame)
|
||||
float32 offset_z # UWB initiator offset in Z axis (NED drone frame)
|
|
@ -1,15 +0,0 @@
|
|||
# UWB distance contains the distance information measured by an ultra-wideband positioning system,
|
||||
# such as Pozyx or NXP Rddrone.
|
||||
|
||||
uint64 timestamp # time since system start (microseconds)
|
||||
uint32 time_uwb_ms # Time of UWB device in ms
|
||||
uint32 counter # Number of Ranges since last Start of Ranging
|
||||
uint32 sessionid # UWB SessionID
|
||||
uint32 time_offset # Time between Ranging Rounds in ms
|
||||
uint16 status # status feedback #
|
||||
|
||||
uint16[12] anchor_distance # distance in cm to each UWB Anchor (UWB Device which takes part in Ranging)
|
||||
bool[12] nlos # Visual line of sight yes/no
|
||||
float32[12] aoafirst # Angle of arrival of first incoming RX msg
|
||||
|
||||
float32[3] position # Position of the Landing point in relation to the Drone (x,y,z in Meters NED)
|
|
@ -1,25 +0,0 @@
|
|||
# UWB report message contains the grid information measured by an ultra-wideband positioning system,
|
||||
# such as Pozyx or NXP Rddrone.
|
||||
|
||||
uint64 timestamp # time since system start (microseconds)
|
||||
uint16 initator_time # time to synchronize clocks (microseconds)
|
||||
uint8 num_anchors # Number of anchors
|
||||
|
||||
float64[4] target_gps # GPS position of target of the UWB system (Lat / Lon / Alt / Yaw Offset to true North)
|
||||
|
||||
# Grid position information
|
||||
# Position in x,y,z in (x,y,z in centimeters NED)
|
||||
# staring with POI and Anchor 0 up to Anchor 11
|
||||
int16[3] target_pos # Point of Interest, mostly landing Target x,y,z
|
||||
int16[3] anchor_pos_0
|
||||
int16[3] anchor_pos_1
|
||||
int16[3] anchor_pos_2
|
||||
int16[3] anchor_pos_3
|
||||
int16[3] anchor_pos_4
|
||||
int16[3] anchor_pos_5
|
||||
int16[3] anchor_pos_6
|
||||
int16[3] anchor_pos_7
|
||||
int16[3] anchor_pos_8
|
||||
int16[3] anchor_pos_9
|
||||
int16[3] anchor_pos_10
|
||||
int16[3] anchor_pos_11
|
|
@ -440,6 +440,8 @@ __BEGIN_DECLS
|
|||
|
||||
#if defined(RC_SERIAL_SINGLEWIRE)
|
||||
static inline bool board_rc_singlewire(const char *device) { return strcmp(device, RC_SERIAL_PORT) == 0; }
|
||||
#elif defined(RC_SERIAL_SINGLEWIRE_FORCE)
|
||||
static inline bool board_rc_singlewire(const char *device) { return true; }
|
||||
#else
|
||||
static inline bool board_rc_singlewire(const char *device) { return false; }
|
||||
#endif
|
||||
|
|
|
@ -1 +1 @@
|
|||
Subproject commit 3f77354c0dc88793a47ff3b57595195ab45f7ba9
|
||||
Subproject commit de41e7feaeffaec3ce65327e9569e8fdb553ca3d
|
|
@ -38,144 +38,76 @@
|
|||
|
||||
#include <nuttx/analog/adc.h>
|
||||
|
||||
#include <hardware/s32k1xx_sim.h>
|
||||
|
||||
//todo S32K add ADC fior now steal the kinetis one
|
||||
#include <kinetis.h>
|
||||
#include <hardware/kinetis_adc.h>
|
||||
|
||||
|
||||
#define _REG(_addr) (*(volatile uint32_t *)(_addr))
|
||||
|
||||
/* ADC register accessors */
|
||||
|
||||
#define REG(a, _reg) _REG(KINETIS_ADC##a##_BASE + (_reg))
|
||||
|
||||
#define rSC1A(adc) REG(adc, KINETIS_ADC_SC1A_OFFSET) /* ADC status and control registers 1 */
|
||||
#define rSC1B(adc) REG(adc, KINETIS_ADC_SC1B_OFFSET) /* ADC status and control registers 1 */
|
||||
#define rCFG1(adc) REG(adc, KINETIS_ADC_CFG1_OFFSET) /* ADC configuration register 1 */
|
||||
#define rCFG2(adc) REG(adc, KINETIS_ADC_CFG2_OFFSET) /* Configuration register 2 */
|
||||
#define rRA(adc) REG(adc, KINETIS_ADC_RA_OFFSET) /* ADC data result register */
|
||||
#define rRB(adc) REG(adc, KINETIS_ADC_RB_OFFSET) /* ADC data result register */
|
||||
#define rCV1(adc) REG(adc, KINETIS_ADC_CV1_OFFSET) /* Compare value registers */
|
||||
#define rCV2(adc) REG(adc, KINETIS_ADC_CV2_OFFSET) /* Compare value registers */
|
||||
#define rSC2(adc) REG(adc, KINETIS_ADC_SC2_OFFSET) /* Status and control register 2 */
|
||||
#define rSC3(adc) REG(adc, KINETIS_ADC_SC3_OFFSET) /* Status and control register 3 */
|
||||
#define rOFS(adc) REG(adc, KINETIS_ADC_OFS_OFFSET) /* ADC offset correction register */
|
||||
#define rPG(adc) REG(adc, KINETIS_ADC_PG_OFFSET) /* ADC plus-side gain register */
|
||||
#define rMG(adc) REG(adc, KINETIS_ADC_MG_OFFSET) /* ADC minus-side gain register */
|
||||
#define rCLPD(adc) REG(adc, KINETIS_ADC_CLPD_OFFSET) /* ADC plus-side general calibration value register */
|
||||
#define rCLPS(adc) REG(adc, KINETIS_ADC_CLPS_OFFSET) /* ADC plus-side general calibration value register */
|
||||
#define rCLP4(adc) REG(adc, KINETIS_ADC_CLP4_OFFSET) /* ADC plus-side general calibration value register */
|
||||
#define rCLP3(adc) REG(adc, KINETIS_ADC_CLP3_OFFSET) /* ADC plus-side general calibration value register */
|
||||
#define rCLP2(adc) REG(adc, KINETIS_ADC_CLP2_OFFSET) /* ADC plus-side general calibration value register */
|
||||
#define rCLP1(adc) REG(adc, KINETIS_ADC_CLP1_OFFSET) /* ADC plus-side general calibration value register */
|
||||
#define rCLP0(adc) REG(adc, KINETIS_ADC_CLP0_OFFSET) /* ADC plus-side general calibration value register */
|
||||
#define rCLMD(adc) REG(adc, KINETIS_ADC_CLMD_OFFSET) /* ADC minus-side general calibration value register */
|
||||
#define rCLMS(adc) REG(adc, KINETIS_ADC_CLMS_OFFSET) /* ADC minus-side general calibration value register */
|
||||
#define rCLM4(adc) REG(adc, KINETIS_ADC_CLM4_OFFSET) /* ADC minus-side general calibration value register */
|
||||
#define rCLM3(adc) REG(adc, KINETIS_ADC_CLM3_OFFSET) /* ADC minus-side general calibration value register */
|
||||
#define rCLM2(adc) REG(adc, KINETIS_ADC_CLM2_OFFSET) /* ADC minus-side general calibration value register */
|
||||
#define rCLM1(adc) REG(adc, KINETIS_ADC_CLM1_OFFSET) /* ADC minus-side general calibration value register */
|
||||
#define rCLM0(adc) REG(adc, KINETIS_ADC_CLM0_OFFSET) /* ADC minus-side general calibration value register */
|
||||
#include <hardware/s32k3xx_adc.h>
|
||||
#include <hardware/s32k344_pinmux.h>
|
||||
|
||||
int px4_arch_adc_init(uint32_t base_address)
|
||||
{
|
||||
/* Input is Buss Clock 56 Mhz We will use /8 for 7 Mhz */
|
||||
uint32_t regval;
|
||||
|
||||
irqstate_t flags = px4_enter_critical_section();
|
||||
/* Configure and perform calibration */
|
||||
putreg32(ADC_MCR_ADCLKSEL_DIV4, S32K3XX_ADC2_MCR);
|
||||
|
||||
_REG(KINETIS_SIM_SCGC3) |= SIM_SCGC3_ADC1;
|
||||
rCFG1(1) = ADC_CFG1_ADICLK_BUSCLK | ADC_CFG1_MODE_1213BIT | ADC_CFG1_ADIV_DIV8;
|
||||
rCFG2(1) = 0;
|
||||
rSC2(1) = ADC_SC2_REFSEL_DEFAULT;
|
||||
regval = getreg32(S32K3XX_ADC2_AMSIO);
|
||||
regval |= ADC_AMSIO_HSEN_MASK;
|
||||
putreg32(regval, S32K3XX_ADC2_AMSIO);
|
||||
|
||||
px4_leave_critical_section(flags);
|
||||
regval = getreg32(S32K3XX_ADC2_CAL2);
|
||||
regval &= ~ADC_CAL2_ENX;
|
||||
putreg32(regval, S32K3XX_ADC2_CAL2);
|
||||
|
||||
/* Clear the CALF and begin the calibration */
|
||||
regval = getreg32(S32K3XX_ADC2_CALBISTREG);
|
||||
regval &= ~(ADC_CALBISTREG_TEST_EN | ADC_CALBISTREG_AVG_EN | ADC_CALBISTREG_NR_SMPL_MASK |
|
||||
ADC_CALBISTREG_CALSTFUL | ADC_CALBISTREG_TSAMP_MASK | ADC_CALBISTREG_RESN_MASK);
|
||||
regval |= ADC_CALBISTREG_TEST_EN | ADC_CALBISTREG_AVG_EN | ADC_CALBISTREG_NR_SMPL_4SMPL |
|
||||
ADC_CALBISTREG_CALSTFUL | ADC_CALBISTREG_RESN_14BIT;
|
||||
putreg32(regval, S32K3XX_ADC2_CALBISTREG);
|
||||
|
||||
rSC3(1) = ADC_SC3_CAL | ADC_SC3_CALF;
|
||||
while (getreg32(S32K3XX_ADC2_CALBISTREG) & ADC_CALBISTREG_C_T_BUSY) {};
|
||||
|
||||
while ((rSC1A(1) & ADC_SC1_COCO) == 0) {
|
||||
usleep(100);
|
||||
putreg32(ADC_MCR_PWDN, S32K3XX_ADC2_MCR);
|
||||
|
||||
if (rSC3(1) & ADC_SC3_CALF) {
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
putreg32(22, S32K3XX_ADC2_CTR0);
|
||||
|
||||
/* dummy read to clear COCO of calibration */
|
||||
putreg32(22, S32K3XX_ADC2_CTR1);
|
||||
|
||||
int32_t r = rRA(1);
|
||||
putreg32(0, S32K3XX_ADC2_DMAE);
|
||||
|
||||
/* Check the state of CALF at the end of calibration */
|
||||
putreg32(ADC_MCR_ADCLKSEL_DIV4 | ADC_MCR_AVGS_32CONV | ADC_MCR_AVGEN | ADC_MCR_BCTU_MODE | ADC_MCR_MODE,
|
||||
S32K3XX_ADC2_MCR);
|
||||
|
||||
if (rSC3(1) & ADC_SC3_CALF) {
|
||||
return -1;
|
||||
}
|
||||
putreg32(0x10, S32K3XX_ADC2_NCMR0);
|
||||
|
||||
/* Calculate the calibration values for single ended positive */
|
||||
putreg32(0x10, S32K3XX_ADC2_NCMR1);
|
||||
|
||||
r = rCLP0(1) + rCLP1(1) + rCLP2(1) + rCLP3(1) + rCLP4(1) + rCLPS(1) ;
|
||||
r = 0x8000U | (r >> 1U);
|
||||
rPG(1) = r;
|
||||
regval = getreg32(S32K3XX_ADC2_MCR);
|
||||
|
||||
/* Calculate the calibration values for double ended Negitive */
|
||||
regval |= ADC_MCR_NSTART;
|
||||
|
||||
r = rCLM0(1) + rCLM1(1) + rCLM2(1) + rCLM3(1) + rCLM4(1) + rCLMS(1) ;
|
||||
r = 0x8000U | (r >> 1U);
|
||||
rMG(1) = r;
|
||||
|
||||
/* kick off a sample and wait for it to complete */
|
||||
hrt_abstime now = hrt_absolute_time();
|
||||
|
||||
rSC1A(1) = ADC_SC1_ADCH(ADC_SC1_ADCH_TEMP);
|
||||
|
||||
while (!(rSC1A(1) & ADC_SC1_COCO)) {
|
||||
|
||||
/* don't wait for more than 500us, since that means something broke - should reset here if we see this */
|
||||
if ((hrt_absolute_time() - now) > 500) {
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
putreg32(regval, S32K3XX_ADC2_MCR);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
void px4_arch_adc_uninit(uint32_t base_address)
|
||||
{
|
||||
irqstate_t flags = px4_enter_critical_section();
|
||||
_REG(KINETIS_SIM_SCGC3) &= ~SIM_SCGC3_ADC1;
|
||||
px4_leave_critical_section(flags);
|
||||
}
|
||||
|
||||
uint32_t px4_arch_adc_sample(uint32_t base_address, unsigned channel)
|
||||
{
|
||||
irqstate_t flags = px4_enter_critical_section();
|
||||
uint32_t result = 0;
|
||||
|
||||
/* clear any previous COCC */
|
||||
rRA(1);
|
||||
if (channel == 0) {
|
||||
result = getreg32(S32K3XX_ADC2_PCDR4);
|
||||
|
||||
/* run a single conversion right now - should take about 35 cycles (5 microseconds) max */
|
||||
rSC1A(1) = ADC_SC1_ADCH(channel);
|
||||
if ((result & ADC_PCDR_VALID) == ADC_PCDR_VALID) {
|
||||
result = result & 0xFFFF;
|
||||
|
||||
/* wait for the conversion to complete */
|
||||
const hrt_abstime now = hrt_absolute_time();
|
||||
|
||||
while (!(rSC1A(1) & ADC_SC1_COCO)) {
|
||||
|
||||
/* don't wait for more than 10us, since that means something broke - should reset here if we see this */
|
||||
if ((hrt_absolute_time() - now) > 10) {
|
||||
px4_leave_critical_section(flags);
|
||||
return 0xffff;
|
||||
} else {
|
||||
result = 0;
|
||||
}
|
||||
}
|
||||
|
||||
/* read the result and clear EOC */
|
||||
uint32_t result = rRA(1);
|
||||
|
||||
px4_leave_critical_section(flags);
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
|
@ -186,10 +118,10 @@ float px4_arch_adc_reference_v()
|
|||
|
||||
uint32_t px4_arch_adc_temp_sensor_mask()
|
||||
{
|
||||
return 1 << (ADC_SC1_ADCH_TEMP >> ADC_SC1_ADCH_SHIFT);
|
||||
return 0; // No temp sensor
|
||||
}
|
||||
|
||||
uint32_t px4_arch_adc_dn_fullcount()
|
||||
{
|
||||
return 1 << 12; // 12 bit ADC
|
||||
return 1 << 15; // 15 bit conversion data
|
||||
}
|
||||
|
|
|
@ -48,7 +48,6 @@ const char *_device;
|
|||
|
||||
ModalIo::ModalIo() :
|
||||
OutputModuleInterface(MODULE_NAME, px4::serial_port_to_wq(MODAL_IO_DEFAULT_PORT)),
|
||||
_mixing_output{"MODAL_IO", MODAL_IO_OUTPUT_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, false, false},
|
||||
_cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")),
|
||||
_output_update_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": output update interval"))
|
||||
{
|
||||
|
|
|
@ -174,7 +174,7 @@ private:
|
|||
} led_rsc_t;
|
||||
|
||||
ch_assign_t _output_map[MODAL_IO_OUTPUT_CHANNELS] {{1, 1}, {2, 1}, {3, 1}, {4, 1}};
|
||||
MixingOutput _mixing_output;
|
||||
MixingOutput _mixing_output{"MODAL_IO", MODAL_IO_OUTPUT_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
|
||||
|
||||
perf_counter_t _cycle_perf;
|
||||
perf_counter_t _output_update_perf;
|
||||
|
|
|
@ -250,7 +250,7 @@ ICP201XX::RunImpl()
|
|||
case STATE::CONFIG: {
|
||||
if (configure()) {
|
||||
_state = STATE::WAIT_READ;
|
||||
ScheduleDelayed(10_ms);
|
||||
ScheduleDelayed(30_ms);
|
||||
|
||||
} else {
|
||||
if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
|
||||
|
|
|
@ -61,41 +61,11 @@ __BEGIN_DECLS
|
|||
*/
|
||||
#define PWM_LOWEST_MIN 90
|
||||
|
||||
/**
|
||||
* Default value for a shutdown motor
|
||||
*/
|
||||
#define PWM_MOTOR_OFF 900
|
||||
|
||||
/**
|
||||
* Default minimum PWM in us
|
||||
*/
|
||||
#define PWM_DEFAULT_MIN 1000
|
||||
|
||||
/**
|
||||
* Highest PWM allowed as the minimum PWM
|
||||
*/
|
||||
#define PWM_HIGHEST_MIN 1600
|
||||
|
||||
/**
|
||||
* Highest maximum PWM in us
|
||||
*/
|
||||
#define PWM_HIGHEST_MAX 2500
|
||||
|
||||
/**
|
||||
* Default maximum PWM in us
|
||||
*/
|
||||
#define PWM_DEFAULT_MAX 2000
|
||||
|
||||
/**
|
||||
* Default trim PWM in us
|
||||
*/
|
||||
#define PWM_DEFAULT_TRIM 0
|
||||
|
||||
/**
|
||||
* Lowest PWM allowed as the maximum PWM
|
||||
*/
|
||||
#define PWM_LOWEST_MAX 200
|
||||
|
||||
#endif // not PX4_PWM_ALTERNATE_RANGES
|
||||
|
||||
/**
|
||||
|
|
|
@ -82,6 +82,7 @@
|
|||
#define DRV_IMU_DEVTYPE_IIM42652 0x2B
|
||||
#define DRV_IMU_DEVTYPE_IAM20680HP 0x2C
|
||||
#define DRV_IMU_DEVTYPE_ICM42686P 0x2D
|
||||
#define DRV_IMU_DEVTYPE_IIM42653 0x2E
|
||||
|
||||
#define DRV_RNG_DEVTYPE_MB12XX 0x31
|
||||
#define DRV_RNG_DEVTYPE_LL40LS 0x32
|
||||
|
@ -157,6 +158,8 @@
|
|||
#define DRV_LED_DEVTYPE_RGBLED 0x7a
|
||||
#define DRV_LED_DEVTYPE_RGBLED_NCP5623C 0x7b
|
||||
#define DRV_LED_DEVTYPE_RGBLED_IS31FL3195 0xbf
|
||||
#define DRV_LED_DEVTYPE_RGBLED_LP5562 0xc0
|
||||
|
||||
#define DRV_BAT_DEVTYPE_SMBUS 0x7c
|
||||
#define DRV_SENS_DEVTYPE_IRLOCK 0x7d
|
||||
#define DRV_SENS_DEVTYPE_PCF8583 0x7e
|
||||
|
|
|
@ -143,7 +143,7 @@ private:
|
|||
|
||||
void handle_vehicle_commands();
|
||||
|
||||
MixingOutput _mixing_output {PARAM_PREFIX, DIRECT_PWM_OUTPUT_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
|
||||
MixingOutput _mixing_output{PARAM_PREFIX, DIRECT_PWM_OUTPUT_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
|
||||
uint32_t _reversible_outputs{};
|
||||
|
||||
Telemetry *_telemetry{nullptr};
|
||||
|
|
|
@ -60,6 +60,7 @@
|
|||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/PublicationMulti.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/SubscriptionMultiArray.hpp>
|
||||
#include <uORB/topics/gps_dump.h>
|
||||
#include <uORB/topics/gps_inject_data.h>
|
||||
#include <uORB/topics/sensor_gps.h>
|
||||
|
@ -203,7 +204,7 @@ private:
|
|||
|
||||
const Instance _instance;
|
||||
|
||||
uORB::Subscription _orb_inject_data_sub{ORB_ID(gps_inject_data)};
|
||||
uORB::SubscriptionMultiArray<gps_inject_data_s, gps_inject_data_s::MAX_INSTANCES> _orb_inject_data_sub{ORB_ID::gps_inject_data};
|
||||
uORB::Publication<gps_inject_data_s> _gps_inject_data_pub{ORB_ID(gps_inject_data)};
|
||||
uORB::Publication<gps_dump_s> _dump_communication_pub{ORB_ID(gps_dump)};
|
||||
gps_dump_s *_dump_to_device{nullptr};
|
||||
|
@ -530,13 +531,15 @@ void GPS::handleInjectDataTopic()
|
|||
// If there has not been a valid RTCM message for a while, try to switch to a different RTCM link
|
||||
if ((hrt_absolute_time() - _last_rtcm_injection_time) > 5_s) {
|
||||
|
||||
for (uint8_t i = 0; i < gps_inject_data_s::MAX_INSTANCES; i++) {
|
||||
if (_orb_inject_data_sub.ChangeInstance(i)) {
|
||||
if (_orb_inject_data_sub.copy(&msg)) {
|
||||
for (int instance = 0; instance < _orb_inject_data_sub.size(); instance++) {
|
||||
const bool exists = _orb_inject_data_sub[instance].advertised();
|
||||
|
||||
if (exists) {
|
||||
if (_orb_inject_data_sub[instance].copy(&msg)) {
|
||||
if ((hrt_absolute_time() - msg.timestamp) < 5_s) {
|
||||
// Remember that we already did a copy on this instance.
|
||||
already_copied = true;
|
||||
_selected_rtcm_instance = i;
|
||||
_selected_rtcm_instance = instance;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
@ -544,9 +547,6 @@ void GPS::handleInjectDataTopic()
|
|||
}
|
||||
}
|
||||
|
||||
// Reset instance in case we didn't actually want to switch
|
||||
_orb_inject_data_sub.ChangeInstance(_selected_rtcm_instance);
|
||||
|
||||
bool updated = already_copied;
|
||||
|
||||
// Limit maximum number of GPS injections to 8 since usually
|
||||
|
@ -574,7 +574,7 @@ void GPS::handleInjectDataTopic()
|
|||
}
|
||||
}
|
||||
|
||||
updated = _orb_inject_data_sub.update(&msg);
|
||||
updated = _orb_inject_data_sub[_selected_rtcm_instance].update(&msg);
|
||||
|
||||
} while (updated && num_injections < max_num_injections);
|
||||
}
|
||||
|
|
|
@ -342,9 +342,9 @@ ADIS16497::read_reg16(uint8_t reg)
|
|||
|
||||
cmd[0] = ((reg | DIR_READ) << 8) & 0xff00;
|
||||
transferhword(cmd, nullptr, 1);
|
||||
up_udelay(T_STALL);
|
||||
px4_udelay(T_STALL);
|
||||
transferhword(nullptr, cmd, 1);
|
||||
up_udelay(T_STALL);
|
||||
px4_udelay(T_STALL);
|
||||
|
||||
return cmd[0];
|
||||
}
|
||||
|
@ -367,9 +367,9 @@ ADIS16497::write_reg16(uint8_t reg, uint16_t value)
|
|||
cmd[1] = (((reg + 0x1) | DIR_WRITE) << 8) | ((0xff00 & value) >> 8);
|
||||
|
||||
transferhword(cmd, nullptr, 1);
|
||||
up_udelay(T_STALL);
|
||||
px4_udelay(T_STALL);
|
||||
transferhword(cmd + 1, nullptr, 1);
|
||||
up_udelay(T_STALL);
|
||||
px4_udelay(T_STALL);
|
||||
}
|
||||
|
||||
void
|
||||
|
|
|
@ -33,8 +33,6 @@
|
|||
|
||||
#include "BMI055_Gyroscope.hpp"
|
||||
|
||||
#include <px4_platform/board_dma_alloc.h>
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
namespace Bosch::BMI055::Gyroscope
|
||||
|
|
|
@ -33,8 +33,6 @@
|
|||
|
||||
#include "BMI088_Gyroscope.hpp"
|
||||
|
||||
#include <px4_platform/board_dma_alloc.h>
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
namespace Bosch::BMI088::Gyroscope
|
||||
|
|
|
@ -0,0 +1,48 @@
|
|||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2022 PX4 Development Team. All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
# are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# 2. Redistributions in binary form must reproduce the above copyright
|
||||
# notice, this list of conditions and the following disclaimer in
|
||||
# the documentation and/or other materials provided with the
|
||||
# distribution.
|
||||
# 3. Neither the name PX4 nor the names of its contributors may be
|
||||
# used to endorse or promote products derived from this software
|
||||
# without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
############################################################################
|
||||
px4_add_module(
|
||||
MODULE drivers__imu__invensense__iim42653
|
||||
MAIN iim42653
|
||||
COMPILE_FLAGS
|
||||
${MAX_CUSTOM_OPT_LEVEL}
|
||||
#-DDEBUG_BUILD
|
||||
SRCS
|
||||
iim42653_main.cpp
|
||||
IIM42653.cpp
|
||||
IIM42653.hpp
|
||||
InvenSense_IIM42653_registers.hpp
|
||||
DEPENDS
|
||||
px4_work_queue
|
||||
drivers_accelerometer
|
||||
drivers_gyroscope
|
||||
)
|
|
@ -0,0 +1,861 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2023 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 "IIM42653.hpp"
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
static constexpr int16_t combine(uint8_t msb, uint8_t lsb)
|
||||
{
|
||||
return (msb << 8u) | lsb;
|
||||
}
|
||||
|
||||
static constexpr uint16_t combine_uint(uint8_t msb, uint8_t lsb)
|
||||
{
|
||||
return (msb << 8u) | lsb;
|
||||
}
|
||||
|
||||
IIM42653::IIM42653(const I2CSPIDriverConfig &config) :
|
||||
SPI(config),
|
||||
I2CSPIDriver(config),
|
||||
_drdy_gpio(config.drdy_gpio),
|
||||
_px4_accel(get_device_id(), config.rotation),
|
||||
_px4_gyro(get_device_id(), config.rotation)
|
||||
{
|
||||
if (config.drdy_gpio != 0) {
|
||||
_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed");
|
||||
}
|
||||
|
||||
if (config.custom1 != 0) {
|
||||
_enable_clock_input = true;
|
||||
_input_clock_freq = config.custom1;
|
||||
ConfigureCLKIN();
|
||||
|
||||
} else {
|
||||
_enable_clock_input = false;
|
||||
}
|
||||
|
||||
ConfigureSampleRate(_px4_gyro.get_max_rate_hz());
|
||||
}
|
||||
|
||||
IIM42653::~IIM42653()
|
||||
{
|
||||
perf_free(_bad_register_perf);
|
||||
perf_free(_bad_transfer_perf);
|
||||
perf_free(_fifo_empty_perf);
|
||||
perf_free(_fifo_overflow_perf);
|
||||
perf_free(_fifo_reset_perf);
|
||||
perf_free(_drdy_missed_perf);
|
||||
}
|
||||
|
||||
int IIM42653::init()
|
||||
{
|
||||
int ret = SPI::init();
|
||||
|
||||
if (ret != PX4_OK) {
|
||||
DEVICE_DEBUG("SPI::init failed (%i)", ret);
|
||||
return ret;
|
||||
}
|
||||
|
||||
return Reset() ? 0 : -1;
|
||||
}
|
||||
|
||||
bool IIM42653::Reset()
|
||||
{
|
||||
_state = STATE::RESET;
|
||||
DataReadyInterruptDisable();
|
||||
ScheduleClear();
|
||||
ScheduleNow();
|
||||
return true;
|
||||
}
|
||||
|
||||
void IIM42653::exit_and_cleanup()
|
||||
{
|
||||
DataReadyInterruptDisable();
|
||||
I2CSPIDriverBase::exit_and_cleanup();
|
||||
}
|
||||
|
||||
void IIM42653::print_status()
|
||||
{
|
||||
I2CSPIDriverBase::print_status();
|
||||
|
||||
PX4_INFO("FIFO empty interval: %d us (%.1f Hz)", _fifo_empty_interval_us, 1e6 / _fifo_empty_interval_us);
|
||||
PX4_INFO("Clock input: %s", _enable_clock_input ? "enabled" : "disabled");
|
||||
|
||||
perf_print_counter(_bad_register_perf);
|
||||
perf_print_counter(_bad_transfer_perf);
|
||||
perf_print_counter(_fifo_empty_perf);
|
||||
perf_print_counter(_fifo_overflow_perf);
|
||||
perf_print_counter(_fifo_reset_perf);
|
||||
perf_print_counter(_drdy_missed_perf);
|
||||
}
|
||||
|
||||
int IIM42653::probe()
|
||||
{
|
||||
for (int i = 0; i < 3; i++) {
|
||||
uint8_t whoami = RegisterRead(Register::BANK_0::WHO_AM_I);
|
||||
|
||||
if (whoami == WHOAMI) {
|
||||
return PX4_OK;
|
||||
|
||||
} else {
|
||||
DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami);
|
||||
|
||||
uint8_t reg_bank_sel = RegisterRead(Register::BANK_0::REG_BANK_SEL);
|
||||
int bank = reg_bank_sel >> 4;
|
||||
|
||||
if (bank >= 1 && bank <= 3) {
|
||||
DEVICE_DEBUG("incorrect register bank for WHO_AM_I REG_BANK_SEL:0x%02x, bank:%d", reg_bank_sel, bank);
|
||||
// force bank selection and retry
|
||||
SelectRegisterBank(REG_BANK_SEL_BIT::BANK_SEL_0, true);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
void IIM42653::RunImpl()
|
||||
{
|
||||
const hrt_abstime now = hrt_absolute_time();
|
||||
|
||||
switch (_state) {
|
||||
case STATE::RESET:
|
||||
// DEVICE_CONFIG: Software reset configuration
|
||||
RegisterWrite(Register::BANK_0::DEVICE_CONFIG, DEVICE_CONFIG_BIT::SOFT_RESET_CONFIG);
|
||||
_reset_timestamp = now;
|
||||
_failure_count = 0;
|
||||
_state = STATE::WAIT_FOR_RESET;
|
||||
ScheduleDelayed(1_ms); // wait 1 ms for soft reset to be effective
|
||||
break;
|
||||
|
||||
case STATE::WAIT_FOR_RESET:
|
||||
if ((RegisterRead(Register::BANK_0::WHO_AM_I) == WHOAMI)
|
||||
&& (RegisterRead(Register::BANK_0::DEVICE_CONFIG) == 0x00)
|
||||
&& (RegisterRead(Register::BANK_0::INT_STATUS) & INT_STATUS_BIT::RESET_DONE_INT)) {
|
||||
|
||||
// Wakeup accel and gyro and schedule remaining configuration
|
||||
RegisterWrite(Register::BANK_0::PWR_MGMT0, PWR_MGMT0_BIT::GYRO_MODE_LOW_NOISE | PWR_MGMT0_BIT::ACCEL_MODE_LOW_NOISE);
|
||||
_state = STATE::CONFIGURE;
|
||||
ScheduleDelayed(30_ms); // 30 ms gyro startup time, 10 ms accel from sleep to valid data
|
||||
|
||||
} 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);
|
||||
}
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case STATE::CONFIGURE:
|
||||
if (Configure()) {
|
||||
// if configure succeeded then reset the FIFO
|
||||
_state = STATE::FIFO_RESET;
|
||||
ScheduleDelayed(1_ms);
|
||||
|
||||
} 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::FIFO_RESET:
|
||||
|
||||
_state = STATE::FIFO_READ;
|
||||
FIFOReset();
|
||||
|
||||
if (DataReadyInterruptConfigure()) {
|
||||
_data_ready_interrupt_enabled = true;
|
||||
|
||||
// backup schedule as a watchdog timeout
|
||||
ScheduleDelayed(100_ms);
|
||||
|
||||
} else {
|
||||
_data_ready_interrupt_enabled = false;
|
||||
ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us);
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case STATE::FIFO_READ: {
|
||||
hrt_abstime timestamp_sample = now;
|
||||
uint8_t samples = 0;
|
||||
|
||||
if (_data_ready_interrupt_enabled) {
|
||||
// scheduled from interrupt if _drdy_timestamp_sample was set as expected
|
||||
const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
|
||||
|
||||
if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
|
||||
timestamp_sample = drdy_timestamp_sample;
|
||||
samples = _fifo_gyro_samples;
|
||||
|
||||
} else {
|
||||
perf_count(_drdy_missed_perf);
|
||||
}
|
||||
|
||||
// push backup schedule back
|
||||
ScheduleDelayed(_fifo_empty_interval_us * 2);
|
||||
}
|
||||
|
||||
if (samples == 0) {
|
||||
// check current FIFO count
|
||||
const uint16_t fifo_count = FIFOReadCount();
|
||||
|
||||
if (fifo_count >= FIFO::SIZE) {
|
||||
FIFOReset();
|
||||
perf_count(_fifo_overflow_perf);
|
||||
|
||||
} else if (fifo_count == 0) {
|
||||
perf_count(_fifo_empty_perf);
|
||||
|
||||
} else {
|
||||
// FIFO count (size in bytes)
|
||||
samples = (fifo_count / sizeof(FIFO::DATA));
|
||||
|
||||
// tolerate minor jitter, leave sample to next iteration if behind by only 1
|
||||
if (samples == _fifo_gyro_samples + 1) {
|
||||
timestamp_sample -= static_cast<int>(FIFO_SAMPLE_DT);
|
||||
samples--;
|
||||
}
|
||||
|
||||
if (samples > FIFO_MAX_SAMPLES) {
|
||||
// not technically an overflow, but more samples than we expected or can publish
|
||||
FIFOReset();
|
||||
perf_count(_fifo_overflow_perf);
|
||||
samples = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool success = false;
|
||||
|
||||
if (samples >= 1) {
|
||||
if (FIFORead(timestamp_sample, samples)) {
|
||||
success = true;
|
||||
|
||||
if (_failure_count > 0) {
|
||||
_failure_count--;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
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_bank0_cfg[_checked_register_bank0])
|
||||
&& RegisterCheck(_register_bank1_cfg[_checked_register_bank1])
|
||||
&& RegisterCheck(_register_bank2_cfg[_checked_register_bank2])
|
||||
) {
|
||||
_last_config_check_timestamp = now;
|
||||
_checked_register_bank0 = (_checked_register_bank0 + 1) % size_register_bank0_cfg;
|
||||
_checked_register_bank1 = (_checked_register_bank1 + 1) % size_register_bank1_cfg;
|
||||
_checked_register_bank2 = (_checked_register_bank2 + 1) % size_register_bank2_cfg;
|
||||
|
||||
} else {
|
||||
// register check failed, force reset
|
||||
perf_count(_bad_register_perf);
|
||||
Reset();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void IIM42653::ConfigureSampleRate(int sample_rate)
|
||||
{
|
||||
// round down to nearest FIFO sample dt
|
||||
const float min_interval = FIFO_SAMPLE_DT;
|
||||
_fifo_empty_interval_us = math::max(roundf((1e6f / (float)sample_rate) / min_interval) * min_interval, min_interval);
|
||||
|
||||
_fifo_gyro_samples = roundf(math::min((float)_fifo_empty_interval_us / (1e6f / GYRO_RATE), (float)FIFO_MAX_SAMPLES));
|
||||
|
||||
// recompute FIFO empty interval (us) with actual gyro sample limit
|
||||
_fifo_empty_interval_us = _fifo_gyro_samples * (1e6f / GYRO_RATE);
|
||||
|
||||
ConfigureFIFOWatermark(_fifo_gyro_samples);
|
||||
}
|
||||
|
||||
void IIM42653::ConfigureFIFOWatermark(uint8_t samples)
|
||||
{
|
||||
// FIFO watermark threshold in number of bytes
|
||||
const uint16_t fifo_watermark_threshold = samples * sizeof(FIFO::DATA);
|
||||
|
||||
for (auto &r : _register_bank0_cfg) {
|
||||
if (r.reg == Register::BANK_0::FIFO_CONFIG2) {
|
||||
// FIFO_WM[7:0] FIFO_CONFIG2
|
||||
r.set_bits = fifo_watermark_threshold & 0xFF;
|
||||
|
||||
} else if (r.reg == Register::BANK_0::FIFO_CONFIG3) {
|
||||
// FIFO_WM[11:8] FIFO_CONFIG3
|
||||
r.set_bits = (fifo_watermark_threshold >> 8) & 0x0F;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void IIM42653::ConfigureCLKIN()
|
||||
{
|
||||
for (auto &r0 : _register_bank0_cfg) {
|
||||
if (r0.reg == Register::BANK_0::INTF_CONFIG1) {
|
||||
r0.set_bits = INTF_CONFIG1_BIT::RTC_MODE;
|
||||
r0.set_bits = INTF_CONFIG1_BIT::CLKSEL;
|
||||
r0.clear_bits = INTF_CONFIG1_BIT::CLKSEL_CLEAR;
|
||||
}
|
||||
}
|
||||
|
||||
for (auto &r1 : _register_bank1_cfg) {
|
||||
if (r1.reg == Register::BANK_1::INTF_CONFIG5) {
|
||||
r1.set_bits = INTF_CONFIG5_BIT::PIN9_FUNCTION_CLKIN_SET;
|
||||
r1.clear_bits = INTF_CONFIG5_BIT::PIN9_FUNCTION_CLKIN_CLEAR;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void IIM42653::SelectRegisterBank(enum REG_BANK_SEL_BIT bank, bool force)
|
||||
{
|
||||
if (bank != _last_register_bank || force) {
|
||||
// select BANK_0
|
||||
uint8_t cmd_bank_sel[2] {};
|
||||
cmd_bank_sel[0] = static_cast<uint8_t>(Register::BANK_0::REG_BANK_SEL);
|
||||
cmd_bank_sel[1] = bank;
|
||||
transfer(cmd_bank_sel, cmd_bank_sel, sizeof(cmd_bank_sel));
|
||||
|
||||
_last_register_bank = bank;
|
||||
}
|
||||
}
|
||||
|
||||
bool IIM42653::Configure()
|
||||
{
|
||||
// first set and clear all configured register bits
|
||||
for (const auto ®_cfg : _register_bank0_cfg) {
|
||||
RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
|
||||
}
|
||||
|
||||
for (const auto ®_cfg : _register_bank1_cfg) {
|
||||
RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
|
||||
}
|
||||
|
||||
for (const auto ®_cfg : _register_bank2_cfg) {
|
||||
RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
|
||||
}
|
||||
|
||||
// now check that all are configured
|
||||
bool success = true;
|
||||
|
||||
for (const auto ®_cfg : _register_bank0_cfg) {
|
||||
if (!RegisterCheck(reg_cfg)) {
|
||||
success = false;
|
||||
}
|
||||
}
|
||||
|
||||
for (const auto ®_cfg : _register_bank1_cfg) {
|
||||
if (!RegisterCheck(reg_cfg)) {
|
||||
success = false;
|
||||
}
|
||||
}
|
||||
|
||||
for (const auto ®_cfg : _register_bank2_cfg) {
|
||||
if (!RegisterCheck(reg_cfg)) {
|
||||
success = false;
|
||||
}
|
||||
}
|
||||
|
||||
// 20-bits data format used
|
||||
// the only FSR settings that are operational are ±2000dps for gyroscope and ±16g for accelerometer
|
||||
_px4_accel.set_range(16.f * CONSTANTS_ONE_G);
|
||||
_px4_gyro.set_range(math::radians(2000.f));
|
||||
|
||||
return success;
|
||||
}
|
||||
|
||||
int IIM42653::DataReadyInterruptCallback(int irq, void *context, void *arg)
|
||||
{
|
||||
static_cast<IIM42653 *>(arg)->DataReady();
|
||||
return 0;
|
||||
}
|
||||
|
||||
void IIM42653::DataReady()
|
||||
{
|
||||
_drdy_timestamp_sample.store(hrt_absolute_time());
|
||||
ScheduleNow();
|
||||
}
|
||||
|
||||
bool IIM42653::DataReadyInterruptConfigure()
|
||||
{
|
||||
if (_drdy_gpio == 0) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// Setup data ready on falling edge
|
||||
return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &DataReadyInterruptCallback, this) == 0;
|
||||
}
|
||||
|
||||
bool IIM42653::DataReadyInterruptDisable()
|
||||
{
|
||||
if (_drdy_gpio == 0) {
|
||||
return false;
|
||||
}
|
||||
|
||||
return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
bool IIM42653::RegisterCheck(const T ®_cfg)
|
||||
{
|
||||
bool success = true;
|
||||
|
||||
const uint8_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;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
uint8_t IIM42653::RegisterRead(T reg)
|
||||
{
|
||||
uint8_t cmd[2] {};
|
||||
cmd[0] = static_cast<uint8_t>(reg) | DIR_READ;
|
||||
SelectRegisterBank(reg);
|
||||
transfer(cmd, cmd, sizeof(cmd));
|
||||
return cmd[1];
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
void IIM42653::RegisterWrite(T reg, uint8_t value)
|
||||
{
|
||||
uint8_t cmd[2] { (uint8_t)reg, value };
|
||||
SelectRegisterBank(reg);
|
||||
transfer(cmd, cmd, sizeof(cmd));
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
void IIM42653::RegisterSetAndClearBits(T reg, uint8_t setbits, uint8_t clearbits)
|
||||
{
|
||||
const uint8_t orig_val = RegisterRead(reg);
|
||||
|
||||
uint8_t val = (orig_val & ~clearbits) | setbits;
|
||||
|
||||
if (orig_val != val) {
|
||||
RegisterWrite(reg, val);
|
||||
}
|
||||
}
|
||||
|
||||
uint16_t IIM42653::FIFOReadCount()
|
||||
{
|
||||
// read FIFO count
|
||||
uint8_t fifo_count_buf[3] {};
|
||||
fifo_count_buf[0] = static_cast<uint8_t>(Register::BANK_0::FIFO_COUNTH) | DIR_READ;
|
||||
SelectRegisterBank(REG_BANK_SEL_BIT::BANK_SEL_0);
|
||||
|
||||
if (transfer(fifo_count_buf, fifo_count_buf, sizeof(fifo_count_buf)) != PX4_OK) {
|
||||
perf_count(_bad_transfer_perf);
|
||||
return 0;
|
||||
}
|
||||
|
||||
return combine(fifo_count_buf[1], fifo_count_buf[2]);
|
||||
}
|
||||
|
||||
bool IIM42653::FIFORead(const hrt_abstime ×tamp_sample, uint8_t samples)
|
||||
{
|
||||
FIFOTransferBuffer buffer{};
|
||||
const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 4, FIFO::SIZE);
|
||||
SelectRegisterBank(REG_BANK_SEL_BIT::BANK_SEL_0);
|
||||
|
||||
if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) {
|
||||
perf_count(_bad_transfer_perf);
|
||||
return false;
|
||||
}
|
||||
|
||||
if (buffer.INT_STATUS & INT_STATUS_BIT::FIFO_FULL_INT) {
|
||||
perf_count(_fifo_overflow_perf);
|
||||
FIFOReset();
|
||||
return false;
|
||||
}
|
||||
|
||||
const uint16_t fifo_count_bytes = combine(buffer.FIFO_COUNTH, buffer.FIFO_COUNTL);
|
||||
|
||||
if (fifo_count_bytes >= FIFO::SIZE) {
|
||||
perf_count(_fifo_overflow_perf);
|
||||
FIFOReset();
|
||||
return false;
|
||||
}
|
||||
|
||||
const uint8_t fifo_count_samples = fifo_count_bytes / sizeof(FIFO::DATA);
|
||||
|
||||
if (fifo_count_samples == 0) {
|
||||
perf_count(_fifo_empty_perf);
|
||||
return false;
|
||||
}
|
||||
|
||||
// check FIFO header in every sample
|
||||
uint8_t valid_samples = 0;
|
||||
|
||||
for (int i = 0; i < math::min(samples, fifo_count_samples); i++) {
|
||||
bool valid = true;
|
||||
|
||||
// With FIFO_ACCEL_EN and FIFO_GYRO_EN header should be 8’b_0110_10xx
|
||||
const uint8_t FIFO_HEADER = buffer.f[i].FIFO_Header;
|
||||
|
||||
if (FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_MSG) {
|
||||
// FIFO sample empty if HEADER_MSG set
|
||||
valid = false;
|
||||
|
||||
} else if (!(FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ACCEL)) {
|
||||
// accel bit not set
|
||||
valid = false;
|
||||
|
||||
} else if (!(FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_GYRO)) {
|
||||
// gyro bit not set
|
||||
valid = false;
|
||||
|
||||
} else if (!(FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_20)) {
|
||||
// Packet does not contain a new and valid extended 20-bit data
|
||||
valid = false;
|
||||
|
||||
} else if ((FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_TIMESTAMP_FSYNC) != Bit3) {
|
||||
// Packet does not contain ODR timestamp
|
||||
valid = false;
|
||||
|
||||
} else if (FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ODR_ACCEL) {
|
||||
// accel ODR changed
|
||||
valid = false;
|
||||
|
||||
} else if (FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ODR_GYRO) {
|
||||
// gyro ODR changed
|
||||
valid = false;
|
||||
}
|
||||
|
||||
if (valid) {
|
||||
valid_samples++;
|
||||
|
||||
} else {
|
||||
perf_count(_bad_transfer_perf);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (valid_samples > 0) {
|
||||
if (ProcessTemperature(buffer.f, valid_samples)) {
|
||||
ProcessGyro(timestamp_sample, buffer.f, valid_samples);
|
||||
ProcessAccel(timestamp_sample, buffer.f, valid_samples);
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
void IIM42653::FIFOReset()
|
||||
{
|
||||
perf_count(_fifo_reset_perf);
|
||||
|
||||
// SIGNAL_PATH_RESET: FIFO flush
|
||||
RegisterSetBits(Register::BANK_0::SIGNAL_PATH_RESET, SIGNAL_PATH_RESET_BIT::FIFO_FLUSH);
|
||||
|
||||
// reset while FIFO is disabled
|
||||
_drdy_timestamp_sample.store(0);
|
||||
}
|
||||
|
||||
static constexpr int32_t reassemble_20bit(const uint32_t a, const uint32_t b, const uint32_t c)
|
||||
{
|
||||
// 0xXXXAABBC
|
||||
uint32_t high = ((a << 12) & 0x000FF000);
|
||||
uint32_t low = ((b << 4) & 0x00000FF0);
|
||||
uint32_t lowest = (c & 0x0000000F);
|
||||
|
||||
uint32_t x = high | low | lowest;
|
||||
|
||||
if (a & Bit7) {
|
||||
// sign extend
|
||||
x |= 0xFFF00000u;
|
||||
}
|
||||
|
||||
return static_cast<int32_t>(x);
|
||||
}
|
||||
|
||||
void IIM42653::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DATA fifo[], const uint8_t samples)
|
||||
{
|
||||
sensor_accel_fifo_s accel{};
|
||||
accel.timestamp_sample = timestamp_sample;
|
||||
accel.samples = 0;
|
||||
|
||||
// 18-bits of accelerometer data
|
||||
bool scale_20bit = false;
|
||||
|
||||
// first pass
|
||||
for (int i = 0; i < samples; i++) {
|
||||
|
||||
uint16_t timestamp_fifo = combine_uint(fifo[i].TimeStamp_h, fifo[i].TimeStamp_l);
|
||||
|
||||
if (_enable_clock_input) {
|
||||
accel.dt = (float)timestamp_fifo * ((1.f / _input_clock_freq) * 1e6f);
|
||||
|
||||
} else {
|
||||
accel.dt = (float)timestamp_fifo * FIFO_TIMESTAMP_SCALING;
|
||||
}
|
||||
|
||||
// 20 bit hires mode
|
||||
// Sign extension + Accel [19:12] + Accel [11:4] + Accel [3:2] (20 bit extension byte)
|
||||
// Accel data is 18 bit ()
|
||||
int32_t accel_x = reassemble_20bit(fifo[i].ACCEL_DATA_X1, fifo[i].ACCEL_DATA_X0,
|
||||
fifo[i].Ext_Accel_X_Gyro_X & 0xF0 >> 4);
|
||||
int32_t accel_y = reassemble_20bit(fifo[i].ACCEL_DATA_Y1, fifo[i].ACCEL_DATA_Y0,
|
||||
fifo[i].Ext_Accel_Y_Gyro_Y & 0xF0 >> 4);
|
||||
int32_t accel_z = reassemble_20bit(fifo[i].ACCEL_DATA_Z1, fifo[i].ACCEL_DATA_Z0,
|
||||
fifo[i].Ext_Accel_Z_Gyro_Z & 0xF0 >> 4);
|
||||
|
||||
// sample invalid if -524288
|
||||
if (accel_x != -524288 && accel_y != -524288 && accel_z != -524288) {
|
||||
// check if any values are going to exceed int16 limits
|
||||
static constexpr int16_t max_accel = INT16_MAX;
|
||||
static constexpr int16_t min_accel = INT16_MIN;
|
||||
|
||||
if (accel_x >= max_accel || accel_x <= min_accel) {
|
||||
scale_20bit = true;
|
||||
}
|
||||
|
||||
if (accel_y >= max_accel || accel_y <= min_accel) {
|
||||
scale_20bit = true;
|
||||
}
|
||||
|
||||
if (accel_z >= max_accel || accel_z <= min_accel) {
|
||||
scale_20bit = true;
|
||||
}
|
||||
|
||||
// shift by 2 (2 least significant bits are always 0)
|
||||
accel.x[accel.samples] = accel_x / 4;
|
||||
accel.y[accel.samples] = accel_y / 4;
|
||||
accel.z[accel.samples] = accel_z / 4;
|
||||
accel.samples++;
|
||||
}
|
||||
}
|
||||
|
||||
if (!scale_20bit) {
|
||||
// if highres enabled accel data is always 4096 LSB/g
|
||||
_px4_accel.set_scale(CONSTANTS_ONE_G / 4096.f);
|
||||
|
||||
} else {
|
||||
// 20 bit data scaled to 16 bit (2^4)
|
||||
for (int i = 0; i < samples; i++) {
|
||||
// 20 bit hires mode
|
||||
// Sign extension + Accel [19:12] + Accel [11:4] + Accel [3:2] (20 bit extension byte)
|
||||
// Accel data is 18 bit ()
|
||||
int16_t accel_x = combine(fifo[i].ACCEL_DATA_X1, fifo[i].ACCEL_DATA_X0);
|
||||
int16_t accel_y = combine(fifo[i].ACCEL_DATA_Y1, fifo[i].ACCEL_DATA_Y0);
|
||||
int16_t accel_z = combine(fifo[i].ACCEL_DATA_Z1, fifo[i].ACCEL_DATA_Z0);
|
||||
|
||||
accel.x[i] = accel_x;
|
||||
accel.y[i] = accel_y;
|
||||
accel.z[i] = accel_z;
|
||||
}
|
||||
|
||||
_px4_accel.set_scale(CONSTANTS_ONE_G / 1024.f);
|
||||
}
|
||||
|
||||
// correct frame for publication
|
||||
for (int i = 0; i < accel.samples; i++) {
|
||||
// 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)
|
||||
accel.x[i] = accel.x[i];
|
||||
accel.y[i] = (accel.y[i] == INT16_MIN) ? INT16_MAX : -accel.y[i];
|
||||
accel.z[i] = (accel.z[i] == INT16_MIN) ? INT16_MAX : -accel.z[i];
|
||||
}
|
||||
|
||||
_px4_accel.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
|
||||
perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf));
|
||||
|
||||
if (accel.samples > 0) {
|
||||
_px4_accel.updateFIFO(accel);
|
||||
}
|
||||
}
|
||||
|
||||
void IIM42653::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::DATA fifo[], const uint8_t samples)
|
||||
{
|
||||
sensor_gyro_fifo_s gyro{};
|
||||
gyro.timestamp_sample = timestamp_sample;
|
||||
gyro.samples = 0;
|
||||
|
||||
// 20-bits of gyroscope data
|
||||
bool scale_20bit = false;
|
||||
|
||||
// first pass
|
||||
for (int i = 0; i < samples; i++) {
|
||||
|
||||
uint16_t timestamp_fifo = combine_uint(fifo[i].TimeStamp_h, fifo[i].TimeStamp_l);
|
||||
|
||||
if (_enable_clock_input) {
|
||||
gyro.dt = (float)timestamp_fifo * ((1.f / _input_clock_freq) * 1e6f);
|
||||
|
||||
} else {
|
||||
gyro.dt = (float)timestamp_fifo * FIFO_TIMESTAMP_SCALING;
|
||||
}
|
||||
|
||||
// 20 bit hires mode
|
||||
// Gyro [19:12] + Gyro [11:4] + Gyro [3:0] (bottom 4 bits of 20 bit extension byte)
|
||||
int32_t gyro_x = reassemble_20bit(fifo[i].GYRO_DATA_X1, fifo[i].GYRO_DATA_X0, fifo[i].Ext_Accel_X_Gyro_X & 0x0F);
|
||||
int32_t gyro_y = reassemble_20bit(fifo[i].GYRO_DATA_Y1, fifo[i].GYRO_DATA_Y0, fifo[i].Ext_Accel_Y_Gyro_Y & 0x0F);
|
||||
int32_t gyro_z = reassemble_20bit(fifo[i].GYRO_DATA_Z1, fifo[i].GYRO_DATA_Z0, fifo[i].Ext_Accel_Z_Gyro_Z & 0x0F);
|
||||
|
||||
// check if any values are going to exceed int16 limits
|
||||
static constexpr int16_t max_gyro = INT16_MAX;
|
||||
static constexpr int16_t min_gyro = INT16_MIN;
|
||||
|
||||
if (gyro_x >= max_gyro || gyro_x <= min_gyro) {
|
||||
scale_20bit = true;
|
||||
}
|
||||
|
||||
if (gyro_y >= max_gyro || gyro_y <= min_gyro) {
|
||||
scale_20bit = true;
|
||||
}
|
||||
|
||||
if (gyro_z >= max_gyro || gyro_z <= min_gyro) {
|
||||
scale_20bit = true;
|
||||
}
|
||||
|
||||
gyro.x[gyro.samples] = gyro_x / 2;
|
||||
gyro.y[gyro.samples] = gyro_y / 2;
|
||||
gyro.z[gyro.samples] = gyro_z / 2;
|
||||
gyro.samples++;
|
||||
}
|
||||
|
||||
if (!scale_20bit) {
|
||||
// if highres enabled gyro data is always 65.5 LSB/dps
|
||||
_px4_gyro.set_scale(math::radians(1.f / 65.5f));
|
||||
|
||||
} else {
|
||||
// 20 bit data scaled to 16 bit (2^4)
|
||||
for (int i = 0; i < samples; i++) {
|
||||
gyro.x[i] = combine(fifo[i].GYRO_DATA_X1, fifo[i].GYRO_DATA_X0);
|
||||
gyro.y[i] = combine(fifo[i].GYRO_DATA_Y1, fifo[i].GYRO_DATA_Y0);
|
||||
gyro.z[i] = combine(fifo[i].GYRO_DATA_Z1, fifo[i].GYRO_DATA_Z0);
|
||||
}
|
||||
|
||||
_px4_gyro.set_scale(math::radians(4000.f / 32768.f));
|
||||
}
|
||||
|
||||
// correct frame for publication
|
||||
for (int i = 0; i < gyro.samples; i++) {
|
||||
// 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)
|
||||
gyro.x[i] = gyro.x[i];
|
||||
gyro.y[i] = (gyro.y[i] == INT16_MIN) ? INT16_MAX : -gyro.y[i];
|
||||
gyro.z[i] = (gyro.z[i] == INT16_MIN) ? INT16_MAX : -gyro.z[i];
|
||||
}
|
||||
|
||||
_px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
|
||||
perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf));
|
||||
|
||||
if (gyro.samples > 0) {
|
||||
_px4_gyro.updateFIFO(gyro);
|
||||
}
|
||||
}
|
||||
|
||||
bool IIM42653::ProcessTemperature(const FIFO::DATA fifo[], const uint8_t samples)
|
||||
{
|
||||
int16_t temperature[FIFO_MAX_SAMPLES];
|
||||
float temperature_sum{0};
|
||||
|
||||
int valid_samples = 0;
|
||||
|
||||
for (int i = 0; i < samples; i++) {
|
||||
const int16_t t = combine(fifo[i].TEMP_DATA1, fifo[i].TEMP_DATA0);
|
||||
|
||||
// sample invalid if -32768
|
||||
if (t != -32768) {
|
||||
temperature_sum += t;
|
||||
temperature[valid_samples] = t;
|
||||
valid_samples++;
|
||||
}
|
||||
}
|
||||
|
||||
if (valid_samples > 0) {
|
||||
const float temperature_avg = temperature_sum / valid_samples;
|
||||
|
||||
for (int i = 0; i < valid_samples; i++) {
|
||||
// temperature changing wildly is an indication of a transfer error
|
||||
if (fabsf(temperature[i] - temperature_avg) > 1000) {
|
||||
perf_count(_bad_transfer_perf);
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
// use average temperature reading
|
||||
const float TEMP_degC = (temperature_avg / TEMPERATURE_SENSITIVITY) + TEMPERATURE_OFFSET;
|
||||
|
||||
if (PX4_ISFINITE(TEMP_degC)) {
|
||||
_px4_accel.set_temperature(TEMP_degC);
|
||||
_px4_gyro.set_temperature(TEMP_degC);
|
||||
return true;
|
||||
|
||||
} else {
|
||||
perf_count(_bad_transfer_perf);
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
|
@ -0,0 +1,227 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2023 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 IIM42653.hpp
|
||||
*
|
||||
* Driver for the Invensense IIM42653 connected via SPI.
|
||||
*
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "InvenSense_IIM42653_registers.hpp"
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
|
||||
#include <lib/drivers/device/spi.h>
|
||||
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
|
||||
#include <lib/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 InvenSense_IIM42653;
|
||||
|
||||
class IIM42653 : public device::SPI, public I2CSPIDriver<IIM42653>
|
||||
{
|
||||
public:
|
||||
IIM42653(const I2CSPIDriverConfig &config);
|
||||
~IIM42653() override;
|
||||
|
||||
static void print_usage();
|
||||
|
||||
void RunImpl();
|
||||
|
||||
int init() override;
|
||||
void print_status() override;
|
||||
|
||||
private:
|
||||
void exit_and_cleanup() override;
|
||||
|
||||
// Sensor Configuration
|
||||
static constexpr float FIFO_SAMPLE_DT{1e6f / 8000.f}; // 8000 Hz accel & gyro ODR configured
|
||||
static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT};
|
||||
static constexpr float ACCEL_RATE{1e6f / FIFO_SAMPLE_DT};
|
||||
|
||||
static constexpr float FIFO_TIMESTAMP_SCALING{16.f *(32.f / 30.f)}; // Used when not using clock input
|
||||
|
||||
// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
|
||||
static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
|
||||
|
||||
// Transfer data
|
||||
struct FIFOTransferBuffer {
|
||||
uint8_t cmd{static_cast<uint8_t>(Register::BANK_0::INT_STATUS) | DIR_READ};
|
||||
uint8_t INT_STATUS{0};
|
||||
uint8_t FIFO_COUNTH{0};
|
||||
uint8_t FIFO_COUNTL{0};
|
||||
FIFO::DATA f[FIFO_MAX_SAMPLES] {};
|
||||
};
|
||||
// ensure no struct padding
|
||||
static_assert(sizeof(FIFOTransferBuffer) == (4 + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)));
|
||||
|
||||
struct register_bank0_config_t {
|
||||
Register::BANK_0 reg;
|
||||
uint8_t set_bits{0};
|
||||
uint8_t clear_bits{0};
|
||||
};
|
||||
|
||||
struct register_bank1_config_t {
|
||||
Register::BANK_1 reg;
|
||||
uint8_t set_bits{0};
|
||||
uint8_t clear_bits{0};
|
||||
};
|
||||
|
||||
struct register_bank2_config_t {
|
||||
Register::BANK_2 reg;
|
||||
uint8_t set_bits{0};
|
||||
uint8_t clear_bits{0};
|
||||
};
|
||||
|
||||
int probe() override;
|
||||
|
||||
bool Reset();
|
||||
|
||||
bool Configure();
|
||||
void ConfigureSampleRate(int sample_rate);
|
||||
void ConfigureFIFOWatermark(uint8_t samples);
|
||||
void ConfigureCLKIN();
|
||||
|
||||
void SelectRegisterBank(enum REG_BANK_SEL_BIT bank, bool force = false);
|
||||
void SelectRegisterBank(Register::BANK_0 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::BANK_SEL_0); }
|
||||
void SelectRegisterBank(Register::BANK_1 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::BANK_SEL_1); }
|
||||
void SelectRegisterBank(Register::BANK_2 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::BANK_SEL_2); }
|
||||
|
||||
static int DataReadyInterruptCallback(int irq, void *context, void *arg);
|
||||
void DataReady();
|
||||
bool DataReadyInterruptConfigure();
|
||||
bool DataReadyInterruptDisable();
|
||||
|
||||
template <typename T> bool RegisterCheck(const T ®_cfg);
|
||||
template <typename T> uint8_t RegisterRead(T reg);
|
||||
template <typename T> void RegisterWrite(T reg, uint8_t value);
|
||||
template <typename T> void RegisterSetAndClearBits(T reg, uint8_t setbits, uint8_t clearbits);
|
||||
template <typename T> void RegisterSetBits(T reg, uint8_t setbits) { RegisterSetAndClearBits(reg, setbits, 0); }
|
||||
template <typename T> void RegisterClearBits(T reg, uint8_t clearbits) { RegisterSetAndClearBits(reg, 0, clearbits); }
|
||||
|
||||
uint16_t FIFOReadCount();
|
||||
bool FIFORead(const hrt_abstime ×tamp_sample, uint8_t samples);
|
||||
void FIFOReset();
|
||||
|
||||
void ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DATA fifo[], const uint8_t samples);
|
||||
void ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::DATA fifo[], const uint8_t samples);
|
||||
bool ProcessTemperature(const FIFO::DATA fifo[], const uint8_t samples);
|
||||
|
||||
const spi_drdy_gpio_t _drdy_gpio;
|
||||
|
||||
PX4Accelerometer _px4_accel;
|
||||
PX4Gyroscope _px4_gyro;
|
||||
|
||||
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")};
|
||||
perf_counter_t _fifo_empty_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO empty")};
|
||||
perf_counter_t _fifo_overflow_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO overflow")};
|
||||
perf_counter_t _fifo_reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO reset")};
|
||||
perf_counter_t _drdy_missed_perf{nullptr};
|
||||
|
||||
hrt_abstime _reset_timestamp{0};
|
||||
hrt_abstime _last_config_check_timestamp{0};
|
||||
hrt_abstime _temperature_update_timestamp{0};
|
||||
int _failure_count{0};
|
||||
|
||||
bool _enable_clock_input{false};
|
||||
float _input_clock_freq{0.f};
|
||||
|
||||
enum REG_BANK_SEL_BIT _last_register_bank {REG_BANK_SEL_BIT::BANK_SEL_0};
|
||||
|
||||
px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
|
||||
bool _data_ready_interrupt_enabled{false};
|
||||
|
||||
enum class STATE : uint8_t {
|
||||
RESET,
|
||||
WAIT_FOR_RESET,
|
||||
CONFIGURE,
|
||||
FIFO_RESET,
|
||||
FIFO_READ,
|
||||
} _state{STATE::RESET};
|
||||
|
||||
uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
|
||||
int32_t _fifo_gyro_samples{static_cast<int32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
|
||||
|
||||
uint8_t _checked_register_bank0{0};
|
||||
static constexpr uint8_t size_register_bank0_cfg{16};
|
||||
register_bank0_config_t _register_bank0_cfg[size_register_bank0_cfg] {
|
||||
// Register | Set bits, Clear bits
|
||||
{ Register::BANK_0::INT_CONFIG, INT_CONFIG_BIT::INT1_MODE | INT_CONFIG_BIT::INT1_DRIVE_CIRCUIT, INT_CONFIG_BIT::INT1_POLARITY },
|
||||
{ Register::BANK_0::FIFO_CONFIG, FIFO_CONFIG_BIT::FIFO_MODE_STOP_ON_FULL, 0 },
|
||||
{ Register::BANK_0::INTF_CONFIG1, 0, 0}, // RTC_MODE[2] set at runtime
|
||||
{ Register::BANK_0::PWR_MGMT0, PWR_MGMT0_BIT::GYRO_MODE_LOW_NOISE | PWR_MGMT0_BIT::ACCEL_MODE_LOW_NOISE, 0 },
|
||||
{ Register::BANK_0::GYRO_CONFIG0, GYRO_CONFIG0_BIT::GYRO_FS_SEL_4000_DPS | GYRO_CONFIG0_BIT::GYRO_ODR_8KHZ_SET, GYRO_CONFIG0_BIT::GYRO_ODR_8KHZ_CLEAR },
|
||||
{ Register::BANK_0::ACCEL_CONFIG0, ACCEL_CONFIG0_BIT::ACCEL_FS_SEL_32G | ACCEL_CONFIG0_BIT::ACCEL_ODR_8KHZ_SET, ACCEL_CONFIG0_BIT::ACCEL_ODR_8KHZ_CLEAR },
|
||||
{ Register::BANK_0::GYRO_CONFIG1, 0, GYRO_CONFIG1_BIT::GYRO_UI_FILT_ORD },
|
||||
{ Register::BANK_0::GYRO_ACCEL_CONFIG0, 0, GYRO_ACCEL_CONFIG0_BIT::ACCEL_UI_FILT_BW | GYRO_ACCEL_CONFIG0_BIT::GYRO_UI_FILT_BW },
|
||||
{ Register::BANK_0::ACCEL_CONFIG1, 0, ACCEL_CONFIG1_BIT::ACCEL_UI_FILT_ORD },
|
||||
{ Register::BANK_0::TMST_CONFIG, TMST_CONFIG_BIT::TMST_EN | TMST_CONFIG_BIT::TMST_DELTA_EN | TMST_CONFIG_BIT::TMST_TO_REGS_EN | TMST_CONFIG_BIT::TMST_RES, TMST_CONFIG_BIT::TMST_FSYNC_EN },
|
||||
{ Register::BANK_0::FIFO_CONFIG1, FIFO_CONFIG1_BIT::FIFO_WM_GT_TH | FIFO_CONFIG1_BIT::FIFO_HIRES_EN | FIFO_CONFIG1_BIT::FIFO_TEMP_EN | FIFO_CONFIG1_BIT::FIFO_GYRO_EN | FIFO_CONFIG1_BIT::FIFO_ACCEL_EN, FIFO_CONFIG1_BIT::FIFO_TMST_FSYNC_EN },
|
||||
{ Register::BANK_0::FIFO_CONFIG2, 0, 0 }, // FIFO_WM[7:0] set at runtime
|
||||
{ Register::BANK_0::FIFO_CONFIG3, 0, 0 }, // FIFO_WM[11:8] set at runtime
|
||||
{ Register::BANK_0::INT_CONFIG0, INT_CONFIG0_BIT::CLEAR_ON_FIFO_READ, 0 },
|
||||
{ Register::BANK_0::INT_CONFIG1, 0, INT_CONFIG1_BIT::INT_ASYNC_RESET },
|
||||
{ Register::BANK_0::INT_SOURCE0, INT_SOURCE0_BIT::FIFO_THS_INT1_EN, 0 },
|
||||
};
|
||||
|
||||
uint8_t _checked_register_bank1{0};
|
||||
static constexpr uint8_t size_register_bank1_cfg{5};
|
||||
register_bank1_config_t _register_bank1_cfg[size_register_bank1_cfg] {
|
||||
// Register | Set bits, Clear bits
|
||||
{ Register::BANK_1::GYRO_CONFIG_STATIC2, 0, GYRO_CONFIG_STATIC2_BIT::GYRO_NF_DIS | GYRO_CONFIG_STATIC2_BIT::GYRO_AAF_DIS },
|
||||
{ Register::BANK_1::GYRO_CONFIG_STATIC3, GYRO_CONFIG_STATIC3_BIT::GYRO_AAF_DELT_585HZ_SET, GYRO_CONFIG_STATIC3_BIT::GYRO_AAF_DELT_585HZ_CLEAR},
|
||||
{ Register::BANK_1::GYRO_CONFIG_STATIC4, GYRO_CONFIG_STATIC4_BIT::GYRO_AAF_DELTSQR_LSB_585HZ_SET, GYRO_CONFIG_STATIC4_BIT::GYRO_AAF_DELTSQR_LSB_585HZ_CLEAR},
|
||||
{ Register::BANK_1::GYRO_CONFIG_STATIC5, GYRO_CONFIG_STATIC5_BIT::GYRO_AAF_BITSHIFT_585HZ_SET | GYRO_CONFIG_STATIC5_BIT::GYRO_AAF_DELTSQR_MSB_585HZ_SET, GYRO_CONFIG_STATIC5_BIT::GYRO_AAF_BITSHIFT_585HZ_CLEAR | GYRO_CONFIG_STATIC5_BIT::GYRO_AAF_DELTSQR_MSB_585HZ_CLEAR},
|
||||
{ Register::BANK_1::INTF_CONFIG5, 0, 0 },
|
||||
};
|
||||
|
||||
uint8_t _checked_register_bank2{0};
|
||||
static constexpr uint8_t size_register_bank2_cfg{8};
|
||||
register_bank2_config_t _register_bank2_cfg[size_register_bank2_cfg] {
|
||||
// Register | Set bits, Clear bits
|
||||
{ Register::BANK_2::ACCEL_CONFIG_STATIC2, ACCEL_CONFIG_STATIC2_BIT::ACCEL_AAF_DELT_585HZ_SET, ACCEL_CONFIG_STATIC2_BIT::ACCEL_AAF_DELT_585HZ_CLEAR | ACCEL_CONFIG_STATIC2_BIT::ACCEL_AAF_DIS },
|
||||
{ Register::BANK_2::ACCEL_CONFIG_STATIC3, ACCEL_CONFIG_STATIC3_BIT::ACCEL_AAF_DELTSQR_LSB_585HZ_SET, ACCEL_CONFIG_STATIC3_BIT::ACCEL_AAF_DELTSQR_LSB_585HZ_CLEAR },
|
||||
{ Register::BANK_2::ACCEL_CONFIG_STATIC4, ACCEL_CONFIG_STATIC4_BIT::ACCEL_AAF_BITSHIFT_585HZ_SET | ACCEL_CONFIG_STATIC4_BIT::ACCEL_AAF_DELTSQR_MSB_SET, ACCEL_CONFIG_STATIC4_BIT::ACCEL_AAF_BITSHIFT_585HZ_CLEAR | ACCEL_CONFIG_STATIC4_BIT::ACCEL_AAF_DELTSQR_MSB_CLEAR },
|
||||
{ Register::BANK_2::AUX1_CONFIG1, 0, AUX1_CONFIG1_BIT::AUX1_ACCEL_LP_CLK_SEL | AUX1_CONFIG1_BIT::GYRO_AUX1_EN | AUX1_CONFIG1_BIT::ACCEL_AUX1_EN},
|
||||
{ Register::BANK_2::AUX1_CONFIG2, AUX1_CONFIG2_BIT::GYRO_AUX1_HPF_DIS, 0},
|
||||
{ Register::BANK_2::AUX1_SPI_REG1, AUX1_SPI_REG1_BIT::AUX1_SPI_REG1_SET, AUX1_SPI_REG1_BIT::AUX1_SPI_REG1_CLEAR },
|
||||
{ Register::BANK_2::AUX1_SPI_REG2, AUX1_SPI_REG2_BIT::AUX1_SPI_REG2_SET, AUX1_SPI_REG2_BIT::AUX1_SPI_REG2_CLEAR },
|
||||
{ Register::BANK_2::AUX1_SPI_REG3, AUX1_SPI_REG3_BIT::AUX1_SPI_REG3_SET, AUX1_SPI_REG3_BIT::AUX1_SPI_REG3_CLEAR },
|
||||
};
|
||||
};
|
|
@ -0,0 +1,453 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2023 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 InvenSense_IIM42653_registers.hpp
|
||||
*
|
||||
* Invensense IIM-42653 registers.
|
||||
*
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <cstdint>
|
||||
#include <cstddef>
|
||||
|
||||
namespace InvenSense_IIM42653
|
||||
{
|
||||
// TODO: move to a central header
|
||||
static constexpr uint8_t Bit0 = (1 << 0);
|
||||
static constexpr uint8_t Bit1 = (1 << 1);
|
||||
static constexpr uint8_t Bit2 = (1 << 2);
|
||||
static constexpr uint8_t Bit3 = (1 << 3);
|
||||
static constexpr uint8_t Bit4 = (1 << 4);
|
||||
static constexpr uint8_t Bit5 = (1 << 5);
|
||||
static constexpr uint8_t Bit6 = (1 << 6);
|
||||
static constexpr uint8_t Bit7 = (1 << 7);
|
||||
|
||||
static constexpr uint32_t SPI_SPEED = 24 * 1000 * 1000; // 24 MHz SPI
|
||||
static constexpr uint8_t DIR_READ = 0x80;
|
||||
|
||||
static constexpr uint8_t WHOAMI = 0x56;
|
||||
|
||||
static constexpr float TEMPERATURE_SENSITIVITY = 132.48f; // LSB/C
|
||||
static constexpr float TEMPERATURE_OFFSET = 25.f; // C
|
||||
|
||||
namespace Register
|
||||
{
|
||||
|
||||
enum class BANK_0 : uint8_t {
|
||||
DEVICE_CONFIG = 0x11,
|
||||
|
||||
INT_CONFIG = 0x14,
|
||||
|
||||
FIFO_CONFIG = 0x16,
|
||||
|
||||
TEMP_DATA1 = 0x1D,
|
||||
TEMP_DATA0 = 0x1E,
|
||||
|
||||
INT_STATUS = 0x2D,
|
||||
FIFO_COUNTH = 0x2E,
|
||||
FIFO_COUNTL = 0x2F,
|
||||
FIFO_DATA = 0x30,
|
||||
|
||||
SIGNAL_PATH_RESET = 0x4B,
|
||||
INTF_CONFIG0 = 0x4C,
|
||||
INTF_CONFIG1 = 0x4D,
|
||||
PWR_MGMT0 = 0x4E,
|
||||
GYRO_CONFIG0 = 0x4F,
|
||||
ACCEL_CONFIG0 = 0x50,
|
||||
GYRO_CONFIG1 = 0x51,
|
||||
GYRO_ACCEL_CONFIG0 = 0x52,
|
||||
ACCEL_CONFIG1 = 0x53,
|
||||
TMST_CONFIG = 0x54,
|
||||
|
||||
FIFO_CONFIG1 = 0x5F,
|
||||
FIFO_CONFIG2 = 0x60,
|
||||
FIFO_CONFIG3 = 0x61,
|
||||
|
||||
INT_CONFIG0 = 0x63,
|
||||
INT_CONFIG1 = 0x64,
|
||||
|
||||
INT_SOURCE0 = 0x65,
|
||||
|
||||
SELF_TEST_CONFIG = 0x70,
|
||||
|
||||
WHO_AM_I = 0x75,
|
||||
REG_BANK_SEL = 0x76,
|
||||
};
|
||||
|
||||
enum class BANK_1 : uint8_t {
|
||||
GYRO_CONFIG_STATIC2 = 0x0B,
|
||||
GYRO_CONFIG_STATIC3 = 0x0C,
|
||||
GYRO_CONFIG_STATIC4 = 0x0D,
|
||||
GYRO_CONFIG_STATIC5 = 0x0E,
|
||||
|
||||
INTF_CONFIG5 = 0x7B,
|
||||
};
|
||||
|
||||
enum class BANK_2 : uint8_t {
|
||||
ACCEL_CONFIG_STATIC2 = 0x03,
|
||||
ACCEL_CONFIG_STATIC3 = 0x04,
|
||||
ACCEL_CONFIG_STATIC4 = 0x05,
|
||||
AUX1_CONFIG1 = 0x44,
|
||||
AUX1_CONFIG2 = 0x45,
|
||||
AUX1_CONFIG3 = 0x46,
|
||||
AUX1_SPI_REG1 = 0x71,
|
||||
AUX1_SPI_REG2 = 0x72,
|
||||
AUX1_SPI_REG3 = 0x73,
|
||||
};
|
||||
|
||||
};
|
||||
|
||||
//---------------- BANK0 Register bits
|
||||
|
||||
// DEVICE_CONFIG
|
||||
enum DEVICE_CONFIG_BIT : uint8_t {
|
||||
SOFT_RESET_CONFIG = Bit0, //
|
||||
};
|
||||
|
||||
// INT_CONFIG
|
||||
enum INT_CONFIG_BIT : uint8_t {
|
||||
INT1_MODE = Bit2,
|
||||
INT1_DRIVE_CIRCUIT = Bit1,
|
||||
INT1_POLARITY = Bit0,
|
||||
};
|
||||
|
||||
// FIFO_CONFIG
|
||||
enum FIFO_CONFIG_BIT : uint8_t {
|
||||
// 7:6 FIFO_MODE
|
||||
FIFO_MODE_STOP_ON_FULL = Bit7 | Bit6, // 11: STOP-on-FULL Mode
|
||||
};
|
||||
|
||||
// INT_STATUS
|
||||
enum INT_STATUS_BIT : uint8_t {
|
||||
RESET_DONE_INT = Bit4,
|
||||
DATA_RDY_INT = Bit3,
|
||||
FIFO_THS_INT = Bit2,
|
||||
FIFO_FULL_INT = Bit1,
|
||||
};
|
||||
|
||||
// SIGNAL_PATH_RESET
|
||||
enum SIGNAL_PATH_RESET_BIT : uint8_t {
|
||||
ABORT_AND_RESET = Bit3,
|
||||
FIFO_FLUSH = Bit1,
|
||||
};
|
||||
|
||||
enum INTF_CONFIG1_BIT : uint8_t {
|
||||
RTC_MODE = Bit2, // 0: No input RTC clock is required, 1: RTC clock input is required
|
||||
CLKSEL = Bit0,
|
||||
CLKSEL_CLEAR = Bit1,
|
||||
};
|
||||
|
||||
// PWR_MGMT0
|
||||
enum PWR_MGMT0_BIT : uint8_t {
|
||||
GYRO_MODE_LOW_NOISE = Bit3 | Bit2, // 11: Places gyroscope in Low Noise (LN) Mode
|
||||
ACCEL_MODE_LOW_NOISE = Bit1 | Bit0, // 11: Places accelerometer in Low Noise (LN) Mode
|
||||
};
|
||||
|
||||
// GYRO_CONFIG0
|
||||
enum GYRO_CONFIG0_BIT : uint8_t {
|
||||
// 7:5 GYRO_FS_SEL
|
||||
GYRO_FS_SEL_4000_DPS = 0, // 0b000 = ±4000dps (default)
|
||||
GYRO_FS_SEL_2000_DPS = Bit5,
|
||||
GYRO_FS_SEL_1000_DPS = Bit6,
|
||||
GYRO_FS_SEL_500_DPS = Bit6 | Bit5,
|
||||
GYRO_FS_SEL_250_DPS = Bit7,
|
||||
|
||||
|
||||
// 3:0 GYRO_ODR
|
||||
// 0001: 32kHz
|
||||
GYRO_ODR_32KHZ_SET = Bit0,
|
||||
GYRO_ODR_32KHZ_CLEAR = Bit3 | Bit2 | Bit0,
|
||||
// 0010: 16kHz
|
||||
GYRO_ODR_16KHZ_SET = Bit1,
|
||||
GYRO_ODR_16KHZ_CLEAR = Bit3 | Bit2 | Bit0,
|
||||
// 0011: 8kHz
|
||||
GYRO_ODR_8KHZ_SET = Bit1 | Bit0,
|
||||
GYRO_ODR_8KHZ_CLEAR = Bit3 | Bit2,
|
||||
// 0110: 1kHz (default)
|
||||
GYRO_ODR_1KHZ_SET = Bit2 | Bit1,
|
||||
GYRO_ODR_1KHZ_CLEAR = Bit3 | Bit0,
|
||||
};
|
||||
|
||||
// ACCEL_CONFIG0
|
||||
enum ACCEL_CONFIG0_BIT : uint8_t {
|
||||
// 7:5 ACCEL_FS_SEL
|
||||
ACCEL_FS_SEL_32G = 0, // 000: ±32g (default)
|
||||
ACCEL_FS_SEL_16G = Bit5,
|
||||
ACCEL_FS_SEL_8G = Bit6,
|
||||
ACCEL_FS_SEL_4G = Bit6 | Bit5,
|
||||
|
||||
|
||||
// 3:0 ACCEL_ODR
|
||||
// 0001: 32kHz
|
||||
ACCEL_ODR_32KHZ_SET = Bit0,
|
||||
ACCEL_ODR_32KHZ_CLEAR = Bit3 | Bit2 | Bit0,
|
||||
// 0010: 16kHz
|
||||
ACCEL_ODR_16KHZ_SET = Bit1,
|
||||
ACCEL_ODR_16KHZ_CLEAR = Bit3 | Bit2 | Bit0,
|
||||
// 0011: 8kHz
|
||||
ACCEL_ODR_8KHZ_SET = Bit1 | Bit0,
|
||||
ACCEL_ODR_8KHZ_CLEAR = Bit3 | Bit2,
|
||||
// 0110: 1kHz (default)
|
||||
ACCEL_ODR_1KHZ_SET = Bit2 | Bit1,
|
||||
ACCEL_ODR_1KHZ_CLEAR = Bit3 | Bit0,
|
||||
};
|
||||
|
||||
// GYRO_CONFIG1
|
||||
enum GYRO_CONFIG1_BIT : uint8_t {
|
||||
GYRO_UI_FILT_ORD = Bit3 | Bit2, // 00: 1st Order
|
||||
};
|
||||
|
||||
// GYRO_ACCEL_CONFIG0
|
||||
enum GYRO_ACCEL_CONFIG0_BIT : uint8_t {
|
||||
// 7:4 ACCEL_UI_FILT_BW
|
||||
ACCEL_UI_FILT_BW = Bit7 | Bit6 | Bit5 | Bit4, // 0: BW=ODR/2
|
||||
|
||||
// 3:0 GYRO_UI_FILT_BW
|
||||
GYRO_UI_FILT_BW = Bit3 | Bit2 | Bit1 | Bit0, // 0: BW=ODR/2
|
||||
};
|
||||
|
||||
// ACCEL_CONFIG1
|
||||
enum ACCEL_CONFIG1_BIT : uint8_t {
|
||||
ACCEL_UI_FILT_ORD = Bit4 | Bit3, // 00: 1st Order
|
||||
};
|
||||
|
||||
// TMST_CONFIG
|
||||
enum TMST_CONFIG_BIT : uint8_t {
|
||||
TMST_TO_REGS_EN = Bit4, // 1: TMST_VALUE[19:0] read returns timestamp value
|
||||
TMST_RES = Bit3, // 0: 1us resolution, 1: 16us resolution
|
||||
TMST_DELTA_EN = Bit2, // 1: Time Stamp delta enable
|
||||
TMST_FSYNC_EN = Bit1, // 1: The contents of the Timestamp feature of FSYNC is enabled
|
||||
TMST_EN = Bit0, // 1: Time Stamp register enable (default)
|
||||
};
|
||||
|
||||
// FIFO_CONFIG1
|
||||
enum FIFO_CONFIG1_BIT : uint8_t {
|
||||
FIFO_RESUME_PARTIAL_RD = Bit6,
|
||||
FIFO_WM_GT_TH = Bit5,
|
||||
FIFO_HIRES_EN = Bit4,
|
||||
FIFO_TMST_FSYNC_EN = Bit3,
|
||||
FIFO_TEMP_EN = Bit2,
|
||||
FIFO_GYRO_EN = Bit1,
|
||||
FIFO_ACCEL_EN = Bit0,
|
||||
};
|
||||
|
||||
// INT_CONFIG0
|
||||
enum INT_CONFIG0_BIT : uint8_t {
|
||||
// 3:2 FIFO_THS_INT_CLEAR
|
||||
CLEAR_ON_FIFO_READ = Bit3,
|
||||
};
|
||||
|
||||
// INT_CONFIG1
|
||||
enum INT_CONFIG1_BIT : uint8_t {
|
||||
INT_ASYNC_RESET = Bit4,
|
||||
};
|
||||
|
||||
// INT_SOURCE0
|
||||
enum INT_SOURCE0_BIT : uint8_t {
|
||||
UI_FSYNC_INT1_EN = Bit6,
|
||||
PLL_RDY_INT1_EN = Bit5,
|
||||
RESET_DONE_INT1_EN = Bit4,
|
||||
UI_DRDY_INT1_EN = Bit3,
|
||||
FIFO_THS_INT1_EN = Bit2, // FIFO threshold interrupt routed to INT1
|
||||
FIFO_FULL_INT1_EN = Bit1,
|
||||
UI_AGC_RDY_INT1_EN = Bit0,
|
||||
};
|
||||
|
||||
// REG_BANK_SEL
|
||||
enum REG_BANK_SEL_BIT : uint8_t {
|
||||
// 2:0 BANK_SEL
|
||||
BANK_SEL_0 = 0, // 000: Bank 0 (default)
|
||||
BANK_SEL_1 = Bit0, // 001: Bank 1
|
||||
BANK_SEL_2 = Bit1, // 010: Bank 2
|
||||
BANK_SEL_3 = Bit1 | Bit0, // 011: Bank 3
|
||||
BANK_SEL_4 = Bit2, // 100: Bank 4
|
||||
};
|
||||
|
||||
|
||||
//---------------- BANK1 Register bits
|
||||
|
||||
// GYRO_CONFIG_STATIC2
|
||||
enum GYRO_CONFIG_STATIC2_BIT : uint8_t {
|
||||
GYRO_AAF_DIS = Bit1, // 1: Disable gyroscope anti-aliasing filter
|
||||
GYRO_NF_DIS = Bit0, // 1: Disable Notch Filter
|
||||
};
|
||||
|
||||
// GYRO_CONFIG_STATIC3
|
||||
enum GYRO_CONFIG_STATIC3_BIT : uint8_t {
|
||||
// 5:0 GYRO_AAF_DELT
|
||||
// 585 Hz = 13 (0b00'1101)
|
||||
GYRO_AAF_DELT_585HZ_SET = Bit3 | Bit2 | Bit0,
|
||||
GYRO_AAF_DELT_585HZ_CLEAR = Bit5 | Bit4 | Bit1,
|
||||
};
|
||||
|
||||
// GYRO_CONFIG_STATIC4
|
||||
enum GYRO_CONFIG_STATIC4_BIT : uint8_t {
|
||||
// 7:0 GYRO_AAF_DELTSQR
|
||||
// 585 Hz = 170 (0b1010'1010)
|
||||
GYRO_AAF_DELTSQR_LSB_585HZ_SET = Bit7 | Bit5 | Bit3 | Bit1,
|
||||
GYRO_AAF_DELTSQR_LSB_585HZ_CLEAR = Bit6 | Bit4 | Bit2 | Bit0,
|
||||
};
|
||||
|
||||
// GYRO_CONFIG_STATIC5
|
||||
enum GYRO_CONFIG_STATIC5_BIT : uint8_t {
|
||||
// 7:4 GYRO_AAF_BITSHIFT
|
||||
// 585 Hz = 8 (0b1000)
|
||||
GYRO_AAF_BITSHIFT_585HZ_SET = Bit7,
|
||||
GYRO_AAF_BITSHIFT_585HZ_CLEAR = Bit6 | Bit5 | Bit4,
|
||||
|
||||
// 3:0 GYRO_AAF_DELTSQR[11:8]
|
||||
// 585 Hz = 170 (0b0000'1010'1010)
|
||||
GYRO_AAF_DELTSQR_MSB_585HZ_SET = 0,
|
||||
GYRO_AAF_DELTSQR_MSB_585HZ_CLEAR = Bit3 | Bit2 | Bit1 | Bit0,
|
||||
};
|
||||
|
||||
// INTF_CONFIG5
|
||||
enum INTF_CONFIG5_BIT : uint8_t {
|
||||
// 2:1 PIN9_FUNCTION
|
||||
PIN9_FUNCTION_CLKIN_SET = Bit2, // 0b10: CLKIN
|
||||
PIN9_FUNCTION_CLKIN_CLEAR = Bit1,
|
||||
|
||||
PIN9_FUNCTION_RESET_SET = 0,
|
||||
PIN9_FUNCTION_RESET_CLEAR = Bit2 | Bit1,
|
||||
};
|
||||
|
||||
//---------------- BANK2 Register bits
|
||||
|
||||
// ACCEL_CONFIG_STATIC2
|
||||
enum ACCEL_CONFIG_STATIC2_BIT : uint8_t {
|
||||
// 6:1 ACCEL_AAF_DELT
|
||||
// 585 Hz = 13 (0b00'1101)
|
||||
ACCEL_AAF_DELT_585HZ_SET = Bit4 | Bit3 | Bit1,
|
||||
ACCEL_AAF_DELT_585HZ_CLEAR = Bit6 | Bit5 | Bit2,
|
||||
|
||||
// 0 ACCEL_AAF_DIS
|
||||
ACCEL_AAF_DIS = Bit0, // 0: Enable accelerometer anti-aliasing filter (default)
|
||||
};
|
||||
|
||||
// ACCEL_CONFIG_STATIC3
|
||||
enum ACCEL_CONFIG_STATIC3_BIT : uint8_t {
|
||||
// 7:0 ACCEL_AAF_DELTSQR[7:0]
|
||||
// 585 Hz = 170 (0b0000'1010'1010)
|
||||
ACCEL_AAF_DELTSQR_LSB_585HZ_SET = Bit7 | Bit5 | Bit3 | Bit1,
|
||||
ACCEL_AAF_DELTSQR_LSB_585HZ_CLEAR = Bit6 | Bit4 | Bit2 | Bit0,
|
||||
};
|
||||
|
||||
// ACCEL_CONFIG_STATIC4
|
||||
enum ACCEL_CONFIG_STATIC4_BIT : uint8_t {
|
||||
// 7:4 ACCEL_AAF_BITSHIFT
|
||||
// 585 Hz = 8 (0b1000)
|
||||
ACCEL_AAF_BITSHIFT_585HZ_SET = Bit7,
|
||||
ACCEL_AAF_BITSHIFT_585HZ_CLEAR = Bit6 | Bit5 | Bit4,
|
||||
|
||||
// 3:0 ACCEL_AAF_DELTSQR[11:8]
|
||||
// 585 Hz = 170 (0b0000'1010'1010)
|
||||
ACCEL_AAF_DELTSQR_MSB_SET = 0,
|
||||
ACCEL_AAF_DELTSQR_MSB_CLEAR = Bit3 | Bit2 | Bit1 | Bit0,
|
||||
};
|
||||
|
||||
// AUX1_CONFIG1
|
||||
enum AUX1_CONFIG1_BIT : uint8_t {
|
||||
AUX1_ACCEL_LP_CLK_SEL = Bit5,
|
||||
GYRO_AUX1_EN = Bit1,
|
||||
ACCEL_AUX1_EN = Bit0,
|
||||
};
|
||||
|
||||
// AUX1_CONFIG2
|
||||
enum AUX1_CONFIG2_BIT : uint8_t {
|
||||
GYRO_AUX1_HPF_DIS = Bit6,
|
||||
};
|
||||
|
||||
// AUX1_SPI_REG1
|
||||
enum AUX1_SPI_REG1_BIT : uint8_t {
|
||||
AUX1_SPI_REG1_CLEAR = Bit1,
|
||||
AUX1_SPI_REG1_SET = Bit0,
|
||||
};
|
||||
|
||||
// AUX1_SPI_REG2
|
||||
enum AUX1_SPI_REG2_BIT : uint8_t {
|
||||
AUX1_SPI_REG2_CLEAR = Bit1,
|
||||
AUX1_SPI_REG2_SET = Bit0,
|
||||
};
|
||||
|
||||
// AUX1_SPI_REG3
|
||||
enum AUX1_SPI_REG3_BIT : uint8_t {
|
||||
AUX1_SPI_REG3_CLEAR = Bit1,
|
||||
AUX1_SPI_REG3_SET = Bit0,
|
||||
};
|
||||
|
||||
namespace FIFO
|
||||
{
|
||||
static constexpr size_t SIZE = 2048;
|
||||
|
||||
// FIFO_DATA layout when FIFO_CONFIG1 has FIFO_GYRO_EN and FIFO_ACCEL_EN set
|
||||
|
||||
// Packet 4
|
||||
struct DATA {
|
||||
uint8_t FIFO_Header;
|
||||
uint8_t ACCEL_DATA_X1; // Accel X [19:12]
|
||||
uint8_t ACCEL_DATA_X0; // Accel X [11:4]
|
||||
uint8_t ACCEL_DATA_Y1; // Accel Y [19:12]
|
||||
uint8_t ACCEL_DATA_Y0; // Accel Y [11:4]
|
||||
uint8_t ACCEL_DATA_Z1; // Accel Z [19:12]
|
||||
uint8_t ACCEL_DATA_Z0; // Accel Z [11:4]
|
||||
uint8_t GYRO_DATA_X1; // Gyro X [19:12]
|
||||
uint8_t GYRO_DATA_X0; // Gyro X [11:4]
|
||||
uint8_t GYRO_DATA_Y1; // Gyro Y [19:12]
|
||||
uint8_t GYRO_DATA_Y0; // Gyro Y [11:4]
|
||||
uint8_t GYRO_DATA_Z1; // Gyro Z [19:12]
|
||||
uint8_t GYRO_DATA_Z0; // Gyro Z [11:4]
|
||||
uint8_t TEMP_DATA1; // Temperature[15:8]
|
||||
uint8_t TEMP_DATA0; // Temperature[7:0]
|
||||
uint8_t TimeStamp_h; // TimeStamp[15:8]
|
||||
uint8_t TimeStamp_l; // TimeStamp[7:0]
|
||||
uint8_t Ext_Accel_X_Gyro_X; // Accel X [3:0] Gyro X [3:0]
|
||||
uint8_t Ext_Accel_Y_Gyro_Y; // Accel Y [3:0] Gyro Y [3:0]
|
||||
uint8_t Ext_Accel_Z_Gyro_Z; // Accel Z [3:0] Gyro Z [3:0]
|
||||
};
|
||||
|
||||
// With FIFO_ACCEL_EN and FIFO_GYRO_EN header should be 8’b_0110_10xx
|
||||
enum FIFO_HEADER_BIT : uint8_t {
|
||||
HEADER_MSG = Bit7, // 1: FIFO is empty
|
||||
HEADER_ACCEL = Bit6, // 1: Packet is sized so that accel data have location in the packet, FIFO_ACCEL_EN must be 1
|
||||
HEADER_GYRO = Bit5, // 1: Packet is sized so that gyro data have location in the packet, FIFO_GYRO_EN must be1
|
||||
HEADER_20 = Bit4, // 1: Packet has a new and valid sample of extended 20-bit data for gyro and/or accel
|
||||
HEADER_TIMESTAMP_FSYNC = Bit3 | Bit2, // 10: Packet contains ODR Timestamp
|
||||
HEADER_ODR_ACCEL = Bit1, // 1: The ODR for accel is different for this accel data packet compared to the previous accel packet
|
||||
HEADER_ODR_GYRO = Bit0, // 1: The ODR for gyro is different for this gyro data packet compared to the previous gyro packet
|
||||
};
|
||||
|
||||
}
|
||||
} // namespace InvenSense_IIM42653
|
|
@ -0,0 +1,5 @@
|
|||
menuconfig DRIVERS_IMU_INVENSENSE_IIM42653
|
||||
bool "iim42653"
|
||||
default n
|
||||
---help---
|
||||
Enable support for iim42653
|
|
@ -0,0 +1,92 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2023 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 "IIM42653.hpp"
|
||||
|
||||
#include <px4_platform_common/getopt.h>
|
||||
#include <px4_platform_common/module.h>
|
||||
|
||||
void IIM42653::print_usage()
|
||||
{
|
||||
PRINT_MODULE_USAGE_NAME("iim42653", "driver");
|
||||
PRINT_MODULE_USAGE_SUBCATEGORY("imu");
|
||||
PRINT_MODULE_USAGE_COMMAND("start");
|
||||
PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true);
|
||||
PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true);
|
||||
PRINT_MODULE_USAGE_PARAM_INT('C', 0, 0, 35000, "Input clock frequency (Hz)", true);
|
||||
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
|
||||
}
|
||||
|
||||
extern "C" int iim42653_main(int argc, char *argv[])
|
||||
{
|
||||
int ch;
|
||||
using ThisDriver = IIM42653;
|
||||
BusCLIArguments cli{false, true};
|
||||
cli.default_spi_frequency = SPI_SPEED;
|
||||
|
||||
while ((ch = cli.getOpt(argc, argv, "C:R:")) != EOF) {
|
||||
switch (ch) {
|
||||
case 'C':
|
||||
cli.custom1 = atoi(cli.optArg());
|
||||
break;
|
||||
|
||||
case 'R':
|
||||
cli.rotation = (enum Rotation)atoi(cli.optArg());
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
const char *verb = cli.optArg();
|
||||
|
||||
if (!verb) {
|
||||
ThisDriver::print_usage();
|
||||
return -1;
|
||||
}
|
||||
|
||||
BusInstanceIterator iterator(MODULE_NAME, cli, DRV_IMU_DEVTYPE_IIM42653);
|
||||
|
||||
if (!strcmp(verb, "start")) {
|
||||
return ThisDriver::module_start(cli, iterator);
|
||||
}
|
||||
|
||||
if (!strcmp(verb, "stop")) {
|
||||
return ThisDriver::module_stop(iterator);
|
||||
}
|
||||
|
||||
if (!strcmp(verb, "status")) {
|
||||
return ThisDriver::module_status(iterator);
|
||||
}
|
||||
|
||||
ThisDriver::print_usage();
|
||||
return -1;
|
||||
}
|
|
@ -35,4 +35,5 @@
|
|||
add_subdirectory(rgbled)
|
||||
add_subdirectory(rgbled_ncp5623c)
|
||||
add_subdirectory(rgbled_is31fl3195)
|
||||
add_subdirectory(rgbled_lp5562)
|
||||
#add_subdirectory(rgbled_pwm) # requires board support (BOARD_HAS_LED_PWM/BOARD_HAS_UI_LED_PWM)
|
||||
|
|
|
@ -5,6 +5,7 @@ menu "Lights"
|
|||
select DRIVERS_LIGHTS_RGBLED
|
||||
select DRIVERS_LIGHTS_RGBLED_NCP5623C
|
||||
select DRIVERS_LIGHTS_RGBLED_IS31FL3195
|
||||
select DRIVERS_LIGHTS_RGBLED_LP5562
|
||||
---help---
|
||||
Enable default set of light drivers
|
||||
rsource "*/Kconfig"
|
||||
|
|
|
@ -0,0 +1,42 @@
|
|||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2023 PX4 Development Team. All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
# are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# 2. Redistributions in binary form must reproduce the above copyright
|
||||
# notice, this list of conditions and the following disclaimer in
|
||||
# the documentation and/or other materials provided with the
|
||||
# distribution.
|
||||
# 3. Neither the name PX4 nor the names of its contributors may be
|
||||
# used to endorse or promote products derived from this software
|
||||
# without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
px4_add_module(
|
||||
MODULE drivers__rgbled_lp5562
|
||||
MAIN rgbled_lp5562
|
||||
SRCS
|
||||
rgbled_lp5562.cpp
|
||||
DEPENDS
|
||||
drivers__device
|
||||
led
|
||||
)
|
|
@ -0,0 +1,5 @@
|
|||
menuconfig DRIVERS_LIGHTS_RGBLED_LP5562
|
||||
bool "rgbled lp5562"
|
||||
default n
|
||||
---help---
|
||||
Enable support for RGBLED LP5562 driver
|
|
@ -0,0 +1,329 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2023 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 rgbled_lp5562.cpp
|
||||
*
|
||||
* Driver for the RGB LED controller Texas Instruments LP5562 connected via I2C.
|
||||
*
|
||||
* @author Julian Oes <julian@oes.ch>
|
||||
*/
|
||||
|
||||
#include <stdint.h>
|
||||
#include <string.h>
|
||||
|
||||
#include <drivers/device/i2c.h>
|
||||
#include <lib/led/led.h>
|
||||
#include <lib/parameters/param.h>
|
||||
#include <px4_platform_common/getopt.h>
|
||||
#include <px4_platform_common/i2c_spi_buses.h>
|
||||
#include <px4_platform_common/module.h>
|
||||
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
// The addresses are 0x60, 0x62, 0x64, 0x66 according to the datasheet page 27.
|
||||
// We specify 7bit addresses, hence 0x60 becomes 0x30.
|
||||
#define I2C_ADDR 0x30
|
||||
|
||||
// Unfortunately, there is no WHO_AM_I or device id register, so
|
||||
// instead we query the W_CURRENT which has a certain pattern
|
||||
// after reset, and we don't use it or change it, so we don't have
|
||||
// to reset it and therefore don't mess with a device that we're
|
||||
// not sure what it is.
|
||||
static constexpr uint8_t LED_MAP_ADDR = 0x70;
|
||||
static constexpr uint8_t LED_MAP_ALL_PWM = 0b00000000;
|
||||
|
||||
static constexpr uint8_t ENABLE_ADDR = 0x00;
|
||||
static constexpr uint8_t ENABLE_CHIP_EN = 0b01000000;
|
||||
|
||||
static constexpr uint8_t CONFIG_ADDR = 0x08;
|
||||
static constexpr uint8_t CONFIG_ENABLE_INTERNAL_CLOCK = 0b00000001;
|
||||
|
||||
static constexpr uint8_t RESET_ADDR = 0x0D;
|
||||
static constexpr uint8_t RESET_DO_RESET = 0xFF;
|
||||
|
||||
static constexpr uint8_t B_PWM_ADDR = 0x02;
|
||||
|
||||
static constexpr uint8_t B_CURRENT_ADDR = 0x05;
|
||||
|
||||
static constexpr uint8_t W_CURRENT_ADDR = 0x0F;
|
||||
static constexpr uint8_t W_CURRENT_DEFAULT = 0b10101111;
|
||||
|
||||
|
||||
class RGBLED_LP5562: public device::I2C, public I2CSPIDriver<RGBLED_LP5562>
|
||||
{
|
||||
public:
|
||||
RGBLED_LP5562(const I2CSPIDriverConfig &config);
|
||||
virtual ~RGBLED_LP5562() = default;
|
||||
|
||||
static void print_usage();
|
||||
|
||||
int init() override;
|
||||
int probe() override;
|
||||
|
||||
void RunImpl();
|
||||
|
||||
private:
|
||||
int read(uint8_t address, uint8_t *data, unsigned count);
|
||||
int write(uint8_t address, uint8_t *data, unsigned count);
|
||||
int send_led_rgb(uint8_t r, uint8_t g, uint8_t b);
|
||||
|
||||
LedController _led_controller;
|
||||
uint8_t _current = 175; // matching default current of 17.5mA
|
||||
};
|
||||
|
||||
RGBLED_LP5562::RGBLED_LP5562(const I2CSPIDriverConfig &config) :
|
||||
I2C(config),
|
||||
I2CSPIDriver(config)
|
||||
{
|
||||
_current = config.custom1;
|
||||
}
|
||||
|
||||
int
|
||||
RGBLED_LP5562::init()
|
||||
{
|
||||
int ret = I2C::init();
|
||||
|
||||
if (ret != OK) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
uint8_t command[1] = {ENABLE_CHIP_EN};
|
||||
ret = write(ENABLE_ADDR, command, sizeof(command));
|
||||
|
||||
if (ret != OK) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
// We have to wait 500us after enable.
|
||||
px4_usleep(500);
|
||||
|
||||
command[0] = CONFIG_ENABLE_INTERNAL_CLOCK;
|
||||
ret = write(CONFIG_ADDR, command, sizeof(command));
|
||||
|
||||
if (ret != OK) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
command[0] = LED_MAP_ALL_PWM;
|
||||
ret = write(LED_MAP_ADDR, command, sizeof(command));
|
||||
|
||||
if (ret != OK) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
// Write all 3 colors at once.
|
||||
uint8_t currents[3] = {_current, _current, _current};
|
||||
ret = write(B_CURRENT_ADDR, currents, sizeof(currents));
|
||||
|
||||
if (ret != OK) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
ScheduleNow();
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
int
|
||||
RGBLED_LP5562::probe()
|
||||
{
|
||||
uint8_t result[1] = {0};
|
||||
int ret = read(W_CURRENT_ADDR, result, sizeof(result));
|
||||
|
||||
if (ret != OK) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
_retries = 1;
|
||||
|
||||
return (result[0] == W_CURRENT_DEFAULT) ? OK : ERROR;
|
||||
}
|
||||
|
||||
int
|
||||
RGBLED_LP5562::read(uint8_t address, uint8_t *data, unsigned count)
|
||||
{
|
||||
uint8_t cmd = address;
|
||||
return transfer(&cmd, 1, (uint8_t *)data, count);
|
||||
}
|
||||
|
||||
int
|
||||
RGBLED_LP5562::write(uint8_t address, uint8_t *data, unsigned count)
|
||||
{
|
||||
uint8_t buf[4];
|
||||
|
||||
if (sizeof(buf) < (count + 1)) {
|
||||
return -EIO;
|
||||
}
|
||||
|
||||
buf[0] = address;
|
||||
memcpy(&buf[1], data, count);
|
||||
|
||||
return transfer(&buf[0], count + 1, nullptr, 0);
|
||||
}
|
||||
|
||||
void
|
||||
RGBLED_LP5562::RunImpl()
|
||||
{
|
||||
if (should_exit()) {
|
||||
send_led_rgb(0, 0, 0);
|
||||
return;
|
||||
}
|
||||
|
||||
LedControlData led_control_data;
|
||||
|
||||
if (_led_controller.update(led_control_data) == 1) {
|
||||
|
||||
const uint8_t on = led_control_data.leds[0].brightness;
|
||||
|
||||
switch (led_control_data.leds[0].color) {
|
||||
case led_control_s::COLOR_RED:
|
||||
send_led_rgb(on, 0, 0);
|
||||
break;
|
||||
|
||||
case led_control_s::COLOR_GREEN:
|
||||
send_led_rgb(0, on, 0);
|
||||
break;
|
||||
|
||||
case led_control_s::COLOR_BLUE:
|
||||
send_led_rgb(0, 0, on);
|
||||
break;
|
||||
|
||||
case led_control_s::COLOR_AMBER: // same as yellow
|
||||
case led_control_s::COLOR_YELLOW:
|
||||
send_led_rgb(on, on, 0);
|
||||
break;
|
||||
|
||||
case led_control_s::COLOR_PURPLE:
|
||||
send_led_rgb(on, 0, on);
|
||||
break;
|
||||
|
||||
case led_control_s::COLOR_CYAN:
|
||||
send_led_rgb(0, on, on);
|
||||
break;
|
||||
|
||||
case led_control_s::COLOR_WHITE:
|
||||
send_led_rgb(on, on, on);
|
||||
break;
|
||||
|
||||
case led_control_s::COLOR_OFF:
|
||||
default:
|
||||
send_led_rgb(0, 0, 0);
|
||||
break;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
ScheduleDelayed(_led_controller.maximum_update_interval());
|
||||
}
|
||||
|
||||
int
|
||||
RGBLED_LP5562::send_led_rgb(uint8_t r, uint8_t g, uint8_t b)
|
||||
{
|
||||
uint8_t leds[3] = {b, g, r};
|
||||
return write(B_PWM_ADDR, leds, sizeof(leds));
|
||||
}
|
||||
|
||||
void
|
||||
RGBLED_LP5562::print_usage()
|
||||
{
|
||||
PRINT_MODULE_DESCRIPTION(
|
||||
R"DESCR_STR(
|
||||
### Description
|
||||
Driver for [LP5562](https://www.ti.com/product/LP5562) LED driver connected via I2C.
|
||||
|
||||
This used in some GPS modules by Holybro for [PX4 status notification](../getting_started/led_meanings.md)
|
||||
|
||||
The driver is included by default in firmware (KConfig key DRIVERS_LIGHTS_RGBLED_LP5562) and is always enabled.
|
||||
)DESCR_STR");
|
||||
PRINT_MODULE_USAGE_NAME("rgbled_lp5562", "driver");
|
||||
PRINT_MODULE_USAGE_COMMAND("start");
|
||||
PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
|
||||
PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(I2C_ADDR);
|
||||
PRINT_MODULE_USAGE_PARAM_FLOAT('u', 17.5f, 0.1f, 25.5f, "Current in mA", true);
|
||||
|
||||
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
|
||||
}
|
||||
|
||||
extern "C" __EXPORT int rgbled_lp5562_main(int argc, char *argv[])
|
||||
{
|
||||
int ch;
|
||||
using ThisDriver = RGBLED_LP5562;
|
||||
BusCLIArguments cli{true, false};
|
||||
cli.default_i2c_frequency = 100000;
|
||||
cli.i2c_address = I2C_ADDR;
|
||||
cli.custom1 = 175;
|
||||
|
||||
while ((ch = cli.getOpt(argc, argv, "u:")) != EOF) {
|
||||
switch (ch) {
|
||||
case 'u':
|
||||
float v = atof(cli.optArg());
|
||||
|
||||
if (v >= 0.1f && v <= 25.5f) {
|
||||
cli.custom1 = ((uint8_t)(v * 10.f));
|
||||
|
||||
} else {
|
||||
PX4_ERR("current out of range");
|
||||
return -1;
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
const char *verb = cli.optArg();
|
||||
|
||||
if (!verb) {
|
||||
ThisDriver::print_usage();
|
||||
return -1;
|
||||
}
|
||||
|
||||
BusInstanceIterator iterator(MODULE_NAME, cli, DRV_LED_DEVTYPE_RGBLED_LP5562);
|
||||
|
||||
if (!strcmp(verb, "start")) {
|
||||
return ThisDriver::module_start(cli, iterator);
|
||||
}
|
||||
|
||||
if (!strcmp(verb, "stop")) {
|
||||
return ThisDriver::module_stop(iterator);
|
||||
}
|
||||
|
||||
if (!strcmp(verb, "status")) {
|
||||
return ThisDriver::module_status(iterator);
|
||||
}
|
||||
|
||||
ThisDriver::print_usage();
|
||||
return -1;
|
||||
}
|
|
@ -4,7 +4,7 @@ actuator_output:
|
|||
- param_prefix: PWM_MAIN
|
||||
channel_label: 'Channel'
|
||||
standard_params:
|
||||
disarmed: { min: 800, max: 2200, default: 900 }
|
||||
disarmed: { min: 800, max: 2200, default: 1000 }
|
||||
min: { min: 800, max: 1400, default: 1000 }
|
||||
max: { min: 1600, max: 2200, default: 2000 }
|
||||
failsafe: { min: 800, max: 2200 }
|
||||
|
|
|
@ -40,7 +40,6 @@
|
|||
#include <px4_platform_common/px4_config.h>
|
||||
|
||||
#include <assert.h>
|
||||
#include <debug.h>
|
||||
#include <errno.h>
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
|
|
|
@ -40,7 +40,6 @@
|
|||
#include <px4_platform_common/px4_config.h>
|
||||
|
||||
#include <assert.h>
|
||||
#include <debug.h>
|
||||
#include <errno.h>
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
|
|
|
@ -40,7 +40,6 @@
|
|||
#include <px4_platform_common/px4_config.h>
|
||||
|
||||
#include <assert.h>
|
||||
#include <debug.h>
|
||||
#include <errno.h>
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
|
|
|
@ -40,7 +40,6 @@
|
|||
#include <px4_platform_common/px4_config.h>
|
||||
|
||||
#include <assert.h>
|
||||
#include <debug.h>
|
||||
#include <errno.h>
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
|
|
|
@ -40,7 +40,6 @@
|
|||
#include <px4_platform_common/px4_config.h>
|
||||
|
||||
#include <assert.h>
|
||||
#include <debug.h>
|
||||
#include <errno.h>
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
|
|
|
@ -40,7 +40,6 @@
|
|||
#include <px4_platform_common/px4_config.h>
|
||||
|
||||
#include <assert.h>
|
||||
#include <debug.h>
|
||||
#include <errno.h>
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
|
|
|
@ -1,6 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2022-2023 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
|
||||
|
@ -194,13 +194,13 @@ void PAA3905::RunImpl()
|
|||
_state = STATE::READ;
|
||||
|
||||
if (DataReadyInterruptConfigure()) {
|
||||
_data_ready_interrupt_enabled = true;
|
||||
_motion_interrupt_enabled = true;
|
||||
|
||||
// backup schedule as a watchdog timeout
|
||||
ScheduleDelayed(1_s);
|
||||
|
||||
} else {
|
||||
_data_ready_interrupt_enabled = false;
|
||||
_motion_interrupt_enabled = false;
|
||||
ScheduleOnInterval(_scheduled_interval_us, _scheduled_interval_us);
|
||||
}
|
||||
|
||||
|
@ -222,7 +222,7 @@ void PAA3905::RunImpl()
|
|||
case STATE::READ: {
|
||||
hrt_abstime timestamp_sample = now;
|
||||
|
||||
if (_data_ready_interrupt_enabled) {
|
||||
if (_motion_interrupt_enabled) {
|
||||
// scheduled from interrupt if _drdy_timestamp_sample was set as expected
|
||||
const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
|
||||
|
||||
|
@ -258,12 +258,6 @@ void PAA3905::RunImpl()
|
|||
PX4_ERR("invalid RawData_Sum > 0x98");
|
||||
}
|
||||
|
||||
// Number of Features = SQUAL * 4
|
||||
// RawData_Sum maximum register value is 0x98
|
||||
bool data_valid = (buffer.data.SQUAL > 0)
|
||||
&& (buffer.data.RawData_Sum <= 0x98)
|
||||
&& (_discard_reading == 0);
|
||||
|
||||
// Bit [5:0] check if chip is working correctly
|
||||
// 0x3F: chip is working correctly
|
||||
if ((buffer.data.Observation & 0x3F) != 0x3F) {
|
||||
|
@ -313,7 +307,7 @@ void PAA3905::RunImpl()
|
|||
|
||||
if (prev_mode != _mode) {
|
||||
// update scheduling on mode change
|
||||
if (!_data_ready_interrupt_enabled) {
|
||||
if (!_motion_interrupt_enabled) {
|
||||
ScheduleOnInterval(_scheduled_interval_us, _scheduled_interval_us);
|
||||
}
|
||||
}
|
||||
|
@ -348,6 +342,14 @@ void PAA3905::RunImpl()
|
|||
|
||||
const uint32_t shutter = (shutter_upper << 16) | (shutter_middle << 8) | shutter_lower;
|
||||
|
||||
// Number of Features = SQUAL * 4
|
||||
// RawData_Sum maximum register value is 0x98
|
||||
bool data_valid = (buffer.data.SQUAL > 0)
|
||||
&& (buffer.data.RawData_Sum > 0)
|
||||
&& (buffer.data.RawData_Sum <= 0x98)
|
||||
&& (shutter > 0)
|
||||
&& (_discard_reading == 0);
|
||||
|
||||
switch (_mode) {
|
||||
case Mode::Bright:
|
||||
sensor_optical_flow.integration_timespan_us = SAMPLE_INTERVAL_MODE_0;
|
||||
|
@ -392,12 +394,17 @@ void PAA3905::RunImpl()
|
|||
// motion in burst transfer
|
||||
const bool motion_reported = (buffer.data.Motion & Motion_Bit::MotionOccurred);
|
||||
|
||||
if (data_valid) {
|
||||
if (motion_reported) {
|
||||
// only populate flow if data valid (motion and quality > 0)
|
||||
const int16_t delta_x_raw = combine(buffer.data.Delta_X_H, buffer.data.Delta_X_L);
|
||||
const int16_t delta_y_raw = combine(buffer.data.Delta_Y_H, buffer.data.Delta_Y_L);
|
||||
const int16_t delta_x_raw = combine(buffer.data.Delta_X_H, buffer.data.Delta_X_L);
|
||||
const int16_t delta_y_raw = combine(buffer.data.Delta_Y_H, buffer.data.Delta_Y_L);
|
||||
|
||||
if (data_valid) {
|
||||
|
||||
const bool zero_flow = (delta_x_raw == 0) && (delta_y_raw == 0);
|
||||
const bool little_to_no_flow = (abs(delta_x_raw) <= 1) && (abs(delta_y_raw) <= 1);
|
||||
|
||||
bool publish = false;
|
||||
|
||||
if (motion_reported) {
|
||||
// rotate measurements in yaw from sensor frame to body frame
|
||||
const matrix::Vector3f pixel_flow_rotated = _rotation * matrix::Vector3f{(float)delta_x_raw, (float)delta_y_raw, 0.f};
|
||||
|
||||
|
@ -412,15 +419,53 @@ void PAA3905::RunImpl()
|
|||
sensor_optical_flow.pixel_flow[1] = pixel_flow_rotated(1) * SCALE;
|
||||
|
||||
sensor_optical_flow.quality = buffer.data.SQUAL;
|
||||
|
||||
publish = true;
|
||||
|
||||
_last_motion = timestamp_sample;
|
||||
|
||||
} else if (zero_flow && (timestamp_sample > _last_motion)) {
|
||||
// no motion, but burst read looks valid and we should have seen new data by now if there was any motion
|
||||
const bool burst_read_changed = (delta_x_raw != _delta_x_raw_prev) || (delta_y_raw != _delta_y_raw_prev)
|
||||
|| (shutter != _shutter_prev)
|
||||
|| (buffer.data.RawData_Sum != _raw_data_sum_prev)
|
||||
|| (buffer.data.SQUAL != _quality_prev);
|
||||
|
||||
if (burst_read_changed) {
|
||||
|
||||
sensor_optical_flow.pixel_flow[0] = 0;
|
||||
sensor_optical_flow.pixel_flow[1] = 0;
|
||||
|
||||
sensor_optical_flow.quality = buffer.data.SQUAL;
|
||||
|
||||
publish = true;
|
||||
}
|
||||
}
|
||||
|
||||
// only publish when there's motion or at least every second
|
||||
if (motion_reported || (hrt_elapsed_time(&_last_publish) >= kBackupScheduleIntervalUs)) {
|
||||
// only publish when there's valid data or on timeout
|
||||
if (publish || (hrt_elapsed_time(&_last_publish) >= kBackupScheduleIntervalUs)) {
|
||||
|
||||
sensor_optical_flow.timestamp = hrt_absolute_time();
|
||||
_sensor_optical_flow_pub.publish(sensor_optical_flow);
|
||||
|
||||
_last_publish = sensor_optical_flow.timestamp;
|
||||
_last_publish = sensor_optical_flow.timestamp_sample;
|
||||
}
|
||||
|
||||
// backup schedule if we're reliant on the motion interrupt and there's very little flow
|
||||
if (_motion_interrupt_enabled && little_to_no_flow) {
|
||||
switch (_mode) {
|
||||
case Mode::Bright:
|
||||
ScheduleDelayed(SAMPLE_INTERVAL_MODE_0);
|
||||
break;
|
||||
|
||||
case Mode::LowLight:
|
||||
ScheduleDelayed(SAMPLE_INTERVAL_MODE_1);
|
||||
break;
|
||||
|
||||
case Mode::SuperLowLight:
|
||||
ScheduleDelayed(SAMPLE_INTERVAL_MODE_2);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
success = true;
|
||||
|
@ -430,6 +475,12 @@ void PAA3905::RunImpl()
|
|||
}
|
||||
}
|
||||
|
||||
_delta_x_raw_prev = delta_x_raw;
|
||||
_delta_y_raw_prev = delta_y_raw;
|
||||
_shutter_prev = shutter;
|
||||
_raw_data_sum_prev = buffer.data.RawData_Sum;
|
||||
_quality_prev = buffer.data.SQUAL;
|
||||
|
||||
} else {
|
||||
perf_count(_bad_transfer_perf);
|
||||
}
|
||||
|
|
|
@ -1,6 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2022-2023 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
|
||||
|
@ -114,14 +114,22 @@ private:
|
|||
|
||||
hrt_abstime _reset_timestamp{0};
|
||||
hrt_abstime _last_publish{0};
|
||||
hrt_abstime _last_motion{0};
|
||||
|
||||
int16_t _delta_x_raw_prev{0};
|
||||
int16_t _delta_y_raw_prev{0};
|
||||
uint32_t _shutter_prev{0};
|
||||
uint8_t _quality_prev{0};
|
||||
uint8_t _raw_data_sum_prev{0};
|
||||
|
||||
int _failure_count{0};
|
||||
int _discard_reading{0};
|
||||
|
||||
px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
|
||||
bool _data_ready_interrupt_enabled{false};
|
||||
bool _motion_interrupt_enabled{false};
|
||||
|
||||
uint32_t _scheduled_interval_us{SAMPLE_INTERVAL_MODE_0 / 2};
|
||||
static constexpr uint32_t kBackupScheduleIntervalUs{200_ms};
|
||||
static constexpr uint32_t kBackupScheduleIntervalUs{SAMPLE_INTERVAL_MODE_2}; // longest expected interval
|
||||
|
||||
Mode _mode{Mode::LowLight};
|
||||
|
||||
|
|
|
@ -1,6 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2019-2022 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2019-2023 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
|
||||
|
@ -199,13 +199,13 @@ void PAW3902::RunImpl()
|
|||
_state = STATE::READ;
|
||||
|
||||
if (DataReadyInterruptConfigure()) {
|
||||
_data_ready_interrupt_enabled = true;
|
||||
_motion_interrupt_enabled = true;
|
||||
|
||||
// backup schedule as a watchdog timeout
|
||||
ScheduleDelayed(1_s);
|
||||
|
||||
} else {
|
||||
_data_ready_interrupt_enabled = false;
|
||||
_motion_interrupt_enabled = false;
|
||||
ScheduleOnInterval(_scheduled_interval_us, _scheduled_interval_us);
|
||||
}
|
||||
|
||||
|
@ -227,7 +227,7 @@ void PAW3902::RunImpl()
|
|||
case STATE::READ: {
|
||||
hrt_abstime timestamp_sample = now;
|
||||
|
||||
if (_data_ready_interrupt_enabled) {
|
||||
if (_motion_interrupt_enabled) {
|
||||
// scheduled from interrupt if _drdy_timestamp_sample was set as expected
|
||||
const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
|
||||
|
||||
|
@ -263,12 +263,6 @@ void PAW3902::RunImpl()
|
|||
PX4_ERR("invalid RawData_Sum > 0x98");
|
||||
}
|
||||
|
||||
// Number of Features = SQUAL * 4
|
||||
// RawData_Sum maximum register value is 0x98
|
||||
bool data_valid = (buffer.data.SQUAL > 0)
|
||||
&& (buffer.data.RawData_Sum <= 0x98)
|
||||
&& (_discard_reading == 0);
|
||||
|
||||
// publish sensor_optical_flow
|
||||
sensor_optical_flow_s sensor_optical_flow{};
|
||||
sensor_optical_flow.timestamp_sample = timestamp_sample;
|
||||
|
@ -293,6 +287,14 @@ void PAW3902::RunImpl()
|
|||
|
||||
const uint16_t shutter = (shutter_upper << 8) | shutter_lower;
|
||||
|
||||
// Number of Features = SQUAL * 4
|
||||
// RawData_Sum maximum register value is 0x98
|
||||
bool data_valid = (buffer.data.SQUAL > 0)
|
||||
&& (buffer.data.RawData_Sum > 0)
|
||||
&& (buffer.data.RawData_Sum <= 0x98)
|
||||
&& (shutter > 0)
|
||||
&& (_discard_reading == 0);
|
||||
|
||||
switch (_mode) {
|
||||
case Mode::Bright:
|
||||
sensor_optical_flow.integration_timespan_us = SAMPLE_INTERVAL_MODE_0;
|
||||
|
@ -395,12 +397,17 @@ void PAW3902::RunImpl()
|
|||
// motion in burst transfer
|
||||
const bool motion_reported = (buffer.data.Motion & Motion_Bit::MOT);
|
||||
|
||||
if (data_valid) {
|
||||
if (motion_reported) {
|
||||
// only populate flow if data valid (motion and quality > 0)
|
||||
const int16_t delta_x_raw = combine(buffer.data.Delta_X_H, buffer.data.Delta_X_L);
|
||||
const int16_t delta_y_raw = combine(buffer.data.Delta_Y_H, buffer.data.Delta_Y_L);
|
||||
const int16_t delta_x_raw = combine(buffer.data.Delta_X_H, buffer.data.Delta_X_L);
|
||||
const int16_t delta_y_raw = combine(buffer.data.Delta_Y_H, buffer.data.Delta_Y_L);
|
||||
|
||||
if (data_valid) {
|
||||
|
||||
const bool zero_flow = (delta_x_raw == 0) && (delta_y_raw == 0);
|
||||
const bool little_to_no_flow = (abs(delta_x_raw) <= 1) && (abs(delta_y_raw) <= 1);
|
||||
|
||||
bool publish = false;
|
||||
|
||||
if (motion_reported) {
|
||||
// rotate measurements in yaw from sensor frame to body frame
|
||||
const matrix::Vector3f pixel_flow_rotated = _rotation * matrix::Vector3f{(float)delta_x_raw, (float)delta_y_raw, 0.f};
|
||||
|
||||
|
@ -415,15 +422,53 @@ void PAW3902::RunImpl()
|
|||
sensor_optical_flow.pixel_flow[1] = pixel_flow_rotated(1) * SCALE;
|
||||
|
||||
sensor_optical_flow.quality = buffer.data.SQUAL;
|
||||
|
||||
publish = true;
|
||||
|
||||
_last_motion = timestamp_sample;
|
||||
|
||||
} else if (zero_flow && (timestamp_sample > _last_motion)) {
|
||||
// no motion, but burst read looks valid and we should have seen new data by now if there was any motion
|
||||
const bool burst_read_changed = (delta_x_raw != _delta_x_raw_prev) || (delta_y_raw != _delta_y_raw_prev)
|
||||
|| (shutter != _shutter_prev)
|
||||
|| (buffer.data.RawData_Sum != _raw_data_sum_prev)
|
||||
|| (buffer.data.SQUAL != _quality_prev);
|
||||
|
||||
if (burst_read_changed) {
|
||||
|
||||
sensor_optical_flow.pixel_flow[0] = 0;
|
||||
sensor_optical_flow.pixel_flow[1] = 0;
|
||||
|
||||
sensor_optical_flow.quality = buffer.data.SQUAL;
|
||||
|
||||
publish = true;
|
||||
}
|
||||
}
|
||||
|
||||
// only publish when there's motion or at least every second
|
||||
if (motion_reported || (hrt_elapsed_time(&_last_publish) >= kBackupScheduleIntervalUs)) {
|
||||
// only publish when there's valid data or on timeout
|
||||
if (publish || (hrt_elapsed_time(&_last_publish) >= kBackupScheduleIntervalUs)) {
|
||||
|
||||
sensor_optical_flow.timestamp = hrt_absolute_time();
|
||||
_sensor_optical_flow_pub.publish(sensor_optical_flow);
|
||||
|
||||
_last_publish = sensor_optical_flow.timestamp;
|
||||
_last_publish = sensor_optical_flow.timestamp_sample;
|
||||
}
|
||||
|
||||
// backup schedule if we're reliant on the motion interrupt and there's very little flow
|
||||
if (_motion_interrupt_enabled && little_to_no_flow) {
|
||||
switch (_mode) {
|
||||
case Mode::Bright:
|
||||
ScheduleDelayed(SAMPLE_INTERVAL_MODE_0);
|
||||
break;
|
||||
|
||||
case Mode::LowLight:
|
||||
ScheduleDelayed(SAMPLE_INTERVAL_MODE_1);
|
||||
break;
|
||||
|
||||
case Mode::SuperLowLight:
|
||||
ScheduleDelayed(SAMPLE_INTERVAL_MODE_2);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
success = true;
|
||||
|
@ -433,6 +478,12 @@ void PAW3902::RunImpl()
|
|||
}
|
||||
}
|
||||
|
||||
_delta_x_raw_prev = delta_x_raw;
|
||||
_delta_y_raw_prev = delta_y_raw;
|
||||
_shutter_prev = shutter;
|
||||
_raw_data_sum_prev = buffer.data.RawData_Sum;
|
||||
_quality_prev = buffer.data.SQUAL;
|
||||
|
||||
} else {
|
||||
perf_count(_bad_transfer_perf);
|
||||
}
|
||||
|
|
|
@ -1,6 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2019-2022 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2019-2023 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
|
||||
|
@ -114,14 +114,22 @@ private:
|
|||
|
||||
hrt_abstime _reset_timestamp{0};
|
||||
hrt_abstime _last_publish{0};
|
||||
hrt_abstime _last_motion{0};
|
||||
|
||||
int16_t _delta_x_raw_prev{0};
|
||||
int16_t _delta_y_raw_prev{0};
|
||||
uint16_t _shutter_prev{0};
|
||||
uint8_t _quality_prev{0};
|
||||
uint8_t _raw_data_sum_prev{0};
|
||||
|
||||
int _failure_count{0};
|
||||
int _discard_reading{0};
|
||||
|
||||
px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
|
||||
bool _data_ready_interrupt_enabled{false};
|
||||
bool _motion_interrupt_enabled{false};
|
||||
|
||||
uint32_t _scheduled_interval_us{SAMPLE_INTERVAL_MODE_0 / 2};
|
||||
static constexpr uint32_t kBackupScheduleIntervalUs{200_ms};
|
||||
static constexpr uint32_t kBackupScheduleIntervalUs{SAMPLE_INTERVAL_MODE_2}; // longest expected interval
|
||||
|
||||
Mode _mode{Mode::LowLight};
|
||||
|
||||
|
|
|
@ -133,6 +133,15 @@ int PCA9685Wrapper::init()
|
|||
return ret;
|
||||
}
|
||||
|
||||
param_t param_h = param_find("PCA9685_RATE");
|
||||
|
||||
if (param_h != PARAM_INVALID) {
|
||||
param_get(param_h, &_targetFreq);
|
||||
|
||||
} else {
|
||||
PX4_DEBUG("PARAM_INVALID: PCA9685_RATE");
|
||||
}
|
||||
|
||||
this->ChangeWorkQueue(px4::device_bus_to_wq(pca9685->get_device_id()));
|
||||
|
||||
PX4_INFO("running on I2C bus %d address 0x%.2x", pca9685->get_device_bus(), pca9685->get_device_address());
|
||||
|
|
|
@ -4,8 +4,24 @@ actuator_output:
|
|||
- param_prefix: PCA9685
|
||||
channel_label: 'Channel'
|
||||
standard_params:
|
||||
disarmed: { min: 800, max: 2200, default: 900 }
|
||||
disarmed: { min: 800, max: 2200, default: 1000 }
|
||||
min: { min: 800, max: 1400, default: 1000 }
|
||||
max: { min: 1600, max: 2200, default: 2000 }
|
||||
failsafe: { min: 800, max: 2200 }
|
||||
num_channels: 16
|
||||
parameters:
|
||||
- group: PCA9685
|
||||
definitions:
|
||||
PCA9685_RATE:
|
||||
description:
|
||||
short: PWM frequency for PCA9685
|
||||
long: |
|
||||
Update rate at which the PWM signal is sent to the ESC.
|
||||
type: float
|
||||
decimal: 1
|
||||
increment: 0.1
|
||||
default: 50
|
||||
min: 50
|
||||
max: 450
|
||||
unit: Hz
|
||||
reboot_required: true
|
||||
|
|
|
@ -218,42 +218,55 @@ void PWMOut::update_params()
|
|||
|
||||
updateParams();
|
||||
|
||||
// Automatically set the PWM rate and disarmed value when a channel is first set to a servo
|
||||
// Automatically set PWM configuration when a channel is first assigned
|
||||
if (!_first_update_cycle) {
|
||||
for (size_t i = 0; i < _num_outputs; i++) {
|
||||
if ((previously_set_functions & (1u << i)) == 0 && _mixing_output.functionParamHandle(i) != PARAM_INVALID) {
|
||||
int32_t output_function;
|
||||
|
||||
if (param_get(_mixing_output.functionParamHandle(i), &output_function) == 0
|
||||
&& output_function >= (int)OutputFunction::Servo1
|
||||
&& output_function <= (int)OutputFunction::ServoMax) { // Function got set to a servo
|
||||
int32_t val = 1500;
|
||||
PX4_INFO("Setting disarmed to %i for channel %i", (int) val, i);
|
||||
param_set(_mixing_output.disarmedParamHandle(i), &val);
|
||||
if (param_get(_mixing_output.functionParamHandle(i), &output_function) == 0) {
|
||||
// Servos need PWM rate 50Hz and disramed value 1500us
|
||||
if (output_function >= (int)OutputFunction::Servo1
|
||||
&& output_function <= (int)OutputFunction::ServoMax) { // Function got set to a servo
|
||||
int32_t val = 1500;
|
||||
PX4_INFO("Setting channel %i disarmed to %i", (int) val, i);
|
||||
param_set(_mixing_output.disarmedParamHandle(i), &val);
|
||||
|
||||
// If the whole timer group was not set previously, then set the pwm rate to 50 Hz
|
||||
for (int timer = 0; timer < MAX_IO_TIMERS; ++timer) {
|
||||
// If the whole timer group was not set previously, then set the pwm rate to 50 Hz
|
||||
for (int timer = 0; timer < MAX_IO_TIMERS; ++timer) {
|
||||
|
||||
uint32_t channels = io_timer_get_group(timer);
|
||||
uint32_t channels = io_timer_get_group(timer);
|
||||
|
||||
if ((channels & (1u << i)) == 0) {
|
||||
continue;
|
||||
}
|
||||
if ((channels & (1u << i)) == 0) {
|
||||
continue;
|
||||
}
|
||||
|
||||
if ((channels & previously_set_functions) == 0) { // None of the channels was set
|
||||
char param_name[17];
|
||||
snprintf(param_name, sizeof(param_name), "%s_TIM%u", _mixing_output.paramPrefix(), timer);
|
||||
if ((channels & previously_set_functions) == 0) { // None of the channels was set
|
||||
char param_name[17];
|
||||
snprintf(param_name, sizeof(param_name), "%s_TIM%u", _mixing_output.paramPrefix(), timer);
|
||||
|
||||
int32_t tim_config = 0;
|
||||
param_t handle = param_find(param_name);
|
||||
int32_t tim_config = 0;
|
||||
param_t handle = param_find(param_name);
|
||||
|
||||
if (param_get(handle, &tim_config) == 0 && tim_config == 400) {
|
||||
tim_config = 50;
|
||||
PX4_INFO("setting timer %i to %i Hz", timer, (int) tim_config);
|
||||
param_set(handle, &tim_config);
|
||||
if (param_get(handle, &tim_config) == 0 && tim_config == 400) {
|
||||
tim_config = 50;
|
||||
PX4_INFO("setting timer %i to %i Hz", timer, (int) tim_config);
|
||||
param_set(handle, &tim_config);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Motors need a minimum value that idles the motor and have a deadzone at the top of the range
|
||||
if (output_function >= (int)OutputFunction::Motor1
|
||||
&& output_function <= (int)OutputFunction::MotorMax) { // Function got set to a motor
|
||||
int32_t val = 1100;
|
||||
PX4_INFO("Setting channel %i minimum to %i", (int) val, i);
|
||||
param_set(_mixing_output.minParamHandle(i), &val);
|
||||
val = 1900;
|
||||
PX4_INFO("Setting channel %i maximum to %i", (int) val, i);
|
||||
param_set(_mixing_output.maxParamHandle(i), &val);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
@ -5,7 +5,7 @@ actuator_output:
|
|||
param_prefix: '${PWM_MAIN_OR_AUX}'
|
||||
channel_labels: ['${PWM_MAIN_OR_AUX}', '${PWM_MAIN_OR_AUX_CAP}']
|
||||
standard_params:
|
||||
disarmed: { min: 800, max: 2200, default: 900 }
|
||||
disarmed: { min: 800, max: 2200, default: 1000 }
|
||||
min: { min: 800, max: 1400, default: 1000 }
|
||||
max: { min: 1600, max: 2200, default: 2000 }
|
||||
failsafe: { min: 800, max: 2200 }
|
||||
|
|
|
@ -7,7 +7,7 @@ actuator_output:
|
|||
channel_label_module_name_prefix: false
|
||||
timer_config_file: "boards/px4/io-v2/src/timer_config.cpp"
|
||||
standard_params:
|
||||
disarmed: { min: 800, max: 2200, default: 900 }
|
||||
disarmed: { min: 800, max: 2200, default: 1000 }
|
||||
min: { min: 800, max: 1400, default: 1000 }
|
||||
max: { min: 1600, max: 2200, default: 2000 }
|
||||
failsafe: { min: 800, max: 2200 }
|
||||
|
|
|
@ -702,36 +702,48 @@ void PX4IO::update_params()
|
|||
if ((previously_set_functions & (1u << i)) == 0 && _mixing_output.functionParamHandle(i) != PARAM_INVALID) {
|
||||
int32_t output_function;
|
||||
|
||||
if (param_get(_mixing_output.functionParamHandle(i), &output_function) == 0
|
||||
&& output_function >= (int)OutputFunction::Servo1
|
||||
&& output_function <= (int)OutputFunction::ServoMax) { // Function got set to a servo
|
||||
int32_t val = 1500;
|
||||
PX4_INFO("Setting disarmed to %i for channel %i", (int) val, i);
|
||||
param_set(_mixing_output.disarmedParamHandle(i), &val);
|
||||
if (param_get(_mixing_output.functionParamHandle(i), &output_function) == 0) {
|
||||
if (output_function >= (int)OutputFunction::Servo1
|
||||
&& output_function <= (int)OutputFunction::ServoMax) { // Function got set to a servo
|
||||
int32_t val = 1500;
|
||||
PX4_INFO("Setting channel %i disarmed to %i", (int) val, i);
|
||||
param_set(_mixing_output.disarmedParamHandle(i), &val);
|
||||
|
||||
// If the whole timer group was not set previously, then set the pwm rate to 50 Hz
|
||||
for (int timer = 0; timer < (int)(sizeof(_group_channels) / sizeof(_group_channels[0])); ++timer) {
|
||||
// If the whole timer group was not set previously, then set the pwm rate to 50 Hz
|
||||
for (int timer = 0; timer < (int)(sizeof(_group_channels) / sizeof(_group_channels[0])); ++timer) {
|
||||
|
||||
uint32_t channels = _group_channels[timer];
|
||||
uint32_t channels = _group_channels[timer];
|
||||
|
||||
if ((channels & (1u << i)) == 0) {
|
||||
continue;
|
||||
}
|
||||
if ((channels & (1u << i)) == 0) {
|
||||
continue;
|
||||
}
|
||||
|
||||
if ((channels & previously_set_functions) == 0) { // None of the channels was set
|
||||
char param_name[17];
|
||||
snprintf(param_name, sizeof(param_name), "%s_TIM%u", _mixing_output.paramPrefix(), timer);
|
||||
if ((channels & previously_set_functions) == 0) { // None of the channels was set
|
||||
char param_name[17];
|
||||
snprintf(param_name, sizeof(param_name), "%s_TIM%u", _mixing_output.paramPrefix(), timer);
|
||||
|
||||
int32_t tim_config = 0;
|
||||
param_t handle = param_find(param_name);
|
||||
int32_t tim_config = 0;
|
||||
param_t handle = param_find(param_name);
|
||||
|
||||
if (param_get(handle, &tim_config) == 0 && tim_config == 400) {
|
||||
tim_config = 50;
|
||||
PX4_INFO("setting timer %i to %i Hz", timer, (int) tim_config);
|
||||
param_set(handle, &tim_config);
|
||||
if (param_get(handle, &tim_config) == 0 && tim_config == 400) {
|
||||
tim_config = 50;
|
||||
PX4_INFO("setting timer %i to %i Hz", timer, (int) tim_config);
|
||||
param_set(handle, &tim_config);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Motors need a minimum value that idles the motor
|
||||
if (output_function >= (int)OutputFunction::Motor1
|
||||
&& output_function <= (int)OutputFunction::MotorMax) { // Function got set to a motor
|
||||
int32_t val = 1100;
|
||||
PX4_INFO("Setting channel %i minimum to %i", (int) val, i);
|
||||
param_set(_mixing_output.minParamHandle(i), &val);
|
||||
val = 1900;
|
||||
PX4_INFO("Setting channel %i maximum to %i", (int) val, i);
|
||||
param_set(_mixing_output.maxParamHandle(i), &val);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
@ -293,9 +293,9 @@ bool CrsfParser_TryParseCrsfPacket(CrsfPacket_t *const new_packet, CrsfParserSta
|
|||
} else {
|
||||
// We don't know what this packet is, so we'll let the parser continue
|
||||
// just so that we can dequeue it in one shot
|
||||
working_segment_size = packet_size + PACKET_SIZE_TYPE_SIZE;
|
||||
working_segment_size = packet_size - PACKET_SIZE_TYPE_SIZE;
|
||||
|
||||
if (working_segment_size > CRSF_MAX_PACKET_LEN) {
|
||||
if (working_index + working_segment_size + CRC_SIZE > CRSF_MAX_PACKET_LEN) {
|
||||
parser_statistics->invalid_unknown_packet_sizes++;
|
||||
parser_state = PARSER_STATE_HEADER;
|
||||
working_segment_size = HEADER_SIZE;
|
||||
|
|
|
@ -467,29 +467,61 @@ void UavcanGnssBridge::update()
|
|||
// to work.
|
||||
void UavcanGnssBridge::handleInjectDataTopic()
|
||||
{
|
||||
// Limit maximum number of GPS injections to 6 since usually
|
||||
// GPS injections should consist of 1-4 packets (GPS, Glonass, BeiDou, Galileo).
|
||||
// Looking at 6 packets thus guarantees, that at least a full injection
|
||||
// data set is evaluated.
|
||||
static constexpr size_t MAX_NUM_INJECTIONS = 6;
|
||||
// We don't want to call copy again further down if we have already done a
|
||||
// copy in the selection process.
|
||||
bool already_copied = false;
|
||||
gps_inject_data_s msg;
|
||||
|
||||
size_t num_injections = 0;
|
||||
gps_inject_data_s gps_inject_data;
|
||||
// If there has not been a valid RTCM message for a while, try to switch to a different RTCM link
|
||||
if ((hrt_absolute_time() - _last_rtcm_injection_time) > 5_s) {
|
||||
|
||||
while ((num_injections <= MAX_NUM_INJECTIONS) && _gps_inject_data_sub.update(&gps_inject_data)) {
|
||||
// Write the message to the gps device. Note that the message could be fragmented.
|
||||
// But as we don't write anywhere else to the device during operation, we don't
|
||||
// need to assemble the message first.
|
||||
if (_publish_rtcm_stream) {
|
||||
PublishRTCMStream(gps_inject_data.data, gps_inject_data.len);
|
||||
for (int instance = 0; instance < _orb_inject_data_sub.size(); instance++) {
|
||||
const bool exists = _orb_inject_data_sub[instance].advertised();
|
||||
|
||||
if (exists) {
|
||||
if (_orb_inject_data_sub[instance].copy(&msg)) {
|
||||
if ((hrt_absolute_time() - msg.timestamp) < 5_s) {
|
||||
// Remember that we already did a copy on this instance.
|
||||
already_copied = true;
|
||||
_selected_rtcm_instance = instance;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (_publish_moving_baseline_data) {
|
||||
PublishMovingBaselineData(gps_inject_data.data, gps_inject_data.len);
|
||||
}
|
||||
|
||||
num_injections++;
|
||||
}
|
||||
|
||||
bool updated = already_copied;
|
||||
|
||||
// Limit maximum number of GPS injections to 8 since usually
|
||||
// GPS injections should consist of 1-4 packets (GPS, Glonass, BeiDou, Galileo).
|
||||
// Looking at 8 packets thus guarantees, that at least a full injection
|
||||
// data set is evaluated.
|
||||
// Moving Base reuires a higher rate, so we allow up to 8 packets.
|
||||
const size_t max_num_injections = gps_inject_data_s::ORB_QUEUE_LENGTH;
|
||||
size_t num_injections = 0;
|
||||
|
||||
do {
|
||||
if (updated) {
|
||||
num_injections++;
|
||||
|
||||
// Write the message to the gps device. Note that the message could be fragmented.
|
||||
// But as we don't write anywhere else to the device during operation, we don't
|
||||
// need to assemble the message first.
|
||||
if (_publish_rtcm_stream) {
|
||||
PublishRTCMStream(msg.data, msg.len);
|
||||
}
|
||||
|
||||
if (_publish_moving_baseline_data) {
|
||||
PublishMovingBaselineData(msg.data, msg.len);
|
||||
}
|
||||
|
||||
_last_rtcm_injection_time = hrt_absolute_time();
|
||||
}
|
||||
|
||||
updated = _orb_inject_data_sub[_selected_rtcm_instance].update(&msg);
|
||||
|
||||
} while (updated && num_injections < max_num_injections);
|
||||
}
|
||||
|
||||
bool UavcanGnssBridge::PublishRTCMStream(const uint8_t *const data, const size_t data_len)
|
||||
|
|
|
@ -45,6 +45,7 @@
|
|||
#pragma once
|
||||
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/SubscriptionMultiArray.hpp>
|
||||
#include <uORB/PublicationMulti.hpp>
|
||||
#include <uORB/topics/sensor_gps.h>
|
||||
#include <uORB/topics/gps_inject_data.h>
|
||||
|
@ -123,7 +124,9 @@ private:
|
|||
float _last_gnss_auxiliary_hdop{0.0f};
|
||||
float _last_gnss_auxiliary_vdop{0.0f};
|
||||
|
||||
uORB::Subscription _gps_inject_data_sub{ORB_ID(gps_inject_data)};
|
||||
uORB::SubscriptionMultiArray<gps_inject_data_s, gps_inject_data_s::MAX_INSTANCES> _orb_inject_data_sub{ORB_ID::gps_inject_data};
|
||||
hrt_abstime _last_rtcm_injection_time{0}; ///< time of last rtcm injection
|
||||
uint8_t _selected_rtcm_instance{0}; ///< uorb instance that is being used for RTCM corrections
|
||||
|
||||
bool _system_clock_set{false}; ///< Have we set the system clock at least once from GNSS data?
|
||||
|
||||
|
|
|
@ -4,12 +4,11 @@ serial_config:
|
|||
port_config_param:
|
||||
name: UWB_PORT_CFG
|
||||
group: UWB
|
||||
default: ""
|
||||
default: "TEL2"
|
||||
|
||||
parameters:
|
||||
- group: UWB
|
||||
definitions:
|
||||
|
||||
UWB_INIT_OFF_X:
|
||||
description:
|
||||
short: UWB sensor X offset in body frame
|
||||
|
@ -32,7 +31,7 @@ parameters:
|
|||
|
||||
UWB_INIT_OFF_Z:
|
||||
description:
|
||||
short: UWB sensor Y offset in body frame
|
||||
short: UWB sensor Z offset in body frame
|
||||
long: UWB sensor positioning in relation to Drone in NED. Z offset.
|
||||
type: float
|
||||
unit: m
|
||||
|
@ -40,14 +39,52 @@ parameters:
|
|||
increment: 0.01
|
||||
default: 0.00
|
||||
|
||||
UWB_INIT_OFF_YAW:
|
||||
UWB_SENS_ROT:
|
||||
description:
|
||||
short: UWB sensor YAW offset in body frame
|
||||
long: UWB sensor positioning in relation to Drone in NED. Yaw rotation in relation to direction of FMU.
|
||||
type: float
|
||||
unit: deg
|
||||
decimal: 1
|
||||
increment: 0.1
|
||||
default: 0.00
|
||||
|
||||
|
||||
short: UWB sensor orientation
|
||||
long: The orientation of the sensor relative to the forward direction of the body frame. Look up table in src/lib/conversion/rotation.h
|
||||
Default position is the antannaes downward facing, UWB board parallel with body frame.
|
||||
type: enum
|
||||
values:
|
||||
0: ROTATION_NONE
|
||||
1: ROTATION_YAW_45
|
||||
2: ROTATION_YAW_90
|
||||
3: ROTATION_YAW_135
|
||||
4: ROTATION_YAW_180
|
||||
5: ROTATION_YAW_225
|
||||
6: ROTATION_YAW_270
|
||||
7: ROTATION_YAW_315
|
||||
8: ROTATION_ROLL_180
|
||||
9: ROTATION_ROLL_180_YAW_45
|
||||
10: ROTATION_ROLL_180_YAW_90
|
||||
11: ROTATION_ROLL_180_YAW_135
|
||||
12: ROTATION_PITCH_180
|
||||
13: ROTATION_ROLL_180_YAW_225
|
||||
14: ROTATION_ROLL_180_YAW_270
|
||||
15: ROTATION_ROLL_180_YAW_315
|
||||
16: ROTATION_ROLL_90
|
||||
17: ROTATION_ROLL_90_YAW_45
|
||||
18: ROTATION_ROLL_90_YAW_90
|
||||
19: ROTATION_ROLL_90_YAW_135
|
||||
20: ROTATION_ROLL_270
|
||||
21: ROTATION_ROLL_270_YAW_45
|
||||
22: ROTATION_ROLL_270_YAW_90
|
||||
23: ROTATION_ROLL_270_YAW_135
|
||||
24: ROTATION_PITCH_90
|
||||
25: ROTATION_PITCH_270
|
||||
26: ROTATION_PITCH_180_YAW_90
|
||||
27: ROTATION_PITCH_180_YAW_270
|
||||
28: ROTATION_ROLL_90_PITCH_90
|
||||
29: ROTATION_ROLL_180_PITCH_90
|
||||
30: ROTATION_ROLL_270_PITCH_90
|
||||
31: ROTATION_ROLL_90_PITCH_180
|
||||
32: ROTATION_ROLL_270_PITCH_180
|
||||
33: ROTATION_ROLL_90_PITCH_270
|
||||
34: ROTATION_ROLL_180_PITCH_270
|
||||
35: ROTATION_ROLL_270_PITCH_270
|
||||
36: ROTATION_ROLL_90_PITCH_180_YAW_90
|
||||
37: ROTATION_ROLL_90_YAW_270
|
||||
38: ROTATION_ROLL_90_PITCH_68_YAW_293
|
||||
39: ROTATION_PITCH_315
|
||||
40: ROTATION_ROLL_90_PITCH_315
|
||||
default: 0
|
||||
|
|
|
@ -1,6 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2020-2023 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
|
||||
|
@ -63,128 +63,130 @@
|
|||
// The default baudrate of the uwb_sr150 module before configuration
|
||||
#define DEFAULT_BAUD B115200
|
||||
|
||||
const uint8_t CMD_RANGING_STOP[UWB_CMD_LEN ] = {UWB_CMD, 0x00, 0x02, UWB_DRONE_CTL, UWB_CMD_STOP};
|
||||
const uint8_t CMD_RANGING_START[UWB_CMD_LEN ] = {UWB_CMD, 0x00, 0x02, UWB_DRONE_CTL, UWB_CMD_START};
|
||||
const uint8_t CMD_APP_START[UWB_CMD_LEN ] = {0x01, 0x00, 0x02, UWB_APP_START, UWB_PRECNAV_APP};
|
||||
const uint8_t CMD_APP_STOP[0x04 ] = {0x01, 0x00, 0x01, UWB_APP_STOP};
|
||||
|
||||
extern "C" __EXPORT int uwb_sr150_main(int argc, char *argv[]);
|
||||
|
||||
UWB_SR150::UWB_SR150(const char *device_name, speed_t baudrate, bool uwb_pos_debug):
|
||||
UWB_SR150::UWB_SR150(const char *port):
|
||||
ModuleParams(nullptr),
|
||||
ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)),
|
||||
_read_count_perf(perf_alloc(PC_COUNT, "uwb_sr150_count")),
|
||||
_read_err_perf(perf_alloc(PC_COUNT, "uwb_sr150_err"))
|
||||
{
|
||||
_uwb_pos_debug = uwb_pos_debug;
|
||||
// start serial port
|
||||
_uart = open(device_name, O_RDWR | O_NOCTTY);
|
||||
|
||||
if (_uart < 0) { err(1, "could not open %s", device_name); }
|
||||
|
||||
int ret = 0;
|
||||
struct termios uart_config {};
|
||||
ret = tcgetattr(_uart, &uart_config);
|
||||
|
||||
if (ret < 0) { err(1, "failed to get attr"); }
|
||||
|
||||
uart_config.c_oflag &= ~ONLCR; // no CR for every LF
|
||||
ret = cfsetispeed(&uart_config, baudrate);
|
||||
|
||||
if (ret < 0) { err(1, "failed to set input speed"); }
|
||||
|
||||
ret = cfsetospeed(&uart_config, baudrate);
|
||||
|
||||
if (ret < 0) { err(1, "failed to set output speed"); }
|
||||
|
||||
ret = tcsetattr(_uart, TCSANOW, &uart_config);
|
||||
|
||||
if (ret < 0) { err(1, "failed to set attr"); }
|
||||
/* store port name */
|
||||
strncpy(_port, port, sizeof(_port) - 1);
|
||||
/* enforce null termination */
|
||||
_port[sizeof(_port) - 1] = '\0';
|
||||
}
|
||||
|
||||
UWB_SR150::~UWB_SR150()
|
||||
{
|
||||
printf("UWB: Ranging Stopped\t\n");
|
||||
int written = write(_uart, &CMD_APP_STOP, sizeof(CMD_APP_STOP));
|
||||
|
||||
if (written < (int) sizeof(CMD_APP_STOP)) {
|
||||
PX4_ERR("Only wrote %d bytes out of %d.", written, (int) sizeof(CMD_APP_STOP));
|
||||
}
|
||||
|
||||
// stop{}; will be implemented when this is changed to a scheduled work task
|
||||
perf_free(_read_err_perf);
|
||||
perf_free(_read_count_perf);
|
||||
|
||||
close(_uart);
|
||||
}
|
||||
|
||||
void UWB_SR150::run()
|
||||
bool UWB_SR150::init()
|
||||
{
|
||||
// Subscribe to parameter_update message
|
||||
parameters_update();
|
||||
|
||||
//TODO replace with BLE grid configuration
|
||||
grid_info_read(&_uwb_grid_info.target_pos);
|
||||
_uwb_grid_info.num_anchors = 4;
|
||||
_uwb_grid_info.initator_time = hrt_absolute_time();
|
||||
_uwb_grid_info.mac_mode = 0x00;
|
||||
|
||||
/* Grid Info Message*/
|
||||
_uwb_grid.timestamp = hrt_absolute_time();
|
||||
_uwb_grid.initator_time = _uwb_grid_info.initator_time;
|
||||
_uwb_grid.num_anchors = _uwb_grid_info.num_anchors;
|
||||
|
||||
memcpy(&_uwb_grid.target_pos, &_uwb_grid_info.target_pos, sizeof(position_t) * 5); //write Grid positions
|
||||
|
||||
_uwb_grid_pub.publish(_uwb_grid);
|
||||
|
||||
/* Ranging Command */
|
||||
int written = write(_uart, CMD_RANGING_START, UWB_CMD_LEN);
|
||||
|
||||
if (written < UWB_CMD_LEN) {
|
||||
PX4_ERR("Only wrote %d bytes out of %d.", written, (int) sizeof(uint8_t) * UWB_CMD_LEN);
|
||||
// execute Run() on every sensor_accel publication
|
||||
if (!_sensor_uwb_sub.registerCallback()) {
|
||||
PX4_ERR("callback registration failed");
|
||||
return false;
|
||||
}
|
||||
|
||||
while (!should_exit()) {
|
||||
written = UWB_SR150::distance(); //evaluate Ranging Messages until Stop
|
||||
}
|
||||
// alternatively, Run on fixed interval
|
||||
// ScheduleOnInterval(5000_us); // 2000 us interval, 200 Hz rate
|
||||
|
||||
if (!written) { printf("ERROR: Distance Failed"); }
|
||||
|
||||
// Automatic Stop. This should not be reachable
|
||||
written = write(_uart, &CMD_RANGING_STOP, UWB_CMD_LEN);
|
||||
|
||||
if (written < (int) sizeof(CMD_RANGING_STOP)) {
|
||||
PX4_ERR("Only wrote %d bytes out of %d.", written, (int) sizeof(CMD_RANGING_STOP));
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
void UWB_SR150::grid_info_read(position_t *grid)
|
||||
void UWB_SR150::start()
|
||||
{
|
||||
//place holder, until UWB initiator can respond with Grid info
|
||||
/* This Reads the position of each Anchor in the Grid.
|
||||
Right now the Grid configuration is saved on the SD card.
|
||||
In the future, we would like, that the Initiator responds with the Grid Information (Including Position). */
|
||||
PX4_INFO("Reading UWB GRID from SD... \t\n");
|
||||
FILE *file;
|
||||
file = fopen(UWB_GRID_CONFIG, "r");
|
||||
/* schedule a cycle to start things */
|
||||
ScheduleNow();
|
||||
}
|
||||
|
||||
int bread = 0;
|
||||
void UWB_SR150::stop()
|
||||
{
|
||||
ScheduleClear();
|
||||
}
|
||||
|
||||
for (int i = 0; i < 5; i++) {
|
||||
bread += fscanf(file, "%hd,%hd,%hd\n", &grid[i].x, &grid[i].y, &grid[i].z);
|
||||
void UWB_SR150::Run()
|
||||
{
|
||||
if (should_exit()) {
|
||||
ScheduleClear();
|
||||
exit_and_cleanup();
|
||||
return;
|
||||
}
|
||||
|
||||
if (bread == 5 * 3) {
|
||||
PX4_INFO("GRID INFO READ! bytes read: %d \t\n", bread);
|
||||
if (_uart < 0) {
|
||||
/* open fd */
|
||||
_uart = ::open(_port, O_RDWR | O_NOCTTY | O_NONBLOCK);
|
||||
|
||||
} else { //use UUID from Grid survey
|
||||
PX4_INFO("GRID INFO Missing! bytes read: %d \t\n Using standrd Grid \t\n", bread);
|
||||
position_t grid_setup[5] = {{0x0, 0x0, 0x0}, {(int16_t)0xff68, 0x0, 0x0a}, {0x99, 0x0, 0x0a}, {0x0, 0x96, 0x64}, {0x0, (int16_t)0xff6a, 0x63}};
|
||||
memcpy(grid, &grid_setup, sizeof(grid_setup));
|
||||
PX4_INFO("Insert \"uwb_grid_config.csv\" containing gridinfo in cm at \"/fs/microsd/etc/\".\t\n");
|
||||
PX4_INFO("n + 1 Anchor Positions starting with Landing Target. Int16 Format: \"x,y,z\" \t\n");
|
||||
if (_uart < 0) {
|
||||
PX4_ERR("open failed (%i)", errno);
|
||||
return;
|
||||
}
|
||||
|
||||
struct termios uart_config;
|
||||
|
||||
int termios_state;
|
||||
|
||||
/* fill the struct for the new configuration */
|
||||
tcgetattr(_uart, &uart_config);
|
||||
|
||||
/* clear ONLCR flag (which appends a CR for every LF) */
|
||||
uart_config.c_oflag &= ~ONLCR;
|
||||
|
||||
//TODO: should I keep this?
|
||||
/* no parity, one stop bit */
|
||||
uart_config.c_cflag &= ~(CSTOPB | PARENB);
|
||||
|
||||
unsigned speed = DEFAULT_BAUD;
|
||||
|
||||
/* set baud rate */
|
||||
if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
|
||||
PX4_ERR("CFG: %d ISPD", termios_state);
|
||||
}
|
||||
|
||||
if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
|
||||
PX4_ERR("CFG: %d OSPD", termios_state);
|
||||
}
|
||||
|
||||
if ((termios_state = tcsetattr(_uart, TCSANOW, &uart_config)) < 0) {
|
||||
PX4_ERR("baud %d ATTR", termios_state);
|
||||
}
|
||||
}
|
||||
|
||||
fclose(file);
|
||||
/* perform collection */
|
||||
int collect_ret = collectData();
|
||||
|
||||
if (collect_ret == -EAGAIN) {
|
||||
/* reschedule to grab the missing bits, time to transmit 8 bytes @ 9600 bps */
|
||||
ScheduleDelayed(1042 * 8);
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
if (OK != collect_ret) {
|
||||
|
||||
/* we know the sensor needs about four seconds to initialize */
|
||||
if (hrt_absolute_time() > 5 * 1000 * 1000LL && _consecutive_fail_count < 5) {
|
||||
PX4_ERR("collection error #%u", _consecutive_fail_count);
|
||||
}
|
||||
|
||||
_consecutive_fail_count++;
|
||||
|
||||
/* restart the measurement state machine */
|
||||
start();
|
||||
return;
|
||||
|
||||
} else {
|
||||
/* apparently success */
|
||||
_consecutive_fail_count = 0;
|
||||
}
|
||||
}
|
||||
|
||||
int UWB_SR150::custom_command(int argc, char *argv[])
|
||||
|
@ -214,43 +216,20 @@ $ uwb start -d /dev/ttyS2
|
|||
PRINT_MODULE_USAGE_COMMAND("start");
|
||||
PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, "<file:dev>", "Name of device for serial communication with UWB", false);
|
||||
PRINT_MODULE_USAGE_PARAM_STRING('b', nullptr, "<int>", "Baudrate for serial communication", false);
|
||||
PRINT_MODULE_USAGE_PARAM_STRING('p', nullptr, "<int>", "Position Debug: displays errors in Multilateration", false);
|
||||
PRINT_MODULE_USAGE_COMMAND("stop");
|
||||
PRINT_MODULE_USAGE_COMMAND("status");
|
||||
return 0;
|
||||
}
|
||||
|
||||
int UWB_SR150::task_spawn(int argc, char *argv[])
|
||||
{
|
||||
int task_id = px4_task_spawn_cmd(
|
||||
"uwb_driver",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_DEFAULT,
|
||||
2048,
|
||||
&run_trampoline,
|
||||
argv
|
||||
);
|
||||
|
||||
if (task_id < 0) {
|
||||
return -errno;
|
||||
|
||||
} else {
|
||||
_task_id = task_id;
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
UWB_SR150 *UWB_SR150::instantiate(int argc, char *argv[])
|
||||
{
|
||||
int ch;
|
||||
int option_index = 1;
|
||||
const char *option_arg;
|
||||
const char *device_name = nullptr;
|
||||
bool error_flag = false;
|
||||
const char *device_name = UWB_DEFAULT_PORT;
|
||||
int baudrate = 0;
|
||||
bool uwb_pos_debug = false; // Display UWB position calculation debug Messages
|
||||
|
||||
while ((ch = px4_getopt(argc, argv, "d:b:p", &option_index, &option_arg)) != EOF) {
|
||||
while ((ch = px4_getopt(argc, argv, "d:b", &option_index, &option_arg)) != EOF) {
|
||||
switch (ch) {
|
||||
case 'd':
|
||||
device_name = option_arg;
|
||||
|
@ -260,47 +239,54 @@ UWB_SR150 *UWB_SR150::instantiate(int argc, char *argv[])
|
|||
px4_get_parameter_value(option_arg, baudrate);
|
||||
break;
|
||||
|
||||
case 'p':
|
||||
|
||||
uwb_pos_debug = true;
|
||||
break;
|
||||
|
||||
default:
|
||||
PX4_WARN("Unrecognized flag: %c", ch);
|
||||
error_flag = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (!error_flag && device_name == nullptr) {
|
||||
print_usage("Device name not provided. Using default Device: TEL1:/dev/ttyS4 \n");
|
||||
device_name = "TEL2";
|
||||
error_flag = true;
|
||||
}
|
||||
UWB_SR150 *instance = new UWB_SR150(device_name);
|
||||
|
||||
if (!error_flag && baudrate == 0) {
|
||||
printf("Baudrate not provided. Using default Baud: 115200 \n");
|
||||
baudrate = B115200;
|
||||
}
|
||||
if (instance) {
|
||||
_object.store(instance);
|
||||
_task_id = task_id_is_work_queue;
|
||||
|
||||
if (!error_flag && uwb_pos_debug == true) {
|
||||
printf("UWB Position algorithm Debugging \n");
|
||||
}
|
||||
instance->ScheduleOnInterval(5000_us);
|
||||
|
||||
if (error_flag) {
|
||||
PX4_WARN("Failed to start UWB driver. \n");
|
||||
return nullptr;
|
||||
if (instance->init()) {
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
} else {
|
||||
PX4_INFO("Constructing UWB_SR150. Device: %s", device_name);
|
||||
return new UWB_SR150(device_name, baudrate, uwb_pos_debug);
|
||||
PX4_ERR("alloc failed");
|
||||
}
|
||||
|
||||
delete instance;
|
||||
_object.store(nullptr);
|
||||
_task_id = -1;
|
||||
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
int uwb_sr150_main(int argc, char *argv[])
|
||||
{
|
||||
return UWB_SR150::main(argc, argv);
|
||||
}
|
||||
|
||||
void UWB_SR150::parameters_update()
|
||||
{
|
||||
if (_parameter_update_sub.updated()) {
|
||||
parameter_update_s param_update;
|
||||
_parameter_update_sub.copy(¶m_update);
|
||||
|
||||
// If any parameter updated, call updateParams() to check if
|
||||
// this class attributes need updating (and do so).
|
||||
updateParams();
|
||||
}
|
||||
}
|
||||
|
||||
int UWB_SR150::distance()
|
||||
int UWB_SR150::collectData()
|
||||
{
|
||||
_uwb_init_offset_v3f = matrix::Vector3f(_uwb_init_off_x.get(), _uwb_init_off_y.get(),
|
||||
_uwb_init_off_z.get()); //set offset at the start
|
||||
uint8_t *buffer = (uint8_t *) &_distance_result_msg;
|
||||
|
||||
FD_ZERO(&_uart_set);
|
||||
|
@ -350,366 +336,54 @@ int UWB_SR150::distance()
|
|||
perf_count(_read_count_perf);
|
||||
|
||||
// All of the following criteria must be met for the message to be acceptable:
|
||||
// - Size of message == sizeof(distance_msg_t) (51 bytes)
|
||||
// - Size of message == sizeof(distance_msg_t) (36 bytes)
|
||||
// - status == 0x00
|
||||
// - Values of all 3 position measurements are reasonable
|
||||
// (If one or more anchors is missed, then position might be an unreasonably large number.)
|
||||
bool ok = (buffer_location == sizeof(distance_msg_t) && _distance_result_msg.stop == 0x1b); //||
|
||||
//(buffer_location == sizeof(grid_msg_t) && _distance_result_msg.stop == 0x1b)
|
||||
//);
|
||||
bool ok = (buffer_location == sizeof(distance_msg_t) && _distance_result_msg.stop == 0x1b);
|
||||
|
||||
if (ok) {
|
||||
|
||||
/* Ranging Message*/
|
||||
_uwb_distance.timestamp = hrt_absolute_time();
|
||||
_uwb_distance.time_uwb_ms = _distance_result_msg.time_uwb_ms;
|
||||
_uwb_distance.counter = _distance_result_msg.seq_ctr;
|
||||
_uwb_distance.sessionid = _distance_result_msg.sessionId;
|
||||
_uwb_distance.time_offset = _distance_result_msg.range_interval;
|
||||
_sensor_uwb.timestamp = hrt_absolute_time();
|
||||
|
||||
for (int i = 0; i < 4; i++) {
|
||||
_uwb_distance.anchor_distance[i] = _distance_result_msg.measurements[i].distance;
|
||||
_uwb_distance.nlos[i] = _distance_result_msg.measurements[i].nLos;
|
||||
_uwb_distance.aoafirst[i] = float(_distance_result_msg.measurements[i].aoaFirst) /
|
||||
128; // Angle of Arrival has Format Q9.7; dividing by 2^7 results in the correct value
|
||||
}
|
||||
_sensor_uwb.sessionid = _distance_result_msg.sessionId;
|
||||
_sensor_uwb.time_offset = _distance_result_msg.range_interval;
|
||||
_sensor_uwb.counter = _distance_result_msg.seq_ctr;
|
||||
_sensor_uwb.mac = _distance_result_msg.MAC;
|
||||
|
||||
// Algorithm goes here
|
||||
UWB_POS_ERROR_CODES UWB_POS_ERROR = UWB_SR150::localization();
|
||||
_sensor_uwb.mac_dest = _distance_result_msg.measurements.MAC;
|
||||
_sensor_uwb.status = _distance_result_msg.measurements.status;
|
||||
_sensor_uwb.nlos = _distance_result_msg.measurements.nLos;
|
||||
_sensor_uwb.distance = double(_distance_result_msg.measurements.distance) / 100;
|
||||
|
||||
_uwb_distance.status = UWB_POS_ERROR;
|
||||
|
||||
if (UWB_OK == UWB_POS_ERROR) {
|
||||
/*Angle of Arrival has Format Q9.7; dividing by 2^7 and negating results in the correct value*/
|
||||
_sensor_uwb.aoa_azimuth_dev = - double(_distance_result_msg.measurements.aoa_azimuth) / 128;
|
||||
_sensor_uwb.aoa_elevation_dev = - double(_distance_result_msg.measurements.aoa_elevation) / 128;
|
||||
_sensor_uwb.aoa_azimuth_resp = - double(_distance_result_msg.measurements.aoa_dest_azimuth) / 128;
|
||||
_sensor_uwb.aoa_elevation_resp = - double(_distance_result_msg.measurements.aoa_dest_elevation) / 128;
|
||||
|
||||
_uwb_distance.position[0] = _rel_pos(0);
|
||||
_uwb_distance.position[1] = _rel_pos(1);
|
||||
_uwb_distance.position[2] = _rel_pos(2);
|
||||
|
||||
} else {
|
||||
//only print the error if debug is enabled
|
||||
if (_uwb_pos_debug) {
|
||||
switch (UWB_POS_ERROR) { //UWB POSITION ALGORItHM Errors
|
||||
case UWB_ANC_BELOW_THREE:
|
||||
PX4_INFO("UWB not enough anchors for doing localization");
|
||||
break;
|
||||
/*Angle of Arrival has Format Q9.7; dividing by 2^7 and negating results in the correct value*/
|
||||
_sensor_uwb.aoa_azimuth_fom = - double(_distance_result_msg.measurements.aoa_azimuth) / 128;
|
||||
_sensor_uwb.aoa_elevation_fom = - double(_distance_result_msg.measurements.aoa_elevation) / 128;
|
||||
_sensor_uwb.aoa_dest_azimuth_fom = - double(_distance_result_msg.measurements.aoa_dest_azimuth) / 128;
|
||||
_sensor_uwb.aoa_dest_elevation_fom = - double(_distance_result_msg.measurements.aoa_dest_elevation) / 128;
|
||||
|
||||
case UWB_LIN_DEP_FOR_THREE:
|
||||
PX4_INFO("UWB localization: linear dependent with 3 Anchors");
|
||||
break;
|
||||
/* Sensor physical offset*/ //for now we propagate the physical configuration via Uorb
|
||||
_sensor_uwb.orientation = _sensor_rot.get();
|
||||
_sensor_uwb.offset_x = _offset_x.get();
|
||||
_sensor_uwb.offset_y = _offset_y.get();
|
||||
_sensor_uwb.offset_z = _offset_z.get();
|
||||
|
||||
case UWB_ANC_ON_ONE_LEVEL:
|
||||
PX4_INFO("UWB localization: Anchors are on a X,Y Plane and there are not enought Anchors");
|
||||
break;
|
||||
|
||||
case UWB_LIN_DEP_FOR_FOUR:
|
||||
PX4_INFO("UWB localization: linear dependent with four or more Anchors");
|
||||
break;
|
||||
|
||||
case UWB_RANK_ZERO:
|
||||
PX4_INFO("UWB localization: rank is zero");
|
||||
break;
|
||||
|
||||
default:
|
||||
PX4_INFO("UWB localization: Unknown failure in Position Algorithm");
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
_uwb_distance_pub.publish(_uwb_distance);
|
||||
_sensor_uwb_pub.publish(_sensor_uwb);
|
||||
|
||||
} else {
|
||||
//PX4_ERR("Read %d bytes instead of %d.", (int) buffer_location, (int) sizeof(distance_msg_t));
|
||||
perf_count(_read_err_perf);
|
||||
|
||||
if (buffer_location == 0) {
|
||||
PX4_WARN("UWB module is not responding.");
|
||||
|
||||
//TODO add retry Ranging Start Message. Sometimes the UWB devices Crashes. (Check Power)
|
||||
}
|
||||
}
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
UWB_POS_ERROR_CODES UWB_SR150::localization()
|
||||
{
|
||||
// WIP
|
||||
/******************************************************
|
||||
****************** 3D Localization *******************
|
||||
*****************************************************/
|
||||
|
||||
/*!@brief: This function calculates the 3D position of the initiator from the anchor distances and positions using least squared errors.
|
||||
* The function expects more than 4 anchors. The used equation system looks like follows:\n
|
||||
\verbatim
|
||||
- -
|
||||
| M_11 M_12 M_13 | x b[0]
|
||||
| M_12 M_22 M_23 | * y = b[1]
|
||||
| M_23 M_13 M_33 | z b[2]
|
||||
- -
|
||||
\endverbatim
|
||||
* @param distances_cm_in_pt: Pointer to array that contains the distances to the anchors in cm (including invalid results)
|
||||
* @param no_distances: Number of valid distances in distance array (it's not the size of the array)
|
||||
* @param anchor_pos: Pointer to array that contains anchor positions in cm (including positions related to invalid results)
|
||||
* @param no_anc_positions: Number of valid anchor positions in the position array (it's not the size of the array)
|
||||
* @param position_result_pt: Pointer toposition. position_t variable that holds the result of this calculation
|
||||
* @return: The function returns a status code. */
|
||||
|
||||
/* Algorithm used:
|
||||
* Linear Least Sqaures to solve Multilateration
|
||||
* with a Special case if there are only 3 Anchors.
|
||||
* Output is the Coordinates of the Initiator in relation to Anchor 0 in NEU (North-East-Up) Framing
|
||||
* In cm
|
||||
*/
|
||||
|
||||
/* Resulting Position Vector*/
|
||||
int64_t x_pos = 0;
|
||||
int64_t y_pos = 0;
|
||||
int64_t z_pos = 0;
|
||||
/* Matrix components (3*3 Matrix resulting from least square error method) [cm^2] */
|
||||
int64_t M_11 = 0;
|
||||
int64_t M_12 = 0; // = M_21
|
||||
int64_t M_13 = 0; // = M_31
|
||||
int64_t M_22 = 0;
|
||||
int64_t M_23 = 0; // = M_23
|
||||
int64_t M_33 = 0;
|
||||
|
||||
/* Vector components (3*1 Vector resulting from least square error method) [cm^3] */
|
||||
int64_t b[3] = {0};
|
||||
|
||||
/* Miscellaneous variables */
|
||||
int64_t temp = 0;
|
||||
int64_t temp2 = 0;
|
||||
int64_t nominator = 0;
|
||||
int64_t denominator = 0;
|
||||
bool anchors_on_x_y_plane = true; // Is true, if all anchors are on the same height => x-y-plane
|
||||
bool lin_dep = true; // All vectors are linear dependent, if this variable is true
|
||||
uint8_t ind_y_indi =
|
||||
0; //numberr of independet vectors // First anchor index, for which the second row entry of the matrix [(x_1 - x_0) (x_2 - x_0) ... ; (y_1 - x_0) (y_2 - x_0) ...] is non-zero => linear independent
|
||||
|
||||
/* Arrays for used distances and anchor positions (without rejected ones) */
|
||||
uint8_t no_distances = _uwb_grid_info.num_anchors;
|
||||
uint32_t distances_cm[no_distances];
|
||||
position_t anchor_pos[no_distances]; //position in CM
|
||||
uint8_t no_valid_distances = 0;
|
||||
|
||||
/* Reject invalid distances (including related anchor position) */
|
||||
for (int i = 0; i < no_distances; i++) {
|
||||
if (_distance_result_msg.measurements[i].distance != 0xFFFFu) {
|
||||
//excludes any distance that is 0xFFFFU (int16 Maximum Value)
|
||||
distances_cm[no_valid_distances] = _distance_result_msg.measurements[i].distance;
|
||||
anchor_pos[no_valid_distances] = _uwb_grid_info.anchor_pos[i];
|
||||
no_valid_distances++;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
/* Check, if there are enough valid results for doing the localization at all */
|
||||
if (no_valid_distances < 3) {
|
||||
return UWB_ANC_BELOW_THREE;
|
||||
}
|
||||
|
||||
/* Check, if anchors are on the same x-y plane */
|
||||
for (int i = 1; i < no_valid_distances; i++) {
|
||||
if (anchor_pos[i].z != anchor_pos[0].z) {
|
||||
anchors_on_x_y_plane = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/**** Check, if there are enough linear independent anchor positions ****/
|
||||
|
||||
/* Check, if the matrix |(x_1 - x_0) (x_2 - x_0) ... | has rank 2
|
||||
* |(y_1 - y_0) (y_2 - y_0) ... | */
|
||||
|
||||
for (ind_y_indi = 2; ((ind_y_indi < no_valid_distances) && (lin_dep == true)); ind_y_indi++) {
|
||||
temp = ((int64_t)anchor_pos[ind_y_indi].y - (int64_t)anchor_pos[0].y) * ((int64_t)anchor_pos[1].x -
|
||||
(int64_t)anchor_pos[0].x);
|
||||
temp2 = ((int64_t)anchor_pos[1].y - (int64_t)anchor_pos[0].y) * ((int64_t)anchor_pos[ind_y_indi].x -
|
||||
(int64_t)anchor_pos[0].x);
|
||||
|
||||
if ((temp - temp2) != 0) {
|
||||
lin_dep = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/* Leave function, if rank is below 2 */
|
||||
if (lin_dep == true) {
|
||||
return UWB_LIN_DEP_FOR_THREE;
|
||||
}
|
||||
|
||||
/* If the anchors are not on the same plane, three vectors must be independent => check */
|
||||
if (!anchors_on_x_y_plane) {
|
||||
/* Check, if there are enough valid results for doing the localization */
|
||||
if (no_valid_distances < 4) {
|
||||
return UWB_ANC_ON_ONE_LEVEL;
|
||||
}
|
||||
|
||||
/* Check, if the matrix |(x_1 - x_0) (x_2 - x_0) (x_3 - x_0) ... | has rank 3 (Rank y, y already checked)
|
||||
* |(y_1 - y_0) (y_2 - y_0) (y_3 - y_0) ... |
|
||||
* |(z_1 - z_0) (z_2 - z_0) (z_3 - z_0) ... | */
|
||||
lin_dep = true;
|
||||
|
||||
for (int i = 2; ((i < no_valid_distances) && (lin_dep == true)); i++) {
|
||||
if (i != ind_y_indi) {
|
||||
/* (x_1 - x_0)*[(y_2 - y_0)(z_n - z_0) - (y_n - y_0)(z_2 - z_0)] */
|
||||
temp = ((int64_t)anchor_pos[ind_y_indi].y - (int64_t)anchor_pos[0].y) * ((int64_t)anchor_pos[i].z -
|
||||
(int64_t)anchor_pos[0].z);
|
||||
temp -= ((int64_t)anchor_pos[i].y - (int64_t)anchor_pos[0].y) * ((int64_t)anchor_pos[ind_y_indi].z -
|
||||
(int64_t)anchor_pos[0].z);
|
||||
temp2 = ((int64_t)anchor_pos[1].x - (int64_t)anchor_pos[0].x) * temp;
|
||||
|
||||
/* Add (x_2 - x_0)*[(y_n - y_0)(z_1 - z_0) - (y_1 - y_0)(z_n - z_0)] */
|
||||
temp = ((int64_t)anchor_pos[i].y - (int64_t)anchor_pos[0].y) * ((int64_t)anchor_pos[1].z - (int64_t)anchor_pos[0].z);
|
||||
temp -= ((int64_t)anchor_pos[1].y - (int64_t)anchor_pos[0].y) * ((int64_t)anchor_pos[i].z - (int64_t)anchor_pos[0].z);
|
||||
temp2 += ((int64_t)anchor_pos[ind_y_indi].x - (int64_t)anchor_pos[0].x) * temp;
|
||||
|
||||
/* Add (x_n - x_0)*[(y_1 - y_0)(z_2 - z_0) - (y_2 - y_0)(z_1 - z_0)] */
|
||||
temp = ((int64_t)anchor_pos[1].y - (int64_t)anchor_pos[0].y) * ((int64_t)anchor_pos[ind_y_indi].z -
|
||||
(int64_t)anchor_pos[0].z);
|
||||
temp -= ((int64_t)anchor_pos[ind_y_indi].y - (int64_t)anchor_pos[0].y) * ((int64_t)anchor_pos[1].z -
|
||||
(int64_t)anchor_pos[0].z);
|
||||
temp2 += ((int64_t)anchor_pos[i].x - (int64_t)anchor_pos[0].x) * temp;
|
||||
|
||||
if (temp2 != 0) { lin_dep = false; }
|
||||
}
|
||||
}
|
||||
|
||||
/* Leave function, if rank is below 3 */
|
||||
if (lin_dep == true) {
|
||||
return UWB_LIN_DEP_FOR_FOUR;
|
||||
}
|
||||
}
|
||||
|
||||
/************************************************** Algorithm ***********************************************************************/
|
||||
|
||||
/* Writing values resulting from least square error method (A_trans*A*x = A_trans*r; row 0 was used to remove x^2,y^2,z^2 entries => index starts at 1) */
|
||||
for (int i = 1; i < no_valid_distances; i++) {
|
||||
/* Matrix (needed to be multiplied with 2, afterwards) */
|
||||
M_11 += sq((int64_t)(anchor_pos[i].x - anchor_pos[0].x));
|
||||
M_12 += (int64_t)((int64_t)(anchor_pos[i].x - anchor_pos[0].x) * (int64_t)(anchor_pos[i].y - anchor_pos[0].y));
|
||||
M_13 += (int64_t)((int64_t)(anchor_pos[i].x - anchor_pos[0].x) * (int64_t)(anchor_pos[i].z - anchor_pos[0].z));
|
||||
M_22 += sq((int64_t)(anchor_pos[i].y - anchor_pos[0].y));
|
||||
M_23 += (int64_t)((int64_t)(anchor_pos[i].y - anchor_pos[0].y) * (int64_t)(anchor_pos[i].z - anchor_pos[0].z));
|
||||
M_33 += sq((int64_t)(anchor_pos[i].z - anchor_pos[0].z));
|
||||
|
||||
/* Vector */
|
||||
temp = sq(distances_cm[0]) - sq(distances_cm[i])
|
||||
+ sq(anchor_pos[i].x) + sq(anchor_pos[i].y)
|
||||
+ sq(anchor_pos[i].z) - sq(anchor_pos[0].x)
|
||||
- sq(anchor_pos[0].y) - sq(anchor_pos[0].z);
|
||||
|
||||
b[0] += (int64_t)((int64_t)(anchor_pos[i].x - anchor_pos[0].x) * temp);
|
||||
b[1] += (int64_t)((int64_t)(anchor_pos[i].y - anchor_pos[0].y) * temp);
|
||||
b[2] += (int64_t)((int64_t)(anchor_pos[i].z - anchor_pos[0].z) * temp);
|
||||
}
|
||||
|
||||
M_11 = 2 * M_11;
|
||||
M_12 = 2 * M_12;
|
||||
M_13 = 2 * M_13;
|
||||
M_22 = 2 * M_22;
|
||||
M_23 = 2 * M_23;
|
||||
M_33 = 2 * M_33;
|
||||
|
||||
/* Calculating the z-position, if calculation is possible (at least one anchor at z != 0) */
|
||||
if (anchors_on_x_y_plane == false) {
|
||||
nominator = b[0] * (M_12 * M_23 - M_13 * M_22) + b[1] * (M_12 * M_13 - M_11 * M_23) + b[2] *
|
||||
(M_11 * M_22 - M_12 * M_12); // [cm^7]
|
||||
denominator = M_11 * (M_33 * M_22 - M_23 * M_23) + 2 * M_12 * M_13 * M_23 - M_33 * M_12 * M_12 - M_22 * M_13 *
|
||||
M_13; // [cm^6]
|
||||
|
||||
/* Check, if denominator is zero (Rank of matrix not high enough) */
|
||||
if (denominator == 0) {
|
||||
return UWB_RANK_ZERO;
|
||||
}
|
||||
|
||||
z_pos = ((nominator * 10) / denominator + 5) / 10; // [cm]
|
||||
}
|
||||
|
||||
/* Else prepare for different calculation approach (after x and y were calculated) */
|
||||
else {
|
||||
z_pos = 0;
|
||||
}
|
||||
|
||||
/* Calculating the y-position */
|
||||
nominator = b[1] * M_11 - b[0] * M_12 - (z_pos * (M_11 * M_23 - M_12 * M_13)); // [cm^5]
|
||||
denominator = M_11 * M_22 - M_12 * M_12;// [cm^4]
|
||||
|
||||
/* Check, if denominator is zero (Rank of matrix not high enough) */
|
||||
if (denominator == 0) {
|
||||
return UWB_RANK_ZERO;
|
||||
}
|
||||
|
||||
y_pos = ((nominator * 10) / denominator + 5) / 10; // [cm]
|
||||
|
||||
/* Calculating the x-position */
|
||||
nominator = b[0] - z_pos * M_13 - y_pos * M_12; // [cm^3]
|
||||
denominator = M_11; // [cm^2]
|
||||
|
||||
x_pos = ((nominator * 10) / denominator + 5) / 10;// [cm]
|
||||
|
||||
/* Calculate z-position form x and y coordinates, if z can't be determined by previous steps (All anchors at z_n = 0) */
|
||||
if (anchors_on_x_y_plane == true) {
|
||||
/* Calculate z-positon relative to the anchor grid's height */
|
||||
for (int i = 0; i < no_distances; i++) {
|
||||
/* z² = dis_meas_n² - (x - x_anc_n)² - (y - y_anc_n)² */
|
||||
temp = (int64_t)((int64_t)pow(distances_cm[i], 2)
|
||||
- (int64_t)pow((x_pos - (int64_t)anchor_pos[i].x), 2)
|
||||
- (int64_t)pow((y_pos - (int64_t)anchor_pos[i].y), 2));
|
||||
|
||||
/* z² must be positive, else x and y must be wrong => calculate positive sqrt and sum up all calculated heights, if positive */
|
||||
if (temp >= 0) {
|
||||
z_pos += (int64_t)sqrt(temp);
|
||||
|
||||
} else {
|
||||
z_pos = 0;
|
||||
}
|
||||
}
|
||||
|
||||
z_pos = z_pos / no_distances; // Divide sum by number of distances to get the average
|
||||
|
||||
/* Add height of the anchor grid's height */
|
||||
z_pos += anchor_pos[0].z;
|
||||
}
|
||||
|
||||
//Output is the Coordinates of the Initiator in relation to 0,0,0 in NEU (North-East-Up) Framing
|
||||
// The end goal of this math is to get the position relative to the landing point in the NED frame.
|
||||
_current_position_uwb_init = matrix::Vector3f(x_pos, y_pos, z_pos);
|
||||
|
||||
// Construct the rotation from the UWB_R4frame to the NWU frame.
|
||||
// The UWB_SR150 frame is just NWU, rotated by some amount about the Z (up) axis. (Parameter Yaw offset)
|
||||
// To get back to NWU, just rotate by negative this amount about Z.
|
||||
_uwb_init_to_nwu = matrix::Dcmf(matrix::Eulerf(0.0f, 0.0f,
|
||||
-(_uwb_init_off_yaw.get() * M_PI_F / 180.0f))); //
|
||||
// The actual conversion:
|
||||
// - Subtract _landing_point to get the position relative to the landing point, in UWB_R4 frame
|
||||
// - Rotate by _rddrone_to_nwu to get into the NWU frame
|
||||
// - Rotate by _nwu_to_ned to get into the NED frame
|
||||
_current_position_ned = _nwu_to_ned * _uwb_init_to_nwu * _current_position_uwb_init;
|
||||
|
||||
// Now the position is the landing point relative to the vehicle.
|
||||
// so the only thing left is to convert cm to Meters and to add the Initiator offset
|
||||
_rel_pos = _current_position_ned / 100 + _uwb_init_offset_v3f;
|
||||
|
||||
// The UWB report contains the position of the vehicle relative to the landing point.
|
||||
|
||||
return UWB_OK;
|
||||
}
|
||||
|
||||
int uwb_sr150_main(int argc, char *argv[])
|
||||
{
|
||||
return UWB_SR150::main(argc, argv);
|
||||
}
|
||||
|
||||
void UWB_SR150::parameters_update()
|
||||
{
|
||||
if (_parameter_update_sub.updated()) {
|
||||
parameter_update_s param_update;
|
||||
_parameter_update_sub.copy(¶m_update);
|
||||
|
||||
// If any parameter updated, call updateParams() to check if
|
||||
// this class attributes need updating (and do so).
|
||||
updateParams();
|
||||
}
|
||||
}
|
||||
|
|
|
@ -1,6 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2020-2023 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
|
||||
|
@ -38,101 +38,63 @@
|
|||
#include <poll.h>
|
||||
#include <sys/select.h>
|
||||
#include <sys/time.h>
|
||||
#include <perf/perf_counter.h>
|
||||
#include <lib/conversion/rotation.h>
|
||||
|
||||
#include <px4_platform_common/module_params.h>
|
||||
#include <px4_platform_common/module.h>
|
||||
#include <perf/perf_counter.h>
|
||||
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
|
||||
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/topics/landing_target_pose.h>
|
||||
#include <uORB/topics/uwb_grid.h>
|
||||
#include <uORB/topics/uwb_distance.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/SubscriptionCallback.hpp>
|
||||
#include <uORB/SubscriptionInterval.hpp>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/sensor_uwb.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
|
||||
#include <matrix/math.hpp>
|
||||
#include <matrix/Matrix.hpp>
|
||||
|
||||
#define UWB_DEFAULT_PORT "/dev/ttyS1"
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
#define UWB_CMD 0x8e
|
||||
#define UWB_CMD_START 0x01
|
||||
#define UWB_CMD_STOP 0x00
|
||||
#define UWB_CMD_RANGING 0x0A
|
||||
#define STOP_B 0x0A
|
||||
|
||||
#define UWB_PRECNAV_APP 0x04
|
||||
#define UWB_APP_START 0x10
|
||||
#define UWB_APP_STOP 0x11
|
||||
#define UWB_SESSION_START 0x22
|
||||
#define UWB_SESSION_STOP 0x23
|
||||
#define UWB_RANGING_START 0x01
|
||||
#define UWB_RANGING_STOP 0x00
|
||||
#define UWB_DRONE_CTL 0x0A
|
||||
|
||||
#define UWB_CMD_LEN 0x05
|
||||
#define UWB_CMD_DISTANCE_LEN 0x21
|
||||
#define UWB_MAC_MODE 2
|
||||
#define MAX_ANCHORS 12
|
||||
#define UWB_GRID_CONFIG "/fs/microsd/etc/uwb_grid_config.csv"
|
||||
|
||||
typedef struct { //needs higher accuracy?
|
||||
float lat, lon, alt, yaw; //offset to true North
|
||||
} gps_pos_t;
|
||||
|
||||
typedef struct {
|
||||
int16_t x, y, z; //axis in cm
|
||||
} position_t; // Position of a device or target in 3D space
|
||||
|
||||
enum UWB_POS_ERROR_CODES {
|
||||
UWB_OK,
|
||||
UWB_ANC_BELOW_THREE,
|
||||
UWB_LIN_DEP_FOR_THREE,
|
||||
UWB_ANC_ON_ONE_LEVEL,
|
||||
UWB_LIN_DEP_FOR_FOUR,
|
||||
UWB_RANK_ZERO
|
||||
};
|
||||
|
||||
typedef struct {
|
||||
uint8_t MAC[2]; // MAC Adress of UWB device
|
||||
uint8_t status; // Status of Measurement
|
||||
uint16_t distance; // Distance in cm
|
||||
uint8_t nLos; // line of sight y/n
|
||||
uint16_t aoaFirst; // Angle of Arrival of incoming msg
|
||||
uint16_t MAC; // MAC address of UWB device
|
||||
uint8_t status; // Status of Measurement
|
||||
uint16_t distance; // Distance in cm
|
||||
uint8_t nLos; // line of sight y/n
|
||||
int16_t aoa_azimuth; // AOA of incoming msg for Azimuth antenna pairing
|
||||
int16_t aoa_elevation; // AOA of incoming msg for Altitude antenna pairing
|
||||
int16_t aoa_dest_azimuth; // AOA destination Azimuth
|
||||
int16_t aoa_dest_elevation; // AOA destination elevation
|
||||
uint8_t aoa_azimuth_FOM; // AOA Azimuth FOM
|
||||
uint8_t aoa_elevation_FOM; // AOA Elevation FOM
|
||||
uint8_t aoa_dest_azimuth_FOM; // AOA Azimuth FOM
|
||||
uint8_t aoa_dest_elevation_FOM; // AOA Elevation FOM
|
||||
} __attribute__((packed)) UWB_range_meas_t;
|
||||
|
||||
typedef struct {
|
||||
uint32_t initator_time; //timestamp of init
|
||||
uint32_t sessionId; // Session ID of UWB session
|
||||
uint8_t num_anchors; //number of anchors
|
||||
gps_pos_t target_gps; //GPS position of Landing Point
|
||||
uint8_t mac_mode; // MAC adress mode, either 2 Byte or 8 Byte
|
||||
uint8_t MAC[UWB_MAC_MODE][MAX_ANCHORS];
|
||||
position_t target_pos; //target position
|
||||
position_t anchor_pos[MAX_ANCHORS]; // Position of each anchor
|
||||
uint8_t stop; // Should be 27
|
||||
} grid_msg_t;
|
||||
|
||||
typedef struct {
|
||||
uint8_t cmd; // Should be 0x8E for distance result message
|
||||
uint16_t len; // Should be 0x30 for distance result message
|
||||
uint32_t time_uwb_ms; // Timestamp of UWB device in ms
|
||||
uint32_t seq_ctr; // Number of Ranges since last Start of Ranging
|
||||
uint32_t sessionId; // Session ID of UWB session
|
||||
uint32_t range_interval; // Time between ranging rounds
|
||||
uint8_t mac_mode; // MAC adress mode, either 2 Byte or 8 Byte
|
||||
uint8_t no_measurements; // MAC adress mode, either 2 Byte or 8 Byte
|
||||
UWB_range_meas_t measurements[4]; //Raw anchor_distance distances in CM 2*9
|
||||
uint8_t stop; // Should be 0x1B
|
||||
uint8_t cmd; // Should be 0x8E for distance result message
|
||||
uint16_t len; // Should be 0x30 for distance result message
|
||||
uint32_t seq_ctr; // Number of Ranges since last Start of Ranging
|
||||
uint32_t sessionId; // Session ID of UWB session
|
||||
uint32_t range_interval; // Time between ranging rounds
|
||||
uint16_t MAC; // MAC address of UWB device
|
||||
UWB_range_meas_t measurements; //Raw anchor_distance distances in CM 2*9
|
||||
uint8_t stop; // Should be 0x1B
|
||||
} __attribute__((packed)) distance_msg_t;
|
||||
|
||||
class UWB_SR150 : public ModuleBase<UWB_SR150>, public ModuleParams
|
||||
class UWB_SR150 : public ModuleBase<UWB_SR150>, public ModuleParams, public px4::ScheduledWorkItem
|
||||
{
|
||||
public:
|
||||
UWB_SR150(const char *device_name, speed_t baudrate, bool uwb_pos_debug);
|
||||
|
||||
UWB_SR150(const char *port);
|
||||
~UWB_SR150();
|
||||
|
||||
/**
|
||||
* @see ModuleBase::task_spawn
|
||||
*/
|
||||
static int task_spawn(int argc, char *argv[]);
|
||||
|
||||
/**
|
||||
* @see ModuleBase::custom_command
|
||||
*/
|
||||
|
@ -143,67 +105,51 @@ public:
|
|||
*/
|
||||
static int print_usage(const char *reason = nullptr);
|
||||
|
||||
/**
|
||||
* @see ModuleBase::Multilateration
|
||||
*/
|
||||
UWB_POS_ERROR_CODES localization();
|
||||
bool init();
|
||||
|
||||
/**
|
||||
* @see ModuleBase::Distance Result
|
||||
*/
|
||||
int distance();
|
||||
void start();
|
||||
|
||||
/**
|
||||
* @see ModuleBase::task_spawn
|
||||
*/
|
||||
static int task_spawn(int argc, char *argv[]);
|
||||
void stop();
|
||||
|
||||
static UWB_SR150 *instantiate(int argc, char *argv[]);
|
||||
|
||||
void run() override;
|
||||
int collectData();
|
||||
|
||||
private:
|
||||
static constexpr int64_t sq(int64_t x) { return x * x; }
|
||||
|
||||
void parameters_update();
|
||||
|
||||
void grid_info_read(position_t *grid);
|
||||
void Run() override;
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamFloat<px4::params::UWB_INIT_OFF_X>) _uwb_init_off_x,
|
||||
(ParamFloat<px4::params::UWB_INIT_OFF_Y>) _uwb_init_off_y,
|
||||
(ParamFloat<px4::params::UWB_INIT_OFF_Z>) _uwb_init_off_z,
|
||||
(ParamFloat<px4::params::UWB_INIT_OFF_YAW>) _uwb_init_off_yaw
|
||||
)
|
||||
// Publications
|
||||
uORB::Publication<sensor_uwb_s> _sensor_uwb_pub{ORB_ID(sensor_uwb)};
|
||||
|
||||
// Subscriptions
|
||||
uORB::SubscriptionCallbackWorkItem _sensor_uwb_sub{this, ORB_ID(sensor_uwb)};
|
||||
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
|
||||
|
||||
int _uart;
|
||||
fd_set _uart_set;
|
||||
struct timeval _uart_timeout {};
|
||||
bool _uwb_pos_debug;
|
||||
|
||||
uORB::Publication<uwb_grid_s> _uwb_grid_pub{ORB_ID(uwb_grid)};
|
||||
uwb_grid_s _uwb_grid{};
|
||||
|
||||
uORB::Publication<uwb_distance_s> _uwb_distance_pub{ORB_ID(uwb_distance)};
|
||||
uwb_distance_s _uwb_distance{};
|
||||
|
||||
uORB::Publication<landing_target_pose_s> _landing_target_pub{ORB_ID(landing_target_pose)};
|
||||
landing_target_pose_s _landing_target{};
|
||||
|
||||
grid_msg_t _uwb_grid_info{};
|
||||
distance_msg_t _distance_result_msg{};
|
||||
matrix::Vector3f _rel_pos;
|
||||
|
||||
matrix::Dcmf _uwb_init_to_nwu;
|
||||
matrix::Dcmf _nwu_to_ned{matrix::Eulerf(M_PI_F, 0.0f, 0.0f)};
|
||||
matrix::Vector3f _current_position_uwb_init;
|
||||
matrix::Vector3f _current_position_ned;
|
||||
matrix::Vector3f _uwb_init_offset_v3f;
|
||||
|
||||
// Parameters
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamInt<px4::params::UWB_PORT_CFG>) _uwb_port_cfg,
|
||||
(ParamFloat<px4::params::UWB_INIT_OFF_X>) _offset_x,
|
||||
(ParamFloat<px4::params::UWB_INIT_OFF_Y>) _offset_y,
|
||||
(ParamFloat<px4::params::UWB_INIT_OFF_Z>) _offset_z,
|
||||
(ParamInt<px4::params::UWB_SENS_ROT>) _sensor_rot
|
||||
)
|
||||
// Performance (perf) counters
|
||||
perf_counter_t _read_count_perf;
|
||||
perf_counter_t _read_err_perf;
|
||||
};
|
||||
|
||||
sensor_uwb_s _sensor_uwb{};
|
||||
|
||||
char _port[20] {};
|
||||
hrt_abstime param_timestamp{0};
|
||||
|
||||
int _uart{-1};
|
||||
fd_set _uart_set;
|
||||
struct timeval _uart_timeout {};
|
||||
|
||||
unsigned _consecutive_fail_count;
|
||||
int _interval{100000};
|
||||
|
||||
distance_msg_t _distance_result_msg{};
|
||||
};
|
||||
#endif //PX4_RDDRONE_H
|
||||
|
|
|
@ -75,7 +75,7 @@ void ActuatorTest::update(int num_outputs, float thrust_curve)
|
|||
float value = actuator_test.value;
|
||||
|
||||
// handle motors
|
||||
if (actuator_test.function >= (int)OutputFunction::Motor1 && actuator_test.function <= (int)OutputFunction::MotorMax) {
|
||||
if ((int)OutputFunction::Motor1 <= actuator_test.function && actuator_test.function <= (int)OutputFunction::MotorMax) {
|
||||
actuator_motors_s motors;
|
||||
motors.reversible_flags = 0;
|
||||
_actuator_motors_sub.copy(&motors);
|
||||
|
@ -84,7 +84,7 @@ void ActuatorTest::update(int num_outputs, float thrust_curve)
|
|||
}
|
||||
|
||||
// handle servos: add trim
|
||||
if (actuator_test.function >= (int)OutputFunction::Servo1 && actuator_test.function <= (int)OutputFunction::ServoMax) {
|
||||
if ((int)OutputFunction::Servo1 <= actuator_test.function && actuator_test.function <= (int)OutputFunction::ServoMax) {
|
||||
actuator_servos_trim_s trim{};
|
||||
_actuator_servos_trim_sub.copy(&trim);
|
||||
int idx = actuator_test.function - (int)OutputFunction::Servo1;
|
||||
|
|
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in New Issue