FailureDetector: check if ESCs have current

And increase the timeout to 300ms, as some ESCs only update with 10Hz.

.ci/Jenkinsfile-compile

@@ -105,6 +105,7 @@ pipeline {
                      "px4_fmu-v5_stackcheck",
                      "px4_fmu-v5_uavcanv0periph",
                      "px4_fmu-v5x_default",
+                     "px4_fmu-v6c_default",
                      "px4_fmu-v6u_default",
                      "px4_fmu-v6x_default",
                      "px4_io-v2_default",

.ci/Jenkinsfile-hardware

@@ -794,7 +794,7 @@ void resetParameters() {
 void runTests() {
 
   // test loading a range of airframes
-  sh './Tools/HIL/test_airframes.sh `find /dev/serial -name *usb-*` 1000 1001 2100 3000 4001 6001 8001 10016'
+  sh './Tools/HIL/test_airframes.sh `find /dev/serial -name *usb-*` 2100 3000 4001 6001 8001 10016'
 
   resetParameters()
 

.github/ISSUE_TEMPLATE/1_Bug_report.md

@@ -1,32 +1,34 @@
 ---
-name: Bug report
+name: 🐛 Bug report
 about: Create a report to help us improve
+labels: bug-report
 
 ---
 
-**Describe the bug**
+## Describe the bug
 A clear and concise description of the bug.
 
-**To Reproduce**
+## To Reproduce
 Steps to reproduce the behavior:
 1. Drone switched on '...'
 2. Uploaded mission '....' (attach QGC mission file)
 3. Took off '....'
 4. See error
 
-**Expected behavior**
+## Expected behavior
 A clear and concise description of what you expected to happen.
 
-**Log Files and Screenshots**
+## Log Files and Screenshots
 *Always* provide a link to the flight log file:
 - Download the flight log file from the vehicle ([tutorial](https://docs.px4.io/master/en/getting_started/flight_reporting.html)).
-- Share the link to a log showing the problem on [PX4 Flight Review](http://logs.px4.io/).
+- Upload the log to the [PX4 Flight Review](http://logs.px4.io/)
+- Share the link to the log (Copy and paste the URL of the log)
 
 Add screenshots to help explain your problem.
 
-**Drone (please complete the following information):**
+## Drone (please complete the following information):
 - Describe the type of drone.
 - Photo of the IMU / autopilot setup if possible.
 
-**Additional context**
+## Additional context
 Add any other context about the problem here.

.github/ISSUE_TEMPLATE/2_Feature_request.md

@@ -1,19 +1,20 @@
 ---
 name: 🚀 Feature Request
 about: Suggest an idea for this project
+labels: feature-request
 
 ---
 
 For general questions please use [PX4 Discuss](http://discuss.px4.io/) or Slack (you can find an invite link on this project README).
 
-**Describe problem solved by the proposed feature**
+## Describe problem solved by the proposed feature
 A clear and concise description of the problem, if any, this feature will solve. E.g. I'm always frustrated when ...
 
-**Describe your preferred solution**
+## Describe your preferred solution
 A clear and concise description of what you want to happen.
 
-**Describe possible alternatives**
+## Describe possible alternatives
 A clear and concise description of alternative solutions or features you've considered.
 
-**Additional context**
+## Additional context
 Add any other context or screenshots for the feature request here.

.github/ISSUE_TEMPLATE/3_Support_question.md

@@ -1,10 +1,13 @@
 ---
 name: ⛔ Support Question
-about: See [PX4 Discuss](http://discuss.px4.io/) for questions about using PX4.
+about: See http://discuss.px4.io/ for questions about using PX4.
 
 ---
 
-We use GitHub issues only to discuss PX4 bugs and new features. For
-questions about using PX4 or related components, please use [PX4 Discuss](http://discuss.px4.io/).
+## Attention! Please read the note below
+
+We use GitHub issues only to discuss PX4 bugs and new features.
+
+**For questions about using PX4 or related components, please use [PX4 Discuss](http://discuss.px4.io/).**
 
 Thanks!

.github/ISSUE_TEMPLATE/4_Documentation_issue.md

@@ -1,9 +1,11 @@
 ---
 name: ⛔ Documentation Issue
-about: See https://github.com/PX4/Devguide for documentation issues
+about: See https://github.com/PX4/px4_user_guide for documentation issues
 
 ---
 
-PX4 has dedicated repositories for developer documentation (https://github.com/PX4/Devguide) and user documentation (https://github.com/PX4/px4_user_guide).
+## Attention! Please read the note below
+
+**Please submit the documentation issue to the [User Guide](https://github.com/PX4/px4_user_guide) repository.**
 
 Thanks!

.github/PULL_REQUEST_TEMPLATE.md

@@ -1,17 +1,17 @@
 Please use [PX4 Discuss](http://discuss.px4.io/) or [Slack](http://slack.px4.io/) to align on pull requests if necessary. You can then open draft pull requests to get early feedback.
 
-**Describe problem solved by this pull request**
-A clear and concise description of the problem this proposed change will solve.
+## Describe problem solved by this pull request
+A clear and concise description of the problem this proposed change will solve. Or, what it will improve.
 E.g. For this use case I ran into...
 
-**Describe your solution**
+## Describe your solution
 A clear and concise description of what you have implemented.
 
-**Describe possible alternatives**
+## Describe possible alternatives
 A clear and concise description of alternative solutions or features you've considered.
 
-**Test data / coverage**
+## Test data / coverage
 How was it tested? What cases were covered? Logs uploaded to https://review.px4.io/ and screenshots of the important plot parts.
 
-**Additional context**
+## Additional context
 Add any other related context or media.

.github/workflows/compile_nuttx.yml

@@ -61,6 +61,7 @@ jobs:
           px4_fmu-v4pro,
           px4_fmu-v5,
           px4_fmu-v5x,
+          px4_fmu-v6c,
           px4_fmu-v6u,
           px4_fmu-v6x,
           raspberrypi_pico,

Makefile

@@ -325,6 +325,7 @@ px4io_update: px4_io-v2_default cubepilot_io-v2_default
 	cp build/px4_io-v2_default/px4_io-v2_default.bin boards/px4/fmu-v5/extras/px4_io-v2_default.bin
 	cp build/px4_io-v2_default/px4_io-v2_default.bin boards/px4/fmu-v5x/extras/px4_io-v2_default.bin
 	cp build/px4_io-v2_default/px4_io-v2_default.bin boards/px4/fmu-v6x/extras/px4_io-v2_default.bin
+	cp build/px4_io-v2_default/px4_io-v2_default.bin boards/px4/fmu-v6c/extras/px4_io-v2_default.bin
 	# cubepilot_io-v2_default
 	cp build/cubepilot_io-v2_default/cubepilot_io-v2_default.bin boards/cubepilot/cubeorange/extras/cubepilot_io-v2_default.bin
 	cp build/cubepilot_io-v2_default/cubepilot_io-v2_default.bin boards/cubepilot/cubeyellow/extras/cubepilot_io-v2_default.bin

ROMFS/px4fmu_common/init.d/airframes/1000_rc_fw_easystar.hil

@@ -61,6 +61,8 @@ param set-default HIL_ACT_FUNC6 400
 
 param set SYS_HITL 1
 
+param set UAVCAN_ENABLE 0
+
 # disable some checks to allow to fly
 # - with usb
 param set-default CBRK_USB_CHK 197848

ROMFS/px4fmu_common/init.d/airframes/1001_rc_quad_x.hil

@@ -15,6 +15,8 @@ set MIXER quad_x
 
 param set SYS_HITL 1
 
+param set UAVCAN_ENABLE 0
+
 param set-default CA_ROTOR_COUNT 4
 param set-default CA_ROTOR0_PX 0.15
 param set-default CA_ROTOR0_PY 0.15

ROMFS/px4fmu_common/init.d/airframes/1002_standard_vtol.hil

@@ -94,6 +94,8 @@ param set-default HIL_ACT_FUNC8 203
 
 param set SYS_HITL 1
 
+param set UAVCAN_ENABLE 0
+
 # disable some checks to allow to fly
 # - with usb
 param set-default CBRK_USB_CHK 197848

ROMFS/px4fmu_common/init.d/airframes/13013_deltaquad

@@ -131,7 +131,6 @@ param set-default VT_F_TRANS_DUR 1
 param set-default VT_IDLE_PWM_MC 1025
 param set-default VT_B_REV_OUT 0.5
 param set-default VT_B_TRANS_THR 0.7
-param set-default VT_FW_PERM_STAB 1
 param set-default VT_TRANS_TIMEOUT 22
 param set-default VT_F_TRANS_RAMP 4
 

Tools/px_uploader.py

@@ -136,8 +136,11 @@ class firmware(object):
 
         self.image = bytearray(zlib.decompress(base64.b64decode(self.desc['image'])))
 
-        # pad image to 4-byte length
-        while ((len(self.image) % 4) != 0):
+        # PX4 bootloader in theory requires only 4-byte padding,
+        # but a bug exists with the flash block caching where if the data
+        # does not fill up the block, it will not be written (left as all 1s).
+        # So pad up to the flash block size (8 words).
+        while ((len(self.image) % (4 * 8)) != 0):
             self.image.extend(b'\xff')
 
     def property(self, propname):

Tools/setup/ubuntu.sh

@@ -200,13 +200,12 @@ if [[ $INSTALL_SIM == "true" ]]; then
 
 	if [[ "${UBUNTU_RELEASE}" == "18.04" ]]; then
 		java_version=11
-		gazebo_version=9
 	elif [[ "${UBUNTU_RELEASE}" == "20.04" ]]; then
 		java_version=13
-		gazebo_version=11
+	elif [[ "${UBUNTU_RELEASE}" == "22.04" ]]; then
+		java_version=11
 	else
 		java_version=14
-		gazebo_version=11
 	fi
 	# Java (jmavsim or fastrtps)
 	sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends install \
@@ -220,20 +219,30 @@ if [[ $INSTALL_SIM == "true" ]]; then
 	sudo update-alternatives --set java $(update-alternatives --list java | grep "java-$java_version")
 
 	# Gazebo
+	if [[ "${UBUNTU_RELEASE}" == "18.04" ]]; then
+		gazebo_version=9
+		gazebo_packages="gazebo$gazebo_version libgazebo$gazebo_version-dev"
+	elif [[ "${UBUNTU_RELEASE}" == "22.04" ]]; then
+		gazebo_packages="gazebo libgazebo-dev"
+	else
+		# default and Ubuntu 20.04
+		gazebo_version=11
+		gazebo_packages="gazebo$gazebo_version libgazebo$gazebo_version-dev"
+	fi
+
 	sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list'
 	wget http://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add -
 	# Update list, since new gazebo-stable.list has been added
 	sudo apt-get update -y --quiet
 	sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends install \
 		dmidecode \
-		gazebo$gazebo_version \
+		$gazebo_packages \
 		gstreamer1.0-plugins-bad \
 		gstreamer1.0-plugins-base \
 		gstreamer1.0-plugins-good \
 		gstreamer1.0-plugins-ugly \
 		gstreamer1.0-libav \
 		libeigen3-dev \
-		libgazebo$gazebo_version-dev \
 		libgstreamer-plugins-base1.0-dev \
 		libimage-exiftool-perl \
 		libopencv-dev \

Tools/sitl_gazebo

@@ -1 +1 @@
-Subproject commit 2cf56d0bf8a9119cadc1a44d20d641ab24a6a42d
+Subproject commit 5eb5df80450a412076bfc24e7dd343839020f056

boards/matek/h743-mini/default.px4board

@@ -9,14 +9,15 @@ CONFIG_DRIVERS_ADC_BOARD_ADC=y
 CONFIG_DRIVERS_BAROMETER_DPS310=y
 CONFIG_DRIVERS_CAMERA_CAPTURE=n
 CONFIG_DRIVERS_CAMERA_TRIGGER=n
-CONFIG_COMMON_DISTANCE_SENSOR=y
 CONFIG_DRIVERS_DSHOT=y
 CONFIG_DRIVERS_GPS=y
 CONFIG_DRIVERS_IMU_INVENSENSE_MPU6000=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM42605=y
+CONFIG_DRIVERS_MAGNETOMETER_QMC5883L=y
+CONFIG_COMMON_DISTANCE_SENSOR=n
 CONFIG_COMMON_LIGHT=n
-CONFIG_COMMON_MAGNETOMETER=y
+CONFIG_COMMON_MAGNETOMETER=n
 CONFIG_COMMON_OPTICAL_FLOW=y
 CONFIG_DRIVERS_OSD=y
 CONFIG_DRIVERS_PWM_OUT=y

boards/nxp/ucans32k146/src/bringup.c

@@ -159,10 +159,12 @@ int s32k1xx_bringup(void)
 	if (s32k1xx_gpioread(BOARD_REVISION_DETECT_PIN)) {
 		/* STB high -> active CAN phy */
 		s32k1xx_pinconfig(PIN_CAN0_STB  | GPIO_OUTPUT_ONE);
+		s32k1xx_pinconfig(PIN_CAN1_STB  | GPIO_OUTPUT_ONE);
 
 	} else {
 		/* STB low -> active CAN phy */
 		s32k1xx_pinconfig(PIN_CAN0_STB  | GPIO_OUTPUT_ZERO);
+		s32k1xx_pinconfig(PIN_CAN1_STB  | GPIO_OUTPUT_ZERO);
 	}
 
 #endif

boards/px4/fmu-v2/fixedwing.px4board

@@ -9,5 +9,4 @@ CONFIG_DRIVERS_DISTANCE_SENSOR_LIGHTWARE_LASER_I2C=y
 CONFIG_DRIVERS_DISTANCE_SENSOR_LIGHTWARE_LASER_SERIAL=y
 CONFIG_MODULES_AIRSPEED_SELECTOR=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
-CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL_L1=y

boards/px4/fmu-v5x/nuttx-config/nsh/defconfig

@@ -54,16 +54,9 @@ CONFIG_CDCACM_TXBUFSIZE=12000
 CONFIG_CDCACM_VENDORID=0x3185
 CONFIG_CDCACM_VENDORSTR="Auterion"
 CONFIG_CLOCK_MONOTONIC=y
-CONFIG_DEBUG_ASSERTIONS=y
-CONFIG_DEBUG_ERROR=y
-CONFIG_DEBUG_FEATURES=y
 CONFIG_DEBUG_FULLOPT=y
 CONFIG_DEBUG_HARDFAULT_ALERT=y
-CONFIG_DEBUG_SCHED=y
-CONFIG_DEBUG_SCHED_ERROR=y
-CONFIG_DEBUG_SCHED_WARN=y
 CONFIG_DEBUG_SYMBOLS=y
-CONFIG_DEBUG_WARN=y
 CONFIG_DEFAULT_SMALL=y
 CONFIG_DEV_FIFO_SIZE=0
 CONFIG_DEV_GPIO=y

boards/px4/fmu-v6c/bootloader.px4board

@@ -0,0 +1,3 @@
+CONFIG_BOARD_TOOLCHAIN="arm-none-eabi"
+CONFIG_BOARD_ARCHITECTURE="cortex-m7"
+CONFIG_BOARD_ROMFSROOT=""

boards/px4/fmu-v6c/default.px4board

@@ -0,0 +1,91 @@
+CONFIG_BOARD_TOOLCHAIN="arm-none-eabi"
+CONFIG_BOARD_ARCHITECTURE="cortex-m7"
+CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS0"
+CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS6"
+CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS5"
+CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS3"
+CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS1"
+CONFIG_DRIVERS_ADC_BOARD_ADC=y
+CONFIG_COMMON_BAROMETERS=y
+CONFIG_DRIVERS_BATT_SMBUS=y
+CONFIG_DRIVERS_CAMERA_CAPTURE=y
+CONFIG_DRIVERS_CAMERA_TRIGGER=y
+CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
+CONFIG_COMMON_DISTANCE_SENSOR=y
+CONFIG_DRIVERS_DSHOT=y
+CONFIG_DRIVERS_GPS=y
+CONFIG_DRIVERS_HEATER=y
+CONFIG_DRIVERS_IMU_BOSCH_BMI055=y
+CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
+CONFIG_COMMON_LIGHT=y
+CONFIG_COMMON_MAGNETOMETER=y
+CONFIG_COMMON_OPTICAL_FLOW=y
+CONFIG_DRIVERS_POWER_MONITOR_INA226=y
+CONFIG_DRIVERS_POWER_MONITOR_INA228=y
+CONFIG_DRIVERS_POWER_MONITOR_INA238=y
+CONFIG_DRIVERS_PWM_OUT=y
+CONFIG_DRIVERS_PWM_OUT_SIM=y
+CONFIG_DRIVERS_PX4IO=y
+CONFIG_COMMON_TELEMETRY=y
+CONFIG_DRIVERS_TONE_ALARM=y
+CONFIG_DRIVERS_UAVCAN=y
+CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2
+CONFIG_MODULES_AIRSPEED_SELECTOR=y
+CONFIG_MODULES_BATTERY_STATUS=y
+CONFIG_MODULES_CAMERA_FEEDBACK=y
+CONFIG_MODULES_COMMANDER=y
+CONFIG_MODULES_CONTROL_ALLOCATOR=y
+CONFIG_MODULES_DATAMAN=y
+CONFIG_MODULES_EKF2=y
+CONFIG_MODULES_ESC_BATTERY=y
+CONFIG_MODULES_EVENTS=y
+CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
+CONFIG_MODULES_FW_ATT_CONTROL=y
+CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
+CONFIG_MODULES_FW_POS_CONTROL_L1=y
+CONFIG_MODULES_GIMBAL=y
+CONFIG_MODULES_GYRO_CALIBRATION=y
+CONFIG_MODULES_GYRO_FFT=y
+CONFIG_MODULES_LAND_DETECTOR=y
+CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
+CONFIG_MODULES_LOAD_MON=y
+CONFIG_MODULES_LOGGER=y
+CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
+CONFIG_MODULES_MANUAL_CONTROL=y
+CONFIG_MODULES_MAVLINK=y
+CONFIG_MODULES_MC_ATT_CONTROL=y
+CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y
+CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
+CONFIG_MODULES_MC_POS_CONTROL=y
+CONFIG_MODULES_MC_RATE_CONTROL=y
+CONFIG_MODULES_NAVIGATOR=y
+CONFIG_MODULES_RC_UPDATE=y
+CONFIG_MODULES_ROVER_POS_CONTROL=y
+CONFIG_MODULES_SENSORS=y
+CONFIG_MODULES_SIH=y
+CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
+CONFIG_MODULES_VTOL_ATT_CONTROL=y
+CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
+CONFIG_SYSTEMCMDS_BL_UPDATE=y
+CONFIG_SYSTEMCMDS_DMESG=y
+CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
+CONFIG_SYSTEMCMDS_I2CDETECT=y
+CONFIG_SYSTEMCMDS_LED_CONTROL=y
+CONFIG_SYSTEMCMDS_MFT=y
+CONFIG_SYSTEMCMDS_MIXER=y
+CONFIG_SYSTEMCMDS_MOTOR_TEST=y
+CONFIG_SYSTEMCMDS_MTD=y
+CONFIG_SYSTEMCMDS_NSHTERM=y
+CONFIG_SYSTEMCMDS_PARAM=y
+CONFIG_SYSTEMCMDS_PERF=y
+CONFIG_SYSTEMCMDS_PWM=y
+CONFIG_SYSTEMCMDS_REBOOT=y
+CONFIG_SYSTEMCMDS_SD_BENCH=y
+CONFIG_SYSTEMCMDS_SERIAL_TEST=y
+CONFIG_SYSTEMCMDS_SYSTEM_TIME=y
+CONFIG_SYSTEMCMDS_TOP=y
+CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
+CONFIG_SYSTEMCMDS_TUNE_CONTROL=y
+CONFIG_SYSTEMCMDS_UORB=y
+CONFIG_SYSTEMCMDS_VER=y
+CONFIG_SYSTEMCMDS_WORK_QUEUE=y

boards/px4/fmu-v6c/firmware.prototype

@@ -0,0 +1,13 @@
+{
+    "board_id": 56,
+    "magic": "PX4FWv1",
+    "description": "Firmware for the PX4FMUv6C board",
+    "image": "",
+    "build_time": 0,
+    "summary": "PX4FMUv6C",
+    "version": "0.1",
+    "image_size": 0,
+    "image_maxsize": 1966080,
+    "git_identity": "",
+    "board_revision": 0
+}

boards/px4/fmu-v6c/init/rc.board_defaults

@@ -0,0 +1,20 @@
+#!/bin/sh
+#
+# board specific defaults
+#------------------------------------------------------------------------------
+
+
+param set-default BAT1_V_DIV 18.1
+param set-default BAT2_V_DIV 18.1
+
+param set-default BAT1_A_PER_V 36.367515152
+param set-default BAT2_A_PER_V 36.367515152
+
+# Mavlink ethernet (CFG 1000)
+param set-default MAV_2_CONFIG 1000
+param set-default MAV_2_BROADCAST 1
+param set-default MAV_2_MODE 0
+param set-default MAV_2_RADIO_CTL 0
+param set-default MAV_2_RATE 100000
+param set-default MAV_2_REMOTE_PRT 14550
+param set-default MAV_2_UDP_PRT 14550

boards/px4/fmu-v6c/init/rc.board_sensors

@@ -0,0 +1,21 @@
+#!/bin/sh
+#
+# PX4 FMUv6C specific board sensors init
+#------------------------------------------------------------------------------
+board_adc start
+
+# Internal SPI bus BMI055 accel/gyro
+bmi055 -A -R 4 -s start
+bmi055 -G -R 4 -s start
+
+# Internal SPI bus ICM42688p
+icm42688p -R 6 -s start
+
+# Internal barometer on I2C4
+ms5611 -I -b 4 -a 0x77 start
+
+# Internal compass on IMU I2C4
+ist8310 -I -b 4 -a 0xc -R 6 start
+
+# External compass on GPS/I2C1 (the 3rd external bus): standard Holybro Pixhawk 4 or CUAV V5 GPS/compass puck (with lights, safety button, and buzzer)
+ist8310 -X -b 1 -R 10 start

boards/px4/fmu-v6c/nuttx-config/Kconfig

@@ -0,0 +1,17 @@
+#
+# For a description of the syntax of this configuration file,
+# see misc/tools/kconfig-language.txt.
+#
+config BOARD_HAS_PROBES
+	bool "Board provides GPIO or other Hardware for signaling to timing analyze."
+	default y
+	---help---
+		This board provides GPIO FMU-CH1-5, CAP1-6 as PROBE_1-11 to provide timing signals from selected drivers.
+
+config BOARD_USE_PROBES
+	bool "Enable the use the board provided FMU-CH1-5, CAP1-6 as PROBE_1-11"
+	default n
+	depends on BOARD_HAS_PROBES
+
+	---help---
+		Select to use GPIO FMU-CH1-5, CAP1-6  to provide timing signals from selected drivers.

boards/px4/fmu-v6c/nuttx-config/bootloader/defconfig

@@ -0,0 +1,96 @@
+#
+# 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_DEV_CONSOLE is not set
+# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set
+# CONFIG_SPI_EXCHANGE is not set
+# CONFIG_STM32H7_SYSCFG is not set
+CONFIG_ARCH="arm"
+CONFIG_ARCH_BOARD_CUSTOM=y
+CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/px4/fmu-v6c/nuttx-config"
+CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y
+CONFIG_ARCH_BOARD_CUSTOM_NAME="px4"
+CONFIG_ARCH_CHIP="stm32h7"
+CONFIG_ARCH_CHIP_STM32H753II=y
+CONFIG_ARCH_CHIP_STM32H7=y
+CONFIG_ARCH_INTERRUPTSTACK=512
+CONFIG_ARMV7M_BASEPRI_WAR=y
+CONFIG_ARMV7M_ICACHE=y
+CONFIG_ARMV7M_MEMCPY=y
+CONFIG_ARMV7M_USEBASEPRI=y
+CONFIG_BOARDCTL_RESET=y
+CONFIG_BOARD_INITTHREAD_PRIORITY=254
+CONFIG_BOARD_LATE_INITIALIZE=y
+CONFIG_BOARD_LOOPSPERMSEC=95150
+CONFIG_BOARD_RESET_ON_ASSERT=2
+CONFIG_C99_BOOL8=y
+CONFIG_CDCACM=y
+CONFIG_CDCACM_IFLOWCONTROL=y
+CONFIG_CDCACM_PRODUCTID=0x0038
+CONFIG_CDCACM_PRODUCTSTR="PX4 BL FMU v6C.x"
+CONFIG_CDCACM_RXBUFSIZE=600
+CONFIG_CDCACM_TXBUFSIZE=12000
+CONFIG_CDCACM_VENDORID=0x3185
+CONFIG_CDCACM_VENDORSTR="Auterion"
+CONFIG_CLOCK_MONOTONIC=y
+CONFIG_DEBUG_FULLOPT=y
+CONFIG_DEBUG_SYMBOLS=y
+CONFIG_DEFAULT_SMALL=y
+CONFIG_DISABLE_MQUEUE=y
+CONFIG_DISABLE_PTHREAD=y
+CONFIG_EXPERIMENTAL=y
+CONFIG_FDCLONE_DISABLE=y
+CONFIG_FDCLONE_STDIO=y
+CONFIG_HAVE_CXX=y
+CONFIG_HAVE_CXXINITIALIZE=y
+CONFIG_IDLETHREAD_STACKSIZE=750
+CONFIG_LIBC_FLOATINGPOINT=y
+CONFIG_LIBC_LONG_LONG=y
+CONFIG_LIBC_STRERROR=y
+CONFIG_LIB_BOARDCTL=y
+CONFIG_MEMSET_64BIT=y
+CONFIG_MEMSET_OPTSPEED=y
+CONFIG_PREALLOC_TIMERS=50
+CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAM_SIZE=245760
+CONFIG_RAM_START=0x20010000
+CONFIG_RAW_BINARY=y
+CONFIG_SDCLONE_DISABLE=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_SPI=y
+CONFIG_STACK_COLORATION=y
+CONFIG_START_DAY=30
+CONFIG_START_MONTH=11
+CONFIG_STDIO_BUFFER_SIZE=32
+CONFIG_STM32H7_BKPSRAM=y
+CONFIG_STM32H7_DMA1=y
+CONFIG_STM32H7_DMA2=y
+CONFIG_STM32H7_OTGFS=y
+CONFIG_STM32H7_PROGMEM=y
+CONFIG_STM32H7_SERIAL_DISABLE_REORDERING=y
+CONFIG_STM32H7_TIM1=y
+CONFIG_STM32H7_UART7=y
+CONFIG_SYSTEMTICK_HOOK=y
+CONFIG_SYSTEM_CDCACM=y
+CONFIG_TASK_NAME_SIZE=24
+CONFIG_TTY_SIGINT=y
+CONFIG_TTY_SIGINT_CHAR=0x03
+CONFIG_TTY_SIGTSTP=y
+CONFIG_UART7_RXBUFSIZE=512
+CONFIG_UART7_RXDMA=y
+CONFIG_UART7_TXBUFSIZE=512
+CONFIG_UART7_TXDMA=y
+CONFIG_USBDEV=y
+CONFIG_USBDEV_BUSPOWERED=y
+CONFIG_USBDEV_MAXPOWER=500
+CONFIG_USEC_PER_TICK=1000
+CONFIG_USERMAIN_STACKSIZE=2944
+CONFIG_USER_ENTRYPOINT="bootloader_main"

boards/px4/fmu-v6c/nuttx-config/include/board.h

@@ -0,0 +1,505 @@
+/************************************************************************************
+ * nuttx-configs/px4_fmu-v6c/include/board.h
+ *
+ *   Copyright (C) 2016-2019 Gregory Nutt. All rights reserved.
+ *   Authors: David Sidrane <david.sidrane@nscdg.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name 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.
+ *
+ ************************************************************************************/
+#ifndef __NUTTX_CONFIG_PX4_FMU_V6C_INCLUDE_BOARD_H
+#define __NUTTX_CONFIG_PX4_FMU_V6C_INCLUDE_BOARD_H
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include "board_dma_map.h"
+
+#include <nuttx/config.h>
+
+#ifndef __ASSEMBLY__
+# include <stdint.h>
+#endif
+
+#include "stm32_rcc.h"
+#include "stm32_sdmmc.h"
+
+/************************************************************************************
+ * Pre-processor Definitions
+ ************************************************************************************/
+
+/* Clocking *************************************************************************/
+/* The px4_fmu-v6C  board provides the following clock sources:
+ *
+ *   X1: 16 MHz crystal for HSE
+ *
+ * So we have these clock source available within the STM32
+ *
+ *   HSI: 16 MHz RC factory-trimmed
+ *   HSE: 16 MHz crystal for HSE
+ */
+
+#define STM32_BOARD_XTAL        16000000ul
+
+#define STM32_HSI_FREQUENCY     16000000ul
+#define STM32_LSI_FREQUENCY     32000
+#define STM32_HSE_FREQUENCY     STM32_BOARD_XTAL
+#define STM32_LSE_FREQUENCY     32768
+
+/* Main PLL Configuration.
+ *
+ * PLL source is HSE = 16,000,000
+ *
+ * PLL_VCOx = (STM32_HSE_FREQUENCY / PLLM) * PLLN
+ * Subject to:
+ *
+ *     1 <= PLLM <= 63
+ *     4 <= PLLN <= 512
+ *   150 MHz <= PLL_VCOL <= 420MHz
+ *   192 MHz <= PLL_VCOH <= 836MHz
+ *
+ * SYSCLK  = PLL_VCO / PLLP
+ * CPUCLK  = SYSCLK / D1CPRE
+ * Subject to
+ *
+ *   PLLP1   = {2, 4, 6, 8, ..., 128}
+ *   PLLP2,3 = {2, 3, 4, ..., 128}
+ *   CPUCLK <= 480 MHz
+ */
+
+#define STM32_BOARD_USEHSE
+
+#define STM32_PLLCFG_PLLSRC      RCC_PLLCKSELR_PLLSRC_HSE
+
+/* PLL1, wide 4 - 8 MHz input, enable DIVP, DIVQ, DIVR
+ *
+ *   PLL1_VCO = (16,000,000 / 1) * 60 = 960 MHz
+ *
+ *   PLL1P = PLL1_VCO/2  = 960 MHz / 2   = 480 MHz
+ *   PLL1Q = PLL1_VCO/4  = 960 MHz / 4   = 240 MHz
+ *   PLL1R = PLL1_VCO/8  = 960 MHz / 8   = 120 MHz
+ */
+
+#define STM32_PLLCFG_PLL1CFG    (RCC_PLLCFGR_PLL1VCOSEL_WIDE | \
+				 RCC_PLLCFGR_PLL1RGE_4_8_MHZ | \
+				 RCC_PLLCFGR_DIVP1EN | \
+				 RCC_PLLCFGR_DIVQ1EN | \
+				 RCC_PLLCFGR_DIVR1EN)
+#define STM32_PLLCFG_PLL1M       RCC_PLLCKSELR_DIVM1(1)
+#define STM32_PLLCFG_PLL1N       RCC_PLL1DIVR_N1(60)
+#define STM32_PLLCFG_PLL1P       RCC_PLL1DIVR_P1(2)
+#define STM32_PLLCFG_PLL1Q       RCC_PLL1DIVR_Q1(4)
+#define STM32_PLLCFG_PLL1R       RCC_PLL1DIVR_R1(8)
+
+#define STM32_VCO1_FREQUENCY     ((STM32_HSE_FREQUENCY / 1) * 60)
+#define STM32_PLL1P_FREQUENCY    (STM32_VCO1_FREQUENCY / 2)
+#define STM32_PLL1Q_FREQUENCY    (STM32_VCO1_FREQUENCY / 4)
+#define STM32_PLL1R_FREQUENCY    (STM32_VCO1_FREQUENCY / 8)
+
+/* PLL2 */
+
+#define STM32_PLLCFG_PLL2CFG     (RCC_PLLCFGR_PLL2VCOSEL_WIDE | \
+				  RCC_PLLCFGR_PLL2RGE_4_8_MHZ | \
+				  RCC_PLLCFGR_DIVP2EN | \
+				  RCC_PLLCFGR_DIVQ2EN | \
+				  RCC_PLLCFGR_DIVR2EN)
+#define STM32_PLLCFG_PLL2M       RCC_PLLCKSELR_DIVM2(4)
+#define STM32_PLLCFG_PLL2N       RCC_PLL2DIVR_N2(48)
+#define STM32_PLLCFG_PLL2P       RCC_PLL2DIVR_P2(2)
+#define STM32_PLLCFG_PLL2Q       RCC_PLL2DIVR_Q2(2)
+#define STM32_PLLCFG_PLL2R       RCC_PLL2DIVR_R2(2)
+
+#define STM32_VCO2_FREQUENCY     ((STM32_HSE_FREQUENCY / 4) * 48)
+#define STM32_PLL2P_FREQUENCY    (STM32_VCO2_FREQUENCY / 2)
+#define STM32_PLL2Q_FREQUENCY    (STM32_VCO2_FREQUENCY / 2)
+#define STM32_PLL2R_FREQUENCY    (STM32_VCO2_FREQUENCY / 2)
+
+/* PLL3 */
+
+#define STM32_PLLCFG_PLL3CFG    (RCC_PLLCFGR_PLL3VCOSEL_WIDE | \
+				 RCC_PLLCFGR_PLL3RGE_4_8_MHZ | \
+				 RCC_PLLCFGR_DIVQ3EN)
+#define STM32_PLLCFG_PLL3M      RCC_PLLCKSELR_DIVM3(4)
+#define STM32_PLLCFG_PLL3N      RCC_PLL3DIVR_N3(48)
+#define STM32_PLLCFG_PLL3P      RCC_PLL3DIVR_P3(2)
+#define STM32_PLLCFG_PLL3Q      RCC_PLL3DIVR_Q3(4)
+#define STM32_PLLCFG_PLL3R      RCC_PLL3DIVR_R3(2)
+
+#define STM32_VCO3_FREQUENCY    ((STM32_HSE_FREQUENCY / 4) * 48)
+#define STM32_PLL3P_FREQUENCY   (STM32_VCO3_FREQUENCY / 2)
+#define STM32_PLL3Q_FREQUENCY   (STM32_VCO3_FREQUENCY / 4)
+#define STM32_PLL3R_FREQUENCY   (STM32_VCO3_FREQUENCY / 2)
+
+/* SYSCLK = PLL1P = 480MHz
+ * CPUCLK = SYSCLK / 1 = 480 MHz
+ */
+
+#define STM32_RCC_D1CFGR_D1CPRE  (RCC_D1CFGR_D1CPRE_SYSCLK)
+#define STM32_SYSCLK_FREQUENCY   (STM32_PLL1P_FREQUENCY)
+#define STM32_CPUCLK_FREQUENCY   (STM32_SYSCLK_FREQUENCY / 1)
+
+/* Configure Clock Assignments */
+
+/* AHB clock (HCLK) is SYSCLK/2 (240 MHz max)
+ * HCLK1 = HCLK2 = HCLK3 = HCLK4 = 240
+ */
+
+#define STM32_RCC_D1CFGR_HPRE   RCC_D1CFGR_HPRE_SYSCLKd2        /* HCLK  = SYSCLK / 2 */
+#define STM32_ACLK_FREQUENCY    (STM32_CPUCLK_FREQUENCY / 2)    /* ACLK in D1, HCLK3 in D1 */
+#define STM32_HCLK_FREQUENCY    (STM32_CPUCLK_FREQUENCY / 2)    /* HCLK in D2, HCLK4 in D3 */
+#define STM32_BOARD_HCLK        STM32_HCLK_FREQUENCY            /* same as above, to satisfy compiler */
+
+/* APB1 clock (PCLK1) is HCLK/2 (120 MHz) */
+
+#define STM32_RCC_D2CFGR_D2PPRE1  RCC_D2CFGR_D2PPRE1_HCLKd2       /* PCLK1 = HCLK / 2 */
+#define STM32_PCLK1_FREQUENCY     (STM32_HCLK_FREQUENCY/2)
+
+/* APB2 clock (PCLK2) is HCLK/2 (120 MHz) */
+
+#define STM32_RCC_D2CFGR_D2PPRE2  RCC_D2CFGR_D2PPRE2_HCLKd2       /* PCLK2 = HCLK / 2 */
+#define STM32_PCLK2_FREQUENCY     (STM32_HCLK_FREQUENCY/2)
+
+/* APB3 clock (PCLK3) is HCLK/2 (120 MHz) */
+
+#define STM32_RCC_D1CFGR_D1PPRE   RCC_D1CFGR_D1PPRE_HCLKd2        /* PCLK3 = HCLK / 2 */
+#define STM32_PCLK3_FREQUENCY     (STM32_HCLK_FREQUENCY/2)
+
+/* APB4 clock (PCLK4) is HCLK/4 (120 MHz) */
+
+#define STM32_RCC_D3CFGR_D3PPRE   RCC_D3CFGR_D3PPRE_HCLKd2       /* PCLK4 = HCLK / 2 */
+#define STM32_PCLK4_FREQUENCY     (STM32_HCLK_FREQUENCY/2)
+
+/* Timer clock frequencies */
+
+/* Timers driven from APB1 will be twice PCLK1 */
+
+#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_TIM6_CLKIN   (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM7_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)
+
+/* Timers driven from APB2 will be twice PCLK2 */
+
+#define STM32_APB2_TIM1_CLKIN   (2*STM32_PCLK2_FREQUENCY)
+#define STM32_APB2_TIM8_CLKIN   (2*STM32_PCLK2_FREQUENCY)
+#define STM32_APB2_TIM15_CLKIN  (2*STM32_PCLK2_FREQUENCY)
+#define STM32_APB2_TIM16_CLKIN  (2*STM32_PCLK2_FREQUENCY)
+#define STM32_APB2_TIM17_CLKIN  (2*STM32_PCLK2_FREQUENCY)
+
+/* Kernel Clock Configuration
+ *
+ * Note: look at Table 54 in ST Manual
+ */
+
+/* I2C123 clock source */
+
+#define STM32_RCC_D2CCIP2R_I2C123SRC RCC_D2CCIP2R_I2C123SEL_HSI
+
+/* I2C4 clock source */
+
+#define STM32_RCC_D3CCIPR_I2C4SRC    RCC_D3CCIPR_I2C4SEL_HSI
+
+/* SPI123 clock source */
+
+#define STM32_RCC_D2CCIP1R_SPI123SRC RCC_D2CCIP1R_SPI123SEL_PLL2
+
+/* SPI45 clock source */
+
+#define STM32_RCC_D2CCIP1R_SPI45SRC  RCC_D2CCIP1R_SPI45SEL_PLL2
+
+/* SPI6 clock source */
+
+#define STM32_RCC_D3CCIPR_SPI6SRC    RCC_D3CCIPR_SPI6SEL_PLL2
+
+/* USB 1 and 2 clock source */
+
+#define STM32_RCC_D2CCIP2R_USBSRC    RCC_D2CCIP2R_USBSEL_PLL3
+
+/* ADC 1 2 3 clock source */
+
+#define STM32_RCC_D3CCIPR_ADCSEL     RCC_D3CCIPR_ADCSEL_PLL2
+
+/* FDCAN 1 2 clock source */
+
+#define STM32_RCC_D2CCIP1R_FDCANSEL  RCC_D2CCIP1R_FDCANSEL_HSE   /* FDCAN 1 2 clock source */
+
+#define STM32_FDCANCLK               STM32_HSE_FREQUENCY
+
+/* FLASH wait states
+ *
+ *  ------------ ---------- -----------
+ *  Vcore        MAX ACLK   WAIT STATES
+ *  ------------ ---------- -----------
+ *  1.15-1.26 V     70 MHz    0
+ *  (VOS1 level)   140 MHz    1
+ *                 210 MHz    2
+ *  1.05-1.15 V     55 MHz    0
+ *  (VOS2 level)   110 MHz    1
+ *                 165 MHz    2
+ *                 220 MHz    3
+ *  0.95-1.05 V     45 MHz    0
+ *  (VOS3 level)    90 MHz    1
+ *                 135 MHz    2
+ *                 180 MHz    3
+ *                 225 MHz    4
+ *  ------------ ---------- -----------
+ */
+
+#define BOARD_FLASH_WAITSTATES 2
+
+/* SDMMC definitions ********************************************************/
+
+/* Init 400kHz, freq = PLL1Q/(2*div)  div =  PLL1Q/(2*freq) */
+
+#define STM32_SDMMC_INIT_CLKDIV     (300 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT)
+
+/* 25 MHz Max for now, 25 mHZ = PLL1Q/(2*div), div =  PLL1Q/(2*freq)
+ * div = 4.8 = 240 / 50, So round up to 5 for default speed 24 MB/s
+ */
+
+#if defined(CONFIG_STM32H7_SDMMC_XDMA) || defined(CONFIG_STM32H7_SDMMC_IDMA)
+#  define STM32_SDMMC_MMCXFR_CLKDIV   (5 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT)
+#else
+#  define STM32_SDMMC_MMCXFR_CLKDIV   (100 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT)
+#endif
+#if defined(CONFIG_STM32H7_SDMMC_XDMA) || defined(CONFIG_STM32H7_SDMMC_IDMA)
+#  define STM32_SDMMC_SDXFR_CLKDIV    (5 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT)
+#else
+#  define STM32_SDMMC_SDXFR_CLKDIV    (100 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT)
+#endif
+
+#define STM32_SDMMC_CLKCR_EDGE      STM32_SDMMC_CLKCR_NEGEDGE
+
+/* LED definitions ******************************************************************/
+/* The PX4 FMUV6C board has three, LED_GREEN a Green LED, LED_BLUE a Blue LED and
+ * LED_RED a Red LED, that can be controlled by software.
+ *
+ * If CONFIG_ARCH_LEDS is not defined, then the user can control the LEDs in any way.
+ * The following definitions are used to access individual LEDs.
+ */
+
+/* LED index values for use with board_userled() */
+
+/* LED definitions ******************************************************************/
+/* The px4_fmu-v6c board has three, LED_GREEN a Green LED, LED_BLUE a Blue LED and
+ * LED_RED a Red LED, that can be controlled by software.
+ *
+ * If CONFIG_ARCH_LEDS is not defined, then the user can control the LEDs in any way.
+ * The following definitions are used to access individual LEDs.
+ */
+
+/* LED index values for use with board_userled() */
+
+#define BOARD_LED1        0
+#define BOARD_LED2        1
+#define BOARD_LED3        2
+#define BOARD_NLEDS       3
+
+#define BOARD_LED_RED     BOARD_LED1
+#define BOARD_LED_GREEN   BOARD_LED2
+#define BOARD_LED_BLUE    BOARD_LED3
+
+/* LED bits for use with board_userled_all() */
+
+#define BOARD_LED1_BIT    (1 << BOARD_LED1)
+#define BOARD_LED2_BIT    (1 << BOARD_LED2)
+#define BOARD_LED3_BIT    (1 << BOARD_LED3)
+
+/* If CONFIG_ARCH_LEDS is defined, the usage by the board port is defined in
+ * include/board.h and src/stm32_leds.c. The LEDs are used to encode OS-related
+ * events as follows:
+ *
+ *
+ *   SYMBOL                     Meaning                      LED state
+ *                                                        Red   Green Blue
+ *   ----------------------  --------------------------  ------ ------ ----*/
+
+#define LED_STARTED        0 /* NuttX has been started   OFF    OFF   OFF  */
+#define LED_HEAPALLOCATE   1 /* Heap has been allocated  OFF    OFF   ON   */
+#define LED_IRQSENABLED    2 /* Interrupts enabled       OFF    ON    OFF  */
+#define LED_STACKCREATED   3 /* Idle stack created       OFF    ON    ON   */
+#define LED_INIRQ          4 /* In an interrupt          N/C    N/C   GLOW */
+#define LED_SIGNAL         5 /* In a signal handler      N/C    GLOW  N/C  */
+#define LED_ASSERTION      6 /* An assertion failed      GLOW   N/C   GLOW */
+#define LED_PANIC          7 /* The system has crashed   Blink  OFF   N/C  */
+#define LED_IDLE           8 /* MCU is is sleep mode     ON     OFF   OFF  */
+
+/* Thus if the Green LED is statically on, NuttX has successfully booted and
+ * is, apparently, running normally.  If the Red LED is flashing at
+ * approximately 2Hz, then a fatal error has been detected and the system
+ * has halted.
+ */
+
+/* Alternate function pin selections ************************************************/
+
+#define GPIO_USART1_RX   GPIO_USART1_RX_2    /* PA10 */
+#define GPIO_USART1_TX   GPIO_USART1_TX_3    /* PB6 */
+
+#define GPIO_USART2_RX   GPIO_USART2_RX_1   /* PA3   */
+#define GPIO_USART2_TX   GPIO_USART2_TX_2   /* PD5   */
+
+#define GPIO_USART3_RX   GPIO_USART3_RX_3   /* PD9   */
+#define GPIO_USART3_TX   GPIO_USART3_TX_3   /* PD8   */
+
+#define GPIO_UART5_RX    GPIO_UART5_RX_3    /* PD2  */
+#define GPIO_UART5_TX    GPIO_UART5_TX_3    /* PC12 */
+// GPIO_UART5_RTS   no remap                /* PC8  */
+// GPIO_UART5_CTS  No remap                 /* PC9  */
+
+#define GPIO_USART6_RX   GPIO_USART6_RX_1   /* PC7 */
+#define GPIO_USART6_TX   GPIO_USART6_TX_1   /* PC6  */
+
+#define GPIO_UART7_RX    GPIO_UART7_RX_3    /* PE7  */
+#define GPIO_UART7_TX    GPIO_UART7_TX_3    /* PE8  */
+#define GPIO_UART7_RTS   GPIO_UART7_RTS_1   /* PE9  */
+#define GPIO_UART7_CTS   GPIO_UART7_CTS_1   /* PE10 */
+
+#define GPIO_UART8_RX    GPIO_UART8_RX_1    /* PE0 */
+#define GPIO_UART8_TX    GPIO_UART8_TX_1    /* PE1 */
+
+
+/* CAN
+ *
+ * CAN1 is routed to transceiver.
+ * CAN2 is routed to transceiver.
+ */
+#define GPIO_CAN1_RX     GPIO_CAN1_RX_3     /* PD0  */
+#define GPIO_CAN1_TX     GPIO_CAN1_TX_3     /* PD1  */
+#define GPIO_CAN2_RX     GPIO_CAN2_RX_2     /* PB5 */
+#define GPIO_CAN2_TX     GPIO_CAN2_TX_1     /* PB13  */
+
+/* SPI
+ * SPI1 is sensors
+ * SPI2 is FRAM
+ *
+ */
+
+#define ADJ_SLEW_RATE(p) (((p) & ~GPIO_SPEED_MASK) | (GPIO_SPEED_2MHz))
+
+#define GPIO_SPI1_MISO   GPIO_SPI1_MISO_1               /* PA6  */
+#define GPIO_SPI1_MOSI   GPIO_SPI1_MOSI_1               /* PA7  */
+#define GPIO_SPI1_SCK    ADJ_SLEW_RATE(GPIO_SPI1_SCK_1) /* PA5  */
+
+#define GPIO_SPI2_MISO   GPIO_SPI2_MISO_2               /* PC2  */
+#define GPIO_SPI2_MOSI   GPIO_SPI2_MOSI_3               /* PC3  */
+#define GPIO_SPI2_SCK    ADJ_SLEW_RATE(GPIO_SPI2_SCK_5) /* PD3  */
+
+/* I2C
+ *
+ *   The optional _GPIO configurations allow the I2C driver to manually
+ *   reset the bus to clear stuck slaves.  They match the pin configuration,
+ *   but are normally-high GPIOs.
+ *
+ */
+
+#define GPIO_I2C1_SCL GPIO_I2C1_SCL_2       /* PB8  */
+#define GPIO_I2C1_SDA GPIO_I2C1_SDA_1       /* PB7  */
+
+#define GPIO_I2C1_SCL_GPIO                  (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTB | GPIO_PIN8)
+#define GPIO_I2C1_SDA_GPIO                  (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTB | GPIO_PIN7)
+
+#define GPIO_I2C2_SCL GPIO_I2C2_SCL_1       /* PB10 */
+#define GPIO_I2C2_SDA GPIO_I2C2_SDA_1       /* PB11*/
+
+#define GPIO_I2C2_SCL_GPIO                  (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTB | GPIO_PIN10)
+#define GPIO_I2C2_SDA_GPIO                  (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTB | GPIO_PIN11)
+
+#define GPIO_I2C4_SCL GPIO_I2C4_SCL_1       /* PD12 */
+#define GPIO_I2C4_SDA GPIO_I2C4_SDA_1       /* PD13 */
+
+#define GPIO_I2C4_SCL_GPIO                  (GPIO_OUTPUT | GPIO_OPENDRAIN | GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTD | GPIO_PIN12)
+#define GPIO_I2C4_SDA_GPIO                  (GPIO_OUTPUT | GPIO_OPENDRAIN | GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTD | GPIO_PIN13)
+
+/* SDMMC2
+ *
+ *      VDD 3.3
+ *      GND
+ *      SDMMC2_CK                           PD6
+ *      SDMMC2_CMD                          PD7
+ *      SDMMC2_D0                           PB14
+ *      SDMMC2_D1                           PB15
+ *      SDMMC2_D2                           PB3
+ *      SDMMC2_D3                           PB4
+ */
+
+#define GPIO_SDMMC2_CK   GPIO_SDMMC2_CK_1  /* PD6  */
+#define GPIO_SDMMC2_CMD  GPIO_SDMMC2_CMD_1 /* PD7  */
+//      GPIO_SDMMC2_D0   No Remap          /* PB14 */
+//      GPIO_SDMMC2_D1   No Remap          /* PB15 */
+#define GPIO_SDMMC2_D2   GPIO_SDMMC2_D2_2  /* PB3  */
+//      GPIO_SDMMC2_D3    No Remap         /* PB4  */
+
+/* USB
+ *
+ *      OTG_FS_DM                           PA11
+ *      OTG_FS_DP                           PA12
+ *      VBUS                                PA9
+ */
+
+
+/* Board provides GPIO or other Hardware for signaling to timing analyzer */
+
+#if defined(CONFIG_BOARD_USE_PROBES)
+# include "stm32_gpio.h"
+# define PROBE_N(n) (1<<((n)-1))
+# define PROBE_1    (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN8)   /* PA8  AUX1 */
+# define PROBE_2    (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN11)  /* PE11 AUX2 */
+# define PROBE_3    (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN13)  /* PE13 AUX3 */
+# define PROBE_4    (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN14)  /* PE14 AUX4 */
+# define PROBE_5    (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN14)  /* PD14 AUX5 */
+# define PROBE_6    (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN15)  /* PD15 AUX6 */
+# define PROBE_7    (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN0)   /* PA0  AUX7 */
+# define PROBE_8    (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN2)   /* PA1  AUX8 */
+
+# define PROBE_INIT(mask) \
+	do { \
+		if ((mask)& PROBE_N(1)) { stm32_configgpio(PROBE_1); } \
+		if ((mask)& PROBE_N(2)) { stm32_configgpio(PROBE_2); } \
+		if ((mask)& PROBE_N(3)) { stm32_configgpio(PROBE_3); } \
+		if ((mask)& PROBE_N(4)) { stm32_configgpio(PROBE_4); } \
+		if ((mask)& PROBE_N(5)) { stm32_configgpio(PROBE_5); } \
+		if ((mask)& PROBE_N(6)) { stm32_configgpio(PROBE_6); } \
+		if ((mask)& PROBE_N(7)) { stm32_configgpio(PROBE_7); } \
+		if ((mask)& PROBE_N(8)) { stm32_configgpio(PROBE_8); } \
+	} while(0)
+
+# define PROBE(n,s)  do {stm32_gpiowrite(PROBE_##n,(s));}while(0)
+# define PROBE_MARK(n) PROBE(n,false);PROBE(n,true)
+#else
+# define PROBE_INIT(mask)
+# define PROBE(n,s)
+# define PROBE_MARK(n)
+#endif
+
+#endif  /*__NUTTX_CONFIG_PX4_FMU_V6C_INCLUDE_BOARD_H  */

boards/px4/fmu-v6c/nuttx-config/include/board_dma_map.h

@@ -0,0 +1,79 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+// DMAMUX1 Using at most 8 Channels on DMA1 --------   Assigned
+//                                                     V
+
+#define DMAMAP_SPI1_RX    DMAMAP_DMA12_SPI1RX_0     /* 1 DMA1:37 BMI055, ICM-42688-P */
+#define DMAMAP_SPI1_TX    DMAMAP_DMA12_SPI1TX_0     /* 2 DMA1:38 BMI055, ICM-42688-P  */
+
+#define DMAMAP_SPI2_RX    DMAMAP_DMA12_SPI2RX_0     /* 3 DMA1:39 FRAM */
+#define DMAMAP_SPI2_TX    DMAMAP_DMA12_SPI2TX_0     /* 4 DMA1:40 FRAM */
+
+//#define DMAMAP_USART1_RX  DMAMAP_DMA12_USART1RX_0 /*  DMA1:41  GPS1 */
+//#define DMAMAP_USART1_TX  DMAMAP_DMA12_USART1TX_0 /*  DMA1:42  GPS1 */
+
+//#define DMAMAP_USART2_RX  DMAMAP_DMA12_USART2RX_0 /*  DMA1:43 Telem3 */
+//#define DMAMAP_USART2_TX  DMAMAP_DMA12_USART2TX_0 /*  DMA1:44 Telem3 */
+
+//#define DMAMAP_USART3_RX  DMAMAP_DMA12_USART3RX_0 /*  DMA1:45 DEBUG */
+//#define DMAMAP_USART3_TX  DMAMAP_DMA12_USART3TX_0 /*  DMA1:46 DEBUG */
+
+//#define DMAMAP_UART4_RX   DMAMAP_DMA12_UART4RX_0  /*  DMA1:63 EXT2 */
+//#define DMAMAP_UART4_TX   DMAMAP_DMA12_UART4TX_0  /*  DMA1:64 EXT2 */
+
+#define DMAMAP_USART6_RX  DMAMAP_DMA12_USART6RX_0   /* 5 DMA1:71 PX4IO */
+#define DMAMAP_USART6_TX  DMAMAP_DMA12_USART6TX_0   /* 6 DMA1:72 PX4IO */
+
+// Assigned in timer_config.cpp
+
+// Timer 4                                          /* 7 DMA1:32 TIM4UP */
+// Timer 5                                          /* 8 DMA1:50 TIM5UP */
+
+// DMAMUX2 Using at most 8 Channels on DMA2 --------   Assigned
+//                                                     V
+
+
+#define DMAMAP_USART3_RX  DMAMAP_DMA12_USART3RX_1   /* 3 DMA2:45 DEBUG */
+#define DMAMAP_USART3_TX  DMAMAP_DMA12_USART3TX_1   /* 4 DMA2:46 DEBUG */
+
+#define DMAMAP_UART5_RX   DMAMAP_DMA12_UART5RX_1    /* 5 DMA2:65 TELEM2 */
+#define DMAMAP_UART5_TX   DMAMAP_DMA12_UART5TX_1    /* 6 DMA2:66  TELEM2 */
+
+#define DMAMAP_UART7_RX   DMAMAP_DMA12_UART7RX_1    /* 7 DMA1:79  TELEM1 */
+#define DMAMAP_UART7_TX   DMAMAP_DMA12_UART7TX_1    /* 8 DMA1:80  TELEM1 */
+
+//#define DMAMAP_UART8_RX   DMAMAP_DMA12_UART8RX_1  /*  DMA1:81  GPS2 */
+//#define DMAMAP_UART8_TX   DMAMAP_DMA12_UART8TX_1  /*  DMA1:82  GPS2 */

boards/px4/fmu-v6c/nuttx-config/nsh/defconfig

@@ -0,0 +1,241 @@
+#
+# 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_MMCSD_HAVE_CARDDETECT is not set
+# CONFIG_MMCSD_HAVE_WRITEPROTECT is not set
+# CONFIG_MMCSD_MMCSUPPORT is not set
+# CONFIG_MMCSD_SPI is not set
+# CONFIG_NSH_DISABLEBG is not set
+# CONFIG_NSH_DISABLESCRIPT is not set
+# CONFIG_NSH_DISABLE_DF is not set
+# CONFIG_NSH_DISABLE_EXEC is not set
+# CONFIG_NSH_DISABLE_EXIT is not set
+# CONFIG_NSH_DISABLE_GET is not set
+# CONFIG_NSH_DISABLE_ITEF is not set
+# CONFIG_NSH_DISABLE_LOOPS is not set
+# CONFIG_NSH_DISABLE_MKFATFS is not set
+# CONFIG_NSH_DISABLE_SEMICOLON is not set
+# CONFIG_NSH_DISABLE_TIME is not set
+CONFIG_ARCH="arm"
+CONFIG_ARCH_BOARD_CUSTOM=y
+CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/px4/fmu-v6c/nuttx-config"
+CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y
+CONFIG_ARCH_BOARD_CUSTOM_NAME="px4"
+CONFIG_ARCH_CHIP="stm32h7"
+CONFIG_ARCH_CHIP_STM32H743VI=y
+CONFIG_ARCH_CHIP_STM32H7=y
+CONFIG_ARCH_INTERRUPTSTACK=512
+CONFIG_ARCH_STACKDUMP=y
+CONFIG_ARMV7M_BASEPRI_WAR=y
+CONFIG_ARMV7M_DCACHE=y
+CONFIG_ARMV7M_DTCM=y
+CONFIG_ARMV7M_ICACHE=y
+CONFIG_ARMV7M_MEMCPY=y
+CONFIG_ARMV7M_USEBASEPRI=y
+CONFIG_ARM_MPU_EARLY_RESET=y
+CONFIG_BOARDCTL_RESET=y
+CONFIG_BOARD_CRASHDUMP=y
+CONFIG_BOARD_LOOPSPERMSEC=95751
+CONFIG_BOARD_RESET_ON_ASSERT=2
+CONFIG_BUILTIN=y
+CONFIG_C99_BOOL8=y
+CONFIG_CDCACM=y
+CONFIG_CDCACM_IFLOWCONTROL=y
+CONFIG_CDCACM_PRODUCTID=0x0038
+CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v6C.x"
+CONFIG_CDCACM_RXBUFSIZE=600
+CONFIG_CDCACM_TXBUFSIZE=12000
+CONFIG_CDCACM_VENDORID=0x3185
+CONFIG_CDCACM_VENDORSTR="Auterion"
+CONFIG_CLOCK_MONOTONIC=y
+CONFIG_DEBUG_FULLOPT=y
+CONFIG_DEBUG_HARDFAULT_ALERT=y
+CONFIG_DEBUG_MEMFAULT=y
+CONFIG_DEBUG_SYMBOLS=y
+CONFIG_DEFAULT_SMALL=y
+CONFIG_DEV_FIFO_SIZE=0
+CONFIG_DEV_PIPE_MAXSIZE=1024
+CONFIG_DEV_PIPE_SIZE=70
+CONFIG_EXPERIMENTAL=y
+CONFIG_FAT_DMAMEMORY=y
+CONFIG_FAT_LCNAMES=y
+CONFIG_FAT_LFN=y
+CONFIG_FAT_LFN_ALIAS_HASH=y
+CONFIG_FDCLONE_STDIO=y
+CONFIG_FS_BINFS=y
+CONFIG_FS_CROMFS=y
+CONFIG_FS_FAT=y
+CONFIG_FS_FATTIME=y
+CONFIG_FS_PROCFS=y
+CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y
+CONFIG_FS_PROCFS_MAX_TASKS=64
+CONFIG_FS_PROCFS_REGISTER=y
+CONFIG_FS_ROMFS=y
+CONFIG_GRAN=y
+CONFIG_GRAN_INTR=y
+CONFIG_HAVE_CXX=y
+CONFIG_HAVE_CXXINITIALIZE=y
+CONFIG_I2C=y
+CONFIG_I2C_RESET=y
+CONFIG_IDLETHREAD_STACKSIZE=750
+CONFIG_IOB_NBUFFERS=24
+CONFIG_IOB_NCHAINS=24
+CONFIG_LIBC_FLOATINGPOINT=y
+CONFIG_LIBC_LONG_LONG=y
+CONFIG_LIBC_STRERROR=y
+CONFIG_MEMSET_64BIT=y
+CONFIG_MEMSET_OPTSPEED=y
+CONFIG_MMCSD=y
+CONFIG_MMCSD_SDIO=y
+CONFIG_MMCSD_SDIOWAIT_WRCOMPLETE=y
+CONFIG_MM_IOB=y
+CONFIG_MM_REGIONS=4
+CONFIG_MTD=y
+CONFIG_MTD_BYTE_WRITE=y
+CONFIG_MTD_PARTITION=y
+CONFIG_MTD_PROGMEM=y
+CONFIG_MTD_RAMTRON=y
+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_STRERROR=y
+CONFIG_NSH_VARS=y
+CONFIG_OTG_ID_GPIO_DISABLE=y
+CONFIG_PIPES=y
+CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
+CONFIG_PTHREAD_MUTEX_ROBUST=y
+CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_SETSPEED=y
+CONFIG_RAM_SIZE=245760
+CONFIG_RAM_START=0x20010000
+CONFIG_RAW_BINARY=y
+CONFIG_READLINE_CMD_HISTORY=y
+CONFIG_READLINE_TABCOMPLETION=y
+CONFIG_RTC_DATETIME=y
+CONFIG_SCHED_ATEXIT=y
+CONFIG_SCHED_HPWORK=y
+CONFIG_SCHED_HPWORKPRIORITY=249
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
+CONFIG_SCHED_INSTRUMENTATION=y
+CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
+CONFIG_SCHED_WAITPID=y
+CONFIG_SDCLONE_DISABLE=y
+CONFIG_SEM_NNESTPRIO=8
+CONFIG_SEM_PREALLOCHOLDERS=0
+CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=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=256
+CONFIG_STM32H7_ADC1=y
+CONFIG_STM32H7_ADC3=y
+CONFIG_STM32H7_BBSRAM=y
+CONFIG_STM32H7_BBSRAM_FILES=5
+CONFIG_STM32H7_BDMA=y
+CONFIG_STM32H7_BKPSRAM=y
+CONFIG_STM32H7_DMA1=y
+CONFIG_STM32H7_DMA2=y
+CONFIG_STM32H7_DMACAPABLE=y
+CONFIG_STM32H7_FLOWCONTROL_BROKEN=y
+CONFIG_STM32H7_I2C1=y
+CONFIG_STM32H7_I2C2=y
+CONFIG_STM32H7_I2C4=y
+CONFIG_STM32H7_I2C_DYNTIMEO=y
+CONFIG_STM32H7_I2C_DYNTIMEO_STARTSTOP=10
+CONFIG_STM32H7_OTGFS=y
+CONFIG_STM32H7_PROGMEM=y
+CONFIG_STM32H7_RTC=y
+CONFIG_STM32H7_RTC_HSECLOCK=y
+CONFIG_STM32H7_RTC_MAGIC_REG=1
+CONFIG_STM32H7_SAVE_CRASHDUMP=y
+CONFIG_STM32H7_SDMMC2=y
+CONFIG_STM32H7_SERIALBRK_BSDCOMPAT=y
+CONFIG_STM32H7_SERIAL_DISABLE_REORDERING=y
+CONFIG_STM32H7_SPI1=y
+CONFIG_STM32H7_SPI1_DMA=y
+CONFIG_STM32H7_SPI1_DMA_BUFFER=1024
+CONFIG_STM32H7_SPI2=y
+CONFIG_STM32H7_SPI2_DMA=y
+CONFIG_STM32H7_SPI2_DMA_BUFFER=4096
+CONFIG_STM32H7_SPI_DMA=y
+CONFIG_STM32H7_TIM12=y
+CONFIG_STM32H7_TIM1=y
+CONFIG_STM32H7_TIM4=y
+CONFIG_STM32H7_TIM5=y
+CONFIG_STM32H7_UART5=y
+CONFIG_STM32H7_UART7=y
+CONFIG_STM32H7_UART8=y
+CONFIG_STM32H7_USART1=y
+CONFIG_STM32H7_USART2=y
+CONFIG_STM32H7_USART3=y
+CONFIG_STM32H7_USART6=y
+CONFIG_STM32H7_USART_BREAKS=y
+CONFIG_STM32H7_USART_INVERT=y
+CONFIG_STM32H7_USART_SINGLEWIRE=y
+CONFIG_STM32H7_USART_SWAP=y
+CONFIG_SYSTEM_CDCACM=y
+CONFIG_SYSTEM_NSH=y
+CONFIG_TASK_NAME_SIZE=24
+CONFIG_UART5_IFLOWCONTROL=y
+CONFIG_UART5_OFLOWCONTROL=y
+CONFIG_UART5_RXDMA=y
+CONFIG_UART5_TXBUFSIZE=3000
+CONFIG_UART5_TXDMA=y
+CONFIG_UART7_BAUD=57600
+CONFIG_UART7_IFLOWCONTROL=y
+CONFIG_UART7_OFLOWCONTROL=y
+CONFIG_UART7_RXBUFSIZE=600
+CONFIG_UART7_RXDMA=y
+CONFIG_UART7_TXBUFSIZE=3000
+CONFIG_UART7_TXDMA=y
+CONFIG_UART8_BAUD=57600
+CONFIG_UART8_RXBUFSIZE=600
+CONFIG_UART8_TXBUFSIZE=1500
+CONFIG_USART1_BAUD=57600
+CONFIG_USART1_RXBUFSIZE=600
+CONFIG_USART1_TXBUFSIZE=1500
+CONFIG_USART2_BAUD=57600
+CONFIG_USART2_RXBUFSIZE=600
+CONFIG_USART2_TXBUFSIZE=3000
+CONFIG_USART3_BAUD=57600
+CONFIG_USART3_RXBUFSIZE=180
+CONFIG_USART3_RXDMA=y
+CONFIG_USART3_SERIAL_CONSOLE=y
+CONFIG_USART3_TXBUFSIZE=1500
+CONFIG_USART3_TXDMA=y
+CONFIG_USART6_BAUD=57600
+CONFIG_USART6_RXBUFSIZE=600
+CONFIG_USART6_TXBUFSIZE=1500
+CONFIG_USBDEV=y
+CONFIG_USBDEV_BUSPOWERED=y
+CONFIG_USBDEV_MAXPOWER=500
+CONFIG_USEC_PER_TICK=1000
+CONFIG_USERMAIN_STACKSIZE=2944
+CONFIG_USER_ENTRYPOINT="nsh_main"
+CONFIG_WATCHDOG=y
+CONFIG_WQUEUE_NOTIFIER=y

boards/px4/fmu-v6c/nuttx-config/scripts/bootloader_script.ld

@@ -0,0 +1,215 @@
+/****************************************************************************
+ * scripts/script.ld
+ *
+ *   Copyright (C) 2016, 2019 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/* The PX4 FMUV6C uses an STM32H753II has 2048Kb of main FLASH memory.
+ * The flash memory is partitioned into a User Flash memory and a System
+ * Flash memory. Each of these memories has two banks:
+ *
+ *   1) User Flash memory:
+ *
+ *      Bank 1: Start address 0x0800:0000 to 0x080F:FFFF with 8 sectors, 128Kb each
+ *      Bank 2: Start address 0x0810:0000 to 0x081F:FFFF with 8 sectors, 128Kb each
+ *
+ *   2) System Flash memory:
+ *
+ *      Bank 1: Start address 0x1FF0:0000 to 0x1FF1:FFFF with 1 x 128Kb sector
+ *      Bank 1: Start address 0x1FF4:0000 to 0x1FF5:FFFF with 1 x 128Kb sector
+ *
+ *   3) User option bytes for user configuration, only in Bank 1.
+ *
+ * In the STM32H743II, two different boot spaces can be selected through
+ * the BOOT pin and the boot base address programmed in the BOOT_ADD0 and
+ * BOOT_ADD1 option bytes:
+ *
+ *   1) BOOT=0: Boot address defined by user option byte BOOT_ADD0[15:0].
+ *      ST programmed value: Flash memory at 0x0800:0000
+ *   2) BOOT=1: Boot address defined by user option byte BOOT_ADD1[15:0].
+ *      ST programmed value: System bootloader at 0x1FF0:0000
+ *
+ * The PX4 FMUV6C has a test point on board, the BOOT0 pin is at ground so by
+ * default, the STM32 will boot to address 0x0800:0000 in FLASH unless the test
+ * point is pulled to 3.3V.then the boot will be from 0x1FF0:0000
+ *
+ * The STM32H743II also has 1024Kb of data SRAM.
+ * SRAM is split up into several blocks and into three power domains:
+ *
+ *   1) TCM SRAMs are dedicated to the Cortex-M7 and are accessible with
+ *      0 wait states by the Cortex-M7 and by MDMA through AHBS slave bus
+ *
+ *      1.1) 128Kb of DTCM-RAM beginning at address 0x2000:0000
+ *
+ *           The DTCM-RAM is organized as 2 x 64Kb DTCM-RAMs on 2 x 32 bit
+ *           DTCM ports. The DTCM-RAM could be used for critical real-time
+ *           data, such as interrupt service routines or stack / heap memory.
+ *           Both DTCM-RAMs can be used in parallel (for load/store operations)
+ *           thanks to the Cortex-M7 dual issue capability.
+ *
+ *      1.2)  64Kb of ITCM-RAM beginning at address 0x0000:0000
+ *
+ *           This RAM is connected to ITCM 64-bit interface designed for
+ *           execution of critical real-times routines by the CPU.
+ *
+ *   2) AXI SRAM (D1 domain) accessible by all system masters except BDMA
+ *      through D1 domain AXI bus matrix
+ *
+ *      2.1) 512Kb of SRAM beginning at address 0x2400:0000
+ *
+ *   3) AHB SRAM (D2 domain) accessible by all system masters except BDMA
+ *      through D2 domain AHB bus matrix
+ *
+ *      3.1) 128Kb of SRAM1 beginning at address 0x3000:0000
+ *      3.2) 128Kb of SRAM2 beginning at address 0x3002:0000
+ *      3.3)  32Kb of SRAM3 beginning at address 0x3004:0000
+ *
+ *      SRAM1 - SRAM3 are one contiguous block: 288Kb at address 0x3000:0000
+ *
+ *   4) AHB SRAM (D3 domain) accessible by most of system masters
+ *      through D3 domain AHB bus matrix
+ *
+ *      4.1)  64Kb of SRAM4 beginning at address 0x3800:0000
+ *      4.1)   4Kb of backup RAM beginning at address 0x3880: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 bootloader uses the first sector of the flash, which is 128K in length.
+ */
+
+MEMORY
+{
+	itcm  (rwx) : ORIGIN = 0x00000000, LENGTH =   64K
+	flash (rx)  : ORIGIN = 0x08000000, LENGTH =  128K
+	dtcm1 (rwx) : ORIGIN = 0x20000000, LENGTH =   64K
+	dtcm2 (rwx) : ORIGIN = 0x20010000, LENGTH =   64K
+	sram  (rwx) : ORIGIN = 0x24000000, LENGTH =  512K
+	sram1 (rwx) : ORIGIN = 0x30000000, LENGTH =  128K
+	sram2 (rwx) : ORIGIN = 0x30020000, LENGTH =  128K
+	sram3 (rwx) : ORIGIN = 0x30040000, LENGTH =   32K
+	sram4 (rwx) : ORIGIN = 0x38000000, LENGTH =   64K
+	bbram (rwx) : ORIGIN = 0x38800000, LENGTH =    4K
+}
+
+OUTPUT_ARCH(arm)
+EXTERN(_vectors)
+ENTRY(_stext)
+
+/*
+ * 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(32);
+		/*
+		This signature provides the bootloader with a way to delay booting
+		*/
+		_bootdelay_signature = ABSOLUTE(.);
+		FILL(0xffecc2925d7d05c5)
+		. += 8;
+		*(.text .text.*)
+		*(.fixup)
+		*(.gnu.warning)
+		*(.rodata .rodata.*)
+		*(.gnu.linkonce.t.*)
+		*(.glue_7)
+		*(.glue_7t)
+		*(.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) }
+}

boards/px4/fmu-v6c/nuttx-config/scripts/script.ld

@@ -0,0 +1,223 @@
+/****************************************************************************
+ * scripts/script.ld
+ *
+ *   Copyright (C) 2016, 2019 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/* The PX4 FMUV6C uses an STM32H753II has 2048Kb of main FLASH memory.
+ * The flash memory is partitioned into a User Flash memory and a System
+ * Flash memory. Each of these memories has two banks:
+ *
+ *   1) User Flash memory:
+ *
+ *      Bank 1: Start address 0x0800:0000 to 0x080F:FFFF with 8 sectors, 128Kb each
+ *      Bank 2: Start address 0x0810:0000 to 0x081F:FFFF with 8 sectors, 128Kb each
+ *
+ *   2) System Flash memory:
+ *
+ *      Bank 1: Start address 0x1FF0:0000 to 0x1FF1:FFFF with 1 x 128Kb sector
+ *      Bank 1: Start address 0x1FF4:0000 to 0x1FF5:FFFF with 1 x 128Kb sector
+ *
+ *   3) User option bytes for user configuration, only in Bank 1.
+ *
+ * In the STM32H743II, two different boot spaces can be selected through
+ * the BOOT pin and the boot base address programmed in the BOOT_ADD0 and
+ * BOOT_ADD1 option bytes:
+ *
+ *   1) BOOT=0: Boot address defined by user option byte BOOT_ADD0[15:0].
+ *      ST programmed value: Flash memory at 0x0800:0000
+ *   2) BOOT=1: Boot address defined by user option byte BOOT_ADD1[15:0].
+ *      ST programmed value: System bootloader at 0x1FF0:0000
+ *
+ * The PX4 FMUV6C has a test point on board, the BOOT0 pin is at ground so by
+ * default, the STM32 will boot to address 0x0800:0000 in FLASH unless the test
+ * point is pulled to 3.3V.then the boot will be from 0x1FF0:0000
+ *
+ * The STM32H743II also has 1024Kb of data SRAM.
+ * SRAM is split up into several blocks and into three power domains:
+ *
+ *   1) TCM SRAMs are dedicated to the Cortex-M7 and are accessible with
+ *      0 wait states by the Cortex-M7 and by MDMA through AHBS slave bus
+ *
+ *      1.1) 128Kb of DTCM-RAM beginning at address 0x2000:0000
+ *
+ *           The DTCM-RAM is organized as 2 x 64Kb DTCM-RAMs on 2 x 32 bit
+ *           DTCM ports. The DTCM-RAM could be used for critical real-time
+ *           data, such as interrupt service routines or stack / heap memory.
+ *           Both DTCM-RAMs can be used in parallel (for load/store operations)
+ *           thanks to the Cortex-M7 dual issue capability.
+ *
+ *      1.2)  64Kb of ITCM-RAM beginning at address 0x0000:0000
+ *
+ *           This RAM is connected to ITCM 64-bit interface designed for
+ *           execution of critical real-times routines by the CPU.
+ *
+ *   2) AXI SRAM (D1 domain) accessible by all system masters except BDMA
+ *      through D1 domain AXI bus matrix
+ *
+ *      2.1) 512Kb of SRAM beginning at address 0x2400:0000
+ *
+ *   3) AHB SRAM (D2 domain) accessible by all system masters except BDMA
+ *      through D2 domain AHB bus matrix
+ *
+ *      3.1) 128Kb of SRAM1 beginning at address 0x3000:0000
+ *      3.2) 128Kb of SRAM2 beginning at address 0x3002:0000
+ *      3.3)  32Kb of SRAM3 beginning at address 0x3004:0000
+ *
+ *      SRAM1 - SRAM3 are one contiguous block: 288Kb at address 0x3000:0000
+ *
+ *   4) AHB SRAM (D3 domain) accessible by most of system masters
+ *      through D3 domain AHB bus matrix
+ *
+ *      4.1)  64Kb of SRAM4 beginning at address 0x3800:0000
+ *      4.1)   4Kb of backup RAM beginning at address 0x3880: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.
+ */
+
+MEMORY
+{
+	itcm  (rwx) : ORIGIN = 0x00000000, LENGTH =   64K
+	flash (rx)  : ORIGIN = 0x08020000, LENGTH = 1920K
+	dtcm1 (rwx) : ORIGIN = 0x20000000, LENGTH =   64K
+	dtcm2 (rwx) : ORIGIN = 0x20010000, LENGTH =   64K
+	sram  (rwx) : ORIGIN = 0x24000000, LENGTH =  512K
+	sram1 (rwx) : ORIGIN = 0x30000000, LENGTH =  128K
+	sram2 (rwx) : ORIGIN = 0x30020000, LENGTH =  128K
+	sram3 (rwx) : ORIGIN = 0x30040000, LENGTH =   32K
+	sram4 (rwx) : ORIGIN = 0x38000000, LENGTH =   64K
+	bbram (rwx) : ORIGIN = 0x38800000, LENGTH =    4K
+}
+
+OUTPUT_ARCH(arm)
+EXTERN(_vectors)
+ENTRY(_stext)
+
+/*
+ * 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)
+EXTERN(board_get_manifest)
+
+SECTIONS
+{
+	.text : {
+		_stext = ABSOLUTE(.);
+		*(.vectors)
+		. = ALIGN(32);
+		/*
+		This signature provides the bootloader with a way to delay booting
+		*/
+		_bootdelay_signature = ABSOLUTE(.);
+		FILL(0xffecc2925d7d05c5)
+		. += 8;
+		*(.text .text.*)
+		*(.fixup)
+		*(.gnu.warning)
+		*(.rodata .rodata.*)
+		*(.gnu.linkonce.t.*)
+		*(.glue_7)
+		*(.glue_7t)
+		*(.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
+
+	/* Emit the the D3 power domain section for locating BDMA data */
+
+	.sram4_reserve (NOLOAD) :
+	{
+		*(.sram4)
+		. = ALIGN(4);
+		_sram4_heap_start = ABSOLUTE(.);
+	} > sram4
+
+	/* 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) }
+}

boards/px4/fmu-v6c/src/CMakeLists.txt

@@ -0,0 +1,75 @@
+############################################################################
+#
+#   Copyright (c) 2016 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  "bootloader")
+	add_compile_definitions(BOOTLOADER)
+	add_library(drivers_board
+		bootloader_main.c
+		init.c
+		usb.c
+		timer_config.cpp
+	)
+	target_link_libraries(drivers_board
+		PRIVATE
+			nuttx_arch # sdio
+			nuttx_drivers # sdio
+			px4_layer #gpio
+			arch_io_pins # iotimer
+			bootloader
+	)
+	target_include_directories(drivers_board PRIVATE ${PX4_SOURCE_DIR}/platforms/nuttx/src/bootloader/common)
+
+else()
+	add_library(drivers_board
+		can.c
+		i2c.cpp
+		init.c
+		led.c
+		mtd.cpp
+		manifest.c
+		sdio.c
+		spi.cpp
+		timer_config.cpp
+		usb.c
+	)
+	add_dependencies(drivers_board arch_board_hw_info)
+
+	target_link_libraries(drivers_board
+		PRIVATE
+			arch_spi
+			arch_board_hw_info
+			drivers__led # drv_led_start
+			nuttx_arch # sdio
+			nuttx_drivers # sdio
+			px4_layer
+	)
+endif()

boards/px4/fmu-v6c/src/board_config.h

@@ -0,0 +1,321 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file board_config.h
+ *
+ * PX4FMU-v6c internal definitions
+ */
+
+#pragma once
+
+/****************************************************************************************************
+ * Included Files
+ ****************************************************************************************************/
+
+#include <px4_platform_common/px4_config.h>
+#include <nuttx/compiler.h>
+#include <stdint.h>
+
+
+#include <stm32_gpio.h>
+
+/****************************************************************************************************
+ * Definitions
+ ****************************************************************************************************/
+
+#undef TRACE_PINS
+
+/* PX4IO connection configuration */
+
+#define BOARD_USES_PX4IO_VERSION       2
+#define PX4IO_SERIAL_DEVICE            "/dev/ttyS4"
+#define PX4IO_SERIAL_TX_GPIO           GPIO_USART6_TX
+#define PX4IO_SERIAL_RX_GPIO           GPIO_USART6_RX
+#define PX4IO_SERIAL_BASE              STM32_USART6_BASE
+#define PX4IO_SERIAL_VECTOR            STM32_IRQ_USART6
+#define PX4IO_SERIAL_TX_DMAMAP         DMAMAP_USART6_TX
+#define PX4IO_SERIAL_RX_DMAMAP         DMAMAP_USART6_RX
+#define PX4IO_SERIAL_RCC_REG           STM32_RCC_APB2ENR
+#define PX4IO_SERIAL_RCC_EN            RCC_APB2ENR_USART6EN
+#define PX4IO_SERIAL_CLOCK             STM32_PCLK2_FREQUENCY
+#define PX4IO_SERIAL_BITRATE           1500000               /* 1.5Mbps -> max rate for IO */
+
+/* PX4FMU GPIOs ***********************************************************************************/
+
+
+/* LEDs are driven with push pull Anodes to 3.3V */
+
+#define GPIO_nLED_RED        /* PD10 */  (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10)
+#define GPIO_nLED_BLUE       /* PD11 */  (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN11)
+
+#define BOARD_HAS_CONTROL_STATUS_LEDS      1
+#define BOARD_OVERLOAD_LED     LED_RED
+#define BOARD_ARMED_STATE_LED  LED_BLUE
+
+/*
+ * ADC channels
+ *
+ * These are the channel numbers of the ADCs of the microcontroller that
+ * can be used by the Px4 Firmware in the adc driver
+ */
+
+/* ADC defines to be used in sensors.cpp to read from a particular channel */
+
+#define ADC1_CH(n)                  (n)
+
+/* N.B. there is no offset mapping needed for ADC3 because */
+#define ADC3_CH(n)                  (n)
+
+/* We are only use ADC3 for REV/VER. */
+
+/* Define GPIO pins used as ADC N.B. Channel numbers must match below  */
+
+#define PX4_ADC_GPIO  \
+	/* PC4  */  GPIO_ADC12_INP4,   \
+	/* PB1  */  GPIO_ADC12_INP5,   \
+	/* PC5  */  GPIO_ADC12_INP8,   \
+	/* PC0  */  GPIO_ADC123_INP10, \
+	/* PC1  */  GPIO_ADC123_INP11, \
+	/* PA2  */  GPIO_ADC12_INP14,  \
+	/* PA4  */  GPIO_ADC12_INP18   \
+
+/* Define Channel numbers must match above GPIO pin IN(n)*/
+#define ADC_BATTERY1_CURRENT_CHANNEL            /* PC4 */  ADC1_CH(4)
+#define ADC_BATTERY2_VOLTAGE_CHANNEL            /* PB1 */  ADC1_CH(5)
+#define ADC_BATTERY1_VOLTAGE_CHANNEL            /* PC5 */  ADC1_CH(8)
+#define ADC_HW_REV_SENSE_CHANNEL                /* PC0 */  ADC3_CH(10)
+#define ADC_HW_VER_SENSE_CHANNEL                /* PC1 */  ADC3_CH(11)
+#define ADC_BATTERY2_CURRENT_CHANNEL            /* PA2 */  ADC1_CH(14)
+#define ADC_SCALED_V5_CHANNEL                   /* PA4 */  ADC1_CH(18)
+
+#define ADC_CHANNELS \
+	((1 << ADC_BATTERY1_CURRENT_CHANNEL) | \
+	 (1 << ADC_BATTERY2_VOLTAGE_CHANNEL) | \
+	 (1 << ADC_BATTERY1_VOLTAGE_CHANNEL) | \
+	 (1 << ADC_BATTERY2_CURRENT_CHANNEL) | \
+	 (1 << ADC_SCALED_V5_CHANNEL       ))
+
+#define HW_REV_VER_ADC_BASE STM32_ADC3_BASE
+
+#define SYSTEM_ADC_BASE STM32_ADC1_BASE
+
+/* HW has to large of R termination on ADC todo:change when HW value is chosen */
+#define BOARD_ADC_OPEN_CIRCUIT_V     (5.6f)
+
+/* HW Version and Revision drive signals Default to 1 to detect */
+#define BOARD_HAS_HW_VERSIONING
+
+#define GPIO_HW_VER_REV_DRIVE  /* PE12 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN12)
+#define GPIO_HW_REV_SENSE      /* PC0 */  GPIO_ADC123_INP10
+#define GPIO_HW_VER_SENSE      /* PC1 */  GPIO_ADC123_INP11
+#define HW_INFO_INIT           {'V','6','C','x', 'x',0}
+#define HW_INFO_INIT_VER       3 /* Offset in above string of the VER */
+#define HW_INFO_INIT_REV       4 /* Offset in above string of the REV */
+
+#define BOARD_NUM_SPI_CFG_HW_VERSIONS 2 // Rev 0, 10 Sensor sets
+//                 Base/FMUM
+#define V6C00   HW_VER_REV(0x0,0x0) // FMUV6C,                 Rev 0
+#define V6C10   HW_VER_REV(0x1,0x0) // NO PX4IO,               Rev 0
+
+
+/* HEATER
+ * PWM in future
+ */
+#define GPIO_HEATER_OUTPUT   /* PB9  T17CH1 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN9)
+#define HEATER_OUTPUT_EN(on_true)	       px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, (on_true))
+
+/* PWM
+ */
+#define DIRECT_PWM_OUTPUT_CHANNELS   8
+
+
+/* Power supply control and monitoring GPIOs */
+
+#define GPIO_nPOWER_IN_A                /* PA15  */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN15)
+#define GPIO_nPOWER_IN_B                /* PB12  */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN12)
+#define GPIO_nPOWER_IN_C                /* PE15  */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN15)
+
+#define GPIO_nVDD_BRICK1_VALID          GPIO_nPOWER_IN_A /* Brick 1 Is Chosen */
+#define GPIO_nVDD_BRICK2_VALID          GPIO_nPOWER_IN_B /* Brick 2 Is Chosen  */
+#define BOARD_NUMBER_BRICKS             2
+#define GPIO_nVDD_USB_VALID             GPIO_nPOWER_IN_C /* USB     Is Chosen */
+
+#define GPIO_VDD_5V_PERIPH_nEN          /* PE2  */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN2)
+#define GPIO_VDD_5V_PERIPH_nOC          /* PE3  */ (GPIO_INPUT |GPIO_FLOAT|GPIO_PORTE|GPIO_PIN3)
+#define GPIO_VDD_5V_HIPOWER_nEN         /* PC10 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN10)
+#define GPIO_VDD_5V_HIPOWER_nOC         /* PC11 */ (GPIO_INPUT |GPIO_FLOAT|GPIO_PORTC|GPIO_PIN11)
+#define GPIO_VDD_3V3_SENSORS_EN         /* PB2  */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN2)
+
+/* Define True logic Power Control in arch agnostic form */
+
+#define VDD_5V_PERIPH_EN(on_true)          px4_arch_gpiowrite(GPIO_VDD_5V_PERIPH_nEN, !(on_true))
+#define VDD_5V_HIPOWER_EN(on_true)         px4_arch_gpiowrite(GPIO_VDD_5V_HIPOWER_nEN, !(on_true))
+#define VDD_3V3_SENSORS_EN(on_true)       px4_arch_gpiowrite(GPIO_VDD_3V3_SENSORS4_EN, (on_true))
+
+/* Tone alarm output */
+
+#define TONE_ALARM_TIMER        3  /* Timer 3 */
+#define TONE_ALARM_CHANNEL      3  /* PB0 GPIO_TIM3_CH3OUT_1 */
+
+#define GPIO_BUZZER_1           /* PPB0 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN0)
+
+#define GPIO_TONE_ALARM_IDLE    GPIO_BUZZER_1
+#define GPIO_TONE_ALARM         GPIO_TIM3_CH3OUT_1
+
+/* USB OTG FS
+ *
+ * PA9  OTG_FS_VBUS VBUS sensing
+ */
+#define GPIO_OTGFS_VBUS         /* PA9 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_100MHz|GPIO_PORTA|GPIO_PIN9)
+
+/* High-resolution timer */
+#define HRT_TIMER               8  /* use timer8 for the HRT */
+#define HRT_TIMER_CHANNEL       3  /* use capture/compare channel 3 */
+
+/* PWM input driver. Use FMU AUX5 pins attached to timer4 channel 3 */
+#define PWMIN_TIMER                       4
+#define PWMIN_TIMER_CHANNEL    /* T4C3 */ 3
+#define GPIO_PWM_IN            /* PD14 */ GPIO_TIM4_CH3IN_2
+
+#define SDIO_SLOTNO                    0  /* Only one slot */
+#define SDIO_MINOR                     0
+
+/* SD card bringup does not work if performed on the IDLE thread because it
+ * will cause waiting.  Use either:
+ *
+ *  CONFIG_LIB_BOARDCTL=y, OR
+ *  CONFIG_BOARD_INITIALIZE=y && CONFIG_BOARD_INITTHREAD=y
+ */
+
+#if defined(CONFIG_BOARD_INITIALIZE) && !defined(CONFIG_LIB_BOARDCTL) && \
+   !defined(CONFIG_BOARD_INITTHREAD)
+#  warning SDIO initialization cannot be perfomed on the IDLE thread
+#endif
+
+/* By Providing BOARD_ADC_USB_CONNECTED (using the px4_arch abstraction)
+ * this board support the ADC system_power interface, and therefore
+ * provides the true logic GPIO BOARD_ADC_xxxx macros.
+ */
+#define BOARD_ADC_USB_CONNECTED (px4_arch_gpioread(GPIO_OTGFS_VBUS))
+#define BOARD_ADC_USB_VALID     (!px4_arch_gpioread(GPIO_nVDD_USB_VALID))
+
+/* FMUv6C never powers off the Servo rail */
+
+#define BOARD_ADC_SERVO_VALID     (1)
+
+#define BOARD_ADC_BRICK1_VALID  (!px4_arch_gpioread(GPIO_nVDD_BRICK1_VALID))
+#define BOARD_ADC_BRICK2_VALID  (!px4_arch_gpioread(GPIO_nVDD_BRICK2_VALID))
+
+#define BOARD_ADC_PERIPH_5V_OC  (!px4_arch_gpioread(GPIO_VDD_5V_PERIPH_nOC))
+#define BOARD_ADC_HIPOWER_5V_OC (!px4_arch_gpioread(GPIO_VDD_5V_HIPOWER_nOC))
+
+
+/* This board provides a DMA pool and APIs */
+#define BOARD_DMA_ALLOC_POOL_SIZE 5120
+
+/* This board provides the board_on_reset interface */
+
+#define BOARD_HAS_ON_RESET 1
+
+#define PX4_GPIO_INIT_LIST { \
+		PX4_ADC_GPIO,                     \
+		GPIO_HW_VER_REV_DRIVE,            \
+		GPIO_CAN1_TX,                     \
+		GPIO_CAN1_RX,                     \
+		GPIO_CAN2_TX,                     \
+		GPIO_CAN2_RX,                     \
+		GPIO_HEATER_OUTPUT,               \
+		GPIO_nPOWER_IN_A,                 \
+		GPIO_nPOWER_IN_B,                 \
+		GPIO_nPOWER_IN_C,                 \
+		GPIO_VDD_5V_PERIPH_nEN,           \
+		GPIO_VDD_5V_PERIPH_nOC,           \
+		GPIO_VDD_5V_HIPOWER_nEN,          \
+		GPIO_VDD_5V_HIPOWER_nOC,          \
+		GPIO_VDD_3V3_SENSORS_EN,          \
+		GPIO_TONE_ALARM_IDLE,             \
+	}
+
+#define BOARD_ENABLE_CONSOLE_BUFFER
+
+#define PX4_I2C_BUS_MTD      4,5
+
+
+#define BOARD_NUM_IO_TIMERS 5
+
+__BEGIN_DECLS
+
+/****************************************************************************************************
+ * Public Types
+ ****************************************************************************************************/
+
+/****************************************************************************************************
+ * Public data
+ ****************************************************************************************************/
+
+#ifndef __ASSEMBLY__
+
+/****************************************************************************************************
+ * Public Functions
+ ****************************************************************************************************/
+
+/****************************************************************************
+ * Name: stm32_sdio_initialize
+ *
+ * Description:
+ *   Initialize SDIO-based MMC/SD card support
+ *
+ ****************************************************************************/
+
+int stm32_sdio_initialize(void);
+
+/****************************************************************************************************
+ * Name: stm32_spiinitialize
+ *
+ * Description:
+ *   Called to configure SPI chip select GPIO pins for the PX4FMU board.
+ *
+ ****************************************************************************************************/
+
+extern void stm32_spiinitialize(void);
+
+extern void stm32_usbinitialize(void);
+
+extern void board_peripheral_reset(int ms);
+
+#include <px4_platform_common/board_common.h>
+
+#endif /* __ASSEMBLY__ */
+
+__END_DECLS

boards/px4/fmu-v6c/src/bootloader_main.c

@@ -0,0 +1,62 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019, 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 bootloader_main.c
+ *
+ * FMU-specific early startup code for bootloader
+*/
+
+#include "board_config.h"
+#include "bl.h"
+
+#include <nuttx/config.h>
+#include <nuttx/board.h>
+#include <chip.h>
+#include <stm32_uart.h>
+#include <arch/board/board.h>
+#include "arm_internal.h"
+#include <px4_platform_common/init.h>
+
+extern int sercon_main(int c, char **argv);
+
+void board_late_initialize(void)
+{
+	sercon_main(0, NULL);
+}
+
+extern void sys_tick_handler(void);
+void board_timerhook(void)
+{
+	sys_tick_handler();
+}

boards/px4/fmu-v6c/src/can.c

@@ -0,0 +1,128 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 px4fmu_can.c
+ *
+ * Board-specific CAN functions.
+ */
+
+#ifdef CONFIG_CAN
+
+#include <errno.h>
+#include <debug.h>
+
+#include <nuttx/can/can.h>
+#include <arch/board/board.h>
+
+#include "chip.h"
+#include "arm_arch.h"
+
+#include "chip.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
+
+#endif /* CONFIG_CAN */

boards/px4/fmu-v6c/src/hw_config.h

@@ -0,0 +1,128 @@
+/*
+ * hw_config.h
+ *
+ *  Created on: May 17, 2015
+ *      Author: david_s5
+ */
+
+#ifndef HW_CONFIG_H_
+#define HW_CONFIG_H_
+
+/****************************************************************************
+ * 10-8--2016:
+ *  To simplify the ripple effect on the tools, we will be using
+ *  /dev/serial/by-id/<asterisk>PX4<asterisk> to locate PX4 devices. Therefore
+ *  moving forward all Bootloaders must contain the prefix "PX4 BL "
+ *  in the USBDEVICESTRING
+ *  This Change will be made in an upcoming BL release
+ ****************************************************************************/
+/*
+ * Define usage to configure a bootloader
+ *
+ *
+ * Constant                example          Usage
+ * APP_LOAD_ADDRESS     0x08004000            - The address in Linker Script, where the app fw is org-ed
+ * BOOTLOADER_DELAY     5000                  - Ms to wait while under USB pwr or bootloader request
+ * BOARD_FMUV2
+ * INTERFACE_USB        1                     - (Optional) Scan and use the USB interface for bootloading
+ * INTERFACE_USART      1                     - (Optional) Scan and use the Serial interface for bootloading
+ * USBDEVICESTRING      "PX4 BL FMU v2.x"     - USB id string
+ * USBPRODUCTID         0x0011                - PID Should match defconfig
+ * BOOT_DELAY_ADDRESS   0x000001a0            - (Optional) From the linker script from Linker Script to get a custom
+ *                                               delay provided by an APP FW
+ * BOARD_TYPE           9                     - Must match .prototype boad_id
+ * _FLASH_KBYTES        (*(uint16_t *)0x1fff7a22) - Run time flash size detection
+ * BOARD_FLASH_SECTORS  ((_FLASH_KBYTES == 0x400) ? 11 : 23) - Run time determine the physical last sector
+ * BOARD_FLASH_SECTORS   11                   - Hard coded zero based last sector
+ * BOARD_FLASH_SIZE     (_FLASH_KBYTES*1024)-   Total Flash size of device, determined at run time.
+ *                         (1024 * 1024)      - Hard coded Total Flash of device - The bootloader and app reserved will be deducted
+ *                                              programmatically
+ *
+ * BOARD_FIRST_FLASH_SECTOR_TO_ERASE  2        - Optional sectors index in the flash_sectors table  (F4 only), to begin erasing.
+ *                                               This is to allow sectors to be reserved for app fw usage. That will NOT be erased
+ *                                               during a FW upgrade.
+ *                                               The default is 0, and selects the first sector to be erased, as the 0th entry in the
+ *                                               flash_sectors table. Which is the second physical sector of FLASH in the device.
+ *                                               The first physical sector of FLASH is used by the bootloader, and is not defined
+ *                                               in the table.
+ *
+ * APP_RESERVATION_SIZE (BOARD_FIRST_FLASH_SECTOR_TO_ERASE * 16 * 1024) - Number of bytes reserved by the APP FW. This number plus
+ *                                                                        BOOTLOADER_RESERVATION_SIZE  will be deducted from
+ *                                                                        BOARD_FLASH_SIZE to determine the size of the App FW
+ *                                                                        and hence the address space of FLASH to erase and program.
+ * USBMFGSTRING            "PX4 AP"            - Optional USB MFG string (default is '3D Robotics' if not defined.)
+ * SERIAL_BREAK_DETECT_DISABLED                -  Optional prevent break selection on Serial port from entering or staying in BL
+ *
+ * * Other defines are somewhat self explanatory.
+ */
+
+/* Boot device selection list*/
+#define USB0_DEV       0x01
+#define SERIAL0_DEV    0x02
+#define SERIAL1_DEV    0x04
+
+#define APP_LOAD_ADDRESS               0x08020000
+#define BOOTLOADER_DELAY               5000
+#define INTERFACE_USB                  1
+#define INTERFACE_USB_CONFIG           "/dev/ttyACM0"
+#define BOARD_VBUS                     MK_GPIO_INPUT(GPIO_OTGFS_VBUS)
+
+//#define USE_VBUS_PULL_DOWN
+#define INTERFACE_USART                1
+#define INTERFACE_USART_CONFIG         "/dev/ttyS0,115200"
+#define BOOT_DELAY_ADDRESS             0x000001a0
+#define BOARD_TYPE                     56
+#define _FLASH_KBYTES                  (*(uint32_t *)0x1FF1E880)
+#define BOARD_FLASH_SECTORS            (15)
+#define BOARD_FLASH_SIZE               (_FLASH_KBYTES * 1024)
+
+#define OSC_FREQ                       16
+
+#define BOARD_PIN_LED_ACTIVITY         GPIO_nLED_BLUE // BLUE
+#define BOARD_PIN_LED_BOOTLOADER       GPIO_nLED_RED // RED
+#define BOARD_LED_ON                   0
+#define BOARD_LED_OFF                  1
+
+#define SERIAL_BREAK_DETECT_DISABLED   1
+
+/*
+ * Uncommenting this allows to force the bootloader through
+ * a PWM output pin. As this can accidentally initialize
+ * an ESC prematurely, it is not recommended. This feature
+ * has not been used and hence defaults now to off.
+ *
+ * # define BOARD_FORCE_BL_PIN_OUT         (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN14)
+ * # define BOARD_FORCE_BL_PIN_IN          (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN11)
+ *
+ * # define BOARD_POWER_PIN_OUT           (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN4)
+ * # define BOARD_POWER_ON                 1
+ * # define BOARD_POWER_OFF                0
+ * # undef  BOARD_POWER_PIN_RELEASE        // Leave pin enabling Power - un comment to release (disable power)
+ *
+*/
+
+#if !defined(ARCH_SN_MAX_LENGTH)
+# define ARCH_SN_MAX_LENGTH 12
+#endif
+
+#if !defined(APP_RESERVATION_SIZE)
+#  define APP_RESERVATION_SIZE 0
+#endif
+
+#if !defined(BOARD_FIRST_FLASH_SECTOR_TO_ERASE)
+#  define BOARD_FIRST_FLASH_SECTOR_TO_ERASE 1
+#endif
+
+#if !defined(USB_DATA_ALIGN)
+# define USB_DATA_ALIGN
+#endif
+
+#ifndef BOOT_DEVICES_SELECTION
+#  define BOOT_DEVICES_SELECTION USB0_DEV|SERIAL0_DEV|SERIAL1_DEV
+#endif
+
+#ifndef BOOT_DEVICES_FILTER_ONUSB
+#  define BOOT_DEVICES_FILTER_ONUSB USB0_DEV|SERIAL0_DEV|SERIAL1_DEV
+#endif
+
+#endif /* HW_CONFIG_H_ */

boards/px4/fmu-v6c/src/i2c.cpp

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

boards/px4/fmu-v6c/src/init.c

@@ -0,0 +1,259 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file init.c
+ *
+ * PX4FMU-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.
+ */
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include "board_config.h"
+
+#include <stdbool.h>
+#include <stdio.h>
+#include <string.h>
+#include <debug.h>
+#include <errno.h>
+#include <syslog.h>
+
+#include <nuttx/config.h>
+#include <nuttx/board.h>
+#include <nuttx/spi/spi.h>
+#include <nuttx/sdio.h>
+#include <nuttx/mmcsd.h>
+#include <nuttx/analog/adc.h>
+#include <nuttx/mm/gran.h>
+#include <chip.h>
+#include <stm32_uart.h>
+#include <arch/board/board.h>
+#include "arm_internal.h"
+
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_board_led.h>
+#include <systemlib/px4_macros.h>
+#include <px4_arch/io_timer.h>
+#include <px4_platform_common/init.h>
+#include <px4_platform/gpio.h>
+#include <px4_platform/board_determine_hw_info.h>
+#include <px4_platform/board_dma_alloc.h>
+
+/****************************************************************************
+ * Pre-Processor Definitions
+ ****************************************************************************/
+
+/* Configuration ************************************************************/
+
+/*
+ * Ideally we'd be able to get these from arm_internal.h,
+ * but since we want to be able to disable the NuttX use
+ * of leds for system indication at will and there is no
+ * separate switch, we need to build independent of the
+ * CONFIG_ARCH_LEDS configuration switch.
+ */
+__BEGIN_DECLS
+extern void led_init(void);
+extern void led_on(int led);
+extern void led_off(int led);
+__END_DECLS
+
+
+/************************************************************************************
+ * Name: board_peripheral_reset
+ *
+ * Description:
+ *
+ ************************************************************************************/
+__EXPORT void board_peripheral_reset(int ms)
+{
+	/* set the peripheral rails off */
+
+	VDD_5V_PERIPH_EN(false);
+	board_control_spi_sensors_power(false, 0xffff);
+
+	/* wait for the peripheral rail to reach GND */
+	usleep(ms * 1000);
+	syslog(LOG_DEBUG, "reset done, %d ms\n", ms);
+
+	/* re-enable power */
+
+	/* switch the peripheral rail back on */
+	board_control_spi_sensors_power(true, 0xffff);
+	VDD_5V_PERIPH_EN(true);
+
+}
+
+/************************************************************************************
+ * Name: board_on_reset
+ *
+ * Description:
+ * Optionally provided function called on entry to board_system_reset
+ * It should perform any house keeping prior to the rest.
+ *
+ * status - 1 if resetting to boot loader
+ *          0 if just resetting
+ *
+ ************************************************************************************/
+__EXPORT void board_on_reset(int status)
+{
+	for (int i = 0; i < DIRECT_PWM_OUTPUT_CHANNELS; ++i) {
+		px4_arch_configgpio(io_timer_channel_get_gpio_output(i));
+	}
+
+	if (status >= 0) {
+		up_mdelay(6);
+	}
+}
+
+/************************************************************************************
+ * 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)
+{
+	board_on_reset(-1); /* Reset PWM first thing */
+
+	/* configure LEDs */
+
+	board_autoled_initialize();
+
+	/* configure pins */
+
+	const uint32_t gpio[] = PX4_GPIO_INIT_LIST;
+	px4_gpio_init(gpio, arraySize(gpio));
+
+	/* configure USB interfaces */
+
+	stm32_usbinitialize();
+}
+
+/****************************************************************************
+ * 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)
+{
+#if !defined(BOOTLOADER)
+
+	/* Power on Interfaces */
+	VDD_5V_PERIPH_EN(true);
+	VDD_5V_HIPOWER_EN(true);
+
+	/* Need hrt running before using the ADC */
+
+	px4_platform_init();
+
+
+	if (OK == board_determine_hw_info()) {
+		syslog(LOG_INFO, "[boot] Rev 0x%1x : Ver 0x%1x %s\n", board_get_hw_revision(), board_get_hw_version(),
+		       board_get_hw_type_name());
+
+	} else {
+		syslog(LOG_ERR, "[boot] Failed to read HW revision and version\n");
+	}
+
+	stm32_spiinitialize();
+
+	board_spi_reset(10, 0xffff);
+
+	/* configure the DMA allocator */
+
+	if (board_dma_alloc_init() < 0) {
+		syslog(LOG_ERR, "[boot] DMA alloc FAILED\n");
+	}
+
+#  if defined(SERIAL_HAVE_RXDMA)
+	// set up the serial DMA polling at 1ms intervals for received bytes that have not triggered a DMA event.
+	static struct hrt_call serial_dma_call;
+	hrt_call_every(&serial_dma_call, 1000, 1000, (hrt_callout)stm32_serial_dma_poll, NULL);
+#  endif
+
+	/* initial LED state */
+	drv_led_start();
+	led_off(LED_RED);
+	led_off(LED_BLUE);
+
+	if (board_hardfault_init(2, true) != 0) {
+		led_on(LED_RED);
+	}
+
+#  ifdef CONFIG_MMCSD
+	int ret = stm32_sdio_initialize();
+
+	if (ret != OK) {
+		led_on(LED_RED);
+	}
+
+#  endif /* CONFIG_MMCSD */
+
+	/* Configure the HW based on the manifest */
+
+	px4_platform_configure();
+#endif
+	return OK;
+}

boards/px4/fmu-v6c/src/led.c

@@ -0,0 +1,235 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 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 px4fmu2_led.c
+ *
+ * PX4FMU LED backend.
+ */
+
+#include <px4_platform_common/px4_config.h>
+
+#include <stdbool.h>
+
+#include "chip.h"
+#include "stm32_gpio.h"
+#include "board_config.h"
+
+#include <nuttx/board.h>
+#include <arch/board/board.h>
+
+/*
+ * Ideally we'd be able to get these from arm_internal.h,
+ * but since we want to be able to disable the NuttX use
+ * of leds for system indication at will and there is no
+ * separate switch, we need to build independent of the
+ * CONFIG_ARCH_LEDS configuration switch.
+ */
+__BEGIN_DECLS
+extern void led_init(void);
+extern void led_on(int led);
+extern void led_off(int led);
+extern void led_toggle(int led);
+__END_DECLS
+
+#ifdef CONFIG_ARCH_LEDS
+static bool nuttx_owns_leds = true;
+//                                B  R  S  G
+//                                0  1  2  3
+static const uint8_t xlatpx4[] = {1, 2, 4, 0};
+#  define xlat(p) xlatpx4[(p)]
+static uint32_t g_ledmap[] = {
+	GPIO_nLED_GREEN,   // Indexed by BOARD_LED_GREEN
+	GPIO_nLED_BLUE,    // Indexed by BOARD_LED_BLUE
+	GPIO_nLED_RED,     // Indexed by BOARD_LED_RED
+	GPIO_nSAFETY_SWITCH_LED_OUT,  // Indexed by LED_SAFETY by xlatpx4
+};
+
+#else
+
+#  define xlat(p) (p)
+static uint32_t g_ledmap[] = {
+	GPIO_nLED_BLUE,                     // Indexed by LED_BLUE
+	GPIO_nLED_RED,                      // Indexed by LED_RED, LED_AMBER
+	0,                                  // Indexed by LED_SAFETY (defaulted to an input)
+	0,                                  // Indexed by LED_GREEN
+};
+
+#endif
+
+__EXPORT void led_init(void)
+{
+	for (size_t l = 0; l < (sizeof(g_ledmap) / sizeof(g_ledmap[0])); l++) {
+		if (g_ledmap[l] != 0) {
+			stm32_configgpio(g_ledmap[l]);
+		}
+	}
+}
+
+static void phy_set_led(int led, bool state)
+{
+	/* Drive Low to switch on */
+
+	if (g_ledmap[led] != 0) {
+		stm32_gpiowrite(g_ledmap[led], !state);
+	}
+}
+
+static bool phy_get_led(int led)
+{
+	/* If Low it is on */
+	if (g_ledmap[led] != 0) {
+		return !stm32_gpioread(g_ledmap[led]);
+	}
+
+	return false;
+}
+
+__EXPORT void led_on(int led)
+{
+	phy_set_led(xlat(led), true);
+}
+
+__EXPORT void led_off(int led)
+{
+	phy_set_led(xlat(led), false);
+}
+
+__EXPORT void led_toggle(int led)
+{
+	phy_set_led(xlat(led), !phy_get_led(xlat(led)));
+}
+
+#ifdef CONFIG_ARCH_LEDS
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: board_autoled_initialize
+ ****************************************************************************/
+
+void board_autoled_initialize(void)
+{
+	led_init();
+}
+
+/****************************************************************************
+ * Name: board_autoled_on
+ ****************************************************************************/
+
+void board_autoled_on(int led)
+{
+	if (!nuttx_owns_leds) {
+		return;
+	}
+
+	switch (led) {
+	default:
+		break;
+
+	case LED_HEAPALLOCATE:
+		phy_set_led(BOARD_LED_BLUE, true);
+		break;
+
+	case LED_IRQSENABLED:
+		phy_set_led(BOARD_LED_BLUE, false);
+		phy_set_led(BOARD_LED_GREEN, true);
+		break;
+
+	case LED_STACKCREATED:
+		phy_set_led(BOARD_LED_GREEN, true);
+		phy_set_led(BOARD_LED_BLUE, true);
+		break;
+
+	case LED_INIRQ:
+		phy_set_led(BOARD_LED_BLUE, true);
+		break;
+
+	case LED_SIGNAL:
+		phy_set_led(BOARD_LED_GREEN, true);
+		break;
+
+	case LED_ASSERTION:
+		phy_set_led(BOARD_LED_RED, true);
+		phy_set_led(BOARD_LED_BLUE, true);
+		break;
+
+	case LED_PANIC:
+		phy_set_led(BOARD_LED_RED, true);
+		break;
+
+	case LED_IDLE : /* IDLE */
+		phy_set_led(BOARD_LED_RED, true);
+		break;
+	}
+}
+
+/****************************************************************************
+ * Name: board_autoled_off
+ ****************************************************************************/
+
+void board_autoled_off(int led)
+{
+	if (!nuttx_owns_leds) {
+		return;
+	}
+
+	switch (led) {
+	default:
+		break;
+
+	case LED_SIGNAL:
+		phy_set_led(BOARD_LED_GREEN, false);
+		break;
+
+	case LED_INIRQ:
+		phy_set_led(BOARD_LED_BLUE, false);
+		break;
+
+	case LED_ASSERTION:
+		phy_set_led(BOARD_LED_RED, false);
+		phy_set_led(BOARD_LED_BLUE, false);
+		break;
+
+	case LED_PANIC:
+		phy_set_led(BOARD_LED_RED, false);
+		break;
+
+	case LED_IDLE : /* IDLE */
+		phy_set_led(BOARD_LED_RED, false);
+		break;
+	}
+}
+
+#endif /* CONFIG_ARCH_LEDS */

boards/px4/fmu-v6c/src/manifest.c

@@ -0,0 +1,147 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018, 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 manifest.c
+ *
+ * This module supplies the interface to the manifest of hardware that is
+ * optional and dependent on the HW REV and HW VER IDs
+ *
+ * The manifest allows the system to know whether a hardware option
+ * say for example the PX4IO is an no-pop option vs it is broken.
+ *
+ */
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+#include <board_config.h>
+
+#include <inttypes.h>
+#include <stdbool.h>
+#include <syslog.h>
+
+#include "systemlib/px4_macros.h"
+
+/****************************************************************************
+ * Pre-Processor Definitions
+ ****************************************************************************/
+
+typedef struct {
+	uint32_t                hw_ver_rev; /* the version and revision */
+	const px4_hw_mft_item_t *mft;       /* The first entry */
+	uint32_t                entries;    /* the lenght of the list */
+} px4_hw_mft_list_entry_t;
+
+typedef px4_hw_mft_list_entry_t *px4_hw_mft_list_entry;
+#define px4_hw_mft_list_uninitialized (px4_hw_mft_list_entry) -1
+
+static const px4_hw_mft_item_t device_unsupported = {0, 0, 0};
+
+// List of components on a specific board configuration
+// The index of those components is given by the enum (px4_hw_mft_item_id_t)
+// declared in board_common.h
+static const px4_hw_mft_item_t hw_mft_list_v0600[] = {
+	{
+		.present     = 1,
+		.mandatory   = 1,
+		.connection  = px4_hw_con_onboard,
+	},
+};
+
+static const px4_hw_mft_item_t hw_mft_list_v0610[] = {
+	{
+		.present     = 0,
+		.mandatory   = 0,
+		.connection  = px4_hw_con_unknown,
+	},
+	{
+		.present     = 1,
+		.mandatory   = 1,
+		.connection  = px4_hw_con_onboard,
+	},
+};
+
+
+static px4_hw_mft_list_entry_t mft_lists[] = {
+//  ver_rev
+	{V6C00, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)},
+	{V6C10, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // No PX4IO
+};
+
+/************************************************************************************
+ * Name: board_query_manifest
+ *
+ * Description:
+ *   Optional returns manifest item.
+ *
+ * Input Parameters:
+ *   manifest_id - the ID for the manifest item to retrieve
+ *
+ * Returned Value:
+ *   0 - item is not in manifest => assume legacy operations
+ *   pointer to a manifest item
+ *
+ ************************************************************************************/
+
+__EXPORT px4_hw_mft_item board_query_manifest(px4_hw_mft_item_id_t id)
+{
+	static px4_hw_mft_list_entry boards_manifest = px4_hw_mft_list_uninitialized;
+
+	if (boards_manifest == px4_hw_mft_list_uninitialized) {
+		uint32_t ver_rev = board_get_hw_version() << 8;
+		ver_rev |= board_get_hw_revision();
+
+		for (unsigned i = 0; i < arraySize(mft_lists); i++) {
+			if (mft_lists[i].hw_ver_rev == ver_rev) {
+				boards_manifest = &mft_lists[i];
+				break;
+			}
+		}
+
+		if (boards_manifest == px4_hw_mft_list_uninitialized) {
+			syslog(LOG_ERR, "[boot] Board %4" PRIx32 " is not supported!\n", ver_rev);
+		}
+	}
+
+	px4_hw_mft_item rv = &device_unsupported;
+
+	if (boards_manifest != px4_hw_mft_list_uninitialized &&
+	    id < boards_manifest->entries) {
+		rv = &boards_manifest->mft[id];
+	}
+
+	return rv;
+}

boards/px4/fmu-v6c/src/mtd.cpp

@@ -0,0 +1,104 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2020 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <nuttx/spi/spi.h>
+#include <px4_platform_common/px4_manifest.h>
+//                                                              KiB BS    nB
+static const px4_mft_device_t spi2 = {             // FM25V02A on FMUM 32K 512 X 64
+	.bus_type = px4_mft_device_t::SPI,
+	.devid    = SPIDEV_FLASH(0)
+};
+
+static const px4_mft_device_t i2c4 = {             // 24LC64T on IMU   8K 32 X 256
+	.bus_type =  px4_mft_device_t::I2C,
+	.devid    =  PX4_MK_I2C_DEVID(4, 0x50)
+};
+
+
+static const px4_mtd_entry_t fmum_fram = {
+	.device = &spi2,
+	.npart = 2,
+	.partd = {
+		{
+			.type = MTD_PARAMETERS,
+			.path = "/fs/mtd_params",
+			.nblocks = 32
+		},
+		{
+			.type = MTD_WAYPOINTS,
+			.path = "/fs/mtd_waypoints",
+			.nblocks = 32
+
+		}
+	},
+};
+
+static const px4_mtd_entry_t imu_eeprom = {
+	.device = &i2c4,
+	.npart = 2,
+	.partd = {
+		{
+			.type = MTD_CALDATA,
+			.path = "/fs/mtd_caldata",
+			.nblocks = 248
+		},
+		{
+			.type = MTD_ID,
+			.path = "/fs/mtd_id",
+			.nblocks = 8 // 256 = 32 * 8
+		}
+	},
+};
+
+static const px4_mtd_manifest_t board_mtd_config = {
+	.nconfigs   = 2,
+	.entries = {
+		&fmum_fram,
+		&imu_eeprom
+	}
+};
+
+static const px4_mft_entry_s mtd_mft = {
+	.type = MTD,
+	.pmft = (void *) &board_mtd_config,
+};
+
+static const px4_mft_s mft = {
+	.nmft = 1,
+	.mfts = &mtd_mft
+};
+
+const px4_mft_s *board_get_manifest(void)
+{
+	return &mft;
+}

boards/px4/fmu-v6c/src/sdio.c

@@ -0,0 +1,177 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2014, 2016 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+#include <board_config.h>
+
+#include <stdbool.h>
+#include <stdio.h>
+#include <debug.h>
+#include <errno.h>
+
+#include <nuttx/sdio.h>
+#include <nuttx/mmcsd.h>
+
+#include "chip.h"
+#include "board_config.h"
+#include "stm32_gpio.h"
+#include "stm32_sdmmc.h"
+
+#ifdef CONFIG_MMCSD
+
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+/* Card detections requires card support and a card detection GPIO */
+
+#define HAVE_NCD   1
+#if !defined(GPIO_SDMMC1_NCD)
+#  undef HAVE_NCD
+#endif
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+static FAR struct sdio_dev_s *sdio_dev;
+#ifdef HAVE_NCD
+static bool g_sd_inserted = 0xff; /* Impossible value */
+#endif
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: stm32_ncd_interrupt
+ *
+ * Description:
+ *   Card detect interrupt handler.
+ *
+ ****************************************************************************/
+
+#ifdef HAVE_NCD
+static int stm32_ncd_interrupt(int irq, FAR void *context)
+{
+	bool present;
+
+	present = !stm32_gpioread(GPIO_SDMMC1_NCD);
+
+	if (sdio_dev && present != g_sd_inserted) {
+		sdio_mediachange(sdio_dev, present);
+		g_sd_inserted = present;
+	}
+
+	return OK;
+}
+#endif
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: stm32_sdio_initialize
+ *
+ * Description:
+ *   Initialize SDIO-based MMC/SD card support
+ *
+ ****************************************************************************/
+
+int stm32_sdio_initialize(void)
+{
+	int ret;
+
+#ifdef HAVE_NCD
+	/* Card detect */
+
+	bool cd_status;
+
+	/* Configure the card detect GPIO */
+
+	stm32_configgpio(GPIO_SDMMC1_NCD);
+
+	/* Register an interrupt handler for the card detect pin */
+
+	stm32_gpiosetevent(GPIO_SDMMC1_NCD, true, true, true, stm32_ncd_interrupt);
+#endif
+
+	/* Mount the SDIO-based MMC/SD block driver */
+	/* First, get an instance of the SDIO interface */
+
+	finfo("Initializing SDIO slot %d\n", SDIO_SLOTNO);
+
+	sdio_dev = sdio_initialize(SDIO_SLOTNO);
+
+	if (!sdio_dev) {
+		syslog(LOG_ERR, "[boot] Failed to initialize SDIO slot %d\n", SDIO_SLOTNO);
+		return -ENODEV;
+	}
+
+	/* Now bind the SDIO interface to the MMC/SD driver */
+
+	finfo("Bind SDIO to the MMC/SD driver, minor=%d\n", SDIO_MINOR);
+
+	ret = mmcsd_slotinitialize(SDIO_MINOR, sdio_dev);
+
+	if (ret != OK) {
+		syslog(LOG_ERR, "[boot] Failed to bind SDIO to the MMC/SD driver: %d\n", ret);
+		return ret;
+	}
+
+	finfo("Successfully bound SDIO to the MMC/SD driver\n");
+
+#ifdef HAVE_NCD
+	/* Use SD card detect pin to check if a card is g_sd_inserted */
+
+	cd_status = !stm32_gpioread(GPIO_SDMMC1_NCD);
+	finfo("Card detect : %d\n", cd_status);
+
+	sdio_mediachange(sdio_dev, cd_status);
+#else
+	/* Assume that the SD card is inserted.  What choice do we have? */
+
+	sdio_mediachange(sdio_dev, true);
+#endif
+
+	return OK;
+}
+
+#endif /* CONFIG_MMCSD */

boards/px4/fmu-v6c/src/spi.cpp

@@ -0,0 +1,61 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2020, 2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <px4_arch/spi_hw_description.h>
+#include <drivers/drv_sensor.h>
+#include <nuttx/spi/spi.h>
+
+constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSIONS] = {
+	initSPIHWVersion(V6C00, {
+		initSPIBus(SPI::Bus::SPI1, {
+			initSPIDevice(DRV_GYR_DEVTYPE_BMI055,  SPI::CS{GPIO::PortC, GPIO::Pin14}, SPI::DRDY{GPIO::PortE, GPIO::Pin5}),
+			initSPIDevice(DRV_ACC_DEVTYPE_BMI055,  SPI::CS{GPIO::PortC, GPIO::Pin15}, SPI::DRDY{GPIO::PortE, GPIO::Pin4}),
+			initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortC, GPIO::Pin13}, SPI::DRDY{GPIO::PortE, GPIO::Pin6}),
+		}, {GPIO::PortB, GPIO::Pin2}),
+		initSPIBus(SPI::Bus::SPI2, {
+			initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortD, GPIO::Pin4})
+		}),
+	}),
+	initSPIHWVersion(V6C10, {
+		initSPIBus(SPI::Bus::SPI1, {
+			initSPIDevice(DRV_GYR_DEVTYPE_BMI055,  SPI::CS{GPIO::PortC, GPIO::Pin14}, SPI::DRDY{GPIO::PortE, GPIO::Pin5}),
+			initSPIDevice(DRV_ACC_DEVTYPE_BMI055,  SPI::CS{GPIO::PortC, GPIO::Pin15}, SPI::DRDY{GPIO::PortE, GPIO::Pin4}),
+			initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortC, GPIO::Pin13}, SPI::DRDY{GPIO::PortE, GPIO::Pin6}),
+		}, {GPIO::PortB, GPIO::Pin2}),
+		initSPIBus(SPI::Bus::SPI2, {
+			initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortD, GPIO::Pin4})
+		}),
+	}),
+};
+
+static constexpr bool unused = validateSPIConfig(px4_spi_buses_all_hw);

boards/px4/fmu-v6c/src/timer_config.cpp

@@ -0,0 +1,73 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <px4_arch/io_timer_hw_description.h>
+
+/* Timer allocation
+ *
+ * TIM1_CH1  T FMU_CH1
+ * TIM1_CH2  T FMU_CH2
+ * TIM1_CH3  T FMU_CH3
+ * TIM1_CH4  T FMU_CH4
+ *
+ * TIM4_CH3  T FMU_CH5
+ * TIM4_CH4  T FMU_CH6
+ *
+ * TIM5_CH1  T FMU_CH7
+ * TIM5_CH2  T FMU_CH8
+ *
+ * TIM17_CH1 T HEATER                 > PWM OUT or GPIO
+ *
+ * TIM3_CH3  T BUZZER_1              - Driven by other driver
+ */
+
+constexpr io_timers_t io_timers[MAX_IO_TIMERS] = {
+	initIOTimer(Timer::Timer1, DMA{DMA::Index1}),
+	initIOTimer(Timer::Timer4, DMA{DMA::Index1}),
+	initIOTimer(Timer::Timer5),
+	initIOTimer(Timer::Timer17),
+};
+
+constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = {
+	initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel1}, {GPIO::PortA, GPIO::Pin8}),
+	initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel2}, {GPIO::PortE, GPIO::Pin11}),
+	initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel3}, {GPIO::PortE, GPIO::Pin13}),
+	initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel4}, {GPIO::PortE, GPIO::Pin14}),
+	initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel3}, {GPIO::PortD, GPIO::Pin14}),
+	initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel4}, {GPIO::PortD, GPIO::Pin15}),
+	initIOTimerChannel(io_timers, {Timer::Timer5, Timer::Channel1}, {GPIO::PortA, GPIO::Pin0}),
+	initIOTimerChannel(io_timers, {Timer::Timer5, Timer::Channel2}, {GPIO::PortA, GPIO::Pin1}),
+};
+
+constexpr io_timers_channel_mapping_t io_timers_channel_mapping =
+	initIOTimerChannelMapping(io_timers, timer_io_channels);

boards/px4/fmu-v6c/src/usb.c

@@ -0,0 +1,105 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2016 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 px4fmu_usb.c
+ *
+ * Board-specific USB functions.
+ */
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <px4_platform_common/px4_config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <debug.h>
+
+#include <nuttx/usb/usbdev.h>
+#include <nuttx/usb/usbdev_trace.h>
+
+#include <arm_arch.h>
+#include <chip.h>
+#include <stm32_gpio.h>
+#include <stm32_otg.h>
+#include "board_config.h"
+
+/************************************************************************************
+ * Definitions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Private Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Public Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Name: stm32_usbinitialize
+ *
+ * Description:
+ *   Called to setup USB-related GPIO pins for the PX4FMU board.
+ *
+ ************************************************************************************/
+
+__EXPORT void stm32_usbinitialize(void)
+{
+	/* The OTG FS has an internal soft pull-up */
+
+	/* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */
+
+#ifdef CONFIG_STM32H7_OTGFS
+	stm32_configgpio(GPIO_OTGFS_VBUS);
+#endif
+}
+
+/************************************************************************************
+ * Name:  stm32_usbsuspend
+ *
+ * Description:
+ *   Board logic must provide the stm32_usbsuspend logic if the USBDEV driver is
+ *   used.  This function is called whenever the USB enters or leaves suspend mode.
+ *   This is an opportunity for the board logic to shutdown clocks, power, etc.
+ *   while the USB is suspended.
+ *
+ ************************************************************************************/
+
+__EXPORT void stm32_usbsuspend(FAR struct usbdev_s *dev, bool resume)
+{
+	uinfo("resume: %d\n", resume);
+}

boards/px4/fmu-v6x/init/rc.board_sensors

@@ -25,17 +25,23 @@ then
 	ina238 -X -b 2 -t 2 -k start
 fi
 
+if ver hwtypecmp V6X04 V6X14
+then
+	# Internal SPI bus ICM20649
+	icm20649 -s -R 6 start
+else
 # Internal SPI BMI088
-bmi088 -A -R 4 -s start
-bmi088 -G -R 4 -s start
+	bmi088 -A -R 4 -s start
+	bmi088 -G -R 4 -s start
+fi
 
 # Internal SPI bus ICM42688p
 icm42688p -R 6 -s start
 
-if ver hwtypecmp V6X03 V6X13
+if ver hwtypecmp V6X03 V6X13 V6X04 V6X14
 then
 	# Internal SPI bus ICM-42670-P (hard-mounted)
-	icm42670p -R 14 -s start
+	icm42670p -R 10 -s start
 else
 	# Internal SPI bus ICM-20649 (hard-mounted)
 	icm20649 -R 14 -s start

boards/px4/fmu-v6x/src/board_config.h

@@ -215,13 +215,15 @@
 #define HW_INFO_INIT_VER       3 /* Offset in above string of the VER */
 #define HW_INFO_INIT_REV       4 /* Offset in above string of the REV */
 
-#define BOARD_NUM_SPI_CFG_HW_VERSIONS 2 // Rev 0 and Rev 3 Sensor sets
+#define BOARD_NUM_SPI_CFG_HW_VERSIONS 3 // Rev 0 and Rev 3,4 Sensor sets
 //                 Base/FMUM
 #define V6X00   HW_VER_REV(0x0,0x0) // FMUV6X,                 Rev 0
 #define V6X01   HW_VER_REV(0x0,0x1) // FMUV6X,     BMI388 I2C2 Rev 1
 #define V6X03   HW_VER_REV(0x0,0x3) // FMUV6X,     Sensor Set  Rev 3
+#define V6X04   HW_VER_REV(0x0,0x4) // FMUV6X,     Sensor Set  Rev 4
 #define V6X10   HW_VER_REV(0x1,0x0) // NO PX4IO,               Rev 0
 #define V6X13   HW_VER_REV(0x1,0x3) // NO PX4IO,   Sensor Set  Rev 3
+#define V6X14   HW_VER_REV(0x1,0x4) // NO PX4IO,   Sensor Set  Rev 4
 
 
 /* HEATER

boards/px4/fmu-v6x/src/manifest.c

@@ -102,6 +102,8 @@ static px4_hw_mft_list_entry_t mft_lists[] = {
 	{V6X01, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2
 	{V6X03, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2, Sensor Set 3
 	{V6X13, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // No PX4IO BMP388 moved to I2C2, Sensor Set 3
+	{V6X04, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2, Sensor Set 4
+	{V6X14, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // No PX4IO BMP388 moved to I2C2, Sensor Set 4
 };
 
 /************************************************************************************

boards/px4/fmu-v6x/src/spi.cpp

@@ -83,6 +83,29 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION
 			initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}),
 		}),
 	}),
+
+	initSPIHWVersion(HW_VER_REV(0, 4), {
+		initSPIBus(SPI::Bus::SPI1, {
+			initSPIDevice(DRV_IMU_DEVTYPE_ICM42670P, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}),
+		}, {GPIO::PortI, GPIO::Pin11}),
+		initSPIBus(SPI::Bus::SPI2, {
+			initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}),
+		}, {GPIO::PortF, GPIO::Pin4}),
+		initSPIBus(SPI::Bus::SPI3, {
+			initSPIDevice(DRV_IMU_DEVTYPE_ICM20649, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}),
+		}, {GPIO::PortE, GPIO::Pin7}),
+		//  initSPIBus(SPI::Bus::SPI4, {
+		//    // no devices
+		// TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h
+		//  }, {GPIO::PortG, GPIO::Pin8}),
+		initSPIBus(SPI::Bus::SPI5, {
+			initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7})
+		}),
+		initSPIBusExternal(SPI::Bus::SPI6, {
+			initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}),
+			initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}),
+		}),
+	}),
 };
 
 static constexpr bool unused = validateSPIConfig(px4_spi_buses_all_hw);

msg/CMakeLists.txt

@@ -137,7 +137,6 @@ set(msg_files
 	rc_parameter_map.msg
 	rpm.msg
 	rtl_time_estimate.msg
-	safety.msg
 	satellite_info.msg
 	sensor_accel.msg
 	sensor_accel_fifo.msg
@@ -164,6 +163,7 @@ set(msg_files
 	timesync.msg
 	timesync_status.msg
 	trajectory_bezier.msg
+	trajectory_setpoint.msg
 	trajectory_waypoint.msg
 	transponder_report.msg
 	tune_control.msg

msg/px4io_status.msg

@@ -19,7 +19,7 @@ bool status_rc_ppm
 bool status_rc_sbus
 bool status_rc_st24
 bool status_rc_sumd
-bool status_safety_off
+bool status_safety_button_event # px4io safety button was pressed for longer than 1 second
 
 # PX4IO alarms (PX4IO_P_STATUS_ALARMS)
 bool alarm_pwm_error

msg/safety.msg

@@ -1,3 +0,0 @@
-uint64 timestamp		# time since system start (microseconds)
-bool safety_switch_available		# Set to true if a safety switch is connected
-bool safety_off			# Set to true if safety is off

msg/sensor_combined.msg

@@ -2,22 +2,24 @@
 # These fields are scaled and offset-compensated where possible and do not
 # change with board revisions and sensor updates.
 
-uint64 timestamp				# time since system start (microseconds)
+uint64 timestamp                                # time since system start (microseconds)
 
-int32 RELATIVE_TIMESTAMP_INVALID = 2147483647	# (0x7fffffff) If one of the relative timestamps is set to this value, it means the associated sensor values are invalid
+int32 RELATIVE_TIMESTAMP_INVALID = 2147483647   # (0x7fffffff) If one of the relative timestamps is set to this value, it means the associated sensor values are invalid
 
 # gyro timstamp is equal to the timestamp of the message
-float32[3] gyro_rad			# average angular rate measured in the FRD body frame XYZ-axis in rad/s over the last gyro sampling period
-uint32 gyro_integral_dt		# gyro measurement sampling period in microseconds
+float32[3] gyro_rad                     # average angular rate measured in the FRD body frame XYZ-axis in rad/s over the last gyro sampling period
+uint32 gyro_integral_dt                 # gyro measurement sampling period in microseconds
 
-int32 accelerometer_timestamp_relative	# timestamp + accelerometer_timestamp_relative = Accelerometer timestamp
-float32[3] accelerometer_m_s2		# average value acceleration measured in the FRD body frame XYZ-axis in m/s^2 over the last accelerometer sampling period
-uint32 accelerometer_integral_dt	# accelerometer measurement sampling period in microseconds
+int32 accelerometer_timestamp_relative  # timestamp + accelerometer_timestamp_relative = Accelerometer timestamp
+float32[3] accelerometer_m_s2           # average value acceleration measured in the FRD body frame XYZ-axis in m/s^2 over the last accelerometer sampling period
+uint32 accelerometer_integral_dt        # accelerometer measurement sampling period in microseconds
 
 uint8 CLIPPING_X = 1
 uint8 CLIPPING_Y = 2
 uint8 CLIPPING_Z = 4
-uint8 accelerometer_clipping            # bitfield indicating if there was any accelerometer clipping (per axis) during the sampling period
 
-uint8 accel_calibration_count  	# Calibration changed counter. Monotonically increases whenever accelermeter calibration changes.
-uint8 gyro_calibration_count   	# Calibration changed counter. Monotonically increases whenever rate gyro calibration changes.
+uint8 accelerometer_clipping    # bitfield indicating if there was any accelerometer clipping (per axis) during the integration time frame
+uint8 gyro_clipping             # bitfield indicating if there was any gyro clipping (per axis) during the integration time frame
+
+uint8 accel_calibration_count   # Calibration changed counter. Monotonically increases whenever accelermeter calibration changes.
+uint8 gyro_calibration_count    # Calibration changed counter. Monotonically increases whenever rate gyro calibration changes.

msg/sensor_gyro.msg

@@ -11,6 +11,8 @@ float32 temperature       # temperature in degrees Celsius
 
 uint32 error_count
 
+uint8[3] clip_counter     # clip count per axis in the sample period
+
 uint8 samples             # number of raw samples that went into this message
 
 uint8 ORB_QUEUE_LENGTH = 8

msg/tools/urtps_bridge_topics.yaml

@@ -59,8 +59,7 @@ rtps:
     send:    true
   - msg:     vehicle_local_position_setpoint
     receive: true
-  - msg:     trajectory_setpoint # multi-topic / alias of vehicle_local_position_setpoint
-    base:    vehicle_local_position_setpoint
+  - msg:     trajectory_setpoint
     receive: true
   - msg:     vehicle_odometry
     send:    true

msg/trajectory_setpoint.msg

@@ -0,0 +1,15 @@
+# Trajectory setpoint in NED frame
+# Input to PID position controller.
+# Needs to be kinematically consistent and feasible for smooth flight.
+# setting a value to NaN means the state should not be controlled
+
+uint64 timestamp # time since system start (microseconds)
+
+# NED local world frame
+float32[3] position # in meters
+float32[3] velocity # in meters/second
+float32[3] acceleration # in meters/second^2
+float32[3] jerk # in meters/second^3 (for logging only)
+
+float32 yaw # euler angle of desired attitude in radians -PI..+PI
+float32 yawspeed # angular velocity around NED frame z-axis in radians/second

msg/vehicle_attitude.msg

@@ -1,12 +1,12 @@
 # This is similar to the mavlink message ATTITUDE_QUATERNION, but for onboard use
 
-uint64 timestamp		    # time since system start (microseconds)
+uint64 timestamp                # time since system start (microseconds)
 
-uint64 timestamp_sample     # the timestamp of the raw data (microseconds)
+uint64 timestamp_sample         # the timestamp of the raw data (microseconds)
 
-float32[4] q			    # Quaternion rotation from the FRD body frame to the NED earth frame
-float32[4] delta_q_reset 	# Amount by which quaternion has changed during last reset
-uint8 quat_reset_counter	# Quaternion reset counter
+float32[4] q                    # Quaternion rotation from the FRD body frame to the NED earth frame
+float32[4] delta_q_reset        # Amount by which quaternion has changed during last reset
+uint8 quat_reset_counter        # Quaternion reset counter
 
 # TOPICS vehicle_attitude vehicle_attitude_groundtruth vehicle_vision_attitude
 # TOPICS estimator_attitude

msg/vehicle_imu.msg

@@ -8,12 +8,15 @@ 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
 
 uint8 CLIPPING_X = 1
 uint8 CLIPPING_Y = 2
 uint8 CLIPPING_Z = 4
+
+uint8 delta_angle_clipping     # bitfield indicating if there was any gyro clipping (per axis) during the integration time frame
 uint8 delta_velocity_clipping   # bitfield indicating if there was any accelerometer clipping (per axis) during the integration time frame
 
 uint8 accel_calibration_count  	# Calibration changed counter. Monotonically increases whenever accelermeter calibration changes.

msg/vehicle_imu_status.msg

@@ -4,6 +4,7 @@ uint32 accel_device_id          # unique device ID for the sensor that does not
 uint32 gyro_device_id           # unique device ID for the sensor that does not change between power cycles
 
 uint32[3] accel_clipping        # total clipping per axis
+uint32[3] gyro_clipping         # total clipping per axis
 
 uint32 accel_error_count
 uint32 gyro_error_count

msg/vehicle_local_position_setpoint.msg

@@ -1,18 +1,19 @@
 # Local position setpoint in NED frame
-# setting something to NaN means the state should not be controlled
+# Telemetry of PID position controller to monitor tracking.
+# NaN means the state was not controlled
 
 uint64 timestamp	# time since system start (microseconds)
 
 float32 x		# in meters NED
 float32 y		# in meters NED
 float32 z		# in meters NED
-float32 yaw		# in radians NED -PI..+PI
-float32 yawspeed	# in radians/sec
+
 float32 vx		# in meters/sec
 float32 vy		# in meters/sec
 float32 vz		# in meters/sec
+
 float32[3] acceleration # in meters/sec^2
-float32[3] jerk # in meters/sec^3
 float32[3] thrust	# normalized thrust vector in NED
 
-# TOPICS vehicle_local_position_setpoint trajectory_setpoint
+float32 yaw		# in radians NED -PI..+PI
+float32 yawspeed	# in radians/sec

msg/vehicle_status.msg

@@ -1,5 +1,3 @@
-# If you change the order, add or remove arming_state_t states make sure to update the arrays
-# in state_machine_helper.cpp as well.
 uint64 timestamp				# time since system start (microseconds)
 
 uint8 ARMING_STATE_INIT = 0
@@ -69,13 +67,12 @@ uint8 system_type			# system type, contains mavlink MAV_TYPE
 uint8 system_id			# system id, contains MAVLink's system ID field
 uint8 component_id			# subsystem / component id, contains MAVLink's component ID field
 
-uint8 vehicle_type          # Type of vehicle (fixed-wing, rotary wing, ground)
+uint8 vehicle_type          # Type of vehicle (rotary wing/fixed-wing/rover/airship, see above)
                             # If the vehicle is a VTOL, then this value will be VEHICLE_TYPE_ROTARY_WING while flying as a multicopter,
                             # and VEHICLE_TYPE_FIXED_WING when flying as a fixed-wing
 
 bool is_vtol				# True if the system is VTOL capable
 bool is_vtol_tailsitter                 # True if the system performs a 90° pitch down rotation during transition from MC to FW
-bool vtol_fw_permanent_stab		# True if VTOL should stabilize attitude for fw in manual mode
 bool in_transition_mode			# True if VTOL is doing a transition
 bool in_transition_to_fw		# True if VTOL is doing a transition from MC to FW
 
@@ -119,3 +116,6 @@ uint8 latest_disarming_reason
 uint64 armed_time       # Arming timestamp (microseconds)
 
 uint64 takeoff_time     # Takeoff timestamp (microseconds)
+
+bool safety_button_available	# Set to true if a safety button is connected
+bool safety_off					# Set to true if safety is off

msg/vtol_vehicle_status.msg

@@ -7,8 +7,6 @@ uint8 VEHICLE_VTOL_STATE_FW = 4
 
 uint64 timestamp			# time since system start (microseconds)
 
-bool vtol_in_rw_mode			# true: vtol vehicle is in rotating wing mode
-bool vtol_in_trans_mode
-bool in_transition_to_fw		# True if VTOL is doing a transition from MC to FW
+uint8 vehicle_vtol_state		# current state of the vtol, see VEHICLE_VTOL_STATE
+
 bool vtol_transition_failsafe		# vtol in transition failsafe mode
-bool fw_permanent_stab			# In fw mode stabilize attitude even if in manual mode

platforms/nuttx/NuttX/nuttx

@@ -1 +1 @@
-Subproject commit 22dbf796a3f61e5edbaae0e2716e1794eb10debe
+Subproject commit c5c7d2b4f26f52f1dfb425ebde83328606b65d4f

platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/hw_description.h

@@ -92,7 +92,13 @@ enum Timer {
 	Timer13,
 	Timer14,
 #ifdef STM32_TIM15_BASE
-	Timer15
+	Timer15,
+#endif
+#ifdef STM32_TIM16_BASE
+	Timer16,
+#endif
+#ifdef STM32_TIM17_BASE
+	Timer17
 #endif
 };
 enum Channel {
@@ -161,6 +167,16 @@ static inline constexpr uint32_t timerBaseRegister(Timer::Timer timer)
 	case Timer::Timer15: return STM32_TIM15_BASE;
 #endif
 
+#ifdef STM32_TIM16_BASE
+
+	case Timer::Timer16: return STM32_TIM16_BASE;
+#endif
+
+#ifdef STM32_TIM17_BASE
+
+	case Timer::Timer17: return STM32_TIM17_BASE;
+#endif
+
 	default: break;
 	}
 

platforms/nuttx/src/px4/stm/stm32h7/include/px4_arch/hw_description.h

@@ -86,6 +86,8 @@ static inline constexpr uint32_t getTimerUpdateDMAMap(Timer::Timer timer, const
 	case Timer::Timer13:
 	case Timer::Timer14:
 	case Timer::Timer15:
+	case Timer::Timer16:
+	case Timer::Timer17:
 		break;
 	}
 

platforms/nuttx/src/px4/stm/stm32h7/include/px4_arch/io_timer_hw_description.h

@@ -49,6 +49,8 @@ static inline constexpr timer_io_channels_t initIOTimerGPIOInOut(Timer::TimerCha
 	switch (timer.timer) {
 	case Timer::Timer1:
 	case Timer::Timer2:
+	case Timer::Timer16:
+	case Timer::Timer17:
 		gpio_af = GPIO_AF1;
 		break;
 
@@ -274,6 +276,28 @@ static inline constexpr io_timers_t initIOTimer(Timer::Timer timer, DMA dshot_dm
 		ret.vectorno =  STM32_IRQ_TIM15;
 #ifdef CONFIG_STM32_TIM15
 		nuttx_config_timer_enabled = true;
+#endif
+		break;
+
+	case Timer::Timer16:
+		ret.base = STM32_TIM16_BASE;
+		ret.clock_register = STM32_RCC_APB2ENR;
+		ret.clock_bit = RCC_APB2ENR_TIM16EN;
+		ret.clock_freq = STM32_APB2_TIM16_CLKIN;
+		ret.vectorno =  STM32_IRQ_TIM16;
+#ifdef CONFIG_STM32_TIM16
+		nuttx_config_timer_enabled = true;
+#endif
+		break;
+
+	case Timer::Timer17:
+		ret.base = STM32_TIM17_BASE;
+		ret.clock_register = STM32_RCC_APB2ENR;
+		ret.clock_bit = RCC_APB2ENR_TIM17EN;
+		ret.clock_freq = STM32_APB2_TIM17_CLKIN;
+		ret.vectorno =  STM32_IRQ_TIM17;
+#ifdef CONFIG_STM32_TIM17
+		nuttx_config_timer_enabled = true;
 #endif
 		break;
 	}

src/drivers/drv_pwm_output.h

@@ -188,12 +188,6 @@ typedef uint16_t	servo_position_t;
 /** get the maximum PWM value the output will send */
 #define PWM_SERVO_GET_MAX_PWM	_PX4_IOC(_PWM_SERVO_BASE, 19)
 
-/** force safety switch off (to disable use of safety switch) */
-#define PWM_SERVO_SET_FORCE_SAFETY_OFF		_PX4_IOC(_PWM_SERVO_BASE, 25)
-
-/** force safety switch on (to enable use of safety switch) */
-#define PWM_SERVO_SET_FORCE_SAFETY_ON		_PX4_IOC(_PWM_SERVO_BASE, 28)
-
 /*
  *
  *

src/drivers/dshot/DShot.cpp

@@ -455,29 +455,44 @@ bool DShot::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
 	} else {
 		int telemetry_index = 0;
 
-		const uint32_t reversible_outputs = _mixing_output.reversibleOutputs();
-
 		for (int i = 0; i < (int)num_outputs; i++) {
 
 			uint16_t output = outputs[i];
 
-			// DShot 3D splits the throttle ranges in two.
-			// This is in terms of DShot values, code below is in terms of actuator_output
-			// Direction 1) 48 is the slowest, 1047 is the fastest.
-			// Direction 2) 1049 is the slowest, 2047 is the fastest.
-			if (_param_dshot_3d_enable.get() || (reversible_outputs & (1u << i))) {
-				if (output >= _param_dshot_3d_dead_l.get() && output <= _param_dshot_3d_dead_h.get()) {
-					output = DSHOT_DISARM_VALUE;
-
-				} else if (output < 1000 && output > 0) { //Todo: allow actuator 0 or dshot 48 to be used
-					output = 999 - output;
-				}
-			}
-
 			if (output == DSHOT_DISARM_VALUE) {
 				up_dshot_motor_command(i, DShot_cmd_motor_stop, telemetry_index == requested_telemetry_index);
 
 			} else {
+
+				// DShot 3D splits the throttle ranges in two.
+				// This is in terms of DShot values, code below is in terms of actuator_output
+				// Direction 1) 48 is the slowest, 1047 is the fastest.
+				// Direction 2) 1049 is the slowest, 2047 is the fastest.
+				if (_param_dshot_3d_enable.get() || (_reversible_outputs & (1u << i))) {
+					if (output >= _param_dshot_3d_dead_l.get() && output < _param_dshot_3d_dead_h.get()) {
+						output = DSHOT_DISARM_VALUE;
+
+					} else {
+						bool upper_range = output >= 1000;
+
+						if (upper_range) {
+							output -= 1000;
+
+						} else {
+							output = 999 - output; // lower range is inverted
+						}
+
+						float max_output = 999.f;
+						float min_output = max_output * _param_dshot_min.get();
+						output = math::min(max_output, (min_output + output * (max_output - min_output) / max_output));
+
+						if (upper_range) {
+							output += 1000;
+						}
+
+					}
+				}
+
 				up_dshot_motor_data_set(i, math::min(output, static_cast<uint16_t>(DSHOT_MAX_THROTTLE)),
 							telemetry_index == requested_telemetry_index);
 			}
@@ -556,6 +571,13 @@ void DShot::Run()
 
 	handle_vehicle_commands();
 
+	if (!_mixing_output.armed().armed) {
+		if (_reversible_outputs != _mixing_output.reversibleOutputs()) {
+			_reversible_outputs = _mixing_output.reversibleOutputs();
+			update_params();
+		}
+	}
+
 	// check at end of cycle (updateSubscriptions() can potentially change to a different WorkQueue thread)
 	_mixing_output.updateSubscriptions(true);
 
@@ -649,6 +671,13 @@ void DShot::update_params()
 	_mixing_output.setAllMinValues(math::constrain(static_cast<int>((_param_dshot_min.get() *
 				       static_cast<float>(DSHOT_MAX_THROTTLE))),
 				       DSHOT_MIN_THROTTLE, DSHOT_MAX_THROTTLE));
+
+	// Do not use the minimum parameter for reversible outputs
+	for (unsigned i = 0; i < _num_outputs; ++i) {
+		if ((1 << i) & _reversible_outputs) {
+			_mixing_output.minValue(i) = DSHOT_MIN_THROTTLE;
+		}
+	}
 }
 
 int DShot::ioctl(file *filp, int cmd, unsigned long arg)

src/drivers/dshot/DShot.h

@@ -148,6 +148,7 @@ private:
 	void handle_vehicle_commands();
 
 	MixingOutput _mixing_output {PARAM_PREFIX, DIRECT_PWM_OUTPUT_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
+	uint32_t _reversible_outputs{};
 
 	Telemetry *_telemetry{nullptr};
 

src/drivers/imu/invensense/icm42670p/CMakeLists.txt

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2021-2022 PX4 Development Team. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions

src/drivers/imu/invensense/icm42670p/ICM42670P.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2021-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -47,7 +47,7 @@ ICM42670P::ICM42670P(const I2CSPIDriverConfig &config):
 	_px4_accel(get_device_id(), config.rotation),
 	_px4_gyro(get_device_id(), config.rotation)
 {
-	if (_drdy_gpio != 0) {
+	if (config.drdy_gpio != 0) {
 		_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed");
 	}
 
@@ -123,7 +123,7 @@ void ICM42670P::RunImpl()
 
 	switch (_state) {
 	case STATE::RESET:
-		// DEVICE_CONFIG: Software reset configuration
+		// SIGNAL_PATH_RESET: Software Reset (auto clear bit)
 		RegisterWrite(Register::BANK_0::SIGNAL_PATH_RESET, SIGNAL_PATH_RESET_BIT::SOFT_RESET_DEVICE_CONFIG);
 		_reset_timestamp = now;
 		_failure_count = 0;
@@ -133,7 +133,7 @@ void ICM42670P::RunImpl()
 
 	case STATE::WAIT_FOR_RESET:
 		if ((RegisterRead(Register::BANK_0::WHO_AM_I) == WHOAMI)
-		    && (RegisterRead(Register::BANK_0::DEVICE_CONFIG) == 0x04)
+		    && (RegisterRead(Register::BANK_0::SIGNAL_PATH_RESET) == 0x00)
 		    && (RegisterRead(Register::BANK_0::INT_STATUS) & INT_STATUS_BIT::RESET_DONE_INT)) {
 
 			// Wakeup accel and gyro and schedule remaining configuration
@@ -190,15 +190,19 @@ void ICM42670P::RunImpl()
 		break;
 
 	case STATE::FIFO_READ: {
-			uint32_t samples = 0;
+			hrt_abstime timestamp_sample = now;
+			uint8_t samples = 0;
 
 			if (_data_ready_interrupt_enabled) {
-				// scheduled from interrupt if _drdy_fifo_read_samples was set as expected
-				if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_gyro_samples) {
-					perf_count(_drdy_missed_perf);
+				// scheduled from interrupt if _drdy_timestamp_sample was set as expected
+				const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0);
+
+				if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) {
+					timestamp_sample = drdy_timestamp_sample;
+					samples = _fifo_gyro_samples;
 
 				} else {
-					samples = _fifo_gyro_samples;
+					perf_count(_drdy_missed_perf);
 				}
 
 				// push backup schedule back
@@ -209,8 +213,6 @@ void ICM42670P::RunImpl()
 				// check current FIFO count
 				const uint16_t fifo_count = FIFOReadCount();
 
-				// PX4_WARN("fifo_count = %d ",fifo_count);
-
 				if (fifo_count >= FIFO::SIZE) {
 					FIFOReset();
 					perf_count(_fifo_overflow_perf);
@@ -222,12 +224,17 @@ void ICM42670P::RunImpl()
 					// FIFO count (size in bytes)
 					samples = (fifo_count / sizeof(FIFO::DATA));
 
+					// tolerate minor jitter, leave sample to next iteration if behind by only 1
+					if (samples == _fifo_gyro_samples + 1) {
+						timestamp_sample -= static_cast<int>(FIFO_SAMPLE_DT);
+						samples--;
+					}
+
 					if (samples > FIFO_MAX_SAMPLES) {
 						// not technically an overflow, but more samples than we expected or can publish
 						FIFOReset();
 						perf_count(_fifo_overflow_perf);
 						samples = 0;
-						// PX4_WARN("samples 1  = %d ",samples);
 					}
 				}
 			}
@@ -235,12 +242,9 @@ void ICM42670P::RunImpl()
 			bool success = false;
 
 			if (samples >= 1) {
-				// PX4_WARN("samples 2  = %d ",samples);
-				if (FIFORead(now, samples)) {
+				if (FIFORead(timestamp_sample, samples)) {
 					success = true;
 
-					// PX4_WARN("success  = %d ",success);
-
 					if (_failure_count > 0) {
 						_failure_count--;
 					}
@@ -260,9 +264,11 @@ void ICM42670P::RunImpl()
 			if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) {
 				// check configuration registers periodically or immediately following any failure
 				if (RegisterCheck(_register_bank0_cfg[_checked_register_bank0])
+				    && RegisterCheck(_register_mreg1_cfg[_checked_register_mreg1])
 				   ) {
 					_last_config_check_timestamp = now;
 					_checked_register_bank0 = (_checked_register_bank0 + 1) % size_register_bank0_cfg;
+					_checked_register_mreg1 = (_checked_register_mreg1 + 1) % size_register_mreg1_cfg;
 
 				} else {
 					// register check failed, force reset
@@ -283,68 +289,8 @@ void ICM42670P::RunImpl()
 	}
 }
 
-void ICM42670P::ConfigureAccel()
-{
-	const uint8_t ACCEL_FS_SEL = RegisterRead(Register::BANK_0::ACCEL_CONFIG0) & (Bit7 | Bit6 | Bit5); // 7:5 ACCEL_FS_SEL
-
-	switch (ACCEL_FS_SEL) {
-	case ACCEL_FS_SEL_2G:
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 16384.f);
-		_px4_accel.set_range(2.f * CONSTANTS_ONE_G);
-		break;
-
-	case ACCEL_FS_SEL_4G:
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 8192.f);
-		_px4_accel.set_range(4.f * CONSTANTS_ONE_G);
-		break;
-
-	case ACCEL_FS_SEL_8G:
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 4096.f);
-		_px4_accel.set_range(8.f * CONSTANTS_ONE_G);
-		break;
-
-	case ACCEL_FS_SEL_16G:
-		_px4_accel.set_scale(CONSTANTS_ONE_G / 2048.f);
-		_px4_accel.set_range(16.f * CONSTANTS_ONE_G);
-		break;
-	}
-}
-
-void ICM42670P::ConfigureGyro()
-{
-	const uint8_t GYRO_FS_SEL = RegisterRead(Register::BANK_0::GYRO_CONFIG0) & (Bit7 | Bit6 | Bit5); // 7:5 GYRO_FS_SEL
-
-	float range_dps = 0.f;
-
-	switch (GYRO_FS_SEL) {
-
-	case GYRO_FS_SEL_250_DPS:
-		range_dps = 250.f;
-		break;
-
-	case GYRO_FS_SEL_500_DPS:
-		range_dps = 500.f;
-		break;
-
-	case GYRO_FS_SEL_1000_DPS:
-		range_dps = 1000.f;
-		break;
-
-	case GYRO_FS_SEL_2000_DPS:
-		range_dps = 2000.f;
-		break;
-	}
-
-	_px4_gyro.set_scale(math::radians(range_dps / 32768.f));
-	_px4_gyro.set_range(math::radians(range_dps));
-}
-
 void ICM42670P::ConfigureSampleRate(int sample_rate)
 {
-	if (sample_rate == 0) {
-		sample_rate = 800; // default to 800 Hz
-	}
-
 	// round down to nearest FIFO sample dt
 	const float min_interval = FIFO_SAMPLE_DT;
 	_fifo_empty_interval_us = math::max(roundf((1e6f / (float)sample_rate) / min_interval) * min_interval, min_interval);
@@ -374,8 +320,6 @@ void ICM42670P::ConfigureFIFOWatermark(uint8_t samples)
 	}
 }
 
-
-
 bool ICM42670P::Configure()
 {
 	// first set and clear all configured register bits
@@ -383,7 +327,9 @@ bool ICM42670P::Configure()
 		RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
 	}
 
-	Mreg1Config();
+	for (const auto &reg_cfg : _register_mreg1_cfg) {
+		RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
+	}
 
 	// now check that all are configured
 	bool success = true;
@@ -394,11 +340,17 @@ bool ICM42670P::Configure()
 		}
 	}
 
-	success = Mreg1Check();
+	for (const auto &reg_cfg : _register_mreg1_cfg) {
+		if (!RegisterCheck(reg_cfg)) {
+			success = false;
+		}
+	}
 
+	_px4_accel.set_scale(CONSTANTS_ONE_G / 2048.f);
+	_px4_accel.set_range(16.f * CONSTANTS_ONE_G);
 
-	ConfigureAccel();
-	ConfigureGyro();
+	_px4_gyro.set_scale(math::radians(2000.f / 32768.f));
+	_px4_gyro.set_range(math::radians(2000.f));
 
 	return success;
 }
@@ -411,11 +363,8 @@ int ICM42670P::DataReadyInterruptCallback(int irq, void *context, void *arg)
 
 void ICM42670P::DataReady()
 {
-	uint32_t expected = 0;
-
-	if (_drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) {
-		ScheduleNow();
-	}
+	_drdy_timestamp_sample.store(hrt_absolute_time());
+	ScheduleNow();
 }
 
 bool ICM42670P::DataReadyInterruptConfigure()
@@ -445,18 +394,19 @@ bool ICM42670P::RegisterCheck(const T &reg_cfg)
 	const uint8_t reg_value = RegisterRead(reg_cfg.reg);
 
 	if (reg_cfg.set_bits && ((reg_value & reg_cfg.set_bits) != reg_cfg.set_bits)) {
+		PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits);
 		success = false;
 	}
 
 	if (reg_cfg.clear_bits && ((reg_value & reg_cfg.clear_bits) != 0)) {
+		PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits);
 		success = false;
 	}
 
 	return success;
 }
 
-template <typename T>
-uint8_t ICM42670P::RegisterRead(T reg)
+uint8_t ICM42670P::RegisterRead(Register::BANK_0 reg)
 {
 	uint8_t cmd[2] {};
 	cmd[0] = static_cast<uint8_t>(reg) | DIR_READ;
@@ -464,14 +414,50 @@ uint8_t ICM42670P::RegisterRead(T reg)
 	return cmd[1];
 }
 
-template <typename T>
-void ICM42670P::RegisterWrite(T reg, uint8_t value)
+uint8_t ICM42670P::RegisterRead(Register::MREG1 reg)
+{
+	// BLK_SEL_R must be set to 0
+	RegisterWrite(Register::BANK_0::BLK_SEL_R, 0x00);
+
+	// MADDR_R must be set to 0x14 (address of the MREG1 register being accessed)
+	RegisterWrite(Register::BANK_0::MADDR_R, (uint8_t)reg);
+
+	// Wait for 10 µs
+	px4_udelay(10);
+
+	// Read register M_R to access the value in MREG1 register 0x14
+	uint8_t value = RegisterRead(Register::BANK_0::M_R);
+
+	// Wait for 10 µs
+	// Host must not access any other register for 10 µs once MREG1, MREG2 or MREG3 access is kicked off.
+	px4_udelay(10);
+
+	return value;
+}
+
+void ICM42670P::RegisterWrite(Register::BANK_0 reg, uint8_t value)
 {
 	uint8_t cmd[2] { (uint8_t)reg, value };
-
 	transfer(cmd, cmd, sizeof(cmd));
 }
 
+void ICM42670P::RegisterWrite(Register::MREG1 reg, uint8_t value)
+{
+	// BLK_SEL_W must be set to 0
+	RegisterWrite(Register::BANK_0::BLK_SEL_W, 0x00);
+
+	// MADDR_W must be set to address of the MREG1 register being accessed
+	RegisterWrite(Register::BANK_0::MADDR_W, (uint8_t)reg);
+
+	// M_W must be set to the desired value
+	RegisterWrite(Register::BANK_0::M_W, value);
+
+	// Wait for 10 µs
+	// Host must not access any other register for 10 µs once MREG1, MREG2 or MREG3 access is kicked off.
+	px4_udelay(10);
+}
+
+
 template <typename T>
 void ICM42670P::RegisterSetAndClearBits(T reg, uint8_t setbits, uint8_t clearbits)
 {
@@ -498,12 +484,10 @@ uint16_t ICM42670P::FIFOReadCount()
 	return combine(fifo_count_buf[1], fifo_count_buf[2]);
 }
 
-
-
 bool ICM42670P::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
 {
 	FIFOTransferBuffer buffer{};
-	const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 4 + 2, FIFO::SIZE);
+	const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 6, FIFO::SIZE);
 
 	if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) {
 		perf_count(_bad_transfer_perf);
@@ -518,7 +502,6 @@ bool ICM42670P::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
 
 	const uint16_t fifo_count_bytes = combine(buffer.FIFO_COUNTH, buffer.FIFO_COUNTL);
 
-
 	if (fifo_count_bytes >= FIFO::SIZE) {
 		perf_count(_fifo_overflow_perf);
 		FIFOReset();
@@ -552,6 +535,18 @@ bool ICM42670P::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
 		} else if (!(FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_GYRO)) {
 			// gyro bit not set
 			valid = false;
+
+		} else if (FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_20) {
+			// Packet does not contain a new and valid extended 20-bit data
+			valid = false;
+
+		} else if (FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ODR_ACCEL) {
+			// accel ODR changed
+			valid = false;
+
+		} else if (FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ODR_GYRO) {
+			// gyro ODR changed
+			valid = false;
 		}
 
 		if (valid) {
@@ -563,7 +558,6 @@ bool ICM42670P::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
 		}
 	}
 
-	// PX4_WARN("valid_samples  = %d ",valid_samples);
 	if (valid_samples > 0) {
 		ProcessGyro(timestamp_sample, buffer.f, valid_samples);
 		ProcessAccel(timestamp_sample, buffer.f, valid_samples);
@@ -577,18 +571,30 @@ void ICM42670P::FIFOReset()
 {
 	perf_count(_fifo_reset_perf);
 
+	// FIFO flush requires the following programming sequence:
+	//  Write FIFO_FLUSH = 1
+	//  Wait for 1.5 µs
+	//  Read FIFO_FLUSH, it should now be 0
+
 	// SIGNAL_PATH_RESET: FIFO flush
 	RegisterSetBits(Register::BANK_0::SIGNAL_PATH_RESET, SIGNAL_PATH_RESET_BIT::FIFO_FLUSH);
+	px4_udelay(2); // Wait for 1.5 µs
+
+	const uint8_t SIGNAL_PATH_RESET = RegisterRead(Register::BANK_0::SIGNAL_PATH_RESET);
+
+	if ((SIGNAL_PATH_RESET & SIGNAL_PATH_RESET_BIT::FIFO_FLUSH) != 0) {
+		PX4_DEBUG("SIGNAL_PATH_RESET FIFO_FLUSH failed");
+	}
 
 	// reset while FIFO is disabled
-	_drdy_fifo_read_samples.store(0);
+	_drdy_timestamp_sample.store(0);
 }
 
 void ICM42670P::ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples)
 {
 	sensor_accel_fifo_s accel{};
 	accel.timestamp_sample = timestamp_sample;
-	accel.samples = 0;
+	accel.samples = samples;
 	accel.dt = FIFO_SAMPLE_DT;
 
 	for (int i = 0; i < samples; i++) {
@@ -598,18 +604,15 @@ void ICM42670P::ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DA
 
 		// sensor's frame is +x forward, +y left, +z up
 		//  flip y & z to publish right handed with z down (x forward, y right, z down)
-		accel.x[accel.samples] = accel_x;
-		accel.y[accel.samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y;
-		accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z;
-		accel.samples++;
+		accel.x[i] = accel_x;
+		accel.y[i] = math::negate(accel_y);
+		accel.z[i] = math::negate(accel_z);
 	}
 
 	_px4_accel.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
 				   perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf));
 
-	if (accel.samples > 0) {
-		_px4_accel.updateFIFO(accel);
-	}
+	_px4_accel.updateFIFO(accel);
 }
 
 void ICM42670P::ProcessGyro(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples)
@@ -627,8 +630,8 @@ void ICM42670P::ProcessGyro(const hrt_abstime &timestamp_sample, const FIFO::DAT
 		// sensor's frame is +x forward, +y left, +z up
 		//  flip y & z to publish right handed with z down (x forward, y right, z down)
 		gyro.x[i] = gyro_x;
-		gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y;
-		gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z;
+		gyro.y[i] = math::negate(gyro_y);
+		gyro.z[i] = math::negate(gyro_z);
 	}
 
 	_px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
@@ -658,88 +661,3 @@ void ICM42670P::UpdateTemperature()
 		_px4_gyro.set_temperature(TEMP_degC);
 	}
 }
-
-
-uint8_t ICM42670P::RegisterReadBank1(uint8_t reg)
-{
-	uint8_t value;
-
-	RegisterWrite((uint8_t)Register::BANK_0::BLK_SEL_R, 0x00);
-	RegisterWrite((uint8_t)Register::BANK_0::MADDR_R, reg);
-	ScheduleDelayed(10_us);
-	value = RegisterRead((uint8_t)Register::BANK_0::M_R);
-	ScheduleDelayed(10_us);
-	RegisterWrite((uint8_t)Register::BANK_0::BLK_SEL_R, 0x00);
-
-	return value;
-}
-
-void ICM42670P::RegisterWriteBank1(uint8_t reg, uint8_t value)
-{
-	RegisterWrite((uint8_t)Register::BANK_0::BLK_SEL_W, 0x00);
-	RegisterWrite((uint8_t)Register::BANK_0::MADDR_W, reg);
-	RegisterWrite((uint8_t)Register::BANK_0::M_W, value);
-	ScheduleDelayed(10_us);
-	RegisterWrite((uint8_t)Register::BANK_0::BLK_SEL_W, 0x00);
-}
-
-void ICM42670P::Mreg1Config()
-{
-	uint8_t data;
-	uint8_t set_bits;
-	uint8_t clear_bits;
-
-	clear_bits = Bit7 | Bit6 | Bit4 | Bit3;
-	set_bits = FIFO_CONFIG5_BIT::FIFO_WM_GT_TH  | FIFO_CONFIG5_BIT::FIFO_TMST_FSYNC_EN | FIFO_CONFIG5_BIT::FIFO_GYRO_EN |
-		   FIFO_CONFIG5_BIT::FIFO_ACCEL_EN;
-
-	data = RegisterReadBank1(0x01);
-	data &= ~clear_bits;
-	data |= set_bits;
-	RegisterWriteBank1(0x01, data);
-
-
-	clear_bits = Bit7 | Bit6 | Bit5 | Bit4 | Bit2 | Bit1 | Bit0;
-	set_bits = INT_CONFIG0_BIT::CLEAR_ON_FIFO_READ;
-	data = RegisterReadBank1(0x04);
-	data &= ~clear_bits;
-	data |= set_bits;
-	RegisterWriteBank1(0x04, data);
-}
-
-bool ICM42670P::Mreg1Check()
-{
-	uint8_t set_bits;
-	uint8_t clear_bits;
-	uint8_t reg_value;
-	bool success;
-	success = true;
-
-	reg_value = RegisterReadBank1(0x01);
-
-	clear_bits = Bit7 | Bit6 | Bit4 | Bit3;
-	set_bits = FIFO_CONFIG5_BIT::FIFO_WM_GT_TH  | FIFO_CONFIG5_BIT::FIFO_TMST_FSYNC_EN | FIFO_CONFIG5_BIT::FIFO_GYRO_EN |
-		   FIFO_CONFIG5_BIT::FIFO_ACCEL_EN;
-
-	if (set_bits && ((reg_value & set_bits) != set_bits)) {
-		success = false;
-	}
-
-	if (clear_bits && ((reg_value & clear_bits) != 0)) {
-		success = false;
-	}
-
-	reg_value = RegisterReadBank1(0x04);
-	clear_bits = Bit7 | Bit6 | Bit5 | Bit4 | Bit2 | Bit1 | Bit0;
-	set_bits = INT_CONFIG0_BIT::CLEAR_ON_FIFO_READ;
-
-	if (set_bits && ((reg_value & set_bits) != set_bits)) {
-		success = false;
-	}
-
-	if (clear_bits && ((reg_value & clear_bits) != 0)) {
-		success = false;
-	}
-
-	return success;
-}

src/drivers/imu/invensense/icm42670p/ICM42670P.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2021-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -57,6 +57,7 @@ class ICM42670P : public device::SPI, public I2CSPIDriver<ICM42670P>
 public:
 	ICM42670P(const I2CSPIDriverConfig &config);
 	~ICM42670P() override;
+
 	static void print_usage();
 
 	void RunImpl();
@@ -68,12 +69,12 @@ private:
 	void exit_and_cleanup() override;
 
 	// Sensor Configuration
-	static constexpr float FIFO_SAMPLE_DT{1e6f / 800.f};     // 8000 Hz accel & gyro ODR configured
+	static constexpr float FIFO_SAMPLE_DT{1e6f / 1600.f};     // 1600 Hz accel & gyro ODR configured
 	static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT};
 	static constexpr float ACCEL_RATE{1e6f / FIFO_SAMPLE_DT};
 
 	// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
-	static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
+	static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
 
 	// Transfer data
 	struct FIFOTransferBuffer {
@@ -85,10 +86,8 @@ private:
 		uint8_t FIFO_COUNTL{0};
 		FIFO::DATA f[FIFO_MAX_SAMPLES] {};
 	};
-
-
 	// ensure no struct padding
-	static_assert(sizeof(FIFOTransferBuffer) == (4 + 2 + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)));
+	static_assert(sizeof(FIFOTransferBuffer) == (6 + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)));
 
 	struct register_bank0_config_t {
 		Register::BANK_0 reg;
@@ -96,13 +95,17 @@ private:
 		uint8_t clear_bits{0};
 	};
 
+	struct register_mreg1_config_t {
+		Register::MREG1 reg;
+		uint8_t set_bits{0};
+		uint8_t clear_bits{0};
+	};
+
 	int probe() override;
 
 	bool Reset();
 
 	bool Configure();
-	void ConfigureAccel();
-	void ConfigureGyro();
 	void ConfigureSampleRate(int sample_rate);
 	void ConfigureFIFOWatermark(uint8_t samples);
 
@@ -111,9 +114,13 @@ private:
 	bool DataReadyInterruptConfigure();
 	bool DataReadyInterruptDisable();
 
+	uint8_t RegisterRead(Register::BANK_0 reg);
+	uint8_t RegisterRead(Register::MREG1 reg);
+
+	void RegisterWrite(Register::BANK_0 reg, uint8_t value);
+	void RegisterWrite(Register::MREG1 reg, uint8_t value);
+
 	template <typename T> bool RegisterCheck(const T &reg_cfg);
-	template <typename T> uint8_t RegisterRead(T reg);
-	template <typename T> void RegisterWrite(T reg, uint8_t value);
 	template <typename T> void RegisterSetAndClearBits(T reg, uint8_t setbits, uint8_t clearbits);
 	template <typename T> void RegisterSetBits(T reg, uint8_t setbits) { RegisterSetAndClearBits(reg, setbits, 0); }
 	template <typename T> void RegisterClearBits(T reg, uint8_t clearbits) { RegisterSetAndClearBits(reg, 0, clearbits); }
@@ -126,12 +133,6 @@ private:
 	void ProcessGyro(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples);
 	void UpdateTemperature();
 
-	uint8_t RegisterReadBank1(uint8_t reg);
-	void RegisterWriteBank1(uint8_t reg, uint8_t value);
-
-	void Mreg1Config();
-	bool Mreg1Check();
-
 	const spi_drdy_gpio_t _drdy_gpio;
 
 	PX4Accelerometer _px4_accel;
@@ -149,9 +150,7 @@ private:
 	hrt_abstime _temperature_update_timestamp{0};
 	int _failure_count{0};
 
-	enum REG_BANK_SEL_BIT _last_register_bank {REG_BANK_SEL_BIT::USER_BANK_0};
-
-	px4::atomic<uint32_t> _drdy_fifo_read_samples{0};
+	px4::atomic<hrt_abstime> _drdy_timestamp_sample{0};
 	bool _data_ready_interrupt_enabled{false};
 
 	enum class STATE : uint8_t {
@@ -159,26 +158,32 @@ private:
 		WAIT_FOR_RESET,
 		CONFIGURE,
 		FIFO_READ,
-	};
-
-	STATE _state{STATE::RESET};
+	} _state{STATE::RESET};
 
 	uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
-	uint32_t _fifo_gyro_samples{static_cast<uint32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
+	int32_t _fifo_gyro_samples{static_cast<int32_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
 
 	uint8_t _checked_register_bank0{0};
 	static constexpr uint8_t size_register_bank0_cfg{10};
 	register_bank0_config_t _register_bank0_cfg[size_register_bank0_cfg] {
-		// Register                        | Set bits, Clear bits
-		{ Register::BANK_0::INT_CONFIG,    INT_CONFIG_BIT::INT1_MODE | INT_CONFIG_BIT::INT1_DRIVE_CIRCUIT, INT_CONFIG_BIT::INT1_POLARITY },
-		{ Register::BANK_0::FIFO_CONFIG1,   FIFO_CONFIG1_BIT::FIFO_MODE_STOP_ON_FULL, Bit0 },
-		{ Register::BANK_0::PWR_MGMT0,     PWR_MGMT0_BIT::GYRO_MODE_LOW_NOISE | PWR_MGMT0_BIT::ACCEL_MODE_LOW_NOISE, 0 },
-		{ Register::BANK_0::GYRO_CONFIG0,  GYRO_CONFIG0_BIT::GYRO_ODR_1600Hz | GYRO_CONFIG0_BIT::GYRO_FS_SEL_2000_DPS, Bit7 | Bit3 | Bit0 },
-		{ Register::BANK_0::ACCEL_CONFIG0, ACCEL_CONFIG0_BIT::ACCEL_ODR_1600Hz | ACCEL_CONFIG0_BIT::ACCEL_FS_SEL_16G, Bit6 | Bit5 | Bit3 | Bit0 },
-		{ Register::BANK_0::GYRO_CONFIG1,  GYRO_CONFIG1_BIT::GYRO_UI_FILT_BW_34Hz, Bit1 },
-		{ Register::BANK_0::ACCEL_CONFIG1, ACCEL_CONFIG1_BIT::ACCEL_UI_FILT_BW_34Hz, Bit1 },
-		{ Register::BANK_0::FIFO_CONFIG2,  0, 0 }, // FIFO_WM[7:0] set at runtime
-		{ Register::BANK_0::FIFO_CONFIG3,  0, 0 }, // FIFO_WM[11:8] set at runtime
-		{ Register::BANK_0::INT_SOURCE0,   INT_SOURCE0_BIT::FIFO_THS_INT1_EN, 0 },
+		// Register                              | Set bits, Clear bits
+		{ Register::BANK_0::INT_CONFIG,           INT_CONFIG_BIT::INT1_MODE | INT_CONFIG_BIT::INT1_DRIVE_CIRCUIT, INT_CONFIG_BIT::INT1_POLARITY },
+		{ Register::BANK_0::PWR_MGMT0,            PWR_MGMT0_BIT::GYRO_MODE_LOW_NOISE | PWR_MGMT0_BIT::ACCEL_MODE_LOW_NOISE, 0 },
+		{ Register::BANK_0::GYRO_CONFIG0,         GYRO_CONFIG0_BIT::GYRO_FS_SEL_2000_DPS_SET | GYRO_CONFIG0_BIT::GYRO_ODR_1600HZ_SET, GYRO_CONFIG0_BIT::GYRO_FS_SEL_2000_DPS_CLEAR | GYRO_CONFIG0_BIT::GYRO_ODR_1600HZ_CLEAR },
+		{ Register::BANK_0::ACCEL_CONFIG0,        ACCEL_CONFIG0_BIT::ACCEL_UI_FS_SEL_16G_SET | ACCEL_CONFIG0_BIT::ACCEL_ODR_1600HZ_SET, ACCEL_CONFIG0_BIT::ACCEL_UI_FS_SEL_16G_SET | ACCEL_CONFIG0_BIT::ACCEL_ODR_1600HZ_CLEAR },
+		{ Register::BANK_0::GYRO_CONFIG1,         0, GYRO_CONFIG1_BIT::GYRO_UI_FILT_BW_BYPASSED_CLEAR },
+		{ Register::BANK_0::ACCEL_CONFIG1,        0, ACCEL_CONFIG1_BIT::ACCEL_UI_FILT_BW_BYPASSED_CLEAR },
+		{ Register::BANK_0::FIFO_CONFIG1,         FIFO_CONFIG1_BIT::FIFO_MODE_STOP_ON_FULL, FIFO_CONFIG1_BIT::FIFO_BYPASS },
+		{ Register::BANK_0::FIFO_CONFIG2,         0, 0 }, // FIFO_WM[7:0] set at runtime
+		{ Register::BANK_0::FIFO_CONFIG3,         0, 0 }, // FIFO_WM[11:8] set at runtime
+		{ Register::BANK_0::INT_SOURCE0,          INT_SOURCE0_BIT::FIFO_THS_INT1_EN, 0 },
+	};
+
+	uint8_t _checked_register_mreg1{0};
+	static constexpr uint8_t size_register_mreg1_cfg{2};
+	register_mreg1_config_t _register_mreg1_cfg[size_register_mreg1_cfg] {
+		// Register                              | Set bits, Clear bits
+		{ Register::MREG1::FIFO_CONFIG5,          FIFO_CONFIG5_BIT::FIFO_GYRO_EN | FIFO_CONFIG5_BIT::FIFO_ACCEL_EN, 0 },
+		{ Register::MREG1::INT_CONFIG0,           INT_CONFIG0_BIT::FIFO_THS_INT_CLEAR, 0 },
 	};
 };

src/drivers/imu/invensense/icm42670p/InvenSense_ICM42670P_registers.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2021-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -54,7 +54,7 @@ static constexpr uint8_t Bit5 = (1 << 5);
 static constexpr uint8_t Bit6 = (1 << 6);
 static constexpr uint8_t Bit7 = (1 << 7);
 
-static constexpr uint32_t SPI_SPEED = 12 * 1000 * 1000; // 24 MHz SPI
+static constexpr uint32_t SPI_SPEED = 24 * 1000 * 1000; // 24 MHz SPI
 static constexpr uint8_t DIR_READ = 0x80;
 
 static constexpr uint8_t WHOAMI = 0x67;
@@ -66,21 +66,13 @@ namespace Register
 {
 
 enum class BANK_0 : uint8_t {
-	DEVICE_CONFIG     = 0x01,
+	SIGNAL_PATH_RESET = 0x02,
 
 	INT_CONFIG        = 0x06,
 
-
 	TEMP_DATA1        = 0x09,
 	TEMP_DATA0        = 0x0A,
 
-	INT_STATUS        = 0x3A,
-	FIFO_COUNTH       = 0x3D,
-	FIFO_COUNTL       = 0x3E,
-	FIFO_DATA         = 0x3F,
-
-	SIGNAL_PATH_RESET = 0x02,
-
 	PWR_MGMT0         = 0x1F,
 	GYRO_CONFIG0      = 0x20,
 	ACCEL_CONFIG0     = 0x21,
@@ -90,87 +82,93 @@ enum class BANK_0 : uint8_t {
 	FIFO_CONFIG1      = 0x28,
 	FIFO_CONFIG2      = 0x29,
 	FIFO_CONFIG3      = 0x2A,
-
 	INT_SOURCE0       = 0x2B,
 
+	INT_STATUS        = 0x3A,
+	FIFO_COUNTH       = 0x3D,
+	FIFO_COUNTL       = 0x3E,
+	FIFO_DATA         = 0x3F,
+
 	WHO_AM_I          = 0x75,
-	// REG_BANK_SEL      = 0x76,
 
 	BLK_SEL_W         = 0x79,
 	MADDR_W           = 0x7A,
 	M_W               = 0x7B,
-
 	BLK_SEL_R         = 0x7C,
 	MADDR_R           = 0x7D,
 	M_R               = 0x7E,
-
 };
 
-enum class MREG_1 : uint8_t {
-	FIFO_CONFIG5_MREG1      = 0x01,
-	INT_CONFIG0_MREG1       = 0x04,
+enum class MREG1 : uint8_t {
+	FIFO_CONFIG5 = 0x01,
+
+	INT_CONFIG0  = 0x04,
 };
 
-enum class MREG_2 : uint8_t {
-	OTP_CTRL7_MREG2  = 0x06,
-};
-
-enum class MREG_3 : uint8_t {
-	XA_ST_DATA_MREG3 = 0x00,
-	YA_ST_DATA_MREG3 = 0x01,
-	ZA_ST_DATA_MREG3 = 0x02,
-	XG_ST_DATA_MREG3 = 0x03,
-	YG_ST_DATA_MREG3 = 0x04,
-	ZG_ST_DATA_MREG3 = 0x05,
-};
-
-
-
 };
 
 //---------------- BANK0 Register bits
 
 // SIGNAL_PATH_RESET
 enum SIGNAL_PATH_RESET_BIT : uint8_t {
-	SOFT_RESET_DEVICE_CONFIG  = Bit4, //
-	FIFO_FLUSH                = Bit2,
+	SOFT_RESET_DEVICE_CONFIG  = Bit4, // 1: Software reset enabled
+	FIFO_FLUSH                = Bit2, // When set to 1, FIFO will get flushed
 };
 
 // INT_CONFIG
 enum INT_CONFIG_BIT : uint8_t {
-	INT1_MODE           = Bit2,
-	INT1_DRIVE_CIRCUIT  = Bit1,
-	INT1_POLARITY       = Bit0,
+	INT1_MODE          = Bit2, // INT1 interrupt mode 1: Latched mode
+	INT1_DRIVE_CIRCUIT = Bit1, // INT1 drive circuit 1: Push pull
+	INT1_POLARITY      = Bit0, // INT1 interrupt polarity 0: Active low
 };
+
+// PWR_MGMT0
+enum PWR_MGMT0_BIT : uint8_t {
+	// 3:2 GYRO_MODE
+	GYRO_MODE_LOW_NOISE  = Bit3 | Bit2, // 11: Places gyroscope in Low Noise (LN) Mode
+	// 1:0 ACCEL_MODE
+	ACCEL_MODE_LOW_NOISE = Bit1 | Bit0, // 11: Places accelerometer in Low Noise (LN) Mode
+};
+
+// GYRO_CONFIG0
+enum GYRO_CONFIG0_BIT : uint8_t {
+	// 6:5 GYRO_FS_SEL
+	GYRO_FS_SEL_2000_DPS_SET   = 0,           // 0b000 = ±2000dps
+	GYRO_FS_SEL_2000_DPS_CLEAR = Bit6 | Bit5,
+
+	// 3:0 GYRO_ODR
+	GYRO_ODR_1600HZ_SET        = Bit2 | Bit0, // 0101: 1600Hz
+	GYRO_ODR_1600HZ_CLEAR      = Bit3 | Bit1,
+};
+
+// ACCEL_CONFIG0
+enum ACCEL_CONFIG0_BIT : uint8_t {
+	// 6:5 ACCEL_UI_FS_SEL
+	ACCEL_UI_FS_SEL_16G_SET   = 0,           // 000: ±16g
+	ACCEL_UI_FS_SEL_16G_CLEAR = Bit6 | Bit5,
+
+	// 3:0 ACCEL_ODR
+	ACCEL_ODR_1600HZ_SET      = Bit2 | Bit0, // 0101: 1600Hz
+	ACCEL_ODR_1600HZ_CLEAR    = Bit3 | Bit1,
+};
+
 // GYRO_CONFIG1
 enum GYRO_CONFIG1_BIT : uint8_t {
-
-	// 2:0 GYRO_ODR
-	GYRO_UI_FILT_BW_16Hz   = Bit2 | Bit1 | Bit0,   // 111: 16Hz
-	GYRO_UI_FILT_BW_25Hz   = Bit2 | Bit1,          // 110: 25Hz
-	GYRO_UI_FILT_BW_34Hz   = Bit2 | Bit0,  	// 101: 34Hz
-	GYRO_UI_FILT_BW_53Hz   = Bit2,         	// 100: 53Hz
-	GYRO_UI_FILT_BW_73Hz   = Bit1 | Bit0,  	// 011: 73Hz
-	GYRO_UI_FILT_BW_121Hz  = Bit1,        		// 010: 121Hz
-	GYRO_UI_FILT_BW_180Hz  = Bit0,        		// 001: 180Hz
+	// 2:0 GYRO_UI_FILT_BW
+	GYRO_UI_FILT_BW_BYPASSED_CLEAR = Bit2 | Bit1 | Bit0, // 000: Low pass filter bypassed
 };
 
 // ACCEL_CONFIG1
 enum ACCEL_CONFIG1_BIT : uint8_t {
-
-	// 2:0 ACCEL_ODR
-	ACCEL_UI_FILT_BW_16Hz   = Bit2 | Bit1 | Bit0,   // 111: 16Hz
-	ACCEL_UI_FILT_BW_25Hz   = Bit2 | Bit1,          // 110: 25Hz
-	ACCEL_UI_FILT_BW_34Hz   = Bit2 | Bit0,  	// 101: 34Hz
-	ACCEL_UI_FILT_BW_53Hz   = Bit2,         	// 100: 53Hz
-	ACCEL_UI_FILT_BW_73Hz   = Bit1 | Bit0,  	// 011: 73Hz
-	ACCEL_UI_FILT_BW_121Hz  = Bit1,        		// 010: 121Hz
-	ACCEL_UI_FILT_BW_180Hz  = Bit0,        		// 001: 180Hz
+	// 2:0 ACCEL_UI_FILT_BW
+	ACCEL_UI_FILT_BW_BYPASSED_CLEAR = Bit2 | Bit1 | Bit0, // 000: Low pass filter bypassed
 };
+
 // FIFO_CONFIG1
 enum FIFO_CONFIG1_BIT : uint8_t {
-	// 1 FIFO_MODE
-	FIFO_MODE_STOP_ON_FULL = Bit1, // 11: STOP-on-FULL Mode
+	// FIFO_MODE
+	FIFO_MODE_STOP_ON_FULL = Bit1, // 1: STOP-on-FULL Mode
+	FIFO_BYPASS            = Bit0, // 0: FIFO is not bypassed
 };
 
 // INT_STATUS
@@ -180,113 +178,63 @@ enum INT_STATUS_BIT : uint8_t {
 	FIFO_FULL_INT  = Bit1,
 };
 
-// PWR_MGMT0
-enum PWR_MGMT0_BIT : uint8_t {
-	GYRO_MODE_LOW_NOISE  = Bit3 | Bit2, // 11: Places gyroscope in Low Noise (LN) Mode
-	ACCEL_MODE_LOW_NOISE = Bit1 | Bit0, // 11: Places accelerometer in Low Noise (LN) Mode
+// INT_SOURCE0
+enum INT_SOURCE0_BIT : uint8_t {
+	RESET_DONE_INT1_EN = Bit4, // 1: Reset done interrupt routed to INT1
+	FIFO_THS_INT1_EN   = Bit2, // FIFO threshold interrupt routed to INT1
 };
 
-// GYRO_CONFIG0
-enum GYRO_CONFIG0_BIT : uint8_t {
-	// 6:5 GYRO_FS_SEL
-	GYRO_FS_SEL_2000_DPS = 0,            // 0b000 = ±2000dps
-	GYRO_FS_SEL_1000_DPS = Bit5,         // 0b001 = ±1000 dps
-	GYRO_FS_SEL_500_DPS  = Bit6,         // 0b010 = ±500 dps
-	GYRO_FS_SEL_250_DPS  = Bit6 | Bit5,  // 0b011 = ±250 dps
 
-	// 3:0 GYRO_ODR
-	GYRO_ODR_1600Hz      = Bit2 | Bit0,         // 0101: 1600Hz
-	GYRO_ODR_800Hz       = Bit2 | Bit1,         // 0110: 800Hz
-	GYRO_ODR_400Hz       = Bit2 | Bit1 | Bit0,  // 0111: 400Hz
-	GYRO_ODR_200Hz       = Bit3,         // 1000: 200Hz
-};
-
-// ACCEL_CONFIG0
-enum ACCEL_CONFIG0_BIT : uint8_t {
-	// 6:5 ACCEL_FS_SEL
-	ACCEL_FS_SEL_16G = 0,           // 000: ±16g
-	ACCEL_FS_SEL_8G  = Bit5,        // 001: ±8g
-	ACCEL_FS_SEL_4G  = Bit6,        // 010: ±4g
-	ACCEL_FS_SEL_2G  = Bit6 | Bit5, // 011: ±2g
-
-	// 3:0 ACCEL_ODR
-	ACCEL_ODR_1600Hz  = Bit2 | Bit0,         // 0101: 1600Hz
-	ACCEL_ODR_800Hz  = Bit2 | Bit1,         // 0110: 800Hz
-	ACCEL_ODR_400Hz   = Bit2 | Bit1 | Bit0,  // 0111: 400Hz
-	ACCEL_ODR_200Hz   = Bit3,         	// 1000: 200Hz
-};
+//---------------- USER BANK MREG1 Register bits
 
 // FIFO_CONFIG5
 enum FIFO_CONFIG5_BIT : uint8_t {
-	FIFO_RESUME_PARTIAL_RD = Bit4,
-	FIFO_WM_GT_TH          = Bit5,
-	FIFO_HIRES_EN          = Bit3,
-	FIFO_TMST_FSYNC_EN     = Bit2,
-	FIFO_GYRO_EN           = Bit1,
-	FIFO_ACCEL_EN          = Bit0,
+	FIFO_WM_GT_TH = Bit5, // 0: Trigger FIFO Watermark interrupt when FIFO_COUNT = FIFO_WM
+	FIFO_GYRO_EN  = Bit1, // 1: Enables Gyro packets to go to FIFO
+	FIFO_ACCEL_EN = Bit0, // 1: Enables Accel packets to go to FIFO
 };
 
 // INT_CONFIG0
 enum INT_CONFIG0_BIT : uint8_t {
 	// 3:2 FIFO_THS_INT_CLEAR
-	CLEAR_ON_FIFO_READ = Bit3,
-};
-
-// INT_SOURCE0
-enum INT_SOURCE0_BIT : uint8_t {
-	ST_INT1_END        = Bit7,
-	FSYNC_INT1_EN      = Bit6,
-	PLL_RDY_INT1_EN    = Bit5,
-	RESET_DONE_INT1_EN = Bit4,
-	DRDY_INT1_EN       = Bit3,
-	FIFO_THS_INT1_EN   = Bit2, // FIFO threshold interrupt routed to INT1
-	FIFO_FULL_INT1_EN  = Bit1,
-	AGC_RDY_INT1_EN    = Bit0,
-};
-
-// REG_BANK_SEL
-enum REG_BANK_SEL_BIT : uint8_t {
-	USER_BANK_0 = 0,           // 0: Select USER BANK 0.
-	USER_BANK_1 = Bit4,        // 1: Select USER BANK 1.
-	USER_BANK_2 = Bit5,        // 2: Select USER BANK 2.
-	USER_BANK_3 = Bit5 | Bit4, // 3: Select USER BANK 3.
+	FIFO_THS_INT_CLEAR = Bit3, // 10: Clear on FIFO data 1Byte Read
 };
 
 namespace FIFO
 {
-static constexpr size_t SIZE = 2048;
+static constexpr size_t SIZE = 1024;
 
 // FIFO_DATA layout when FIFO_CONFIG1 has FIFO_GYRO_EN and FIFO_ACCEL_EN set
 
 // Packet 3
 struct DATA {
 	uint8_t FIFO_Header;
-	uint8_t ACCEL_DATA_X1;
-	uint8_t ACCEL_DATA_X0;
-	uint8_t ACCEL_DATA_Y1;
-	uint8_t ACCEL_DATA_Y0;
-	uint8_t ACCEL_DATA_Z1;
-	uint8_t ACCEL_DATA_Z0;
-	uint8_t GYRO_DATA_X1;
-	uint8_t GYRO_DATA_X0;
-	uint8_t GYRO_DATA_Y1;
-	uint8_t GYRO_DATA_Y0;
-	uint8_t GYRO_DATA_Z1;
-	uint8_t GYRO_DATA_Z0;
-	uint8_t temperature;  // Temperature[7:0]
-	uint8_t timestamp_l;
-	uint8_t timestamp_h;
+	uint8_t ACCEL_DATA_X1; // Accel X [19:12]
+	uint8_t ACCEL_DATA_X0; // Accel X [11:4]
+	uint8_t ACCEL_DATA_Y1; // Accel Y [19:12]
+	uint8_t ACCEL_DATA_Y0; // Accel Y [11:4]
+	uint8_t ACCEL_DATA_Z1; // Accel Z [19:12]
+	uint8_t ACCEL_DATA_Z0; // Accel Z [11:4]
+	uint8_t GYRO_DATA_X1;  // Gyro X [19:12]
+	uint8_t GYRO_DATA_X0;  // Gyro X [11:4]
+	uint8_t GYRO_DATA_Y1;  // Gyro Y [19:12]
+	uint8_t GYRO_DATA_Y0;  // Gyro Y [11:4]
+	uint8_t GYRO_DATA_Z1;  // Gyro Z [19:12]
+	uint8_t GYRO_DATA_Z0;  // Gyro Z [11:4]
+	uint8_t temperature;   // Temperature[7:0]
+	uint8_t TimeStamp_h;   // TimeStamp[15:8]
+	uint8_t TimeStamp_l;   // TimeStamp[7:0]
 };
 
 // With FIFO_ACCEL_EN and FIFO_GYRO_EN header should be 8’b_0110_10xx
 enum FIFO_HEADER_BIT : uint8_t {
 	HEADER_MSG             = Bit7, // 1: FIFO is empty
-	HEADER_ACCEL           = Bit6,
-	HEADER_GYRO            = Bit5,
-	HEADER_20              = Bit4,
+	HEADER_ACCEL           = Bit6, // 1: Packet is sized so that accel data have location in the packet, FIFO_ACCEL_EN must be 1
+	HEADER_GYRO            = Bit5, // 1: Packet is sized so that gyro data have location in the packet, FIFO_GYRO_EN must be1
+	HEADER_20              = Bit4, // 1: Packet has a new and valid sample of extended 20-bit data for gyro and/or accel
 	HEADER_TIMESTAMP_FSYNC = Bit3 | Bit2,
-	HEADER_ODR_ACCEL       = Bit1,
-	HEADER_ODR_GYRO        = Bit0,
+	HEADER_ODR_ACCEL       = Bit1, // 1: The ODR for accel is different for this accel data packet compared to the previous accel packet
+	HEADER_ODR_GYRO        = Bit0, // 1: The ODR for gyro is different for this gyro data packet compared to the previous gyro packet
 };
 
 }

src/drivers/pca9685_pwm_out/main.cpp

@@ -492,9 +492,7 @@ int PCA9685Wrapper::ioctl(cdev::file_t *filep, int cmd, unsigned long arg)
 		break;
 
 	case PWM_SERVO_SET_ARM_OK:
-	case PWM_SERVO_SET_FORCE_SAFETY_OFF:
 	case PWM_SERVO_CLEAR_ARM_OK:
-	case PWM_SERVO_SET_FORCE_SAFETY_ON:
 	case PWM_SERVO_ARM:
 	case PWM_SERVO_DISARM:
 		break;

src/drivers/pwm_out/PWMOut.cpp

@@ -717,8 +717,6 @@ int PWMOut::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg)
 
 	case PWM_SERVO_SET_ARM_OK:
 	case PWM_SERVO_CLEAR_ARM_OK:
-	case PWM_SERVO_SET_FORCE_SAFETY_OFF:
-	case PWM_SERVO_SET_FORCE_SAFETY_ON:
 		break;
 
 	case PWM_SERVO_DISARM:

src/drivers/px4io/px4io.cpp

@@ -74,6 +74,7 @@
 #include <uORB/topics/input_rc.h>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_command_ack.h>
+#include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/px4io_status.h>
 #include <uORB/topics/parameter_update.h>
 
@@ -196,7 +197,8 @@ private:
 	uint16_t		_last_written_arming_c{0};	///< the last written arming state reg
 
 	uORB::Subscription	_t_actuator_armed{ORB_ID(actuator_armed)};		///< system armed control topic
-	uORB::Subscription	_t_vehicle_command{ORB_ID(vehicle_command)};		///< vehicle command topic
+	uORB::Subscription	_t_vehicle_command{ORB_ID(vehicle_command)};	///< vehicle command topic
+	uORB::Subscription	_t_vehicle_status{ORB_ID(vehicle_status)};		///< vehicle status topic
 
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 
@@ -443,15 +445,14 @@ int PX4IO::init()
 		// the startup script to be able to load a new IO
 		// firmware
 
-		// If IO has already safety off it won't accept going into bootloader mode,
-		// therefore we need to set safety on first.
-		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_ON, PX4IO_FORCE_SAFETY_MAGIC);
-
 		// Now the reboot into bootloader mode should succeed.
 		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_REBOOT_BL, PX4IO_REBOOT_BL_MAGIC);
 		return -1;
 	}
 
+	/* Set safety_off to false when FMU boot*/
+	io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SAFETY_OFF, 0);
+
 	if (_max_rc_input > input_rc_s::RC_INPUT_MAX_CHANNELS) {
 		_max_rc_input = input_rc_s::RC_INPUT_MAX_CHANNELS;
 	}
@@ -476,14 +477,6 @@ int PX4IO::init()
 		return ret;
 	}
 
-	/* set safety to off if circuit breaker enabled */
-	if (circuit_breaker_enabled("CBRK_IO_SAFETY", CBRK_IO_SAFETY_KEY)) {
-		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_OFF, PX4IO_FORCE_SAFETY_MAGIC);
-
-	} else {
-		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_ON, PX4IO_FORCE_SAFETY_MAGIC);
-	}
-
 	/* try to claim the generic PWM output device node as well - it's OK if we fail at this */
 	if (_param_sys_hitl.get() <= 0 && _param_sys_use_io.get() == 1) {
 		_class_instance = register_class_devname(PWM_OUTPUT_BASE_DEVICE_PATH);
@@ -621,11 +614,6 @@ void PX4IO::Run()
 
 			update_params();
 
-			/* Check if the IO safety circuit breaker has been updated */
-			bool circuit_breaker_io_safety_enabled = circuit_breaker_enabled("CBRK_IO_SAFETY", CBRK_IO_SAFETY_KEY);
-			/* Bypass IO safety switch logic by setting FORCE_SAFETY_OFF */
-			io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_OFF, circuit_breaker_io_safety_enabled);
-
 			/* Check if the flight termination circuit breaker has been updated */
 			bool disable_flighttermination = circuit_breaker_enabled("CBRK_FLIGHTTERM", CBRK_FLIGHTTERM_KEY);
 			/* Tell IO that it can terminate the flight if FMU is not responding or if a failure has been reported by the FailureDetector logic */
@@ -974,15 +962,12 @@ int PX4IO::io_handle_status(uint16_t status)
 	 */
 
 	/* check for IO reset - force it back to armed if necessary */
-	if (_status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF && !(status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)
-	    && !(status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) {
+	if (!(status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) {
 		/* set the arming flag */
-		ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0,
-				    PX4IO_P_STATUS_FLAGS_SAFETY_OFF | PX4IO_P_STATUS_FLAGS_ARM_SYNC);
+		ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_ARM_SYNC);
 
 		/* set new status */
 		_status = status;
-		_status &= PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
 
 	} else if (!(_status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) {
 
@@ -999,18 +984,26 @@ int PX4IO::io_handle_status(uint16_t status)
 	}
 
 	/**
-	 * Get and handle the safety status
+	 * Get and handle the safety button status
 	 */
-	const bool safety_off = status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
+	const bool safety_button_pressed = status & PX4IO_P_STATUS_FLAGS_SAFETY_BUTTON_EVENT;
 
-	/* px4io board will change safety_off from false to true and stay like that until the vehicle is rebooted
-	 * where safety will change back to false. Here we are triggering the safety button event once.
-	 * TODO: change px4io firmware to act on the event. This will enable the "force safety on disarming" feature. */
-	if (_previous_safety_off != safety_off) {
+	if (safety_button_pressed) {
+		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SAFETY_BUTTON_ACK, 0);
 		_button_publisher.safetyButtonTriggerEvent();
 	}
 
-	_previous_safety_off = safety_off;
+	/**
+	 * Inform PX4IO board about safety_off state for LED control
+	 */
+	vehicle_status_s vehicle_status;
+
+	if (_t_vehicle_status.update(&vehicle_status)) {
+		if (_previous_safety_off != vehicle_status.safety_off) {
+			io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SAFETY_OFF, vehicle_status.safety_off);
+			_previous_safety_off = vehicle_status.safety_off;
+		}
+	}
 
 	return ret;
 }
@@ -1033,37 +1026,17 @@ int PX4IO::dsm_bind_ioctl(int dsmMode)
 		return -EINVAL;
 	}
 
-	// Check if safety was off
-	bool safety_off = (_status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF);
-	int ret = -1;
-
-	// Put safety on
-	if (safety_off) {
-		// re-enable safety
-		ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, PX4IO_P_STATUS_FLAGS_SAFETY_OFF, 0);
-
-		// set new status
-		_status &= ~(PX4IO_P_STATUS_FLAGS_SAFETY_OFF);
-	}
-
 	PX4_INFO("Binding DSM%s RX", (dsmMode == DSM2_BIND_PULSES) ? "2" : ((dsmMode == DSMX_BIND_PULSES) ? "-X" : "-X8"));
 
-	io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down);
+	int ret = OK;
+	ret |= io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down);
 	px4_usleep(500000);
-	io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_set_rx_out);
-	io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_up);
+	ret |= io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_set_rx_out);
+	ret |= io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_up);
 	px4_usleep(72000);
-	io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_send_pulses | (dsmMode << 4));
+	ret |= io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_send_pulses | (dsmMode << 4));
 	px4_usleep(50000);
-	io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_reinit_uart);
-	ret = OK;
-
-	// Put safety back off
-	if (safety_off) {
-		ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0,
-				    PX4IO_P_STATUS_FLAGS_SAFETY_OFF);
-		_status |= PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
-	}
+	ret |= io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_reinit_uart);
 
 	if (ret != OK) {
 		PX4_INFO("Binding DSM failed");
@@ -1133,7 +1106,7 @@ int PX4IO::io_get_status()
 		status.status_arm_sync        = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_ARM_SYNC;
 		status.status_init_ok         = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_INIT_OK;
 		status.status_failsafe        = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_FAILSAFE;
-		status.status_safety_off      = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
+		status.status_safety_button_event = STATUS_FLAGS & PX4IO_P_STATUS_FLAGS_SAFETY_BUTTON_EVENT;
 
 		// PX4IO_P_STATUS_ALARMS
 		status.alarm_rc_lost       = STATUS_ALARMS & PX4IO_P_STATUS_ALARMS_RC_LOST;
@@ -1644,18 +1617,6 @@ int PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 		*(unsigned *)arg = _max_actuators;
 		break;
 
-	case PWM_SERVO_SET_FORCE_SAFETY_OFF:
-		PX4_DEBUG("PWM_SERVO_SET_FORCE_SAFETY_OFF");
-		/* force safety swith off */
-		ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_OFF, PX4IO_FORCE_SAFETY_MAGIC);
-		break;
-
-	case PWM_SERVO_SET_FORCE_SAFETY_ON:
-		PX4_DEBUG("PWM_SERVO_SET_FORCE_SAFETY_ON");
-		/* force safety switch on */
-		ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_ON, PX4IO_FORCE_SAFETY_MAGIC);
-		break;
-
 	case DSM_BIND_START:
 		/* bind a DSM receiver */
 		ret = dsm_bind_ioctl(arg);
@@ -1725,16 +1686,6 @@ int PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 
 		}
 
-		// re-enable safety
-		ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_ON, PX4IO_FORCE_SAFETY_MAGIC);
-
-		if (ret != PX4_OK) {
-			PX4_WARN("IO refused to re-enable safety");
-		}
-
-		// set new status
-		_status &= ~(PX4IO_P_STATUS_FLAGS_SAFETY_OFF);
-
 		/* reboot into bootloader - arg must be PX4IO_REBOOT_BL_MAGIC */
 		usleep(1);
 		ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_REBOOT_BL, arg);
@@ -2058,29 +2009,6 @@ int PX4IO::custom_command(int argc, char *argv[])
 		return 1;
 	}
 
-
-	if (!strcmp(verb, "safety_off")) {
-		int ret = get_instance()->ioctl(NULL, PWM_SERVO_SET_FORCE_SAFETY_OFF, 0);
-
-		if (ret != OK) {
-			PX4_ERR("failed to disable safety (%i)", ret);
-			return 1;
-		}
-
-		return 0;
-	}
-
-	if (!strcmp(verb, "safety_on")) {
-		int ret = get_instance()->ioctl(NULL, PWM_SERVO_SET_FORCE_SAFETY_ON, 0);
-
-		if (ret != OK) {
-			PX4_ERR("failed to enable safety (%i)", ret);
-			return 1;
-		}
-
-		return 0;
-	}
-
 	if (!strcmp(verb, "debug")) {
 		if (argc <= 1) {
 			PX4_ERR("usage: px4io debug LEVEL");
@@ -2162,8 +2090,6 @@ Output driver communicating with the IO co-processor.
 	PRINT_MODULE_USAGE_ARG("<filename>", "Firmware file", false);
 	PRINT_MODULE_USAGE_COMMAND_DESCR("update", "Update IO firmware");
 	PRINT_MODULE_USAGE_ARG("<filename>", "Firmware file", true);
-	PRINT_MODULE_USAGE_COMMAND_DESCR("safety_off", "Turn off safety (force)");
-	PRINT_MODULE_USAGE_COMMAND_DESCR("safety_on", "Turn on safety (force)");
 	PRINT_MODULE_USAGE_COMMAND_DESCR("debug", "set IO debug level");
 	PRINT_MODULE_USAGE_ARG("<debug_level>", "0=disabled, 9=max verbosity", false);
 	PRINT_MODULE_USAGE_COMMAND_DESCR("bind", "DSM bind");

src/drivers/uavcan/uavcan_main.cpp

@@ -951,8 +951,6 @@ UavcanNode::ioctl(file *filp, int cmd, unsigned long arg)
 	switch (cmd) {
 	case PWM_SERVO_SET_ARM_OK:
 	case PWM_SERVO_CLEAR_ARM_OK:
-	case PWM_SERVO_SET_FORCE_SAFETY_OFF:
-		// these are no-ops, as no safety switch
 		break;
 
 	case MIXERIOCRESET:

src/lib/drivers/gyroscope/PX4Gyroscope.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2018-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2018-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -50,6 +50,22 @@ static constexpr int32_t sum(const int16_t samples[], uint8_t len)
 	return sum;
 }
 
+static constexpr uint8_t clipping(const int16_t samples[], uint8_t len)
+{
+	unsigned clip_count = 0;
+
+	for (int n = 0; n < len; n++) {
+		// - consider data clipped/saturated if it's INT16_MIN/INT16_MAX or within 1
+		// - this accommodates rotated data (|INT16_MIN| = INT16_MAX + 1)
+		//   and sensors that may re-use the lowest bit for other purposes (sync indicator, etc)
+		if ((samples[n] <= INT16_MIN + 1) || (samples[n] >= INT16_MAX - 1)) {
+			clip_count++;
+		}
+	}
+
+	return clip_count;
+}
+
 PX4Gyroscope::PX4Gyroscope(uint32_t device_id, enum Rotation rotation) :
 	_device_id{device_id},
 	_rotation{rotation}
@@ -90,6 +106,8 @@ void PX4Gyroscope::set_scale(float scale)
 		}
 
 		_scale = scale;
+
+		UpdateClipLimit();
 	}
 }
 
@@ -107,6 +125,9 @@ void PX4Gyroscope::update(const hrt_abstime &timestamp_sample, float x, float y,
 	report.x = x * _scale;
 	report.y = y * _scale;
 	report.z = z * _scale;
+	report.clip_counter[0] = (fabsf(x) >= _clip_limit);
+	report.clip_counter[1] = (fabsf(y) >= _clip_limit);
+	report.clip_counter[2] = (fabsf(z) >= _clip_limit);
 	report.samples = 1;
 	report.timestamp = hrt_absolute_time();
 
@@ -145,8 +166,17 @@ void PX4Gyroscope::updateFIFO(sensor_gyro_fifo_s &sample)
 	_last_sample[1] = sample.y[N - 1];
 	_last_sample[2] = sample.z[N - 1];
 
+	report.clip_counter[0] = clipping(sample.x, N);
+	report.clip_counter[1] = clipping(sample.y, N);
+	report.clip_counter[2] = clipping(sample.z, N);
 	report.samples = N;
 	report.timestamp = hrt_absolute_time();
 
 	_sensor_pub.publish(report);
 }
+
+void PX4Gyroscope::UpdateClipLimit()
+{
+	// 99.9% of potential max
+	_clip_limit = math::constrain((_range / _scale) * 0.999f, 0.f, (float)INT16_MAX);
+}

src/lib/drivers/gyroscope/PX4Gyroscope.hpp

@@ -1,7 +1,6 @@
-
 /****************************************************************************
  *
- *   Copyright (c) 2018-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2018-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -53,7 +52,7 @@ public:
 	void set_device_id(uint32_t device_id) { _device_id = device_id; }
 	void set_device_type(uint8_t devtype);
 	void set_error_count(uint32_t error_count) { _error_count = error_count; }
-	void set_range(float range) { _range = range; }
+	void set_range(float range) { _range = range; UpdateClipLimit(); }
 	void set_scale(float scale);
 	void set_temperature(float temperature) { _temperature = temperature; }
 
@@ -64,6 +63,8 @@ public:
 	int get_instance() { return _sensor_pub.get_instance(); };
 
 private:
+	void UpdateClipLimit();
+
 	uORB::PublicationMulti<sensor_gyro_s> _sensor_pub{ORB_ID(sensor_gyro)};
 	uORB::PublicationMulti<sensor_gyro_fifo_s>  _sensor_fifo_pub{ORB_ID(sensor_gyro_fifo)};
 
@@ -76,6 +77,8 @@ private:
 	float			_scale{1.f};
 	float			_temperature{NAN};
 
+	float			_clip_limit{_range / _scale};
+
 	uint32_t		_error_count{0};
 
 	int16_t			_last_sample[3] {};

src/lib/events/enums.json

@@ -146,38 +146,34 @@
                             "description": "mission start"
                         },
                         "6": {
-                            "name": "safety_button",
-                            "description": "safety button"
-                        },
-                        "7": {
                             "name": "auto_disarm_land",
                             "description": "landing"
                         },
-                        "8": {
+                        "7": {
                             "name": "auto_disarm_preflight",
                             "description": "auto preflight disarming"
                         },
-                        "9": {
+                        "8": {
                             "name": "kill_switch",
                             "description": "kill switch"
                         },
-                        "10": {
+                        "9": {
                             "name": "lockdown",
                             "description": "lockdown"
                         },
-                        "11": {
+                        "10": {
                             "name": "failure_detector",
                             "description": "failure detector"
                         },
-                        "12": {
+                        "11": {
                             "name": "shutdown",
                             "description": "shutdown request"
                         },
-                        "13": {
+                        "12": {
                             "name": "unit_test",
                             "description": "unit tests"
                         },
-                        "14": {
+                        "13": {
                             "name": "rc_button",
                             "description": "RC (button)"
                         }

src/lib/parameters/param_translation.cpp

@@ -146,6 +146,25 @@ bool param_modify_on_import(bson_node_t node)
 		}
 	}
 
+	// 2022-04-11: translate VT_PTCH_MIN to VT_PITCH_MIN
+	{
+		if (strcmp("VT_PTCH_MIN", node->name) == 0) {
+			strcpy(node->name, "VT_PITCH_MIN");
+			PX4_INFO("copying %s -> %s", "VT_PTCH_MIN", "VT_PITCH_MIN");
+			return true;
+		}
+	}
+
+	// 2022-04-11: translate VT_LND_PTCH_MIN to VT_LND_PITCH_MIN
+	{
+		if (strcmp("VT_LND_PTCH_MIN", node->name) == 0) {
+			strcpy(node->name, "VT_LND_PITCH_MIN");
+			PX4_INFO("copying %s -> %s", "VT_LND_PTCH_MIN", "VT_LND_PITCH_MIN");
+			return true;
+		}
+	}
+
+
 	// 2021-10-21: translate NAV_GPSF_LT to FW_GPSF_LT and NAV_GPSF_R to FW_GPSF_R
 	{
 		if (strcmp("NAV_GPSF_LT", node->name) == 0) {

src/lib/world_magnetic_model/geo_magnetic_tables.hpp

@@ -47,80 +47,80 @@ static constexpr int LON_DIM = 37;
 // Magnetic declination data in radians * 10^-4
 // Model: WMM-2020,
 // Version: 0.5.1.11,
-// Date: 2022.2767,
+// Date: 2022.3835,
 static constexpr const int16_t declination_table[19][37] {
 	//    LONGITUDE:   -180,  -170,  -160,  -150,  -140,  -130,  -120,  -110,  -100,   -90,   -80,   -70,   -60,   -50,   -40,   -30,   -20,   -10,     0,    10,    20,    30,    40,    50,    60,    70,    80,    90,   100,   110,   120,   130,   140,   150,   160,   170,   180,
-	/* LAT: -90 */ {  25994, 24248, 22503, 20758, 19012, 17267, 15522, 13776, 12031, 10286,  8540,  6795,  5050,  3304,  1559,  -186, -1931, -3677, -5422, -7167, -8912,-10658,-12403,-14148,-15894,-17639,-19384,-21130,-22875,-24621,-26366,-28111,-29857, 31230, 29484, 27739, 25994, },
-	/* LAT: -80 */ {  22561, 20428, 18487, 16712, 15071, 13532, 12068, 10657,  9281,  7931,  6599,  5280,  3971,  2666,  1359,    40, -1301, -2672, -4082, -5533, -7026, -8562,-10139,-11758,-13421,-15139,-16924,-18795,-20777,-22895,-25169,-27603,-30172, 30020, 27400, 24893, 22561, },
-	/* LAT: -70 */ {  14974, 13576, 12451, 11491, 10623,  9793,  8952,  8065,  7113,  6094,  5024,  3931,  2845,  1789,   767,  -244, -1280, -2384, -3584, -4882, -6261, -7690, -9137,-10578,-12004,-13419,-14847,-16332,-17953,-19864,-22379,-26169, 30744, 24158, 19625, 16846, 14974, },
-	/* LAT: -60 */ {   8407,  8164,  7886,  7614,  7363,  7111,  6805,  6374,  5760,  4942,  3942,  2830,  1709,   680,  -204,  -963, -1692, -2513, -3514, -4708, -6036, -7404, -8724, -9935,-11000,-11899,-12616,-13111,-13283,-12834,-10737, -3530,  4869,  7630,  8408,  8538,  8407, },
-	/* LAT: -50 */ {   5474,  5512,  5458,  5371,  5299,  5264,  5230,  5105,  4763,  4102,  3094,  1822,   481,  -702, -1577, -2145, -2543, -2982, -3663, -4671, -5911, -7186, -8327, -9231, -9831,-10074, -9883, -9128, -7622, -5263, -2360,   387,  2496,  3921,  4795,  5270,  5474, },
-	/* LAT: -40 */ {   3947,  4040,  4049,  4007,  3949,  3916,  3922,  3913,  3741,  3209,  2189,   749,  -815, -2133, -3001, -3457, -3643, -3705, -3871, -4433, -5403, -6478, -7364, -7906, -8017, -7644, -6759, -5376, -3664, -1960,  -499,   722,  1765,  2630,  3285,  3715,  3947, },
-	/* LAT: -30 */ {   2977,  3063,  3095,  3083,  3027,  2950,  2891,  2858,  2733,  2258,  1226,  -290, -1898, -3156, -3900, -4250, -4333, -4128, -3695, -3466, -3821, -4566, -5266, -5620, -5499, -4916, -3959, -2752, -1542,  -595,    83,   683,  1307,  1908,  2417,  2778,  2977, },
-	/* LAT: -20 */ {   2334,  2381,  2400,  2405,  2365,  2272,  2166,  2090,  1946,  1453,   401, -1085, -2564, -3630, -4169, -4290, -4091, -3538, -2678, -1876, -1593, -1942, -2591, -3063, -3103, -2747, -2113, -1296,  -518,   -28,   235,   537,   977,  1454,  1875,  2182,  2334, },
-	/* LAT: -10 */ {   1941,  1935,  1916,  1915,  1890,  1807,  1699,  1609,  1425,   877,  -181, -1556, -2832, -3666, -3940, -3717, -3146, -2367, -1521,  -764,  -295,  -323,  -790, -1297, -1512, -1424, -1112,  -612,  -114,   128,   178,   338,   708,  1144,  1536,  1822,  1941, },
-	/* LAT:   0 */ {   1728,  1695,  1641,  1635,  1626,  1559,  1456,  1344,  1091,   472,  -565, -1792, -2849, -3441, -3446, -2952, -2186, -1404,  -750,  -212,   202,   317,    41,  -375,  -632,  -689,  -591,  -328,   -37,    53,    -7,    82,   422,   862,  1281,  1599,  1728, },
-	/* LAT:  10 */ {   1593,  1601,  1561,  1579,  1607,  1559,  1440,  1256,   881,   165,  -855, -1932, -2762, -3107, -2909, -2307, -1531,  -818,  -301,    81,   407,   558,   399,    77,  -159,  -267,  -290,  -209,  -104,  -144,  -283,  -256,    45,   497,   977,  1383,  1593, },
-	/* LAT:  20 */ {   1410,  1559,  1621,  1715,  1803,  1784,  1632,  1332,   788,   -67, -1110, -2063, -2664, -2779, -2455, -1856, -1146,  -495,   -33,   278,   538,   685,   596,   353,   153,    35,   -56,  -120,  -199,  -388,  -625,  -683,  -450,    -4,   536,  1054,  1410, },
-	/* LAT:  30 */ {   1111,  1478,  1739,  1964,  2125,  2136,  1951,  1530,   804,  -224, -1343, -2223, -2643, -2586, -2190, -1612,  -957,  -338,   126,   434,   666,   814,   795,   647,   497,   372,   216,     6,  -276,  -650, -1018, -1179, -1020,  -595,   -19,   590,  1111, },
-	/* LAT:  40 */ {    756,  1343,  1837,  2230,  2480,  2524,  2310,  1778,   868,  -359, -1600, -2472, -2797, -2648, -2204, -1613,  -959,  -329,   183,   549,   821,  1021,  1116,  1106,  1030,   882,   614,   205,  -332,  -939, -1461, -1706, -1591, -1175,  -576,    97,   756, },
-	/* LAT:  50 */ {    469,  1216,  1897,  2454,  2821,  2928,  2700,  2046,   905,  -593, -2020, -2939, -3238, -3048, -2559, -1914, -1203,  -504,   113,   621,  1041,  1397,  1679,  1851,  1871,  1685,  1241,   535,  -357, -1260, -1941, -2229, -2106, -1665, -1029,  -296,   469, },
-	/* LAT:  60 */ {    277,  1130,  1936,  2631,  3136,  3348,  3134,  2322,   806, -1158, -2881, -3865, -4120, -3860, -3287, -2541, -1714,  -872,   -62,   690,  1384,  2016,  2561,  2958,  3120,  2936,  2300,  1187,  -226, -1547, -2419, -2730, -2566, -2071, -1377,  -574,   277, },
-	/* LAT:  70 */ {     50,   987,  1885,  2684,  3298,  3592,  3338,  2200,   -46, -2770, -4727, -5553, -5561, -5076, -4304, -3367, -2341, -1273,  -196,   865,  1892,  2860,  3729,  4438,  4888,  4924,  4320,  2853,   664, -1445, -2747, -3193, -3034, -2502, -1751,  -879,    50, },
-	/* LAT:  80 */ {   -631,   292,  1145,  1838,  2223,  2036,   828, -1805, -5037, -7161, -7906, -7771, -7138, -6217, -5123, -3921, -2653, -1345,   -18,  1311,  2626,  3909,  5137,  6277,  7274,  8035,  8381,  7944,  6036,  2293, -1297, -3024, -3398, -3075, -2402, -1554,  -631, },
-	/* LAT:  90 */ { -29941,-28196,-26450,-24705,-22959,-21214,-19469,-17723,-15978,-14233,-12488,-10742, -8997, -7252, -5507, -3762, -2017,  -271,  1474,  3219,  4964,  6709,  8455, 10200, 11945, 13691, 15436, 17181, 18927, 20672, 22418, 24163, 25909, 27654, 29400, 31145,-29941, },
+	/* LAT: -90 */ {  25991, 24246, 22500, 20755, 19010, 17264, 15519, 13774, 12028, 10283,  8538,  6792,  5047,  3302,  1557,  -189, -1934, -3679, -5425, -7170, -8915,-10660,-12406,-14151,-15896,-17642,-19387,-21133,-22878,-24623,-26369,-28114,-29859, 31227, 29482, 27736, 25991, },
+	/* LAT: -80 */ {  22558, 20425, 18485, 16710, 15069, 13531, 12067, 10655,  9280,  7930,  6598,  5279,  3970,  2665,  1357,    38, -1303, -2674, -4084, -5535, -7029, -8565,-10142,-11761,-13425,-15142,-16927,-18799,-20781,-22899,-25173,-27607,-30176, 30016, 27397, 24889, 22558, },
+	/* LAT: -70 */ {  14975, 13577, 12452, 11491, 10623,  9792,  8951,  8064,  7112,  6093,  5023,  3929,  2844,  1788,   766,  -244, -1281, -2385, -3585, -4884, -6264, -7693, -9140,-10581,-12007,-13423,-14851,-16336,-17958,-19870,-22386,-26179, 30734, 24153, 19624, 16847, 14975, },
+	/* LAT: -60 */ {   8410,  8167,  7888,  7615,  7364,  7112,  6805,  6373,  5759,  4941,  3940,  2829,  1708,   679,  -203,  -962, -1691, -2513, -3514, -4710, -6038, -7407, -8728, -9938,-11003,-11903,-12619,-13114,-13286,-12837,-10739, -3521,  4880,  7637,  8413,  8542,  8410, },
+	/* LAT: -50 */ {   5477,  5514,  5460,  5372,  5300,  5264,  5231,  5105,  4763,  4101,  3092,  1820,   479,  -703, -1576, -2143, -2541, -2980, -3662, -4672, -5914, -7190, -8331, -9234, -9834,-10075, -9883, -9128, -7621, -5261, -2357,   390,  2499,  3923,  4798,  5272,  5477, },
+	/* LAT: -40 */ {   3948,  4042,  4051,  4008,  3949,  3916,  3922,  3912,  3740,  3208,  2187,   747,  -818, -2134, -3001, -3455, -3641, -3701, -3868, -4433, -5406, -6482, -7368, -7909, -8019, -7644, -6757, -5374, -3662, -1959,  -499,   723,  1766,  2631,  3287,  3717,  3948, },
+	/* LAT: -30 */ {   2978,  3065,  3097,  3084,  3027,  2949,  2890,  2857,  2732,  2256,  1223,  -294, -1902, -3158, -3901, -4249, -4331, -4124, -3690, -3465, -3823, -4569, -5269, -5621, -5499, -4914, -3956, -2750, -1540,  -595,    83,   683,  1307,  1909,  2418,  2779,  2978, },
+	/* LAT: -20 */ {   2336,  2382,  2401,  2405,  2364,  2271,  2165,  2089,  1945,  1451,   398, -1089, -2568, -3633, -4169, -4288, -4088, -3533, -2673, -1873, -1592, -1944, -2594, -3064, -3103, -2746, -2111, -1295,  -518,   -28,   235,   536,   977,  1454,  1876,  2183,  2336, },
+	/* LAT: -10 */ {   1942,  1937,  1917,  1916,  1889,  1806,  1698,  1608,  1424,   875,  -184, -1560, -2835, -3668, -3939, -3715, -3142, -2362, -1516,  -760,  -293,  -323,  -791, -1298, -1512, -1424, -1111,  -612,  -114,   128,   177,   337,   707,  1144,  1536,  1823,  1942, },
+	/* LAT:   0 */ {   1729,  1696,  1642,  1635,  1625,  1558,  1455,  1342,  1089,   470,  -569, -1795, -2851, -3441, -3445, -2948, -2182, -1400,  -747,  -210,   204,   318,    41,  -376,  -632,  -688,  -590,  -328,   -38,    52,    -9,    80,   420,   862,  1281,  1600,  1729, },
+	/* LAT:  10 */ {   1594,  1602,  1561,  1579,  1606,  1559,  1439,  1254,   879,   162,  -858, -1934, -2763, -3107, -2907, -2303, -1528,  -815,  -298,    83,   409,   559,   399,    77,  -158,  -266,  -290,  -209,  -104,  -145,  -285,  -258,    43,   497,   977,  1384,  1594, },
+	/* LAT:  20 */ {   1411,  1560,  1621,  1715,  1802,  1783,  1630,  1330,   786,   -69, -1112, -2064, -2664, -2777, -2453, -1853, -1143,  -492,   -31,   280,   539,   686,   597,   354,   154,    35,   -56,  -120,  -200,  -390,  -626,  -685,  -452,    -5,   536,  1054,  1411, },
+	/* LAT:  30 */ {   1111,  1478,  1738,  1963,  2124,  2135,  1949,  1528,   802,  -226, -1344, -2224, -2642, -2583, -2187, -1609,  -954,  -335,   129,   436,   668,   815,   796,   647,   498,   373,   216,     5,  -277,  -651, -1020, -1180, -1021,  -596,   -20,   590,  1111, },
+	/* LAT:  40 */ {    755,  1341,  1836,  2229,  2478,  2523,  2308,  1777,   866,  -361, -1601, -2470, -2795, -2645, -2201, -1609,  -956,  -326,   186,   552,   823,  1022,  1117,  1107,  1031,   883,   614,   204,  -334,  -941, -1462, -1707, -1592, -1176,  -577,    95,   755, },
+	/* LAT:  50 */ {    466,  1213,  1895,  2452,  2819,  2926,  2698,  2045,   904,  -593, -2018, -2937, -3234, -3044, -2555, -1910, -1199,  -500,   117,   624,  1043,  1400,  1681,  1852,  1872,  1685,  1241,   533,  -360, -1263, -1943, -2231, -2107, -1666, -1031,  -298,   466, },
+	/* LAT:  60 */ {    273,  1126,  1932,  2627,  3132,  3345,  3132,  2321,   806, -1155, -2876, -3859, -4114, -3854, -3282, -2536, -1709,  -867,   -58,   695,  1388,  2020,  2564,  2961,  3122,  2937,  2299,  1184,  -230, -1551, -2421, -2732, -2567, -2073, -1379,  -578,   273, },
+	/* LAT:  70 */ {     42,   978,  1876,  2676,  3291,  3585,  3333,  2199,   -41, -2759, -4714, -5542, -5551, -5066, -4295, -3360, -2334, -1266,  -190,   871,  1897,  2865,  3734,  4442,  4891,  4926,  4319,  2850,   658, -1451, -2752, -3197, -3039, -2507, -1756,  -886,    42, },
+	/* LAT:  80 */ {   -650,   272,  1126,  1818,  2203,  2020,   821, -1791, -5004, -7127, -7878, -7749, -7120, -6201, -5109, -3909, -2642, -1335,    -9,  1320,  2635,  3918,  5146,  6285,  7283,  8044,  8389,  7950,  6035,  2276, -1323, -3048, -3419, -3095, -2421, -1572,  -650, },
+	/* LAT:  90 */ { -29901,-28155,-26410,-24665,-22919,-21174,-19428,-17683,-15938,-14193,-12447,-10702, -8957, -7212, -5467, -3722, -1976,  -231,  1514,  3259,  5004,  6750,  8495, 10240, 11986, 13731, 15476, 17222, 18967, 20713, 22458, 24204, 25949, 27695, 29440, 31186,-29901, },
 };
 
 // Magnetic inclination data in radians * 10^-4
 // Model: WMM-2020,
 // Version: 0.5.1.11,
-// Date: 2022.2767,
+// Date: 2022.3835,
 static constexpr const int16_t inclination_table[19][37] {
 	//    LONGITUDE:   -180,  -170,  -160,  -150,  -140,  -130,  -120,  -110,  -100,   -90,   -80,   -70,   -60,   -50,   -40,   -30,   -20,   -10,     0,    10,    20,    30,    40,    50,    60,    70,    80,    90,   100,   110,   120,   130,   140,   150,   160,   170,   180,
-	/* LAT: -90 */ { -12573,-12573,-12573,-12573,-12573,-12573,-12573,-12573,-12573,-12573,-12573,-12573,-12573,-12573,-12573,-12573,-12573,-12573,-12573,-12573,-12573,-12573,-12573,-12573,-12573,-12573,-12573,-12573,-12573,-12573,-12573,-12573,-12573,-12573,-12573,-12573,-12573, },
-	/* LAT: -80 */ { -13658,-13525,-13364,-13184,-12990,-12788,-12584,-12384,-12192,-12016,-11859,-11725,-11615,-11528,-11465,-11423,-11404,-11406,-11433,-11488,-11571,-11685,-11830,-12003,-12201,-12418,-12648,-12881,-13108,-13321,-13508,-13658,-13762,-13814,-13811,-13757,-13658, },
-	/* LAT: -70 */ { -14107,-13789,-13469,-13146,-12814,-12470,-12115,-11757,-11414,-11105,-10852,-10668,-10555,-10502,-10489,-10492,-10500,-10510,-10533,-10589,-10697,-10871,-11117,-11431,-11803,-12221,-12668,-13132,-13598,-14051,-14469,-14815,-15004,-14951,-14721,-14423,-14107, },
-	/* LAT: -60 */ { -13519,-13166,-12828,-12496,-12152,-11780,-11365,-10910,-10442,-10011, -9680, -9503, -9499, -9635, -9839,-10033,-10159,-10204,-10193,-10178,-10225,-10383,-10671,-11078,-11576,-12131,-12717,-13313,-13902,-14463,-14960,-15250,-15075,-14691,-14285,-13892,-13519, },
-	/* LAT: -50 */ { -12496,-12155,-11824,-11502,-11179,-10832,-10433, -9961, -9431, -8909, -8518, -8390, -8588, -9048, -9612,-10125,-10487,-10652,-10625,-10475,-10332,-10336,-10555,-10969,-11509,-12101,-12688,-13227,-13674,-13972,-14081,-14008,-13805,-13521,-13193,-12846,-12496, },
-	/* LAT: -40 */ { -11240,-10892,-10545,-10200, -9861, -9523, -9162, -8736, -8214, -7649, -7220, -7175, -7632, -8453, -9375,-10206,-10867,-11304,-11448,-11284,-10944,-10670,-10660,-10938,-11399,-11904,-12350,-12673,-12833,-12839,-12748,-12611,-12432,-12200,-11913,-11586,-11240, },
-	/* LAT: -30 */ {  -9602, -9224, -8846, -8456, -8064, -7688, -7331, -6939, -6426, -5814, -5360, -5438, -6202, -7402, -8653, -9750,-10666,-11379,-11782,-11777,-11409,-10901,-10565,-10563,-10813,-11134,-11391,-11506,-11444,-11268,-11094,-10959,-10812,-10605,-10324, -9979, -9602, },
-	/* LAT: -20 */ {  -7371, -6933, -6517, -6090, -5645, -5215, -4823, -4409, -3842, -3156, -2705, -2960, -4065, -5674, -7303, -8675, -9744,-10522,-10964,-11003,-10643,-10030, -9471, -9225, -9273, -9435, -9576, -9595, -9423, -9151, -8957, -8869, -8764, -8556, -8239, -7828, -7371, },
-	/* LAT: -10 */ {  -4414, -3882, -3430, -2995, -2537, -2086, -1671, -1215,  -587,   118,   483,    61, -1262, -3177, -5153, -6765, -7865, -8502, -8770, -8710, -8303, -7621, -6957, -6611, -6570, -6663, -6781, -6805, -6618, -6322, -6166, -6174, -6133, -5916, -5532, -5006, -4414, },
-	/* LAT:   0 */ {   -906,  -286,   176,   578,   996,  1413,  1804,  2249,  2836,  3415,  3621,  3129,  1838,   -69, -2113, -3767, -4780, -5212, -5273, -5104, -4666, -3952, -3247, -2875, -2814, -2884, -3009, -3083, -2953, -2718, -2662, -2804, -2865, -2677, -2253, -1625,  -906, },
-	/* LAT:  10 */ {   2562,  3185,  3616,  3956,  4311,  4678,  5031,  5420,  5874,  6246,  6284,  5799,  4720,  3157,  1467,    89,  -719,  -970,  -880,  -646,  -236,   401,  1034,  1372,  1434,  1389,  1288,  1197,  1239,  1343,  1268,  1004,   814,   887,  1239,  1840,  2562, },
-	/* LAT:  20 */ {   5416,  5942,  6320,  6614,  6927,  7271,  7615,  7964,  8299,  8501,  8415,  7955,  7122,  6028,  4902,  3992,  3461,  3340,  3488,  3731,  4060,  4525,  4984,  5240,  5296,  5278,  5229,  5172,  5161,  5143,  4970,  4639,  4343,  4254,  4426,  4848,  5416, },
-	/* LAT:  30 */ {   7569,  7940,  8257,  8539,  8848,  9196,  9554,  9896, 10174, 10289, 10146,  9726,  9098,  8388,  7727,  7215,  6922,  6879,  7020,  7230,  7476,  7775,  8063,  8237,  8293,  8306,  8307,  8298,  8276,  8194,  7973,  7622,  7268,  7048,  7032,  7226,  7569, },
-	/* LAT:  40 */ {   9266,  9486,  9742, 10027, 10354, 10715, 11084, 11423, 11676, 11760, 11611, 11252, 10779, 10306,  9910,  9626,  9476,  9471,  9580,  9741,  9916, 10099, 10271, 10396, 10474, 10534, 10588, 10621, 10605, 10496, 10255,  9905,  9539,  9254,  9111,  9123,  9266, },
-	/* LAT:  50 */ {  10801, 10923, 11124, 11394, 11718, 12072, 12427, 12744, 12967, 13027, 12888, 12590, 12226, 11880, 11605, 11418, 11324, 11320, 11385, 11488, 11601, 11717, 11833, 11948, 12066, 12188, 12301, 12374, 12365, 12241, 11996, 11669, 11329, 11045, 10855, 10775, 10801, },
-	/* LAT:  60 */ {  12319, 12392, 12543, 12761, 13032, 13333, 13636, 13902, 14078, 14106, 13971, 13721, 13432, 13161, 12940, 12783, 12692, 12660, 12675, 12722, 12790, 12875, 12980, 13112, 13271, 13446, 13611, 13720, 13727, 13608, 13384, 13105, 12826, 12589, 12419, 12327, 12319, },
-	/* LAT:  70 */ {  13758, 13801, 13897, 14039, 14219, 14424, 14632, 14814, 14918, 14898, 14760, 14558, 14340, 14136, 13963, 13828, 13733, 13678, 13659, 13671, 13713, 13785, 13888, 14024, 14190, 14377, 14561, 14702, 14749, 14678, 14517, 14318, 14124, 13959, 13839, 13771, 13758, },
-	/* LAT:  80 */ {  14998, 15011, 15049, 15109, 15186, 15272, 15351, 15396, 15377, 15298, 15185, 15060, 14937, 14823, 14723, 14641, 14579, 14538, 14519, 14523, 14549, 14598, 14670, 14762, 14874, 15000, 15135, 15267, 15374, 15419, 15381, 15295, 15200, 15116, 15052, 15012, 14998, },
+	/* LAT: -90 */ { -12573,-12573,-12573,-12573,-12573,-12573,-12572,-12572,-12572,-12572,-12572,-12572,-12572,-12572,-12572,-12572,-12572,-12572,-12572,-12572,-12572,-12572,-12572,-12572,-12572,-12572,-12572,-12573,-12573,-12573,-12573,-12573,-12573,-12573,-12573,-12573,-12573, },
+	/* LAT: -80 */ { -13658,-13524,-13363,-13183,-12989,-12787,-12583,-12383,-12192,-12016,-11859,-11724,-11614,-11528,-11464,-11423,-11403,-11406,-11433,-11487,-11571,-11685,-11830,-12003,-12201,-12418,-12647,-12880,-13108,-13321,-13507,-13658,-13762,-13813,-13810,-13756,-13658, },
+	/* LAT: -70 */ { -14106,-13788,-13468,-13145,-12813,-12469,-12114,-11757,-11413,-11104,-10851,-10667,-10555,-10502,-10488,-10492,-10499,-10509,-10532,-10589,-10697,-10871,-11117,-11431,-11803,-12221,-12668,-13133,-13599,-14051,-14469,-14814,-15004,-14951,-14720,-14422,-14106, },
+	/* LAT: -60 */ { -13519,-13166,-12828,-12495,-12151,-11779,-11364,-10909,-10441,-10010, -9680, -9503, -9499, -9636, -9840,-10033,-10159,-10204,-10192,-10177,-10224,-10382,-10670,-11078,-11576,-12131,-12718,-13314,-13902,-14464,-14960,-15250,-15075,-14691,-14285,-13892,-13519, },
+	/* LAT: -50 */ { -12496,-12154,-11824,-11502,-11178,-10832,-10432, -9961, -9431, -8909, -8518, -8391, -8590, -9050, -9614,-10127,-10488,-10652,-10624,-10473,-10331,-10335,-10554,-10969,-11510,-12102,-12689,-13228,-13675,-13973,-14081,-14008,-13805,-13521,-13193,-12846,-12496, },
+	/* LAT: -40 */ { -11240,-10892,-10545,-10200, -9860, -9522, -9161, -8736, -8214, -7649, -7221, -7177, -7635, -8457, -9379,-10209,-10869,-11306,-11448,-11283,-10941,-10668,-10659,-10939,-11400,-11905,-12351,-12674,-12833,-12838,-12748,-12611,-12432,-12200,-11913,-11586,-11240, },
+	/* LAT: -30 */ {  -9602, -9224, -8845, -8455, -8063, -7687, -7330, -6939, -6426, -5814, -5361, -5441, -6207, -7408, -8659, -9755,-10670,-11382,-11783,-11777,-11407,-10899,-10564,-10563,-10813,-11135,-11392,-11506,-11443,-11268,-11093,-10959,-10812,-10605,-10324, -9979, -9602, },
+	/* LAT: -20 */ {  -7371, -6932, -6516, -6088, -5644, -5214, -4822, -4409, -3842, -3156, -2707, -2964, -4071, -5682, -7311, -8681, -9748,-10526,-10966,-11003,-10641,-10028, -9470, -9225, -9272, -9435, -9576, -9594, -9422, -9150, -8956, -8869, -8765, -8557, -8240, -7829, -7371, },
+	/* LAT: -10 */ {  -4415, -3881, -3429, -2993, -2535, -2085, -1670, -1215,  -587,   117,   481,    56, -1270, -3186, -5161, -6772, -7870, -8505, -8771, -8710, -8302, -7619, -6955, -6609, -6569, -6662, -6779, -6804, -6616, -6320, -6165, -6174, -6134, -5917, -5534, -5007, -4415, },
+	/* LAT:   0 */ {   -906,  -285,   178,   580,   998,  1415,  1805,  2250,  2836,  3414,  3618,  3124,  1831,   -78, -2122, -3773, -4784, -5214, -5274, -5104, -4664, -3949, -3244, -2873, -2812, -2882, -3007, -3080, -2951, -2715, -2661, -2804, -2866, -2680, -2255, -1626,  -906, },
+	/* LAT:  10 */ {   2562,  3185,  3618,  3958,  4313,  4680,  5032,  5420,  5873,  6245,  6282,  5795,  4714,  3150,  1460,    84,  -723,  -971,  -881,  -645,  -234,   403,  1036,  1373,  1436,  1391,  1291,  1199,  1242,  1346,  1270,  1004,   812,   885,  1237,  1838,  2562, },
+	/* LAT:  20 */ {   5416,  5943,  6321,  6616,  6929,  7272,  7616,  7964,  8299,  8500,  8413,  7952,  7117,  6022,  4897,  3988,  3458,  3339,  3488,  3731,  4061,  4526,  4986,  5241,  5298,  5280,  5231,  5174,  5163,  5145,  4971,  4639,  4342,  4253,  4425,  4847,  5416, },
+	/* LAT:  30 */ {   7569,  7941,  8258,  8540,  8849,  9196,  9554,  9896, 10173, 10288, 10144,  9723,  9095,  8384,  7724,  7212,  6920,  6878,  7020,  7230,  7477,  7776,  8064,  8238,  8295,  8307,  8309,  8300,  8278,  8196,  7974,  7622,  7267,  7047,  7031,  7225,  7569, },
+	/* LAT:  40 */ {   9266,  9486,  9742, 10027, 10354, 10715, 11083, 11423, 11676, 11758, 11609, 11250, 10777, 10304,  9908,  9624,  9475,  9470,  9580,  9741,  9916, 10100, 10272, 10397, 10475, 10535, 10590, 10623, 10607, 10497, 10255,  9906,  9539,  9254,  9111,  9123,  9266, },
+	/* LAT:  50 */ {  10802, 10923, 11124, 11394, 11717, 12072, 12426, 12743, 12966, 13025, 12886, 12589, 12224, 11879, 11603, 11417, 11323, 11320, 11385, 11488, 11602, 11717, 11834, 11949, 12067, 12189, 12303, 12375, 12366, 12242, 11996, 11669, 11330, 11045, 10855, 10775, 10802, },
+	/* LAT:  60 */ {  12319, 12392, 12543, 12761, 13031, 13332, 13635, 13901, 14077, 14105, 13969, 13720, 13431, 13160, 12939, 12782, 12691, 12660, 12675, 12723, 12790, 12875, 12981, 13113, 13272, 13447, 13612, 13721, 13728, 13608, 13384, 13105, 12826, 12589, 12419, 12328, 12319, },
+	/* LAT:  70 */ {  13758, 13801, 13896, 14039, 14218, 14422, 14631, 14812, 14917, 14897, 14760, 14558, 14340, 14136, 13962, 13827, 13733, 13678, 13659, 13672, 13714, 13785, 13888, 14024, 14191, 14378, 14562, 14703, 14750, 14678, 14517, 14319, 14124, 13959, 13839, 13771, 13758, },
+	/* LAT:  80 */ {  14998, 15011, 15048, 15108, 15185, 15270, 15349, 15394, 15376, 15297, 15184, 15060, 14937, 14823, 14723, 14641, 14579, 14538, 14520, 14523, 14550, 14599, 14670, 14763, 14874, 15001, 15136, 15268, 15375, 15420, 15382, 15295, 15200, 15116, 15052, 15012, 14998, },
 	/* LAT:  90 */ {  15395, 15395, 15395, 15395, 15395, 15395, 15395, 15395, 15395, 15395, 15395, 15395, 15395, 15395, 15395, 15395, 15395, 15395, 15395, 15395, 15395, 15395, 15395, 15395, 15395, 15395, 15395, 15395, 15395, 15395, 15395, 15395, 15395, 15395, 15395, 15395, 15395, },
 };
 
 // Magnetic strength data in milli-Gauss * 10
 // Model: WMM-2020,
 // Version: 0.5.1.11,
-// Date: 2022.2767,
+// Date: 2022.3835,
 static constexpr const int16_t strength_table[19][37] {
 	//    LONGITUDE:  -180, -170, -160, -150, -140, -130, -120, -110, -100,  -90,  -80,  -70,  -60,  -50,  -40,  -30,  -20,  -10,    0,   10,   20,   30,   40,   50,   60,   70,   80,   90,  100,  110,  120,  130,  140,  150,  160,  170,  180,
-	/* LAT: -90 */ {  5453, 5453, 5453, 5453, 5453, 5453, 5453, 5453, 5453, 5453, 5453, 5453, 5453, 5453, 5453, 5453, 5453, 5453, 5453, 5453, 5453, 5453, 5453, 5453, 5453, 5453, 5453, 5453, 5453, 5453, 5453, 5453, 5453, 5453, 5453, 5453, 5453, },
-	/* LAT: -80 */ {  6059, 5996, 5917, 5825, 5723, 5611, 5493, 5371, 5249, 5129, 5015, 4910, 4816, 4736, 4673, 4628, 4603, 4601, 4622, 4668, 4739, 4832, 4947, 5078, 5222, 5372, 5521, 5665, 5796, 5909, 6002, 6071, 6116, 6135, 6131, 6105, 6059, },
-	/* LAT: -70 */ {  6304, 6171, 6021, 5857, 5678, 5486, 5281, 5066, 4847, 4632, 4429, 4247, 4089, 3958, 3853, 3777, 3729, 3717, 3745, 3821, 3948, 4128, 4356, 4624, 4919, 5226, 5529, 5813, 6063, 6267, 6418, 6514, 6556, 6549, 6499, 6415, 6304, },
-	/* LAT: -60 */ {  6189, 5997, 5796, 5588, 5369, 5135, 4878, 4600, 4308, 4018, 3751, 3523, 3343, 3208, 3109, 3036, 2985, 2965, 2991, 3081, 3249, 3500, 3826, 4211, 4633, 5067, 5489, 5873, 6198, 6447, 6610, 6689, 6691, 6629, 6515, 6365, 6189, },
-	/* LAT: -50 */ {  5846, 5616, 5384, 5154, 4922, 4677, 4407, 4104, 3775, 3443, 3141, 2900, 2739, 2647, 2599, 2564, 2531, 2505, 2510, 2582, 2754, 3043, 3438, 3909, 4416, 4921, 5397, 5816, 6155, 6397, 6535, 6577, 6534, 6424, 6263, 6065, 5846, },
-	/* LAT: -40 */ {  5394, 5149, 4904, 4665, 4431, 4194, 3940, 3656, 3342, 3016, 2717, 2494, 2376, 2350, 2369, 2391, 2396, 2384, 2372, 2400, 2529, 2803, 3219, 3732, 4279, 4803, 5270, 5659, 5952, 6140, 6228, 6231, 6162, 6032, 5851, 5633, 5394, },
-	/* LAT: -30 */ {  4879, 4639, 4400, 4166, 3940, 3721, 3503, 3271, 3016, 2741, 2483, 2300, 2229, 2253, 2320, 2391, 2457, 2508, 2531, 2543, 2609, 2806, 3166, 3653, 4184, 4682, 5101, 5419, 5622, 5722, 5749, 5722, 5643, 5511, 5332, 5115, 4879, },
-	/* LAT: -20 */ {  4321, 4109, 3901, 3697, 3500, 3318, 3150, 2986, 2809, 2611, 2420, 2286, 2244, 2286, 2375, 2486, 2614, 2743, 2832, 2869, 2894, 2988, 3226, 3608, 4058, 4485, 4834, 5070, 5176, 5184, 5154, 5107, 5025, 4897, 4731, 4534, 4321, },
-	/* LAT: -10 */ {  3790, 3630, 3477, 3331, 3196, 3076, 2973, 2881, 2784, 2670, 2548, 2448, 2402, 2425, 2510, 2639, 2794, 2954, 3078, 3142, 3156, 3182, 3304, 3555, 3880, 4202, 4467, 4631, 4667, 4615, 4547, 4483, 4395, 4270, 4121, 3958, 3790, },
-	/* LAT:   0 */ {  3412, 3320, 3236, 3163, 3108, 3070, 3044, 3026, 3002, 2955, 2876, 2781, 2700, 2668, 2708, 2810, 2943, 3078, 3194, 3270, 3301, 3323, 3397, 3553, 3761, 3974, 4155, 4261, 4268, 4201, 4113, 4020, 3909, 3777, 3643, 3519, 3412, },
-	/* LAT:  10 */ {  3283, 3252, 3232, 3229, 3253, 3301, 3357, 3411, 3447, 3437, 3368, 3254, 3126, 3031, 3003, 3043, 3124, 3222, 3322, 3408, 3472, 3534, 3621, 3739, 3873, 4011, 4131, 4202, 4205, 4143, 4034, 3891, 3729, 3570, 3435, 3338, 3283, },
-	/* LAT:  20 */ {  3400, 3403, 3430, 3484, 3576, 3698, 3827, 3945, 4026, 4039, 3965, 3823, 3656, 3515, 3438, 3425, 3459, 3532, 3628, 3725, 3816, 3914, 4024, 4136, 4244, 4355, 4457, 4523, 4534, 4476, 4339, 4138, 3911, 3701, 3539, 3438, 3400, },
-	/* LAT:  30 */ {  3723, 3730, 3785, 3885, 4029, 4200, 4376, 4533, 4641, 4668, 4595, 4440, 4250, 4085, 3978, 3931, 3934, 3984, 4071, 4168, 4266, 4371, 4487, 4605, 4724, 4850, 4971, 5057, 5084, 5027, 4871, 4632, 4358, 4104, 3905, 3777, 3723, },
-	/* LAT:  40 */ {  4222, 4221, 4287, 4411, 4579, 4767, 4950, 5107, 5212, 5238, 5169, 5020, 4833, 4658, 4529, 4452, 4426, 4447, 4507, 4586, 4672, 4769, 4884, 5017, 5168, 5329, 5480, 5591, 5630, 5577, 5422, 5184, 4910, 4651, 4440, 4295, 4222, },
-	/* LAT:  50 */ {  4832, 4825, 4882, 4993, 5140, 5299, 5449, 5570, 5644, 5653, 5589, 5462, 5298, 5134, 4996, 4898, 4843, 4830, 4854, 4904, 4973, 5064, 5182, 5332, 5508, 5692, 5858, 5977, 6021, 5978, 5848, 5652, 5427, 5211, 5031, 4902, 4832, },
-	/* LAT:  60 */ {  5392, 5381, 5410, 5473, 5559, 5654, 5741, 5806, 5839, 5830, 5776, 5681, 5560, 5431, 5312, 5216, 5149, 5114, 5112, 5139, 5195, 5282, 5399, 5546, 5713, 5881, 6029, 6133, 6178, 6156, 6073, 5947, 5801, 5658, 5535, 5445, 5392, },
-	/* LAT:  70 */ {  5726, 5707, 5704, 5717, 5739, 5766, 5789, 5803, 5803, 5783, 5744, 5686, 5615, 5539, 5465, 5401, 5352, 5324, 5319, 5339, 5384, 5453, 5545, 5653, 5770, 5885, 5984, 6057, 6097, 6100, 6071, 6018, 5951, 5880, 5815, 5763, 5726, },
-	/* LAT:  80 */ {  5789, 5772, 5758, 5746, 5736, 5727, 5717, 5705, 5690, 5671, 5649, 5624, 5596, 5569, 5544, 5524, 5510, 5505, 5509, 5524, 5550, 5585, 5628, 5676, 5726, 5774, 5816, 5851, 5875, 5888, 5891, 5884, 5870, 5851, 5830, 5809, 5789, },
+	/* LAT: -90 */ {  5452, 5452, 5452, 5452, 5452, 5452, 5452, 5452, 5452, 5452, 5452, 5452, 5452, 5452, 5452, 5452, 5452, 5452, 5452, 5452, 5452, 5452, 5452, 5452, 5452, 5452, 5452, 5452, 5452, 5452, 5452, 5452, 5452, 5452, 5452, 5452, 5452, },
+	/* LAT: -80 */ {  6059, 5995, 5916, 5825, 5722, 5610, 5492, 5370, 5248, 5128, 5014, 4909, 4815, 4735, 4672, 4627, 4603, 4600, 4622, 4668, 4738, 4832, 4946, 5078, 5222, 5371, 5521, 5664, 5795, 5909, 6002, 6071, 6115, 6135, 6131, 6105, 6059, },
+	/* LAT: -70 */ {  6303, 6170, 6020, 5856, 5677, 5485, 5280, 5065, 4846, 4631, 4428, 4246, 4088, 3957, 3853, 3776, 3729, 3716, 3744, 3820, 3948, 4128, 4356, 4624, 4919, 5226, 5529, 5813, 6063, 6267, 6418, 6514, 6556, 6548, 6499, 6415, 6303, },
+	/* LAT: -60 */ {  6188, 5997, 5795, 5587, 5368, 5134, 4877, 4599, 4306, 4017, 3750, 3522, 3342, 3208, 3109, 3035, 2985, 2965, 2990, 3080, 3249, 3500, 3826, 4212, 4634, 5068, 5489, 5873, 6198, 6447, 6610, 6689, 6691, 6628, 6515, 6364, 6188, },
+	/* LAT: -50 */ {  5845, 5616, 5384, 5153, 4921, 4676, 4406, 4103, 3774, 3442, 3140, 2900, 2738, 2647, 2598, 2564, 2530, 2504, 2509, 2581, 2754, 3043, 3439, 3909, 4416, 4922, 5397, 5816, 6155, 6397, 6536, 6577, 6534, 6424, 6263, 6065, 5845, },
+	/* LAT: -40 */ {  5394, 5149, 4904, 4664, 4430, 4193, 3939, 3655, 3341, 3015, 2716, 2493, 2375, 2349, 2369, 2390, 2395, 2383, 2371, 2399, 2529, 2803, 3220, 3733, 4280, 4804, 5271, 5660, 5952, 6140, 6229, 6232, 6162, 6032, 5851, 5633, 5394, },
+	/* LAT: -30 */ {  4879, 4638, 4400, 4166, 3939, 3721, 3502, 3271, 3015, 2740, 2482, 2299, 2229, 2253, 2320, 2391, 2457, 2507, 2530, 2542, 2609, 2806, 3167, 3654, 4185, 4683, 5102, 5419, 5622, 5722, 5749, 5722, 5643, 5511, 5331, 5115, 4879, },
+	/* LAT: -20 */ {  4321, 4109, 3901, 3696, 3500, 3317, 3149, 2986, 2808, 2610, 2419, 2285, 2244, 2286, 2375, 2486, 2614, 2743, 2832, 2868, 2893, 2988, 3226, 3609, 4059, 4486, 4835, 5071, 5176, 5184, 5154, 5107, 5025, 4897, 4731, 4534, 4321, },
+	/* LAT: -10 */ {  3790, 3630, 3477, 3331, 3195, 3075, 2972, 2880, 2784, 2669, 2547, 2448, 2401, 2425, 2510, 2639, 2795, 2954, 3078, 3141, 3156, 3182, 3304, 3555, 3881, 4202, 4468, 4631, 4667, 4615, 4547, 4483, 4395, 4270, 4121, 3958, 3790, },
+	/* LAT:   0 */ {  3412, 3320, 3236, 3163, 3108, 3070, 3044, 3026, 3002, 2954, 2875, 2781, 2700, 2667, 2708, 2811, 2943, 3079, 3194, 3270, 3301, 3323, 3397, 3553, 3762, 3975, 4155, 4262, 4268, 4201, 4113, 4021, 3909, 3777, 3643, 3520, 3412, },
+	/* LAT:  10 */ {  3283, 3252, 3232, 3228, 3253, 3300, 3356, 3410, 3446, 3436, 3367, 3253, 3126, 3030, 3003, 3043, 3124, 3222, 3323, 3408, 3472, 3534, 3621, 3739, 3874, 4012, 4132, 4202, 4205, 4144, 4034, 3891, 3729, 3570, 3435, 3338, 3283, },
+	/* LAT:  20 */ {  3400, 3403, 3429, 3483, 3576, 3697, 3826, 3944, 4025, 4037, 3964, 3822, 3655, 3515, 3438, 3425, 3460, 3532, 3628, 3725, 3816, 3914, 4025, 4136, 4245, 4356, 4458, 4524, 4535, 4477, 4340, 4138, 3911, 3702, 3539, 3438, 3400, },
+	/* LAT:  30 */ {  3723, 3730, 3785, 3885, 4028, 4200, 4375, 4532, 4640, 4667, 4594, 4439, 4249, 4084, 3978, 3931, 3934, 3985, 4071, 4169, 4266, 4372, 4488, 4606, 4725, 4851, 4972, 5058, 5085, 5028, 4872, 4632, 4359, 4104, 3905, 3777, 3723, },
+	/* LAT:  40 */ {  4222, 4221, 4287, 4411, 4578, 4766, 4949, 5106, 5211, 5236, 5168, 5019, 4832, 4657, 4528, 4452, 4426, 4447, 4508, 4587, 4673, 4770, 4884, 5018, 5169, 5330, 5481, 5591, 5631, 5577, 5422, 5184, 4911, 4651, 4441, 4296, 4222, },
+	/* LAT:  50 */ {  4832, 4825, 4881, 4992, 5139, 5298, 5448, 5569, 5643, 5652, 5588, 5461, 5298, 5134, 4996, 4898, 4843, 4831, 4855, 4905, 4974, 5064, 5183, 5333, 5509, 5693, 5859, 5977, 6022, 5978, 5848, 5652, 5427, 5211, 5031, 4902, 4832, },
+	/* LAT:  60 */ {  5392, 5380, 5409, 5472, 5558, 5653, 5740, 5806, 5839, 5830, 5775, 5681, 5560, 5431, 5312, 5216, 5149, 5115, 5112, 5140, 5196, 5282, 5400, 5547, 5713, 5882, 6029, 6134, 6178, 6156, 6074, 5947, 5801, 5658, 5536, 5445, 5392, },
+	/* LAT:  70 */ {  5726, 5706, 5704, 5716, 5739, 5765, 5788, 5803, 5802, 5783, 5744, 5686, 5615, 5539, 5465, 5401, 5353, 5325, 5320, 5340, 5384, 5454, 5545, 5654, 5771, 5885, 5985, 6058, 6097, 6101, 6072, 6018, 5951, 5881, 5816, 5763, 5726, },
+	/* LAT:  80 */ {  5789, 5772, 5758, 5746, 5736, 5726, 5716, 5705, 5690, 5671, 5649, 5624, 5597, 5569, 5544, 5524, 5510, 5505, 5510, 5525, 5550, 5586, 5628, 5676, 5726, 5774, 5817, 5851, 5876, 5889, 5891, 5884, 5870, 5851, 5830, 5809, 5789, },
 	/* LAT:  90 */ {  5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, 5681, },
 };

src/modules/airship_att_control/airship_att_control.hpp

@@ -46,11 +46,6 @@
 
 using namespace time_literals;
 
-/**
- * Airship attitude control app start / stop handling function
- */
-extern "C" __EXPORT int airship_att_control_main(int argc, char *argv[]);
-
 class AirshipAttitudeControl : public ModuleBase<AirshipAttitudeControl>, public ModuleParams,
 	public px4::WorkItem
 {
@@ -80,27 +75,27 @@ private:
 	/**
 	 * Check for parameter update and handle it.
 	 */
-	void		parameter_update_poll();
+	void parameter_update_poll();
 
-	void		publish_actuator_controls();
+	void publish_actuator_controls();
 
 	void publishTorqueSetpoint(const hrt_abstime &timestamp_sample);
 	void publishThrustSetpoint(const hrt_abstime &timestamp_sample);
 
-	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};		/**< parameter updates subscription */
-	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};			/**< vehicle status subscription */
-	uORB::Subscription _manual_control_sp_sub{ORB_ID(manual_control_setpoint)};	/**< manual control setpoint subscription */
+	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};        /**< parameter updates subscription */
+	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};                         /**< vehicle status subscription */
+	uORB::Subscription _manual_control_sp_sub{ORB_ID(manual_control_setpoint)};             /**< manual control setpoint subscription */
 
 	uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)};
 
-	uORB::Publication<actuator_controls_s>		_actuators_0_pub;
-	uORB::Publication<vehicle_thrust_setpoint_s>	_vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)};
-	uORB::Publication<vehicle_torque_setpoint_s>	_vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)};
+	uORB::Publication<actuator_controls_s>          _actuator_controls_0_pub;
+	uORB::Publication<vehicle_thrust_setpoint_s>    _vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)};
+	uORB::Publication<vehicle_torque_setpoint_s>    _vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)};
 
-	struct manual_control_setpoint_s	_manual_control_sp {};	/**< manual control setpoint */
-	struct vehicle_status_s			_vehicle_status {};	/**< vehicle status */
-	struct actuator_controls_s		_actuators {};		/**< actuator controls */
+	manual_control_setpoint_s       _manual_control_sp {};  /**< manual control setpoint */
+	vehicle_status_s                _vehicle_status {};     /**< vehicle status */
+	actuator_controls_s             _actuator_controls {};          /**< actuator controls */
 
-	perf_counter_t	_loop_perf;			/**< loop performance counter */
+	perf_counter_t  _loop_perf;     /**< loop performance counter */
 
 };

src/modules/airship_att_control/airship_att_control_main.cpp

@@ -45,7 +45,7 @@ using namespace matrix;
 AirshipAttitudeControl::AirshipAttitudeControl() :
 	ModuleParams(nullptr),
 	WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl),
-	_actuators_0_pub(ORB_ID(actuator_controls_0)),
+	_actuator_controls_0_pub(ORB_ID(actuator_controls_0)),
 	_loop_perf(perf_alloc(PC_ELAPSED, "airship_att_control"))
 {
 }
@@ -86,20 +86,20 @@ AirshipAttitudeControl::publish_actuator_controls()
 	// zero actuators if not armed
 	if (_vehicle_status.arming_state != vehicle_status_s::ARMING_STATE_ARMED) {
 		for (uint8_t i = 0 ; i < 4 ; i++) {
-			_actuators.control[i] = 0.0f;
+			_actuator_controls.control[i] = 0.0f;
 		}
 
 	} else {
-		_actuators.control[0] = 0.0f;
-		_actuators.control[1] = _manual_control_sp.x;
-		_actuators.control[2] = _manual_control_sp.r;
-		_actuators.control[3] = _manual_control_sp.z;
+		_actuator_controls.control[0] = 0.0f;
+		_actuator_controls.control[1] = _manual_control_sp.x;
+		_actuator_controls.control[2] = _manual_control_sp.r;
+		_actuator_controls.control[3] = _manual_control_sp.z;
 	}
 
-	// note: _actuators.timestamp_sample is set in AirshipAttitudeControl::Run()
-	_actuators.timestamp = hrt_absolute_time();
+	// note: _actuator_controls.timestamp_sample is set in AirshipAttitudeControl::Run()
+	_actuator_controls.timestamp = hrt_absolute_time();
 
-	_actuators_0_pub.publish(_actuators);
+	_actuator_controls_0_pub.publish(_actuator_controls);
 }
 
 void AirshipAttitudeControl::publishTorqueSetpoint(const hrt_abstime &timestamp_sample)
@@ -107,9 +107,9 @@ void AirshipAttitudeControl::publishTorqueSetpoint(const hrt_abstime &timestamp_
 	vehicle_torque_setpoint_s v_torque_sp = {};
 	v_torque_sp.timestamp = hrt_absolute_time();
 	v_torque_sp.timestamp_sample = timestamp_sample;
-	v_torque_sp.xyz[0] = _actuators.control[0];
-	v_torque_sp.xyz[1] = _actuators.control[1];
-	v_torque_sp.xyz[2] = _actuators.control[2];
+	v_torque_sp.xyz[0] = _actuator_controls.control[0];
+	v_torque_sp.xyz[1] = _actuator_controls.control[1];
+	v_torque_sp.xyz[2] = _actuator_controls.control[2];
 
 	_vehicle_torque_setpoint_pub.publish(v_torque_sp);
 }
@@ -119,7 +119,7 @@ void AirshipAttitudeControl::publishThrustSetpoint(const hrt_abstime &timestamp_
 	vehicle_thrust_setpoint_s v_thrust_sp = {};
 	v_thrust_sp.timestamp = hrt_absolute_time();
 	v_thrust_sp.timestamp_sample = timestamp_sample;
-	v_thrust_sp.xyz[0] = _actuators.control[3];
+	v_thrust_sp.xyz[0] = _actuator_controls.control[3];
 	v_thrust_sp.xyz[1] = 0.0f;
 	v_thrust_sp.xyz[2] = 0.0f;
 
@@ -142,7 +142,7 @@ AirshipAttitudeControl::Run()
 
 	if (_vehicle_angular_velocity_sub.update(&angular_velocity)) {
 
-		_actuators.timestamp_sample = angular_velocity.timestamp_sample;
+		_actuator_controls.timestamp_sample = angular_velocity.timestamp_sample;
 
 		/* run the rate controller immediately after a gyro update */
 		publish_actuator_controls();
@@ -190,7 +190,7 @@ int AirshipAttitudeControl::print_status()
 
 	perf_print_counter(_loop_perf);
 
-	print_message(ORB_ID(actuator_controls), _actuators);
+	print_message(ORB_ID(actuator_controls), _actuator_controls);
 
 	return 0;
 }
@@ -227,7 +227,10 @@ To reduce control latency, the module directly polls on the gyro topic published
 	return 0;
 }
 
-int airship_att_control_main(int argc, char *argv[])
+/**
+ * Airship attitude control app start / stop handling function
+ */
+extern "C" __EXPORT int airship_att_control_main(int argc, char *argv[])
 {
 	return AirshipAttitudeControl::main(argc, argv);
 }

src/modules/airspeed_selector/airspeed_selector_main.cpp

@@ -513,7 +513,8 @@ void AirspeedModule::update_wind_estimator_sideslip()
 	// update wind and airspeed estimator
 	_wind_estimator_sideslip.update(_time_now_usec);
 
-	if (_vehicle_local_position_valid && !_vtol_vehicle_status.vtol_in_rw_mode) {
+	if (_vehicle_local_position_valid
+	    && _vtol_vehicle_status.vehicle_vtol_state == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW) {
 		Vector3f vI(_vehicle_local_position.vx, _vehicle_local_position.vy, _vehicle_local_position.vz);
 		Quatf q(_vehicle_attitude.q);