diff --git a/.github/workflows/dev_container.yml b/.github/workflows/dev_container.yml index bcfbdc2c6b..f64d29b684 100644 --- a/.github/workflows/dev_container.yml +++ b/.github/workflows/dev_container.yml @@ -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 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10040_sihsim_quadx b/ROMFS/px4fmu_common/init.d-posix/airframes/10040_sihsim_quadx index 077d908ec5..623ee41f1f 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/10040_sihsim_quadx +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10040_sihsim_quadx @@ -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 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10041_sihsim_airplane b/ROMFS/px4fmu_common/init.d-posix/airframes/10041_sihsim_airplane index 36a69b7714..4f1ddf2635 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/10041_sihsim_airplane +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10041_sihsim_airplane @@ -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 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10042_sihsim_xvert b/ROMFS/px4fmu_common/init.d-posix/airframes/10042_sihsim_xvert index d82b92c1ef..ee5ea9401c 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/10042_sihsim_xvert +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10042_sihsim_xvert @@ -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 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10043_sihsim_standard_vtol b/ROMFS/px4fmu_common/init.d-posix/airframes/10043_sihsim_standard_vtol new file mode 100644 index 0000000000..4abc9de264 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10043_sihsim_standard_vtol @@ -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 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt index da741d2a64..51d9eab72e 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt @@ -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 diff --git a/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator b/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator index 90e86aa688..fd0954744e 100644 --- a/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator +++ b/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator @@ -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" diff --git a/ROMFS/px4fmu_common/init.d/airframes/1002_standard_vtol.hil b/ROMFS/px4fmu_common/init.d/airframes/1002_standard_vtol.hil index b5a633dc1b..4129d2569c 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/1002_standard_vtol.hil +++ b/ROMFS/px4fmu_common/init.d/airframes/1002_standard_vtol.hil @@ -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 # # @board px4_fmu-v2 exclude diff --git a/ROMFS/px4fmu_common/init.d/airframes/1101_rc_plane_sih.hil b/ROMFS/px4fmu_common/init.d/airframes/1101_rc_plane_sih.hil index 2bcfc14a2e..2b54650a07 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/1101_rc_plane_sih.hil +++ b/ROMFS/px4fmu_common/init.d/airframes/1101_rc_plane_sih.hil @@ -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 diff --git a/ROMFS/px4fmu_common/init.d/airframes/1103_standard_vtol_sih.hil b/ROMFS/px4fmu_common/init.d/airframes/1103_standard_vtol_sih.hil new file mode 100644 index 0000000000..5ac9e6b1ab --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/airframes/1103_standard_vtol_sih.hil @@ -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 diff --git a/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt index 06d123d857..d9142ec1c5 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt @@ -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() diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_defaults b/ROMFS/px4fmu_common/init.d/rc.fw_defaults index db15c3d09d..e6271d8144 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.fw_defaults @@ -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 diff --git a/ROMFS/px4fmu_common/init.d/rc.heli_defaults b/ROMFS/px4fmu_common/init.d/rc.heli_defaults index 42d2aca268..93e6f0d33f 100644 --- a/ROMFS/px4fmu_common/init.d/rc.heli_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.heli_defaults @@ -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 diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_defaults b/ROMFS/px4fmu_common/init.d/rc.mc_defaults index 7b2b9b095d..db11cedbb3 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.mc_defaults @@ -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 diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 5cdd34e698..807a26ed03 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -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. diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index 4aee08f388..34f3be05ca 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -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 diff --git a/Tools/simulation/gazebo-classic/sitl_gazebo-classic b/Tools/simulation/gazebo-classic/sitl_gazebo-classic index c0e0751341..fbd8e9e6bb 160000 --- a/Tools/simulation/gazebo-classic/sitl_gazebo-classic +++ b/Tools/simulation/gazebo-classic/sitl_gazebo-classic @@ -1 +1 @@ -Subproject commit c0e0751341d46108377bbae2ae1bb6da8a5d4106 +Subproject commit fbd8e9e6bbd2188de81677494f15885dead99c48 diff --git a/boards/ark/fmu-v6x/init/rc.board_defaults b/boards/ark/fmu-v6x/init/rc.board_defaults index 52d098101d..d0732e90ef 100644 --- a/boards/ark/fmu-v6x/init/rc.board_defaults +++ b/boards/ark/fmu-v6x/init/rc.board_defaults @@ -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 diff --git a/boards/ark/fmu-v6x/nuttx-config/nsh/defconfig b/boards/ark/fmu-v6x/nuttx-config/nsh/defconfig index f5e59d2c37..94fbcc11b5 100644 --- a/boards/ark/fmu-v6x/nuttx-config/nsh/defconfig +++ b/boards/ark/fmu-v6x/nuttx-config/nsh/defconfig @@ -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 diff --git a/boards/ark/fpv/default.px4board b/boards/ark/fpv/default.px4board index 92d697f015..8c50ddc5b0 100644 --- a/boards/ark/fpv/default.px4board +++ b/boards/ark/fpv/default.px4board @@ -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 diff --git a/boards/ark/pi6x/init/rc.board_defaults b/boards/ark/pi6x/init/rc.board_defaults index 717632839b..1b92949d1d 100644 --- a/boards/ark/pi6x/init/rc.board_defaults +++ b/boards/ark/pi6x/init/rc.board_defaults @@ -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 diff --git a/boards/bitcraze/crazyflie/default.px4board b/boards/bitcraze/crazyflie/default.px4board index 84139b6dec..35abd8e338 100644 --- a/boards/bitcraze/crazyflie/default.px4board +++ b/boards/bitcraze/crazyflie/default.px4board @@ -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 diff --git a/boards/bitcraze/crazyflie21/default.px4board b/boards/bitcraze/crazyflie21/default.px4board index 13c44d61d9..0db161f13e 100644 --- a/boards/bitcraze/crazyflie21/default.px4board +++ b/boards/bitcraze/crazyflie21/default.px4board @@ -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 diff --git a/boards/cuav/7-nano/src/board_config.h b/boards/cuav/7-nano/src/board_config.h index 269a4890f3..a2c564ae89 100644 --- a/boards/cuav/7-nano/src/board_config.h +++ b/boards/cuav/7-nano/src/board_config.h @@ -352,6 +352,7 @@ GPIO_nSAFETY_SWITCH_LED_OUT, \ GPIO_SAFETY_SWITCH_IN, \ GPIO_nARMED, \ + GPIO_PWM_LEVEL_CONTROL, \ } #define BOARD_ENABLE_CONSOLE_BUFFER diff --git a/boards/diatone/mamba-f405-mk2/default.px4board b/boards/diatone/mamba-f405-mk2/default.px4board index 14e8219028..efb1faeb49 100644 --- a/boards/diatone/mamba-f405-mk2/default.px4board +++ b/boards/diatone/mamba-f405-mk2/default.px4board @@ -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 diff --git a/boards/flywoo/gn-f405/default.px4board b/boards/flywoo/gn-f405/default.px4board index bd7ea9015d..c1f2f643bc 100644 --- a/boards/flywoo/gn-f405/default.px4board +++ b/boards/flywoo/gn-f405/default.px4board @@ -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 diff --git a/boards/holybro/kakutef7/default.px4board b/boards/holybro/kakutef7/default.px4board index 49b8f9e0d5..82a3261e87 100644 --- a/boards/holybro/kakutef7/default.px4board +++ b/boards/holybro/kakutef7/default.px4board @@ -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 diff --git a/boards/holybro/kakuteh7/default.px4board b/boards/holybro/kakuteh7/default.px4board index 807121aa3a..bde1cf04f2 100644 --- a/boards/holybro/kakuteh7/default.px4board +++ b/boards/holybro/kakuteh7/default.px4board @@ -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 diff --git a/boards/holybro/kakuteh7mini/default.px4board b/boards/holybro/kakuteh7mini/default.px4board index e2b652acff..7e2e76bfd6 100644 --- a/boards/holybro/kakuteh7mini/default.px4board +++ b/boards/holybro/kakuteh7mini/default.px4board @@ -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 diff --git a/boards/holybro/kakuteh7v2/default.px4board b/boards/holybro/kakuteh7v2/default.px4board index c27160493f..bd440170ba 100644 --- a/boards/holybro/kakuteh7v2/default.px4board +++ b/boards/holybro/kakuteh7v2/default.px4board @@ -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 diff --git a/boards/micoair/h743-aio/default.px4board b/boards/micoair/h743-aio/default.px4board index e82b317983..7af585274e 100644 --- a/boards/micoair/h743-aio/default.px4board +++ b/boards/micoair/h743-aio/default.px4board @@ -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 diff --git a/boards/micoair/h743-v2/default.px4board b/boards/micoair/h743-v2/default.px4board index ca206835a5..518132aa8d 100644 --- a/boards/micoair/h743-v2/default.px4board +++ b/boards/micoair/h743-v2/default.px4board @@ -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 diff --git a/boards/micoair/h743-v2/nuttx-config/bootloader/defconfig b/boards/micoair/h743-v2/nuttx-config/bootloader/defconfig index b2c2fc88ca..10f43ff17d 100644 --- a/boards/micoair/h743-v2/nuttx-config/bootloader/defconfig +++ b/boards/micoair/h743-v2/nuttx-config/bootloader/defconfig @@ -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 diff --git a/boards/micoair/h743-v2/nuttx-config/nsh/defconfig b/boards/micoair/h743-v2/nuttx-config/nsh/defconfig index 5a1f555257..dab70e35fa 100644 --- a/boards/micoair/h743-v2/nuttx-config/nsh/defconfig +++ b/boards/micoair/h743-v2/nuttx-config/nsh/defconfig @@ -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 diff --git a/boards/micoair/h743/default.px4board b/boards/micoair/h743/default.px4board index b4b18d027d..207bb39bbb 100644 --- a/boards/micoair/h743/default.px4board +++ b/boards/micoair/h743/default.px4board @@ -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 diff --git a/boards/modalai/fc-v1/default.px4board b/boards/modalai/fc-v1/default.px4board index 202c1e5a5f..7f60b557f2 100644 --- a/boards/modalai/fc-v1/default.px4board +++ b/boards/modalai/fc-v1/default.px4board @@ -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 diff --git a/boards/nxp/tropic-community/default.px4board b/boards/nxp/tropic-community/default.px4board index 471ddd1412..c1beb1fafd 100644 --- a/boards/nxp/tropic-community/default.px4board +++ b/boards/nxp/tropic-community/default.px4board @@ -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 diff --git a/boards/nxp/tropic-community/nuttx-config/nsh/defconfig b/boards/nxp/tropic-community/nuttx-config/nsh/defconfig index 4b789eb358..15122b439d 100644 --- a/boards/nxp/tropic-community/nuttx-config/nsh/defconfig +++ b/boards/nxp/tropic-community/nuttx-config/nsh/defconfig @@ -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 diff --git a/boards/nxp/tropic-community/nuttx-config/scripts/itcm_static_functions.ld b/boards/nxp/tropic-community/nuttx-config/scripts/itcm_static_functions.ld index 8b228a22f8..2f6259209a 100644 --- a/boards/nxp/tropic-community/nuttx-config/scripts/itcm_static_functions.ld +++ b/boards/nxp/tropic-community/nuttx-config/scripts/itcm_static_functions.ld @@ -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) diff --git a/boards/nxp/tropic-community/nuttx-config/scripts/script.ld b/boards/nxp/tropic-community/nuttx-config/scripts/script.ld index a715059912..01d2e8d951 100644 --- a/boards/nxp/tropic-community/nuttx-config/scripts/script.ld +++ b/boards/nxp/tropic-community/nuttx-config/scripts/script.ld @@ -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 diff --git a/boards/nxp/tropic-community/src/CMakeLists.txt b/boards/nxp/tropic-community/src/CMakeLists.txt index 586c60841e..6c37994652 100644 --- a/boards/nxp/tropic-community/src/CMakeLists.txt +++ b/boards/nxp/tropic-community/src/CMakeLists.txt @@ -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 diff --git a/boards/nxp/tropic-community/src/board_config.h b/boards/nxp/tropic-community/src/board_config.h index cef895ab6c..83bbd101e2 100644 --- a/boards/nxp/tropic-community/src/board_config.h +++ b/boards/nxp/tropic-community/src/board_config.h @@ -326,6 +326,8 @@ extern void fmurt1062_timer_initialize(void); #include +int imxrt_flexspi_storage_initialize(void); + #endif /* __ASSEMBLY__ */ __END_DECLS diff --git a/boards/nxp/tropic-community/src/imxrt_flexspi_storage.c b/boards/nxp/tropic-community/src/imxrt_flexspi_storage.c new file mode 100644 index 0000000000..f3ac06804f --- /dev/null +++ b/boards/nxp/tropic-community/src/imxrt_flexspi_storage.c @@ -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 + +#include +#include +#include + +#include +#include +#include +#include + +#include +#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 */ diff --git a/boards/nxp/tropic-community/src/imxrt_ocram_initialize.c b/boards/nxp/tropic-community/src/imxrt_ocram_initialize.c new file mode 100644 index 0000000000..67c1a4ca36 --- /dev/null +++ b/boards/nxp/tropic-community/src/imxrt_ocram_initialize.c @@ -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; + } +} diff --git a/boards/nxp/tropic-community/src/init.c b/boards/nxp/tropic-community/src/init.c index f3b43fc4d5..8e5db86b37 100644 --- a/boards/nxp/tropic-community/src/init.c +++ b/boards/nxp/tropic-community/src/init.c @@ -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; } diff --git a/boards/omnibus/f4sd/default.px4board b/boards/omnibus/f4sd/default.px4board index 470b95db5d..5aeb871f81 100644 --- a/boards/omnibus/f4sd/default.px4board +++ b/boards/omnibus/f4sd/default.px4board @@ -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 diff --git a/boards/px4/fmu-v2/default.px4board b/boards/px4/fmu-v2/default.px4board index 479e394f6f..c5255743df 100644 --- a/boards/px4/fmu-v2/default.px4board +++ b/boards/px4/fmu-v2/default.px4board @@ -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 diff --git a/boards/px4/fmu-v5/cryptotest.px4board b/boards/px4/fmu-v5/cryptotest.px4board index cc31ed3c2e..a74698d42d 100644 --- a/boards/px4/fmu-v5/cryptotest.px4board +++ b/boards/px4/fmu-v5/cryptotest.px4board @@ -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" diff --git a/boards/px4/fmu-v5/rover.px4board b/boards/px4/fmu-v5/rover.px4board index 880d996b39..5fa964bc3a 100644 --- a/boards/px4/fmu-v5/rover.px4board +++ b/boards/px4/fmu-v5/rover.px4board @@ -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 diff --git a/boards/px4/fmu-v5/stackcheck.px4board b/boards/px4/fmu-v5/stackcheck.px4board index 6ef7f40397..b182a1c83a 100644 --- a/boards/px4/fmu-v5/stackcheck.px4board +++ b/boards/px4/fmu-v5/stackcheck.px4board @@ -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 diff --git a/boards/px4/fmu-v5x/rover.px4board b/boards/px4/fmu-v5x/rover.px4board index 101ea6fb1d..2490c28c6b 100644 --- a/boards/px4/fmu-v5x/rover.px4board +++ b/boards/px4/fmu-v5x/rover.px4board @@ -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 diff --git a/boards/px4/fmu-v6c/rover.px4board b/boards/px4/fmu-v6c/rover.px4board index e7a155c673..ee7cddbe05 100644 --- a/boards/px4/fmu-v6c/rover.px4board +++ b/boards/px4/fmu-v6c/rover.px4board @@ -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 diff --git a/boards/px4/fmu-v6u/rover.px4board b/boards/px4/fmu-v6u/rover.px4board index e7a155c673..ee7cddbe05 100644 --- a/boards/px4/fmu-v6u/rover.px4board +++ b/boards/px4/fmu-v6u/rover.px4board @@ -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 diff --git a/boards/px4/fmu-v6x/multicopter.px4board b/boards/px4/fmu-v6x/multicopter.px4board index e0c8293069..b501d6fe4a 100644 --- a/boards/px4/fmu-v6x/multicopter.px4board +++ b/boards/px4/fmu-v6x/multicopter.px4board @@ -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 diff --git a/boards/px4/fmu-v6x/rover.px4board b/boards/px4/fmu-v6x/rover.px4board index e7a155c673..ee7cddbe05 100644 --- a/boards/px4/fmu-v6x/rover.px4board +++ b/boards/px4/fmu-v6x/rover.px4board @@ -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 diff --git a/boards/px4/fmu-v6xrt/rover.px4board b/boards/px4/fmu-v6xrt/rover.px4board index 3310f5d8e4..a2e269939a 100644 --- a/boards/px4/fmu-v6xrt/rover.px4board +++ b/boards/px4/fmu-v6xrt/rover.px4board @@ -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 diff --git a/boards/px4/fmu-v6xrt/src/CMakeLists.txt b/boards/px4/fmu-v6xrt/src/CMakeLists.txt index e34b9a134c..1ab333085b 100644 --- a/boards/px4/fmu-v6xrt/src/CMakeLists.txt +++ b/boards/px4/fmu-v6xrt/src/CMakeLists.txt @@ -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 diff --git a/boards/px4/fmu-v6xrt/src/imxrt_ocram_initialize.c b/boards/px4/fmu-v6xrt/src/imxrt_ocram_initialize.c new file mode 100644 index 0000000000..5a063bd46a --- /dev/null +++ b/boards/px4/fmu-v6xrt/src/imxrt_ocram_initialize.c @@ -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 +} diff --git a/boards/px4/fmu-v6xrt/src/init.c b/boards/px4/fmu-v6xrt/src/init.c index 4b2edbc658..57ac95ce20 100644 --- a/boards/px4/fmu-v6xrt/src/init.c +++ b/boards/px4/fmu-v6xrt/src/init.c @@ -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 diff --git a/boards/px4/sitl/default.px4board b/boards/px4/sitl/default.px4board index e091ca76bd..296db85c4f 100644 --- a/boards/px4/sitl/default.px4board +++ b/boards/px4/sitl/default.px4board @@ -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 diff --git a/boards/raspberrypi/pico/default.px4board b/boards/raspberrypi/pico/default.px4board index d481cf3478..27329c23bc 100644 --- a/boards/raspberrypi/pico/default.px4board +++ b/boards/raspberrypi/pico/default.px4board @@ -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 diff --git a/msg/Ekf2Timestamps.msg b/msg/Ekf2Timestamps.msg index ae3ac06766..2487cf0200 100644 --- a/msg/Ekf2Timestamps.msg +++ b/msg/Ekf2Timestamps.msg @@ -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 diff --git a/msg/EstimatorStatusFlags.msg b/msg/EstimatorStatusFlags.msg index 705a5f9a71..0e5525d545 100644 --- a/msg/EstimatorStatusFlags.msg +++ b/msg/EstimatorStatusFlags.msg @@ -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 diff --git a/msg/PwmInput.msg b/msg/PwmInput.msg index fcc7dbe4ac..805d6fbd6b 100644 --- a/msg/PwmInput.msg +++ b/msg/PwmInput.msg @@ -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) diff --git a/msg/Rpm.msg b/msg/Rpm.msg index ca69e50fdb..b4de2cf422 100644 --- a/msg/Rpm.msg +++ b/msg/Rpm.msg @@ -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 diff --git a/msg/VehicleImu.msg b/msg/VehicleImu.msg index a71bb7a01f..24912e06fb 100644 --- a/msg/VehicleImu.msg +++ b/msg/VehicleImu.msg @@ -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 diff --git a/msg/versioned/VehicleLocalPosition.msg b/msg/versioned/VehicleLocalPosition.msg index aec6311ab3..8c04b4153c 100644 --- a/msg/versioned/VehicleLocalPosition.msg +++ b/msg/versioned/VehicleLocalPosition.msg @@ -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 diff --git a/platforms/nuttx/CMakeLists.txt b/platforms/nuttx/CMakeLists.txt index ba89894efe..19fcae552e 100644 --- a/platforms/nuttx/CMakeLists.txt +++ b/platforms/nuttx/CMakeLists.txt @@ -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() diff --git a/platforms/nuttx/NuttX/nuttx b/platforms/nuttx/NuttX/nuttx index e61fdd019d..9a213327fb 160000 --- a/platforms/nuttx/NuttX/nuttx +++ b/platforms/nuttx/NuttX/nuttx @@ -1 +1 @@ -Subproject commit e61fdd019de6ee7685c071760a965961c5ef5227 +Subproject commit 9a213327fbb2dab252185d468fb29348e1833d8c diff --git a/src/drivers/distance_sensor/lightware_sf45_serial/CMakeLists.txt b/src/drivers/distance_sensor/lightware_sf45_serial/CMakeLists.txt index 561b7755c7..6d561f72db 100755 --- a/src/drivers/distance_sensor/lightware_sf45_serial/CMakeLists.txt +++ b/src/drivers/distance_sensor/lightware_sf45_serial/CMakeLists.txt @@ -41,6 +41,7 @@ px4_add_module( DEPENDS drivers_rangefinder px4_work_queue + CollisionPrevention MODULE_CONFIG module.yaml ) diff --git a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp index a8ed20383d..7f16fa5d26 100755 --- a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp +++ b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp @@ -33,19 +33,18 @@ #include "lightware_sf45_serial.hpp" -#include #include +#include +#include #include + #include #include - -#include +#include +#include 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 + (_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(_current_bin_dist); + float scaled_yaw_rad = math::radians(static_cast(scaled_yaw)); + ObstacleMath::project_distance_on_horizontal_plane(current_bin_dist, scaled_yaw_rad, _vehicle_attitude); + _current_bin_dist = static_cast(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; } diff --git a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp index c5cae5e049..d308a474d4 100755 --- a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp +++ b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp @@ -49,7 +49,9 @@ #include #include +#include #include +#include #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 diff --git a/src/drivers/distance_sensor/ulanding_radar/ulanding_radar_main.cpp b/src/drivers/distance_sensor/ulanding_radar/ulanding_radar_main.cpp index 25219f6a7d..d8898fbb68 100644 --- a/src/drivers/distance_sensor/ulanding_radar/ulanding_radar_main.cpp +++ b/src/drivers/distance_sensor/ulanding_radar/ulanding_radar_main.cpp @@ -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 diff --git a/src/drivers/drv_dshot.h b/src/drivers/drv_dshot.h index 9c36dfd52d..431a56787d 100644 --- a/src/drivers/drv_dshot.h +++ b/src/drivers/drv_dshot.h @@ -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); diff --git a/src/drivers/dshot/DShot.cpp b/src/drivers/dshot/DShot.cpp index 940019d9bf..4bd9bdef19 100644 --- a/src/drivers/dshot/DShot.cpp +++ b/src/drivers/dshot/DShot.cpp @@ -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 diff --git a/src/drivers/dshot/DShot.h b/src/drivers/dshot/DShot.h index e60b33b862..4fc312c3e9 100644 --- a/src/drivers/dshot/DShot.h +++ b/src/drivers/dshot/DShot.h @@ -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 { diff --git a/src/drivers/gps/devices b/src/drivers/gps/devices index e048340d0f..bbdd5767a9 160000 --- a/src/drivers/gps/devices +++ b/src/drivers/gps/devices @@ -1 +1 @@ -Subproject commit e048340d0f6a395a3189292c33d08174bb309143 +Subproject commit bbdd5767a961cc17bdcc577c79b1c708d2a7999a diff --git a/src/drivers/ins/vectornav/VectorNav.cpp b/src/drivers/ins/vectornav/VectorNav.cpp index 85dd4c1846..e4e9821d47 100644 --- a/src/drivers/ins/vectornav/VectorNav.cpp +++ b/src/drivers/ins/vectornav/VectorNav.cpp @@ -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(); diff --git a/src/drivers/power_monitor/ina238/ina238.cpp b/src/drivers/power_monitor/ina238/ina238.cpp index 42f5e12858..b46b277a84 100644 --- a/src/drivers/power_monitor/ina238/ina238.cpp +++ b/src/drivers/power_monitor/ina238/ina238.cpp @@ -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(bus_voltage * INA238_VSCALE)); + _battery.updateCurrent(static_cast(current * _current_lsb)); + _battery.updateTemperature(static_cast(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(bus_voltage * INA238_VSCALE)); - _battery.updateCurrent(static_cast(current * _current_lsb)); - _battery.updateTemperature(static_cast(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; } } diff --git a/src/drivers/pwm_out/module.yaml b/src/drivers/pwm_out/module.yaml index c63b4195d2..8c2049cceb 100644 --- a/src/drivers/pwm_out/module.yaml +++ b/src/drivers/pwm_out/module.yaml @@ -23,7 +23,6 @@ actuator_output: -5: DShot150 -4: DShot300 -3: DShot600 - -2: DShot1200 -1: OneShot 50: PWM 50 Hz 100: PWM 100 Hz diff --git a/src/drivers/uavcan/uavcan_main.cpp b/src/drivers/uavcan/uavcan_main.cpp index 87f3110cac..ad39a943a7 100644 --- a/src/drivers/uavcan/uavcan_main.cpp +++ b/src/drivers/uavcan/uavcan_main.cpp @@ -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 diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index 69ab23aacc..8f58109b89 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -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) diff --git a/src/lib/collision_prevention/CMakeLists.txt b/src/lib/collision_prevention/CMakeLists.txt index 663c9e9fee..ae42b65454 100644 --- a/src/lib/collision_prevention/CMakeLists.txt +++ b/src/lib/collision_prevention/CMakeLists.txt @@ -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) diff --git a/src/lib/collision_prevention/CollisionPrevention.cpp b/src/lib/collision_prevention/CollisionPrevention.cpp index b2c62d4906..3f1344ffdd 100644 --- a/src/lib/collision_prevention/CollisionPrevention.cpp +++ b/src/lib/collision_prevention/CollisionPrevention.cpp @@ -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 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(100.0f * distance_sensor.max_distance + 0.5f); // convert to cm diff --git a/src/lib/collision_prevention/CollisionPrevention.hpp b/src/lib/collision_prevention/CollisionPrevention.hpp index 8fcc7c28f3..7d9d16cd7f 100644 --- a/src/lib/collision_prevention/CollisionPrevention.hpp +++ b/src/lib/collision_prevention/CollisionPrevention.hpp @@ -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 diff --git a/src/lib/collision_prevention/ObstacleMath.cpp b/src/lib/collision_prevention/ObstacleMath.cpp new file mode 100644 index 0000000000..0605b664ec --- /dev/null +++ b/src/lib/collision_prevention/ObstacleMath.cpp @@ -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 + +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 diff --git a/src/lib/collision_prevention/ObstacleMath.hpp b/src/lib/collision_prevention/ObstacleMath.hpp new file mode 100644 index 0000000000..f5317aeaa7 --- /dev/null +++ b/src/lib/collision_prevention/ObstacleMath.hpp @@ -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 + +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 diff --git a/src/lib/collision_prevention/ObstacleMathTest.cpp b/src/lib/collision_prevention/ObstacleMathTest.cpp new file mode 100644 index 0000000000..cea54aa4a2 --- /dev/null +++ b/src/lib/collision_prevention/ObstacleMathTest.cpp @@ -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 +#include +#include +#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); +} diff --git a/src/lib/control_allocation/CMakeLists.txt b/src/lib/control_allocation/CMakeLists.txt new file mode 100644 index 0000000000..f7c710b7f2 --- /dev/null +++ b/src/lib/control_allocation/CMakeLists.txt @@ -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) diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.cpp b/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.cpp similarity index 83% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.cpp rename to src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.cpp index 901c469746..b36da8230f 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.cpp +++ b/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.cpp @@ -32,7 +32,6 @@ ****************************************************************************/ #include "ActuatorEffectiveness.hpp" -#include "../ControlAllocation/ControlAllocation.hpp" #include @@ -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]++; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp b/src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp similarity index 100% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp rename to src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp diff --git a/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt b/src/lib/control_allocation/actuator_effectiveness/CMakeLists.txt similarity index 62% rename from src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt rename to src/lib/control_allocation/actuator_effectiveness/CMakeLists.txt index 2406b81bf8..b8a3d0076d 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt +++ b/src/lib/control_allocation/actuator_effectiveness/CMakeLists.txt @@ -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) diff --git a/src/modules/control_allocator/ControlAllocation/CMakeLists.txt b/src/lib/control_allocation/control_allocation/CMakeLists.txt similarity index 100% rename from src/modules/control_allocator/ControlAllocation/CMakeLists.txt rename to src/lib/control_allocation/control_allocation/CMakeLists.txt diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp b/src/lib/control_allocation/control_allocation/ControlAllocation.cpp similarity index 100% rename from src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp rename to src/lib/control_allocation/control_allocation/ControlAllocation.cpp diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp b/src/lib/control_allocation/control_allocation/ControlAllocation.hpp similarity index 99% rename from src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp rename to src/lib/control_allocation/control_allocation/ControlAllocation.hpp index c60784a03c..a6e2e2b254 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp +++ b/src/lib/control_allocation/control_allocation/ControlAllocation.hpp @@ -71,7 +71,7 @@ #include -#include "ActuatorEffectiveness/ActuatorEffectiveness.hpp" +#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp" class ControlAllocation { diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp b/src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.cpp similarity index 92% rename from src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp rename to src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.cpp index 5d86814d7e..0dc512b4ed 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp +++ b/src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.cpp @@ -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_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; } } diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.hpp b/src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.hpp similarity index 95% rename from src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.hpp rename to src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.hpp index 27c367376b..d4527cc1f4 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.hpp +++ b/src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.hpp @@ -57,11 +57,13 @@ public: void setEffectivenessMatrix(const matrix::Matrix &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 _mix; bool _mix_update_needed{false}; + bool _metric_allocation{false}; /** * Recalculate pseudo inverse if required. diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverseTest.cpp b/src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverseTest.cpp similarity index 76% rename from src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverseTest.cpp rename to src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverseTest.cpp index 89faab8c92..0eafbc6c26 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverseTest.cpp +++ b/src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverseTest.cpp @@ -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 control_sp; + matrix::Vector control_allocated; + matrix::Vector control_allocated_expected; + matrix::Matrix effectiveness; + matrix::Vector actuator_sp; + matrix::Vector actuator_trim; + matrix::Vector linearization_point; + matrix::Vector 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); +} diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp b/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturation.cpp similarity index 100% rename from src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp rename to src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturation.cpp diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.hpp b/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturation.hpp similarity index 100% rename from src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.hpp rename to src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturation.hpp diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturationTest.cpp b/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturationTest.cpp similarity index 92% rename from src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturationTest.cpp rename to src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturationTest.cpp index 60392330c9..cd478a1b00 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturationTest.cpp +++ b/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturationTest.cpp @@ -40,17 +40,27 @@ #include #include -#include <../ActuatorEffectiveness/ActuatorEffectivenessRotors.hpp> using namespace matrix; namespace { +struct RotorGeometryTest { + matrix::Vector3f position; + matrix::Vector3f axis; + float thrust_coef; + float moment_ratio; +}; + +struct GeometryTest { + RotorGeometryTest rotors[ActuatorEffectiveness::NUM_ACTUATORS]; + int num_rotors{0}; +}; // Makes and returns a Geometry object for a "standard" quad-x quadcopter. -ActuatorEffectivenessRotors::Geometry make_quad_x_geometry() +GeometryTest make_quad_x_geometry() { - ActuatorEffectivenessRotors::Geometry geometry = {}; + GeometryTest geometry = {}; geometry.rotors[0].position(0) = 1.0f; geometry.rotors[0].position(1) = 1.0f; geometry.rotors[0].position(2) = 0.0f; @@ -88,7 +98,6 @@ ActuatorEffectivenessRotors::Geometry make_quad_x_geometry() geometry.rotors[3].moment_ratio = -0.05f; geometry.num_rotors = 4; - return geometry; } @@ -98,7 +107,48 @@ ActuatorEffectiveness::EffectivenessMatrix make_quad_x_effectiveness() ActuatorEffectiveness::EffectivenessMatrix effectiveness; effectiveness.setZero(); const auto geometry = make_quad_x_geometry(); - ActuatorEffectivenessRotors::computeEffectivenessMatrix(geometry, effectiveness); + + // Minimalistically copied from ActuatorEffectivenessRotors::computeEffectivenessMatrix + for (int i = 0; i < geometry.num_rotors; i++) { + + // Get rotor axis + Vector3f axis = geometry.rotors[i].axis; + + // Normalize axis + float axis_norm = axis.norm(); + + if (axis_norm > FLT_EPSILON) { + axis /= axis_norm; + + } else { + // Bad axis definition, ignore this rotor + continue; + } + + // Get rotor position + const Vector3f &position = geometry.rotors[i].position; + + // Get coefficients + float ct = geometry.rotors[i].thrust_coef; + float km = geometry.rotors[i].moment_ratio; + + if (fabsf(ct) < FLT_EPSILON) { + continue; + } + + // Compute thrust generated by this rotor + matrix::Vector3f thrust = ct * axis; + + // Compute moment generated by this rotor + matrix::Vector3f moment = ct * position.cross(axis) - ct * km * axis; + + // Fill corresponding items in effectiveness matrix + for (int j = 0; j < 3; j++) { + effectiveness(j, i) = moment(j); + effectiveness(j + 3, i) = thrust(j); + } + } + return effectiveness; } diff --git a/src/lib/mathlib/math/filter/AlphaFilter.hpp b/src/lib/mathlib/math/filter/AlphaFilter.hpp index b7a6e86953..b80e2a73f5 100644 --- a/src/lib/mathlib/math/filter/AlphaFilter.hpp +++ b/src/lib/mathlib/math/filter/AlphaFilter.hpp @@ -89,6 +89,16 @@ public: return true; } + void setCutoffFreq(float cutoff_freq) + { + if (cutoff_freq > FLT_EPSILON) { + _time_constant = 1.f / (M_TWOPI_F * cutoff_freq); + + } else { + _time_constant = 0.f; + } + } + /** * Set filter parameter alpha directly without time abstraction * diff --git a/src/lib/systemlib/system_params.c b/src/lib/systemlib/system_params.c index 1ca273be84..d396039f3f 100644 --- a/src/lib/systemlib/system_params.c +++ b/src/lib/systemlib/system_params.c @@ -189,11 +189,10 @@ PARAM_DEFINE_INT32(SYS_CAL_TMAX, 10); PARAM_DEFINE_INT32(SYS_HAS_GPS, 1); /** - * Control if the vehicle has a magnetometer + * Control if and how many magnetometers are expected * - * Set this to 0 if the board has no magnetometer. - * If set to 0, the preflight checks will not check for the presence of a - * magnetometer, otherwise N sensors are required. + * 0: System has no magnetometer, preflight checks should pass without one. + * 1-N: Require the presence of N magnetometer sensors for check to pass. * * @reboot_required true * @group System diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 5fc8ee4d8f..fef03b92c2 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -2268,7 +2268,8 @@ void Commander::handleAutoDisarm() _auto_disarm_landed.set_state_and_update(_vehicle_land_detected.landed, hrt_absolute_time()); } else if (_param_com_disarm_prflt.get() > 0 && !_have_taken_off_since_arming) { - _auto_disarm_landed.set_hysteresis_time_from(false, _param_com_disarm_prflt.get() * 1_s); + _auto_disarm_landed.set_hysteresis_time_from(false, + (_param_com_spoolup_time.get() + _param_com_disarm_prflt.get()) * 1_s); _auto_disarm_landed.set_state_and_update(true, hrt_absolute_time()); } diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index cf69743af6..04c1a0d05a 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -330,7 +330,6 @@ private: param_t _param_rc_map_fltmode{PARAM_INVALID}; DEFINE_PARAMETERS( - (ParamFloat) _param_com_disarm_land, (ParamFloat) _param_com_disarm_prflt, (ParamBool) _param_com_disarm_man, @@ -347,6 +346,7 @@ private: (ParamFloat) _param_com_obc_loss_t, (ParamInt) _param_com_prearm_mode, (ParamInt) _param_com_rc_override, + (ParamFloat) _param_com_spoolup_time, (ParamInt) _param_com_flight_uuid, (ParamInt) _param_com_takeoff_act, (ParamFloat) _param_com_cpu_max diff --git a/src/modules/commander/HealthAndArmingChecks/Common.cpp b/src/modules/commander/HealthAndArmingChecks/Common.cpp index fb5639087e..16e074ba18 100644 --- a/src/modules/commander/HealthAndArmingChecks/Common.cpp +++ b/src/modules/commander/HealthAndArmingChecks/Common.cpp @@ -73,6 +73,14 @@ void Report::armingCheckFailure(NavModes required_modes, HealthComponentIndex co addEvent(event_id, log_levels, message, (uint32_t)reportedModes(required_modes), component.index); } +void Report::armingCheckFailure(NavModesMessageFail required_modes, HealthComponentIndex component, + uint32_t event_id, const events::LogLevels &log_levels, const char *message) +{ + armingCheckFailure(required_modes.fail_modes, component, log_levels.external); + addEvent(event_id, log_levels, message, + (uint32_t)reportedModes(required_modes.message_modes | required_modes.fail_modes), component.index); +} + Report::EventBufferHeader *Report::addEventToBuffer(uint32_t event_id, const events::LogLevels &log_levels, uint32_t modes, unsigned args_size) { diff --git a/src/modules/commander/HealthAndArmingChecks/Common.hpp b/src/modules/commander/HealthAndArmingChecks/Common.hpp index 826aa7629c..e069788ddf 100644 --- a/src/modules/commander/HealthAndArmingChecks/Common.hpp +++ b/src/modules/commander/HealthAndArmingChecks/Common.hpp @@ -74,6 +74,12 @@ static_assert(sizeof(navigation_mode_group_t) == sizeof(NavModes), "type mismatc static_assert(vehicle_status_s::NAVIGATION_STATE_MAX <= CHAR_BIT *sizeof(navigation_mode_group_t), "type too small, use next larger type"); +// Type to pass two mode groups in one struct to have the same number of function arguments to facilitate events parsing +struct NavModesMessageFail { + NavModes message_modes; ///< modes in which there's user messageing but arming is allowed + NavModes fail_modes; ///< modes in which checks fail which must be a subset of message_modes +}; + static inline NavModes operator|(NavModes a, NavModes b) { return static_cast(static_cast(a) | static_cast(b)); @@ -251,6 +257,14 @@ public: void armingCheckFailure(NavModes required_modes, HealthComponentIndex component, uint32_t event_id, const events::LogLevels &log_levels, const char *message); + /** + * Overloaded variant of armingCheckFailure() which allows to separately specify modes in which a message should be emitted and a subset in which arming is blocked + * @param required_modes .message_modes modes in which to put out the event and hence user message. + * .failing_modes modes in which to to fail arming. Has to be a subset of message_modes to never disallow arming without a reason. + */ + void armingCheckFailure(NavModesMessageFail required_modes, HealthComponentIndex component, + uint32_t event_id, const events::LogLevels &log_levels, const char *message); + void clearArmingBits(NavModes modes); /** diff --git a/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.cpp index d453c724d5..85a19cf550 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.cpp @@ -206,19 +206,62 @@ void BatteryChecks::checkAndReport(const Context &context, Report &reporter) || (configured_arm_threshold_in_use && below_configured_arm_threshold) ? NavModes::All : NavModes::None; events::LogLevel log_level = critical_or_higher || below_configured_arm_threshold ? events::Log::Critical : events::Log::Warning; - /* EVENT - * @description - * The battery state of charge of the worst battery is below the threshold. - * - * - * This check can be configured via BAT_LOW_THR, BAT_CRIT_THR, BAT_EMERGEN_THR and COM_ARM_BAT_MIN parameters. - * - */ - reporter.armingCheckFailure(affected_modes, health_component_t::battery, events::ID("check_battery_low"), log_level, - "Low battery"); - if (reporter.mavlink_log_pub()) { - mavlink_log_emergency(reporter.mavlink_log_pub(), "Low battery level\t"); + switch (reporter.failsafeFlags().battery_warning) { + default: + case battery_status_s::BATTERY_WARNING_LOW: + /* EVENT + * @description + * The lowest battery state of charge is below the low threshold. + * + * + * Can be configured with BAT_LOW_THR. + * + */ + reporter.armingCheckFailure(affected_modes, health_component_t::battery, events::ID("check_battery_low"), + log_level, "Low battery"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_emergency(reporter.mavlink_log_pub(), "Low battery\t"); + } + + break; + + case battery_status_s::BATTERY_WARNING_CRITICAL: + /* EVENT + * @description + * The lowest battery state of charge is below the critical threshold. + * + * + * Can be configured with BAT_CRIT_THR and from when to disalow arming with COM_ARM_BAT_MIN. + * + */ + reporter.armingCheckFailure(affected_modes, health_component_t::battery, events::ID("check_battery_critical"), + log_level, "Critical battery"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_emergency(reporter.mavlink_log_pub(), "Critical battery\t"); + } + + break; + + case battery_status_s::BATTERY_WARNING_EMERGENCY: + /* EVENT + * @description + * The lowest battery state of charge is below the emergency threshold. + * + * + * Can be configured with BAT_EMERGEN_THR. + * + */ + reporter.armingCheckFailure(affected_modes, health_component_t::battery, events::ID("check_battery_emergency"), + log_level, "Emergency battery level"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_emergency(reporter.mavlink_log_pub(), "Emergency battery level\t"); + } + + break; } } diff --git a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp index 1cd140fe5b..96b32c947c 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp @@ -273,7 +273,7 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor } if (!context.isArmed() && ekf_gps_check_fail) { - NavModes required_groups_gps; + NavModesMessageFail required_modes; events::Log log_level; switch (static_cast(_param_com_arm_wo_gps.get())) { @@ -281,17 +281,21 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor /* FALLTHROUGH */ case GnssArmingCheck::DenyArming: - required_groups_gps = required_groups; + required_modes.message_modes = required_modes.fail_modes = NavModes::All; log_level = events::Log::Error; break; case GnssArmingCheck::WarningOnly: - required_groups_gps = NavModes::None; // optional + required_modes.message_modes = (NavModes)(reporter.failsafeFlags().mode_req_local_position + | reporter.failsafeFlags().mode_req_local_position_relaxed + | reporter.failsafeFlags().mode_req_global_position); + // Only warn and don't block arming because there could still be a valid position estimate from another source e.g. optical flow, VIO + required_modes.fail_modes = NavModes::None; log_level = events::Log::Warning; break; case GnssArmingCheck::Disabled: - required_groups_gps = NavModes::None; + required_modes.message_modes = required_modes.fail_modes = NavModes::None; log_level = events::Log::Disabled; break; } @@ -304,10 +308,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor /* EVENT * @description * - * This check can be configured via EKF2_GPS_CHECK parameter. + * Can be configured with EKF2_GPS_CHECK and COM_ARM_WO_GPS. * */ - reporter.armingCheckFailure(required_groups_gps, health_component_t::gps, + reporter.armingCheckFailure(required_modes, health_component_t::gps, events::ID("check_estimator_gps_fix_too_low"), log_level, "GPS fix too low"); @@ -316,10 +320,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor /* EVENT * @description * - * This check can be configured via EKF2_GPS_CHECK parameter. + * Can be configured with EKF2_GPS_CHECK and COM_ARM_WO_GPS. * */ - reporter.armingCheckFailure(required_groups_gps, health_component_t::gps, + reporter.armingCheckFailure(required_modes, health_component_t::gps, events::ID("check_estimator_gps_num_sats_too_low"), log_level, "Not enough GPS Satellites"); @@ -328,10 +332,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor /* EVENT * @description * - * This check can be configured via EKF2_GPS_CHECK parameter. + * Can be configured with EKF2_GPS_CHECK and COM_ARM_WO_GPS. * */ - reporter.armingCheckFailure(required_groups_gps, health_component_t::gps, + reporter.armingCheckFailure(required_modes, health_component_t::gps, events::ID("check_estimator_gps_pdop_too_high"), log_level, "GPS PDOP too high"); @@ -340,10 +344,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor /* EVENT * @description * - * This check can be configured via EKF2_GPS_CHECK parameter. + * Can be configured with EKF2_GPS_CHECK and COM_ARM_WO_GPS. * */ - reporter.armingCheckFailure(required_groups_gps, health_component_t::gps, + reporter.armingCheckFailure(required_modes, health_component_t::gps, events::ID("check_estimator_gps_hor_pos_err_too_high"), log_level, "GPS Horizontal Position Error too high"); @@ -352,10 +356,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor /* EVENT * @description * - * This check can be configured via EKF2_GPS_CHECK parameter. + * Can be configured with EKF2_GPS_CHECK and COM_ARM_WO_GPS. * */ - reporter.armingCheckFailure(required_groups_gps, health_component_t::gps, + reporter.armingCheckFailure(required_modes, health_component_t::gps, events::ID("check_estimator_gps_vert_pos_err_too_high"), log_level, "GPS Vertical Position Error too high"); @@ -364,10 +368,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor /* EVENT * @description * - * This check can be configured via EKF2_GPS_CHECK parameter. + * Can be configured with EKF2_GPS_CHECK and COM_ARM_WO_GPS. * */ - reporter.armingCheckFailure(required_groups_gps, health_component_t::gps, + reporter.armingCheckFailure(required_modes, health_component_t::gps, events::ID("check_estimator_gps_speed_acc_too_low"), log_level, "GPS Speed Accuracy too low"); @@ -376,10 +380,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor /* EVENT * @description * - * This check can be configured via EKF2_GPS_CHECK parameter. + * Can be configured with EKF2_GPS_CHECK and COM_ARM_WO_GPS. * */ - reporter.armingCheckFailure(required_groups_gps, health_component_t::gps, + reporter.armingCheckFailure(required_modes, health_component_t::gps, events::ID("check_estimator_gps_hor_pos_drift_too_high"), log_level, "GPS Horizontal Position Drift too high"); @@ -388,10 +392,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor /* EVENT * @description * - * This check can be configured via EKF2_GPS_CHECK parameter. + * Can be configured with EKF2_GPS_CHECK and COM_ARM_WO_GPS. * */ - reporter.armingCheckFailure(required_groups_gps, health_component_t::gps, + reporter.armingCheckFailure(required_modes, health_component_t::gps, events::ID("check_estimator_gps_vert_pos_drift_too_high"), log_level, "GPS Vertical Position Drift too high"); @@ -400,10 +404,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor /* EVENT * @description * - * This check can be configured via EKF2_GPS_CHECK parameter. + * Can be configured with EKF2_GPS_CHECK and COM_ARM_WO_GPS. * */ - reporter.armingCheckFailure(required_groups_gps, health_component_t::gps, + reporter.armingCheckFailure(required_modes, health_component_t::gps, events::ID("check_estimator_gps_hor_speed_drift_too_high"), log_level, "GPS Horizontal Speed Drift too high"); @@ -412,10 +416,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor /* EVENT * @description * - * This check can be configured via EKF2_GPS_CHECK parameter. + * Can be configured with EKF2_GPS_CHECK and COM_ARM_WO_GPS. * */ - reporter.armingCheckFailure(required_groups_gps, health_component_t::gps, + reporter.armingCheckFailure(required_modes, health_component_t::gps, events::ID("check_estimator_gps_vert_speed_drift_too_high"), log_level, "GPS Vertical Speed Drift too high"); @@ -424,10 +428,10 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor /* EVENT * @description * - * This check can be configured via EKF2_GPS_CHECK parameter. + * Can be configured with EKF2_GPS_CHECK and COM_ARM_WO_GPS. * */ - reporter.armingCheckFailure(required_groups_gps, health_component_t::gps, + reporter.armingCheckFailure(required_modes, health_component_t::gps, events::ID("check_estimator_gps_spoofed"), log_level, "GPS signal spoofed"); @@ -437,7 +441,7 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor message = "Preflight%s: Estimator not using GPS"; /* EVENT */ - reporter.armingCheckFailure(required_groups_gps, health_component_t::gps, + reporter.armingCheckFailure(required_modes, health_component_t::gps, events::ID("check_estimator_gps_not_fusing"), log_level, "Estimator not using GPS"); @@ -446,7 +450,7 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor message = "Preflight%s: Poor GPS Quality"; /* EVENT */ - reporter.armingCheckFailure(required_groups_gps, health_component_t::gps, + reporter.armingCheckFailure(required_modes, health_component_t::gps, events::ID("check_estimator_gps_generic"), log_level, "Poor GPS Quality"); } diff --git a/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.hpp b/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.hpp index 7129e46203..bbba418000 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.hpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.hpp @@ -38,6 +38,7 @@ #include #include #include +#include static_assert((1ull << arming_check_reply_s::HEALTH_COMPONENT_INDEX_AVOIDANCE) == (uint64_t) health_component_t::avoidance, "enum definition missmatch"); @@ -66,7 +67,7 @@ public: void update(); bool isUnresponsive(int registration_id); - + bool allowUpdateWhileArmed() const { return _param_com_mode_arm_chk.get(); } private: static constexpr hrt_abstime REQUEST_TIMEOUT = 50_ms; static constexpr hrt_abstime UPDATE_INTERVAL = 300_ms; @@ -109,4 +110,7 @@ private: uORB::Subscription _arming_check_reply_sub{ORB_ID(arming_check_reply)}; uORB::Publication _arming_check_request_pub{ORB_ID(arming_check_request)}; + DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase, + (ParamBool) _param_com_mode_arm_chk + ); }; diff --git a/src/modules/commander/ModeManagement.cpp b/src/modules/commander/ModeManagement.cpp index 14ba7ba28f..4430a0f563 100644 --- a/src/modules/commander/ModeManagement.cpp +++ b/src/modules/commander/ModeManagement.cpp @@ -370,11 +370,7 @@ void ModeManagement::update(bool armed, uint8_t user_intended_nav_state, bool fa _failsafe_action_active = failsafe_action_active; _external_checks.update(); - bool allow_update_while_armed = false; -#if defined(CONFIG_ARCH_BOARD_PX4_SITL) - // For simulation, allow registering modes while armed for developer convenience - allow_update_while_armed = true; -#endif + bool allow_update_while_armed = _external_checks.allowUpdateWhileArmed(); if (armed && !allow_update_while_armed) { // Reject registration requests @@ -408,7 +404,8 @@ void ModeManagement::update(bool armed, uint8_t user_intended_nav_state, bool fa } } - // As we're disarmed we can use the user intended mode, as no failsafe will be active + // As we're disarmed we can use the user intended mode, as no failsafe will be active. + // Note that this might not be true if COM_MODE_ARM_CHK is set checkNewRegistrations(update_request); checkUnregistrations(user_intended_nav_state, update_request); } diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 48b0cce522..37dcbdc9a9 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -1018,3 +1018,14 @@ PARAM_DEFINE_FLOAT(COM_THROW_SPEED, 5); * @increment 1 */ PARAM_DEFINE_INT32(COM_FLTT_LOW_ACT, 3); + +/** + * Allow external mode registration while armed. + * + * By default disabled for safety reasons + * + * @group Commander + * @boolean + * + */ +PARAM_DEFINE_INT32(COM_MODE_ARM_CHK, 0); diff --git a/src/modules/control_allocator/CMakeLists.txt b/src/modules/control_allocator/CMakeLists.txt index 65872dc432..36d5c7e171 100644 --- a/src/modules/control_allocator/CMakeLists.txt +++ b/src/modules/control_allocator/CMakeLists.txt @@ -32,8 +32,7 @@ ############################################################################ include_directories(${CMAKE_CURRENT_SOURCE_DIR}) -add_subdirectory(ActuatorEffectiveness) -add_subdirectory(ControlAllocation) +add_subdirectory(VehicleActuatorEffectiveness) px4_add_module( MODULE modules__control_allocator @@ -50,6 +49,7 @@ px4_add_module( DEPENDS mathlib ActuatorEffectiveness + VehicleActuatorEffectiveness ControlAllocation px4_work_queue SlewRate diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index 48363eeb1e..0bc3f9c86e 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -314,7 +314,7 @@ ControlAllocator::Run() #endif // Check if parameters have changed - if (_parameter_update_sub.updated() && !_armed) { + if (_parameter_update_sub.updated()) { // clear update parameter_update_s param_update; _parameter_update_sub.copy(¶m_update); diff --git a/src/modules/control_allocator/Kconfig b/src/modules/control_allocator/Kconfig index 4352269d50..63c1c61500 100644 --- a/src/modules/control_allocator/Kconfig +++ b/src/modules/control_allocator/Kconfig @@ -10,3 +10,10 @@ menuconfig USER_CONTROL_ALLOCATOR depends on BOARD_PROTECTED && MODULES_CONTROL_ALLOCATOR ---help--- Put control_allocator in userspace memory + +menuconfig CONTROL_ALLOCATOR_RPM_CONTROL + bool "Include RPM control for Helicopter rotor" + default n + depends on MODULES_CONTROL_ALLOCATOR + ---help--- + Add support for controlling the helicopter main rotor rpm diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp similarity index 100% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp similarity index 97% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp index a0b8b06c16..7047363cc8 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.hpp @@ -33,7 +33,7 @@ #pragma once -#include "ActuatorEffectiveness.hpp" +#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp" #include #include diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.cpp similarity index 100% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.cpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.cpp diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.hpp similarity index 97% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.hpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.hpp index 0cc1390bd2..b7906669f2 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessCustom.hpp @@ -33,7 +33,7 @@ #pragma once -#include "ActuatorEffectiveness.hpp" +#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp" #include "ActuatorEffectivenessRotors.hpp" #include "ActuatorEffectivenessControlSurfaces.hpp" diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp similarity index 98% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp index c7c2bba7d7..0a4516ba56 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp @@ -32,7 +32,6 @@ ****************************************************************************/ #include "ActuatorEffectivenessFixedWing.hpp" -#include using namespace matrix; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp similarity index 97% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp index 5b4b7785b2..f0b095709e 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp @@ -33,7 +33,7 @@ #pragma once -#include "ActuatorEffectiveness.hpp" +#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp" #include "ActuatorEffectivenessRotors.hpp" #include "ActuatorEffectivenessControlSurfaces.hpp" diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp similarity index 95% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp index f403424b67..643b218c47 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp @@ -134,9 +134,16 @@ void ActuatorEffectivenessHelicopter::updateSetpoint(const matrix::Vector @@ -41,6 +41,8 @@ #include #include +#include "RpmControl.hpp" + class ActuatorEffectivenessHelicopter : public ModuleParams, public ActuatorEffectiveness { public: @@ -131,4 +133,8 @@ private: bool _main_motor_engaged{true}; const ActuatorType _tail_actuator_type; + +#if CONTROL_ALLOCATOR_RPM_CONTROL + RpmControl _rpm_control {this}; +#endif // CONTROL_ALLOCATOR_RPM_CONTROL }; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp similarity index 100% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp similarity index 98% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp index d5316bf498..a507aee2dd 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp @@ -33,7 +33,7 @@ #pragma once -#include "ActuatorEffectiveness.hpp" +#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp" #include diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterTest.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterTest.cpp similarity index 100% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterTest.cpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessHelicopterTest.cpp diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp similarity index 100% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp similarity index 97% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp index 848c8b8853..0b12482781 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp @@ -33,7 +33,7 @@ #pragma once -#include "ActuatorEffectiveness.hpp" +#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp" #include "ActuatorEffectivenessRotors.hpp" #include "ActuatorEffectivenessTilts.hpp" diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp similarity index 100% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMultirotor.cpp diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp similarity index 96% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp index e0a355edd2..e5b7f3dcf8 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp @@ -33,7 +33,7 @@ #pragma once -#include "ActuatorEffectiveness.hpp" +#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp" #include "ActuatorEffectivenessRotors.hpp" class ActuatorEffectivenessMultirotor : public ModuleParams, public ActuatorEffectiveness diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotors.cpp similarity index 100% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.cpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotors.cpp diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotors.hpp similarity index 98% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.hpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotors.hpp index 3844df4c84..c6f0425569 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotors.hpp @@ -41,7 +41,7 @@ #pragma once -#include "ActuatorEffectiveness.hpp" +#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp" #include #include diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotorsTest.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotorsTest.cpp similarity index 100% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotorsTest.cpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRotorsTest.cpp diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.cpp similarity index 98% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.cpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.cpp index 94f5db16f3..e9eda4c538 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.cpp @@ -32,7 +32,6 @@ ****************************************************************************/ #include "ActuatorEffectivenessRoverAckermann.hpp" -#include using namespace matrix; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.hpp similarity index 96% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.hpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.hpp index 294e453ec6..f6108b4baf 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.hpp @@ -33,7 +33,7 @@ #pragma once -#include "ActuatorEffectiveness.hpp" +#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp" class ActuatorEffectivenessRoverAckermann : public ActuatorEffectiveness { diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp similarity index 98% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp index 79853c7518..f15624dd67 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp @@ -32,7 +32,6 @@ ****************************************************************************/ #include "ActuatorEffectivenessStandardVTOL.hpp" -#include using namespace matrix; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp similarity index 97% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp index 0e5d1bfa44..9f945a6cd8 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp @@ -41,7 +41,7 @@ #pragma once -#include "ActuatorEffectiveness.hpp" +#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp" #include "ActuatorEffectivenessRotors.hpp" #include "ActuatorEffectivenessControlSurfaces.hpp" diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp similarity index 100% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp similarity index 97% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp index 604f05f9e0..708f104faa 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp @@ -39,7 +39,7 @@ #pragma once -#include "ActuatorEffectiveness.hpp" +#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp" #include "ActuatorEffectivenessRotors.hpp" #include "ActuatorEffectivenessControlSurfaces.hpp" diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp similarity index 100% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp similarity index 98% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp index 310d937064..512bd41f41 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp @@ -41,7 +41,7 @@ #pragma once -#include "ActuatorEffectiveness.hpp" +#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp" #include "ActuatorEffectivenessRotors.hpp" #include "ActuatorEffectivenessControlSurfaces.hpp" #include "ActuatorEffectivenessTilts.hpp" diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTilts.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTilts.cpp similarity index 100% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTilts.cpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTilts.cpp diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTilts.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTilts.hpp similarity index 97% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTilts.hpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTilts.hpp index d885a09155..1255d1b282 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTilts.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessTilts.hpp @@ -33,7 +33,7 @@ #pragma once -#include "ActuatorEffectiveness.hpp" +#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp" #include "ActuatorEffectivenessRotors.hpp" #include diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessUUV.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.cpp similarity index 100% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessUUV.cpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.cpp diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessUUV.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.hpp similarity index 97% rename from src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessUUV.hpp rename to src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.hpp index 57e86f9436..1b89da8d9b 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessUUV.hpp +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessUUV.hpp @@ -33,7 +33,7 @@ #pragma once -#include "ActuatorEffectiveness.hpp" +#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp" #include "ActuatorEffectivenessRotors.hpp" class ActuatorEffectivenessUUV : public ModuleParams, public ActuatorEffectiveness diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/CMakeLists.txt b/src/modules/control_allocator/VehicleActuatorEffectiveness/CMakeLists.txt new file mode 100644 index 0000000000..349893a3ea --- /dev/null +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/CMakeLists.txt @@ -0,0 +1,43 @@ +px4_add_library(VehicleActuatorEffectiveness + 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 + RpmControl.hpp + RpmControl.cpp +) + +target_compile_options(VehicleActuatorEffectiveness PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) +target_include_directories(VehicleActuatorEffectiveness PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) +target_link_libraries(VehicleActuatorEffectiveness + PRIVATE + mathlib + ActuatorEffectiveness +) + +px4_add_functional_gtest(SRC ActuatorEffectivenessHelicopterTest.cpp LINKLIBS VehicleActuatorEffectiveness) +px4_add_functional_gtest(SRC ActuatorEffectivenessRotorsTest.cpp LINKLIBS VehicleActuatorEffectiveness) diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/RpmControl.cpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/RpmControl.cpp new file mode 100644 index 0000000000..53d9766e2d --- /dev/null +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/RpmControl.cpp @@ -0,0 +1,85 @@ +/**************************************************************************** + * + * Copyright (c) 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 + * 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 "RpmControl.hpp" + +#include + +using namespace time_literals; + +RpmControl::RpmControl(ModuleParams *parent) : ModuleParams(parent) +{ + _pid.setOutputLimit(PID_OUTPUT_LIMIT); + _pid.setIntegralLimit(PID_OUTPUT_LIMIT); +}; + +void RpmControl::setSpoolupProgress(float spoolup_progress) +{ + _spoolup_progress = spoolup_progress; + _pid.setSetpoint(_spoolup_progress * _param_ca_heli_rpm_sp.get()); + + if (_spoolup_progress < SPOOLUP_PROGRESS_WITH_CONTROLLER_ENGAGED) { + _pid.resetIntegral(); + } +} + +float RpmControl::getActuatorCorrection() +{ + hrt_abstime now = hrt_absolute_time(); + + // RPM measurement update + if (_rpm_sub.updated()) { + rpm_s rpm{}; + + if (_rpm_sub.copy(&rpm)) { + const float dt = math::min((now - _timestamp_last_measurement) * 1e-6f, 1.f); + _timestamp_last_measurement = rpm.timestamp; + + const float gain_scale = math::interpolate(_spoolup_progress, .8f, 1.f, 0.f, 1e-3f); + _pid.setGains(_param_ca_heli_rpm_p.get() * gain_scale, _param_ca_heli_rpm_i.get() * gain_scale, 0.f); + _actuator_correction = _pid.update(rpm.rpm_estimate, dt, true); + + _rpm_invalid = rpm.rpm_estimate < 1.f; + } + } + + // Timeout + const bool timeout = now > _timestamp_last_measurement + 1_s; + + if (_rpm_invalid || timeout) { + _pid.resetIntegral(); + _actuator_correction = 0.f; + } + + return _actuator_correction; +} diff --git a/src/modules/control_allocator/VehicleActuatorEffectiveness/RpmControl.hpp b/src/modules/control_allocator/VehicleActuatorEffectiveness/RpmControl.hpp new file mode 100644 index 0000000000..5fd0c96d91 --- /dev/null +++ b/src/modules/control_allocator/VehicleActuatorEffectiveness/RpmControl.hpp @@ -0,0 +1,77 @@ +/**************************************************************************** + * + * Copyright (c) 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 + * 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 RpmControl.hpp + * + * Control rpm of a helicopter rotor. + * Input: PWM input pulse period from an rpm sensor + * Output: Duty cycle command for the ESC + * + * @author Matthias Grob + */ + +#pragma once + +#include +#include +#include +#include +#include + +class RpmControl : public ModuleParams +{ +public: + RpmControl(ModuleParams *parent); + ~RpmControl() = default; + + void setSpoolupProgress(float spoolup_progress); + float getActuatorCorrection(); + +private: + static constexpr float SPOOLUP_PROGRESS_WITH_CONTROLLER_ENGAGED = .8f; // [0,1] + static constexpr float PID_OUTPUT_LIMIT = .5f; // [0,1] + + uORB::Subscription _rpm_sub{ORB_ID(rpm)}; + bool _rpm_invalid{true}; + PID _pid; + float _spoolup_progress{0.f}; // [0,1] + hrt_abstime _timestamp_last_measurement{0}; // for dt and timeout + float _actuator_correction{0.f}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_ca_heli_rpm_sp, + (ParamFloat) _param_ca_heli_rpm_p, + (ParamFloat) _param_ca_heli_rpm_i + ) +}; diff --git a/src/modules/control_allocator/module.yaml b/src/modules/control_allocator/module.yaml index d085704df4..32f2862c9d 100644 --- a/src/modules/control_allocator/module.yaml +++ b/src/modules/control_allocator/module.yaml @@ -71,15 +71,14 @@ parameters: description: short: Motor ${i} slew rate limit long: | - Minimum time allowed for the motor input signal to pass through - the full output range. A value x means that the motor signal - can only go from 0 to 1 in minimum x seconds (in case of - reversible motors, the range is -1 to 1). + Forces the motor output signal to take at least the configured time (in seconds) + to traverse its full range (normally [0, 1], or if reversible [-1, 1]). Zero means that slew rate limiting is disabled. type: float decimal: 2 increment: 0.01 + unit: s num_instances: *max_num_mc_motors min: 0 max: 10 @@ -90,14 +89,14 @@ parameters: description: short: Servo ${i} slew rate limit long: | - Minimum time allowed for the servo input signal to pass through - the full output range. A value x means that the servo signal - can only go from -1 to 1 in minimum x seconds. + Forces the servo output signal to take at least the configured time (in seconds) + to traverse its full range [-100%, 100%]. Zero means that slew rate limiting is disabled. type: float decimal: 2 increment: 0.05 + unit: s num_instances: *max_num_servos min: 0 max: 10 @@ -528,6 +527,41 @@ parameters: which is mostly the case when the main rotor turns counter-clockwise. type: boolean default: 0 + CA_HELI_RPM_SP: + description: + short: Setpoint for main rotor rpm + long: | + Requires rpm feedback for the controller. + type: float + decimal: 0 + increment: 1 + min: 100 + max: 10000 + default: 1500 + CA_HELI_RPM_P: + description: + short: Proportional gain for rpm control + long: | + Ratio between rpm error devided by 1000 to how much normalized output gets added to correct for it. + + motor_command = throttle_curve + CA_HELI_RPM_P * (rpm_setpoint - rpm_measurement) / 1000 + type: float + decimal: 3 + increment: 0.1 + min: 0 + max: 10 + default: 0.0 + CA_HELI_RPM_I: + description: + short: Integral gain for rpm control + long: | + Same definition as the proportional gain but for integral. + type: float + decimal: 3 + increment: 0.1 + min: 0 + max: 10 + default: 0.0 # Others CA_FAILURE_MODE: diff --git a/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp index 119c6fa1b3..2d20c64c81 100644 --- a/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp @@ -65,6 +65,7 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample) if ((_baro_counter == 0) || baro_sample.reset) { _baro_lpf.reset(measurement); _baro_counter = 1; + _control_status.flags.baro_fault = false; } else { _baro_lpf.update(measurement); @@ -113,7 +114,7 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample) const bool continuing_conditions_passing = (_params.baro_ctrl == 1) && measurement_valid && (_baro_counter > _obs_buffer_length) - && !_baro_hgt_faulty; + && !_control_status.flags.baro_fault; const bool starting_conditions_passing = continuing_conditions_passing && isNewestSampleRecent(_time_last_baro_buffer_push, 2 * BARO_MAX_INTERVAL); @@ -148,7 +149,7 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample) if (isRecent(_time_last_hgt_fuse, _params.hgt_fusion_timeout_max)) { // Some other height source is still working - _baro_hgt_faulty = true; + _control_status.flags.baro_fault = true; } } diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp index e21ee82ff9..81b3fcc76b 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp @@ -102,11 +102,13 @@ bool Ekf::runGnssChecks(const gnssSample &gps) // Calculate the horizontal and vertical drift velocity components and limit to 10x the threshold const Vector3f vel_limit(_params.req_hdrift, _params.req_hdrift, _params.req_vdrift); - Vector3f pos_derived(delta_pos_n, delta_pos_e, (_gps_alt_prev - gps.alt)); - pos_derived = matrix::constrain(pos_derived / dt, -10.0f * vel_limit, 10.0f * vel_limit); + Vector3f delta_pos(delta_pos_n, delta_pos_e, (_gps_alt_prev - gps.alt)); // Apply a low pass filter - _gps_pos_deriv_filt = pos_derived * filter_coef + _gps_pos_deriv_filt * (1.0f - filter_coef); + _gps_pos_deriv_filt = delta_pos / dt * filter_coef + _gps_pos_deriv_filt * (1.0f - filter_coef); + + // Apply anti-windup to the state instead of the input to avoid generating a bias on asymmetric signals + _gps_pos_deriv_filt = matrix::constrain(_gps_pos_deriv_filt, -10.0f * vel_limit, 10.0f * vel_limit); // hdrift: calculate the horizontal drift speed and fail if too high _gps_horizontal_position_drift_rate_m_s = Vector2f(_gps_pos_deriv_filt.xy()).norm(); diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp index 889af97a08..6ce0cebc66 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp @@ -393,7 +393,11 @@ bool Ekf::isYawEmergencyEstimateAvailable() const return false; } - return _yawEstimator.getYawVar() < sq(_params.EKFGSF_yaw_err_max); + const float yaw_var = _yawEstimator.getYawVar(); + + return (yaw_var > 0.f) + && (yaw_var < sq(_params.EKFGSF_yaw_err_max)) + && PX4_ISFINITE(yaw_var); } bool Ekf::isYawFailure() const diff --git a/src/modules/ekf2/EKF/aid_sources/sideslip/sideslip_fusion.cpp b/src/modules/ekf2/EKF/aid_sources/sideslip/sideslip_fusion.cpp index a75c1bb54e..d529e5e994 100644 --- a/src/modules/ekf2/EKF/aid_sources/sideslip/sideslip_fusion.cpp +++ b/src/modules/ekf2/EKF/aid_sources/sideslip/sideslip_fusion.cpp @@ -43,14 +43,14 @@ #include "ekf.h" #include -#include +#include #include void Ekf::controlBetaFusion(const imuSample &imu_delayed) { _control_status.flags.fuse_beta = _params.beta_fusion_enabled - && _control_status.flags.fixed_wing + && (_control_status.flags.fixed_wing || _control_status.flags.fuse_aspd) && _control_status.flags.in_air && !_control_status.flags.fake_pos; @@ -127,10 +127,9 @@ bool Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip) _fault_status.flags.bad_sideslip = false; const float epsilon = 1e-3f; - VectorState H; // Observation jacobian - VectorState K; // Kalman gain vector - sym::ComputeSideslipHAndK(_state.vector(), P, sideslip.innovation_variance, epsilon, &H, &K); + const VectorState H = sym::ComputeSideslipH(_state.vector(), epsilon); + VectorState K = P * H / sideslip.innovation_variance; if (update_wind_only) { const Vector2f K_wind = K.slice(State::wind_vel.idx, 0); @@ -143,7 +142,5 @@ bool Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip) sideslip.fused = true; sideslip.time_last_fuse = _time_delayed_us; - _fault_status.flags.bad_sideslip = false; - return true; } diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 85d075e0f5..95efc971bc 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -620,6 +620,7 @@ uint64_t mag_heading_consistent : uint64_t opt_flow_terrain : 1; ///< 40 - true if we are fusing flow data for terrain uint64_t valid_fake_pos : 1; ///< 41 - true if a valid constant position is being fused uint64_t constant_pos : 1; ///< 42 - true if the vehicle is at a constant position + uint64_t baro_fault : 1; ///< 43 - true when the baro has been declared faulty and is no longer being used } flags; uint64_t value; diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp index 0c9a5df6cb..e7b2eea396 100644 --- a/src/modules/ekf2/EKF/covariance.cpp +++ b/src/modules/ekf2/EKF/covariance.cpp @@ -280,7 +280,17 @@ void Ekf::constrainStateVariances() void Ekf::constrainStateVar(const IdxDof &state, float min, float max) { for (unsigned i = state.idx; i < (state.idx + state.dof); i++) { - P(i, i) = math::constrain(P(i, i), min, max); + if (P(i, i) < min) { + P(i, i) = min; + + } else if (P(i, i) > max) { + // Constrain the variance growth by fusing zero innovation as clipping the variance + // would artifically increase the correlation between states and destabilize the filter. + const float innov = 0.f; + const float R = 10.f * P(i, i); // This reduces the variance by ~10% as K = P / (P + R) + const float innov_var = P(i, i) + R; + fuseDirectStateMeasurement(innov, innov_var, R, i); + } } } @@ -298,9 +308,7 @@ void Ekf::constrainStateVarLimitRatio(const IdxDof &state, float min, float max, float limited_max = math::constrain(state_var_max, min, max); float limited_min = math::constrain(limited_max / max_ratio, min, max); - for (unsigned i = state.idx; i < (state.idx + state.dof); i++) { - P(i, i) = math::constrain(P(i, i), limited_min, limited_max); - } + constrainStateVar(state, limited_min, limited_max); } void Ekf::resetQuatCov(const float yaw_noise) diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 8df41e6ba8..47320e8c12 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -208,8 +208,9 @@ public: // vxy_max : Maximum ground relative horizontal speed (meters/sec). NaN when limiting is not needed. // vz_max : Maximum ground relative vertical speed (meters/sec). NaN when limiting is not needed. // hagl_min : Minimum height above ground (meters). NaN when limiting is not needed. - // hagl_max : Maximum height above ground (meters). NaN when limiting is not needed. - void get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max) const; + // hagl_max_z : Maximum height above ground for vertical altitude control (meters). NaN when limiting is not needed. + // hagl_max_xy : Maximum height above ground for horizontal position control (meters). NaN when limiting is not needed. + void get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max_z, float *hagl_max_xy) const; void resetGyroBias(); void resetGyroBiasCov(); @@ -601,7 +602,6 @@ private: HeightBiasEstimator _baro_b_est{HeightSensor::BARO, _height_sensor_ref}; - bool _baro_hgt_faulty{false}; ///< true if baro data have been declared faulty TODO: move to fault flags #endif // CONFIG_EKF2_BAROMETER #if defined(CONFIG_EKF2_MAGNETOMETER) diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 664613a2b6..0d8fac2fb5 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -327,13 +327,15 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const *ekf_evv = sqrtf(P(State::vel.idx + 2, State::vel.idx + 2)); } -void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max) const +void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max_z, + float *hagl_max_xy) const { // Do not require limiting by default *vxy_max = NAN; *vz_max = NAN; *hagl_min = NAN; - *hagl_max = NAN; + *hagl_max_z = NAN; + *hagl_max_xy = NAN; #if defined(CONFIG_EKF2_RANGE_FINDER) // Calculate range finder limits @@ -347,7 +349,7 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl if (relying_on_rangefinder) { *hagl_min = rangefinder_hagl_min; - *hagl_max = rangefinder_hagl_max; + *hagl_max_z = rangefinder_hagl_max; } # if defined(CONFIG_EKF2_OPTICAL_FLOW) @@ -370,11 +372,12 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl const float flow_constrained_height = math::constrain(getHagl(), flow_hagl_min, flow_hagl_max); // Allow ground relative velocity to use 50% of available flow sensor range to allow for angular motion - const float flow_vxy_max = 0.5f * _flow_max_rate * flow_constrained_height; + float flow_vxy_max = 0.5f * _flow_max_rate * flow_constrained_height; + flow_hagl_max = math::max(flow_hagl_max * 0.9f, flow_hagl_max - 1.0f); *vxy_max = flow_vxy_max; *hagl_min = flow_hagl_min; - *hagl_max = flow_hagl_max; + *hagl_max_xy = flow_hagl_max; } # endif // CONFIG_EKF2_OPTICAL_FLOW diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py index 0ad3fef4ed..adbc761ae9 100755 --- a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py +++ b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py @@ -354,10 +354,8 @@ def compute_sideslip_innov_and_innov_var( return (innov, innov_var) -def compute_sideslip_h_and_k( +def compute_sideslip_h( state: VState, - P: MTangent, - innov_var: sf.Scalar, epsilon: sf.Scalar ) -> (VTangent, VTangent): @@ -366,9 +364,7 @@ def compute_sideslip_h_and_k( H = jacobian_chain_rule(sideslip_pred, state) - K = P * H.T / sf.Max(innov_var, epsilon) - - return (H.T, K) + return (H.T) def predict_vel_body( state: VState @@ -739,7 +735,7 @@ if not args.disable_wind: generate_px4_function(compute_airspeed_innov_and_innov_var, output_names=["innov", "innov_var"]) generate_px4_function(compute_drag_x_innov_var_and_h, output_names=["innov_var", "Hx"]) generate_px4_function(compute_drag_y_innov_var_and_h, output_names=["innov_var", "Hy"]) - generate_px4_function(compute_sideslip_h_and_k, output_names=["H", "K"]) + generate_px4_function(compute_sideslip_h, output_names=None) generate_px4_function(compute_sideslip_innov_and_innov_var, output_names=["innov", "innov_var"]) generate_px4_function(compute_wind_init_and_cov_from_airspeed, output_names=["wind", "P_wind"]) generate_px4_function(compute_wind_init_and_cov_from_wind_speed_and_direction, output_names=["wind", "P_wind"]) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_sideslip_h.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_sideslip_h.h new file mode 100644 index 0000000000..436b0c07e7 --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_sideslip_h.h @@ -0,0 +1,96 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +namespace sym { + +/** + * This function was autogenerated from a symbolic function. Do not modify by hand. + * + * Symbolic function: compute_sideslip_h + * + * Args: + * state: Matrix25_1 + * epsilon: Scalar + * + * Outputs: + * res: Matrix24_1 + */ +template +matrix::Matrix ComputeSideslipH(const matrix::Matrix& state, + const Scalar epsilon) { + // Total ops: 131 + + // Input arrays + + // Intermediate terms (37) + const Scalar _tmp0 = -state(22, 0) + state(4, 0); + const Scalar _tmp1 = 2 * state(1, 0); + const Scalar _tmp2 = 2 * state(6, 0); + const Scalar _tmp3 = _tmp2 * state(3, 0); + const Scalar _tmp4 = 1 - 2 * std::pow(state(3, 0), Scalar(2)); + const Scalar _tmp5 = _tmp4 - 2 * std::pow(state(2, 0), Scalar(2)); + const Scalar _tmp6 = 2 * state(0, 0); + const Scalar _tmp7 = _tmp6 * state(3, 0); + const Scalar _tmp8 = 2 * state(2, 0); + const Scalar _tmp9 = _tmp8 * state(1, 0); + const Scalar _tmp10 = _tmp7 + _tmp9; + const Scalar _tmp11 = -state(23, 0) + state(5, 0); + const Scalar _tmp12 = _tmp1 * state(3, 0) - _tmp8 * state(0, 0); + const Scalar _tmp13 = _tmp0 * _tmp5 + _tmp10 * _tmp11 + _tmp12 * state(6, 0); + const Scalar _tmp14 = _tmp13 + epsilon * ((((_tmp13) > 0) - ((_tmp13) < 0)) + Scalar(0.5)); + const Scalar _tmp15 = Scalar(1.0) / (_tmp14); + const Scalar _tmp16 = _tmp2 * state(0, 0); + const Scalar _tmp17 = std::pow(_tmp14, Scalar(2)); + const Scalar _tmp18 = _tmp4 - 2 * std::pow(state(1, 0), Scalar(2)); + const Scalar _tmp19 = -_tmp7 + _tmp9; + const Scalar _tmp20 = _tmp6 * state(1, 0) + _tmp8 * state(3, 0); + const Scalar _tmp21 = _tmp0 * _tmp19 + _tmp11 * _tmp18 + _tmp20 * state(6, 0); + const Scalar _tmp22 = _tmp21 / _tmp17; + const Scalar _tmp23 = _tmp17 / (_tmp17 + std::pow(_tmp21, Scalar(2))); + const Scalar _tmp24 = (Scalar(1) / Scalar(2)) * _tmp23; + const Scalar _tmp25 = _tmp24 * (_tmp15 * (_tmp0 * _tmp1 + _tmp3) - + _tmp22 * (-4 * _tmp0 * state(2, 0) + _tmp1 * _tmp11 - _tmp16)); + const Scalar _tmp26 = 2 * state(3, 0); + const Scalar _tmp27 = _tmp2 * state(1, 0); + const Scalar _tmp28 = _tmp2 * state(2, 0); + const Scalar _tmp29 = + _tmp24 * (_tmp15 * (-_tmp0 * _tmp26 + _tmp27) - _tmp22 * (_tmp11 * _tmp26 - _tmp28)); + const Scalar _tmp30 = _tmp24 * (_tmp15 * (_tmp0 * _tmp8 - 4 * _tmp11 * state(1, 0) + _tmp16) - + _tmp22 * (_tmp11 * _tmp8 + _tmp3)); + const Scalar _tmp31 = 4 * state(3, 0); + const Scalar _tmp32 = _tmp24 * (_tmp15 * (-_tmp0 * _tmp6 - _tmp11 * _tmp31 + _tmp28) - + _tmp22 * (-_tmp0 * _tmp31 + _tmp11 * _tmp6 + _tmp27)); + const Scalar _tmp33 = _tmp22 * _tmp5; + const Scalar _tmp34 = _tmp15 * _tmp19; + const Scalar _tmp35 = _tmp15 * _tmp18; + const Scalar _tmp36 = _tmp10 * _tmp22; + + // Output terms (1) + matrix::Matrix _res; + + _res.setZero(); + + _res(0, 0) = + -_tmp25 * state(3, 0) - _tmp29 * state(1, 0) + _tmp30 * state(0, 0) + _tmp32 * state(2, 0); + _res(1, 0) = + _tmp25 * state(0, 0) - _tmp29 * state(2, 0) + _tmp30 * state(3, 0) - _tmp32 * state(1, 0); + _res(2, 0) = + _tmp25 * state(1, 0) - _tmp29 * state(3, 0) - _tmp30 * state(2, 0) + _tmp32 * state(0, 0); + _res(3, 0) = _tmp23 * (-_tmp33 + _tmp34); + _res(4, 0) = _tmp23 * (_tmp35 - _tmp36); + _res(5, 0) = _tmp23 * (-_tmp12 * _tmp22 + _tmp15 * _tmp20); + _res(21, 0) = _tmp23 * (_tmp33 - _tmp34); + _res(22, 0) = _tmp23 * (-_tmp35 + _tmp36); + + return _res; +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_sideslip_h_and_k.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_sideslip_h_and_k.h deleted file mode 100644 index 6181936f8f..0000000000 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_sideslip_h_and_k.h +++ /dev/null @@ -1,188 +0,0 @@ -// ----------------------------------------------------------------------------- -// This file was autogenerated by symforce from template: -// function/FUNCTION.h.jinja -// Do NOT modify by hand. -// ----------------------------------------------------------------------------- - -#pragma once - -#include - -namespace sym { - -/** - * This function was autogenerated from a symbolic function. Do not modify by hand. - * - * Symbolic function: compute_sideslip_h_and_k - * - * Args: - * state: Matrix25_1 - * P: Matrix24_24 - * innov_var: Scalar - * epsilon: Scalar - * - * Outputs: - * H: Matrix24_1 - * K: Matrix24_1 - */ -template -void ComputeSideslipHAndK(const matrix::Matrix& state, - const matrix::Matrix& P, const Scalar innov_var, - const Scalar epsilon, matrix::Matrix* const H = nullptr, - matrix::Matrix* const K = nullptr) { - // Total ops: 518 - - // Input arrays - - // Intermediate terms (50) - const Scalar _tmp0 = -state(22, 0) + state(4, 0); - const Scalar _tmp1 = 2 * _tmp0; - const Scalar _tmp2 = 2 * state(3, 0); - const Scalar _tmp3 = _tmp2 * state(6, 0); - const Scalar _tmp4 = 1 - 2 * std::pow(state(3, 0), Scalar(2)); - const Scalar _tmp5 = _tmp4 - 2 * std::pow(state(2, 0), Scalar(2)); - const Scalar _tmp6 = _tmp2 * state(0, 0); - const Scalar _tmp7 = 2 * state(1, 0); - const Scalar _tmp8 = _tmp7 * state(2, 0); - const Scalar _tmp9 = _tmp6 + _tmp8; - const Scalar _tmp10 = -state(23, 0) + state(5, 0); - const Scalar _tmp11 = 2 * state(2, 0); - const Scalar _tmp12 = -_tmp11 * state(0, 0) + _tmp2 * state(1, 0); - const Scalar _tmp13 = _tmp0 * _tmp5 + _tmp10 * _tmp9 + _tmp12 * state(6, 0); - const Scalar _tmp14 = _tmp13 + epsilon * ((((_tmp13) > 0) - ((_tmp13) < 0)) + Scalar(0.5)); - const Scalar _tmp15 = Scalar(1.0) / (_tmp14); - const Scalar _tmp16 = 2 * _tmp10; - const Scalar _tmp17 = 2 * state(0, 0) * state(6, 0); - const Scalar _tmp18 = std::pow(_tmp14, Scalar(2)); - const Scalar _tmp19 = _tmp4 - 2 * std::pow(state(1, 0), Scalar(2)); - const Scalar _tmp20 = -_tmp6 + _tmp8; - const Scalar _tmp21 = _tmp2 * state(2, 0) + _tmp7 * state(0, 0); - const Scalar _tmp22 = _tmp0 * _tmp20 + _tmp10 * _tmp19 + _tmp21 * state(6, 0); - const Scalar _tmp23 = _tmp22 / _tmp18; - const Scalar _tmp24 = _tmp15 * (_tmp1 * state(1, 0) + _tmp3) - - _tmp23 * (-4 * _tmp0 * state(2, 0) + _tmp16 * state(1, 0) - _tmp17); - const Scalar _tmp25 = _tmp18 / (_tmp18 + std::pow(_tmp22, Scalar(2))); - const Scalar _tmp26 = (Scalar(1) / Scalar(2)) * _tmp25; - const Scalar _tmp27 = _tmp26 * state(3, 0); - const Scalar _tmp28 = _tmp7 * state(6, 0); - const Scalar _tmp29 = _tmp11 * state(6, 0); - const Scalar _tmp30 = - _tmp15 * (-_tmp1 * state(3, 0) + _tmp28) - _tmp23 * (_tmp16 * state(3, 0) - _tmp29); - const Scalar _tmp31 = _tmp26 * state(1, 0); - const Scalar _tmp32 = _tmp15 * (_tmp1 * state(2, 0) - 4 * _tmp10 * state(1, 0) + _tmp17) - - _tmp23 * (_tmp16 * state(2, 0) + _tmp3); - const Scalar _tmp33 = _tmp26 * state(0, 0); - const Scalar _tmp34 = 4 * state(3, 0); - const Scalar _tmp35 = _tmp15 * (-_tmp1 * state(0, 0) - _tmp10 * _tmp34 + _tmp29) - - _tmp23 * (-_tmp0 * _tmp34 + _tmp16 * state(0, 0) + _tmp28); - const Scalar _tmp36 = _tmp26 * state(2, 0); - const Scalar _tmp37 = -_tmp24 * _tmp27 - _tmp30 * _tmp31 + _tmp32 * _tmp33 + _tmp35 * _tmp36; - const Scalar _tmp38 = _tmp24 * _tmp33 + _tmp27 * _tmp32 - _tmp30 * _tmp36 - _tmp31 * _tmp35; - const Scalar _tmp39 = _tmp24 * _tmp31 - _tmp27 * _tmp30 - _tmp32 * _tmp36 + _tmp33 * _tmp35; - const Scalar _tmp40 = _tmp23 * _tmp5; - const Scalar _tmp41 = _tmp15 * _tmp20; - const Scalar _tmp42 = _tmp25 * (-_tmp40 + _tmp41); - const Scalar _tmp43 = _tmp15 * _tmp19; - const Scalar _tmp44 = _tmp23 * _tmp9; - const Scalar _tmp45 = _tmp25 * (_tmp43 - _tmp44); - const Scalar _tmp46 = _tmp25 * (-_tmp12 * _tmp23 + _tmp15 * _tmp21); - const Scalar _tmp47 = _tmp25 * (_tmp40 - _tmp41); - const Scalar _tmp48 = _tmp25 * (-_tmp43 + _tmp44); - const Scalar _tmp49 = Scalar(1.0) / (math::max(epsilon, innov_var)); - - // Output terms (2) - if (H != nullptr) { - matrix::Matrix& _h = (*H); - - _h.setZero(); - - _h(0, 0) = _tmp37; - _h(1, 0) = _tmp38; - _h(2, 0) = _tmp39; - _h(3, 0) = _tmp42; - _h(4, 0) = _tmp45; - _h(5, 0) = _tmp46; - _h(21, 0) = _tmp47; - _h(22, 0) = _tmp48; - } - - if (K != nullptr) { - matrix::Matrix& _k = (*K); - - _k(0, 0) = - _tmp49 * (P(0, 0) * _tmp37 + P(0, 1) * _tmp38 + P(0, 2) * _tmp39 + P(0, 21) * _tmp47 + - P(0, 22) * _tmp48 + P(0, 3) * _tmp42 + P(0, 4) * _tmp45 + P(0, 5) * _tmp46); - _k(1, 0) = - _tmp49 * (P(1, 0) * _tmp37 + P(1, 1) * _tmp38 + P(1, 2) * _tmp39 + P(1, 21) * _tmp47 + - P(1, 22) * _tmp48 + P(1, 3) * _tmp42 + P(1, 4) * _tmp45 + P(1, 5) * _tmp46); - _k(2, 0) = - _tmp49 * (P(2, 0) * _tmp37 + P(2, 1) * _tmp38 + P(2, 2) * _tmp39 + P(2, 21) * _tmp47 + - P(2, 22) * _tmp48 + P(2, 3) * _tmp42 + P(2, 4) * _tmp45 + P(2, 5) * _tmp46); - _k(3, 0) = - _tmp49 * (P(3, 0) * _tmp37 + P(3, 1) * _tmp38 + P(3, 2) * _tmp39 + P(3, 21) * _tmp47 + - P(3, 22) * _tmp48 + P(3, 3) * _tmp42 + P(3, 4) * _tmp45 + P(3, 5) * _tmp46); - _k(4, 0) = - _tmp49 * (P(4, 0) * _tmp37 + P(4, 1) * _tmp38 + P(4, 2) * _tmp39 + P(4, 21) * _tmp47 + - P(4, 22) * _tmp48 + P(4, 3) * _tmp42 + P(4, 4) * _tmp45 + P(4, 5) * _tmp46); - _k(5, 0) = - _tmp49 * (P(5, 0) * _tmp37 + P(5, 1) * _tmp38 + P(5, 2) * _tmp39 + P(5, 21) * _tmp47 + - P(5, 22) * _tmp48 + P(5, 3) * _tmp42 + P(5, 4) * _tmp45 + P(5, 5) * _tmp46); - _k(6, 0) = - _tmp49 * (P(6, 0) * _tmp37 + P(6, 1) * _tmp38 + P(6, 2) * _tmp39 + P(6, 21) * _tmp47 + - P(6, 22) * _tmp48 + P(6, 3) * _tmp42 + P(6, 4) * _tmp45 + P(6, 5) * _tmp46); - _k(7, 0) = - _tmp49 * (P(7, 0) * _tmp37 + P(7, 1) * _tmp38 + P(7, 2) * _tmp39 + P(7, 21) * _tmp47 + - P(7, 22) * _tmp48 + P(7, 3) * _tmp42 + P(7, 4) * _tmp45 + P(7, 5) * _tmp46); - _k(8, 0) = - _tmp49 * (P(8, 0) * _tmp37 + P(8, 1) * _tmp38 + P(8, 2) * _tmp39 + P(8, 21) * _tmp47 + - P(8, 22) * _tmp48 + P(8, 3) * _tmp42 + P(8, 4) * _tmp45 + P(8, 5) * _tmp46); - _k(9, 0) = - _tmp49 * (P(9, 0) * _tmp37 + P(9, 1) * _tmp38 + P(9, 2) * _tmp39 + P(9, 21) * _tmp47 + - P(9, 22) * _tmp48 + P(9, 3) * _tmp42 + P(9, 4) * _tmp45 + P(9, 5) * _tmp46); - _k(10, 0) = - _tmp49 * (P(10, 0) * _tmp37 + P(10, 1) * _tmp38 + P(10, 2) * _tmp39 + P(10, 21) * _tmp47 + - P(10, 22) * _tmp48 + P(10, 3) * _tmp42 + P(10, 4) * _tmp45 + P(10, 5) * _tmp46); - _k(11, 0) = - _tmp49 * (P(11, 0) * _tmp37 + P(11, 1) * _tmp38 + P(11, 2) * _tmp39 + P(11, 21) * _tmp47 + - P(11, 22) * _tmp48 + P(11, 3) * _tmp42 + P(11, 4) * _tmp45 + P(11, 5) * _tmp46); - _k(12, 0) = - _tmp49 * (P(12, 0) * _tmp37 + P(12, 1) * _tmp38 + P(12, 2) * _tmp39 + P(12, 21) * _tmp47 + - P(12, 22) * _tmp48 + P(12, 3) * _tmp42 + P(12, 4) * _tmp45 + P(12, 5) * _tmp46); - _k(13, 0) = - _tmp49 * (P(13, 0) * _tmp37 + P(13, 1) * _tmp38 + P(13, 2) * _tmp39 + P(13, 21) * _tmp47 + - P(13, 22) * _tmp48 + P(13, 3) * _tmp42 + P(13, 4) * _tmp45 + P(13, 5) * _tmp46); - _k(14, 0) = - _tmp49 * (P(14, 0) * _tmp37 + P(14, 1) * _tmp38 + P(14, 2) * _tmp39 + P(14, 21) * _tmp47 + - P(14, 22) * _tmp48 + P(14, 3) * _tmp42 + P(14, 4) * _tmp45 + P(14, 5) * _tmp46); - _k(15, 0) = - _tmp49 * (P(15, 0) * _tmp37 + P(15, 1) * _tmp38 + P(15, 2) * _tmp39 + P(15, 21) * _tmp47 + - P(15, 22) * _tmp48 + P(15, 3) * _tmp42 + P(15, 4) * _tmp45 + P(15, 5) * _tmp46); - _k(16, 0) = - _tmp49 * (P(16, 0) * _tmp37 + P(16, 1) * _tmp38 + P(16, 2) * _tmp39 + P(16, 21) * _tmp47 + - P(16, 22) * _tmp48 + P(16, 3) * _tmp42 + P(16, 4) * _tmp45 + P(16, 5) * _tmp46); - _k(17, 0) = - _tmp49 * (P(17, 0) * _tmp37 + P(17, 1) * _tmp38 + P(17, 2) * _tmp39 + P(17, 21) * _tmp47 + - P(17, 22) * _tmp48 + P(17, 3) * _tmp42 + P(17, 4) * _tmp45 + P(17, 5) * _tmp46); - _k(18, 0) = - _tmp49 * (P(18, 0) * _tmp37 + P(18, 1) * _tmp38 + P(18, 2) * _tmp39 + P(18, 21) * _tmp47 + - P(18, 22) * _tmp48 + P(18, 3) * _tmp42 + P(18, 4) * _tmp45 + P(18, 5) * _tmp46); - _k(19, 0) = - _tmp49 * (P(19, 0) * _tmp37 + P(19, 1) * _tmp38 + P(19, 2) * _tmp39 + P(19, 21) * _tmp47 + - P(19, 22) * _tmp48 + P(19, 3) * _tmp42 + P(19, 4) * _tmp45 + P(19, 5) * _tmp46); - _k(20, 0) = - _tmp49 * (P(20, 0) * _tmp37 + P(20, 1) * _tmp38 + P(20, 2) * _tmp39 + P(20, 21) * _tmp47 + - P(20, 22) * _tmp48 + P(20, 3) * _tmp42 + P(20, 4) * _tmp45 + P(20, 5) * _tmp46); - _k(21, 0) = - _tmp49 * (P(21, 0) * _tmp37 + P(21, 1) * _tmp38 + P(21, 2) * _tmp39 + P(21, 21) * _tmp47 + - P(21, 22) * _tmp48 + P(21, 3) * _tmp42 + P(21, 4) * _tmp45 + P(21, 5) * _tmp46); - _k(22, 0) = - _tmp49 * (P(22, 0) * _tmp37 + P(22, 1) * _tmp38 + P(22, 2) * _tmp39 + P(22, 21) * _tmp47 + - P(22, 22) * _tmp48 + P(22, 3) * _tmp42 + P(22, 4) * _tmp45 + P(22, 5) * _tmp46); - _k(23, 0) = - _tmp49 * (P(23, 0) * _tmp37 + P(23, 1) * _tmp38 + P(23, 2) * _tmp39 + P(23, 21) * _tmp47 + - P(23, 22) * _tmp48 + P(23, 3) * _tmp42 + P(23, 4) * _tmp45 + P(23, 5) * _tmp46); - } -} // NOLINT(readability/fn_size) - -// NOLINTNEXTLINE(readability/fn_size) -} // namespace sym diff --git a/src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp b/src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp index 2b2293904c..89ce999146 100644 --- a/src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp +++ b/src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp @@ -167,6 +167,10 @@ void EKFGSF_yaw::fuseVelocity(const Vector2f &vel_NE, const float vel_accuracy, const float yaw_delta = wrap_pi(_ekf_gsf[model_index].X(2) - _gsf_yaw); _gsf_yaw_variance += _model_weights(model_index) * (_ekf_gsf[model_index].P(2, 2) + yaw_delta * yaw_delta); } + + if (_gsf_yaw_variance <= 0.f || !PX4_ISFINITE(_gsf_yaw_variance)) { + reset(); + } } } diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index c85527b912..9e1f8f5378 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -745,6 +745,7 @@ void EKF2::Run() ekf2_timestamps_s ekf2_timestamps { .timestamp = now, .airspeed_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID, + .airspeed_validated_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID, .distance_sensor_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID, .optical_flow_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID, .vehicle_air_data_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID, @@ -1628,7 +1629,7 @@ void EKF2::PublishLocalPosition(const hrt_abstime ×tamp) || _ekf.control_status_flags().wind_dead_reckoning; // get control limit information - _ekf.get_ekf_ctrl_limits(&lpos.vxy_max, &lpos.vz_max, &lpos.hagl_min, &lpos.hagl_max); + _ekf.get_ekf_ctrl_limits(&lpos.vxy_max, &lpos.vz_max, &lpos.hagl_min, &lpos.hagl_max_z, &lpos.hagl_max_xy); // convert NaN to INFINITY if (!PX4_ISFINITE(lpos.vxy_max)) { @@ -1643,8 +1644,12 @@ void EKF2::PublishLocalPosition(const hrt_abstime ×tamp) lpos.hagl_min = INFINITY; } - if (!PX4_ISFINITE(lpos.hagl_max)) { - lpos.hagl_max = INFINITY; + if (!PX4_ISFINITE(lpos.hagl_max_z)) { + lpos.hagl_max_z = INFINITY; + } + + if (!PX4_ISFINITE(lpos.hagl_max_xy)) { + lpos.hagl_max_xy = INFINITY; } // publish vehicle local position data @@ -1928,6 +1933,7 @@ void EKF2::PublishStatusFlags(const hrt_abstime ×tamp) status_flags.cs_opt_flow_terrain = _ekf.control_status_flags().opt_flow_terrain; status_flags.cs_valid_fake_pos = _ekf.control_status_flags().valid_fake_pos; status_flags.cs_constant_pos = _ekf.control_status_flags().constant_pos; + status_flags.cs_baro_fault = _ekf.control_status_flags().baro_fault; status_flags.fault_status_changes = _filter_fault_status_changes; status_flags.fs_bad_mag_x = _ekf.fault_status_flags().bad_mag_x; @@ -2076,6 +2082,9 @@ void EKF2::UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps) } _airspeed_validated_timestamp_last = airspeed_validated.timestamp; + + ekf2_timestamps.airspeed_validated_timestamp_rel = (int16_t)((int64_t)airspeed_validated.timestamp / 100 - + (int64_t)ekf2_timestamps.timestamp / 100); } } else if (((ekf2_timestamps.timestamp - _airspeed_validated_timestamp_last) > 3_s) && _airspeed_sub.updated()) { diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 0bec478e0d..deb08c5a60 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -573,12 +573,9 @@ private: #endif // CONFIG_EKF2_AIRSPEED #if defined(CONFIG_EKF2_SIDESLIP) - (ParamExtFloat) - _param_ekf2_beta_gate, ///< synthetic sideslip innovation consistency gate size (STD) - (ParamExtFloat) _param_ekf2_beta_noise, ///< synthetic sideslip noise (rad) - - (ParamExtInt) - _param_ekf2_fuse_beta, ///< Controls synthetic sideslip fusion, 0 disables, 1 enables + (ParamExtFloat) _param_ekf2_beta_gate, + (ParamExtFloat) _param_ekf2_beta_noise, + (ParamExtInt) _param_ekf2_fuse_beta, #endif // CONFIG_EKF2_SIDESLIP #if defined(CONFIG_EKF2_MAGNETOMETER) diff --git a/src/modules/ekf2/Kconfig b/src/modules/ekf2/Kconfig index 93f96a656c..04bcfc104c 100644 --- a/src/modules/ekf2/Kconfig +++ b/src/modules/ekf2/Kconfig @@ -34,7 +34,7 @@ depends on MODULES_EKF2 menuconfig EKF2_AUX_GLOBAL_POSITION depends on MODULES_EKF2 bool "aux global position fusion support" - default n + default y ---help--- EKF2 auxiliary global position fusion support. diff --git a/src/modules/ekf2/params_range_finder.yaml b/src/modules/ekf2/params_range_finder.yaml index 78c30bbcca..97f60bdff4 100644 --- a/src/modules/ekf2/params_range_finder.yaml +++ b/src/modules/ekf2/params_range_finder.yaml @@ -158,7 +158,7 @@ parameters: If there's a jump from larger than RNG_FOG to smaller than EKF2_RNG_FOG, the measurement may gets rejected. 0 - disabled type: float - default: 1.0 + default: 3.0 min: 0.0 max: 20.0 unit: m diff --git a/src/modules/ekf2/params_sideslip.yaml b/src/modules/ekf2/params_sideslip.yaml index 49d5467fcc..0dcec672ad 100644 --- a/src/modules/ekf2/params_sideslip.yaml +++ b/src/modules/ekf2/params_sideslip.yaml @@ -6,8 +6,8 @@ parameters: description: short: Enable synthetic sideslip fusion long: 'For reliable wind estimation both sideslip and airspeed fusion (see - EKF2_ARSP_THR) should be enabled. Only applies to fixed-wing vehicles (or - VTOLs in fixed-wing mode). Note: side slip fusion is currently not supported + EKF2_ARSP_THR) should be enabled. Only applies to vehicles in fixed-wing mode + or with airspeed fusion active. Note: side slip fusion is currently not supported for tailsitters.' type: boolean default: 0 diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp index 834f79465f..8e19ffd5a3 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp @@ -282,12 +282,6 @@ void FlightTaskAuto::_prepareLandSetpoints() sticks_xy.setZero(); } - // If ground distance estimate valid (distance sensor) during nudging then limit horizontal speed - if (PX4_ISFINITE(_dist_to_bottom)) { - // Below 50cm no horizontal speed, above allow per meter altitude 0.5m/s speed - max_speed = math::max(0.f, math::min(max_speed, (_dist_to_bottom - .5f) * .5f)); - } - _stick_acceleration_xy.setVelocityConstraint(max_speed); _stick_acceleration_xy.generateSetpoints(sticks_xy, _yaw, _land_heading, _position, _velocity_setpoint_feedback.xy(), _deltatime); diff --git a/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp b/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp index a1ea2b6b80..6b79aa1833 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp @@ -59,8 +59,29 @@ bool FlightTaskManualAcceleration::activate(const trajectory_setpoint_s &last_se bool FlightTaskManualAcceleration::update() { + const vehicle_local_position_s vehicle_local_pos = _sub_vehicle_local_position.get(); + setMaxDistanceToGround(vehicle_local_pos.hagl_max_xy); bool ret = FlightTaskManualAltitudeSmoothVel::update(); + float max_hagl_ratio = 0.0f; + + if (PX4_ISFINITE(vehicle_local_pos.hagl_max_xy) && vehicle_local_pos.hagl_max_xy > FLT_EPSILON) { + max_hagl_ratio = (vehicle_local_pos.dist_bottom) / vehicle_local_pos.hagl_max_xy; + } + + // limit horizontal velocity near max hagl to decrease chance of larger gound distance jumps + static constexpr float factor_threshold = 0.8f; // threshold ratio of max_hagl + static constexpr float min_vel = 2.f; // minimum max-velocity near max_hagl + + if (max_hagl_ratio > factor_threshold) { + max_hagl_ratio = math::min(max_hagl_ratio, 1.f); + const float vxy_max = math::min(vehicle_local_pos.vxy_max, _param_mpc_vel_manual.get()); + _stick_acceleration_xy.setVelocityConstraint(interpolate(vxy_max, factor_threshold, min_vel, vxy_max, min_vel)); + + } else { + _stick_acceleration_xy.setVelocityConstraint(math::min(_param_mpc_vel_manual.get(), vehicle_local_pos.vxy_max)); + } + _stick_acceleration_xy.generateSetpoints(_sticks.getPitchRollExpo(), _yaw, _yaw_setpoint, _position, _velocity_setpoint_feedback.xy(), _deltatime); _stick_acceleration_xy.getSetpoints(_position_setpoint, _velocity_setpoint, _acceleration_setpoint); diff --git a/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.hpp b/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.hpp index 0ae3d35bb6..ac19500236 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.hpp +++ b/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.hpp @@ -52,4 +52,9 @@ protected: StickAccelerationXY _stick_acceleration_xy{this}; WeatherVane _weathervane{this}; /**< weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */ + + DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask, + (ParamFloat) _param_mpc_vel_manual, + (ParamFloat) _param_mpc_acc_hor + ) }; diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp index 280aea617f..7ea9289cb6 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp @@ -81,11 +81,8 @@ void FlightTaskManualAltitude::_updateConstraintsFromEstimator() _min_distance_to_ground = -INFINITY; } - if (PX4_ISFINITE(_sub_vehicle_local_position.get().hagl_max)) { - _max_distance_to_ground = _sub_vehicle_local_position.get().hagl_max; - - } else { - _max_distance_to_ground = INFINITY; + if (!PX4_ISFINITE(_max_distance_to_ground) && PX4_ISFINITE(_sub_vehicle_local_position.get().hagl_max_z)) { + _max_distance_to_ground = _sub_vehicle_local_position.get().hagl_max_z; } } @@ -154,8 +151,6 @@ void FlightTaskManualAltitude::_updateAltitudeLock() if ((_param_mpc_alt_mode.get() == 1 || _terrain_hold) && PX4_ISFINITE(_dist_to_bottom)) { // terrain following _terrainFollowing(apply_brake, stopped); - // respect maximum altitude - _respectMaxAltitude(); } else { // normal mode where height is dependent on local frame @@ -185,9 +180,10 @@ void FlightTaskManualAltitude::_updateAltitudeLock() // user demands velocity change _position_setpoint(2) = NAN; // ensure that maximum altitude is respected - _respectMaxAltitude(); } } + + _respectMaxAltitude(); } void FlightTaskManualAltitude::_respectMinAltitude() @@ -229,29 +225,20 @@ void FlightTaskManualAltitude::_respectMaxAltitude() { if (PX4_ISFINITE(_dist_to_bottom)) { - // if there is a valid maximum distance to ground, linearly increase speed limit with distance - // below the maximum, preserving control loop vertical position error gain. - // TODO: manipulate the velocity setpoint instead of tweaking the saturation of the controller + float vel_constrained = _param_mpc_z_p.get() * (_max_distance_to_ground - _dist_to_bottom); + if (PX4_ISFINITE(_max_distance_to_ground)) { - _constraints.speed_up = math::constrain(_param_mpc_z_p.get() * (_max_distance_to_ground - _dist_to_bottom), - -_param_mpc_z_vel_max_dn.get(), _param_mpc_z_vel_max_up.get()); + _constraints.speed_up = math::constrain(vel_constrained, -_param_mpc_z_vel_max_dn.get(), _param_mpc_z_vel_max_up.get()); } else { _constraints.speed_up = _param_mpc_z_vel_max_up.get(); } - // if distance to bottom exceeded maximum distance, slowly approach maximum distance - if (_dist_to_bottom > _max_distance_to_ground) { - // difference between current distance to ground and maximum distance to ground - const float delta_distance_to_max = _dist_to_bottom - _max_distance_to_ground; - // set position setpoint to maximum distance to ground - _position_setpoint(2) = _position(2) + delta_distance_to_max; - // limit speed downwards to 0.7m/s - _constraints.speed_down = math::min(_param_mpc_z_vel_max_dn.get(), 0.7f); - - } else { - _constraints.speed_down = _param_mpc_z_vel_max_dn.get(); + if (_dist_to_bottom > _max_distance_to_ground && !(_sticks.getThrottleZeroCenteredExpo() < FLT_EPSILON)) { + _velocity_setpoint(2) = math::constrain(-vel_constrained, 0.f, _param_mpc_z_vel_max_dn.get()); } + + _constraints.speed_down = _param_mpc_z_vel_max_dn.get(); } } @@ -306,6 +293,7 @@ bool FlightTaskManualAltitude::update() _scaleSticks(); _updateSetpoints(); _constraints.want_takeoff = _checkTakeoff(); + _max_distance_to_ground = INFINITY; return ret; } diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp index 600e81fe36..5370de28ff 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp @@ -53,6 +53,7 @@ public: bool activate(const trajectory_setpoint_s &last_setpoint) override; bool updateInitialize() override; bool update() override; + void setMaxDistanceToGround(float max_distance) { _max_distance_to_ground = max_distance; } protected: void _ekfResetHandlerHeading(float delta_psi) override; /**< adjust heading setpoint in case of EKF reset event */ diff --git a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp index f823f929e8..c36d024ead 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp +++ b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp @@ -73,8 +73,20 @@ void StickAccelerationXY::resetAcceleration(const matrix::Vector2f &acceleration void StickAccelerationXY::generateSetpoints(Vector2f stick_xy, const float yaw, const float yaw_sp, const Vector3f &pos, const matrix::Vector2f &vel_sp_feedback, const float dt) { + // gradually adjust velocity constraint because good tracking is required for the drag estimation + if (fabsf(_targeted_velocity_constraint - _current_velocity_constraint) > 0.1f) { + if (!PX4_ISFINITE(_current_velocity_constraint) || !PX4_ISFINITE(_targeted_velocity_constraint)) { + _current_velocity_constraint = _targeted_velocity_constraint; + + } else { + _current_velocity_constraint = math::constrain(_targeted_velocity_constraint, + _current_velocity_constraint - dt * _param_mpc_acc_hor.get(), + _current_velocity_constraint + dt * _param_mpc_acc_hor.get()); + } + } + // maximum commanded velocity can be constrained dynamically - const float velocity_sc = fminf(_param_mpc_vel_manual.get(), _velocity_constraint); + const float velocity_sc = fminf(_param_mpc_vel_manual.get(), _current_velocity_constraint); Vector2f velocity_scale(velocity_sc, velocity_sc); // maximum commanded acceleration is scaled down with velocity const float acceleration_sc = _param_mpc_acc_hor.get() * (velocity_sc / _param_mpc_vel_manual.get()); diff --git a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp index 9fff1da1c4..1b29580c4f 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp +++ b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp @@ -63,7 +63,8 @@ public: void getSetpoints(matrix::Vector3f &pos_sp, matrix::Vector3f &vel_sp, matrix::Vector3f &acc_sp); float getMaxAcceleration() { return _param_mpc_acc_hor.get(); }; float getMaxJerk() { return _param_mpc_jerk_max.get(); }; - void setVelocityConstraint(float vel) { _velocity_constraint = fmaxf(vel, FLT_EPSILON); }; + void setVelocityConstraint(float vel) { _targeted_velocity_constraint = fmaxf(vel, FLT_EPSILON); }; + float getVelocityConstraint() { return _current_velocity_constraint; }; private: CollisionPrevention _collision_prevention{this}; @@ -85,7 +86,8 @@ private: matrix::Vector2f _acceleration_setpoint; matrix::Vector2f _acceleration_setpoint_prev; - float _velocity_constraint{INFINITY}; + float _targeted_velocity_constraint{INFINITY}; + float _current_velocity_constraint{INFINITY}; DEFINE_PARAMETERS( (ParamFloat) _param_mpc_vel_manual, diff --git a/src/modules/gimbal/output_mavlink.cpp b/src/modules/gimbal/output_mavlink.cpp index ca562ec870..ae5222c674 100644 --- a/src/modules/gimbal/output_mavlink.cpp +++ b/src/modules/gimbal/output_mavlink.cpp @@ -109,6 +109,9 @@ void OutputMavlinkV1::update(const ControlData &control_data, bool new_setpoints _stream_device_attitude_status(); + // If the output is MAVLink v1, then we signal this by referring to compid 1. + gimbal_device_id = 1; + _last_update = now; } diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index ec8157034b..4ed4a353ab 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -71,9 +71,13 @@ bool FixedwingLandDetector::_get_landed_state() } else if (hrt_elapsed_time(&_vehicle_local_position.timestamp) < 1_s) { - // Horizontal velocity complimentary filter. - float val = 0.97f * _velocity_xy_filtered + 0.03f * sqrtf(_vehicle_local_position.vx * _vehicle_local_position.vx + - _vehicle_local_position.vy * _vehicle_local_position.vy); + float val = 0.0f; + + if (_vehicle_local_position.v_xy_valid) { + // Horizontal velocity complimentary filter. + val = 0.97f * _velocity_xy_filtered + 0.03f * sqrtf(_vehicle_local_position.vx * _vehicle_local_position.vx + + _vehicle_local_position.vy * _vehicle_local_position.vy); + } if (PX4_ISFINITE(val)) { _velocity_xy_filtered = val; @@ -105,15 +109,26 @@ bool FixedwingLandDetector::_get_landed_state() const float acc_hor = matrix::Vector2f(_acceleration).norm(); _xy_accel_filtered = _xy_accel_filtered * 0.8f + acc_hor * 0.18f; + // Check for angular velocity + const float rot_vel_hor = _angular_velocity.norm(); + val = _velocity_rot_filtered * 0.95f + rot_vel_hor * 0.05f; + + if (PX4_ISFINITE(val)) { + _velocity_rot_filtered = val; + } + // make groundspeed threshold tighter if airspeed is invalid - const float vel_xy_max_threshold = airspeed_invalid ? 0.7f * _param_lndfw_vel_xy_max.get() : - _param_lndfw_vel_xy_max.get(); + const float vel_xy_max_threshold = airspeed_invalid ? 0.7f * _param_lndfw_vel_xy_max.get() : + _param_lndfw_vel_xy_max.get(); + + const float max_rotation_threshold = math::radians(_param_lndfw_rot_max.get()) ; // Crude land detector for fixedwing. - landDetected = _airspeed_filtered < _param_lndfw_airspd.get() - && _velocity_xy_filtered < vel_xy_max_threshold - && _velocity_z_filtered < _param_lndfw_vel_z_max.get() - && _xy_accel_filtered < _param_lndfw_xyaccel_max.get(); + landDetected = _airspeed_filtered < _param_lndfw_airspd.get() + && _velocity_xy_filtered < vel_xy_max_threshold + && _velocity_z_filtered < _param_lndfw_vel_z_max.get() + && _xy_accel_filtered < _param_lndfw_xyaccel_max.get() + && _velocity_rot_filtered < max_rotation_threshold; } else { // Control state topic has timed out and we need to assume we're landed. diff --git a/src/modules/land_detector/FixedwingLandDetector.h b/src/modules/land_detector/FixedwingLandDetector.h index 981fdc2fb5..3f3fa9ad35 100644 --- a/src/modules/land_detector/FixedwingLandDetector.h +++ b/src/modules/land_detector/FixedwingLandDetector.h @@ -75,6 +75,7 @@ private: float _velocity_xy_filtered{0.0f}; float _velocity_z_filtered{0.0f}; float _xy_accel_filtered{0.0f}; + float _velocity_rot_filtered{0.0f}; DEFINE_PARAMETERS_CUSTOM_PARENT( LandDetector, @@ -82,6 +83,7 @@ private: (ParamFloat) _param_lndfw_airspd, (ParamFloat) _param_lndfw_vel_xy_max, (ParamFloat) _param_lndfw_vel_z_max, + (ParamFloat) _param_lndfw_rot_max, (ParamFloat) _param_lndfw_trig_time ); }; diff --git a/src/modules/land_detector/land_detector_params_fw.c b/src/modules/land_detector/land_detector_params_fw.c index 02ab459032..6ab78fb867 100644 --- a/src/modules/land_detector/land_detector_params_fw.c +++ b/src/modules/land_detector/land_detector_params_fw.c @@ -102,3 +102,14 @@ PARAM_DEFINE_FLOAT(LNDFW_AIRSPD_MAX, 6.00f); * @group Land Detector */ PARAM_DEFINE_FLOAT(LNDFW_TRIG_TIME, 2.f); + +/** + * Fixed-wing land detector: max rotational speed + * + * Maximum allowed norm of the angular velocity in the landed state. + * + * @unit deg/s + * @decimal 1 + * @group Land Detector + */ +PARAM_DEFINE_FLOAT(LNDFW_ROT_MAX, 0.5f); diff --git a/src/modules/land_detector/land_detector_params_mc.c b/src/modules/land_detector/land_detector_params_mc.c index 121b365bb5..1d61a7e00b 100644 --- a/src/modules/land_detector/land_detector_params_mc.c +++ b/src/modules/land_detector/land_detector_params_mc.c @@ -76,9 +76,9 @@ PARAM_DEFINE_FLOAT(LNDMC_Z_VEL_MAX, 0.25f); PARAM_DEFINE_FLOAT(LNDMC_XY_VEL_MAX, 1.5f); /** - * Multicopter max rotation + * Multicopter max rotational speed * - * Maximum allowed angular velocity around each axis allowed in the landed state. + * Maximum allowed norm of the angular velocity (roll, pitch) in the landed state. * * @unit deg/s * @decimal 1 diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp index 8517ff03b0..f28bb78c08 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -643,7 +643,8 @@ void BlockLocalPositionEstimator::publishLocalPos() _pub_lpos.get().vxy_max = INFINITY; _pub_lpos.get().vz_max = INFINITY; _pub_lpos.get().hagl_min = INFINITY; - _pub_lpos.get().hagl_max = INFINITY; + _pub_lpos.get().hagl_max_z = INFINITY; + _pub_lpos.get().hagl_max_xy = INFINITY; _pub_lpos.get().timestamp = hrt_absolute_time();; _pub_lpos.update(); } diff --git a/src/modules/logger/log_writer_file.cpp b/src/modules/logger/log_writer_file.cpp index 14a2ad0136..530076f49b 100644 --- a/src/modules/logger/log_writer_file.cpp +++ b/src/modules/logger/log_writer_file.cpp @@ -88,7 +88,7 @@ LogWriterFile::~LogWriterFile() } #if defined(PX4_CRYPTO) -bool LogWriterFile::init_logfile_encryption(const char *filename) +bool LogWriterFile::init_logfile_encryption(const LogType type) { if (_algorithm == CRYPTO_NONE) { _min_blocksize = 1; @@ -151,16 +151,16 @@ bool LogWriterFile::init_logfile_encryption(const char *filename) rsa_crypto.close(); - // Write the encrypted key to the disk - int key_fd = ::open((const char *)filename, O_CREAT | O_WRONLY | O_DIRECT | O_SYNC, PX4_O_MODE_666); + // Write the encrypted key to the beginning of the opened log file + int key_fd = _buffers[(int)type].fd(); if (key_fd < 0) { - PX4_ERR("Can't open key file, errno: %d", errno); + PX4_ERR("Log file not open for storing the key, errno: %d", errno); free(key); return false; } - // write the header to the combined key exchange & cipherdata file + // write header and key to the beginning of the log file struct ulog_key_header_s keyfile_header = { .magic = {'U', 'L', 'o', 'g', 'E', 'n', 'c'}, .hdr_ver = 1, @@ -171,20 +171,14 @@ bool LogWriterFile::init_logfile_encryption(const char *filename) .initdata_size = (uint16_t)nonce_size }; - size_t hdr_sz = ::write(key_fd, (uint8_t *)&keyfile_header, sizeof(keyfile_header)); - size_t written = 0; - - if (hdr_sz == sizeof(keyfile_header)) { - // Header write succeeded, write the key - written = ::write(key_fd, key, key_size + nonce_size); - } + size_t written = ::write(key_fd, (uint8_t *)&keyfile_header, sizeof(keyfile_header)); + written += ::write(key_fd, key, key_size + nonce_size); // Free temporary memory allocations free(key); - ::close(key_fd); // Check that writing to the disk succeeded - if (written != key_size + nonce_size) { + if (written != sizeof(keyfile_header) + key_size + nonce_size) { PX4_ERR("Writing the encryption key to disk fail"); return false; } @@ -220,18 +214,22 @@ bool LogWriterFile::start_log(LogType type, const char *filename) } } -#if PX4_CRYPTO - bool enc_init = init_logfile_encryption(filename); + if (_buffers[(int)type].start_log(filename)) { - if (!enc_init) { - PX4_ERR("Failed to start encrypted logging"); - _crypto.close(); - return false; - } +#if PX4_CRYPTO + bool enc_init = init_logfile_encryption(type); + + if (!enc_init) { + PX4_ERR("Failed to start encrypted logging"); + _crypto.close(); + _buffers[(int)type]._should_run = false; + _buffers[(int)type].close_file(); + _buffers[(int)type].reset(); + return false; + } #endif - if (_buffers[(int)type].start_log(filename)) { PX4_INFO("Opened %s log file: %s", log_type_str(type), filename); notify(); return true; @@ -637,11 +635,7 @@ size_t LogWriterFile::LogFileBuffer::get_read_ptr(void **ptr, bool *is_part) bool LogWriterFile::LogFileBuffer::start_log(const char *filename) { -#if defined(PX4_CRYPTO) - _fd = ::open(filename, O_APPEND | O_WRONLY, PX4_O_MODE_666); -#else _fd = ::open(filename, O_CREAT | O_WRONLY, PX4_O_MODE_666); -#endif _had_write_error.store(false); if (_fd < 0) { diff --git a/src/modules/logger/log_writer_file.h b/src/modules/logger/log_writer_file.h index 7654878938..4739b5b799 100644 --- a/src/modules/logger/log_writer_file.h +++ b/src/modules/logger/log_writer_file.h @@ -224,7 +224,7 @@ private: pthread_cond_t _cv; pthread_t _thread = 0; #if defined(PX4_CRYPTO) - bool init_logfile_encryption(const char *filename); + bool init_logfile_encryption(const LogType type); PX4Crypto _crypto; int _min_blocksize; px4_crypto_algorithm_t _algorithm; diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index ac0fbd7b90..4fdd9ef052 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -53,7 +53,6 @@ void LoggedTopics::add_default_topics() add_optional_topic("autotune_attitude_control_status", 100); add_optional_topic("camera_capture"); add_optional_topic("camera_trigger"); - add_optional_topic("can_interface_status", 10); add_topic("cellular_status", 200); add_topic("commander_state"); add_topic("config_overrides"); @@ -255,6 +254,10 @@ void LoggedTopics::add_default_topics() add_optional_topic_multi("yaw_estimator_status"); #endif /* CONFIG_ARCH_BOARD_PX4_SITL */ + +#ifdef CONFIG_BOARD_UAVCAN_INTERFACES + add_topic_multi("can_interface_status", 100, CONFIG_BOARD_UAVCAN_INTERFACES); +#endif } void LoggedTopics::add_high_rate_topics() @@ -295,6 +298,7 @@ void LoggedTopics::add_estimator_replay_topics() // current EKF2 subscriptions add_topic("airspeed"); + add_topic("airspeed_validated"); add_topic("vehicle_optical_flow"); add_topic("sensor_combined"); add_topic("sensor_selection"); diff --git a/src/modules/mavlink/mavlink b/src/modules/mavlink/mavlink index 5e3a42b8f3..619947d8bc 160000 --- a/src/modules/mavlink/mavlink +++ b/src/modules/mavlink/mavlink @@ -1 +1 @@ -Subproject commit 5e3a42b8f3f53038f2779f9f69bd64767b913bb8 +Subproject commit 619947d8bc29e80eecff18b0f4fecc42d9e171dd diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 1c63f8adee..6e81849600 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1744,6 +1744,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("CAMERA_TRIGGER", unlimited_rate); configure_stream_local("LOCAL_POSITION_NED", 30.0f); configure_stream_local("ATTITUDE", 20.0f); + configure_stream_local("ATTITUDE_QUATERNION", 20.0f); configure_stream_local("ALTITUDE", 10.0f); configure_stream_local("DISTANCE_SENSOR", 10.0f); configure_stream_local("MOUNT_ORIENTATION", 10.0f); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index e698c85186..aa8b50d01a 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -2738,7 +2738,8 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) hil_local_pos.vxy_max = INFINITY; hil_local_pos.vz_max = INFINITY; hil_local_pos.hagl_min = INFINITY; - hil_local_pos.hagl_max = INFINITY; + hil_local_pos.hagl_max_z = INFINITY; + hil_local_pos.hagl_max_xy = INFINITY; hil_local_pos.timestamp = hrt_absolute_time(); _local_pos_pub.publish(hil_local_pos); } @@ -3074,12 +3075,7 @@ MavlinkReceiver::handle_message_gimbal_device_information(mavlink_message_t *msg gimbal_information.yaw_max = gimbal_device_info_msg.yaw_max; gimbal_information.yaw_min = gimbal_device_info_msg.yaw_min; - if (gimbal_device_info_msg.gimbal_device_id == 0) { - gimbal_information.gimbal_device_id = msg->compid; - - } else { - gimbal_information.gimbal_device_id = gimbal_device_info_msg.gimbal_device_id; - } + gimbal_information.gimbal_device_id = msg->compid; _gimbal_device_information_pub.publish(gimbal_information); } diff --git a/src/modules/mavlink/streams/GIMBAL_DEVICE_ATTITUDE_STATUS.hpp b/src/modules/mavlink/streams/GIMBAL_DEVICE_ATTITUDE_STATUS.hpp index c8338996ae..ae7f3cbad1 100644 --- a/src/modules/mavlink/streams/GIMBAL_DEVICE_ATTITUDE_STATUS.hpp +++ b/src/modules/mavlink/streams/GIMBAL_DEVICE_ATTITUDE_STATUS.hpp @@ -74,48 +74,31 @@ private: return false; } - if (gimbal_device_attitude_status.gimbal_device_id >= 1 && gimbal_device_attitude_status.gimbal_device_id <= 6) { - // A non-MAVLink gimbal is signalled and addressed using 1 to 6 as the gimbal_device_id - mavlink_gimbal_device_attitude_status_t msg{}; + mavlink_gimbal_device_attitude_status_t msg{}; - msg.target_system = gimbal_device_attitude_status.target_system; - msg.target_component = gimbal_device_attitude_status.target_component; + msg.target_system = gimbal_device_attitude_status.target_system; + msg.target_component = gimbal_device_attitude_status.target_component; - msg.time_boot_ms = gimbal_device_attitude_status.timestamp / 1000; + msg.time_boot_ms = gimbal_device_attitude_status.timestamp / 1000; - msg.flags = gimbal_device_attitude_status.device_flags; + msg.flags = gimbal_device_attitude_status.device_flags; - msg.q[0] = gimbal_device_attitude_status.q[0]; - msg.q[1] = gimbal_device_attitude_status.q[1]; - msg.q[2] = gimbal_device_attitude_status.q[2]; - msg.q[3] = gimbal_device_attitude_status.q[3]; + msg.q[0] = gimbal_device_attitude_status.q[0]; + msg.q[1] = gimbal_device_attitude_status.q[1]; + msg.q[2] = gimbal_device_attitude_status.q[2]; + msg.q[3] = gimbal_device_attitude_status.q[3]; - msg.angular_velocity_x = gimbal_device_attitude_status.angular_velocity_x; - msg.angular_velocity_y = gimbal_device_attitude_status.angular_velocity_y; - msg.angular_velocity_z = gimbal_device_attitude_status.angular_velocity_z; + msg.angular_velocity_x = gimbal_device_attitude_status.angular_velocity_x; + msg.angular_velocity_y = gimbal_device_attitude_status.angular_velocity_y; + msg.angular_velocity_z = gimbal_device_attitude_status.angular_velocity_z; - msg.failure_flags = gimbal_device_attitude_status.failure_flags; - msg.gimbal_device_id = gimbal_device_attitude_status.gimbal_device_id; + msg.failure_flags = gimbal_device_attitude_status.failure_flags; + msg.gimbal_device_id = gimbal_device_attitude_status.gimbal_device_id; - msg.delta_yaw = gimbal_device_attitude_status.delta_yaw; - msg.delta_yaw_velocity = gimbal_device_attitude_status.delta_yaw_velocity; + msg.delta_yaw = gimbal_device_attitude_status.delta_yaw; + msg.delta_yaw_velocity = gimbal_device_attitude_status.delta_yaw_velocity; - mavlink_msg_gimbal_device_attitude_status_send_struct(_mavlink->get_channel(), &msg); - - } else { - // We have a Mavlink gimbal. We simulate its mavlink instance by spoofing the component ID - mavlink_message_t message; - mavlink_msg_gimbal_device_attitude_status_pack_chan(_mavlink->get_system_id(), MAV_COMP_ID_GIMBAL, - _mavlink->get_channel(), &message, - gimbal_device_attitude_status.target_system, gimbal_device_attitude_status.target_component, - gimbal_device_attitude_status.timestamp / 1000, - gimbal_device_attitude_status.device_flags, gimbal_device_attitude_status.q, - gimbal_device_attitude_status.angular_velocity_x, - gimbal_device_attitude_status.angular_velocity_y, gimbal_device_attitude_status.angular_velocity_z, - gimbal_device_attitude_status.failure_flags, - 0, 0, 0); - _mavlink->forward_message(&message, _mavlink); - } + mavlink_msg_gimbal_device_attitude_status_send_struct(_mavlink->get_channel(), &msg); return true; } diff --git a/src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp b/src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp index 6f1585f801..02f3894f0e 100644 --- a/src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp +++ b/src/modules/mc_att_control/AttitudeControl/AttitudeControl.cpp @@ -68,17 +68,21 @@ matrix::Vector3f AttitudeControl::update(const Quatf &q) const qd_red = qd; } else { - // transform rotation from current to desired thrust vector into a world frame reduced desired attitude + // Transform rotation from current to desired thrust vector into a world frame reduced desired attitude. + // This is a right multiplication as the tilt error quaternion is obtained from two Z vectors expressed in the world frame. qd_red *= q; } - // mix full and reduced desired attitude - Quatf q_mix = qd_red.inversed() * qd; - q_mix.canonicalize(); + // With a full desired attitude given by: qd = qd_red * qd_dyaw, extract the delta yaw component. + // By definition, the delta yaw quaternion has the form (cos(angle/2), 0, 0, sin(angle/2)) + Quatf qd_dyaw = qd_red.inversed() * qd; + qd_dyaw.canonicalize(); // catch numerical problems with the domain of acosf and asinf - q_mix(0) = math::constrain(q_mix(0), -1.f, 1.f); - q_mix(3) = math::constrain(q_mix(3), -1.f, 1.f); - qd = qd_red * Quatf(cosf(_yaw_w * acosf(q_mix(0))), 0, 0, sinf(_yaw_w * asinf(q_mix(3)))); + qd_dyaw(0) = math::constrain(qd_dyaw(0), -1.f, 1.f); + qd_dyaw(3) = math::constrain(qd_dyaw(3), -1.f, 1.f); + + // scale the delta yaw angle and re-combine the desired attitude + qd = qd_red * Quatf(cosf(_yaw_w * acosf(qd_dyaw(0))), 0.f, 0.f, sinf(_yaw_w * asinf(qd_dyaw(3)))); // quaternion attitude control law, qe is rotation from q to qd const Quatf qe = q.inversed() * qd; diff --git a/src/modules/mc_att_control/mc_att_control.hpp b/src/modules/mc_att_control/mc_att_control.hpp index db1dcd3c15..995eea4256 100644 --- a/src/modules/mc_att_control/mc_att_control.hpp +++ b/src/modules/mc_att_control/mc_att_control.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-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 @@ -47,6 +47,7 @@ #include #include #include +#include #include #include #include @@ -98,9 +99,11 @@ private: uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + uORB::Subscription _hover_thrust_estimate_sub{ORB_ID(hover_thrust_estimate)}; + uORB::Subscription _vehicle_attitude_setpoint_sub{ORB_ID(vehicle_attitude_setpoint)}; + uORB::Subscription _v_control_mode_sub{ORB_ID(vehicle_control_mode)}; uORB::Subscription _autotune_attitude_control_status_sub{ORB_ID(autotune_attitude_control_status)}; uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; - uORB::Subscription _vehicle_attitude_setpoint_sub{ORB_ID(vehicle_attitude_setpoint)}; uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; @@ -118,8 +121,10 @@ private: matrix::Vector3f _thrust_setpoint_body; /**< body frame 3D thrust vector */ - float _man_yaw_sp{0.f}; /**< current yaw setpoint in manual mode */ - float _man_tilt_max; /**< maximum tilt allowed for manual flight [rad] */ + float _hover_thrust{NAN}; + + float _man_yaw_sp{0.f}; /**< current yaw setpoint in manual mode */ + float _man_tilt_max{0.f}; /**< maximum tilt allowed for manual flight [rad] */ SlewRate _manual_throttle_minimum{0.f}; ///< 0 when landed and ramped to MPC_MANTHR_MIN in air SlewRate _manual_throttle_maximum{0.f}; ///< 0 when disarmed ramped to 1 when spooled up diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index f6cc5db43d..22b78d519c 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-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 @@ -103,15 +103,31 @@ MulticopterAttitudeControl::throttle_curve(float throttle_stick_input) { float thrust = 0.f; + { + hover_thrust_estimate_s hte; + + if (_hover_thrust_estimate_sub.update(&hte)) { + if (hte.valid) { + _hover_thrust = hte.hover_thrust; + } + } + } + + if (!PX4_ISFINITE(_hover_thrust)) { + _hover_thrust = _param_mpc_thr_hover.get(); + } + + // throttle_stick_input is in range [-1, 1] switch (_param_mpc_thr_curve.get()) { case 1: // no rescaling to hover throttle thrust = math::interpolate(throttle_stick_input, -1.f, 1.f, _manual_throttle_minimum.getState(), _param_mpc_thr_max.get()); break; - default: // 0 or other: rescale such that a centered throttle stick corresponds to hover throttle - thrust = math::interpolateNXY(throttle_stick_input, {-1.f, 0.f, 1.f}, - {_manual_throttle_minimum.getState(), _param_mpc_thr_hover.get(), _param_mpc_thr_max.get()}); + default: // 0 or other: rescale to hover throttle at 0 stick input + thrust = math::interpolateNXY(throttle_stick_input, + {-1.f, 0.f, 1.f}, + {_manual_throttle_minimum.getState(), _hover_thrust, _param_mpc_thr_max.get()}); break; } diff --git a/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp b/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp index 07c23bbb5d..d8b97336b9 100644 --- a/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp +++ b/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp @@ -134,11 +134,6 @@ void MulticopterHoverThrustEstimator::Run() } } - // new local position setpoint needed every iteration - if (!_vehicle_local_position_setpoint_sub.updated()) { - return; - } - // check for parameter updates if (_parameter_update_sub.updated()) { // clear update @@ -166,10 +161,25 @@ void MulticopterHoverThrustEstimator::Run() _hover_thrust_ekf.predict(dt); - vehicle_local_position_setpoint_s local_pos_sp; + vehicle_attitude_s vehicle_attitude{}; + _vehicle_attitude_sub.copy(&vehicle_attitude); - if (_vehicle_local_position_setpoint_sub.copy(&local_pos_sp)) { - if (PX4_ISFINITE(local_pos_sp.thrust[2])) { + vehicle_thrust_setpoint_s vehicle_thrust_setpoint; + control_allocator_status_s control_allocator_status; + + if (_vehicle_thrust_setpoint_sub.update(&vehicle_thrust_setpoint) + && _control_allocator_status_sub.update(&control_allocator_status) + && (hrt_elapsed_time(&vehicle_thrust_setpoint.timestamp) < 20_ms) + && (hrt_elapsed_time(&vehicle_attitude.timestamp) < 20_ms) + ) { + matrix::Vector3f thrust_body_sp(vehicle_thrust_setpoint.xyz); + matrix::Vector3f thrust_body_unallocated(control_allocator_status.unallocated_thrust); + matrix::Vector3f thrust_body_allocated = thrust_body_sp - thrust_body_unallocated; + + const matrix::Quatf q_att{vehicle_attitude.q}; + matrix::Vector3f thrust_allocated = q_att.rotateVector(thrust_body_allocated); + + if (PX4_ISFINITE(thrust_allocated(2))) { // Inform the hover thrust estimator about the measured vertical // acceleration (positive acceleration is up) and the current thrust (positive thrust is up) // Guard against fast up and down motions biasing the estimator due to large drag and prop wash effects @@ -179,7 +189,7 @@ void MulticopterHoverThrustEstimator::Run() 1.f); _hover_thrust_ekf.setMeasurementNoiseScale(fmaxf(meas_noise_coeff_xy, meas_noise_coeff_z)); - _hover_thrust_ekf.fuseAccZ(-local_pos.az, -local_pos_sp.thrust[2]); + _hover_thrust_ekf.fuseAccZ(-local_pos.az, -thrust_allocated(2)); bool valid = (_hover_thrust_ekf.getHoverThrustEstimateVar() < 0.001f); @@ -191,7 +201,7 @@ void MulticopterHoverThrustEstimator::Run() _valid_hysteresis.set_state_and_update(valid, local_pos.timestamp); _valid = _valid_hysteresis.get_state(); - publishStatus(local_pos.timestamp); + publishStatus(vehicle_thrust_setpoint.timestamp); } } diff --git a/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.hpp b/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.hpp index b1d0324298..1738cfe785 100644 --- a/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.hpp +++ b/src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.hpp @@ -54,10 +54,12 @@ #include #include #include +#include #include #include -#include #include +#include +#include #include "zero_order_hover_thrust_ekf.hpp" @@ -101,9 +103,12 @@ private: uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + uORB::Subscription _hover_thrust_estimate_sub{ORB_ID(hover_thrust_estimate)}; uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; - uORB::Subscription _vehicle_local_position_setpoint_sub{ORB_ID(vehicle_local_position_setpoint)}; + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _vehicle_thrust_setpoint_sub{ORB_ID(vehicle_thrust_setpoint)}; + uORB::Subscription _control_allocator_status_sub{ORB_ID(control_allocator_status)}; hrt_abstime _timestamp_last{0}; diff --git a/src/modules/mc_rate_control/MulticopterRateControl.cpp b/src/modules/mc_rate_control/MulticopterRateControl.cpp index fea041022e..3931125289 100644 --- a/src/modules/mc_rate_control/MulticopterRateControl.cpp +++ b/src/modules/mc_rate_control/MulticopterRateControl.cpp @@ -95,6 +95,8 @@ MulticopterRateControl::parameters_updated() // manual rate control acro mode rate limits _acro_rate_max = Vector3f(radians(_param_mc_acro_r_max.get()), radians(_param_mc_acro_p_max.get()), radians(_param_mc_acro_y_max.get())); + + _output_lpf_yaw.setCutoffFreq(_param_mc_yaw_tq_cutoff.get()); } void @@ -214,9 +216,12 @@ MulticopterRateControl::Run() } // run rate controller - const Vector3f torque_setpoint = + Vector3f torque_setpoint = _rate_control.update(rates, _rates_setpoint, angular_accel, dt, _maybe_landed || _landed); + // apply low-pass filtering on yaw axis to reduce high frequency torque caused by rotor acceleration + torque_setpoint(2) = _output_lpf_yaw.update(torque_setpoint(2), dt); + // publish rate controller status rate_ctrl_status_s rate_ctrl_status{}; _rate_control.getRateControlStatus(rate_ctrl_status); diff --git a/src/modules/mc_rate_control/MulticopterRateControl.hpp b/src/modules/mc_rate_control/MulticopterRateControl.hpp index 0f402c12c3..1a2427da18 100644 --- a/src/modules/mc_rate_control/MulticopterRateControl.hpp +++ b/src/modules/mc_rate_control/MulticopterRateControl.hpp @@ -34,6 +34,7 @@ #pragma once #include +#include #include #include #include @@ -129,6 +130,8 @@ private: float _energy_integration_time{0.0f}; float _control_energy[4] {}; + AlphaFilter _output_lpf_yaw; + DEFINE_PARAMETERS( (ParamFloat) _param_mc_rollrate_p, (ParamFloat) _param_mc_rollrate_i, @@ -150,6 +153,7 @@ private: (ParamFloat) _param_mc_yawrate_d, (ParamFloat) _param_mc_yawrate_ff, (ParamFloat) _param_mc_yawrate_k, + (ParamFloat) _param_mc_yaw_tq_cutoff, (ParamFloat) _param_mc_acro_r_max, (ParamFloat) _param_mc_acro_p_max, diff --git a/src/modules/mc_rate_control/mc_rate_control_params.c b/src/modules/mc_rate_control/mc_rate_control_params.c index 2571a70c1f..b37c52e746 100644 --- a/src/modules/mc_rate_control/mc_rate_control_params.c +++ b/src/modules/mc_rate_control/mc_rate_control_params.c @@ -292,3 +292,17 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_K, 1.0f); * @group Multicopter Rate Control */ PARAM_DEFINE_INT32(MC_BAT_SCALE_EN, 0); + +/** + * Low pass filter cutoff frequency for yaw torque setpoint + * + * Reduces vibrations by lowering high frequency torque caused by rotor acceleration. + * 0 disables the filter + * + * @min 0 + * @max 10 + * @unit Hz + * @decimal 3 + * @group Multicopter Rate Control + */ +PARAM_DEFINE_FLOAT(MC_YAW_TQ_CUTOFF, 2.f); diff --git a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp index dbaf173efa..c35d2c1bde 100644 --- a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp +++ b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp @@ -46,12 +46,7 @@ FeasibilityChecker::FeasibilityChecker() : void FeasibilityChecker::reset() { - _mission_validity_failed = false; - _takeoff_failed = false; - _land_pattern_validity_failed = false; - _distance_between_waypoints_failed = false; - _fixed_wing_land_approach_failed = false; - _takeoff_land_available_failed = false; + _checks_failed.value = 0; _found_item_with_position = false; _has_vtol_takeoff = false; @@ -155,11 +150,11 @@ bool FeasibilityChecker::processNextItem(mission_item_s &mission_item, const int updateData(); } - if (!_mission_validity_failed) { - _mission_validity_failed = !checkMissionItemValidity(mission_item, current_index); + if (!_checks_failed.flags.mission_validity_failed) { + _checks_failed.flags.mission_validity_failed = !checkMissionItemValidity(mission_item, current_index); } - if (_mission_validity_failed) { + if (_checks_failed.flags.mission_validity_failed) { // if a mission item is not valid then abort the other checks return false; } @@ -177,7 +172,7 @@ bool FeasibilityChecker::processNextItem(mission_item_s &mission_item, const int } if (current_index == total_count - 1) { - _takeoff_land_available_failed = !checkTakeoffLandAvailable(); + _checks_failed.flags.takeoff_land_available_failed = !checkTakeoffLandAvailable(); } _mission_item_previous = mission_item; @@ -188,39 +183,39 @@ bool FeasibilityChecker::processNextItem(mission_item_s &mission_item, const int void FeasibilityChecker::doCommonChecks(mission_item_s &mission_item, const int current_index) { - if (!_distance_between_waypoints_failed) { - _distance_between_waypoints_failed = !checkDistancesBetweenWaypoints(mission_item); + if (!_checks_failed.flags.distance_between_waypoints_failed) { + _checks_failed.flags.distance_between_waypoints_failed = !checkDistancesBetweenWaypoints(mission_item); } if (!_first_waypoint_found) { checkHorizontalDistanceToFirstWaypoint(mission_item); } - if (!_takeoff_failed) { - _takeoff_failed = !checkTakeoff(mission_item); + if (!_checks_failed.flags.takeoff_failed) { + _checks_failed.flags.takeoff_failed = !checkTakeoff(mission_item); } - if (!_items_fit_to_vehicle_type_failed) { - _items_fit_to_vehicle_type_failed = !checkItemsFitToVehicleType(mission_item); + if (!_checks_failed.flags.items_fit_to_vehicle_type_failed) { + _checks_failed.flags.items_fit_to_vehicle_type_failed = !checkItemsFitToVehicleType(mission_item); } } void FeasibilityChecker::doVtolChecks(mission_item_s &mission_item, const int current_index, const int last_index) { - if (!_land_pattern_validity_failed) { - _land_pattern_validity_failed = !checkLandPatternValidity(mission_item, current_index, last_index); + if (!_checks_failed.flags.land_pattern_validity_failed) { + _checks_failed.flags.land_pattern_validity_failed = !checkLandPatternValidity(mission_item, current_index, last_index); } } void FeasibilityChecker::doFixedWingChecks(mission_item_s &mission_item, const int current_index, const int last_index) { - if (!_land_pattern_validity_failed) { - _land_pattern_validity_failed = !checkLandPatternValidity(mission_item, current_index, last_index); + if (!_checks_failed.flags.land_pattern_validity_failed) { + _checks_failed.flags.land_pattern_validity_failed = !checkLandPatternValidity(mission_item, current_index, last_index); } - if (!_fixed_wing_land_approach_failed) { - _fixed_wing_land_approach_failed = !checkFixedWindLandApproach(mission_item, current_index); + if (!_checks_failed.flags.fixed_wing_land_approach_failed) { + _checks_failed.flags.fixed_wing_land_approach_failed = !checkFixedWindLandApproach(mission_item, current_index); } } diff --git a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp index d0905d3635..1c002baf9a 100644 --- a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp +++ b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp @@ -77,12 +77,7 @@ public: */ bool someCheckFailed() { - return _takeoff_failed || - _distance_between_waypoints_failed || - _land_pattern_validity_failed || - _fixed_wing_land_approach_failed || - _mission_validity_failed || - _takeoff_land_available_failed; + return _checks_failed.value != 0; } /** @@ -110,14 +105,18 @@ private: matrix::Vector2d _home_lat_lon = matrix::Vector2d((double)NAN, (double)NAN); VehicleType _vehicle_type{VehicleType::RotaryWing}; - // internal flags to keep track of which checks failed - bool _mission_validity_failed{false}; - bool _takeoff_failed{false}; - bool _land_pattern_validity_failed{false}; - bool _distance_between_waypoints_failed{false}; - bool _fixed_wing_land_approach_failed{false}; - bool _takeoff_land_available_failed{false}; - bool _items_fit_to_vehicle_type_failed{false}; + union checks_failed_u { + struct { + bool mission_validity_failed : 1; + bool takeoff_failed : 1; + bool land_pattern_validity_failed : 1; + bool distance_between_waypoints_failed : 1; + bool fixed_wing_land_approach_failed : 1; + bool takeoff_land_available_failed : 1; + bool items_fit_to_vehicle_type_failed : 1; + } flags; + uint16_t value {0}; + } _checks_failed{}; // internal checkTakeoff related variables bool _found_item_with_position{false}; diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index cbcefcd128..1b05628a6c 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -622,11 +622,11 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi sp->lon = item.lon; sp->alt = get_absolute_altitude_for_item(item); sp->yaw = item.yaw; - sp->loiter_radius = (fabsf(item.loiter_radius) > NAV_EPSILON_POSITION) ? fabsf(item.loiter_radius) : + sp->loiter_radius = (fabsf(item.loiter_radius) > FLT_EPSILON) ? fabsf(item.loiter_radius) : _navigator->get_loiter_radius(); sp->loiter_direction_counter_clockwise = item.loiter_radius < 0; - if (item.acceptance_radius > 0.001f && PX4_ISFINITE(item.acceptance_radius)) { + if (item.acceptance_radius > FLT_EPSILON && PX4_ISFINITE(item.acceptance_radius)) { // if the mission item has a specified acceptance radius, overwrite the default one from parameters sp->acceptance_radius = item.acceptance_radius; @@ -1026,7 +1026,8 @@ void MissionBlock::updateMaxHaglFailsafe() const float target_alt = _navigator->get_position_setpoint_triplet()->current.alt; if (_navigator->get_global_position()->terrain_alt_valid - && ((target_alt - _navigator->get_global_position()->terrain_alt) > _navigator->get_local_position()->hagl_max)) { + && ((target_alt - _navigator->get_global_position()->terrain_alt) + > math::min(_navigator->get_local_position()->hagl_max_z, _navigator->get_local_position()->hagl_max_xy))) { // Handle case where the altitude setpoint is above the maximum HAGL (height above ground level) mavlink_log_info(_navigator->get_mavlink_log_pub(), "Target altitude higher than max HAGL\t"); events::send(events::ID("navigator_fail_max_hagl"), events::Log::Error, "Target altitude higher than max HAGL"); diff --git a/src/modules/navigator/navigation.h b/src/modules/navigator/navigation.h index ecf616ffc4..520430c014 100644 --- a/src/modules/navigator/navigation.h +++ b/src/modules/navigator/navigation.h @@ -56,8 +56,6 @@ # define NUM_MISSIONS_SUPPORTED 500 #endif -#define NAV_EPSILON_POSITION 0.001f /**< Anything smaller than this is considered zero */ - /* compatible to mavlink MAV_CMD */ enum NAV_CMD { NAV_CMD_IDLE = 0, diff --git a/src/modules/navigator/vtol_takeoff.cpp b/src/modules/navigator/vtol_takeoff.cpp index f9a552ef58..503a0279f9 100644 --- a/src/modules/navigator/vtol_takeoff.cpp +++ b/src/modules/navigator/vtol_takeoff.cpp @@ -118,7 +118,7 @@ VtolTakeoff::on_active() _mission_item.time_inside = 1.f; _mission_item.loiter_radius = _navigator->get_loiter_radius(); _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - _mission_item.altitude = _navigator->get_home_position()->alt + _param_loiter_alt.get(); + _mission_item.altitude = _takeoff_alt_msl + _param_loiter_alt.get(); mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); pos_sp_triplet->current.lat = _loiter_location(0); @@ -170,6 +170,8 @@ VtolTakeoff::set_takeoff_position() // set current mission item to takeoff set_takeoff_item(&_mission_item, _transition_alt_amsl); + _takeoff_alt_msl = _navigator->get_global_position()->alt; + _mission_item.lat = _navigator->get_global_position()->lat; _mission_item.lon = _navigator->get_global_position()->lon; diff --git a/src/modules/navigator/vtol_takeoff.h b/src/modules/navigator/vtol_takeoff.h index a353d89ce8..3ba32ce7df 100644 --- a/src/modules/navigator/vtol_takeoff.h +++ b/src/modules/navigator/vtol_takeoff.h @@ -70,6 +70,7 @@ private: } _takeoff_state; float _transition_alt_amsl{0.f}; // absolute altitude at which vehicle will transition to forward flight + float _takeoff_alt_msl{0.f}; matrix::Vector2d _loiter_location; float _loiter_height{0}; diff --git a/src/modules/rc_update/params.c b/src/modules/rc_update/params.c index 7f8a21605c..8ada8e9561 100644 --- a/src/modules/rc_update/params.c +++ b/src/modules/rc_update/params.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-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 @@ -1592,8 +1592,6 @@ PARAM_DEFINE_INT32(RC_MAP_FLTM_BTN, 0); /** * AUX1 Passthrough RC channel * - * Default function: Camera pitch - * * @min 0 * @max 18 * @group Radio Calibration @@ -1622,8 +1620,6 @@ PARAM_DEFINE_INT32(RC_MAP_AUX1, 0); /** * AUX2 Passthrough RC channel * - * Default function: Camera roll - * * @min 0 * @max 18 * @group Radio Calibration @@ -1652,8 +1648,6 @@ PARAM_DEFINE_INT32(RC_MAP_AUX2, 0); /** * AUX3 Passthrough RC channel * - * Default function: Camera azimuth / yaw - * * @min 0 * @max 18 * @group Radio Calibration diff --git a/src/modules/replay/ReplayEkf2.cpp b/src/modules/replay/ReplayEkf2.cpp index 89f4874ce5..e03072b59e 100644 --- a/src/modules/replay/ReplayEkf2.cpp +++ b/src/modules/replay/ReplayEkf2.cpp @@ -39,6 +39,7 @@ // for ekf2 replay #include +#include #include #include #include @@ -91,6 +92,9 @@ ReplayEkf2::onSubscriptionAdded(Subscription &sub, uint16_t msg_id) } else if (sub.orb_meta == ORB_ID(airspeed)) { _airspeed_msg_id = msg_id; + } else if (sub.orb_meta == ORB_ID(airspeed_validated)) { + _airspeed_validated_msg_id = msg_id; + } else if (sub.orb_meta == ORB_ID(distance_sensor)) { _distance_sensor_msg_id = msg_id; @@ -138,6 +142,7 @@ ReplayEkf2::publishEkf2Topics(const ekf2_timestamps_s &ekf2_timestamps, std::ifs }; handle_sensor_publication(ekf2_timestamps.airspeed_timestamp_rel, _airspeed_msg_id); + handle_sensor_publication(ekf2_timestamps.airspeed_validated_timestamp_rel, _airspeed_validated_msg_id); handle_sensor_publication(ekf2_timestamps.distance_sensor_timestamp_rel, _distance_sensor_msg_id); handle_sensor_publication(ekf2_timestamps.optical_flow_timestamp_rel, _optical_flow_msg_id); handle_sensor_publication(ekf2_timestamps.vehicle_air_data_timestamp_rel, _vehicle_air_data_msg_id); @@ -225,6 +230,7 @@ ReplayEkf2::onExitMainLoop() PX4_INFO("Topic, Num Published, Num Error (no timestamp match found):"); print_sensor_statistics(_airspeed_msg_id, "airspeed"); + print_sensor_statistics(_airspeed_validated_msg_id, "airspeed_validated"); print_sensor_statistics(_distance_sensor_msg_id, "distance_sensor"); print_sensor_statistics(_optical_flow_msg_id, "vehicle_optical_flow"); print_sensor_statistics(_sensor_combined_msg_id, "sensor_combined"); diff --git a/src/modules/replay/ReplayEkf2.hpp b/src/modules/replay/ReplayEkf2.hpp index 13cb3d8be5..519b4242d2 100644 --- a/src/modules/replay/ReplayEkf2.hpp +++ b/src/modules/replay/ReplayEkf2.hpp @@ -82,6 +82,7 @@ private: static constexpr uint16_t msg_id_invalid = 0xffff; uint16_t _airspeed_msg_id = msg_id_invalid; + uint16_t _airspeed_validated_msg_id = msg_id_invalid; uint16_t _distance_sensor_msg_id = msg_id_invalid; uint16_t _optical_flow_msg_id = msg_id_invalid; uint16_t _sensor_combined_msg_id = msg_id_invalid; diff --git a/src/modules/sensors/Integrator.hpp b/src/modules/sensors/Integrator.hpp index b6c26f331e..7895bd0c5b 100644 --- a/src/modules/sensors/Integrator.hpp +++ b/src/modules/sensors/Integrator.hpp @@ -55,7 +55,7 @@ public: ~Integrator() = default; static constexpr float DT_MIN{1e-6f}; // 1 microsecond - static constexpr float DT_MAX{static_cast(UINT16_MAX) * 1e-6f}; + static constexpr float DT_MAX{static_cast(UINT32_MAX) * 1e-6f}; /** * Put an item into the integral. @@ -111,7 +111,7 @@ public: * @param integral_dt Get the dt in us of the current integration. * @return true if integral valid */ - bool reset(matrix::Vector3f &integral, uint16_t &integral_dt) + bool reset(matrix::Vector3f &integral, uint32_t &integral_dt) { if (integral_ready()) { integral = _alpha; @@ -200,7 +200,7 @@ public: * @param integral_dt Get the dt in us of the current integration. * @return true if integral valid */ - bool reset(matrix::Vector3f &integral, uint16_t &integral_dt) + bool reset(matrix::Vector3f &integral, uint32_t &integral_dt) { if (Integrator::reset(integral, integral_dt)) { // apply coning corrections diff --git a/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp b/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp index 041da27c09..24996c84da 100644 --- a/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp +++ b/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp @@ -141,6 +141,9 @@ void VehicleAirData::Run() AirTemperatureUpdate(); + estimator_status_flags_s estimator_status_flags; + const bool estimator_status_flags_updated = _estimator_status_flags_sub.update(&estimator_status_flags); + bool updated[MAX_SENSOR_COUNT] {}; for (int uorb_index = 0; uorb_index < MAX_SENSOR_COUNT; uorb_index++) { @@ -196,6 +199,11 @@ void VehicleAirData::Run() ParametersUpdate(true); } + if (estimator_status_flags_updated && _selected_sensor_sub_index >= 0 && _selected_sensor_sub_index == uorb_index + && estimator_status_flags.cs_baro_fault) { + _priority[uorb_index] = 1; // 1 is min priority while still being enabled + } + // pressure corrected with offset (if available) _calibration[uorb_index].SensorCorrectionsUpdate(); const float pressure_corrected = _calibration[uorb_index].Correct(report.pressure); diff --git a/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp b/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp index fd28de34f8..ce7a8a117c 100644 --- a/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp +++ b/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp @@ -54,6 +54,7 @@ #include #include #include +#include using namespace time_literals; @@ -89,6 +90,8 @@ private: uORB::Subscription _differential_pressure_sub{ORB_ID(differential_pressure)}; + uORB::Subscription _estimator_status_flags_sub{ORB_ID(estimator_status_flags)}; + uORB::SubscriptionCallbackWorkItem _sensor_sub[MAX_SENSOR_COUNT] { {this, ORB_ID(sensor_baro), 0}, {this, ORB_ID(sensor_baro), 1}, diff --git a/src/modules/sensors/vehicle_optical_flow/CMakeLists.txt b/src/modules/sensors/vehicle_optical_flow/CMakeLists.txt index ba8761aa00..bf543c0636 100644 --- a/src/modules/sensors/vehicle_optical_flow/CMakeLists.txt +++ b/src/modules/sensors/vehicle_optical_flow/CMakeLists.txt @@ -36,3 +36,7 @@ px4_add_library(vehicle_optical_flow VehicleOpticalFlow.hpp ) target_link_libraries(vehicle_optical_flow PRIVATE px4_work_queue) + +if(BUILD_TESTING) + add_subdirectory(test) +endif() diff --git a/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp b/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp index 334df661bb..f7d573d927 100644 --- a/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp +++ b/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp @@ -161,7 +161,7 @@ void VehicleOpticalFlow::Run() } Vector3f delta_angle{NAN, NAN, NAN}; - uint16_t delta_angle_dt; + uint32_t delta_angle_dt; if (_gyro_integrator.reset(delta_angle, delta_angle_dt)) { _delta_angle += delta_angle; diff --git a/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.hpp b/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.hpp index 5a5ed1663b..dd3022c709 100644 --- a/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.hpp +++ b/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.hpp @@ -73,9 +73,12 @@ public: void PrintStatus(); +protected: + void UpdateDistanceSensor(); + int _distance_sensor_selected{-1}; // because we can have several distance sensor instances with different orientations + private: void ClearAccumulatedData(); - void UpdateDistanceSensor(); void UpdateSensorGyro(); void Run() override; @@ -117,7 +120,6 @@ private: uint16_t _quality_sum{0}; uint8_t _accumulated_count{0}; - int _distance_sensor_selected{-1}; // because we can have several distance sensor instances with different orientations hrt_abstime _last_range_sensor_update{0}; bool _delta_angle_available{false}; diff --git a/src/modules/sensors/vehicle_optical_flow/test/CMakeLists.txt b/src/modules/sensors/vehicle_optical_flow/test/CMakeLists.txt new file mode 100644 index 0000000000..e3d89c05a2 --- /dev/null +++ b/src/modules/sensors/vehicle_optical_flow/test/CMakeLists.txt @@ -0,0 +1,35 @@ +############################################################################ +# +# Copyright (c) 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 +# 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. +# +############################################################################ + +px4_add_functional_gtest(SRC VehicleOpticalFlowTest.cpp LINKLIBS vehicle_optical_flow uORB sensor_calibration) diff --git a/src/modules/sensors/vehicle_optical_flow/test/VehicleOpticalFlowTest.cpp b/src/modules/sensors/vehicle_optical_flow/test/VehicleOpticalFlowTest.cpp new file mode 100644 index 0000000000..a97a6ba1ee --- /dev/null +++ b/src/modules/sensors/vehicle_optical_flow/test/VehicleOpticalFlowTest.cpp @@ -0,0 +1,118 @@ +/**************************************************************************** + * + * Copyright (c) 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 + * 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. + * + ****************************************************************************/ + +/** + * Test for VehicleOpticalFlow + */ + +#include +#include "../VehicleOpticalFlow.hpp" +#include +#include + + +distance_sensor_s createDistanceSensorMessage(uint16_t orientation) +{ + distance_sensor_s message; + message.timestamp = hrt_absolute_time(); + message.min_distance = 1.f; + message.max_distance = 100.f; + message.current_distance = 1.1f; + + message.variance = 0.1f; + message.signal_quality = 100; + message.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER; + message.orientation = orientation; + message.h_fov = math::radians(50.f); + message.v_fov = math::radians(30.f); + return message; + +} + +class VehicleOpticalFlowTest : public ::testing::Test +{ +public: + + class VehicleOpticalFlowTestable : public sensors::VehicleOpticalFlow + { + public: + void UpdateDistanceSensorPublic() + { + VehicleOpticalFlow::UpdateDistanceSensor(); + } + bool IsDistanceSensorSelected() + { + return _distance_sensor_selected >= 0; + + } + }; + + void SetUp() override + { + uORB::Manager::initialize(); + + } + void TearDown() override + { + uORB::Manager::terminate(); + } +}; + + +TEST_F(VehicleOpticalFlowTest, CameraFacingDown) +{ + // GIVEN: message with sensor camera facing down + distance_sensor_s message = createDistanceSensorMessage(distance_sensor_s::ROTATION_DOWNWARD_FACING); + orb_advertise(ORB_ID(distance_sensor), &message); + + // WHEN: update distance sensor + VehicleOpticalFlowTest::VehicleOpticalFlowTestable testable; + testable.UpdateDistanceSensorPublic(); + + // THEN: sensor selected + EXPECT_TRUE(testable.IsDistanceSensorSelected()); +} + +TEST_F(VehicleOpticalFlowTest, CameraFacingForward) +{ + // GIVEN: message with sensor camera facing forward + distance_sensor_s message = createDistanceSensorMessage(distance_sensor_s::ROTATION_FORWARD_FACING); + orb_advertise(ORB_ID(distance_sensor), &message); + + // WHEN: update distance sensor + VehicleOpticalFlowTest::VehicleOpticalFlowTestable testable; + testable.UpdateDistanceSensorPublic(); + + // THEN: sensor is not selected + EXPECT_FALSE(testable.IsDistanceSensorSelected()); +} diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index 2dc1ef13e4..23684e76c1 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.cpp +++ b/src/modules/simulation/gz_bridge/GZBridge.cpp @@ -95,12 +95,6 @@ int GZBridge::init() model_pose_v.push_back(0.0); } - // If model position z is less equal than 0, move above floor to prevent floor glitching - if (model_pose_v[2] <= 0.0) { - PX4_INFO("Model position z is less or equal 0.0, moving upwards"); - model_pose_v[2] = 0.5; - } - gz::msgs::Pose *p = req.mutable_pose(); gz::msgs::Vector3d *position = p->mutable_position(); position->set_x(model_pose_v[0]); @@ -141,7 +135,7 @@ int GZBridge::init() // If Gazebo has not been called, wait 2 seconds and try again. else { - PX4_WARN("Service call timed out as Gazebo has not been detected."); + PX4_WARN("Service call timed out as Gazebo has not been detected. Retrying..."); system_usleep(2000000); } } @@ -159,7 +153,7 @@ int GZBridge::init() while (scene_created == false) { if (!callSceneInfoMsgService(scene_info_service)) { - PX4_WARN("Service call timed out as Gazebo has not been detected."); + PX4_WARN("Service call timed out as Gazebo has not been detected. Retrying..."); system_usleep(2000000); } else { @@ -919,7 +913,7 @@ bool GZBridge::callEntityFactoryService(const std::string &service, const gz::ms } } else { - PX4_ERR("Service call timed out. Check GZ_SIM_RESOURCE_PATH is set correctly."); + PX4_WARN("Service call timed out. Check GZ_SIM_RESOURCE_PATH is set correctly."); return false; } @@ -932,7 +926,7 @@ bool GZBridge::callSceneInfoMsgService(const std::string &service) gz::msgs::Empty req; gz::msgs::Scene rep; - if (_node.Request(service, req, 1000, rep, result)) { + if (_node.Request(service, req, 3000, rep, result)) { if (!result) { PX4_ERR("Scene Info service call failed."); return false; @@ -942,7 +936,7 @@ bool GZBridge::callSceneInfoMsgService(const std::string &service) } } else { - PX4_ERR("Service call timed out. Check GZ_SIM_RESOURCE_PATH is set correctly."); + PX4_WARN("Service call timed out. Check GZ_SIM_RESOURCE_PATH is set correctly."); return false; } @@ -955,7 +949,7 @@ bool GZBridge::callStringMsgService(const std::string &service, const gz::msgs:: gz::msgs::Boolean rep; - if (_node.Request(service, req, 1000, rep, result)) { + if (_node.Request(service, req, 3000, rep, result)) { if (!rep.data() || !result) { PX4_ERR("String service call failed"); return false; @@ -964,7 +958,7 @@ bool GZBridge::callStringMsgService(const std::string &service, const gz::msgs:: } else { - PX4_ERR("Service call timed out: %s", service.c_str()); + PX4_WARN("Service call timed out: %s", service.c_str()); return false; } @@ -977,7 +971,7 @@ bool GZBridge::callVector3dService(const std::string &service, const gz::msgs::V gz::msgs::Boolean rep; - if (_node.Request(service, req, 1000, rep, result)) { + if (_node.Request(service, req, 3000, rep, result)) { if (!rep.data() || !result) { PX4_ERR("String service call failed"); return false; @@ -986,7 +980,7 @@ bool GZBridge::callVector3dService(const std::string &service, const gz::msgs::V } else { - PX4_ERR("Service call timed out: %s", service.c_str()); + PX4_WARN("Service call timed out: %s", service.c_str()); return false; } diff --git a/src/modules/simulation/gz_bridge/GZGimbal.hpp b/src/modules/simulation/gz_bridge/GZGimbal.hpp index 00b46becc4..662268abd2 100644 --- a/src/modules/simulation/gz_bridge/GZGimbal.hpp +++ b/src/modules/simulation/gz_bridge/GZGimbal.hpp @@ -115,7 +115,7 @@ private: const float _yaw_min = NAN; // infinite yaw const float _yaw_max = NAN; // infinite yaw - const uint8_t _gimbal_device_id = 154; // TODO the implementation differs from the protocol + const uint8_t _gimbal_device_id = 1; // Gimbal is implemented by the same component: options are 1..6 uint16_t _gimbal_device_flags = 0; // GIMBAL_DEVICE_FLAGS bool init(const std::string &world_name, const std::string &model_name); diff --git a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp index 58e6ece481..92c5fe73a5 100644 --- a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp +++ b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp @@ -607,7 +607,8 @@ void SimulatorMavlink::handle_message_hil_state_quaternion(const mavlink_message hil_lpos.vxy_max = std::numeric_limits::infinity(); hil_lpos.vz_max = std::numeric_limits::infinity(); hil_lpos.hagl_min = std::numeric_limits::infinity(); - hil_lpos.hagl_max = std::numeric_limits::infinity(); + hil_lpos.hagl_max_z = std::numeric_limits::infinity(); + hil_lpos.hagl_max_xy = std::numeric_limits::infinity(); // always publish ground truth attitude message _lpos_ground_truth_pub.publish(hil_lpos); diff --git a/src/modules/simulation/simulator_sih/CMakeLists.txt b/src/modules/simulation/simulator_sih/CMakeLists.txt index b8d3bf1232..7ae8483c51 100644 --- a/src/modules/simulation/simulator_sih/CMakeLists.txt +++ b/src/modules/simulation/simulator_sih/CMakeLists.txt @@ -53,6 +53,7 @@ if(PX4_PLATFORM MATCHES "posix") airplane quadx xvert + standard_vtol ) # find corresponding airframes diff --git a/src/modules/simulation/simulator_sih/sih.cpp b/src/modules/simulation/simulator_sih/sih.cpp index ea6425f21a..07c24fe642 100644 --- a/src/modules/simulation/simulator_sih/sih.cpp +++ b/src/modules/simulation/simulator_sih/sih.cpp @@ -76,7 +76,7 @@ void Sih::run() _airspeed_time = task_start; _dist_snsr_time = task_start; _vehicle = (VehicleType)constrain(_sih_vtype.get(), static_cast(0), - static_cast(2)); + static_cast(3)); _actuator_out_sub = uORB::Subscription{ORB_ID(actuator_outputs_sim)}; @@ -216,7 +216,8 @@ void Sih::sensor_step() reconstruct_sensors_signals(now); - if ((_vehicle == VehicleType::FW || _vehicle == VehicleType::TS) && now - _airspeed_time >= 50_ms) { + if ((_vehicle == VehicleType::FW || _vehicle == VehicleType::TS || _vehicle == VehicleType::SVTOL) + && now - _airspeed_time >= 50_ms) { _airspeed_time = now; send_airspeed(now); } @@ -302,7 +303,7 @@ void Sih::read_motors(const float dt) if (_actuator_out_sub.update(&actuators_out)) { _last_actuator_output_time = actuators_out.timestamp; - for (int i = 0; i < NB_MOTORS; i++) { // saturate the motor signals + for (int i = 0; i < NUM_ACTUATORS_MAX; i++) { // saturate the motor signals if ((_vehicle == VehicleType::FW && i < 3) || (_vehicle == VehicleType::TS && i > 3)) { _u[i] = actuators_out.output[i]; @@ -328,7 +329,7 @@ void Sih::generate_force_and_torques() _T_B = Vector3f(_T_MAX * _u[3], 0.0f, 0.0f); // forward thruster // _Mt_B = Vector3f(_Q_MAX*_u[3], 0.0f,0.0f); // thruster torque _Mt_B = Vector3f(); - generate_fw_aerodynamics(); + generate_fw_aerodynamics(_u[0], _u[1], _u[2], _u[3]); } else if (_vehicle == VehicleType::TS) { _T_B = Vector3f(0.0f, 0.0f, -_T_MAX * (_u[0] + _u[1])); @@ -337,17 +338,31 @@ void Sih::generate_force_and_torques() // _Fa_E = -_KDV * _v_E; // first order drag to slow down the aircraft // _Ma_B = -_KDW * _w_B; // first order angular damper + + } else if (_vehicle == VehicleType::SVTOL) { + + _T_B = Vector3f(_T_MAX * 2 * _u[7], 0.0f, -_T_MAX * (+_u[0] + _u[1] + _u[2] + _u[3])); + _Mt_B = Vector3f(_L_ROLL * _T_MAX * (-_u[0] + _u[1] + _u[2] - _u[3]), + _L_PITCH * _T_MAX * (+_u[0] - _u[1] + _u[2] - _u[3]), + _Q_MAX * (+_u[0] + _u[1] - _u[2] - _u[3])); + + // thrust 0 because it is already contained in _T_B. in + // equations_of_motion they are all summed into sum_of_forces_E + generate_fw_aerodynamics(_u[4], _u[5], _u[6], 0); } } -void Sih::generate_fw_aerodynamics() +void Sih::generate_fw_aerodynamics(const float roll_cmd, const float pitch_cmd, const float yaw_cmd, + const float throttle_cmd) { const Vector3f v_B = _q_E.rotateVectorInverse(_v_E); const float &alt = _lla.altitude(); - _wing_l.update_aero(v_B, _w_B, alt, -_u[0]*FLAP_MAX); - _wing_r.update_aero(v_B, _w_B, alt, _u[0]*FLAP_MAX); - _tailplane.update_aero(v_B, _w_B, alt, -_u[1]*FLAP_MAX, _T_MAX * _u[3]); - _fin.update_aero(v_B, _w_B, alt, _u[2]*FLAP_MAX, _T_MAX * _u[3]); + + _wing_l.update_aero(v_B, _w_B, alt, roll_cmd * FLAP_MAX); + _wing_r.update_aero(v_B, _w_B, alt, -roll_cmd * FLAP_MAX); + + _tailplane.update_aero(v_B, _w_B, alt, -pitch_cmd * FLAP_MAX, _T_MAX * throttle_cmd); + _fin.update_aero(v_B, _w_B, alt, yaw_cmd * FLAP_MAX, _T_MAX * throttle_cmd); _fuselage.update_aero(v_B, _w_B, alt); // sum of aerodynamic forces @@ -412,7 +427,7 @@ void Sih::equations_of_motion(const float dt) Vector3f ground_force_E; if ((_lla.altitude() - _lpos_ref_alt) < 0.f && force_down > 0.f) { - if (_vehicle == VehicleType::MC || _vehicle == VehicleType::TS) { + if (_vehicle == VehicleType::MC || _vehicle == VehicleType::TS || _vehicle == VehicleType::SVTOL) { ground_force_E = -sum_of_forces_E; if (!_grounded) { @@ -537,6 +552,8 @@ void Sih::send_airspeed(const hrt_abstime &time_now_us) // TODO: send differential pressure instead? airspeed_s airspeed{}; airspeed.timestamp_sample = time_now_us; + + // regardless of vehicle type, body frame, etc this holds as long as wind=0 airspeed.true_airspeed_m_s = fmaxf(0.1f, _v_E.norm() + generate_wgn() * 0.2f); airspeed.indicated_airspeed_m_s = airspeed.true_airspeed_m_s * sqrtf(_wing_l.get_rho() / RHO); airspeed.air_temperature_celsius = NAN; @@ -554,7 +571,7 @@ void Sih::send_dist_snsr(const hrt_abstime &time_now_us) device_id.devid_s.devtype = DRV_DIST_DEVTYPE_SIM; distance_sensor_s distance_sensor{}; - //distance_sensor.timestamp_sample = time_now_us; + // distance_sensor.timestamp_sample = time_now_us; distance_sensor.device_id = device_id.devid; distance_sensor.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER; distance_sensor.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING; @@ -706,6 +723,9 @@ int Sih::print_status() PX4_INFO("aoa [deg]: %d", (int)(degrees(_ts[4].get_aoa()))); PX4_INFO("v segment (m/s)"); _ts[4].get_vS().print(); + + } else if (_vehicle == VehicleType::SVTOL) { + PX4_INFO("Running Standard VTOL"); } PX4_INFO("vehicle landed: %d", _grounded); diff --git a/src/modules/simulation/simulator_sih/sih.hpp b/src/modules/simulation/simulator_sih/sih.hpp index 588d231855..51f6e17dae 100644 --- a/src/modules/simulation/simulator_sih/sih.hpp +++ b/src/modules/simulation/simulator_sih/sih.hpp @@ -134,7 +134,7 @@ private: uORB::Subscription _actuator_out_sub{ORB_ID(actuator_outputs)}; // hard constants - static constexpr uint16_t NB_MOTORS = 6; + static constexpr uint16_t NUM_ACTUATORS_MAX = 9; static constexpr float T1_C = 15.0f; // ground temperature in Celsius static constexpr float T1_K = T1_C - atmosphere::kAbsoluteNullCelsius; // ground temperature in Kelvin static constexpr float TEMP_GRADIENT = -6.5f / 1000.0f; // temperature gradient in degrees per metre @@ -161,7 +161,7 @@ private: void send_airspeed(const hrt_abstime &time_now_us); void send_dist_snsr(const hrt_abstime &time_now_us); void publish_ground_truth(const hrt_abstime &time_now_us); - void generate_fw_aerodynamics(); + void generate_fw_aerodynamics(const float roll_cmd, const float pitch_cmd, const float yaw_cmd, const float thrust); void generate_ts_aerodynamics(); void sensor_step(); static float computeGravity(double lat); @@ -218,9 +218,14 @@ private: matrix::Vector3f _v_E_dot{}; matrix::Dcmf _R_N2E; // local navigation to ECEF frame rotation matrix - float _u[NB_MOTORS] {}; // thruster signals + float _u[NUM_ACTUATORS_MAX] {}; // thruster signals + + // MC = Multicopter + // FW = Fixed Wing + // TS = Tailsitter VTOL + // SVTOL = Standard VTOL + enum class VehicleType {MC, FW, TS, SVTOL}; - enum class VehicleType {MC, FW, TS}; VehicleType _vehicle = VehicleType::MC; // aerodynamic segments for the fixedwing diff --git a/src/modules/simulation/simulator_sih/sih_params.c b/src/modules/simulation/simulator_sih/sih_params.c index 16c58430f6..f9e053a5eb 100644 --- a/src/modules/simulation/simulator_sih/sih_params.c +++ b/src/modules/simulation/simulator_sih/sih_params.c @@ -332,6 +332,7 @@ PARAM_DEFINE_FLOAT(SIH_T_TAU, 0.05f); * @value 0 Multicopter * @value 1 Fixed-Wing * @value 2 Tailsitter + * @value 3 Standard VTOL * @reboot_required true * @group Simulation In Hardware */ diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c index 3bcb56e498..3b8114de3f 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -354,7 +354,7 @@ PARAM_DEFINE_FLOAT(VT_B_DEC_I, 0.1f); * Minimum pitch angle during hover. * * Any pitch setpoint below this value is translated to a forward force by the fixed-wing forward actuation if - * VT_FW_TRHUST_EN is set to 1. + * VT_FWD_TRHUST_EN is set. * * @unit deg * @min -10.0 diff --git a/test/mavsdk_tests/vtol_mission_straight_south.plan b/test/mavsdk_tests/vtol_mission_straight_south.plan index fa6d37472b..1507af13f5 100644 --- a/test/mavsdk_tests/vtol_mission_straight_south.plan +++ b/test/mavsdk_tests/vtol_mission_straight_south.plan @@ -41,7 +41,7 @@ "AltitudeMode": 1, "MISSION_ITEM_ID": "2", "autoContinue": true, - "command": 21, + "command": 85, "doJumpId": 2, "frame": 3, "params": [