forked from Archive/PX4-Autopilot
Compare commits
181 Commits
pr-mc_att_
...
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 | |
Daniel Agar | 8243b4f474 | |
Daniel Agar | 22b957696d | |
Daniel Agar | c338891677 | |
Daniel Agar | c4c41c49e5 | |
Daniel Agar | 021dd0d0af | |
bresch | c221da27a7 | |
Matthias Grob | 51fe4351c6 | |
Silvan Fuhrer | 8a75733511 | |
Daniel Agar | 1032dd3470 | |
Konrad | 424c3cd2cb | |
Konrad | 68100650da | |
Cyril C | 74303a79e1 | |
Daniel Agar | 8dc3975456 | |
Matthias Grob | 84a7d42566 | |
Matthias Grob | f26df8492f | |
Konrad | cb09dde606 | |
Konrad | 1a1891073e | |
Daniel Agar | b8714f8980 | |
PX4 BuildBot | 0c099f2b56 | |
Frederik Markus | bb53781b8f | |
Silvan Fuhrer | c9ad60e3cc | |
Silvan Fuhrer | a6ef7b6da9 | |
Silvan Fuhrer | 6957818603 | |
Matthias Grob | cb03835124 | |
Silvan Fuhrer | b19e35ec7c | |
PX4 BuildBot | dce53a626e | |
Daniel Agar | 5f589bdda3 | |
Matthias Grob | 1998f54ea6 | |
PerFrivik | bef694f9ba | |
PerFrivik | 560d6a9d4b | |
PerFrivik | f996caa5bd | |
PerFrivik | bb0dfba4e6 | |
PerFrivik | d197d94889 | |
Matthias Grob | 396ef222ee | |
Matthias Grob | f85144ca76 | |
Matthias Grob | b54b4f7dce | |
Matthias Grob | fc90e235f1 | |
Matthias Grob | f7baeae1a0 | |
PerFrivik | e457a5baed | |
PX4 BuildBot | 0f3925b21d | |
muramura | f636414ca7 | |
muramura | 00a9e4c76b | |
Alex Klimaj | 31bbda0b58 | |
Matthias Grob | b355c16141 | |
Matthias Grob | 7fdb5ef3cb | |
fury1895 | 3de7d83e5f | |
Matthias Grob | 97cb933cff | |
Silvan Fuhrer | 584d8abe1e |
|
@ -120,7 +120,7 @@ pipeline {
|
|||
"px4_fmu-v6xrt_rover",
|
||||
"px4_io-v2_default",
|
||||
"raspberrypi_pico_default",
|
||||
"siyi_n7_default"
|
||||
"siyi_n7_default",
|
||||
"sky-drones_smartap-airlink_default",
|
||||
"spracing_h7extreme_default",
|
||||
"thepeach_k1_default",
|
||||
|
|
|
@ -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
|
|
@ -25,6 +25,7 @@ jobs:
|
|||
ark_can-rtk-gps,
|
||||
ark_cannode,
|
||||
ark_fmu-v6x,
|
||||
ark_septentrio-gps,
|
||||
atl_mantis-edu,
|
||||
av_x-v1,
|
||||
bitcraze_crazyflie,
|
||||
|
|
|
@ -81,5 +81,5 @@
|
|||
url = https://github.com/PX4/PX4-gazebo-models.git
|
||||
branch = main
|
||||
[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
|
||||
|
|
|
@ -151,6 +151,16 @@ CONFIG:
|
|||
buildType: MinSizeRel
|
||||
settings:
|
||||
CONFIG: ark_can-rtk-gps_canbootloader
|
||||
ark_septentrio_gps_default:
|
||||
short: ark_septentrio_gps_default
|
||||
buildType: MinSizeRel
|
||||
settings:
|
||||
CONFIG: ark_septentrio_gps_default
|
||||
ark_septentrio_gps_canbootloader:
|
||||
short: ark_septentrio_gps_canbootloader
|
||||
buildType: MinSizeRel
|
||||
settings:
|
||||
CONFIG: ark_septentrio_gps_canbootloader
|
||||
ark_cannode_default:
|
||||
short: ark_cannode_default
|
||||
buildType: MinSizeRel
|
||||
|
|
|
@ -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
|
||||
4009_gz_r1_rover
|
||||
4010_gz_x500_mono_cam
|
||||
4011_gz_lawnmower
|
||||
|
||||
6011_gazebo-classic_typhoon_h480
|
||||
6011_gazebo-classic_typhoon_h480.post
|
||||
|
|
|
@ -41,19 +41,6 @@ elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" = "1" ]; then
|
|||
|
||||
echo "INFO [init] Gazebo simulator"
|
||||
|
||||
# set local coordinate frame reference
|
||||
if [ -n "${PX4_HOME_LAT}" ]; then
|
||||
param set SIM_GZ_HOME_LAT ${PX4_HOME_LAT}
|
||||
fi
|
||||
|
||||
if [ -n "${PX4_HOME_LON}" ]; then
|
||||
param set SIM_GZ_HOME_LON ${PX4_HOME_LON}
|
||||
fi
|
||||
|
||||
if [ -n "${PX4_HOME_ALT}" ]; then
|
||||
param set SIM_GZ_HOME_ALT ${PX4_HOME_ALT}
|
||||
fi
|
||||
|
||||
# Only start up Gazebo if PX4_GZ_STANDALONE is not set.
|
||||
if [ -z "${PX4_GZ_STANDALONE}" ]; then
|
||||
|
||||
|
@ -180,6 +167,12 @@ elif [ "$PX4_SIM_MODEL" = "jmavsim_iris" ] || [ "$(param show -q SYS_AUTOSTART)"
|
|||
|
||||
else
|
||||
# otherwise start simulator (mavlink) module
|
||||
|
||||
# EKF2 specifics
|
||||
param set-default EKF2_GPS_DELAY 10
|
||||
param set-default EKF2_MULTI_IMU 3
|
||||
param set-default SENS_IMU_MODE 0
|
||||
|
||||
simulator_tcp_port=$((4560+px4_instance))
|
||||
|
||||
# Check if PX4_SIM_HOSTNAME environment variable is empty
|
||||
|
|
|
@ -164,10 +164,6 @@ param set-default COM_RC_IN_MODE 1
|
|||
# Speedup SITL startup
|
||||
param set-default EKF2_REQ_GPS_H 0.5
|
||||
|
||||
# Multi-EKF
|
||||
param set-default EKF2_MULTI_IMU 3
|
||||
param set-default SENS_IMU_MODE 0
|
||||
|
||||
param set-default IMU_GYRO_FFT_EN 1
|
||||
param set-default MAV_PROTO_VER 2 # Ensures QGC does not drop the first few packets after a SITL restart due to MAVLINK 1 packets
|
||||
|
||||
|
|
|
@ -34,28 +34,79 @@
|
|||
add_subdirectory(airframes)
|
||||
|
||||
px4_add_romfs_files(
|
||||
rc.airship_apps
|
||||
rc.airship_defaults
|
||||
|
||||
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
|
||||
rc.sensors
|
||||
rc.thermal_cal
|
||||
rc.rover_apps
|
||||
rc.rover_defaults
|
||||
rc.rover_differential_apps
|
||||
rc.rover_differential_defaults
|
||||
rc.uuv_apps
|
||||
rc.uuv_defaults
|
||||
rc.vehicle_setup
|
||||
rc.vtol_apps
|
||||
rc.vtol_defaults
|
||||
|
||||
# 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(
|
||||
# [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
|
||||
# [0-999] Reserved (historical)
|
||||
|
||||
# [18000, 18999] High-altitude balloons
|
||||
18001_TF-B1
|
||||
|
||||
# [22000, 22999] Reserve for custom models
|
||||
|
||||
24001_dodeca_cox
|
||||
|
||||
50000_generic_ground_vehicle
|
||||
50004_nxpcup_car_dfrobot_gpx
|
||||
50003_aion_robotics_r1_rover
|
||||
|
||||
# [60000, 61000] (Unmanned) Underwater Robots
|
||||
60000_uuv_generic
|
||||
60001_uuv_hippocampus
|
||||
60002_uuv_bluerov2_heavy
|
||||
)
|
||||
|
||||
if(CONFIG_MODULES_SIMULATION_PWM_OUT_SIM)
|
||||
px4_add_romfs_files(
|
||||
# [1000, 1999] Simulation setups
|
||||
1001_rc_quad_x.hil
|
||||
1002_standard_vtol.hil
|
||||
1100_rc_quad_x_sih.hil
|
||||
1101_rc_plane_sih.hil
|
||||
1102_tailsitter_duo_sih.hil
|
||||
)
|
||||
endif()
|
||||
|
||||
if(CONFIG_MODULES_MC_RATE_CONTROL)
|
||||
px4_add_romfs_files(
|
||||
# [4000, 4999] Quadrotor x
|
||||
4001_quad_x
|
||||
4014_s500
|
||||
4015_holybro_s500
|
||||
4016_holybro_px4vision
|
||||
4017_nxp_hovergames
|
||||
4019_x500_v2
|
||||
4020_holybro_px4vision_v1_5
|
||||
4041_beta75x
|
||||
4050_generic_250
|
||||
4052_holybro_qav250
|
||||
4053_holybro_kopis2
|
||||
4061_atl_mantis_edu
|
||||
4071_ifo
|
||||
4073_ifo-s
|
||||
4500_clover4
|
||||
4901_crazyflie21
|
||||
|
||||
# [5000, 5999] Quadrotor +
|
||||
5001_quad_+
|
||||
|
||||
# [6000, 6999] Hexarotor x
|
||||
6001_hexa_x
|
||||
6002_draco_r
|
||||
|
||||
# [7000, 7999] Hexarotor +
|
||||
7001_hexa_+
|
||||
|
||||
# [8000, 8999] Octorotor +
|
||||
8001_octo_x
|
||||
|
||||
# [9000, 9999] Octorotor +
|
||||
9001_octo_+
|
||||
|
||||
# [11000, 11999] Hexa Cox
|
||||
11001_hexa_cox
|
||||
|
||||
# [12000, 12999] Octo Cox
|
||||
12001_octo_cox
|
||||
|
||||
# [14000, 14999] MC with tilt
|
||||
14001_generic_mc_with_tilt
|
||||
|
||||
16001_helicopter
|
||||
|
||||
24001_dodeca_cox
|
||||
)
|
||||
endif()
|
||||
|
||||
if(CONFIG_MODULES_FW_RATE_CONTROL)
|
||||
px4_add_romfs_files(
|
||||
# [2000, 2999] Standard planes
|
||||
2100_standard_plane
|
||||
2106_albatross
|
||||
|
||||
# [3000, 3999] Flying wing
|
||||
3000_generic_wing
|
||||
|
||||
# [17000, 17999] Autogyro
|
||||
17002_TF-AutoG2
|
||||
17003_TF-G2
|
||||
)
|
||||
endif()
|
||||
|
||||
if(CONFIG_MODULES_AIRSHIP_ATT_CONTROL)
|
||||
px4_add_romfs_files(
|
||||
2507_cloudship
|
||||
)
|
||||
endif()
|
||||
|
||||
if(CONFIG_MODULES_VTOL_ATT_CONTROL)
|
||||
px4_add_romfs_files(
|
||||
# [13000, 13999] VTOL
|
||||
13000_generic_vtol_standard
|
||||
13100_generic_vtol_tiltrotor
|
||||
13013_deltaquad
|
||||
13014_vtol_babyshark
|
||||
13030_generic_vtol_quad_tiltrotor
|
||||
13200_generic_vtol_tailsitter
|
||||
)
|
||||
endif()
|
||||
|
||||
if(CONFIG_MODULES_ROVER_POS_CONTROL)
|
||||
px4_add_romfs_files(
|
||||
50000_generic_ground_vehicle
|
||||
50004_nxpcup_car_dfrobot_gpx
|
||||
)
|
||||
endif()
|
||||
|
||||
if(CONFIG_MODULES_DIFFERENTIAL_DRIVE)
|
||||
px4_add_romfs_files(
|
||||
50003_aion_robotics_r1_rover
|
||||
)
|
||||
endif()
|
||||
|
||||
if(CONFIG_MODULES_UUV_ATT_CONTROL)
|
||||
px4_add_romfs_files(
|
||||
# [60000, 61000] (Unmanned) Underwater Robots
|
||||
60000_uuv_generic
|
||||
60001_uuv_hippocampus
|
||||
60002_uuv_bluerov2_heavy
|
||||
)
|
||||
endif()
|
||||
|
|
|
@ -28,7 +28,6 @@ param set-default EKF2_ARSP_THR 8
|
|||
param set-default EKF2_FUSE_BETA 1
|
||||
param set-default EKF2_GPS_CHECK 21
|
||||
param set-default EKF2_MAG_ACCLIM 0
|
||||
param set-default EKF2_MAG_YAWLIM 0
|
||||
param set-default EKF2_REQ_EPH 10
|
||||
param set-default EKF2_REQ_EPV 10
|
||||
param set-default EKF2_REQ_HDRIFT 0.5
|
||||
|
|
|
@ -5,7 +5,7 @@
|
|||
ekf2 start &
|
||||
|
||||
# Start rover differential drive controller.
|
||||
differential_drive_control start
|
||||
differential_drive start
|
||||
|
||||
# Start Land Detector.
|
||||
land_detector start rover
|
||||
|
|
|
@ -439,7 +439,12 @@ else
|
|||
#
|
||||
# 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.
|
||||
|
@ -500,7 +505,12 @@ else
|
|||
#
|
||||
# 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.
|
||||
|
|
|
@ -30,4 +30,5 @@ exec find boards msg src platforms test \
|
|||
-path src/lib/cdrstream/cyclonedds -prune -o \
|
||||
-path src/lib/cdrstream/rosidl -prune -o \
|
||||
-path src/modules/zenoh/zenoh-pico -prune -o \
|
||||
-path boards/modalai/voxl2/libfc-sensor-api -prune -o \
|
||||
-type f \( -name "*.c" -o -name "*.h" -o -name "*.cpp" -o -name "*.hpp" \) | grep $PATTERN
|
||||
|
|
|
@ -73,9 +73,16 @@ struct_size, padding_end_size = add_padding_bytes(sorted_fields, search_path)
|
|||
#include <lib/matrix/matrix/math.hpp>
|
||||
#include <lib/mathlib/mathlib.h>
|
||||
|
||||
@{
|
||||
queue_length = 1
|
||||
for constant in spec.constants:
|
||||
if constant.name == 'ORB_QUEUE_LENGTH':
|
||||
queue_length = constant.val
|
||||
}@
|
||||
|
||||
@[for topic in topics]@
|
||||
static_assert(static_cast<orb_id_size_t>(ORB_ID::@topic) == @(all_topics.index(topic)), "ORB_ID index mismatch");
|
||||
ORB_DEFINE(@topic, struct @uorb_struct, @(struct_size-padding_end_size), @(message_hash)u, static_cast<orb_id_size_t>(ORB_ID::@topic));
|
||||
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]
|
||||
|
||||
void print_message(const orb_metadata *meta, const @uorb_struct& message)
|
||||
|
|
|
@ -1 +1 @@
|
|||
Subproject commit 33ac87a37676fb597de110e926bbf0a080169ffe
|
||||
Subproject commit da7206e057703cc645770f02437013358b71e1c0
|
|
@ -1 +1 @@
|
|||
Subproject commit c78f7f01417168e8faab7a83ade2129c0d26b39d
|
||||
Subproject commit 6b4ed09d1b495fbff663f098979cc046df013abd
|
|
@ -4,7 +4,7 @@
|
|||
"description": "Firmware for the ARK flow board",
|
||||
"image": "",
|
||||
"build_time": 0,
|
||||
"summary": "ARFFLOW",
|
||||
"summary": "ARKFLOW",
|
||||
"version": "0.1",
|
||||
"image_size": 0,
|
||||
"image_maxsize": 2080768,
|
||||
|
|
Binary file not shown.
|
@ -0,0 +1,5 @@
|
|||
CONFIG_BOARD_TOOLCHAIN="arm-none-eabi"
|
||||
CONFIG_BOARD_ARCHITECTURE="cortex-m4"
|
||||
CONFIG_BOARD_ROMFSROOT=""
|
||||
CONFIG_BOARD_CONSTRAINED_MEMORY=y
|
||||
CONFIG_DRIVERS_BOOTLOADERS=y
|
|
@ -0,0 +1,38 @@
|
|||
CONFIG_BOARD_TOOLCHAIN="arm-none-eabi"
|
||||
CONFIG_BOARD_ARCHITECTURE="cortex-m4"
|
||||
CONFIG_BOARD_ROMFSROOT="cannode"
|
||||
CONFIG_BOARD_NO_HELP=y
|
||||
CONFIG_BOARD_CONSTRAINED_MEMORY=y
|
||||
CONFIG_DRIVERS_BAROMETER_BMP388=y
|
||||
CONFIG_DRIVERS_BOOTLOADERS=y
|
||||
CONFIG_DRIVERS_GPS=y
|
||||
CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
|
||||
CONFIG_DRIVERS_MAGNETOMETER_BOSCH_BMM150=y
|
||||
CONFIG_DRIVERS_SAFETY_BUTTON=y
|
||||
CONFIG_DRIVERS_TONE_ALARM=y
|
||||
CONFIG_BOARD_UAVCAN_INTERFACES=1
|
||||
CONFIG_DRIVERS_UAVCANNODE=y
|
||||
CONFIG_UAVCANNODE_BEEP_COMMAND=y
|
||||
CONFIG_UAVCANNODE_GNSS_FIX=y
|
||||
CONFIG_UAVCANNODE_LIGHTS_COMMAND=y
|
||||
CONFIG_UAVCANNODE_MAGNETIC_FIELD_STRENGTH=y
|
||||
CONFIG_UAVCANNODE_RTK_DATA=y
|
||||
CONFIG_UAVCANNODE_SAFETY_BUTTON=y
|
||||
CONFIG_UAVCANNODE_STATIC_PRESSURE=y
|
||||
CONFIG_UAVCANNODE_STATIC_TEMPERATURE=y
|
||||
CONFIG_MODULES_EKF2=y
|
||||
CONFIG_MODULES_GYRO_CALIBRATION=y
|
||||
CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
|
||||
CONFIG_MODULES_SENSORS=y
|
||||
# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set
|
||||
# CONFIG_SENSORS_VEHICLE_AIR_DATA is not set
|
||||
# CONFIG_SENSORS_VEHICLE_GPS_POSITION is not set
|
||||
CONFIG_SYSTEMCMDS_PARAM=y
|
||||
CONFIG_SYSTEMCMDS_PERF=y
|
||||
CONFIG_SYSTEMCMDS_REBOOT=y
|
||||
CONFIG_SYSTEMCMDS_SYSTEM_TIME=y
|
||||
CONFIG_SYSTEMCMDS_TOP=y
|
||||
CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
|
||||
CONFIG_SYSTEMCMDS_UORB=y
|
||||
CONFIG_SYSTEMCMDS_VER=y
|
||||
CONFIG_SYSTEMCMDS_WORK_QUEUE=y
|
|
@ -0,0 +1,13 @@
|
|||
{
|
||||
"board_id": 84,
|
||||
"magic": "PX4FWv1",
|
||||
"description": "Firmware for the ARK Septentrio GPS board",
|
||||
"image": "",
|
||||
"build_time": 0,
|
||||
"summary": "ARKSEPTENTRIOGPS",
|
||||
"version": "0.1",
|
||||
"image_size": 0,
|
||||
"image_maxsize": 2080768,
|
||||
"git_identity": "",
|
||||
"board_revision": 0
|
||||
}
|
|
@ -0,0 +1,15 @@
|
|||
#!/bin/sh
|
||||
#
|
||||
# board specific defaults
|
||||
#------------------------------------------------------------------------------
|
||||
|
||||
param set-default CBRK_IO_SAFETY 0
|
||||
param set-default CANNODE_SUB_MBD 1
|
||||
param set-default CANNODE_SUB_RTCM 1
|
||||
param set-default MBE_ENABLE 1
|
||||
param set-default SENS_IMU_CLPNOTI 0
|
||||
|
||||
safety_button start
|
||||
tone_alarm start
|
||||
|
||||
ekf2 start
|
|
@ -0,0 +1,11 @@
|
|||
#!/bin/sh
|
||||
#
|
||||
# board sensors init
|
||||
#------------------------------------------------------------------------------
|
||||
gps start -d /dev/ttyS0 -p sbf
|
||||
|
||||
icm42688p -R 0 -s start
|
||||
|
||||
bmp388 -I -b 1 start
|
||||
|
||||
bmm150 -I -b 1 start
|
|
@ -0,0 +1,57 @@
|
|||
#
|
||||
# This file is autogenerated: PLEASE DO NOT EDIT IT.
|
||||
#
|
||||
# You can use "make menuconfig" to make any modifications to the installed .config file.
|
||||
# You can then do "make savedefconfig" to generate a new defconfig file that includes your
|
||||
# modifications.
|
||||
#
|
||||
CONFIG_ARCH="arm"
|
||||
CONFIG_ARCH_BOARD_CUSTOM=y
|
||||
CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/ark/septentrio-gps/nuttx-config"
|
||||
CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y
|
||||
CONFIG_ARCH_BOARD_CUSTOM_NAME="px4"
|
||||
CONFIG_ARCH_CHIP="stm32"
|
||||
CONFIG_ARCH_CHIP_STM32=y
|
||||
CONFIG_ARCH_CHIP_STM32F412CE=y
|
||||
CONFIG_ARCH_INTERRUPTSTACK=4096
|
||||
CONFIG_ARMV7M_MEMCPY=y
|
||||
CONFIG_ARMV7M_USEBASEPRI=y
|
||||
CONFIG_BINFMT_DISABLE=y
|
||||
CONFIG_BOARDCTL=y
|
||||
CONFIG_BOARD_LOOPSPERMSEC=16717
|
||||
CONFIG_DEBUG_FULLOPT=y
|
||||
CONFIG_DEBUG_SYMBOLS=y
|
||||
CONFIG_DEBUG_TCBINFO=y
|
||||
CONFIG_DEFAULT_SMALL=y
|
||||
CONFIG_DISABLE_MOUNTPOINT=y
|
||||
CONFIG_EXPERIMENTAL=y
|
||||
CONFIG_FDCLONE_DISABLE=y
|
||||
CONFIG_FDCLONE_STDIO=y
|
||||
CONFIG_HAVE_CXX=y
|
||||
CONFIG_HAVE_CXXINITIALIZE=y
|
||||
CONFIG_IDLETHREAD_STACKSIZE=4096
|
||||
CONFIG_INIT_STACKSIZE=4096
|
||||
CONFIG_LIBC_FLOATINGPOINT=y
|
||||
CONFIG_LIBC_LONG_LONG=y
|
||||
CONFIG_LIBC_STRERROR=y
|
||||
CONFIG_MM_REGIONS=2
|
||||
CONFIG_NAME_MAX=0
|
||||
CONFIG_NUNGET_CHARS=0
|
||||
CONFIG_PREALLOC_TIMERS=0
|
||||
CONFIG_PTHREAD_STACK_MIN=512
|
||||
CONFIG_RAM_SIZE=262144
|
||||
CONFIG_RAM_START=0x20010000
|
||||
CONFIG_RAW_BINARY=y
|
||||
CONFIG_SIG_DEFAULT=y
|
||||
CONFIG_SIG_SIGALRM_ACTION=y
|
||||
CONFIG_SIG_SIGUSR1_ACTION=y
|
||||
CONFIG_SIG_SIGUSR2_ACTION=y
|
||||
CONFIG_STACK_COLORATION=y
|
||||
CONFIG_START_DAY=30
|
||||
CONFIG_START_MONTH=11
|
||||
CONFIG_STDIO_DISABLE_BUFFERING=y
|
||||
CONFIG_STM32_FLASH_CONFIG_G=y
|
||||
CONFIG_STM32_NOEXT_VECTORS=y
|
||||
CONFIG_STM32_TIM8=y
|
||||
CONFIG_TASK_NAME_SIZE=0
|
||||
CONFIG_USEC_PER_TICK=1000
|
|
@ -0,0 +1,152 @@
|
|||
/************************************************************************************
|
||||
* configs/px4fmu/include/board.h
|
||||
* include/arch/board/board.h
|
||||
*
|
||||
* Copyright (C) 2009 Gregory Nutt. All rights reserved.
|
||||
* Author: Gregory Nutt <gnutt@nuttx.org>
|
||||
*
|
||||
* 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 NuttX 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 "board_dma_map.h"
|
||||
|
||||
#ifndef __ARCH_BOARD_BOARD_H
|
||||
#define __ARCH_BOARD_BOARD_H
|
||||
|
||||
#include <nuttx/config.h>
|
||||
#ifndef __ASSEMBLY__
|
||||
# include <stdint.h>
|
||||
#endif
|
||||
|
||||
#include <stm32.h>
|
||||
|
||||
/* HSI - 8 MHz RC factory-trimmed
|
||||
* LSI - 32 KHz RC
|
||||
* HSE - 8 MHz Crystal
|
||||
* LSE - not installed
|
||||
*/
|
||||
#define STM32_BOARD_USEHSE 1
|
||||
#define STM32_BOARD_XTAL 8000000
|
||||
#define STM32_HSE_FREQUENCY STM32_BOARD_XTAL
|
||||
|
||||
#define STM32_HSI_FREQUENCY 16000000ul
|
||||
#define STM32_LSI_FREQUENCY 32000
|
||||
|
||||
/* Main PLL Configuration */
|
||||
#define STM32_PLLCFG_PLLM RCC_PLLCFG_PLLM(8)
|
||||
#define STM32_PLLCFG_PLLN RCC_PLLCFG_PLLN(384)
|
||||
#define STM32_PLLCFG_PLLP RCC_PLLCFG_PLLP_4
|
||||
#define STM32_PLLCFG_PLLQ RCC_PLLCFG_PLLQ(8)
|
||||
#define STM32_PLLCFG_PLLR RCC_PLLCFG_PLLR(2)
|
||||
|
||||
#define STM32_RCC_PLLI2SCFGR_PLLI2SM RCC_PLLI2SCFGR_PLLI2SM(16)
|
||||
#define STM32_RCC_PLLI2SCFGR_PLLI2SN RCC_PLLI2SCFGR_PLLI2SN(192)
|
||||
#define STM32_RCC_PLLI2SCFGR_PLLI2SQ RCC_PLLI2SCFGR_PLLI2SQ(2)
|
||||
#define STM32_RCC_PLLI2SCFGR_PLLI2SR RCC_PLLI2SCFGR_PLLI2SR(2)
|
||||
#define STM32_RCC_PLLI2SCFGR_PLLI2SSRC RCC_PLLI2SCFGR_PLLI2SSRC(0) /* HSE or HSI depending on PLLSRC of PLLCFGR*/
|
||||
|
||||
#define STM32_RCC_DCKCFGR2_CK48MSEL RCC_DCKCFGR2_CK48MSEL_PLL
|
||||
#define STM32_RCC_DCKCFGR2_FMPI2C1SEL RCC_DCKCFGR2_FMPI2C1SEL_APB
|
||||
#define STM32_RCC_DCKCFGR2_SDIOSEL RCC_DCKCFGR2_SDIOSEL_48MHZ
|
||||
|
||||
#define STM32_SYSCLK_FREQUENCY 96000000ul
|
||||
|
||||
/* AHB clock (HCLK) is SYSCLK (96MHz) */
|
||||
#define STM32_RCC_CFGR_HPRE RCC_CFGR_HPRE_SYSCLK /* HCLK = SYSCLK / 1 */
|
||||
#define STM32_HCLK_FREQUENCY STM32_SYSCLK_FREQUENCY
|
||||
#define STM32_BOARD_HCLK STM32_HCLK_FREQUENCY /* Same as above, to satisfy compiler */
|
||||
|
||||
/* APB1 clock (PCLK1) is HCLK/2 (48MHz) */
|
||||
#define STM32_RCC_CFGR_PPRE1 RCC_CFGR_PPRE1_HCLKd2 /* PCLK1 = HCLK / 2 */
|
||||
#define STM32_PCLK1_FREQUENCY (STM32_HCLK_FREQUENCY/2)
|
||||
|
||||
/* Timers driven from APB1 will be twice PCLK1 (see page 112 of reference manual) */
|
||||
#define STM32_APB1_TIM2_CLKIN (2*STM32_PCLK1_FREQUENCY)
|
||||
#define STM32_APB1_TIM3_CLKIN (2*STM32_PCLK1_FREQUENCY)
|
||||
#define STM32_APB1_TIM4_CLKIN (2*STM32_PCLK1_FREQUENCY)
|
||||
#define STM32_APB1_TIM5_CLKIN (2*STM32_PCLK1_FREQUENCY)
|
||||
#define STM32_APB1_TIM12_CLKIN (2*STM32_PCLK1_FREQUENCY)
|
||||
#define STM32_APB1_TIM13_CLKIN (2*STM32_PCLK1_FREQUENCY)
|
||||
#define STM32_APB1_TIM14_CLKIN (2*STM32_PCLK1_FREQUENCY)
|
||||
|
||||
/* APB2 clock (PCLK2) is HCLK (96MHz) */
|
||||
#define STM32_RCC_CFGR_PPRE2 RCC_CFGR_PPRE2_HCLK /* PCLK2 = HCLK */
|
||||
#define STM32_PCLK2_FREQUENCY (STM32_HCLK_FREQUENCY)
|
||||
|
||||
/* Timers driven from APB2 will be PCLK2 since no prescale division */
|
||||
#define STM32_APB2_TIM1_CLKIN (STM32_PCLK2_FREQUENCY)
|
||||
#define STM32_APB2_TIM8_CLKIN (STM32_PCLK2_FREQUENCY)
|
||||
#define STM32_APB2_TIM9_CLKIN (STM32_PCLK2_FREQUENCY)
|
||||
#define STM32_APB2_TIM10_CLKIN (STM32_PCLK2_FREQUENCY)
|
||||
#define STM32_APB2_TIM11_CLKIN (STM32_PCLK2_FREQUENCY)
|
||||
|
||||
/* Timer Frequencies, if APBx is set to 1, frequency is same to APBx otherwise frequency is 2xAPBx. */
|
||||
#define BOARD_TIM2_FREQUENCY (2 * STM32_PCLK1_FREQUENCY)
|
||||
#define BOARD_TIM3_FREQUENCY (2 * STM32_PCLK1_FREQUENCY)
|
||||
#define BOARD_TIM4_FREQUENCY (2 * STM32_PCLK1_FREQUENCY)
|
||||
#define BOARD_TIM5_FREQUENCY (2 * STM32_PCLK1_FREQUENCY)
|
||||
#define BOARD_TIM6_FREQUENCY (2 * STM32_PCLK1_FREQUENCY)
|
||||
#define BOARD_TIM7_FREQUENCY (2 * STM32_PCLK1_FREQUENCY)
|
||||
#define BOARD_TIM8_FREQUENCY (2 * STM32_PCLK2_FREQUENCY)
|
||||
|
||||
/* Alternate function pin selections ************************************************/
|
||||
|
||||
/* UARTs */
|
||||
#define GPIO_USART1_RX GPIO_USART1_RX_2
|
||||
#define GPIO_USART1_TX GPIO_USART1_TX_3
|
||||
|
||||
#define GPIO_USART2_RX GPIO_USART2_RX_1
|
||||
#define GPIO_USART2_TX GPIO_USART2_TX_1
|
||||
|
||||
/* CAN */
|
||||
#define GPIO_CAN1_RX GPIO_CAN1_RX_1
|
||||
#define GPIO_CAN1_TX GPIO_CAN1_TX_1
|
||||
|
||||
/* I2C */
|
||||
|
||||
#define GPIO_MCU_I2C1_SCL
|
||||
#define GPIO_MCU_I2C1_SDA
|
||||
|
||||
#define GPIO_I2C1_SCL GPIO_I2C1_SCL_1
|
||||
#define GPIO_I2C1_SDA GPIO_I2C1_SDA_1
|
||||
|
||||
#define GPIO_I2C1_SCL_GPIO (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTB | GPIO_PIN6)
|
||||
#define GPIO_I2C1_SDA_GPIO (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTB | GPIO_PIN7)
|
||||
|
||||
#define GPIO_I2C2_SCL GPIO_I2C2_SCL_1
|
||||
#define GPIO_I2C2_SDA GPIO_I2C2_SDA_4
|
||||
|
||||
#define GPIO_I2C2_SCL_GPIO (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTB | GPIO_PIN9)
|
||||
#define GPIO_I2C2_SDA_GPIO (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTB | GPIO_PIN10)
|
||||
|
||||
/* SPI */
|
||||
#define GPIO_SPI1_MISO GPIO_SPI1_MISO_1
|
||||
#define GPIO_SPI1_MOSI GPIO_SPI1_MOSI_1
|
||||
#define GPIO_SPI1_SCK GPIO_SPI1_SCK_1
|
||||
|
||||
#endif /* __ARCH_BOARD_BOARD_H */
|
|
@ -0,0 +1,46 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
// DMA1 Channel/Stream Selections
|
||||
//--------------------------------------------//---------------------------//----------------
|
||||
|
||||
|
||||
// DMA2 Channel/Stream Selections
|
||||
//--------------------------------------------//---------------------------//----------------
|
||||
#define DMACHAN_SPI1_RX DMAMAP_SPI1_RX_1 // DMA2, Stream 0, Channel 3
|
||||
#define DMACHAN_SPI1_TX DMAMAP_SPI1_TX_1 // DMA2, Stream 3, Channel 3
|
||||
#define DMACHAN_USART1_RX DMAMAP_USART1_RX_1 // DMA2, Stream 2, Channel 4
|
||||
#define DMAMAP_USART1_RX DMAMAP_USART1_RX_1
|
||||
//#define DMACHAN_USART1_TX DMAMAP_USART1_TX // DMA2, Stream 7, Channel 4
|
|
@ -0,0 +1,153 @@
|
|||
#
|
||||
# This file is autogenerated: PLEASE DO NOT EDIT IT.
|
||||
#
|
||||
# You can use "make menuconfig" to make any modifications to the installed .config file.
|
||||
# You can then do "make savedefconfig" to generate a new defconfig file that includes your
|
||||
# modifications.
|
||||
#
|
||||
# CONFIG_DISABLE_ENVIRON is not set
|
||||
# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set
|
||||
# CONFIG_DISABLE_PTHREAD is not set
|
||||
# CONFIG_NSH_DISABLEBG is not set
|
||||
# CONFIG_NSH_DISABLESCRIPT is not set
|
||||
# CONFIG_NSH_DISABLE_CAT is not set
|
||||
# CONFIG_NSH_DISABLE_CD is not set
|
||||
# CONFIG_NSH_DISABLE_CP is not set
|
||||
# CONFIG_NSH_DISABLE_DATE is not set
|
||||
# CONFIG_NSH_DISABLE_DF is not set
|
||||
# CONFIG_NSH_DISABLE_ECHO is not set
|
||||
# CONFIG_NSH_DISABLE_ENV is not set
|
||||
# CONFIG_NSH_DISABLE_EXEC is not set
|
||||
# CONFIG_NSH_DISABLE_EXPORT is not set
|
||||
# CONFIG_NSH_DISABLE_FREE is not set
|
||||
# CONFIG_NSH_DISABLE_GET is not set
|
||||
# CONFIG_NSH_DISABLE_HELP is not set
|
||||
# CONFIG_NSH_DISABLE_ITEF is not set
|
||||
# CONFIG_NSH_DISABLE_KILL is not set
|
||||
# CONFIG_NSH_DISABLE_LOOPS is not set
|
||||
# CONFIG_NSH_DISABLE_LS is not set
|
||||
# CONFIG_NSH_DISABLE_MKDIR is not set
|
||||
# CONFIG_NSH_DISABLE_MOUNT is not set
|
||||
# CONFIG_NSH_DISABLE_MV is not set
|
||||
# CONFIG_NSH_DISABLE_PS is not set
|
||||
# CONFIG_NSH_DISABLE_PSSTACKUSAGE is not set
|
||||
# CONFIG_NSH_DISABLE_PWD is not set
|
||||
# CONFIG_NSH_DISABLE_RM is not set
|
||||
# CONFIG_NSH_DISABLE_RMDIR is not set
|
||||
# CONFIG_NSH_DISABLE_SEMICOLON is not set
|
||||
# CONFIG_NSH_DISABLE_SET is not set
|
||||
# CONFIG_NSH_DISABLE_SLEEP is not set
|
||||
# CONFIG_NSH_DISABLE_SOURCE is not set
|
||||
# CONFIG_NSH_DISABLE_TEST is not set
|
||||
# CONFIG_NSH_DISABLE_TIME is not set
|
||||
# CONFIG_NSH_DISABLE_UMOUNT is not set
|
||||
# CONFIG_NSH_DISABLE_UNSET is not set
|
||||
# CONFIG_NSH_DISABLE_USLEEP is not set
|
||||
CONFIG_ARCH="arm"
|
||||
CONFIG_ARCH_BOARD_CUSTOM=y
|
||||
CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/ark/septentrio-gps/nuttx-config"
|
||||
CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y
|
||||
CONFIG_ARCH_BOARD_CUSTOM_NAME="px4"
|
||||
CONFIG_ARCH_CHIP="stm32"
|
||||
CONFIG_ARCH_CHIP_STM32=y
|
||||
CONFIG_ARCH_CHIP_STM32F412CE=y
|
||||
CONFIG_ARCH_INTERRUPTSTACK=768
|
||||
CONFIG_ARCH_STACKDUMP=y
|
||||
CONFIG_ARMV7M_MEMCPY=y
|
||||
CONFIG_ARMV7M_USEBASEPRI=y
|
||||
CONFIG_BOARDCTL_RESET=y
|
||||
CONFIG_BOARD_ASSERT_RESET_VALUE=0
|
||||
CONFIG_BOARD_LOOPSPERMSEC=16717
|
||||
CONFIG_BOARD_RESET_ON_ASSERT=2
|
||||
CONFIG_BUILTIN=y
|
||||
CONFIG_DEBUG_FULLOPT=y
|
||||
CONFIG_DEBUG_HARDFAULT_ALERT=y
|
||||
CONFIG_DEBUG_SYMBOLS=y
|
||||
CONFIG_DEBUG_TCBINFO=y
|
||||
CONFIG_DEFAULT_SMALL=y
|
||||
CONFIG_FDCLONE_STDIO=y
|
||||
CONFIG_FS_CROMFS=y
|
||||
CONFIG_FS_ROMFS=y
|
||||
CONFIG_GRAN=y
|
||||
CONFIG_GRAN_INTR=y
|
||||
CONFIG_HAVE_CXX=y
|
||||
CONFIG_HAVE_CXXINITIALIZE=y
|
||||
CONFIG_I2C=y
|
||||
CONFIG_I2C_RESET=y
|
||||
CONFIG_IDLETHREAD_STACKSIZE=750
|
||||
CONFIG_INIT_ENTRYPOINT="nsh_main"
|
||||
CONFIG_INIT_STACKSIZE=2624
|
||||
CONFIG_LIBC_FLOATINGPOINT=y
|
||||
CONFIG_LIBC_LONG_LONG=y
|
||||
CONFIG_LIBC_MAX_EXITFUNS=1
|
||||
CONFIG_MEMSET_64BIT=y
|
||||
CONFIG_MEMSET_OPTSPEED=y
|
||||
CONFIG_MM_REGIONS=2
|
||||
CONFIG_NAME_MAX=40
|
||||
CONFIG_NSH_ARCHINIT=y
|
||||
CONFIG_NSH_ARGCAT=y
|
||||
CONFIG_NSH_BUILTIN_APPS=y
|
||||
CONFIG_NSH_CMDPARMS=y
|
||||
CONFIG_NSH_CROMFSETC=y
|
||||
CONFIG_NSH_LINELEN=128
|
||||
CONFIG_NSH_MAXARGUMENTS=15
|
||||
CONFIG_NSH_NESTDEPTH=8
|
||||
CONFIG_NSH_QUOTE=y
|
||||
CONFIG_NSH_ROMFSETC=y
|
||||
CONFIG_NSH_ROMFSSECTSIZE=128
|
||||
CONFIG_NSH_VARS=y
|
||||
CONFIG_PREALLOC_TIMERS=50
|
||||
CONFIG_PTHREAD_MUTEX_ROBUST=y
|
||||
CONFIG_PTHREAD_STACK_MIN=512
|
||||
CONFIG_RAM_SIZE=262144
|
||||
CONFIG_RAM_START=0x20000000
|
||||
CONFIG_RAW_BINARY=y
|
||||
CONFIG_SCHED_HPWORK=y
|
||||
CONFIG_SCHED_HPWORKPRIORITY=249
|
||||
CONFIG_SCHED_HPWORKSTACKSIZE=1280
|
||||
CONFIG_SCHED_INSTRUMENTATION=y
|
||||
CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
|
||||
CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
|
||||
CONFIG_SCHED_WAITPID=y
|
||||
CONFIG_SERIAL_TERMIOS=y
|
||||
CONFIG_SIG_DEFAULT=y
|
||||
CONFIG_SIG_SIGALRM_ACTION=y
|
||||
CONFIG_SIG_SIGUSR1_ACTION=y
|
||||
CONFIG_SIG_SIGUSR2_ACTION=y
|
||||
CONFIG_SIG_SIGWORK=4
|
||||
CONFIG_STACK_COLORATION=y
|
||||
CONFIG_START_DAY=30
|
||||
CONFIG_START_MONTH=11
|
||||
CONFIG_STDIO_BUFFER_SIZE=32
|
||||
CONFIG_STM32_ADC1=y
|
||||
CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG=y
|
||||
CONFIG_STM32_DMA1=y
|
||||
CONFIG_STM32_DMA2=y
|
||||
CONFIG_STM32_FLASH_CONFIG_G=y
|
||||
CONFIG_STM32_FLASH_PREFETCH=y
|
||||
CONFIG_STM32_FLOWCONTROL_BROKEN=y
|
||||
CONFIG_STM32_I2C1=y
|
||||
CONFIG_STM32_I2C2=y
|
||||
CONFIG_STM32_JTAG_SW_ENABLE=y
|
||||
CONFIG_STM32_PWR=y
|
||||
CONFIG_STM32_SERIALBRK_BSDCOMPAT=y
|
||||
CONFIG_STM32_SERIAL_DISABLE_REORDERING=y
|
||||
CONFIG_STM32_SPI1=y
|
||||
CONFIG_STM32_SPI1_DMA=y
|
||||
CONFIG_STM32_SPI1_DMA_BUFFER=1024
|
||||
CONFIG_STM32_TIM8=y
|
||||
CONFIG_STM32_USART1=y
|
||||
CONFIG_STM32_USART2=y
|
||||
CONFIG_STM32_USART_BREAKS=y
|
||||
CONFIG_STM32_WWDG=y
|
||||
CONFIG_SYSTEM_NSH=y
|
||||
CONFIG_TASK_NAME_SIZE=24
|
||||
CONFIG_USART1_BAUD=57600
|
||||
CONFIG_USART1_RXBUFSIZE=2000
|
||||
CONFIG_USART1_RXDMA=y
|
||||
CONFIG_USART1_TXBUFSIZE=2000
|
||||
CONFIG_USART2_BAUD=57600
|
||||
CONFIG_USART2_RXBUFSIZE=600
|
||||
CONFIG_USART2_SERIAL_CONSOLE=y
|
||||
CONFIG_USART2_TXBUFSIZE=1100
|
||||
CONFIG_USEC_PER_TICK=1000
|
|
@ -0,0 +1,134 @@
|
|||
/****************************************************************************
|
||||
* nuttx-config/scripts/canbootloader_script.ld
|
||||
*
|
||||
* Copyright (C) 2015 Gregory Nutt. All rights reserved.
|
||||
* Author: Gregory Nutt <gnutt@nuttx.org>
|
||||
*
|
||||
* 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 NuttX 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/* The STM32F412 has 1M of FLASH beginning at address 0x0800:0000 and
|
||||
* 256Kb of SRAM. SRAM is split up into three blocks:
|
||||
*
|
||||
* 1) 112Kb of SRAM beginning at address 0x2000:0000
|
||||
* 2) 16Kb of SRAM beginning at address 0x2001:c000
|
||||
* 3) 64Kb of SRAM beginning at address 0x2002:0000
|
||||
* 4) 64Kb of TCM SRAM beginning at address 0x1000:0000
|
||||
*
|
||||
* When booting from FLASH, FLASH memory is aliased to address 0x0000:0000
|
||||
* where the code expects to begin execution by jumping to the entry point in
|
||||
* the 0x0800:0000 address range.
|
||||
*
|
||||
* The first 0x10000 of flash is reserved for the bootloader.
|
||||
*/
|
||||
|
||||
MEMORY
|
||||
{
|
||||
flash (rx) : ORIGIN = 0x08000000, LENGTH = 32K
|
||||
sram (rwx) : ORIGIN = 0x20000000, LENGTH = 192K
|
||||
}
|
||||
|
||||
OUTPUT_ARCH(arm)
|
||||
|
||||
ENTRY(__start) /* treat __start as the anchor for dead code stripping */
|
||||
EXTERN(_vectors) /* force the vectors to be included in the output */
|
||||
|
||||
/*
|
||||
* Ensure that abort() is present in the final object. The exception handling
|
||||
* code pulled in by libgcc.a requires it (and that code cannot be easily avoided).
|
||||
*/
|
||||
EXTERN(abort)
|
||||
SECTIONS
|
||||
{
|
||||
.text : {
|
||||
_stext = ABSOLUTE(.);
|
||||
*(.vectors)
|
||||
*(.text .text.*)
|
||||
*(.fixup)
|
||||
*(.gnu.warning)
|
||||
*(.rodata .rodata.*)
|
||||
*(.gnu.linkonce.t.*)
|
||||
*(.got)
|
||||
*(.gcc_except_table)
|
||||
*(.gnu.linkonce.r.*)
|
||||
_etext = ABSOLUTE(.);
|
||||
} > flash
|
||||
|
||||
/*
|
||||
* Init functions (static constructors and the like)
|
||||
*/
|
||||
.init_section : {
|
||||
_sinit = ABSOLUTE(.);
|
||||
KEEP(*(.init_array .init_array.*))
|
||||
_einit = ABSOLUTE(.);
|
||||
} > flash
|
||||
|
||||
.ARM.extab : {
|
||||
*(.ARM.extab*)
|
||||
} > flash
|
||||
|
||||
__exidx_start = ABSOLUTE(.);
|
||||
.ARM.exidx : {
|
||||
*(.ARM.exidx*)
|
||||
} > flash
|
||||
__exidx_end = ABSOLUTE(.);
|
||||
|
||||
_eronly = ABSOLUTE(.);
|
||||
|
||||
.data : {
|
||||
_sdata = ABSOLUTE(.);
|
||||
*(.data .data.*)
|
||||
*(.gnu.linkonce.d.*)
|
||||
CONSTRUCTORS
|
||||
_edata = ABSOLUTE(.);
|
||||
} > sram AT > flash
|
||||
|
||||
.bss : {
|
||||
_sbss = ABSOLUTE(.);
|
||||
*(.bss .bss.*)
|
||||
*(.gnu.linkonce.b.*)
|
||||
*(COMMON)
|
||||
. = ALIGN(4);
|
||||
_ebss = ABSOLUTE(.);
|
||||
} > sram
|
||||
|
||||
/* Stabs debugging sections. */
|
||||
.stab 0 : { *(.stab) }
|
||||
.stabstr 0 : { *(.stabstr) }
|
||||
.stab.excl 0 : { *(.stab.excl) }
|
||||
.stab.exclstr 0 : { *(.stab.exclstr) }
|
||||
.stab.index 0 : { *(.stab.index) }
|
||||
.stab.indexstr 0 : { *(.stab.indexstr) }
|
||||
.comment 0 : { *(.comment) }
|
||||
.debug_abbrev 0 : { *(.debug_abbrev) }
|
||||
.debug_info 0 : { *(.debug_info) }
|
||||
.debug_line 0 : { *(.debug_line) }
|
||||
.debug_pubnames 0 : { *(.debug_pubnames) }
|
||||
.debug_aranges 0 : { *(.debug_aranges) }
|
||||
}
|
|
@ -0,0 +1,146 @@
|
|||
/****************************************************************************
|
||||
* scripts/ld.script
|
||||
*
|
||||
* Copyright (C) 2011 Gregory Nutt. All rights reserved.
|
||||
* Author: Gregory Nutt <gnutt@nuttx.org>
|
||||
*
|
||||
* 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 NuttX 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/* The STM32F412 has 1M of FLASH beginning at address 0x0800:0000 and
|
||||
* 256Kb of SRAM. SRAM is split up into three blocks:
|
||||
*
|
||||
* 1) 112Kb of SRAM beginning at address 0x2000:0000
|
||||
* 2) 16Kb of SRAM beginning at address 0x2001:c000
|
||||
* 3) 64Kb of SRAM beginning at address 0x2002:0000
|
||||
* 4) 64Kb of TCM SRAM beginning at address 0x1000:0000
|
||||
*
|
||||
* When booting from FLASH, FLASH memory is aliased to address 0x0000:0000
|
||||
* where the code expects to begin execution by jumping to the entry point in
|
||||
* the 0x0800:0000 address range.
|
||||
*
|
||||
* The first 0x10000 of flash is reserved for the bootloader.
|
||||
*/
|
||||
|
||||
MEMORY
|
||||
{
|
||||
flash (rx) : ORIGIN = 0x08010000, LENGTH = 928K
|
||||
sram (rwx) : ORIGIN = 0x20000000, LENGTH = 192K
|
||||
}
|
||||
|
||||
OUTPUT_ARCH(arm)
|
||||
|
||||
ENTRY(__start) /* treat __start as the anchor for dead code stripping */
|
||||
EXTERN(_vectors) /* force the vectors to be included in the output */
|
||||
|
||||
/*
|
||||
* Ensure that abort() is present in the final object. The exception handling
|
||||
* code pulled in by libgcc.a requires it (and that code cannot be easily avoided).
|
||||
*/
|
||||
EXTERN(abort)
|
||||
EXTERN(_bootdelay_signature)
|
||||
|
||||
SECTIONS
|
||||
{
|
||||
.text : {
|
||||
_stext = ABSOLUTE(.);
|
||||
*(.vectors)
|
||||
. = ALIGN(8);
|
||||
/*
|
||||
* This section positions the app_descriptor_t used
|
||||
* by the make_can_boot_descriptor.py tool to set
|
||||
* the application image's descriptor so that the
|
||||
* uavcan bootloader has the ability to validate the
|
||||
* image crc, size etc
|
||||
*/
|
||||
KEEP(*(.app_descriptor))
|
||||
*(.text .text.*)
|
||||
*(.fixup)
|
||||
*(.gnu.warning)
|
||||
*(.rodata .rodata.*)
|
||||
*(.gnu.linkonce.t.*)
|
||||
*(.got)
|
||||
*(.gcc_except_table)
|
||||
*(.gnu.linkonce.r.*)
|
||||
_etext = ABSOLUTE(.);
|
||||
} > flash
|
||||
|
||||
/*
|
||||
* Init functions (static constructors and the like)
|
||||
*/
|
||||
.init_section : {
|
||||
_sinit = ABSOLUTE(.);
|
||||
KEEP(*(.init_array .init_array.*))
|
||||
_einit = ABSOLUTE(.);
|
||||
} > flash
|
||||
|
||||
|
||||
.ARM.extab : {
|
||||
*(.ARM.extab*)
|
||||
} > flash
|
||||
|
||||
__exidx_start = ABSOLUTE(.);
|
||||
.ARM.exidx : {
|
||||
*(.ARM.exidx*)
|
||||
} > flash
|
||||
__exidx_end = ABSOLUTE(.);
|
||||
|
||||
_eronly = ABSOLUTE(.);
|
||||
|
||||
.data : {
|
||||
_sdata = ABSOLUTE(.);
|
||||
*(.data .data.*)
|
||||
*(.gnu.linkonce.d.*)
|
||||
CONSTRUCTORS
|
||||
_edata = ABSOLUTE(.);
|
||||
} > sram AT > flash
|
||||
|
||||
.bss : {
|
||||
_sbss = ABSOLUTE(.);
|
||||
*(.bss .bss.*)
|
||||
*(.gnu.linkonce.b.*)
|
||||
*(COMMON)
|
||||
. = ALIGN(4);
|
||||
_ebss = ABSOLUTE(.);
|
||||
} > sram
|
||||
|
||||
/* Stabs debugging sections. */
|
||||
.stab 0 : { *(.stab) }
|
||||
.stabstr 0 : { *(.stabstr) }
|
||||
.stab.excl 0 : { *(.stab.excl) }
|
||||
.stab.exclstr 0 : { *(.stab.exclstr) }
|
||||
.stab.index 0 : { *(.stab.index) }
|
||||
.stab.indexstr 0 : { *(.stab.indexstr) }
|
||||
.comment 0 : { *(.comment) }
|
||||
.debug_abbrev 0 : { *(.debug_abbrev) }
|
||||
.debug_info 0 : { *(.debug_info) }
|
||||
.debug_line 0 : { *(.debug_line) }
|
||||
.debug_pubnames 0 : { *(.debug_pubnames) }
|
||||
.debug_aranges 0 : { *(.debug_aranges) }
|
||||
}
|
|
@ -0,0 +1,67 @@
|
|||
############################################################################
|
||||
#
|
||||
# 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.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
if("${PX4_BOARD_LABEL}" STREQUAL "canbootloader")
|
||||
|
||||
add_library(drivers_board
|
||||
boot_config.h
|
||||
boot.c
|
||||
led.c
|
||||
led.h
|
||||
)
|
||||
target_link_libraries(drivers_board
|
||||
PRIVATE
|
||||
nuttx_arch
|
||||
nuttx_drivers
|
||||
canbootloader
|
||||
)
|
||||
target_include_directories(drivers_board PRIVATE ${PX4_SOURCE_DIR}/platforms/nuttx/src/canbootloader)
|
||||
|
||||
else()
|
||||
add_library(drivers_board
|
||||
can.c
|
||||
i2c.cpp
|
||||
init.c
|
||||
led.c
|
||||
spi.cpp
|
||||
)
|
||||
|
||||
target_link_libraries(drivers_board
|
||||
PRIVATE
|
||||
arch_spi
|
||||
drivers__led # drv_led_start
|
||||
nuttx_arch
|
||||
nuttx_drivers
|
||||
px4_layer
|
||||
)
|
||||
endif()
|
|
@ -0,0 +1,125 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file board_config.h
|
||||
*
|
||||
* board internal definitions
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#include <nuttx/compiler.h>
|
||||
#include <stdint.h>
|
||||
|
||||
/* BUTTON */
|
||||
#define GPIO_BTN_SAFETY /* PB15 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTB|GPIO_PIN15)
|
||||
|
||||
/* Safety LED */
|
||||
#define GPIO_LED_SAFETY /* PA1 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN1)
|
||||
|
||||
/* Tone alarm output. */
|
||||
#define TONE_ALARM_TIMER 2 /* timer 2 */
|
||||
#define TONE_ALARM_CHANNEL 1 /* channel 1 */
|
||||
#define GPIO_TONE_ALARM_IDLE /* PA0 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN0)
|
||||
#define GPIO_TONE_ALARM /* PA0 */ (GPIO_ALT|GPIO_AF1|GPIO_SPEED_2MHz|GPIO_PUSHPULL|GPIO_PORTA|GPIO_PIN0)
|
||||
|
||||
/* CAN Silent mode control */
|
||||
#define GPIO_CAN1_SILENT_S0 /* PB12 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN12)
|
||||
|
||||
/* CAN termination software control */
|
||||
#define GPIO_CAN1_TERMINATION /* PB13 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN13)
|
||||
#define GPIO_CAN_TERM GPIO_CAN1_TERMINATION
|
||||
|
||||
/* ICM42688p FSYNC */
|
||||
#define GPIO_42688P_FSYNC /* PB8 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN8)
|
||||
|
||||
/* Boot config */
|
||||
#define GPIO_BOOT_CONFIG /* PC15 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN15|GPIO_EXTI)
|
||||
|
||||
/* LEDs are driven with open drain to support Anode to 5V or 3.3V */
|
||||
#define GPIO_TIM1_CH1 /* PA8 */ (GPIO_TIM1_CH1_1|GPIO_OPENDRAIN|GPIO_SPEED_2MHz)
|
||||
#define GPIO_TIM1_CH2 /* PA9 */ (GPIO_TIM1_CH2_1|GPIO_OPENDRAIN|GPIO_SPEED_2MHz)
|
||||
#define GPIO_TIM1_CH3 /* PA10 */ (GPIO_TIM1_CH3_1|GPIO_OPENDRAIN|GPIO_SPEED_2MHz)
|
||||
|
||||
#define GPIO_I2C1_SCL_RESET /* PB6 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN6)
|
||||
#define GPIO_I2C1_SDA_RESET /* PB7 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN7)
|
||||
|
||||
#define GPIO_I2C2_SCL_RESET /* PB10 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10)
|
||||
#define GPIO_I2C2_SDA_RESET /* PB9 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN9)
|
||||
|
||||
#define GPIO_I2C2_SCL_RESET /* PB10 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10)
|
||||
#define GPIO_I2C2_SDA_RESET /* PB9 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN9)
|
||||
|
||||
#define GPIO_USART1_RX_GPIO (GPIO_INPUT|GPIO_PULLUP|GPIO_SPEED_50MHz|GPIO_PORTB|GPIO_PIN3)
|
||||
#define GPIO_USART1_TX_GPIO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_PULLUP|GPIO_SPEED_50MHz|GPIO_PORTA|GPIO_PIN15)
|
||||
|
||||
#define GPIO_USART2_RX_GPIO (GPIO_INPUT|GPIO_PULLUP|GPIO_SPEED_50MHz|GPIO_PORTA|GPIO_PIN3)
|
||||
#define GPIO_USART2_TX_GPIO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_PULLUP|GPIO_SPEED_50MHz|GPIO_PORTA|GPIO_PIN2)
|
||||
|
||||
#define FLASH_BASED_PARAMS
|
||||
|
||||
/* High-resolution timer */
|
||||
#define HRT_TIMER 3 /* use timer 3 for the HRT */
|
||||
#define HRT_TIMER_CHANNEL 4 /* use capture/compare channel 4 */
|
||||
|
||||
#define PX4_GPIO_INIT_LIST { \
|
||||
GPIO_BTN_SAFETY, \
|
||||
GPIO_LED_SAFETY, \
|
||||
GPIO_I2C1_SCL_RESET, \
|
||||
GPIO_I2C1_SDA_RESET, \
|
||||
GPIO_I2C2_SCL_RESET, \
|
||||
GPIO_I2C2_SDA_RESET, \
|
||||
GPIO_42688P_FSYNC, \
|
||||
GPIO_BOOT_CONFIG, \
|
||||
GPIO_CAN1_TX, \
|
||||
GPIO_CAN1_RX, \
|
||||
GPIO_CAN1_SILENT_S0, \
|
||||
GPIO_CAN1_TERMINATION, \
|
||||
}
|
||||
|
||||
__BEGIN_DECLS
|
||||
|
||||
#define BOARD_HAS_N_S_RGB_LED 1
|
||||
#define BOARD_MAX_LEDS BOARD_HAS_N_S_RGB_LED
|
||||
|
||||
#ifndef __ASSEMBLY__
|
||||
|
||||
extern void stm32_spiinitialize(void);
|
||||
|
||||
#include <px4_platform_common/board_common.h>
|
||||
|
||||
#endif /* __ASSEMBLY__ */
|
||||
|
||||
__END_DECLS
|
|
@ -0,0 +1,188 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
|
||||
* Author: Ben Dyer <ben_dyer@mac.com>
|
||||
* Pavel Kirienko <pavel.kirienko@zubax.com>
|
||||
* David Sidrane <david_s5@nscdg.com>
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Included Files
|
||||
****************************************************************************/
|
||||
|
||||
#include <px4_config.h>
|
||||
#include <stdint.h>
|
||||
#include "boot_config.h"
|
||||
#include "board.h"
|
||||
|
||||
#include <debug.h>
|
||||
#include <string.h>
|
||||
#include <arch/board/board.h>
|
||||
|
||||
#include <nuttx/board.h>
|
||||
#include "led.h"
|
||||
|
||||
/************************************************************************************
|
||||
* Name: stm32_boardinitialize
|
||||
*
|
||||
* Description:
|
||||
* All STM32 architectures must provide the following entry point. This entry point
|
||||
* is called early in the initialization -- after all memory has been configured
|
||||
* and mapped but before any devices have been initialized.
|
||||
*
|
||||
************************************************************************************/
|
||||
|
||||
__EXPORT void stm32_boardinitialize(void)
|
||||
{
|
||||
putreg32(getreg32(STM32_RCC_APB1ENR) | RCC_APB1ENR_CAN1EN, STM32_RCC_APB1ENR);
|
||||
stm32_configgpio(GPIO_CAN1_RX);
|
||||
stm32_configgpio(GPIO_CAN1_TX);
|
||||
stm32_configgpio(GPIO_CAN1_SILENT_S0);
|
||||
stm32_configgpio(GPIO_CAN1_TERMINATION);
|
||||
putreg32(getreg32(STM32_RCC_APB1RSTR) | RCC_APB1RSTR_CAN1RST, STM32_RCC_APB1RSTR);
|
||||
putreg32(getreg32(STM32_RCC_APB1RSTR) & ~RCC_APB1RSTR_CAN1RST, STM32_RCC_APB1RSTR);
|
||||
|
||||
#if defined(OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO)
|
||||
stm32_configgpio(GPIO_GETNODEINFO_JUMPER);
|
||||
#endif
|
||||
|
||||
}
|
||||
|
||||
/************************************************************************************
|
||||
* Name: board_deinitialize
|
||||
*
|
||||
* Description:
|
||||
* This function is called by the bootloader code prior to booting
|
||||
* the application. Is should place the HW into an benign initialized state.
|
||||
*
|
||||
************************************************************************************/
|
||||
|
||||
void board_deinitialize(void)
|
||||
{
|
||||
putreg32(getreg32(STM32_RCC_APB1RSTR) | RCC_APB1RSTR_CAN1RST, STM32_RCC_APB1RSTR);
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: board_get_product_name
|
||||
*
|
||||
* Description:
|
||||
* Called to retrieve the product name. The returned value is a assumed
|
||||
* to be written to a pascal style string that will be length prefixed
|
||||
* and not null terminated
|
||||
*
|
||||
* Input Parameters:
|
||||
* product_name - A pointer to a buffer to write the name.
|
||||
* maxlen - The maximum number of charter that can be written
|
||||
*
|
||||
* Returned Value:
|
||||
* The length of characters written to the buffer.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
uint8_t board_get_product_name(uint8_t *product_name, size_t maxlen)
|
||||
{
|
||||
DEBUGASSERT(maxlen > UAVCAN_STRLEN(HW_UAVCAN_NAME));
|
||||
memcpy(product_name, HW_UAVCAN_NAME, UAVCAN_STRLEN(HW_UAVCAN_NAME));
|
||||
return UAVCAN_STRLEN(HW_UAVCAN_NAME);
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: board_get_hardware_version
|
||||
*
|
||||
* Description:
|
||||
* Called to retrieve the hardware version information. The function
|
||||
* will first initialize the the callers struct to all zeros.
|
||||
*
|
||||
* Input Parameters:
|
||||
* hw_version - A pointer to a uavcan_hardwareversion_t.
|
||||
*
|
||||
* Returned Value:
|
||||
* Length of the unique_id
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
size_t board_get_hardware_version(uavcan_HardwareVersion_t *hw_version)
|
||||
{
|
||||
memset(hw_version, 0, sizeof(uavcan_HardwareVersion_t));
|
||||
|
||||
hw_version->major = HW_VERSION_MAJOR;
|
||||
hw_version->minor = HW_VERSION_MINOR;
|
||||
|
||||
return board_get_mfguid(*(mfguid_t *) hw_version->unique_id);
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: board_indicate
|
||||
*
|
||||
* Description:
|
||||
* Provides User feedback to indicate the state of the bootloader
|
||||
* on board specific hardware.
|
||||
*
|
||||
* Input Parameters:
|
||||
* indication - A member of the uiindication_t
|
||||
*
|
||||
* Returned Value:
|
||||
* None
|
||||
*
|
||||
****************************************************************************/
|
||||
#define led(n, code, r , g , b, h) {.red = (r),.green = (g), .blue = (b),.hz = (h)}
|
||||
|
||||
typedef begin_packed_struct struct led_t {
|
||||
uint8_t red;
|
||||
uint8_t green;
|
||||
uint8_t blue;
|
||||
uint8_t hz;
|
||||
} end_packed_struct led_t;
|
||||
|
||||
static const led_t i2l[] = {
|
||||
|
||||
led(0, off, 0, 0, 0, 0),
|
||||
led(1, reset, 128, 128, 128, 30),
|
||||
led(2, autobaud_start, 0, 128, 0, 1),
|
||||
led(3, autobaud_end, 0, 128, 0, 2),
|
||||
led(4, allocation_start, 0, 0, 64, 2),
|
||||
led(5, allocation_end, 0, 128, 64, 3),
|
||||
led(6, fw_update_start, 32, 128, 64, 3),
|
||||
led(7, fw_update_erase_fail, 32, 128, 32, 3),
|
||||
led(8, fw_update_invalid_response, 64, 0, 0, 1),
|
||||
led(9, fw_update_timeout, 64, 0, 0, 2),
|
||||
led(a, fw_update_invalid_crc, 64, 0, 0, 4),
|
||||
led(b, jump_to_app, 0, 128, 0, 10),
|
||||
|
||||
};
|
||||
|
||||
void board_indicate(uiindication_t indication)
|
||||
{
|
||||
rgb_led(i2l[indication].red,
|
||||
i2l[indication].green,
|
||||
i2l[indication].blue,
|
||||
i2l[indication].hz);
|
||||
}
|
|
@ -0,0 +1,130 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/*
|
||||
* @file boot_config.h
|
||||
*
|
||||
* bootloader definitions that configures the behavior and options
|
||||
* of the Boot loader
|
||||
* This file is relies on the parent folder's boot_config.h file and defines
|
||||
* different usages of the hardware for bootloading
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
/************************************************************************************
|
||||
* Included Files
|
||||
************************************************************************************/
|
||||
|
||||
/* Bring in the board_config.h definitions
|
||||
* todo:make this be pulled in from a targed's build
|
||||
* files in nuttx*/
|
||||
|
||||
#include "board_config.h"
|
||||
#include "uavcan.h"
|
||||
#include <nuttx/compiler.h>
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#include <stm32_flash.h>
|
||||
|
||||
/****************************************************************************
|
||||
* Pre-processor Definitions
|
||||
****************************************************************************/
|
||||
|
||||
#define OPT_PREFERRED_NODE_ID ANY_NODE_ID
|
||||
|
||||
//todo:wrap OPT_x in in ifdefs for command line definitions
|
||||
#define OPT_TBOOT_MS 3000
|
||||
#define OPT_NODE_STATUS_RATE_MS 800
|
||||
#define OPT_NODE_INFO_RATE_MS 50
|
||||
#define OPT_BL_NUMBER_TIMERS 7
|
||||
|
||||
/*
|
||||
* This Option set is set to 1 ensure a provider of firmware has an
|
||||
* opportunity update the node's firmware.
|
||||
* This Option is the default policy and can be overridden by
|
||||
* a jumper
|
||||
* When this Policy is set, the node will ignore tboot and
|
||||
* wait indefinitely for a GetNodeInfo request before booting.
|
||||
*
|
||||
* OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO_INVERT is used to allow
|
||||
* the polarity of the jumper to be True Active
|
||||
*
|
||||
* wait OPT_WAIT_FOR_GETNODEINFO OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO
|
||||
* Jumper
|
||||
* yes 1 0 x
|
||||
* yes 1 1 Active
|
||||
* no 1 1 Not Active
|
||||
* no 0 0 X
|
||||
* yes 0 1 Active
|
||||
* no 0 1 Not Active
|
||||
*
|
||||
*/
|
||||
#define OPT_WAIT_FOR_GETNODEINFO 0
|
||||
#define OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO 1
|
||||
#define OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO_INVERT 1
|
||||
|
||||
#define OPT_ENABLE_WD 1
|
||||
|
||||
#define OPT_RESTART_TIMEOUT_MS 20000
|
||||
|
||||
/* Reserved for the Booloader */
|
||||
#define OPT_BOOTLOADER_SIZE_IN_K (1024*64)
|
||||
|
||||
/* Reserved for the application out of the total
|
||||
* system flash minus the BOOTLOADER_SIZE_IN_K
|
||||
*/
|
||||
#define OPT_APPLICATION_RESERVER_IN_K 0
|
||||
|
||||
#define OPT_APPLICATION_IMAGE_OFFSET OPT_BOOTLOADER_SIZE_IN_K
|
||||
#define OPT_APPLICATION_IMAGE_LENGTH (FLASH_SIZE-(OPT_BOOTLOADER_SIZE_IN_K+OPT_APPLICATION_RESERVER_IN_K))
|
||||
|
||||
|
||||
#define FLASH_BASE STM32_FLASH_BASE
|
||||
#define FLASH_SIZE STM32_FLASH_SIZE
|
||||
|
||||
#define APPLICATION_LOAD_ADDRESS (FLASH_BASE + OPT_APPLICATION_IMAGE_OFFSET)
|
||||
#define APPLICATION_SIZE (FLASH_SIZE-OPT_APPLICATION_IMAGE_OFFSET)
|
||||
#define APPLICATION_LAST_8BIT_ADDRRESS ((uint8_t *)((APPLICATION_LOAD_ADDRESS+APPLICATION_SIZE)-sizeof(uint8_t)))
|
||||
#define APPLICATION_LAST_32BIT_ADDRRESS ((uint32_t *)((APPLICATION_LOAD_ADDRESS+APPLICATION_SIZE)-sizeof(uint32_t)))
|
||||
#define APPLICATION_LAST_64BIT_ADDRRESS ((uint64_t *)((APPLICATION_LOAD_ADDRESS+APPLICATION_SIZE)-sizeof(uint64_t)))
|
||||
|
||||
/* If this board uses big flash that have large sectors */
|
||||
|
||||
#define OPT_USE_YIELD
|
||||
|
||||
/* Bootloader Option*****************************************************************
|
||||
*
|
||||
*/
|
||||
#define GPIO_GETNODEINFO_JUMPER (GPIO_BOOT_CONFIG & ~GPIO_EXTI)
|
|
@ -0,0 +1,130 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2021 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file can.c
|
||||
*
|
||||
* Board-specific CAN functions.
|
||||
*/
|
||||
|
||||
/************************************************************************************
|
||||
* Included Files
|
||||
************************************************************************************/
|
||||
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
|
||||
#include <errno.h>
|
||||
#include <debug.h>
|
||||
|
||||
#include <nuttx/can/can.h>
|
||||
#include <arch/board/board.h>
|
||||
|
||||
#include "chip.h"
|
||||
#include "arm_internal.h"
|
||||
|
||||
#include "stm32.h"
|
||||
#include "stm32_can.h"
|
||||
#include "board_config.h"
|
||||
|
||||
#ifdef CONFIG_CAN
|
||||
|
||||
/************************************************************************************
|
||||
* Pre-processor Definitions
|
||||
************************************************************************************/
|
||||
/* Configuration ********************************************************************/
|
||||
|
||||
#if defined(CONFIG_STM32_CAN1) && defined(CONFIG_STM32_CAN2)
|
||||
# warning "Both CAN1 and CAN2 are enabled. Assuming only CAN1."
|
||||
# undef CONFIG_STM32_CAN2
|
||||
#endif
|
||||
|
||||
#ifdef CONFIG_STM32_CAN1
|
||||
# define CAN_PORT 1
|
||||
#else
|
||||
# define CAN_PORT 2
|
||||
#endif
|
||||
|
||||
/************************************************************************************
|
||||
* Private Functions
|
||||
************************************************************************************/
|
||||
|
||||
/************************************************************************************
|
||||
* Public Functions
|
||||
************************************************************************************/
|
||||
int can_devinit(void);
|
||||
|
||||
/************************************************************************************
|
||||
* Name: can_devinit
|
||||
*
|
||||
* Description:
|
||||
* All STM32 architectures must provide the following interface to work with
|
||||
* examples/can.
|
||||
*
|
||||
************************************************************************************/
|
||||
|
||||
int can_devinit(void)
|
||||
{
|
||||
static bool initialized = false;
|
||||
struct can_dev_s *can;
|
||||
int ret;
|
||||
|
||||
/* Check if we have already initialized */
|
||||
|
||||
if (!initialized) {
|
||||
/* Call stm32_caninitialize() to get an instance of the CAN interface */
|
||||
|
||||
can = stm32_caninitialize(CAN_PORT);
|
||||
|
||||
if (can == NULL) {
|
||||
canerr("ERROR: Failed to get CAN interface\n");
|
||||
return -ENODEV;
|
||||
}
|
||||
|
||||
/* Register the CAN driver at "/dev/can0" */
|
||||
|
||||
ret = can_register("/dev/can0", can);
|
||||
|
||||
if (ret < 0) {
|
||||
canerr("ERROR: can_register failed: %d\n", ret);
|
||||
return ret;
|
||||
}
|
||||
|
||||
/* Now we are initialized */
|
||||
|
||||
initialized = true;
|
||||
}
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
#endif
|
|
@ -0,0 +1,39 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2021 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include <px4_arch/i2c_hw_description.h>
|
||||
|
||||
constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = {
|
||||
initI2CBusInternal(1),
|
||||
initI2CBusInternal(2),
|
||||
};
|
|
@ -0,0 +1,168 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file init.c
|
||||
*
|
||||
* board specific early startup code. This file implements the
|
||||
* board_app_initialize() function that is called early by nsh during startup.
|
||||
*
|
||||
* Code here is run before the rcS script is invoked; it should start required
|
||||
* subsystems and perform board-specific initialization.
|
||||
*/
|
||||
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#include <px4_platform_common/tasks.h>
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <debug.h>
|
||||
#include <errno.h>
|
||||
#include <syslog.h>
|
||||
|
||||
#include <nuttx/board.h>
|
||||
|
||||
#include <stm32.h>
|
||||
#include "board_config.h"
|
||||
#include "led.h"
|
||||
#include <stm32_uart.h>
|
||||
|
||||
#include <arch/board/board.h>
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <drivers/drv_board_led.h>
|
||||
#include <drivers/drv_watchdog.h>
|
||||
|
||||
#include <systemlib/px4_macros.h>
|
||||
|
||||
#include <px4_platform_common/init.h>
|
||||
#include <px4_platform/gpio.h>
|
||||
|
||||
# if defined(FLASH_BASED_PARAMS)
|
||||
# include <parameters/flashparams/flashfs.h>
|
||||
#endif
|
||||
|
||||
/************************************************************************************
|
||||
* Name: stm32_boardinitialize
|
||||
*
|
||||
* Description:
|
||||
* All STM32 architectures must provide the following entry point. This entry point
|
||||
* is called early in the initialization -- after all memory has been configured
|
||||
* and mapped but before any devices have been initialized.
|
||||
*
|
||||
************************************************************************************/
|
||||
|
||||
__EXPORT void stm32_boardinitialize(void)
|
||||
{
|
||||
watchdog_init();
|
||||
|
||||
/* configure pins */
|
||||
const uint32_t gpio[] = PX4_GPIO_INIT_LIST;
|
||||
px4_gpio_init(gpio, arraySize(gpio));
|
||||
|
||||
// Configure SPI all interfaces GPIO & enable power.
|
||||
stm32_spiinitialize();
|
||||
|
||||
// Check if button is held. If so go into gps passthrough mode
|
||||
if (stm32_gpioread(GPIO_BTN_SAFETY)) {
|
||||
rgb_led(128, 128, 128, 10);
|
||||
stm32_configgpio(GPIO_USART1_TX_GPIO);
|
||||
stm32_configgpio(GPIO_USART1_RX_GPIO);
|
||||
stm32_configgpio(GPIO_USART2_TX_GPIO);
|
||||
stm32_configgpio(GPIO_USART2_RX_GPIO);
|
||||
|
||||
while (1) {
|
||||
watchdog_pet();
|
||||
stm32_gpiowrite(GPIO_USART2_TX_GPIO, stm32_gpioread(GPIO_USART1_RX_GPIO));
|
||||
stm32_gpiowrite(GPIO_USART1_TX_GPIO, stm32_gpioread(GPIO_USART2_RX_GPIO));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: board_app_initialize
|
||||
*
|
||||
* Description:
|
||||
* Perform application specific initialization. This function is never
|
||||
* called directly from application code, but only indirectly via the
|
||||
* (non-standard) boardctl() interface using the command BOARDIOC_INIT.
|
||||
*
|
||||
* Input Parameters:
|
||||
* arg - The boardctl() argument is passed to the board_app_initialize()
|
||||
* implementation without modification. The argument has no
|
||||
* meaning to NuttX; the meaning of the argument is a contract
|
||||
* between the board-specific initalization logic and the the
|
||||
* matching application logic. The value cold be such things as a
|
||||
* mode enumeration value, a set of DIP switch switch settings, a
|
||||
* pointer to configuration data read from a file or serial FLASH,
|
||||
* or whatever you would like to do with it. Every implementation
|
||||
* should accept zero/NULL as a default configuration.
|
||||
*
|
||||
* Returned Value:
|
||||
* Zero (OK) is returned on success; a negated errno value is returned on
|
||||
* any failure to indicate the nature of the failure.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
__EXPORT int board_app_initialize(uintptr_t arg)
|
||||
{
|
||||
px4_platform_init();
|
||||
|
||||
#if defined(SERIAL_HAVE_RXDMA)
|
||||
// set up the serial DMA polling at 1ms intervals for received bytes that have not triggered a DMA event.
|
||||
static struct hrt_call serial_dma_call;
|
||||
hrt_call_every(&serial_dma_call, 1000, 1000, (hrt_callout)stm32_serial_dma_poll, NULL);
|
||||
#endif
|
||||
|
||||
#if defined(FLASH_BASED_PARAMS)
|
||||
static sector_descriptor_t params_sector_map[] = {
|
||||
{2, 16 * 1024, 0x08008000},
|
||||
{3, 16 * 1024, 0x0800C000},
|
||||
{0, 0, 0},
|
||||
};
|
||||
|
||||
/* Initialize the flashfs layer to use heap allocated memory */
|
||||
int result = parameter_flashfs_init(params_sector_map, NULL, 0);
|
||||
|
||||
if (result != OK) {
|
||||
syslog(LOG_ERR, "[boot] FAILED to init params in FLASH %d\n", result);
|
||||
}
|
||||
|
||||
#endif // FLASH_BASED_PARAMS
|
||||
|
||||
/* Configure the HW based on the manifest */
|
||||
//px4_platform_configure();
|
||||
|
||||
return OK;
|
||||
}
|
|
@ -0,0 +1,124 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file led.c
|
||||
*
|
||||
* LED backend.
|
||||
*/
|
||||
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
|
||||
#include <stdbool.h>
|
||||
|
||||
#include "chip.h"
|
||||
#include "stm32_gpio.h"
|
||||
#include "board_config.h"
|
||||
|
||||
#include <nuttx/board.h>
|
||||
#include <arch/board/board.h>
|
||||
|
||||
#include "led.h"
|
||||
|
||||
#define TMR_BASE STM32_TIM1_BASE
|
||||
#define TMR_FREQUENCY STM32_APB2_TIM1_CLKIN
|
||||
#define TMR_REG(o) (TMR_BASE+(o))
|
||||
|
||||
void rgb_led(int r, int g, int b, int freqs)
|
||||
{
|
||||
|
||||
long fosc = TMR_FREQUENCY;
|
||||
long prescale = 2048;
|
||||
long p1s = fosc / prescale;
|
||||
long p0p5s = p1s / 2;
|
||||
uint16_t val;
|
||||
static bool once = 0;
|
||||
|
||||
if (!once) {
|
||||
once = 1;
|
||||
|
||||
/* Enabel Clock to Block */
|
||||
modifyreg32(STM32_RCC_APB2ENR, 0, RCC_APB2ENR_TIM1EN);
|
||||
|
||||
/* Reload */
|
||||
|
||||
val = getreg16(TMR_REG(STM32_BTIM_EGR_OFFSET));
|
||||
val |= ATIM_EGR_UG;
|
||||
putreg16(val, TMR_REG(STM32_BTIM_EGR_OFFSET));
|
||||
|
||||
/* Set Prescaler STM32_TIM_SETCLOCK */
|
||||
|
||||
putreg16(prescale, TMR_REG(STM32_BTIM_PSC_OFFSET));
|
||||
|
||||
/* Enable STM32_TIM_SETMODE*/
|
||||
|
||||
putreg16(ATIM_CR1_CEN | ATIM_CR1_ARPE, TMR_REG(STM32_BTIM_CR1_OFFSET));
|
||||
|
||||
|
||||
putreg16((ATIM_CCMR_MODE_PWM1 << ATIM_CCMR1_OC1M_SHIFT) | ATIM_CCMR1_OC1PE |
|
||||
(ATIM_CCMR_MODE_PWM1 << ATIM_CCMR1_OC2M_SHIFT) | ATIM_CCMR1_OC2PE, TMR_REG(STM32_GTIM_CCMR1_OFFSET));
|
||||
putreg16((ATIM_CCMR_MODE_PWM1 << ATIM_CCMR2_OC3M_SHIFT) | ATIM_CCMR2_OC3PE, TMR_REG(STM32_GTIM_CCMR2_OFFSET));
|
||||
putreg16(ATIM_CCER_CC3E | ATIM_CCER_CC3P |
|
||||
ATIM_CCER_CC2E | ATIM_CCER_CC2P |
|
||||
ATIM_CCER_CC1E | ATIM_CCER_CC1P, TMR_REG(STM32_GTIM_CCER_OFFSET));
|
||||
|
||||
|
||||
stm32_configgpio(GPIO_TIM1_CH1);
|
||||
stm32_configgpio(GPIO_TIM1_CH2);
|
||||
stm32_configgpio(GPIO_TIM1_CH3);
|
||||
|
||||
/* master output enable = on */
|
||||
putreg16(ATIM_BDTR_MOE, (TMR_REG(STM32_ATIM_BDTR_OFFSET)));
|
||||
}
|
||||
|
||||
long p = freqs == 0 ? p1s : p1s / freqs;
|
||||
putreg32(p, TMR_REG(STM32_BTIM_ARR_OFFSET));
|
||||
|
||||
p = freqs == 0 ? p1s + 1 : p0p5s / freqs;
|
||||
|
||||
putreg32((r * p) / 255, TMR_REG(STM32_GTIM_CCR1_OFFSET));
|
||||
putreg32((g * p) / 255, TMR_REG(STM32_GTIM_CCR2_OFFSET));
|
||||
putreg32((b * p) / 255, TMR_REG(STM32_GTIM_CCR3_OFFSET));
|
||||
|
||||
val = getreg16(TMR_REG(STM32_BTIM_CR1_OFFSET));
|
||||
|
||||
if (freqs == 0) {
|
||||
val &= ~ATIM_CR1_CEN;
|
||||
|
||||
} else {
|
||||
val |= ATIM_CR1_CEN;
|
||||
}
|
||||
|
||||
putreg16(val, TMR_REG(STM32_BTIM_CR1_OFFSET));
|
||||
|
||||
}
|
|
@ -0,0 +1,37 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2015 PX4 Development Team. All rights reserved.
|
||||
* Author: David Sidrane<david_s5@nscdg.com>
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
__BEGIN_DECLS
|
||||
void rgb_led(int r, int g, int b, int freqs);
|
||||
__END_DECLS
|
|
@ -0,0 +1,44 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2021 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include <px4_arch/spi_hw_description.h>
|
||||
#include <drivers/drv_sensor.h>
|
||||
#include <nuttx/spi/spi.h>
|
||||
|
||||
constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = {
|
||||
initSPIBus(SPI::Bus::SPI1, {
|
||||
initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortB, GPIO::Pin0}, SPI::DRDY{GPIO::PortB, GPIO::Pin1}),
|
||||
}),
|
||||
};
|
||||
|
||||
static constexpr bool unused = validateSPIConfig(px4_spi_buses);
|
|
@ -0,0 +1,17 @@
|
|||
# UAVCAN boot loadable Module ID
|
||||
set(uavcanblid_sw_version_major ${PX4_VERSION_MAJOR})
|
||||
set(uavcanblid_sw_version_minor ${PX4_VERSION_MINOR})
|
||||
add_definitions(
|
||||
-DAPP_VERSION_MAJOR=${uavcanblid_sw_version_major}
|
||||
-DAPP_VERSION_MINOR=${uavcanblid_sw_version_minor}
|
||||
)
|
||||
|
||||
set(uavcanblid_hw_version_major 0)
|
||||
set(uavcanblid_hw_version_minor 84)
|
||||
set(uavcanblid_name "\"org.ark.septentrio-gps\"")
|
||||
|
||||
add_definitions(
|
||||
-DHW_UAVCAN_NAME=${uavcanblid_name}
|
||||
-DHW_VERSION_MAJOR=${uavcanblid_hw_version_major}
|
||||
-DHW_VERSION_MINOR=${uavcanblid_hw_version_minor}
|
||||
)
|
|
@ -64,9 +64,6 @@
|
|||
// Hacks for MAVLink RC button input
|
||||
#define ATL_MANTIS_RC_INPUT_HACKS
|
||||
|
||||
// Hacks for MAVLink RC button input
|
||||
#define ATL_MANTIS_RC_INPUT_HACKS
|
||||
|
||||
/*
|
||||
* ADC channels
|
||||
*
|
||||
|
|
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
@ -60,7 +60,7 @@ CONFIG_MODULES_NAVIGATOR=y
|
|||
CONFIG_MODULES_RC_UPDATE=y
|
||||
CONFIG_MODULES_ROVER_POS_CONTROL=y
|
||||
CONFIG_MODULES_SENSORS=y
|
||||
CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
|
||||
# CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
|
||||
CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
|
||||
CONFIG_MODULES_UXRCE_DDS_CLIENT=y
|
||||
CONFIG_MODULES_VTOL_ATT_CONTROL=y
|
||||
|
|
|
@ -4,13 +4,14 @@ CONFIG_DRIVERS_ACTUATORS_VOXL_ESC=y
|
|||
CONFIG_DRIVERS_BAROMETER_INVENSENSE_ICP101XX=y
|
||||
CONFIG_DRIVERS_DISTANCE_SENSOR_VL53L0X=y
|
||||
CONFIG_DRIVERS_DISTANCE_SENSOR_VL53L1X=y
|
||||
CONFIG_DRIVERS_GPS=y
|
||||
CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
|
||||
CONFIG_DRIVERS_LIGHTS_RGBLED_NCP5623C=y
|
||||
CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8308=y
|
||||
CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y
|
||||
CONFIG_DRIVERS_MAGNETOMETER_QMC5883L=y
|
||||
CONFIG_DRIVERS_POWER_MONITOR_VOXLPM=y
|
||||
CONFIG_DRIVERS_QSHELL_QURT=y
|
||||
CONFIG_DRIVERS_VOXL2_IO=y
|
||||
CONFIG_MODULES_COMMANDER=y
|
||||
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
||||
CONFIG_MODULES_EKF2=y
|
||||
|
@ -28,4 +29,6 @@ CONFIG_MODULES_RC_UPDATE=y
|
|||
CONFIG_MODULES_SENSORS=y
|
||||
CONFIG_MODULES_SIMULATION_PWM_OUT_SIM=y
|
||||
CONFIG_SYSTEMCMDS_UORB=y
|
||||
CONFIG_SYSTEMCMDS_PARAM=y
|
||||
CONFIG_ORB_COMMUNICATOR=y
|
||||
CONFIG_PARAM_REMOTE=y
|
||||
|
|
|
@ -44,11 +44,10 @@ add_library(drivers_board
|
|||
)
|
||||
|
||||
# Add custom drivers for SLPI
|
||||
# add_subdirectory(${PX4_BOARD_DIR}/src/drivers/icm42688p)
|
||||
add_subdirectory(${PX4_BOARD_DIR}/src/drivers/rc_controller)
|
||||
add_subdirectory(${PX4_BOARD_DIR}/src/drivers/mavlink_rc_in)
|
||||
# add_subdirectory(${PX4_BOARD_DIR}/src/drivers/spektrum_rc)
|
||||
# add_subdirectory(${PX4_BOARD_DIR}/src/drivers/ghst_rc)
|
||||
# add_subdirectory(${PX4_BOARD_DIR}/src/drivers/dsp_hitl)
|
||||
add_subdirectory(${PX4_BOARD_DIR}/src/drivers/dsp_hitl)
|
||||
# add_subdirectory(${PX4_BOARD_DIR}/src/drivers/dsp_sbus)
|
||||
add_subdirectory(${PX4_BOARD_DIR}/src/drivers/elrs_led)
|
||||
|
|
|
@ -62,6 +62,8 @@
|
|||
#include <uORB/topics/vehicle_odometry.h>
|
||||
#include <uORB/topics/sensor_baro.h>
|
||||
#include <uORB/topics/esc_status.h>
|
||||
#include <uORB/topics/distance_sensor.h>
|
||||
#include <uORB/topics/sensor_optical_flow.h>
|
||||
|
||||
#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
|
||||
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
|
||||
|
@ -88,21 +90,22 @@ static bool _is_running = false;
|
|||
volatile bool _task_should_exit = false;
|
||||
static px4_task_t _task_handle = -1;
|
||||
int _uart_fd = -1;
|
||||
bool debug = false;
|
||||
bool _debug = false;
|
||||
std::string port = "2";
|
||||
int baudrate = 921600;
|
||||
const unsigned mode_flag_custom = 1;
|
||||
const unsigned mode_flag_armed = 128;
|
||||
bool _send_gps = false;
|
||||
bool _send_mag = false;
|
||||
bool _send_distance = false;
|
||||
|
||||
uORB::Publication<battery_status_s> _battery_pub{ORB_ID(battery_status)};
|
||||
uORB::PublicationMulti<sensor_gps_s> _sensor_gps_pub{ORB_ID(sensor_gps)};
|
||||
uORB::Publication<differential_pressure_s> _differential_pressure_pub{ORB_ID(differential_pressure)};
|
||||
uORB::Publication<vehicle_odometry_s> _visual_odometry_pub{ORB_ID(vehicle_visual_odometry)};
|
||||
uORB::Publication<vehicle_odometry_s> _mocap_odometry_pub{ORB_ID(vehicle_mocap_odometry)};
|
||||
uORB::PublicationMulti<sensor_baro_s> _sensor_baro_pub{ORB_ID(sensor_baro)};
|
||||
uORB::Publication<esc_status_s> _esc_status_pub{ORB_ID(esc_status)};
|
||||
uORB::Publication<distance_sensor_s> _distance_sensor_pub{ORB_ID(distance_sensor)};
|
||||
uORB::Publication<sensor_optical_flow_s> _sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)};
|
||||
uORB::Subscription _battery_status_sub{ORB_ID(battery_status)};
|
||||
|
||||
int32_t _output_functions[actuator_outputs_s::NUM_ACTUATOR_OUTPUTS] {};
|
||||
|
@ -128,13 +131,42 @@ float x_gyro = 0;
|
|||
float y_gyro = 0;
|
||||
float z_gyro = 0;
|
||||
uint64_t gyro_accel_time = 0;
|
||||
bool _use_software_mav_throttling{false};
|
||||
|
||||
int heartbeat_counter = 0;
|
||||
int imu_counter = 0;
|
||||
int hil_sensor_counter = 0;
|
||||
int vision_msg_counter = 0;
|
||||
int gps_counter = 0;
|
||||
// Status counters
|
||||
uint32_t heartbeat_received_counter = 0;
|
||||
uint32_t heartbeat_sent_counter = 0;
|
||||
uint32_t imu_counter = 0;
|
||||
uint32_t hil_sensor_counter = 0;
|
||||
uint32_t mag_counter = 0;
|
||||
uint32_t baro_counter = 0;
|
||||
uint32_t actuator_sent_counter = 0;
|
||||
uint32_t odometry_received_counter = 0;
|
||||
uint32_t odometry_sent_counter = 0;
|
||||
uint32_t gps_received_counter = 0;
|
||||
uint32_t gps_sent_counter = 0;
|
||||
uint32_t distance_received_counter = 0;
|
||||
uint32_t distance_sent_counter = 0;
|
||||
uint32_t flow_received_counter = 0;
|
||||
uint32_t flow_sent_counter = 0;
|
||||
uint32_t unknown_msg_received_counter = 0;
|
||||
|
||||
enum class position_source {GPS, VIO, FLOW, NUM_POSITION_SOURCES};
|
||||
|
||||
struct position_source_data_s {
|
||||
char label[8];
|
||||
bool send;
|
||||
bool fail;
|
||||
uint32_t failure_duration;
|
||||
uint64_t failure_duration_start;
|
||||
} position_source_data[(int) position_source::NUM_POSITION_SOURCES] = {
|
||||
{"GPS", false, false, 0, 0},
|
||||
{"VIO", false, false, 0, 0},
|
||||
{"FLOW", false, false, 0, 0}
|
||||
};
|
||||
|
||||
uint64_t first_sensor_msg_timestamp = 0;
|
||||
uint64_t first_sensor_report_timestamp = 0;
|
||||
uint64_t last_sensor_report_timestamp = 0;
|
||||
|
||||
vehicle_status_s _vehicle_status{};
|
||||
vehicle_control_mode_s _control_mode{};
|
||||
|
@ -144,7 +176,6 @@ battery_status_s _battery_status{};
|
|||
sensor_accel_fifo_s accel_fifo{};
|
||||
sensor_gyro_fifo_s gyro_fifo{};
|
||||
|
||||
|
||||
int openPort(const char *dev, speed_t speed);
|
||||
int closePort();
|
||||
|
||||
|
@ -153,7 +184,8 @@ int writeResponse(void *buf, size_t len);
|
|||
|
||||
int start(int argc, char *argv[]);
|
||||
int stop();
|
||||
int get_status();
|
||||
void print_status();
|
||||
void clear_status_counters();
|
||||
bool isOpen() { return _uart_fd >= 0; };
|
||||
|
||||
void usage();
|
||||
|
@ -163,50 +195,65 @@ void *send_actuator(void *);
|
|||
void send_actuator_data();
|
||||
|
||||
void handle_message_hil_sensor_dsp(mavlink_message_t *msg);
|
||||
void handle_message_hil_optical_flow(mavlink_message_t *msg);
|
||||
void handle_message_distance_sensor(mavlink_message_t *msg);
|
||||
void handle_message_hil_gps_dsp(mavlink_message_t *msg);
|
||||
void handle_message_odometry_dsp(mavlink_message_t *msg);
|
||||
void handle_message_vision_position_estimate_dsp(mavlink_message_t *msg);
|
||||
void handle_message_command_long_dsp(mavlink_message_t *msg);
|
||||
|
||||
void handle_message_dsp(mavlink_message_t *msg);
|
||||
void actuator_controls_from_outputs_dsp(mavlink_hil_actuator_controls_t *msg);
|
||||
void send_esc_telemetry_dsp(mavlink_hil_actuator_controls_t hil_act_control);
|
||||
void send_esc_status(mavlink_hil_actuator_controls_t hil_act_control);
|
||||
|
||||
void
|
||||
handle_message_dsp(mavlink_message_t *msg)
|
||||
{
|
||||
switch (msg->msgid) {
|
||||
case MAVLINK_MSG_ID_HIL_SENSOR:
|
||||
hil_sensor_counter++;
|
||||
handle_message_hil_sensor_dsp(msg);
|
||||
break;
|
||||
|
||||
case MAVLINK_MSG_ID_HIL_GPS:
|
||||
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;
|
||||
|
||||
case MAVLINK_MSG_ID_ODOMETRY:
|
||||
handle_message_odometry_dsp(msg);
|
||||
break;
|
||||
odometry_received_counter++;
|
||||
|
||||
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;
|
||||
|
||||
case MAVLINK_MSG_ID_HEARTBEAT:
|
||||
PX4_DEBUG("Heartbeat msg received");
|
||||
heartbeat_received_counter++;
|
||||
|
||||
if (_debug) { PX4_INFO("Heartbeat msg received"); }
|
||||
|
||||
break;
|
||||
|
||||
case MAVLINK_MSG_ID_SYSTEM_TIME:
|
||||
PX4_DEBUG("MAVLINK SYSTEM TIME");
|
||||
case MAVLINK_MSG_ID_HIL_OPTICAL_FLOW:
|
||||
flow_received_counter++;
|
||||
|
||||
if (position_source_data[(int) position_source::FLOW].send) { handle_message_hil_optical_flow(msg); }
|
||||
|
||||
break;
|
||||
|
||||
case MAVLINK_MSG_ID_DISTANCE_SENSOR:
|
||||
distance_received_counter++;
|
||||
|
||||
if (_send_distance) { handle_message_distance_sensor(msg); }
|
||||
|
||||
break;
|
||||
|
||||
default:
|
||||
PX4_DEBUG("Unknown msg ID: %d", msg->msgid);
|
||||
unknown_msg_received_counter++;
|
||||
|
||||
if (_debug) { PX4_INFO("Unknown msg ID: %d", msg->msgid); }
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
@ -228,7 +275,6 @@ void send_actuator_data()
|
|||
bool first_sent = false;
|
||||
|
||||
while (true) {
|
||||
|
||||
bool controls_updated = false;
|
||||
(void) orb_check(_vehicle_control_mode_sub_, &controls_updated);
|
||||
|
||||
|
@ -239,45 +285,50 @@ void send_actuator_data()
|
|||
bool actuator_updated = false;
|
||||
(void) orb_check(_actuator_outputs_sub, &actuator_updated);
|
||||
|
||||
uint8_t newBuf[512];
|
||||
uint16_t newBufLen = 0;
|
||||
|
||||
mavlink_hil_actuator_controls_t hil_act_control;
|
||||
actuator_controls_from_outputs_dsp(&hil_act_control);
|
||||
|
||||
mavlink_message_t message{};
|
||||
mavlink_msg_hil_actuator_controls_encode(1, 1, &message, &hil_act_control);
|
||||
|
||||
if (actuator_updated) {
|
||||
orb_copy(ORB_ID(actuator_outputs), _actuator_outputs_sub, &_actuator_outputs);
|
||||
px4_lockstep_wait_for_components();
|
||||
|
||||
if (_actuator_outputs.timestamp > 0) {
|
||||
mavlink_hil_actuator_controls_t hil_act_control;
|
||||
actuator_controls_from_outputs_dsp(&hil_act_control);
|
||||
|
||||
mavlink_message_t message{};
|
||||
mavlink_msg_hil_actuator_controls_encode(1, 1, &message, &hil_act_control);
|
||||
previous_timestamp = _actuator_outputs.timestamp;
|
||||
previous_uorb_timestamp = _actuator_outputs.timestamp;
|
||||
uint8_t newBuf[512];
|
||||
uint16_t newBufLen = 0;
|
||||
|
||||
newBufLen = mavlink_msg_to_send_buffer(newBuf, &message);
|
||||
int writeRetval = writeResponse(&newBuf, newBufLen);
|
||||
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;
|
||||
send_esc_telemetry_dsp(hil_act_control);
|
||||
|
||||
send_esc_status(hil_act_control);
|
||||
}
|
||||
|
||||
} else if (!actuator_updated && first_sent && differential > 4000) {
|
||||
mavlink_hil_actuator_controls_t hil_act_control;
|
||||
actuator_controls_from_outputs_dsp(&hil_act_control);
|
||||
} else if (! actuator_updated && first_sent && differential > 4000) {
|
||||
previous_timestamp = hrt_absolute_time();
|
||||
|
||||
mavlink_message_t message{};
|
||||
mavlink_msg_hil_actuator_controls_encode(1, 1, &message, &hil_act_control);
|
||||
uint8_t newBuf[512];
|
||||
uint16_t newBufLen = 0;
|
||||
newBufLen = mavlink_msg_to_send_buffer(newBuf, &message);
|
||||
int writeRetval = writeResponse(&newBuf, newBufLen);
|
||||
//PX4_INFO("Sending from NOT UPDTE AND TIMEOUT: %i", differential);
|
||||
|
||||
PX4_DEBUG("Succesful write of actuator back to jMAVSim: %d at %llu", writeRetval, hrt_absolute_time());
|
||||
send_esc_telemetry_dsp(hil_act_control);
|
||||
actuator_sent_counter++;
|
||||
|
||||
if (_debug) { PX4_INFO("Succesful write of actuator back to jMAVSim: %d at %llu", writeRetval, hrt_absolute_time()); }
|
||||
|
||||
send_esc_status(hil_act_control);
|
||||
}
|
||||
|
||||
differential = hrt_absolute_time() - previous_timestamp;
|
||||
|
||||
px4_usleep(1000);
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -287,14 +338,10 @@ void task_main(int argc, char *argv[])
|
|||
int myoptind = 1;
|
||||
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) {
|
||||
case 's':
|
||||
_use_software_mav_throttling = true;
|
||||
break;
|
||||
|
||||
case 'd':
|
||||
debug = true;
|
||||
_debug = true;
|
||||
break;
|
||||
|
||||
case 'p':
|
||||
|
@ -310,7 +357,19 @@ void task_main(int argc, char *argv[])
|
|||
break;
|
||||
|
||||
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;
|
||||
|
||||
default:
|
||||
|
@ -319,11 +378,13 @@ void task_main(int argc, char *argv[])
|
|||
}
|
||||
|
||||
const char *charport = port.c_str();
|
||||
int openRetval = openPort(charport, (speed_t) baudrate);
|
||||
int open = isOpen();
|
||||
(void) openPort(charport, (speed_t) baudrate);
|
||||
|
||||
if (open) {
|
||||
PX4_ERR("Port is open: %d", openRetval);
|
||||
if ((_debug) && (isOpen())) { PX4_INFO("DSP HITL serial port initialized. Baudrate: %d", baudrate); }
|
||||
|
||||
if (! isOpen()) {
|
||||
PX4_ERR("DSP HITL failed to open serial port");
|
||||
return;
|
||||
}
|
||||
|
||||
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);
|
||||
|
||||
int _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
|
||||
PX4_INFO("Got %d from orb_subscribe", _vehicle_status_sub);
|
||||
|
||||
_is_running = true;
|
||||
|
||||
while (!_task_should_exit) {
|
||||
|
||||
uint8_t rx_buf[1024];
|
||||
//rx_buf[511] = '\0';
|
||||
|
||||
uint64_t timestamp = hrt_absolute_time();
|
||||
|
||||
|
@ -357,8 +415,8 @@ void task_main(int argc, char *argv[])
|
|||
if (got_first_sensor_msg) {
|
||||
uint64_t delta_time = timestamp - last_imu_update_timestamp;
|
||||
|
||||
if (delta_time > 15000) {
|
||||
PX4_ERR("Sending updates at %llu, delta %llu", timestamp, delta_time);
|
||||
if ((imu_counter) && (delta_time > 15000)) {
|
||||
PX4_WARN("Sending updates at %llu, delta %llu", timestamp, delta_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);
|
||||
(void) writeResponse(&hb_newBuf, hb_newBufLen);
|
||||
last_heartbeat_timestamp = timestamp;
|
||||
heartbeat_counter++;
|
||||
heartbeat_sent_counter++;
|
||||
}
|
||||
|
||||
bool vehicle_updated = false;
|
||||
|
@ -416,7 +474,7 @@ void task_main(int argc, char *argv[])
|
|||
_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.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);
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
handle_message_command_long_dsp(mavlink_message_t *msg)
|
||||
{
|
||||
/* command */
|
||||
mavlink_command_long_t cmd_mavlink;
|
||||
mavlink_msg_command_long_decode(msg, &cmd_mavlink);
|
||||
|
||||
if (debug) {
|
||||
PX4_INFO("Value of command_long.command: %d", cmd_mavlink.command);
|
||||
}
|
||||
if (_debug) { PX4_INFO("Value of command_long.command: %d", cmd_mavlink.command); }
|
||||
|
||||
mavlink_command_ack_t ack = {};
|
||||
ack.result = MAV_RESULT_UNSUPPORTED;
|
||||
|
@ -470,46 +524,140 @@ handle_message_command_long_dsp(mavlink_message_t *msg)
|
|||
uint16_t acknewBufLen = 0;
|
||||
acknewBufLen = mavlink_msg_to_send_buffer(acknewBuf, &ack_message);
|
||||
int writeRetval = writeResponse(&acknewBuf, acknewBufLen);
|
||||
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
|
||||
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_msg_vision_position_estimate_decode(msg, &vpe);
|
||||
mavlink_hil_optical_flow_t flow;
|
||||
mavlink_msg_hil_optical_flow_decode(msg, &flow);
|
||||
|
||||
// fill vehicle_odometry from Mavlink VISION_POSITION_ESTIMATE
|
||||
vehicle_odometry_s odom{};
|
||||
uint64_t timestamp = hrt_absolute_time();
|
||||
odom.timestamp_sample = timestamp;
|
||||
if ((_debug) && (!(flow_debug_counter % 10))) {
|
||||
PX4_INFO("optflow: time: %llu, quality %d", flow.time_usec, (int) flow.quality);
|
||||
PX4_INFO("optflow: x: %.2f y: %.2f", (double) flow.integrated_x, (double) flow.integrated_y);
|
||||
}
|
||||
|
||||
odom.pose_frame = vehicle_odometry_s::POSE_FRAME_NED;
|
||||
odom.position[0] = vpe.x;
|
||||
odom.position[1] = vpe.y;
|
||||
odom.position[2] = vpe.z;
|
||||
flow_debug_counter++;
|
||||
|
||||
const matrix::Quatf q(matrix::Eulerf(vpe.roll, vpe.pitch, vpe.yaw));
|
||||
q.copyTo(odom.q);
|
||||
device::Device::DeviceId device_id;
|
||||
device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_MAVLINK;
|
||||
device_id.devid_s.bus = 1;
|
||||
device_id.devid_s.address = msg->sysid;
|
||||
device_id.devid_s.devtype = DRV_FLOW_DEVTYPE_SIM;
|
||||
|
||||
// VISION_POSITION_ESTIMATE covariance
|
||||
// Row-major representation of pose 6x6 cross-covariance matrix upper right triangle
|
||||
// (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.).
|
||||
// If unknown, assign NaN value to first element in the array.
|
||||
odom.position_variance[0] = vpe.covariance[0]; // X row 0, col 0
|
||||
odom.position_variance[1] = vpe.covariance[6]; // Y row 1, col 1
|
||||
odom.position_variance[2] = vpe.covariance[11]; // Z row 2, col 2
|
||||
sensor_optical_flow_s sensor_optical_flow{};
|
||||
|
||||
odom.orientation_variance[0] = vpe.covariance[15]; // R row 3, col 3
|
||||
odom.orientation_variance[1] = vpe.covariance[18]; // P row 4, col 4
|
||||
odom.orientation_variance[2] = vpe.covariance[20]; // Y row 5, col 5
|
||||
sensor_optical_flow.timestamp_sample = hrt_absolute_time();
|
||||
sensor_optical_flow.device_id = device_id.devid;
|
||||
|
||||
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);
|
||||
vision_msg_counter++;
|
||||
int index = (int) position_source::FLOW;
|
||||
|
||||
if (position_source_data[index].fail) {
|
||||
uint32_t duration = position_source_data[index].failure_duration;
|
||||
hrt_abstime start = position_source_data[index].failure_duration_start;
|
||||
|
||||
if (duration) {
|
||||
if (hrt_elapsed_time(&start) > (duration * 1000000)) {
|
||||
PX4_INFO("Optical flow failure ending");
|
||||
position_source_data[index].fail = false;
|
||||
position_source_data[index].failure_duration = 0;
|
||||
position_source_data[index].failure_duration_start = 0;
|
||||
|
||||
} else {
|
||||
sensor_optical_flow.quality = 0;
|
||||
}
|
||||
|
||||
} else {
|
||||
sensor_optical_flow.quality = 0;
|
||||
}
|
||||
}
|
||||
|
||||
const matrix::Vector3f integrated_gyro(flow.integrated_xgyro, flow.integrated_ygyro, flow.integrated_zgyro);
|
||||
|
||||
if (integrated_gyro.isAllFinite()) {
|
||||
integrated_gyro.copyTo(sensor_optical_flow.delta_angle);
|
||||
sensor_optical_flow.delta_angle_available = true;
|
||||
}
|
||||
|
||||
sensor_optical_flow.max_flow_rate = NAN;
|
||||
sensor_optical_flow.min_ground_distance = NAN;
|
||||
sensor_optical_flow.max_ground_distance = NAN;
|
||||
|
||||
// Use distance value for distance sensor topic
|
||||
// if (PX4_ISFINITE(flow.distance) && (flow.distance >= 0.f)) {
|
||||
// // Positive value (including zero): distance known. Negative value: Unknown distance.
|
||||
// sensor_optical_flow.distance_m = flow.distance;
|
||||
// sensor_optical_flow.distance_available = true;
|
||||
// }
|
||||
|
||||
// Emulate voxl-flow-server where distance comes in a separate
|
||||
// distance sensor topic message
|
||||
sensor_optical_flow.distance_m = 0.0f;
|
||||
sensor_optical_flow.distance_available = false;
|
||||
|
||||
sensor_optical_flow.timestamp = hrt_absolute_time();
|
||||
|
||||
_sensor_optical_flow_pub.publish(sensor_optical_flow);
|
||||
|
||||
flow_sent_counter++;
|
||||
}
|
||||
|
||||
int distance_debug_counter = 0;
|
||||
|
||||
void handle_message_distance_sensor(mavlink_message_t *msg)
|
||||
{
|
||||
mavlink_distance_sensor_t dist_sensor;
|
||||
mavlink_msg_distance_sensor_decode(msg, &dist_sensor);
|
||||
|
||||
if ((_debug) && (!(distance_debug_counter % 10))) {
|
||||
PX4_INFO("distance: time: %u, quality: %u, height: %u",
|
||||
dist_sensor.time_boot_ms, dist_sensor.signal_quality,
|
||||
dist_sensor.current_distance);
|
||||
}
|
||||
|
||||
distance_debug_counter++;
|
||||
|
||||
distance_sensor_s ds{};
|
||||
|
||||
device::Device::DeviceId device_id;
|
||||
device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_MAVLINK;
|
||||
device_id.devid_s.bus = 1;
|
||||
device_id.devid_s.address = msg->sysid;
|
||||
device_id.devid_s.devtype = DRV_DIST_DEVTYPE_MAVLINK;
|
||||
|
||||
ds.timestamp = hrt_absolute_time(); /* Use system time for now, don't trust sender to attach correct timestamp */
|
||||
ds.min_distance = static_cast<float>(dist_sensor.min_distance) * 1e-2f; /* cm to m */
|
||||
ds.max_distance = static_cast<float>(dist_sensor.max_distance) * 1e-2f; /* cm to m */
|
||||
ds.current_distance = static_cast<float>(dist_sensor.current_distance) * 1e-2f; /* cm to m */
|
||||
ds.variance = dist_sensor.covariance * 1e-4f; /* cm^2 to m^2 */
|
||||
ds.h_fov = dist_sensor.horizontal_fov;
|
||||
ds.v_fov = dist_sensor.vertical_fov;
|
||||
ds.q[0] = dist_sensor.quaternion[0];
|
||||
ds.q[1] = dist_sensor.quaternion[1];
|
||||
ds.q[2] = dist_sensor.quaternion[2];
|
||||
ds.q[3] = dist_sensor.quaternion[3];
|
||||
ds.type = dist_sensor.type;
|
||||
ds.device_id = device_id.devid;
|
||||
ds.orientation = dist_sensor.orientation;
|
||||
|
||||
// MAVLink DISTANCE_SENSOR signal_quality value of 0 means unset/unknown
|
||||
// quality value. Also it comes normalised between 1 and 100 while the uORB
|
||||
// signal quality is normalised between 0 and 100.
|
||||
ds.signal_quality = dist_sensor.signal_quality == 0 ? -1 : 100 * (dist_sensor.signal_quality - 1) / 99;
|
||||
|
||||
_distance_sensor_pub.publish(ds);
|
||||
|
||||
distance_sent_counter++;
|
||||
}
|
||||
|
||||
void
|
||||
|
@ -518,6 +666,8 @@ handle_message_odometry_dsp(mavlink_message_t *msg)
|
|||
mavlink_odometry_t odom_in;
|
||||
mavlink_msg_odometry_decode(msg, &odom_in);
|
||||
|
||||
odometry_sent_counter++;
|
||||
|
||||
// fill vehicle_odometry from Mavlink ODOMETRY
|
||||
vehicle_odometry_s odom{};
|
||||
uint64_t timestamp = hrt_absolute_time();
|
||||
|
@ -699,6 +849,28 @@ handle_message_odometry_dsp(mavlink_message_t *msg)
|
|||
odom.reset_counter = odom_in.reset_counter;
|
||||
odom.quality = odom_in.quality;
|
||||
|
||||
int index = (int) position_source::VIO;
|
||||
|
||||
if (position_source_data[index].fail) {
|
||||
uint32_t duration = position_source_data[index].failure_duration;
|
||||
hrt_abstime start = position_source_data[index].failure_duration_start;
|
||||
|
||||
if (duration) {
|
||||
if (hrt_elapsed_time(&start) > (duration * 1000000)) {
|
||||
PX4_INFO("VIO failure ending");
|
||||
position_source_data[index].fail = false;
|
||||
position_source_data[index].failure_duration = 0;
|
||||
position_source_data[index].failure_duration_start = 0;
|
||||
|
||||
} else {
|
||||
odom.quality = 0;
|
||||
}
|
||||
|
||||
} else {
|
||||
odom.quality = 0;
|
||||
}
|
||||
}
|
||||
|
||||
switch (odom_in.estimator_type) {
|
||||
case MAV_ESTIMATOR_TYPE_UNKNOWN: // accept MAV_ESTIMATOR_TYPE_UNKNOWN for legacy support
|
||||
case MAV_ESTIMATOR_TYPE_NAIVE:
|
||||
|
@ -745,10 +917,6 @@ void actuator_controls_from_outputs_dsp(mavlink_hil_actuator_controls_t *msg)
|
|||
msg->mode = mode_flag_custom;
|
||||
msg->mode |= (armed) ? mode_flag_armed : 0;
|
||||
msg->flags = 0;
|
||||
|
||||
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
|
||||
msg->flags |= 1;
|
||||
#endif
|
||||
}
|
||||
|
||||
int openPort(const char *dev, speed_t speed)
|
||||
|
@ -759,7 +927,8 @@ int openPort(const char *dev, speed_t speed)
|
|||
}
|
||||
|
||||
_uart_fd = qurt_uart_open(dev, speed);
|
||||
PX4_DEBUG("qurt_uart_opened");
|
||||
|
||||
if (_debug) { PX4_INFO("qurt_uart_opened"); }
|
||||
|
||||
if (_uart_fd < 0) {
|
||||
PX4_ERR("Error opening port: %s (%i)", dev, errno);
|
||||
|
@ -840,25 +1009,50 @@ int stop()
|
|||
|
||||
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("Status of IMU_Data counter: %i", imu_counter);
|
||||
PX4_INFO("Value of current accel x, y, z data: %f, %f, %f", double(x_accel), double(y_accel), double(z_accel));
|
||||
PX4_INFO("Value of current gyro x, y, z data: %f, %f, %f", double(x_gyro), double(y_gyro), double(z_gyro));
|
||||
PX4_INFO("Value of HIL_Sensor counter: %i", hil_sensor_counter);
|
||||
PX4_INFO("Value of Heartbeat counter: %i", heartbeat_counter);
|
||||
PX4_INFO("Value of Vision data counter: %i", vision_msg_counter);
|
||||
PX4_INFO("Value of GPS Data counter: %i", gps_counter);
|
||||
return 0;
|
||||
PX4_INFO("HIL Sensor received: %i", hil_sensor_counter);
|
||||
PX4_INFO("IMU updates: %i", imu_counter);
|
||||
PX4_INFO("\tCurrent accel x, y, z: %f, %f, %f", double(x_accel), double(y_accel), double(z_accel));
|
||||
PX4_INFO("\tCurrent gyro x, y, z: %f, %f, %f", double(x_gyro), double(y_gyro), double(z_gyro));
|
||||
PX4_INFO("Magnetometer sent: %i", mag_counter);
|
||||
PX4_INFO("Barometer sent: %i", baro_counter);
|
||||
PX4_INFO("Heartbeat received: %i, sent: %i", heartbeat_received_counter, heartbeat_sent_counter);
|
||||
PX4_INFO("Odometry received: %i, sent: %i", odometry_received_counter, odometry_sent_counter);
|
||||
PX4_INFO("GPS received: %i, sent: %i", gps_received_counter, gps_sent_counter);
|
||||
PX4_INFO("Distance sensor received: %i, sent: %i", distance_received_counter, distance_sent_counter);
|
||||
PX4_INFO("Optical flow received: %i, sent: %i", flow_received_counter, flow_sent_counter);
|
||||
PX4_INFO("Actuator updates sent: %i", actuator_sent_counter);
|
||||
PX4_INFO("Unknown messages received: %i", unknown_msg_received_counter);
|
||||
}
|
||||
|
||||
uint64_t first_sensor_msg_timestamp = 0;
|
||||
uint64_t first_sensor_report_timestamp = 0;
|
||||
uint64_t last_sensor_report_timestamp = 0;
|
||||
void
|
||||
clear_status_counters()
|
||||
{
|
||||
heartbeat_received_counter = 0;
|
||||
heartbeat_sent_counter = 0;
|
||||
imu_counter = 0;
|
||||
hil_sensor_counter = 0;
|
||||
mag_counter = 0;
|
||||
baro_counter = 0;
|
||||
actuator_sent_counter = 0;
|
||||
odometry_received_counter = 0;
|
||||
odometry_sent_counter = 0;
|
||||
gps_received_counter = 0;
|
||||
gps_sent_counter = 0;
|
||||
distance_received_counter = 0;
|
||||
distance_sent_counter = 0;
|
||||
flow_received_counter = 0;
|
||||
flow_sent_counter = 0;
|
||||
unknown_msg_received_counter = 0;
|
||||
}
|
||||
|
||||
void
|
||||
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);
|
||||
|
||||
mag_counter++;
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -942,17 +1138,8 @@ handle_message_hil_sensor_dsp(mavlink_message_t *msg)
|
|||
sensor_baro.error_count = 0;
|
||||
sensor_baro.timestamp = hrt_absolute_time();
|
||||
_sensor_baro_pub.publish(sensor_baro);
|
||||
}
|
||||
|
||||
// differential pressure
|
||||
if ((hil_sensor.fields_updated & SensorSource::DIFF_PRESS) == SensorSource::DIFF_PRESS) {
|
||||
differential_pressure_s report{};
|
||||
report.timestamp_sample = gyro_accel_time;
|
||||
report.device_id = 1377548; // 1377548: DRV_DIFF_PRESS_DEVTYPE_SIM, BUS: 1, ADDR: 5, TYPE: SIMULATION
|
||||
report.temperature = hil_sensor.temperature;
|
||||
report.differential_pressure_pa = hil_sensor.diff_pressure * 100.0f; // hPa to Pa
|
||||
report.timestamp = hrt_absolute_time();
|
||||
_differential_pressure_pub.publish(report);
|
||||
baro_counter++;
|
||||
}
|
||||
|
||||
// battery status
|
||||
|
@ -970,7 +1157,6 @@ handle_message_hil_sensor_dsp(mavlink_message_t *msg)
|
|||
|
||||
_battery_pub.publish(hil_battery_status);
|
||||
}
|
||||
hil_sensor_counter++;
|
||||
}
|
||||
|
||||
void
|
||||
|
@ -989,15 +1175,41 @@ handle_message_hil_gps_dsp(mavlink_message_t *msg)
|
|||
|
||||
gps.device_id = device_id.devid;
|
||||
|
||||
gps.lat = hil_gps.lat;
|
||||
gps.lon = hil_gps.lon;
|
||||
gps.alt = hil_gps.alt;
|
||||
gps.alt_ellipsoid = hil_gps.alt;
|
||||
gps.latitude_deg = hil_gps.lat * 1e-7;
|
||||
gps.longitude_deg = hil_gps.lon * 1e-7;
|
||||
gps.altitude_msl_m = hil_gps.alt * 1e-3;
|
||||
gps.altitude_ellipsoid_m = hil_gps.alt * 1e-3;
|
||||
|
||||
gps.s_variance_m_s = 0.25f;
|
||||
gps.c_variance_rad = 0.5f;
|
||||
|
||||
gps.satellites_used = hil_gps.satellites_visible;
|
||||
gps.fix_type = hil_gps.fix_type;
|
||||
|
||||
int index = (int) position_source::GPS;
|
||||
|
||||
if (position_source_data[index].fail) {
|
||||
uint32_t duration = position_source_data[index].failure_duration;
|
||||
hrt_abstime start = position_source_data[index].failure_duration_start;
|
||||
|
||||
if (duration) {
|
||||
if (hrt_elapsed_time(&start) > (duration * 1000000)) {
|
||||
PX4_INFO("GPS failure ending");
|
||||
position_source_data[index].fail = false;
|
||||
position_source_data[index].failure_duration = 0;
|
||||
position_source_data[index].failure_duration_start = 0;
|
||||
|
||||
} else {
|
||||
gps.satellites_used = 1;
|
||||
gps.fix_type = 0;
|
||||
}
|
||||
|
||||
} else {
|
||||
gps.satellites_used = 1;
|
||||
gps.fix_type = 0;
|
||||
}
|
||||
}
|
||||
|
||||
gps.eph = (float)hil_gps.eph * 1e-2f; // cm -> m
|
||||
gps.epv = (float)hil_gps.epv * 1e-2f; // cm -> m
|
||||
|
||||
|
@ -1021,7 +1233,6 @@ handle_message_hil_gps_dsp(mavlink_message_t *msg)
|
|||
gps.timestamp_time_relative = 0;
|
||||
gps.time_utc_usec = hil_gps.time_usec;
|
||||
|
||||
gps.satellites_used = hil_gps.satellites_visible;
|
||||
|
||||
gps.heading = NAN;
|
||||
gps.heading_offset = NAN;
|
||||
|
@ -1029,10 +1240,51 @@ handle_message_hil_gps_dsp(mavlink_message_t *msg)
|
|||
gps.timestamp = hrt_absolute_time();
|
||||
|
||||
_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 myoptind = 1;
|
||||
|
@ -1044,20 +1296,47 @@ int dsp_hitl_main(int argc, char *argv[])
|
|||
|
||||
const char *verb = argv[myoptind];
|
||||
|
||||
|
||||
if (!strcmp(verb, "start")) {
|
||||
return dsp_hitl::start(argc - 1, argv + 1);
|
||||
}
|
||||
|
||||
else if (!strcmp(verb, "stop")) {
|
||||
} else if (!strcmp(verb, "stop")) {
|
||||
return dsp_hitl::stop();
|
||||
}
|
||||
|
||||
else if (!strcmp(verb, "status")) {
|
||||
return dsp_hitl::get_status();
|
||||
}
|
||||
} else if (!strcmp(verb, "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();
|
||||
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
|
||||
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
|
||||
${module_libraries}
|
||||
)
|
||||
|
|
|
@ -11,7 +11,6 @@ CONFIG_DRIVERS_VOXL2_IO=y
|
|||
CONFIG_MODULES_COMMANDER=y
|
||||
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
||||
CONFIG_MODULES_DATAMAN=y
|
||||
CONFIG_MODULES_LOAD_MON=y
|
||||
CONFIG_MODULES_LOGGER=y
|
||||
CONFIG_MODULES_MAVLINK=y
|
||||
CONFIG_MODULES_MUORB_APPS=y
|
||||
|
@ -26,3 +25,4 @@ CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
|
|||
CONFIG_SYSTEMCMDS_UORB=y
|
||||
CONFIG_SYSTEMCMDS_VER=y
|
||||
CONFIG_ORB_COMMUNICATOR=y
|
||||
CONFIG_PARAM_PRIMARY=y
|
||||
|
|
|
@ -1,10 +1,9 @@
|
|||
#!/bin/bash
|
||||
|
||||
cd src/modules/muorb/apps/libfc-sensor-api
|
||||
cd boards/modalai/voxl2/libfc-sensor-api
|
||||
rm -fR build
|
||||
mkdir build
|
||||
cd build
|
||||
CC=/home/4.1.0.4/tools/linaro64/bin/aarch64-linux-gnu-gcc cmake ..
|
||||
make
|
||||
cd ../../../../../..
|
||||
|
||||
cd ../../../../..
|
||||
|
|
|
@ -76,12 +76,13 @@ qshell flight_mode_manager start
|
|||
# Start all of the processing modules on the applications processor
|
||||
dataman start
|
||||
navigator start
|
||||
load_mon start
|
||||
|
||||
# Start microdds_client for ros2 offboard messages from agent over localhost
|
||||
microdds_client start -t udp -h 127.0.0.1 -p 8888
|
||||
|
||||
qshell pwm_out_sim start -m hil
|
||||
# g = gps, m = mag, o = odometry (vio), h = distance sensor, f = optic flow
|
||||
# qshell dsp_hitl start -g -m -o -h -f
|
||||
qshell dsp_hitl start -g -m
|
||||
|
||||
# start the onboard fast link to connect to voxl-mavlink-server
|
||||
|
|
|
@ -207,7 +207,6 @@ qshell flight_mode_manager start
|
|||
# Start all of the processing modules on the applications processor
|
||||
dataman start
|
||||
navigator start
|
||||
load_mon start
|
||||
|
||||
# This bridge allows raw data packets to be sent over UART to the ESC
|
||||
voxl2_io_bridge start
|
||||
|
|
|
@ -1,9 +1,11 @@
|
|||
CONFIG_BOARD_TOOLCHAIN="arm-none-eabi"
|
||||
CONFIG_BOARD_ARCHITECTURE="cortex-m7"
|
||||
CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS2"
|
||||
CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS0"
|
||||
CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS1"
|
||||
CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS4"
|
||||
CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS3"
|
||||
CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS4"
|
||||
CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS1"
|
||||
CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS2"
|
||||
CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS5"
|
||||
CONFIG_BOARD_SERIAL_TEL4="/dev/ttyS0"
|
||||
CONFIG_DRIVERS_ADC_ADS1115=y
|
||||
CONFIG_DRIVERS_ADC_BOARD_ADC=y
|
||||
CONFIG_DRIVERS_BAROMETER_DPS310=y
|
||||
|
@ -25,6 +27,7 @@ CONFIG_DRIVERS_PCA9685_PWM_OUT=y
|
|||
CONFIG_DRIVERS_POWER_MONITOR_INA226=y
|
||||
CONFIG_DRIVERS_PWM_OUT=y
|
||||
CONFIG_DRIVERS_RC_INPUT=y
|
||||
CONFIG_DRIVERS_SAFETY_BUTTON=y
|
||||
CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
|
||||
CONFIG_COMMON_TELEMETRY=y
|
||||
CONFIG_DRIVERS_TONE_ALARM=y
|
||||
|
@ -46,7 +49,6 @@ CONFIG_MODULES_FW_POS_CONTROL=y
|
|||
CONFIG_MODULES_FW_RATE_CONTROL=y
|
||||
CONFIG_MODULES_GIMBAL=y
|
||||
CONFIG_MODULES_GYRO_CALIBRATION=y
|
||||
CONFIG_MODULES_GYRO_FFT=y
|
||||
CONFIG_MODULES_LAND_DETECTOR=y
|
||||
CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
|
||||
CONFIG_MODULES_LOAD_MON=y
|
||||
|
@ -96,4 +98,3 @@ CONFIG_SYSTEMCMDS_UORB=y
|
|||
CONFIG_SYSTEMCMDS_USB_CONNECTED=y
|
||||
CONFIG_SYSTEMCMDS_VER=y
|
||||
CONFIG_SYSTEMCMDS_WORK_QUEUE=y
|
||||
CONFIG_EXAMPLES_FAKE_GPS=y
|
||||
|
|
|
@ -6,4 +6,4 @@
|
|||
param set-default BAT1_V_DIV 10.1
|
||||
param set-default BAT1_A_PER_V 17
|
||||
|
||||
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)
|
||||
dps310 -s -b 2 start
|
||||
|
||||
safety_button start
|
||||
|
|
|
@ -15,7 +15,7 @@ CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/mro/ctrl-zero-classic/nuttx-con
|
|||
CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y
|
||||
CONFIG_ARCH_BOARD_CUSTOM_NAME="px4"
|
||||
CONFIG_ARCH_CHIP="stm32h7"
|
||||
CONFIG_ARCH_CHIP_STM32H743II=y
|
||||
CONFIG_ARCH_CHIP_STM32H743ZI=y
|
||||
CONFIG_ARCH_CHIP_STM32H7=y
|
||||
CONFIG_ARCH_INTERRUPTSTACK=768
|
||||
CONFIG_ARMV7M_BASEPRI_WAR=y
|
||||
|
|
|
@ -222,6 +222,9 @@
|
|||
|
||||
|
||||
/* UART/USART */
|
||||
#define GPIO_USART1_TX GPIO_USART1_TX_3 /* PB6 */
|
||||
#define GPIO_USART1_RX GPIO_USART1_RX_3 /* PB7 */
|
||||
|
||||
#define GPIO_USART2_TX GPIO_USART2_TX_2 /* PD5 */
|
||||
#define GPIO_USART2_RX GPIO_USART2_RX_2 /* PD6 */
|
||||
#define GPIO_USART2_CTS GPIO_USART2_CTS_NSS_2 /* PD3 */
|
||||
|
@ -235,9 +238,6 @@
|
|||
#define GPIO_UART4_TX GPIO_UART4_TX_2 /* PA0 */
|
||||
#define GPIO_UART4_RX GPIO_UART4_RX_2 /* PA1 */
|
||||
|
||||
#define GPIO_USART6_TX 0 /* USART6 is RX-only */
|
||||
#define GPIO_USART6_RX GPIO_USART6_RX_1 /* PC7 */
|
||||
|
||||
#define GPIO_UART7_TX GPIO_UART7_TX_3 /* PE8 */
|
||||
#define GPIO_UART7_RX GPIO_UART7_RX_3 /* PE7 */
|
||||
|
||||
|
@ -268,13 +268,14 @@
|
|||
#define GPIO_SPI5_MISO GPIO_SPI5_MISO_1 /* PF8 */
|
||||
#define GPIO_SPI5_MOSI GPIO_SPI5_MOSI_2 /* PF9 */
|
||||
|
||||
#define GPIO_SPI6_SCK ADJ_SLEW_RATE(GPIO_SPI6_SCK_1) /* PG13 */
|
||||
#define GPIO_SPI6_MISO GPIO_SPI6_MISO_1 /* PG12 */
|
||||
#define GPIO_SPI6_MOSI GPIO_SPI6_MOSI_1 /* PG14 */
|
||||
|
||||
|
||||
/* I2C */
|
||||
#define GPIO_I2C1_SCL GPIO_I2C1_SCL_2 /* PB8 */
|
||||
#define GPIO_I2C1_SDA GPIO_I2C1_SDA_2 /* PB9 */
|
||||
|
||||
#define GPIO_I2C3_SCL GPIO_I2C3_SCL_2 /* PH7 */
|
||||
#define GPIO_I2C3_SDA GPIO_I2C3_SDA_2 /* PH8 */
|
||||
|
||||
#define GPIO_I2C4_SCL GPIO_I2C4_SCL_4 /* PB6 */
|
||||
#define GPIO_I2C4_SDA GPIO_I2C4_SDA_4 /* PB7 */
|
||||
#define GPIO_I2C4_SCL GPIO_I2C4_SCL_2 /* PF14 */
|
||||
#define GPIO_I2C4_SDA GPIO_I2C4_SDA_2 /* PF15 */
|
||||
|
|
|
@ -56,7 +56,7 @@ CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/mro/ctrl-zero-classic/nuttx-con
|
|||
CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y
|
||||
CONFIG_ARCH_BOARD_CUSTOM_NAME="px4"
|
||||
CONFIG_ARCH_CHIP="stm32h7"
|
||||
CONFIG_ARCH_CHIP_STM32H743II=y
|
||||
CONFIG_ARCH_CHIP_STM32H743ZI=y
|
||||
CONFIG_ARCH_CHIP_STM32H7=y
|
||||
CONFIG_ARCH_INTERRUPTSTACK=768
|
||||
CONFIG_ARCH_STACKDUMP=y
|
||||
|
@ -192,7 +192,6 @@ CONFIG_STM32H7_DMA2=y
|
|||
CONFIG_STM32H7_DMACAPABLE=y
|
||||
CONFIG_STM32H7_FLOWCONTROL_BROKEN=y
|
||||
CONFIG_STM32H7_I2C1=y
|
||||
CONFIG_STM32H7_I2C3=y
|
||||
CONFIG_STM32H7_I2C4=y
|
||||
CONFIG_STM32H7_I2C_DYNTIMEO=y
|
||||
CONFIG_STM32H7_I2C_DYNTIMEO_STARTSTOP=10
|
||||
|
@ -212,17 +211,20 @@ CONFIG_STM32H7_SPI2=y
|
|||
CONFIG_STM32H7_SPI5=y
|
||||
CONFIG_STM32H7_SPI5_DMA=y
|
||||
CONFIG_STM32H7_SPI5_DMA_BUFFER=1024
|
||||
CONFIG_STM32H7_SPI6=y
|
||||
CONFIG_STM32H7_TIM15=y
|
||||
CONFIG_STM32H7_TIM16=y
|
||||
CONFIG_STM32H7_TIM1=y
|
||||
CONFIG_STM32H7_TIM2=y
|
||||
CONFIG_STM32H7_TIM3=y
|
||||
CONFIG_STM32H7_TIM4=y
|
||||
CONFIG_STM32H7_TIM8=y
|
||||
CONFIG_STM32H7_USART1=y
|
||||
CONFIG_STM32H7_USART2=y
|
||||
CONFIG_STM32H7_USART3=y
|
||||
CONFIG_STM32H7_UART4=y
|
||||
CONFIG_STM32H7_UART7=y
|
||||
CONFIG_STM32H7_UART8=y
|
||||
CONFIG_STM32H7_USART2=y
|
||||
CONFIG_STM32H7_USART3=y
|
||||
CONFIG_STM32H7_USART6=y
|
||||
CONFIG_STM32H7_USART_BREAKS=y
|
||||
CONFIG_STM32H7_USART_INVERT=y
|
||||
CONFIG_STM32H7_USART_SINGLEWIRE=y
|
||||
|
@ -252,9 +254,6 @@ CONFIG_USART3_IFLOWCONTROL=y
|
|||
CONFIG_USART3_OFLOWCONTROL=y
|
||||
CONFIG_USART3_RXBUFSIZE=600
|
||||
CONFIG_USART3_TXBUFSIZE=3000
|
||||
CONFIG_USART6_BAUD=57600
|
||||
CONFIG_USART6_RXBUFSIZE=600
|
||||
CONFIG_USART6_TXBUFSIZE=1500
|
||||
CONFIG_USBDEV=y
|
||||
CONFIG_USBDEV_BUSPOWERED=y
|
||||
CONFIG_USBDEV_MAXPOWER=500
|
||||
|
|
|
@ -45,95 +45,111 @@
|
|||
#include <stm32_gpio.h>
|
||||
|
||||
/* 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_GREEN /* PB1 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN1)
|
||||
#define GPIO_nLED_RED /* PB4 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN4)
|
||||
#define GPIO_nLED_GREEN /* PB5 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN5)
|
||||
#define GPIO_nLED_BLUE /* PB3 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN3)
|
||||
|
||||
#define GPIO_LED_SAFETY /* PE6 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN6)
|
||||
|
||||
#define BOARD_HAS_CONTROL_STATUS_LEDS 1
|
||||
#define BOARD_OVERLOAD_LED LED_RED
|
||||
#define BOARD_ARMED_STATE_LED LED_BLUE
|
||||
|
||||
/* ADC channels */
|
||||
#define PX4_ADC_GPIO \
|
||||
/* PA2 */ GPIO_ADC12_INP14, \
|
||||
/* PC4 */ GPIO_ADC12_INP4, \
|
||||
/* PA3 */ GPIO_ADC12_INP15, \
|
||||
/* 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 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_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 \
|
||||
((1 << ADC_BATTERY_VOLTAGE_CHANNEL) | \
|
||||
(1 << ADC_BATTERY_CURRENT_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 */
|
||||
#define BOARD_ADC_OPEN_CIRCUIT_V (5.6f)
|
||||
|
||||
/* 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_CAN2_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 /* PF12 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN12)
|
||||
|
||||
/* PWM */
|
||||
#define DIRECT_PWM_OUTPUT_CHANNELS 12
|
||||
|
||||
/* 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 BOARD_NUMBER_BRICKS 1
|
||||
|
||||
#define GPIO_VDD_3V3_SPEKTRUM_POWER_EN /* PE4 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN4)
|
||||
|
||||
#define GPIO_LEVEL_SHIFTER_OE /* PC13 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13)
|
||||
|
||||
/* Define True logic Power Control in arch agnostic form */
|
||||
#define VDD_3V3_SPEKTRUM_POWER_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_3V3_SPEKTRUM_POWER_EN, (!on_true))
|
||||
#define READ_VDD_3V3_SPEKTRUM_POWER_EN() (px4_arch_gpioread(GPIO_VDD_3V3_SPEKTRUM_POWER_EN) == 0)
|
||||
|
||||
/* Tone alarm output */
|
||||
#define TONE_ALARM_TIMER 2 /* timer 2 */
|
||||
#define TONE_ALARM_CHANNEL 1 /* PA15 TIM2_CH1 */
|
||||
#define TONE_ALARM_TIMER 16 /* timer 16 */
|
||||
#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 GPIO_TIM2_CH1OUT_2
|
||||
#define GPIO_TONE_ALARM GPIO_TIM16_CH1OUT_2
|
||||
|
||||
/* USB OTG FS */
|
||||
#define GPIO_OTGFS_VBUS /* PA9 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_100MHz|GPIO_PORTA|GPIO_PIN9)
|
||||
|
||||
/* High-resolution timer */
|
||||
#define HRT_TIMER 3 /* use timer3 for the HRT */
|
||||
#define HRT_TIMER_CHANNEL 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 GPIO_PPM_IN /* PB0 T3C3 */ GPIO_TIM3_CH3IN_1
|
||||
#define HRT_PPM_CHANNEL /* T3C2 */ 2 /* use capture/compare channel 2 */
|
||||
#define GPIO_PPM_IN /* PC7 T3C2 */ GPIO_TIM3_CH2IN_3
|
||||
|
||||
/* 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) */
|
||||
#define GPIO_SAFETY_SWITCH_IN /* PC4 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN4)
|
||||
/* Enable the FMU to use the switch it if there is no px4io fixme:This should be BOARD_SAFTY_BUTTON() */
|
||||
#define GPIO_SAFETY_SWITCH_IN /* PA10 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN10)
|
||||
/* Enable the FMU to use the switch it if there is no px4io fixme:This should be BOARD_SAFETY_BUTTON() */
|
||||
#define GPIO_BTN_SAFETY GPIO_SAFETY_SWITCH_IN /* Enable the FMU to control it if there is no px4io */
|
||||
|
||||
/* No PX4IO processor present */
|
||||
#define PX4_MFT_HW_SUPPORTED_PX4_MFT_PX4IO 0
|
||||
|
||||
/* Power switch controls ******************************************************/
|
||||
#define SPEKTRUM_POWER(_on_true) VDD_3V3_SPEKTRUM_POWER_EN(_on_true)
|
||||
|
||||
/*
|
||||
* Board has a separate RC_IN
|
||||
*
|
||||
* GPIO PPM_IN on PB0 T3CH3
|
||||
* GPIO PPM_IN on PC7 T3CH2
|
||||
* SPEKTRUM_RX (it's TX or RX in Bind) on UART6 PC7
|
||||
* Inversion is possible in the UART and can drive GPIO_PPM_IN as an output
|
||||
*/
|
||||
#define GPIO_PPM_IN_AS_OUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_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_UART() /* Can be left as uart */
|
||||
#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_NUM_IO_TIMERS 5
|
||||
#define BOARD_NUM_IO_TIMERS 6
|
||||
|
||||
|
||||
#define BOARD_ENABLE_CONSOLE_BUFFER
|
||||
|
@ -171,9 +187,12 @@
|
|||
GPIO_CAN2_SILENT_S0, \
|
||||
GPIO_nPOWER_IN_A, \
|
||||
GPIO_VDD_3V3_SPEKTRUM_POWER_EN, \
|
||||
GPIO_LEVEL_SHIFTER_OE, \
|
||||
GPIO_TONE_ALARM_IDLE, \
|
||||
GPIO_SAFETY_SWITCH_IN, \
|
||||
GPIO_OTGFS_VBUS, \
|
||||
GPIO_BTN_SAFETY, \
|
||||
GPIO_LED_SAFETY, \
|
||||
}
|
||||
|
||||
__BEGIN_DECLS
|
||||
|
|
|
@ -35,6 +35,5 @@
|
|||
|
||||
constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = {
|
||||
initI2CBusExternal(1),
|
||||
initI2CBusExternal(3),
|
||||
initI2CBusExternal(4),
|
||||
};
|
||||
|
|
|
@ -37,7 +37,7 @@
|
|||
|
||||
constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = {
|
||||
initSPIBus(SPI::Bus::SPI1, {
|
||||
initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::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}),
|
||||
}, {GPIO::PortE, GPIO::Pin3}),
|
||||
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, {
|
||||
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>
|
||||
|
||||
/* Timer allocation
|
||||
*
|
||||
* TIM1_CH4 T FMU_CH1
|
||||
* TIM1_CH3 T FMU_CH2
|
||||
* TIM1_CH2 T FMU_CH3
|
||||
* TIM1_CH1 T FMU_CH4
|
||||
*
|
||||
* TIM4_CH2 T FMU_CH5
|
||||
* TIM4_CH3 T FMU_CH6
|
||||
* TIM2_CH3 T FMU_CH7
|
||||
* TIM2_CH1 T FMU_CH8
|
||||
*
|
||||
* TIM2_CH4 T FMU_CH9
|
||||
* TIM15_CH1 T FMU_CH10
|
||||
*
|
||||
* TIM8_CH1 T FMU_CH11
|
||||
*
|
||||
* TIM4_CH4 T FMU_CH12
|
||||
*
|
||||
* TIM16_CH1 T BUZZER - Driven by other driver
|
||||
*/
|
||||
|
||||
constexpr io_timers_t io_timers[MAX_IO_TIMERS] = {
|
||||
initIOTimer(Timer::Timer1, DMA{DMA::Index1}),
|
||||
initIOTimer(Timer::Timer4, DMA{DMA::Index1}),
|
||||
|
|
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
@ -18,4 +18,4 @@ CONFIG_MODULES_UUV_POS_CONTROL=n
|
|||
CONFIG_MODULES_VTOL_ATT_CONTROL=n
|
||||
CONFIG_EKF2_AUX_GLOBAL_POSITION=y
|
||||
# CONFIG_EKF2_WIND is not set
|
||||
CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y
|
||||
CONFIG_MODULES_DIFFERENTIAL_DRIVE=y
|
||||
|
|
|
@ -53,7 +53,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
|
|||
CONFIG_MODULES_COMMANDER=y
|
||||
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
||||
CONFIG_MODULES_DATAMAN=y
|
||||
CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y
|
||||
CONFIG_MODULES_DIFFERENTIAL_DRIVE=y
|
||||
CONFIG_MODULES_EKF2=y
|
||||
CONFIG_MODULES_ESC_BATTERY=y
|
||||
CONFIG_MODULES_EVENTS=y
|
||||
|
@ -81,7 +81,6 @@ CONFIG_MODULES_NAVIGATOR=y
|
|||
CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF=y
|
||||
CONFIG_MODULES_PAYLOAD_DELIVERER=y
|
||||
CONFIG_MODULES_RC_UPDATE=y
|
||||
CONFIG_MODULES_ROVER_POS_CONTROL=y
|
||||
CONFIG_MODULES_SENSORS=y
|
||||
CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
|
||||
CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
|
||||
|
@ -102,9 +101,7 @@ CONFIG_SYSTEMCMDS_NSHTERM=y
|
|||
CONFIG_SYSTEMCMDS_PARAM=y
|
||||
CONFIG_SYSTEMCMDS_PERF=y
|
||||
CONFIG_SYSTEMCMDS_REBOOT=y
|
||||
CONFIG_SYSTEMCMDS_REFLECT=y
|
||||
CONFIG_SYSTEMCMDS_SD_BENCH=y
|
||||
CONFIG_SYSTEMCMDS_SD_STRESS=y
|
||||
CONFIG_SYSTEMCMDS_SYSTEM_TIME=y
|
||||
CONFIG_SYSTEMCMDS_TOP=y
|
||||
CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
|
||||
|
|
Binary file not shown.
|
@ -16,7 +16,7 @@ param set-default SENS_EN_INA238 0
|
|||
param set-default SENS_EN_INA228 0
|
||||
param set-default SENS_EN_INA226 1
|
||||
|
||||
if ver hwbasecmp 008 009 00a 010
|
||||
if ver hwbasecmp 008 009 00a 010 011
|
||||
then
|
||||
# Skynode: use the "custom participant" config for uxrce_dds_client
|
||||
param set-default UXRCE_DDS_PTCFG 2
|
||||
|
|
|
@ -3,7 +3,7 @@
|
|||
# board specific MAVLink startup script.
|
||||
#------------------------------------------------------------------------------
|
||||
|
||||
if ver hwbasecmp 008 009 00a 010
|
||||
if ver hwbasecmp 008 009 00a 010 011
|
||||
then
|
||||
# Start MAVLink on the UART connected to the mission computer
|
||||
mavlink start -d /dev/ttyS4 -b 3000000 -r 290000 -m onboard_low_bandwidth -x -z
|
||||
|
|
|
@ -49,7 +49,7 @@ then
|
|||
fi
|
||||
fi
|
||||
|
||||
if ver hwbasecmp 008 009 00a 010
|
||||
if ver hwbasecmp 008 009 00a 010 011
|
||||
then
|
||||
#SKYNODE base fmu board orientation
|
||||
|
||||
|
|
|
@ -1,21 +1,16 @@
|
|||
CONFIG_DRIVERS_IRLOCK=n
|
||||
CONFIG_MODULES_AIRSPEED_SELECTOR=n
|
||||
CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n
|
||||
CONFIG_MODULES_FLIGHT_MODE_MANAGER=n
|
||||
CONFIG_MODULES_FW_ATT_CONTROL=n
|
||||
CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n
|
||||
CONFIG_MODULES_FW_POS_CONTROL=n
|
||||
CONFIG_MODULES_FW_RATE_CONTROL=n
|
||||
CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=n
|
||||
CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n
|
||||
CONFIG_MODULES_MC_ATT_CONTROL=n
|
||||
CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n
|
||||
CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=n
|
||||
CONFIG_MODULES_MC_POS_CONTROL=n
|
||||
CONFIG_MODULES_MC_RATE_CONTROL=n
|
||||
CONFIG_MODULES_UUV_ATT_CONTROL=n
|
||||
CONFIG_MODULES_UUV_POS_CONTROL=n
|
||||
CONFIG_MODULES_VTOL_ATT_CONTROL=n
|
||||
CONFIG_EKF2_AUX_GLOBAL_POSITION=y
|
||||
# CONFIG_EKF2_WIND is not set
|
||||
CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y
|
||||
|
|
Binary file not shown.
|
@ -1,21 +1,16 @@
|
|||
CONFIG_DRIVERS_IRLOCK=n
|
||||
CONFIG_MODULES_AIRSPEED_SELECTOR=n
|
||||
CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n
|
||||
CONFIG_MODULES_FLIGHT_MODE_MANAGER=n
|
||||
CONFIG_MODULES_FW_ATT_CONTROL=n
|
||||
CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n
|
||||
CONFIG_MODULES_FW_POS_CONTROL=n
|
||||
CONFIG_MODULES_FW_RATE_CONTROL=n
|
||||
CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=n
|
||||
CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n
|
||||
CONFIG_MODULES_MC_ATT_CONTROL=n
|
||||
CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n
|
||||
CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=n
|
||||
CONFIG_MODULES_MC_POS_CONTROL=n
|
||||
CONFIG_MODULES_MC_RATE_CONTROL=n
|
||||
CONFIG_MODULES_UUV_ATT_CONTROL=n
|
||||
CONFIG_MODULES_UUV_POS_CONTROL=n
|
||||
CONFIG_MODULES_VTOL_ATT_CONTROL=n
|
||||
CONFIG_EKF2_AUX_GLOBAL_POSITION=y
|
||||
# CONFIG_EKF2_WIND is not set
|
||||
CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y
|
||||
CONFIG_MODULES_DIFFERENTIAL_DRIVE=y
|
||||
|
|
|
@ -13,4 +13,4 @@ CONFIG_MODULES_MC_RATE_CONTROL=n
|
|||
CONFIG_MODULES_VTOL_ATT_CONTROL=n
|
||||
CONFIG_EKF2_AUX_GLOBAL_POSITION=y
|
||||
# CONFIG_EKF2_WIND is not set
|
||||
CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y
|
||||
CONFIG_MODULES_DIFFERENTIAL_DRIVE=y
|
||||
|
|
Binary file not shown.
|
@ -18,7 +18,7 @@ param set-default SENS_EN_INA238 0
|
|||
param set-default SENS_EN_INA228 0
|
||||
param set-default SENS_EN_INA226 0
|
||||
|
||||
if ver hwbasecmp 009 010
|
||||
if ver hwbasecmp 009 010 011
|
||||
then
|
||||
# Skynode: use the "custom participant" config for uxrce_dds_client
|
||||
param set-default UXRCE_DDS_PTCFG 2
|
||||
|
|
|
@ -4,7 +4,7 @@
|
|||
#------------------------------------------------------------------------------
|
||||
|
||||
# if skynode base board is detected start Mavlink on Telem2
|
||||
if ver hwbasecmp 009 010
|
||||
if ver hwbasecmp 009 010 011
|
||||
then
|
||||
mavlink start -d /dev/ttyS4 -b 3000000 -r 290000 -m onboard_low_bandwidth -x -z
|
||||
|
||||
|
|
|
@ -12,6 +12,7 @@ fi
|
|||
if param compare -s ADC_ADS1115_EN 1
|
||||
then
|
||||
ads1115 start -X
|
||||
board_adc start -n
|
||||
else
|
||||
board_adc start
|
||||
fi
|
||||
|
@ -55,7 +56,7 @@ then
|
|||
fi
|
||||
|
||||
#Start Auterion Power Module selector for Skynode boards
|
||||
if ver hwbasecmp 009 010
|
||||
if ver hwbasecmp 009 010 011
|
||||
then
|
||||
pm_selector_auterion start
|
||||
else
|
||||
|
@ -92,7 +93,7 @@ else
|
|||
icm20649 -s -R 6 start
|
||||
else
|
||||
# Internal SPI BMI088
|
||||
if ver hwbasecmp 009 010
|
||||
if ver hwbasecmp 009 010 011
|
||||
then
|
||||
bmi088 -A -R 6 -s start
|
||||
bmi088 -G -R 6 -s start
|
||||
|
@ -109,7 +110,7 @@ else
|
|||
fi
|
||||
|
||||
# Internal SPI bus ICM42688p
|
||||
if ver hwbasecmp 009 010
|
||||
if ver hwbasecmp 009 010 011
|
||||
then
|
||||
icm42688p -R 12 -s start
|
||||
else
|
||||
|
@ -126,7 +127,7 @@ else
|
|||
# Internal SPI bus ICM-42670-P (hard-mounted)
|
||||
icm42670p -R 10 -s start
|
||||
else
|
||||
if ver hwbasecmp 009 010
|
||||
if ver hwbasecmp 009 010 011
|
||||
then
|
||||
icm20602 -R 6 -s start
|
||||
else
|
||||
|
|
|
@ -13,4 +13,4 @@ CONFIG_MODULES_MC_RATE_CONTROL=n
|
|||
CONFIG_MODULES_VTOL_ATT_CONTROL=n
|
||||
CONFIG_EKF2_AUX_GLOBAL_POSITION=y
|
||||
# CONFIG_EKF2_WIND is not set
|
||||
CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y
|
||||
CONFIG_MODULES_DIFFERENTIAL_DRIVE=y
|
||||
|
|
|
@ -6,21 +6,30 @@ CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS3"
|
|||
CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS2"
|
||||
CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS5"
|
||||
CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS6"
|
||||
CONFIG_DRIVERS_ADC_ADS1115=y
|
||||
CONFIG_DRIVERS_ADC_BOARD_ADC=y
|
||||
CONFIG_DRIVERS_BAROMETER_BMP388=y
|
||||
CONFIG_DRIVERS_BAROMETER_INVENSENSE_ICP201XX=y
|
||||
CONFIG_DRIVERS_BAROMETER_MS5611=y
|
||||
CONFIG_DRIVERS_CAMERA_CAPTURE=y
|
||||
CONFIG_DRIVERS_CAMERA_TRIGGER=y
|
||||
CONFIG_DRIVERS_DISTANCE_SENSOR_LIGHTWARE_LASER_I2C=y
|
||||
CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
|
||||
CONFIG_COMMON_DISTANCE_SENSOR=y
|
||||
CONFIG_DRIVERS_DSHOT=y
|
||||
CONFIG_DRIVERS_GPS=y
|
||||
CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16470=y
|
||||
CONFIG_DRIVERS_IMU_BOSCH_BMI088=y
|
||||
CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y
|
||||
CONFIG_DRIVERS_IMU_INVENSENSE_ICM20649=y
|
||||
CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y
|
||||
CONFIG_DRIVERS_IMU_INVENSENSE_ICM42670P=y
|
||||
CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
|
||||
CONFIG_DRIVERS_IMU_INVENSENSE_ICM45686=y
|
||||
CONFIG_DRIVERS_IMU_INVENSENSE_IIM42652=y
|
||||
CONFIG_COMMON_INS=y
|
||||
CONFIG_COMMON_LIGHT=y
|
||||
CONFIG_DRIVERS_MAGNETOMETER_BOSCH_BMM150=y
|
||||
CONFIG_DRIVERS_MAGNETOMETER_HMC5883=y
|
||||
CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y
|
||||
CONFIG_COMMON_MAGNETOMETER=y
|
||||
CONFIG_DRIVERS_OSD_MSP_OSD=y
|
||||
CONFIG_DRIVERS_POWER_MONITOR_INA226=y
|
||||
CONFIG_DRIVERS_POWER_MONITOR_INA228=y
|
||||
CONFIG_DRIVERS_POWER_MONITOR_INA238=y
|
||||
|
@ -32,14 +41,20 @@ CONFIG_COMMON_TELEMETRY=y
|
|||
CONFIG_DRIVERS_TONE_ALARM=y
|
||||
CONFIG_DRIVERS_UAVCAN=y
|
||||
CONFIG_COMMON_UWB=y
|
||||
CONFIG_MODULES_AIRSPEED_SELECTOR=y
|
||||
CONFIG_MODULES_BATTERY_STATUS=y
|
||||
CONFIG_MODULES_CAMERA_FEEDBACK=y
|
||||
CONFIG_MODULES_COMMANDER=y
|
||||
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
||||
CONFIG_MODULES_DATAMAN=y
|
||||
CONFIG_MODULES_EKF2=y
|
||||
CONFIG_MODULES_ESC_BATTERY=y
|
||||
CONFIG_MODULES_EVENTS=y
|
||||
CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
|
||||
CONFIG_MODULES_FW_ATT_CONTROL=y
|
||||
CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
|
||||
CONFIG_MODULES_FW_POS_CONTROL=y
|
||||
CONFIG_MODULES_FW_RATE_CONTROL=y
|
||||
CONFIG_MODULES_GIMBAL=y
|
||||
CONFIG_MODULES_GYRO_CALIBRATION=y
|
||||
CONFIG_MODULES_GYRO_FFT=y
|
||||
|
@ -57,11 +72,15 @@ CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
|
|||
CONFIG_MODULES_MC_POS_CONTROL=y
|
||||
CONFIG_MODULES_MC_RATE_CONTROL=y
|
||||
CONFIG_MODULES_NAVIGATOR=y
|
||||
CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF=y
|
||||
CONFIG_MODULES_RC_UPDATE=y
|
||||
CONFIG_MODULES_ROVER_POS_CONTROL=y
|
||||
CONFIG_MODULES_SENSORS=y
|
||||
CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
|
||||
CONFIG_MODULES_UXRCE_DDS_CLIENT=y
|
||||
CONFIG_MODULES_VTOL_ATT_CONTROL=y
|
||||
CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
|
||||
CONFIG_SYSTEMCMDS_BSONDUMP=y
|
||||
CONFIG_SYSTEMCMDS_DMESG=y
|
||||
CONFIG_SYSTEMCMDS_DUMPFILE=y
|
||||
CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
|
||||
|
|
|
@ -4,7 +4,7 @@
|
|||
#------------------------------------------------------------------------------
|
||||
|
||||
# if skynode base board is detected start Mavlink on Telem2
|
||||
if ver hwbasecmp 009 010
|
||||
if ver hwbasecmp 009 010 011
|
||||
then
|
||||
mavlink start -d /dev/ttyS5 -b 3000000 -r 290000 -m onboard_low_bandwidth -x -z
|
||||
|
||||
|
|
|
@ -257,7 +257,7 @@ CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
|
|||
CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
|
||||
CONFIG_SCHED_LPWORK=y
|
||||
CONFIG_SCHED_LPWORKPRIORITY=50
|
||||
CONFIG_SCHED_LPWORKSTACKSIZE=1632
|
||||
CONFIG_SCHED_LPWORKSTACKSIZE=2032
|
||||
CONFIG_SCHED_WAITPID=y
|
||||
CONFIG_SDIO_BLOCKSETUP=y
|
||||
CONFIG_SEM_PREALLOCHOLDERS=32
|
||||
|
|
|
@ -1,4 +1,9 @@
|
|||
CONFIG_MODULES_AIRSPEED_SELECTOR=n
|
||||
CONFIG_MODULES_FLIGHT_MODE_MANAGER=n
|
||||
CONFIG_MODULES_FW_ATT_CONTROL=n
|
||||
CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n
|
||||
CONFIG_MODULES_FW_POS_CONTROL=n
|
||||
CONFIG_MODULES_FW_RATE_CONTROL=n
|
||||
CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=n
|
||||
CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n
|
||||
CONFIG_MODULES_MC_ATT_CONTROL=n
|
||||
|
@ -6,6 +11,7 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n
|
|||
CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=n
|
||||
CONFIG_MODULES_MC_POS_CONTROL=n
|
||||
CONFIG_MODULES_MC_RATE_CONTROL=n
|
||||
CONFIG_MODULES_VTOL_ATT_CONTROL=n
|
||||
CONFIG_EKF2_AUX_GLOBAL_POSITION=y
|
||||
# CONFIG_EKF2_WIND is not set
|
||||
CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y
|
||||
CONFIG_MODULES_DIFFERENTIAL_DRIVE=y
|
||||
|
|
|
@ -12,7 +12,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
|
|||
CONFIG_MODULES_COMMANDER=y
|
||||
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
||||
CONFIG_MODULES_DATAMAN=y
|
||||
CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y
|
||||
CONFIG_MODULES_DIFFERENTIAL_DRIVE=y
|
||||
CONFIG_MODULES_EKF2=y
|
||||
CONFIG_EKF2_VERBOSE_STATUS=y
|
||||
CONFIG_EKF2_AUX_GLOBAL_POSITION=y
|
||||
|
|
|
@ -58,6 +58,7 @@ set(msg_files
|
|||
CameraCapture.msg
|
||||
CameraStatus.msg
|
||||
CameraTrigger.msg
|
||||
CanInterfaceStatus.msg
|
||||
CellularStatus.msg
|
||||
CollisionConstraints.msg
|
||||
CollisionReport.msg
|
||||
|
@ -99,6 +100,7 @@ set(msg_files
|
|||
FollowTargetStatus.msg
|
||||
GeneratorStatus.msg
|
||||
GeofenceResult.msg
|
||||
GeofenceStatus.msg
|
||||
GimbalControls.msg
|
||||
GimbalDeviceAttitudeStatus.msg
|
||||
GimbalDeviceInformation.msg
|
||||
|
@ -153,6 +155,10 @@ set(msg_files
|
|||
OrbTest.msg
|
||||
OrbTestLarge.msg
|
||||
OrbTestMedium.msg
|
||||
ParameterResetRequest.msg
|
||||
ParameterSetUsedRequest.msg
|
||||
ParameterSetValueRequest.msg
|
||||
ParameterSetValueResponse.msg
|
||||
ParameterUpdate.msg
|
||||
Ping.msg
|
||||
PositionControllerLandingStatus.msg
|
||||
|
@ -173,6 +179,7 @@ set(msg_files
|
|||
RegisterExtComponentReply.msg
|
||||
RegisterExtComponentRequest.msg
|
||||
Rpm.msg
|
||||
RtlStatus.msg
|
||||
RtlTimeEstimate.msg
|
||||
SatelliteInfo.msg
|
||||
SensorAccel.msg
|
||||
|
@ -283,6 +290,9 @@ foreach(msg_file ${msg_files})
|
|||
list(APPEND uorb_json_files ${msg_source_out_path}/${msg}.json)
|
||||
endforeach()
|
||||
|
||||
# set parent scope msg_files for ROS
|
||||
set(msg_files ${msg_files} PARENT_SCOPE)
|
||||
|
||||
# Generate uORB headers
|
||||
add_custom_command(
|
||||
OUTPUT
|
||||
|
|
|
@ -0,0 +1,6 @@
|
|||
uint64 timestamp # time since system start (microseconds)
|
||||
uint8 interface
|
||||
|
||||
uint64 io_errors
|
||||
uint64 frames_tx
|
||||
uint64 frames_rx
|
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in New Issue