Compare commits

..

1 Commits

Author SHA1 Message Date
Matthias Grob 33807a0002 Commander: add possibility to ignore loss of non-essential stick input in external modes 2024-01-26 10:16:37 +01:00
480 changed files with 10260 additions and 15583 deletions

View File

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

View File

@ -1,14 +0,0 @@
root = true
[*]
insert_final_newline = false
[{*.{c,cpp,cc,h,hpp},CMakeLists.txt,Kconfig}]
indent_style = tab
tab_width = 8
# Not in the official standard, but supported by many editors
max_line_length = 120
[*.yaml]
indent_style = space
indent_size = 2

View File

@ -25,7 +25,6 @@ jobs:
ark_can-rtk-gps,
ark_cannode,
ark_fmu-v6x,
ark_septentrio-gps,
atl_mantis-edu,
av_x-v1,
bitcraze_crazyflie,

2
.gitmodules vendored
View File

@ -81,5 +81,5 @@
url = https://github.com/PX4/PX4-gazebo-models.git
branch = main
[submodule "boards/modalai/voxl2/libfc-sensor-api"]
path = boards/modalai/voxl2/libfc-sensor-api
path = src/modules/muorb/apps/libfc-sensor-api
url = https://gitlab.com/voxl-public/voxl-sdk/core-libs/libfc-sensor-api.git

View File

@ -151,16 +151,6 @@ 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

View File

@ -1,80 +0,0 @@
#!/bin/sh
# @name Gazebo lawnmower
# @type Rover
# @class Rover
. ${R}etc/init.d/rc.rover_differential_defaults
PX4_SIMULATOR=${PX4_SIMULATOR:=gz}
PX4_GZ_WORLD=${PX4_GZ_WORLD:=default}
PX4_SIM_MODEL=${PX4_SIM_MODEL:=lawnmower}
param set-default SIM_GZ_EN 1 # Gazebo bridge
# Simulated sensors
param set-default SENS_EN_GPSSIM 1
param set-default SENS_EN_BAROSIM 0
param set-default SENS_EN_MAGSIM 1
param set-default SENS_EN_ARSPDSIM 1
# We can arm and drive in manual mode when it slides and GPS check fails:
param set-default COM_ARM_WO_GPS 1
# Set Differential Drive Kinematics Library parameters:
param set RDD_WHEEL_BASE 0.9
param set RDD_WHEEL_RADIUS 0.22
param set RDD_WHEEL_SPEED 10.0 # Maximum wheel speed rad/s, approx 8 km/h
# Actuator mapping - set SITL motors/servos output parameters:
# "Motors" - motor channels 0 (Right) and 1 (Left) - via Wheels GZ bridge:
param set-default SIM_GZ_WH_FUNC1 101 # right wheel
#param set-default SIM_GZ_WH_MIN1 0
#param set-default SIM_GZ_WH_MAX1 200
#param set-default SIM_GZ_WH_DIS1 100
#param set-default SIM_GZ_WH_FAIL1 100
param set-default SIM_GZ_WH_FUNC2 102 # left wheel
#param set-default SIM_GZ_WH_MIN2 0
#param set-default SIM_GZ_WH_MAX2 200
#aram set-default SIM_GZ_WH_DIS2 100
#param set-default SIM_GZ_WH_FAIL2 100
param set-default SIM_GZ_WH_REV 0 # no need to reverse any wheels
# Note: The servo configurations ( SIM_GZ_SV_FUNC*) outlined below are intended for educational purposes in this simulation.
# They do not have physical effects in the simulated environment, except for actuating the joints. Their definitions are meant to demonstrate
# how actuators could be mapped and configured in a real-world application, providing a foundation for understanding and implementing actuator
# controls in practical scenarios.
# Cutter deck blades clutch, PCA9685 servo channel 3, "RC FLAPS" (406) - leftmost switch, or "Servo 3" (203):
param set-default SIM_GZ_SV_FUNC3 203
param set-default SIM_GZ_SV_MIN3 0
param set-default SIM_GZ_SV_MAX3 1000
param set-default SIM_GZ_SV_DIS3 500
param set-default SIM_GZ_SV_FAIL3 500
# Gas engine throttle, PCA9685 servo channel 4, "RC AUX1" (407) - left knob, or "Servo 4" (204):
# - on minimum when disarmed or failed:
param set-default SIM_GZ_SV_FUNC4 204
param set-default SIM_GZ_SV_MIN4 0
param set-default SIM_GZ_SV_MAX4 1000
param set-default SIM_GZ_SV_DIS4 500
param set-default SIM_GZ_SV_FAIL4 500
# Controlling PCA9685 servos 5,6,7,8 directly via "Servo 5..8" setting, by publishing actuator_servos.control[]:
# Strobes, PCA9685 servo channel 5, "Servo 5" (205) - flashing indicates Mission mode:
#param set-default SIM_GZ_SV_FUNC5 205
#param set-default SIM_GZ_SV_MIN5 1000
#param set-default SIM_GZ_SV_MAX5 2000
#param set-default SIM_GZ_SV_DIS5 1000
#param set-default SIM_GZ_SV_FAIL5 1000
# Horn, PCA9685 servo channel 6, "Servo 6" (206) - for alarms like GPS failure:
#param set-default SIM_GZ_SV_FUNC6 206
# Spare PCA9685 servo channel 7 on "RC AUX2" (408) - right knob, or "Servo 7" (207):
#param set-default SIM_GZ_SV_FUNC7 207
# Spare PCA9685 servo channel 8 - "Servo 8" (208):
#param set-default SIM_GZ_SV_FUNC8 208

View File

@ -82,7 +82,6 @@ px4_add_romfs_files(
4008_gz_advanced_plane
4009_gz_r1_rover
4010_gz_x500_mono_cam
4011_gz_lawnmower
6011_gazebo-classic_typhoon_h480
6011_gazebo-classic_typhoon_h480.post

View File

@ -41,6 +41,19 @@ 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
@ -51,13 +64,6 @@ elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" = "1" ]; then
# "gz sim" from Garden on
gz_command="gz"
gz_sub_command="sim"
# Specify render engine if `GZ_SIM_RENDER_ENGINE` is set
# (for example, if you want to use Ogre 1.x instead of Ogre 2.x):
if [ -n "${PX4_GZ_SIM_RENDER_ENGINE}" ]; then
echo "INFO [init] Setting Gazebo render engine to '${PX4_GZ_SIM_RENDER_ENGINE}'!"
gz_sub_command="${gz_sub_command} --render-engine ${PX4_GZ_SIM_RENDER_ENGINE}"
fi
else
echo "ERROR [init] Gazebo gz please install gz-garden"
exit 1
@ -167,12 +173,6 @@ 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

View File

@ -164,6 +164,10 @@ 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
@ -244,12 +248,7 @@ then
fi
load_mon start
if param compare SIM_BAT_ENABLE 1
then
battery_simulator start
fi
battery_simulator start
tone_alarm start
rc_update start
manual_control start

View File

@ -34,79 +34,28 @@
add_subdirectory(airframes)
px4_add_romfs_files(
rc.airship_apps
rc.airship_defaults
rc.autostart_ext
rcS
rc.sensors
rc.vehicle_setup
# TODO
rc.balloon_apps
rc.balloon_defaults
rc.boat_defaults
rc.fw_apps
rc.fw_defaults
rc.heli_defaults
rc.logging
rc.mc_apps
rc.mc_defaults
rcS
rc.sensors
rc.thermal_cal
rc.rover_apps
rc.rover_defaults
rc.rover_differential_apps
rc.rover_differential_defaults
rc.uuv_apps
rc.uuv_defaults
rc.vehicle_setup
rc.vtol_apps
rc.vtol_defaults
)
if(CONFIG_MODULES_AIRSHIP_ATT_CONTROL)
px4_add_romfs_files(
rc.airship_apps
rc.airship_defaults
)
endif()
if(CONFIG_MODULES_FW_RATE_CONTROL)
px4_add_romfs_files(
rc.fw_apps
rc.fw_defaults
)
endif()
if(CONFIG_MODULES_MC_RATE_CONTROL)
px4_add_romfs_files(
rc.heli_defaults
rc.mc_apps
rc.mc_defaults
)
endif()
if(CONFIG_MODULES_ROVER_POS_CONTROL)
px4_add_romfs_files(
rc.rover_apps
rc.rover_defaults
rc.boat_defaults # hack
)
endif()
if(CONFIG_MODULES_DIFFERENTIAL_DRIVE)
px4_add_romfs_files(
rc.rover_differential_apps
rc.rover_differential_defaults
)
endif()
if(CONFIG_MODULES_UUV_ATT_CONTROL)
px4_add_romfs_files(
rc.uuv_apps
rc.uuv_defaults
)
endif()
if(CONFIG_MODULES_VTOL_ATT_CONTROL)
px4_add_romfs_files(
rc.vtol_apps
rc.vtol_defaults
)
endif()
if(CONFIG_MODULES_LOGGER)
px4_add_romfs_files(
rc.logging
)
endif()
if(CONFIG_MODULES_TEMPERATURE_COMPENSATION)
px4_add_romfs_files(
rc.thermal_cal
)
endif()

View File

@ -32,127 +32,94 @@
############################################################################
px4_add_romfs_files(
# [0-999] Reserved (historical)
# [0-999] Reserved (historical)"
# [1000, 1999] Simulation setups"
1001_rc_quad_x.hil
1002_standard_vtol.hil
1100_rc_quad_x_sih.hil
1101_rc_plane_sih.hil
1102_tailsitter_duo_sih.hil
# [2000, 2999] Standard planes"
2100_standard_plane
2106_albatross
2507_cloudship
# [3000, 3999] Flying wing"
3000_generic_wing
# [4000, 4999] Quadrotor x"
4001_quad_x
4014_s500
4015_holybro_s500
4016_holybro_px4vision
4017_nxp_hovergames
4019_x500_v2
4020_holybro_px4vision_v1_5
4041_beta75x
4050_generic_250
4052_holybro_qav250
4053_holybro_kopis2
4061_atl_mantis_edu
4071_ifo
4073_ifo-s
4500_clover4
4901_crazyflie21
# [5000, 5999] Quadrotor +"
5001_quad_+
# [6000, 6999] Hexarotor x"
6001_hexa_x
6002_draco_r
# [7000, 7999] Hexarotor +"
7001_hexa_+
# [8000, 8999] Octorotor +"
8001_octo_x
# [9000, 9999] Octorotor +"
9001_octo_+
# [11000, 11999] Hexa Cox
11001_hexa_cox
# [12000, 12999] Octo Cox
12001_octo_cox
# [13000, 13999] VTOL
13000_generic_vtol_standard
13100_generic_vtol_tiltrotor
13013_deltaquad
13014_vtol_babyshark
13030_generic_vtol_quad_tiltrotor
13200_generic_vtol_tailsitter
# [14000, 14999] MC with tilt
14001_generic_mc_with_tilt
16001_helicopter
# [17000, 17999] Autogyro
17002_TF-AutoG2
17003_TF-G2
# [18000, 18999] High-altitude balloons
18001_TF-B1
# [22000, 22999] Reserve for custom models
24001_dodeca_cox
50000_generic_ground_vehicle
50004_nxpcup_car_dfrobot_gpx
50003_aion_robotics_r1_rover
# [60000, 61000] (Unmanned) Underwater Robots
60000_uuv_generic
60001_uuv_hippocampus
60002_uuv_bluerov2_heavy
)
if(CONFIG_MODULES_SIMULATION_PWM_OUT_SIM)
px4_add_romfs_files(
# [1000, 1999] Simulation setups
1001_rc_quad_x.hil
1002_standard_vtol.hil
1100_rc_quad_x_sih.hil
1101_rc_plane_sih.hil
1102_tailsitter_duo_sih.hil
)
endif()
if(CONFIG_MODULES_MC_RATE_CONTROL)
px4_add_romfs_files(
# [4000, 4999] Quadrotor x
4001_quad_x
4014_s500
4015_holybro_s500
4016_holybro_px4vision
4017_nxp_hovergames
4019_x500_v2
4020_holybro_px4vision_v1_5
4041_beta75x
4050_generic_250
4052_holybro_qav250
4053_holybro_kopis2
4061_atl_mantis_edu
4071_ifo
4073_ifo-s
4500_clover4
4901_crazyflie21
# [5000, 5999] Quadrotor +
5001_quad_+
# [6000, 6999] Hexarotor x
6001_hexa_x
6002_draco_r
# [7000, 7999] Hexarotor +
7001_hexa_+
# [8000, 8999] Octorotor +
8001_octo_x
# [9000, 9999] Octorotor +
9001_octo_+
# [11000, 11999] Hexa Cox
11001_hexa_cox
# [12000, 12999] Octo Cox
12001_octo_cox
# [14000, 14999] MC with tilt
14001_generic_mc_with_tilt
16001_helicopter
24001_dodeca_cox
)
endif()
if(CONFIG_MODULES_FW_RATE_CONTROL)
px4_add_romfs_files(
# [2000, 2999] Standard planes
2100_standard_plane
2106_albatross
# [3000, 3999] Flying wing
3000_generic_wing
# [17000, 17999] Autogyro
17002_TF-AutoG2
17003_TF-G2
)
endif()
if(CONFIG_MODULES_AIRSHIP_ATT_CONTROL)
px4_add_romfs_files(
2507_cloudship
)
endif()
if(CONFIG_MODULES_VTOL_ATT_CONTROL)
px4_add_romfs_files(
# [13000, 13999] VTOL
13000_generic_vtol_standard
13100_generic_vtol_tiltrotor
13013_deltaquad
13014_vtol_babyshark
13030_generic_vtol_quad_tiltrotor
13200_generic_vtol_tailsitter
)
endif()
if(CONFIG_MODULES_ROVER_POS_CONTROL)
px4_add_romfs_files(
50000_generic_ground_vehicle
50004_nxpcup_car_dfrobot_gpx
)
endif()
if(CONFIG_MODULES_DIFFERENTIAL_DRIVE)
px4_add_romfs_files(
50003_aion_robotics_r1_rover
)
endif()
if(CONFIG_MODULES_UUV_ATT_CONTROL)
px4_add_romfs_files(
# [60000, 61000] (Unmanned) Underwater Robots
60000_uuv_generic
60001_uuv_hippocampus
60002_uuv_bluerov2_heavy
)
endif()

View File

@ -28,6 +28,7 @@ param set-default EKF2_ARSP_THR 8
param set-default EKF2_FUSE_BETA 1
param set-default EKF2_GPS_CHECK 21
param set-default EKF2_MAG_ACCLIM 0
param set-default EKF2_MAG_YAWLIM 0
param set-default EKF2_REQ_EPH 10
param set-default EKF2_REQ_EPV 10
param set-default EKF2_REQ_HDRIFT 0.5

View File

@ -5,7 +5,7 @@
ekf2 start &
# Start rover differential drive controller.
differential_drive start
differential_drive_control start
# Start Land Detector.
land_detector start rover

View File

@ -170,7 +170,7 @@ else
param select-backup $PARAM_BACKUP_FILE
fi
if mft query -q -k MFT -s MFT_ETHERNET -v 1
if ver hwcmp PX4_FMU_V5X PX4_FMU_V6X ARK_FMU_V6X PX4_FMU_V6XRT
then
netman update -i eth0
fi
@ -439,12 +439,7 @@ else
#
# Start a thermal calibration if required.
#
set RC_THERMAL_CAL ${R}etc/init.d/rc.thermal_cal
if [ -f ${RC_THERMAL_CAL} ]
then
. ${RC_THERMAL_CAL}
fi
unset RC_THERMAL_CAL
. ${R}etc/init.d/rc.thermal_cal
#
# Start gimbal to control mounts such as gimbals, disabled by default.
@ -505,12 +500,7 @@ else
#
# Start the logger.
#
set RC_LOGGING ${R}etc/init.d/rc.logging
if [ -f ${RC_LOGGING} ]
then
. ${RC_LOGGING}
fi
unset RC_LOGGING
. ${R}etc/init.d/rc.logging
#
# Set additional parameters and env variables for selected AUTOSTART.

View File

@ -30,5 +30,4 @@ exec find boards msg src platforms test \
-path src/lib/cdrstream/cyclonedds -prune -o \
-path src/lib/cdrstream/rosidl -prune -o \
-path src/modules/zenoh/zenoh-pico -prune -o \
-path boards/modalai/voxl2/libfc-sensor-api -prune -o \
-type f \( -name "*.c" -o -name "*.h" -o -name "*.cpp" -o -name "*.hpp" \) | grep $PATTERN

View File

@ -73,16 +73,9 @@ struct_size, padding_end_size = add_padding_bytes(sorted_fields, search_path)
#include <lib/matrix/matrix/math.hpp>
#include <lib/mathlib/mathlib.h>
@{
queue_length = 1
for constant in spec.constants:
if constant.name == 'ORB_QUEUE_LENGTH':
queue_length = constant.val
}@
@[for topic in topics]@
static_assert(static_cast<orb_id_size_t>(ORB_ID::@topic) == @(all_topics.index(topic)), "ORB_ID index mismatch");
ORB_DEFINE(@topic, struct @uorb_struct, @(struct_size-padding_end_size), @(message_hash)u, static_cast<orb_id_size_t>(ORB_ID::@topic), @queue_length);
ORB_DEFINE(@topic, struct @uorb_struct, @(struct_size-padding_end_size), @(message_hash)u, static_cast<orb_id_size_t>(ORB_ID::@topic));
@[end for]
void print_message(const orb_metadata *meta, const @uorb_struct& message)

@ -1 +1 @@
Subproject commit da7206e057703cc645770f02437013358b71e1c0
Subproject commit 33ac87a37676fb597de110e926bbf0a080169ffe

@ -1 +1 @@
Subproject commit 6b4ed09d1b495fbff663f098979cc046df013abd
Subproject commit c78f7f01417168e8faab7a83ade2129c0d26b39d

View File

@ -4,7 +4,7 @@
"description": "Firmware for the ARK flow board",
"image": "",
"build_time": 0,
"summary": "ARKFLOW",
"summary": "ARFFLOW",
"version": "0.1",
"image_size": 0,
"image_maxsize": 2080768,

View File

@ -22,12 +22,12 @@ param set-default SENS_IMU_TEMP 10.0
#param set-default SENS_IMU_TEMP_I 0.025
#param set-default SENS_IMU_TEMP_P 1.0
if ver hwtypecmp ARKV6X000
if ver hwtypecmp ARKV6X000000 ARKV6X001000 ARKV6X002000 ARKV6X003000 ARKV6X004000 ARKV6X005000 ARKV6X006000 ARKV6X007000
then
param set-default SENS_TEMP_ID 2818058
fi
if ver hwtypecmp ARKV6X001
if ver hwtypecmp ARKV6X000001 ARKV6X001001 ARKV6X002001 ARKV6X003001 ARKV6X004001 ARKV6X005001 ARKV6X006001 ARKV6X007001
then
param set-default SENS_TEMP_ID 3014666
fi

View File

@ -5,7 +5,7 @@
set HAVE_PM2 yes
set HAVE_PM3 yes
if mft query -q -k MFT -s MFT_PM2 -v 0
if ver hwtypecmp ARKV6X005000 ARKV6X005001 ARKV6X005002 ARKV6X005003 ARKV6X005004
then
set HAVE_PM2 no
set HAVE_PM3 no
@ -66,7 +66,7 @@ then
fi
fi
if ver hwtypecmp ARKV6X000
if ver hwtypecmp ARKV6X000000 ARKV6X001000 ARKV6X002000 ARKV6X003000 ARKV6X004000 ARKV6X005000 ARKV6X006000 ARKV6X007000
then
# Internal SPI bus IIM42652 with SPIX measured frequency of 32.051kHz
iim42652 -R 3 -s -b 1 -C 32051 start
@ -78,7 +78,7 @@ then
icm42688p -R 6 -s -b 3 -C 32051 start
fi
if ver hwtypecmp ARKV6X001
if ver hwtypecmp ARKV6X000001 ARKV6X001001 ARKV6X002001 ARKV6X003001 ARKV6X004001 ARKV6X005001 ARKV6X006001 ARKV6X007001
then
# Internal SPI bus IIM42653 with SPIX measured frequency of 32.051kHz
#iim42653 -R 3 -s -b 1 -C 32051 start

View File

@ -248,7 +248,7 @@
/* ADC 1 2 3 clock source */
#define STM32_RCC_D3CCIPR_ADCSRC RCC_D3CCIPR_ADCSEL_PLL2
#define STM32_RCC_D3CCIPR_ADCSEL RCC_D3CCIPR_ADCSEL_PLL2
/* FDCAN 1 2 clock source */
@ -380,7 +380,7 @@
#define GPIO_UART5_RX GPIO_UART5_RX_3 /* PD2 */
#define GPIO_UART5_TX GPIO_UART5_TX_3 /* PC12 */
// GPIO_UART5_RTS No remap /* PC8 */
#define GPIO_UART5_CTS (GPIO_ALT|GPIO_AF8|GPIO_PORTC|GPIO_PIN9|GPIO_PULLDOWN) /* PC9 */
// GPIO_UART5_CTS No remap /* PC9 */
#define GPIO_USART6_RX GPIO_USART6_RX_1 /* PC7 */
#define GPIO_USART6_TX GPIO_USART6_TX_1 /* PC6 */

View File

@ -295,14 +295,12 @@ CONFIG_UART5_IFLOWCONTROL=y
CONFIG_UART5_OFLOWCONTROL=y
CONFIG_UART5_RXDMA=y
CONFIG_UART5_TXBUFSIZE=10000
CONFIG_UART5_TXDMA=y
CONFIG_UART7_BAUD=57600
CONFIG_UART7_IFLOWCONTROL=y
CONFIG_UART7_OFLOWCONTROL=y
CONFIG_UART7_RXBUFSIZE=600
CONFIG_UART7_RXDMA=y
CONFIG_UART7_TXBUFSIZE=3000
CONFIG_UART7_TXDMA=y
CONFIG_UART8_BAUD=57600
CONFIG_UART8_RXBUFSIZE=600
CONFIG_UART8_TXBUFSIZE=1500
@ -319,7 +317,6 @@ CONFIG_USART3_RXBUFSIZE=180
CONFIG_USART3_RXDMA=y
CONFIG_USART3_SERIAL_CONSOLE=y
CONFIG_USART3_TXBUFSIZE=1500
CONFIG_USART3_TXDMA=y
CONFIG_USART6_BAUD=57600
CONFIG_USART6_RXBUFSIZE=600
CONFIG_USART6_TXBUFSIZE=1500

View File

@ -55,6 +55,7 @@ else()
init.c
led.c
mtd.cpp
manifest.c
sdio.c
spi.cpp
spix_sync.c

View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2016-2024 PX4 Development Team. All rights reserved.
* Copyright (c) 2016-2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -204,17 +204,25 @@
#define BOARD_ADC_OPEN_CIRCUIT_V (5.6f)
/* HW Version and Revision drive signals Default to 1 to detect */
#define BOARD_HAS_HW_SPLIT_VERSIONING
#define BOARD_HAS_HW_VERSIONING
#define GPIO_HW_VER_REV_DRIVE /* PG0 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTG|GPIO_PIN0)
#define GPIO_HW_REV_SENSE /* PH4 */ GPIO_ADC3_INP15
#define GPIO_HW_VER_SENSE /* PH3 */ GPIO_ADC3_INP14
#define HW_INFO_INIT_PREFIX "ARKV6X"
#define BOARD_NUM_SPI_CFG_HW_VERSIONS 2
#define BOARD_NUM_SPI_CFG_HW_VERSIONS 8 // Rev 0 and Rev 1
// Base/FMUM
#define ARKV6X_0 HW_FMUM_ID(0x0) // ARKV6X, Sensor Set Rev 0
#define ARKV6X_1 HW_FMUM_ID(0x1) // ARKV6X, Sensor Set Rev 1
#define ARKV6X00 HW_VER_REV(0x0,0x0) // ARKV6X, Sensor Set Rev 0
#define ARKV6X01 HW_VER_REV(0x0,0x1) // ARKV6X, Sensor Set Rev 1
//#define ARKV6X03 HW_VER_REV(0x0,0x3) // ARKV6X, Sensor Set Rev 3
//#define ARKV6X04 HW_VER_REV(0x0,0x4) // ARKV6X, Sensor Set Rev 4
#define ARKV6X10 HW_VER_REV(0x1,0x0) // NO PX4IO, Sensor Set Rev 0
#define ARKV6X11 HW_VER_REV(0x1,0x1) // NO PX4IO, Sensor Set Rev 1
#define ARKV6X40 HW_VER_REV(0x4,0x0) // ARKV6X, Sensor Set Rev 0 HB CM4 base Rev 3
#define ARKV6X41 HW_VER_REV(0x4,0x1) // ARKV6X, Sensor Set Rev 1 HB CM4 base Rev 4
#define ARKV6X50 HW_VER_REV(0x5,0x0) // ARKV6X, Sensor Set Rev 0 HB Mini Rev 5
#define ARKV6X51 HW_VER_REV(0x5,0x1) // ARKV6X, Sensor Set Rev 1 HB Mini Rev 1 // never shipped
#define UAVCAN_NUM_IFACES_RUNTIME 1

View File

@ -0,0 +1,216 @@
/****************************************************************************
*
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file manifest.c
*
* This module supplies the interface to the manifest of hardware that is
* optional and dependent on the HW REV and HW VER IDs
*
* The manifest allows the system to know whether a hardware option
* say for example the PX4IO is an no-pop option vs it is broken.
*
*/
/****************************************************************************
* Included Files
****************************************************************************/
#include <nuttx/config.h>
#include <board_config.h>
#include <inttypes.h>
#include <stdbool.h>
#include <syslog.h>
#include "systemlib/px4_macros.h"
/****************************************************************************
* Pre-Processor Definitions
****************************************************************************/
typedef struct {
uint32_t hw_ver_rev; /* the version and revision */
const px4_hw_mft_item_t *mft; /* The first entry */
uint32_t entries; /* the lenght of the list */
} px4_hw_mft_list_entry_t;
typedef px4_hw_mft_list_entry_t *px4_hw_mft_list_entry;
#define px4_hw_mft_list_uninitialized (px4_hw_mft_list_entry) -1
static const px4_hw_mft_item_t device_unsupported = {0, 0, 0};
// List of components on a specific board configuration
// The index of those components is given by the enum (px4_hw_mft_item_id_t)
// declared in board_common.h
static const px4_hw_mft_item_t hw_mft_list_v0600[] = {
{
// PX4_MFT_PX4IO
.present = 1,
.mandatory = 1,
.connection = px4_hw_con_onboard,
},
{
// PX4_MFT_USB
.present = 1,
.mandatory = 1,
.connection = px4_hw_con_onboard,
},
{
// PX4_MFT_CAN2
.present = 1,
.mandatory = 1,
.connection = px4_hw_con_onboard,
},
};
static const px4_hw_mft_item_t hw_mft_list_v0610[] = {
{
// PX4_MFT_PX4IO
.present = 0,
.mandatory = 0,
.connection = px4_hw_con_unknown,
},
{
// PX4_MFT_USB
.present = 1,
.mandatory = 1,
.connection = px4_hw_con_onboard,
},
{
// PX4_MFT_CAN2
.present = 1,
.mandatory = 1,
.connection = px4_hw_con_onboard,
},
};
static const px4_hw_mft_item_t hw_mft_list_v0640[] = {
{
// PX4_MFT_PX4IO
.present = 1,
.mandatory = 1,
.connection = px4_hw_con_onboard,
},
{
// PX4_MFT_USB
.present = 1,
.mandatory = 1,
.connection = px4_hw_con_onboard,
},
{
// PX4_MFT_CAN2
.present = 1,
.mandatory = 1,
.connection = px4_hw_con_onboard,
},
};
static const px4_hw_mft_item_t hw_mft_list_v0650[] = {
{
// PX4_MFT_PX4IO
.present = 1,
.mandatory = 1,
.connection = px4_hw_con_unknown,
},
{
// PX4_MFT_USB
.present = 1,
.mandatory = 1,
.connection = px4_hw_con_onboard,
},
{
// PX4_MFT_CAN2
.present = 0,
.mandatory = 0,
.connection = px4_hw_con_unknown,
},
};
static px4_hw_mft_list_entry_t mft_lists[] = {
// ver_rev
{ARKV6X00, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // ARKV6X, Sensor Set Rev 0
{ARKV6X01, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // ARKV6X, Sensor Set Rev 1
{ARKV6X10, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // NO PX4IO, Sensor Set Rev 0
{ARKV6X11, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // NO PX4IO, Sensor Set Rev 1
{ARKV6X40, hw_mft_list_v0640, arraySize(hw_mft_list_v0640)}, // ARKV6X, Sensor Set Rev 0 HB CM4 base Rev 3
{ARKV6X41, hw_mft_list_v0640, arraySize(hw_mft_list_v0640)}, // ARKV6X, Sensor Set Rev 1 HB CM4 base Rev 4
{ARKV6X50, hw_mft_list_v0650, arraySize(hw_mft_list_v0650)}, // ARKV6X, Sensor Set Rev 0 HB Mini Rev 5
{ARKV6X51, hw_mft_list_v0650, arraySize(hw_mft_list_v0650)}, // ARKV6X, Sensor Set Rev 1 HB Mini Rev 1 // never shipped
};
/************************************************************************************
* Name: board_query_manifest
*
* Description:
* Optional returns manifest item.
*
* Input Parameters:
* manifest_id - the ID for the manifest item to retrieve
*
* Returned Value:
* 0 - item is not in manifest => assume legacy operations
* pointer to a manifest item
*
************************************************************************************/
__EXPORT px4_hw_mft_item board_query_manifest(px4_hw_mft_item_id_t id)
{
static px4_hw_mft_list_entry boards_manifest = px4_hw_mft_list_uninitialized;
if (boards_manifest == px4_hw_mft_list_uninitialized) {
uint32_t ver_rev = board_get_hw_version() << 16;
ver_rev |= board_get_hw_revision();
for (unsigned i = 0; i < arraySize(mft_lists); i++) {
if (mft_lists[i].hw_ver_rev == ver_rev) {
boards_manifest = &mft_lists[i];
break;
}
}
if (boards_manifest == px4_hw_mft_list_uninitialized) {
syslog(LOG_ERR, "[boot] Board %08" PRIx32 " is not supported!\n", ver_rev);
}
}
px4_hw_mft_item rv = &device_unsupported;
if (boards_manifest != px4_hw_mft_list_uninitialized &&
id < boards_manifest->entries) {
rv = &boards_manifest->mft[id];
}
return rv;
}

View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2024 PX4 Development Team. All rights reserved.
* Copyright (C) 2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -31,9 +31,6 @@
*
****************************************************************************/
#include <nuttx/config.h>
#include <board_config.h>
#include <nuttx/spi/spi.h>
#include <px4_platform_common/px4_manifest.h>
// KiB BS nB
@ -89,16 +86,10 @@ static const px4_mft_entry_s mtd_mft = {
.pmft = (void *) &board_mtd_config,
};
static const px4_mft_entry_s mft_mft = {
.type = MFT,
.pmft = (void *) system_query_manifest,
};
static const px4_mft_s mft = {
.nmft = 2,
.nmft = 1,
.mfts = {
&mtd_mft,
&mft_mft,
&mtd_mft
}
};

View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2024 PX4 Development Team. All rights reserved.
* Copyright (C) 2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -36,7 +36,7 @@
#include <nuttx/spi/spi.h>
constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSIONS] = {
initSPIFmumID(ARKV6X_0, {
initSPIHWVersion(ARKV6X00, {
initSPIBus(SPI::Bus::SPI1, {
initSPIDevice(DRV_IMU_DEVTYPE_IIM42652, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}),
}, {GPIO::PortI, GPIO::Pin11}),
@ -59,7 +59,145 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION
}),
}),
initSPIFmumID(ARKV6X_1, {
initSPIHWVersion(ARKV6X01, {
initSPIBus(SPI::Bus::SPI1, {
initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}),
}, {GPIO::PortI, GPIO::Pin11}),
initSPIBus(SPI::Bus::SPI2, {
initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}),
}, {GPIO::PortF, GPIO::Pin4}),
initSPIBus(SPI::Bus::SPI3, {
initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin6}),
}, {GPIO::PortE, GPIO::Pin7}),
// initSPIBus(SPI::Bus::SPI4, {
// // no devices
// TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h
// }, {GPIO::PortG, GPIO::Pin8}),
initSPIBus(SPI::Bus::SPI5, {
initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7})
}),
initSPIBusExternal(SPI::Bus::SPI6, {
initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}),
initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}),
}),
}),
initSPIHWVersion(ARKV6X10, {
initSPIBus(SPI::Bus::SPI1, {
initSPIDevice(DRV_IMU_DEVTYPE_IIM42652, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}),
}, {GPIO::PortI, GPIO::Pin11}),
initSPIBus(SPI::Bus::SPI2, {
initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}),
}, {GPIO::PortF, GPIO::Pin4}),
initSPIBus(SPI::Bus::SPI3, {
initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin6}),
}, {GPIO::PortE, GPIO::Pin7}),
// initSPIBus(SPI::Bus::SPI4, {
// // no devices
// TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h
// }, {GPIO::PortG, GPIO::Pin8}),
initSPIBus(SPI::Bus::SPI5, {
initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7})
}),
initSPIBusExternal(SPI::Bus::SPI6, {
initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}),
initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}),
}),
}),
initSPIHWVersion(ARKV6X11, {
initSPIBus(SPI::Bus::SPI1, {
initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}),
}, {GPIO::PortI, GPIO::Pin11}),
initSPIBus(SPI::Bus::SPI2, {
initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}),
}, {GPIO::PortF, GPIO::Pin4}),
initSPIBus(SPI::Bus::SPI3, {
initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin6}),
}, {GPIO::PortE, GPIO::Pin7}),
// initSPIBus(SPI::Bus::SPI4, {
// // no devices
// TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h
// }, {GPIO::PortG, GPIO::Pin8}),
initSPIBus(SPI::Bus::SPI5, {
initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7})
}),
initSPIBusExternal(SPI::Bus::SPI6, {
initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}),
initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}),
}),
}),
initSPIHWVersion(ARKV6X40, {
initSPIBus(SPI::Bus::SPI1, {
initSPIDevice(DRV_IMU_DEVTYPE_IIM42652, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}),
}, {GPIO::PortI, GPIO::Pin11}),
initSPIBus(SPI::Bus::SPI2, {
initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}),
}, {GPIO::PortF, GPIO::Pin4}),
initSPIBus(SPI::Bus::SPI3, {
initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin6}),
}, {GPIO::PortE, GPIO::Pin7}),
// initSPIBus(SPI::Bus::SPI4, {
// // no devices
// TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h
// }, {GPIO::PortG, GPIO::Pin8}),
initSPIBus(SPI::Bus::SPI5, {
initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7})
}),
initSPIBusExternal(SPI::Bus::SPI6, {
initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}),
initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}),
}),
}),
initSPIHWVersion(ARKV6X41, {
initSPIBus(SPI::Bus::SPI1, {
initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}),
}, {GPIO::PortI, GPIO::Pin11}),
initSPIBus(SPI::Bus::SPI2, {
initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}),
}, {GPIO::PortF, GPIO::Pin4}),
initSPIBus(SPI::Bus::SPI3, {
initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin6}),
}, {GPIO::PortE, GPIO::Pin7}),
// initSPIBus(SPI::Bus::SPI4, {
// // no devices
// TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h
// }, {GPIO::PortG, GPIO::Pin8}),
initSPIBus(SPI::Bus::SPI5, {
initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7})
}),
initSPIBusExternal(SPI::Bus::SPI6, {
initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}),
initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}),
}),
}),
initSPIHWVersion(ARKV6X50, {
initSPIBus(SPI::Bus::SPI1, {
initSPIDevice(DRV_IMU_DEVTYPE_IIM42652, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}),
}, {GPIO::PortI, GPIO::Pin11}),
initSPIBus(SPI::Bus::SPI2, {
initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}),
}, {GPIO::PortF, GPIO::Pin4}),
initSPIBus(SPI::Bus::SPI3, {
initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin6}),
}, {GPIO::PortE, GPIO::Pin7}),
// initSPIBus(SPI::Bus::SPI4, {
// // no devices
// TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h
// }, {GPIO::PortG, GPIO::Pin8}),
initSPIBus(SPI::Bus::SPI5, {
initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7})
}),
initSPIBusExternal(SPI::Bus::SPI6, {
initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}),
initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}),
}),
}),
initSPIHWVersion(ARKV6X51, {
initSPIBus(SPI::Bus::SPI1, {
initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}),
}, {GPIO::PortI, GPIO::Pin11}),

View File

@ -1,5 +0,0 @@
CONFIG_BOARD_TOOLCHAIN="arm-none-eabi"
CONFIG_BOARD_ARCHITECTURE="cortex-m4"
CONFIG_BOARD_ROMFSROOT=""
CONFIG_BOARD_CONSTRAINED_MEMORY=y
CONFIG_DRIVERS_BOOTLOADERS=y

View File

@ -1,38 +0,0 @@
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

View File

@ -1,13 +0,0 @@
{
"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
}

View File

@ -1,15 +0,0 @@
#!/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

View File

@ -1,11 +0,0 @@
#!/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

View File

@ -1,57 +0,0 @@
#
# 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

View File

@ -1,152 +0,0 @@
/************************************************************************************
* 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 */

View File

@ -1,46 +0,0 @@
/****************************************************************************
*
* 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

View File

@ -1,153 +0,0 @@
#
# 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

View File

@ -1,134 +0,0 @@
/****************************************************************************
* 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) }
}

View File

@ -1,146 +0,0 @@
/****************************************************************************
* 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) }
}

View File

@ -1,67 +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.
#
############################################################################
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()

View File

@ -1,125 +0,0 @@
/****************************************************************************
*
* 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

View File

@ -1,188 +0,0 @@
/****************************************************************************
*
* 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);
}

View File

@ -1,130 +0,0 @@
/****************************************************************************
*
* 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)

View File

@ -1,130 +0,0 @@
/****************************************************************************
*
* 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

View File

@ -1,39 +0,0 @@
/****************************************************************************
*
* 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),
};

View File

@ -1,168 +0,0 @@
/****************************************************************************
*
* 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;
}

View File

@ -1,124 +0,0 @@
/****************************************************************************
*
* 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));
}

View File

@ -1,37 +0,0 @@
/****************************************************************************
*
* 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

View File

@ -1,44 +0,0 @@
/****************************************************************************
*
* 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);

View File

@ -1,17 +0,0 @@
# 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}
)

View File

@ -64,6 +64,9 @@
// Hacks for MAVLink RC button input
#define ATL_MANTIS_RC_INPUT_HACKS
// Hacks for MAVLink RC button input
#define ATL_MANTIS_RC_INPUT_HACKS
/*
* ADC channels
*

View File

@ -192,7 +192,7 @@
#define STM32_RCC_D2CCIP1R_SPI45SRC RCC_D2CCIP1R_SPI45SEL_PLL2 /* SPI45 clock source */
#define STM32_RCC_D3CCIPR_SPI6SRC RCC_D3CCIPR_SPI6SEL_PLL2 /* SPI6 clock source */
#define STM32_RCC_D2CCIP2R_USBSRC RCC_D2CCIP2R_USBSEL_PLL3 /* USB 1 and 2 clock source */
#define STM32_RCC_D3CCIPR_ADCSRC RCC_D3CCIPR_ADCSEL_PLL2 /* ADC 1 2 3 clock source */
#define STM32_RCC_D3CCIPR_ADCSEL RCC_D3CCIPR_ADCSEL_PLL2 /* ADC 1 2 3 clock source */
#define STM32_RCC_D2CCIP1R_FDCANSEL RCC_D2CCIP1R_FDCANSEL_HSE /* FDCAN 1 2 clock source */
#define STM32_FDCANCLK STM32_HSE_FREQUENCY

View File

@ -192,7 +192,7 @@
#define STM32_RCC_D2CCIP1R_SPI45SRC RCC_D2CCIP1R_SPI45SEL_PLL2 /* SPI45 clock source */
#define STM32_RCC_D3CCIPR_SPI6SRC RCC_D3CCIPR_SPI6SEL_PLL2 /* SPI6 clock source */
#define STM32_RCC_D2CCIP2R_USBSRC RCC_D2CCIP2R_USBSEL_PLL3 /* USB 1 and 2 clock source */
#define STM32_RCC_D3CCIPR_ADCSRC RCC_D3CCIPR_ADCSEL_PLL2 /* ADC 1 2 3 clock source */
#define STM32_RCC_D3CCIPR_ADCSEL RCC_D3CCIPR_ADCSEL_PLL2 /* ADC 1 2 3 clock source */
#define STM32_RCC_D2CCIP1R_FDCANSEL RCC_D2CCIP1R_FDCANSEL_HSE /* FDCAN 1 2 clock source */
#define STM32_FDCANCLK STM32_HSE_FREQUENCY

View File

@ -192,7 +192,7 @@
#define STM32_RCC_D2CCIP2R_USBSRC RCC_D2CCIP2R_USBSEL_PLL3 /* USB 1 and 2 clock source */
#define STM32_RCC_D2CCIP1R_FDCANSEL RCC_D2CCIP1R_FDCANSEL_HSE /* FDCAN 1 2 clock source */
#define STM32_RCC_D3CCIPR_ADCSRC RCC_D3CCIPR_ADCSEL_PLL2 /* ADC 1 2 3 clock source */
#define STM32_RCC_D3CCIPR_ADCSEL RCC_D3CCIPR_ADCSEL_PLL2 /* ADC 1 2 3 clock source */
/* FLASH wait states */
#define BOARD_FLASH_WAITSTATES 2

View File

@ -193,7 +193,7 @@
#define STM32_RCC_D2CCIP2R_USBSRC RCC_D2CCIP2R_USBSEL_PLL3 /* USB 1 and 2 clock source */
#define STM32_RCC_D2CCIP1R_FDCANSEL RCC_D2CCIP1R_FDCANSEL_HSE /* FDCAN 1 2 clock source */
#define STM32_RCC_D3CCIPR_ADCSRC RCC_D3CCIPR_ADCSEL_PLL2 /* ADC 1 2 3 clock source */
#define STM32_RCC_D3CCIPR_ADCSEL RCC_D3CCIPR_ADCSEL_PLL2 /* ADC 1 2 3 clock source */
/* FLASH wait states */
#define BOARD_FLASH_WAITSTATES 2

View File

@ -248,7 +248,7 @@
/* ADC 1 2 3 clock source */
#define STM32_RCC_D3CCIPR_ADCSRC RCC_D3CCIPR_ADCSEL_PLL2
#define STM32_RCC_D3CCIPR_ADCSEL RCC_D3CCIPR_ADCSEL_PLL2
/* FDCAN 1 2 clock source */

View File

@ -245,7 +245,7 @@
/* ADC 1 2 3 clock source */
#define STM32_RCC_D3CCIPR_ADCSRC RCC_D3CCIPR_ADCSEL_PLL2
#define STM32_RCC_D3CCIPR_ADCSEL RCC_D3CCIPR_ADCSEL_PLL2
/* FDCAN 1 2 clock source */

View File

@ -245,7 +245,7 @@
/* ADC 1 2 3 clock source */
#define STM32_RCC_D3CCIPR_ADCSRC RCC_D3CCIPR_ADCSEL_PLL2
#define STM32_RCC_D3CCIPR_ADCSEL RCC_D3CCIPR_ADCSEL_PLL2
/* FDCAN 1 2 clock source */

View File

@ -245,7 +245,7 @@
/* ADC 1 2 3 clock source */
#define STM32_RCC_D3CCIPR_ADCSRC RCC_D3CCIPR_ADCSEL_PLL2
#define STM32_RCC_D3CCIPR_ADCSEL RCC_D3CCIPR_ADCSEL_PLL2
/* FDCAN 1 2 clock source */

View File

@ -248,7 +248,7 @@
/* ADC 1 2 3 clock source */
#define STM32_RCC_D3CCIPR_ADCSRC RCC_D3CCIPR_ADCSEL_PLL2
#define STM32_RCC_D3CCIPR_ADCSEL RCC_D3CCIPR_ADCSEL_PLL2
/* FDCAN 1 clock source */

View File

@ -248,7 +248,7 @@
/* ADC 1 2 3 clock source */
#define STM32_RCC_D3CCIPR_ADCSRC RCC_D3CCIPR_ADCSEL_PLL2
#define STM32_RCC_D3CCIPR_ADCSEL RCC_D3CCIPR_ADCSEL_PLL2
/* FDCAN 1 clock source */

View File

@ -248,7 +248,7 @@
/* ADC 1 2 3 clock source */
#define STM32_RCC_D3CCIPR_ADCSRC RCC_D3CCIPR_ADCSEL_PLL2
#define STM32_RCC_D3CCIPR_ADCSEL RCC_D3CCIPR_ADCSEL_PLL2
/* FDCAN 1 clock source */

View File

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

View File

@ -248,7 +248,7 @@
/* ADC 1 2 3 clock source */
#define STM32_RCC_D3CCIPR_ADCSRC RCC_D3CCIPR_ADCSEL_PLL2
#define STM32_RCC_D3CCIPR_ADCSEL RCC_D3CCIPR_ADCSEL_PLL2
/* FDCAN 1 2 clock source */

View File

@ -4,14 +4,13 @@ CONFIG_DRIVERS_ACTUATORS_VOXL_ESC=y
CONFIG_DRIVERS_BAROMETER_INVENSENSE_ICP101XX=y
CONFIG_DRIVERS_DISTANCE_SENSOR_VL53L0X=y
CONFIG_DRIVERS_DISTANCE_SENSOR_VL53L1X=y
CONFIG_DRIVERS_GPS=y
CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
CONFIG_DRIVERS_LIGHTS_RGBLED_NCP5623C=y
CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8308=y
CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y
CONFIG_DRIVERS_MAGNETOMETER_QMC5883L=y
CONFIG_DRIVERS_POWER_MONITOR_VOXLPM=y
CONFIG_DRIVERS_QSHELL_QURT=y
CONFIG_DRIVERS_VOXL2_IO=y
CONFIG_MODULES_COMMANDER=y
CONFIG_MODULES_CONTROL_ALLOCATOR=y
CONFIG_MODULES_EKF2=y
@ -29,6 +28,4 @@ CONFIG_MODULES_RC_UPDATE=y
CONFIG_MODULES_SENSORS=y
CONFIG_MODULES_SIMULATION_PWM_OUT_SIM=y
CONFIG_SYSTEMCMDS_UORB=y
CONFIG_SYSTEMCMDS_PARAM=y
CONFIG_ORB_COMMUNICATOR=y
CONFIG_PARAM_REMOTE=y

View File

@ -44,10 +44,11 @@ add_library(drivers_board
)
# Add custom drivers for SLPI
# add_subdirectory(${PX4_BOARD_DIR}/src/drivers/icm42688p)
add_subdirectory(${PX4_BOARD_DIR}/src/drivers/rc_controller)
add_subdirectory(${PX4_BOARD_DIR}/src/drivers/mavlink_rc_in)
# add_subdirectory(${PX4_BOARD_DIR}/src/drivers/spektrum_rc)
# add_subdirectory(${PX4_BOARD_DIR}/src/drivers/ghst_rc)
add_subdirectory(${PX4_BOARD_DIR}/src/drivers/dsp_hitl)
# add_subdirectory(${PX4_BOARD_DIR}/src/drivers/dsp_hitl)
# add_subdirectory(${PX4_BOARD_DIR}/src/drivers/dsp_sbus)
add_subdirectory(${PX4_BOARD_DIR}/src/drivers/elrs_led)

View File

@ -62,8 +62,6 @@
#include <uORB/topics/vehicle_odometry.h>
#include <uORB/topics/sensor_baro.h>
#include <uORB/topics/esc_status.h>
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/sensor_optical_flow.h>
#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
@ -90,22 +88,21 @@ static bool _is_running = false;
volatile bool _task_should_exit = false;
static px4_task_t _task_handle = -1;
int _uart_fd = -1;
bool _debug = false;
bool debug = false;
std::string port = "2";
int baudrate = 921600;
const unsigned mode_flag_custom = 1;
const unsigned mode_flag_armed = 128;
bool _send_gps = false;
bool _send_mag = false;
bool _send_distance = false;
uORB::Publication<battery_status_s> _battery_pub{ORB_ID(battery_status)};
uORB::PublicationMulti<sensor_gps_s> _sensor_gps_pub{ORB_ID(sensor_gps)};
uORB::Publication<differential_pressure_s> _differential_pressure_pub{ORB_ID(differential_pressure)};
uORB::Publication<vehicle_odometry_s> _visual_odometry_pub{ORB_ID(vehicle_visual_odometry)};
uORB::Publication<vehicle_odometry_s> _mocap_odometry_pub{ORB_ID(vehicle_mocap_odometry)};
uORB::PublicationMulti<sensor_baro_s> _sensor_baro_pub{ORB_ID(sensor_baro)};
uORB::Publication<esc_status_s> _esc_status_pub{ORB_ID(esc_status)};
uORB::Publication<distance_sensor_s> _distance_sensor_pub{ORB_ID(distance_sensor)};
uORB::Publication<sensor_optical_flow_s> _sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)};
uORB::Subscription _battery_status_sub{ORB_ID(battery_status)};
int32_t _output_functions[actuator_outputs_s::NUM_ACTUATOR_OUTPUTS] {};
@ -131,42 +128,13 @@ float x_gyro = 0;
float y_gyro = 0;
float z_gyro = 0;
uint64_t gyro_accel_time = 0;
bool _use_software_mav_throttling{false};
// Status counters
uint32_t heartbeat_received_counter = 0;
uint32_t heartbeat_sent_counter = 0;
uint32_t imu_counter = 0;
uint32_t hil_sensor_counter = 0;
uint32_t mag_counter = 0;
uint32_t baro_counter = 0;
uint32_t actuator_sent_counter = 0;
uint32_t odometry_received_counter = 0;
uint32_t odometry_sent_counter = 0;
uint32_t gps_received_counter = 0;
uint32_t gps_sent_counter = 0;
uint32_t distance_received_counter = 0;
uint32_t distance_sent_counter = 0;
uint32_t flow_received_counter = 0;
uint32_t flow_sent_counter = 0;
uint32_t unknown_msg_received_counter = 0;
enum class position_source {GPS, VIO, FLOW, NUM_POSITION_SOURCES};
struct position_source_data_s {
char label[8];
bool send;
bool fail;
uint32_t failure_duration;
uint64_t failure_duration_start;
} position_source_data[(int) position_source::NUM_POSITION_SOURCES] = {
{"GPS", false, false, 0, 0},
{"VIO", false, false, 0, 0},
{"FLOW", false, false, 0, 0}
};
uint64_t first_sensor_msg_timestamp = 0;
uint64_t first_sensor_report_timestamp = 0;
uint64_t last_sensor_report_timestamp = 0;
int heartbeat_counter = 0;
int imu_counter = 0;
int hil_sensor_counter = 0;
int vision_msg_counter = 0;
int gps_counter = 0;
vehicle_status_s _vehicle_status{};
vehicle_control_mode_s _control_mode{};
@ -176,6 +144,7 @@ battery_status_s _battery_status{};
sensor_accel_fifo_s accel_fifo{};
sensor_gyro_fifo_s gyro_fifo{};
int openPort(const char *dev, speed_t speed);
int closePort();
@ -184,8 +153,7 @@ int writeResponse(void *buf, size_t len);
int start(int argc, char *argv[]);
int stop();
void print_status();
void clear_status_counters();
int get_status();
bool isOpen() { return _uart_fd >= 0; };
void usage();
@ -195,65 +163,50 @@ void *send_actuator(void *);
void send_actuator_data();
void handle_message_hil_sensor_dsp(mavlink_message_t *msg);
void handle_message_hil_optical_flow(mavlink_message_t *msg);
void handle_message_distance_sensor(mavlink_message_t *msg);
void handle_message_hil_gps_dsp(mavlink_message_t *msg);
void handle_message_odometry_dsp(mavlink_message_t *msg);
void handle_message_vision_position_estimate_dsp(mavlink_message_t *msg);
void handle_message_command_long_dsp(mavlink_message_t *msg);
void handle_message_dsp(mavlink_message_t *msg);
void actuator_controls_from_outputs_dsp(mavlink_hil_actuator_controls_t *msg);
void send_esc_status(mavlink_hil_actuator_controls_t hil_act_control);
void send_esc_telemetry_dsp(mavlink_hil_actuator_controls_t hil_act_control);
void
handle_message_dsp(mavlink_message_t *msg)
{
switch (msg->msgid) {
case MAVLINK_MSG_ID_HIL_SENSOR:
hil_sensor_counter++;
handle_message_hil_sensor_dsp(msg);
break;
case MAVLINK_MSG_ID_HIL_GPS:
gps_received_counter++;
if (_send_gps) { handle_message_hil_gps_dsp(msg); }
if (position_source_data[(int) position_source::GPS].send) { handle_message_hil_gps_dsp(msg); }
break;
case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE:
handle_message_vision_position_estimate_dsp(msg);
break;
case MAVLINK_MSG_ID_ODOMETRY:
odometry_received_counter++;
if (position_source_data[(int) position_source::VIO].send) { handle_message_odometry_dsp(msg); }
handle_message_odometry_dsp(msg);
break;
case MAVLINK_MSG_ID_COMMAND_LONG:
handle_message_command_long_dsp(msg);
break;
case MAVLINK_MSG_ID_HEARTBEAT:
heartbeat_received_counter++;
if (_debug) { PX4_INFO("Heartbeat msg received"); }
PX4_DEBUG("Heartbeat msg received");
break;
case MAVLINK_MSG_ID_HIL_OPTICAL_FLOW:
flow_received_counter++;
if (position_source_data[(int) position_source::FLOW].send) { handle_message_hil_optical_flow(msg); }
break;
case MAVLINK_MSG_ID_DISTANCE_SENSOR:
distance_received_counter++;
if (_send_distance) { handle_message_distance_sensor(msg); }
case MAVLINK_MSG_ID_SYSTEM_TIME:
PX4_DEBUG("MAVLINK SYSTEM TIME");
break;
default:
unknown_msg_received_counter++;
if (_debug) { PX4_INFO("Unknown msg ID: %d", msg->msgid); }
PX4_DEBUG("Unknown msg ID: %d", msg->msgid);
break;
}
}
@ -275,6 +228,7 @@ void send_actuator_data()
bool first_sent = false;
while (true) {
bool controls_updated = false;
(void) orb_check(_vehicle_control_mode_sub_, &controls_updated);
@ -285,50 +239,45 @@ void send_actuator_data()
bool actuator_updated = false;
(void) orb_check(_actuator_outputs_sub, &actuator_updated);
uint8_t newBuf[512];
uint16_t newBufLen = 0;
mavlink_hil_actuator_controls_t hil_act_control;
actuator_controls_from_outputs_dsp(&hil_act_control);
mavlink_message_t message{};
mavlink_msg_hil_actuator_controls_encode(1, 1, &message, &hil_act_control);
if (actuator_updated) {
orb_copy(ORB_ID(actuator_outputs), _actuator_outputs_sub, &_actuator_outputs);
px4_lockstep_wait_for_components();
if (_actuator_outputs.timestamp > 0) {
mavlink_hil_actuator_controls_t hil_act_control;
actuator_controls_from_outputs_dsp(&hil_act_control);
mavlink_message_t message{};
mavlink_msg_hil_actuator_controls_encode(1, 1, &message, &hil_act_control);
previous_timestamp = _actuator_outputs.timestamp;
previous_uorb_timestamp = _actuator_outputs.timestamp;
uint8_t newBuf[512];
uint16_t newBufLen = 0;
newBufLen = mavlink_msg_to_send_buffer(newBuf, &message);
int writeRetval = writeResponse(&newBuf, newBufLen);
actuator_sent_counter++;
if (_debug) { PX4_INFO("Succesful write of actuator back to jMAVSim: %d at %llu", writeRetval, hrt_absolute_time()); }
PX4_DEBUG("Succesful write of actuator back to jMAVSim: %d at %llu", writeRetval, hrt_absolute_time());
first_sent = true;
send_esc_status(hil_act_control);
send_esc_telemetry_dsp(hil_act_control);
}
} else if (! actuator_updated && first_sent && differential > 4000) {
} else if (!actuator_updated && first_sent && differential > 4000) {
mavlink_hil_actuator_controls_t hil_act_control;
actuator_controls_from_outputs_dsp(&hil_act_control);
previous_timestamp = hrt_absolute_time();
mavlink_message_t message{};
mavlink_msg_hil_actuator_controls_encode(1, 1, &message, &hil_act_control);
uint8_t newBuf[512];
uint16_t newBufLen = 0;
newBufLen = mavlink_msg_to_send_buffer(newBuf, &message);
int writeRetval = writeResponse(&newBuf, newBufLen);
//PX4_INFO("Sending from NOT UPDTE AND TIMEOUT: %i", differential);
actuator_sent_counter++;
if (_debug) { PX4_INFO("Succesful write of actuator back to jMAVSim: %d at %llu", writeRetval, hrt_absolute_time()); }
send_esc_status(hil_act_control);
PX4_DEBUG("Succesful write of actuator back to jMAVSim: %d at %llu", writeRetval, hrt_absolute_time());
send_esc_telemetry_dsp(hil_act_control);
}
differential = hrt_absolute_time() - previous_timestamp;
px4_usleep(1000);
}
}
@ -338,10 +287,14 @@ void task_main(int argc, char *argv[])
int myoptind = 1;
const char *myoptarg = nullptr;
while ((ch = px4_getopt(argc, argv, "odmghfp:b:", &myoptind, &myoptarg)) != EOF) {
while ((ch = px4_getopt(argc, argv, "vsdcmgp:b:", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
case 's':
_use_software_mav_throttling = true;
break;
case 'd':
_debug = true;
debug = true;
break;
case 'p':
@ -357,19 +310,7 @@ void task_main(int argc, char *argv[])
break;
case 'g':
position_source_data[(int) position_source::GPS].send = true;
break;
case 'o':
position_source_data[(int) position_source::VIO].send = true;
break;
case 'h':
_send_distance = true;
break;
case 'f':
position_source_data[(int) position_source::FLOW].send = true;
_send_gps = true;
break;
default:
@ -378,13 +319,11 @@ void task_main(int argc, char *argv[])
}
const char *charport = port.c_str();
(void) openPort(charport, (speed_t) baudrate);
int openRetval = openPort(charport, (speed_t) baudrate);
int open = isOpen();
if ((_debug) && (isOpen())) { PX4_INFO("DSP HITL serial port initialized. Baudrate: %d", baudrate); }
if (! isOpen()) {
PX4_ERR("DSP HITL failed to open serial port");
return;
if (open) {
PX4_ERR("Port is open: %d", openRetval);
}
uint64_t last_heartbeat_timestamp = hrt_absolute_time();
@ -403,11 +342,14 @@ void task_main(int argc, char *argv[])
pthread_attr_destroy(&sender_thread_attr);
int _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
PX4_INFO("Got %d from orb_subscribe", _vehicle_status_sub);
_is_running = true;
while (!_task_should_exit) {
uint8_t rx_buf[1024];
//rx_buf[511] = '\0';
uint64_t timestamp = hrt_absolute_time();
@ -415,8 +357,8 @@ void task_main(int argc, char *argv[])
if (got_first_sensor_msg) {
uint64_t delta_time = timestamp - last_imu_update_timestamp;
if ((imu_counter) && (delta_time > 15000)) {
PX4_WARN("Sending updates at %llu, delta %llu", timestamp, delta_time);
if (delta_time > 15000) {
PX4_ERR("Sending updates at %llu, delta %llu", timestamp, delta_time);
}
uint64_t _px4_gyro_accel_timestamp = hrt_absolute_time();
@ -454,7 +396,7 @@ void task_main(int argc, char *argv[])
hb_newBufLen = mavlink_msg_to_send_buffer(hb_newBuf, &hb_message);
(void) writeResponse(&hb_newBuf, hb_newBufLen);
last_heartbeat_timestamp = timestamp;
heartbeat_sent_counter++;
heartbeat_counter++;
}
bool vehicle_updated = false;
@ -474,7 +416,7 @@ void task_main(int argc, char *argv[])
_is_running = false;
}
void send_esc_status(mavlink_hil_actuator_controls_t hil_act_control)
void send_esc_telemetry_dsp(mavlink_hil_actuator_controls_t hil_act_control)
{
esc_status_s esc_status{};
esc_status.timestamp = hrt_absolute_time();
@ -506,13 +448,17 @@ void send_esc_status(mavlink_hil_actuator_controls_t hil_act_control)
_esc_status_pub.publish(esc_status);
}
void
handle_message_command_long_dsp(mavlink_message_t *msg)
{
/* command */
mavlink_command_long_t cmd_mavlink;
mavlink_msg_command_long_decode(msg, &cmd_mavlink);
if (_debug) { PX4_INFO("Value of command_long.command: %d", cmd_mavlink.command); }
if (debug) {
PX4_INFO("Value of command_long.command: %d", cmd_mavlink.command);
}
mavlink_command_ack_t ack = {};
ack.result = MAV_RESULT_UNSUPPORTED;
@ -524,140 +470,46 @@ handle_message_command_long_dsp(mavlink_message_t *msg)
uint16_t acknewBufLen = 0;
acknewBufLen = mavlink_msg_to_send_buffer(acknewBuf, &ack_message);
int writeRetval = writeResponse(&acknewBuf, acknewBufLen);
if (_debug) { PX4_INFO("Succesful write of ACK back over UART: %d at %llu", writeRetval, hrt_absolute_time()); }
PX4_INFO("Succesful write of ACK back over UART: %d at %llu", writeRetval, hrt_absolute_time());
}
int flow_debug_counter = 0;
void
handle_message_hil_optical_flow(mavlink_message_t *msg)
handle_message_vision_position_estimate_dsp(mavlink_message_t *msg)
{
mavlink_hil_optical_flow_t flow;
mavlink_msg_hil_optical_flow_decode(msg, &flow);
mavlink_vision_position_estimate_t vpe;
mavlink_msg_vision_position_estimate_decode(msg, &vpe);
if ((_debug) && (!(flow_debug_counter % 10))) {
PX4_INFO("optflow: time: %llu, quality %d", flow.time_usec, (int) flow.quality);
PX4_INFO("optflow: x: %.2f y: %.2f", (double) flow.integrated_x, (double) flow.integrated_y);
}
// fill vehicle_odometry from Mavlink VISION_POSITION_ESTIMATE
vehicle_odometry_s odom{};
uint64_t timestamp = hrt_absolute_time();
odom.timestamp_sample = timestamp;
flow_debug_counter++;
odom.pose_frame = vehicle_odometry_s::POSE_FRAME_NED;
odom.position[0] = vpe.x;
odom.position[1] = vpe.y;
odom.position[2] = vpe.z;
device::Device::DeviceId device_id;
device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_MAVLINK;
device_id.devid_s.bus = 1;
device_id.devid_s.address = msg->sysid;
device_id.devid_s.devtype = DRV_FLOW_DEVTYPE_SIM;
const matrix::Quatf q(matrix::Eulerf(vpe.roll, vpe.pitch, vpe.yaw));
q.copyTo(odom.q);
sensor_optical_flow_s sensor_optical_flow{};
// VISION_POSITION_ESTIMATE covariance
// Row-major representation of pose 6x6 cross-covariance matrix upper right triangle
// (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.).
// If unknown, assign NaN value to first element in the array.
odom.position_variance[0] = vpe.covariance[0]; // X row 0, col 0
odom.position_variance[1] = vpe.covariance[6]; // Y row 1, col 1
odom.position_variance[2] = vpe.covariance[11]; // Z row 2, col 2
sensor_optical_flow.timestamp_sample = hrt_absolute_time();
sensor_optical_flow.device_id = device_id.devid;
odom.orientation_variance[0] = vpe.covariance[15]; // R row 3, col 3
odom.orientation_variance[1] = vpe.covariance[18]; // P row 4, col 4
odom.orientation_variance[2] = vpe.covariance[20]; // Y row 5, col 5
sensor_optical_flow.pixel_flow[0] = flow.integrated_x;
sensor_optical_flow.pixel_flow[1] = flow.integrated_y;
odom.reset_counter = vpe.reset_counter;
sensor_optical_flow.integration_timespan_us = flow.integration_time_us;
sensor_optical_flow.quality = flow.quality;
odom.timestamp = hrt_absolute_time();
int index = (int) position_source::FLOW;
if (position_source_data[index].fail) {
uint32_t duration = position_source_data[index].failure_duration;
hrt_abstime start = position_source_data[index].failure_duration_start;
if (duration) {
if (hrt_elapsed_time(&start) > (duration * 1000000)) {
PX4_INFO("Optical flow failure ending");
position_source_data[index].fail = false;
position_source_data[index].failure_duration = 0;
position_source_data[index].failure_duration_start = 0;
} else {
sensor_optical_flow.quality = 0;
}
} else {
sensor_optical_flow.quality = 0;
}
}
const matrix::Vector3f integrated_gyro(flow.integrated_xgyro, flow.integrated_ygyro, flow.integrated_zgyro);
if (integrated_gyro.isAllFinite()) {
integrated_gyro.copyTo(sensor_optical_flow.delta_angle);
sensor_optical_flow.delta_angle_available = true;
}
sensor_optical_flow.max_flow_rate = NAN;
sensor_optical_flow.min_ground_distance = NAN;
sensor_optical_flow.max_ground_distance = NAN;
// Use distance value for distance sensor topic
// if (PX4_ISFINITE(flow.distance) && (flow.distance >= 0.f)) {
// // Positive value (including zero): distance known. Negative value: Unknown distance.
// sensor_optical_flow.distance_m = flow.distance;
// sensor_optical_flow.distance_available = true;
// }
// Emulate voxl-flow-server where distance comes in a separate
// distance sensor topic message
sensor_optical_flow.distance_m = 0.0f;
sensor_optical_flow.distance_available = false;
sensor_optical_flow.timestamp = hrt_absolute_time();
_sensor_optical_flow_pub.publish(sensor_optical_flow);
flow_sent_counter++;
}
int distance_debug_counter = 0;
void handle_message_distance_sensor(mavlink_message_t *msg)
{
mavlink_distance_sensor_t dist_sensor;
mavlink_msg_distance_sensor_decode(msg, &dist_sensor);
if ((_debug) && (!(distance_debug_counter % 10))) {
PX4_INFO("distance: time: %u, quality: %u, height: %u",
dist_sensor.time_boot_ms, dist_sensor.signal_quality,
dist_sensor.current_distance);
}
distance_debug_counter++;
distance_sensor_s ds{};
device::Device::DeviceId device_id;
device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_MAVLINK;
device_id.devid_s.bus = 1;
device_id.devid_s.address = msg->sysid;
device_id.devid_s.devtype = DRV_DIST_DEVTYPE_MAVLINK;
ds.timestamp = hrt_absolute_time(); /* Use system time for now, don't trust sender to attach correct timestamp */
ds.min_distance = static_cast<float>(dist_sensor.min_distance) * 1e-2f; /* cm to m */
ds.max_distance = static_cast<float>(dist_sensor.max_distance) * 1e-2f; /* cm to m */
ds.current_distance = static_cast<float>(dist_sensor.current_distance) * 1e-2f; /* cm to m */
ds.variance = dist_sensor.covariance * 1e-4f; /* cm^2 to m^2 */
ds.h_fov = dist_sensor.horizontal_fov;
ds.v_fov = dist_sensor.vertical_fov;
ds.q[0] = dist_sensor.quaternion[0];
ds.q[1] = dist_sensor.quaternion[1];
ds.q[2] = dist_sensor.quaternion[2];
ds.q[3] = dist_sensor.quaternion[3];
ds.type = dist_sensor.type;
ds.device_id = device_id.devid;
ds.orientation = dist_sensor.orientation;
// MAVLink DISTANCE_SENSOR signal_quality value of 0 means unset/unknown
// quality value. Also it comes normalised between 1 and 100 while the uORB
// signal quality is normalised between 0 and 100.
ds.signal_quality = dist_sensor.signal_quality == 0 ? -1 : 100 * (dist_sensor.signal_quality - 1) / 99;
_distance_sensor_pub.publish(ds);
distance_sent_counter++;
_visual_odometry_pub.publish(odom);
vision_msg_counter++;
}
void
@ -666,8 +518,6 @@ handle_message_odometry_dsp(mavlink_message_t *msg)
mavlink_odometry_t odom_in;
mavlink_msg_odometry_decode(msg, &odom_in);
odometry_sent_counter++;
// fill vehicle_odometry from Mavlink ODOMETRY
vehicle_odometry_s odom{};
uint64_t timestamp = hrt_absolute_time();
@ -849,28 +699,6 @@ handle_message_odometry_dsp(mavlink_message_t *msg)
odom.reset_counter = odom_in.reset_counter;
odom.quality = odom_in.quality;
int index = (int) position_source::VIO;
if (position_source_data[index].fail) {
uint32_t duration = position_source_data[index].failure_duration;
hrt_abstime start = position_source_data[index].failure_duration_start;
if (duration) {
if (hrt_elapsed_time(&start) > (duration * 1000000)) {
PX4_INFO("VIO failure ending");
position_source_data[index].fail = false;
position_source_data[index].failure_duration = 0;
position_source_data[index].failure_duration_start = 0;
} else {
odom.quality = 0;
}
} else {
odom.quality = 0;
}
}
switch (odom_in.estimator_type) {
case MAV_ESTIMATOR_TYPE_UNKNOWN: // accept MAV_ESTIMATOR_TYPE_UNKNOWN for legacy support
case MAV_ESTIMATOR_TYPE_NAIVE:
@ -917,6 +745,10 @@ void actuator_controls_from_outputs_dsp(mavlink_hil_actuator_controls_t *msg)
msg->mode = mode_flag_custom;
msg->mode |= (armed) ? mode_flag_armed : 0;
msg->flags = 0;
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
msg->flags |= 1;
#endif
}
int openPort(const char *dev, speed_t speed)
@ -927,8 +759,7 @@ int openPort(const char *dev, speed_t speed)
}
_uart_fd = qurt_uart_open(dev, speed);
if (_debug) { PX4_INFO("qurt_uart_opened"); }
PX4_DEBUG("qurt_uart_opened");
if (_uart_fd < 0) {
PX4_ERR("Error opening port: %s (%i)", dev, errno);
@ -1009,50 +840,25 @@ int stop()
void usage()
{
PX4_INFO("Usage: dsp_hitl {start|status|clear|failure|stop}");
PX4_INFO(" failure <source> <duration>");
PX4_INFO(" source: gps, vio, flow");
PX4_INFO(" duration: 0 (toggle state), else seconds");
PX4_INFO("Usage: dsp_hitl {start|info|status|stop}");
}
void print_status()
int get_status()
{
PX4_INFO("Running: %s", _is_running ? "yes" : "no");
PX4_INFO("HIL Sensor received: %i", hil_sensor_counter);
PX4_INFO("IMU updates: %i", imu_counter);
PX4_INFO("\tCurrent accel x, y, z: %f, %f, %f", double(x_accel), double(y_accel), double(z_accel));
PX4_INFO("\tCurrent gyro x, y, z: %f, %f, %f", double(x_gyro), double(y_gyro), double(z_gyro));
PX4_INFO("Magnetometer sent: %i", mag_counter);
PX4_INFO("Barometer sent: %i", baro_counter);
PX4_INFO("Heartbeat received: %i, sent: %i", heartbeat_received_counter, heartbeat_sent_counter);
PX4_INFO("Odometry received: %i, sent: %i", odometry_received_counter, odometry_sent_counter);
PX4_INFO("GPS received: %i, sent: %i", gps_received_counter, gps_sent_counter);
PX4_INFO("Distance sensor received: %i, sent: %i", distance_received_counter, distance_sent_counter);
PX4_INFO("Optical flow received: %i, sent: %i", flow_received_counter, flow_sent_counter);
PX4_INFO("Actuator updates sent: %i", actuator_sent_counter);
PX4_INFO("Unknown messages received: %i", unknown_msg_received_counter);
PX4_INFO("Status of IMU_Data counter: %i", imu_counter);
PX4_INFO("Value of current accel x, y, z data: %f, %f, %f", double(x_accel), double(y_accel), double(z_accel));
PX4_INFO("Value of current gyro x, y, z data: %f, %f, %f", double(x_gyro), double(y_gyro), double(z_gyro));
PX4_INFO("Value of HIL_Sensor counter: %i", hil_sensor_counter);
PX4_INFO("Value of Heartbeat counter: %i", heartbeat_counter);
PX4_INFO("Value of Vision data counter: %i", vision_msg_counter);
PX4_INFO("Value of GPS Data counter: %i", gps_counter);
return 0;
}
void
clear_status_counters()
{
heartbeat_received_counter = 0;
heartbeat_sent_counter = 0;
imu_counter = 0;
hil_sensor_counter = 0;
mag_counter = 0;
baro_counter = 0;
actuator_sent_counter = 0;
odometry_received_counter = 0;
odometry_sent_counter = 0;
gps_received_counter = 0;
gps_sent_counter = 0;
distance_received_counter = 0;
distance_sent_counter = 0;
flow_received_counter = 0;
flow_sent_counter = 0;
unknown_msg_received_counter = 0;
}
uint64_t first_sensor_msg_timestamp = 0;
uint64_t first_sensor_report_timestamp = 0;
uint64_t last_sensor_report_timestamp = 0;
void
handle_message_hil_sensor_dsp(mavlink_message_t *msg)
@ -1122,8 +928,6 @@ handle_message_hil_sensor_dsp(mavlink_message_t *msg)
}
_px4_mag->update(gyro_accel_time, hil_sensor.xmag, hil_sensor.ymag, hil_sensor.zmag);
mag_counter++;
}
}
@ -1138,8 +942,17 @@ handle_message_hil_sensor_dsp(mavlink_message_t *msg)
sensor_baro.error_count = 0;
sensor_baro.timestamp = hrt_absolute_time();
_sensor_baro_pub.publish(sensor_baro);
}
baro_counter++;
// differential pressure
if ((hil_sensor.fields_updated & SensorSource::DIFF_PRESS) == SensorSource::DIFF_PRESS) {
differential_pressure_s report{};
report.timestamp_sample = gyro_accel_time;
report.device_id = 1377548; // 1377548: DRV_DIFF_PRESS_DEVTYPE_SIM, BUS: 1, ADDR: 5, TYPE: SIMULATION
report.temperature = hil_sensor.temperature;
report.differential_pressure_pa = hil_sensor.diff_pressure * 100.0f; // hPa to Pa
report.timestamp = hrt_absolute_time();
_differential_pressure_pub.publish(report);
}
// battery status
@ -1157,6 +970,7 @@ handle_message_hil_sensor_dsp(mavlink_message_t *msg)
_battery_pub.publish(hil_battery_status);
}
hil_sensor_counter++;
}
void
@ -1175,41 +989,15 @@ handle_message_hil_gps_dsp(mavlink_message_t *msg)
gps.device_id = device_id.devid;
gps.latitude_deg = hil_gps.lat * 1e-7;
gps.longitude_deg = hil_gps.lon * 1e-7;
gps.altitude_msl_m = hil_gps.alt * 1e-3;
gps.altitude_ellipsoid_m = hil_gps.alt * 1e-3;
gps.lat = hil_gps.lat;
gps.lon = hil_gps.lon;
gps.alt = hil_gps.alt;
gps.alt_ellipsoid = hil_gps.alt;
gps.s_variance_m_s = 0.25f;
gps.c_variance_rad = 0.5f;
gps.satellites_used = hil_gps.satellites_visible;
gps.fix_type = hil_gps.fix_type;
int index = (int) position_source::GPS;
if (position_source_data[index].fail) {
uint32_t duration = position_source_data[index].failure_duration;
hrt_abstime start = position_source_data[index].failure_duration_start;
if (duration) {
if (hrt_elapsed_time(&start) > (duration * 1000000)) {
PX4_INFO("GPS failure ending");
position_source_data[index].fail = false;
position_source_data[index].failure_duration = 0;
position_source_data[index].failure_duration_start = 0;
} else {
gps.satellites_used = 1;
gps.fix_type = 0;
}
} else {
gps.satellites_used = 1;
gps.fix_type = 0;
}
}
gps.eph = (float)hil_gps.eph * 1e-2f; // cm -> m
gps.epv = (float)hil_gps.epv * 1e-2f; // cm -> m
@ -1233,6 +1021,7 @@ handle_message_hil_gps_dsp(mavlink_message_t *msg)
gps.timestamp_time_relative = 0;
gps.time_utc_usec = hil_gps.time_usec;
gps.satellites_used = hil_gps.satellites_visible;
gps.heading = NAN;
gps.heading_offset = NAN;
@ -1240,51 +1029,10 @@ handle_message_hil_gps_dsp(mavlink_message_t *msg)
gps.timestamp = hrt_absolute_time();
_sensor_gps_pub.publish(gps);
gps_sent_counter++;
gps_counter++;
}
int
process_failure(dsp_hitl::position_source src, int duration)
{
if (src >= position_source::NUM_POSITION_SOURCES) {
return 1;
}
int index = (int) src;
if (position_source_data[index].send) {
if (duration <= 0) {
// Toggle state
if (position_source_data[index].fail) {
PX4_INFO("Ending indefinite %s failure", position_source_data[index].label);
position_source_data[index].fail = false;
} else {
PX4_INFO("Starting indefinite %s failure", position_source_data[index].label);
position_source_data[index].fail = true;
}
position_source_data[index].failure_duration = 0;
position_source_data[index].failure_duration_start = 0;
} else {
PX4_INFO("%s failure for %d seconds", position_source_data[index].label, duration);
position_source_data[index].fail = true;
position_source_data[index].failure_duration = duration;
position_source_data[index].failure_duration_start = hrt_absolute_time();
}
} else {
PX4_ERR("%s not active, cannot create failure", position_source_data[index].label);
return 1;
}
return 0;
}
} // End dsp_hitl namespace
int dsp_hitl_main(int argc, char *argv[])
{
int myoptind = 1;
@ -1296,47 +1044,20 @@ int dsp_hitl_main(int argc, char *argv[])
const char *verb = argv[myoptind];
if (!strcmp(verb, "start")) {
return dsp_hitl::start(argc - 1, argv + 1);
}
} else if (!strcmp(verb, "stop")) {
else if (!strcmp(verb, "stop")) {
return dsp_hitl::stop();
}
} else if (!strcmp(verb, "status")) {
dsp_hitl::print_status();
return 0;
else if (!strcmp(verb, "status")) {
return dsp_hitl::get_status();
}
} else if (!strcmp(verb, "clear")) {
dsp_hitl::clear_status_counters();
return 0;
} else if (!strcmp(verb, "failure")) {
if (argc != 4) {
dsp_hitl::usage();
return 1;
}
const char *source = argv[myoptind + 1];
int duration = atoi(argv[myoptind + 2]);
if (!strcmp(source, "gps")) {
return dsp_hitl::process_failure(dsp_hitl::position_source::GPS, duration);
} else if (!strcmp(source, "vio")) {
return dsp_hitl::process_failure(dsp_hitl::position_source::VIO, duration);
} else if (!strcmp(source, "flow")) {
return dsp_hitl::process_failure(dsp_hitl::position_source::FLOW, duration);
} else {
PX4_ERR("Unknown failure source %s, duration %d", source, duration);
dsp_hitl::usage();
return 1;
}
return 0;
} else {
else {
dsp_hitl::usage();
return 1;
}

View File

@ -1,6 +1,6 @@
############################################################################
#
# Copyright (c) 2024 PX4 Development Team. All rights reserved.
# Copyright (c) 2020 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
@ -30,5 +30,19 @@
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
px4_add_library(rtl_time_estimator rtl_time_estimator.cpp)
px4_add_module(
MODULE drivers__imu__invensense__icm42688p
MAIN icm42688p
COMPILE_FLAGS
${MAX_CUSTOM_OPT_LEVEL}
SRCS
icm42688p_main.cpp
ICM42688P.cpp
ICM42688P.hpp
InvenSense_ICM42688P_registers.hpp
DEPENDS
px4_work_queue
drivers_accelerometer
drivers_gyroscope
drivers__device
)

View File

@ -0,0 +1,991 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "ICM42688P.hpp"
bool hitl_mode = false;
using namespace time_literals;
static constexpr int16_t combine(uint8_t msb, uint8_t lsb)
{
return (msb << 8u) | lsb;
}
ICM42688P::ICM42688P(const I2CSPIDriverConfig &config) :
// SPI(DRV_IMU_DEVTYPE_ICM42688P, MODULE_NAME, bus, device, spi_mode, bus_frequency),
// I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus),
// _drdy_gpio(drdy_gpio)
SPI(config),
I2CSPIDriver(config),
_drdy_gpio(config.drdy_gpio),
_px4_accel(get_device_id(), config.rotation),
_px4_gyro(get_device_id(), config.rotation)
{
if (config.drdy_gpio != 0) {
_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed");
}
if (!hitl_mode) {
// _px4_accel = std::make_shared<PX4Accelerometer>(get_device_id(), rotation);
// _px4_gyro = std::make_shared<PX4Gyroscope>(get_device_id(), rotation);
ConfigureSampleRate(_px4_gyro.get_max_rate_hz());
// _imu_server_pub.advertise();
} else {
ConfigureSampleRate(0);
}
}
ICM42688P::~ICM42688P()
{
perf_free(_bad_register_perf);
perf_free(_bad_transfer_perf);
perf_free(_fifo_empty_perf);
perf_free(_fifo_overflow_perf);
perf_free(_fifo_reset_perf);
perf_free(_drdy_missed_perf);
// if (!hitl_mode){
// _imu_server_pub.unadvertise();
// }
}
int ICM42688P::init()
{
int ret = SPI::init();
if (ret != PX4_OK) {
DEVICE_DEBUG("SPI::init failed (%i)", ret);
return ret;
}
return Reset() ? 0 : -1;
}
bool ICM42688P::Reset()
{
_state = STATE::RESET;
DataReadyInterruptDisable();
ScheduleClear();
ScheduleNow();
return true;
}
void ICM42688P::exit_and_cleanup()
{
DataReadyInterruptDisable();
I2CSPIDriverBase::exit_and_cleanup();
}
void ICM42688P::print_status()
{
I2CSPIDriverBase::print_status();
PX4_INFO("FIFO empty interval: %d us (%.1f Hz)", _fifo_empty_interval_us, 1e6 / _fifo_empty_interval_us);
perf_print_counter(_bad_register_perf);
perf_print_counter(_bad_transfer_perf);
perf_print_counter(_fifo_empty_perf);
perf_print_counter(_fifo_overflow_perf);
perf_print_counter(_fifo_reset_perf);
perf_print_counter(_drdy_missed_perf);
}
int ICM42688P::probe()
{
for (int i = 0; i < 3; i++) {
uint8_t whoami = RegisterRead(Register::BANK_0::WHO_AM_I);
if (whoami == WHOAMI) {
PX4_INFO("ICM42688P::probe successful!");
return PX4_OK;
} else {
DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami);
uint8_t reg_bank_sel = RegisterRead(Register::BANK_0::REG_BANK_SEL);
int bank = reg_bank_sel >> 4;
if (bank >= 1 && bank <= 3) {
DEVICE_DEBUG("incorrect register bank for WHO_AM_I REG_BANK_SEL:0x%02x, bank:%d", reg_bank_sel, bank);
// force bank selection and retry
SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0, true);
}
}
}
return PX4_ERROR;
}
void ICM42688P::RunImpl()
{
PX4_INFO(">>> ICM42688P this: %p", this);
const hrt_abstime now = hrt_absolute_time();
switch (_state) {
case STATE::RESET:
// DEVICE_CONFIG: Software reset configuration
RegisterWrite(Register::BANK_0::DEVICE_CONFIG, DEVICE_CONFIG_BIT::SOFT_RESET_CONFIG);
_reset_timestamp = now;
_failure_count = 0;
_state = STATE::WAIT_FOR_RESET;
ScheduleDelayed(2_ms); // to be safe wait 2 ms for soft reset to be effective
break;
case STATE::WAIT_FOR_RESET:
if ((RegisterRead(Register::BANK_0::WHO_AM_I) == WHOAMI)
&& (RegisterRead(Register::BANK_0::DEVICE_CONFIG) == 0x00)
&& (RegisterRead(Register::BANK_0::INT_STATUS) & INT_STATUS_BIT::RESET_DONE_INT)) {
_state = STATE::CONFIGURE;
ScheduleDelayed(10_ms); // 30 ms gyro startup time, 10 ms accel from sleep to valid data
} else {
// RESET not complete
if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
PX4_DEBUG("Reset failed, retrying");
_state = STATE::RESET;
ScheduleDelayed(100_ms);
} else {
PX4_DEBUG("Reset not complete, check again in 10 ms");
ScheduleDelayed(10_ms);
}
}
break;
case STATE::CONFIGURE:
if (Configure()) {
// Wakeup accel and gyro after configuring registers
ScheduleDelayed(1_ms); // add a delay here to be safe
RegisterWrite(Register::BANK_0::PWR_MGMT0, PWR_MGMT0_BIT::GYRO_MODE_LOW_NOISE | PWR_MGMT0_BIT::ACCEL_MODE_LOW_NOISE);
ScheduleDelayed(30_ms); // 30 ms gyro startup time, 10 ms accel from sleep to valid data
// if configure succeeded then start reading from FIFO
_state = STATE::FIFO_READ;
if (DataReadyInterruptConfigure()) {
_data_ready_interrupt_enabled = true;
// backup schedule as a watchdog timeout
ScheduleDelayed(100_ms);
} else {
PX4_ERR("ICM42688P::RunImpl interrupt configuration failed");
_data_ready_interrupt_enabled = false;
ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us);
}
FIFOReset();
} else {
PX4_ERR("ICM42688P::RunImpl configuration failed");
// CONFIGURE not complete
if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
PX4_DEBUG("Configure failed, resetting");
_state = STATE::RESET;
} else {
PX4_DEBUG("Configure failed, retrying");
}
ScheduleDelayed(100_ms);
}
break;
case STATE::FIFO_READ: {
#ifndef __PX4_QURT
uint32_t samples = 0;
if (_data_ready_interrupt_enabled) {
// scheduled from interrupt if _drdy_fifo_read_samples was set as expected
if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_gyro_samples) {
perf_count(_drdy_missed_perf);
} else {
samples = _fifo_gyro_samples;
}
// push backup schedule back
ScheduleDelayed(_fifo_empty_interval_us * 2);
}
if (samples == 0) {
// check current FIFO count
const uint16_t fifo_count = FIFOReadCount();
if (fifo_count >= FIFO::SIZE) {
FIFOReset();
perf_count(_fifo_overflow_perf);
} else if (fifo_count == 0) {
perf_count(_fifo_empty_perf);
} else {
// FIFO count (size in bytes)
samples = (fifo_count / sizeof(FIFO::DATA));
if (samples > FIFO_MAX_SAMPLES) {
// not technically an overflow, but more samples than we expected or can publish
FIFOReset();
perf_count(_fifo_overflow_perf);
samples = 0;
}
}
}
bool success = false;
if (samples >= 1) {
if (FIFORead(now, samples)) {
success = true;
if (_failure_count > 0) {
_failure_count--;
}
}
}
if (!success) {
_failure_count++;
// full reset if things are failing consistently
if (_failure_count > 10) {
Reset();
return;
}
}
// check configuration registers periodically or immediately following any failure
if (RegisterCheck(_register_bank0_cfg[_checked_register_bank0])
&& RegisterCheck(_register_bank1_cfg[_checked_register_bank1])
&& RegisterCheck(_register_bank2_cfg[_checked_register_bank2])
) {
_last_config_check_timestamp = now;
_checked_register_bank0 = (_checked_register_bank0 + 1) % size_register_bank0_cfg;
_checked_register_bank1 = (_checked_register_bank1 + 1) % size_register_bank1_cfg;
_checked_register_bank2 = (_checked_register_bank2 + 1) % size_register_bank2_cfg;
} else {
// register check failed, force reset
perf_count(_bad_register_perf);
Reset();
}
#endif
}
break;
}
}
void ICM42688P::ConfigureSampleRate(int sample_rate)
{
if (sample_rate == 0) {
sample_rate = 800; // default to 800 Hz
}
// round down to nearest FIFO sample dt
const float min_interval = FIFO_SAMPLE_DT;
_fifo_empty_interval_us = math::max(roundf((1e6f / (float)sample_rate) / min_interval) * min_interval, min_interval);
_fifo_gyro_samples = roundf(math::min((float)_fifo_empty_interval_us / (1e6f / GYRO_RATE), (float)FIFO_MAX_SAMPLES));
// recompute FIFO empty interval (us) with actual gyro sample limit
_fifo_empty_interval_us = _fifo_gyro_samples * (1e6f / GYRO_RATE);
ConfigureFIFOWatermark(_fifo_gyro_samples);
}
void ICM42688P::ConfigureFIFOWatermark(uint8_t samples)
{
// FIFO watermark threshold in number of bytes
const uint16_t fifo_watermark_threshold = samples * sizeof(FIFO::DATA);
for (auto &r : _register_bank0_cfg) {
if (r.reg == Register::BANK_0::FIFO_CONFIG2) {
// FIFO_WM[7:0] FIFO_CONFIG2
r.set_bits = fifo_watermark_threshold & 0xFF;
} else if (r.reg == Register::BANK_0::FIFO_CONFIG3) {
// FIFO_WM[11:8] FIFO_CONFIG3
r.set_bits = (fifo_watermark_threshold >> 8) & 0x0F;
}
}
}
void ICM42688P::SelectRegisterBank(enum REG_BANK_SEL_BIT bank, bool force)
{
if (bank != _last_register_bank || force) {
// select BANK_0
uint8_t cmd_bank_sel[2] {};
cmd_bank_sel[0] = static_cast<uint8_t>(Register::BANK_0::REG_BANK_SEL);
cmd_bank_sel[1] = bank;
transfer(cmd_bank_sel, cmd_bank_sel, sizeof(cmd_bank_sel));
_last_register_bank = bank;
}
}
bool ICM42688P::Configure()
{
// first set and clear all configured register bits
for (const auto &reg_cfg : _register_bank0_cfg) {
RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
}
for (const auto &reg_cfg : _register_bank1_cfg) {
RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
}
for (const auto &reg_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 &reg_cfg : _register_bank0_cfg) {
if (!RegisterCheck(reg_cfg)) {
success = false;
}
}
for (const auto &reg_cfg : _register_bank1_cfg) {
if (!RegisterCheck(reg_cfg)) {
success = false;
}
}
for (const auto &reg_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 &reg_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 &timestamp_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 8b_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 &timestamp_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 &timestamp_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 &timestamp_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;
}

View File

@ -0,0 +1,235 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file ICM42688P.hpp
*
* Driver for the Invensense ICM42688P connected via SPI.
*
*/
#pragma once
#include "InvenSense_ICM42688P_registers.hpp"
#include <drivers/drv_hrt.h>
#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
#include <lib/drivers/device/spi.h>
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
#include <lib/geo/geo.h>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/atomic.h>
#include <px4_platform_common/i2c_spi_buses.h>
#include <uORB/topics/imu_server.h>
#include <uORB/topics/sensor_accel_fifo.h>
#include <uORB/topics/sensor_gyro_fifo.h>
#include <memory>
using namespace InvenSense_ICM42688P;
extern bool hitl_mode;
class ICM42688P : public device::SPI, public I2CSPIDriver<ICM42688P>
{
public:
// ICM42688P(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency,
// spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio);
ICM42688P(const I2CSPIDriverConfig &config);
~ICM42688P() override;
// static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
// int runtime_instance);
static void print_usage();
void RunImpl();
int init() override;
void print_status() override;
private:
void exit_and_cleanup() override;
// Sensor Configuration
static constexpr float IMU_ODR{8000.f}; // 8kHz accel & gyro ODR configured
static constexpr float FIFO_SAMPLE_DT{1e6f / IMU_ODR};
static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT};
static constexpr float ACCEL_RATE{1e6f / FIFO_SAMPLE_DT};
// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
// static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
static constexpr uint32_t FIFO_MAX_SAMPLES{10};
// Transfer data
struct FIFOTransferBuffer {
uint8_t cmd{static_cast<uint8_t>(Register::BANK_0::INT_STATUS) | DIR_READ};
uint8_t INT_STATUS{0};
uint8_t FIFO_COUNTH{0};
uint8_t FIFO_COUNTL{0};
FIFO::DATA f[FIFO_MAX_SAMPLES] {};
};
// ensure no struct padding
static_assert(sizeof(FIFOTransferBuffer) == (4 + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)),
"Invalid FIFOTransferBuffer size");
struct register_bank0_config_t {
Register::BANK_0 reg;
uint8_t set_bits{0};
uint8_t clear_bits{0};
};
struct register_bank1_config_t {
Register::BANK_1 reg;
uint8_t set_bits{0};
uint8_t clear_bits{0};
};
struct register_bank2_config_t {
Register::BANK_2 reg;
uint8_t set_bits{0};
uint8_t clear_bits{0};
};
int probe() override;
bool Reset();
bool Configure();
void ConfigureSampleRate(int sample_rate);
void ConfigureFIFOWatermark(uint8_t samples);
void SelectRegisterBank(enum REG_BANK_SEL_BIT bank, bool force = false);
void SelectRegisterBank(Register::BANK_0 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0); }
void SelectRegisterBank(Register::BANK_1 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_1); }
void SelectRegisterBank(Register::BANK_2 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_2); }
static int DataReadyInterruptCallback(int irq, void *context, void *arg);
void DataReady();
bool DataReadyInterruptConfigure();
bool DataReadyInterruptDisable();
template <typename T> bool RegisterCheck(const T &reg_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 &timestamp_sample, uint16_t samples);
void FIFOReset();
void ProcessIMU(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples);
void ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples);
void ProcessGyro(const hrt_abstime &timestamp_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)};
};

View File

@ -0,0 +1,430 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file InvenSense_ICM42688P_registers.hpp
*
* Invensense ICM-42688-P registers.
*
*/
#pragma once
#include <cstdint>
namespace InvenSense_ICM42688P
{
// TODO: move to a central header
static constexpr uint8_t Bit0 = (1 << 0);
static constexpr uint8_t Bit1 = (1 << 1);
static constexpr uint8_t Bit2 = (1 << 2);
static constexpr uint8_t Bit3 = (1 << 3);
static constexpr uint8_t Bit4 = (1 << 4);
static constexpr uint8_t Bit5 = (1 << 5);
static constexpr uint8_t Bit6 = (1 << 6);
static constexpr uint8_t Bit7 = (1 << 7);
static constexpr uint32_t SPI_SPEED = 24 * 1000 * 1000; // 24 MHz SPI
static constexpr uint8_t DIR_READ = 0x80;
static constexpr uint8_t WHOAMI = 0x47;
static constexpr float TEMPERATURE_SENSITIVITY = 132.48f; // LSB/C
static constexpr float TEMPERATURE_OFFSET = 25.f; // C
namespace Register
{
enum class BANK_0 : uint8_t {
DEVICE_CONFIG = 0x11,
INT_CONFIG = 0x14,
FIFO_CONFIG = 0x16,
TEMP_DATA1 = 0x1D,
TEMP_DATA0 = 0x1E,
INT_STATUS = 0x2D,
FIFO_COUNTH = 0x2E,
FIFO_COUNTL = 0x2F,
FIFO_DATA = 0x30,
SIGNAL_PATH_RESET = 0x4B,
INTF_CONFIG0 = 0x4C,
INTF_CONFIG1 = 0x4D,
PWR_MGMT0 = 0x4E,
GYRO_CONFIG0 = 0x4F,
ACCEL_CONFIG0 = 0x50,
GYRO_CONFIG1 = 0x51,
GYRO_ACCEL_CONFIG0 = 0x52,
ACCEL_CONFIG1 = 0x53,
FIFO_CONFIG1 = 0x5F,
FIFO_CONFIG2 = 0x60,
FIFO_CONFIG3 = 0x61,
INT_CONFIG0 = 0x63,
INT_SOURCE0 = 0x65,
SELF_TEST_CONFIG = 0x70,
WHO_AM_I = 0x75,
REG_BANK_SEL = 0x76,
};
enum class BANK_1 : uint8_t {
GYRO_CONFIG_STATIC2 = 0x0B,
GYRO_CONFIG_STATIC3 = 0x0C,
GYRO_CONFIG_STATIC4 = 0x0D,
GYRO_CONFIG_STATIC5 = 0x0E,
INTF_CONFIG5 = 0x7B,
};
enum class BANK_2 : uint8_t {
ACCEL_CONFIG_STATIC2 = 0x03,
ACCEL_CONFIG_STATIC3 = 0x04,
ACCEL_CONFIG_STATIC4 = 0x05,
};
};
//---------------- BANK0 Register bits
// DEVICE_CONFIG
enum DEVICE_CONFIG_BIT : uint8_t {
SOFT_RESET_CONFIG = Bit0, //
};
// INT_CONFIG
enum INT_CONFIG_BIT : uint8_t {
INT1_MODE = Bit2,
INT1_DRIVE_CIRCUIT = Bit1,
INT1_POLARITY = Bit0,
};
// FIFO_CONFIG
enum FIFO_CONFIG_BIT : uint8_t {
// 7:6 FIFO_MODE
FIFO_MODE_STOP_ON_FULL = Bit7 | Bit6, // 11: STOP-on-FULL Mode
};
// INT_STATUS
enum INT_STATUS_BIT : uint8_t {
RESET_DONE_INT = Bit4,
DATA_RDY_INT = Bit3,
FIFO_THS_INT = Bit2,
FIFO_FULL_INT = Bit1,
};
// SIGNAL_PATH_RESET
enum SIGNAL_PATH_RESET_BIT : uint8_t {
ABORT_AND_RESET = Bit3,
FIFO_FLUSH = Bit1,
};
// PWR_MGMT0
enum PWR_MGMT0_BIT : uint8_t {
GYRO_MODE_LOW_NOISE = Bit3 | Bit2, // 11: Places gyroscope in Low Noise (LN) Mode
ACCEL_MODE_LOW_NOISE = Bit1 | Bit0, // 11: Places accelerometer in Low Noise (LN) Mode
};
// GYRO_CONFIG0
enum GYRO_CONFIG0_BIT : uint8_t {
// 7:5 GYRO_FS_SEL
GYRO_FS_SEL_2000_DPS = 0, // 0b000 = ±2000dps (default)
GYRO_FS_SEL_1000_DPS = Bit5,
GYRO_FS_SEL_500_DPS = Bit6,
GYRO_FS_SEL_250_DPS = Bit6 | Bit5,
GYRO_FS_SEL_125_DPS = Bit7,
// 3:0 GYRO_ODR
// 0001: 32kHz
GYRO_ODR_32KHZ_SET = Bit0,
GYRO_ODR_32KHZ_CLEAR = Bit3 | Bit2 | Bit0,
// 0010: 16kHz
GYRO_ODR_16KHZ_SET = Bit1,
GYRO_ODR_16KHZ_CLEAR = Bit3 | Bit2 | Bit0,
// 0011: 8kHz
GYRO_ODR_8KHZ_SET = Bit1 | Bit0,
GYRO_ODR_8KHZ_CLEAR = Bit3 | Bit2,
// 0110: 1kHz (default)
GYRO_ODR_1KHZ_SET = Bit2 | Bit1,
GYRO_ODR_1KHZ_CLEAR = Bit3 | Bit0,
};
// ACCEL_CONFIG0
enum ACCEL_CONFIG0_BIT : uint8_t {
// 7:5 ACCEL_FS_SEL
ACCEL_FS_SEL_16G = 0, // 000: ±16g (default)
ACCEL_FS_SEL_8G = Bit5,
ACCEL_FS_SEL_4G = Bit6,
ACCEL_FS_SEL_2G = Bit6 | Bit5,
// 3:0 ACCEL_ODR
// 0001: 32kHz
ACCEL_ODR_32KHZ_SET = Bit0,
ACCEL_ODR_32KHZ_CLEAR = Bit3 | Bit2 | Bit0,
// 0010: 16kHz
ACCEL_ODR_16KHZ_SET = Bit1,
ACCEL_ODR_16KHZ_CLEAR = Bit3 | Bit2 | Bit0,
// 0011: 8kHz
ACCEL_ODR_8KHZ_SET = Bit1 | Bit0,
ACCEL_ODR_8KHZ_CLEAR = Bit3 | Bit2,
// 0110: 1kHz (default)
ACCEL_ODR_1KHZ_SET = Bit2 | Bit1,
ACCEL_ODR_1KHZ_CLEAR = Bit3 | Bit0,
};
// GYRO_CONFIG1
enum GYRO_CONFIG1_BIT : uint8_t {
GYRO_UI_FILT_ORD = Bit3 | Bit2, // 00: 1st Order
};
// GYRO_ACCEL_CONFIG0
enum GYRO_ACCEL_CONFIG0_BIT : uint8_t {
// 7:4 ACCEL_UI_FILT_BW
ACCEL_UI_FILT_BW = Bit7 | Bit6 | Bit5 | Bit4, // 0: BW=ODR/2
// 3:0 GYRO_UI_FILT_BW
GYRO_UI_FILT_BW = Bit3 | Bit2 | Bit1 | Bit0, // 0: BW=ODR/2
};
// ACCEL_CONFIG1
enum ACCEL_CONFIG1_BIT : uint8_t {
ACCEL_UI_FILT_ORD = Bit4 | Bit3, // 00: 1st Order
};
// FIFO_CONFIG1
enum FIFO_CONFIG1_BIT : uint8_t {
FIFO_RESUME_PARTIAL_RD = Bit6,
FIFO_WM_GT_TH = Bit5,
FIFO_HIRES_EN = Bit4,
FIFO_TEMP_EN = Bit2,
FIFO_GYRO_EN = Bit1,
FIFO_ACCEL_EN = Bit0,
};
// INT_CONFIG0
enum INT_CONFIG0_BIT : uint8_t {
// 3:2 FIFO_THS_INT_CLEAR
CLEAR_ON_FIFO_READ = Bit3,
};
// INT_SOURCE0
enum INT_SOURCE0_BIT : uint8_t {
UI_FSYNC_INT1_EN = Bit6,
PLL_RDY_INT1_EN = Bit5,
RESET_DONE_INT1_EN = Bit4,
UI_DRDY_INT1_EN = Bit3,
FIFO_THS_INT1_EN = Bit2, // FIFO threshold interrupt routed to INT1
FIFO_FULL_INT1_EN = Bit1,
UI_AGC_RDY_INT1_EN = Bit0,
};
// REG_BANK_SEL
enum REG_BANK_SEL_BIT : uint8_t {
USER_BANK_0 = 0, // 0: Select USER BANK 0.
USER_BANK_1 = Bit0, // 1: Select USER BANK 1.
USER_BANK_2 = Bit1, // 2: Select USER BANK 2.
USER_BANK_3 = Bit1 | Bit0, // 3: Select USER BANK 3.
};
//---------------- BANK1 Register bits
// GYRO_CONFIG_STATIC2
enum GYRO_CONFIG_STATIC2_BIT : uint8_t {
GYRO_AAF_DIS = Bit1,
GYRO_NF_DIS = Bit0,
};
// GYRO_CONFIG_STATIC3
enum GYRO_CONFIG_STATIC3_BIT : uint8_t {
// 585 Hz
GYRO_AAF_DELT_SET = Bit3 | Bit2 | Bit0, //13
GYRO_AAF_DELT_CLEAR = Bit5 | Bit4 | Bit1,
// 213 Hz
// GYRO_AAF_DELT_SET = Bit2 | Bit0, //5
// GYRO_AAF_DELT_CLEAR = Bit5 | Bit4 | Bit3 | Bit1,
// 126 Hz
//GYRO_AAF_DELT_SET = Bit1 | Bit0, //3
//GYRO_AAF_DELT_CLEAR = Bit5 | Bit4 | Bit3 | Bit2,
// 42 Hz
// GYRO_AAF_DELT_SET = Bit0, //1
// GYRO_AAF_DELT_CLEAR = Bit5 | Bit4 | Bit3 | Bit2 | Bit1,
};
// GYRO_CONFIG_STATIC4
enum GYRO_CONFIG_STATIC4_BIT : uint8_t {
// 585 Hz
GYRO_AAF_DELTSQR_LOW_SET = Bit7 | Bit5 | Bit3 | Bit1, //170
GYRO_AAF_DELTSQR_LOW_CLEAR = Bit6 | Bit4 | Bit2 | Bit0,
// 213 Hz
// GYRO_AAF_DELTSQR_LOW_SET = Bit4 | Bit3 | Bit0, //25
// GYRO_AAF_DELTSQR_LOW_CLEAR = Bit7 | Bit6 | Bit5 | Bit2 | Bit1,
// 126 Hz
//GYRO_AAF_DELTSQR_LOW_SET = Bit3 | Bit0, //9
//GYRO_AAF_DELTSQR_LOW_CLEAR = Bit7 | Bit6 | Bit5 | Bit4 | Bit2 | Bit1,
// 42 Hz
// GYRO_AAF_DELTSQR_LOW_SET = Bit0, //1
// GYRO_AAF_DELTSQR_LOW_CLEAR = Bit7 | Bit6 | Bit5 | Bit4 | Bit3 | Bit2 | Bit1,
};
// GYRO_CONFIG_STATIC5
enum GYRO_CONFIG_STATIC5_BIT : uint8_t {
// 585 Hz
GYRO_AAF_DELTSQR_HIGH_SET = 0,
GYRO_AAF_DELTSQR_HIGH_CLEAR = Bit3 | Bit2 | Bit1 | Bit0,
GYRO_AAF_BITSHIFT_SET = Bit7, // 8 << 4
GYRO_AAF_BITSHIFT_CLEAR = Bit6 | Bit5 | Bit4,
// 213 Hz
// GYRO_AAF_DELTSQR_HIGH_SET = 0,
// GYRO_AAF_DELTSQR_HIGH_CLEAR = Bit3 | Bit2 | Bit1 | Bit0,
// GYRO_AAF_BITSHIFT_SET = Bit7 | Bit5, //10
// GYRO_AAF_BITSHIFT_CLEAR = Bit6 | Bit4,
// 126 Hz
// GYRO_AAF_BITSHIFT_SET = Bit7 | Bit6, //12
// GYRO_AAF_BITSHIFT_CLEAR = Bit5 | Bit4,
// 42 Hz
// GYRO_AAF_BITSHIFT_SET = Bit7 | Bit6 | Bit5 | Bit4, //15
// GYRO_AAF_BITSHIFT_CLEAR = 0,
};
//---------------- BANK2 Register bits
// ACCEL_CONFIG_STATIC2
enum ACCEL_CONFIG_STATIC2_BIT : uint8_t {
ACCEL_AAF_DIS = Bit0,
ACCEL_AAF_DELT = Bit3 | Bit1,
// 213 Hz
ACCEL_AAF_DELT_SET = Bit3 | Bit1, //5
ACCEL_AAF_DELT_CLEAR = Bit6 | Bit5 | Bit4 | Bit2,
// 42 Hz
// ACCEL_AAF_DELT_SET = Bit1, //1
// ACCEL_AAF_DELT_CLEAR = Bit6 | Bit5 | Bit4 | Bit3 | Bit2,
};
// ACCEL_CONFIG_STATIC3
enum ACCEL_CONFIG_STATIC3_BIT : uint8_t {
ACCEL_AAF_DELTSQR_LOW = Bit4 | Bit3 | Bit0,
// 213 Hz
ACCEL_AAF_DELTSQR_LOW_SET = Bit4 | Bit3 | Bit0, //25
ACCEL_AAF_DELTSQR_LOW_CLEAR = Bit7 | Bit6 | Bit5 | Bit2 | Bit1,
// 42 Hz
// ACCEL_AAF_DELTSQR_LOW_SET = Bit0, //1
// ACCEL_AAF_DELTSQR_LOW_CLEAR = Bit7 | Bit6 | Bit5 | Bit4 | Bit3 | Bit2 | Bit1,
};
// ACCEL_CONFIG_STATIC4
enum ACCEL_CONFIG_STATIC4_BIT : uint8_t {
ACCEL_AAF_BITSHIFT = Bit7 | Bit5,
ACCEL_AAF_DELTSQR_HIGH = 0,
// 213 Hz
ACCEL_AAF_BITSHIFT_SET = Bit7 | Bit5, //10
ACCEL_AAF_BITSHIFT_CLEAR = Bit6 | Bit4,
// 42 Hz
// ACCEL_AAF_BITSHIFT_SET = Bit7 | Bit6 | Bit5 | Bit4, //15
// ACCEL_AAF_BITSHIFT_CLEAR = 0,
ACCEL_AAF_DELTSQR_HIGH_SET = 0,
ACCEL_AAF_DELTSQR_HIGH_CLEAR = Bit3 | Bit2 | Bit1 | Bit0,
};
namespace FIFO
{
static constexpr size_t SIZE = 2048;
// FIFO_DATA layout when FIFO_CONFIG1 has FIFO_GYRO_EN and FIFO_ACCEL_EN set
// Packet 4
struct DATA {
uint8_t FIFO_Header;
uint8_t ACCEL_DATA_X1; // Accel X [19:12]
uint8_t ACCEL_DATA_X0; // Accel X [11:4]
uint8_t ACCEL_DATA_Y1; // Accel Y [19:12]
uint8_t ACCEL_DATA_Y0; // Accel Y [11:4]
uint8_t ACCEL_DATA_Z1; // Accel Z [19:12]
uint8_t ACCEL_DATA_Z0; // Accel Z [11:4]
uint8_t GYRO_DATA_X1; // Gyro X [19:12]
uint8_t GYRO_DATA_X0; // Gyro X [11:4]
uint8_t GYRO_DATA_Y1; // Gyro Y [19:12]
uint8_t GYRO_DATA_Y0; // Gyro Y [11:4]
uint8_t GYRO_DATA_Z1; // Gyro Z [19:12]
uint8_t GYRO_DATA_Z0; // Gyro Z [11:4]
uint8_t TEMP_DATA1; // Temperature[15:8]
uint8_t TEMP_DATA0; // Temperature[7:0]
uint8_t TimeStamp_h; // TimeStamp[15:8]
uint8_t TimeStamp_l; // TimeStamp[7:0]
uint8_t Ext_Accel_X_Gyro_X; // Accel X [3:0] Gyro X [3:0]
uint8_t Ext_Accel_Y_Gyro_Y; // Accel Y [3:0] Gyro Y [3:0]
uint8_t Ext_Accel_Z_Gyro_Z; // Accel Z [3:0] Gyro Z [3:0]
};
// With FIFO_ACCEL_EN and FIFO_GYRO_EN header should be 8b_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

View File

@ -0,0 +1,116 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "ICM42688P.hpp"
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/module.h>
#include <string>
void ICM42688P::print_usage()
{
PRINT_MODULE_USAGE_NAME("icm42688p", "driver");
PRINT_MODULE_USAGE_SUBCATEGORY("imu");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true);
PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true);
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
}
// I2CSPIDriverBase *ICM42688P::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
// int runtime_instance)
// {
// ICM42688P *instance = new ICM42688P(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.rotation,
// cli.bus_frequency, cli.spi_mode, iterator.DRDYGPIO());
//
// if (!instance) {
// PX4_ERR("alloc failed");
// return nullptr;
// }
//
// if (OK != instance->init()) {
// delete instance;
// return nullptr;
// }
//
// return instance;
// }
extern "C" int icm42688p_main(int argc, char *argv[])
{
for (int i = 0; i <= argc - 1; i++) {
if (std::string(argv[i]) == "-h") {
argv[i] = 0;
hitl_mode = true;
break;
}
}
int ch;
using ThisDriver = ICM42688P;
BusCLIArguments cli{false, true};
cli.default_spi_frequency = SPI_SPEED;
while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) {
switch (ch) {
case 'R':
cli.rotation = (enum Rotation)atoi(cli.optArg());
break;
}
}
const char *verb = cli.optArg();
if (!verb) {
ThisDriver::print_usage();
return -1;
}
BusInstanceIterator iterator(MODULE_NAME, cli, DRV_IMU_DEVTYPE_ICM42688P);
if (!strcmp(verb, "start")) {
return ThisDriver::module_start(cli, iterator);
}
if (!strcmp(verb, "stop")) {
return ThisDriver::module_stop(iterator);
}
if (!strcmp(verb, "status")) {
return ThisDriver::module_status(iterator);
}
ThisDriver::print_usage();
return -1;
}

View File

@ -1,34 +0,0 @@
############################################################################
#
# Copyright (c) 2024 ModalAI, Inc. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
include_directories(${PX4_BOARD_DIR}/libfc-sensor-api/inc)

View File

@ -1,7 +1,7 @@
# Link against the public stub version of the proprietary fc sensor library
target_link_libraries(px4 PRIVATE
${PX4_BOARD_DIR}/libfc-sensor-api/build/libfc_sensor.so
${PX4_SOURCE_DIR}/src//modules/muorb/apps/libfc-sensor-api/build/libfc_sensor.so
px4_layer
${module_libraries}
)

View File

@ -11,6 +11,7 @@ CONFIG_DRIVERS_VOXL2_IO=y
CONFIG_MODULES_COMMANDER=y
CONFIG_MODULES_CONTROL_ALLOCATOR=y
CONFIG_MODULES_DATAMAN=y
CONFIG_MODULES_LOAD_MON=y
CONFIG_MODULES_LOGGER=y
CONFIG_MODULES_MAVLINK=y
CONFIG_MODULES_MUORB_APPS=y
@ -25,4 +26,3 @@ CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
CONFIG_SYSTEMCMDS_UORB=y
CONFIG_SYSTEMCMDS_VER=y
CONFIG_ORB_COMMUNICATOR=y
CONFIG_PARAM_PRIMARY=y

View File

@ -1,9 +1,10 @@
#!/bin/bash
cd boards/modalai/voxl2/libfc-sensor-api
cd src/modules/muorb/apps/libfc-sensor-api
rm -fR build
mkdir build
cd build
CC=/home/4.1.0.4/tools/linaro64/bin/aarch64-linux-gnu-gcc cmake ..
make
cd ../../../../..
cd ../../../../../..

View File

@ -76,13 +76,12 @@ qshell flight_mode_manager start
# Start all of the processing modules on the applications processor
dataman start
navigator start
load_mon start
# Start microdds_client for ros2 offboard messages from agent over localhost
microdds_client start -t udp -h 127.0.0.1 -p 8888
qshell pwm_out_sim start -m hil
# g = gps, m = mag, o = odometry (vio), h = distance sensor, f = optic flow
# qshell dsp_hitl start -g -m -o -h -f
qshell dsp_hitl start -g -m
# start the onboard fast link to connect to voxl-mavlink-server

View File

@ -207,6 +207,7 @@ qshell flight_mode_manager start
# Start all of the processing modules on the applications processor
dataman start
navigator start
load_mon start
# This bridge allows raw data packets to be sent over UART to the ESC
voxl2_io_bridge start

View File

@ -1,11 +1,9 @@
CONFIG_BOARD_TOOLCHAIN="arm-none-eabi"
CONFIG_BOARD_ARCHITECTURE="cortex-m7"
CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS3"
CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS4"
CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS1"
CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS2"
CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS5"
CONFIG_BOARD_SERIAL_TEL4="/dev/ttyS0"
CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS2"
CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS0"
CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS1"
CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS4"
CONFIG_DRIVERS_ADC_ADS1115=y
CONFIG_DRIVERS_ADC_BOARD_ADC=y
CONFIG_DRIVERS_BAROMETER_DPS310=y
@ -27,7 +25,6 @@ CONFIG_DRIVERS_PCA9685_PWM_OUT=y
CONFIG_DRIVERS_POWER_MONITOR_INA226=y
CONFIG_DRIVERS_PWM_OUT=y
CONFIG_DRIVERS_RC_INPUT=y
CONFIG_DRIVERS_SAFETY_BUTTON=y
CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
CONFIG_COMMON_TELEMETRY=y
CONFIG_DRIVERS_TONE_ALARM=y
@ -49,6 +46,7 @@ CONFIG_MODULES_FW_POS_CONTROL=y
CONFIG_MODULES_FW_RATE_CONTROL=y
CONFIG_MODULES_GIMBAL=y
CONFIG_MODULES_GYRO_CALIBRATION=y
CONFIG_MODULES_GYRO_FFT=y
CONFIG_MODULES_LAND_DETECTOR=y
CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
CONFIG_MODULES_LOAD_MON=y
@ -98,3 +96,4 @@ CONFIG_SYSTEMCMDS_UORB=y
CONFIG_SYSTEMCMDS_USB_CONNECTED=y
CONFIG_SYSTEMCMDS_VER=y
CONFIG_SYSTEMCMDS_WORK_QUEUE=y
CONFIG_EXAMPLES_FAKE_GPS=y

View File

@ -6,4 +6,4 @@
param set-default BAT1_V_DIV 10.1
param set-default BAT1_A_PER_V 17
param set-default TEL_FRSKY_CONFIG 103
safety_button start

View File

@ -16,5 +16,3 @@ icm20948 -s -b 1 -R 8 -M start
# Interal DPS310 (barometer)
dps310 -s -b 2 start
safety_button start

View File

@ -15,7 +15,7 @@ CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/mro/ctrl-zero-classic/nuttx-con
CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y
CONFIG_ARCH_BOARD_CUSTOM_NAME="px4"
CONFIG_ARCH_CHIP="stm32h7"
CONFIG_ARCH_CHIP_STM32H743ZI=y
CONFIG_ARCH_CHIP_STM32H743II=y
CONFIG_ARCH_CHIP_STM32H7=y
CONFIG_ARCH_INTERRUPTSTACK=768
CONFIG_ARMV7M_BASEPRI_WAR=y

View File

@ -192,7 +192,7 @@
#define STM32_RCC_D2CCIP1R_SPI45SRC RCC_D2CCIP1R_SPI45SEL_PLL2 /* SPI45 clock source */
#define STM32_RCC_D3CCIPR_SPI6SRC RCC_D3CCIPR_SPI6SEL_PLL2 /* SPI6 clock source */
#define STM32_RCC_D2CCIP2R_USBSRC RCC_D2CCIP2R_USBSEL_PLL3 /* USB 1 and 2 clock source */
#define STM32_RCC_D3CCIPR_ADCSRC RCC_D3CCIPR_ADCSEL_PLL2 /* ADC 1 2 3 clock source */
#define STM32_RCC_D3CCIPR_ADCSEL RCC_D3CCIPR_ADCSEL_PLL2 /* ADC 1 2 3 clock source */
#define STM32_RCC_D2CCIP1R_FDCANSEL RCC_D2CCIP1R_FDCANSEL_HSE /* FDCAN 1 2 clock source */
#define STM32_FDCANCLK STM32_HSE_FREQUENCY
@ -222,9 +222,6 @@
/* UART/USART */
#define GPIO_USART1_TX GPIO_USART1_TX_3 /* PB6 */
#define GPIO_USART1_RX GPIO_USART1_RX_3 /* PB7 */
#define GPIO_USART2_TX GPIO_USART2_TX_2 /* PD5 */
#define GPIO_USART2_RX GPIO_USART2_RX_2 /* PD6 */
#define GPIO_USART2_CTS GPIO_USART2_CTS_NSS_2 /* PD3 */
@ -238,6 +235,9 @@
#define GPIO_UART4_TX GPIO_UART4_TX_2 /* PA0 */
#define GPIO_UART4_RX GPIO_UART4_RX_2 /* PA1 */
#define GPIO_USART6_TX 0 /* USART6 is RX-only */
#define GPIO_USART6_RX GPIO_USART6_RX_1 /* PC7 */
#define GPIO_UART7_TX GPIO_UART7_TX_3 /* PE8 */
#define GPIO_UART7_RX GPIO_UART7_RX_3 /* PE7 */
@ -268,14 +268,13 @@
#define GPIO_SPI5_MISO GPIO_SPI5_MISO_1 /* PF8 */
#define GPIO_SPI5_MOSI GPIO_SPI5_MOSI_2 /* PF9 */
#define GPIO_SPI6_SCK ADJ_SLEW_RATE(GPIO_SPI6_SCK_1) /* PG13 */
#define GPIO_SPI6_MISO GPIO_SPI6_MISO_1 /* PG12 */
#define GPIO_SPI6_MOSI GPIO_SPI6_MOSI_1 /* PG14 */
/* I2C */
#define GPIO_I2C1_SCL GPIO_I2C1_SCL_2 /* PB8 */
#define GPIO_I2C1_SDA GPIO_I2C1_SDA_2 /* PB9 */
#define GPIO_I2C4_SCL GPIO_I2C4_SCL_2 /* PF14 */
#define GPIO_I2C4_SDA GPIO_I2C4_SDA_2 /* PF15 */
#define GPIO_I2C3_SCL GPIO_I2C3_SCL_2 /* PH7 */
#define GPIO_I2C3_SDA GPIO_I2C3_SDA_2 /* PH8 */
#define GPIO_I2C4_SCL GPIO_I2C4_SCL_4 /* PB6 */
#define GPIO_I2C4_SDA GPIO_I2C4_SDA_4 /* PB7 */

View File

@ -56,7 +56,7 @@ CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/mro/ctrl-zero-classic/nuttx-con
CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y
CONFIG_ARCH_BOARD_CUSTOM_NAME="px4"
CONFIG_ARCH_CHIP="stm32h7"
CONFIG_ARCH_CHIP_STM32H743ZI=y
CONFIG_ARCH_CHIP_STM32H743II=y
CONFIG_ARCH_CHIP_STM32H7=y
CONFIG_ARCH_INTERRUPTSTACK=768
CONFIG_ARCH_STACKDUMP=y
@ -192,6 +192,7 @@ CONFIG_STM32H7_DMA2=y
CONFIG_STM32H7_DMACAPABLE=y
CONFIG_STM32H7_FLOWCONTROL_BROKEN=y
CONFIG_STM32H7_I2C1=y
CONFIG_STM32H7_I2C3=y
CONFIG_STM32H7_I2C4=y
CONFIG_STM32H7_I2C_DYNTIMEO=y
CONFIG_STM32H7_I2C_DYNTIMEO_STARTSTOP=10
@ -211,20 +212,17 @@ CONFIG_STM32H7_SPI2=y
CONFIG_STM32H7_SPI5=y
CONFIG_STM32H7_SPI5_DMA=y
CONFIG_STM32H7_SPI5_DMA_BUFFER=1024
CONFIG_STM32H7_SPI6=y
CONFIG_STM32H7_TIM15=y
CONFIG_STM32H7_TIM16=y
CONFIG_STM32H7_TIM1=y
CONFIG_STM32H7_TIM2=y
CONFIG_STM32H7_TIM3=y
CONFIG_STM32H7_TIM4=y
CONFIG_STM32H7_TIM8=y
CONFIG_STM32H7_USART1=y
CONFIG_STM32H7_USART2=y
CONFIG_STM32H7_USART3=y
CONFIG_STM32H7_UART4=y
CONFIG_STM32H7_UART7=y
CONFIG_STM32H7_UART8=y
CONFIG_STM32H7_USART2=y
CONFIG_STM32H7_USART3=y
CONFIG_STM32H7_USART6=y
CONFIG_STM32H7_USART_BREAKS=y
CONFIG_STM32H7_USART_INVERT=y
CONFIG_STM32H7_USART_SINGLEWIRE=y
@ -254,6 +252,9 @@ CONFIG_USART3_IFLOWCONTROL=y
CONFIG_USART3_OFLOWCONTROL=y
CONFIG_USART3_RXBUFSIZE=600
CONFIG_USART3_TXBUFSIZE=3000
CONFIG_USART6_BAUD=57600
CONFIG_USART6_RXBUFSIZE=600
CONFIG_USART6_TXBUFSIZE=1500
CONFIG_USBDEV=y
CONFIG_USBDEV_BUSPOWERED=y
CONFIG_USBDEV_MAXPOWER=500

View File

@ -45,111 +45,95 @@
#include <stm32_gpio.h>
/* LEDs are driven with push open drain to support Anode to 5V or 3.3V */
#define GPIO_nLED_RED /* PB4 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN4)
#define GPIO_nLED_GREEN /* PB5 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN5)
#define GPIO_nLED_RED /* PB11 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN11)
#define GPIO_nLED_GREEN /* PB1 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN1)
#define GPIO_nLED_BLUE /* PB3 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN3)
#define GPIO_LED_SAFETY /* PE6 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN6)
#define BOARD_HAS_CONTROL_STATUS_LEDS 1
#define BOARD_OVERLOAD_LED LED_RED
#define BOARD_ARMED_STATE_LED LED_BLUE
/* ADC channels */
#define PX4_ADC_GPIO \
/* PC4 */ GPIO_ADC12_INP4, \
/* PA2 */ GPIO_ADC12_INP14, \
/* PA3 */ GPIO_ADC12_INP15, \
/* PA4 */ GPIO_ADC12_INP18, \
/* PC0 */ GPIO_ADC123_INP10, \
/* PC5 */ GPIO_ADC12_INP8, \
/* PB0 */ GPIO_ADC12_INP9, \
/* PB1 */ GPIO_ADC12_INP5
/* PC1 */ GPIO_ADC123_INP11
/* Define Channel numbers must match above GPIO pins */
#define ADC_BATTERY_VOLTAGE_CHANNEL 4 /* PC4 BATT_VOLT_SENS */
#define ADC_BATTERY_VOLTAGE_CHANNEL 14 /* PA2 BATT_VOLT_SENS */
#define ADC_BATTERY_CURRENT_CHANNEL 15 /* PA3 BATT_CURRENT_SENS */
#define ADC_SCALED_V5_CHANNEL 18 /* PA4 VDD_5V_SENS */
#define ADC_RC_RSSI_CHANNEL 10 /* PC0 */
#define ADC_AIRSPEED_VOLTAGE_CHANNEL 8 /* PC5 */
#define ADC_AUX1_VOLTAGE_CHANNEL 9 /* PB0 */
#define ADC_AUX2_VOLTAGE_CHANNEL 5 /* PB1 */
#define ADC_RC_RSSI_CHANNEL 11 /* PC1 */
#define ADC_CHANNELS \
((1 << ADC_BATTERY_VOLTAGE_CHANNEL) | \
(1 << ADC_BATTERY_CURRENT_CHANNEL) | \
(1 << ADC_SCALED_V5_CHANNEL) | \
(1 << ADC_RC_RSSI_CHANNEL) | \
(1 << ADC_AIRSPEED_VOLTAGE_CHANNEL) | \
(1 << ADC_AUX1_VOLTAGE_CHANNEL) | \
(1 << ADC_AUX2_VOLTAGE_CHANNEL))
(1 << ADC_RC_RSSI_CHANNEL))
/* HW has to large of R termination on ADC todo:change when HW value is chosen */
#define BOARD_ADC_OPEN_CIRCUIT_V (5.6f)
/* CAN Silence: Silent mode control */
#define GPIO_CAN1_SILENT_S0 /* PF11 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN11)
#define GPIO_CAN2_SILENT_S0 /* PF12 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN12)
#define GPIO_CAN1_SILENT_S0 /* PF5 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN5)
#define GPIO_CAN2_SILENT_S0 /* PF5 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN5)
/* PWM */
#define DIRECT_PWM_OUTPUT_CHANNELS 12
/* Power supply control and monitoring GPIOs */
#define GPIO_nPOWER_IN_A /* PC1 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN1)
#define GPIO_nPOWER_IN_A /* PB5 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5)
#define GPIO_VDD_BRICK1_VALID GPIO_nPOWER_IN_A /* Brick 1 Is Chosen */
#define BOARD_NUMBER_BRICKS 1
#define GPIO_VDD_3V3_SPEKTRUM_POWER_EN /* PE4 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN4)
#define GPIO_LEVEL_SHIFTER_OE /* PC13 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13)
/* Define True logic Power Control in arch agnostic form */
#define VDD_3V3_SPEKTRUM_POWER_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_3V3_SPEKTRUM_POWER_EN, (!on_true))
#define READ_VDD_3V3_SPEKTRUM_POWER_EN() (px4_arch_gpioread(GPIO_VDD_3V3_SPEKTRUM_POWER_EN) == 0)
/* Tone alarm output */
#define TONE_ALARM_TIMER 16 /* timer 16 */
#define TONE_ALARM_CHANNEL 1 /* PF6 TIM16_CH1 */
#define TONE_ALARM_TIMER 2 /* timer 2 */
#define TONE_ALARM_CHANNEL 1 /* PA15 TIM2_CH1 */
#define GPIO_BUZZER_1 /* PF6 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN6)
#define GPIO_BUZZER_1 /* PA15 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN15)
#define GPIO_TONE_ALARM_IDLE GPIO_BUZZER_1
#define GPIO_TONE_ALARM GPIO_TIM16_CH1OUT_2
#define GPIO_TONE_ALARM GPIO_TIM2_CH1OUT_2
/* USB OTG FS */
#define GPIO_OTGFS_VBUS /* PA9 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_100MHz|GPIO_PORTA|GPIO_PIN9)
/* High-resolution timer */
#define HRT_TIMER 3 /* use timer3 for the HRT */
#define HRT_TIMER_CHANNEL 2 /* use capture/compare channel 1 */
#define HRT_TIMER_CHANNEL 1 /* use capture/compare channel 1 */
#define HRT_PPM_CHANNEL /* T3C2 */ 2 /* use capture/compare channel 2 */
#define GPIO_PPM_IN /* PC7 T3C2 */ GPIO_TIM3_CH2IN_3
#define HRT_PPM_CHANNEL /* T3C3 */ 3 /* use capture/compare channel 3 */
#define GPIO_PPM_IN /* PB0 T3C3 */ GPIO_TIM3_CH3IN_1
/* RC Serial port */
#define RC_SERIAL_PORT "/dev/ttyS5"
#define RC_SERIAL_PORT "/dev/ttyS3"
#define GPIO_RSSI_IN /* PC0 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN0)
#define GPIO_RSSI_IN /* PC1 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN1)
/* Safety Switch: Enable the FMU to control it if there is no px4io fixme:This should be BOARD_SAFETY_LED(__ontrue) */
#define GPIO_SAFETY_SWITCH_IN /* PA10 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN10)
/* Enable the FMU to use the switch it if there is no px4io fixme:This should be BOARD_SAFETY_BUTTON() */
#define GPIO_SAFETY_SWITCH_IN /* PC4 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN4)
/* Enable the FMU to use the switch it if there is no px4io fixme:This should be BOARD_SAFTY_BUTTON() */
#define GPIO_BTN_SAFETY GPIO_SAFETY_SWITCH_IN /* Enable the FMU to control it if there is no px4io */
/* No PX4IO processor present */
#define PX4_MFT_HW_SUPPORTED_PX4_MFT_PX4IO 0
/* Power switch controls ******************************************************/
#define SPEKTRUM_POWER(_on_true) VDD_3V3_SPEKTRUM_POWER_EN(_on_true)
/*
* Board has a separate RC_IN
*
* GPIO PPM_IN on PC7 T3CH2
* GPIO PPM_IN on PB0 T3CH3
* SPEKTRUM_RX (it's TX or RX in Bind) on UART6 PC7
* Inversion is possible in the UART and can drive GPIO_PPM_IN as an output
*/
#define GPIO_PPM_IN_AS_OUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN7)
#define GPIO_PPM_IN_AS_OUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN0)
#define SPEKTRUM_RX_AS_GPIO_OUTPUT() px4_arch_configgpio(GPIO_PPM_IN_AS_OUT)
#define SPEKTRUM_RX_AS_UART() /* Can be left as uart */
#define SPEKTRUM_OUT(_one_true) px4_arch_gpiowrite(GPIO_PPM_IN_AS_OUT, (_one_true))
@ -172,7 +156,7 @@
#define BOARD_HAS_STATIC_MANIFEST 1
#define BOARD_NUM_IO_TIMERS 6
#define BOARD_NUM_IO_TIMERS 5
#define BOARD_ENABLE_CONSOLE_BUFFER
@ -187,12 +171,9 @@
GPIO_CAN2_SILENT_S0, \
GPIO_nPOWER_IN_A, \
GPIO_VDD_3V3_SPEKTRUM_POWER_EN, \
GPIO_LEVEL_SHIFTER_OE, \
GPIO_TONE_ALARM_IDLE, \
GPIO_SAFETY_SWITCH_IN, \
GPIO_OTGFS_VBUS, \
GPIO_BTN_SAFETY, \
GPIO_LED_SAFETY, \
}
__BEGIN_DECLS

View File

@ -35,5 +35,6 @@
constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = {
initI2CBusExternal(1),
initI2CBusExternal(3),
initI2CBusExternal(4),
};

View File

@ -37,7 +37,7 @@
constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = {
initSPIBus(SPI::Bus::SPI1, {
initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortG, GPIO::Pin3}, SPI::DRDY{GPIO::PortG, GPIO::Pin2}),
initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortC, GPIO::Pin2}, SPI::DRDY{GPIO::PortD, GPIO::Pin15}),
initSPIDevice(DRV_IMU_DEVTYPE_ICM20948, SPI::CS{GPIO::PortE, GPIO::Pin15}, SPI::DRDY{GPIO::PortE, GPIO::Pin12}),
}, {GPIO::PortE, GPIO::Pin3}),
initSPIBus(SPI::Bus::SPI2, {
@ -46,10 +46,7 @@ constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = {
}),
initSPIBus(SPI::Bus::SPI5, {
initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortF, GPIO::Pin10}, SPI::DRDY{GPIO::PortF, GPIO::Pin3}),
initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortF, GPIO::Pin0}, SPI::DRDY{GPIO::PortF, GPIO::Pin1}),
}),
initSPIBusExternal(SPI::Bus::SPI6, {
initSPIConfigExternal(SPI::CS{GPIO::PortG, GPIO::Pin9}),
initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortF, GPIO::Pin6}, SPI::DRDY{GPIO::PortF, GPIO::Pin1}),
}),
};

View File

@ -33,28 +33,6 @@
#include <px4_arch/io_timer_hw_description.h>
/* Timer allocation
*
* TIM1_CH4 T FMU_CH1
* TIM1_CH3 T FMU_CH2
* TIM1_CH2 T FMU_CH3
* TIM1_CH1 T FMU_CH4
*
* TIM4_CH2 T FMU_CH5
* TIM4_CH3 T FMU_CH6
* TIM2_CH3 T FMU_CH7
* TIM2_CH1 T FMU_CH8
*
* TIM2_CH4 T FMU_CH9
* TIM15_CH1 T FMU_CH10
*
* TIM8_CH1 T FMU_CH11
*
* TIM4_CH4 T FMU_CH12
*
* TIM16_CH1 T BUZZER - Driven by other driver
*/
constexpr io_timers_t io_timers[MAX_IO_TIMERS] = {
initIOTimer(Timer::Timer1, DMA{DMA::Index1}),
initIOTimer(Timer::Timer4, DMA{DMA::Index1}),

View File

@ -192,7 +192,7 @@
#define STM32_RCC_D2CCIP1R_SPI45SRC RCC_D2CCIP1R_SPI45SEL_PLL2 /* SPI45 clock source */
#define STM32_RCC_D3CCIPR_SPI6SRC RCC_D3CCIPR_SPI6SEL_PLL2 /* SPI6 clock source */
#define STM32_RCC_D2CCIP2R_USBSRC RCC_D2CCIP2R_USBSEL_PLL3 /* USB 1 and 2 clock source */
#define STM32_RCC_D3CCIPR_ADCSRC RCC_D3CCIPR_ADCSEL_PLL2 /* ADC 1 2 3 clock source */
#define STM32_RCC_D3CCIPR_ADCSEL RCC_D3CCIPR_ADCSEL_PLL2 /* ADC 1 2 3 clock source */
#define STM32_RCC_D2CCIP1R_FDCANSEL RCC_D2CCIP1R_FDCANSEL_HSE /* FDCAN 1 2 clock source */
#define STM32_FDCANCLK STM32_HSE_FREQUENCY

View File

@ -191,7 +191,7 @@
#define STM32_RCC_D2CCIP1R_SPI45SRC RCC_D2CCIP1R_SPI45SEL_PLL2 /* SPI45 clock source */
#define STM32_RCC_D3CCIPR_SPI6SRC RCC_D3CCIPR_SPI6SEL_PLL2 /* SPI6 clock source */
#define STM32_RCC_D2CCIP2R_USBSRC RCC_D2CCIP2R_USBSEL_PLL3 /* USB 1 and 2 clock source */
#define STM32_RCC_D3CCIPR_ADCSRC RCC_D3CCIPR_ADCSEL_PLL2 /* ADC 1 2 3 clock source */
#define STM32_RCC_D3CCIPR_ADCSEL RCC_D3CCIPR_ADCSEL_PLL2 /* ADC 1 2 3 clock source */
#define STM32_RCC_D2CCIP1R_FDCANSEL RCC_D2CCIP1R_FDCANSEL_HSE /* FDCAN 1 2 clock source */
#define STM32_FDCANCLK STM32_HSE_FREQUENCY

View File

@ -191,7 +191,7 @@
#define STM32_RCC_D2CCIP1R_SPI45SRC RCC_D2CCIP1R_SPI45SEL_PLL2 /* SPI45 clock source */
#define STM32_RCC_D3CCIPR_SPI6SRC RCC_D3CCIPR_SPI6SEL_PLL2 /* SPI6 clock source */
#define STM32_RCC_D2CCIP2R_USBSRC RCC_D2CCIP2R_USBSEL_PLL3 /* USB 1 and 2 clock source */
#define STM32_RCC_D3CCIPR_ADCSRC RCC_D3CCIPR_ADCSEL_PLL2 /* ADC 1 2 3 clock source */
#define STM32_RCC_D3CCIPR_ADCSEL RCC_D3CCIPR_ADCSEL_PLL2 /* ADC 1 2 3 clock source */
#define STM32_RCC_D2CCIP1R_FDCANSEL RCC_D2CCIP1R_FDCANSEL_HSE /* FDCAN 1 2 clock source */
#define STM32_FDCANCLK STM32_HSE_FREQUENCY

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