forked from Archive/PX4-Autopilot
Compare commits
1 Commits
main
...
maetugr/ba
| Author | SHA1 | Date |
|---|---|---|
Matthias Grob
|
7ae28f6dc9 |
|
|
@ -120,7 +120,7 @@ pipeline {
|
|||
"px4_fmu-v6xrt_rover",
|
||||
"px4_io-v2_default",
|
||||
"raspberrypi_pico_default",
|
||||
"siyi_n7_default",
|
||||
"siyi_n7_default"
|
||||
"sky-drones_smartap-airlink_default",
|
||||
"spracing_h7extreme_default",
|
||||
"thepeach_k1_default",
|
||||
|
|
|
|||
|
|
@ -1,14 +0,0 @@
|
|||
root = true
|
||||
|
||||
[*]
|
||||
insert_final_newline = false
|
||||
|
||||
[{*.{c,cpp,cc,h,hpp},CMakeLists.txt,Kconfig}]
|
||||
indent_style = tab
|
||||
tab_width = 8
|
||||
# Not in the official standard, but supported by many editors
|
||||
max_line_length = 120
|
||||
|
||||
[*.yaml]
|
||||
indent_style = space
|
||||
indent_size = 2
|
||||
|
|
@ -81,5 +81,5 @@
|
|||
url = https://github.com/PX4/PX4-gazebo-models.git
|
||||
branch = main
|
||||
[submodule "boards/modalai/voxl2/libfc-sensor-api"]
|
||||
path = boards/modalai/voxl2/libfc-sensor-api
|
||||
path = src/modules/muorb/apps/libfc-sensor-api
|
||||
url = https://gitlab.com/voxl-public/voxl-sdk/core-libs/libfc-sensor-api.git
|
||||
|
|
|
|||
|
|
@ -1,80 +0,0 @@
|
|||
#!/bin/sh
|
||||
# @name Gazebo lawnmower
|
||||
# @type Rover
|
||||
# @class Rover
|
||||
|
||||
. ${R}etc/init.d/rc.rover_differential_defaults
|
||||
|
||||
PX4_SIMULATOR=${PX4_SIMULATOR:=gz}
|
||||
PX4_GZ_WORLD=${PX4_GZ_WORLD:=default}
|
||||
PX4_SIM_MODEL=${PX4_SIM_MODEL:=lawnmower}
|
||||
|
||||
param set-default SIM_GZ_EN 1 # Gazebo bridge
|
||||
|
||||
# Simulated sensors
|
||||
param set-default SENS_EN_GPSSIM 1
|
||||
param set-default SENS_EN_BAROSIM 0
|
||||
param set-default SENS_EN_MAGSIM 1
|
||||
param set-default SENS_EN_ARSPDSIM 1
|
||||
# We can arm and drive in manual mode when it slides and GPS check fails:
|
||||
param set-default COM_ARM_WO_GPS 1
|
||||
|
||||
# Set Differential Drive Kinematics Library parameters:
|
||||
param set RDD_WHEEL_BASE 0.9
|
||||
param set RDD_WHEEL_RADIUS 0.22
|
||||
param set RDD_WHEEL_SPEED 10.0 # Maximum wheel speed rad/s, approx 8 km/h
|
||||
|
||||
# Actuator mapping - set SITL motors/servos output parameters:
|
||||
|
||||
# "Motors" - motor channels 0 (Right) and 1 (Left) - via Wheels GZ bridge:
|
||||
param set-default SIM_GZ_WH_FUNC1 101 # right wheel
|
||||
#param set-default SIM_GZ_WH_MIN1 0
|
||||
#param set-default SIM_GZ_WH_MAX1 200
|
||||
#param set-default SIM_GZ_WH_DIS1 100
|
||||
#param set-default SIM_GZ_WH_FAIL1 100
|
||||
|
||||
param set-default SIM_GZ_WH_FUNC2 102 # left wheel
|
||||
#param set-default SIM_GZ_WH_MIN2 0
|
||||
#param set-default SIM_GZ_WH_MAX2 200
|
||||
#aram set-default SIM_GZ_WH_DIS2 100
|
||||
#param set-default SIM_GZ_WH_FAIL2 100
|
||||
|
||||
param set-default SIM_GZ_WH_REV 0 # no need to reverse any wheels
|
||||
|
||||
# Note: The servo configurations ( SIM_GZ_SV_FUNC*) outlined below are intended for educational purposes in this simulation.
|
||||
# They do not have physical effects in the simulated environment, except for actuating the joints. Their definitions are meant to demonstrate
|
||||
# how actuators could be mapped and configured in a real-world application, providing a foundation for understanding and implementing actuator
|
||||
# controls in practical scenarios.
|
||||
|
||||
# Cutter deck blades clutch, PCA9685 servo channel 3, "RC FLAPS" (406) - leftmost switch, or "Servo 3" (203):
|
||||
param set-default SIM_GZ_SV_FUNC3 203
|
||||
param set-default SIM_GZ_SV_MIN3 0
|
||||
param set-default SIM_GZ_SV_MAX3 1000
|
||||
param set-default SIM_GZ_SV_DIS3 500
|
||||
param set-default SIM_GZ_SV_FAIL3 500
|
||||
|
||||
# Gas engine throttle, PCA9685 servo channel 4, "RC AUX1" (407) - left knob, or "Servo 4" (204):
|
||||
# - on minimum when disarmed or failed:
|
||||
param set-default SIM_GZ_SV_FUNC4 204
|
||||
param set-default SIM_GZ_SV_MIN4 0
|
||||
param set-default SIM_GZ_SV_MAX4 1000
|
||||
param set-default SIM_GZ_SV_DIS4 500
|
||||
param set-default SIM_GZ_SV_FAIL4 500
|
||||
|
||||
# Controlling PCA9685 servos 5,6,7,8 directly via "Servo 5..8" setting, by publishing actuator_servos.control[]:
|
||||
|
||||
# Strobes, PCA9685 servo channel 5, "Servo 5" (205) - flashing indicates Mission mode:
|
||||
#param set-default SIM_GZ_SV_FUNC5 205
|
||||
#param set-default SIM_GZ_SV_MIN5 1000
|
||||
#param set-default SIM_GZ_SV_MAX5 2000
|
||||
#param set-default SIM_GZ_SV_DIS5 1000
|
||||
#param set-default SIM_GZ_SV_FAIL5 1000
|
||||
|
||||
# Horn, PCA9685 servo channel 6, "Servo 6" (206) - for alarms like GPS failure:
|
||||
#param set-default SIM_GZ_SV_FUNC6 206
|
||||
|
||||
# Spare PCA9685 servo channel 7 on "RC AUX2" (408) - right knob, or "Servo 7" (207):
|
||||
#param set-default SIM_GZ_SV_FUNC7 207
|
||||
|
||||
# Spare PCA9685 servo channel 8 - "Servo 8" (208):
|
||||
#param set-default SIM_GZ_SV_FUNC8 208
|
||||
|
|
@ -82,7 +82,6 @@ px4_add_romfs_files(
|
|||
4008_gz_advanced_plane
|
||||
4009_gz_r1_rover
|
||||
4010_gz_x500_mono_cam
|
||||
4011_gz_lawnmower
|
||||
|
||||
6011_gazebo-classic_typhoon_h480
|
||||
6011_gazebo-classic_typhoon_h480.post
|
||||
|
|
|
|||
|
|
@ -34,79 +34,28 @@
|
|||
add_subdirectory(airframes)
|
||||
|
||||
px4_add_romfs_files(
|
||||
|
||||
rc.airship_apps
|
||||
rc.airship_defaults
|
||||
rc.autostart_ext
|
||||
|
||||
rcS
|
||||
rc.sensors
|
||||
rc.vehicle_setup
|
||||
|
||||
# TODO
|
||||
rc.balloon_apps
|
||||
rc.balloon_defaults
|
||||
rc.boat_defaults
|
||||
rc.fw_apps
|
||||
rc.fw_defaults
|
||||
rc.heli_defaults
|
||||
rc.logging
|
||||
rc.mc_apps
|
||||
rc.mc_defaults
|
||||
rcS
|
||||
rc.sensors
|
||||
rc.thermal_cal
|
||||
rc.rover_apps
|
||||
rc.rover_defaults
|
||||
rc.rover_differential_apps
|
||||
rc.rover_differential_defaults
|
||||
rc.uuv_apps
|
||||
rc.uuv_defaults
|
||||
rc.vehicle_setup
|
||||
rc.vtol_apps
|
||||
rc.vtol_defaults
|
||||
)
|
||||
|
||||
if(CONFIG_MODULES_AIRSHIP_ATT_CONTROL)
|
||||
px4_add_romfs_files(
|
||||
rc.airship_apps
|
||||
rc.airship_defaults
|
||||
)
|
||||
endif()
|
||||
|
||||
if(CONFIG_MODULES_FW_RATE_CONTROL)
|
||||
px4_add_romfs_files(
|
||||
rc.fw_apps
|
||||
rc.fw_defaults
|
||||
)
|
||||
endif()
|
||||
|
||||
if(CONFIG_MODULES_MC_RATE_CONTROL)
|
||||
px4_add_romfs_files(
|
||||
rc.heli_defaults
|
||||
rc.mc_apps
|
||||
rc.mc_defaults
|
||||
)
|
||||
endif()
|
||||
|
||||
if(CONFIG_MODULES_ROVER_POS_CONTROL)
|
||||
px4_add_romfs_files(
|
||||
rc.rover_apps
|
||||
rc.rover_defaults
|
||||
|
||||
rc.boat_defaults # hack
|
||||
)
|
||||
endif()
|
||||
|
||||
if(CONFIG_MODULES_DIFFERENTIAL_DRIVE)
|
||||
px4_add_romfs_files(
|
||||
rc.rover_differential_apps
|
||||
rc.rover_differential_defaults
|
||||
)
|
||||
endif()
|
||||
|
||||
if(CONFIG_MODULES_UUV_ATT_CONTROL)
|
||||
px4_add_romfs_files(
|
||||
rc.uuv_apps
|
||||
rc.uuv_defaults
|
||||
)
|
||||
endif()
|
||||
|
||||
if(CONFIG_MODULES_VTOL_ATT_CONTROL)
|
||||
px4_add_romfs_files(
|
||||
rc.vtol_apps
|
||||
rc.vtol_defaults
|
||||
)
|
||||
endif()
|
||||
|
||||
|
||||
if(CONFIG_MODULES_LOGGER)
|
||||
px4_add_romfs_files(
|
||||
rc.logging
|
||||
)
|
||||
endif()
|
||||
|
||||
if(CONFIG_MODULES_TEMPERATURE_COMPENSATION)
|
||||
px4_add_romfs_files(
|
||||
rc.thermal_cal
|
||||
)
|
||||
endif()
|
||||
|
|
|
|||
|
|
@ -32,127 +32,94 @@
|
|||
############################################################################
|
||||
|
||||
px4_add_romfs_files(
|
||||
# [0-999] Reserved (historical)
|
||||
# [0-999] Reserved (historical)"
|
||||
|
||||
# [1000, 1999] Simulation setups"
|
||||
1001_rc_quad_x.hil
|
||||
1002_standard_vtol.hil
|
||||
1100_rc_quad_x_sih.hil
|
||||
1101_rc_plane_sih.hil
|
||||
1102_tailsitter_duo_sih.hil
|
||||
|
||||
# [2000, 2999] Standard planes"
|
||||
2100_standard_plane
|
||||
2106_albatross
|
||||
|
||||
2507_cloudship
|
||||
|
||||
# [3000, 3999] Flying wing"
|
||||
3000_generic_wing
|
||||
|
||||
# [4000, 4999] Quadrotor x"
|
||||
4001_quad_x
|
||||
4014_s500
|
||||
4015_holybro_s500
|
||||
4016_holybro_px4vision
|
||||
4017_nxp_hovergames
|
||||
4019_x500_v2
|
||||
4020_holybro_px4vision_v1_5
|
||||
4041_beta75x
|
||||
4050_generic_250
|
||||
4052_holybro_qav250
|
||||
4053_holybro_kopis2
|
||||
4061_atl_mantis_edu
|
||||
4071_ifo
|
||||
4073_ifo-s
|
||||
4500_clover4
|
||||
4901_crazyflie21
|
||||
|
||||
# [5000, 5999] Quadrotor +"
|
||||
5001_quad_+
|
||||
|
||||
# [6000, 6999] Hexarotor x"
|
||||
6001_hexa_x
|
||||
6002_draco_r
|
||||
|
||||
# [7000, 7999] Hexarotor +"
|
||||
7001_hexa_+
|
||||
|
||||
# [8000, 8999] Octorotor +"
|
||||
8001_octo_x
|
||||
|
||||
# [9000, 9999] Octorotor +"
|
||||
9001_octo_+
|
||||
|
||||
# [11000, 11999] Hexa Cox
|
||||
11001_hexa_cox
|
||||
|
||||
# [12000, 12999] Octo Cox
|
||||
12001_octo_cox
|
||||
|
||||
# [13000, 13999] VTOL
|
||||
13000_generic_vtol_standard
|
||||
13100_generic_vtol_tiltrotor
|
||||
13013_deltaquad
|
||||
13014_vtol_babyshark
|
||||
13030_generic_vtol_quad_tiltrotor
|
||||
13200_generic_vtol_tailsitter
|
||||
|
||||
# [14000, 14999] MC with tilt
|
||||
14001_generic_mc_with_tilt
|
||||
|
||||
16001_helicopter
|
||||
|
||||
# [17000, 17999] Autogyro
|
||||
17002_TF-AutoG2
|
||||
17003_TF-G2
|
||||
|
||||
# [18000, 18999] High-altitude balloons
|
||||
18001_TF-B1
|
||||
|
||||
# [22000, 22999] Reserve for custom models
|
||||
|
||||
24001_dodeca_cox
|
||||
|
||||
50000_generic_ground_vehicle
|
||||
50004_nxpcup_car_dfrobot_gpx
|
||||
50003_aion_robotics_r1_rover
|
||||
|
||||
# [60000, 61000] (Unmanned) Underwater Robots
|
||||
60000_uuv_generic
|
||||
60001_uuv_hippocampus
|
||||
60002_uuv_bluerov2_heavy
|
||||
)
|
||||
|
||||
if(CONFIG_MODULES_SIMULATION_PWM_OUT_SIM)
|
||||
px4_add_romfs_files(
|
||||
# [1000, 1999] Simulation setups
|
||||
1001_rc_quad_x.hil
|
||||
1002_standard_vtol.hil
|
||||
1100_rc_quad_x_sih.hil
|
||||
1101_rc_plane_sih.hil
|
||||
1102_tailsitter_duo_sih.hil
|
||||
)
|
||||
endif()
|
||||
|
||||
if(CONFIG_MODULES_MC_RATE_CONTROL)
|
||||
px4_add_romfs_files(
|
||||
# [4000, 4999] Quadrotor x
|
||||
4001_quad_x
|
||||
4014_s500
|
||||
4015_holybro_s500
|
||||
4016_holybro_px4vision
|
||||
4017_nxp_hovergames
|
||||
4019_x500_v2
|
||||
4020_holybro_px4vision_v1_5
|
||||
4041_beta75x
|
||||
4050_generic_250
|
||||
4052_holybro_qav250
|
||||
4053_holybro_kopis2
|
||||
4061_atl_mantis_edu
|
||||
4071_ifo
|
||||
4073_ifo-s
|
||||
4500_clover4
|
||||
4901_crazyflie21
|
||||
|
||||
# [5000, 5999] Quadrotor +
|
||||
5001_quad_+
|
||||
|
||||
# [6000, 6999] Hexarotor x
|
||||
6001_hexa_x
|
||||
6002_draco_r
|
||||
|
||||
# [7000, 7999] Hexarotor +
|
||||
7001_hexa_+
|
||||
|
||||
# [8000, 8999] Octorotor +
|
||||
8001_octo_x
|
||||
|
||||
# [9000, 9999] Octorotor +
|
||||
9001_octo_+
|
||||
|
||||
# [11000, 11999] Hexa Cox
|
||||
11001_hexa_cox
|
||||
|
||||
# [12000, 12999] Octo Cox
|
||||
12001_octo_cox
|
||||
|
||||
# [14000, 14999] MC with tilt
|
||||
14001_generic_mc_with_tilt
|
||||
|
||||
16001_helicopter
|
||||
|
||||
24001_dodeca_cox
|
||||
)
|
||||
endif()
|
||||
|
||||
if(CONFIG_MODULES_FW_RATE_CONTROL)
|
||||
px4_add_romfs_files(
|
||||
# [2000, 2999] Standard planes
|
||||
2100_standard_plane
|
||||
2106_albatross
|
||||
|
||||
# [3000, 3999] Flying wing
|
||||
3000_generic_wing
|
||||
|
||||
# [17000, 17999] Autogyro
|
||||
17002_TF-AutoG2
|
||||
17003_TF-G2
|
||||
)
|
||||
endif()
|
||||
|
||||
if(CONFIG_MODULES_AIRSHIP_ATT_CONTROL)
|
||||
px4_add_romfs_files(
|
||||
2507_cloudship
|
||||
)
|
||||
endif()
|
||||
|
||||
if(CONFIG_MODULES_VTOL_ATT_CONTROL)
|
||||
px4_add_romfs_files(
|
||||
# [13000, 13999] VTOL
|
||||
13000_generic_vtol_standard
|
||||
13100_generic_vtol_tiltrotor
|
||||
13013_deltaquad
|
||||
13014_vtol_babyshark
|
||||
13030_generic_vtol_quad_tiltrotor
|
||||
13200_generic_vtol_tailsitter
|
||||
)
|
||||
endif()
|
||||
|
||||
if(CONFIG_MODULES_ROVER_POS_CONTROL)
|
||||
px4_add_romfs_files(
|
||||
50000_generic_ground_vehicle
|
||||
50004_nxpcup_car_dfrobot_gpx
|
||||
)
|
||||
endif()
|
||||
|
||||
if(CONFIG_MODULES_DIFFERENTIAL_DRIVE)
|
||||
px4_add_romfs_files(
|
||||
50003_aion_robotics_r1_rover
|
||||
)
|
||||
endif()
|
||||
|
||||
if(CONFIG_MODULES_UUV_ATT_CONTROL)
|
||||
px4_add_romfs_files(
|
||||
# [60000, 61000] (Unmanned) Underwater Robots
|
||||
60000_uuv_generic
|
||||
60001_uuv_hippocampus
|
||||
60002_uuv_bluerov2_heavy
|
||||
)
|
||||
endif()
|
||||
|
|
|
|||
|
|
@ -28,6 +28,7 @@ param set-default EKF2_ARSP_THR 8
|
|||
param set-default EKF2_FUSE_BETA 1
|
||||
param set-default EKF2_GPS_CHECK 21
|
||||
param set-default EKF2_MAG_ACCLIM 0
|
||||
param set-default EKF2_MAG_YAWLIM 0
|
||||
param set-default EKF2_REQ_EPH 10
|
||||
param set-default EKF2_REQ_EPV 10
|
||||
param set-default EKF2_REQ_HDRIFT 0.5
|
||||
|
|
|
|||
|
|
@ -439,12 +439,7 @@ else
|
|||
#
|
||||
# Start a thermal calibration if required.
|
||||
#
|
||||
set RC_THERMAL_CAL ${R}etc/init.d/rc.thermal_cal
|
||||
if [ -f ${RC_THERMAL_CAL} ]
|
||||
then
|
||||
. ${RC_THERMAL_CAL}
|
||||
fi
|
||||
unset RC_THERMAL_CAL
|
||||
. ${R}etc/init.d/rc.thermal_cal
|
||||
|
||||
#
|
||||
# Start gimbal to control mounts such as gimbals, disabled by default.
|
||||
|
|
@ -505,12 +500,7 @@ else
|
|||
#
|
||||
# Start the logger.
|
||||
#
|
||||
set RC_LOGGING ${R}etc/init.d/rc.logging
|
||||
if [ -f ${RC_LOGGING} ]
|
||||
then
|
||||
. ${RC_LOGGING}
|
||||
fi
|
||||
unset RC_LOGGING
|
||||
. ${R}etc/init.d/rc.logging
|
||||
|
||||
#
|
||||
# Set additional parameters and env variables for selected AUTOSTART.
|
||||
|
|
|
|||
|
|
@ -30,5 +30,4 @@ exec find boards msg src platforms test \
|
|||
-path src/lib/cdrstream/cyclonedds -prune -o \
|
||||
-path src/lib/cdrstream/rosidl -prune -o \
|
||||
-path src/modules/zenoh/zenoh-pico -prune -o \
|
||||
-path boards/modalai/voxl2/libfc-sensor-api -prune -o \
|
||||
-type f \( -name "*.c" -o -name "*.h" -o -name "*.cpp" -o -name "*.hpp" \) | grep $PATTERN
|
||||
|
|
|
|||
|
|
@ -73,16 +73,9 @@ struct_size, padding_end_size = add_padding_bytes(sorted_fields, search_path)
|
|||
#include <lib/matrix/matrix/math.hpp>
|
||||
#include <lib/mathlib/mathlib.h>
|
||||
|
||||
@{
|
||||
queue_length = 1
|
||||
for constant in spec.constants:
|
||||
if constant.name == 'ORB_QUEUE_LENGTH':
|
||||
queue_length = constant.val
|
||||
}@
|
||||
|
||||
@[for topic in topics]@
|
||||
static_assert(static_cast<orb_id_size_t>(ORB_ID::@topic) == @(all_topics.index(topic)), "ORB_ID index mismatch");
|
||||
ORB_DEFINE(@topic, struct @uorb_struct, @(struct_size-padding_end_size), @(message_hash)u, static_cast<orb_id_size_t>(ORB_ID::@topic), @queue_length);
|
||||
ORB_DEFINE(@topic, struct @uorb_struct, @(struct_size-padding_end_size), @(message_hash)u, static_cast<orb_id_size_t>(ORB_ID::@topic));
|
||||
@[end for]
|
||||
|
||||
void print_message(const orb_metadata *meta, const @uorb_struct& message)
|
||||
|
|
|
|||
|
|
@ -1 +1 @@
|
|||
Subproject commit da7206e057703cc645770f02437013358b71e1c0
|
||||
Subproject commit 33ac87a37676fb597de110e926bbf0a080169ffe
|
||||
|
|
@ -1 +1 @@
|
|||
Subproject commit 6b4ed09d1b495fbff663f098979cc046df013abd
|
||||
Subproject commit f1c461fffb8567d6f0af770fb533f60f6ec62c22
|
||||
|
|
@ -4,7 +4,7 @@
|
|||
"description": "Firmware for the ARK flow board",
|
||||
"image": "",
|
||||
"build_time": 0,
|
||||
"summary": "ARKFLOW",
|
||||
"summary": "ARFFLOW",
|
||||
"version": "0.1",
|
||||
"image_size": 0,
|
||||
"image_maxsize": 2080768,
|
||||
|
|
|
|||
Binary file not shown.
|
|
@ -20,7 +20,6 @@ CONFIG_UAVCANNODE_RTK_DATA=y
|
|||
CONFIG_UAVCANNODE_SAFETY_BUTTON=y
|
||||
CONFIG_UAVCANNODE_STATIC_PRESSURE=y
|
||||
CONFIG_UAVCANNODE_STATIC_TEMPERATURE=y
|
||||
CONFIG_MODULES_EKF2=y
|
||||
CONFIG_MODULES_GYRO_CALIBRATION=y
|
||||
CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
|
||||
CONFIG_MODULES_SENSORS=y
|
||||
|
|
|
|||
|
|
@ -11,5 +11,3 @@ param set-default SENS_IMU_CLPNOTI 0
|
|||
|
||||
safety_button start
|
||||
tone_alarm start
|
||||
|
||||
ekf2 start
|
||||
|
|
|
|||
|
|
@ -50,7 +50,6 @@ CONFIG_STACK_COLORATION=y
|
|||
CONFIG_START_DAY=30
|
||||
CONFIG_START_MONTH=11
|
||||
CONFIG_STDIO_DISABLE_BUFFERING=y
|
||||
CONFIG_STM32_FLASH_CONFIG_G=y
|
||||
CONFIG_STM32_NOEXT_VECTORS=y
|
||||
CONFIG_STM32_TIM8=y
|
||||
CONFIG_TASK_NAME_SIZE=0
|
||||
|
|
|
|||
|
|
@ -123,7 +123,6 @@ CONFIG_STM32_ADC1=y
|
|||
CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG=y
|
||||
CONFIG_STM32_DMA1=y
|
||||
CONFIG_STM32_DMA2=y
|
||||
CONFIG_STM32_FLASH_CONFIG_G=y
|
||||
CONFIG_STM32_FLASH_PREFETCH=y
|
||||
CONFIG_STM32_FLOWCONTROL_BROKEN=y
|
||||
CONFIG_STM32_I2C1=y
|
||||
|
|
|
|||
|
|
@ -33,7 +33,7 @@
|
|||
*
|
||||
****************************************************************************/
|
||||
|
||||
/* The STM32F412 has 1M of FLASH beginning at address 0x0800:0000 and
|
||||
/* The STM32F412 has 512Kb of FLASH beginning at address 0x0800:0000 and
|
||||
* 256Kb of SRAM. SRAM is split up into three blocks:
|
||||
*
|
||||
* 1) 112Kb of SRAM beginning at address 0x2000:0000
|
||||
|
|
|
|||
|
|
@ -33,7 +33,7 @@
|
|||
*
|
||||
****************************************************************************/
|
||||
|
||||
/* The STM32F412 has 1M of FLASH beginning at address 0x0800:0000 and
|
||||
/* The STM32F412 has 512Kb of FLASH beginning at address 0x0800:0000 and
|
||||
* 256Kb of SRAM. SRAM is split up into three blocks:
|
||||
*
|
||||
* 1) 112Kb of SRAM beginning at address 0x2000:0000
|
||||
|
|
|
|||
|
|
@ -64,6 +64,9 @@
|
|||
// Hacks for MAVLink RC button input
|
||||
#define ATL_MANTIS_RC_INPUT_HACKS
|
||||
|
||||
// Hacks for MAVLink RC button input
|
||||
#define ATL_MANTIS_RC_INPUT_HACKS
|
||||
|
||||
/*
|
||||
* ADC channels
|
||||
*
|
||||
|
|
|
|||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
|
@ -60,7 +60,7 @@ CONFIG_MODULES_NAVIGATOR=y
|
|||
CONFIG_MODULES_RC_UPDATE=y
|
||||
CONFIG_MODULES_ROVER_POS_CONTROL=y
|
||||
CONFIG_MODULES_SENSORS=y
|
||||
# CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
|
||||
CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
|
||||
CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
|
||||
CONFIG_MODULES_UXRCE_DDS_CLIENT=y
|
||||
CONFIG_MODULES_VTOL_ATT_CONTROL=y
|
||||
|
|
|
|||
|
|
@ -4,14 +4,13 @@ CONFIG_DRIVERS_ACTUATORS_VOXL_ESC=y
|
|||
CONFIG_DRIVERS_BAROMETER_INVENSENSE_ICP101XX=y
|
||||
CONFIG_DRIVERS_DISTANCE_SENSOR_VL53L0X=y
|
||||
CONFIG_DRIVERS_DISTANCE_SENSOR_VL53L1X=y
|
||||
CONFIG_DRIVERS_GPS=y
|
||||
CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
|
||||
CONFIG_DRIVERS_LIGHTS_RGBLED_NCP5623C=y
|
||||
CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8308=y
|
||||
CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y
|
||||
CONFIG_DRIVERS_MAGNETOMETER_QMC5883L=y
|
||||
CONFIG_DRIVERS_POWER_MONITOR_VOXLPM=y
|
||||
CONFIG_DRIVERS_QSHELL_QURT=y
|
||||
CONFIG_DRIVERS_VOXL2_IO=y
|
||||
CONFIG_MODULES_COMMANDER=y
|
||||
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
||||
CONFIG_MODULES_EKF2=y
|
||||
|
|
@ -29,6 +28,4 @@ CONFIG_MODULES_RC_UPDATE=y
|
|||
CONFIG_MODULES_SENSORS=y
|
||||
CONFIG_MODULES_SIMULATION_PWM_OUT_SIM=y
|
||||
CONFIG_SYSTEMCMDS_UORB=y
|
||||
CONFIG_SYSTEMCMDS_PARAM=y
|
||||
CONFIG_ORB_COMMUNICATOR=y
|
||||
CONFIG_PARAM_REMOTE=y
|
||||
|
|
|
|||
|
|
@ -44,10 +44,11 @@ add_library(drivers_board
|
|||
)
|
||||
|
||||
# Add custom drivers for SLPI
|
||||
# add_subdirectory(${PX4_BOARD_DIR}/src/drivers/icm42688p)
|
||||
add_subdirectory(${PX4_BOARD_DIR}/src/drivers/rc_controller)
|
||||
add_subdirectory(${PX4_BOARD_DIR}/src/drivers/mavlink_rc_in)
|
||||
# add_subdirectory(${PX4_BOARD_DIR}/src/drivers/spektrum_rc)
|
||||
# add_subdirectory(${PX4_BOARD_DIR}/src/drivers/ghst_rc)
|
||||
add_subdirectory(${PX4_BOARD_DIR}/src/drivers/dsp_hitl)
|
||||
# add_subdirectory(${PX4_BOARD_DIR}/src/drivers/dsp_hitl)
|
||||
# add_subdirectory(${PX4_BOARD_DIR}/src/drivers/dsp_sbus)
|
||||
add_subdirectory(${PX4_BOARD_DIR}/src/drivers/elrs_led)
|
||||
|
|
|
|||
|
|
@ -62,8 +62,6 @@
|
|||
#include <uORB/topics/vehicle_odometry.h>
|
||||
#include <uORB/topics/sensor_baro.h>
|
||||
#include <uORB/topics/esc_status.h>
|
||||
#include <uORB/topics/distance_sensor.h>
|
||||
#include <uORB/topics/sensor_optical_flow.h>
|
||||
|
||||
#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
|
||||
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
|
||||
|
|
@ -90,22 +88,21 @@ static bool _is_running = false;
|
|||
volatile bool _task_should_exit = false;
|
||||
static px4_task_t _task_handle = -1;
|
||||
int _uart_fd = -1;
|
||||
bool _debug = false;
|
||||
bool debug = false;
|
||||
std::string port = "2";
|
||||
int baudrate = 921600;
|
||||
const unsigned mode_flag_custom = 1;
|
||||
const unsigned mode_flag_armed = 128;
|
||||
bool _send_gps = false;
|
||||
bool _send_mag = false;
|
||||
bool _send_distance = false;
|
||||
|
||||
uORB::Publication<battery_status_s> _battery_pub{ORB_ID(battery_status)};
|
||||
uORB::PublicationMulti<sensor_gps_s> _sensor_gps_pub{ORB_ID(sensor_gps)};
|
||||
uORB::Publication<differential_pressure_s> _differential_pressure_pub{ORB_ID(differential_pressure)};
|
||||
uORB::Publication<vehicle_odometry_s> _visual_odometry_pub{ORB_ID(vehicle_visual_odometry)};
|
||||
uORB::Publication<vehicle_odometry_s> _mocap_odometry_pub{ORB_ID(vehicle_mocap_odometry)};
|
||||
uORB::PublicationMulti<sensor_baro_s> _sensor_baro_pub{ORB_ID(sensor_baro)};
|
||||
uORB::Publication<esc_status_s> _esc_status_pub{ORB_ID(esc_status)};
|
||||
uORB::Publication<distance_sensor_s> _distance_sensor_pub{ORB_ID(distance_sensor)};
|
||||
uORB::Publication<sensor_optical_flow_s> _sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)};
|
||||
uORB::Subscription _battery_status_sub{ORB_ID(battery_status)};
|
||||
|
||||
int32_t _output_functions[actuator_outputs_s::NUM_ACTUATOR_OUTPUTS] {};
|
||||
|
|
@ -131,42 +128,13 @@ float x_gyro = 0;
|
|||
float y_gyro = 0;
|
||||
float z_gyro = 0;
|
||||
uint64_t gyro_accel_time = 0;
|
||||
bool _use_software_mav_throttling{false};
|
||||
|
||||
// Status counters
|
||||
uint32_t heartbeat_received_counter = 0;
|
||||
uint32_t heartbeat_sent_counter = 0;
|
||||
uint32_t imu_counter = 0;
|
||||
uint32_t hil_sensor_counter = 0;
|
||||
uint32_t mag_counter = 0;
|
||||
uint32_t baro_counter = 0;
|
||||
uint32_t actuator_sent_counter = 0;
|
||||
uint32_t odometry_received_counter = 0;
|
||||
uint32_t odometry_sent_counter = 0;
|
||||
uint32_t gps_received_counter = 0;
|
||||
uint32_t gps_sent_counter = 0;
|
||||
uint32_t distance_received_counter = 0;
|
||||
uint32_t distance_sent_counter = 0;
|
||||
uint32_t flow_received_counter = 0;
|
||||
uint32_t flow_sent_counter = 0;
|
||||
uint32_t unknown_msg_received_counter = 0;
|
||||
|
||||
enum class position_source {GPS, VIO, FLOW, NUM_POSITION_SOURCES};
|
||||
|
||||
struct position_source_data_s {
|
||||
char label[8];
|
||||
bool send;
|
||||
bool fail;
|
||||
uint32_t failure_duration;
|
||||
uint64_t failure_duration_start;
|
||||
} position_source_data[(int) position_source::NUM_POSITION_SOURCES] = {
|
||||
{"GPS", false, false, 0, 0},
|
||||
{"VIO", false, false, 0, 0},
|
||||
{"FLOW", false, false, 0, 0}
|
||||
};
|
||||
|
||||
uint64_t first_sensor_msg_timestamp = 0;
|
||||
uint64_t first_sensor_report_timestamp = 0;
|
||||
uint64_t last_sensor_report_timestamp = 0;
|
||||
int heartbeat_counter = 0;
|
||||
int imu_counter = 0;
|
||||
int hil_sensor_counter = 0;
|
||||
int vision_msg_counter = 0;
|
||||
int gps_counter = 0;
|
||||
|
||||
vehicle_status_s _vehicle_status{};
|
||||
vehicle_control_mode_s _control_mode{};
|
||||
|
|
@ -176,6 +144,7 @@ battery_status_s _battery_status{};
|
|||
sensor_accel_fifo_s accel_fifo{};
|
||||
sensor_gyro_fifo_s gyro_fifo{};
|
||||
|
||||
|
||||
int openPort(const char *dev, speed_t speed);
|
||||
int closePort();
|
||||
|
||||
|
|
@ -184,8 +153,7 @@ int writeResponse(void *buf, size_t len);
|
|||
|
||||
int start(int argc, char *argv[]);
|
||||
int stop();
|
||||
void print_status();
|
||||
void clear_status_counters();
|
||||
int get_status();
|
||||
bool isOpen() { return _uart_fd >= 0; };
|
||||
|
||||
void usage();
|
||||
|
|
@ -195,65 +163,50 @@ void *send_actuator(void *);
|
|||
void send_actuator_data();
|
||||
|
||||
void handle_message_hil_sensor_dsp(mavlink_message_t *msg);
|
||||
void handle_message_hil_optical_flow(mavlink_message_t *msg);
|
||||
void handle_message_distance_sensor(mavlink_message_t *msg);
|
||||
void handle_message_hil_gps_dsp(mavlink_message_t *msg);
|
||||
void handle_message_odometry_dsp(mavlink_message_t *msg);
|
||||
void handle_message_vision_position_estimate_dsp(mavlink_message_t *msg);
|
||||
void handle_message_command_long_dsp(mavlink_message_t *msg);
|
||||
|
||||
void handle_message_dsp(mavlink_message_t *msg);
|
||||
void actuator_controls_from_outputs_dsp(mavlink_hil_actuator_controls_t *msg);
|
||||
void send_esc_status(mavlink_hil_actuator_controls_t hil_act_control);
|
||||
void send_esc_telemetry_dsp(mavlink_hil_actuator_controls_t hil_act_control);
|
||||
|
||||
void
|
||||
handle_message_dsp(mavlink_message_t *msg)
|
||||
{
|
||||
switch (msg->msgid) {
|
||||
case MAVLINK_MSG_ID_HIL_SENSOR:
|
||||
hil_sensor_counter++;
|
||||
handle_message_hil_sensor_dsp(msg);
|
||||
break;
|
||||
|
||||
case MAVLINK_MSG_ID_HIL_GPS:
|
||||
gps_received_counter++;
|
||||
if (_send_gps) { handle_message_hil_gps_dsp(msg); }
|
||||
|
||||
if (position_source_data[(int) position_source::GPS].send) { handle_message_hil_gps_dsp(msg); }
|
||||
break;
|
||||
|
||||
case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE:
|
||||
handle_message_vision_position_estimate_dsp(msg);
|
||||
break;
|
||||
|
||||
case MAVLINK_MSG_ID_ODOMETRY:
|
||||
odometry_received_counter++;
|
||||
|
||||
if (position_source_data[(int) position_source::VIO].send) { handle_message_odometry_dsp(msg); }
|
||||
handle_message_odometry_dsp(msg);
|
||||
break;
|
||||
|
||||
case MAVLINK_MSG_ID_COMMAND_LONG:
|
||||
handle_message_command_long_dsp(msg);
|
||||
break;
|
||||
|
||||
case MAVLINK_MSG_ID_HEARTBEAT:
|
||||
heartbeat_received_counter++;
|
||||
|
||||
if (_debug) { PX4_INFO("Heartbeat msg received"); }
|
||||
|
||||
PX4_DEBUG("Heartbeat msg received");
|
||||
break;
|
||||
|
||||
case MAVLINK_MSG_ID_HIL_OPTICAL_FLOW:
|
||||
flow_received_counter++;
|
||||
|
||||
if (position_source_data[(int) position_source::FLOW].send) { handle_message_hil_optical_flow(msg); }
|
||||
|
||||
break;
|
||||
|
||||
case MAVLINK_MSG_ID_DISTANCE_SENSOR:
|
||||
distance_received_counter++;
|
||||
|
||||
if (_send_distance) { handle_message_distance_sensor(msg); }
|
||||
|
||||
case MAVLINK_MSG_ID_SYSTEM_TIME:
|
||||
PX4_DEBUG("MAVLINK SYSTEM TIME");
|
||||
break;
|
||||
|
||||
default:
|
||||
unknown_msg_received_counter++;
|
||||
|
||||
if (_debug) { PX4_INFO("Unknown msg ID: %d", msg->msgid); }
|
||||
|
||||
PX4_DEBUG("Unknown msg ID: %d", msg->msgid);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
|
@ -275,6 +228,7 @@ void send_actuator_data()
|
|||
bool first_sent = false;
|
||||
|
||||
while (true) {
|
||||
|
||||
bool controls_updated = false;
|
||||
(void) orb_check(_vehicle_control_mode_sub_, &controls_updated);
|
||||
|
||||
|
|
@ -285,50 +239,45 @@ void send_actuator_data()
|
|||
bool actuator_updated = false;
|
||||
(void) orb_check(_actuator_outputs_sub, &actuator_updated);
|
||||
|
||||
uint8_t newBuf[512];
|
||||
uint16_t newBufLen = 0;
|
||||
|
||||
mavlink_hil_actuator_controls_t hil_act_control;
|
||||
actuator_controls_from_outputs_dsp(&hil_act_control);
|
||||
|
||||
mavlink_message_t message{};
|
||||
mavlink_msg_hil_actuator_controls_encode(1, 1, &message, &hil_act_control);
|
||||
|
||||
if (actuator_updated) {
|
||||
orb_copy(ORB_ID(actuator_outputs), _actuator_outputs_sub, &_actuator_outputs);
|
||||
px4_lockstep_wait_for_components();
|
||||
|
||||
if (_actuator_outputs.timestamp > 0) {
|
||||
mavlink_hil_actuator_controls_t hil_act_control;
|
||||
actuator_controls_from_outputs_dsp(&hil_act_control);
|
||||
|
||||
mavlink_message_t message{};
|
||||
mavlink_msg_hil_actuator_controls_encode(1, 1, &message, &hil_act_control);
|
||||
previous_timestamp = _actuator_outputs.timestamp;
|
||||
previous_uorb_timestamp = _actuator_outputs.timestamp;
|
||||
|
||||
uint8_t newBuf[512];
|
||||
uint16_t newBufLen = 0;
|
||||
newBufLen = mavlink_msg_to_send_buffer(newBuf, &message);
|
||||
int writeRetval = writeResponse(&newBuf, newBufLen);
|
||||
|
||||
actuator_sent_counter++;
|
||||
|
||||
if (_debug) { PX4_INFO("Succesful write of actuator back to jMAVSim: %d at %llu", writeRetval, hrt_absolute_time()); }
|
||||
|
||||
PX4_DEBUG("Succesful write of actuator back to jMAVSim: %d at %llu", writeRetval, hrt_absolute_time());
|
||||
first_sent = true;
|
||||
|
||||
send_esc_status(hil_act_control);
|
||||
send_esc_telemetry_dsp(hil_act_control);
|
||||
}
|
||||
|
||||
} else if (! actuator_updated && first_sent && differential > 4000) {
|
||||
} else if (!actuator_updated && first_sent && differential > 4000) {
|
||||
mavlink_hil_actuator_controls_t hil_act_control;
|
||||
actuator_controls_from_outputs_dsp(&hil_act_control);
|
||||
previous_timestamp = hrt_absolute_time();
|
||||
|
||||
mavlink_message_t message{};
|
||||
mavlink_msg_hil_actuator_controls_encode(1, 1, &message, &hil_act_control);
|
||||
uint8_t newBuf[512];
|
||||
uint16_t newBufLen = 0;
|
||||
newBufLen = mavlink_msg_to_send_buffer(newBuf, &message);
|
||||
int writeRetval = writeResponse(&newBuf, newBufLen);
|
||||
//PX4_INFO("Sending from NOT UPDTE AND TIMEOUT: %i", differential);
|
||||
|
||||
actuator_sent_counter++;
|
||||
|
||||
if (_debug) { PX4_INFO("Succesful write of actuator back to jMAVSim: %d at %llu", writeRetval, hrt_absolute_time()); }
|
||||
|
||||
send_esc_status(hil_act_control);
|
||||
PX4_DEBUG("Succesful write of actuator back to jMAVSim: %d at %llu", writeRetval, hrt_absolute_time());
|
||||
send_esc_telemetry_dsp(hil_act_control);
|
||||
}
|
||||
|
||||
differential = hrt_absolute_time() - previous_timestamp;
|
||||
|
||||
px4_usleep(1000);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -338,10 +287,14 @@ void task_main(int argc, char *argv[])
|
|||
int myoptind = 1;
|
||||
const char *myoptarg = nullptr;
|
||||
|
||||
while ((ch = px4_getopt(argc, argv, "odmghfp:b:", &myoptind, &myoptarg)) != EOF) {
|
||||
while ((ch = px4_getopt(argc, argv, "vsdcmgp:b:", &myoptind, &myoptarg)) != EOF) {
|
||||
switch (ch) {
|
||||
case 's':
|
||||
_use_software_mav_throttling = true;
|
||||
break;
|
||||
|
||||
case 'd':
|
||||
_debug = true;
|
||||
debug = true;
|
||||
break;
|
||||
|
||||
case 'p':
|
||||
|
|
@ -357,19 +310,7 @@ void task_main(int argc, char *argv[])
|
|||
break;
|
||||
|
||||
case 'g':
|
||||
position_source_data[(int) position_source::GPS].send = true;
|
||||
break;
|
||||
|
||||
case 'o':
|
||||
position_source_data[(int) position_source::VIO].send = true;
|
||||
break;
|
||||
|
||||
case 'h':
|
||||
_send_distance = true;
|
||||
break;
|
||||
|
||||
case 'f':
|
||||
position_source_data[(int) position_source::FLOW].send = true;
|
||||
_send_gps = true;
|
||||
break;
|
||||
|
||||
default:
|
||||
|
|
@ -378,13 +319,11 @@ void task_main(int argc, char *argv[])
|
|||
}
|
||||
|
||||
const char *charport = port.c_str();
|
||||
(void) openPort(charport, (speed_t) baudrate);
|
||||
int openRetval = openPort(charport, (speed_t) baudrate);
|
||||
int open = isOpen();
|
||||
|
||||
if ((_debug) && (isOpen())) { PX4_INFO("DSP HITL serial port initialized. Baudrate: %d", baudrate); }
|
||||
|
||||
if (! isOpen()) {
|
||||
PX4_ERR("DSP HITL failed to open serial port");
|
||||
return;
|
||||
if (open) {
|
||||
PX4_ERR("Port is open: %d", openRetval);
|
||||
}
|
||||
|
||||
uint64_t last_heartbeat_timestamp = hrt_absolute_time();
|
||||
|
|
@ -403,11 +342,14 @@ void task_main(int argc, char *argv[])
|
|||
pthread_attr_destroy(&sender_thread_attr);
|
||||
|
||||
int _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
|
||||
PX4_INFO("Got %d from orb_subscribe", _vehicle_status_sub);
|
||||
|
||||
_is_running = true;
|
||||
|
||||
while (!_task_should_exit) {
|
||||
|
||||
uint8_t rx_buf[1024];
|
||||
//rx_buf[511] = '\0';
|
||||
|
||||
uint64_t timestamp = hrt_absolute_time();
|
||||
|
||||
|
|
@ -415,8 +357,8 @@ void task_main(int argc, char *argv[])
|
|||
if (got_first_sensor_msg) {
|
||||
uint64_t delta_time = timestamp - last_imu_update_timestamp;
|
||||
|
||||
if ((imu_counter) && (delta_time > 15000)) {
|
||||
PX4_WARN("Sending updates at %llu, delta %llu", timestamp, delta_time);
|
||||
if (delta_time > 15000) {
|
||||
PX4_ERR("Sending updates at %llu, delta %llu", timestamp, delta_time);
|
||||
}
|
||||
|
||||
uint64_t _px4_gyro_accel_timestamp = hrt_absolute_time();
|
||||
|
|
@ -454,7 +396,7 @@ void task_main(int argc, char *argv[])
|
|||
hb_newBufLen = mavlink_msg_to_send_buffer(hb_newBuf, &hb_message);
|
||||
(void) writeResponse(&hb_newBuf, hb_newBufLen);
|
||||
last_heartbeat_timestamp = timestamp;
|
||||
heartbeat_sent_counter++;
|
||||
heartbeat_counter++;
|
||||
}
|
||||
|
||||
bool vehicle_updated = false;
|
||||
|
|
@ -474,7 +416,7 @@ void task_main(int argc, char *argv[])
|
|||
_is_running = false;
|
||||
}
|
||||
|
||||
void send_esc_status(mavlink_hil_actuator_controls_t hil_act_control)
|
||||
void send_esc_telemetry_dsp(mavlink_hil_actuator_controls_t hil_act_control)
|
||||
{
|
||||
esc_status_s esc_status{};
|
||||
esc_status.timestamp = hrt_absolute_time();
|
||||
|
|
@ -506,13 +448,17 @@ void send_esc_status(mavlink_hil_actuator_controls_t hil_act_control)
|
|||
_esc_status_pub.publish(esc_status);
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
handle_message_command_long_dsp(mavlink_message_t *msg)
|
||||
{
|
||||
/* command */
|
||||
mavlink_command_long_t cmd_mavlink;
|
||||
mavlink_msg_command_long_decode(msg, &cmd_mavlink);
|
||||
|
||||
if (_debug) { PX4_INFO("Value of command_long.command: %d", cmd_mavlink.command); }
|
||||
if (debug) {
|
||||
PX4_INFO("Value of command_long.command: %d", cmd_mavlink.command);
|
||||
}
|
||||
|
||||
mavlink_command_ack_t ack = {};
|
||||
ack.result = MAV_RESULT_UNSUPPORTED;
|
||||
|
|
@ -524,140 +470,46 @@ handle_message_command_long_dsp(mavlink_message_t *msg)
|
|||
uint16_t acknewBufLen = 0;
|
||||
acknewBufLen = mavlink_msg_to_send_buffer(acknewBuf, &ack_message);
|
||||
int writeRetval = writeResponse(&acknewBuf, acknewBufLen);
|
||||
|
||||
if (_debug) { PX4_INFO("Succesful write of ACK back over UART: %d at %llu", writeRetval, hrt_absolute_time()); }
|
||||
PX4_INFO("Succesful write of ACK back over UART: %d at %llu", writeRetval, hrt_absolute_time());
|
||||
}
|
||||
|
||||
int flow_debug_counter = 0;
|
||||
|
||||
void
|
||||
handle_message_hil_optical_flow(mavlink_message_t *msg)
|
||||
handle_message_vision_position_estimate_dsp(mavlink_message_t *msg)
|
||||
{
|
||||
mavlink_hil_optical_flow_t flow;
|
||||
mavlink_msg_hil_optical_flow_decode(msg, &flow);
|
||||
mavlink_vision_position_estimate_t vpe;
|
||||
mavlink_msg_vision_position_estimate_decode(msg, &vpe);
|
||||
|
||||
if ((_debug) && (!(flow_debug_counter % 10))) {
|
||||
PX4_INFO("optflow: time: %llu, quality %d", flow.time_usec, (int) flow.quality);
|
||||
PX4_INFO("optflow: x: %.2f y: %.2f", (double) flow.integrated_x, (double) flow.integrated_y);
|
||||
}
|
||||
// fill vehicle_odometry from Mavlink VISION_POSITION_ESTIMATE
|
||||
vehicle_odometry_s odom{};
|
||||
uint64_t timestamp = hrt_absolute_time();
|
||||
odom.timestamp_sample = timestamp;
|
||||
|
||||
flow_debug_counter++;
|
||||
odom.pose_frame = vehicle_odometry_s::POSE_FRAME_NED;
|
||||
odom.position[0] = vpe.x;
|
||||
odom.position[1] = vpe.y;
|
||||
odom.position[2] = vpe.z;
|
||||
|
||||
device::Device::DeviceId device_id;
|
||||
device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_MAVLINK;
|
||||
device_id.devid_s.bus = 1;
|
||||
device_id.devid_s.address = msg->sysid;
|
||||
device_id.devid_s.devtype = DRV_FLOW_DEVTYPE_SIM;
|
||||
const matrix::Quatf q(matrix::Eulerf(vpe.roll, vpe.pitch, vpe.yaw));
|
||||
q.copyTo(odom.q);
|
||||
|
||||
sensor_optical_flow_s sensor_optical_flow{};
|
||||
// VISION_POSITION_ESTIMATE covariance
|
||||
// Row-major representation of pose 6x6 cross-covariance matrix upper right triangle
|
||||
// (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.).
|
||||
// If unknown, assign NaN value to first element in the array.
|
||||
odom.position_variance[0] = vpe.covariance[0]; // X row 0, col 0
|
||||
odom.position_variance[1] = vpe.covariance[6]; // Y row 1, col 1
|
||||
odom.position_variance[2] = vpe.covariance[11]; // Z row 2, col 2
|
||||
|
||||
sensor_optical_flow.timestamp_sample = hrt_absolute_time();
|
||||
sensor_optical_flow.device_id = device_id.devid;
|
||||
odom.orientation_variance[0] = vpe.covariance[15]; // R row 3, col 3
|
||||
odom.orientation_variance[1] = vpe.covariance[18]; // P row 4, col 4
|
||||
odom.orientation_variance[2] = vpe.covariance[20]; // Y row 5, col 5
|
||||
|
||||
sensor_optical_flow.pixel_flow[0] = flow.integrated_x;
|
||||
sensor_optical_flow.pixel_flow[1] = flow.integrated_y;
|
||||
odom.reset_counter = vpe.reset_counter;
|
||||
|
||||
sensor_optical_flow.integration_timespan_us = flow.integration_time_us;
|
||||
sensor_optical_flow.quality = flow.quality;
|
||||
odom.timestamp = hrt_absolute_time();
|
||||
|
||||
int index = (int) position_source::FLOW;
|
||||
|
||||
if (position_source_data[index].fail) {
|
||||
uint32_t duration = position_source_data[index].failure_duration;
|
||||
hrt_abstime start = position_source_data[index].failure_duration_start;
|
||||
|
||||
if (duration) {
|
||||
if (hrt_elapsed_time(&start) > (duration * 1000000)) {
|
||||
PX4_INFO("Optical flow failure ending");
|
||||
position_source_data[index].fail = false;
|
||||
position_source_data[index].failure_duration = 0;
|
||||
position_source_data[index].failure_duration_start = 0;
|
||||
|
||||
} else {
|
||||
sensor_optical_flow.quality = 0;
|
||||
}
|
||||
|
||||
} else {
|
||||
sensor_optical_flow.quality = 0;
|
||||
}
|
||||
}
|
||||
|
||||
const matrix::Vector3f integrated_gyro(flow.integrated_xgyro, flow.integrated_ygyro, flow.integrated_zgyro);
|
||||
|
||||
if (integrated_gyro.isAllFinite()) {
|
||||
integrated_gyro.copyTo(sensor_optical_flow.delta_angle);
|
||||
sensor_optical_flow.delta_angle_available = true;
|
||||
}
|
||||
|
||||
sensor_optical_flow.max_flow_rate = NAN;
|
||||
sensor_optical_flow.min_ground_distance = NAN;
|
||||
sensor_optical_flow.max_ground_distance = NAN;
|
||||
|
||||
// Use distance value for distance sensor topic
|
||||
// if (PX4_ISFINITE(flow.distance) && (flow.distance >= 0.f)) {
|
||||
// // Positive value (including zero): distance known. Negative value: Unknown distance.
|
||||
// sensor_optical_flow.distance_m = flow.distance;
|
||||
// sensor_optical_flow.distance_available = true;
|
||||
// }
|
||||
|
||||
// Emulate voxl-flow-server where distance comes in a separate
|
||||
// distance sensor topic message
|
||||
sensor_optical_flow.distance_m = 0.0f;
|
||||
sensor_optical_flow.distance_available = false;
|
||||
|
||||
sensor_optical_flow.timestamp = hrt_absolute_time();
|
||||
|
||||
_sensor_optical_flow_pub.publish(sensor_optical_flow);
|
||||
|
||||
flow_sent_counter++;
|
||||
}
|
||||
|
||||
int distance_debug_counter = 0;
|
||||
|
||||
void handle_message_distance_sensor(mavlink_message_t *msg)
|
||||
{
|
||||
mavlink_distance_sensor_t dist_sensor;
|
||||
mavlink_msg_distance_sensor_decode(msg, &dist_sensor);
|
||||
|
||||
if ((_debug) && (!(distance_debug_counter % 10))) {
|
||||
PX4_INFO("distance: time: %u, quality: %u, height: %u",
|
||||
dist_sensor.time_boot_ms, dist_sensor.signal_quality,
|
||||
dist_sensor.current_distance);
|
||||
}
|
||||
|
||||
distance_debug_counter++;
|
||||
|
||||
distance_sensor_s ds{};
|
||||
|
||||
device::Device::DeviceId device_id;
|
||||
device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_MAVLINK;
|
||||
device_id.devid_s.bus = 1;
|
||||
device_id.devid_s.address = msg->sysid;
|
||||
device_id.devid_s.devtype = DRV_DIST_DEVTYPE_MAVLINK;
|
||||
|
||||
ds.timestamp = hrt_absolute_time(); /* Use system time for now, don't trust sender to attach correct timestamp */
|
||||
ds.min_distance = static_cast<float>(dist_sensor.min_distance) * 1e-2f; /* cm to m */
|
||||
ds.max_distance = static_cast<float>(dist_sensor.max_distance) * 1e-2f; /* cm to m */
|
||||
ds.current_distance = static_cast<float>(dist_sensor.current_distance) * 1e-2f; /* cm to m */
|
||||
ds.variance = dist_sensor.covariance * 1e-4f; /* cm^2 to m^2 */
|
||||
ds.h_fov = dist_sensor.horizontal_fov;
|
||||
ds.v_fov = dist_sensor.vertical_fov;
|
||||
ds.q[0] = dist_sensor.quaternion[0];
|
||||
ds.q[1] = dist_sensor.quaternion[1];
|
||||
ds.q[2] = dist_sensor.quaternion[2];
|
||||
ds.q[3] = dist_sensor.quaternion[3];
|
||||
ds.type = dist_sensor.type;
|
||||
ds.device_id = device_id.devid;
|
||||
ds.orientation = dist_sensor.orientation;
|
||||
|
||||
// MAVLink DISTANCE_SENSOR signal_quality value of 0 means unset/unknown
|
||||
// quality value. Also it comes normalised between 1 and 100 while the uORB
|
||||
// signal quality is normalised between 0 and 100.
|
||||
ds.signal_quality = dist_sensor.signal_quality == 0 ? -1 : 100 * (dist_sensor.signal_quality - 1) / 99;
|
||||
|
||||
_distance_sensor_pub.publish(ds);
|
||||
|
||||
distance_sent_counter++;
|
||||
_visual_odometry_pub.publish(odom);
|
||||
vision_msg_counter++;
|
||||
}
|
||||
|
||||
void
|
||||
|
|
@ -666,8 +518,6 @@ handle_message_odometry_dsp(mavlink_message_t *msg)
|
|||
mavlink_odometry_t odom_in;
|
||||
mavlink_msg_odometry_decode(msg, &odom_in);
|
||||
|
||||
odometry_sent_counter++;
|
||||
|
||||
// fill vehicle_odometry from Mavlink ODOMETRY
|
||||
vehicle_odometry_s odom{};
|
||||
uint64_t timestamp = hrt_absolute_time();
|
||||
|
|
@ -849,28 +699,6 @@ handle_message_odometry_dsp(mavlink_message_t *msg)
|
|||
odom.reset_counter = odom_in.reset_counter;
|
||||
odom.quality = odom_in.quality;
|
||||
|
||||
int index = (int) position_source::VIO;
|
||||
|
||||
if (position_source_data[index].fail) {
|
||||
uint32_t duration = position_source_data[index].failure_duration;
|
||||
hrt_abstime start = position_source_data[index].failure_duration_start;
|
||||
|
||||
if (duration) {
|
||||
if (hrt_elapsed_time(&start) > (duration * 1000000)) {
|
||||
PX4_INFO("VIO failure ending");
|
||||
position_source_data[index].fail = false;
|
||||
position_source_data[index].failure_duration = 0;
|
||||
position_source_data[index].failure_duration_start = 0;
|
||||
|
||||
} else {
|
||||
odom.quality = 0;
|
||||
}
|
||||
|
||||
} else {
|
||||
odom.quality = 0;
|
||||
}
|
||||
}
|
||||
|
||||
switch (odom_in.estimator_type) {
|
||||
case MAV_ESTIMATOR_TYPE_UNKNOWN: // accept MAV_ESTIMATOR_TYPE_UNKNOWN for legacy support
|
||||
case MAV_ESTIMATOR_TYPE_NAIVE:
|
||||
|
|
@ -917,6 +745,10 @@ void actuator_controls_from_outputs_dsp(mavlink_hil_actuator_controls_t *msg)
|
|||
msg->mode = mode_flag_custom;
|
||||
msg->mode |= (armed) ? mode_flag_armed : 0;
|
||||
msg->flags = 0;
|
||||
|
||||
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
|
||||
msg->flags |= 1;
|
||||
#endif
|
||||
}
|
||||
|
||||
int openPort(const char *dev, speed_t speed)
|
||||
|
|
@ -927,8 +759,7 @@ int openPort(const char *dev, speed_t speed)
|
|||
}
|
||||
|
||||
_uart_fd = qurt_uart_open(dev, speed);
|
||||
|
||||
if (_debug) { PX4_INFO("qurt_uart_opened"); }
|
||||
PX4_DEBUG("qurt_uart_opened");
|
||||
|
||||
if (_uart_fd < 0) {
|
||||
PX4_ERR("Error opening port: %s (%i)", dev, errno);
|
||||
|
|
@ -1009,50 +840,25 @@ int stop()
|
|||
|
||||
void usage()
|
||||
{
|
||||
PX4_INFO("Usage: dsp_hitl {start|status|clear|failure|stop}");
|
||||
PX4_INFO(" failure <source> <duration>");
|
||||
PX4_INFO(" source: gps, vio, flow");
|
||||
PX4_INFO(" duration: 0 (toggle state), else seconds");
|
||||
PX4_INFO("Usage: dsp_hitl {start|info|status|stop}");
|
||||
}
|
||||
|
||||
void print_status()
|
||||
int get_status()
|
||||
{
|
||||
PX4_INFO("Running: %s", _is_running ? "yes" : "no");
|
||||
PX4_INFO("HIL Sensor received: %i", hil_sensor_counter);
|
||||
PX4_INFO("IMU updates: %i", imu_counter);
|
||||
PX4_INFO("\tCurrent accel x, y, z: %f, %f, %f", double(x_accel), double(y_accel), double(z_accel));
|
||||
PX4_INFO("\tCurrent gyro x, y, z: %f, %f, %f", double(x_gyro), double(y_gyro), double(z_gyro));
|
||||
PX4_INFO("Magnetometer sent: %i", mag_counter);
|
||||
PX4_INFO("Barometer sent: %i", baro_counter);
|
||||
PX4_INFO("Heartbeat received: %i, sent: %i", heartbeat_received_counter, heartbeat_sent_counter);
|
||||
PX4_INFO("Odometry received: %i, sent: %i", odometry_received_counter, odometry_sent_counter);
|
||||
PX4_INFO("GPS received: %i, sent: %i", gps_received_counter, gps_sent_counter);
|
||||
PX4_INFO("Distance sensor received: %i, sent: %i", distance_received_counter, distance_sent_counter);
|
||||
PX4_INFO("Optical flow received: %i, sent: %i", flow_received_counter, flow_sent_counter);
|
||||
PX4_INFO("Actuator updates sent: %i", actuator_sent_counter);
|
||||
PX4_INFO("Unknown messages received: %i", unknown_msg_received_counter);
|
||||
PX4_INFO("Status of IMU_Data counter: %i", imu_counter);
|
||||
PX4_INFO("Value of current accel x, y, z data: %f, %f, %f", double(x_accel), double(y_accel), double(z_accel));
|
||||
PX4_INFO("Value of current gyro x, y, z data: %f, %f, %f", double(x_gyro), double(y_gyro), double(z_gyro));
|
||||
PX4_INFO("Value of HIL_Sensor counter: %i", hil_sensor_counter);
|
||||
PX4_INFO("Value of Heartbeat counter: %i", heartbeat_counter);
|
||||
PX4_INFO("Value of Vision data counter: %i", vision_msg_counter);
|
||||
PX4_INFO("Value of GPS Data counter: %i", gps_counter);
|
||||
return 0;
|
||||
}
|
||||
|
||||
void
|
||||
clear_status_counters()
|
||||
{
|
||||
heartbeat_received_counter = 0;
|
||||
heartbeat_sent_counter = 0;
|
||||
imu_counter = 0;
|
||||
hil_sensor_counter = 0;
|
||||
mag_counter = 0;
|
||||
baro_counter = 0;
|
||||
actuator_sent_counter = 0;
|
||||
odometry_received_counter = 0;
|
||||
odometry_sent_counter = 0;
|
||||
gps_received_counter = 0;
|
||||
gps_sent_counter = 0;
|
||||
distance_received_counter = 0;
|
||||
distance_sent_counter = 0;
|
||||
flow_received_counter = 0;
|
||||
flow_sent_counter = 0;
|
||||
unknown_msg_received_counter = 0;
|
||||
}
|
||||
uint64_t first_sensor_msg_timestamp = 0;
|
||||
uint64_t first_sensor_report_timestamp = 0;
|
||||
uint64_t last_sensor_report_timestamp = 0;
|
||||
|
||||
void
|
||||
handle_message_hil_sensor_dsp(mavlink_message_t *msg)
|
||||
|
|
@ -1122,8 +928,6 @@ handle_message_hil_sensor_dsp(mavlink_message_t *msg)
|
|||
}
|
||||
|
||||
_px4_mag->update(gyro_accel_time, hil_sensor.xmag, hil_sensor.ymag, hil_sensor.zmag);
|
||||
|
||||
mag_counter++;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -1138,8 +942,17 @@ handle_message_hil_sensor_dsp(mavlink_message_t *msg)
|
|||
sensor_baro.error_count = 0;
|
||||
sensor_baro.timestamp = hrt_absolute_time();
|
||||
_sensor_baro_pub.publish(sensor_baro);
|
||||
}
|
||||
|
||||
baro_counter++;
|
||||
// differential pressure
|
||||
if ((hil_sensor.fields_updated & SensorSource::DIFF_PRESS) == SensorSource::DIFF_PRESS) {
|
||||
differential_pressure_s report{};
|
||||
report.timestamp_sample = gyro_accel_time;
|
||||
report.device_id = 1377548; // 1377548: DRV_DIFF_PRESS_DEVTYPE_SIM, BUS: 1, ADDR: 5, TYPE: SIMULATION
|
||||
report.temperature = hil_sensor.temperature;
|
||||
report.differential_pressure_pa = hil_sensor.diff_pressure * 100.0f; // hPa to Pa
|
||||
report.timestamp = hrt_absolute_time();
|
||||
_differential_pressure_pub.publish(report);
|
||||
}
|
||||
|
||||
// battery status
|
||||
|
|
@ -1157,6 +970,7 @@ handle_message_hil_sensor_dsp(mavlink_message_t *msg)
|
|||
|
||||
_battery_pub.publish(hil_battery_status);
|
||||
}
|
||||
hil_sensor_counter++;
|
||||
}
|
||||
|
||||
void
|
||||
|
|
@ -1175,41 +989,15 @@ handle_message_hil_gps_dsp(mavlink_message_t *msg)
|
|||
|
||||
gps.device_id = device_id.devid;
|
||||
|
||||
gps.latitude_deg = hil_gps.lat * 1e-7;
|
||||
gps.longitude_deg = hil_gps.lon * 1e-7;
|
||||
gps.altitude_msl_m = hil_gps.alt * 1e-3;
|
||||
gps.altitude_ellipsoid_m = hil_gps.alt * 1e-3;
|
||||
gps.lat = hil_gps.lat;
|
||||
gps.lon = hil_gps.lon;
|
||||
gps.alt = hil_gps.alt;
|
||||
gps.alt_ellipsoid = hil_gps.alt;
|
||||
|
||||
gps.s_variance_m_s = 0.25f;
|
||||
gps.c_variance_rad = 0.5f;
|
||||
|
||||
gps.satellites_used = hil_gps.satellites_visible;
|
||||
gps.fix_type = hil_gps.fix_type;
|
||||
|
||||
int index = (int) position_source::GPS;
|
||||
|
||||
if (position_source_data[index].fail) {
|
||||
uint32_t duration = position_source_data[index].failure_duration;
|
||||
hrt_abstime start = position_source_data[index].failure_duration_start;
|
||||
|
||||
if (duration) {
|
||||
if (hrt_elapsed_time(&start) > (duration * 1000000)) {
|
||||
PX4_INFO("GPS failure ending");
|
||||
position_source_data[index].fail = false;
|
||||
position_source_data[index].failure_duration = 0;
|
||||
position_source_data[index].failure_duration_start = 0;
|
||||
|
||||
} else {
|
||||
gps.satellites_used = 1;
|
||||
gps.fix_type = 0;
|
||||
}
|
||||
|
||||
} else {
|
||||
gps.satellites_used = 1;
|
||||
gps.fix_type = 0;
|
||||
}
|
||||
}
|
||||
|
||||
gps.eph = (float)hil_gps.eph * 1e-2f; // cm -> m
|
||||
gps.epv = (float)hil_gps.epv * 1e-2f; // cm -> m
|
||||
|
||||
|
|
@ -1233,6 +1021,7 @@ handle_message_hil_gps_dsp(mavlink_message_t *msg)
|
|||
gps.timestamp_time_relative = 0;
|
||||
gps.time_utc_usec = hil_gps.time_usec;
|
||||
|
||||
gps.satellites_used = hil_gps.satellites_visible;
|
||||
|
||||
gps.heading = NAN;
|
||||
gps.heading_offset = NAN;
|
||||
|
|
@ -1240,51 +1029,10 @@ handle_message_hil_gps_dsp(mavlink_message_t *msg)
|
|||
gps.timestamp = hrt_absolute_time();
|
||||
|
||||
_sensor_gps_pub.publish(gps);
|
||||
|
||||
gps_sent_counter++;
|
||||
gps_counter++;
|
||||
}
|
||||
|
||||
int
|
||||
process_failure(dsp_hitl::position_source src, int duration)
|
||||
{
|
||||
if (src >= position_source::NUM_POSITION_SOURCES) {
|
||||
return 1;
|
||||
}
|
||||
|
||||
int index = (int) src;
|
||||
|
||||
if (position_source_data[index].send) {
|
||||
if (duration <= 0) {
|
||||
// Toggle state
|
||||
if (position_source_data[index].fail) {
|
||||
PX4_INFO("Ending indefinite %s failure", position_source_data[index].label);
|
||||
position_source_data[index].fail = false;
|
||||
|
||||
} else {
|
||||
PX4_INFO("Starting indefinite %s failure", position_source_data[index].label);
|
||||
position_source_data[index].fail = true;
|
||||
}
|
||||
|
||||
position_source_data[index].failure_duration = 0;
|
||||
position_source_data[index].failure_duration_start = 0;
|
||||
|
||||
} else {
|
||||
PX4_INFO("%s failure for %d seconds", position_source_data[index].label, duration);
|
||||
position_source_data[index].fail = true;
|
||||
position_source_data[index].failure_duration = duration;
|
||||
position_source_data[index].failure_duration_start = hrt_absolute_time();
|
||||
}
|
||||
|
||||
} else {
|
||||
PX4_ERR("%s not active, cannot create failure", position_source_data[index].label);
|
||||
return 1;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
} // End dsp_hitl namespace
|
||||
|
||||
int dsp_hitl_main(int argc, char *argv[])
|
||||
{
|
||||
int myoptind = 1;
|
||||
|
|
@ -1296,47 +1044,20 @@ int dsp_hitl_main(int argc, char *argv[])
|
|||
|
||||
const char *verb = argv[myoptind];
|
||||
|
||||
|
||||
if (!strcmp(verb, "start")) {
|
||||
return dsp_hitl::start(argc - 1, argv + 1);
|
||||
}
|
||||
|
||||
} else if (!strcmp(verb, "stop")) {
|
||||
else if (!strcmp(verb, "stop")) {
|
||||
return dsp_hitl::stop();
|
||||
}
|
||||
|
||||
} else if (!strcmp(verb, "status")) {
|
||||
dsp_hitl::print_status();
|
||||
return 0;
|
||||
else if (!strcmp(verb, "status")) {
|
||||
return dsp_hitl::get_status();
|
||||
}
|
||||
|
||||
} else if (!strcmp(verb, "clear")) {
|
||||
dsp_hitl::clear_status_counters();
|
||||
return 0;
|
||||
|
||||
} else if (!strcmp(verb, "failure")) {
|
||||
if (argc != 4) {
|
||||
dsp_hitl::usage();
|
||||
return 1;
|
||||
}
|
||||
|
||||
const char *source = argv[myoptind + 1];
|
||||
int duration = atoi(argv[myoptind + 2]);
|
||||
|
||||
if (!strcmp(source, "gps")) {
|
||||
return dsp_hitl::process_failure(dsp_hitl::position_source::GPS, duration);
|
||||
|
||||
} else if (!strcmp(source, "vio")) {
|
||||
return dsp_hitl::process_failure(dsp_hitl::position_source::VIO, duration);
|
||||
|
||||
} else if (!strcmp(source, "flow")) {
|
||||
return dsp_hitl::process_failure(dsp_hitl::position_source::FLOW, duration);
|
||||
|
||||
} else {
|
||||
PX4_ERR("Unknown failure source %s, duration %d", source, duration);
|
||||
dsp_hitl::usage();
|
||||
return 1;
|
||||
}
|
||||
|
||||
return 0;
|
||||
|
||||
} else {
|
||||
else {
|
||||
dsp_hitl::usage();
|
||||
return 1;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2024 PX4 Development Team. All rights reserved.
|
||||
# Copyright (c) 2020 PX4 Development Team. All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
|
|
@ -30,5 +30,19 @@
|
|||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
px4_add_library(rtl_time_estimator rtl_time_estimator.cpp)
|
||||
px4_add_module(
|
||||
MODULE drivers__imu__invensense__icm42688p
|
||||
MAIN icm42688p
|
||||
COMPILE_FLAGS
|
||||
${MAX_CUSTOM_OPT_LEVEL}
|
||||
SRCS
|
||||
icm42688p_main.cpp
|
||||
ICM42688P.cpp
|
||||
ICM42688P.hpp
|
||||
InvenSense_ICM42688P_registers.hpp
|
||||
DEPENDS
|
||||
px4_work_queue
|
||||
drivers_accelerometer
|
||||
drivers_gyroscope
|
||||
drivers__device
|
||||
)
|
||||
|
|
@ -0,0 +1,991 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include "ICM42688P.hpp"
|
||||
|
||||
bool hitl_mode = false;
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
static constexpr int16_t combine(uint8_t msb, uint8_t lsb)
|
||||
{
|
||||
return (msb << 8u) | lsb;
|
||||
}
|
||||
|
||||
ICM42688P::ICM42688P(const I2CSPIDriverConfig &config) :
|
||||
// SPI(DRV_IMU_DEVTYPE_ICM42688P, MODULE_NAME, bus, device, spi_mode, bus_frequency),
|
||||
// I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus),
|
||||
// _drdy_gpio(drdy_gpio)
|
||||
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 (!hitl_mode) {
|
||||
// _px4_accel = std::make_shared<PX4Accelerometer>(get_device_id(), rotation);
|
||||
// _px4_gyro = std::make_shared<PX4Gyroscope>(get_device_id(), rotation);
|
||||
ConfigureSampleRate(_px4_gyro.get_max_rate_hz());
|
||||
// _imu_server_pub.advertise();
|
||||
|
||||
} else {
|
||||
ConfigureSampleRate(0);
|
||||
}
|
||||
}
|
||||
|
||||
ICM42688P::~ICM42688P()
|
||||
{
|
||||
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);
|
||||
|
||||
// if (!hitl_mode){
|
||||
// _imu_server_pub.unadvertise();
|
||||
// }
|
||||
}
|
||||
|
||||
int ICM42688P::init()
|
||||
{
|
||||
int ret = SPI::init();
|
||||
|
||||
if (ret != PX4_OK) {
|
||||
DEVICE_DEBUG("SPI::init failed (%i)", ret);
|
||||
return ret;
|
||||
}
|
||||
|
||||
return Reset() ? 0 : -1;
|
||||
}
|
||||
|
||||
bool ICM42688P::Reset()
|
||||
{
|
||||
_state = STATE::RESET;
|
||||
DataReadyInterruptDisable();
|
||||
ScheduleClear();
|
||||
ScheduleNow();
|
||||
return true;
|
||||
}
|
||||
|
||||
void ICM42688P::exit_and_cleanup()
|
||||
{
|
||||
DataReadyInterruptDisable();
|
||||
I2CSPIDriverBase::exit_and_cleanup();
|
||||
}
|
||||
|
||||
void ICM42688P::print_status()
|
||||
{
|
||||
I2CSPIDriverBase::print_status();
|
||||
|
||||
PX4_INFO("FIFO empty interval: %d us (%.1f Hz)", _fifo_empty_interval_us, 1e6 / _fifo_empty_interval_us);
|
||||
|
||||
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 ICM42688P::probe()
|
||||
{
|
||||
for (int i = 0; i < 3; i++) {
|
||||
uint8_t whoami = RegisterRead(Register::BANK_0::WHO_AM_I);
|
||||
|
||||
if (whoami == WHOAMI) {
|
||||
PX4_INFO("ICM42688P::probe successful!");
|
||||
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::USER_BANK_0, true);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
void ICM42688P::RunImpl()
|
||||
{
|
||||
PX4_INFO(">>> ICM42688P this: %p", this);
|
||||
|
||||
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(2_ms); // to be safe wait 2 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)) {
|
||||
|
||||
_state = STATE::CONFIGURE;
|
||||
ScheduleDelayed(10_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()) {
|
||||
|
||||
// Wakeup accel and gyro after configuring registers
|
||||
ScheduleDelayed(1_ms); // add a delay here to be safe
|
||||
RegisterWrite(Register::BANK_0::PWR_MGMT0, PWR_MGMT0_BIT::GYRO_MODE_LOW_NOISE | PWR_MGMT0_BIT::ACCEL_MODE_LOW_NOISE);
|
||||
ScheduleDelayed(30_ms); // 30 ms gyro startup time, 10 ms accel from sleep to valid data
|
||||
|
||||
// if configure succeeded then start reading from FIFO
|
||||
_state = STATE::FIFO_READ;
|
||||
|
||||
if (DataReadyInterruptConfigure()) {
|
||||
_data_ready_interrupt_enabled = true;
|
||||
|
||||
// backup schedule as a watchdog timeout
|
||||
ScheduleDelayed(100_ms);
|
||||
|
||||
} else {
|
||||
PX4_ERR("ICM42688P::RunImpl interrupt configuration failed");
|
||||
|
||||
_data_ready_interrupt_enabled = false;
|
||||
ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us);
|
||||
}
|
||||
|
||||
FIFOReset();
|
||||
|
||||
} else {
|
||||
|
||||
PX4_ERR("ICM42688P::RunImpl configuration failed");
|
||||
|
||||
// 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_READ: {
|
||||
#ifndef __PX4_QURT
|
||||
uint32_t samples = 0;
|
||||
|
||||
if (_data_ready_interrupt_enabled) {
|
||||
// scheduled from interrupt if _drdy_fifo_read_samples was set as expected
|
||||
if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_gyro_samples) {
|
||||
perf_count(_drdy_missed_perf);
|
||||
|
||||
} else {
|
||||
samples = _fifo_gyro_samples;
|
||||
}
|
||||
|
||||
// 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));
|
||||
|
||||
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(now, 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;
|
||||
}
|
||||
}
|
||||
|
||||
// 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();
|
||||
}
|
||||
|
||||
#endif
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void ICM42688P::ConfigureSampleRate(int sample_rate)
|
||||
{
|
||||
if (sample_rate == 0) {
|
||||
sample_rate = 800; // default to 800 Hz
|
||||
}
|
||||
|
||||
// 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 ICM42688P::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 ICM42688P::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 ICM42688P::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
|
||||
if (!hitl_mode) {
|
||||
_px4_accel.set_range(16.f * CONSTANTS_ONE_G);
|
||||
_px4_accel.set_scale(CONSTANTS_ONE_G / 8192.f);
|
||||
_px4_gyro.set_range(math::radians(2000.f));
|
||||
_px4_gyro.set_scale(math::radians(1.f / 131.f));
|
||||
}
|
||||
|
||||
return success;
|
||||
}
|
||||
|
||||
static bool interrupt_debug = false;
|
||||
static uint32_t interrupt_debug_count = 0;
|
||||
static const uint32_t interrupt_debug_trigger = 800;
|
||||
static hrt_abstime last_interrupt_time = 0;
|
||||
static hrt_abstime avg_interrupt_delta = 0;
|
||||
static hrt_abstime max_interrupt_delta = 0;
|
||||
static hrt_abstime min_interrupt_delta = 60 * 1000 * 1000;
|
||||
static hrt_abstime cumulative_interrupt_delta = 0;
|
||||
|
||||
int ICM42688P::DataReadyInterruptCallback(int irq, void *context, void *arg)
|
||||
{
|
||||
hrt_abstime current_interrupt_time = hrt_absolute_time();
|
||||
|
||||
if (interrupt_debug) {
|
||||
if (last_interrupt_time) {
|
||||
hrt_abstime interrupt_delta_time = current_interrupt_time - last_interrupt_time;
|
||||
|
||||
if (interrupt_delta_time > max_interrupt_delta) { max_interrupt_delta = interrupt_delta_time; }
|
||||
|
||||
if (interrupt_delta_time < min_interrupt_delta) { min_interrupt_delta = interrupt_delta_time; }
|
||||
|
||||
cumulative_interrupt_delta += interrupt_delta_time;
|
||||
}
|
||||
|
||||
last_interrupt_time = current_interrupt_time;
|
||||
|
||||
interrupt_debug_count++;
|
||||
|
||||
if (interrupt_debug_count == interrupt_debug_trigger) {
|
||||
avg_interrupt_delta = cumulative_interrupt_delta / interrupt_debug_trigger;
|
||||
PX4_INFO(">>> Max: %llu, Min: %llu, Avg: %llu", max_interrupt_delta,
|
||||
min_interrupt_delta, avg_interrupt_delta);
|
||||
interrupt_debug_count = 0;
|
||||
cumulative_interrupt_delta = 0;
|
||||
}
|
||||
}
|
||||
|
||||
static_cast<ICM42688P *>(arg)->DataReady();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
void ICM42688P::DataReady()
|
||||
{
|
||||
#ifndef __PX4_QURT
|
||||
uint32_t expected = 0;
|
||||
|
||||
if (_drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) {
|
||||
ScheduleNow();
|
||||
}
|
||||
|
||||
#else
|
||||
uint16_t fifo_byte_count = FIFOReadCount();
|
||||
|
||||
FIFORead(hrt_absolute_time(), fifo_byte_count / sizeof(FIFO::DATA));
|
||||
#endif
|
||||
}
|
||||
|
||||
bool ICM42688P::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 ICM42688P::DataReadyInterruptDisable()
|
||||
{
|
||||
if (_drdy_gpio == 0) {
|
||||
return false;
|
||||
}
|
||||
|
||||
return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
bool ICM42688P::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 ICM42688P::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 ICM42688P::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 ICM42688P::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 ICM42688P::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::USER_BANK_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]);
|
||||
}
|
||||
|
||||
// static uint32_t debug_decimator = 0;
|
||||
// static hrt_abstime last_sample_time = 0;
|
||||
// static bool imu_debug = true;
|
||||
|
||||
bool ICM42688P::FIFORead(const hrt_abstime ×tamp_sample, uint16_t samples)
|
||||
{
|
||||
FIFOTransferBuffer buffer{};
|
||||
const size_t max_transfer_size = 10 * sizeof(FIFO::DATA) + 4;
|
||||
const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 4, max_transfer_size);
|
||||
SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_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 uint16_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
|
||||
uint16_t valid_samples = 0;
|
||||
|
||||
// for (int i = 0; i < math::min(samples, fifo_count_samples); i++) {
|
||||
for (int i = 0; i < math::min(samples, (uint16_t) 10); 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_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 (imu_debug) {
|
||||
// debug_decimator++;
|
||||
// if (debug_decimator == 801) {
|
||||
// debug_decimator = 0;
|
||||
// PX4_INFO("Initial: %u Next: %u Valid: %u Delta: %llu", samples, fifo_count_samples, valid_samples, timestamp_sample - last_sample_time);
|
||||
// }
|
||||
// last_sample_time = timestamp_sample;
|
||||
// }
|
||||
|
||||
if (valid_samples > 0) {
|
||||
if (ProcessTemperature(buffer.f, valid_samples)) {
|
||||
ProcessGyro(timestamp_sample, buffer.f, valid_samples);
|
||||
ProcessAccel(timestamp_sample, buffer.f, valid_samples);
|
||||
ProcessIMU(timestamp_sample, buffer.f, valid_samples);
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
void ICM42688P::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_fifo_read_samples.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 ICM42688P::ProcessIMU(const hrt_abstime ×tamp_sample, const FIFO::DATA fifo[], const uint8_t samples)
|
||||
{
|
||||
float accel_x = 0.0, accel_y = 0.0, accel_z = 0.0;
|
||||
float gyro_x = 0.0, gyro_y = 0.0, gyro_z = 0.0;
|
||||
|
||||
for (int i = 0; i < samples; i++) {
|
||||
_imu_server_decimator++;
|
||||
|
||||
if (_imu_server_decimator == 8) {
|
||||
_imu_server_decimator = 0;
|
||||
// 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 temp_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 temp_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 temp_accel_z = reassemble_20bit(fifo[i].ACCEL_DATA_Z1, fifo[i].ACCEL_DATA_Z0,
|
||||
fifo[i].Ext_Accel_Z_Gyro_Z & 0xF0 >> 4);
|
||||
|
||||
// Gyro [19:12] + Gyro [11:4] + Gyro [3:0] (bottom 4 bits of 20 bit extension byte)
|
||||
int32_t temp_gyro_x = reassemble_20bit(fifo[i].GYRO_DATA_X1, fifo[i].GYRO_DATA_X0,
|
||||
fifo[i].Ext_Accel_X_Gyro_X & 0x0F);
|
||||
int32_t temp_gyro_y = reassemble_20bit(fifo[i].GYRO_DATA_Y1, fifo[i].GYRO_DATA_Y0,
|
||||
fifo[i].Ext_Accel_Y_Gyro_Y & 0x0F);
|
||||
int32_t temp_gyro_z = reassemble_20bit(fifo[i].GYRO_DATA_Z1, fifo[i].GYRO_DATA_Z0,
|
||||
fifo[i].Ext_Accel_Z_Gyro_Z & 0x0F);
|
||||
|
||||
// accel samples invalid if -524288
|
||||
if (temp_accel_x != -524288 && temp_accel_y != -524288 && temp_accel_z != -524288) {
|
||||
// shift accel by 2 (2 least significant bits are always 0)
|
||||
accel_x = (float) temp_accel_x / 4.f;
|
||||
accel_y = (float) temp_accel_y / 4.f;
|
||||
accel_z = (float) temp_accel_z / 4.f;
|
||||
|
||||
// shift gyro by 1 (least significant bit is always 0)
|
||||
gyro_x = (float) temp_gyro_x / 2.f;
|
||||
gyro_y = (float) temp_gyro_y / 2.f;
|
||||
gyro_z = (float) temp_gyro_z / 2.f;
|
||||
|
||||
// correct frame for publication
|
||||
// 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_y = -accel_y;
|
||||
accel_z = -accel_z;
|
||||
gyro_y = -gyro_y;
|
||||
gyro_z = -gyro_z;
|
||||
|
||||
// Scale everything appropriately
|
||||
float accel_scale_factor = (CONSTANTS_ONE_G / 8192.f);
|
||||
accel_x *= accel_scale_factor;
|
||||
accel_y *= accel_scale_factor;
|
||||
accel_z *= accel_scale_factor;
|
||||
|
||||
float gyro_scale_factor = math::radians(1.f / 131.f);
|
||||
gyro_x *= gyro_scale_factor;
|
||||
gyro_y *= gyro_scale_factor;
|
||||
gyro_z *= gyro_scale_factor;
|
||||
|
||||
// Store the data in our array
|
||||
_imu_server_data.accel_x[_imu_server_index] = accel_x;
|
||||
_imu_server_data.accel_y[_imu_server_index] = accel_y;
|
||||
_imu_server_data.accel_z[_imu_server_index] = accel_z;
|
||||
_imu_server_data.gyro_x[_imu_server_index] = gyro_x;
|
||||
_imu_server_data.gyro_y[_imu_server_index] = gyro_y;
|
||||
_imu_server_data.gyro_z[_imu_server_index] = gyro_z;
|
||||
_imu_server_data.ts[_imu_server_index] = timestamp_sample - (125 * (samples - 1 - i));
|
||||
_imu_server_index++;
|
||||
|
||||
// If array is full, publish the data
|
||||
if (_imu_server_index == 10) {
|
||||
_imu_server_index = 0;
|
||||
_imu_server_data.timestamp = hrt_absolute_time();
|
||||
_imu_server_data.temperature = 0; // Not used right now
|
||||
_imu_server_pub.publish(_imu_server_data);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void ICM42688P::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;
|
||||
accel.dt = FIFO_SAMPLE_DT;
|
||||
|
||||
// 18-bits of accelerometer data
|
||||
bool scale_20bit = false;
|
||||
|
||||
// first pass
|
||||
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 ()
|
||||
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 8192 LSB/g
|
||||
if (!hitl_mode) {
|
||||
_px4_accel.set_scale(CONSTANTS_ONE_G / 8192.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;
|
||||
}
|
||||
|
||||
if (!hitl_mode) {
|
||||
_px4_accel.set_scale(CONSTANTS_ONE_G / 2048.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];
|
||||
}
|
||||
|
||||
if (!hitl_mode) {
|
||||
_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) {
|
||||
if (!hitl_mode) {
|
||||
_px4_accel.updateFIFO(accel);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void ICM42688P::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;
|
||||
gyro.dt = FIFO_SAMPLE_DT;
|
||||
|
||||
// 20-bits of gyroscope data
|
||||
bool scale_20bit = false;
|
||||
|
||||
// first pass
|
||||
for (int i = 0; i < samples; i++) {
|
||||
// 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 131 LSB/dps
|
||||
if (!hitl_mode) {
|
||||
_px4_gyro.set_scale(math::radians(1.f / 131.f));
|
||||
}
|
||||
|
||||
} 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);
|
||||
}
|
||||
|
||||
if (!hitl_mode) {
|
||||
_px4_gyro.set_scale(math::radians(2000.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];
|
||||
}
|
||||
|
||||
if (!hitl_mode) {
|
||||
_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) {
|
||||
if (!hitl_mode) {
|
||||
_px4_gyro.updateFIFO(gyro);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool ICM42688P::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)) {
|
||||
if (!hitl_mode) {
|
||||
_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,235 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file ICM42688P.hpp
|
||||
*
|
||||
* Driver for the Invensense ICM42688P connected via SPI.
|
||||
*
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "InvenSense_ICM42688P_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>
|
||||
#include <uORB/topics/imu_server.h>
|
||||
#include <uORB/topics/sensor_accel_fifo.h>
|
||||
#include <uORB/topics/sensor_gyro_fifo.h>
|
||||
#include <memory>
|
||||
|
||||
using namespace InvenSense_ICM42688P;
|
||||
|
||||
extern bool hitl_mode;
|
||||
|
||||
class ICM42688P : public device::SPI, public I2CSPIDriver<ICM42688P>
|
||||
{
|
||||
public:
|
||||
// ICM42688P(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency,
|
||||
// spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio);
|
||||
ICM42688P(const I2CSPIDriverConfig &config);
|
||||
~ICM42688P() override;
|
||||
|
||||
// static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
|
||||
// int runtime_instance);
|
||||
static void print_usage();
|
||||
|
||||
void RunImpl();
|
||||
|
||||
int init() override;
|
||||
void print_status() override;
|
||||
|
||||
private:
|
||||
void exit_and_cleanup() override;
|
||||
|
||||
// Sensor Configuration
|
||||
static constexpr float IMU_ODR{8000.f}; // 8kHz accel & gyro ODR configured
|
||||
static constexpr float FIFO_SAMPLE_DT{1e6f / IMU_ODR};
|
||||
static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT};
|
||||
static constexpr float ACCEL_RATE{1e6f / FIFO_SAMPLE_DT};
|
||||
|
||||
// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
|
||||
// static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(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))};
|
||||
static constexpr uint32_t FIFO_MAX_SAMPLES{10};
|
||||
|
||||
// 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)),
|
||||
"Invalid FIFOTransferBuffer size");
|
||||
|
||||
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 SelectRegisterBank(enum REG_BANK_SEL_BIT bank, bool force = false);
|
||||
void SelectRegisterBank(Register::BANK_0 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0); }
|
||||
void SelectRegisterBank(Register::BANK_1 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_1); }
|
||||
void SelectRegisterBank(Register::BANK_2 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_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, uint16_t samples);
|
||||
void FIFOReset();
|
||||
|
||||
void ProcessIMU(const hrt_abstime ×tamp_sample, const FIFO::DATA fifo[], const uint8_t samples);
|
||||
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;
|
||||
|
||||
// std::shared_ptr<PX4Accelerometer> _px4_accel;
|
||||
// std::shared_ptr<PX4Gyroscope> _px4_gyro;
|
||||
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};
|
||||
|
||||
enum REG_BANK_SEL_BIT _last_register_bank {REG_BANK_SEL_BIT::USER_BANK_0};
|
||||
|
||||
px4::atomic<uint32_t> _drdy_fifo_read_samples{0};
|
||||
bool _data_ready_interrupt_enabled{false};
|
||||
|
||||
enum class STATE : uint8_t {
|
||||
RESET,
|
||||
WAIT_FOR_RESET,
|
||||
CONFIGURE,
|
||||
FIFO_READ,
|
||||
} _state{STATE::RESET};
|
||||
|
||||
uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
|
||||
uint32_t _fifo_gyro_samples{static_cast<uint32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
|
||||
|
||||
uint8_t _checked_register_bank0{0};
|
||||
static constexpr uint8_t size_register_bank0_cfg{12};
|
||||
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::GYRO_CONFIG0, GYRO_CONFIG0_BIT::GYRO_FS_SEL_2000_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_16G | 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::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, 0 },
|
||||
{ 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_SOURCE0, INT_SOURCE0_BIT::FIFO_THS_INT1_EN, 0 },
|
||||
};
|
||||
|
||||
uint8_t _checked_register_bank1{0};
|
||||
static constexpr uint8_t size_register_bank1_cfg{4};
|
||||
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_SET, GYRO_CONFIG_STATIC3_BIT::GYRO_AAF_DELT_CLEAR},
|
||||
{ Register::BANK_1::GYRO_CONFIG_STATIC4, GYRO_CONFIG_STATIC4_BIT::GYRO_AAF_DELTSQR_LOW_SET, GYRO_CONFIG_STATIC4_BIT::GYRO_AAF_DELTSQR_LOW_CLEAR},
|
||||
{ Register::BANK_1::GYRO_CONFIG_STATIC5, GYRO_CONFIG_STATIC5_BIT::GYRO_AAF_BITSHIFT_SET | GYRO_CONFIG_STATIC5_BIT::GYRO_AAF_DELTSQR_HIGH_SET, GYRO_CONFIG_STATIC5_BIT::GYRO_AAF_BITSHIFT_CLEAR | GYRO_CONFIG_STATIC5_BIT::GYRO_AAF_DELTSQR_HIGH_CLEAR},
|
||||
};
|
||||
|
||||
uint8_t _checked_register_bank2{0};
|
||||
static constexpr uint8_t size_register_bank2_cfg{3};
|
||||
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_SET, ACCEL_CONFIG_STATIC2_BIT::ACCEL_AAF_DELT_CLEAR | ACCEL_CONFIG_STATIC2_BIT::ACCEL_AAF_DIS },
|
||||
{ Register::BANK_2::ACCEL_CONFIG_STATIC3, ACCEL_CONFIG_STATIC3_BIT::ACCEL_AAF_DELTSQR_LOW_SET, ACCEL_CONFIG_STATIC3_BIT::ACCEL_AAF_DELTSQR_LOW_CLEAR },
|
||||
{ Register::BANK_2::ACCEL_CONFIG_STATIC4, ACCEL_CONFIG_STATIC4_BIT::ACCEL_AAF_BITSHIFT_SET | ACCEL_CONFIG_STATIC4_BIT::ACCEL_AAF_DELTSQR_HIGH_SET, ACCEL_CONFIG_STATIC4_BIT::ACCEL_AAF_BITSHIFT_CLEAR | ACCEL_CONFIG_STATIC4_BIT::ACCEL_AAF_DELTSQR_HIGH_CLEAR },
|
||||
};
|
||||
|
||||
uint32_t _temperature_samples{0};
|
||||
|
||||
// Support for the IMU server
|
||||
uint32_t _imu_server_index{0};
|
||||
uint32_t _imu_server_decimator{0};
|
||||
imu_server_s _imu_server_data;
|
||||
uORB::Publication<imu_server_s> _imu_server_pub{ORB_ID(imu_server)};
|
||||
|
||||
};
|
||||
|
|
@ -0,0 +1,430 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file InvenSense_ICM42688P_registers.hpp
|
||||
*
|
||||
* Invensense ICM-42688-P registers.
|
||||
*
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <cstdint>
|
||||
|
||||
namespace InvenSense_ICM42688P
|
||||
{
|
||||
// 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 = 0x47;
|
||||
|
||||
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,
|
||||
|
||||
FIFO_CONFIG1 = 0x5F,
|
||||
FIFO_CONFIG2 = 0x60,
|
||||
FIFO_CONFIG3 = 0x61,
|
||||
|
||||
INT_CONFIG0 = 0x63,
|
||||
|
||||
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,
|
||||
};
|
||||
|
||||
};
|
||||
|
||||
//---------------- 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,
|
||||
};
|
||||
|
||||
// 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_2000_DPS = 0, // 0b000 = ±2000dps (default)
|
||||
GYRO_FS_SEL_1000_DPS = Bit5,
|
||||
GYRO_FS_SEL_500_DPS = Bit6,
|
||||
GYRO_FS_SEL_250_DPS = Bit6 | Bit5,
|
||||
GYRO_FS_SEL_125_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_16G = 0, // 000: ±16g (default)
|
||||
ACCEL_FS_SEL_8G = Bit5,
|
||||
ACCEL_FS_SEL_4G = Bit6,
|
||||
ACCEL_FS_SEL_2G = 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
|
||||
};
|
||||
|
||||
// FIFO_CONFIG1
|
||||
enum FIFO_CONFIG1_BIT : uint8_t {
|
||||
FIFO_RESUME_PARTIAL_RD = Bit6,
|
||||
FIFO_WM_GT_TH = Bit5,
|
||||
FIFO_HIRES_EN = Bit4,
|
||||
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_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 {
|
||||
USER_BANK_0 = 0, // 0: Select USER BANK 0.
|
||||
USER_BANK_1 = Bit0, // 1: Select USER BANK 1.
|
||||
USER_BANK_2 = Bit1, // 2: Select USER BANK 2.
|
||||
USER_BANK_3 = Bit1 | Bit0, // 3: Select USER BANK 3.
|
||||
};
|
||||
|
||||
|
||||
//---------------- BANK1 Register bits
|
||||
|
||||
// GYRO_CONFIG_STATIC2
|
||||
enum GYRO_CONFIG_STATIC2_BIT : uint8_t {
|
||||
GYRO_AAF_DIS = Bit1,
|
||||
GYRO_NF_DIS = Bit0,
|
||||
};
|
||||
|
||||
// GYRO_CONFIG_STATIC3
|
||||
enum GYRO_CONFIG_STATIC3_BIT : uint8_t {
|
||||
|
||||
// 585 Hz
|
||||
GYRO_AAF_DELT_SET = Bit3 | Bit2 | Bit0, //13
|
||||
GYRO_AAF_DELT_CLEAR = Bit5 | Bit4 | Bit1,
|
||||
|
||||
// 213 Hz
|
||||
// GYRO_AAF_DELT_SET = Bit2 | Bit0, //5
|
||||
// GYRO_AAF_DELT_CLEAR = Bit5 | Bit4 | Bit3 | Bit1,
|
||||
|
||||
// 126 Hz
|
||||
//GYRO_AAF_DELT_SET = Bit1 | Bit0, //3
|
||||
//GYRO_AAF_DELT_CLEAR = Bit5 | Bit4 | Bit3 | Bit2,
|
||||
|
||||
// 42 Hz
|
||||
// GYRO_AAF_DELT_SET = Bit0, //1
|
||||
// GYRO_AAF_DELT_CLEAR = Bit5 | Bit4 | Bit3 | Bit2 | Bit1,
|
||||
|
||||
};
|
||||
|
||||
// GYRO_CONFIG_STATIC4
|
||||
enum GYRO_CONFIG_STATIC4_BIT : uint8_t {
|
||||
|
||||
// 585 Hz
|
||||
GYRO_AAF_DELTSQR_LOW_SET = Bit7 | Bit5 | Bit3 | Bit1, //170
|
||||
GYRO_AAF_DELTSQR_LOW_CLEAR = Bit6 | Bit4 | Bit2 | Bit0,
|
||||
|
||||
// 213 Hz
|
||||
// GYRO_AAF_DELTSQR_LOW_SET = Bit4 | Bit3 | Bit0, //25
|
||||
// GYRO_AAF_DELTSQR_LOW_CLEAR = Bit7 | Bit6 | Bit5 | Bit2 | Bit1,
|
||||
|
||||
// 126 Hz
|
||||
//GYRO_AAF_DELTSQR_LOW_SET = Bit3 | Bit0, //9
|
||||
//GYRO_AAF_DELTSQR_LOW_CLEAR = Bit7 | Bit6 | Bit5 | Bit4 | Bit2 | Bit1,
|
||||
|
||||
// 42 Hz
|
||||
// GYRO_AAF_DELTSQR_LOW_SET = Bit0, //1
|
||||
// GYRO_AAF_DELTSQR_LOW_CLEAR = Bit7 | Bit6 | Bit5 | Bit4 | Bit3 | Bit2 | Bit1,
|
||||
};
|
||||
|
||||
// GYRO_CONFIG_STATIC5
|
||||
enum GYRO_CONFIG_STATIC5_BIT : uint8_t {
|
||||
|
||||
// 585 Hz
|
||||
GYRO_AAF_DELTSQR_HIGH_SET = 0,
|
||||
GYRO_AAF_DELTSQR_HIGH_CLEAR = Bit3 | Bit2 | Bit1 | Bit0,
|
||||
GYRO_AAF_BITSHIFT_SET = Bit7, // 8 << 4
|
||||
GYRO_AAF_BITSHIFT_CLEAR = Bit6 | Bit5 | Bit4,
|
||||
|
||||
// 213 Hz
|
||||
// GYRO_AAF_DELTSQR_HIGH_SET = 0,
|
||||
// GYRO_AAF_DELTSQR_HIGH_CLEAR = Bit3 | Bit2 | Bit1 | Bit0,
|
||||
// GYRO_AAF_BITSHIFT_SET = Bit7 | Bit5, //10
|
||||
// GYRO_AAF_BITSHIFT_CLEAR = Bit6 | Bit4,
|
||||
|
||||
// 126 Hz
|
||||
// GYRO_AAF_BITSHIFT_SET = Bit7 | Bit6, //12
|
||||
// GYRO_AAF_BITSHIFT_CLEAR = Bit5 | Bit4,
|
||||
|
||||
// 42 Hz
|
||||
// GYRO_AAF_BITSHIFT_SET = Bit7 | Bit6 | Bit5 | Bit4, //15
|
||||
// GYRO_AAF_BITSHIFT_CLEAR = 0,
|
||||
|
||||
|
||||
};
|
||||
|
||||
|
||||
//---------------- BANK2 Register bits
|
||||
|
||||
// ACCEL_CONFIG_STATIC2
|
||||
enum ACCEL_CONFIG_STATIC2_BIT : uint8_t {
|
||||
ACCEL_AAF_DIS = Bit0,
|
||||
ACCEL_AAF_DELT = Bit3 | Bit1,
|
||||
|
||||
// 213 Hz
|
||||
ACCEL_AAF_DELT_SET = Bit3 | Bit1, //5
|
||||
ACCEL_AAF_DELT_CLEAR = Bit6 | Bit5 | Bit4 | Bit2,
|
||||
|
||||
// 42 Hz
|
||||
// ACCEL_AAF_DELT_SET = Bit1, //1
|
||||
// ACCEL_AAF_DELT_CLEAR = Bit6 | Bit5 | Bit4 | Bit3 | Bit2,
|
||||
};
|
||||
|
||||
// ACCEL_CONFIG_STATIC3
|
||||
enum ACCEL_CONFIG_STATIC3_BIT : uint8_t {
|
||||
ACCEL_AAF_DELTSQR_LOW = Bit4 | Bit3 | Bit0,
|
||||
// 213 Hz
|
||||
ACCEL_AAF_DELTSQR_LOW_SET = Bit4 | Bit3 | Bit0, //25
|
||||
ACCEL_AAF_DELTSQR_LOW_CLEAR = Bit7 | Bit6 | Bit5 | Bit2 | Bit1,
|
||||
|
||||
// 42 Hz
|
||||
// ACCEL_AAF_DELTSQR_LOW_SET = Bit0, //1
|
||||
// ACCEL_AAF_DELTSQR_LOW_CLEAR = Bit7 | Bit6 | Bit5 | Bit4 | Bit3 | Bit2 | Bit1,
|
||||
|
||||
};
|
||||
|
||||
// ACCEL_CONFIG_STATIC4
|
||||
enum ACCEL_CONFIG_STATIC4_BIT : uint8_t {
|
||||
ACCEL_AAF_BITSHIFT = Bit7 | Bit5,
|
||||
ACCEL_AAF_DELTSQR_HIGH = 0,
|
||||
// 213 Hz
|
||||
ACCEL_AAF_BITSHIFT_SET = Bit7 | Bit5, //10
|
||||
ACCEL_AAF_BITSHIFT_CLEAR = Bit6 | Bit4,
|
||||
|
||||
// 42 Hz
|
||||
// ACCEL_AAF_BITSHIFT_SET = Bit7 | Bit6 | Bit5 | Bit4, //15
|
||||
// ACCEL_AAF_BITSHIFT_CLEAR = 0,
|
||||
|
||||
ACCEL_AAF_DELTSQR_HIGH_SET = 0,
|
||||
ACCEL_AAF_DELTSQR_HIGH_CLEAR = Bit3 | Bit2 | Bit1 | 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,
|
||||
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_ICM42688P
|
||||
|
|
@ -0,0 +1,116 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include "ICM42688P.hpp"
|
||||
|
||||
#include <px4_platform_common/getopt.h>
|
||||
#include <px4_platform_common/module.h>
|
||||
#include <string>
|
||||
|
||||
void ICM42688P::print_usage()
|
||||
{
|
||||
PRINT_MODULE_USAGE_NAME("icm42688p", "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_DEFAULT_COMMANDS();
|
||||
}
|
||||
|
||||
// I2CSPIDriverBase *ICM42688P::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
|
||||
// int runtime_instance)
|
||||
// {
|
||||
// ICM42688P *instance = new ICM42688P(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.rotation,
|
||||
// cli.bus_frequency, cli.spi_mode, iterator.DRDYGPIO());
|
||||
//
|
||||
// if (!instance) {
|
||||
// PX4_ERR("alloc failed");
|
||||
// return nullptr;
|
||||
// }
|
||||
//
|
||||
// if (OK != instance->init()) {
|
||||
// delete instance;
|
||||
// return nullptr;
|
||||
// }
|
||||
//
|
||||
// return instance;
|
||||
// }
|
||||
|
||||
extern "C" int icm42688p_main(int argc, char *argv[])
|
||||
{
|
||||
|
||||
for (int i = 0; i <= argc - 1; i++) {
|
||||
if (std::string(argv[i]) == "-h") {
|
||||
argv[i] = 0;
|
||||
hitl_mode = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
int ch;
|
||||
using ThisDriver = ICM42688P;
|
||||
BusCLIArguments cli{false, true};
|
||||
cli.default_spi_frequency = SPI_SPEED;
|
||||
|
||||
while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) {
|
||||
switch (ch) {
|
||||
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_ICM42688P);
|
||||
|
||||
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;
|
||||
}
|
||||
|
|
@ -1,34 +0,0 @@
|
|||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2024 ModalAI, Inc. 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_directories(${PX4_BOARD_DIR}/libfc-sensor-api/inc)
|
||||
|
|
@ -1,7 +1,7 @@
|
|||
|
||||
# Link against the public stub version of the proprietary fc sensor library
|
||||
target_link_libraries(px4 PRIVATE
|
||||
${PX4_BOARD_DIR}/libfc-sensor-api/build/libfc_sensor.so
|
||||
${PX4_SOURCE_DIR}/src//modules/muorb/apps/libfc-sensor-api/build/libfc_sensor.so
|
||||
px4_layer
|
||||
${module_libraries}
|
||||
)
|
||||
|
|
|
|||
|
|
@ -11,6 +11,7 @@ CONFIG_DRIVERS_VOXL2_IO=y
|
|||
CONFIG_MODULES_COMMANDER=y
|
||||
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
||||
CONFIG_MODULES_DATAMAN=y
|
||||
CONFIG_MODULES_LOAD_MON=y
|
||||
CONFIG_MODULES_LOGGER=y
|
||||
CONFIG_MODULES_MAVLINK=y
|
||||
CONFIG_MODULES_MUORB_APPS=y
|
||||
|
|
@ -25,4 +26,3 @@ CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
|
|||
CONFIG_SYSTEMCMDS_UORB=y
|
||||
CONFIG_SYSTEMCMDS_VER=y
|
||||
CONFIG_ORB_COMMUNICATOR=y
|
||||
CONFIG_PARAM_PRIMARY=y
|
||||
|
|
|
|||
|
|
@ -1,9 +1,10 @@
|
|||
#!/bin/bash
|
||||
|
||||
cd boards/modalai/voxl2/libfc-sensor-api
|
||||
cd src/modules/muorb/apps/libfc-sensor-api
|
||||
rm -fR build
|
||||
mkdir build
|
||||
cd build
|
||||
CC=/home/4.1.0.4/tools/linaro64/bin/aarch64-linux-gnu-gcc cmake ..
|
||||
make
|
||||
cd ../../../../..
|
||||
cd ../../../../../..
|
||||
|
||||
|
|
|
|||
|
|
@ -76,13 +76,12 @@ qshell flight_mode_manager start
|
|||
# Start all of the processing modules on the applications processor
|
||||
dataman start
|
||||
navigator start
|
||||
load_mon start
|
||||
|
||||
# Start microdds_client for ros2 offboard messages from agent over localhost
|
||||
microdds_client start -t udp -h 127.0.0.1 -p 8888
|
||||
|
||||
qshell pwm_out_sim start -m hil
|
||||
# g = gps, m = mag, o = odometry (vio), h = distance sensor, f = optic flow
|
||||
# qshell dsp_hitl start -g -m -o -h -f
|
||||
qshell dsp_hitl start -g -m
|
||||
|
||||
# start the onboard fast link to connect to voxl-mavlink-server
|
||||
|
|
|
|||
|
|
@ -207,6 +207,7 @@ qshell flight_mode_manager start
|
|||
# Start all of the processing modules on the applications processor
|
||||
dataman start
|
||||
navigator start
|
||||
load_mon start
|
||||
|
||||
# This bridge allows raw data packets to be sent over UART to the ESC
|
||||
voxl2_io_bridge start
|
||||
|
|
|
|||
|
|
@ -1,11 +1,9 @@
|
|||
CONFIG_BOARD_TOOLCHAIN="arm-none-eabi"
|
||||
CONFIG_BOARD_ARCHITECTURE="cortex-m7"
|
||||
CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS3"
|
||||
CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS4"
|
||||
CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS1"
|
||||
CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS2"
|
||||
CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS5"
|
||||
CONFIG_BOARD_SERIAL_TEL4="/dev/ttyS0"
|
||||
CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS2"
|
||||
CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS0"
|
||||
CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS1"
|
||||
CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS4"
|
||||
CONFIG_DRIVERS_ADC_ADS1115=y
|
||||
CONFIG_DRIVERS_ADC_BOARD_ADC=y
|
||||
CONFIG_DRIVERS_BAROMETER_DPS310=y
|
||||
|
|
@ -27,7 +25,6 @@ CONFIG_DRIVERS_PCA9685_PWM_OUT=y
|
|||
CONFIG_DRIVERS_POWER_MONITOR_INA226=y
|
||||
CONFIG_DRIVERS_PWM_OUT=y
|
||||
CONFIG_DRIVERS_RC_INPUT=y
|
||||
CONFIG_DRIVERS_SAFETY_BUTTON=y
|
||||
CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
|
||||
CONFIG_COMMON_TELEMETRY=y
|
||||
CONFIG_DRIVERS_TONE_ALARM=y
|
||||
|
|
@ -49,6 +46,7 @@ CONFIG_MODULES_FW_POS_CONTROL=y
|
|||
CONFIG_MODULES_FW_RATE_CONTROL=y
|
||||
CONFIG_MODULES_GIMBAL=y
|
||||
CONFIG_MODULES_GYRO_CALIBRATION=y
|
||||
CONFIG_MODULES_GYRO_FFT=y
|
||||
CONFIG_MODULES_LAND_DETECTOR=y
|
||||
CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
|
||||
CONFIG_MODULES_LOAD_MON=y
|
||||
|
|
@ -98,3 +96,4 @@ CONFIG_SYSTEMCMDS_UORB=y
|
|||
CONFIG_SYSTEMCMDS_USB_CONNECTED=y
|
||||
CONFIG_SYSTEMCMDS_VER=y
|
||||
CONFIG_SYSTEMCMDS_WORK_QUEUE=y
|
||||
CONFIG_EXAMPLES_FAKE_GPS=y
|
||||
|
|
|
|||
|
|
@ -6,4 +6,4 @@
|
|||
param set-default BAT1_V_DIV 10.1
|
||||
param set-default BAT1_A_PER_V 17
|
||||
|
||||
param set-default TEL_FRSKY_CONFIG 103
|
||||
safety_button start
|
||||
|
|
|
|||
|
|
@ -16,5 +16,3 @@ icm20948 -s -b 1 -R 8 -M start
|
|||
|
||||
# Interal DPS310 (barometer)
|
||||
dps310 -s -b 2 start
|
||||
|
||||
safety_button start
|
||||
|
|
|
|||
|
|
@ -15,7 +15,7 @@ CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/mro/ctrl-zero-classic/nuttx-con
|
|||
CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y
|
||||
CONFIG_ARCH_BOARD_CUSTOM_NAME="px4"
|
||||
CONFIG_ARCH_CHIP="stm32h7"
|
||||
CONFIG_ARCH_CHIP_STM32H743ZI=y
|
||||
CONFIG_ARCH_CHIP_STM32H743II=y
|
||||
CONFIG_ARCH_CHIP_STM32H7=y
|
||||
CONFIG_ARCH_INTERRUPTSTACK=768
|
||||
CONFIG_ARMV7M_BASEPRI_WAR=y
|
||||
|
|
|
|||
|
|
@ -222,9 +222,6 @@
|
|||
|
||||
|
||||
/* UART/USART */
|
||||
#define GPIO_USART1_TX GPIO_USART1_TX_3 /* PB6 */
|
||||
#define GPIO_USART1_RX GPIO_USART1_RX_3 /* PB7 */
|
||||
|
||||
#define GPIO_USART2_TX GPIO_USART2_TX_2 /* PD5 */
|
||||
#define GPIO_USART2_RX GPIO_USART2_RX_2 /* PD6 */
|
||||
#define GPIO_USART2_CTS GPIO_USART2_CTS_NSS_2 /* PD3 */
|
||||
|
|
@ -238,6 +235,9 @@
|
|||
#define GPIO_UART4_TX GPIO_UART4_TX_2 /* PA0 */
|
||||
#define GPIO_UART4_RX GPIO_UART4_RX_2 /* PA1 */
|
||||
|
||||
#define GPIO_USART6_TX 0 /* USART6 is RX-only */
|
||||
#define GPIO_USART6_RX GPIO_USART6_RX_1 /* PC7 */
|
||||
|
||||
#define GPIO_UART7_TX GPIO_UART7_TX_3 /* PE8 */
|
||||
#define GPIO_UART7_RX GPIO_UART7_RX_3 /* PE7 */
|
||||
|
||||
|
|
@ -268,14 +268,13 @@
|
|||
#define GPIO_SPI5_MISO GPIO_SPI5_MISO_1 /* PF8 */
|
||||
#define GPIO_SPI5_MOSI GPIO_SPI5_MOSI_2 /* PF9 */
|
||||
|
||||
#define GPIO_SPI6_SCK ADJ_SLEW_RATE(GPIO_SPI6_SCK_1) /* PG13 */
|
||||
#define GPIO_SPI6_MISO GPIO_SPI6_MISO_1 /* PG12 */
|
||||
#define GPIO_SPI6_MOSI GPIO_SPI6_MOSI_1 /* PG14 */
|
||||
|
||||
|
||||
/* I2C */
|
||||
#define GPIO_I2C1_SCL GPIO_I2C1_SCL_2 /* PB8 */
|
||||
#define GPIO_I2C1_SDA GPIO_I2C1_SDA_2 /* PB9 */
|
||||
|
||||
#define GPIO_I2C4_SCL GPIO_I2C4_SCL_2 /* PF14 */
|
||||
#define GPIO_I2C4_SDA GPIO_I2C4_SDA_2 /* PF15 */
|
||||
#define GPIO_I2C3_SCL GPIO_I2C3_SCL_2 /* PH7 */
|
||||
#define GPIO_I2C3_SDA GPIO_I2C3_SDA_2 /* PH8 */
|
||||
|
||||
#define GPIO_I2C4_SCL GPIO_I2C4_SCL_4 /* PB6 */
|
||||
#define GPIO_I2C4_SDA GPIO_I2C4_SDA_4 /* PB7 */
|
||||
|
|
|
|||
|
|
@ -56,7 +56,7 @@ CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/mro/ctrl-zero-classic/nuttx-con
|
|||
CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y
|
||||
CONFIG_ARCH_BOARD_CUSTOM_NAME="px4"
|
||||
CONFIG_ARCH_CHIP="stm32h7"
|
||||
CONFIG_ARCH_CHIP_STM32H743ZI=y
|
||||
CONFIG_ARCH_CHIP_STM32H743II=y
|
||||
CONFIG_ARCH_CHIP_STM32H7=y
|
||||
CONFIG_ARCH_INTERRUPTSTACK=768
|
||||
CONFIG_ARCH_STACKDUMP=y
|
||||
|
|
@ -192,6 +192,7 @@ CONFIG_STM32H7_DMA2=y
|
|||
CONFIG_STM32H7_DMACAPABLE=y
|
||||
CONFIG_STM32H7_FLOWCONTROL_BROKEN=y
|
||||
CONFIG_STM32H7_I2C1=y
|
||||
CONFIG_STM32H7_I2C3=y
|
||||
CONFIG_STM32H7_I2C4=y
|
||||
CONFIG_STM32H7_I2C_DYNTIMEO=y
|
||||
CONFIG_STM32H7_I2C_DYNTIMEO_STARTSTOP=10
|
||||
|
|
@ -211,20 +212,17 @@ CONFIG_STM32H7_SPI2=y
|
|||
CONFIG_STM32H7_SPI5=y
|
||||
CONFIG_STM32H7_SPI5_DMA=y
|
||||
CONFIG_STM32H7_SPI5_DMA_BUFFER=1024
|
||||
CONFIG_STM32H7_SPI6=y
|
||||
CONFIG_STM32H7_TIM15=y
|
||||
CONFIG_STM32H7_TIM16=y
|
||||
CONFIG_STM32H7_TIM1=y
|
||||
CONFIG_STM32H7_TIM2=y
|
||||
CONFIG_STM32H7_TIM3=y
|
||||
CONFIG_STM32H7_TIM4=y
|
||||
CONFIG_STM32H7_TIM8=y
|
||||
CONFIG_STM32H7_USART1=y
|
||||
CONFIG_STM32H7_USART2=y
|
||||
CONFIG_STM32H7_USART3=y
|
||||
CONFIG_STM32H7_UART4=y
|
||||
CONFIG_STM32H7_UART7=y
|
||||
CONFIG_STM32H7_UART8=y
|
||||
CONFIG_STM32H7_USART2=y
|
||||
CONFIG_STM32H7_USART3=y
|
||||
CONFIG_STM32H7_USART6=y
|
||||
CONFIG_STM32H7_USART_BREAKS=y
|
||||
CONFIG_STM32H7_USART_INVERT=y
|
||||
CONFIG_STM32H7_USART_SINGLEWIRE=y
|
||||
|
|
@ -254,6 +252,9 @@ CONFIG_USART3_IFLOWCONTROL=y
|
|||
CONFIG_USART3_OFLOWCONTROL=y
|
||||
CONFIG_USART3_RXBUFSIZE=600
|
||||
CONFIG_USART3_TXBUFSIZE=3000
|
||||
CONFIG_USART6_BAUD=57600
|
||||
CONFIG_USART6_RXBUFSIZE=600
|
||||
CONFIG_USART6_TXBUFSIZE=1500
|
||||
CONFIG_USBDEV=y
|
||||
CONFIG_USBDEV_BUSPOWERED=y
|
||||
CONFIG_USBDEV_MAXPOWER=500
|
||||
|
|
|
|||
|
|
@ -45,111 +45,95 @@
|
|||
#include <stm32_gpio.h>
|
||||
|
||||
/* LEDs are driven with push open drain to support Anode to 5V or 3.3V */
|
||||
#define GPIO_nLED_RED /* PB4 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN4)
|
||||
#define GPIO_nLED_GREEN /* PB5 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN5)
|
||||
#define GPIO_nLED_RED /* PB11 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN11)
|
||||
#define GPIO_nLED_GREEN /* PB1 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN1)
|
||||
#define GPIO_nLED_BLUE /* PB3 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN3)
|
||||
|
||||
#define GPIO_LED_SAFETY /* PE6 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN6)
|
||||
|
||||
#define BOARD_HAS_CONTROL_STATUS_LEDS 1
|
||||
#define BOARD_OVERLOAD_LED LED_RED
|
||||
#define BOARD_ARMED_STATE_LED LED_BLUE
|
||||
|
||||
/* ADC channels */
|
||||
#define PX4_ADC_GPIO \
|
||||
/* PC4 */ GPIO_ADC12_INP4, \
|
||||
/* PA2 */ GPIO_ADC12_INP14, \
|
||||
/* PA3 */ GPIO_ADC12_INP15, \
|
||||
/* PA4 */ GPIO_ADC12_INP18, \
|
||||
/* PC0 */ GPIO_ADC123_INP10, \
|
||||
/* PC5 */ GPIO_ADC12_INP8, \
|
||||
/* PB0 */ GPIO_ADC12_INP9, \
|
||||
/* PB1 */ GPIO_ADC12_INP5
|
||||
/* PC1 */ GPIO_ADC123_INP11
|
||||
|
||||
/* Define Channel numbers must match above GPIO pins */
|
||||
#define ADC_BATTERY_VOLTAGE_CHANNEL 4 /* PC4 BATT_VOLT_SENS */
|
||||
#define ADC_BATTERY_VOLTAGE_CHANNEL 14 /* PA2 BATT_VOLT_SENS */
|
||||
#define ADC_BATTERY_CURRENT_CHANNEL 15 /* PA3 BATT_CURRENT_SENS */
|
||||
#define ADC_SCALED_V5_CHANNEL 18 /* PA4 VDD_5V_SENS */
|
||||
#define ADC_RC_RSSI_CHANNEL 10 /* PC0 */
|
||||
#define ADC_AIRSPEED_VOLTAGE_CHANNEL 8 /* PC5 */
|
||||
#define ADC_AUX1_VOLTAGE_CHANNEL 9 /* PB0 */
|
||||
#define ADC_AUX2_VOLTAGE_CHANNEL 5 /* PB1 */
|
||||
#define ADC_RC_RSSI_CHANNEL 11 /* PC1 */
|
||||
|
||||
#define ADC_CHANNELS \
|
||||
((1 << ADC_BATTERY_VOLTAGE_CHANNEL) | \
|
||||
(1 << ADC_BATTERY_CURRENT_CHANNEL) | \
|
||||
(1 << ADC_SCALED_V5_CHANNEL) | \
|
||||
(1 << ADC_RC_RSSI_CHANNEL) | \
|
||||
(1 << ADC_AIRSPEED_VOLTAGE_CHANNEL) | \
|
||||
(1 << ADC_AUX1_VOLTAGE_CHANNEL) | \
|
||||
(1 << ADC_AUX2_VOLTAGE_CHANNEL))
|
||||
(1 << ADC_RC_RSSI_CHANNEL))
|
||||
|
||||
/* HW has to large of R termination on ADC todo:change when HW value is chosen */
|
||||
#define BOARD_ADC_OPEN_CIRCUIT_V (5.6f)
|
||||
|
||||
/* CAN Silence: Silent mode control */
|
||||
#define GPIO_CAN1_SILENT_S0 /* PF11 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN11)
|
||||
#define GPIO_CAN2_SILENT_S0 /* PF12 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN12)
|
||||
#define GPIO_CAN1_SILENT_S0 /* PF5 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN5)
|
||||
#define GPIO_CAN2_SILENT_S0 /* PF5 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN5)
|
||||
|
||||
/* PWM */
|
||||
#define DIRECT_PWM_OUTPUT_CHANNELS 12
|
||||
|
||||
/* Power supply control and monitoring GPIOs */
|
||||
#define GPIO_nPOWER_IN_A /* PC1 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN1)
|
||||
#define GPIO_nPOWER_IN_A /* PB5 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5)
|
||||
|
||||
#define GPIO_VDD_BRICK1_VALID GPIO_nPOWER_IN_A /* Brick 1 Is Chosen */
|
||||
#define BOARD_NUMBER_BRICKS 1
|
||||
|
||||
#define GPIO_VDD_3V3_SPEKTRUM_POWER_EN /* PE4 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN4)
|
||||
|
||||
#define GPIO_LEVEL_SHIFTER_OE /* PC13 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13)
|
||||
|
||||
/* Define True logic Power Control in arch agnostic form */
|
||||
#define VDD_3V3_SPEKTRUM_POWER_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_3V3_SPEKTRUM_POWER_EN, (!on_true))
|
||||
#define READ_VDD_3V3_SPEKTRUM_POWER_EN() (px4_arch_gpioread(GPIO_VDD_3V3_SPEKTRUM_POWER_EN) == 0)
|
||||
|
||||
/* Tone alarm output */
|
||||
#define TONE_ALARM_TIMER 16 /* timer 16 */
|
||||
#define TONE_ALARM_CHANNEL 1 /* PF6 TIM16_CH1 */
|
||||
#define TONE_ALARM_TIMER 2 /* timer 2 */
|
||||
#define TONE_ALARM_CHANNEL 1 /* PA15 TIM2_CH1 */
|
||||
|
||||
#define GPIO_BUZZER_1 /* PF6 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN6)
|
||||
#define GPIO_BUZZER_1 /* PA15 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN15)
|
||||
|
||||
#define GPIO_TONE_ALARM_IDLE GPIO_BUZZER_1
|
||||
#define GPIO_TONE_ALARM GPIO_TIM16_CH1OUT_2
|
||||
#define GPIO_TONE_ALARM GPIO_TIM2_CH1OUT_2
|
||||
|
||||
/* USB OTG FS */
|
||||
#define GPIO_OTGFS_VBUS /* PA9 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_100MHz|GPIO_PORTA|GPIO_PIN9)
|
||||
|
||||
/* High-resolution timer */
|
||||
#define HRT_TIMER 3 /* use timer3 for the HRT */
|
||||
#define HRT_TIMER_CHANNEL 2 /* use capture/compare channel 1 */
|
||||
#define HRT_TIMER_CHANNEL 1 /* use capture/compare channel 1 */
|
||||
|
||||
#define HRT_PPM_CHANNEL /* T3C2 */ 2 /* use capture/compare channel 2 */
|
||||
#define GPIO_PPM_IN /* PC7 T3C2 */ GPIO_TIM3_CH2IN_3
|
||||
#define HRT_PPM_CHANNEL /* T3C3 */ 3 /* use capture/compare channel 3 */
|
||||
#define GPIO_PPM_IN /* PB0 T3C3 */ GPIO_TIM3_CH3IN_1
|
||||
|
||||
/* RC Serial port */
|
||||
#define RC_SERIAL_PORT "/dev/ttyS5"
|
||||
#define RC_SERIAL_PORT "/dev/ttyS3"
|
||||
|
||||
#define GPIO_RSSI_IN /* PC0 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN0)
|
||||
#define GPIO_RSSI_IN /* PC1 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN1)
|
||||
|
||||
/* Safety Switch: Enable the FMU to control it if there is no px4io fixme:This should be BOARD_SAFETY_LED(__ontrue) */
|
||||
#define GPIO_SAFETY_SWITCH_IN /* PA10 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN10)
|
||||
/* Enable the FMU to use the switch it if there is no px4io fixme:This should be BOARD_SAFETY_BUTTON() */
|
||||
#define GPIO_SAFETY_SWITCH_IN /* PC4 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN4)
|
||||
/* Enable the FMU to use the switch it if there is no px4io fixme:This should be BOARD_SAFTY_BUTTON() */
|
||||
#define GPIO_BTN_SAFETY GPIO_SAFETY_SWITCH_IN /* Enable the FMU to control it if there is no px4io */
|
||||
|
||||
/* No PX4IO processor present */
|
||||
#define PX4_MFT_HW_SUPPORTED_PX4_MFT_PX4IO 0
|
||||
|
||||
/* Power switch controls ******************************************************/
|
||||
#define SPEKTRUM_POWER(_on_true) VDD_3V3_SPEKTRUM_POWER_EN(_on_true)
|
||||
|
||||
/*
|
||||
* Board has a separate RC_IN
|
||||
*
|
||||
* GPIO PPM_IN on PC7 T3CH2
|
||||
* GPIO PPM_IN on PB0 T3CH3
|
||||
* SPEKTRUM_RX (it's TX or RX in Bind) on UART6 PC7
|
||||
* Inversion is possible in the UART and can drive GPIO_PPM_IN as an output
|
||||
*/
|
||||
#define GPIO_PPM_IN_AS_OUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN7)
|
||||
#define GPIO_PPM_IN_AS_OUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN0)
|
||||
#define SPEKTRUM_RX_AS_GPIO_OUTPUT() px4_arch_configgpio(GPIO_PPM_IN_AS_OUT)
|
||||
#define SPEKTRUM_RX_AS_UART() /* Can be left as uart */
|
||||
#define SPEKTRUM_OUT(_one_true) px4_arch_gpiowrite(GPIO_PPM_IN_AS_OUT, (_one_true))
|
||||
|
|
@ -172,7 +156,7 @@
|
|||
#define BOARD_HAS_STATIC_MANIFEST 1
|
||||
|
||||
|
||||
#define BOARD_NUM_IO_TIMERS 6
|
||||
#define BOARD_NUM_IO_TIMERS 5
|
||||
|
||||
|
||||
#define BOARD_ENABLE_CONSOLE_BUFFER
|
||||
|
|
@ -187,12 +171,9 @@
|
|||
GPIO_CAN2_SILENT_S0, \
|
||||
GPIO_nPOWER_IN_A, \
|
||||
GPIO_VDD_3V3_SPEKTRUM_POWER_EN, \
|
||||
GPIO_LEVEL_SHIFTER_OE, \
|
||||
GPIO_TONE_ALARM_IDLE, \
|
||||
GPIO_SAFETY_SWITCH_IN, \
|
||||
GPIO_OTGFS_VBUS, \
|
||||
GPIO_BTN_SAFETY, \
|
||||
GPIO_LED_SAFETY, \
|
||||
}
|
||||
|
||||
__BEGIN_DECLS
|
||||
|
|
|
|||
|
|
@ -35,5 +35,6 @@
|
|||
|
||||
constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = {
|
||||
initI2CBusExternal(1),
|
||||
initI2CBusExternal(3),
|
||||
initI2CBusExternal(4),
|
||||
};
|
||||
|
|
|
|||
|
|
@ -37,7 +37,7 @@
|
|||
|
||||
constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = {
|
||||
initSPIBus(SPI::Bus::SPI1, {
|
||||
initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortG, GPIO::Pin3}, SPI::DRDY{GPIO::PortG, GPIO::Pin2}),
|
||||
initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortC, GPIO::Pin2}, SPI::DRDY{GPIO::PortD, GPIO::Pin15}),
|
||||
initSPIDevice(DRV_IMU_DEVTYPE_ICM20948, SPI::CS{GPIO::PortE, GPIO::Pin15}, SPI::DRDY{GPIO::PortE, GPIO::Pin12}),
|
||||
}, {GPIO::PortE, GPIO::Pin3}),
|
||||
initSPIBus(SPI::Bus::SPI2, {
|
||||
|
|
@ -46,10 +46,7 @@ constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = {
|
|||
}),
|
||||
initSPIBus(SPI::Bus::SPI5, {
|
||||
initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortF, GPIO::Pin10}, SPI::DRDY{GPIO::PortF, GPIO::Pin3}),
|
||||
initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortF, GPIO::Pin0}, SPI::DRDY{GPIO::PortF, GPIO::Pin1}),
|
||||
}),
|
||||
initSPIBusExternal(SPI::Bus::SPI6, {
|
||||
initSPIConfigExternal(SPI::CS{GPIO::PortG, GPIO::Pin9}),
|
||||
initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortF, GPIO::Pin6}, SPI::DRDY{GPIO::PortF, GPIO::Pin1}),
|
||||
}),
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -33,28 +33,6 @@
|
|||
|
||||
#include <px4_arch/io_timer_hw_description.h>
|
||||
|
||||
/* Timer allocation
|
||||
*
|
||||
* TIM1_CH4 T FMU_CH1
|
||||
* TIM1_CH3 T FMU_CH2
|
||||
* TIM1_CH2 T FMU_CH3
|
||||
* TIM1_CH1 T FMU_CH4
|
||||
*
|
||||
* TIM4_CH2 T FMU_CH5
|
||||
* TIM4_CH3 T FMU_CH6
|
||||
* TIM2_CH3 T FMU_CH7
|
||||
* TIM2_CH1 T FMU_CH8
|
||||
*
|
||||
* TIM2_CH4 T FMU_CH9
|
||||
* TIM15_CH1 T FMU_CH10
|
||||
*
|
||||
* TIM8_CH1 T FMU_CH11
|
||||
*
|
||||
* TIM4_CH4 T FMU_CH12
|
||||
*
|
||||
* TIM16_CH1 T BUZZER - Driven by other driver
|
||||
*/
|
||||
|
||||
constexpr io_timers_t io_timers[MAX_IO_TIMERS] = {
|
||||
initIOTimer(Timer::Timer1, DMA{DMA::Index1}),
|
||||
initIOTimer(Timer::Timer4, DMA{DMA::Index1}),
|
||||
|
|
|
|||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
|
@ -18,4 +18,4 @@ CONFIG_MODULES_UUV_POS_CONTROL=n
|
|||
CONFIG_MODULES_VTOL_ATT_CONTROL=n
|
||||
CONFIG_EKF2_AUX_GLOBAL_POSITION=y
|
||||
# CONFIG_EKF2_WIND is not set
|
||||
CONFIG_MODULES_DIFFERENTIAL_DRIVE=y
|
||||
CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y
|
||||
|
|
|
|||
|
|
@ -81,6 +81,7 @@ CONFIG_MODULES_NAVIGATOR=y
|
|||
CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF=y
|
||||
CONFIG_MODULES_PAYLOAD_DELIVERER=y
|
||||
CONFIG_MODULES_RC_UPDATE=y
|
||||
CONFIG_MODULES_ROVER_POS_CONTROL=y
|
||||
CONFIG_MODULES_SENSORS=y
|
||||
CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
|
||||
CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
|
||||
|
|
@ -101,7 +102,9 @@ CONFIG_SYSTEMCMDS_NSHTERM=y
|
|||
CONFIG_SYSTEMCMDS_PARAM=y
|
||||
CONFIG_SYSTEMCMDS_PERF=y
|
||||
CONFIG_SYSTEMCMDS_REBOOT=y
|
||||
CONFIG_SYSTEMCMDS_REFLECT=y
|
||||
CONFIG_SYSTEMCMDS_SD_BENCH=y
|
||||
CONFIG_SYSTEMCMDS_SD_STRESS=y
|
||||
CONFIG_SYSTEMCMDS_SYSTEM_TIME=y
|
||||
CONFIG_SYSTEMCMDS_TOP=y
|
||||
CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
|
||||
|
|
|
|||
Binary file not shown.
|
|
@ -16,7 +16,7 @@ param set-default SENS_EN_INA238 0
|
|||
param set-default SENS_EN_INA228 0
|
||||
param set-default SENS_EN_INA226 1
|
||||
|
||||
if ver hwbasecmp 008 009 00a 010 011
|
||||
if ver hwbasecmp 008 009 00a 010
|
||||
then
|
||||
# Skynode: use the "custom participant" config for uxrce_dds_client
|
||||
param set-default UXRCE_DDS_PTCFG 2
|
||||
|
|
|
|||
|
|
@ -3,7 +3,7 @@
|
|||
# board specific MAVLink startup script.
|
||||
#------------------------------------------------------------------------------
|
||||
|
||||
if ver hwbasecmp 008 009 00a 010 011
|
||||
if ver hwbasecmp 008 009 00a 010
|
||||
then
|
||||
# Start MAVLink on the UART connected to the mission computer
|
||||
mavlink start -d /dev/ttyS4 -b 3000000 -r 290000 -m onboard_low_bandwidth -x -z
|
||||
|
|
|
|||
|
|
@ -49,7 +49,7 @@ then
|
|||
fi
|
||||
fi
|
||||
|
||||
if ver hwbasecmp 008 009 00a 010 011
|
||||
if ver hwbasecmp 008 009 00a 010
|
||||
then
|
||||
#SKYNODE base fmu board orientation
|
||||
|
||||
|
|
|
|||
Binary file not shown.
|
|
@ -13,4 +13,4 @@ CONFIG_MODULES_MC_RATE_CONTROL=n
|
|||
CONFIG_MODULES_VTOL_ATT_CONTROL=n
|
||||
CONFIG_EKF2_AUX_GLOBAL_POSITION=y
|
||||
# CONFIG_EKF2_WIND is not set
|
||||
CONFIG_MODULES_DIFFERENTIAL_DRIVE=y
|
||||
CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y
|
||||
|
|
|
|||
|
|
@ -13,4 +13,4 @@ CONFIG_MODULES_MC_RATE_CONTROL=n
|
|||
CONFIG_MODULES_VTOL_ATT_CONTROL=n
|
||||
CONFIG_EKF2_AUX_GLOBAL_POSITION=y
|
||||
# CONFIG_EKF2_WIND is not set
|
||||
CONFIG_MODULES_DIFFERENTIAL_DRIVE=y
|
||||
CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y
|
||||
|
|
|
|||
Binary file not shown.
|
|
@ -18,7 +18,7 @@ param set-default SENS_EN_INA238 0
|
|||
param set-default SENS_EN_INA228 0
|
||||
param set-default SENS_EN_INA226 0
|
||||
|
||||
if ver hwbasecmp 009 010 011
|
||||
if ver hwbasecmp 009 010
|
||||
then
|
||||
# Skynode: use the "custom participant" config for uxrce_dds_client
|
||||
param set-default UXRCE_DDS_PTCFG 2
|
||||
|
|
|
|||
|
|
@ -4,7 +4,7 @@
|
|||
#------------------------------------------------------------------------------
|
||||
|
||||
# if skynode base board is detected start Mavlink on Telem2
|
||||
if ver hwbasecmp 009 010 011
|
||||
if ver hwbasecmp 009 010
|
||||
then
|
||||
mavlink start -d /dev/ttyS4 -b 3000000 -r 290000 -m onboard_low_bandwidth -x -z
|
||||
|
||||
|
|
|
|||
|
|
@ -56,7 +56,7 @@ then
|
|||
fi
|
||||
|
||||
#Start Auterion Power Module selector for Skynode boards
|
||||
if ver hwbasecmp 009 010 011
|
||||
if ver hwbasecmp 009 010
|
||||
then
|
||||
pm_selector_auterion start
|
||||
else
|
||||
|
|
@ -93,7 +93,7 @@ else
|
|||
icm20649 -s -R 6 start
|
||||
else
|
||||
# Internal SPI BMI088
|
||||
if ver hwbasecmp 009 010 011
|
||||
if ver hwbasecmp 009 010
|
||||
then
|
||||
bmi088 -A -R 6 -s start
|
||||
bmi088 -G -R 6 -s start
|
||||
|
|
@ -110,7 +110,7 @@ else
|
|||
fi
|
||||
|
||||
# Internal SPI bus ICM42688p
|
||||
if ver hwbasecmp 009 010 011
|
||||
if ver hwbasecmp 009 010
|
||||
then
|
||||
icm42688p -R 12 -s start
|
||||
else
|
||||
|
|
@ -127,7 +127,7 @@ else
|
|||
# Internal SPI bus ICM-42670-P (hard-mounted)
|
||||
icm42670p -R 10 -s start
|
||||
else
|
||||
if ver hwbasecmp 009 010 011
|
||||
if ver hwbasecmp 009 010
|
||||
then
|
||||
icm20602 -R 6 -s start
|
||||
else
|
||||
|
|
|
|||
|
|
@ -13,4 +13,4 @@ CONFIG_MODULES_MC_RATE_CONTROL=n
|
|||
CONFIG_MODULES_VTOL_ATT_CONTROL=n
|
||||
CONFIG_EKF2_AUX_GLOBAL_POSITION=y
|
||||
# CONFIG_EKF2_WIND is not set
|
||||
CONFIG_MODULES_DIFFERENTIAL_DRIVE=y
|
||||
CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y
|
||||
|
|
|
|||
|
|
@ -6,30 +6,21 @@ CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS3"
|
|||
CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS2"
|
||||
CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS5"
|
||||
CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS6"
|
||||
CONFIG_DRIVERS_ADC_ADS1115=y
|
||||
CONFIG_DRIVERS_ADC_BOARD_ADC=y
|
||||
CONFIG_DRIVERS_BAROMETER_BMP388=y
|
||||
CONFIG_DRIVERS_BAROMETER_INVENSENSE_ICP201XX=y
|
||||
CONFIG_DRIVERS_BAROMETER_MS5611=y
|
||||
CONFIG_DRIVERS_CAMERA_CAPTURE=y
|
||||
CONFIG_DRIVERS_CAMERA_TRIGGER=y
|
||||
CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
|
||||
CONFIG_COMMON_DISTANCE_SENSOR=y
|
||||
CONFIG_DRIVERS_DISTANCE_SENSOR_LIGHTWARE_LASER_I2C=y
|
||||
CONFIG_DRIVERS_DSHOT=y
|
||||
CONFIG_DRIVERS_GPS=y
|
||||
CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16470=y
|
||||
CONFIG_DRIVERS_IMU_BOSCH_BMI088=y
|
||||
CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y
|
||||
CONFIG_DRIVERS_IMU_INVENSENSE_ICM20649=y
|
||||
CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y
|
||||
CONFIG_DRIVERS_IMU_INVENSENSE_ICM42670P=y
|
||||
CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
|
||||
CONFIG_DRIVERS_IMU_INVENSENSE_ICM45686=y
|
||||
CONFIG_DRIVERS_IMU_INVENSENSE_IIM42652=y
|
||||
CONFIG_COMMON_INS=y
|
||||
CONFIG_COMMON_LIGHT=y
|
||||
CONFIG_COMMON_MAGNETOMETER=y
|
||||
CONFIG_DRIVERS_OSD_MSP_OSD=y
|
||||
CONFIG_DRIVERS_MAGNETOMETER_BOSCH_BMM150=y
|
||||
CONFIG_DRIVERS_MAGNETOMETER_HMC5883=y
|
||||
CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y
|
||||
CONFIG_DRIVERS_POWER_MONITOR_INA226=y
|
||||
CONFIG_DRIVERS_POWER_MONITOR_INA228=y
|
||||
CONFIG_DRIVERS_POWER_MONITOR_INA238=y
|
||||
|
|
@ -41,20 +32,14 @@ CONFIG_COMMON_TELEMETRY=y
|
|||
CONFIG_DRIVERS_TONE_ALARM=y
|
||||
CONFIG_DRIVERS_UAVCAN=y
|
||||
CONFIG_COMMON_UWB=y
|
||||
CONFIG_MODULES_AIRSPEED_SELECTOR=y
|
||||
CONFIG_MODULES_BATTERY_STATUS=y
|
||||
CONFIG_MODULES_CAMERA_FEEDBACK=y
|
||||
CONFIG_MODULES_COMMANDER=y
|
||||
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
||||
CONFIG_MODULES_DATAMAN=y
|
||||
CONFIG_MODULES_EKF2=y
|
||||
CONFIG_MODULES_ESC_BATTERY=y
|
||||
CONFIG_MODULES_EVENTS=y
|
||||
CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
|
||||
CONFIG_MODULES_FW_ATT_CONTROL=y
|
||||
CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
|
||||
CONFIG_MODULES_FW_POS_CONTROL=y
|
||||
CONFIG_MODULES_FW_RATE_CONTROL=y
|
||||
CONFIG_MODULES_GIMBAL=y
|
||||
CONFIG_MODULES_GYRO_CALIBRATION=y
|
||||
CONFIG_MODULES_GYRO_FFT=y
|
||||
|
|
@ -72,15 +57,11 @@ CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
|
|||
CONFIG_MODULES_MC_POS_CONTROL=y
|
||||
CONFIG_MODULES_MC_RATE_CONTROL=y
|
||||
CONFIG_MODULES_NAVIGATOR=y
|
||||
CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF=y
|
||||
CONFIG_MODULES_RC_UPDATE=y
|
||||
CONFIG_MODULES_ROVER_POS_CONTROL=y
|
||||
CONFIG_MODULES_SENSORS=y
|
||||
CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
|
||||
CONFIG_MODULES_UXRCE_DDS_CLIENT=y
|
||||
CONFIG_MODULES_VTOL_ATT_CONTROL=y
|
||||
CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
|
||||
CONFIG_SYSTEMCMDS_BSONDUMP=y
|
||||
CONFIG_SYSTEMCMDS_DMESG=y
|
||||
CONFIG_SYSTEMCMDS_DUMPFILE=y
|
||||
CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
|
||||
|
|
|
|||
|
|
@ -4,7 +4,7 @@
|
|||
#------------------------------------------------------------------------------
|
||||
|
||||
# if skynode base board is detected start Mavlink on Telem2
|
||||
if ver hwbasecmp 009 010 011
|
||||
if ver hwbasecmp 009 010
|
||||
then
|
||||
mavlink start -d /dev/ttyS5 -b 3000000 -r 290000 -m onboard_low_bandwidth -x -z
|
||||
|
||||
|
|
|
|||
|
|
@ -257,7 +257,7 @@ CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
|
|||
CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
|
||||
CONFIG_SCHED_LPWORK=y
|
||||
CONFIG_SCHED_LPWORKPRIORITY=50
|
||||
CONFIG_SCHED_LPWORKSTACKSIZE=2032
|
||||
CONFIG_SCHED_LPWORKSTACKSIZE=1632
|
||||
CONFIG_SCHED_WAITPID=y
|
||||
CONFIG_SDIO_BLOCKSETUP=y
|
||||
CONFIG_SEM_PREALLOCHOLDERS=32
|
||||
|
|
|
|||
|
|
@ -1,9 +1,4 @@
|
|||
CONFIG_MODULES_AIRSPEED_SELECTOR=n
|
||||
CONFIG_MODULES_FLIGHT_MODE_MANAGER=n
|
||||
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
|
||||
CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=n
|
||||
CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n
|
||||
CONFIG_MODULES_MC_ATT_CONTROL=n
|
||||
|
|
@ -11,7 +6,6 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n
|
|||
CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=n
|
||||
CONFIG_MODULES_MC_POS_CONTROL=n
|
||||
CONFIG_MODULES_MC_RATE_CONTROL=n
|
||||
CONFIG_MODULES_VTOL_ATT_CONTROL=n
|
||||
CONFIG_EKF2_AUX_GLOBAL_POSITION=y
|
||||
# CONFIG_EKF2_WIND is not set
|
||||
CONFIG_MODULES_DIFFERENTIAL_DRIVE=y
|
||||
CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y
|
||||
|
|
|
|||
|
|
@ -58,7 +58,6 @@ set(msg_files
|
|||
CameraCapture.msg
|
||||
CameraStatus.msg
|
||||
CameraTrigger.msg
|
||||
CanInterfaceStatus.msg
|
||||
CellularStatus.msg
|
||||
CollisionConstraints.msg
|
||||
CollisionReport.msg
|
||||
|
|
@ -100,7 +99,6 @@ set(msg_files
|
|||
FollowTargetStatus.msg
|
||||
GeneratorStatus.msg
|
||||
GeofenceResult.msg
|
||||
GeofenceStatus.msg
|
||||
GimbalControls.msg
|
||||
GimbalDeviceAttitudeStatus.msg
|
||||
GimbalDeviceInformation.msg
|
||||
|
|
@ -155,10 +153,6 @@ set(msg_files
|
|||
OrbTest.msg
|
||||
OrbTestLarge.msg
|
||||
OrbTestMedium.msg
|
||||
ParameterResetRequest.msg
|
||||
ParameterSetUsedRequest.msg
|
||||
ParameterSetValueRequest.msg
|
||||
ParameterSetValueResponse.msg
|
||||
ParameterUpdate.msg
|
||||
Ping.msg
|
||||
PositionControllerLandingStatus.msg
|
||||
|
|
@ -179,7 +173,6 @@ set(msg_files
|
|||
RegisterExtComponentReply.msg
|
||||
RegisterExtComponentRequest.msg
|
||||
Rpm.msg
|
||||
RtlStatus.msg
|
||||
RtlTimeEstimate.msg
|
||||
SatelliteInfo.msg
|
||||
SensorAccel.msg
|
||||
|
|
@ -290,9 +283,6 @@ foreach(msg_file ${msg_files})
|
|||
list(APPEND uorb_json_files ${msg_source_out_path}/${msg}.json)
|
||||
endforeach()
|
||||
|
||||
# set parent scope msg_files for ROS
|
||||
set(msg_files ${msg_files} PARENT_SCOPE)
|
||||
|
||||
# Generate uORB headers
|
||||
add_custom_command(
|
||||
OUTPUT
|
||||
|
|
|
|||
|
|
@ -1,6 +0,0 @@
|
|||
uint64 timestamp # time since system start (microseconds)
|
||||
uint8 interface
|
||||
|
||||
uint64 io_errors
|
||||
uint64 frames_tx
|
||||
uint64 frames_rx
|
||||
|
|
@ -55,9 +55,15 @@ bool fs_bad_airspeed # 5 - true if fusion of the airspeed has encounte
|
|||
bool fs_bad_sideslip # 6 - true if fusion of the synthetic sideslip constraint has encountered a numerical error
|
||||
bool fs_bad_optflow_x # 7 - true if fusion of the optical flow X axis has encountered a numerical error
|
||||
bool fs_bad_optflow_y # 8 - true if fusion of the optical flow Y axis has encountered a numerical error
|
||||
bool fs_bad_acc_bias # 9 - true if bad delta velocity bias estimates have been detected
|
||||
bool fs_bad_acc_vertical # 10 - true if bad vertical accelerometer data has been detected
|
||||
bool fs_bad_acc_clipping # 11 - true if delta velocity data contains clipping (asymmetric railing)
|
||||
bool fs_bad_vel_n # 9 - true if fusion of the North velocity has encountered a numerical error
|
||||
bool fs_bad_vel_e # 10 - true if fusion of the East velocity has encountered a numerical error
|
||||
bool fs_bad_vel_d # 11 - true if fusion of the Down velocity has encountered a numerical error
|
||||
bool fs_bad_pos_n # 12 - true if fusion of the North position has encountered a numerical error
|
||||
bool fs_bad_pos_e # 13 - true if fusion of the East position has encountered a numerical error
|
||||
bool fs_bad_pos_d # 14 - true if fusion of the Down position has encountered a numerical error
|
||||
bool fs_bad_acc_bias # 15 - true if bad delta velocity bias estimates have been detected
|
||||
bool fs_bad_acc_vertical # 16 - true if bad vertical accelerometer data has been detected
|
||||
bool fs_bad_acc_clipping # 17 - true if delta velocity data contains clipping (asymmetric railing)
|
||||
|
||||
|
||||
# innovation test failures
|
||||
|
|
|
|||
|
|
@ -1,7 +0,0 @@
|
|||
uint64 timestamp # time since system start (microseconds)
|
||||
|
||||
uint32 geofence_id # loaded geofence id
|
||||
uint8 status # Current geofence status
|
||||
|
||||
uint8 GF_STATUS_LOADING = 0
|
||||
uint8 GF_STATUS_READY = 1
|
||||
|
|
@ -1,8 +1,8 @@
|
|||
uint64 timestamp # time since system start (microseconds)
|
||||
|
||||
uint32 mission_id # Id for the mission for which the result was generated
|
||||
uint32 geofence_id # Id for the corresponding geofence for which the result was generated (used for mission feasibility)
|
||||
uint32 home_position_counter # Counter of the home position for which the result was generated (used for mission feasibility)
|
||||
uint16 mission_id # Id for the mission for which the result was generated
|
||||
uint16 geofence_id # Id for the corresponding geofence for which the result was generated (used for mission feasibility)
|
||||
uint64 home_position_counter # Counter of the home position for which the result was generated (used for mission feasibility)
|
||||
|
||||
int32 seq_reached # Sequence of the mission item which has been reached, default -1
|
||||
uint16 seq_current # Sequence of the current mission item
|
||||
|
|
|
|||
|
|
@ -4,6 +4,4 @@ int32 val
|
|||
|
||||
uint8[64] junk
|
||||
|
||||
uint8 ORB_QUEUE_LENGTH = 16
|
||||
|
||||
# TOPICS orb_test_medium orb_test_medium_multi orb_test_medium_wrap_around orb_test_medium_queue orb_test_medium_queue_poll
|
||||
|
|
|
|||
|
|
@ -1,8 +0,0 @@
|
|||
# ParameterResetRequest : Used by the primary to reset one or all parameter value(s) on the remote
|
||||
|
||||
uint64 timestamp
|
||||
uint16 parameter_index
|
||||
|
||||
bool reset_all # If this is true then ignore parameter_index
|
||||
|
||||
uint8 ORB_QUEUE_LENGTH = 4
|
||||
|
|
@ -1,6 +0,0 @@
|
|||
# ParameterSetUsedRequest : Used by a remote to update the used flag for a parameter on the primary
|
||||
|
||||
uint64 timestamp
|
||||
uint16 parameter_index
|
||||
|
||||
uint8 ORB_QUEUE_LENGTH = 64
|
||||
|
|
@ -1,11 +0,0 @@
|
|||
# ParameterSetValueRequest : Used by a remote or primary to update the value for a parameter at the other end
|
||||
|
||||
uint64 timestamp
|
||||
uint16 parameter_index
|
||||
|
||||
int32 int_value # Optional value for an integer parameter
|
||||
float32 float_value # Optional value for a float parameter
|
||||
|
||||
uint8 ORB_QUEUE_LENGTH = 32
|
||||
|
||||
# TOPICS parameter_set_value_request parameter_remote_set_value_request parameter_primary_set_value_request
|
||||
|
|
@ -1,9 +0,0 @@
|
|||
# ParameterSetValueResponse : Response to a set value request by either primary or secondary
|
||||
|
||||
uint64 timestamp
|
||||
uint64 request_timestamp
|
||||
uint16 parameter_index
|
||||
|
||||
uint8 ORB_QUEUE_LENGTH = 4
|
||||
|
||||
# TOPICS parameter_set_value_response parameter_remote_set_value_response parameter_primary_set_value_response
|
||||
|
|
@ -1,15 +0,0 @@
|
|||
uint64 timestamp # time since system start (microseconds)
|
||||
|
||||
uint32 safe_points_id # unique ID of active set of safe_point_items
|
||||
bool is_evaluation_pending # flag if the RTL point needs reevaluation (e.g. new safe points available, but need loading).
|
||||
|
||||
bool has_vtol_approach # flag if approaches are defined for current RTL_TYPE parameter setting
|
||||
|
||||
uint8 rtl_type # Type of RTL chosen
|
||||
uint8 safe_point_index # index of the chosen safe point, if in RTL_STATUS_TYPE_DIRECT_SAFE_POINT mode
|
||||
|
||||
uint8 RTL_STATUS_TYPE_NONE=0 # RTL type is pending if evaluation can't pe performed currently e.g. when it is still loading the safe points
|
||||
uint8 RTL_STATUS_TYPE_DIRECT_SAFE_POINT=1 # RTL type is chosen to directly go to a safe point or home position
|
||||
uint8 RTL_STATUS_TYPE_DIRECT_MISSION_LAND=2 # RTL type is going straight to the beginning of the mission landing
|
||||
uint8 RTL_STATUS_TYPE_FOLLOW_MISSION=3 # RTL type is following the mission from closest point to mission landing
|
||||
uint8 RTL_STATUS_TYPE_FOLLOW_MISSION_REVERSE=4 # RTL type is following the mission in reverse to the start position
|
||||
|
|
@ -47,4 +47,4 @@ uint16 ADSB_EMITTER_TYPE_SERVICE_SURFACE=18
|
|||
uint16 ADSB_EMITTER_TYPE_POINT_OBSTACLE=19
|
||||
uint16 ADSB_EMITTER_TYPE_ENUM_END=20
|
||||
|
||||
uint8 ORB_QUEUE_LENGTH = 16
|
||||
uint8 ORB_QUEUE_LENGTH = 8
|
||||
|
|
|
|||
|
|
@ -52,7 +52,6 @@ add_library(px4_platform STATIC
|
|||
shutdown.cpp
|
||||
spi.cpp
|
||||
pab_manifest.c
|
||||
Serial.cpp
|
||||
${SRCS}
|
||||
)
|
||||
target_link_libraries(px4_platform prebuild_targets px4_work_queue)
|
||||
|
|
|
|||
|
|
@ -1,138 +0,0 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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 <px4_platform_common/Serial.hpp>
|
||||
|
||||
namespace device
|
||||
{
|
||||
|
||||
Serial::Serial(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits,
|
||||
FlowControl flowcontrol) :
|
||||
_impl(port, baudrate, bytesize, parity, stopbits, flowcontrol)
|
||||
{
|
||||
// If no baudrate was specified then set it to a reasonable default value
|
||||
if (baudrate == 0) {
|
||||
(void) _impl.setBaudrate(9600);
|
||||
}
|
||||
}
|
||||
|
||||
Serial::~Serial()
|
||||
{
|
||||
}
|
||||
|
||||
bool Serial::open()
|
||||
{
|
||||
return _impl.open();
|
||||
}
|
||||
|
||||
bool Serial::isOpen() const
|
||||
{
|
||||
return _impl.isOpen();
|
||||
}
|
||||
|
||||
bool Serial::close()
|
||||
{
|
||||
return _impl.close();
|
||||
}
|
||||
|
||||
ssize_t Serial::read(uint8_t *buffer, size_t buffer_size)
|
||||
{
|
||||
return _impl.read(buffer, buffer_size);
|
||||
}
|
||||
|
||||
ssize_t Serial::readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count, uint32_t timeout_us)
|
||||
{
|
||||
return _impl.readAtLeast(buffer, buffer_size, character_count, timeout_us);
|
||||
}
|
||||
|
||||
ssize_t Serial::write(const void *buffer, size_t buffer_size)
|
||||
{
|
||||
return _impl.write(buffer, buffer_size);
|
||||
}
|
||||
|
||||
uint32_t Serial::getBaudrate() const
|
||||
{
|
||||
return _impl.getBaudrate();
|
||||
}
|
||||
|
||||
bool Serial::setBaudrate(uint32_t baudrate)
|
||||
{
|
||||
return _impl.setBaudrate(baudrate);
|
||||
}
|
||||
|
||||
ByteSize Serial::getBytesize() const
|
||||
{
|
||||
return _impl.getBytesize();
|
||||
}
|
||||
|
||||
bool Serial::setBytesize(ByteSize bytesize)
|
||||
{
|
||||
return _impl.setBytesize(bytesize);
|
||||
}
|
||||
|
||||
Parity Serial::getParity() const
|
||||
{
|
||||
return _impl.getParity();
|
||||
}
|
||||
|
||||
bool Serial::setParity(Parity parity)
|
||||
{
|
||||
return _impl.setParity(parity);
|
||||
}
|
||||
|
||||
StopBits Serial::getStopbits() const
|
||||
{
|
||||
return _impl.getStopbits();
|
||||
}
|
||||
|
||||
bool Serial::setStopbits(StopBits stopbits)
|
||||
{
|
||||
return _impl.setStopbits(stopbits);
|
||||
}
|
||||
|
||||
FlowControl Serial::getFlowcontrol() const
|
||||
{
|
||||
return _impl.getFlowcontrol();
|
||||
}
|
||||
|
||||
bool Serial::setFlowcontrol(FlowControl flowcontrol)
|
||||
{
|
||||
return _impl.setFlowcontrol(flowcontrol);
|
||||
}
|
||||
|
||||
const char *Serial::getPort() const
|
||||
{
|
||||
return _impl.getPort();
|
||||
}
|
||||
|
||||
} // namespace device
|
||||
|
|
@ -1,97 +0,0 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <SerialImpl.hpp>
|
||||
|
||||
#include <px4_platform_common/SerialCommon.hpp>
|
||||
|
||||
using device::SerialConfig::ByteSize;
|
||||
using device::SerialConfig::Parity;
|
||||
using device::SerialConfig::StopBits;
|
||||
using device::SerialConfig::FlowControl;
|
||||
|
||||
namespace device __EXPORT
|
||||
{
|
||||
|
||||
class Serial
|
||||
{
|
||||
public:
|
||||
Serial(const char *port, uint32_t baudrate = 57600,
|
||||
ByteSize bytesize = ByteSize::EightBits, Parity parity = Parity::None,
|
||||
StopBits stopbits = StopBits::One, FlowControl flowcontrol = FlowControl::Disabled);
|
||||
virtual ~Serial();
|
||||
|
||||
// Open sets up the port and gets it configured based on desired configuration
|
||||
bool open();
|
||||
bool isOpen() const;
|
||||
|
||||
bool close();
|
||||
|
||||
ssize_t read(uint8_t *buffer, size_t buffer_size);
|
||||
ssize_t readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count = 1, uint32_t timeout_us = 0);
|
||||
|
||||
ssize_t write(const void *buffer, size_t buffer_size);
|
||||
|
||||
// If port is already open then the following configuration functions
|
||||
// will reconfigure the port. If the port is not yet open then they will
|
||||
// simply store the configuration in preparation for the port to be opened.
|
||||
|
||||
uint32_t getBaudrate() const;
|
||||
bool setBaudrate(uint32_t baudrate);
|
||||
|
||||
ByteSize getBytesize() const;
|
||||
bool setBytesize(ByteSize bytesize);
|
||||
|
||||
Parity getParity() const;
|
||||
bool setParity(Parity parity);
|
||||
|
||||
StopBits getStopbits() const;
|
||||
bool setStopbits(StopBits stopbits);
|
||||
|
||||
FlowControl getFlowcontrol() const;
|
||||
bool setFlowcontrol(FlowControl flowcontrol);
|
||||
|
||||
const char *getPort() const;
|
||||
|
||||
private:
|
||||
// Disable copy constructors
|
||||
Serial(const Serial &);
|
||||
Serial &operator=(const Serial &);
|
||||
|
||||
// platform implementation
|
||||
SerialImpl _impl;
|
||||
};
|
||||
|
||||
} // namespace device
|
||||
|
|
@ -1,70 +0,0 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
namespace device
|
||||
{
|
||||
namespace SerialConfig
|
||||
{
|
||||
|
||||
|
||||
// ByteSize: number of data bits
|
||||
enum class ByteSize {
|
||||
FiveBits = 5,
|
||||
SixBits = 6,
|
||||
SevenBits = 7,
|
||||
EightBits = 8,
|
||||
};
|
||||
|
||||
// Parity: enable parity checking
|
||||
enum class Parity {
|
||||
None = 0,
|
||||
Odd = 1,
|
||||
Even = 2,
|
||||
};
|
||||
|
||||
// StopBits: number of stop bits
|
||||
enum class StopBits {
|
||||
One = 1,
|
||||
Two = 2
|
||||
};
|
||||
|
||||
// FlowControl: enable flow control
|
||||
enum class FlowControl {
|
||||
Disabled = 0,
|
||||
Enabled = 1,
|
||||
};
|
||||
|
||||
} // namespace SerialConfig
|
||||
} // namespace device
|
||||
|
|
@ -13,21 +13,11 @@ __END_DECLS
|
|||
#define px4_clock_gettime system_clock_gettime
|
||||
#endif
|
||||
|
||||
#if defined(ENABLE_LOCKSTEP_SCHEDULER) || defined(__PX4_QURT)
|
||||
|
||||
__BEGIN_DECLS
|
||||
__EXPORT int px4_clock_settime(clockid_t clk_id, const struct timespec *tp);
|
||||
__END_DECLS
|
||||
|
||||
#else
|
||||
|
||||
#define px4_clock_settime system_clock_settime
|
||||
|
||||
#endif
|
||||
|
||||
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
|
||||
|
||||
__BEGIN_DECLS
|
||||
__EXPORT int px4_clock_settime(clockid_t clk_id, const struct timespec *tp);
|
||||
|
||||
__EXPORT int px4_usleep(useconds_t usec);
|
||||
__EXPORT unsigned int px4_sleep(unsigned int seconds);
|
||||
__EXPORT int px4_pthread_cond_timedwait(pthread_cond_t *cond,
|
||||
|
|
@ -37,6 +27,7 @@ __END_DECLS
|
|||
|
||||
#else
|
||||
|
||||
#define px4_clock_settime system_clock_settime
|
||||
#define px4_usleep system_usleep
|
||||
#define px4_sleep system_sleep
|
||||
#define px4_pthread_cond_timedwait system_pthread_cond_timedwait
|
||||
|
|
|
|||
|
|
@ -48,8 +48,6 @@
|
|||
|
||||
#include <board_config.h>
|
||||
|
||||
#if defined(BOARD_HAS_HW_SPLIT_VERSIONING)
|
||||
|
||||
#include <inttypes.h>
|
||||
#include <stdbool.h>
|
||||
#include <syslog.h>
|
||||
|
|
@ -59,6 +57,7 @@
|
|||
/****************************************************************************
|
||||
* Pre-Processor Definitions
|
||||
****************************************************************************/
|
||||
#if defined(BOARD_HAS_HW_SPLIT_VERSIONING)
|
||||
|
||||
typedef struct {
|
||||
hw_base_id_t hw_base_id; /* The ID of the Base */
|
||||
|
|
@ -293,6 +292,12 @@ static const px4_hw_mft_item_t base_configuration_9[] = {
|
|||
.mandatory = 1,
|
||||
.connection = px4_hw_con_onboard,
|
||||
},
|
||||
{
|
||||
.id = PX4_MFT_PM2,
|
||||
.present = 1,
|
||||
.mandatory = 1,
|
||||
.connection = px4_hw_con_onboard,
|
||||
},
|
||||
{
|
||||
.id = PX4_MFT_ETHERNET,
|
||||
.present = 1,
|
||||
|
|
@ -310,52 +315,6 @@ static const px4_hw_mft_item_t base_configuration_9[] = {
|
|||
// BASE ID 10 Skynode QS no USB Alaised to ID 9
|
||||
// BASE ID 16 Auterion Skynode RC10, RC11, RC12, RC13 Alaised to ID 0
|
||||
|
||||
// BASE ID 17 Auterion Skynode RC13 with many parts removed
|
||||
static const px4_hw_mft_item_t base_configuration_17[] = {
|
||||
{
|
||||
.id = PX4_MFT_PX4IO,
|
||||
.present = 0,
|
||||
.mandatory = 0,
|
||||
.connection = px4_hw_con_onboard,
|
||||
},
|
||||
{
|
||||
.id = PX4_MFT_USB,
|
||||
.present = 1,
|
||||
.mandatory = 1,
|
||||
.connection = px4_hw_con_unknown,
|
||||
},
|
||||
{
|
||||
.id = PX4_MFT_CAN2,
|
||||
.present = 0,
|
||||
.mandatory = 0,
|
||||
.connection = px4_hw_con_onboard,
|
||||
},
|
||||
{
|
||||
.id = PX4_MFT_CAN3,
|
||||
.present = 0,
|
||||
.mandatory = 0,
|
||||
.connection = px4_hw_con_unknown,
|
||||
},
|
||||
{
|
||||
.id = PX4_MFT_PM2,
|
||||
.present = 0,
|
||||
.mandatory = 0,
|
||||
.connection = px4_hw_con_onboard,
|
||||
},
|
||||
{
|
||||
.id = PX4_MFT_ETHERNET,
|
||||
.present = 1,
|
||||
.mandatory = 1,
|
||||
.connection = px4_hw_con_connector,
|
||||
},
|
||||
{
|
||||
.id = PX4_MFT_T100_ETH,
|
||||
.present = 1,
|
||||
.mandatory = 1,
|
||||
.connection = px4_hw_con_onboard,
|
||||
},
|
||||
};
|
||||
|
||||
|
||||
static px4_hw_mft_list_entry_t mft_lists[] = {
|
||||
// ver_rev
|
||||
|
|
@ -369,7 +328,6 @@ static px4_hw_mft_list_entry_t mft_lists[] = {
|
|||
{HW_BASE_ID(9), base_configuration_9, arraySize(base_configuration_9)}, // Auterion Skynode ver 9
|
||||
{HW_BASE_ID(10), base_configuration_9, arraySize(base_configuration_9)}, // Auterion Skynode ver 10
|
||||
{HW_BASE_ID(16), base_configuration_0, arraySize(base_configuration_0)}, // Auterion Skynode ver 16
|
||||
{HW_BASE_ID(17), base_configuration_17, arraySize(base_configuration_17)}, // Auterion Skynode ver 17
|
||||
};
|
||||
|
||||
/************************************************************************************
|
||||
|
|
|
|||
|
|
@ -74,7 +74,7 @@ void px4_log_initialize(void)
|
|||
log_message.severity = 6; // info
|
||||
strcpy((char *)log_message.text, "initialized uORB logging");
|
||||
log_message.timestamp = hrt_absolute_time();
|
||||
orb_log_message_pub = orb_advertise(ORB_ID(log_message), &log_message);
|
||||
orb_log_message_pub = orb_advertise_queue(ORB_ID(log_message), &log_message, log_message_s::ORB_QUEUE_LENGTH);
|
||||
}
|
||||
|
||||
__EXPORT void px4_log_modulename(int level, const char *module_name, const char *fmt, ...)
|
||||
|
|
|
|||
|
|
@ -48,6 +48,24 @@
|
|||
namespace uORB
|
||||
{
|
||||
|
||||
template <typename U> class DefaultQueueSize
|
||||
{
|
||||
private:
|
||||
template <typename T>
|
||||
static constexpr uint8_t get_queue_size(decltype(T::ORB_QUEUE_LENGTH) *)
|
||||
{
|
||||
return T::ORB_QUEUE_LENGTH;
|
||||
}
|
||||
|
||||
template <typename T> static constexpr uint8_t get_queue_size(...)
|
||||
{
|
||||
return 1;
|
||||
}
|
||||
|
||||
public:
|
||||
static constexpr unsigned value = get_queue_size<U>(nullptr);
|
||||
};
|
||||
|
||||
class PublicationBase
|
||||
{
|
||||
public:
|
||||
|
|
@ -79,7 +97,7 @@ protected:
|
|||
/**
|
||||
* uORB publication wrapper class
|
||||
*/
|
||||
template<typename T>
|
||||
template<typename T, uint8_t ORB_QSIZE = DefaultQueueSize<T>::value>
|
||||
class Publication : public PublicationBase
|
||||
{
|
||||
public:
|
||||
|
|
@ -95,7 +113,7 @@ public:
|
|||
bool advertise()
|
||||
{
|
||||
if (!advertised()) {
|
||||
_handle = orb_advertise(get_topic(), nullptr);
|
||||
_handle = orb_advertise_queue(get_topic(), nullptr, ORB_QSIZE);
|
||||
}
|
||||
|
||||
return advertised();
|
||||
|
|
|
|||
|
|
@ -51,7 +51,7 @@ namespace uORB
|
|||
/**
|
||||
* Base publication multi wrapper class
|
||||
*/
|
||||
template<typename T>
|
||||
template<typename T, uint8_t QSIZE = DefaultQueueSize<T>::value>
|
||||
class PublicationMulti : public PublicationBase
|
||||
{
|
||||
public:
|
||||
|
|
@ -73,7 +73,7 @@ public:
|
|||
{
|
||||
if (!advertised()) {
|
||||
int instance = 0;
|
||||
_handle = orb_advertise_multi(get_topic(), nullptr, &instance);
|
||||
_handle = orb_advertise_multi_queue(get_topic(), nullptr, &instance, QSIZE);
|
||||
}
|
||||
|
||||
return advertised();
|
||||
|
|
|
|||
|
|
@ -56,6 +56,31 @@ public:
|
|||
SubscriptionBlocking(const orb_metadata *meta, uint32_t interval_us = 0, uint8_t instance = 0) :
|
||||
SubscriptionCallback(meta, interval_us, instance)
|
||||
{
|
||||
// pthread_mutexattr_init
|
||||
pthread_mutexattr_t attr;
|
||||
int ret_attr_init = pthread_mutexattr_init(&attr);
|
||||
|
||||
if (ret_attr_init != 0) {
|
||||
PX4_ERR("pthread_mutexattr_init failed, status=%d", ret_attr_init);
|
||||
}
|
||||
|
||||
#if defined(PTHREAD_PRIO_NONE)
|
||||
// pthread_mutexattr_settype
|
||||
// PTHREAD_PRIO_NONE not available on cygwin
|
||||
int ret_mutexattr_settype = pthread_mutexattr_settype(&attr, PTHREAD_PRIO_NONE);
|
||||
|
||||
if (ret_mutexattr_settype != 0) {
|
||||
PX4_ERR("pthread_mutexattr_settype failed, status=%d", ret_mutexattr_settype);
|
||||
}
|
||||
|
||||
#endif // PTHREAD_PRIO_NONE
|
||||
|
||||
// pthread_mutex_init
|
||||
int ret_mutex_init = pthread_mutex_init(&_mutex, &attr);
|
||||
|
||||
if (ret_mutex_init != 0) {
|
||||
PX4_ERR("pthread_mutex_init failed, status=%d", ret_mutex_init);
|
||||
}
|
||||
}
|
||||
|
||||
virtual ~SubscriptionBlocking()
|
||||
|
|
|
|||
|
|
@ -118,11 +118,22 @@ orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data)
|
|||
return uORB::Manager::get_instance()->orb_advertise(meta, data);
|
||||
}
|
||||
|
||||
orb_advert_t orb_advertise_queue(const struct orb_metadata *meta, const void *data, unsigned int queue_size)
|
||||
{
|
||||
return uORB::Manager::get_instance()->orb_advertise(meta, data, queue_size);
|
||||
}
|
||||
|
||||
orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance)
|
||||
{
|
||||
return uORB::Manager::get_instance()->orb_advertise_multi(meta, data, instance);
|
||||
}
|
||||
|
||||
orb_advert_t orb_advertise_multi_queue(const struct orb_metadata *meta, const void *data, int *instance,
|
||||
unsigned int queue_size)
|
||||
{
|
||||
return uORB::Manager::get_instance()->orb_advertise_multi(meta, data, instance, queue_size);
|
||||
}
|
||||
|
||||
int orb_unadvertise(orb_advert_t handle)
|
||||
{
|
||||
return uORB::Manager::get_instance()->orb_unadvertise(handle);
|
||||
|
|
@ -216,14 +227,6 @@ const char *orb_get_c_type(unsigned char short_type)
|
|||
return nullptr;
|
||||
}
|
||||
|
||||
uint8_t orb_get_queue_size(const struct orb_metadata *meta)
|
||||
{
|
||||
if (meta) {
|
||||
return meta->o_queue;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
void orb_print_message_internal(const orb_metadata *meta, const void *data, bool print_topic_name)
|
||||
{
|
||||
|
|
|
|||
|
|
@ -53,8 +53,6 @@ struct orb_metadata {
|
|||
const uint16_t o_size_no_padding; /**< object size w/o padding at the end (for logger) */
|
||||
uint32_t message_hash; /**< Hash over all fields for message compatibility checks */
|
||||
orb_id_size_t o_id; /**< ORB_ID enum */
|
||||
uint8_t o_queue; /**< queue size */
|
||||
|
||||
};
|
||||
|
||||
typedef const struct orb_metadata *orb_id_t;
|
||||
|
|
@ -104,16 +102,14 @@ typedef const struct orb_metadata *orb_id_t;
|
|||
* @param _size_no_padding Struct size w/o padding at the end
|
||||
* @param _message_hash 32 bit message hash over all fields
|
||||
* @param _orb_id_enum ORB ID enum e.g.: ORB_ID::vehicle_status
|
||||
* @param _queue_size Queue size from topic definition
|
||||
*/
|
||||
#define ORB_DEFINE(_name, _struct, _size_no_padding, _message_hash, _orb_id_enum, _queue_size) \
|
||||
const struct orb_metadata __orb_##_name = { \
|
||||
#_name, \
|
||||
sizeof(_struct), \
|
||||
_size_no_padding, \
|
||||
_message_hash, \
|
||||
_orb_id_enum, \
|
||||
_queue_size \
|
||||
#define ORB_DEFINE(_name, _struct, _size_no_padding, _message_hash, _orb_id_enum) \
|
||||
const struct orb_metadata __orb_##_name = { \
|
||||
#_name, \
|
||||
sizeof(_struct), \
|
||||
_size_no_padding, \
|
||||
_message_hash, \
|
||||
_orb_id_enum \
|
||||
}; struct hack
|
||||
|
||||
__BEGIN_DECLS
|
||||
|
|
@ -139,11 +135,23 @@ typedef void *orb_advert_t;
|
|||
*/
|
||||
extern orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data) __EXPORT;
|
||||
|
||||
/**
|
||||
* @see uORB::Manager::orb_advertise()
|
||||
*/
|
||||
extern orb_advert_t orb_advertise_queue(const struct orb_metadata *meta, const void *data,
|
||||
unsigned int queue_size) __EXPORT;
|
||||
|
||||
/**
|
||||
* @see uORB::Manager::orb_advertise_multi()
|
||||
*/
|
||||
extern orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance) __EXPORT;
|
||||
|
||||
/**
|
||||
* @see uORB::Manager::orb_advertise_multi()
|
||||
*/
|
||||
extern orb_advert_t orb_advertise_multi_queue(const struct orb_metadata *meta, const void *data, int *instance,
|
||||
unsigned int queue_size) __EXPORT;
|
||||
|
||||
/**
|
||||
* @see uORB::Manager::orb_unadvertise()
|
||||
*/
|
||||
|
|
@ -152,7 +160,7 @@ extern int orb_unadvertise(orb_advert_t handle) __EXPORT;
|
|||
/**
|
||||
* @see uORB::Manager::orb_publish()
|
||||
*/
|
||||
extern int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data) __EXPORT;
|
||||
extern int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data) __EXPORT;
|
||||
|
||||
/**
|
||||
* Advertise as the publisher of a topic.
|
||||
|
|
@ -233,12 +241,6 @@ extern int orb_get_interval(int handle, unsigned *interval) __EXPORT;
|
|||
*/
|
||||
const char *orb_get_c_type(unsigned char short_type);
|
||||
|
||||
/**
|
||||
* Returns the queue size of a topic
|
||||
* @param meta orb topic metadata
|
||||
*/
|
||||
extern uint8_t orb_get_queue_size(const struct orb_metadata *meta);
|
||||
|
||||
/**
|
||||
* Print a topic to console. Do not call this directly, use print_message() instead.
|
||||
* @param meta orb topic metadata
|
||||
|
|
|
|||
|
|
@ -48,10 +48,37 @@
|
|||
|
||||
static uORB::SubscriptionInterval *filp_to_subscription(cdev::file_t *filp) { return static_cast<uORB::SubscriptionInterval *>(filp->f_priv); }
|
||||
|
||||
uORB::DeviceNode::DeviceNode(const struct orb_metadata *meta, const uint8_t instance, const char *path) :
|
||||
// round up to nearest power of two
|
||||
// Such as 0 => 1, 1 => 1, 2 => 2 ,3 => 4, 10 => 16, 60 => 64, 65...255 => 128
|
||||
// Note: When the input value > 128, the output is always 128
|
||||
static inline uint8_t round_pow_of_two_8(uint8_t n)
|
||||
{
|
||||
if (n == 0) {
|
||||
return 1;
|
||||
}
|
||||
|
||||
// Avoid is already a power of 2
|
||||
uint8_t value = n - 1;
|
||||
|
||||
// Fill 1
|
||||
value |= value >> 1U;
|
||||
value |= value >> 2U;
|
||||
value |= value >> 4U;
|
||||
|
||||
// Unable to round-up, take the value of round-down
|
||||
if (value == UINT8_MAX) {
|
||||
value >>= 1U;
|
||||
}
|
||||
|
||||
return value + 1;
|
||||
}
|
||||
|
||||
uORB::DeviceNode::DeviceNode(const struct orb_metadata *meta, const uint8_t instance, const char *path,
|
||||
uint8_t queue_size) :
|
||||
CDev(strdup(path)), // success is checked in CDev::init
|
||||
_meta(meta),
|
||||
_instance(instance)
|
||||
_instance(instance),
|
||||
_queue_size(round_pow_of_two_8(queue_size))
|
||||
{
|
||||
}
|
||||
|
||||
|
|
@ -159,7 +186,7 @@ uORB::DeviceNode::write(cdev::file_t *filp, const char *buffer, size_t buflen)
|
|||
|
||||
/* re-check size */
|
||||
if (nullptr == _data) {
|
||||
const size_t data_size = _meta->o_size * _meta->o_queue;
|
||||
const size_t data_size = _meta->o_size * _queue_size;
|
||||
_data = (uint8_t *) px4_cache_aligned_alloc(data_size);
|
||||
|
||||
if (_data) {
|
||||
|
|
@ -190,7 +217,7 @@ uORB::DeviceNode::write(cdev::file_t *filp, const char *buffer, size_t buflen)
|
|||
/* wrap-around happens after ~49 days, assuming a publisher rate of 1 kHz */
|
||||
unsigned generation = _generation.fetch_add(1);
|
||||
|
||||
memcpy(_data + (_meta->o_size * (generation % _meta->o_queue)), buffer, _meta->o_size);
|
||||
memcpy(_data + (_meta->o_size * (generation % _queue_size)), buffer, _meta->o_size);
|
||||
|
||||
// callbacks
|
||||
for (auto item : _callbacks) {
|
||||
|
|
@ -227,6 +254,13 @@ uORB::DeviceNode::ioctl(cdev::file_t *filp, int cmd, unsigned long arg)
|
|||
*(uintptr_t *)arg = (uintptr_t)this;
|
||||
return PX4_OK;
|
||||
|
||||
case ORBIOCSETQUEUESIZE: {
|
||||
lock();
|
||||
int ret = update_queue_size(arg);
|
||||
unlock();
|
||||
return ret;
|
||||
}
|
||||
|
||||
case ORBIOCGETINTERVAL:
|
||||
*(unsigned *)arg = filp_to_subscription(filp)->get_interval_us();
|
||||
return PX4_OK;
|
||||
|
|
@ -355,11 +389,12 @@ uORB::DeviceNode::print_statistics(int max_topic_length)
|
|||
|
||||
const uint8_t instance = get_instance();
|
||||
const int8_t sub_count = subscriber_count();
|
||||
const uint8_t queue_size = get_queue_size();
|
||||
|
||||
unlock();
|
||||
|
||||
PX4_INFO_RAW("%-*s %2i %4i %2i %4i %s\n", max_topic_length, get_meta()->o_name, (int)instance, (int)sub_count,
|
||||
get_meta()->o_queue, get_meta()->o_size, get_devname());
|
||||
queue_size, get_meta()->o_size, get_devname());
|
||||
|
||||
return true;
|
||||
}
|
||||
|
|
@ -412,10 +447,7 @@ int16_t uORB::DeviceNode::process_add_subscription()
|
|||
uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator();
|
||||
|
||||
if (_data != nullptr && ch != nullptr) { // _data will not be null if there is a publisher.
|
||||
// Only send the most recent data to initialize the remote end.
|
||||
if (_data_valid) {
|
||||
ch->send_message(_meta->o_name, _meta->o_size, _data + (_meta->o_size * ((_generation.load() - 1) % _meta->o_queue)));
|
||||
}
|
||||
ch->send_message(_meta->o_name, _meta->o_size, _data);
|
||||
}
|
||||
|
||||
return PX4_OK;
|
||||
|
|
@ -451,6 +483,21 @@ int16_t uORB::DeviceNode::process_received_message(int32_t length, uint8_t *data
|
|||
}
|
||||
#endif /* CONFIG_ORB_COMMUNICATOR */
|
||||
|
||||
int uORB::DeviceNode::update_queue_size(unsigned int queue_size)
|
||||
{
|
||||
if (_queue_size == queue_size) {
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
//queue size is limited to 255 for the single reason that we use uint8 to store it
|
||||
if (_data || _queue_size > queue_size || queue_size > 255) {
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
_queue_size = round_pow_of_two_8(queue_size);
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
unsigned uORB::DeviceNode::get_initial_generation()
|
||||
{
|
||||
ATOMIC_ENTER;
|
||||
|
|
|
|||
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in New Issue