forked from Archive/PX4-Autopilot
Compare commits
133 Commits
yannick/ma
...
main
Author | SHA1 | Date |
---|---|---|
Matthias Grob | b5f6699f2e | |
Matthias Grob | 1096384a38 | |
Matthias Grob | 999a71c4dd | |
Thomas Frans | bcbae86b9f | |
Eric Katzfey | 4a553938fb | |
Daniel Agar | c024ea396a | |
Eric Katzfey | 69028f37a9 | |
Thomas Frans | bb9f4d42f3 | |
Beat Küng | 2e12e14a23 | |
Thomas Frans | d0251b8688 | |
Eric Katzfey | 82a1aa37db | |
Eric Katzfey | 5f6dc1c5d0 | |
Øyvind Taksdal Stubhaug | 710286da72 | |
Daniel Agar | 34c19b2e5a | |
alexklimaj | af16544809 | |
Daniel Agar | 35532609c9 | |
bresch | 62b8db153b | |
bresch | 638e17d551 | |
bresch | 6d819343aa | |
bresch | cb2bb2e098 | |
bresch | c9221b91ad | |
Drone-Lab | 37caddedbb | |
muramura | 63850873eb | |
Hamish Willee | 95627ea098 | |
bresch | 2e6dd243af | |
muramura | 32aa3263a6 | |
Beat Küng | 70346a5b2f | |
Silvan Fuhrer | da39d075ac | |
Silvan Fuhrer | f6430a27d6 | |
Silvan Fuhrer | 4ae2fbd171 | |
Silvan Fuhrer | 6e15dd5328 | |
Silvan Fuhrer | d330d47495 | |
Silvan Fuhrer | 6d8273483c | |
bresch | 7c6ecd95a8 | |
bresch | b2f1122372 | |
bresch | ee63f3e664 | |
bresch | e3f67d5c1a | |
Silvan Fuhrer | 6373d8d243 | |
Silvan Fuhrer | 68fcfc43ef | |
Eric Katzfey | ae947513d7 | |
Alexander Lampalzer | ad50afda10 | |
Niklas Hauser | 23c5c0b12d | |
Daniel Agar | a1cce7e961 | |
Daniel Agar | b115d3cd44 | |
Silvan Fuhrer | 9f4ae0a85d | |
Silvan Fuhrer | 00cc68baa1 | |
Silvan Fuhrer | 7fe5ee64fe | |
Silvan Fuhrer | 7f370ac6df | |
Thomas Stauber | 1ad83a8002 | |
Eric Katzfey | 18d53c3bfd | |
Eric Katzfey | f4ebfa6130 | |
Konrad | c5fde63440 | |
Silvan Fuhrer | cb8520427c | |
Silvan Fuhrer | 86c074378f | |
Konrad | e9fda548fa | |
Konrad | 91d1342f20 | |
Konrad | 89844625b4 | |
Konrad | fde71cd15e | |
Konrad | 14e4169473 | |
KonradRudin | 8dcfcf5b9e | |
alexklimaj | a80a5a92f4 | |
alexklimaj | b81ad8841e | |
Eric Katzfey | 57df7e35b2 | |
Alexis Guijarro | 006dcfafb7 | |
Silvan Fuhrer | 85a882e1ce | |
Konrad | 04099ed483 | |
Konrad | 1aa26a5a91 | |
Konrad | acd750e033 | |
Konrad | 6c6142ba79 | |
Konrad | 7fb584adbe | |
Konrad | fb3aab1fb0 | |
Konrad | 1b03ac4d2b | |
Konrad | 815cea2abb | |
Konrad | 51321c605e | |
Konrad | a0ae073d8c | |
Silvan Fuhrer | 7884e0a3f7 | |
Silvan Fuhrer | f799141a19 | |
Eric Katzfey | e20215087f | |
bresch | 0d0978b3b9 | |
bresch | 0639f5370c | |
bresch | 2bacb4b65d | |
bresch | 421f13e4b5 | |
Silvan Fuhrer | 1e253a9626 | |
cuav-liu1 | bb5efa5577 | |
Daniel Agar | 1c741836c0 | |
Daniel Agar | 8b6c70e0f2 | |
Daniel Agar | 1fc38aab92 | |
Daniel Agar | 2bf1eeb003 | |
Daniel Agar | 87960c04d8 | |
Daniel Agar | d96970a2b9 | |
Silvan Fuhrer | c5835a48de | |
bresch | 6f9a378247 | |
PX4 BuildBot | 67e68783cf | |
Peter van der Perk | d1ae242a91 | |
Peter van der Perk | 9cef834624 | |
Sihyun Noh | 23a41299fa | |
Niklas Hauser | 0186d687b2 | |
Peter van der Perk | d28653b605 | |
Peter van der Perk | 87d79aeb75 | |
Matthias Grob | 1bd65f8beb | |
Peter van der Perk | e0b49afe81 | |
Don Gagne | f02b44bec5 | |
bresch | 28db3e1c8c | |
bresch | e9d43015ce | |
bresch | b46fc9a67d | |
bresch | b80f15f7b5 | |
Silvan Fuhrer | 086656dc7f | |
bresch | 051baec9c4 | |
Matthias Grob | 2491548a0f | |
DanielePettenuzzo | 18f96c16ce | |
GuillaumeLaine | 63495ddac3 | |
Peter van der Perk | efbbd64ec0 | |
Daniel Agar | 8001132d33 | |
bresch | 08a2a6c836 | |
bresch | d501d8e1d4 | |
bresch | 9d9766c6cf | |
makekam | d988005216 | |
Silvan Fuhrer | 5dfdf8c071 | |
Daniel Agar | b2b7439060 | |
bresch | 37a40d3fc2 | |
Sergei Grichine | b405d75553 | |
Matthias Grob | 4e3bd4f196 | |
Matthias Grob | 0cc4b41a51 | |
Matthias Grob | f602228048 | |
PerFrivik | 9b122adae4 | |
Eric Katzfey | 1ec0ba4736 | |
Eric Katzfey | 8da8b88a54 | |
Eric Katzfey | be08c57a0a | |
Eric Katzfey | a436a8f3b8 | |
Eric Katzfey | 5ad0e68d8e | |
Eric Katzfey | f07eeaa776 | |
Daniel Agar | 506c60c471 | |
Niklas Hauser | 643d3e3bf3 |
|
@ -120,7 +120,7 @@ pipeline {
|
||||||
"px4_fmu-v6xrt_rover",
|
"px4_fmu-v6xrt_rover",
|
||||||
"px4_io-v2_default",
|
"px4_io-v2_default",
|
||||||
"raspberrypi_pico_default",
|
"raspberrypi_pico_default",
|
||||||
"siyi_n7_default"
|
"siyi_n7_default",
|
||||||
"sky-drones_smartap-airlink_default",
|
"sky-drones_smartap-airlink_default",
|
||||||
"spracing_h7extreme_default",
|
"spracing_h7extreme_default",
|
||||||
"thepeach_k1_default",
|
"thepeach_k1_default",
|
||||||
|
|
|
@ -0,0 +1,14 @@
|
||||||
|
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
|
url = https://github.com/PX4/PX4-gazebo-models.git
|
||||||
branch = main
|
branch = main
|
||||||
[submodule "boards/modalai/voxl2/libfc-sensor-api"]
|
[submodule "boards/modalai/voxl2/libfc-sensor-api"]
|
||||||
path = src/modules/muorb/apps/libfc-sensor-api
|
path = boards/modalai/voxl2/libfc-sensor-api
|
||||||
url = https://gitlab.com/voxl-public/voxl-sdk/core-libs/libfc-sensor-api.git
|
url = https://gitlab.com/voxl-public/voxl-sdk/core-libs/libfc-sensor-api.git
|
||||||
|
|
|
@ -0,0 +1,80 @@
|
||||||
|
#!/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,6 +82,7 @@ px4_add_romfs_files(
|
||||||
4008_gz_advanced_plane
|
4008_gz_advanced_plane
|
||||||
4009_gz_r1_rover
|
4009_gz_r1_rover
|
||||||
4010_gz_x500_mono_cam
|
4010_gz_x500_mono_cam
|
||||||
|
4011_gz_lawnmower
|
||||||
|
|
||||||
6011_gazebo-classic_typhoon_h480
|
6011_gazebo-classic_typhoon_h480
|
||||||
6011_gazebo-classic_typhoon_h480.post
|
6011_gazebo-classic_typhoon_h480.post
|
||||||
|
|
|
@ -34,28 +34,79 @@
|
||||||
add_subdirectory(airframes)
|
add_subdirectory(airframes)
|
||||||
|
|
||||||
px4_add_romfs_files(
|
px4_add_romfs_files(
|
||||||
rc.airship_apps
|
|
||||||
rc.airship_defaults
|
|
||||||
rc.autostart_ext
|
rc.autostart_ext
|
||||||
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
|
rcS
|
||||||
rc.sensors
|
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.vehicle_setup
|
||||||
rc.vtol_apps
|
|
||||||
rc.vtol_defaults
|
# TODO
|
||||||
|
rc.balloon_apps
|
||||||
|
rc.balloon_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,94 +32,127 @@
|
||||||
############################################################################
|
############################################################################
|
||||||
|
|
||||||
px4_add_romfs_files(
|
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
|
# [18000, 18999] High-altitude balloons
|
||||||
18001_TF-B1
|
18001_TF-B1
|
||||||
|
|
||||||
# [22000, 22999] Reserve for custom models
|
# [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,7 +28,6 @@ param set-default EKF2_ARSP_THR 8
|
||||||
param set-default EKF2_FUSE_BETA 1
|
param set-default EKF2_FUSE_BETA 1
|
||||||
param set-default EKF2_GPS_CHECK 21
|
param set-default EKF2_GPS_CHECK 21
|
||||||
param set-default EKF2_MAG_ACCLIM 0
|
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_EPH 10
|
||||||
param set-default EKF2_REQ_EPV 10
|
param set-default EKF2_REQ_EPV 10
|
||||||
param set-default EKF2_REQ_HDRIFT 0.5
|
param set-default EKF2_REQ_HDRIFT 0.5
|
||||||
|
|
|
@ -439,7 +439,12 @@ else
|
||||||
#
|
#
|
||||||
# Start a thermal calibration if required.
|
# Start a thermal calibration if required.
|
||||||
#
|
#
|
||||||
. ${R}etc/init.d/rc.thermal_cal
|
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
|
||||||
|
|
||||||
#
|
#
|
||||||
# Start gimbal to control mounts such as gimbals, disabled by default.
|
# Start gimbal to control mounts such as gimbals, disabled by default.
|
||||||
|
@ -500,7 +505,12 @@ else
|
||||||
#
|
#
|
||||||
# Start the logger.
|
# Start the logger.
|
||||||
#
|
#
|
||||||
. ${R}etc/init.d/rc.logging
|
set RC_LOGGING ${R}etc/init.d/rc.logging
|
||||||
|
if [ -f ${RC_LOGGING} ]
|
||||||
|
then
|
||||||
|
. ${RC_LOGGING}
|
||||||
|
fi
|
||||||
|
unset RC_LOGGING
|
||||||
|
|
||||||
#
|
#
|
||||||
# Set additional parameters and env variables for selected AUTOSTART.
|
# Set additional parameters and env variables for selected AUTOSTART.
|
||||||
|
|
|
@ -30,4 +30,5 @@ exec find boards msg src platforms test \
|
||||||
-path src/lib/cdrstream/cyclonedds -prune -o \
|
-path src/lib/cdrstream/cyclonedds -prune -o \
|
||||||
-path src/lib/cdrstream/rosidl -prune -o \
|
-path src/lib/cdrstream/rosidl -prune -o \
|
||||||
-path src/modules/zenoh/zenoh-pico -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
|
-type f \( -name "*.c" -o -name "*.h" -o -name "*.cpp" -o -name "*.hpp" \) | grep $PATTERN
|
||||||
|
|
|
@ -73,9 +73,16 @@ struct_size, padding_end_size = add_padding_bytes(sorted_fields, search_path)
|
||||||
#include <lib/matrix/matrix/math.hpp>
|
#include <lib/matrix/matrix/math.hpp>
|
||||||
#include <lib/mathlib/mathlib.h>
|
#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]@
|
@[for topic in topics]@
|
||||||
static_assert(static_cast<orb_id_size_t>(ORB_ID::@topic) == @(all_topics.index(topic)), "ORB_ID index mismatch");
|
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));
|
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);
|
||||||
@[end for]
|
@[end for]
|
||||||
|
|
||||||
void print_message(const orb_metadata *meta, const @uorb_struct& message)
|
void print_message(const orb_metadata *meta, const @uorb_struct& message)
|
||||||
|
|
|
@ -1 +1 @@
|
||||||
Subproject commit 33ac87a37676fb597de110e926bbf0a080169ffe
|
Subproject commit da7206e057703cc645770f02437013358b71e1c0
|
|
@ -1 +1 @@
|
||||||
Subproject commit f1c461fffb8567d6f0af770fb533f60f6ec62c22
|
Subproject commit 6b4ed09d1b495fbff663f098979cc046df013abd
|
|
@ -4,7 +4,7 @@
|
||||||
"description": "Firmware for the ARK flow board",
|
"description": "Firmware for the ARK flow board",
|
||||||
"image": "",
|
"image": "",
|
||||||
"build_time": 0,
|
"build_time": 0,
|
||||||
"summary": "ARFFLOW",
|
"summary": "ARKFLOW",
|
||||||
"version": "0.1",
|
"version": "0.1",
|
||||||
"image_size": 0,
|
"image_size": 0,
|
||||||
"image_maxsize": 2080768,
|
"image_maxsize": 2080768,
|
||||||
|
|
Binary file not shown.
|
@ -20,6 +20,7 @@ CONFIG_UAVCANNODE_RTK_DATA=y
|
||||||
CONFIG_UAVCANNODE_SAFETY_BUTTON=y
|
CONFIG_UAVCANNODE_SAFETY_BUTTON=y
|
||||||
CONFIG_UAVCANNODE_STATIC_PRESSURE=y
|
CONFIG_UAVCANNODE_STATIC_PRESSURE=y
|
||||||
CONFIG_UAVCANNODE_STATIC_TEMPERATURE=y
|
CONFIG_UAVCANNODE_STATIC_TEMPERATURE=y
|
||||||
|
CONFIG_MODULES_EKF2=y
|
||||||
CONFIG_MODULES_GYRO_CALIBRATION=y
|
CONFIG_MODULES_GYRO_CALIBRATION=y
|
||||||
CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
|
CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
|
||||||
CONFIG_MODULES_SENSORS=y
|
CONFIG_MODULES_SENSORS=y
|
||||||
|
|
|
@ -11,3 +11,5 @@ param set-default SENS_IMU_CLPNOTI 0
|
||||||
|
|
||||||
safety_button start
|
safety_button start
|
||||||
tone_alarm start
|
tone_alarm start
|
||||||
|
|
||||||
|
ekf2 start
|
||||||
|
|
|
@ -50,6 +50,7 @@ CONFIG_STACK_COLORATION=y
|
||||||
CONFIG_START_DAY=30
|
CONFIG_START_DAY=30
|
||||||
CONFIG_START_MONTH=11
|
CONFIG_START_MONTH=11
|
||||||
CONFIG_STDIO_DISABLE_BUFFERING=y
|
CONFIG_STDIO_DISABLE_BUFFERING=y
|
||||||
|
CONFIG_STM32_FLASH_CONFIG_G=y
|
||||||
CONFIG_STM32_NOEXT_VECTORS=y
|
CONFIG_STM32_NOEXT_VECTORS=y
|
||||||
CONFIG_STM32_TIM8=y
|
CONFIG_STM32_TIM8=y
|
||||||
CONFIG_TASK_NAME_SIZE=0
|
CONFIG_TASK_NAME_SIZE=0
|
||||||
|
|
|
@ -123,6 +123,7 @@ CONFIG_STM32_ADC1=y
|
||||||
CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG=y
|
CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG=y
|
||||||
CONFIG_STM32_DMA1=y
|
CONFIG_STM32_DMA1=y
|
||||||
CONFIG_STM32_DMA2=y
|
CONFIG_STM32_DMA2=y
|
||||||
|
CONFIG_STM32_FLASH_CONFIG_G=y
|
||||||
CONFIG_STM32_FLASH_PREFETCH=y
|
CONFIG_STM32_FLASH_PREFETCH=y
|
||||||
CONFIG_STM32_FLOWCONTROL_BROKEN=y
|
CONFIG_STM32_FLOWCONTROL_BROKEN=y
|
||||||
CONFIG_STM32_I2C1=y
|
CONFIG_STM32_I2C1=y
|
||||||
|
|
|
@ -33,7 +33,7 @@
|
||||||
*
|
*
|
||||||
****************************************************************************/
|
****************************************************************************/
|
||||||
|
|
||||||
/* The STM32F412 has 512Kb of FLASH beginning at address 0x0800:0000 and
|
/* The STM32F412 has 1M of FLASH beginning at address 0x0800:0000 and
|
||||||
* 256Kb of SRAM. SRAM is split up into three blocks:
|
* 256Kb of SRAM. SRAM is split up into three blocks:
|
||||||
*
|
*
|
||||||
* 1) 112Kb of SRAM beginning at address 0x2000:0000
|
* 1) 112Kb of SRAM beginning at address 0x2000:0000
|
||||||
|
|
|
@ -33,7 +33,7 @@
|
||||||
*
|
*
|
||||||
****************************************************************************/
|
****************************************************************************/
|
||||||
|
|
||||||
/* The STM32F412 has 512Kb of FLASH beginning at address 0x0800:0000 and
|
/* The STM32F412 has 1M of FLASH beginning at address 0x0800:0000 and
|
||||||
* 256Kb of SRAM. SRAM is split up into three blocks:
|
* 256Kb of SRAM. SRAM is split up into three blocks:
|
||||||
*
|
*
|
||||||
* 1) 112Kb of SRAM beginning at address 0x2000:0000
|
* 1) 112Kb of SRAM beginning at address 0x2000:0000
|
||||||
|
|
|
@ -64,9 +64,6 @@
|
||||||
// Hacks for MAVLink RC button input
|
// Hacks for MAVLink RC button input
|
||||||
#define ATL_MANTIS_RC_INPUT_HACKS
|
#define ATL_MANTIS_RC_INPUT_HACKS
|
||||||
|
|
||||||
// Hacks for MAVLink RC button input
|
|
||||||
#define ATL_MANTIS_RC_INPUT_HACKS
|
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* ADC channels
|
* 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_RC_UPDATE=y
|
||||||
CONFIG_MODULES_ROVER_POS_CONTROL=y
|
CONFIG_MODULES_ROVER_POS_CONTROL=y
|
||||||
CONFIG_MODULES_SENSORS=y
|
CONFIG_MODULES_SENSORS=y
|
||||||
CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
|
# CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
|
||||||
CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
|
CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
|
||||||
CONFIG_MODULES_UXRCE_DDS_CLIENT=y
|
CONFIG_MODULES_UXRCE_DDS_CLIENT=y
|
||||||
CONFIG_MODULES_VTOL_ATT_CONTROL=y
|
CONFIG_MODULES_VTOL_ATT_CONTROL=y
|
||||||
|
|
|
@ -4,13 +4,14 @@ CONFIG_DRIVERS_ACTUATORS_VOXL_ESC=y
|
||||||
CONFIG_DRIVERS_BAROMETER_INVENSENSE_ICP101XX=y
|
CONFIG_DRIVERS_BAROMETER_INVENSENSE_ICP101XX=y
|
||||||
CONFIG_DRIVERS_DISTANCE_SENSOR_VL53L0X=y
|
CONFIG_DRIVERS_DISTANCE_SENSOR_VL53L0X=y
|
||||||
CONFIG_DRIVERS_DISTANCE_SENSOR_VL53L1X=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_LIGHTS_RGBLED_NCP5623C=y
|
||||||
CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8308=y
|
CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8308=y
|
||||||
CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y
|
CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y
|
||||||
CONFIG_DRIVERS_MAGNETOMETER_QMC5883L=y
|
CONFIG_DRIVERS_MAGNETOMETER_QMC5883L=y
|
||||||
CONFIG_DRIVERS_POWER_MONITOR_VOXLPM=y
|
CONFIG_DRIVERS_POWER_MONITOR_VOXLPM=y
|
||||||
CONFIG_DRIVERS_QSHELL_QURT=y
|
CONFIG_DRIVERS_QSHELL_QURT=y
|
||||||
CONFIG_DRIVERS_VOXL2_IO=y
|
|
||||||
CONFIG_MODULES_COMMANDER=y
|
CONFIG_MODULES_COMMANDER=y
|
||||||
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
||||||
CONFIG_MODULES_EKF2=y
|
CONFIG_MODULES_EKF2=y
|
||||||
|
@ -28,4 +29,6 @@ CONFIG_MODULES_RC_UPDATE=y
|
||||||
CONFIG_MODULES_SENSORS=y
|
CONFIG_MODULES_SENSORS=y
|
||||||
CONFIG_MODULES_SIMULATION_PWM_OUT_SIM=y
|
CONFIG_MODULES_SIMULATION_PWM_OUT_SIM=y
|
||||||
CONFIG_SYSTEMCMDS_UORB=y
|
CONFIG_SYSTEMCMDS_UORB=y
|
||||||
|
CONFIG_SYSTEMCMDS_PARAM=y
|
||||||
CONFIG_ORB_COMMUNICATOR=y
|
CONFIG_ORB_COMMUNICATOR=y
|
||||||
|
CONFIG_PARAM_REMOTE=y
|
||||||
|
|
|
@ -44,11 +44,10 @@ add_library(drivers_board
|
||||||
)
|
)
|
||||||
|
|
||||||
# Add custom drivers for SLPI
|
# 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/rc_controller)
|
||||||
add_subdirectory(${PX4_BOARD_DIR}/src/drivers/mavlink_rc_in)
|
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/spektrum_rc)
|
||||||
# add_subdirectory(${PX4_BOARD_DIR}/src/drivers/ghst_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/dsp_sbus)
|
||||||
add_subdirectory(${PX4_BOARD_DIR}/src/drivers/elrs_led)
|
add_subdirectory(${PX4_BOARD_DIR}/src/drivers/elrs_led)
|
||||||
|
|
|
@ -62,6 +62,8 @@
|
||||||
#include <uORB/topics/vehicle_odometry.h>
|
#include <uORB/topics/vehicle_odometry.h>
|
||||||
#include <uORB/topics/sensor_baro.h>
|
#include <uORB/topics/sensor_baro.h>
|
||||||
#include <uORB/topics/esc_status.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/accelerometer/PX4Accelerometer.hpp>
|
||||||
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
|
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
|
||||||
|
@ -88,21 +90,22 @@ static bool _is_running = false;
|
||||||
volatile bool _task_should_exit = false;
|
volatile bool _task_should_exit = false;
|
||||||
static px4_task_t _task_handle = -1;
|
static px4_task_t _task_handle = -1;
|
||||||
int _uart_fd = -1;
|
int _uart_fd = -1;
|
||||||
bool debug = false;
|
bool _debug = false;
|
||||||
std::string port = "2";
|
std::string port = "2";
|
||||||
int baudrate = 921600;
|
int baudrate = 921600;
|
||||||
const unsigned mode_flag_custom = 1;
|
const unsigned mode_flag_custom = 1;
|
||||||
const unsigned mode_flag_armed = 128;
|
const unsigned mode_flag_armed = 128;
|
||||||
bool _send_gps = false;
|
|
||||||
bool _send_mag = false;
|
bool _send_mag = false;
|
||||||
|
bool _send_distance = false;
|
||||||
|
|
||||||
uORB::Publication<battery_status_s> _battery_pub{ORB_ID(battery_status)};
|
uORB::Publication<battery_status_s> _battery_pub{ORB_ID(battery_status)};
|
||||||
uORB::PublicationMulti<sensor_gps_s> _sensor_gps_pub{ORB_ID(sensor_gps)};
|
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> _visual_odometry_pub{ORB_ID(vehicle_visual_odometry)};
|
||||||
uORB::Publication<vehicle_odometry_s> _mocap_odometry_pub{ORB_ID(vehicle_mocap_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::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<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)};
|
uORB::Subscription _battery_status_sub{ORB_ID(battery_status)};
|
||||||
|
|
||||||
int32_t _output_functions[actuator_outputs_s::NUM_ACTUATOR_OUTPUTS] {};
|
int32_t _output_functions[actuator_outputs_s::NUM_ACTUATOR_OUTPUTS] {};
|
||||||
|
@ -128,13 +131,42 @@ float x_gyro = 0;
|
||||||
float y_gyro = 0;
|
float y_gyro = 0;
|
||||||
float z_gyro = 0;
|
float z_gyro = 0;
|
||||||
uint64_t gyro_accel_time = 0;
|
uint64_t gyro_accel_time = 0;
|
||||||
bool _use_software_mav_throttling{false};
|
|
||||||
|
|
||||||
int heartbeat_counter = 0;
|
// Status counters
|
||||||
int imu_counter = 0;
|
uint32_t heartbeat_received_counter = 0;
|
||||||
int hil_sensor_counter = 0;
|
uint32_t heartbeat_sent_counter = 0;
|
||||||
int vision_msg_counter = 0;
|
uint32_t imu_counter = 0;
|
||||||
int gps_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;
|
||||||
|
|
||||||
vehicle_status_s _vehicle_status{};
|
vehicle_status_s _vehicle_status{};
|
||||||
vehicle_control_mode_s _control_mode{};
|
vehicle_control_mode_s _control_mode{};
|
||||||
|
@ -144,7 +176,6 @@ battery_status_s _battery_status{};
|
||||||
sensor_accel_fifo_s accel_fifo{};
|
sensor_accel_fifo_s accel_fifo{};
|
||||||
sensor_gyro_fifo_s gyro_fifo{};
|
sensor_gyro_fifo_s gyro_fifo{};
|
||||||
|
|
||||||
|
|
||||||
int openPort(const char *dev, speed_t speed);
|
int openPort(const char *dev, speed_t speed);
|
||||||
int closePort();
|
int closePort();
|
||||||
|
|
||||||
|
@ -153,7 +184,8 @@ int writeResponse(void *buf, size_t len);
|
||||||
|
|
||||||
int start(int argc, char *argv[]);
|
int start(int argc, char *argv[]);
|
||||||
int stop();
|
int stop();
|
||||||
int get_status();
|
void print_status();
|
||||||
|
void clear_status_counters();
|
||||||
bool isOpen() { return _uart_fd >= 0; };
|
bool isOpen() { return _uart_fd >= 0; };
|
||||||
|
|
||||||
void usage();
|
void usage();
|
||||||
|
@ -163,50 +195,65 @@ void *send_actuator(void *);
|
||||||
void send_actuator_data();
|
void send_actuator_data();
|
||||||
|
|
||||||
void handle_message_hil_sensor_dsp(mavlink_message_t *msg);
|
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_hil_gps_dsp(mavlink_message_t *msg);
|
||||||
void handle_message_odometry_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_command_long_dsp(mavlink_message_t *msg);
|
||||||
|
|
||||||
void handle_message_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 actuator_controls_from_outputs_dsp(mavlink_hil_actuator_controls_t *msg);
|
||||||
void send_esc_telemetry_dsp(mavlink_hil_actuator_controls_t hil_act_control);
|
void send_esc_status(mavlink_hil_actuator_controls_t hil_act_control);
|
||||||
|
|
||||||
void
|
void
|
||||||
handle_message_dsp(mavlink_message_t *msg)
|
handle_message_dsp(mavlink_message_t *msg)
|
||||||
{
|
{
|
||||||
switch (msg->msgid) {
|
switch (msg->msgid) {
|
||||||
case MAVLINK_MSG_ID_HIL_SENSOR:
|
case MAVLINK_MSG_ID_HIL_SENSOR:
|
||||||
|
hil_sensor_counter++;
|
||||||
handle_message_hil_sensor_dsp(msg);
|
handle_message_hil_sensor_dsp(msg);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case MAVLINK_MSG_ID_HIL_GPS:
|
case MAVLINK_MSG_ID_HIL_GPS:
|
||||||
if (_send_gps) { handle_message_hil_gps_dsp(msg); }
|
gps_received_counter++;
|
||||||
|
|
||||||
break;
|
if (position_source_data[(int) position_source::GPS].send) { handle_message_hil_gps_dsp(msg); }
|
||||||
|
|
||||||
case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE:
|
|
||||||
handle_message_vision_position_estimate_dsp(msg);
|
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case MAVLINK_MSG_ID_ODOMETRY:
|
case MAVLINK_MSG_ID_ODOMETRY:
|
||||||
handle_message_odometry_dsp(msg);
|
odometry_received_counter++;
|
||||||
break;
|
|
||||||
|
if (position_source_data[(int) position_source::VIO].send) { handle_message_odometry_dsp(msg); }
|
||||||
|
|
||||||
case MAVLINK_MSG_ID_COMMAND_LONG:
|
|
||||||
handle_message_command_long_dsp(msg);
|
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case MAVLINK_MSG_ID_HEARTBEAT:
|
case MAVLINK_MSG_ID_HEARTBEAT:
|
||||||
PX4_DEBUG("Heartbeat msg received");
|
heartbeat_received_counter++;
|
||||||
|
|
||||||
|
if (_debug) { PX4_INFO("Heartbeat msg received"); }
|
||||||
|
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case MAVLINK_MSG_ID_SYSTEM_TIME:
|
case MAVLINK_MSG_ID_HIL_OPTICAL_FLOW:
|
||||||
PX4_DEBUG("MAVLINK SYSTEM TIME");
|
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); }
|
||||||
|
|
||||||
break;
|
break;
|
||||||
|
|
||||||
default:
|
default:
|
||||||
PX4_DEBUG("Unknown msg ID: %d", msg->msgid);
|
unknown_msg_received_counter++;
|
||||||
|
|
||||||
|
if (_debug) { PX4_INFO("Unknown msg ID: %d", msg->msgid); }
|
||||||
|
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -228,7 +275,6 @@ void send_actuator_data()
|
||||||
bool first_sent = false;
|
bool first_sent = false;
|
||||||
|
|
||||||
while (true) {
|
while (true) {
|
||||||
|
|
||||||
bool controls_updated = false;
|
bool controls_updated = false;
|
||||||
(void) orb_check(_vehicle_control_mode_sub_, &controls_updated);
|
(void) orb_check(_vehicle_control_mode_sub_, &controls_updated);
|
||||||
|
|
||||||
|
@ -239,45 +285,50 @@ void send_actuator_data()
|
||||||
bool actuator_updated = false;
|
bool actuator_updated = false;
|
||||||
(void) orb_check(_actuator_outputs_sub, &actuator_updated);
|
(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) {
|
if (actuator_updated) {
|
||||||
orb_copy(ORB_ID(actuator_outputs), _actuator_outputs_sub, &_actuator_outputs);
|
orb_copy(ORB_ID(actuator_outputs), _actuator_outputs_sub, &_actuator_outputs);
|
||||||
px4_lockstep_wait_for_components();
|
|
||||||
|
|
||||||
if (_actuator_outputs.timestamp > 0) {
|
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_timestamp = _actuator_outputs.timestamp;
|
||||||
previous_uorb_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);
|
newBufLen = mavlink_msg_to_send_buffer(newBuf, &message);
|
||||||
int writeRetval = writeResponse(&newBuf, newBufLen);
|
int writeRetval = writeResponse(&newBuf, newBufLen);
|
||||||
PX4_DEBUG("Succesful write of actuator back to jMAVSim: %d at %llu", writeRetval, hrt_absolute_time());
|
|
||||||
|
actuator_sent_counter++;
|
||||||
|
|
||||||
|
if (_debug) { PX4_INFO("Succesful write of actuator back to jMAVSim: %d at %llu", writeRetval, hrt_absolute_time()); }
|
||||||
|
|
||||||
first_sent = true;
|
first_sent = true;
|
||||||
send_esc_telemetry_dsp(hil_act_control);
|
|
||||||
|
send_esc_status(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();
|
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);
|
newBufLen = mavlink_msg_to_send_buffer(newBuf, &message);
|
||||||
int writeRetval = writeResponse(&newBuf, newBufLen);
|
int writeRetval = writeResponse(&newBuf, newBufLen);
|
||||||
//PX4_INFO("Sending from NOT UPDTE AND TIMEOUT: %i", differential);
|
|
||||||
|
|
||||||
PX4_DEBUG("Succesful write of actuator back to jMAVSim: %d at %llu", writeRetval, hrt_absolute_time());
|
actuator_sent_counter++;
|
||||||
send_esc_telemetry_dsp(hil_act_control);
|
|
||||||
|
if (_debug) { PX4_INFO("Succesful write of actuator back to jMAVSim: %d at %llu", writeRetval, hrt_absolute_time()); }
|
||||||
|
|
||||||
|
send_esc_status(hil_act_control);
|
||||||
}
|
}
|
||||||
|
|
||||||
differential = hrt_absolute_time() - previous_timestamp;
|
differential = hrt_absolute_time() - previous_timestamp;
|
||||||
|
|
||||||
|
px4_usleep(1000);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -287,14 +338,10 @@ void task_main(int argc, char *argv[])
|
||||||
int myoptind = 1;
|
int myoptind = 1;
|
||||||
const char *myoptarg = nullptr;
|
const char *myoptarg = nullptr;
|
||||||
|
|
||||||
while ((ch = px4_getopt(argc, argv, "vsdcmgp:b:", &myoptind, &myoptarg)) != EOF) {
|
while ((ch = px4_getopt(argc, argv, "odmghfp:b:", &myoptind, &myoptarg)) != EOF) {
|
||||||
switch (ch) {
|
switch (ch) {
|
||||||
case 's':
|
|
||||||
_use_software_mav_throttling = true;
|
|
||||||
break;
|
|
||||||
|
|
||||||
case 'd':
|
case 'd':
|
||||||
debug = true;
|
_debug = true;
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case 'p':
|
case 'p':
|
||||||
|
@ -310,7 +357,19 @@ void task_main(int argc, char *argv[])
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case 'g':
|
case 'g':
|
||||||
_send_gps = true;
|
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;
|
||||||
break;
|
break;
|
||||||
|
|
||||||
default:
|
default:
|
||||||
|
@ -319,11 +378,13 @@ void task_main(int argc, char *argv[])
|
||||||
}
|
}
|
||||||
|
|
||||||
const char *charport = port.c_str();
|
const char *charport = port.c_str();
|
||||||
int openRetval = openPort(charport, (speed_t) baudrate);
|
(void) openPort(charport, (speed_t) baudrate);
|
||||||
int open = isOpen();
|
|
||||||
|
|
||||||
if (open) {
|
if ((_debug) && (isOpen())) { PX4_INFO("DSP HITL serial port initialized. Baudrate: %d", baudrate); }
|
||||||
PX4_ERR("Port is open: %d", openRetval);
|
|
||||||
|
if (! isOpen()) {
|
||||||
|
PX4_ERR("DSP HITL failed to open serial port");
|
||||||
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
uint64_t last_heartbeat_timestamp = hrt_absolute_time();
|
uint64_t last_heartbeat_timestamp = hrt_absolute_time();
|
||||||
|
@ -342,14 +403,11 @@ void task_main(int argc, char *argv[])
|
||||||
pthread_attr_destroy(&sender_thread_attr);
|
pthread_attr_destroy(&sender_thread_attr);
|
||||||
|
|
||||||
int _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
|
int _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
|
||||||
PX4_INFO("Got %d from orb_subscribe", _vehicle_status_sub);
|
|
||||||
|
|
||||||
_is_running = true;
|
_is_running = true;
|
||||||
|
|
||||||
while (!_task_should_exit) {
|
while (!_task_should_exit) {
|
||||||
|
|
||||||
uint8_t rx_buf[1024];
|
uint8_t rx_buf[1024];
|
||||||
//rx_buf[511] = '\0';
|
|
||||||
|
|
||||||
uint64_t timestamp = hrt_absolute_time();
|
uint64_t timestamp = hrt_absolute_time();
|
||||||
|
|
||||||
|
@ -357,8 +415,8 @@ void task_main(int argc, char *argv[])
|
||||||
if (got_first_sensor_msg) {
|
if (got_first_sensor_msg) {
|
||||||
uint64_t delta_time = timestamp - last_imu_update_timestamp;
|
uint64_t delta_time = timestamp - last_imu_update_timestamp;
|
||||||
|
|
||||||
if (delta_time > 15000) {
|
if ((imu_counter) && (delta_time > 15000)) {
|
||||||
PX4_ERR("Sending updates at %llu, delta %llu", timestamp, delta_time);
|
PX4_WARN("Sending updates at %llu, delta %llu", timestamp, delta_time);
|
||||||
}
|
}
|
||||||
|
|
||||||
uint64_t _px4_gyro_accel_timestamp = hrt_absolute_time();
|
uint64_t _px4_gyro_accel_timestamp = hrt_absolute_time();
|
||||||
|
@ -396,7 +454,7 @@ void task_main(int argc, char *argv[])
|
||||||
hb_newBufLen = mavlink_msg_to_send_buffer(hb_newBuf, &hb_message);
|
hb_newBufLen = mavlink_msg_to_send_buffer(hb_newBuf, &hb_message);
|
||||||
(void) writeResponse(&hb_newBuf, hb_newBufLen);
|
(void) writeResponse(&hb_newBuf, hb_newBufLen);
|
||||||
last_heartbeat_timestamp = timestamp;
|
last_heartbeat_timestamp = timestamp;
|
||||||
heartbeat_counter++;
|
heartbeat_sent_counter++;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool vehicle_updated = false;
|
bool vehicle_updated = false;
|
||||||
|
@ -416,7 +474,7 @@ void task_main(int argc, char *argv[])
|
||||||
_is_running = false;
|
_is_running = false;
|
||||||
}
|
}
|
||||||
|
|
||||||
void send_esc_telemetry_dsp(mavlink_hil_actuator_controls_t hil_act_control)
|
void send_esc_status(mavlink_hil_actuator_controls_t hil_act_control)
|
||||||
{
|
{
|
||||||
esc_status_s esc_status{};
|
esc_status_s esc_status{};
|
||||||
esc_status.timestamp = hrt_absolute_time();
|
esc_status.timestamp = hrt_absolute_time();
|
||||||
|
@ -448,17 +506,13 @@ void send_esc_telemetry_dsp(mavlink_hil_actuator_controls_t hil_act_control)
|
||||||
_esc_status_pub.publish(esc_status);
|
_esc_status_pub.publish(esc_status);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
void
|
void
|
||||||
handle_message_command_long_dsp(mavlink_message_t *msg)
|
handle_message_command_long_dsp(mavlink_message_t *msg)
|
||||||
{
|
{
|
||||||
/* command */
|
|
||||||
mavlink_command_long_t cmd_mavlink;
|
mavlink_command_long_t cmd_mavlink;
|
||||||
mavlink_msg_command_long_decode(msg, &cmd_mavlink);
|
mavlink_msg_command_long_decode(msg, &cmd_mavlink);
|
||||||
|
|
||||||
if (debug) {
|
if (_debug) { PX4_INFO("Value of command_long.command: %d", cmd_mavlink.command); }
|
||||||
PX4_INFO("Value of command_long.command: %d", cmd_mavlink.command);
|
|
||||||
}
|
|
||||||
|
|
||||||
mavlink_command_ack_t ack = {};
|
mavlink_command_ack_t ack = {};
|
||||||
ack.result = MAV_RESULT_UNSUPPORTED;
|
ack.result = MAV_RESULT_UNSUPPORTED;
|
||||||
|
@ -470,46 +524,140 @@ handle_message_command_long_dsp(mavlink_message_t *msg)
|
||||||
uint16_t acknewBufLen = 0;
|
uint16_t acknewBufLen = 0;
|
||||||
acknewBufLen = mavlink_msg_to_send_buffer(acknewBuf, &ack_message);
|
acknewBufLen = mavlink_msg_to_send_buffer(acknewBuf, &ack_message);
|
||||||
int writeRetval = writeResponse(&acknewBuf, acknewBufLen);
|
int writeRetval = writeResponse(&acknewBuf, acknewBufLen);
|
||||||
PX4_INFO("Succesful write of ACK back over UART: %d at %llu", writeRetval, hrt_absolute_time());
|
|
||||||
|
if (_debug) { PX4_INFO("Succesful write of ACK back over UART: %d at %llu", writeRetval, hrt_absolute_time()); }
|
||||||
}
|
}
|
||||||
|
|
||||||
|
int flow_debug_counter = 0;
|
||||||
|
|
||||||
void
|
void
|
||||||
handle_message_vision_position_estimate_dsp(mavlink_message_t *msg)
|
handle_message_hil_optical_flow(mavlink_message_t *msg)
|
||||||
{
|
{
|
||||||
mavlink_vision_position_estimate_t vpe;
|
mavlink_hil_optical_flow_t flow;
|
||||||
mavlink_msg_vision_position_estimate_decode(msg, &vpe);
|
mavlink_msg_hil_optical_flow_decode(msg, &flow);
|
||||||
|
|
||||||
// fill vehicle_odometry from Mavlink VISION_POSITION_ESTIMATE
|
if ((_debug) && (!(flow_debug_counter % 10))) {
|
||||||
vehicle_odometry_s odom{};
|
PX4_INFO("optflow: time: %llu, quality %d", flow.time_usec, (int) flow.quality);
|
||||||
uint64_t timestamp = hrt_absolute_time();
|
PX4_INFO("optflow: x: %.2f y: %.2f", (double) flow.integrated_x, (double) flow.integrated_y);
|
||||||
odom.timestamp_sample = timestamp;
|
}
|
||||||
|
|
||||||
odom.pose_frame = vehicle_odometry_s::POSE_FRAME_NED;
|
flow_debug_counter++;
|
||||||
odom.position[0] = vpe.x;
|
|
||||||
odom.position[1] = vpe.y;
|
|
||||||
odom.position[2] = vpe.z;
|
|
||||||
|
|
||||||
const matrix::Quatf q(matrix::Eulerf(vpe.roll, vpe.pitch, vpe.yaw));
|
device::Device::DeviceId device_id;
|
||||||
q.copyTo(odom.q);
|
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;
|
||||||
|
|
||||||
// VISION_POSITION_ESTIMATE covariance
|
sensor_optical_flow_s sensor_optical_flow{};
|
||||||
// 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
|
|
||||||
|
|
||||||
odom.orientation_variance[0] = vpe.covariance[15]; // R row 3, col 3
|
sensor_optical_flow.timestamp_sample = hrt_absolute_time();
|
||||||
odom.orientation_variance[1] = vpe.covariance[18]; // P row 4, col 4
|
sensor_optical_flow.device_id = device_id.devid;
|
||||||
odom.orientation_variance[2] = vpe.covariance[20]; // Y row 5, col 5
|
|
||||||
|
|
||||||
odom.reset_counter = vpe.reset_counter;
|
sensor_optical_flow.pixel_flow[0] = flow.integrated_x;
|
||||||
|
sensor_optical_flow.pixel_flow[1] = flow.integrated_y;
|
||||||
|
|
||||||
odom.timestamp = hrt_absolute_time();
|
sensor_optical_flow.integration_timespan_us = flow.integration_time_us;
|
||||||
|
sensor_optical_flow.quality = flow.quality;
|
||||||
|
|
||||||
_visual_odometry_pub.publish(odom);
|
int index = (int) position_source::FLOW;
|
||||||
vision_msg_counter++;
|
|
||||||
|
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++;
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void
|
||||||
|
@ -518,6 +666,8 @@ handle_message_odometry_dsp(mavlink_message_t *msg)
|
||||||
mavlink_odometry_t odom_in;
|
mavlink_odometry_t odom_in;
|
||||||
mavlink_msg_odometry_decode(msg, &odom_in);
|
mavlink_msg_odometry_decode(msg, &odom_in);
|
||||||
|
|
||||||
|
odometry_sent_counter++;
|
||||||
|
|
||||||
// fill vehicle_odometry from Mavlink ODOMETRY
|
// fill vehicle_odometry from Mavlink ODOMETRY
|
||||||
vehicle_odometry_s odom{};
|
vehicle_odometry_s odom{};
|
||||||
uint64_t timestamp = hrt_absolute_time();
|
uint64_t timestamp = hrt_absolute_time();
|
||||||
|
@ -699,6 +849,28 @@ handle_message_odometry_dsp(mavlink_message_t *msg)
|
||||||
odom.reset_counter = odom_in.reset_counter;
|
odom.reset_counter = odom_in.reset_counter;
|
||||||
odom.quality = odom_in.quality;
|
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) {
|
switch (odom_in.estimator_type) {
|
||||||
case MAV_ESTIMATOR_TYPE_UNKNOWN: // accept MAV_ESTIMATOR_TYPE_UNKNOWN for legacy support
|
case MAV_ESTIMATOR_TYPE_UNKNOWN: // accept MAV_ESTIMATOR_TYPE_UNKNOWN for legacy support
|
||||||
case MAV_ESTIMATOR_TYPE_NAIVE:
|
case MAV_ESTIMATOR_TYPE_NAIVE:
|
||||||
|
@ -745,10 +917,6 @@ void actuator_controls_from_outputs_dsp(mavlink_hil_actuator_controls_t *msg)
|
||||||
msg->mode = mode_flag_custom;
|
msg->mode = mode_flag_custom;
|
||||||
msg->mode |= (armed) ? mode_flag_armed : 0;
|
msg->mode |= (armed) ? mode_flag_armed : 0;
|
||||||
msg->flags = 0;
|
msg->flags = 0;
|
||||||
|
|
||||||
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
|
|
||||||
msg->flags |= 1;
|
|
||||||
#endif
|
|
||||||
}
|
}
|
||||||
|
|
||||||
int openPort(const char *dev, speed_t speed)
|
int openPort(const char *dev, speed_t speed)
|
||||||
|
@ -759,7 +927,8 @@ int openPort(const char *dev, speed_t speed)
|
||||||
}
|
}
|
||||||
|
|
||||||
_uart_fd = qurt_uart_open(dev, speed);
|
_uart_fd = qurt_uart_open(dev, speed);
|
||||||
PX4_DEBUG("qurt_uart_opened");
|
|
||||||
|
if (_debug) { PX4_INFO("qurt_uart_opened"); }
|
||||||
|
|
||||||
if (_uart_fd < 0) {
|
if (_uart_fd < 0) {
|
||||||
PX4_ERR("Error opening port: %s (%i)", dev, errno);
|
PX4_ERR("Error opening port: %s (%i)", dev, errno);
|
||||||
|
@ -840,25 +1009,50 @@ int stop()
|
||||||
|
|
||||||
void usage()
|
void usage()
|
||||||
{
|
{
|
||||||
PX4_INFO("Usage: dsp_hitl {start|info|status|stop}");
|
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");
|
||||||
}
|
}
|
||||||
|
|
||||||
int get_status()
|
void print_status()
|
||||||
{
|
{
|
||||||
PX4_INFO("Running: %s", _is_running ? "yes" : "no");
|
PX4_INFO("Running: %s", _is_running ? "yes" : "no");
|
||||||
PX4_INFO("Status of IMU_Data counter: %i", imu_counter);
|
PX4_INFO("HIL Sensor received: %i", hil_sensor_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("IMU updates: %i", imu_counter);
|
||||||
PX4_INFO("Value of current gyro x, y, z data: %f, %f, %f", double(x_gyro), double(y_gyro), double(z_gyro));
|
PX4_INFO("\tCurrent accel x, y, z: %f, %f, %f", double(x_accel), double(y_accel), double(z_accel));
|
||||||
PX4_INFO("Value of HIL_Sensor counter: %i", hil_sensor_counter);
|
PX4_INFO("\tCurrent gyro x, y, z: %f, %f, %f", double(x_gyro), double(y_gyro), double(z_gyro));
|
||||||
PX4_INFO("Value of Heartbeat counter: %i", heartbeat_counter);
|
PX4_INFO("Magnetometer sent: %i", mag_counter);
|
||||||
PX4_INFO("Value of Vision data counter: %i", vision_msg_counter);
|
PX4_INFO("Barometer sent: %i", baro_counter);
|
||||||
PX4_INFO("Value of GPS Data counter: %i", gps_counter);
|
PX4_INFO("Heartbeat received: %i, sent: %i", heartbeat_received_counter, heartbeat_sent_counter);
|
||||||
return 0;
|
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);
|
||||||
}
|
}
|
||||||
|
|
||||||
uint64_t first_sensor_msg_timestamp = 0;
|
void
|
||||||
uint64_t first_sensor_report_timestamp = 0;
|
clear_status_counters()
|
||||||
uint64_t last_sensor_report_timestamp = 0;
|
{
|
||||||
|
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;
|
||||||
|
}
|
||||||
|
|
||||||
void
|
void
|
||||||
handle_message_hil_sensor_dsp(mavlink_message_t *msg)
|
handle_message_hil_sensor_dsp(mavlink_message_t *msg)
|
||||||
|
@ -928,6 +1122,8 @@ handle_message_hil_sensor_dsp(mavlink_message_t *msg)
|
||||||
}
|
}
|
||||||
|
|
||||||
_px4_mag->update(gyro_accel_time, hil_sensor.xmag, hil_sensor.ymag, hil_sensor.zmag);
|
_px4_mag->update(gyro_accel_time, hil_sensor.xmag, hil_sensor.ymag, hil_sensor.zmag);
|
||||||
|
|
||||||
|
mag_counter++;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -942,17 +1138,8 @@ handle_message_hil_sensor_dsp(mavlink_message_t *msg)
|
||||||
sensor_baro.error_count = 0;
|
sensor_baro.error_count = 0;
|
||||||
sensor_baro.timestamp = hrt_absolute_time();
|
sensor_baro.timestamp = hrt_absolute_time();
|
||||||
_sensor_baro_pub.publish(sensor_baro);
|
_sensor_baro_pub.publish(sensor_baro);
|
||||||
}
|
|
||||||
|
|
||||||
// differential pressure
|
baro_counter++;
|
||||||
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
|
// battery status
|
||||||
|
@ -970,7 +1157,6 @@ handle_message_hil_sensor_dsp(mavlink_message_t *msg)
|
||||||
|
|
||||||
_battery_pub.publish(hil_battery_status);
|
_battery_pub.publish(hil_battery_status);
|
||||||
}
|
}
|
||||||
hil_sensor_counter++;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void
|
||||||
|
@ -989,15 +1175,41 @@ handle_message_hil_gps_dsp(mavlink_message_t *msg)
|
||||||
|
|
||||||
gps.device_id = device_id.devid;
|
gps.device_id = device_id.devid;
|
||||||
|
|
||||||
gps.lat = hil_gps.lat;
|
gps.latitude_deg = hil_gps.lat * 1e-7;
|
||||||
gps.lon = hil_gps.lon;
|
gps.longitude_deg = hil_gps.lon * 1e-7;
|
||||||
gps.alt = hil_gps.alt;
|
gps.altitude_msl_m = hil_gps.alt * 1e-3;
|
||||||
gps.alt_ellipsoid = hil_gps.alt;
|
gps.altitude_ellipsoid_m = hil_gps.alt * 1e-3;
|
||||||
|
|
||||||
gps.s_variance_m_s = 0.25f;
|
gps.s_variance_m_s = 0.25f;
|
||||||
gps.c_variance_rad = 0.5f;
|
gps.c_variance_rad = 0.5f;
|
||||||
|
|
||||||
|
gps.satellites_used = hil_gps.satellites_visible;
|
||||||
gps.fix_type = hil_gps.fix_type;
|
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.eph = (float)hil_gps.eph * 1e-2f; // cm -> m
|
||||||
gps.epv = (float)hil_gps.epv * 1e-2f; // cm -> m
|
gps.epv = (float)hil_gps.epv * 1e-2f; // cm -> m
|
||||||
|
|
||||||
|
@ -1021,7 +1233,6 @@ handle_message_hil_gps_dsp(mavlink_message_t *msg)
|
||||||
gps.timestamp_time_relative = 0;
|
gps.timestamp_time_relative = 0;
|
||||||
gps.time_utc_usec = hil_gps.time_usec;
|
gps.time_utc_usec = hil_gps.time_usec;
|
||||||
|
|
||||||
gps.satellites_used = hil_gps.satellites_visible;
|
|
||||||
|
|
||||||
gps.heading = NAN;
|
gps.heading = NAN;
|
||||||
gps.heading_offset = NAN;
|
gps.heading_offset = NAN;
|
||||||
|
@ -1029,10 +1240,51 @@ handle_message_hil_gps_dsp(mavlink_message_t *msg)
|
||||||
gps.timestamp = hrt_absolute_time();
|
gps.timestamp = hrt_absolute_time();
|
||||||
|
|
||||||
_sensor_gps_pub.publish(gps);
|
_sensor_gps_pub.publish(gps);
|
||||||
gps_counter++;
|
|
||||||
|
gps_sent_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 dsp_hitl_main(int argc, char *argv[])
|
||||||
{
|
{
|
||||||
int myoptind = 1;
|
int myoptind = 1;
|
||||||
|
@ -1044,20 +1296,47 @@ int dsp_hitl_main(int argc, char *argv[])
|
||||||
|
|
||||||
const char *verb = argv[myoptind];
|
const char *verb = argv[myoptind];
|
||||||
|
|
||||||
|
|
||||||
if (!strcmp(verb, "start")) {
|
if (!strcmp(verb, "start")) {
|
||||||
return dsp_hitl::start(argc - 1, argv + 1);
|
return dsp_hitl::start(argc - 1, argv + 1);
|
||||||
}
|
|
||||||
|
|
||||||
else if (!strcmp(verb, "stop")) {
|
} else if (!strcmp(verb, "stop")) {
|
||||||
return dsp_hitl::stop();
|
return dsp_hitl::stop();
|
||||||
}
|
|
||||||
|
|
||||||
else if (!strcmp(verb, "status")) {
|
} else if (!strcmp(verb, "status")) {
|
||||||
return dsp_hitl::get_status();
|
dsp_hitl::print_status();
|
||||||
}
|
return 0;
|
||||||
|
|
||||||
else {
|
} 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 {
|
||||||
dsp_hitl::usage();
|
dsp_hitl::usage();
|
||||||
return 1;
|
return 1;
|
||||||
}
|
}
|
||||||
|
|
|
@ -1,991 +0,0 @@
|
||||||
/****************************************************************************
|
|
||||||
*
|
|
||||||
* 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;
|
|
||||||
}
|
|
|
@ -1,235 +0,0 @@
|
||||||
/****************************************************************************
|
|
||||||
*
|
|
||||||
* 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)};
|
|
||||||
|
|
||||||
};
|
|
|
@ -1,430 +0,0 @@
|
||||||
/****************************************************************************
|
|
||||||
*
|
|
||||||
* 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
|
|
|
@ -1,116 +0,0 @@
|
||||||
/****************************************************************************
|
|
||||||
*
|
|
||||||
* 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;
|
|
||||||
}
|
|
|
@ -0,0 +1,34 @@
|
||||||
|
############################################################################
|
||||||
|
#
|
||||||
|
# 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
|
# Link against the public stub version of the proprietary fc sensor library
|
||||||
target_link_libraries(px4 PRIVATE
|
target_link_libraries(px4 PRIVATE
|
||||||
${PX4_SOURCE_DIR}/src//modules/muorb/apps/libfc-sensor-api/build/libfc_sensor.so
|
${PX4_BOARD_DIR}/libfc-sensor-api/build/libfc_sensor.so
|
||||||
px4_layer
|
px4_layer
|
||||||
${module_libraries}
|
${module_libraries}
|
||||||
)
|
)
|
||||||
|
|
|
@ -11,7 +11,6 @@ CONFIG_DRIVERS_VOXL2_IO=y
|
||||||
CONFIG_MODULES_COMMANDER=y
|
CONFIG_MODULES_COMMANDER=y
|
||||||
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
||||||
CONFIG_MODULES_DATAMAN=y
|
CONFIG_MODULES_DATAMAN=y
|
||||||
CONFIG_MODULES_LOAD_MON=y
|
|
||||||
CONFIG_MODULES_LOGGER=y
|
CONFIG_MODULES_LOGGER=y
|
||||||
CONFIG_MODULES_MAVLINK=y
|
CONFIG_MODULES_MAVLINK=y
|
||||||
CONFIG_MODULES_MUORB_APPS=y
|
CONFIG_MODULES_MUORB_APPS=y
|
||||||
|
@ -26,3 +25,4 @@ CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
|
||||||
CONFIG_SYSTEMCMDS_UORB=y
|
CONFIG_SYSTEMCMDS_UORB=y
|
||||||
CONFIG_SYSTEMCMDS_VER=y
|
CONFIG_SYSTEMCMDS_VER=y
|
||||||
CONFIG_ORB_COMMUNICATOR=y
|
CONFIG_ORB_COMMUNICATOR=y
|
||||||
|
CONFIG_PARAM_PRIMARY=y
|
||||||
|
|
|
@ -1,10 +1,9 @@
|
||||||
#!/bin/bash
|
#!/bin/bash
|
||||||
|
|
||||||
cd src/modules/muorb/apps/libfc-sensor-api
|
cd boards/modalai/voxl2/libfc-sensor-api
|
||||||
rm -fR build
|
rm -fR build
|
||||||
mkdir build
|
mkdir build
|
||||||
cd build
|
cd build
|
||||||
CC=/home/4.1.0.4/tools/linaro64/bin/aarch64-linux-gnu-gcc cmake ..
|
CC=/home/4.1.0.4/tools/linaro64/bin/aarch64-linux-gnu-gcc cmake ..
|
||||||
make
|
make
|
||||||
cd ../../../../../..
|
cd ../../../../..
|
||||||
|
|
||||||
|
|
|
@ -76,12 +76,13 @@ qshell flight_mode_manager start
|
||||||
# Start all of the processing modules on the applications processor
|
# Start all of the processing modules on the applications processor
|
||||||
dataman start
|
dataman start
|
||||||
navigator start
|
navigator start
|
||||||
load_mon start
|
|
||||||
|
|
||||||
# Start microdds_client for ros2 offboard messages from agent over localhost
|
# Start microdds_client for ros2 offboard messages from agent over localhost
|
||||||
microdds_client start -t udp -h 127.0.0.1 -p 8888
|
microdds_client start -t udp -h 127.0.0.1 -p 8888
|
||||||
|
|
||||||
qshell pwm_out_sim start -m hil
|
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
|
qshell dsp_hitl start -g -m
|
||||||
|
|
||||||
# start the onboard fast link to connect to voxl-mavlink-server
|
# start the onboard fast link to connect to voxl-mavlink-server
|
||||||
|
|
|
@ -207,7 +207,6 @@ qshell flight_mode_manager start
|
||||||
# Start all of the processing modules on the applications processor
|
# Start all of the processing modules on the applications processor
|
||||||
dataman start
|
dataman start
|
||||||
navigator start
|
navigator start
|
||||||
load_mon start
|
|
||||||
|
|
||||||
# This bridge allows raw data packets to be sent over UART to the ESC
|
# This bridge allows raw data packets to be sent over UART to the ESC
|
||||||
voxl2_io_bridge start
|
voxl2_io_bridge start
|
||||||
|
|
|
@ -1,9 +1,11 @@
|
||||||
CONFIG_BOARD_TOOLCHAIN="arm-none-eabi"
|
CONFIG_BOARD_TOOLCHAIN="arm-none-eabi"
|
||||||
CONFIG_BOARD_ARCHITECTURE="cortex-m7"
|
CONFIG_BOARD_ARCHITECTURE="cortex-m7"
|
||||||
CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS2"
|
CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS3"
|
||||||
CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS0"
|
CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS4"
|
||||||
CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS1"
|
CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS1"
|
||||||
CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS4"
|
CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS2"
|
||||||
|
CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS5"
|
||||||
|
CONFIG_BOARD_SERIAL_TEL4="/dev/ttyS0"
|
||||||
CONFIG_DRIVERS_ADC_ADS1115=y
|
CONFIG_DRIVERS_ADC_ADS1115=y
|
||||||
CONFIG_DRIVERS_ADC_BOARD_ADC=y
|
CONFIG_DRIVERS_ADC_BOARD_ADC=y
|
||||||
CONFIG_DRIVERS_BAROMETER_DPS310=y
|
CONFIG_DRIVERS_BAROMETER_DPS310=y
|
||||||
|
@ -25,6 +27,7 @@ CONFIG_DRIVERS_PCA9685_PWM_OUT=y
|
||||||
CONFIG_DRIVERS_POWER_MONITOR_INA226=y
|
CONFIG_DRIVERS_POWER_MONITOR_INA226=y
|
||||||
CONFIG_DRIVERS_PWM_OUT=y
|
CONFIG_DRIVERS_PWM_OUT=y
|
||||||
CONFIG_DRIVERS_RC_INPUT=y
|
CONFIG_DRIVERS_RC_INPUT=y
|
||||||
|
CONFIG_DRIVERS_SAFETY_BUTTON=y
|
||||||
CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
|
CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
|
||||||
CONFIG_COMMON_TELEMETRY=y
|
CONFIG_COMMON_TELEMETRY=y
|
||||||
CONFIG_DRIVERS_TONE_ALARM=y
|
CONFIG_DRIVERS_TONE_ALARM=y
|
||||||
|
@ -46,7 +49,6 @@ CONFIG_MODULES_FW_POS_CONTROL=y
|
||||||
CONFIG_MODULES_FW_RATE_CONTROL=y
|
CONFIG_MODULES_FW_RATE_CONTROL=y
|
||||||
CONFIG_MODULES_GIMBAL=y
|
CONFIG_MODULES_GIMBAL=y
|
||||||
CONFIG_MODULES_GYRO_CALIBRATION=y
|
CONFIG_MODULES_GYRO_CALIBRATION=y
|
||||||
CONFIG_MODULES_GYRO_FFT=y
|
|
||||||
CONFIG_MODULES_LAND_DETECTOR=y
|
CONFIG_MODULES_LAND_DETECTOR=y
|
||||||
CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
|
CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
|
||||||
CONFIG_MODULES_LOAD_MON=y
|
CONFIG_MODULES_LOAD_MON=y
|
||||||
|
@ -96,4 +98,3 @@ CONFIG_SYSTEMCMDS_UORB=y
|
||||||
CONFIG_SYSTEMCMDS_USB_CONNECTED=y
|
CONFIG_SYSTEMCMDS_USB_CONNECTED=y
|
||||||
CONFIG_SYSTEMCMDS_VER=y
|
CONFIG_SYSTEMCMDS_VER=y
|
||||||
CONFIG_SYSTEMCMDS_WORK_QUEUE=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_V_DIV 10.1
|
||||||
param set-default BAT1_A_PER_V 17
|
param set-default BAT1_A_PER_V 17
|
||||||
|
|
||||||
safety_button start
|
param set-default TEL_FRSKY_CONFIG 103
|
||||||
|
|
|
@ -16,3 +16,5 @@ icm20948 -s -b 1 -R 8 -M start
|
||||||
|
|
||||||
# Interal DPS310 (barometer)
|
# Interal DPS310 (barometer)
|
||||||
dps310 -s -b 2 start
|
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_DIR_RELPATH=y
|
||||||
CONFIG_ARCH_BOARD_CUSTOM_NAME="px4"
|
CONFIG_ARCH_BOARD_CUSTOM_NAME="px4"
|
||||||
CONFIG_ARCH_CHIP="stm32h7"
|
CONFIG_ARCH_CHIP="stm32h7"
|
||||||
CONFIG_ARCH_CHIP_STM32H743II=y
|
CONFIG_ARCH_CHIP_STM32H743ZI=y
|
||||||
CONFIG_ARCH_CHIP_STM32H7=y
|
CONFIG_ARCH_CHIP_STM32H7=y
|
||||||
CONFIG_ARCH_INTERRUPTSTACK=768
|
CONFIG_ARCH_INTERRUPTSTACK=768
|
||||||
CONFIG_ARMV7M_BASEPRI_WAR=y
|
CONFIG_ARMV7M_BASEPRI_WAR=y
|
||||||
|
|
|
@ -222,6 +222,9 @@
|
||||||
|
|
||||||
|
|
||||||
/* UART/USART */
|
/* 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_TX GPIO_USART2_TX_2 /* PD5 */
|
||||||
#define GPIO_USART2_RX GPIO_USART2_RX_2 /* PD6 */
|
#define GPIO_USART2_RX GPIO_USART2_RX_2 /* PD6 */
|
||||||
#define GPIO_USART2_CTS GPIO_USART2_CTS_NSS_2 /* PD3 */
|
#define GPIO_USART2_CTS GPIO_USART2_CTS_NSS_2 /* PD3 */
|
||||||
|
@ -235,9 +238,6 @@
|
||||||
#define GPIO_UART4_TX GPIO_UART4_TX_2 /* PA0 */
|
#define GPIO_UART4_TX GPIO_UART4_TX_2 /* PA0 */
|
||||||
#define GPIO_UART4_RX GPIO_UART4_RX_2 /* PA1 */
|
#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_TX GPIO_UART7_TX_3 /* PE8 */
|
||||||
#define GPIO_UART7_RX GPIO_UART7_RX_3 /* PE7 */
|
#define GPIO_UART7_RX GPIO_UART7_RX_3 /* PE7 */
|
||||||
|
|
||||||
|
@ -268,13 +268,14 @@
|
||||||
#define GPIO_SPI5_MISO GPIO_SPI5_MISO_1 /* PF8 */
|
#define GPIO_SPI5_MISO GPIO_SPI5_MISO_1 /* PF8 */
|
||||||
#define GPIO_SPI5_MOSI GPIO_SPI5_MOSI_2 /* PF9 */
|
#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 */
|
/* I2C */
|
||||||
#define GPIO_I2C1_SCL GPIO_I2C1_SCL_2 /* PB8 */
|
#define GPIO_I2C1_SCL GPIO_I2C1_SCL_2 /* PB8 */
|
||||||
#define GPIO_I2C1_SDA GPIO_I2C1_SDA_2 /* PB9 */
|
#define GPIO_I2C1_SDA GPIO_I2C1_SDA_2 /* PB9 */
|
||||||
|
|
||||||
#define GPIO_I2C3_SCL GPIO_I2C3_SCL_2 /* PH7 */
|
#define GPIO_I2C4_SCL GPIO_I2C4_SCL_2 /* PF14 */
|
||||||
#define GPIO_I2C3_SDA GPIO_I2C3_SDA_2 /* PH8 */
|
#define GPIO_I2C4_SDA GPIO_I2C4_SDA_2 /* PF15 */
|
||||||
|
|
||||||
#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_DIR_RELPATH=y
|
||||||
CONFIG_ARCH_BOARD_CUSTOM_NAME="px4"
|
CONFIG_ARCH_BOARD_CUSTOM_NAME="px4"
|
||||||
CONFIG_ARCH_CHIP="stm32h7"
|
CONFIG_ARCH_CHIP="stm32h7"
|
||||||
CONFIG_ARCH_CHIP_STM32H743II=y
|
CONFIG_ARCH_CHIP_STM32H743ZI=y
|
||||||
CONFIG_ARCH_CHIP_STM32H7=y
|
CONFIG_ARCH_CHIP_STM32H7=y
|
||||||
CONFIG_ARCH_INTERRUPTSTACK=768
|
CONFIG_ARCH_INTERRUPTSTACK=768
|
||||||
CONFIG_ARCH_STACKDUMP=y
|
CONFIG_ARCH_STACKDUMP=y
|
||||||
|
@ -192,7 +192,6 @@ CONFIG_STM32H7_DMA2=y
|
||||||
CONFIG_STM32H7_DMACAPABLE=y
|
CONFIG_STM32H7_DMACAPABLE=y
|
||||||
CONFIG_STM32H7_FLOWCONTROL_BROKEN=y
|
CONFIG_STM32H7_FLOWCONTROL_BROKEN=y
|
||||||
CONFIG_STM32H7_I2C1=y
|
CONFIG_STM32H7_I2C1=y
|
||||||
CONFIG_STM32H7_I2C3=y
|
|
||||||
CONFIG_STM32H7_I2C4=y
|
CONFIG_STM32H7_I2C4=y
|
||||||
CONFIG_STM32H7_I2C_DYNTIMEO=y
|
CONFIG_STM32H7_I2C_DYNTIMEO=y
|
||||||
CONFIG_STM32H7_I2C_DYNTIMEO_STARTSTOP=10
|
CONFIG_STM32H7_I2C_DYNTIMEO_STARTSTOP=10
|
||||||
|
@ -212,17 +211,20 @@ CONFIG_STM32H7_SPI2=y
|
||||||
CONFIG_STM32H7_SPI5=y
|
CONFIG_STM32H7_SPI5=y
|
||||||
CONFIG_STM32H7_SPI5_DMA=y
|
CONFIG_STM32H7_SPI5_DMA=y
|
||||||
CONFIG_STM32H7_SPI5_DMA_BUFFER=1024
|
CONFIG_STM32H7_SPI5_DMA_BUFFER=1024
|
||||||
|
CONFIG_STM32H7_SPI6=y
|
||||||
|
CONFIG_STM32H7_TIM15=y
|
||||||
|
CONFIG_STM32H7_TIM16=y
|
||||||
CONFIG_STM32H7_TIM1=y
|
CONFIG_STM32H7_TIM1=y
|
||||||
CONFIG_STM32H7_TIM2=y
|
CONFIG_STM32H7_TIM2=y
|
||||||
CONFIG_STM32H7_TIM3=y
|
CONFIG_STM32H7_TIM3=y
|
||||||
CONFIG_STM32H7_TIM4=y
|
CONFIG_STM32H7_TIM4=y
|
||||||
CONFIG_STM32H7_TIM8=y
|
CONFIG_STM32H7_TIM8=y
|
||||||
|
CONFIG_STM32H7_USART1=y
|
||||||
|
CONFIG_STM32H7_USART2=y
|
||||||
|
CONFIG_STM32H7_USART3=y
|
||||||
CONFIG_STM32H7_UART4=y
|
CONFIG_STM32H7_UART4=y
|
||||||
CONFIG_STM32H7_UART7=y
|
CONFIG_STM32H7_UART7=y
|
||||||
CONFIG_STM32H7_UART8=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_BREAKS=y
|
||||||
CONFIG_STM32H7_USART_INVERT=y
|
CONFIG_STM32H7_USART_INVERT=y
|
||||||
CONFIG_STM32H7_USART_SINGLEWIRE=y
|
CONFIG_STM32H7_USART_SINGLEWIRE=y
|
||||||
|
@ -252,9 +254,6 @@ CONFIG_USART3_IFLOWCONTROL=y
|
||||||
CONFIG_USART3_OFLOWCONTROL=y
|
CONFIG_USART3_OFLOWCONTROL=y
|
||||||
CONFIG_USART3_RXBUFSIZE=600
|
CONFIG_USART3_RXBUFSIZE=600
|
||||||
CONFIG_USART3_TXBUFSIZE=3000
|
CONFIG_USART3_TXBUFSIZE=3000
|
||||||
CONFIG_USART6_BAUD=57600
|
|
||||||
CONFIG_USART6_RXBUFSIZE=600
|
|
||||||
CONFIG_USART6_TXBUFSIZE=1500
|
|
||||||
CONFIG_USBDEV=y
|
CONFIG_USBDEV=y
|
||||||
CONFIG_USBDEV_BUSPOWERED=y
|
CONFIG_USBDEV_BUSPOWERED=y
|
||||||
CONFIG_USBDEV_MAXPOWER=500
|
CONFIG_USBDEV_MAXPOWER=500
|
||||||
|
|
|
@ -45,95 +45,111 @@
|
||||||
#include <stm32_gpio.h>
|
#include <stm32_gpio.h>
|
||||||
|
|
||||||
/* LEDs are driven with push open drain to support Anode to 5V or 3.3V */
|
/* LEDs are driven with push open drain to support Anode to 5V or 3.3V */
|
||||||
#define GPIO_nLED_RED /* PB11 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN11)
|
#define GPIO_nLED_RED /* PB4 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN4)
|
||||||
#define GPIO_nLED_GREEN /* PB1 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN1)
|
#define GPIO_nLED_GREEN /* PB5 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN5)
|
||||||
#define GPIO_nLED_BLUE /* PB3 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN3)
|
#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_HAS_CONTROL_STATUS_LEDS 1
|
||||||
#define BOARD_OVERLOAD_LED LED_RED
|
#define BOARD_OVERLOAD_LED LED_RED
|
||||||
#define BOARD_ARMED_STATE_LED LED_BLUE
|
#define BOARD_ARMED_STATE_LED LED_BLUE
|
||||||
|
|
||||||
/* ADC channels */
|
/* ADC channels */
|
||||||
#define PX4_ADC_GPIO \
|
#define PX4_ADC_GPIO \
|
||||||
/* PA2 */ GPIO_ADC12_INP14, \
|
/* PC4 */ GPIO_ADC12_INP4, \
|
||||||
/* PA3 */ GPIO_ADC12_INP15, \
|
/* PA3 */ GPIO_ADC12_INP15, \
|
||||||
/* PA4 */ GPIO_ADC12_INP18, \
|
/* PA4 */ GPIO_ADC12_INP18, \
|
||||||
/* PC1 */ GPIO_ADC123_INP11
|
/* PC0 */ GPIO_ADC123_INP10, \
|
||||||
|
/* PC5 */ GPIO_ADC12_INP8, \
|
||||||
|
/* PB0 */ GPIO_ADC12_INP9, \
|
||||||
|
/* PB1 */ GPIO_ADC12_INP5
|
||||||
|
|
||||||
/* Define Channel numbers must match above GPIO pins */
|
/* Define Channel numbers must match above GPIO pins */
|
||||||
#define ADC_BATTERY_VOLTAGE_CHANNEL 14 /* PA2 BATT_VOLT_SENS */
|
#define ADC_BATTERY_VOLTAGE_CHANNEL 4 /* PC4 BATT_VOLT_SENS */
|
||||||
#define ADC_BATTERY_CURRENT_CHANNEL 15 /* PA3 BATT_CURRENT_SENS */
|
#define ADC_BATTERY_CURRENT_CHANNEL 15 /* PA3 BATT_CURRENT_SENS */
|
||||||
#define ADC_SCALED_V5_CHANNEL 18 /* PA4 VDD_5V_SENS */
|
#define ADC_SCALED_V5_CHANNEL 18 /* PA4 VDD_5V_SENS */
|
||||||
#define ADC_RC_RSSI_CHANNEL 11 /* PC1 */
|
#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_CHANNELS \
|
#define ADC_CHANNELS \
|
||||||
((1 << ADC_BATTERY_VOLTAGE_CHANNEL) | \
|
((1 << ADC_BATTERY_VOLTAGE_CHANNEL) | \
|
||||||
(1 << ADC_BATTERY_CURRENT_CHANNEL) | \
|
(1 << ADC_BATTERY_CURRENT_CHANNEL) | \
|
||||||
(1 << ADC_SCALED_V5_CHANNEL) | \
|
(1 << ADC_SCALED_V5_CHANNEL) | \
|
||||||
(1 << ADC_RC_RSSI_CHANNEL))
|
(1 << ADC_RC_RSSI_CHANNEL) | \
|
||||||
|
(1 << ADC_AIRSPEED_VOLTAGE_CHANNEL) | \
|
||||||
|
(1 << ADC_AUX1_VOLTAGE_CHANNEL) | \
|
||||||
|
(1 << ADC_AUX2_VOLTAGE_CHANNEL))
|
||||||
|
|
||||||
/* HW has to large of R termination on ADC todo:change when HW value is chosen */
|
/* HW has to large of R termination on ADC todo:change when HW value is chosen */
|
||||||
#define BOARD_ADC_OPEN_CIRCUIT_V (5.6f)
|
#define BOARD_ADC_OPEN_CIRCUIT_V (5.6f)
|
||||||
|
|
||||||
/* CAN Silence: Silent mode control */
|
/* CAN Silence: Silent mode control */
|
||||||
#define GPIO_CAN1_SILENT_S0 /* PF5 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN5)
|
#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 /* PF5 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN5)
|
#define GPIO_CAN2_SILENT_S0 /* PF12 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN12)
|
||||||
|
|
||||||
/* PWM */
|
/* PWM */
|
||||||
#define DIRECT_PWM_OUTPUT_CHANNELS 12
|
#define DIRECT_PWM_OUTPUT_CHANNELS 12
|
||||||
|
|
||||||
/* Power supply control and monitoring GPIOs */
|
/* Power supply control and monitoring GPIOs */
|
||||||
#define GPIO_nPOWER_IN_A /* PB5 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5)
|
#define GPIO_nPOWER_IN_A /* PC1 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN1)
|
||||||
|
|
||||||
#define GPIO_VDD_BRICK1_VALID GPIO_nPOWER_IN_A /* Brick 1 Is Chosen */
|
#define GPIO_VDD_BRICK1_VALID GPIO_nPOWER_IN_A /* Brick 1 Is Chosen */
|
||||||
#define BOARD_NUMBER_BRICKS 1
|
#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_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 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 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)
|
#define READ_VDD_3V3_SPEKTRUM_POWER_EN() (px4_arch_gpioread(GPIO_VDD_3V3_SPEKTRUM_POWER_EN) == 0)
|
||||||
|
|
||||||
/* Tone alarm output */
|
/* Tone alarm output */
|
||||||
#define TONE_ALARM_TIMER 2 /* timer 2 */
|
#define TONE_ALARM_TIMER 16 /* timer 16 */
|
||||||
#define TONE_ALARM_CHANNEL 1 /* PA15 TIM2_CH1 */
|
#define TONE_ALARM_CHANNEL 1 /* PF6 TIM16_CH1 */
|
||||||
|
|
||||||
#define GPIO_BUZZER_1 /* PA15 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN15)
|
#define GPIO_BUZZER_1 /* PF6 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN6)
|
||||||
|
|
||||||
#define GPIO_TONE_ALARM_IDLE GPIO_BUZZER_1
|
#define GPIO_TONE_ALARM_IDLE GPIO_BUZZER_1
|
||||||
#define GPIO_TONE_ALARM GPIO_TIM2_CH1OUT_2
|
#define GPIO_TONE_ALARM GPIO_TIM16_CH1OUT_2
|
||||||
|
|
||||||
/* USB OTG FS */
|
/* USB OTG FS */
|
||||||
#define GPIO_OTGFS_VBUS /* PA9 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_100MHz|GPIO_PORTA|GPIO_PIN9)
|
#define GPIO_OTGFS_VBUS /* PA9 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_100MHz|GPIO_PORTA|GPIO_PIN9)
|
||||||
|
|
||||||
/* High-resolution timer */
|
/* High-resolution timer */
|
||||||
#define HRT_TIMER 3 /* use timer3 for the HRT */
|
#define HRT_TIMER 3 /* use timer3 for the HRT */
|
||||||
#define HRT_TIMER_CHANNEL 1 /* use capture/compare channel 1 */
|
#define HRT_TIMER_CHANNEL 2 /* use capture/compare channel 1 */
|
||||||
|
|
||||||
#define HRT_PPM_CHANNEL /* T3C3 */ 3 /* use capture/compare channel 3 */
|
#define HRT_PPM_CHANNEL /* T3C2 */ 2 /* use capture/compare channel 2 */
|
||||||
#define GPIO_PPM_IN /* PB0 T3C3 */ GPIO_TIM3_CH3IN_1
|
#define GPIO_PPM_IN /* PC7 T3C2 */ GPIO_TIM3_CH2IN_3
|
||||||
|
|
||||||
/* RC Serial port */
|
/* RC Serial port */
|
||||||
#define RC_SERIAL_PORT "/dev/ttyS3"
|
#define RC_SERIAL_PORT "/dev/ttyS5"
|
||||||
|
|
||||||
#define GPIO_RSSI_IN /* PC1 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN1)
|
#define GPIO_RSSI_IN /* PC0 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN0)
|
||||||
|
|
||||||
/* Safety Switch: Enable the FMU to control it if there is no px4io fixme:This should be BOARD_SAFETY_LED(__ontrue) */
|
/* 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 /* PC4 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN4)
|
#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_SAFTY_BUTTON() */
|
/* Enable the FMU to use the switch it if there is no px4io fixme:This should be BOARD_SAFETY_BUTTON() */
|
||||||
#define GPIO_BTN_SAFETY GPIO_SAFETY_SWITCH_IN /* Enable the FMU to control it if there is no px4io */
|
#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 ******************************************************/
|
/* Power switch controls ******************************************************/
|
||||||
#define SPEKTRUM_POWER(_on_true) VDD_3V3_SPEKTRUM_POWER_EN(_on_true)
|
#define SPEKTRUM_POWER(_on_true) VDD_3V3_SPEKTRUM_POWER_EN(_on_true)
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* Board has a separate RC_IN
|
* Board has a separate RC_IN
|
||||||
*
|
*
|
||||||
* GPIO PPM_IN on PB0 T3CH3
|
* GPIO PPM_IN on PC7 T3CH2
|
||||||
* SPEKTRUM_RX (it's TX or RX in Bind) on UART6 PC7
|
* 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
|
* 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_PORTB|GPIO_PIN0)
|
#define GPIO_PPM_IN_AS_OUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN7)
|
||||||
#define SPEKTRUM_RX_AS_GPIO_OUTPUT() px4_arch_configgpio(GPIO_PPM_IN_AS_OUT)
|
#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_RX_AS_UART() /* Can be left as uart */
|
||||||
#define SPEKTRUM_OUT(_one_true) px4_arch_gpiowrite(GPIO_PPM_IN_AS_OUT, (_one_true))
|
#define SPEKTRUM_OUT(_one_true) px4_arch_gpiowrite(GPIO_PPM_IN_AS_OUT, (_one_true))
|
||||||
|
@ -156,7 +172,7 @@
|
||||||
#define BOARD_HAS_STATIC_MANIFEST 1
|
#define BOARD_HAS_STATIC_MANIFEST 1
|
||||||
|
|
||||||
|
|
||||||
#define BOARD_NUM_IO_TIMERS 5
|
#define BOARD_NUM_IO_TIMERS 6
|
||||||
|
|
||||||
|
|
||||||
#define BOARD_ENABLE_CONSOLE_BUFFER
|
#define BOARD_ENABLE_CONSOLE_BUFFER
|
||||||
|
@ -171,9 +187,12 @@
|
||||||
GPIO_CAN2_SILENT_S0, \
|
GPIO_CAN2_SILENT_S0, \
|
||||||
GPIO_nPOWER_IN_A, \
|
GPIO_nPOWER_IN_A, \
|
||||||
GPIO_VDD_3V3_SPEKTRUM_POWER_EN, \
|
GPIO_VDD_3V3_SPEKTRUM_POWER_EN, \
|
||||||
|
GPIO_LEVEL_SHIFTER_OE, \
|
||||||
GPIO_TONE_ALARM_IDLE, \
|
GPIO_TONE_ALARM_IDLE, \
|
||||||
GPIO_SAFETY_SWITCH_IN, \
|
GPIO_SAFETY_SWITCH_IN, \
|
||||||
GPIO_OTGFS_VBUS, \
|
GPIO_OTGFS_VBUS, \
|
||||||
|
GPIO_BTN_SAFETY, \
|
||||||
|
GPIO_LED_SAFETY, \
|
||||||
}
|
}
|
||||||
|
|
||||||
__BEGIN_DECLS
|
__BEGIN_DECLS
|
||||||
|
|
|
@ -35,6 +35,5 @@
|
||||||
|
|
||||||
constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = {
|
constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = {
|
||||||
initI2CBusExternal(1),
|
initI2CBusExternal(1),
|
||||||
initI2CBusExternal(3),
|
|
||||||
initI2CBusExternal(4),
|
initI2CBusExternal(4),
|
||||||
};
|
};
|
||||||
|
|
|
@ -37,7 +37,7 @@
|
||||||
|
|
||||||
constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = {
|
constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = {
|
||||||
initSPIBus(SPI::Bus::SPI1, {
|
initSPIBus(SPI::Bus::SPI1, {
|
||||||
initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortC, GPIO::Pin2}, SPI::DRDY{GPIO::PortD, GPIO::Pin15}),
|
initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortG, GPIO::Pin3}, SPI::DRDY{GPIO::PortG, GPIO::Pin2}),
|
||||||
initSPIDevice(DRV_IMU_DEVTYPE_ICM20948, SPI::CS{GPIO::PortE, GPIO::Pin15}, SPI::DRDY{GPIO::PortE, GPIO::Pin12}),
|
initSPIDevice(DRV_IMU_DEVTYPE_ICM20948, SPI::CS{GPIO::PortE, GPIO::Pin15}, SPI::DRDY{GPIO::PortE, GPIO::Pin12}),
|
||||||
}, {GPIO::PortE, GPIO::Pin3}),
|
}, {GPIO::PortE, GPIO::Pin3}),
|
||||||
initSPIBus(SPI::Bus::SPI2, {
|
initSPIBus(SPI::Bus::SPI2, {
|
||||||
|
@ -46,7 +46,10 @@ constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = {
|
||||||
}),
|
}),
|
||||||
initSPIBus(SPI::Bus::SPI5, {
|
initSPIBus(SPI::Bus::SPI5, {
|
||||||
initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortF, GPIO::Pin10}, SPI::DRDY{GPIO::PortF, GPIO::Pin3}),
|
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::Pin6}, SPI::DRDY{GPIO::PortF, GPIO::Pin1}),
|
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}),
|
||||||
}),
|
}),
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
|
@ -33,6 +33,28 @@
|
||||||
|
|
||||||
#include <px4_arch/io_timer_hw_description.h>
|
#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] = {
|
constexpr io_timers_t io_timers[MAX_IO_TIMERS] = {
|
||||||
initIOTimer(Timer::Timer1, DMA{DMA::Index1}),
|
initIOTimer(Timer::Timer1, DMA{DMA::Index1}),
|
||||||
initIOTimer(Timer::Timer4, 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.
|
@ -81,7 +81,6 @@ CONFIG_MODULES_NAVIGATOR=y
|
||||||
CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF=y
|
CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF=y
|
||||||
CONFIG_MODULES_PAYLOAD_DELIVERER=y
|
CONFIG_MODULES_PAYLOAD_DELIVERER=y
|
||||||
CONFIG_MODULES_RC_UPDATE=y
|
CONFIG_MODULES_RC_UPDATE=y
|
||||||
CONFIG_MODULES_ROVER_POS_CONTROL=y
|
|
||||||
CONFIG_MODULES_SENSORS=y
|
CONFIG_MODULES_SENSORS=y
|
||||||
CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
|
CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
|
||||||
CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
|
CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
|
||||||
|
@ -102,9 +101,7 @@ CONFIG_SYSTEMCMDS_NSHTERM=y
|
||||||
CONFIG_SYSTEMCMDS_PARAM=y
|
CONFIG_SYSTEMCMDS_PARAM=y
|
||||||
CONFIG_SYSTEMCMDS_PERF=y
|
CONFIG_SYSTEMCMDS_PERF=y
|
||||||
CONFIG_SYSTEMCMDS_REBOOT=y
|
CONFIG_SYSTEMCMDS_REBOOT=y
|
||||||
CONFIG_SYSTEMCMDS_REFLECT=y
|
|
||||||
CONFIG_SYSTEMCMDS_SD_BENCH=y
|
CONFIG_SYSTEMCMDS_SD_BENCH=y
|
||||||
CONFIG_SYSTEMCMDS_SD_STRESS=y
|
|
||||||
CONFIG_SYSTEMCMDS_SYSTEM_TIME=y
|
CONFIG_SYSTEMCMDS_SYSTEM_TIME=y
|
||||||
CONFIG_SYSTEMCMDS_TOP=y
|
CONFIG_SYSTEMCMDS_TOP=y
|
||||||
CONFIG_SYSTEMCMDS_TOPIC_LISTENER=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_INA228 0
|
||||||
param set-default SENS_EN_INA226 1
|
param set-default SENS_EN_INA226 1
|
||||||
|
|
||||||
if ver hwbasecmp 008 009 00a 010
|
if ver hwbasecmp 008 009 00a 010 011
|
||||||
then
|
then
|
||||||
# Skynode: use the "custom participant" config for uxrce_dds_client
|
# Skynode: use the "custom participant" config for uxrce_dds_client
|
||||||
param set-default UXRCE_DDS_PTCFG 2
|
param set-default UXRCE_DDS_PTCFG 2
|
||||||
|
|
|
@ -3,7 +3,7 @@
|
||||||
# board specific MAVLink startup script.
|
# board specific MAVLink startup script.
|
||||||
#------------------------------------------------------------------------------
|
#------------------------------------------------------------------------------
|
||||||
|
|
||||||
if ver hwbasecmp 008 009 00a 010
|
if ver hwbasecmp 008 009 00a 010 011
|
||||||
then
|
then
|
||||||
# Start MAVLink on the UART connected to the mission computer
|
# 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
|
mavlink start -d /dev/ttyS4 -b 3000000 -r 290000 -m onboard_low_bandwidth -x -z
|
||||||
|
|
|
@ -49,7 +49,7 @@ then
|
||||||
fi
|
fi
|
||||||
fi
|
fi
|
||||||
|
|
||||||
if ver hwbasecmp 008 009 00a 010
|
if ver hwbasecmp 008 009 00a 010 011
|
||||||
then
|
then
|
||||||
#SKYNODE base fmu board orientation
|
#SKYNODE base fmu board orientation
|
||||||
|
|
||||||
|
|
Binary file not shown.
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_INA228 0
|
||||||
param set-default SENS_EN_INA226 0
|
param set-default SENS_EN_INA226 0
|
||||||
|
|
||||||
if ver hwbasecmp 009 010
|
if ver hwbasecmp 009 010 011
|
||||||
then
|
then
|
||||||
# Skynode: use the "custom participant" config for uxrce_dds_client
|
# Skynode: use the "custom participant" config for uxrce_dds_client
|
||||||
param set-default UXRCE_DDS_PTCFG 2
|
param set-default UXRCE_DDS_PTCFG 2
|
||||||
|
|
|
@ -4,7 +4,7 @@
|
||||||
#------------------------------------------------------------------------------
|
#------------------------------------------------------------------------------
|
||||||
|
|
||||||
# if skynode base board is detected start Mavlink on Telem2
|
# if skynode base board is detected start Mavlink on Telem2
|
||||||
if ver hwbasecmp 009 010
|
if ver hwbasecmp 009 010 011
|
||||||
then
|
then
|
||||||
mavlink start -d /dev/ttyS4 -b 3000000 -r 290000 -m onboard_low_bandwidth -x -z
|
mavlink start -d /dev/ttyS4 -b 3000000 -r 290000 -m onboard_low_bandwidth -x -z
|
||||||
|
|
||||||
|
|
|
@ -56,7 +56,7 @@ then
|
||||||
fi
|
fi
|
||||||
|
|
||||||
#Start Auterion Power Module selector for Skynode boards
|
#Start Auterion Power Module selector for Skynode boards
|
||||||
if ver hwbasecmp 009 010
|
if ver hwbasecmp 009 010 011
|
||||||
then
|
then
|
||||||
pm_selector_auterion start
|
pm_selector_auterion start
|
||||||
else
|
else
|
||||||
|
@ -93,7 +93,7 @@ else
|
||||||
icm20649 -s -R 6 start
|
icm20649 -s -R 6 start
|
||||||
else
|
else
|
||||||
# Internal SPI BMI088
|
# Internal SPI BMI088
|
||||||
if ver hwbasecmp 009 010
|
if ver hwbasecmp 009 010 011
|
||||||
then
|
then
|
||||||
bmi088 -A -R 6 -s start
|
bmi088 -A -R 6 -s start
|
||||||
bmi088 -G -R 6 -s start
|
bmi088 -G -R 6 -s start
|
||||||
|
@ -110,7 +110,7 @@ else
|
||||||
fi
|
fi
|
||||||
|
|
||||||
# Internal SPI bus ICM42688p
|
# Internal SPI bus ICM42688p
|
||||||
if ver hwbasecmp 009 010
|
if ver hwbasecmp 009 010 011
|
||||||
then
|
then
|
||||||
icm42688p -R 12 -s start
|
icm42688p -R 12 -s start
|
||||||
else
|
else
|
||||||
|
@ -127,7 +127,7 @@ else
|
||||||
# Internal SPI bus ICM-42670-P (hard-mounted)
|
# Internal SPI bus ICM-42670-P (hard-mounted)
|
||||||
icm42670p -R 10 -s start
|
icm42670p -R 10 -s start
|
||||||
else
|
else
|
||||||
if ver hwbasecmp 009 010
|
if ver hwbasecmp 009 010 011
|
||||||
then
|
then
|
||||||
icm20602 -R 6 -s start
|
icm20602 -R 6 -s start
|
||||||
else
|
else
|
||||||
|
|
|
@ -6,21 +6,30 @@ CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS3"
|
||||||
CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS2"
|
CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS2"
|
||||||
CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS5"
|
CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS5"
|
||||||
CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS6"
|
CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS6"
|
||||||
|
CONFIG_DRIVERS_ADC_ADS1115=y
|
||||||
CONFIG_DRIVERS_ADC_BOARD_ADC=y
|
CONFIG_DRIVERS_ADC_BOARD_ADC=y
|
||||||
CONFIG_DRIVERS_BAROMETER_BMP388=y
|
CONFIG_DRIVERS_BAROMETER_BMP388=y
|
||||||
|
CONFIG_DRIVERS_BAROMETER_INVENSENSE_ICP201XX=y
|
||||||
CONFIG_DRIVERS_BAROMETER_MS5611=y
|
CONFIG_DRIVERS_BAROMETER_MS5611=y
|
||||||
CONFIG_DRIVERS_CAMERA_CAPTURE=y
|
CONFIG_DRIVERS_CAMERA_CAPTURE=y
|
||||||
CONFIG_DRIVERS_CAMERA_TRIGGER=y
|
CONFIG_DRIVERS_CAMERA_TRIGGER=y
|
||||||
CONFIG_DRIVERS_DISTANCE_SENSOR_LIGHTWARE_LASER_I2C=y
|
CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
|
||||||
|
CONFIG_COMMON_DISTANCE_SENSOR=y
|
||||||
CONFIG_DRIVERS_DSHOT=y
|
CONFIG_DRIVERS_DSHOT=y
|
||||||
CONFIG_DRIVERS_GPS=y
|
CONFIG_DRIVERS_GPS=y
|
||||||
|
CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16470=y
|
||||||
CONFIG_DRIVERS_IMU_BOSCH_BMI088=y
|
CONFIG_DRIVERS_IMU_BOSCH_BMI088=y
|
||||||
CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=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_ICM42688P=y
|
||||||
|
CONFIG_DRIVERS_IMU_INVENSENSE_ICM45686=y
|
||||||
|
CONFIG_DRIVERS_IMU_INVENSENSE_IIM42652=y
|
||||||
|
CONFIG_COMMON_INS=y
|
||||||
CONFIG_COMMON_LIGHT=y
|
CONFIG_COMMON_LIGHT=y
|
||||||
CONFIG_DRIVERS_MAGNETOMETER_BOSCH_BMM150=y
|
CONFIG_COMMON_MAGNETOMETER=y
|
||||||
CONFIG_DRIVERS_MAGNETOMETER_HMC5883=y
|
CONFIG_DRIVERS_OSD_MSP_OSD=y
|
||||||
CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y
|
|
||||||
CONFIG_DRIVERS_POWER_MONITOR_INA226=y
|
CONFIG_DRIVERS_POWER_MONITOR_INA226=y
|
||||||
CONFIG_DRIVERS_POWER_MONITOR_INA228=y
|
CONFIG_DRIVERS_POWER_MONITOR_INA228=y
|
||||||
CONFIG_DRIVERS_POWER_MONITOR_INA238=y
|
CONFIG_DRIVERS_POWER_MONITOR_INA238=y
|
||||||
|
@ -32,14 +41,20 @@ CONFIG_COMMON_TELEMETRY=y
|
||||||
CONFIG_DRIVERS_TONE_ALARM=y
|
CONFIG_DRIVERS_TONE_ALARM=y
|
||||||
CONFIG_DRIVERS_UAVCAN=y
|
CONFIG_DRIVERS_UAVCAN=y
|
||||||
CONFIG_COMMON_UWB=y
|
CONFIG_COMMON_UWB=y
|
||||||
|
CONFIG_MODULES_AIRSPEED_SELECTOR=y
|
||||||
CONFIG_MODULES_BATTERY_STATUS=y
|
CONFIG_MODULES_BATTERY_STATUS=y
|
||||||
CONFIG_MODULES_CAMERA_FEEDBACK=y
|
CONFIG_MODULES_CAMERA_FEEDBACK=y
|
||||||
CONFIG_MODULES_COMMANDER=y
|
CONFIG_MODULES_COMMANDER=y
|
||||||
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
||||||
CONFIG_MODULES_DATAMAN=y
|
CONFIG_MODULES_DATAMAN=y
|
||||||
CONFIG_MODULES_EKF2=y
|
CONFIG_MODULES_EKF2=y
|
||||||
|
CONFIG_MODULES_ESC_BATTERY=y
|
||||||
CONFIG_MODULES_EVENTS=y
|
CONFIG_MODULES_EVENTS=y
|
||||||
CONFIG_MODULES_FLIGHT_MODE_MANAGER=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_GIMBAL=y
|
||||||
CONFIG_MODULES_GYRO_CALIBRATION=y
|
CONFIG_MODULES_GYRO_CALIBRATION=y
|
||||||
CONFIG_MODULES_GYRO_FFT=y
|
CONFIG_MODULES_GYRO_FFT=y
|
||||||
|
@ -57,11 +72,15 @@ CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
|
||||||
CONFIG_MODULES_MC_POS_CONTROL=y
|
CONFIG_MODULES_MC_POS_CONTROL=y
|
||||||
CONFIG_MODULES_MC_RATE_CONTROL=y
|
CONFIG_MODULES_MC_RATE_CONTROL=y
|
||||||
CONFIG_MODULES_NAVIGATOR=y
|
CONFIG_MODULES_NAVIGATOR=y
|
||||||
|
CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF=y
|
||||||
CONFIG_MODULES_RC_UPDATE=y
|
CONFIG_MODULES_RC_UPDATE=y
|
||||||
CONFIG_MODULES_ROVER_POS_CONTROL=y
|
CONFIG_MODULES_ROVER_POS_CONTROL=y
|
||||||
CONFIG_MODULES_SENSORS=y
|
CONFIG_MODULES_SENSORS=y
|
||||||
CONFIG_MODULES_TEMPERATURE_COMPENSATION=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_ACTUATOR_TEST=y
|
||||||
|
CONFIG_SYSTEMCMDS_BSONDUMP=y
|
||||||
CONFIG_SYSTEMCMDS_DMESG=y
|
CONFIG_SYSTEMCMDS_DMESG=y
|
||||||
CONFIG_SYSTEMCMDS_DUMPFILE=y
|
CONFIG_SYSTEMCMDS_DUMPFILE=y
|
||||||
CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
|
CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
|
||||||
|
|
|
@ -4,7 +4,7 @@
|
||||||
#------------------------------------------------------------------------------
|
#------------------------------------------------------------------------------
|
||||||
|
|
||||||
# if skynode base board is detected start Mavlink on Telem2
|
# if skynode base board is detected start Mavlink on Telem2
|
||||||
if ver hwbasecmp 009 010
|
if ver hwbasecmp 009 010 011
|
||||||
then
|
then
|
||||||
mavlink start -d /dev/ttyS5 -b 3000000 -r 290000 -m onboard_low_bandwidth -x -z
|
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_INSTRUMENTATION_SWITCH=y
|
||||||
CONFIG_SCHED_LPWORK=y
|
CONFIG_SCHED_LPWORK=y
|
||||||
CONFIG_SCHED_LPWORKPRIORITY=50
|
CONFIG_SCHED_LPWORKPRIORITY=50
|
||||||
CONFIG_SCHED_LPWORKSTACKSIZE=1632
|
CONFIG_SCHED_LPWORKSTACKSIZE=2032
|
||||||
CONFIG_SCHED_WAITPID=y
|
CONFIG_SCHED_WAITPID=y
|
||||||
CONFIG_SDIO_BLOCKSETUP=y
|
CONFIG_SDIO_BLOCKSETUP=y
|
||||||
CONFIG_SEM_PREALLOCHOLDERS=32
|
CONFIG_SEM_PREALLOCHOLDERS=32
|
||||||
|
|
|
@ -1,4 +1,9 @@
|
||||||
|
CONFIG_MODULES_AIRSPEED_SELECTOR=n
|
||||||
CONFIG_MODULES_FLIGHT_MODE_MANAGER=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_LANDING_TARGET_ESTIMATOR=n
|
||||||
CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n
|
CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n
|
||||||
CONFIG_MODULES_MC_ATT_CONTROL=n
|
CONFIG_MODULES_MC_ATT_CONTROL=n
|
||||||
|
@ -6,6 +11,7 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n
|
||||||
CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=n
|
CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=n
|
||||||
CONFIG_MODULES_MC_POS_CONTROL=n
|
CONFIG_MODULES_MC_POS_CONTROL=n
|
||||||
CONFIG_MODULES_MC_RATE_CONTROL=n
|
CONFIG_MODULES_MC_RATE_CONTROL=n
|
||||||
|
CONFIG_MODULES_VTOL_ATT_CONTROL=n
|
||||||
CONFIG_EKF2_AUX_GLOBAL_POSITION=y
|
CONFIG_EKF2_AUX_GLOBAL_POSITION=y
|
||||||
# CONFIG_EKF2_WIND is not set
|
# CONFIG_EKF2_WIND is not set
|
||||||
CONFIG_MODULES_DIFFERENTIAL_DRIVE=y
|
CONFIG_MODULES_DIFFERENTIAL_DRIVE=y
|
||||||
|
|
|
@ -58,6 +58,7 @@ set(msg_files
|
||||||
CameraCapture.msg
|
CameraCapture.msg
|
||||||
CameraStatus.msg
|
CameraStatus.msg
|
||||||
CameraTrigger.msg
|
CameraTrigger.msg
|
||||||
|
CanInterfaceStatus.msg
|
||||||
CellularStatus.msg
|
CellularStatus.msg
|
||||||
CollisionConstraints.msg
|
CollisionConstraints.msg
|
||||||
CollisionReport.msg
|
CollisionReport.msg
|
||||||
|
@ -99,6 +100,7 @@ set(msg_files
|
||||||
FollowTargetStatus.msg
|
FollowTargetStatus.msg
|
||||||
GeneratorStatus.msg
|
GeneratorStatus.msg
|
||||||
GeofenceResult.msg
|
GeofenceResult.msg
|
||||||
|
GeofenceStatus.msg
|
||||||
GimbalControls.msg
|
GimbalControls.msg
|
||||||
GimbalDeviceAttitudeStatus.msg
|
GimbalDeviceAttitudeStatus.msg
|
||||||
GimbalDeviceInformation.msg
|
GimbalDeviceInformation.msg
|
||||||
|
@ -153,6 +155,10 @@ set(msg_files
|
||||||
OrbTest.msg
|
OrbTest.msg
|
||||||
OrbTestLarge.msg
|
OrbTestLarge.msg
|
||||||
OrbTestMedium.msg
|
OrbTestMedium.msg
|
||||||
|
ParameterResetRequest.msg
|
||||||
|
ParameterSetUsedRequest.msg
|
||||||
|
ParameterSetValueRequest.msg
|
||||||
|
ParameterSetValueResponse.msg
|
||||||
ParameterUpdate.msg
|
ParameterUpdate.msg
|
||||||
Ping.msg
|
Ping.msg
|
||||||
PositionControllerLandingStatus.msg
|
PositionControllerLandingStatus.msg
|
||||||
|
@ -284,6 +290,9 @@ foreach(msg_file ${msg_files})
|
||||||
list(APPEND uorb_json_files ${msg_source_out_path}/${msg}.json)
|
list(APPEND uorb_json_files ${msg_source_out_path}/${msg}.json)
|
||||||
endforeach()
|
endforeach()
|
||||||
|
|
||||||
|
# set parent scope msg_files for ROS
|
||||||
|
set(msg_files ${msg_files} PARENT_SCOPE)
|
||||||
|
|
||||||
# Generate uORB headers
|
# Generate uORB headers
|
||||||
add_custom_command(
|
add_custom_command(
|
||||||
OUTPUT
|
OUTPUT
|
||||||
|
|
|
@ -0,0 +1,6 @@
|
||||||
|
uint64 timestamp # time since system start (microseconds)
|
||||||
|
uint8 interface
|
||||||
|
|
||||||
|
uint64 io_errors
|
||||||
|
uint64 frames_tx
|
||||||
|
uint64 frames_rx
|
|
@ -0,0 +1,7 @@
|
||||||
|
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)
|
uint64 timestamp # time since system start (microseconds)
|
||||||
|
|
||||||
uint16 mission_id # Id for the mission for which the result was generated
|
uint32 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)
|
uint32 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)
|
uint32 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
|
int32 seq_reached # Sequence of the mission item which has been reached, default -1
|
||||||
uint16 seq_current # Sequence of the current mission item
|
uint16 seq_current # Sequence of the current mission item
|
||||||
|
|
|
@ -4,4 +4,6 @@ int32 val
|
||||||
|
|
||||||
uint8[64] junk
|
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
|
# TOPICS orb_test_medium orb_test_medium_multi orb_test_medium_wrap_around orb_test_medium_queue orb_test_medium_queue_poll
|
||||||
|
|
|
@ -0,0 +1,8 @@
|
||||||
|
# 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
|
|
@ -0,0 +1,6 @@
|
||||||
|
# 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
|
|
@ -0,0 +1,11 @@
|
||||||
|
# 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
|
|
@ -0,0 +1,9 @@
|
||||||
|
# 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
|
|
@ -47,4 +47,4 @@ uint16 ADSB_EMITTER_TYPE_SERVICE_SURFACE=18
|
||||||
uint16 ADSB_EMITTER_TYPE_POINT_OBSTACLE=19
|
uint16 ADSB_EMITTER_TYPE_POINT_OBSTACLE=19
|
||||||
uint16 ADSB_EMITTER_TYPE_ENUM_END=20
|
uint16 ADSB_EMITTER_TYPE_ENUM_END=20
|
||||||
|
|
||||||
uint8 ORB_QUEUE_LENGTH = 8
|
uint8 ORB_QUEUE_LENGTH = 16
|
||||||
|
|
|
@ -52,6 +52,7 @@ add_library(px4_platform STATIC
|
||||||
shutdown.cpp
|
shutdown.cpp
|
||||||
spi.cpp
|
spi.cpp
|
||||||
pab_manifest.c
|
pab_manifest.c
|
||||||
|
Serial.cpp
|
||||||
${SRCS}
|
${SRCS}
|
||||||
)
|
)
|
||||||
target_link_libraries(px4_platform prebuild_targets px4_work_queue)
|
target_link_libraries(px4_platform prebuild_targets px4_work_queue)
|
||||||
|
|
|
@ -0,0 +1,138 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* 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
|
|
@ -0,0 +1,97 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* 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
|
|
@ -0,0 +1,70 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* 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,11 +13,21 @@ __END_DECLS
|
||||||
#define px4_clock_gettime system_clock_gettime
|
#define px4_clock_gettime system_clock_gettime
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
|
#if defined(ENABLE_LOCKSTEP_SCHEDULER) || defined(__PX4_QURT)
|
||||||
|
|
||||||
__BEGIN_DECLS
|
__BEGIN_DECLS
|
||||||
__EXPORT int px4_clock_settime(clockid_t clk_id, const struct timespec *tp);
|
__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_usleep(useconds_t usec);
|
__EXPORT int px4_usleep(useconds_t usec);
|
||||||
__EXPORT unsigned int px4_sleep(unsigned int seconds);
|
__EXPORT unsigned int px4_sleep(unsigned int seconds);
|
||||||
__EXPORT int px4_pthread_cond_timedwait(pthread_cond_t *cond,
|
__EXPORT int px4_pthread_cond_timedwait(pthread_cond_t *cond,
|
||||||
|
@ -27,7 +37,6 @@ __END_DECLS
|
||||||
|
|
||||||
#else
|
#else
|
||||||
|
|
||||||
#define px4_clock_settime system_clock_settime
|
|
||||||
#define px4_usleep system_usleep
|
#define px4_usleep system_usleep
|
||||||
#define px4_sleep system_sleep
|
#define px4_sleep system_sleep
|
||||||
#define px4_pthread_cond_timedwait system_pthread_cond_timedwait
|
#define px4_pthread_cond_timedwait system_pthread_cond_timedwait
|
||||||
|
|
|
@ -48,6 +48,8 @@
|
||||||
|
|
||||||
#include <board_config.h>
|
#include <board_config.h>
|
||||||
|
|
||||||
|
#if defined(BOARD_HAS_HW_SPLIT_VERSIONING)
|
||||||
|
|
||||||
#include <inttypes.h>
|
#include <inttypes.h>
|
||||||
#include <stdbool.h>
|
#include <stdbool.h>
|
||||||
#include <syslog.h>
|
#include <syslog.h>
|
||||||
|
@ -57,7 +59,6 @@
|
||||||
/****************************************************************************
|
/****************************************************************************
|
||||||
* Pre-Processor Definitions
|
* Pre-Processor Definitions
|
||||||
****************************************************************************/
|
****************************************************************************/
|
||||||
#if defined(BOARD_HAS_HW_SPLIT_VERSIONING)
|
|
||||||
|
|
||||||
typedef struct {
|
typedef struct {
|
||||||
hw_base_id_t hw_base_id; /* The ID of the Base */
|
hw_base_id_t hw_base_id; /* The ID of the Base */
|
||||||
|
@ -292,12 +293,6 @@ static const px4_hw_mft_item_t base_configuration_9[] = {
|
||||||
.mandatory = 1,
|
.mandatory = 1,
|
||||||
.connection = px4_hw_con_onboard,
|
.connection = px4_hw_con_onboard,
|
||||||
},
|
},
|
||||||
{
|
|
||||||
.id = PX4_MFT_PM2,
|
|
||||||
.present = 1,
|
|
||||||
.mandatory = 1,
|
|
||||||
.connection = px4_hw_con_onboard,
|
|
||||||
},
|
|
||||||
{
|
{
|
||||||
.id = PX4_MFT_ETHERNET,
|
.id = PX4_MFT_ETHERNET,
|
||||||
.present = 1,
|
.present = 1,
|
||||||
|
@ -315,6 +310,52 @@ static const px4_hw_mft_item_t base_configuration_9[] = {
|
||||||
// BASE ID 10 Skynode QS no USB Alaised to ID 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 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[] = {
|
static px4_hw_mft_list_entry_t mft_lists[] = {
|
||||||
// ver_rev
|
// ver_rev
|
||||||
|
@ -328,6 +369,7 @@ 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(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(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(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
|
log_message.severity = 6; // info
|
||||||
strcpy((char *)log_message.text, "initialized uORB logging");
|
strcpy((char *)log_message.text, "initialized uORB logging");
|
||||||
log_message.timestamp = hrt_absolute_time();
|
log_message.timestamp = hrt_absolute_time();
|
||||||
orb_log_message_pub = orb_advertise_queue(ORB_ID(log_message), &log_message, log_message_s::ORB_QUEUE_LENGTH);
|
orb_log_message_pub = orb_advertise(ORB_ID(log_message), &log_message);
|
||||||
}
|
}
|
||||||
|
|
||||||
__EXPORT void px4_log_modulename(int level, const char *module_name, const char *fmt, ...)
|
__EXPORT void px4_log_modulename(int level, const char *module_name, const char *fmt, ...)
|
||||||
|
|
|
@ -48,24 +48,6 @@
|
||||||
namespace uORB
|
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
|
class PublicationBase
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
|
@ -97,7 +79,7 @@ protected:
|
||||||
/**
|
/**
|
||||||
* uORB publication wrapper class
|
* uORB publication wrapper class
|
||||||
*/
|
*/
|
||||||
template<typename T, uint8_t ORB_QSIZE = DefaultQueueSize<T>::value>
|
template<typename T>
|
||||||
class Publication : public PublicationBase
|
class Publication : public PublicationBase
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
|
@ -113,7 +95,7 @@ public:
|
||||||
bool advertise()
|
bool advertise()
|
||||||
{
|
{
|
||||||
if (!advertised()) {
|
if (!advertised()) {
|
||||||
_handle = orb_advertise_queue(get_topic(), nullptr, ORB_QSIZE);
|
_handle = orb_advertise(get_topic(), nullptr);
|
||||||
}
|
}
|
||||||
|
|
||||||
return advertised();
|
return advertised();
|
||||||
|
|
|
@ -51,7 +51,7 @@ namespace uORB
|
||||||
/**
|
/**
|
||||||
* Base publication multi wrapper class
|
* Base publication multi wrapper class
|
||||||
*/
|
*/
|
||||||
template<typename T, uint8_t QSIZE = DefaultQueueSize<T>::value>
|
template<typename T>
|
||||||
class PublicationMulti : public PublicationBase
|
class PublicationMulti : public PublicationBase
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
|
@ -73,7 +73,7 @@ public:
|
||||||
{
|
{
|
||||||
if (!advertised()) {
|
if (!advertised()) {
|
||||||
int instance = 0;
|
int instance = 0;
|
||||||
_handle = orb_advertise_multi_queue(get_topic(), nullptr, &instance, QSIZE);
|
_handle = orb_advertise_multi(get_topic(), nullptr, &instance);
|
||||||
}
|
}
|
||||||
|
|
||||||
return advertised();
|
return advertised();
|
||||||
|
|
|
@ -56,31 +56,6 @@ public:
|
||||||
SubscriptionBlocking(const orb_metadata *meta, uint32_t interval_us = 0, uint8_t instance = 0) :
|
SubscriptionBlocking(const orb_metadata *meta, uint32_t interval_us = 0, uint8_t instance = 0) :
|
||||||
SubscriptionCallback(meta, interval_us, instance)
|
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()
|
virtual ~SubscriptionBlocking()
|
||||||
|
|
|
@ -118,22 +118,11 @@ orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data)
|
||||||
return uORB::Manager::get_instance()->orb_advertise(meta, 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)
|
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);
|
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)
|
int orb_unadvertise(orb_advert_t handle)
|
||||||
{
|
{
|
||||||
return uORB::Manager::get_instance()->orb_unadvertise(handle);
|
return uORB::Manager::get_instance()->orb_unadvertise(handle);
|
||||||
|
@ -227,6 +216,14 @@ const char *orb_get_c_type(unsigned char short_type)
|
||||||
return nullptr;
|
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)
|
void orb_print_message_internal(const orb_metadata *meta, const void *data, bool print_topic_name)
|
||||||
{
|
{
|
||||||
|
|
|
@ -53,6 +53,8 @@ struct orb_metadata {
|
||||||
const uint16_t o_size_no_padding; /**< object size w/o padding at the end (for logger) */
|
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 */
|
uint32_t message_hash; /**< Hash over all fields for message compatibility checks */
|
||||||
orb_id_size_t o_id; /**< ORB_ID enum */
|
orb_id_size_t o_id; /**< ORB_ID enum */
|
||||||
|
uint8_t o_queue; /**< queue size */
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|
||||||
typedef const struct orb_metadata *orb_id_t;
|
typedef const struct orb_metadata *orb_id_t;
|
||||||
|
@ -102,14 +104,16 @@ typedef const struct orb_metadata *orb_id_t;
|
||||||
* @param _size_no_padding Struct size w/o padding at the end
|
* @param _size_no_padding Struct size w/o padding at the end
|
||||||
* @param _message_hash 32 bit message hash over all fields
|
* @param _message_hash 32 bit message hash over all fields
|
||||||
* @param _orb_id_enum ORB ID enum e.g.: ORB_ID::vehicle_status
|
* @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) \
|
#define ORB_DEFINE(_name, _struct, _size_no_padding, _message_hash, _orb_id_enum, _queue_size) \
|
||||||
const struct orb_metadata __orb_##_name = { \
|
const struct orb_metadata __orb_##_name = { \
|
||||||
#_name, \
|
#_name, \
|
||||||
sizeof(_struct), \
|
sizeof(_struct), \
|
||||||
_size_no_padding, \
|
_size_no_padding, \
|
||||||
_message_hash, \
|
_message_hash, \
|
||||||
_orb_id_enum \
|
_orb_id_enum, \
|
||||||
|
_queue_size \
|
||||||
}; struct hack
|
}; struct hack
|
||||||
|
|
||||||
__BEGIN_DECLS
|
__BEGIN_DECLS
|
||||||
|
@ -135,23 +139,11 @@ typedef void *orb_advert_t;
|
||||||
*/
|
*/
|
||||||
extern orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data) __EXPORT;
|
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()
|
* @see uORB::Manager::orb_advertise_multi()
|
||||||
*/
|
*/
|
||||||
extern orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance) __EXPORT;
|
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()
|
* @see uORB::Manager::orb_unadvertise()
|
||||||
*/
|
*/
|
||||||
|
@ -160,7 +152,7 @@ extern int orb_unadvertise(orb_advert_t handle) __EXPORT;
|
||||||
/**
|
/**
|
||||||
* @see uORB::Manager::orb_publish()
|
* @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.
|
* Advertise as the publisher of a topic.
|
||||||
|
@ -241,6 +233,12 @@ extern int orb_get_interval(int handle, unsigned *interval) __EXPORT;
|
||||||
*/
|
*/
|
||||||
const char *orb_get_c_type(unsigned char short_type);
|
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.
|
* Print a topic to console. Do not call this directly, use print_message() instead.
|
||||||
* @param meta orb topic metadata
|
* @param meta orb topic metadata
|
||||||
|
|
|
@ -48,37 +48,10 @@
|
||||||
|
|
||||||
static uORB::SubscriptionInterval *filp_to_subscription(cdev::file_t *filp) { return static_cast<uORB::SubscriptionInterval *>(filp->f_priv); }
|
static uORB::SubscriptionInterval *filp_to_subscription(cdev::file_t *filp) { return static_cast<uORB::SubscriptionInterval *>(filp->f_priv); }
|
||||||
|
|
||||||
// round up to nearest power of two
|
uORB::DeviceNode::DeviceNode(const struct orb_metadata *meta, const uint8_t instance, const char *path) :
|
||||||
// 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
|
CDev(strdup(path)), // success is checked in CDev::init
|
||||||
_meta(meta),
|
_meta(meta),
|
||||||
_instance(instance),
|
_instance(instance)
|
||||||
_queue_size(round_pow_of_two_8(queue_size))
|
|
||||||
{
|
{
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -186,7 +159,7 @@ uORB::DeviceNode::write(cdev::file_t *filp, const char *buffer, size_t buflen)
|
||||||
|
|
||||||
/* re-check size */
|
/* re-check size */
|
||||||
if (nullptr == _data) {
|
if (nullptr == _data) {
|
||||||
const size_t data_size = _meta->o_size * _queue_size;
|
const size_t data_size = _meta->o_size * _meta->o_queue;
|
||||||
_data = (uint8_t *) px4_cache_aligned_alloc(data_size);
|
_data = (uint8_t *) px4_cache_aligned_alloc(data_size);
|
||||||
|
|
||||||
if (_data) {
|
if (_data) {
|
||||||
|
@ -217,7 +190,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 */
|
/* wrap-around happens after ~49 days, assuming a publisher rate of 1 kHz */
|
||||||
unsigned generation = _generation.fetch_add(1);
|
unsigned generation = _generation.fetch_add(1);
|
||||||
|
|
||||||
memcpy(_data + (_meta->o_size * (generation % _queue_size)), buffer, _meta->o_size);
|
memcpy(_data + (_meta->o_size * (generation % _meta->o_queue)), buffer, _meta->o_size);
|
||||||
|
|
||||||
// callbacks
|
// callbacks
|
||||||
for (auto item : _callbacks) {
|
for (auto item : _callbacks) {
|
||||||
|
@ -254,13 +227,6 @@ uORB::DeviceNode::ioctl(cdev::file_t *filp, int cmd, unsigned long arg)
|
||||||
*(uintptr_t *)arg = (uintptr_t)this;
|
*(uintptr_t *)arg = (uintptr_t)this;
|
||||||
return PX4_OK;
|
return PX4_OK;
|
||||||
|
|
||||||
case ORBIOCSETQUEUESIZE: {
|
|
||||||
lock();
|
|
||||||
int ret = update_queue_size(arg);
|
|
||||||
unlock();
|
|
||||||
return ret;
|
|
||||||
}
|
|
||||||
|
|
||||||
case ORBIOCGETINTERVAL:
|
case ORBIOCGETINTERVAL:
|
||||||
*(unsigned *)arg = filp_to_subscription(filp)->get_interval_us();
|
*(unsigned *)arg = filp_to_subscription(filp)->get_interval_us();
|
||||||
return PX4_OK;
|
return PX4_OK;
|
||||||
|
@ -389,12 +355,11 @@ uORB::DeviceNode::print_statistics(int max_topic_length)
|
||||||
|
|
||||||
const uint8_t instance = get_instance();
|
const uint8_t instance = get_instance();
|
||||||
const int8_t sub_count = subscriber_count();
|
const int8_t sub_count = subscriber_count();
|
||||||
const uint8_t queue_size = get_queue_size();
|
|
||||||
|
|
||||||
unlock();
|
unlock();
|
||||||
|
|
||||||
PX4_INFO_RAW("%-*s %2i %4i %2i %4i %s\n", max_topic_length, get_meta()->o_name, (int)instance, (int)sub_count,
|
PX4_INFO_RAW("%-*s %2i %4i %2i %4i %s\n", max_topic_length, get_meta()->o_name, (int)instance, (int)sub_count,
|
||||||
queue_size, get_meta()->o_size, get_devname());
|
get_meta()->o_queue, get_meta()->o_size, get_devname());
|
||||||
|
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
@ -447,7 +412,10 @@ int16_t uORB::DeviceNode::process_add_subscription()
|
||||||
uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator();
|
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.
|
if (_data != nullptr && ch != nullptr) { // _data will not be null if there is a publisher.
|
||||||
ch->send_message(_meta->o_name, _meta->o_size, _data);
|
// 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)));
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
return PX4_OK;
|
return PX4_OK;
|
||||||
|
@ -483,21 +451,6 @@ int16_t uORB::DeviceNode::process_received_message(int32_t length, uint8_t *data
|
||||||
}
|
}
|
||||||
#endif /* CONFIG_ORB_COMMUNICATOR */
|
#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()
|
unsigned uORB::DeviceNode::get_initial_generation()
|
||||||
{
|
{
|
||||||
ATOMIC_ENTER;
|
ATOMIC_ENTER;
|
||||||
|
|
|
@ -62,7 +62,7 @@ class UnitTest;
|
||||||
class uORB::DeviceNode : public cdev::CDev, public IntrusiveSortedListNode<uORB::DeviceNode *>
|
class uORB::DeviceNode : public cdev::CDev, public IntrusiveSortedListNode<uORB::DeviceNode *>
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
DeviceNode(const struct orb_metadata *meta, const uint8_t instance, const char *path, uint8_t queue_size = 1);
|
DeviceNode(const struct orb_metadata *meta, const uint8_t instance, const char *path);
|
||||||
virtual ~DeviceNode();
|
virtual ~DeviceNode();
|
||||||
|
|
||||||
// no copy, assignment, move, move assignment
|
// no copy, assignment, move, move assignment
|
||||||
|
@ -179,15 +179,6 @@ public:
|
||||||
|
|
||||||
void mark_as_advertised() { _advertised = true; }
|
void mark_as_advertised() { _advertised = true; }
|
||||||
|
|
||||||
/**
|
|
||||||
* Try to change the size of the queue. This can only be done as long as nobody published yet.
|
|
||||||
* This is the case, for example when orb_subscribe was called before an orb_advertise.
|
|
||||||
* The queue size can only be increased.
|
|
||||||
* @param queue_size new size of the queue
|
|
||||||
* @return PX4_OK if queue size successfully set
|
|
||||||
*/
|
|
||||||
int update_queue_size(unsigned int queue_size);
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Print statistics
|
* Print statistics
|
||||||
* @param max_topic_length max topic name length for printing
|
* @param max_topic_length max topic name length for printing
|
||||||
|
@ -195,7 +186,7 @@ public:
|
||||||
*/
|
*/
|
||||||
bool print_statistics(int max_topic_length);
|
bool print_statistics(int max_topic_length);
|
||||||
|
|
||||||
uint8_t get_queue_size() const { return _queue_size; }
|
uint8_t get_queue_size() const { return _meta->o_queue; }
|
||||||
|
|
||||||
int8_t subscriber_count() const { return _subscriber_count; }
|
int8_t subscriber_count() const { return _subscriber_count; }
|
||||||
|
|
||||||
|
@ -234,7 +225,7 @@ public:
|
||||||
bool copy(void *dst, unsigned &generation)
|
bool copy(void *dst, unsigned &generation)
|
||||||
{
|
{
|
||||||
if ((dst != nullptr) && (_data != nullptr)) {
|
if ((dst != nullptr) && (_data != nullptr)) {
|
||||||
if (_queue_size == 1) {
|
if (_meta->o_queue == 1) {
|
||||||
ATOMIC_ENTER;
|
ATOMIC_ENTER;
|
||||||
memcpy(dst, _data, _meta->o_size);
|
memcpy(dst, _data, _meta->o_size);
|
||||||
generation = _generation.load();
|
generation = _generation.load();
|
||||||
|
@ -253,12 +244,12 @@ public:
|
||||||
}
|
}
|
||||||
|
|
||||||
// Compatible with normal and overflow conditions
|
// Compatible with normal and overflow conditions
|
||||||
if (!is_in_range(current_generation - _queue_size, generation, current_generation - 1)) {
|
if (!is_in_range(current_generation - _meta->o_queue, generation, current_generation - 1)) {
|
||||||
// Reader is too far behind: some messages are lost
|
// Reader is too far behind: some messages are lost
|
||||||
generation = current_generation - _queue_size;
|
generation = current_generation - _meta->o_queue;
|
||||||
}
|
}
|
||||||
|
|
||||||
memcpy(dst, _data + (_meta->o_size * (generation % _queue_size)), _meta->o_size);
|
memcpy(dst, _data + (_meta->o_size * (generation % _meta->o_queue)), _meta->o_size);
|
||||||
ATOMIC_LEAVE;
|
ATOMIC_LEAVE;
|
||||||
|
|
||||||
++generation;
|
++generation;
|
||||||
|
@ -295,7 +286,7 @@ private:
|
||||||
|
|
||||||
const uint8_t _instance; /**< orb multi instance identifier */
|
const uint8_t _instance; /**< orb multi instance identifier */
|
||||||
bool _advertised{false}; /**< has ever been advertised (not necessarily published data yet) */
|
bool _advertised{false}; /**< has ever been advertised (not necessarily published data yet) */
|
||||||
uint8_t _queue_size; /**< maximum number of elements in the queue */
|
|
||||||
int8_t _subscriber_count{0};
|
int8_t _subscriber_count{0};
|
||||||
|
|
||||||
|
|
||||||
|
|
|
@ -265,8 +265,7 @@ int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance)
|
||||||
return ret;
|
return ret;
|
||||||
}
|
}
|
||||||
|
|
||||||
orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance,
|
orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance)
|
||||||
unsigned int queue_size)
|
|
||||||
{
|
{
|
||||||
#ifdef ORB_USE_PUBLISHER_RULES
|
#ifdef ORB_USE_PUBLISHER_RULES
|
||||||
|
|
||||||
|
@ -300,19 +299,10 @@ orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta,
|
||||||
return nullptr;
|
return nullptr;
|
||||||
}
|
}
|
||||||
|
|
||||||
/* Set the queue size. This must be done before the first publication; thus it fails if
|
|
||||||
* this is not the first advertiser.
|
|
||||||
*/
|
|
||||||
int result = px4_ioctl(fd, ORBIOCSETQUEUESIZE, (unsigned long)queue_size);
|
|
||||||
|
|
||||||
if (result < 0 && queue_size > 1) {
|
|
||||||
PX4_WARN("orb_advertise_multi: failed to set queue size");
|
|
||||||
}
|
|
||||||
|
|
||||||
/* get the advertiser handle and close the node */
|
/* get the advertiser handle and close the node */
|
||||||
orb_advert_t advertiser;
|
orb_advert_t advertiser;
|
||||||
|
|
||||||
result = px4_ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser);
|
int result = px4_ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser);
|
||||||
px4_close(fd);
|
px4_close(fd);
|
||||||
|
|
||||||
if (result == PX4_ERROR) {
|
if (result == PX4_ERROR) {
|
||||||
|
@ -602,6 +592,22 @@ int16_t uORB::Manager::process_remote_topic(const char *topic_name)
|
||||||
{
|
{
|
||||||
PX4_DEBUG("entering process_remote_topic: name: %s", topic_name);
|
PX4_DEBUG("entering process_remote_topic: name: %s", topic_name);
|
||||||
|
|
||||||
|
// First make sure this is a valid topic
|
||||||
|
const struct orb_metadata *const *topic_list = orb_get_topics();
|
||||||
|
orb_id_t topic_ptr = nullptr;
|
||||||
|
|
||||||
|
for (size_t i = 0; i < orb_topics_count(); i++) {
|
||||||
|
if (strcmp(topic_list[i]->o_name, topic_name) == 0) {
|
||||||
|
topic_ptr = topic_list[i];
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (! topic_ptr) {
|
||||||
|
PX4_ERR("process_remote_topic meta not found for %s\n", topic_name);
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
|
||||||
// Look to see if we already have a node for this topic
|
// Look to see if we already have a node for this topic
|
||||||
char nodepath[orb_maxpath];
|
char nodepath[orb_maxpath];
|
||||||
int ret = uORB::Utils::node_mkpath(nodepath, topic_name);
|
int ret = uORB::Utils::node_mkpath(nodepath, topic_name);
|
||||||
|
@ -613,7 +619,7 @@ int16_t uORB::Manager::process_remote_topic(const char *topic_name)
|
||||||
uORB::DeviceNode *node = device_master->getDeviceNode(nodepath);
|
uORB::DeviceNode *node = device_master->getDeviceNode(nodepath);
|
||||||
|
|
||||||
if (node) {
|
if (node) {
|
||||||
PX4_INFO("Marking DeviceNode(%s) as advertised in process_remote_topic", topic_name);
|
PX4_DEBUG("Marking DeviceNode(%s) as advertised in process_remote_topic", topic_name);
|
||||||
node->mark_as_advertised();
|
node->mark_as_advertised();
|
||||||
_remote_topics.insert(topic_name);
|
_remote_topics.insert(topic_name);
|
||||||
return 0;
|
return 0;
|
||||||
|
@ -622,27 +628,9 @@ int16_t uORB::Manager::process_remote_topic(const char *topic_name)
|
||||||
}
|
}
|
||||||
|
|
||||||
// We didn't find a node so we need to create it via an advertisement
|
// We didn't find a node so we need to create it via an advertisement
|
||||||
const struct orb_metadata *const *topic_list = orb_get_topics();
|
PX4_DEBUG("Advertising remote topic %s", topic_name);
|
||||||
orb_id_t topic_ptr = nullptr;
|
_remote_topics.insert(topic_name);
|
||||||
|
orb_advertise(topic_ptr, nullptr);
|
||||||
for (size_t i = 0; i < orb_topics_count(); i++) {
|
|
||||||
if (strcmp(topic_list[i]->o_name, topic_name) == 0) {
|
|
||||||
topic_ptr = topic_list[i];
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
if (topic_ptr) {
|
|
||||||
PX4_INFO("Advertising remote topic %s", topic_name);
|
|
||||||
_remote_topics.insert(topic_name);
|
|
||||||
// Add some queue depth when advertising remote topics. These
|
|
||||||
// topics may get aggregated and thus delivered in a batch that
|
|
||||||
// requires some buffering in a queue.
|
|
||||||
orb_advertise(topic_ptr, nullptr, 5);
|
|
||||||
|
|
||||||
} else {
|
|
||||||
PX4_INFO("process_remote_topic meta not found for %s\n", topic_name);
|
|
||||||
}
|
|
||||||
|
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
@ -663,8 +651,11 @@ int16_t uORB::Manager::process_add_subscription(const char *messageName)
|
||||||
PX4_DEBUG("DeviceNode(%s) not created yet", messageName);
|
PX4_DEBUG("DeviceNode(%s) not created yet", messageName);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
// node is present.
|
// node is present. But don't send any data to it if it
|
||||||
node->process_add_subscription();
|
// is a node advertised by the remote side
|
||||||
|
if (_remote_topics.find(messageName) == false) {
|
||||||
|
node->process_add_subscription();
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
|
|
|
@ -215,17 +215,15 @@ public:
|
||||||
* @param data A pointer to the initial data to be published.
|
* @param data A pointer to the initial data to be published.
|
||||||
* For topics updated by interrupt handlers, the advertisement
|
* For topics updated by interrupt handlers, the advertisement
|
||||||
* must be performed from non-interrupt context.
|
* must be performed from non-interrupt context.
|
||||||
* @param queue_size Maximum number of buffered elements. If this is 1, no queuing is
|
|
||||||
* used.
|
|
||||||
* @return nullptr on error, otherwise returns an object pointer
|
* @return nullptr on error, otherwise returns an object pointer
|
||||||
* that can be used to publish to the topic.
|
* that can be used to publish to the topic.
|
||||||
* If the topic in question is not known (due to an
|
* If the topic in question is not known (due to an
|
||||||
* ORB_DEFINE with no corresponding ORB_DECLARE)
|
* ORB_DEFINE with no corresponding ORB_DECLARE)
|
||||||
* this function will return nullptr and set errno to ENOENT.
|
* this function will return nullptr and set errno to ENOENT.
|
||||||
*/
|
*/
|
||||||
orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data, unsigned int queue_size = 1)
|
orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data = nullptr)
|
||||||
{
|
{
|
||||||
return orb_advertise_multi(meta, data, nullptr, queue_size);
|
return orb_advertise_multi(meta, data, nullptr);
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
@ -250,16 +248,13 @@ public:
|
||||||
* @param instance Pointer to an integer which will yield the instance ID (0-based)
|
* @param instance Pointer to an integer which will yield the instance ID (0-based)
|
||||||
* of the publication. This is an output parameter and will be set to the newly
|
* of the publication. This is an output parameter and will be set to the newly
|
||||||
* created instance, ie. 0 for the first advertiser, 1 for the next and so on.
|
* created instance, ie. 0 for the first advertiser, 1 for the next and so on.
|
||||||
* @param queue_size Maximum number of buffered elements. If this is 1, no queuing is
|
|
||||||
* used.
|
|
||||||
* @return nullptr on error, otherwise returns a handle
|
* @return nullptr on error, otherwise returns a handle
|
||||||
* that can be used to publish to the topic.
|
* that can be used to publish to the topic.
|
||||||
* If the topic in question is not known (due to an
|
* If the topic in question is not known (due to an
|
||||||
* ORB_DEFINE with no corresponding ORB_DECLARE)
|
* ORB_DEFINE with no corresponding ORB_DECLARE)
|
||||||
* this function will return nullptr and set errno to ENOENT.
|
* this function will return nullptr and set errno to ENOENT.
|
||||||
*/
|
*/
|
||||||
orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance,
|
orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance);
|
||||||
unsigned int queue_size = 1);
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Unadvertise a topic.
|
* Unadvertise a topic.
|
||||||
|
|
|
@ -89,8 +89,7 @@ int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance)
|
||||||
return data.ret;
|
return data.ret;
|
||||||
}
|
}
|
||||||
|
|
||||||
orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance,
|
orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance)
|
||||||
unsigned int queue_size)
|
|
||||||
{
|
{
|
||||||
/* open the node as an advertiser */
|
/* open the node as an advertiser */
|
||||||
int fd = node_open(meta, true, instance);
|
int fd = node_open(meta, true, instance);
|
||||||
|
@ -100,19 +99,10 @@ orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta,
|
||||||
return nullptr;
|
return nullptr;
|
||||||
}
|
}
|
||||||
|
|
||||||
/* Set the queue size. This must be done before the first publication; thus it fails if
|
|
||||||
* this is not the first advertiser.
|
|
||||||
*/
|
|
||||||
int result = px4_ioctl(fd, ORBIOCSETQUEUESIZE, (unsigned long)queue_size);
|
|
||||||
|
|
||||||
if (result < 0 && queue_size > 1) {
|
|
||||||
PX4_WARN("orb_advertise_multi: failed to set queue size");
|
|
||||||
}
|
|
||||||
|
|
||||||
/* get the advertiser handle and close the node */
|
/* get the advertiser handle and close the node */
|
||||||
orb_advert_t advertiser;
|
orb_advert_t advertiser;
|
||||||
|
|
||||||
result = px4_ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser);
|
int result = px4_ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser);
|
||||||
px4_close(fd);
|
px4_close(fd);
|
||||||
|
|
||||||
if (result == PX4_ERROR) {
|
if (result == PX4_ERROR) {
|
||||||
|
|
|
@ -574,8 +574,8 @@ int uORBTest::UnitTest::test_wrap_around()
|
||||||
bool updated{false};
|
bool updated{false};
|
||||||
|
|
||||||
// Advertise but not publish topics, only generate device_node, which is convenient for modifying DeviceNode::_generation
|
// Advertise but not publish topics, only generate device_node, which is convenient for modifying DeviceNode::_generation
|
||||||
const int queue_size = 16;
|
const int queue_size = orb_get_queue_size(ORB_ID(orb_test_medium_wrap_around));
|
||||||
ptopic = orb_advertise_queue(ORB_ID(orb_test_medium_wrap_around), nullptr, queue_size);
|
ptopic = orb_advertise(ORB_ID(orb_test_medium_wrap_around), nullptr);
|
||||||
|
|
||||||
if (ptopic == nullptr) {
|
if (ptopic == nullptr) {
|
||||||
return test_fail("advertise failed: %d", errno);
|
return test_fail("advertise failed: %d", errno);
|
||||||
|
@ -828,9 +828,9 @@ int uORBTest::UnitTest::test_queue()
|
||||||
return test_fail("subscribe failed: %d", errno);
|
return test_fail("subscribe failed: %d", errno);
|
||||||
}
|
}
|
||||||
|
|
||||||
const int queue_size = 16;
|
const int queue_size = orb_get_queue_size(ORB_ID(orb_test_medium_queue));
|
||||||
orb_test_medium_s t{};
|
orb_test_medium_s t{};
|
||||||
ptopic = orb_advertise_queue(ORB_ID(orb_test_medium_queue), &t, queue_size);
|
ptopic = orb_advertise(ORB_ID(orb_test_medium_queue), &t);
|
||||||
|
|
||||||
if (ptopic == nullptr) {
|
if (ptopic == nullptr) {
|
||||||
return test_fail("advertise failed: %d", errno);
|
return test_fail("advertise failed: %d", errno);
|
||||||
|
@ -935,9 +935,9 @@ int uORBTest::UnitTest::pub_test_queue_main()
|
||||||
{
|
{
|
||||||
orb_test_medium_s t{};
|
orb_test_medium_s t{};
|
||||||
orb_advert_t ptopic{nullptr};
|
orb_advert_t ptopic{nullptr};
|
||||||
const int queue_size = 50;
|
const int queue_size = orb_get_queue_size(ORB_ID(orb_test_medium_queue_poll));
|
||||||
|
|
||||||
if ((ptopic = orb_advertise_queue(ORB_ID(orb_test_medium_queue_poll), &t, queue_size)) == nullptr) {
|
if ((ptopic = orb_advertise(ORB_ID(orb_test_medium_queue_poll), &t)) == nullptr) {
|
||||||
_thread_should_exit = true;
|
_thread_should_exit = true;
|
||||||
return test_fail("advertise failed: %d", errno);
|
return test_fail("advertise failed: %d", errno);
|
||||||
}
|
}
|
||||||
|
|
|
@ -1 +1 @@
|
||||||
Subproject commit 616f7024a479bf209eadce133bba5dc8820a7f99
|
Subproject commit e37940d8535f603a16b8f6f21c21edaf584218aa
|
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in New Issue