diff --git a/.vscode/cmake-variants.yaml b/.vscode/cmake-variants.yaml index 4c2753ba05..03968a6e06 100644 --- a/.vscode/cmake-variants.yaml +++ b/.vscode/cmake-variants.yaml @@ -131,6 +131,11 @@ CONFIG: buildType: MinSizeRel settings: CONFIG: ark_can-flow_canbootloader + ark_can-flow-mr_canbootloader: + short: ark_can-flow-mr_canbootloader + buildType: MinSizeRel + settings: + CONFIG: ark_can-flow-mr_canbootloader ark_can-gps_default: short: ark_can-gps_default buildType: MinSizeRel 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 6a8a3eb6e0..94f2f4d39a 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/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 index c67cf2cc85..5ac9e6b1ab 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/1103_standard_vtol_sih.hil +++ b/ROMFS/px4fmu_common/init.d/airframes/1103_standard_vtol_sih.hil @@ -10,7 +10,7 @@ # @output Motor3 MC motor front left # @output Motor4 MC motor back right # @output Motor5 Forward thrust motor -# @output Servo1 Aileron +# @output Servo1 Ailerons (single channel) # @output Servo2 Elevator # @output Servo3 Rudder # @@ -20,8 +20,7 @@ . ${R}etc/init.d/rc.vtol_defaults param set UAVCAN_ENABLE 0 -param set-default VT_B_TRANS_DUR 5 -param set-default VT_ELEV_MC_LOCK 0 + param set-default VT_TYPE 2 param set-default MPC_MAN_Y_MAX 60 param set-default MC_PITCH_P 5 @@ -40,25 +39,23 @@ 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 0.5 -param set-default CA_SV_CS0_TYPE 2 +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 # rudder -param set HIL_ACT_REV 32 - -param set-default FW_AIRSPD_MAX 12 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 @@ -71,16 +68,12 @@ 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 -# - without safety switch -param set-default CBRK_IO_SAFETY 22027 param set-default SENS_DPRES_OFF 0.001 @@ -93,9 +86,7 @@ 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.145 +param set SIH_L_ROLL 0.2 # sih as standard vtol param set SIH_VEHICLE_TYPE 3 - -param set-default VT_ARSP_TRANS 6 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.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/boards/ark/can-flow-mr/canbootloader.px4board b/boards/ark/can-flow-mr/canbootloader.px4board new file mode 100644 index 0000000000..46917280f6 --- /dev/null +++ b/boards/ark/can-flow-mr/canbootloader.px4board @@ -0,0 +1,5 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m4" +CONFIG_BOARD_ROMFSROOT="" +CONFIG_BOARD_CONSTRAINED_MEMORY=y +CONFIG_DRIVERS_BOOTLOADERS=y diff --git a/boards/ark/can-flow-mr/firmware.prototype b/boards/ark/can-flow-mr/firmware.prototype new file mode 100644 index 0000000000..2f5c9cf0d9 --- /dev/null +++ b/boards/ark/can-flow-mr/firmware.prototype @@ -0,0 +1,13 @@ +{ + "board_id": 88, + "magic": "PX4FWv1", + "description": "Firmware for the ARK Flow MR board", + "image": "", + "build_time": 0, + "summary": "ARKFLOWMR", + "version": "0.1", + "image_size": 0, + "image_maxsize": 2080768, + "git_identity": "", + "board_revision": 0 +} diff --git a/boards/ark/can-flow-mr/init/rc.board_sensors b/boards/ark/can-flow-mr/init/rc.board_sensors new file mode 100644 index 0000000000..2d476fcefb --- /dev/null +++ b/boards/ark/can-flow-mr/init/rc.board_sensors @@ -0,0 +1,20 @@ +#!/bin/sh +# +# board sensors init +#------------------------------------------------------------------------------ + +param set-default IMU_GYRO_RATEMAX 1000 +param set-default SENS_FLOW_RATE 150 +param set-default SENS_IMU_CLPNOTI 0 + +param set-default SENS_AFBR_S_RATE 25 +param set-default SENS_AFBR_L_RATE 10 +param set-default SENS_AFBR_THRESH 8 +param set-default SENS_AFBR_HYSTER 2 + +# Internal SPI +paa3905 -s start -Y 180 + +iim42653 -R 0 -s start + +afbrs50 start diff --git a/boards/ark/can-flow-mr/nuttx-config/canbootloader/defconfig b/boards/ark/can-flow-mr/nuttx-config/canbootloader/defconfig new file mode 100644 index 0000000000..a4540cbe54 --- /dev/null +++ b/boards/ark/can-flow-mr/nuttx-config/canbootloader/defconfig @@ -0,0 +1,57 @@ +# +# This file is autogenerated: PLEASE DO NOT EDIT IT. +# +# You can use "make menuconfig" to make any modifications to the installed .config file. +# You can then do "make savedefconfig" to generate a new defconfig file that includes your +# modifications. +# +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/ark/can-flow-mr/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="stm32" +CONFIG_ARCH_CHIP_STM32=y +CONFIG_ARCH_CHIP_STM32F412CE=y +CONFIG_ARCH_INTERRUPTSTACK=4096 +CONFIG_ARMV7M_MEMCPY=y +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_BINFMT_DISABLE=y +CONFIG_BOARDCTL=y +CONFIG_BOARD_LOOPSPERMSEC=16717 +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEBUG_TCBINFO=y +CONFIG_DEFAULT_SMALL=y +CONFIG_DISABLE_MOUNTPOINT=y +CONFIG_EXPERIMENTAL=y +CONFIG_FDCLONE_DISABLE=y +CONFIG_FDCLONE_STDIO=y +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +CONFIG_IDLETHREAD_STACKSIZE=4096 +CONFIG_INIT_STACKSIZE=4096 +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +CONFIG_LIBC_STRERROR=y +CONFIG_MM_REGIONS=2 +CONFIG_NAME_MAX=0 +CONFIG_NUNGET_CHARS=0 +CONFIG_PREALLOC_TIMERS=0 +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAM_SIZE=262144 +CONFIG_RAM_START=0x20010000 +CONFIG_RAW_BINARY=y +CONFIG_SIG_DEFAULT=y +CONFIG_SIG_SIGALRM_ACTION=y +CONFIG_SIG_SIGUSR1_ACTION=y +CONFIG_SIG_SIGUSR2_ACTION=y +CONFIG_STACK_COLORATION=y +CONFIG_START_DAY=30 +CONFIG_START_MONTH=11 +CONFIG_STDIO_DISABLE_BUFFERING=y +CONFIG_STM32_FLASH_CONFIG_G=y +CONFIG_STM32_NOEXT_VECTORS=y +CONFIG_STM32_TIM8=y +CONFIG_TASK_NAME_SIZE=0 +CONFIG_USEC_PER_TICK=1000 diff --git a/boards/ark/can-flow-mr/nuttx-config/include/board.h b/boards/ark/can-flow-mr/nuttx-config/include/board.h new file mode 100644 index 0000000000..70818cb251 --- /dev/null +++ b/boards/ark/can-flow-mr/nuttx-config/include/board.h @@ -0,0 +1,136 @@ +/************************************************************************************ + * configs/px4fmu/include/board.h + * include/arch/board/board.h + * + * Copyright (C) 2009 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ************************************************************************************/ +#include "board_dma_map.h" + +#ifndef __ARCH_BOARD_BOARD_H +#define __ARCH_BOARD_BOARD_H + +#include +#ifndef __ASSEMBLY__ +# include +#endif + +#include + +/* HSI - 8 MHz RC factory-trimmed + * LSI - 32 KHz RC + * HSE - 8 MHz Crystal + * LSE - not installed + */ +#define STM32_BOARD_USEHSE 1 +#define STM32_BOARD_XTAL 8000000 +#define STM32_HSE_FREQUENCY STM32_BOARD_XTAL + +#define STM32_HSI_FREQUENCY 16000000ul +#define STM32_LSI_FREQUENCY 32000 + +/* Main PLL Configuration */ +#define STM32_PLLCFG_PLLM RCC_PLLCFG_PLLM(8) +#define STM32_PLLCFG_PLLN RCC_PLLCFG_PLLN(384) +#define STM32_PLLCFG_PLLP RCC_PLLCFG_PLLP_4 +#define STM32_PLLCFG_PLLQ RCC_PLLCFG_PLLQ(8) +#define STM32_PLLCFG_PLLR RCC_PLLCFG_PLLR(2) + +#define STM32_RCC_PLLI2SCFGR_PLLI2SM RCC_PLLI2SCFGR_PLLI2SM(16) +#define STM32_RCC_PLLI2SCFGR_PLLI2SN RCC_PLLI2SCFGR_PLLI2SN(192) +#define STM32_RCC_PLLI2SCFGR_PLLI2SQ RCC_PLLI2SCFGR_PLLI2SQ(2) +#define STM32_RCC_PLLI2SCFGR_PLLI2SR RCC_PLLI2SCFGR_PLLI2SR(2) +#define STM32_RCC_PLLI2SCFGR_PLLI2SSRC RCC_PLLI2SCFGR_PLLI2SSRC(0) /* HSE or HSI depending on PLLSRC of PLLCFGR*/ + +#define STM32_RCC_DCKCFGR2_CK48MSEL RCC_DCKCFGR2_CK48MSEL_PLL +#define STM32_RCC_DCKCFGR2_FMPI2C1SEL RCC_DCKCFGR2_FMPI2C1SEL_APB +#define STM32_RCC_DCKCFGR2_SDIOSEL RCC_DCKCFGR2_SDIOSEL_48MHZ + +#define STM32_SYSCLK_FREQUENCY 96000000ul + +/* AHB clock (HCLK) is SYSCLK (96MHz) */ +#define STM32_RCC_CFGR_HPRE RCC_CFGR_HPRE_SYSCLK /* HCLK = SYSCLK / 1 */ +#define STM32_HCLK_FREQUENCY STM32_SYSCLK_FREQUENCY +#define STM32_BOARD_HCLK STM32_HCLK_FREQUENCY /* Same as above, to satisfy compiler */ + +/* APB1 clock (PCLK1) is HCLK/2 (48MHz) */ +#define STM32_RCC_CFGR_PPRE1 RCC_CFGR_PPRE1_HCLKd2 /* PCLK1 = HCLK / 2 */ +#define STM32_PCLK1_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* Timers driven from APB1 will be twice PCLK1 (see page 112 of reference manual) */ +#define STM32_APB1_TIM2_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM3_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM4_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM5_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM12_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM13_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM14_CLKIN (2*STM32_PCLK1_FREQUENCY) + +/* APB2 clock (PCLK2) is HCLK (96MHz) */ +#define STM32_RCC_CFGR_PPRE2 RCC_CFGR_PPRE2_HCLK /* PCLK2 = HCLK */ +#define STM32_PCLK2_FREQUENCY (STM32_HCLK_FREQUENCY) + +/* Timers driven from APB2 will be PCLK2 since no prescale division */ +#define STM32_APB2_TIM1_CLKIN (STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM8_CLKIN (STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM9_CLKIN (STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM10_CLKIN (STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM11_CLKIN (STM32_PCLK2_FREQUENCY) + +/* Timer Frequencies, if APBx is set to 1, frequency is same to APBx otherwise frequency is 2xAPBx. */ +#define BOARD_TIM2_FREQUENCY (2 * STM32_PCLK1_FREQUENCY) +#define BOARD_TIM3_FREQUENCY (2 * STM32_PCLK1_FREQUENCY) +#define BOARD_TIM4_FREQUENCY (2 * STM32_PCLK1_FREQUENCY) +#define BOARD_TIM5_FREQUENCY (2 * STM32_PCLK1_FREQUENCY) +#define BOARD_TIM6_FREQUENCY (2 * STM32_PCLK1_FREQUENCY) +#define BOARD_TIM7_FREQUENCY (2 * STM32_PCLK1_FREQUENCY) +#define BOARD_TIM8_FREQUENCY (2 * STM32_PCLK2_FREQUENCY) + +/* Alternate function pin selections ************************************************/ + +/* UARTs */ +#define GPIO_USART2_RX GPIO_USART2_RX_1 +#define GPIO_USART2_TX GPIO_USART2_TX_1 + +/* CAN */ +#define GPIO_CAN1_RX GPIO_CAN1_RX_1 +#define GPIO_CAN1_TX GPIO_CAN1_TX_1 + +/* SPI */ +#define GPIO_SPI1_MISO GPIO_SPI1_MISO_1 +#define GPIO_SPI1_MOSI GPIO_SPI1_MOSI_1 +#define GPIO_SPI1_SCK GPIO_SPI1_SCK_1 + +#define GPIO_SPI2_MISO GPIO_SPI2_MISO_1 /* PB14 */ +#define GPIO_SPI2_MOSI GPIO_SPI2_MOSI_1 /* PB15 */ +#define GPIO_SPI2_SCK GPIO_SPI2_SCK_1 /* PB10 */ + +#endif /* __ARCH_BOARD_BOARD_H */ diff --git a/boards/ark/can-flow-mr/nuttx-config/include/board_dma_map.h b/boards/ark/can-flow-mr/nuttx-config/include/board_dma_map.h new file mode 100644 index 0000000000..0eb81fc589 --- /dev/null +++ b/boards/ark/can-flow-mr/nuttx-config/include/board_dma_map.h @@ -0,0 +1,44 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +// DMA1 Channel/Stream Selections +//--------------------------------------------//---------------------------//---------------- +#define DMACHAN_SPI2_RX DMAMAP_SPI2_RX // DMA1, Stream 3, Channel 0 +#define DMACHAN_SPI2_TX DMAMAP_SPI2_TX // DMA1, Stream 4, Channel 0 + +// DMA2 Channel/Stream Selections +//--------------------------------------------//---------------------------//---------------- +#define DMACHAN_SPI1_RX DMAMAP_SPI1_RX_2 // DMA2, Stream 2, Channel 3 +#define DMACHAN_SPI1_TX DMAMAP_SPI1_TX_2 // DMA2, Stream 5, Channel 3 diff --git a/boards/ark/can-flow-mr/nuttx-config/nsh/defconfig b/boards/ark/can-flow-mr/nuttx-config/nsh/defconfig new file mode 100644 index 0000000000..ed36c45b2e --- /dev/null +++ b/boards/ark/can-flow-mr/nuttx-config/nsh/defconfig @@ -0,0 +1,149 @@ +# +# This file is autogenerated: PLEASE DO NOT EDIT IT. +# +# You can use "make menuconfig" to make any modifications to the installed .config file. +# You can then do "make savedefconfig" to generate a new defconfig file that includes your +# modifications. +# +# CONFIG_DISABLE_ENVIRON is not set +# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set +# CONFIG_DISABLE_PTHREAD is not set +# CONFIG_NSH_DISABLEBG is not set +# CONFIG_NSH_DISABLESCRIPT is not set +# CONFIG_NSH_DISABLE_CAT is not set +# CONFIG_NSH_DISABLE_CD is not set +# CONFIG_NSH_DISABLE_CP is not set +# CONFIG_NSH_DISABLE_DATE is not set +# CONFIG_NSH_DISABLE_DF is not set +# CONFIG_NSH_DISABLE_ECHO is not set +# CONFIG_NSH_DISABLE_ENV is not set +# CONFIG_NSH_DISABLE_EXEC is not set +# CONFIG_NSH_DISABLE_EXPORT is not set +# CONFIG_NSH_DISABLE_FREE is not set +# CONFIG_NSH_DISABLE_GET is not set +# CONFIG_NSH_DISABLE_HELP is not set +# CONFIG_NSH_DISABLE_ITEF is not set +# CONFIG_NSH_DISABLE_KILL is not set +# CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_LS is not set +# CONFIG_NSH_DISABLE_MKDIR is not set +# CONFIG_NSH_DISABLE_MOUNT is not set +# CONFIG_NSH_DISABLE_MV is not set +# CONFIG_NSH_DISABLE_PS is not set +# CONFIG_NSH_DISABLE_PSSTACKUSAGE is not set +# CONFIG_NSH_DISABLE_PWD is not set +# CONFIG_NSH_DISABLE_RM is not set +# CONFIG_NSH_DISABLE_RMDIR is not set +# CONFIG_NSH_DISABLE_SEMICOLON is not set +# CONFIG_NSH_DISABLE_SET is not set +# CONFIG_NSH_DISABLE_SLEEP is not set +# CONFIG_NSH_DISABLE_SOURCE is not set +# CONFIG_NSH_DISABLE_TEST is not set +# CONFIG_NSH_DISABLE_TIME is not set +# CONFIG_NSH_DISABLE_UMOUNT is not set +# CONFIG_NSH_DISABLE_UNSET is not set +# CONFIG_NSH_DISABLE_USLEEP is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/ark/can-flow-mr/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="stm32" +CONFIG_ARCH_CHIP_STM32=y +CONFIG_ARCH_CHIP_STM32F412CE=y +CONFIG_ARCH_INTERRUPTSTACK=768 +CONFIG_ARCH_STACKDUMP=y +CONFIG_ARMV7M_MEMCPY=y +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_BOARDCTL_RESET=y +CONFIG_BOARD_ASSERT_RESET_VALUE=0 +CONFIG_BOARD_LOOPSPERMSEC=16717 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_BUILTIN=y +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_HARDFAULT_ALERT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEBUG_TCBINFO=y +CONFIG_DEFAULT_SMALL=y +CONFIG_FDCLONE_STDIO=y +CONFIG_FS_CROMFS=y +CONFIG_FS_ROMFS=y +CONFIG_GRAN=y +CONFIG_GRAN_INTR=y +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +CONFIG_IDLETHREAD_STACKSIZE=750 +CONFIG_INIT_ENTRYPOINT="nsh_main" +CONFIG_INIT_STACKSIZE=2624 +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +CONFIG_LIBC_MAX_EXITFUNS=1 +CONFIG_MEMSET_64BIT=y +CONFIG_MEMSET_OPTSPEED=y +CONFIG_MM_REGIONS=2 +CONFIG_NAME_MAX=40 +CONFIG_NSH_ARCHINIT=y +CONFIG_NSH_ARGCAT=y +CONFIG_NSH_BUILTIN_APPS=y +CONFIG_NSH_CMDPARMS=y +CONFIG_NSH_CROMFSETC=y +CONFIG_NSH_LINELEN=128 +CONFIG_NSH_MAXARGUMENTS=15 +CONFIG_NSH_NESTDEPTH=8 +CONFIG_NSH_QUOTE=y +CONFIG_NSH_ROMFSETC=y +CONFIG_NSH_ROMFSSECTSIZE=128 +CONFIG_NSH_VARS=y +CONFIG_PREALLOC_TIMERS=50 +CONFIG_PTHREAD_MUTEX_ROBUST=y +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAM_SIZE=262144 +CONFIG_RAM_START=0x20000000 +CONFIG_RAW_BINARY=y +CONFIG_READLINE_CMD_HISTORY=y +CONFIG_READLINE_TABCOMPLETION=y +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_HPWORKPRIORITY=254 +CONFIG_SCHED_HPWORKSTACKSIZE=3000 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y +CONFIG_SCHED_INSTRUMENTATION_SWITCH=y +CONFIG_SCHED_WAITPID=y +CONFIG_SERIAL_TERMIOS=y +CONFIG_SIG_DEFAULT=y +CONFIG_SIG_SIGALRM_ACTION=y +CONFIG_SIG_SIGUSR1_ACTION=y +CONFIG_SIG_SIGUSR2_ACTION=y +CONFIG_SIG_SIGWORK=4 +CONFIG_STACK_COLORATION=y +CONFIG_START_DAY=30 +CONFIG_START_MONTH=11 +CONFIG_STDIO_BUFFER_SIZE=32 +CONFIG_STM32_ADC1=y +CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG=y +CONFIG_STM32_DMA1=y +CONFIG_STM32_DMA2=y +CONFIG_STM32_FLASH_CONFIG_G=y +CONFIG_STM32_FLASH_PREFETCH=y +CONFIG_STM32_FLOWCONTROL_BROKEN=y +CONFIG_STM32_JTAG_SW_ENABLE=y +CONFIG_STM32_PWR=y +CONFIG_STM32_SERIALBRK_BSDCOMPAT=y +CONFIG_STM32_SERIAL_DISABLE_REORDERING=y +CONFIG_STM32_SPI1=y +CONFIG_STM32_SPI1_DMA=y +CONFIG_STM32_SPI1_DMA_BUFFER=2048 +CONFIG_STM32_SPI2=y +CONFIG_STM32_SPI2_DMA=y +CONFIG_STM32_SPI2_DMA_BUFFER=2048 +CONFIG_STM32_TIM8=y +CONFIG_STM32_USART2=y +CONFIG_STM32_USART_BREAKS=y +CONFIG_STM32_WWDG=y +CONFIG_SYSTEM_NSH=y +CONFIG_TASK_NAME_SIZE=24 +CONFIG_USART2_BAUD=57600 +CONFIG_USART2_RXBUFSIZE=600 +CONFIG_USART2_SERIAL_CONSOLE=y +CONFIG_USART2_TXBUFSIZE=1100 +CONFIG_USEC_PER_TICK=1000 diff --git a/boards/ark/can-flow-mr/nuttx-config/scripts/canbootloader_script.ld b/boards/ark/can-flow-mr/nuttx-config/scripts/canbootloader_script.ld new file mode 100644 index 0000000000..48a59fe92d --- /dev/null +++ b/boards/ark/can-flow-mr/nuttx-config/scripts/canbootloader_script.ld @@ -0,0 +1,134 @@ +/**************************************************************************** + * nuttx-config/scripts/canbootloader_script.ld + * + * Copyright (C) 2015 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* The STM32F412 has 1M of FLASH beginning at address 0x0800:0000 and + * 256Kb of SRAM. SRAM is split up into three blocks: + * + * 1) 112Kb of SRAM beginning at address 0x2000:0000 + * 2) 16Kb of SRAM beginning at address 0x2001:c000 + * 3) 64Kb of SRAM beginning at address 0x2002:0000 + * 4) 64Kb of TCM SRAM beginning at address 0x1000:0000 + * + * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000 + * where the code expects to begin execution by jumping to the entry point in + * the 0x0800:0000 address range. + * + * The first 0x10000 of flash is reserved for the bootloader. + */ + +MEMORY +{ + flash (rx) : ORIGIN = 0x08000000, LENGTH = 32K + sram (rwx) : ORIGIN = 0x20000000, LENGTH = 192K +} + +OUTPUT_ARCH(arm) + +ENTRY(__start) /* treat __start as the anchor for dead code stripping */ +EXTERN(_vectors) /* force the vectors to be included in the output */ + +/* + * Ensure that abort() is present in the final object. The exception handling + * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). + */ +EXTERN(abort) +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + } > flash + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > flash + + .ARM.extab : { + *(.ARM.extab*) + } > flash + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > flash + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + } > sram AT > flash + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + . = ALIGN(4); + _ebss = ABSOLUTE(.); + } > sram + + /* Stabs debugging sections. */ + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } +} diff --git a/boards/ark/can-flow-mr/nuttx-config/scripts/script.ld b/boards/ark/can-flow-mr/nuttx-config/scripts/script.ld new file mode 100644 index 0000000000..2f4769b8f5 --- /dev/null +++ b/boards/ark/can-flow-mr/nuttx-config/scripts/script.ld @@ -0,0 +1,146 @@ +/**************************************************************************** + * scripts/ld.script + * + * Copyright (C) 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* The STM32F412 has 1M of FLASH beginning at address 0x0800:0000 and + * 256Kb of SRAM. SRAM is split up into three blocks: + * + * 1) 112Kb of SRAM beginning at address 0x2000:0000 + * 2) 16Kb of SRAM beginning at address 0x2001:c000 + * 3) 64Kb of SRAM beginning at address 0x2002:0000 + * 4) 64Kb of TCM SRAM beginning at address 0x1000:0000 + * + * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000 + * where the code expects to begin execution by jumping to the entry point in + * the 0x0800:0000 address range. + * + * The first 0x10000 of flash is reserved for the bootloader. + */ + +MEMORY +{ + flash (rx) : ORIGIN = 0x08010000, LENGTH = 928K + sram (rwx) : ORIGIN = 0x20000000, LENGTH = 192K +} + +OUTPUT_ARCH(arm) + +ENTRY(__start) /* treat __start as the anchor for dead code stripping */ +EXTERN(_vectors) /* force the vectors to be included in the output */ + +/* + * Ensure that abort() is present in the final object. The exception handling + * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). + */ +EXTERN(abort) +EXTERN(_bootdelay_signature) + +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + . = ALIGN(8); + /* + * This section positions the app_descriptor_t used + * by the make_can_boot_descriptor.py tool to set + * the application image's descriptor so that the + * uavcan bootloader has the ability to validate the + * image crc, size etc + */ + KEEP(*(.app_descriptor)) + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + } > flash + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > flash + + + .ARM.extab : { + *(.ARM.extab*) + } > flash + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > flash + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + } > sram AT > flash + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + . = ALIGN(4); + _ebss = ABSOLUTE(.); + } > sram + + /* Stabs debugging sections. */ + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } +} diff --git a/boards/ark/can-flow-mr/src/CMakeLists.txt b/boards/ark/can-flow-mr/src/CMakeLists.txt new file mode 100644 index 0000000000..4fae41fc0e --- /dev/null +++ b/boards/ark/can-flow-mr/src/CMakeLists.txt @@ -0,0 +1,65 @@ +############################################################################ +# +# Copyright (c) 2020 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +if("${PX4_BOARD_LABEL}" STREQUAL "canbootloader") + + add_library(drivers_board + boot_config.h + boot.c + led.c + ) + target_link_libraries(drivers_board + PRIVATE + nuttx_arch + nuttx_drivers + canbootloader + ) + target_include_directories(drivers_board PRIVATE ${PX4_SOURCE_DIR}/platforms/nuttx/src/canbootloader) + +else() + add_library(drivers_board + can.c + init.c + led.c + spi.cpp + ) + + target_link_libraries(drivers_board + PRIVATE + arch_spi + drivers__led # drv_led_start + nuttx_arch + nuttx_drivers + px4_layer + ) +endif() diff --git a/boards/ark/can-flow-mr/src/board_config.h b/boards/ark/can-flow-mr/src/board_config.h new file mode 100644 index 0000000000..ad71f05b63 --- /dev/null +++ b/boards/ark/can-flow-mr/src/board_config.h @@ -0,0 +1,95 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file board_config.h + * + * board internal definitions + */ + +#pragma once + +#include +#include +#include + +/* CAN Silent mode control */ +#define GPIO_CAN1_SILENT_S0 /* PB11 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN11) + +/* CAN termination software control */ +#define GPIO_CAN1_TERMINATION /* PB13 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN13) +#define GPIO_CAN_TERM GPIO_CAN1_TERMINATION + +/* Boot config */ +#define GPIO_BOOT_CONFIG /* PC15 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN15|GPIO_EXTI) + +/* LEDs are driven with open drain to support Anode to 5V or 3.3V */ +#define GPIO_TIM1_CH1 /* PA8 */ (GPIO_TIM1_CH1_1|GPIO_OPENDRAIN|GPIO_SPEED_2MHz) +#define GPIO_TIM1_CH2 /* PA9 */ (GPIO_TIM1_CH2_1|GPIO_OPENDRAIN|GPIO_SPEED_2MHz) +#define GPIO_TIM1_CH3 /* PA10 */ (GPIO_TIM1_CH3_1|GPIO_OPENDRAIN|GPIO_SPEED_2MHz) + +#define BROADCOM_AFBR_S50_S2PI_SPI_BUS 2 +#define BROADCOM_AFBR_S50_S2PI_CS /* PB12 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN12) +#define BROADCOM_AFBR_S50_S2PI_IRQ /* PB4 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN4|GPIO_EXTI) +#define BROADCOM_AFBR_S50_S2PI_CLK /* PB10 */ GPIO_SPI2_SCK_1 +#define BROADCOM_AFBR_S50_S2PI_MOSI /* PB15 */ GPIO_SPI2_MOSI_1 +#define BROADCOM_AFBR_S50_S2PI_MISO /* PB14 */ GPIO_SPI2_MISO_1 + +#define FLASH_BASED_PARAMS + +/* High-resolution timer */ +#define HRT_TIMER 3 /* use timer 3 for the HRT */ +#define HRT_TIMER_CHANNEL 4 /* use capture/compare channel 4 */ + +#define PX4_GPIO_INIT_LIST { \ + GPIO_BOOT_CONFIG, \ + GPIO_CAN1_TX, \ + GPIO_CAN1_RX, \ + GPIO_CAN1_SILENT_S0, \ + GPIO_CAN1_TERMINATION, \ + } + +__BEGIN_DECLS + +#define BOARD_HAS_N_S_RGB_LED 1 +#define BOARD_MAX_LEDS BOARD_HAS_N_S_RGB_LED + +#ifndef __ASSEMBLY__ + +extern void stm32_spiinitialize(void); + +#include + +#endif /* __ASSEMBLY__ */ + +__END_DECLS diff --git a/boards/ark/can-flow-mr/src/boot.c b/boards/ark/can-flow-mr/src/boot.c new file mode 100644 index 0000000000..a26034e254 --- /dev/null +++ b/boards/ark/can-flow-mr/src/boot.c @@ -0,0 +1,188 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * Author: Ben Dyer + * Pavel Kirienko + * David Sidrane + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include +#include "boot_config.h" +#include "board.h" + +#include +#include +#include + +#include +#include "led.h" + +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. This entry point + * is called early in the initialization -- after all memory has been configured + * and mapped but before any devices have been initialized. + * + ************************************************************************************/ + +__EXPORT void stm32_boardinitialize(void) +{ + putreg32(getreg32(STM32_RCC_APB1ENR) | RCC_APB1ENR_CAN1EN, STM32_RCC_APB1ENR); + stm32_configgpio(GPIO_CAN1_RX); + stm32_configgpio(GPIO_CAN1_TX); + stm32_configgpio(GPIO_CAN1_SILENT_S0); + stm32_configgpio(GPIO_CAN1_TERMINATION); + putreg32(getreg32(STM32_RCC_APB1RSTR) | RCC_APB1RSTR_CAN1RST, STM32_RCC_APB1RSTR); + putreg32(getreg32(STM32_RCC_APB1RSTR) & ~RCC_APB1RSTR_CAN1RST, STM32_RCC_APB1RSTR); + +#if defined(OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO) + stm32_configgpio(GPIO_GETNODEINFO_JUMPER); +#endif + +} + +/************************************************************************************ + * Name: board_deinitialize + * + * Description: + * This function is called by the bootloader code prior to booting + * the application. Is should place the HW into an benign initialized state. + * + ************************************************************************************/ + +void board_deinitialize(void) +{ + putreg32(getreg32(STM32_RCC_APB1RSTR) | RCC_APB1RSTR_CAN1RST, STM32_RCC_APB1RSTR); +} + +/**************************************************************************** + * Name: board_get_product_name + * + * Description: + * Called to retrieve the product name. The returned value is a assumed + * to be written to a pascal style string that will be length prefixed + * and not null terminated + * + * Input Parameters: + * product_name - A pointer to a buffer to write the name. + * maxlen - The maximum number of charter that can be written + * + * Returned Value: + * The length of characters written to the buffer. + * + ****************************************************************************/ + +uint8_t board_get_product_name(uint8_t *product_name, size_t maxlen) +{ + DEBUGASSERT(maxlen > UAVCAN_STRLEN(HW_UAVCAN_NAME)); + memcpy(product_name, HW_UAVCAN_NAME, UAVCAN_STRLEN(HW_UAVCAN_NAME)); + return UAVCAN_STRLEN(HW_UAVCAN_NAME); +} + +/**************************************************************************** + * Name: board_get_hardware_version + * + * Description: + * Called to retrieve the hardware version information. The function + * will first initialize the the callers struct to all zeros. + * + * Input Parameters: + * hw_version - A pointer to a uavcan_hardwareversion_t. + * + * Returned Value: + * Length of the unique_id + * + ****************************************************************************/ + +size_t board_get_hardware_version(uavcan_HardwareVersion_t *hw_version) +{ + memset(hw_version, 0, sizeof(uavcan_HardwareVersion_t)); + + hw_version->major = HW_VERSION_MAJOR; + hw_version->minor = HW_VERSION_MINOR; + + return board_get_mfguid(*(mfguid_t *) hw_version->unique_id); +} + +/**************************************************************************** + * Name: board_indicate + * + * Description: + * Provides User feedback to indicate the state of the bootloader + * on board specific hardware. + * + * Input Parameters: + * indication - A member of the uiindication_t + * + * Returned Value: + * None + * + ****************************************************************************/ +#define led(n, code, r , g , b, h) {.red = (r),.green = (g), .blue = (b),.hz = (h)} + +typedef begin_packed_struct struct led_t { + uint8_t red; + uint8_t green; + uint8_t blue; + uint8_t hz; +} end_packed_struct led_t; + +static const led_t i2l[] = { + + led(0, off, 0, 0, 0, 0), + led(1, reset, 128, 128, 128, 30), + led(2, autobaud_start, 0, 128, 0, 1), + led(3, autobaud_end, 0, 128, 0, 2), + led(4, allocation_start, 0, 0, 64, 2), + led(5, allocation_end, 0, 128, 64, 3), + led(6, fw_update_start, 32, 128, 64, 3), + led(7, fw_update_erase_fail, 32, 128, 32, 3), + led(8, fw_update_invalid_response, 64, 0, 0, 1), + led(9, fw_update_timeout, 64, 0, 0, 2), + led(a, fw_update_invalid_crc, 64, 0, 0, 4), + led(b, jump_to_app, 0, 128, 0, 10), + +}; + +void board_indicate(uiindication_t indication) +{ + rgb_led(i2l[indication].red, + i2l[indication].green, + i2l[indication].blue, + i2l[indication].hz); +} diff --git a/boards/ark/can-flow-mr/src/boot_config.h b/boards/ark/can-flow-mr/src/boot_config.h new file mode 100644 index 0000000000..76782f9a93 --- /dev/null +++ b/boards/ark/can-flow-mr/src/boot_config.h @@ -0,0 +1,130 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * @file boot_config.h + * + * bootloader definitions that configures the behavior and options + * of the Boot loader + * This file is relies on the parent folder's boot_config.h file and defines + * different usages of the hardware for bootloading + */ + +#pragma once + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +/* Bring in the board_config.h definitions + * todo:make this be pulled in from a targed's build + * files in nuttx*/ + +#include "board_config.h" +#include "uavcan.h" +#include + +#include + +#include + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +#define OPT_PREFERRED_NODE_ID ANY_NODE_ID + +//todo:wrap OPT_x in in ifdefs for command line definitions +#define OPT_TBOOT_MS 3000 +#define OPT_NODE_STATUS_RATE_MS 800 +#define OPT_NODE_INFO_RATE_MS 50 +#define OPT_BL_NUMBER_TIMERS 7 + +/* + * This Option set is set to 1 ensure a provider of firmware has an + * opportunity update the node's firmware. + * This Option is the default policy and can be overridden by + * a jumper + * When this Policy is set, the node will ignore tboot and + * wait indefinitely for a GetNodeInfo request before booting. + * + * OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO_INVERT is used to allow + * the polarity of the jumper to be True Active + * + * wait OPT_WAIT_FOR_GETNODEINFO OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO + * Jumper + * yes 1 0 x + * yes 1 1 Active + * no 1 1 Not Active + * no 0 0 X + * yes 0 1 Active + * no 0 1 Not Active + * + */ +#define OPT_WAIT_FOR_GETNODEINFO 0 +#define OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO 1 +#define OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO_INVERT 1 + +#define OPT_ENABLE_WD 1 + +#define OPT_RESTART_TIMEOUT_MS 20000 + +/* Reserved for the Booloader */ +#define OPT_BOOTLOADER_SIZE_IN_K (1024*64) + +/* Reserved for the application out of the total + * system flash minus the BOOTLOADER_SIZE_IN_K + */ +#define OPT_APPLICATION_RESERVER_IN_K 0 + +#define OPT_APPLICATION_IMAGE_OFFSET OPT_BOOTLOADER_SIZE_IN_K +#define OPT_APPLICATION_IMAGE_LENGTH (FLASH_SIZE-(OPT_BOOTLOADER_SIZE_IN_K+OPT_APPLICATION_RESERVER_IN_K)) + + +#define FLASH_BASE STM32_FLASH_BASE +#define FLASH_SIZE STM32_FLASH_SIZE + +#define APPLICATION_LOAD_ADDRESS (FLASH_BASE + OPT_APPLICATION_IMAGE_OFFSET) +#define APPLICATION_SIZE (FLASH_SIZE-OPT_APPLICATION_IMAGE_OFFSET) +#define APPLICATION_LAST_8BIT_ADDRRESS ((uint8_t *)((APPLICATION_LOAD_ADDRESS+APPLICATION_SIZE)-sizeof(uint8_t))) +#define APPLICATION_LAST_32BIT_ADDRRESS ((uint32_t *)((APPLICATION_LOAD_ADDRESS+APPLICATION_SIZE)-sizeof(uint32_t))) +#define APPLICATION_LAST_64BIT_ADDRRESS ((uint64_t *)((APPLICATION_LOAD_ADDRESS+APPLICATION_SIZE)-sizeof(uint64_t))) + +/* If this board uses big flash that have large sectors */ + +#define OPT_USE_YIELD + +/* Bootloader Option***************************************************************** + * + */ +#define GPIO_GETNODEINFO_JUMPER (GPIO_BOOT_CONFIG & ~GPIO_EXTI) diff --git a/boards/ark/can-flow-mr/src/can.c b/boards/ark/can-flow-mr/src/can.c new file mode 100644 index 0000000000..7737965dc6 --- /dev/null +++ b/boards/ark/can-flow-mr/src/can.c @@ -0,0 +1,130 @@ +/**************************************************************************** + * + * Copyright (C) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file can.c + * + * Board-specific CAN functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include + +#include +#include + +#include "chip.h" +#include "arm_internal.h" + +#include "stm32.h" +#include "stm32_can.h" +#include "board_config.h" + +#ifdef CONFIG_CAN + +/************************************************************************************ + * Pre-processor Definitions + ************************************************************************************/ +/* Configuration ********************************************************************/ + +#if defined(CONFIG_STM32_CAN1) && defined(CONFIG_STM32_CAN2) +# warning "Both CAN1 and CAN2 are enabled. Assuming only CAN1." +# undef CONFIG_STM32_CAN2 +#endif + +#ifdef CONFIG_STM32_CAN1 +# define CAN_PORT 1 +#else +# define CAN_PORT 2 +#endif + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +/************************************************************************************ + * Public Functions + ************************************************************************************/ +int can_devinit(void); + +/************************************************************************************ + * Name: can_devinit + * + * Description: + * All STM32 architectures must provide the following interface to work with + * examples/can. + * + ************************************************************************************/ + +int can_devinit(void) +{ + static bool initialized = false; + struct can_dev_s *can; + int ret; + + /* Check if we have already initialized */ + + if (!initialized) { + /* Call stm32_caninitialize() to get an instance of the CAN interface */ + + can = stm32_caninitialize(CAN_PORT); + + if (can == NULL) { + canerr("ERROR: Failed to get CAN interface\n"); + return -ENODEV; + } + + /* Register the CAN driver at "/dev/can0" */ + + ret = can_register("/dev/can0", can); + + if (ret < 0) { + canerr("ERROR: can_register failed: %d\n", ret); + return ret; + } + + /* Now we are initialized */ + + initialized = true; + } + + return OK; +} + +#endif diff --git a/boards/ark/can-flow-mr/src/init.c b/boards/ark/can-flow-mr/src/init.c new file mode 100644 index 0000000000..a6290bdc7a --- /dev/null +++ b/boards/ark/can-flow-mr/src/init.c @@ -0,0 +1,147 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file init.c + * + * board specific early startup code. This file implements the + * board_app_initialize() function that is called early by nsh during startup. + * + * Code here is run before the rcS script is invoked; it should start required + * subsystems and perform board-specific initialization. + */ + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include "board_config.h" +#include "led.h" +#include + +#include + +#include +#include +#include + +#include + +#include +#include + +# if defined(FLASH_BASED_PARAMS) +# include +#endif + +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. This entry point + * is called early in the initialization -- after all memory has been configured + * and mapped but before any devices have been initialized. + * + ************************************************************************************/ + +__EXPORT void stm32_boardinitialize(void) +{ + watchdog_init(); + + /* configure pins */ + const uint32_t gpio[] = PX4_GPIO_INIT_LIST; + px4_gpio_init(gpio, arraySize(gpio)); + + // Configure SPI all interfaces GPIO & enable power. + stm32_spiinitialize(); +} + +/**************************************************************************** + * Name: board_app_initialize + * + * Description: + * Perform application specific initialization. This function is never + * called directly from application code, but only indirectly via the + * (non-standard) boardctl() interface using the command BOARDIOC_INIT. + * + * Input Parameters: + * arg - The boardctl() argument is passed to the board_app_initialize() + * implementation without modification. The argument has no + * meaning to NuttX; the meaning of the argument is a contract + * between the board-specific initalization logic and the the + * matching application logic. The value cold be such things as a + * mode enumeration value, a set of DIP switch switch settings, a + * pointer to configuration data read from a file or serial FLASH, + * or whatever you would like to do with it. Every implementation + * should accept zero/NULL as a default configuration. + * + * Returned Value: + * Zero (OK) is returned on success; a negated errno value is returned on + * any failure to indicate the nature of the failure. + * + ****************************************************************************/ + +__EXPORT int board_app_initialize(uintptr_t arg) +{ + px4_platform_init(); + +#if defined(FLASH_BASED_PARAMS) + static sector_descriptor_t params_sector_map[] = { + {2, 16 * 1024, 0x08008000}, + {3, 16 * 1024, 0x0800C000}, + {0, 0, 0}, + }; + + /* Initialize the flashfs layer to use heap allocated memory */ + int result = parameter_flashfs_init(params_sector_map, NULL, 0); + + if (result != OK) { + syslog(LOG_ERR, "[boot] FAILED to init params in FLASH %d\n", result); + } + +#endif // FLASH_BASED_PARAMS + + /* Configure the HW based on the manifest */ + //px4_platform_configure(); + + return OK; +} diff --git a/boards/ark/can-flow-mr/src/led.c b/boards/ark/can-flow-mr/src/led.c new file mode 100644 index 0000000000..9a80cae089 --- /dev/null +++ b/boards/ark/can-flow-mr/src/led.c @@ -0,0 +1,124 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file led.c + * + * LED backend. + */ + +#include + +#include + +#include "chip.h" +#include "stm32_gpio.h" +#include "board_config.h" + +#include +#include + +#include "led.h" + +#define TMR_BASE STM32_TIM1_BASE +#define TMR_FREQUENCY STM32_APB2_TIM1_CLKIN +#define TMR_REG(o) (TMR_BASE+(o)) + +void rgb_led(int r, int g, int b, int freqs) +{ + + long fosc = TMR_FREQUENCY; + long prescale = 2048; + long p1s = fosc / prescale; + long p0p5s = p1s / 2; + uint16_t val; + static bool once = 0; + + if (!once) { + once = 1; + + /* Enabel Clock to Block */ + modifyreg32(STM32_RCC_APB2ENR, 0, RCC_APB2ENR_TIM1EN); + + /* Reload */ + + val = getreg16(TMR_REG(STM32_BTIM_EGR_OFFSET)); + val |= ATIM_EGR_UG; + putreg16(val, TMR_REG(STM32_BTIM_EGR_OFFSET)); + + /* Set Prescaler STM32_TIM_SETCLOCK */ + + putreg16(prescale, TMR_REG(STM32_BTIM_PSC_OFFSET)); + + /* Enable STM32_TIM_SETMODE*/ + + putreg16(ATIM_CR1_CEN | ATIM_CR1_ARPE, TMR_REG(STM32_BTIM_CR1_OFFSET)); + + + putreg16((ATIM_CCMR_MODE_PWM1 << ATIM_CCMR1_OC1M_SHIFT) | ATIM_CCMR1_OC1PE | + (ATIM_CCMR_MODE_PWM1 << ATIM_CCMR1_OC2M_SHIFT) | ATIM_CCMR1_OC2PE, TMR_REG(STM32_GTIM_CCMR1_OFFSET)); + putreg16((ATIM_CCMR_MODE_PWM1 << ATIM_CCMR2_OC3M_SHIFT) | ATIM_CCMR2_OC3PE, TMR_REG(STM32_GTIM_CCMR2_OFFSET)); + putreg16(ATIM_CCER_CC3E | ATIM_CCER_CC3P | + ATIM_CCER_CC2E | ATIM_CCER_CC2P | + ATIM_CCER_CC1E | ATIM_CCER_CC1P, TMR_REG(STM32_GTIM_CCER_OFFSET)); + + + stm32_configgpio(GPIO_TIM1_CH1); + stm32_configgpio(GPIO_TIM1_CH2); + stm32_configgpio(GPIO_TIM1_CH3); + + /* master output enable = on */ + putreg16(ATIM_BDTR_MOE, (TMR_REG(STM32_ATIM_BDTR_OFFSET))); + } + + long p = freqs == 0 ? p1s : p1s / freqs; + putreg32(p, TMR_REG(STM32_BTIM_ARR_OFFSET)); + + p = freqs == 0 ? p1s + 1 : p0p5s / freqs; + + putreg32((r * p) / 255, TMR_REG(STM32_GTIM_CCR1_OFFSET)); + putreg32((g * p) / 255, TMR_REG(STM32_GTIM_CCR2_OFFSET)); + putreg32((b * p) / 255, TMR_REG(STM32_GTIM_CCR3_OFFSET)); + + val = getreg16(TMR_REG(STM32_BTIM_CR1_OFFSET)); + + if (freqs == 0) { + val &= ~ATIM_CR1_CEN; + + } else { + val |= ATIM_CR1_CEN; + } + + putreg16(val, TMR_REG(STM32_BTIM_CR1_OFFSET)); + +} diff --git a/boards/ark/can-flow-mr/src/led.h b/boards/ark/can-flow-mr/src/led.h new file mode 100644 index 0000000000..b68e4aa70d --- /dev/null +++ b/boards/ark/can-flow-mr/src/led.h @@ -0,0 +1,37 @@ +/**************************************************************************** + * + * Copyright (C) 2015 PX4 Development Team. All rights reserved. + * Author: David Sidrane + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +__BEGIN_DECLS +void rgb_led(int r, int g, int b, int freqs); +__END_DECLS diff --git a/boards/ark/can-flow-mr/src/spi.cpp b/boards/ark/can-flow-mr/src/spi.cpp new file mode 100644 index 0000000000..697ddbbf1a --- /dev/null +++ b/boards/ark/can-flow-mr/src/spi.cpp @@ -0,0 +1,48 @@ +/**************************************************************************** + * + * Copyright (C) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include +#include + +constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortB, GPIO::Pin0}, SPI::DRDY{GPIO::PortB, GPIO::Pin1}), + initSPIDevice(DRV_FLOW_DEVTYPE_PAA3905, SPI::CS{GPIO::PortB, GPIO::Pin5}, SPI::DRDY{GPIO::PortB, GPIO::Pin2}), + }), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(DRV_DEVTYPE_UNUSED, SPI::CS{GPIO::PortB, GPIO::Pin12}, SPI::DRDY{GPIO::PortB, GPIO::Pin4}), + }), +}; + +static constexpr bool unused = validateSPIConfig(px4_spi_buses); diff --git a/boards/ark/can-flow-mr/uavcan_board_identity b/boards/ark/can-flow-mr/uavcan_board_identity new file mode 100644 index 0000000000..b0ecb3df4f --- /dev/null +++ b/boards/ark/can-flow-mr/uavcan_board_identity @@ -0,0 +1,17 @@ +# UAVCAN boot loadable Module ID +set(uavcanblid_sw_version_major ${PX4_VERSION_MAJOR}) +set(uavcanblid_sw_version_minor ${PX4_VERSION_MINOR}) +add_definitions( + -DAPP_VERSION_MAJOR=${uavcanblid_sw_version_major} + -DAPP_VERSION_MINOR=${uavcanblid_sw_version_minor} +) + +set(uavcanblid_hw_version_major 0) +set(uavcanblid_hw_version_minor 88) +set(uavcanblid_name "\"org.ark.can-flow-mr\"") + +add_definitions( + -DHW_UAVCAN_NAME=${uavcanblid_name} + -DHW_VERSION_MAJOR=${uavcanblid_hw_version_major} + -DHW_VERSION_MINOR=${uavcanblid_hw_version_minor} +) 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 ff3c09f754..a7c34922cf 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 @@ -36,6 +36,7 @@ CONFIG_MODULES_FW_ATT_CONTROL=y CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_FW_POS_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y +CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y CONFIG_MODULES_GYRO_FFT=y CONFIG_MODULES_LAND_DETECTOR=y 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 75e689ed42..82a3261e87 100644 --- a/boards/holybro/kakutef7/default.px4board +++ b/boards/holybro/kakutef7/default.px4board @@ -41,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/micoair/h743-v2/default.px4board b/boards/micoair/h743-v2/default.px4board index fed1a0756b..74f1e2de29 100644 --- a/boards/micoair/h743-v2/default.px4board +++ b/boards/micoair/h743-v2/default.px4board @@ -26,10 +26,10 @@ 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/modalai/fc-v1/default.px4board b/boards/modalai/fc-v1/default.px4board index fbd4d3fdf0..b73fdd18f8 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 @@ -104,4 +105,3 @@ CONFIG_SYSTEMCMDS_USB_CONNECTED=y CONFIG_SYSTEMCMDS_VER=y CONFIG_SYSTEMCMDS_WORK_QUEUE=y CONFIG_EXAMPLES_FAKE_GPS=y -# CONFIG_EKF2_AUX_GLOBAL_POSITION is not set diff --git a/boards/nxp/tropic-community/src/CMakeLists.txt b/boards/nxp/tropic-community/src/CMakeLists.txt index 91d89b5d30..6c37994652 100644 --- a/boards/nxp/tropic-community/src/CMakeLists.txt +++ b/boards/nxp/tropic-community/src/CMakeLists.txt @@ -42,8 +42,12 @@ px4_add_library(drivers_board 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/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 244d6c5fcd..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 * 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 2e2350dd5a..c5255743df 100644 --- a/boards/px4/fmu-v2/default.px4board +++ b/boards/px4/fmu-v2/default.px4board @@ -41,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-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/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/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/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/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp index a7a94c6791..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 @@ -92,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; } @@ -594,7 +599,6 @@ void SF45LaserSerial::sf45_process_replies() 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) { @@ -607,33 +611,10 @@ void SF45LaserSerial::sf45_process_replies() } // 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 float distance_m = raw_distance * SF45_SCALE_FACTOR; @@ -642,7 +623,7 @@ void SF45LaserSerial::sf45_process_replies() 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, + 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()) { @@ -664,6 +645,7 @@ void SF45LaserSerial::sf45_process_replies() 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); @@ -701,48 +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 bfe44f0da2..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 @@ -105,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(); 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/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 5d3ac60b62..ad39a943a7 100644 --- a/src/drivers/uavcan/uavcan_main.cpp +++ b/src/drivers/uavcan/uavcan_main.cpp @@ -1073,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", @@ -1112,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()); @@ -1140,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/collision_prevention/ObstacleMath.cpp b/src/lib/collision_prevention/ObstacleMath.cpp index 22d67c64ae..0605b664ec 100644 --- a/src/lib/collision_prevention/ObstacleMath.cpp +++ b/src/lib/collision_prevention/ObstacleMath.cpp @@ -51,4 +51,62 @@ void project_distance_on_horizontal_plane(float &distance, const float yaw, cons 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 index 3c19eb9110..f5317aeaa7 100644 --- a/src/lib/collision_prevention/ObstacleMath.hpp +++ b/src/lib/collision_prevention/ObstacleMath.hpp @@ -36,6 +36,28 @@ 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 @@ -44,4 +66,26 @@ namespace ObstacleMath */ 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 index e24b95134c..cea54aa4a2 100644 --- a/src/lib/collision_prevention/ObstacleMathTest.cpp +++ b/src/lib/collision_prevention/ObstacleMathTest.cpp @@ -33,6 +33,7 @@ #include #include +#include #include "ObstacleMath.hpp" using namespace matrix; @@ -89,5 +90,152 @@ TEST(ObstacleMathTest, ProjectDistanceOnHorizontalPlane) 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/control_allocation/CMakeLists.txt b/src/lib/control_allocation/control_allocation/CMakeLists.txt index d65bd11fa0..4da638aac8 100644 --- a/src/lib/control_allocation/control_allocation/CMakeLists.txt +++ b/src/lib/control_allocation/control_allocation/CMakeLists.txt @@ -44,4 +44,4 @@ target_include_directories(ControlAllocation PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) target_link_libraries(ControlAllocation PRIVATE mathlib) px4_add_unit_gtest(SRC ControlAllocationPseudoInverseTest.cpp LINKLIBS ControlAllocation) -# px4_add_functional_gtest(SRC ControlAllocationSequentialDesaturationTest.cpp LINKLIBS ControlAllocation ActuatorEffectiveness) +px4_add_functional_gtest(SRC ControlAllocationSequentialDesaturationTest.cpp LINKLIBS ControlAllocation ActuatorEffectiveness) diff --git a/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturationTest.cpp b/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturationTest.cpp index 2e0af6bff4..cd478a1b00 100644 --- a/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturationTest.cpp +++ b/src/lib/control_allocation/control_allocation/ControlAllocationSequentialDesaturationTest.cpp @@ -40,17 +40,27 @@ #include #include -#include 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/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index fef03b92c2..a5e1583175 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -3036,7 +3036,7 @@ The commander module contains the state machine for mode switching and failsafe #ifndef CONSTRAINED_FLASH PRINT_MODULE_USAGE_COMMAND_DESCR("calibrate", "Run sensor calibration"); PRINT_MODULE_USAGE_ARG("mag|baro|accel|gyro|level|esc|airspeed", "Calibration type", false); - PRINT_MODULE_USAGE_ARG("quick", "Quick calibration (accel only, not recommended)", false); + PRINT_MODULE_USAGE_ARG("quick", "Quick calibration [mag, accel (not recommended)]", false); PRINT_MODULE_USAGE_COMMAND_DESCR("check", "Run preflight checks"); PRINT_MODULE_USAGE_COMMAND("arm"); PRINT_MODULE_USAGE_PARAM_FLAG('f', "Force arming (do not run preflight checks)", true); 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/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/sideslip/sideslip_fusion.cpp b/src/modules/ekf2/EKF/aid_sources/sideslip/sideslip_fusion.cpp index b2948629e4..d529e5e994 100644 --- a/src/modules/ekf2/EKF/aid_sources/sideslip/sideslip_fusion.cpp +++ b/src/modules/ekf2/EKF/aid_sources/sideslip/sideslip_fusion.cpp @@ -50,7 +50,7 @@ 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; 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/ekf.h b/src/modules/ekf2/EKF/ekf.h index e4a5e62ef6..47320e8c12 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -602,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/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 4fcafacedd..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, @@ -1932,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; @@ -2080,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/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/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/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 3e91b24619..4b1da7d3a1 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -294,6 +294,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_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 37328503b9..aa8b50d01a 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -3075,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/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 95912a22e3..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; 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/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_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 f90805dd66..07c24fe642 100644 --- a/src/modules/simulation/simulator_sih/sih.cpp +++ b/src/modules/simulation/simulator_sih/sih.cpp @@ -361,7 +361,7 @@ void Sih::generate_fw_aerodynamics(const float roll_cmd, const float pitch_cmd, _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); + _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); diff --git a/src/modules/simulation/simulator_sih/sih.hpp b/src/modules/simulation/simulator_sih/sih.hpp index 490e1dd823..51f6e17dae 100644 --- a/src/modules/simulation/simulator_sih/sih.hpp +++ b/src/modules/simulation/simulator_sih/sih.hpp @@ -220,7 +220,12 @@ private: 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}; + VehicleType _vehicle = VehicleType::MC; // aerodynamic segments for the fixedwing 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": [