fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 05:04:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'main' of github.com:PX4/PX4-Autopilot into pr-fw_ctrl_api

Browse Source
This commit is contained in:
RomanBapst 2025-02-04 11:10:04 +03:00
commit 37fe25570d
220 changed files with 3050 additions and 979 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
.github/workflows/dev_container.yml vendored
View File

@ -17,7 +17,7 @@ on:
jobs:
build:
name: Build and Push Container
runs-on: [runs-on,runner=8cpu-linux-x64,image=ubuntu22-full-x64,"run-id=${{ github.run_id }}",spot=false]
runs-on: [runs-on,runner=8cpu-linux-x64,image=ubuntu24-full-x64,"run-id=${{ github.run_id }}",spot=false]
steps:
- uses: actions/checkout@v4
with:
@ -52,12 +52,7 @@ jobs:
ghcr.io/PX4/px4-dev
${{ (github.event_name != 'pull_request') && 'px4io/px4-dev' || '' }}
tags: |
type=semver,pattern={{version}}
type=semver,pattern={{major}}.{{minor}}
type=semver,pattern={{major}}
type=ref,event=branch,value=${{ steps.px4-tag.outputs.tag }},priority=700
type=ref,event=branch,suffix=-{{date 'YYYY-MM-DD'}},priority=600
type=ref,event=branch,suffix=,priority=500
type=raw,enable=true,value=${{ steps.px4-tag.outputs.tag }},priority=1000
- name: Set up Docker Buildx
uses: docker/setup-buildx-action@v3

4
ROMFS/px4fmu_common/init.d-posix/airframes/10040_sihsim_quadx
View File

@ -16,10 +16,6 @@ param set-default SENS_EN_GPSSIM 1
param set-default SENS_EN_BAROSIM 1
param set-default SENS_EN_MAGSIM 1
# disable some checks to allow to fly:
# - without real battery
param set-default CBRK_SUPPLY_CHK 894281
# Square quadrotor X PX4 numbering
param set-default CA_ROTOR_COUNT 4
param set-default CA_ROTOR0_PX 1

19
ROMFS/px4fmu_common/init.d-posix/airframes/10041_sihsim_airplane
View File

@ -16,12 +16,6 @@ param set-default SENS_EN_BAROSIM 1
param set-default SENS_EN_MAGSIM 1
param set-default SENS_EN_ARSPDSIM 1
# disable some checks to allow to fly:
# - with usb
param set-default CBRK_USB_CHK 197848
# - without real battery
param set-default CBRK_SUPPLY_CHK 894281
param set-default SIH_T_MAX 6
param set-default SIH_MASS 0.3
param set-default SIH_IXX 0.00402
@ -33,18 +27,19 @@ param set-default SIH_KDV 0.2
param set-default SIH_VEHICLE_TYPE 1 # sih as fixed wing
param set-default RWTO_TKOFF 1 # enable takeoff from runway (as opposed to launched)
param set-default CA_AIRFRAME 1
param set-default CA_ROTOR_COUNT 1
param set-default CA_ROTOR0_PX 0.3
# SIH for now hardcodes this configuration which we need to match in the airframe files.
param set-default CA_SV_CS_COUNT 3
param set-default CA_SV_CS0_TRQ_R -0.5
param set-default CA_SV_CS0_TYPE 1
param set-default CA_SV_CS0_TRQ_R 1
param set-default CA_SV_CS0_TYPE 15 # single channel aileron
param set-default CA_SV_CS1_TRQ_P 1
param set-default CA_SV_CS1_TYPE 3
param set-default CA_SV_CS1_TYPE 3 # elevator
param set-default CA_SV_CS2_TRQ_Y 1
param set-default CA_SV_CS2_TYPE 4
param set-default CA_SV_CS2_TYPE 4 # rudder
param set-default PWM_MAIN_FUNC1 201
param set-default PWM_MAIN_FUNC2 202
param set-default PWM_MAIN_FUNC3 203

4
ROMFS/px4fmu_common/init.d-posix/airframes/10042_sihsim_xvert
View File

@ -28,10 +28,6 @@ param set-default MC_PITCH_P 5
param set-default MAV_TYPE 19
# disable some checks to allow to fly:
# - without real battery
param set-default CBRK_SUPPLY_CHK 894281
param set-default SIH_T_MAX 2
param set-default SIH_Q_MAX 0.0165
param set-default SIH_MASS 0.2

96
ROMFS/px4fmu_common/init.d-posix/airframes/10043_sihsim_standard_vtol Normal file
View File

@ -0,0 +1,96 @@
#!/bin/sh
#
# @name SIH Standard VTOL
#
# @type Simulation
# @class VTOL
#
# @output Motor1 MC motor front right
# @output Motor2 MC motor back left
# @output Motor3 MC motor front left
# @output Motor4 MC motor back right
# @output Motor5 Forward thrust motor
# @output Servo1 Ailerons (single channel)
# @output Servo2 Elevator
# @output Servo3 Rudder
#
# @board px4_fmu-v2 exclude
#
. ${R}etc/init.d/rc.vtol_defaults
PX4_SIMULATOR=${PX4_SIMULATOR:=sihsim}
PX4_SIM_MODEL=${PX4_SIM_MODEL:=standard_vtol}
param set-default SENS_EN_GPSSIM 1
param set-default SENS_EN_BAROSIM 1
param set-default SENS_EN_MAGSIM 1
param set-default SENS_EN_ARSPDSIM 1
param set-default EKF2_GPS_DELAY 0
param set-default VT_TYPE 2
param set-default MPC_MAN_Y_MAX 60
param set-default MC_PITCH_P 5
param set-default CA_AIRFRAME 2
param set-default CA_ROTOR_COUNT 5
param set-default CA_ROTOR0_KM 0.05
param set-default CA_ROTOR0_PX 0.2
param set-default CA_ROTOR0_PY 0.2
param set-default CA_ROTOR1_KM 0.05
param set-default CA_ROTOR1_PX -0.2
param set-default CA_ROTOR1_PY -0.2
param set-default CA_ROTOR2_PX 0.2
param set-default CA_ROTOR2_PY -0.2
param set-default CA_ROTOR2_KM -0.05
param set-default CA_ROTOR3_PX -0.2
param set-default CA_ROTOR3_PY 0.2
param set-default CA_ROTOR3_KM -0.05
param set-default CA_ROTOR4_PX -0.3
param set-default CA_ROTOR4_KM 0.05
param set-default CA_ROTOR4_AX 1
param set-default CA_ROTOR4_AZ 0
# SIH for now hardcodes this configuration which we need to match in the airframe files.
param set-default CA_SV_CS_COUNT 3
param set-default CA_SV_CS0_TRQ_R 1
param set-default CA_SV_CS0_TYPE 15 # single channel aileron
param set-default CA_SV_CS1_TRQ_P 1
param set-default CA_SV_CS1_TYPE 3 # elevator
param set-default CA_SV_CS2_TRQ_Y 1
param set-default CA_SV_CS2_TYPE 4 # rudder
param set-default FW_AIRSPD_MIN 7
param set-default FW_AIRSPD_TRIM 10
param set-default FW_AIRSPD_MAX 12
param set-default PWM_MAIN_FUNC1 101
param set-default PWM_MAIN_FUNC2 102
param set-default PWM_MAIN_FUNC3 103
param set-default PWM_MAIN_FUNC4 104
param set-default PWM_MAIN_FUNC5 201
param set-default PWM_MAIN_FUNC6 202
param set-default PWM_MAIN_FUNC7 203
param set-default PWM_MAIN_FUNC8 105
param set-default MAV_TYPE 22
# set SYS_HITL to 2 to start the SIH and avoid sensors startup
# param set-default SYS_HITL 2
param set-default SENS_DPRES_OFF 0.001
param set SIH_T_MAX 2.0
param set SIH_Q_MAX 0.0165
param set SIH_MASS 0.2
# IXX and IZZ are inverted from the thesis as the body frame is pitched by 90 deg
param set SIH_IXX 0.00354
param set SIH_IYY 0.000625
param set SIH_IZZ 0.00300
param set SIH_IXZ 0
param set SIH_KDV 0.2
param set SIH_L_ROLL 0.2
# sih as standard vtol
param set SIH_VEHICLE_TYPE 3

1
ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt
View File

@ -109,6 +109,7 @@ px4_add_romfs_files(
10040_sihsim_quadx
10041_sihsim_airplane
10042_sihsim_xvert
10043_sihsim_standard_vtol
17001_flightgear_tf-g1
17002_flightgear_tf-g2

3
ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator
View File

@ -4,6 +4,9 @@
# Simulator IMU data provided at 250 Hz
param set-default IMU_INTEG_RATE 250
# For simulation, allow registering modes while armed for developer convenience
param set-default COM_MODE_ARM_CHK 1
if [ "$PX4_SIMULATOR" = "sihsim" ] || [ "$(param show -q SYS_AUTOSTART)" -eq "0" ]; then
echo "INFO [init] SIH simulator"

9
ROMFS/px4fmu_common/init.d/airframes/1002_standard_vtol.hil
View File

@ -5,6 +5,15 @@
# @type Standard VTOL
# @class VTOL
#
# @output Motor1 MC motor front right
# @output Motor2 MC motor back left
# @output Motor3 MC motor front left
# @output Motor4 MC motor back right
# @output Motor5 Forward thrust motor
# @output Servo1 Aileron
# @output Servo2 Elevator
# @output Servo3 Rudder
#
# @maintainer Roman Bapst <roman@auterion.com>
#
# @board px4_fmu-v2 exclude

17
ROMFS/px4fmu_common/init.d/airframes/1101_rc_plane_sih.hil
View File

@ -17,15 +17,16 @@ param set UAVCAN_ENABLE 0
param set-default CA_AIRFRAME 1
param set-default CA_ROTOR_COUNT 1
param set-default CA_ROTOR0_PX 0.3
param set-default CA_SV_CS_COUNT 3
param set-default CA_SV_CS0_TRQ_R -0.5
param set-default CA_SV_CS0_TYPE 1
param set-default CA_SV_CS1_TRQ_P 1
param set-default CA_SV_CS1_TYPE 3
param set-default CA_SV_CS2_TRQ_Y 1
param set-default CA_SV_CS2_TYPE 4
param set HIL_ACT_REV 1
# SIH for now hardcodes this configuration which we need to match in the airframe files.
param set-default CA_SV_CS_COUNT 3
param set-default CA_SV_CS0_TRQ_R 1
param set-default CA_SV_CS0_TYPE 15 # single channel aileron
param set-default CA_SV_CS1_TRQ_P 1
param set-default CA_SV_CS1_TYPE 3 # elevator
param set-default CA_SV_CS2_TRQ_Y 1
param set-default CA_SV_CS2_TYPE 4 # rudder
param set HIL_ACT_FUNC1 201
param set HIL_ACT_FUNC2 202
param set HIL_ACT_FUNC3 203

92
ROMFS/px4fmu_common/init.d/airframes/1103_standard_vtol_sih.hil Normal file
View File

@ -0,0 +1,92 @@
#!/bin/sh
#
# @name SIH Standard VTOL QuadPlane
#
# @type Simulation
# @class VTOL
#
# @output Motor1 MC motor front right
# @output Motor2 MC motor back left
# @output Motor3 MC motor front left
# @output Motor4 MC motor back right
# @output Motor5 Forward thrust motor
# @output Servo1 Ailerons (single channel)
# @output Servo2 Elevator
# @output Servo3 Rudder
#
# @board px4_fmu-v2 exclude
#
. ${R}etc/init.d/rc.vtol_defaults
param set UAVCAN_ENABLE 0
param set-default VT_TYPE 2
param set-default MPC_MAN_Y_MAX 60
param set-default MC_PITCH_P 5
param set-default CA_AIRFRAME 2
param set-default CA_ROTOR_COUNT 5
param set-default CA_ROTOR0_KM 0.05
param set-default CA_ROTOR0_PX 0.2
param set-default CA_ROTOR0_PY 0.2
param set-default CA_ROTOR1_KM 0.05
param set-default CA_ROTOR1_PX -0.2
param set-default CA_ROTOR1_PY -0.2
param set-default CA_ROTOR2_PX 0.2
param set-default CA_ROTOR2_PY -0.2
param set-default CA_ROTOR2_KM -0.05
param set-default CA_ROTOR3_PX -0.2
param set-default CA_ROTOR3_PY 0.2
param set-default CA_ROTOR3_KM -0.05
param set-default CA_ROTOR4_PX -0.3
param set-default CA_ROTOR4_KM 0.05
param set-default CA_ROTOR4_AX 1
param set-default CA_ROTOR4_AZ 0
# SIH for now hardcodes this configuration which we need to match in the airframe files.
param set-default CA_SV_CS_COUNT 3
param set-default CA_SV_CS0_TRQ_R 1
param set-default CA_SV_CS0_TYPE 15 # single channel aileron
param set-default CA_SV_CS1_TRQ_P 1
param set-default CA_SV_CS1_TYPE 3 # elevator
param set-default CA_SV_CS2_TRQ_Y 1
param set-default CA_SV_CS2_TYPE 4 # rudder
param set-default FW_AIRSPD_MIN 7
param set-default FW_AIRSPD_TRIM 10
param set-default FW_AIRSPD_MAX 12
param set-default HIL_ACT_FUNC1 101
param set-default HIL_ACT_FUNC2 102
param set-default HIL_ACT_FUNC3 103
param set-default HIL_ACT_FUNC4 104
param set-default HIL_ACT_FUNC5 201
param set-default HIL_ACT_FUNC6 202
param set-default HIL_ACT_FUNC7 203
param set-default HIL_ACT_FUNC8 105
param set-default MAV_TYPE 22
# set SYS_HITL to 2 to start the SIH and avoid sensors startup
param set-default SYS_HITL 2
# disable some checks to allow to fly:
# - without real battery
param set-default CBRK_SUPPLY_CHK 894281
param set-default SENS_DPRES_OFF 0.001
param set SIH_T_MAX 2.0
param set SIH_Q_MAX 0.0165
param set SIH_MASS 0.2
# IXX and IZZ are inverted from the thesis as the body frame is pitched by 90 deg
param set SIH_IXX 0.00354
param set SIH_IYY 0.000625
param set SIH_IZZ 0.00300
param set SIH_IXZ 0
param set SIH_KDV 0.2
param set SIH_L_ROLL 0.2
# sih as standard vtol
param set SIH_VEHICLE_TYPE 3

1
ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt
View File

@ -48,6 +48,7 @@ if(CONFIG_MODULES_SIMULATION_PWM_OUT_SIM)
1100_rc_quad_x_sih.hil
1101_rc_plane_sih.hil
1102_tailsitter_duo_sih.hil
1103_standard_vtol_sih.hil
)
endif()

1
ROMFS/px4fmu_common/init.d/rc.fw_defaults
View File

@ -24,7 +24,6 @@ param set-default COM_DISARM_PRFLT -1
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_REQ_EPH 10
param set-default EKF2_REQ_EPV 10

1
ROMFS/px4fmu_common/init.d/rc.heli_defaults
View File

@ -12,6 +12,7 @@ param set-default MAV_TYPE 4
param set-default COM_PREARM_MODE 2
param set-default COM_SPOOLUP_TIME 10
param set-default COM_DISARM_PRFLT 60
# No need for minimum collective pitch (or airmode) to keep torque authority
param set-default MPC_MANTHR_MIN 0

3
ROMFS/px4fmu_common/init.d/rc.mc_defaults
View File

@ -21,3 +21,6 @@ param set-default RTL_RETURN_ALT 30
param set-default RTL_DESCEND_ALT 10
param set-default GPS_UBX_DYNMODEL 6
# lower RNG_FOG since MC are expected to fly closer over obstacles
param set-default EKF2_RNG_FOG 1.0

130
ROMFS/px4fmu_common/init.d/rcS
View File

@ -302,6 +302,75 @@ else
. $FCONFIG
fi
#
# Sensors System (start before Commander so Preflight checks are properly run).
#
if param greater SYS_HITL 0
then
sensors start -h
# disable GPS
param set GPS_1_CONFIG 0
# start the simulator in hardware if needed
if param compare SYS_HITL 2
then
simulator_sih start
sensor_baro_sim start
sensor_mag_sim start
sensor_gps_sim start
sensor_agp_sim start
fi
else
#
# board sensors: rc.sensors
#
set BOARD_RC_SENSORS ${R}etc/init.d/rc.board_sensors
if [ -f $BOARD_RC_SENSORS ]
then
echo "Board sensors: ${BOARD_RC_SENSORS}"
. $BOARD_RC_SENSORS
fi
unset BOARD_RC_SENSORS
. ${R}etc/init.d/rc.sensors
if param compare -s BAT1_SOURCE 2
then
esc_battery start
fi
if ! param compare BAT1_SOURCE 1
then
battery_status start
fi
sensors start
fi
#
# state estimator selection
#
if param compare -s EKF2_EN 1
then
ekf2 start &
fi
if param compare -s LPE_EN 1
then
local_position_estimator start
fi
if param compare -s ATT_EN 1
then
attitude_estimator_q start
fi
#
# px4io
#
if px4io supported
then
# Check if PX4IO present and update firmware if needed.
@ -369,79 +438,24 @@ else
fi
#
# Sensors System (start before Commander so Preflight checks are properly run).
# Commander needs to be this early for in-air-restarts.
# Commander
#
if param greater SYS_HITL 0
then
commander start -h
if ! pwm_out_sim start -m hil
then
tune_control play error
fi
sensors start -h
commander start -h
# disable GPS
param set GPS_1_CONFIG 0
# start the simulator in hardware if needed
if param compare SYS_HITL 2
then
simulator_sih start
sensor_baro_sim start
sensor_mag_sim start
sensor_gps_sim start
sensor_agp_sim start
fi
else
#
# board sensors: rc.sensors
#
set BOARD_RC_SENSORS ${R}etc/init.d/rc.board_sensors
if [ -f $BOARD_RC_SENSORS ]
then
echo "Board sensors: ${BOARD_RC_SENSORS}"
. $BOARD_RC_SENSORS
fi
unset BOARD_RC_SENSORS
. ${R}etc/init.d/rc.sensors
if param compare -s BAT1_SOURCE 2
then
esc_battery start
fi
if ! param compare BAT1_SOURCE 1
then
battery_status start
fi
sensors start
commander start
dshot start
pwm_out start
fi
#
# state estimator selection
if param compare -s EKF2_EN 1
then
ekf2 start &
fi
if param compare -s LPE_EN 1
then
local_position_estimator start
fi
if param compare -s ATT_EN 1
then
attitude_estimator_q start
fi
#
# Configure vehicle type specific parameters.
# Note: rc.vehicle_setup is the entry point for all vehicle type specific setup.

3
Tools/px_uploader.py
View File

@ -189,7 +189,7 @@ class uploader:
GET_CHIP = b'\x2c' # rev5+ , get chip version
SET_BOOT_DELAY = b'\x2d' # rev5+ , set boot delay
GET_CHIP_DES = b'\x2e' # rev5+ , get chip description in ASCII
GET_VERSION = b'\x2f' # rev5+ , get chip description in ASCII
GET_VERSION = b'\x2f' # rev5+ , get bootloader version in ASCII
CHIP_FULL_ERASE = b'\x40' # full erase of flash, rev6+
MAX_DES_LENGTH = 20
@ -201,7 +201,6 @@ class uploader:
INFO_BOARD_ID = b'\x02' # board type
INFO_BOARD_REV = b'\x03' # board revision
INFO_FLASH_SIZE = b'\x04' # max firmware size in bytes
BL_VERSION = b'\x07' # get bootloader version, e.g. major.minor.patch.githash (up to 20 chars)
PROG_MULTI_MAX = 252 # protocol max is 255, must be multiple of 4
READ_MULTI_MAX = 252 # protocol max is 255

2
Tools/simulation/gazebo-classic/sitl_gazebo-classic

@ -1 +1 @@
Subproject commit c0e0751341d46108377bbae2ae1bb6da8a5d4106
Subproject commit fbd8e9e6bbd2188de81677494f15885dead99c48

3
boards/ark/fmu-v6x/init/rc.board_defaults
View File

@ -3,6 +3,8 @@
# board specific defaults
#------------------------------------------------------------------------------
param set-default EKF2_MULTI_IMU 0
# Mavlink ethernet (CFG 1000)
param set-default MAV_2_CONFIG 1000
param set-default MAV_2_BROADCAST 1
@ -17,6 +19,7 @@ param set-default SENS_EN_INA228 0
param set-default SENS_EN_INA226 1
param set-default SENS_EN_THERMAL 1
param set-default SENS_IMU_MODE 1
param set-default SENS_IMU_TEMP 10.0
#param set-default SENS_IMU_TEMP_FF 0.0
#param set-default SENS_IMU_TEMP_I 0.025

7
boards/ark/fmu-v6x/nuttx-config/nsh/defconfig
View File

@ -145,10 +145,11 @@ CONFIG_NETDB_DNSCLIENT=y
CONFIG_NETDB_DNSCLIENT_ENTRIES=8
CONFIG_NETDB_DNSSERVER_NOADDR=y
CONFIG_NETDEV_PHY_IOCTL=y
CONFIG_NETINIT_DHCPC=y
CONFIG_NETMAN_FALLBACK_IPADDR=0xC0A80004
CONFIG_NETINIT_DNS=y
CONFIG_NETINIT_DNSIPADDR=0xA290AFE
CONFIG_NETINIT_DRIPADDR=0xA290AFE
CONFIG_NETINIT_IPADDR=0xC0A80004
CONFIG_NETINIT_DNSIPADDR=0xC0A800FE
CONFIG_NETINIT_DRIPADDR=0xC0A80001
CONFIG_NETINIT_MONITOR=y
CONFIG_NETINIT_THREAD=y
CONFIG_NETINIT_THREAD_PRIORITY=49

3
boards/ark/fpv/default.px4board
View File

@ -16,9 +16,9 @@ CONFIG_DRIVERS_GPS=y
CONFIG_DRIVERS_HEATER=y
CONFIG_DRIVERS_IMU_INVENSENSE_IIM42653=y
CONFIG_DRIVERS_IMU_MURATA_SCH16T=y
CONFIG_DRIVERS_OSD_MSP_OSD=y
CONFIG_COMMON_LIGHT=y
CONFIG_COMMON_MAGNETOMETER=y
CONFIG_DRIVERS_OSD_MSP_OSD=y
CONFIG_DRIVERS_PWM_OUT=y
CONFIG_COMMON_RC=y
CONFIG_DRIVERS_UAVCAN=y
@ -37,6 +37,7 @@ CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
CONFIG_MODULES_FW_POS_CONTROL=y
CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_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

3
boards/ark/pi6x/init/rc.board_defaults
View File

@ -32,10 +32,11 @@ then
param set-default SENS_TEMP_ID 2490378
fi
param set-default EKF2_MULTI_IMU 2
param set-default EKF2_MULTI_IMU 0
param set-default EKF2_OF_CTRL 1
param set-default EKF2_OF_N_MIN 0.05
param set-default EKF2_RNG_A_HMAX 25
param set-default EKF2_RNG_QLTY_T 0.1
param set-default SENS_FLOW_RATE 150
param set-default SENS_IMU_MODE 1

2
boards/bitcraze/crazyflie/default.px4board
View File

@ -31,7 +31,7 @@ CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
CONFIG_MODULES_MC_POS_CONTROL=y
CONFIG_MODULES_MC_RATE_CONTROL=y
CONFIG_MODULES_NAVIGATOR=y
CONFIG_NAVIGATOR_ADSB=n
# CONFIG_NAVIGATOR_ADSB is not set
CONFIG_MODULES_RC_UPDATE=y
CONFIG_MODULES_SENSORS=y
# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set

2
boards/bitcraze/crazyflie21/default.px4board
View File

@ -32,7 +32,7 @@ CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
CONFIG_MODULES_MC_POS_CONTROL=y
CONFIG_MODULES_MC_RATE_CONTROL=y
CONFIG_MODULES_NAVIGATOR=y
CONFIG_NAVIGATOR_ADSB=n
# CONFIG_NAVIGATOR_ADSB is not set
CONFIG_MODULES_RC_UPDATE=y
CONFIG_MODULES_SENSORS=y
# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set

1
boards/cuav/7-nano/src/board_config.h
View File

@ -352,6 +352,7 @@
GPIO_nSAFETY_SWITCH_LED_OUT, \
GPIO_SAFETY_SWITCH_IN, \
GPIO_nARMED, \
GPIO_PWM_LEVEL_CONTROL, \
}
#define BOARD_ENABLE_CONSOLE_BUFFER

2
boards/diatone/mamba-f405-mk2/default.px4board
View File

@ -36,7 +36,7 @@ CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
CONFIG_MODULES_MC_POS_CONTROL=y
CONFIG_MODULES_MC_RATE_CONTROL=y
CONFIG_MODULES_NAVIGATOR=y
CONFIG_NAVIGATOR_ADSB=n
# CONFIG_NAVIGATOR_ADSB is not set
CONFIG_MODULES_RC_UPDATE=y
CONFIG_MODULES_SENSORS=y
CONFIG_SYSTEMCMDS_DMESG=y

2
boards/flywoo/gn-f405/default.px4board
View File

@ -27,7 +27,7 @@ CONFIG_MODULES_MC_ATT_CONTROL=y
CONFIG_MODULES_MC_POS_CONTROL=y
CONFIG_MODULES_MC_RATE_CONTROL=y
CONFIG_MODULES_NAVIGATOR=y
CONFIG_NAVIGATOR_ADSB=n
# CONFIG_NAVIGATOR_ADSB is not set
CONFIG_MODULES_RC_UPDATE=y
CONFIG_MODULES_SENSORS=y
CONFIG_SYSTEMCMDS_DMESG=y

3
boards/holybro/kakutef7/default.px4board
View File

@ -23,6 +23,7 @@ CONFIG_MODULES_COMMANDER=y
CONFIG_MODULES_CONTROL_ALLOCATOR=y
CONFIG_MODULES_DATAMAN=y
CONFIG_MODULES_EKF2=y
# CONFIG_EKF2_AUX_GLOBAL_POSITION is not set
# CONFIG_EKF2_AUXVEL is not set
# CONFIG_EKF2_BARO_COMPENSATION is not set
# CONFIG_EKF2_DRAG_FUSION is not set
@ -40,7 +41,7 @@ CONFIG_MODULES_MC_ATT_CONTROL=y
CONFIG_MODULES_MC_POS_CONTROL=y
CONFIG_MODULES_MC_RATE_CONTROL=y
CONFIG_MODULES_NAVIGATOR=y
CONFIG_NAVIGATOR_ADSB=n
# CONFIG_NAVIGATOR_ADSB is not set
CONFIG_MODULES_RC_UPDATE=y
CONFIG_MODULES_SENSORS=y
# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set

1
boards/holybro/kakuteh7/default.px4board
View File

@ -34,6 +34,7 @@ CONFIG_MODULES_COMMANDER=y
CONFIG_MODULES_CONTROL_ALLOCATOR=y
CONFIG_MODULES_DATAMAN=y
CONFIG_MODULES_EKF2=y
# CONFIG_EKF2_AUX_GLOBAL_POSITION is not set
# CONFIG_EKF2_AUXVEL is not set
# CONFIG_EKF2_BARO_COMPENSATION is not set
# CONFIG_EKF2_DRAG_FUSION is not set

1
boards/holybro/kakuteh7mini/default.px4board
View File

@ -34,6 +34,7 @@ CONFIG_MODULES_COMMANDER=y
CONFIG_MODULES_CONTROL_ALLOCATOR=y
CONFIG_MODULES_DATAMAN=y
CONFIG_MODULES_EKF2=y
# CONFIG_EKF2_AUX_GLOBAL_POSITION is not set
# CONFIG_EKF2_AUXVEL is not set
# CONFIG_EKF2_BARO_COMPENSATION is not set
# CONFIG_EKF2_DRAG_FUSION is not set

1
boards/holybro/kakuteh7v2/default.px4board
View File

@ -34,6 +34,7 @@ CONFIG_MODULES_COMMANDER=y
CONFIG_MODULES_CONTROL_ALLOCATOR=y
CONFIG_MODULES_DATAMAN=y
CONFIG_MODULES_EKF2=y
# CONFIG_EKF2_AUX_GLOBAL_POSITION is not set
# CONFIG_EKF2_AUXVEL is not set
# CONFIG_EKF2_BARO_COMPENSATION is not set
# CONFIG_EKF2_DRAG_FUSION is not set

6
boards/micoair/h743-aio/default.px4board
View File

@ -10,11 +10,15 @@ CONFIG_BOARD_SERIAL_RC="/dev/ttyS4"
CONFIG_DRIVERS_ADC_BOARD_ADC=y
CONFIG_DRIVERS_BAROMETER_DPS310=y
CONFIG_DRIVERS_CDCACM_AUTOSTART=y
CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_ASP5033=y
CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_AUAV=y
CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_ETS=y
CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_MS4515=y
CONFIG_DRIVERS_DSHOT=y
CONFIG_DRIVERS_GPS=y
CONFIG_DRIVERS_IMU_BOSCH_BMI088=y
CONFIG_DRIVERS_IMU_BOSCH_BMI270=y
CONFIG_COMMON_INS=y
CONFIG_COMMON_MAGNETOMETER=y
CONFIG_DRIVERS_PPS_CAPTURE=y
CONFIG_DRIVERS_PWM_OUT=y

10
boards/micoair/h743-v2/default.px4board
View File

@ -11,21 +11,25 @@ CONFIG_BOARD_SERIAL_RC="/dev/ttyS5"
CONFIG_DRIVERS_ADC_BOARD_ADC=y
CONFIG_DRIVERS_BAROMETER_GOERTEK_SPL06=y
CONFIG_DRIVERS_CDCACM_AUTOSTART=y
CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_ASP5033=y
CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_AUAV=y
CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_ETS=y
CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_MS4515=y
CONFIG_DRIVERS_DSHOT=y
CONFIG_DRIVERS_GPS=y
CONFIG_DRIVERS_IMU_BOSCH_BMI088=y
CONFIG_DRIVERS_IMU_BOSCH_BMI270=y
CONFIG_COMMON_INS=y
CONFIG_COMMON_MAGNETOMETER=y
CONFIG_DRIVERS_PPS_CAPTURE=y
CONFIG_DRIVERS_PWM_OUT=y
CONFIG_COMMON_RC=y
CONFIG_DRIVERS_RC_INPUT=y
CONFIG_DRIVERS_TAP_ESC=y
CONFIG_DRIVERS_UAVCAN=y
CONFIG_BOARD_UAVCAN_INTERFACES=1
CONFIG_COMMON_TELEMETRY=y
CONFIG_DRIVERS_TONE_ALARM=y
CONFIG_DRIVERS_UAVCAN=y
CONFIG_BOARD_UAVCAN_INTERFACES=1
CONFIG_MODULES_BATTERY_STATUS=y
CONFIG_MODULES_COMMANDER=y
CONFIG_MODULES_CONTROL_ALLOCATOR=y

1
boards/micoair/h743-v2/nuttx-config/bootloader/defconfig
View File

@ -35,7 +35,6 @@ CONFIG_CDCACM_PRODUCTID=0x004b
CONFIG_CDCACM_PRODUCTSTR="MicoAir743v2"
CONFIG_CDCACM_RXBUFSIZE=6000
CONFIG_CDCACM_TXBUFSIZE=12000
#TODO：ally for VENDOR ID in the future
CONFIG_CDCACM_VENDORID=0x3162
CONFIG_CDCACM_VENDORSTR="MicoAir"
CONFIG_DEBUG_FULLOPT=y

11
boards/micoair/h743-v2/nuttx-config/nsh/defconfig
View File

@ -196,17 +196,17 @@ CONFIG_STM32H7_SDMMC1=y
CONFIG_STM32H7_SERIALBRK_BSDCOMPAT=y
CONFIG_STM32H7_SERIAL_DISABLE_REORDERING=y
CONFIG_STM32H7_SPI2=y
CONFIG_STM32H7_SPI3=y
CONFIG_STM32H7_SPI2_DMA=y
CONFIG_STM32H7_SPI2_DMA_BUFFER=2048
CONFIG_STM32H7_SPI3=y
CONFIG_STM32H7_SPI3_DMA=y
CONFIG_STM32H7_SPI3_DMA_BUFFER=2048
CONFIG_STM32H7_SPI_DMATHRESHOLD=8
CONFIG_STM32H7_TIM15=y
CONFIG_STM32H7_TIM1=y
CONFIG_STM32H7_TIM3=y
CONFIG_STM32H7_TIM4=y
CONFIG_STM32H7_TIM8=y
CONFIG_STM32H7_TIM15=y
CONFIG_STM32H7_UART4=y
CONFIG_STM32H7_UART5=y
CONFIG_STM32H7_UART7=y
@ -222,25 +222,18 @@ CONFIG_STM32H7_USART_SWAP=y
CONFIG_SYSTEM_CDCACM=y
CONFIG_SYSTEM_NSH=y
CONFIG_TASK_NAME_SIZE=24
CONFIG_UART4_BAUD=115200
CONFIG_UART4_RXBUFSIZE=600
CONFIG_UART4_TXBUFSIZE=800
CONFIG_UART5_BAUD=115200
CONFIG_UART5_RXBUFSIZE=600
CONFIG_UART5_TXBUFSIZE=800
CONFIG_UART7_BAUD=115200
CONFIG_UART7_RXBUFSIZE=600
CONFIG_UART7_TXBUFSIZE=800
CONFIG_UART8_BAUD=115200
CONFIG_UART8_RXBUFSIZE=600
CONFIG_UART8_TXBUFSIZE=800
CONFIG_USART1_BAUD=115200
CONFIG_USART1_RXBUFSIZE=1200
CONFIG_USART1_TXBUFSIZE=1500
CONFIG_USART2_BAUD=115200
CONFIG_USART2_RXBUFSIZE=600
CONFIG_USART2_TXBUFSIZE=800
CONFIG_USART3_BAUD=115200
CONFIG_USART3_RXBUFSIZE=600
CONFIG_USART3_TXBUFSIZE=800
CONFIG_USART6_BAUD=57600

6
boards/micoair/h743/default.px4board
View File

@ -10,11 +10,15 @@ CONFIG_BOARD_SERIAL_RC="/dev/ttyS4"
CONFIG_DRIVERS_ADC_BOARD_ADC=y
CONFIG_DRIVERS_BAROMETER_DPS310=y
CONFIG_DRIVERS_CDCACM_AUTOSTART=y
CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_ASP5033=y
CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_AUAV=y
CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_ETS=y
CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_MS4515=y
CONFIG_DRIVERS_DSHOT=y
CONFIG_DRIVERS_GPS=y
CONFIG_DRIVERS_IMU_BOSCH_BMI088=y
CONFIG_DRIVERS_IMU_BOSCH_BMI270=y
CONFIG_COMMON_INS=y
CONFIG_COMMON_MAGNETOMETER=y
CONFIG_DRIVERS_PPS_CAPTURE=y
CONFIG_DRIVERS_PWM_OUT=y

1
boards/modalai/fc-v1/default.px4board
View File

@ -43,6 +43,7 @@ CONFIG_MODULES_COMMANDER=y
CONFIG_MODULES_CONTROL_ALLOCATOR=y
CONFIG_MODULES_DATAMAN=y
CONFIG_MODULES_EKF2=y
# CONFIG_EKF2_AUX_GLOBAL_POSITION is not set
CONFIG_MODULES_ESC_BATTERY=y
CONFIG_MODULES_EVENTS=y
CONFIG_MODULES_FLIGHT_MODE_MANAGER=y

2
boards/nxp/tropic-community/default.px4board
View File

@ -4,7 +4,7 @@ CONFIG_BOARD_ETHERNET=y
CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS1"
CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS2"
CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS3"
CONFIG_BOARD_PARAM_FILE="/fs/microsd/mtd_params"
CONFIG_BOARD_PARAM_FILE="/fs/nor/mtd_params"
CONFIG_DRIVERS_ADC_BOARD_ADC=y
CONFIG_DRIVERS_BAROMETER_BMP388=y
CONFIG_DRIVERS_BAROMETER_INVENSENSE_ICP201XX=y

6
boards/nxp/tropic-community/nuttx-config/nsh/defconfig
View File

@ -60,6 +60,8 @@ CONFIG_FS_BINFS=y
CONFIG_FS_CROMFS=y
CONFIG_FS_FAT=y
CONFIG_FS_FATTIME=y
CONFIG_FS_LITTLEFS=y
CONFIG_FS_LITTLEFS_PROGRAM_SIZE_FACTOR=2
CONFIG_FS_PROCFS=y
CONFIG_FS_PROCFS_MAX_TASKS=64
CONFIG_FS_PROCFS_REGISTER=y
@ -80,6 +82,8 @@ CONFIG_IMXRT_ENET_NTXBUFFERS=8
CONFIG_IMXRT_ENET_PHYINIT=y
CONFIG_IMXRT_FLEXCAN3=y
CONFIG_IMXRT_FLEXCAN_TXMB=1
CONFIG_IMXRT_FLEXSPI1=y
CONFIG_IMXRT_FLEXSPI1_XIP=y
CONFIG_IMXRT_GPIO1_0_15_IRQ=y
CONFIG_IMXRT_GPIO1_16_31_IRQ=y
CONFIG_IMXRT_GPIO2_0_15_IRQ=y
@ -116,7 +120,7 @@ CONFIG_INTELHEX_BINARY=y
CONFIG_IOB_NBUFFERS=24
CONFIG_IOB_THROTTLE=0
CONFIG_IPCFG_BINARY=y
CONFIG_IPCFG_PATH="/fs/microsd"
CONFIG_IPCFG_PATH="/fs/nor"
CONFIG_LIBC_MAX_EXITFUNS=1
CONFIG_LIBC_STRERROR=y
CONFIG_LPI2C1_DMA=y

16
boards/nxp/tropic-community/nuttx-config/scripts/itcm_static_functions.ld
View File

@ -117,3 +117,19 @@
*(.text.ipv4_input)
*(.text.work_thread)
*(.text.work_queue)
/* Flash Storage */
*(.text.imxrt_flexspi_transfer_blocking)
*(.text.imxrt_flexspi_transfer_blocking_private)
*(.text.imxrt_flexspi_write_blocking)
*(.text.imxrt_flexspi_read_blocking)
*(.text.imxrt_flexspi_check_and_clear_error)
*(.text.imxrt_flexspi_get_bus_idle_status)
*(.text.imxrt_flexspi_configure_prefetch)
*(.text.imxrt_flexspi_configure_prefetch_private)
*(.text.imxrt_flexspi_storage_write_enable)
*(.text.imxrt_flexspi_storage_wait_bus_busy)
*(.text.imxrt_flexspi_storage_read_status)
*(.text.imxrt_flexspi_storage_erase)
*(.text.imxrt_flexspi_storage_bwrite)
*(.text.imxrt_flexspi_storage_page_program)

2
boards/nxp/tropic-community/nuttx-config/scripts/script.ld
View File

@ -22,7 +22,7 @@
MEMORY
{
flash (rx) : ORIGIN = 0x60000000, LENGTH = 7936K
flash (rx) : ORIGIN = 0x60000000, LENGTH = 7936K - 128K
sram (rwx) : ORIGIN = 0x20200000, LENGTH = 512K
itcm (rwx) : ORIGIN = 0x00000000, LENGTH = 384K
dtcm (rwx) : ORIGIN = 0x20000000, LENGTH = 128K

5
boards/nxp/tropic-community/src/CMakeLists.txt
View File

@ -41,8 +41,13 @@ px4_add_library(drivers_board
usb.c
imxrt_flexspi_nor_boot.c
imxrt_flexspi_nor_flash.c
imxrt_flexspi_storage.c
imxrt_ocram_initialize.c
)
# Force compiler not to use builtin functions (like memcpy)
# to optimize for loops in init.c (imxrt_ocram_initialize)
set_source_files_properties(imxrt_ocram_initialize.c PROPERTIES COMPILE_FLAGS -fno-builtin)
target_link_libraries(drivers_board
PRIVATE

2
boards/nxp/tropic-community/src/board_config.h
View File

@ -326,6 +326,8 @@ extern void fmurt1062_timer_initialize(void);
#include <px4_platform_common/board_common.h>
int imxrt_flexspi_storage_initialize(void);
#endif /* __ASSEMBLY__ */
__END_DECLS

614
boards/nxp/tropic-community/src/imxrt_flexspi_storage.c Normal file
View File

@ -0,0 +1,614 @@
/****************************************************************************
* boards/px4/fmu-v6xrt/src/imxrt_flexspi_storage.c
*
* Licensed to the Apache Software Foundation (ASF) under one or more
* contributor license agreements. See the NOTICE file distributed with
* this work for additional information regarding copyright ownership. The
* ASF licenses this file to you under the Apache License, Version 2.0 (the
* "License"); you may not use this file except in compliance with the
* License. You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
* License for the specific language governing permissions and limitations
* under the License.
*
****************************************************************************/
/****************************************************************************
* Included Files
****************************************************************************/
#include <nuttx/config.h>
#include <stdbool.h>
#include <errno.h>
#include <debug.h>
#include <nuttx/kmalloc.h>
#include <nuttx/signal.h>
#include <nuttx/fs/fs.h>
#include <nuttx/mtd/mtd.h>
#include <px4_platform_common/px4_mtd.h>
#include "px4_log.h"
#include "imxrt_flexspi.h"
#include "board_config.h"
#include "hardware/imxrt_pinmux.h"
#ifdef CONFIG_IMXRT_FLEXSPI
/* Used sectors must be multiple of the flash block size
* i.e. W25Q32JV has a block size of 64KB
*/
#define NOR_USED_SECTORS (0x20U) /* 32 * 4KB = 128KB */
#define NOR_TOTAL_SECTORS (0x0800U)
#define NOR_PAGE_SIZE (0x0100U) /* 256 bytes */
#define NOR_SECTOR_SIZE (0x1000U) /* 4KB */
#define NOR_START_SECTOR (NOR_TOTAL_SECTORS - NOR_USED_SECTORS)
#define NOR_START_PAGE ((NOR_START_SECTOR * NOR_SECTOR_SIZE) / NOR_PAGE_SIZE)
#define NOR_STORAGE_ADDR (IMXRT_FLEXCIPHER_BASE + NOR_START_SECTOR * NOR_SECTOR_SIZE)
#define MIN(a, b) (((a) < (b)) ? (a) : (b))
enum {
/* SPI instructions */
READ_FAST = 0,
READ_STATUS_REG = 1,
WRITE_STATUS_REG = 3,
WRITE_ENABLE = 4,
SECTOR_ERASE_4K = 5,
READ_FAST_QUAD_OUTPUT = 6,
PAGE_PROGRAM_QUAD_INPUT = 7,
PAGE_PROGRAM = 9,
CHIP_ERASE = 11,
};
static const uint32_t g_flexspi_nor_lut[][4] = {
[READ_FAST] =
{
FLEXSPI_LUT_SEQ(FLEXSPI_COMMAND_SDR, FLEXSPI_1PAD, 0xeb,
FLEXSPI_COMMAND_RADDR_SDR, FLEXSPI_4PAD, 0x18),
FLEXSPI_LUT_SEQ(FLEXSPI_COMMAND_DUMMY_SDR, FLEXSPI_4PAD, 0x06,
FLEXSPI_COMMAND_READ_SDR, FLEXSPI_4PAD, 0x04),
},
[READ_STATUS_REG] =
{
FLEXSPI_LUT_SEQ(FLEXSPI_COMMAND_SDR, FLEXSPI_1PAD, 0x05,
FLEXSPI_COMMAND_READ_SDR, FLEXSPI_1PAD, 0x04),
},
[WRITE_STATUS_REG] =
{
FLEXSPI_LUT_SEQ(FLEXSPI_COMMAND_SDR, FLEXSPI_1PAD, 0x01,
FLEXSPI_COMMAND_WRITE_SDR, FLEXSPI_1PAD, 0x04),
},
[WRITE_ENABLE] =
{
FLEXSPI_LUT_SEQ(FLEXSPI_COMMAND_SDR, FLEXSPI_1PAD, 0x06,
FLEXSPI_COMMAND_STOP, FLEXSPI_1PAD, 0),
},
[SECTOR_ERASE_4K] =
{
FLEXSPI_LUT_SEQ(FLEXSPI_COMMAND_SDR, FLEXSPI_1PAD, 0x20,
FLEXSPI_COMMAND_RADDR_SDR, FLEXSPI_1PAD, 0x18),
},
[CHIP_ERASE] =
{
FLEXSPI_LUT_SEQ(FLEXSPI_COMMAND_SDR, FLEXSPI_1PAD, 0xc7,
FLEXSPI_COMMAND_STOP, FLEXSPI_1PAD, 0),
},
[PAGE_PROGRAM] =
{
FLEXSPI_LUT_SEQ(FLEXSPI_COMMAND_SDR, FLEXSPI_1PAD, 0x02,
FLEXSPI_COMMAND_RADDR_SDR, FLEXSPI_1PAD, 0x18),
FLEXSPI_LUT_SEQ(FLEXSPI_COMMAND_WRITE_SDR, FLEXSPI_1PAD, 0x04,
FLEXSPI_COMMAND_STOP, FLEXSPI_1PAD, 0x0),
},
[READ_FAST_QUAD_OUTPUT] =
{
FLEXSPI_LUT_SEQ(FLEXSPI_COMMAND_SDR, FLEXSPI_1PAD, 0x6b,
FLEXSPI_COMMAND_RADDR_SDR, FLEXSPI_1PAD, 0x18),
FLEXSPI_LUT_SEQ(FLEXSPI_COMMAND_DUMMY_SDR, FLEXSPI_4PAD, 0x08,
FLEXSPI_COMMAND_READ_SDR, FLEXSPI_4PAD, 0x04),
},
[PAGE_PROGRAM_QUAD_INPUT] =
{
FLEXSPI_LUT_SEQ(FLEXSPI_COMMAND_SDR, FLEXSPI_1PAD, 0x32,
FLEXSPI_COMMAND_RADDR_SDR, FLEXSPI_1PAD, 0x18),
FLEXSPI_LUT_SEQ(FLEXSPI_COMMAND_WRITE_SDR, FLEXSPI_4PAD, 0x04,
FLEXSPI_COMMAND_STOP, FLEXSPI_1PAD, 0),
},
};
/****************************************************************************
* Private Types
****************************************************************************/
/* FlexSPI NOR device private data */
struct imxrt_flexspi_storage_dev_s {
struct mtd_dev_s mtd;
struct flexspi_dev_s *flexspi; /* Saved FlexSPI interface instance */
uint8_t *ahb_base;
enum flexspi_port_e port;
struct flexspi_device_config_s *config;
};
/****************************************************************************
* Private Functions Prototypes
****************************************************************************/
/* MTD driver methods */
static int imxrt_flexspi_storage_erase(struct mtd_dev_s *dev,
off_t startblock,
size_t nblocks);
static ssize_t imxrt_flexspi_storage_read(struct mtd_dev_s *dev,
off_t offset,
size_t nbytes,
uint8_t *buffer);
static ssize_t imxrt_flexspi_storage_bread(struct mtd_dev_s *dev,
off_t startblock,
size_t nblocks,
uint8_t *buffer);
static ssize_t imxrt_flexspi_storage_bwrite(struct mtd_dev_s *dev,
off_t startblock,
size_t nblocks,
const uint8_t *buffer);
static int imxrt_flexspi_storage_ioctl(struct mtd_dev_s *dev,
int cmd,
unsigned long arg);
/****************************************************************************
* Private Data
****************************************************************************/
static struct flexspi_device_config_s g_flexspi_device_config = {
.flexspi_root_clk = 4000000,
.is_sck2_enabled = 0,
.flash_size = NOR_USED_SECTORS * NOR_SECTOR_SIZE / 4,
.cs_interval_unit = FLEXSPI_CS_INTERVAL_UNIT1_SCK_CYCLE,
.cs_interval = 0,
.cs_hold_time = 3,
.cs_setup_time = 3,
.data_valid_time = 0,
.columnspace = 0,
.enable_word_address = 0,
.awr_seq_index = 0,
.awr_seq_number = 0,
.ard_seq_index = READ_FAST,
.ard_seq_number = 1,
.ahb_write_wait_unit = FLEXSPI_AHB_WRITE_WAIT_UNIT2_AHB_CYCLE,
.ahb_write_wait_interval = 0,
.rx_sample_clock = FLEXSPI_READ_SAMPLE_CLK_LOOPBACK_FROM_DQS_PAD,
};
static struct imxrt_flexspi_storage_dev_s g_flexspi_nor = {
.mtd =
{
.erase = imxrt_flexspi_storage_erase,
.bread = imxrt_flexspi_storage_bread,
.bwrite = imxrt_flexspi_storage_bwrite,
.read = imxrt_flexspi_storage_read,
.ioctl = imxrt_flexspi_storage_ioctl,
#ifdef CONFIG_MTD_BYTE_WRITE
.write = NULL,
#endif
.name = "imxrt_flexspi_storage"
},
.flexspi = (void *)0,
.ahb_base = (uint8_t *) NOR_STORAGE_ADDR,
.port = FLEXSPI_PORT_A1,
.config = &g_flexspi_device_config
};
/****************************************************************************
* Private Functions
****************************************************************************/
static int imxrt_flexspi_storage_read_status(
const struct imxrt_flexspi_storage_dev_s *dev,
uint32_t *status)
{
int stat;
struct flexspi_transfer_s transfer = {
.device_address = 0,
.port = dev->port,
.cmd_type = FLEXSPI_READ,
.seq_number = 1,
.seq_index = READ_STATUS_REG,
.data = status,
.data_size = 1,
};
stat = FLEXSPI_TRANSFER(dev->flexspi, &transfer);
if (stat != 0) {
return -EIO;
}
return 0;
}
static int imxrt_flexspi_storage_write_enable(
const struct imxrt_flexspi_storage_dev_s *dev)
{
int stat;
struct flexspi_transfer_s transfer = {
.device_address = 0,
.port = dev->port,
.cmd_type = FLEXSPI_COMMAND,
.seq_number = 1,
.seq_index = WRITE_ENABLE,
.data = NULL,
.data_size = 0,
};
stat = FLEXSPI_TRANSFER(dev->flexspi, &transfer);
if (stat != 0) {
return -EIO;
}
return 0;
}
static int imxrt_flexspi_storage_erase_sector(
const struct imxrt_flexspi_storage_dev_s *dev,
off_t offset)
{
int stat;
struct flexspi_transfer_s transfer = {
.device_address = offset,
.port = dev->port,
.cmd_type = FLEXSPI_COMMAND,
.seq_number = 1,
.seq_index = SECTOR_ERASE_4K,
.data = NULL,
.data_size = 0,
};
stat = FLEXSPI_TRANSFER(dev->flexspi, &transfer);
if (stat != 0) {
return -EIO;
}
return 0;
}
static int imxrt_flexspi_storage_erase_chip(
const struct imxrt_flexspi_storage_dev_s *dev)
{
/* We can't erase the chip we're executing from */
return -EINVAL;
}
static int imxrt_flexspi_storage_page_program(
const struct imxrt_flexspi_storage_dev_s *dev,
off_t offset,
const void *buffer,
size_t len)
{
int stat;
struct flexspi_transfer_s transfer = {
.device_address = offset,
.port = dev->port,
.cmd_type = FLEXSPI_WRITE,
.seq_number = 1,
.seq_index = PAGE_PROGRAM_QUAD_INPUT,
.data = (uint32_t *) buffer,
.data_size = len,
};
stat = FLEXSPI_TRANSFER(dev->flexspi, &transfer);
if (stat != 0) {
return -EIO;
}
return 0;
}
static int imxrt_flexspi_storage_wait_bus_busy(
const struct imxrt_flexspi_storage_dev_s *dev)
{
uint32_t status = 0;
int ret;
do {
ret = imxrt_flexspi_storage_read_status(dev, &status);
if (ret) {
return ret;
}
} while (status & 1);
return 0;
}
static ssize_t imxrt_flexspi_storage_read(struct mtd_dev_s *dev,
off_t offset,
size_t nbytes,
uint8_t *buffer)
{
struct imxrt_flexspi_storage_dev_s *priv =
(struct imxrt_flexspi_storage_dev_s *)dev;
uint8_t *src;
finfo("offset: %08lx nbytes: %d\n", (long)offset, (int)nbytes);
if (priv->port >= FLEXSPI_PORT_COUNT) {
return -EIO;
}
src = priv->ahb_base + offset;
memcpy(buffer, src, nbytes);
finfo("return nbytes: %d\n", (int)nbytes);
return (ssize_t)nbytes;
}
static ssize_t imxrt_flexspi_storage_bread(struct mtd_dev_s *dev,
off_t startblock,
size_t nblocks,
uint8_t *buffer)
{
ssize_t nbytes;
finfo("startblock: %08lx nblocks: %d\n", (long)startblock, (int)nblocks);
/* On this device, we can handle the block read just like the byte-oriented
* read
*/
nbytes = imxrt_flexspi_storage_read(dev, startblock * NOR_PAGE_SIZE,
nblocks * NOR_PAGE_SIZE, buffer);
if (nbytes > 0) {
nbytes /= NOR_PAGE_SIZE;
}
return nbytes;
}
static ssize_t imxrt_flexspi_storage_bwrite(struct mtd_dev_s *dev,
off_t startblock,
size_t nblocks,
const uint8_t *buffer)
{
struct imxrt_flexspi_storage_dev_s *priv =
(struct imxrt_flexspi_storage_dev_s *)dev;
size_t len = nblocks * NOR_PAGE_SIZE;
off_t offset = (startblock + NOR_START_PAGE) * NOR_PAGE_SIZE;
uint8_t *src = (uint8_t *) buffer;
#ifdef CONFIG_ARMV7M_DCACHE
uint8_t *dst = priv->ahb_base + startblock * NOR_PAGE_SIZE;
#endif
int i;
finfo("startblock: %08lx nblocks: %d\n", (long)startblock, (int)nblocks);
FLEXSPI_CONFIGURE_PREFETCH(priv->flexspi, false);
irqstate_t flags = enter_critical_section();
while (len) {
i = MIN(NOR_PAGE_SIZE, len);
imxrt_flexspi_storage_write_enable(priv);
imxrt_flexspi_storage_page_program(priv, offset, src, i);
imxrt_flexspi_storage_wait_bus_busy(priv);
offset += i;
src += i;
len -= i;
}
FLEXSPI_CONFIGURE_PREFETCH(priv->flexspi, true);
leave_critical_section(flags);
#ifdef CONFIG_ARMV7M_DCACHE
up_invalidate_dcache((uintptr_t)dst,
(uintptr_t)dst + nblocks * NOR_PAGE_SIZE);
#endif
return nblocks;
}
static int imxrt_flexspi_storage_erase(struct mtd_dev_s *dev,
off_t startblock,
size_t nblocks)
{
struct imxrt_flexspi_storage_dev_s *priv =
(struct imxrt_flexspi_storage_dev_s *)dev;
size_t blocksleft = nblocks;
#ifdef CONFIG_ARMV7M_DCACHE
uint8_t *dst = priv->ahb_base + startblock * NOR_SECTOR_SIZE;
#endif
startblock += NOR_START_SECTOR;
finfo("startblock: %08lx nblocks: %d\n", (long)startblock, (int)nblocks);
FLEXSPI_CONFIGURE_PREFETCH(priv->flexspi, false);
irqstate_t flags = enter_critical_section();
while (blocksleft-- > 0) {
/* Erase each sector */
imxrt_flexspi_storage_write_enable(priv);
imxrt_flexspi_storage_erase_sector(priv, startblock * NOR_SECTOR_SIZE);
imxrt_flexspi_storage_wait_bus_busy(priv);
startblock++;
}
FLEXSPI_CONFIGURE_PREFETCH(priv->flexspi, true);
leave_critical_section(flags);
#ifdef CONFIG_ARMV7M_DCACHE
up_invalidate_dcache((uintptr_t)dst,
(uintptr_t)dst + nblocks * NOR_SECTOR_SIZE);
#endif
return (int)nblocks;
}
static int imxrt_flexspi_storage_ioctl(struct mtd_dev_s *dev,
int cmd,
unsigned long arg)
{
struct imxrt_flexspi_storage_dev_s *priv =
(struct imxrt_flexspi_storage_dev_s *)dev;
int ret = -EINVAL; /* Assume good command with bad parameters */
finfo("cmd: %d\n", cmd);
switch (cmd) {
case MTDIOC_GEOMETRY: {
struct mtd_geometry_s *geo =
(struct mtd_geometry_s *)((uintptr_t)arg);
if (geo) {
/* Populate the geometry structure with information need to
* know the capacity and how to access the device.
*
* NOTE:
* that the device is treated as though it where just an array
* of fixed size blocks. That is most likely not true, but the
* client will expect the device logic to do whatever is
* necessary to make it appear so.
*/
geo->blocksize = (NOR_PAGE_SIZE);
geo->erasesize = (NOR_SECTOR_SIZE);
geo->neraseblocks = (NOR_USED_SECTORS);
ret = OK;
finfo("blocksize: %lu erasesize: %lu neraseblocks: %lu\n",
geo->blocksize, geo->erasesize, geo->neraseblocks);
}
}
break;
case BIOC_PARTINFO: {
struct partition_info_s *info =
(struct partition_info_s *)arg;
if (info != NULL) {
info->numsectors = (NOR_USED_SECTORS * NOR_SECTOR_SIZE) / NOR_PAGE_SIZE;
info->sectorsize = NOR_PAGE_SIZE;
info->startsector = 0;
info->parent[0] = '\0';
ret = OK;
}
}
break;
case MTDIOC_BULKERASE: {
/* Erase the entire device */
imxrt_flexspi_storage_write_enable(priv);
imxrt_flexspi_storage_erase_chip(priv);
imxrt_flexspi_storage_wait_bus_busy(priv);
ret = OK;
}
break;
default:
ret = -ENOTTY; /* Bad/unsupported command */
break;
}
finfo("return %d\n", ret);
return ret;
}
/****************************************************************************
* Public Functions
****************************************************************************/
/****************************************************************************
* Name: imxrt_flexspi_storage_initialize
*
* Description:
* This function is called by board-bringup logic to configure the
* flash device.
*
* Returned Value:
* Zero is returned on success. Otherwise, a negated errno value is
* returned to indicate the nature of the failure.
*
****************************************************************************/
int imxrt_flexspi_storage_initialize(void)
{
struct mtd_dev_s *mtd_dev = &g_flexspi_nor.mtd;
int ret = -ENODEV;
/* Select FlexSPI1 */
g_flexspi_nor.flexspi = imxrt_flexspi_initialize(0);
if (g_flexspi_nor.flexspi) {
ret = OK;
FLEXSPI_UPDATE_LUT(g_flexspi_nor.flexspi,
0,
(const uint32_t *)g_flexspi_nor_lut,
sizeof(g_flexspi_nor_lut) / 4);
}
/* Register the MTD driver so that it can be accessed from the
* VFS.
*/
ret = register_mtddriver("/dev/nor", mtd_dev, 0755, NULL);
if (ret < 0) {
syslog(LOG_ERR, "ERROR: Failed to register MTD driver: %d\n",
ret);
}
#ifdef CONFIG_FS_LITTLEFS
/* Mount the LittleFS file system */
ret = nx_mount("/dev/nor", "/fs/nor", "littlefs", 0,
"autoformat");
if (ret < 0) {
syslog(LOG_ERR,
"ERROR: Failed to mount LittleFS at /mnt/lfs: %d\n",
ret);
}
#endif
return ret;
}
#endif /* CONFIG_IMXRT_FLEXSPI */

102
boards/nxp/tropic-community/src/imxrt_ocram_initialize.c Normal file
View File

@ -0,0 +1,102 @@
/****************************************************************************
*
* Copyright (c) 2018-2019, 2023 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* 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 imxrt_ocram_initialize.c
*
* PX4 fmu-v6xrt RAM startup early startup code.
*/
/****************************************************************************
* Included Files
****************************************************************************/
#include "arm_internal.h"
#include "imxrt_iomuxc.h"
/****************************************************************************
* Pre-Processor Definitions
****************************************************************************/
__BEGIN_DECLS
extern const uint64_t _fitcmfuncs; /* Copy source address in FLASH */
extern uint64_t _sitcmfuncs; /* Copy destination start address in ITCM */
extern uint64_t _eitcmfuncs; /* Copy destination end address in ITCM */
extern uint64_t _sdtcm; /* Copy destination start address in DTCM */
extern uint64_t _edtcm; /* Copy destination end address in DTCM */
__END_DECLS
/****************************************************************************
* Name: imxrt_ocram_initialize
*
* Description:
* Called off reset vector to reconfigure the flexRAM
* and finish the FLASH to RAM Copy.
* CMakeLists.txt Forces compiler not to use builtin functions using -fno-builtin
*
****************************************************************************/
__EXPORT void imxrt_ocram_initialize(void)
{
uint32_t regval;
register uint64_t *src;
register uint64_t *dest;
/* FlexRAM Configuration
* F = 64K ITCM
* A = 64K DTCM
* 5 = 64K OCRAM
* So 0xFFFFFFAA is
* 384K FlexRAM ITCM
* 128K FlexRAM DTCM
* */
putreg32(0xFFFFFFAA, IMXRT_IOMUXC_GPR_GPR17);
regval = getreg32(IMXRT_IOMUXC_GPR_GPR16);
putreg32(regval | GPR_GPR16_FLEXRAM_BANK_CFG_SEL, IMXRT_IOMUXC_GPR_GPR16);
/* Copy any necessary code sections from FLASH to ITCM. The process is the
* same as the code copying from FLASH to RAM above. */
for (src = (uint64_t *)&_fitcmfuncs, dest = (uint64_t *)&_sitcmfuncs;
dest < (uint64_t *)&_eitcmfuncs;) {
*dest++ = *src++;
}
/* Clear .dtcm. We'll do this inline (vs. calling memset) just to be
* certain that there are no issues with the state of global variables.
*/
for (dest = &_sdtcm; dest < &_edtcm;) {
*dest++ = 0;
}
}

59
boards/nxp/tropic-community/src/init.c
View File

@ -104,11 +104,6 @@ extern void led_off(int led);
extern uint32_t _srodata; /* Start of .rodata */
extern uint32_t _erodata; /* End of .rodata */
extern const uint64_t _fitcmfuncs; /* Copy source address in FLASH */
extern uint64_t _sitcmfuncs; /* Copy destination start address in ITCM */
extern uint64_t _eitcmfuncs; /* Copy destination end address in ITCM */
extern uint64_t _sdtcm; /* Copy destination start address in DTCM */
extern uint64_t _edtcm; /* Copy destination end address in DTCM */
__END_DECLS
/************************************************************************************
@ -192,50 +187,6 @@ void imxrt_flash_setup_prefetch_partition(void)
ARM_DMB();
}
/****************************************************************************
* Name: imxrt_ocram_initialize
*
* Description:
* Called off reset vector to reconfigure the flexRAM
* and finish the FLASH to RAM Copy.
*
****************************************************************************/
__EXPORT void imxrt_ocram_initialize(void)
{
uint32_t regval;
register uint64_t *src;
register uint64_t *dest;
/* FlexRAM Configuration
* F = 64K ITCM
* A = 64K DTCM
* 5 = 64K OCRAM
* So 0xFFFFFFAA is
* 384K FlexRAM ITCM
* 128K FlexRAM DTCM
* */
putreg32(0xFFFFFFAA, IMXRT_IOMUXC_GPR_GPR17);
regval = getreg32(IMXRT_IOMUXC_GPR_GPR16);
putreg32(regval | GPR_GPR16_FLEXRAM_BANK_CFG_SEL, IMXRT_IOMUXC_GPR_GPR16);
/* Copy any necessary code sections from FLASH to ITCM. The process is the
* same as the code copying from FLASH to RAM above. */
for (src = (uint64_t *)&_fitcmfuncs, dest = (uint64_t *)&_sitcmfuncs;
dest < (uint64_t *)&_eitcmfuncs;) {
*dest++ = *src++;
}
/* Clear .dtcm. We'll do this inline (vs. calling memset) just to be
* certain that there are no issues with the state of global variables.
*/
for (dest = &_sdtcm; dest < &_edtcm;) {
*dest++ = 0;
}
}
/****************************************************************************
* Name: imxrt_boardinitialize
*
@ -411,5 +362,15 @@ __EXPORT int board_app_initialize(uintptr_t arg)
imxrt_caninitialize(3);
#endif
#ifdef CONFIG_IMXRT_FLEXSPI
ret = imxrt_flexspi_storage_initialize();
if (ret < 0) {
syslog(LOG_ERR,
"ERROR: imxrt_flexspi_nor_initialize failed: %d\n", ret);
}
#endif
return ret;
}

2
boards/omnibus/f4sd/default.px4board
View File

@ -31,7 +31,7 @@ CONFIG_MODULES_MC_ATT_CONTROL=y
CONFIG_MODULES_MC_POS_CONTROL=y
CONFIG_MODULES_MC_RATE_CONTROL=y
CONFIG_MODULES_NAVIGATOR=y
CONFIG_NAVIGATOR_ADSB=n
# CONFIG_NAVIGATOR_ADSB is not set
CONFIG_MODULES_RC_UPDATE=y
CONFIG_MODULES_SENSORS=y
# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set

3
boards/px4/fmu-v2/default.px4board
View File

@ -25,6 +25,7 @@ CONFIG_MODULES_COMMANDER=y
CONFIG_MODULES_CONTROL_ALLOCATOR=y
CONFIG_MODULES_DATAMAN=y
CONFIG_MODULES_EKF2=y
# CONFIG_EKF2_AUX_GLOBAL_POSITION is not set
# CONFIG_EKF2_AUXVEL is not set
# CONFIG_EKF2_BARO_COMPENSATION is not set
# CONFIG_EKF2_DRAG_FUSION is not set
@ -40,7 +41,7 @@ CONFIG_MODULES_MC_ATT_CONTROL=y
CONFIG_MODULES_MC_POS_CONTROL=y
CONFIG_MODULES_MC_RATE_CONTROL=y
CONFIG_MODULES_NAVIGATOR=y
CONFIG_NAVIGATOR_ADSB=n
# CONFIG_NAVIGATOR_ADSB is not set
CONFIG_MODULES_RC_UPDATE=y
CONFIG_MODULES_SENSORS=y
# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set

1
boards/px4/fmu-v5/cryptotest.px4board
View File

@ -4,5 +4,6 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n
CONFIG_BOARD_CRYPTO=y
CONFIG_DRIVERS_STUB_KEYSTORE=y
CONFIG_DRIVERS_SW_CRYPTO=y
# CONFIG_EKF2_AUX_GLOBAL_POSITION is not set
CONFIG_PUBLIC_KEY0="../../../Tools/test_keys/key0.pub"
CONFIG_PUBLIC_KEY1="../../../Tools/test_keys/rsa2048.pub"

1
boards/px4/fmu-v5/rover.px4board
View File

@ -17,7 +17,6 @@ CONFIG_MODULES_UUV_ATT_CONTROL=n
CONFIG_MODULES_UUV_POS_CONTROL=n
CONFIG_MODULES_VTOL_ATT_CONTROL=n
CONFIG_DRIVERS_ROBOCLAW=y
CONFIG_EKF2_AUX_GLOBAL_POSITION=y
# CONFIG_EKF2_WIND is not set
CONFIG_MODULES_ROVER_ACKERMANN=y
CONFIG_MODULES_ROVER_DIFFERENTIAL=y

1
boards/px4/fmu-v5/stackcheck.px4board
View File

@ -46,4 +46,3 @@ CONFIG_SYSTEMCMDS_REFLECT=n
CONFIG_BOARD_CONSTRAINED_FLASH=y
CONFIG_BOARD_TESTING=y
CONFIG_DRIVERS_BAROMETER_MS5611=y
CONFIG_EKF2_AUX_GLOBAL_POSITION=y

1
boards/px4/fmu-v5x/rover.px4board
View File

@ -14,7 +14,6 @@ CONFIG_MODULES_MC_POS_CONTROL=n
CONFIG_MODULES_MC_RATE_CONTROL=n
CONFIG_MODULES_VTOL_ATT_CONTROL=n
CONFIG_DRIVERS_ROBOCLAW=y
CONFIG_EKF2_AUX_GLOBAL_POSITION=y
# CONFIG_EKF2_WIND is not set
CONFIG_MODULES_ROVER_ACKERMANN=y
CONFIG_MODULES_ROVER_DIFFERENTIAL=y

1
boards/px4/fmu-v6c/rover.px4board
View File

@ -13,7 +13,6 @@ CONFIG_MODULES_MC_POS_CONTROL=n
CONFIG_MODULES_MC_RATE_CONTROL=n
CONFIG_MODULES_VTOL_ATT_CONTROL=n
CONFIG_DRIVERS_ROBOCLAW=y
CONFIG_EKF2_AUX_GLOBAL_POSITION=y
# CONFIG_EKF2_WIND is not set
CONFIG_MODULES_ROVER_ACKERMANN=y
CONFIG_MODULES_ROVER_DIFFERENTIAL=y

1
boards/px4/fmu-v6u/rover.px4board
View File

@ -13,7 +13,6 @@ CONFIG_MODULES_MC_POS_CONTROL=n
CONFIG_MODULES_MC_RATE_CONTROL=n
CONFIG_MODULES_VTOL_ATT_CONTROL=n
CONFIG_DRIVERS_ROBOCLAW=y
CONFIG_EKF2_AUX_GLOBAL_POSITION=y
# CONFIG_EKF2_WIND is not set
CONFIG_MODULES_ROVER_ACKERMANN=y
CONFIG_MODULES_ROVER_DIFFERENTIAL=y

1
boards/px4/fmu-v6x/multicopter.px4board
View File

@ -8,7 +8,6 @@ CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=n
CONFIG_MODULES_FW_RATE_CONTROL=n
CONFIG_MODULES_VTOL_ATT_CONTROL=n
CONFIG_COMMON_RC=y
CONFIG_EKF2_AUX_GLOBAL_POSITION=y
# CONFIG_EKF2_SIDESLIP is not set
CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
# CONFIG_SENSORS_VEHICLE_AIRSPEED is not set

1
boards/px4/fmu-v6x/rover.px4board
View File

@ -13,7 +13,6 @@ CONFIG_MODULES_MC_POS_CONTROL=n
CONFIG_MODULES_MC_RATE_CONTROL=n
CONFIG_MODULES_VTOL_ATT_CONTROL=n
CONFIG_DRIVERS_ROBOCLAW=y
CONFIG_EKF2_AUX_GLOBAL_POSITION=y
# CONFIG_EKF2_WIND is not set
CONFIG_MODULES_ROVER_ACKERMANN=y
CONFIG_MODULES_ROVER_DIFFERENTIAL=y

1
boards/px4/fmu-v6xrt/rover.px4board
View File

@ -14,7 +14,6 @@ CONFIG_MODULES_MC_POS_CONTROL=n
CONFIG_MODULES_MC_RATE_CONTROL=n
CONFIG_MODULES_VTOL_ATT_CONTROL=n
CONFIG_DRIVERS_ROBOCLAW=y
CONFIG_EKF2_AUX_GLOBAL_POSITION=y
# CONFIG_EKF2_WIND is not set
CONFIG_MODULES_ROVER_ACKERMANN=y
CONFIG_MODULES_ROVER_DIFFERENTIAL=y

5
boards/px4/fmu-v6xrt/src/CMakeLists.txt
View File

@ -75,9 +75,14 @@ else()
imxrt_flexspi_nor_boot.c
imxrt_flexspi_nor_flash.c
imxrt_clockconfig.c
imxrt_ocram_initialize.c
${SRCS}
)
# Force compiler not to use builtin functions (like memcpy)
# to optimize for loops in init.c (imxrt_ocram_initialize)
set_source_files_properties(imxrt_ocram_initialize.c PROPERTIES COMPILE_FLAGS -fno-builtin)
target_link_libraries(drivers_board
PRIVATE
arch_board_hw_info

118
boards/px4/fmu-v6xrt/src/imxrt_ocram_initialize.c Normal file
View File

@ -0,0 +1,118 @@
/****************************************************************************
*
* Copyright (c) 2018-2019, 2023 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* 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 imxrt_ocram_initialize.c
*
* PX4 fmu-v6xrt RAM startup early startup code.
*/
/****************************************************************************
* Included Files
****************************************************************************/
#include "arm_internal.h"
#include "imxrt_iomuxc.h"
/****************************************************************************
* Pre-Processor Definitions
****************************************************************************/
__BEGIN_DECLS
extern const uint64_t _fitcmfuncs; /* Copy source address in FLASH */
extern uint64_t _sitcmfuncs; /* Copy destination start address in ITCM */
extern uint64_t _eitcmfuncs; /* Copy destination end address in ITCM */
extern uint64_t _sdtcm; /* Copy destination start address in DTCM */
extern uint64_t _edtcm; /* Copy destination end address in DTCM */
__END_DECLS
/****************************************************************************
* Name: imxrt_ocram_initialize
*
* Description:
* Called off reset vector to reconfigure the flexRAM
* and finish the FLASH to RAM Copy.
* CMakeLists.txt Forces compiler not to use builtin functions using -fno-builtin
*
****************************************************************************/
__EXPORT void imxrt_ocram_initialize(void)
{
uint32_t regval;
register uint64_t *src;
register uint64_t *dest;
/* Reallocate
* Final Configuration is
* No DTCM
* 512k OCRAM M7 (FlexRAM) (2038:0000-203f:ffff)
* 128k OCRAMM7 FlexRAM ECC (2036:0000-2037:ffff)
* 64k OCRAM2 ECC parity (2035:0000-2035:ffff)
* 64k OCRAM1 ECC parity (2034:0000-2034:ffff)
* 512k FlexRAM OCRAM2 (202C:0000-2033:ffff)
* 512k FlexRAM OCRAM1 (2024:0000-202B:ffff)
* 256k System OCRAM M4 (2020:0000-2023:ffff)
*/
putreg32(0x0000FFAA, IMXRT_IOMUXC_GPR_GPR17);
putreg32(0x0000FFAA, IMXRT_IOMUXC_GPR_GPR18);
regval = getreg32(IMXRT_IOMUXC_GPR_GPR16);
putreg32(regval | GPR_GPR16_FLEXRAM_BANK_CFG_SEL_REG, IMXRT_IOMUXC_GPR_GPR16);
/* Copy any necessary code sections from FLASH to ITCM. The process is the
* same as the code copying from FLASH to RAM above. */
for (src = (uint64_t *)&_fitcmfuncs, dest = (uint64_t *)&_sitcmfuncs;
dest < (uint64_t *)&_eitcmfuncs;) {
*dest++ = *src++;
}
/* Clear .dtcm. We'll do this inline (vs. calling memset) just to be
* certain that there are no issues with the state of global variables.
*/
for (dest = &_sdtcm; dest < &_edtcm;) {
*dest++ = 0;
}
#if defined(CONFIG_BOOT_RUNFROMISRAM)
const uint32_t *src;
uint32_t *dest;
for (src = (uint32_t *)(LOCATE_IN_SRC(g_boot_data.start) + g_boot_data.size),
dest = (uint32_t *)(g_boot_data.start + g_boot_data.size);
dest < (uint32_t *) &_etext;) {
*dest++ = *src++;
}
#endif
}

63
boards/px4/fmu-v6xrt/src/init.c
View File

@ -109,11 +109,6 @@ extern void led_off(int led);
extern uint32_t _srodata; /* Start of .rodata */
extern uint32_t _erodata; /* End of .rodata */
extern const uint64_t _fitcmfuncs; /* Copy source address in FLASH */
extern uint64_t _sitcmfuncs; /* Copy destination start address in ITCM */
extern uint64_t _eitcmfuncs; /* Copy destination end address in ITCM */
extern uint64_t _sdtcm; /* Copy destination start address in DTCM */
extern uint64_t _edtcm; /* Copy destination end address in DTCM */
__END_DECLS
/************************************************************************************
@ -225,65 +220,7 @@ void imxrt_flash_setup_prefetch_partition(void)
ARM_ISB();
ARM_DMB();
}
/****************************************************************************
* Name: imxrt_ocram_initialize
*
* Description:
* Called off reset vector to reconfigure the flexRAM
* and finish the FLASH to RAM Copy.
*
****************************************************************************/
__EXPORT void imxrt_ocram_initialize(void)
{
uint32_t regval;
register uint64_t *src;
register uint64_t *dest;
/* Reallocate
* Final Configuration is
* No DTCM
* 512k OCRAM M7 (FlexRAM) (2038:0000-203f:ffff)
* 128k OCRAMM7 FlexRAM ECC (2036:0000-2037:ffff)
* 64k OCRAM2 ECC parity (2035:0000-2035:ffff)
* 64k OCRAM1 ECC parity (2034:0000-2034:ffff)
* 512k FlexRAM OCRAM2 (202C:0000-2033:ffff)
* 512k FlexRAM OCRAM1 (2024:0000-202B:ffff)
* 256k System OCRAM M4 (2020:0000-2023:ffff)
*/
putreg32(0x0000FFAA, IMXRT_IOMUXC_GPR_GPR17);
putreg32(0x0000FFAA, IMXRT_IOMUXC_GPR_GPR18);
regval = getreg32(IMXRT_IOMUXC_GPR_GPR16);
putreg32(regval | GPR_GPR16_FLEXRAM_BANK_CFG_SEL_REG, IMXRT_IOMUXC_GPR_GPR16);
/* Copy any necessary code sections from FLASH to ITCM. The process is the
* same as the code copying from FLASH to RAM above. */
for (src = (uint64_t *)&_fitcmfuncs, dest = (uint64_t *)&_sitcmfuncs;
dest < (uint64_t *)&_eitcmfuncs;) {
*dest++ = *src++;
}
/* Clear .dtcm. We'll do this inline (vs. calling memset) just to be
* certain that there are no issues with the state of global variables.
*/
for (dest = &_sdtcm; dest < &_edtcm;) {
*dest++ = 0;
}
#if defined(CONFIG_BOOT_RUNFROMISRAM)
const uint32_t *src;
uint32_t *dest;
for (src = (uint32_t *)(LOCATE_IN_SRC(g_boot_data.start) + g_boot_data.size),
dest = (uint32_t *)(g_boot_data.start + g_boot_data.size);
dest < (uint32_t *) &_etext;) {
*dest++ = *src++;
}
#endif
}
/****************************************************************************
* Name: imxrt_boardinitialize

1
boards/px4/sitl/default.px4board
View File

@ -15,7 +15,6 @@ CONFIG_MODULES_CONTROL_ALLOCATOR=y
CONFIG_MODULES_DATAMAN=y
CONFIG_MODULES_EKF2=y
CONFIG_EKF2_VERBOSE_STATUS=y
CONFIG_EKF2_AUX_GLOBAL_POSITION=y
CONFIG_MODULES_EVENTS=y
CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
CONFIG_MODULES_FW_ATT_CONTROL=y

2
boards/raspberrypi/pico/default.px4board
View File

@ -32,7 +32,7 @@ CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
CONFIG_MODULES_MC_POS_CONTROL=y
CONFIG_MODULES_MC_RATE_CONTROL=y
CONFIG_MODULES_NAVIGATOR=y
CONFIG_NAVIGATOR_ADSB=n
# CONFIG_NAVIGATOR_ADSB is not set
CONFIG_MODULES_RC_UPDATE=y
CONFIG_MODULES_SENSORS=y
CONFIG_SYSTEMCMDS_DMESG=y

1
msg/Ekf2Timestamps.msg
View File

@ -14,6 +14,7 @@ int16 RELATIVE_TIMESTAMP_INVALID = 32767 # (0x7fff) If one of the relative times
# difference of +-3.2s to the sensor_combined topic.
int16 airspeed_timestamp_rel
int16 airspeed_validated_timestamp_rel
int16 distance_sensor_timestamp_rel
int16 optical_flow_timestamp_rel
int16 vehicle_air_data_timestamp_rel

1
msg/EstimatorStatusFlags.msg
View File

@ -47,6 +47,7 @@ bool cs_rng_terrain # 39 - true if we are fusing range finder data f
bool cs_opt_flow_terrain # 40 - true if we are fusing flow data for terrain
bool cs_valid_fake_pos # 41 - true if a valid constant position is being fused
bool cs_constant_pos # 42 - true if the vehicle is at a constant position
bool cs_baro_fault # 43 - true when the current baro has been declared faulty and is no longer being used
# fault status
uint32 fault_status_changes # number of filter fault status (fs) changes

8
msg/PwmInput.msg
View File

@ -1,4 +1,4 @@
uint64 timestamp # Time since system start (microseconds)
uint64 error_count # Timer overcapture error flag (AUX5 or MAIN5)
uint32 pulse_width # Pulse width, timer counts
uint32 period # Period, timer counts
uint64 timestamp # Time since system start (microseconds)
uint64 error_count # Timer overcapture error flag (AUX5 or MAIN5)
uint32 pulse_width # Pulse width, timer counts (microseconds)
uint32 period # Period, timer counts (microseconds)

1
msg/Rpm.msg
View File

@ -1,4 +1,5 @@
uint64 timestamp # time since system start (microseconds)
# rpm values of 0.0 mean within a timeout there is no movement measured
float32 rpm_estimate # filtered revolutions per minute
float32 rpm_raw

4
msg/VehicleImu.msg
View File

@ -9,8 +9,8 @@ uint32 gyro_device_id # Gyroscope unique device ID for the sensor that
float32[3] delta_angle # delta angle about the FRD body frame XYZ-axis in rad over the integration time frame (delta_angle_dt)
float32[3] delta_velocity # delta velocity in the FRD body frame XYZ-axis in m/s over the integration time frame (delta_velocity_dt)
uint16 delta_angle_dt # integration period in microseconds
uint16 delta_velocity_dt # integration period in microseconds
uint32 delta_angle_dt # integration period in microseconds
uint32 delta_velocity_dt # integration period in microseconds
uint8 CLIPPING_X = 1
uint8 CLIPPING_Y = 2

10
msg/versioned/VehicleLocalPosition.msg
View File

@ -77,10 +77,12 @@ float32 evv # Standard deviation of vertical velocity error, (metres/sec)
bool dead_reckoning # True if this position is estimated through dead-reckoning
# estimator specified vehicle limits
float32 vxy_max # maximum horizontal speed - set to 0 when limiting not required (meters/sec)
float32 vz_max # maximum vertical speed - set to 0 when limiting not required (meters/sec)
float32 hagl_min # minimum height above ground level - set to 0 when limiting not required (meters)
float32 hagl_max # maximum height above ground level - set to 0 when limiting not required (meters)
# set to INFINITY when limiting not required
float32 vxy_max # maximum horizontal speed (meters/sec)
float32 vz_max # maximum vertical speed (meters/sec)
float32 hagl_min # minimum height above ground level (meters)
float32 hagl_max_z # maximum height above ground level for z-control (meters)
float32 hagl_max_xy # maximum height above ground level for xy-control (meters)
# TOPICS vehicle_local_position vehicle_local_position_groundtruth external_ins_local_position
# TOPICS estimator_local_position

5
platforms/nuttx/CMakeLists.txt
View File

@ -506,6 +506,11 @@ if(NOT NUTTX_DIR MATCHES "external")
message(STATUS "Found SVD: ${DEBUG_SVD_FILE_PATH}")
endif()
if(NOT CMAKE_GDB)
find_program(CMAKE_GDB gdb-multiarch)
message(STATUS "Found GDB: ${CMAKE_GDB}")
endif()
if(DEBUG_DEVICE)
configure_file(${CMAKE_CURRENT_SOURCE_DIR}/Debug/launch.json.in ${PX4_SOURCE_DIR}/.vscode/launch.json @ONLY)
endif()

2
platforms/nuttx/NuttX/nuttx

@ -1 +1 @@
Subproject commit e61fdd019de6ee7685c071760a965961c5ef5227
Subproject commit 9a213327fbb2dab252185d468fb29348e1833d8c

1
src/drivers/distance_sensor/lightware_sf45_serial/CMakeLists.txt
View File

@ -41,6 +41,7 @@ px4_add_module(
DEPENDS
drivers_rangefinder
px4_work_queue
CollisionPrevention
MODULE_CONFIG
module.yaml
)

122
src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp
View File

@ -33,19 +33,18 @@
#include "lightware_sf45_serial.hpp"
#include <inttypes.h>
#include <fcntl.h>
#include <float.h>
#include <inttypes.h>
#include <termios.h>
#include <lib/crc/crc.h>
#include <lib/mathlib/mathlib.h>
#include <float.h>
#include <matrix/matrix/math.hpp>
#include <ObstacleMath.hpp>
using namespace time_literals;
/* Configuration Constants */
SF45LaserSerial::SF45LaserSerial(const char *port) :
ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)),
_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")),
@ -93,6 +92,11 @@ int SF45LaserSerial::init()
param_get(param_find("SF45_ORIENT_CFG"), &_orient_cfg);
param_get(param_find("SF45_YAW_CFG"), &_yaw_cfg);
// set the sensor orientation
const float yaw_cfg_angle = ObstacleMath::sensor_orientation_to_yaw_offset(static_cast<ObstacleMath::SensorOrientation>
(_yaw_cfg));
_obstacle_distance.angle_offset = math::degrees(matrix::wrap_2pi(yaw_cfg_angle));
start();
return PX4_OK;
}
@ -136,8 +140,6 @@ int SF45LaserSerial::measure()
int SF45LaserSerial::collect()
{
float distance_m = -1.0f;
if (_sensor_state == STATE_UNINIT) {
perf_begin(_sample_perf);
@ -196,8 +198,7 @@ int SF45LaserSerial::collect()
sf45_get_and_handle_request(payload_length, SF_DISTANCE_DATA_CM);
if (_crc_valid) {
sf45_process_replies(&distance_m);
PX4_DEBUG("val (float): %8.4f, valid: %s", (double)distance_m, ((_crc_valid) ? "OK" : "NO"));
sf45_process_replies();
perf_end(_sample_perf);
return PX4_OK;
}
@ -592,13 +593,12 @@ void SF45LaserSerial::sf45_send(uint8_t msg_id, bool write, int32_t *data, uint8
}
}
void SF45LaserSerial::sf45_process_replies(float *distance_m)
void SF45LaserSerial::sf45_process_replies()
{
switch (rx_field.msg_id) {
case SF_DISTANCE_DATA_CM: {
const float raw_distance = (rx_field.data[0] << 0) | (rx_field.data[1] << 8);
int16_t raw_yaw = ((rx_field.data[2] << 0) | (rx_field.data[3] << 8));
int16_t scaled_yaw = 0;
// The sensor scans from 0 to -160, so extract negative angle from int16 and represent as if a float
if (raw_yaw > 32000) {
@ -611,49 +611,41 @@ void SF45LaserSerial::sf45_process_replies(float *distance_m)
}
// SF45/B product guide {Data output bit: 8 Description: "Yaw angle [1/100 deg] size: int16}"
scaled_yaw = raw_yaw * SF45_SCALE_FACTOR;
float scaled_yaw = raw_yaw * SF45_SCALE_FACTOR;
switch (_yaw_cfg) {
case ROTATION_FORWARD_FACING:
break;
case ROTATION_BACKWARD_FACING:
if (scaled_yaw > 180) {
scaled_yaw = scaled_yaw - 180;
} else {
scaled_yaw = scaled_yaw + 180; // rotation facing aft
}
break;
case ROTATION_RIGHT_FACING:
scaled_yaw = scaled_yaw + 90; // rotation facing right
break;
case ROTATION_LEFT_FACING:
scaled_yaw = scaled_yaw - 90; // rotation facing left
break;
default:
break;
}
// Adjust for sensor orientation
scaled_yaw = sf45_wrap_360(scaled_yaw + _obstacle_distance.angle_offset);
// Convert to meters for the debug message
*distance_m = raw_distance * SF45_SCALE_FACTOR;
float distance_m = raw_distance * SF45_SCALE_FACTOR;
_current_bin_dist = ((uint16_t)raw_distance < _current_bin_dist) ? (uint16_t)raw_distance : _current_bin_dist;
uint8_t current_bin = sf45_convert_angle(scaled_yaw);
if (current_bin != _previous_bin) {
PX4_DEBUG("scaled_yaw: \t %d, \t current_bin: \t %d, \t distance: \t %8.4f\n", scaled_yaw, current_bin,
(double)*distance_m);
PX4_DEBUG("scaled_yaw: \t %f, \t current_bin: \t %d, \t distance: \t %8.4f\n", (double)scaled_yaw, current_bin,
(double)distance_m);
if (_vehicle_attitude_sub.updated()) {
vehicle_attitude_s vehicle_attitude;
if (_vehicle_attitude_sub.copy(&vehicle_attitude)) {
_vehicle_attitude = matrix::Quatf(vehicle_attitude.q);
}
}
float current_bin_dist = static_cast<float>(_current_bin_dist);
float scaled_yaw_rad = math::radians(static_cast<float>(scaled_yaw));
ObstacleMath::project_distance_on_horizontal_plane(current_bin_dist, scaled_yaw_rad, _vehicle_attitude);
_current_bin_dist = static_cast<uint16_t>(current_bin_dist);
if (_current_bin_dist > _obstacle_distance.max_distance) {
_current_bin_dist = _obstacle_distance.max_distance + 1; // As per ObstacleDistance.msg definition
}
hrt_abstime now = hrt_absolute_time();
_obstacle_distance.distances[current_bin] = _current_bin_dist;
_handle_missed_bins(current_bin, _previous_bin, _current_bin_dist, now);
_publish_obstacle_msg(now);
@ -691,47 +683,31 @@ void SF45LaserSerial::_handle_missed_bins(uint8_t current_bin, uint8_t previous_
{
// if the sensor has its cycle delay configured for a low value like 5, it can happen that not every bin gets a measurement.
// in this case we assume the measurement to be valid for all bins between the previous and the current bin.
uint8_t start;
uint8_t end;
if (abs(current_bin - previous_bin) > BIN_COUNT / 4) {
// wrap-around case is assumed to have happend when the distance between the bins is larger than 1/4 of all Bins
// THis is simplyfied as we are not considering the scaning direction
start = math::max(previous_bin, current_bin);
end = math::min(previous_bin, current_bin);
// Shift bin indices such that we can never have the wrap-around case.
const float fov_offset_angle = 360.0f - SF45_FIELDOF_VIEW / 2.f;
const uint16_t current_bin_offset = ObstacleMath::get_offset_bin_index(current_bin, _obstacle_distance.increment,
fov_offset_angle);
const uint16_t previous_bin_offset = ObstacleMath::get_offset_bin_index(previous_bin, _obstacle_distance.increment,
fov_offset_angle);
} else if (previous_bin < current_bin) { // Scanning clockwise
start = previous_bin + 1;
end = current_bin;
const uint16_t start = math::min(current_bin_offset, previous_bin_offset) + 1;
const uint16_t end = math::max(current_bin_offset, previous_bin_offset);
} else { // scanning counter-clockwise
start = current_bin;
end = previous_bin - 1;
}
if (start <= end) {
for (uint8_t i = start; i <= end; i++) {
_obstacle_distance.distances[i] = measurement;
_data_timestamps[i] = now;
}
} else { // wrap-around case
for (uint8_t i = start; i < BIN_COUNT; i++) {
_obstacle_distance.distances[i] = measurement;
_data_timestamps[i] = now;
}
for (uint8_t i = 0; i <= end; i++) {
_obstacle_distance.distances[i] = measurement;
_data_timestamps[i] = now;
}
// populate the missed bins with the measurement
for (uint16_t i = start; i < end; i++) {
uint16_t bin_index = ObstacleMath::get_offset_bin_index(i, _obstacle_distance.increment, -fov_offset_angle);
_obstacle_distance.distances[bin_index] = measurement;
_data_timestamps[bin_index] = now;
}
}
uint8_t SF45LaserSerial::sf45_convert_angle(const int16_t yaw)
{
uint8_t mapped_sector = 0;
float adjusted_yaw = sf45_wrap_360(yaw - _obstacle_distance.angle_offset);
mapped_sector = round(adjusted_yaw / _obstacle_distance.increment);
mapped_sector = floor(adjusted_yaw / _obstacle_distance.increment);
return mapped_sector;
}

7
src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp
View File

@ -49,7 +49,9 @@
#include <lib/perf/perf_counter.h>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/obstacle_distance.h>
#include <uORB/topics/vehicle_attitude.h>
#include "sf45_commands.h"
@ -92,7 +94,7 @@ public:
void sf45_get_and_handle_request(const int payload_length, const SF_SERIAL_CMD msg_id);
void sf45_send(uint8_t msg_id, bool r_w, int32_t *data, uint8_t data_len);
uint16_t sf45_format_crc(uint16_t crc, uint8_t data_value);
void sf45_process_replies(float *data);
void sf45_process_replies();
uint8_t sf45_convert_angle(const int16_t yaw);
float sf45_wrap_360(float f);
@ -103,6 +105,7 @@ private:
obstacle_distance_s::distances[0]);
static constexpr uint64_t SF45_MEAS_TIMEOUT{100_ms};
static constexpr float SF45_SCALE_FACTOR = 0.01f;
static constexpr float SF45_FIELDOF_VIEW = 320.f; // degrees
void start();
void stop();
@ -113,6 +116,7 @@ private:
void _handle_missed_bins(uint8_t current_bin, uint8_t previous_bin, uint16_t measurement, hrt_abstime now);
void _publish_obstacle_msg(hrt_abstime now);
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
uint64_t _data_timestamps[BIN_COUNT];
@ -141,6 +145,7 @@ private:
int32_t _orient_cfg{0};
uint8_t _previous_bin{0};
uint16_t _current_bin_dist{UINT16_MAX};
matrix::Quatf _vehicle_attitude{};
// end of SF45/B data members

2
src/drivers/distance_sensor/ulanding_radar/ulanding_radar_main.cpp
View File

@ -102,7 +102,7 @@ static int usage()
Serial bus driver for the Aerotenna uLanding radar.
Setup/usage information: https://docs.px4.io/v1.9.0/en/sensor/ulanding_radar.html
Setup/usage information: https://docs.px4.io/main/en/sensor/ulanding_radar.html
### Examples

2
src/drivers/drv_dshot.h
View File

@ -88,7 +88,7 @@ typedef enum {
* @param channel_mask Bitmask of channels (LSB = channel 0) to enable.
* This allows some of the channels to remain configured
* as GPIOs or as another function. Already used channels/timers will not be configured as DShot
* @param dshot_pwm_freq Frequency of DSHOT signal. Usually DSHOT150, DSHOT300, DSHOT600 or DSHOT1200
* @param dshot_pwm_freq Frequency of DSHOT signal. Usually DSHOT150, DSHOT300, or DSHOT600
* @return <0 on error, the initialized channels mask.
*/
__EXPORT extern int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bidirectional_dshot);

5
src/drivers/dshot/DShot.cpp
View File

@ -125,9 +125,6 @@ void DShot::enable_dshot_outputs(const bool enabled)
} else if (tim_config == -3) {
dshot_frequency_request = DSHOT600;
} else if (tim_config == -2) {
dshot_frequency_request = DSHOT1200;
} else {
_output_mask &= ~channels; // don't use for dshot
}
@ -824,7 +821,7 @@ On startup, the module tries to occupy all available pins for DShot output.
It skips all pins already in use (e.g. by a camera trigger module).
It supports:
- DShot150, DShot300, DShot600, DShot1200
- DShot150, DShot300, DShot600
- telemetry via separate UART and publishing as esc_status message
- sending DShot commands via CLI

2
src/drivers/dshot/DShot.h
View File

@ -52,7 +52,6 @@ using namespace time_literals;
static constexpr unsigned int DSHOT150 = 150000u;
static constexpr unsigned int DSHOT300 = 300000u;
static constexpr unsigned int DSHOT600 = 600000u;
static constexpr unsigned int DSHOT1200 = 1200000u;
static constexpr int DSHOT_DISARM_VALUE = 0;
static constexpr int DSHOT_MIN_THROTTLE = 1;
@ -107,7 +106,6 @@ private:
DShot150 = 150,
DShot300 = 300,
DShot600 = 600,
DShot1200 = 1200,
};
struct Command {

2
src/drivers/gps/devices

@ -1 +1 @@
Subproject commit e048340d0f6a395a3189292c33d08174bb309143
Subproject commit bbdd5767a961cc17bdcc577c79b1c708d2a7999a

3
src/drivers/ins/vectornav/VectorNav.cpp
View File

@ -328,7 +328,8 @@ void VectorNav::sensorCallback(VnUartPacket *packet)
local_position.vxy_max = INFINITY;
local_position.vz_max = INFINITY;
local_position.hagl_min = INFINITY;
local_position.hagl_max = INFINITY;
local_position.hagl_max_z = INFINITY;
local_position.hagl_max_xy = INFINITY;
local_position.unaided_heading = NAN;
local_position.timestamp = hrt_absolute_time();

29
src/drivers/power_monitor/ina238/ina238.cpp
View File

@ -160,6 +160,8 @@ int INA238::Reset()
int ret = PX4_ERROR;
_retries = 3;
if (RegisterWrite(Register::CONFIG, (uint16_t)(ADC_RESET_BIT)) != PX4_OK) {
return ret;
}
@ -231,6 +233,16 @@ int INA238::collect()
success = success && (RegisterRead(Register::CURRENT, (uint16_t &)current) == PX4_OK);
success = success && (RegisterRead(Register::DIETEMP, (uint16_t &)temperature) == PX4_OK);
if (success) {
_battery.updateVoltage(static_cast<float>(bus_voltage * INA238_VSCALE));
_battery.updateCurrent(static_cast<float>(current * _current_lsb));
_battery.updateTemperature(static_cast<float>(temperature * INA238_TSCALE));
_battery.setConnected(success);
_battery.updateAndPublishBatteryStatus(hrt_absolute_time());
}
if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) {
// check configuration registers periodically or immediately following any failure
if (RegisterCheck(_register_cfg[_checked_register])) {
@ -242,26 +254,21 @@ int INA238::collect()
PX4_DEBUG("register check failed");
perf_count(_bad_register_perf);
success = false;
_battery.setConnected(success);
_battery.updateAndPublishBatteryStatus(hrt_absolute_time());
}
}
if (!success) {
PX4_DEBUG("error reading from sensor");
bus_voltage = current = 0;
}
_battery.setConnected(success);
_battery.updateVoltage(static_cast<float>(bus_voltage * INA238_VSCALE));
_battery.updateCurrent(static_cast<float>(current * _current_lsb));
_battery.updateTemperature(static_cast<float>(temperature * INA238_TSCALE));
_battery.updateAndPublishBatteryStatus(hrt_absolute_time());
perf_end(_sample_perf);
if (success) {
return PX4_OK;
} else {
PX4_DEBUG("error reading from sensor");
return PX4_ERROR;
}
}

1
src/drivers/pwm_out/module.yaml
View File

@ -23,7 +23,6 @@ actuator_output:
-5: DShot150
-4: DShot300
-3: DShot600
-2: DShot1200
-1: OneShot
50: PWM 50 Hz
100: PWM 100 Hz

39
src/drivers/uavcan/uavcan_main.cpp
View File

@ -705,11 +705,12 @@ UavcanNode::Run()
if (can_init_res < 0) {
PX4_ERR("CAN driver init failed %i", can_init_res);
} else {
_instance->init(node_id, can->driver.updateEvent());
_node_init = true;
}
_instance->init(node_id, can->driver.updateEvent());
_node_init = true;
}
pthread_mutex_lock(&_node_mutex);
@ -1072,8 +1073,6 @@ void UavcanMixingInterfaceServo::Run()
void
UavcanNode::print_info()
{
(void)pthread_mutex_lock(&_node_mutex);
// Memory status
printf("Pool allocator status:\n");
printf("\tCapacity hard/soft: %" PRIu16 "/%" PRIu16 " blocks\n",
@ -1111,15 +1110,29 @@ UavcanNode::print_info()
printf("\n");
#if defined(CONFIG_UAVCAN_OUTPUTS_CONTROLLER)
printf("ESC outputs:\n");
_mixing_interface_esc.mixingOutput().printStatus();
printf("Servo outputs:\n");
_mixing_interface_servo.mixingOutput().printStatus();
// Print esc status if at least one channel is enabled
for (int i = 0; i < OutputModuleInterface::MAX_ACTUATORS; i++) {
if (_mixing_interface_esc.mixingOutput().isFunctionSet(i)) {
printf("ESC outputs:\n");
_mixing_interface_esc.mixingOutput().printStatus();
printf("\n");
break;
}
}
// Print servo status if at least one channel is enabled
for (int i = 0; i < OutputModuleInterface::MAX_ACTUATORS; i++) {
if (_mixing_interface_servo.mixingOutput().isFunctionSet(i)) {
printf("Servo outputs:\n");
_mixing_interface_servo.mixingOutput().printStatus();
printf("\n");
break;
}
}
#endif
printf("\n");
// Sensor bridges
for (const auto &br : _sensor_bridges) {
printf("Sensor '%s':\n", br->get_name());
@ -1139,8 +1152,6 @@ UavcanNode::print_info()
perf_print_counter(_cycle_perf);
perf_print_counter(_interval_perf);
(void)pthread_mutex_unlock(&_node_mutex);
}
void

1
src/lib/CMakeLists.txt
View File

@ -43,6 +43,7 @@ add_subdirectory(cdrstream EXCLUDE_FROM_ALL)
add_subdirectory(circuit_breaker EXCLUDE_FROM_ALL)
add_subdirectory(collision_prevention EXCLUDE_FROM_ALL)
add_subdirectory(component_information EXCLUDE_FROM_ALL)
add_subdirectory(control_allocation EXCLUDE_FROM_ALL)
add_subdirectory(controllib EXCLUDE_FROM_ALL)
add_subdirectory(conversion EXCLUDE_FROM_ALL)
add_subdirectory(crc EXCLUDE_FROM_ALL)

8
src/lib/collision_prevention/CMakeLists.txt
View File

@ -31,7 +31,13 @@
#
############################################################################
px4_add_library(CollisionPrevention CollisionPrevention.cpp)
px4_add_library(CollisionPrevention
CollisionPrevention.cpp
ObstacleMath.cpp
)
target_compile_options(CollisionPrevention PRIVATE -Wno-cast-align) # TODO: fix and enable
target_include_directories(CollisionPrevention PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
target_link_libraries(CollisionPrevention PRIVATE mathlib)
px4_add_functional_gtest(SRC CollisionPreventionTest.cpp LINKLIBS CollisionPrevention)
px4_add_unit_gtest(SRC ObstacleMathTest.cpp LINKLIBS CollisionPrevention)

15
src/lib/collision_prevention/CollisionPrevention.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2018 PX4 Development Team. All rights reserved.
* Copyright (c) 2018-2024 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -38,6 +38,7 @@
*/
#include "CollisionPrevention.hpp"
#include "ObstacleMath.hpp"
#include <px4_platform_common/events.h>
using namespace matrix;
@ -400,18 +401,8 @@ CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor,
int lower_bound = (int)round((sensor_yaw_body_deg - math::degrees(distance_sensor.h_fov / 2.0f)) / BIN_SIZE);
int upper_bound = (int)round((sensor_yaw_body_deg + math::degrees(distance_sensor.h_fov / 2.0f)) / BIN_SIZE);
const Quatf q_sensor(Quatf(cosf(sensor_yaw_body_rad / 2.f), 0.f, 0.f, sinf(sensor_yaw_body_rad / 2.f)));
const Vector3f forward_vector(1.0f, 0.0f, 0.0f);
const Quatf q_sensor_rotation = vehicle_attitude * q_sensor;
const Vector3f rotated_sensor_vector = q_sensor_rotation.rotateVector(forward_vector);
const float sensor_dist_scale = rotated_sensor_vector.xy().norm();
if (distance_reading < distance_sensor.max_distance) {
distance_reading = distance_reading * sensor_dist_scale;
ObstacleMath::project_distance_on_horizontal_plane(distance_reading, sensor_yaw_body_rad, vehicle_attitude);
}
uint16_t sensor_range = static_cast<uint16_t>(100.0f * distance_sensor.max_distance + 0.5f); // convert to cm

2
src/lib/collision_prevention/CollisionPrevention.hpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2018 PX4 Development Team. All rights reserved.
* Copyright (c) 2018-2024 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

112
src/lib/collision_prevention/ObstacleMath.cpp Normal file
View File

@ -0,0 +1,112 @@
/****************************************************************************
*
* Copyright (c) 2025 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 "ObstacleMath.hpp"
#include <mathlib/math/Limits.hpp>
using namespace matrix;
namespace ObstacleMath
{
void project_distance_on_horizontal_plane(float &distance, const float yaw, const matrix::Quatf &q_world_vehicle)
{
const Quatf q_vehicle_sensor(Quatf(cosf(yaw / 2.f), 0.f, 0.f, sinf(yaw / 2.f)));
const Quatf q_world_sensor = q_world_vehicle * q_vehicle_sensor;
const Vector3f forward(1.f, 0.f, 0.f);
const Vector3f sensor_direction_in_world = q_world_sensor.rotateVector(forward);
float horizontal_projection_scale = sensor_direction_in_world.xy().norm();
horizontal_projection_scale = math::constrain(horizontal_projection_scale, FLT_EPSILON, 1.0f);
distance *= horizontal_projection_scale;
}
int get_bin_at_angle(float bin_width, float angle)
{
int bin_at_angle = (int)round(matrix::wrap(angle, 0.f, 360.f) / bin_width);
return wrap_bin(bin_at_angle, 360 / bin_width);
}
int get_offset_bin_index(int bin, float bin_width, float angle_offset)
{
int offset = get_bin_at_angle(bin_width, angle_offset);
return wrap_bin(bin - offset, 360 / bin_width);
}
float sensor_orientation_to_yaw_offset(const SensorOrientation orientation)
{
float offset = 0.0f;
switch (orientation) {
case SensorOrientation::ROTATION_YAW_0:
offset = 0.0f;
break;
case SensorOrientation::ROTATION_YAW_45:
offset = M_PI_F / 4.0f;
break;
case SensorOrientation::ROTATION_YAW_90:
offset = M_PI_F / 2.0f;
break;
case SensorOrientation::ROTATION_YAW_135:
offset = 3.0f * M_PI_F / 4.0f;
break;
case SensorOrientation::ROTATION_YAW_180:
offset = M_PI_F;
break;
case SensorOrientation::ROTATION_YAW_225:
offset = -3.0f * M_PI_F / 4.0f;
break;
case SensorOrientation::ROTATION_YAW_270:
offset = -M_PI_F / 2.0f;
break;
case SensorOrientation::ROTATION_YAW_315:
offset = -M_PI_F / 4.0f;
break;
}
return offset;
}
int wrap_bin(int bin, int bin_count)
{
return (bin + bin_count) % bin_count;
}
} // ObstacleMath

91
src/lib/collision_prevention/ObstacleMath.hpp Normal file
View File

@ -0,0 +1,91 @@
/****************************************************************************
*
* Copyright (c) 2025 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 <matrix/math.hpp>
namespace ObstacleMath
{
enum SensorOrientation {
ROTATION_YAW_0 = 0, // MAV_SENSOR_ROTATION_NONE
ROTATION_YAW_45 = 1, // MAV_SENSOR_ROTATION_YAW_45
ROTATION_YAW_90 = 2, // MAV_SENSOR_ROTATION_YAW_90
ROTATION_YAW_135 = 3, // MAV_SENSOR_ROTATION_YAW_135
ROTATION_YAW_180 = 4, // MAV_SENSOR_ROTATION_YAW_180
ROTATION_YAW_225 = 5, // MAV_SENSOR_ROTATION_YAW_225
ROTATION_YAW_270 = 6, // MAV_SENSOR_ROTATION_YAW_270
ROTATION_YAW_315 = 7, // MAV_SENSOR_ROTATION_YAW_315
ROTATION_FORWARD_FACING = 0, // MAV_SENSOR_ROTATION_NONE
ROTATION_RIGHT_FACING = 2, // MAV_SENSOR_ROTATION_YAW_90
ROTATION_BACKWARD_FACING = 4, // MAV_SENSOR_ROTATION_YAW_180
ROTATION_LEFT_FACING = 6 // MAV_SENSOR_ROTATION_YAW_270
};
/**
* Converts a sensor orientation to a yaw offset
* @param orientation sensor orientation
*/
float sensor_orientation_to_yaw_offset(const SensorOrientation orientation);
/**
* Scales a distance measurement taken in the vehicle body horizontal plane onto the world horizontal plane
* @param distance measurement which is scaled down
* @param yaw orientation of the measurement on the body horizontal plane
* @param q_world_vehicle vehicle attitude quaternion
*/
void project_distance_on_horizontal_plane(float &distance, const float yaw, const matrix::Quatf &q_world_vehicle);
/**
* Returns bin index at a given angle from the 0th bin
* @param bin_width width of a bin in degrees
* @param angle clockwise angle from start bin in degrees
*/
int get_bin_at_angle(float bin_width, float angle);
/**
* Returns bin index for the current bin after an angle offset
* @param bin current bin index
* @param bin_width width of a bin in degrees
* @param angle_offset clockwise angle offset in degrees
*/
int get_offset_bin_index(int bin, float bin_width, float angle_offset);
/**
* Wraps a bin index to the range [0, bin_count)
* @param bin bin index
* @param bin_count number of bins
*/
int wrap_bin(int bin, int bin_count);
} // ObstacleMath

241
src/lib/collision_prevention/ObstacleMathTest.cpp Normal file
View File

@ -0,0 +1,241 @@
/****************************************************************************
*
* Copyright (C) 2025 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 <gtest/gtest.h>
#include <matrix/math.hpp>
#include <lib/mathlib/mathlib.h>
#include "ObstacleMath.hpp"
using namespace matrix;
TEST(ObstacleMathTest, ProjectDistanceOnHorizontalPlane)
{
// standard vehicle orientation inputs
Quatf vehicle_pitch_up_45(Eulerf(0.0f, M_PI_4_F, 0.0f));
Quatf vehicle_roll_right_45(Eulerf(M_PI_4_F, 0.0f, 0.0f));
// GIVEN: a distance, sensor orientation, and quaternion representing the vehicle's orientation
float distance = 1.0f;
float sensor_orientation = 0; // radians (forward facing)
// WHEN: we project the distance onto the horizontal plane
ObstacleMath::project_distance_on_horizontal_plane(distance, sensor_orientation, vehicle_pitch_up_45);
// THEN: the distance should be scaled correctly
float expected_scale = sqrtf(2) / 2;
float expected_distance = 1.0f * expected_scale;
EXPECT_NEAR(distance, expected_distance, 1e-5);
// GIVEN: a distance, sensor orientation, and quaternion representing the vehicle's orientation
distance = 1.0f;
ObstacleMath::project_distance_on_horizontal_plane(distance, sensor_orientation, vehicle_roll_right_45);
// THEN: the distance should be scaled correctly
expected_scale = 1.f;
expected_distance = 1.0f * expected_scale;
EXPECT_NEAR(distance, expected_distance, 1e-5);
// GIVEN: a distance, sensor orientation, and quaternion representing the vehicle's orientation
distance = 1.0f;
sensor_orientation = M_PI_2_F; // radians (right facing)
ObstacleMath::project_distance_on_horizontal_plane(distance, sensor_orientation, vehicle_roll_right_45);
// THEN: the distance should be scaled correctly
expected_scale = sqrtf(2) / 2;
expected_distance = 1.0f * expected_scale;
EXPECT_NEAR(distance, expected_distance, 1e-5);
// GIVEN: a distance, sensor orientation, and quaternion representing the vehicle's orientation
distance = 1.0f;
ObstacleMath::project_distance_on_horizontal_plane(distance, sensor_orientation, vehicle_pitch_up_45);
// THEN: the distance should be scaled correctly
expected_scale = 1.f;
expected_distance = 1.0f * expected_scale;
EXPECT_NEAR(distance, expected_distance, 1e-5);
}
TEST(ObstacleMathTest, GetBinAtAngle)
{
float bin_width = 5.0f;
// GIVEN: a start bin, bin width, and angle
float angle = 0.0f;
// WHEN: we calculate the bin index at the angle
uint16_t bin_index = ObstacleMath::get_bin_at_angle(bin_width, angle);
// THEN: the bin index should be correct
EXPECT_EQ(bin_index, 0);
// GIVEN: a start bin, bin width, and angle
angle = 90.0f;
// WHEN: we calculate the bin index at the angle
bin_index = ObstacleMath::get_bin_at_angle(bin_width, angle);
// THEN: the bin index should be correct
EXPECT_EQ(bin_index, 18);
// GIVEN: a start bin, bin width, and angle
angle = -90.0f;
// WHEN: we calculate the bin index at the angle
bin_index = ObstacleMath::get_bin_at_angle(bin_width, angle);
// THEN: the bin index should be correct
EXPECT_EQ(bin_index, 54);
// GIVEN: a start bin, bin width, and angle
angle = 450.0f;
// WHEN: we calculate the bin index at the angle
bin_index = ObstacleMath::get_bin_at_angle(bin_width, angle);
// THEN: the bin index should be correct
EXPECT_EQ(bin_index, 18);
}
TEST(ObstacleMathTest, OffsetBinIndex)
{
// In this test, we want to offset the bin index by a negative and positive angle.
// We take the output of the first offset and offset it by the same angle in the
// opposite direction to return back to the original bin index.
// GIVEN: a bin index, bin width, and a negative angle offset
uint16_t bin = 0;
float bin_width = 5.0f;
float angle_offset = -120.0f;
// WHEN: we offset the bin index by the negative angle
uint16_t new_bin_index = ObstacleMath::get_offset_bin_index(bin, bin_width, angle_offset);
// THEN: the new bin index should be correctly offset by the wrapped angle
EXPECT_EQ(new_bin_index, 24);
// GIVEN: the output bin index of the previous offset, bin width, and the same angle
// offset in positive direction
bin = 24;
bin_width = 5.0f;
angle_offset = 120.0f;
// WHEN: we offset the bin index by the positive angle
new_bin_index = ObstacleMath::get_offset_bin_index(bin, bin_width, angle_offset);
// THEN: the new bin index should return back to the original bin index
EXPECT_EQ(new_bin_index, 0);
}
TEST(ObstacleMathTest, WrapBin)
{
// GIVEN: a bin index within bounds and the number of bins
int bin = 0;
int bin_count = 72;
// WHEN: we wrap a bin index within the bounds
int wrapped_bin = ObstacleMath::wrap_bin(bin, bin_count);
// THEN: the wrapped bin index should stay 0
EXPECT_EQ(wrapped_bin, 0);
// GIVEN: a bin index that is out of bounds, and the number of bins
bin = 73;
bin_count = 72;
// WHEN: we wrap a bin index that is larger than the number of bins
wrapped_bin = ObstacleMath::wrap_bin(bin, bin_count);
// THEN: the wrapped bin index should be wrapped back to the beginning
EXPECT_EQ(wrapped_bin, 1);
// GIVEN: a negative bin index and the number of bins
bin = -1;
bin_count = 72;
// WHEN: we wrap a bin index that is negative
wrapped_bin = ObstacleMath::wrap_bin(bin, bin_count);
// THEN: the wrapped bin index should be wrapped back to the end
EXPECT_EQ(wrapped_bin, 71);
}
TEST(ObstacleMathTest, HandleMissedBins)
{
// In this test, the current and previous bin are adjacent to the bins that are outside
// the sensor field of view. The missed bins (0,1,6 & 7) should be populated, and no
// data should be filled in the bins outside the FOV.
// GIVEN: measurements, current bin, previous bin, bin width, and field of view offset
float measurements[8] = {0, 0, 1, 0, 0, 2, 0, 0};
int current_bin = 2;
int previous_bin = 5;
int bin_width = 45.0f;
float fov = 270.0f;
float fov_offset = 360.0f - fov / 2;
float measurement = measurements[current_bin];
// WHEN: we handle missed bins
int current_bin_offset = ObstacleMath::get_offset_bin_index(current_bin, bin_width, fov_offset);
int previous_bin_offset = ObstacleMath::get_offset_bin_index(previous_bin, bin_width, fov_offset);
int start = math::min(current_bin_offset, previous_bin_offset) + 1;
int end = math::max(current_bin_offset, previous_bin_offset);
EXPECT_EQ(start, 1);
EXPECT_EQ(end, 5);
for (uint16_t i = start; i < end; i++) {
uint16_t bin_index = ObstacleMath::get_offset_bin_index(i, bin_width, -fov_offset);
measurements[bin_index] = measurement;
}
// THEN: the correct missed bins should be populated with the measurement
EXPECT_EQ(measurements[0], 1);
EXPECT_EQ(measurements[1], 1);
EXPECT_EQ(measurements[2], 1);
EXPECT_EQ(measurements[3], 0);
EXPECT_EQ(measurements[4], 0);
EXPECT_EQ(measurements[5], 2);
EXPECT_EQ(measurements[6], 1);
EXPECT_EQ(measurements[7], 1);
}

35
src/lib/control_allocation/CMakeLists.txt Normal file
View File

@ -0,0 +1,35 @@
############################################################################
#
# Copyright (c) 2025 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.
#
############################################################################
add_subdirectory(control_allocation)
add_subdirectory(actuator_effectiveness)

13
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.cpp → src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.cpp
View File

@ -32,7 +32,6 @@
****************************************************************************/
#include "ActuatorEffectiveness.hpp"
#include "../ControlAllocation/ControlAllocation.hpp"
#include <px4_platform_common/log.h>
@ -51,12 +50,12 @@ int ActuatorEffectiveness::Configuration::addActuator(ActuatorType type, const m
return -1;
}
effectiveness_matrices[selected_matrix](ControlAllocation::ControlAxis::ROLL, actuator_idx) = torque(0);
effectiveness_matrices[selected_matrix](ControlAllocation::ControlAxis::PITCH, actuator_idx) = torque(1);
effectiveness_matrices[selected_matrix](ControlAllocation::ControlAxis::YAW, actuator_idx) = torque(2);
effectiveness_matrices[selected_matrix](ControlAllocation::ControlAxis::THRUST_X, actuator_idx) = thrust(0);
effectiveness_matrices[selected_matrix](ControlAllocation::ControlAxis::THRUST_Y, actuator_idx) = thrust(1);
effectiveness_matrices[selected_matrix](ControlAllocation::ControlAxis::THRUST_Z, actuator_idx) = thrust(2);
effectiveness_matrices[selected_matrix](ActuatorEffectiveness::ControlAxis::ROLL, actuator_idx) = torque(0);
effectiveness_matrices[selected_matrix](ActuatorEffectiveness::ControlAxis::PITCH, actuator_idx) = torque(1);
effectiveness_matrices[selected_matrix](ActuatorEffectiveness::ControlAxis::YAW, actuator_idx) = torque(2);
effectiveness_matrices[selected_matrix](ActuatorEffectiveness::ControlAxis::THRUST_X, actuator_idx) = thrust(0);
effectiveness_matrices[selected_matrix](ActuatorEffectiveness::ControlAxis::THRUST_Y, actuator_idx) = thrust(1);
effectiveness_matrices[selected_matrix](ActuatorEffectiveness::ControlAxis::THRUST_Z, actuator_idx) = thrust(2);
matrix_selection_indexes[totalNumActuators()] = selected_matrix;
++num_actuators[(int)type];
return num_actuators_matrix[selected_matrix]++;

0
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp → src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp
View File

32
src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt → src/lib/control_allocation/actuator_effectiveness/CMakeLists.txt
View File

@ -34,34 +34,6 @@
px4_add_library(ActuatorEffectiveness
ActuatorEffectiveness.cpp
ActuatorEffectiveness.hpp
ActuatorEffectivenessUUV.cpp
ActuatorEffectivenessUUV.hpp
ActuatorEffectivenessControlSurfaces.cpp
ActuatorEffectivenessControlSurfaces.hpp
ActuatorEffectivenessCustom.cpp
ActuatorEffectivenessCustom.hpp
ActuatorEffectivenessFixedWing.cpp
ActuatorEffectivenessFixedWing.hpp
ActuatorEffectivenessHelicopter.cpp
ActuatorEffectivenessHelicopter.hpp
ActuatorEffectivenessHelicopterCoaxial.cpp
ActuatorEffectivenessHelicopterCoaxial.hpp
ActuatorEffectivenessMCTilt.cpp
ActuatorEffectivenessMCTilt.hpp
ActuatorEffectivenessMultirotor.cpp
ActuatorEffectivenessMultirotor.hpp
ActuatorEffectivenessTilts.cpp
ActuatorEffectivenessTilts.hpp
ActuatorEffectivenessRotors.cpp
ActuatorEffectivenessRotors.hpp
ActuatorEffectivenessStandardVTOL.cpp
ActuatorEffectivenessStandardVTOL.hpp
ActuatorEffectivenessTiltrotorVTOL.cpp
ActuatorEffectivenessTiltrotorVTOL.hpp
ActuatorEffectivenessTailsitterVTOL.cpp
ActuatorEffectivenessTailsitterVTOL.hpp
ActuatorEffectivenessRoverAckermann.hpp
ActuatorEffectivenessRoverAckermann.cpp
)
target_compile_options(ActuatorEffectiveness PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
@ -69,7 +41,5 @@ target_include_directories(ActuatorEffectiveness PUBLIC ${CMAKE_CURRENT_SOURCE_D
target_link_libraries(ActuatorEffectiveness
PRIVATE
mathlib
PID
)
px4_add_functional_gtest(SRC ActuatorEffectivenessHelicopterTest.cpp LINKLIBS ActuatorEffectiveness)
px4_add_functional_gtest(SRC ActuatorEffectivenessRotorsTest.cpp LINKLIBS ActuatorEffectiveness)

0
src/modules/control_allocator/ControlAllocation/CMakeLists.txt → src/lib/control_allocation/control_allocation/CMakeLists.txt
View File

0
src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp → src/lib/control_allocation/control_allocation/ControlAllocation.cpp
View File

2
src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp → src/lib/control_allocation/control_allocation/ControlAllocation.hpp
View File

@ -71,7 +71,7 @@
#include <matrix/matrix/math.hpp>
#include "ActuatorEffectiveness/ActuatorEffectiveness.hpp"
#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp"
class ControlAllocation
{

16
src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp → src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.cpp
View File

@ -51,6 +51,11 @@ ControlAllocationPseudoInverse::setEffectivenessMatrix(
update_normalization_scale);
_mix_update_needed = true;
_normalization_needs_update = update_normalization_scale;
if (_metric_allocation && update_normalization_scale) {
// adding #include <px4_platform_common/log.h> + PX4_WARN leads to failed linking on test
_normalization_needs_update = false;
}
}
void
@ -59,12 +64,15 @@ ControlAllocationPseudoInverse::updatePseudoInverse()
if (_mix_update_needed) {
matrix::geninv(_effectiveness, _mix);
if (_normalization_needs_update && !_had_actuator_failure) {
updateControlAllocationMatrixScale();
_normalization_needs_update = false;
if (!_metric_allocation) {
if (_normalization_needs_update && !_had_actuator_failure) {
updateControlAllocationMatrixScale();
_normalization_needs_update = false;
}
normalizeControlAllocationMatrix();
}
normalizeControlAllocationMatrix();
_mix_update_needed = false;
}
}

2
src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.hpp → src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.hpp
View File

@ -57,11 +57,13 @@ public:
void setEffectivenessMatrix(const matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &effectiveness,
const ActuatorVector &actuator_trim, const ActuatorVector &linearization_point, int num_actuators,
bool update_normalization_scale) override;
void setMetricAllocation(bool metric_allocation) { _metric_allocation = metric_allocation; }
protected:
matrix::Matrix<float, NUM_ACTUATORS, NUM_AXES> _mix;
bool _mix_update_needed{false};
bool _metric_allocation{false};
/**
* Recalculate pseudo inverse if required.

24
src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverseTest.cpp → src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverseTest.cpp
View File

@ -67,3 +67,27 @@ TEST(ControlAllocationTest, AllZeroCase)
EXPECT_EQ(actuator_sp, actuator_sp_expected);
EXPECT_EQ(control_allocated, control_allocated_expected);
}
TEST(ControlAllocationMetricTest, AllZeroCase)
{
ControlAllocationPseudoInverse method;
matrix::Vector<float, 6> control_sp;
matrix::Vector<float, 6> control_allocated;
matrix::Vector<float, 6> control_allocated_expected;
matrix::Matrix<float, 6, 16> effectiveness;
matrix::Vector<float, 16> actuator_sp;
matrix::Vector<float, 16> actuator_trim;
matrix::Vector<float, 16> linearization_point;
matrix::Vector<float, 16> actuator_sp_expected;
method.setMetricAllocation(true);
method.setEffectivenessMatrix(effectiveness, actuator_trim, linearization_point, 16, false);
method.setControlSetpoint(control_sp);
method.allocate();
actuator_sp = method.getActuatorSetpoint();
control_allocated_expected = method.getAllocatedControl();
EXPECT_EQ(actuator_sp, actuator_sp_expected);
EXPECT_EQ(control_allocated, control_allocated_expected);
}

0
src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp → src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturation.cpp
View File

0
src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.hpp → src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturation.hpp
View File

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