Install microDDS build requirements ubuntu.sh --microdds

Update ubuntu.sh to Jammy
Commander: fix overlooked rebase naming error
2026-06-13 21:40:05 +08:00 · 2022-05-25 21:30:14 -04:00 · 2022-05-25 19:07:15 -04:00 · 2022-05-25 21:10:19 +02:00 · 2022-05-25 17:08:48 +02:00 · 2022-05-25 09:56:40 -04:00
155 changed files with 11045 additions and 8005 deletions
@@ -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",
@@ -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()

@@ -864,12 +864,15 @@ void printTopics() {
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener cpuload" || true'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener distance_sensor" || true'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener ekf2_timestamps" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_gps_status" || true'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener esc_status" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_aid_src_fake_pos" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_aid_src_gnss_pos" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_aid_src_gnss_vel" || true'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_attitude" || true'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_baro_bias" || true'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_event_flags" || true'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_global_position" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_gps_status" || true'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_innovation_test_ratios" || true'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_innovation_variances" || true'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_innovations" || true'
@@ -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.
@@ -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.
@@ -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!
@@ -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!
@@ -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.
@@ -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,
@@ -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
@@ -95,10 +95,9 @@ fi

 if param compare SYS_AUTOCONFIG 1
 then
+	# Reset params except Airframe, RC calibration, sensor calibration, flight modes, total flight time, and next flight UUID.
+	param reset_all SYS_AUTOSTART RC* CAL_* COM_FLTMODE* LND_FLIGHT* TC_* COM_FLIGHT*
 	set AUTOCNF yes
-
-	# Wipe out params except RC*, flight modes, total flight time, accel cal, gyro cal, next flight UUID
-	param reset_all SYS_AUTO* RC* COM_FLTMODE* LND_FLIGHT* TC_* CAL_ACC* CAL_GYRO* COM_FLIGHT*
 fi

 # multi-instance setup
@@ -196,14 +195,6 @@ fi

 . "$autostart_file"

-#
-# If autoconfig parameter was set, reset it and save parameters.
-#
-if [ $AUTOCNF = yes ]
-then
-	param set SYS_AUTOCONFIG 0
-fi
-
 # Simulator IMU data provided at 250 Hz
 param set IMU_INTEG_RATE 250

@@ -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
@@ -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
@@ -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
@@ -181,30 +181,6 @@ then
 	pcf8583 start -X -a 0x51
 fi

-# INA226 digital power monitor
-if param compare SENS_EN_INA226 1
-then
-	ina226 -X start
-fi
-
-# INA228 digital power monitor
-if param compare SENS_EN_INA228 1
-then
-	ina228 -X start
-fi
-
-# INA231 digital power monitor
-if param compare SENS_EN_INA231 1
-then
-	ina231 -X start
-fi
-
-# INA238 digital power monitor
-if param compare SENS_EN_INA238 1
-then
-	ina238 -X start
-fi
-
 # probe for optional external I2C devices
 if param compare SENS_EXT_I2C_PRB 1
 then
@@ -21,7 +21,6 @@ set +e
 # it wastes flash
 #
 set R /
-set AUTOCNF no
 set FCONFIG /fs/microsd/etc/config.txt
 set FEXTRAS /fs/microsd/etc/extras.txt
 set FRC /fs/microsd/etc/rc.txt
@@ -177,13 +176,12 @@ else
 	fi

 	#
-	# Set AUTOCNF flag to use it in AUTOSTART scripts.
+	# If the airframe has been previously reset SYS_AUTCONFIG will have been set to 1 and other params will be reset on the next boot.
 	#
 	if param greater SYS_AUTOCONFIG 0
 	then
-		# Wipe out params except RC*, flight modes, total flight time, calibration parameters, next flight UUID
-		param reset_all SYS_AUTO* RC* COM_FLTMODE* LND_FLIGHT* TC_* CAL_ACC* CAL_GYRO* COM_FLIGHT*
-		set AUTOCNF yes
+		# Reset params except Airframe, RC calibration, sensor calibration, flight modes, total flight time, and next flight UUID.
+		param reset_all SYS_AUTOSTART RC* CAL_* COM_FLTMODE* LND_FLIGHT* TC_* COM_FLIGHT*
 	fi

 	#
@@ -274,14 +272,6 @@ else
 		. $FCONFIG
 	fi

-	#
-	# If autoconfig parameter was set, reset it and save parameters.
-	#
-	if [ $AUTOCNF = yes ]
-	then
-		param set SYS_AUTOCONFIG 0
-	fi
-
 	#
 	# Check if UAVCAN is enabled, default to it for ESCs.
 	#
@@ -565,7 +555,6 @@ fi
 # Unset all script parameters to free RAM.
 #
 unset R
-unset AUTOCNF
 unset FCONFIG
 unset FEXTRAS
 unset FRC
@@ -11,7 +11,7 @@ from pyulog import ULog
 from analysis.detectors import InAirDetector, PreconditionError
 from analysis.metrics import calculate_ecl_ekf_metrics
 from analysis.checks import perform_ecl_ekf_checks
-from analysis.post_processing import get_estimator_check_flags
+from analysis.post_processing import get_gps_check_fail_flags

 def analyse_ekf(
        ulog: ULog, check_levels: Dict[str, float], multi_instance: int = 0,
@@ -40,6 +40,11 @@ def analyse_ekf(
    except:
        raise PreconditionError('could not find estimator_status instance', multi_instance)

+    try:
+        estimator_status_flags = ulog.get_dataset('estimator_status_flags', multi_instance).data
+    except:
+        raise PreconditionError('could not find estimator_status_flags instance', multi_instance)
+
    try:
        _ = ulog.get_dataset('estimator_innovations', multi_instance).data
    except:
@@ -61,14 +66,14 @@ def analyse_ekf(
        'in_air_transition_time': round(in_air.take_off + in_air.log_start, 2),
        'on_ground_transition_time': round(in_air.landing + in_air.log_start, 2)}

-    control_mode, innov_flags, gps_fail_flags = get_estimator_check_flags(estimator_status)
+    gps_fail_flags = get_gps_check_fail_flags(estimator_status)

    sensor_checks, innov_fail_checks = find_checks_that_apply(
-        control_mode, estimator_status,
+        estimator_status_flags, estimator_status,
        pos_checks_when_sensors_not_fused=pos_checks_when_sensors_not_fused)

    metrics = calculate_ecl_ekf_metrics(
-        ulog, innov_flags, innov_fail_checks, sensor_checks, in_air, in_air_no_ground_effects,
+        ulog, estimator_status_flags, innov_fail_checks, sensor_checks, in_air, in_air_no_ground_effects,
        multi_instance, red_thresh=red_thresh, amb_thresh=amb_thresh)

    check_status, master_status = perform_ecl_ekf_checks(
@@ -78,12 +83,12 @@ def analyse_ekf(


 def find_checks_that_apply(
-    control_mode: dict, estimator_status: dict, pos_checks_when_sensors_not_fused: bool = False) ->\
+    estimator_status_flags: dict, estimator_status: dict, pos_checks_when_sensors_not_fused: bool = False) ->\
        Tuple[List[str], List[str]]:
    """
    finds the checks that apply and stores them in lists for the std checks and the innovation
    fail checks.
-    :param control_mode:
+    :param estimator_status_flags:
    :param estimator_status:
    :param b_pos_only_when_sensors_fused:
    :return: a tuple of two lists that contain strings for the std checks and for the innovation
@@ -97,7 +102,7 @@ def find_checks_that_apply(
    innov_fail_checks.append('posv')

    # Magnetometer Sensor Checks
-    if (np.amax(control_mode['yaw_aligned']) > 0.5):
+    if (np.amax(estimator_status_flags['cs_yaw_align']) > 0.5):
        sensor_checks.append('mag')

        innov_fail_checks.append('magx')
@@ -106,13 +111,14 @@ def find_checks_that_apply(
        innov_fail_checks.append('yaw')

    # Velocity Sensor Checks
-    if (np.amax(control_mode['using_gps']) > 0.5):
+    if (np.amax(estimator_status_flags['cs_gps']) > 0.5):
        sensor_checks.append('vel')
-        innov_fail_checks.append('vel')
+        innov_fail_checks.append('velh')
+        innov_fail_checks.append('velv')

    # Position Sensor Checks
-    if (pos_checks_when_sensors_not_fused or (np.amax(control_mode['using_gps']) > 0.5)
-        or (np.amax(control_mode['using_evpos']) > 0.5)):
+    if (pos_checks_when_sensors_not_fused or (np.amax(estimator_status_flags['cs_gps']) > 0.5)
+        or (np.amax(estimator_status_flags['cs_ev_pos']) > 0.5)):
        sensor_checks.append('pos')
        innov_fail_checks.append('posh')

@@ -128,7 +134,7 @@ def find_checks_that_apply(
        innov_fail_checks.append('hagl')

    # optical flow sensor checks
-    if (np.amax(control_mode['using_optflow']) > 0.5):
+    if (np.amax(estimator_status_flags['cs_opt_flow']) > 0.5):
        innov_fail_checks.append('ofx')
        innov_fail_checks.append('ofy')

@@ -123,7 +123,8 @@ def perform_sensor_innov_checks(
                                              ('magy', 'magy_fail_percentage', 'mag'),
                                              ('magz', 'magz_fail_percentage', 'mag'),
                                              ('yaw', 'yaw_fail_percentage', 'yaw'),
-                                              ('vel', 'vel_fail_percentage', 'vel'),
+                                              ('velh', 'vel_fail_percentage', 'vel'),
+                                              ('velv', 'vel_fail_percentage', 'vel'),
                                              ('posh', 'pos_fail_percentage', 'pos'),
                                              ('tas', 'tas_fail_percentage', 'tas'),
                                              ('hagl', 'hagl_fail_percentage', 'hagl'),
@@ -11,7 +11,7 @@ import numpy as np
 from analysis.detectors import InAirDetector

 def calculate_ecl_ekf_metrics(
-        ulog: ULog, innov_flags: Dict[str, float], innov_fail_checks: List[str],
+        ulog: ULog, estimator_status_flags: Dict[str, float], innov_fail_checks: List[str],
        sensor_checks: List[str], in_air: InAirDetector, in_air_no_ground_effects: InAirDetector,
        multi_instance: int = 0, red_thresh: float = 1.0, amb_thresh: float = 0.5) -> Tuple[dict, dict, dict, dict]:

@@ -20,7 +20,7 @@ def calculate_ecl_ekf_metrics(
        red_thresh=red_thresh, amb_thresh=amb_thresh)

    innov_fail_metrics = calculate_innov_fail_metrics(
-        innov_flags, innov_fail_checks, in_air, in_air_no_ground_effects)
+        estimator_status_flags, innov_fail_checks, in_air, in_air_no_ground_effects)

    imu_metrics = calculate_imu_metrics(ulog, multi_instance, in_air_no_ground_effects)

@@ -90,10 +90,10 @@ def calculate_sensor_metrics(


 def calculate_innov_fail_metrics(
-        innov_flags: dict, innov_fail_checks: List[str], in_air: InAirDetector,
+        estimator_status_flags: dict, innov_fail_checks: List[str], in_air: InAirDetector,
        in_air_no_ground_effects: InAirDetector) -> dict:
    """
-    :param innov_flags:
+    :param estimator_status_flags:
    :param innov_fail_checks:
    :param in_air:
    :param in_air_no_ground_effects:
@@ -103,17 +103,18 @@ def calculate_innov_fail_metrics(
    innov_fail_metrics = dict()

    # calculate innovation check fail metrics
-    for signal_id, signal, result in [('posv', 'posv_innov_fail', 'hgt_fail_percentage'),
-                                      ('magx', 'magx_innov_fail', 'magx_fail_percentage'),
-                                      ('magy', 'magy_innov_fail', 'magy_fail_percentage'),
-                                      ('magz', 'magz_innov_fail', 'magz_fail_percentage'),
-                                      ('yaw', 'yaw_innov_fail', 'yaw_fail_percentage'),
-                                      ('vel', 'vel_innov_fail', 'vel_fail_percentage'),
-                                      ('posh', 'posh_innov_fail', 'pos_fail_percentage'),
-                                      ('tas', 'tas_innov_fail', 'tas_fail_percentage'),
-                                      ('hagl', 'hagl_innov_fail', 'hagl_fail_percentage'),
-                                      ('ofx', 'ofx_innov_fail', 'ofx_fail_percentage'),
-                                      ('ofy', 'ofy_innov_fail', 'ofy_fail_percentage')]:
+    for signal_id, signal, result in [('posv', 'reject_ver_pos', 'hgt_fail_percentage'),
+                                      ('magx', 'reject_mag_x', 'magx_fail_percentage'),
+                                      ('magy', 'reject_mag_y', 'magy_fail_percentage'),
+                                      ('magz', 'reject_mag_z', 'magz_fail_percentage'),
+                                      ('yaw', 'reject_yaw', 'yaw_fail_percentage'),
+                                      ('velh', 'reject_hor_vel', 'vel_fail_percentage'),
+                                      ('velv', 'reject_ver_vel', 'vel_fail_percentage'),
+                                      ('posh', 'reject_hor_pos', 'pos_fail_percentage'),
+                                      ('tas', 'reject_airspeed', 'tas_fail_percentage'),
+                                      ('hagl', 'reject_hagl', 'hagl_fail_percentage'),
+                                      ('ofx', 'reject_optflow_x', 'ofx_fail_percentage'),
+                                      ('ofy', 'reject_optflow_y', 'ofy_fail_percentage')]:

        # only run innov fail checks, if they apply.
        if signal_id in innov_fail_checks:
@@ -125,7 +126,7 @@ def calculate_innov_fail_metrics(
                in_air_detector = in_air

            innov_fail_metrics[result] = calculate_stat_from_signal(
-                innov_flags, 'estimator_status', signal, in_air_detector,
+                estimator_status_flags, 'estimator_status_flags', signal, in_air_detector,
                lambda x: 100.0 * np.mean(x > 0.5))

    return innov_fail_metrics
@@ -152,7 +153,7 @@ def calculate_imu_metrics(ulog: ULog, multi_instance, in_air_no_ground_effects:

            if vehicle_imu_status_data['accel_device_id'][0] == estimator_status_data['accel_device_id'][0]:

-                for signal, result in [('delta_angle_coning_metric', 'imu_coning'),
+                for signal, result in [('gyro_coning_vibration', 'imu_coning'),
                                       ('gyro_vibration_metric', 'imu_hfgyro'),
                                       ('accel_vibration_metric', 'imu_hfaccel')]:

@@ -7,115 +7,6 @@ from typing import Tuple

 import numpy as np

-
-def get_estimator_check_flags(estimator_status: dict) -> Tuple[dict, dict, dict]:
-    """
-    :param estimator_status:
-    :return:
-    """
-    control_mode = get_control_mode_flags(estimator_status)
-    innov_flags = get_innovation_check_flags(estimator_status)
-    gps_fail_flags = get_gps_check_fail_flags(estimator_status)
-    return control_mode, innov_flags, gps_fail_flags
-
-
-def get_control_mode_flags(estimator_status: dict) -> dict:
-    """
-    :param estimator_status:
-    :return:
-    """
-
-    control_mode = dict()
-    # extract control mode metadata from estimator_status.control_mode_flags
-    # 0 - true if the filter tilt alignment is complete
-    # 1 - true if the filter yaw alignment is complete
-    # 2 - true if GPS measurements are being fused
-    # 3 - true if optical flow measurements are being fused
-    # 4 - true if a simple magnetic yaw heading is being fused
-    # 5 - true if 3-axis magnetometer measurement are being fused
-    # 6 - true if synthetic magnetic declination measurements are being fused
-    # 7 - true when the vehicle is airborne
-    # 8 - true when wind velocity is being estimated
-    # 9 - true when baro height is being fused as a primary height reference
-    # 10 - true when range finder height is being fused as a primary height reference
-    # 11 - true when range finder height is being fused as a primary height reference
-    # 12 - true when local position data from external vision is being fused
-    # 13 - true when yaw data from external vision measurements is being fused
-    # 14 - true when height data from external vision measurements is being fused
-    # 15 - true when synthetic sideslip measurements are being fused
-    # 16 - true true when the mag field does not match the expected strength
-    # 17 - true true when the vehicle is operating as a fixed wing vehicle
-    # 18 - true when the magnetometer has been declared faulty and is no longer being used
-    # 19 - true true when airspeed measurements are being fused
-    # 20 - true true when protection from ground effect induced static pressure rise is active
-    # 21 - true when rng data wasn't ready for more than 10s and new rng values haven't changed enough
-    # 22 - true when yaw (not ground course) data from a GPS receiver is being fused
-    # 23 - true when the in-flight mag field alignment has been completed
-    # 24 - true when local earth frame velocity data from external vision measurements are being fused
-    # 25 - true when we are using a synthesized measurement for the magnetometer Z component
-    control_mode['tilt_aligned'] = ((2 ** 0 & estimator_status['control_mode_flags']) > 0) * 1
-    control_mode['yaw_aligned'] = ((2 ** 1 & estimator_status['control_mode_flags']) > 0) * 1
-    control_mode['using_gps'] = ((2 ** 2 & estimator_status['control_mode_flags']) > 0) * 1
-    control_mode['using_optflow'] = ((2 ** 3 & estimator_status['control_mode_flags']) > 0) * 1
-    control_mode['using_magyaw'] = ((2 ** 4 & estimator_status['control_mode_flags']) > 0) * 1
-    control_mode['using_mag3d'] = ((2 ** 5 & estimator_status['control_mode_flags']) > 0) * 1
-    control_mode['using_magdecl'] = ((2 ** 6 & estimator_status['control_mode_flags']) > 0) * 1
-    control_mode['airborne'] = ((2 ** 7 & estimator_status['control_mode_flags']) > 0) * 1
-    control_mode['estimating_wind'] = ((2 ** 8 & estimator_status['control_mode_flags']) > 0) * 1
-    control_mode['using_barohgt'] = ((2 ** 9 & estimator_status['control_mode_flags']) > 0) * 1
-    control_mode['using_rnghgt'] = ((2 ** 10 & estimator_status['control_mode_flags']) > 0) * 1
-    control_mode['using_gpshgt'] = ((2 ** 11 & estimator_status['control_mode_flags']) > 0) * 1
-    control_mode['using_evpos'] = ((2 ** 12 & estimator_status['control_mode_flags']) > 0) * 1
-    control_mode['using_evyaw'] = ((2 ** 13 & estimator_status['control_mode_flags']) > 0) * 1
-    control_mode['using_evhgt'] = ((2 ** 14 & estimator_status['control_mode_flags']) > 0) * 1
-    control_mode['fuse_beta'] = ((2 ** 15 & estimator_status['control_mode_flags']) > 0) * 1
-    control_mode['mag_field_disturbed'] = ((2 ** 16 & estimator_status['control_mode_flags']) > 0) * 1
-    control_mode['fixed_wing'] = ((2 ** 17 & estimator_status['control_mode_flags']) > 0) * 1
-    control_mode['mag_fault'] = ((2 ** 18 & estimator_status['control_mode_flags']) > 0) * 1
-    control_mode['fuse_aspd'] = ((2 ** 19 & estimator_status['control_mode_flags']) > 0) * 1
-    control_mode['gnd_effect'] = ((2 ** 20 & estimator_status['control_mode_flags']) > 0) * 1
-    control_mode['rng_stuck'] = ((2 ** 21 & estimator_status['control_mode_flags']) > 0) * 1
-    control_mode['gps_yaw'] = ((2 ** 22 & estimator_status['control_mode_flags']) > 0) * 1
-    control_mode['mag_aligned_in_flight'] = ((2 ** 23 & estimator_status['control_mode_flags']) > 0) * 1
-    control_mode['ev_vel'] = ((2 ** 24 & estimator_status['control_mode_flags']) > 0) * 1
-    control_mode['synthetic_mag_z'] = ((2 ** 25 & estimator_status['control_mode_flags']) > 0) * 1
-    return control_mode
-
-def get_innovation_check_flags(estimator_status: dict) -> dict:
-    """
-    :param estimator_status:
-    :return:
-    """
-
-    innov_flags = dict()
-    # innovation_check_flags summary
-    # 0 - true if velocity observations have been rejected
-    # 1 - true if horizontal position observations have been rejected
-    # 2 - true if true if vertical position observations have been rejected
-    # 3 - true if the X magnetometer observation has been rejected
-    # 4 - true if the Y magnetometer observation has been rejected
-    # 5 - true if the Z magnetometer observation has been rejected
-    # 6 - true if the yaw observation has been rejected
-    # 7 - true if the airspeed observation has been rejected
-    # 8 - true if synthetic sideslip observation has been rejected
-    # 9 - true if the height above ground observation has been rejected
-    # 10 - true if the X optical flow observation has been rejected
-    # 11 - true if the Y optical flow observation has been rejected
-    innov_flags['vel_innov_fail'] = ((2 ** 0 & estimator_status['innovation_check_flags']) > 0) * 1
-    innov_flags['posh_innov_fail'] = ((2 ** 1 & estimator_status['innovation_check_flags']) > 0) * 1
-    innov_flags['posv_innov_fail'] = ((2 ** 2 & estimator_status['innovation_check_flags']) > 0) * 1
-    innov_flags['magx_innov_fail'] = ((2 ** 3 & estimator_status['innovation_check_flags']) > 0) * 1
-    innov_flags['magy_innov_fail'] = ((2 ** 4 & estimator_status['innovation_check_flags']) > 0) * 1
-    innov_flags['magz_innov_fail'] = ((2 ** 5 & estimator_status['innovation_check_flags']) > 0) * 1
-    innov_flags['yaw_innov_fail'] = ((2 ** 6 & estimator_status['innovation_check_flags']) > 0) * 1
-    innov_flags['tas_innov_fail'] = ((2 ** 7 & estimator_status['innovation_check_flags']) > 0) * 1
-    innov_flags['sli_innov_fail'] = ((2 ** 8 & estimator_status['innovation_check_flags']) > 0) * 1
-    innov_flags['hagl_innov_fail'] = ((2 ** 9 & estimator_status['innovation_check_flags']) > 0) * 1
-    innov_flags['ofx_innov_fail'] = ((2 ** 10 & estimator_status['innovation_check_flags']) > 0) * 1
-    innov_flags['ofy_innov_fail'] = ((2 ** 11 & estimator_status['innovation_check_flags']) > 0) * 1
-    return innov_flags
-
-
 def get_gps_check_fail_flags(estimator_status: dict) -> dict:
    """
    :param estimator_status:
@@ -11,7 +11,7 @@ import numpy as np
 from matplotlib.backends.backend_pdf import PdfPages
 from pyulog import ULog

-from analysis.post_processing import magnetic_field_estimates_from_states, get_estimator_check_flags
+from analysis.post_processing import magnetic_field_estimates_from_states, get_gps_check_fail_flags
 from plotting.data_plots import TimeSeriesPlot, InnovationPlot, ControlModeSummaryPlot, \
    CheckFlagsPlot
 from analysis.detectors import PreconditionError
@@ -33,6 +33,11 @@ def create_pdf_report(ulog: ULog, multi_instance: int, output_plot_filename: str
    except:
        raise PreconditionError('could not find estimator_status instance', multi_instance)

+    try:
+        estimator_status_flags = ulog.get_dataset('estimator_status_flags', multi_instance).data
+    except:
+        raise PreconditionError('could not find estimator_status_flags instance', multi_instance)
+
    try:
        estimator_states = ulog.get_dataset('estimator_states', multi_instance).data
    except:
@@ -68,12 +73,13 @@ def create_pdf_report(ulog: ULog, multi_instance: int, output_plot_filename: str
    except:
        raise PreconditionError('could not find innovation data')

-    control_mode, innov_flags, gps_fail_flags = get_estimator_check_flags(estimator_status)
+    gps_fail_flags = get_gps_check_fail_flags(estimator_status)

    status_time = 1e-6 * estimator_status['timestamp']
+    status_flags_time = 1e-6 * estimator_status_flags['timestamp']

    b_finishes_in_air, b_starts_in_air, in_air_duration, in_air_transition_time, \
-    on_ground_transition_time = detect_airtime(control_mode, status_time)
+    on_ground_transition_time = detect_airtime(estimator_status_flags, status_flags_time)

    with PdfPages(output_plot_filename) as pdf_pages:

@@ -173,9 +179,9 @@ def create_pdf_report(ulog: ULog, multi_instance: int, output_plot_filename: str

        # plot control mode summary A
        data_plot = ControlModeSummaryPlot(
-            status_time, control_mode, [['tilt_aligned', 'yaw_aligned'],
-            ['using_gps', 'using_optflow', 'using_evpos'], ['using_barohgt', 'using_gpshgt',
-             'using_rnghgt', 'using_evhgt'], ['using_magyaw', 'using_mag3d', 'using_magdecl']],
+            status_flags_time, estimator_status_flags, [['cs_tilt_align', 'cs_yaw_align'],
+            ['cs_gps', 'cs_opt_flow', 'cs_ev_pos'], ['cs_baro_hgt', 'cs_gps_hgt',
+             'cs_rng_hgt', 'cs_ev_hgt'], ['cs_mag_hdg', 'cs_mag_3d', 'cs_mag_dec']],
            x_label='time (sec)', y_labels=['aligned', 'pos aiding', 'hgt aiding', 'mag aiding'],
            annotation_text=[['tilt alignment', 'yaw alignment'], ['GPS aiding', 'optical flow aiding',
             'external vision aiding'], ['Baro aiding', 'GPS aiding', 'rangefinder aiding',
@@ -188,7 +194,7 @@ def create_pdf_report(ulog: ULog, multi_instance: int, output_plot_filename: str
        # plot control mode summary B
        # construct additional annotations for the airborne plot
        airborne_annotations = list()
-        if np.amin(np.diff(control_mode['airborne'])) > -0.5:
+        if np.amin(np.diff(estimator_status_flags['cs_in_air'])) > -0.5:
            airborne_annotations.append(
                (on_ground_transition_time, 'air to ground transition not detected'))
        else:
@@ -197,7 +203,7 @@ def create_pdf_report(ulog: ULog, multi_instance: int, output_plot_filename: str
        if in_air_duration > 0.0:
            airborne_annotations.append(((in_air_transition_time + on_ground_transition_time) / 2,
                                         'duration = {:.1f} sec'.format(in_air_duration)))
-        if np.amax(np.diff(control_mode['airborne'])) < 0.5:
+        if np.amax(np.diff(estimator_status_flags['cs_in_air'])) < 0.5:
            airborne_annotations.append(
                (in_air_transition_time, 'ground to air transition not detected'))
        else:
@@ -205,7 +211,7 @@ def create_pdf_report(ulog: ULog, multi_instance: int, output_plot_filename: str
                (in_air_transition_time, 'in-air at {:.1f} sec'.format(in_air_transition_time)))

        data_plot = ControlModeSummaryPlot(
-            status_time, control_mode, [['airborne'], ['estimating_wind']],
+            status_flags_time, estimator_status_flags, [['cs_in_air'], ['cs_wind']],
            x_label='time (sec)', y_labels=['airborne', 'estimating wind'], annotation_text=[[], []],
            additional_annotation=[airborne_annotations, []],
            plot_title='EKF Control Status - Figure B', pdf_handle=pdf_pages)
@@ -214,15 +220,15 @@ def create_pdf_report(ulog: ULog, multi_instance: int, output_plot_filename: str

        # plot innovation_check_flags summary
        data_plot = CheckFlagsPlot(
-            status_time, innov_flags, [['vel_innov_fail', 'posh_innov_fail'], ['posv_innov_fail',
-                                                                               'hagl_innov_fail'],
-                                       ['magx_innov_fail', 'magy_innov_fail', 'magz_innov_fail',
-                                        'yaw_innov_fail'], ['tas_innov_fail'], ['sli_innov_fail'],
-                                       ['ofx_innov_fail',
-                                        'ofy_innov_fail']], x_label='time (sec)',
+            status_flags_time, estimator_status_flags, [['reject_hor_vel', 'reject_hor_pos'], ['reject_ver_vel', 'reject_ver_pos',
+                                                                               'reject_hagl'],
+                                       ['reject_mag_x', 'reject_mag_y', 'reject_mag_z',
+                                        'reject_yaw'], ['reject_airspeed'], ['reject_sideslip'],
+                                       ['reject_optflow_x',
+                                        'reject_optflow_y']], x_label='time (sec)',
            y_labels=['failed', 'failed', 'failed', 'failed', 'failed', 'failed'],
            y_lim=(-0.1, 1.1),
-            legend=[['vel NED', 'pos NE'], ['hgt absolute', 'hgt above ground'],
+            legend=[['vel NE', 'pos NE'], ['vel D', 'hgt absolute', 'hgt above ground'],
                    ['mag_x', 'mag_y', 'mag_z', 'yaw'], ['airspeed'], ['sideslip'],
                    ['flow X', 'flow Y']],
            plot_title='EKF Innovation Test Fails', annotate=False, pdf_handle=pdf_pages)
@@ -344,33 +350,33 @@ def create_pdf_report(ulog: ULog, multi_instance: int, output_plot_filename: str
        data_plot.close()


-def detect_airtime(control_mode, status_time):
+def detect_airtime(estimator_status_flags, status_flags_time):
    # define flags for starting and finishing in air
    b_starts_in_air = False
    b_finishes_in_air = False
    # calculate in-air transition time
-    if (np.amin(control_mode['airborne']) < 0.5) and (np.amax(control_mode['airborne']) > 0.5):
-        in_air_transtion_time_arg = np.argmax(np.diff(control_mode['airborne']))
-        in_air_transition_time = status_time[in_air_transtion_time_arg]
-    elif (np.amax(control_mode['airborne']) > 0.5):
-        in_air_transition_time = np.amin(status_time)
+    if (np.amin(estimator_status_flags['cs_in_air']) < 0.5) and (np.amax(estimator_status_flags['cs_in_air']) > 0.5):
+        in_air_transtion_time_arg = np.argmax(np.diff(estimator_status_flags['cs_in_air']))
+        in_air_transition_time = status_flags_time[in_air_transtion_time_arg]
+    elif (np.amax(estimator_status_flags['cs_in_air']) > 0.5):
+        in_air_transition_time = np.amin(status_flags_time)
        print('log starts while in-air at ' + str(round(in_air_transition_time, 1)) + ' sec')
        b_starts_in_air = True
    else:
        in_air_transition_time = float('NaN')
        print('always on ground')
    # calculate on-ground transition time
-    if (np.amin(np.diff(control_mode['airborne'])) < 0.0):
-        on_ground_transition_time_arg = np.argmin(np.diff(control_mode['airborne']))
-        on_ground_transition_time = status_time[on_ground_transition_time_arg]
-    elif (np.amax(control_mode['airborne']) > 0.5):
-        on_ground_transition_time = np.amax(status_time)
+    if (np.amin(np.diff(estimator_status_flags['cs_in_air'])) < 0.0):
+        on_ground_transition_time_arg = np.argmin(np.diff(estimator_status_flags['cs_in_air']))
+        on_ground_transition_time = status_flags_time[on_ground_transition_time_arg]
+    elif (np.amax(estimator_status_flags['cs_in_air']) > 0.5):
+        on_ground_transition_time = np.amax(status_flags_time)
        print('log finishes while in-air at ' + str(round(on_ground_transition_time, 1)) + ' sec')
        b_finishes_in_air = True
    else:
        on_ground_transition_time = float('NaN')
        print('always on ground')
-    if (np.amax(np.diff(control_mode['airborne'])) > 0.5) and (np.amin(np.diff(control_mode['airborne'])) < -0.5):
+    if (np.amax(np.diff(estimator_status_flags['cs_in_air'])) > 0.5) and (np.amin(np.diff(estimator_status_flags['cs_in_air'])) < -0.5):
        if ((on_ground_transition_time - in_air_transition_time) > 0.0):
            in_air_duration = on_ground_transition_time - in_air_transition_time
        else:
@@ -16,6 +16,8 @@ set -e
 INSTALL_NUTTX="true"
 INSTALL_SIM="true"
 INSTALL_ARCH=`uname -m`
+INSTALL_UDDS="false"
+INSTALL_SIM_JAMMY="false"

 # Parse arguments
 for arg in "$@"
@@ -28,6 +30,14 @@ do
 		INSTALL_SIM="false"
 	fi

+	if [[ $arg == "--microdds" ]]; then
+		INSTALL_UDDS="true"
+	fi
+
+	if [[ $arg == "--sim_jammy" ]]; then
+		INSTALL_SIM_JAMMY="true"
+	fi
+
 done

 # detect if running in docker
@@ -67,6 +77,10 @@ elif [[ "${UBUNTU_RELEASE}" == "18.04" ]]; then
 	echo "Ubuntu 18.04"
 elif [[ "${UBUNTU_RELEASE}" == "20.04" ]]; then
 	echo "Ubuntu 20.04"
+elif [[ "${UBUNTU_RELEASE}" == "22.04" ]]; then
+	echo "Ubuntu 22.04, simulation build off by default." 
+	echo "Use --sim_jammy to enable simulation build."
+	INSTALL_SIM=$INSTALL_SIM_JAMMY
 fi


@@ -146,7 +160,7 @@ if [[ $INSTALL_NUTTX == "true" ]]; then
 		util-linux \
 		vim-common \
 		;
-	if [[ "${UBUNTU_RELEASE}" == "20.04" ]]; then
+	if [[ "${UBUNTU_RELEASE}" == "20.04" || "${UBUNTU_RELEASE}" == "22.04" ]]; then
 		sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends install \
 		kconfig-frontends \
 		;
@@ -173,7 +187,7 @@ if [[ $INSTALL_NUTTX == "true" ]]; then

 	else
 		echo "Installing arm-none-eabi-gcc-${NUTTX_GCC_VERSION}";
-		wget -O /tmp/gcc-arm-none-eabi-${NUTTX_GCC_VERSION}-linux.tar.bz2 https://armkeil.blob.core.windows.net/developer/Files/downloads/gnu-rm/${NUTTX_GCC_VERSION_SHORT}/gcc-arm-none-eabi-${NUTTX_GCC_VERSION}-${INSTALL_ARCH}-linux.tar.bz2 && \
+		wget -q --show-progress -O /tmp/gcc-arm-none-eabi-${NUTTX_GCC_VERSION}-linux.tar.bz2 https://armkeil.blob.core.windows.net/developer/Files/downloads/gnu-rm/${NUTTX_GCC_VERSION_SHORT}/gcc-arm-none-eabi-${NUTTX_GCC_VERSION}-${INSTALL_ARCH}-linux.tar.bz2 && \
 			sudo tar -jxf /tmp/gcc-arm-none-eabi-${NUTTX_GCC_VERSION}-linux.tar.bz2 -C /opt/;

 		# add arm-none-eabi-gcc to user's PATH
@@ -187,6 +201,25 @@ if [[ $INSTALL_NUTTX == "true" ]]; then
 	fi
 fi

+# Install microDDS tools (fastrtpsgen)
+if [[ $INSTALL_UDDS == "true" ]] && [ ! -f /usr/local/bin/fastrtpsgen ]; then
+	if command -v gradle &> /dev/null || [ ! -d /opt/gradle ]; then
+		echo "Installing gradle 6.3"
+		wget -q --show-progress -O /tmp/gradle-6.3-bin.zip https://downloads.gradle-dn.com/distributions/gradle-6.3-bin.zip
+		sudo mkdir -p /opt/gradle
+		sudo unzip -d /opt/gradle /tmp/gradle-6.3-bin.zip
+		export PATH=$PATH:/opt/gradle/gradle-6.3/bin
+	fi
+	echo "Installing fastrtpsgen"
+	git clone --recursive https://github.com/eProsima/Fast-DDS-Gen.git -b v1.0.4 /tmp/Fast-DDS-Gen
+	cd /tmp/Fast-DDS-Gen
+	gradle assemble
+	sudo env "PATH=$PATH" gradle install
+elif [[ $INSTALL_UDDS == "true" ]]; then
+	echo "fastrtpsgen already installed."
+fi
+
+
 # Simulation tools
 if [[ $INSTALL_SIM == "true" ]]; then

@@ -231,7 +264,7 @@ if [[ $INSTALL_SIM == "true" ]]; then
 	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 -
+	wget -q --show-progress 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 \
@@ -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
@@ -397,27 +397,15 @@
 #define GPIO_I2C1_SCL GPIO_I2C1_SCL_2       /* PB8]  */
 #define GPIO_I2C1_SDA GPIO_I2C1_SDA_2       /* PB9  */

-#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_PIN9)
-
 #define GPIO_I2C2_SCL GPIO_I2C2_SCL_2       /* PF1 */
 #define GPIO_I2C2_SDA GPIO_I2C2_SDA_2       /* PF0 */

-#define GPIO_I2C2_SCL_GPIO                  (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTF | GPIO_PIN1)
-#define GPIO_I2C2_SDA_GPIO                  (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTF | GPIO_PIN0)
-
 #define GPIO_I2C3_SCL GPIO_I2C3_SCL_2       /* PH7 */
 #define GPIO_I2C3_SDA GPIO_I2C3_SDA_2       /* PH8 */

-#define GPIO_I2C3_SCL_GPIO                  (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTH | GPIO_PIN7)
-#define GPIO_I2C3_SDA_GPIO                  (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTH | GPIO_PIN8)
-
 #define GPIO_I2C4_SCL GPIO_I2C4_SCL_2       /* PF14 */
 #define GPIO_I2C4_SDA GPIO_I2C4_SDA_2       /* PF15 */

-#define GPIO_I2C4_SCL_GPIO                  (GPIO_OUTPUT | GPIO_OPENDRAIN | GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTF | GPIO_PIN14)
-#define GPIO_I2C4_SDA_GPIO                  (GPIO_OUTPUT | GPIO_OPENDRAIN | GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTF | GPIO_PIN15)
-
 /* SDMMC1
 *
 *      VDD 3.3
@@ -459,7 +459,15 @@ static inline bool board_get_external_lockout_state(void)
 		GPIO_RSSI_IN_INIT,                \
 		GPIO_nSAFETY_SWITCH_LED_OUT_INIT, \
 		GPIO_SAFETY_SWITCH_IN,            \
-		GPIO_nARMED_INIT                  \
+		GPIO_nARMED_INIT,                  \
+		PX4_MAKE_GPIO_OUTPUT_CLEAR(GPIO_I2C1_SCL), \
+		PX4_MAKE_GPIO_OUTPUT_CLEAR(GPIO_I2C1_SDA), \
+		PX4_MAKE_GPIO_OUTPUT_CLEAR(GPIO_I2C2_SCL), \
+		PX4_MAKE_GPIO_OUTPUT_CLEAR(GPIO_I2C2_SDA), \
+		PX4_MAKE_GPIO_OUTPUT_CLEAR(GPIO_I2C3_SCL), \
+		PX4_MAKE_GPIO_OUTPUT_CLEAR(GPIO_I2C3_SDA), \
+		PX4_MAKE_GPIO_OUTPUT_CLEAR(GPIO_I2C4_SCL), \
+		PX4_MAKE_GPIO_OUTPUT_CLEAR(GPIO_I2C4_SDA), \
 	}

 #define BOARD_ENABLE_CONSOLE_BUFFER
@@ -33,7 +33,6 @@ CONFIG_DRIVERS_PCA9685=y
 CONFIG_DRIVERS_PCA9685_PWM_OUT=y
 CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_POWER_MONITOR_INA228=y
-CONFIG_DRIVERS_POWER_MONITOR_INA231=y
 CONFIG_DRIVERS_POWER_MONITOR_INA238=y
 CONFIG_DRIVERS_PWM_INPUT=y
 CONFIG_DRIVERS_PWM_OUT=y
@@ -394,27 +394,15 @@
 #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_2       /* PF1 */
 #define GPIO_I2C2_SDA GPIO_I2C2_SDA_2       /* PF0 */

-#define GPIO_I2C2_SCL_GPIO                  (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTF | GPIO_PIN1)
-#define GPIO_I2C2_SDA_GPIO                  (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTF | GPIO_PIN0)
-
 #define GPIO_I2C3_SCL GPIO_I2C3_SCL_2       /* PH7 */
 #define GPIO_I2C3_SDA GPIO_I2C3_SDA_2       /* PH8 */

-#define GPIO_I2C3_SCL_GPIO                  (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTH | GPIO_PIN7)
-#define GPIO_I2C3_SDA_GPIO                  (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTH | GPIO_PIN8)
-
 #define GPIO_I2C4_SCL GPIO_I2C4_SCL_2       /* PF14 */
 #define GPIO_I2C4_SDA GPIO_I2C4_SDA_2       /* PF15 */

-#define GPIO_I2C4_SCL_GPIO                  (GPIO_OUTPUT | GPIO_OPENDRAIN | GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTF | GPIO_PIN14)
-#define GPIO_I2C4_SDA_GPIO                  (GPIO_OUTPUT | GPIO_OPENDRAIN | GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTF | GPIO_PIN15)
-
 /* SDMMC2
 *
 *      VDD 3.3
@@ -423,7 +423,15 @@
 		GPIO_nSAFETY_SWITCH_LED_OUT_INIT, \
 		GPIO_SAFETY_SWITCH_IN,            \
 		GPIO_PG6,                         \
-		GPIO_nARMED_INIT                  \
+		GPIO_nARMED_INIT,                  \
+		PX4_MAKE_GPIO_OUTPUT_CLEAR(GPIO_I2C1_SCL), \
+		PX4_MAKE_GPIO_OUTPUT_CLEAR(GPIO_I2C1_SDA), \
+		PX4_MAKE_GPIO_OUTPUT_CLEAR(GPIO_I2C2_SCL), \
+		PX4_MAKE_GPIO_OUTPUT_CLEAR(GPIO_I2C2_SDA), \
+		PX4_MAKE_GPIO_OUTPUT_CLEAR(GPIO_I2C3_SCL), \
+		PX4_MAKE_GPIO_OUTPUT_CLEAR(GPIO_I2C3_SDA), \
+		PX4_MAKE_GPIO_OUTPUT_CLEAR(GPIO_I2C4_SCL), \
+		PX4_MAKE_GPIO_OUTPUT_CLEAR(GPIO_I2C4_SDA), \
 	}

 #define BOARD_ENABLE_CONSOLE_BUFFER
@@ -0,0 +1,3 @@
+CONFIG_BOARD_TOOLCHAIN="arm-none-eabi"
+CONFIG_BOARD_ARCHITECTURE="cortex-m7"
+CONFIG_BOARD_ROMFSROOT=""
@@ -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
@@ -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
+}
@@ -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
@@ -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
@@ -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.
@@ -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"
@@ -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  */
@@ -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 */
@@ -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
@@ -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) }
+}
@@ -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) }
+}
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+#   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
@@ -30,15 +30,46 @@
 # POSSIBILITY OF SUCH DAMAGE.
 #
 ############################################################################
-px4_add_module(
-	MODULE drivers__power_monitor__ina231
-	MAIN ina231
-	COMPILE_FLAGS
-		-Wno-cast-align # TODO: fix and enable
-	SRCS
-		ina231_main.cpp
-		ina231.cpp
-	DEPENDS
-		battery
-		px4_work_queue
+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()
@@ -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
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *   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
@@ -32,45 +32,31 @@
 ****************************************************************************/

 /**
- * Enable INA231 Power Monitor
+ * @file bootloader_main.c
 *
- * For systems a INA231 Power Monitor, this should be set to true
- *
- * @group Sensors
- * @boolean
- * @reboot_required true
+ * FMU-specific early startup code for bootloader
 */
-PARAM_DEFINE_INT32(SENS_EN_INA231, 0);

-/**
- * INA231 Power Monitor Config
- *
- * @group Sensors
- * @min 0
- * @max 65535
- * @decimal 1
- * @increment 1
-*/
-PARAM_DEFINE_INT32(INA231_CONFIG, 18139);
+#include "board_config.h"
+#include "bl.h"

-/**
- * INA231 Power Monitor Max Current
- *
- * @group Sensors
- * @min 0.1
- * @max 200.0
- * @decimal 2
- * @increment 0.1
- */
-PARAM_DEFINE_FLOAT(INA231_CURRENT, 164.0f);
+#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>

-/**
- * INA231 Power Monitor Shunt
- *
- * @group Sensors
- * @min 0.000000001
- * @max 0.1
- * @decimal 10
- * @increment .000000001
- */
-PARAM_DEFINE_FLOAT(INA231_SHUNT, 0.0005f);
+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();
+}
@@ -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 */
@@ -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_ */
@@ -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),
+};
@@ -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;
+}
@@ -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 */
@@ -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;
+}
@@ -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;
+}
@@ -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 */
@@ -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);
@@ -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);
@@ -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);
+}
@@ -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
@@ -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
@@ -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
 };

 /************************************************************************************
@@ -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);
@@ -77,6 +77,9 @@ set(msg_files
 	estimator_states.msg
 	estimator_status.msg
 	estimator_status_flags.msg
+	estimator_aid_source_1d.msg
+	estimator_aid_source_2d.msg
+	estimator_aid_source_3d.msg
 	event.msg
 	follow_target.msg
 	failure_detector_status.msg
@@ -137,7 +140,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
@@ -0,0 +1,22 @@
+uint64 timestamp             # time since system start (microseconds)
+uint64 timestamp_sample      # the timestamp of the raw data (microseconds)
+
+uint8 estimator_instance
+
+uint32 device_id
+
+uint64 time_last_fuse
+
+float32 observation
+float32 observation_variance
+
+float32 innovation
+float32 innovation_variance
+float32 test_ratio
+
+bool fusion_enabled          # true when measurements are being fused
+bool innovation_rejected     # true if the observation has been rejected
+bool fused                   # true if the sample was successfully fused
+
+# TOPICS estimator_aid_source_1d
+# TOPICS estimator_aid_src_baro_hgt estimator_aid_src_rng_hgt
@@ -0,0 +1,22 @@
+uint64 timestamp             # time since system start (microseconds)
+uint64 timestamp_sample      # the timestamp of the raw data (microseconds)
+
+uint8 estimator_instance
+
+uint32 device_id
+
+uint64[2] time_last_fuse
+
+float32[2] observation
+float32[2] observation_variance
+
+float32[2] innovation
+float32[2] innovation_variance
+float32[2] test_ratio
+
+bool[2] fusion_enabled       # true when measurements are being fused
+bool[2] innovation_rejected  # true if the observation has been rejected
+bool[2] fused                # true if the sample was successfully fused
+
+# TOPICS estimator_aid_source_2d
+# TOPICS estimator_aid_src_fake_pos
@@ -0,0 +1,22 @@
+uint64 timestamp             # time since system start (microseconds)
+uint64 timestamp_sample      # the timestamp of the raw data (microseconds)
+
+uint8 estimator_instance
+
+uint32 device_id
+
+uint64[3] time_last_fuse
+
+float32[3] observation
+float32[3] observation_variance
+
+float32[3] innovation
+float32[3] innovation_variance
+float32[3] test_ratio
+
+bool[3] fusion_enabled       # true when measurements are being fused
+bool[3] innovation_rejected  # true if the observation has been rejected
+bool[3] fused                # true if the sample was successfully fused
+
+# TOPICS estimator_aid_source_3d
+# TOPICS estimator_aid_src_gnss_pos estimator_aid_src_gnss_vel
@@ -16,6 +16,10 @@ bool starting_vision_pos_fusion         #  9 - true when the filter starts using
 bool starting_vision_vel_fusion         # 10 - true when the filter starts using vision system velocity measurements to correct the state estimates
 bool starting_vision_yaw_fusion         # 11 - true when the filter starts using vision system yaw  measurements to correct the state estimates
 bool yaw_aligned_to_imu_gps             # 12 - true when the filter resets the yaw to an estimate derived from IMU and GPS data
+bool reset_hgt_to_baro                  # 13 - true when the vertical position state is reset to the baro measurement
+bool reset_hgt_to_gps                   # 14 - true when the vertical position state is reset to the gps measurement
+bool reset_hgt_to_rng                   # 15 - true when the vertical position state is reset to the rng measurement
+bool reset_hgt_to_ev                    # 16 - true when the vertical position state is reset to the ev measurement

 # warning events
 uint32 warning_event_changes            # number of warning event changes
@@ -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
@@ -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
@@ -116,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
@@ -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;
 	}

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

@@ -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;
 	}
@@ -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)
-
 /*
 *
 *
@@ -149,6 +149,9 @@
 #define DRV_DIST_DEVTYPE_SRF02        0x74
 #define DRV_DIST_DEVTYPE_TERARANGER   0x75
 #define DRV_DIST_DEVTYPE_VL53L0X      0x76
+#define DRV_POWER_DEVTYPE_INA226      0x77
+#define DRV_POWER_DEVTYPE_INA228      0x78
+#define DRV_POWER_DEVTYPE_VOXLPM      0x79

 #define DRV_LED_DEVTYPE_RGBLED        0x7a
 #define DRV_LED_DEVTYPE_RGBLED_NCP5623C 0x7b
@@ -185,6 +188,7 @@
 #define DRV_DIST_DEVTYPE_GY_US42       0x9C

 #define DRV_BAT_DEVTYPE_BATMON_SMBUS   0x9d
+#define DRV_POWER_DEVTYPE_INA238       0x9E
 #define DRV_GPIO_DEVTYPE_MCP23009      0x9F

 #define DRV_GPS_DEVTYPE_ASHTECH 0xA0
@@ -204,12 +208,6 @@

 #define DRV_HYGRO_DEVTYPE_SHT3X 0xB1

-#define DRV_POWER_DEVTYPE_INA226 0xC0
-#define DRV_POWER_DEVTYPE_INA228 0xC1
-#define DRV_POWER_DEVTYPE_INA231 0xC2
-#define DRV_POWER_DEVTYPE_INA238 0xC3
-#define DRV_POWER_DEVTYPE_VOXLPM 0xC4
-
 #define DRV_DEVTYPE_UNUSED		0xff

 #endif /* _DRV_SENSOR_H */
@@ -274,20 +274,22 @@ void ICM42688P::RunImpl()
 				}
 			}

-			// check configuration registers periodically or immediately following any failure
-			if (RegisterCheck(_register_bank0_cfg[_checked_register_bank0])
-			    && RegisterCheck(_register_bank1_cfg[_checked_register_bank1])
-			    && RegisterCheck(_register_bank2_cfg[_checked_register_bank2])
-			   ) {
-				_last_config_check_timestamp = now;
-				_checked_register_bank0 = (_checked_register_bank0 + 1) % size_register_bank0_cfg;
-				_checked_register_bank1 = (_checked_register_bank1 + 1) % size_register_bank1_cfg;
-				_checked_register_bank2 = (_checked_register_bank2 + 1) % size_register_bank2_cfg;
+			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_bank1_cfg[_checked_register_bank1])
+				    && RegisterCheck(_register_bank2_cfg[_checked_register_bank2])
+				   ) {
+					_last_config_check_timestamp = now;
+					_checked_register_bank0 = (_checked_register_bank0 + 1) % size_register_bank0_cfg;
+					_checked_register_bank1 = (_checked_register_bank1 + 1) % size_register_bank1_cfg;
+					_checked_register_bank2 = (_checked_register_bank2 + 1) % size_register_bank2_cfg;

-			} else {
-				// register check failed, force reset
-				perf_count(_bad_register_perf);
-				Reset();
+				} else {
+					// register check failed, force reset
+					perf_count(_bad_register_perf);
+					Reset();
+				}
 			}
 		}

@@ -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;
@@ -1,5 +0,0 @@
-menuconfig DRIVERS_POWER_MONITOR_INA231
-	bool "ina231"
-	default n
-	---help---
-		Enable support for ina231
@@ -1,322 +0,0 @@
-/****************************************************************************
- *
- *   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.
- *
- ****************************************************************************/
-
-#include "ina231.h"
-
-INA231::INA231(const I2CSPIDriverConfig &config, int battery_index) :
-	I2C(config),
-	ModuleParams(nullptr),
-	I2CSPIDriver(config),
-	_sample_perf(perf_alloc(PC_ELAPSED, "ina231_read")),
-	_comms_errors(perf_alloc(PC_COUNT, "ina231_com_err")),
-	_collection_errors(perf_alloc(PC_COUNT, "ina231_collection_err")),
-	_measure_errors(perf_alloc(PC_COUNT, "ina231_measurement_err")),
-	_battery(battery_index, this, INA231_SAMPLE_INTERVAL_US, battery_status_s::BATTERY_SOURCE_POWER_MODULE)
-{
-	float fvalue = MAX_CURRENT;
-	_max_current = fvalue;
-	param_t ph = param_find("INA231_CURRENT");
-
-	if (ph != PARAM_INVALID && param_get(ph, &fvalue) == PX4_OK) {
-		_max_current = fvalue;
-	}
-
-	fvalue = INA231_SHUNT;
-	_rshunt = fvalue;
-	ph = param_find("INA231_SHUNT");
-
-	if (ph != PARAM_INVALID && param_get(ph, &fvalue) == PX4_OK) {
-		_rshunt = fvalue;
-	}
-
-	ph = param_find("INA231_CONFIG");
-	int32_t value = INA231_CONFIG;
-	_config = (uint16_t)value;
-
-	if (ph != PARAM_INVALID && param_get(ph, &value) == PX4_OK) {
-		_config = (uint16_t)value;
-	}
-
-	_mode_triggered = ((_config & INA231_MODE_MASK) >> INA231_MODE_SHIFTS) <=
-			  ((INA231_MODE_SHUNT_BUS_TRIG & INA231_MODE_MASK) >>
-			   INA231_MODE_SHIFTS);
-
-	_current_lsb = _max_current / DN_MAX;
-	_power_lsb = 25 * _current_lsb;
-
-	// We need to publish immediately, to guarantee that the first instance of the driver publishes to uORB instance 0
-	_battery.setConnected(false);
-	_battery.updateVoltage(0.f);
-	_battery.updateCurrent(0.f);
-	_battery.updateAndPublishBatteryStatus(hrt_absolute_time());
-}
-
-INA231::~INA231()
-{
-	/* free perf counters */
-	perf_free(_sample_perf);
-	perf_free(_comms_errors);
-	perf_free(_collection_errors);
-	perf_free(_measure_errors);
-}
-
-int INA231::read(uint8_t address, int16_t &data)
-{
-	// read desired little-endian value via I2C
-	uint16_t received_bytes;
-	int ret = PX4_ERROR;
-
-	for (size_t i = 0; i < 3; i++) {
-		ret = transfer(&address, 1, (uint8_t *)&received_bytes, sizeof(received_bytes));
-
-		if (ret == PX4_OK) {
-			data = swap16(received_bytes);
-			break;
-
-		} else {
-			perf_count(_comms_errors);
-			PX4_DEBUG("i2c::transfer returned %d", ret);
-		}
-	}
-
-	return ret;
-}
-
-int INA231::write(uint8_t address, uint16_t value)
-{
-	uint8_t data[3] = {address, ((uint8_t)((value & 0xff00) >> 8)), (uint8_t)(value & 0xff)};
-	return transfer(data, sizeof(data), nullptr, 0);
-}
-
-int
-INA231::init()
-{
-	int ret = PX4_ERROR;
-
-	/* do I2C init (and probe) first */
-	if (I2C::init() != PX4_OK) {
-		return ret;
-	}
-
-	write(INA231_REG_CONFIGURATION, INA231_RST);
-
-	_cal = INA231_CONST / (_current_lsb * _rshunt);
-
-	if (write(INA231_REG_CALIBRATION, _cal) < 0) {
-		return -3;
-	}
-
-	// If we run in continuous mode then start it here
-
-	if (!_mode_triggered) {
-		ret = write(INA231_REG_CONFIGURATION, _config);
-
-	} else {
-		ret = PX4_OK;
-	}
-
-	start();
-	_sensor_ok = true;
-
-	_initialized = ret == PX4_OK;
-	return ret;
-}
-
-int
-INA231::force_init()
-{
-	int ret = init();
-
-	start();
-
-	return ret;
-}
-
-int
-INA231::probe()
-{
-	int16_t read1 = 0;
-
-	if (read(INA231_REG_CONFIGURATION, read1) != PX4_OK) {
-		return PX4_ERROR;
-	}
-
-	int16_t read2 = 0;
-
-	if (read(INA231_REG_CONFIGURATION, read2) != PX4_OK) {
-		return PX4_ERROR;
-	}
-
-	if (read1 != read2) {
-		return PX4_ERROR;
-	}
-
-	return PX4_OK;
-}
-
-int
-INA231::measure()
-{
-	int ret = PX4_OK;
-
-	if (_mode_triggered) {
-		ret = write(INA231_REG_CONFIGURATION, _config);
-
-		if (ret < 0) {
-			perf_count(_comms_errors);
-			PX4_DEBUG("i2c::transfer returned %d", ret);
-		}
-	}
-
-	return ret;
-}
-
-int
-INA231::collect()
-{
-	perf_begin(_sample_perf);
-
-	if (_parameter_update_sub.updated()) {
-		// Read from topic to clear updated flag
-		parameter_update_s parameter_update;
-		_parameter_update_sub.copy(&parameter_update);
-
-		updateParams();
-	}
-
-	// read from the sensor
-	// Note: If the power module is connected backwards, then the values of _power, _current, and _shunt will be negative but otherwise valid.
-	bool success{true};
-	success = success && (read(INA231_REG_BUSVOLTAGE, _bus_voltage) == PX4_OK);
-	// success = success && (read(INA231_REG_POWER, _power) == PX4_OK);
-	success = success && (read(INA231_REG_CURRENT, _current) == PX4_OK);
-	// success = success && (read(INA231_REG_SHUNTVOLTAGE, _shunt) == PX4_OK);
-
-	if (!success) {
-		PX4_DEBUG("error reading from sensor");
-		_bus_voltage = _power = _current = _shunt = 0;
-	}
-
-	_battery.setConnected(success);
-	_battery.updateVoltage(static_cast<float>(_bus_voltage * INA231_VSCALE));
-	_battery.updateCurrent(static_cast<float>(_current * _current_lsb));
-	_battery.updateAndPublishBatteryStatus(hrt_absolute_time());
-
-	perf_end(_sample_perf);
-
-	if (success) {
-		return PX4_OK;
-
-	} else {
-		return PX4_ERROR;
-	}
-}
-
-void
-INA231::start()
-{
-	ScheduleClear();
-
-	/* reset the report ring and state machine */
-	_collect_phase = false;
-
-	_measure_interval = INA231_CONVERSION_INTERVAL;
-
-	/* schedule a cycle to start things */
-	ScheduleDelayed(5);
-}
-
-void
-INA231::RunImpl()
-{
-	if (_initialized) {
-		if (_collect_phase) {
-			/* perform collection */
-			if (collect() != PX4_OK) {
-				perf_count(_collection_errors);
-				/* if error restart the measurement state machine */
-				start();
-				return;
-			}
-
-			/* next phase is measurement */
-			_collect_phase = !_mode_triggered;
-
-			if (_measure_interval > INA231_CONVERSION_INTERVAL) {
-				/* schedule a fresh cycle call when we are ready to measure again */
-				ScheduleDelayed(_measure_interval - INA231_CONVERSION_INTERVAL);
-				return;
-			}
-		}
-
-		/* Measurement  phase */
-
-		/* Perform measurement */
-		if (measure() != PX4_OK) {
-			perf_count(_measure_errors);
-		}
-
-		/* next phase is collection */
-		_collect_phase = true;
-
-		/* schedule a fresh cycle call when the measurement is done */
-		ScheduleDelayed(INA231_CONVERSION_INTERVAL);
-
-	} else {
-		_battery.setConnected(false);
-		_battery.updateVoltage(0.f);
-		_battery.updateCurrent(0.f);
-		_battery.updateAndPublishBatteryStatus(hrt_absolute_time());
-
-		if (init() != PX4_OK) {
-			ScheduleDelayed(INA231_INIT_RETRY_INTERVAL_US);
-		}
-	}
-}
-
-void
-INA231::print_status()
-{
-	I2CSPIDriverBase::print_status();
-
-	if (_initialized) {
-		perf_print_counter(_sample_perf);
-		perf_print_counter(_comms_errors);
-
-		printf("poll interval:  %u \n", _measure_interval);
-
-	} else {
-		PX4_INFO("Device not initialized. Retrying every %d ms until battery is plugged in.",
-			 INA231_INIT_RETRY_INTERVAL_US / 1000);
-	}
-}
@@ -1,216 +0,0 @@
-/****************************************************************************
- *
- *   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 ina231.h
- *
- */
-
-#pragma once
-
-#include <px4_platform_common/px4_config.h>
-#include <px4_platform_common/getopt.h>
-#include <drivers/device/i2c.h>
-#include <lib/perf/perf_counter.h>
-#include <battery/battery.h>
-#include <drivers/drv_hrt.h>
-#include <uORB/SubscriptionInterval.hpp>
-#include <uORB/topics/parameter_update.h>
-#include <px4_platform_common/i2c_spi_buses.h>
-
-using namespace time_literals;
-
-/* Configuration Constants */
-#define INA231_BASEADDR 	                    0x40 /* 7-bit address. 8-bit address is 0x44 */
-// If initialization is forced (with the -f flag on the command line), but it fails, the drive will try again to
-// connect to the INA231 every this many microseconds
-#define INA231_INIT_RETRY_INTERVAL_US			500000
-
-/* INA231 Registers addresses */
-#define INA231_REG_CONFIGURATION             (0x00)
-#define INA231_REG_SHUNTVOLTAGE              (0x01)
-#define INA231_REG_BUSVOLTAGE                (0x02)
-#define INA231_REG_POWER                     (0x03)
-#define INA231_REG_CURRENT                   (0x04)
-#define INA231_REG_CALIBRATION               (0x05)
-#define INA231_REG_MASKENABLE                (0x06)
-#define INA231_REG_ALERTLIMIT                (0x07)
-#define INA231_MFG_ID                        (0xfe)
-#define INA231_MFG_DIEID                     (0xff)
-
-/* INA231 Configuration Register */
-#define INA231_MODE_SHIFTS                   (0)
-#define INA231_MODE_MASK                     (7 << INA231_MODE_SHIFTS)
-#define INA231_MODE_SHUTDOWN                 (0 << INA231_MODE_SHIFTS)
-#define INA231_MODE_SHUNT_TRIG               (1 << INA231_MODE_SHIFTS)
-#define INA231_MODE_BUS_TRIG                 (2 << INA231_MODE_SHIFTS)
-#define INA231_MODE_SHUNT_BUS_TRIG           (3 << INA231_MODE_SHIFTS)
-#define INA231_MODE_ADC_OFF                  (4 << INA231_MODE_SHIFTS)
-#define INA231_MODE_SHUNT_CONT               (5 << INA231_MODE_SHIFTS)
-#define INA231_MODE_BUS_CONT                 (6 << INA231_MODE_SHIFTS)
-#define INA231_MODE_SHUNT_BUS_CONT           (7 << INA231_MODE_SHIFTS)
-
-#define INA231_VSHCT_SHIFTS                  (3)
-#define INA231_VSHCT_MASK                    (7 << INA231_VSHCT_SHIFTS)
-#define INA231_VSHCT_140US                   (0 << INA231_VSHCT_SHIFTS)
-#define INA231_VSHCT_204US                   (1 << INA231_VSHCT_SHIFTS)
-#define INA231_VSHCT_332US                   (2 << INA231_VSHCT_SHIFTS)
-#define INA231_VSHCT_588US                   (3 << INA231_VSHCT_SHIFTS)
-#define INA231_VSHCT_1100US                  (4 << INA231_VSHCT_SHIFTS)
-#define INA231_VSHCT_2116US                  (5 << INA231_VSHCT_SHIFTS)
-#define INA231_VSHCT_4156US                  (6 << INA231_VSHCT_SHIFTS)
-#define INA231_VSHCT_8244US                  (7 << INA231_VSHCT_SHIFTS)
-
-#define INA231_VBUSCT_SHIFTS                 (6)
-#define INA231_VBUSCT_MASK                   (7 << INA231_VBUSCT_SHIFTS)
-#define INA231_VBUSCT_140US                  (0 << INA231_VBUSCT_SHIFTS)
-#define INA231_VBUSCT_204US                  (1 << INA231_VBUSCT_SHIFTS)
-#define INA231_VBUSCT_332US                  (2 << INA231_VBUSCT_SHIFTS)
-#define INA231_VBUSCT_588US                  (3 << INA231_VBUSCT_SHIFTS)
-#define INA231_VBUSCT_1100US                 (4 << INA231_VBUSCT_SHIFTS)
-#define INA231_VBUSCT_2116US                 (5 << INA231_VBUSCT_SHIFTS)
-#define INA231_VBUSCT_4156US                 (6 << INA231_VBUSCT_SHIFTS)
-#define INA231_VBUSCT_8244US                 (7 << INA231_VBUSCT_SHIFTS)
-
-#define INA231_AVERAGES_SHIFTS                (9)
-#define INA231_AVERAGES_MASK                  (7 << INA231_AVERAGES_SHIFTS)
-#define INA231_AVERAGES_1                     (0 << INA231_AVERAGES_SHIFTS)
-#define INA231_AVERAGES_4                     (1 << INA231_AVERAGES_SHIFTS)
-#define INA231_AVERAGES_16                    (2 << INA231_AVERAGES_SHIFTS)
-#define INA231_AVERAGES_64                    (3 << INA231_AVERAGES_SHIFTS)
-#define INA231_AVERAGES_128                   (4 << INA231_AVERAGES_SHIFTS)
-#define INA231_AVERAGES_256                   (5 << INA231_AVERAGES_SHIFTS)
-#define INA231_AVERAGES_512                   (6 << INA231_AVERAGES_SHIFTS)
-#define INA231_AVERAGES_1024                  (7 << INA231_AVERAGES_SHIFTS)
-
-#define INA231_CONFIG (INA231_MODE_SHUNT_BUS_CONT | INA231_VSHCT_588US | INA231_VBUSCT_588US | INA231_AVERAGES_64)
-
-#define INA231_RST                            (1 << 15)
-
-/* INA231 Enable / Mask Register */
-
-#define INA231_LEN                           (1 << 0)
-#define INA231_APOL                          (1 << 1)
-#define INA231_OVF                           (1 << 2)
-#define INA231_CVRF                          (1 << 3)
-#define INA231_AFF                           (1 << 4)
-
-#define INA231_CNVR                          (1 << 10)
-#define INA231_POL                           (1 << 11)
-#define INA231_BUL                           (1 << 12)
-#define INA231_BOL                           (1 << 13)
-#define INA231_SUL                           (1 << 14)
-#define INA231_SOL                           (1 << 15)
-
-#define INA231_SAMPLE_FREQUENCY_HZ            10
-#define INA231_SAMPLE_INTERVAL_US             (1_s / INA231_SAMPLE_FREQUENCY_HZ)
-#define INA231_CONVERSION_INTERVAL            (INA231_SAMPLE_INTERVAL_US - 7)
-#define MAX_CURRENT                           164.0f    /* 164 Amps */
-#define DN_MAX                                32768.0f  /* 2^15 */
-#define INA231_CONST                          0.00512f  /* is an internal fixed value used to ensure scaling is maintained properly  */
-#define INA231_SHUNT                          0.0005f   /* Shunt is 500 uOhm */
-#define INA231_VSCALE                         0.00125f  /* LSB of voltage is 1.25 mV  */
-
-#define swap16(w)                       __builtin_bswap16((w))
-
-class INA231 : public device::I2C, public ModuleParams, public I2CSPIDriver<INA231>
-{
-public:
-	INA231(const I2CSPIDriverConfig &config, int battery_index);
-	virtual ~INA231();
-
-	static I2CSPIDriverBase *instantiate(const I2CSPIDriverConfig &config, int runtime_instance);
-	static void print_usage();
-
-	void	RunImpl();
-
-	int 		  init() override;
-
-	/**
-	 * Tries to call the init() function. If it fails, then it will schedule to retry again in
-	 * INA231_INIT_RETRY_INTERVAL_US microseconds. It will keep retrying at this interval until initialization succeeds.
-	 *
-	 * @return PX4_OK if initialization succeeded on the first try. Negative value otherwise.
-	 */
-	int force_init();
-
-	/**
-	* Diagnostics - print some basic information about the driver.
-	*/
-	void				      print_status() override;
-
-protected:
-	int	  		probe() override;
-
-private:
-	bool			        _sensor_ok{false};
-	unsigned                        _measure_interval{0};
-	bool			        _collect_phase{false};
-	bool 					_initialized{false};
-
-	perf_counter_t		_sample_perf;
-	perf_counter_t		_comms_errors;
-	perf_counter_t 		_collection_errors;
-	perf_counter_t 		_measure_errors;
-
-	int16_t           _bus_voltage{0};
-	int16_t           _power{0};
-	int16_t           _current{0};
-	int16_t           _shunt{0};
-	int16_t           _cal{0};
-	bool              _mode_triggered{false};
-
-	float             _max_current{MAX_CURRENT};
-	float             _rshunt{INA231_SHUNT};
-	uint16_t          _config{INA231_CONFIG};
-	float             _current_lsb{_max_current / DN_MAX};
-	float             _power_lsb{25.0f * _current_lsb};
-
-	Battery 		  _battery;
-	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
-
-	int read(uint8_t address, int16_t &data);
-	int write(uint8_t address, uint16_t data);
-
-	/**
-	* Initialise the automatic measurement state machine and start it.
-	*
-	* @note This function is called at open and error time.  It might make sense
-	*       to make it more aggressive about resetting the bus in case of errors.
-	*/
-	void				      start();
-
-	int					     measure();
-	int					     collect();
-
-};
@@ -1,131 +0,0 @@
-/****************************************************************************
- *
- *   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.
- *
- ****************************************************************************/
-
-#include <px4_platform_common/getopt.h>
-#include <px4_platform_common/module.h>
-
-#include "ina231.h"
-
-I2CSPIDriverBase *INA231::instantiate(const I2CSPIDriverConfig &config, int runtime_instance)
-{
-	INA231 *instance = new INA231(config, config.custom1);
-
-	if (instance == nullptr) {
-		PX4_ERR("alloc failed");
-		return nullptr;
-	}
-
-	if (config.keep_running) {
-		if (instance->force_init() != PX4_OK) {
-			PX4_INFO("Failed to init INA231 on bus %d, but will try again periodically.", config.bus);
-		}
-
-	} else if (instance->init() != PX4_OK) {
-		delete instance;
-		return nullptr;
-	}
-
-	return instance;
-}
-
-void
-INA231::print_usage()
-{
-	PRINT_MODULE_DESCRIPTION(
-		R"DESCR_STR(
-### Description
-Driver for the INA231 power monitor.
-
-Multiple instances of this driver can run simultaneously, if each instance has a separate bus OR I2C address.
-
-For example, one instance can run on Bus 2, address 0x44, and one can run on Bus 2, address 0x45.
-
-If the INA231 module is not powered, then by default, initialization of the driver will fail. To change this, use
-the -f flag. If this flag is set, then if initialization fails, the driver will keep trying to initialize again
-every 0.5 seconds. With this flag set, you can plug in a battery after the driver starts, and it will work. Without
-this flag set, the battery must be plugged in before starting the driver.
-
-)DESCR_STR");
-
-	PRINT_MODULE_USAGE_NAME("ina231", "driver");
-
-	PRINT_MODULE_USAGE_COMMAND("start");
-	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
-	PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x40);
-	PRINT_MODULE_USAGE_PARAMS_I2C_KEEP_RUNNING_FLAG();
-	PRINT_MODULE_USAGE_PARAM_INT('t', 1, 1, 2, "battery index for calibration values (1 or 2)", true);
-	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
-}
-
-extern "C" int
-ina231_main(int argc, char *argv[])
-{
-	int ch;
-	using ThisDriver = INA231;
-	BusCLIArguments cli{true, false};
-	cli.i2c_address = INA231_BASEADDR;
-	cli.default_i2c_frequency = 100000;
-	cli.support_keep_running = true;
-	cli.custom1 = 1;
-
-	while ((ch = cli.getOpt(argc, argv, "t:")) != EOF) {
-		switch (ch) {
-		case 't': // battery index
-			cli.custom1 = (int)strtol(cli.optArg(), NULL, 0);
-			break;
-		}
-	}
-
-	const char *verb = cli.optArg();
-	if (!verb) {
-		ThisDriver::print_usage();
-		return -1;
-	}
-
-	BusInstanceIterator iterator(MODULE_NAME, cli, DRV_POWER_DEVTYPE_INA231);
-
-	if (!strcmp(verb, "start")) {
-		return ThisDriver::module_start(cli, iterator);
-	}
-
-	if (!strcmp(verb, "stop")) {
-		return ThisDriver::module_stop(iterator);
-	}
-
-	if (!strcmp(verb, "status")) {
-		return ThisDriver::module_status(iterator);
-	}
-
-	ThisDriver::print_usage();
-	return -1;
-}
@@ -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:
@@ -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");
@@ -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:
@@ -198,7 +198,7 @@ void Battery::estimateStateOfCharge(const float voltage_v, const float current_a
 	float cell_voltage = voltage_v / _params.n_cells;

 	// correct battery voltage locally for load drop to avoid estimation fluctuations
-	if (_params.r_internal >= 0.f) {
+	if (_params.r_internal >= 0.f && current_a > FLT_EPSILON) {
 		cell_voltage += _params.r_internal * current_a;

 	} else {
@@ -43,7 +43,7 @@
 * @group Battery Calibration
 * @category system
 */
-PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.5f);
+PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.6f);

 /**
 * This parameter is deprecated. Please use BAT1_V_CHARGED instead.
@@ -21,7 +21,7 @@ parameters:
            reboot_required: true
            num_instances: *max_num_config_instances
            instance_start: 1
-            default: [3.5, 3.5]
+            default: [3.6, 3.6]

        BAT${i}_V_CHARGED:
            description:
@@ -58,7 +58,7 @@ parameters:
            reboot_required: true
            num_instances: *max_num_config_instances
            instance_start: 1
-            default: [0.3, 0.3]
+            default: [0.1, 0.1]

        BAT${i}_R_INTERNAL:
            description:
@@ -71,12 +71,12 @@ parameters:
            unit: Ohm
            min: -1.0
            max: 0.2
-            decimal: 2
-            increment: 0.01
+            decimal: 4
+            increment: 0.0005
            reboot_required: true
            num_instances: *max_num_config_instances
            instance_start: 1
-            default: [-1.0, -1.0]
+            default: [0.005, 0.005]

        BAT${i}_N_CELLS:
            description:
@@ -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)"
                        }
@@ -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, },
 };
--- a/Show More
+++ b/Show More
Author	SHA1	Message	Date
Benjamin	eb9660d744	Install microDDS build requirements ubuntu.sh --microdds	2022-05-25 21:30:14 -04:00
Benjamin	5ff44a2295	Update ubuntu.sh to Jammy	2022-05-25 19:07:15 -04:00
Matthias Grob	c03f5b9481	Commander: fix overlooked rebase naming error	2022-05-25 21:10:19 +02:00
alessandro	29c4119e24	Match commander uORB var names to message names (#19707 ) * match vehicle_status * match home_position * match vehicle_command_ack * match actuator_armed * match vehicle_control_mode * match commander_state	2022-05-25 17:08:48 +02:00
Daniel Agar	98354ba10a	Update submodule GPSDrivers to latest Wed May 25 12:38:58 UTC 2022 - devices in PX4/Firmware (7cc84e0f9e44697475b0f355df9a29de6ea6b9ff): https://github.com/PX4/PX4-GPSDrivers/commit/58968922b718176be8756f11113d16b2cfbc4022 - devices current upstream: https://github.com/PX4/PX4-GPSDrivers/commit/181fae1a4b5e33576d786755782adb2f195ecc48 - Changes: https://github.com/PX4/PX4-GPSDrivers/compare/58968922b718176be8756f11113d16b2cfbc4022...181fae1a4b5e33576d786755782adb2f195ecc48 181fae1 2022-05-23 numan - README: add information for definition.h	2022-05-25 09:56:40 -04:00
Daniel Agar	58ea6235fe	ekf2: estimator aid source status (range height)	2022-05-25 09:25:12 -04:00
Daniel Agar	1e25aee6fa	ekf2: estimator aid source status (baro height)	2022-05-25 09:25:12 -04:00
Silvan Fuhrer	4dbe6f0a1c	simulator_mavlink: fix MAV_TYPE_VTOL_TAILSITTER case This type (23) doesn't specify a motor number, so it can't be properly handled. There are duo (19) and quad (20) tailsitter types that still work in simulation. Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2022-05-24 17:58:44 -04:00
Daniel Agar	376201e64d	boards: px4_fmu-v5/v5x init all I2C pins	2022-05-24 11:02:03 -04:00
bresch	d04a91a3ae	ekf2_post-processing: use estimator_status_flags instead of bitmasks	2022-05-24 16:15:42 +02:00
Beat Küng	016b8aeb35	MulticopterRateControl: fix thrust sign for acro Regression from `fbc109436f`	2022-05-24 15:03:17 +02:00
Matthias Grob	44be415e0e	mission_block: make "Delay until" item work while landed To enable landing, waiting and taking off again e.g. for delivery.	2022-05-24 13:42:03 +02:00
Matthias Grob	0be474430c	Commander: don't disarm on landing amid a mission	2022-05-24 13:42:03 +02:00
Matthias Grob	4f34207c4e	battery_params: increase default empty cellvoltage to 3.6v Based on feedback that very often the battery is used down too low. I observed this happens consistently when the cell voltage is properly load compensated. The default load compensation before #19429 was very inaccurate and resulted in unpredictable estimate. After that if there is a usable current measurement and the battery is within expected tolerances of the default internal resistance the compensation is pretty good and 3.5V is too low for an empty compensated cell voltage. That was seen in various logs where the compensated cell voltage was already dropping fast after 3.6V. In case the voltage is not load compensated the vehicle estimates the state of charge a bit too low which is safer than to high especially for a default configuration.	2022-05-24 11:26:58 +02:00
Mark Sauder	a1fb9fb7c6	rcS: Keep Mag Cal with AUTOCONFIG param reset in rcS, deprecate rcS AUTOCNF param (#19693 ) * Deprecate the rcS AUTOCNF script param and associated logic. * Update posix rcS to match previous commit.	2022-05-23 20:35:22 -04:00
Daniel Agar	639222dd65	ekf2: estimator aid source status (GPS pos, GPS vel)	2022-05-23 16:21:49 -04:00
Daniel Agar	1ae467e9cd	ekf2: estimator aid source status (starting with fake position)	2022-05-23 16:21:49 -04:00
Igor Misic	79a34b5aed	px4iofirmware: fix for bug introduced with #19558	2022-05-23 14:59:11 -04:00
Daniel Agar	d25a784a3a	icm42688p: only check configured registers periodically (as intended)	2022-05-23 14:58:23 -04:00
bresch	c57c575cfe	ekf2: use explicit flags instead of bitmask position This prevents bitmask mismatch when a new flag is inserted	2022-05-23 14:43:02 -04:00
Matthias Grob	62edcc7a57	battery: only compensate using sane current	2022-05-23 16:40:32 +02:00
Matthias Grob	2cbdcc9671	battery: default to reasonable internal resistance instead of disabling the load compensation using current.	2022-05-23 16:40:32 +02:00
Matthias Grob	58b1139a21	battery: allow setting precise internal resistance with parameter	2022-05-23 16:40:32 +02:00
Matthias Grob	2c5a7ea118	battery: rather undercompensate for load than over compensate which makes the estimate to high and breaks batteries beause they get flown for too long.	2022-05-23 16:40:32 +02:00
Serhat Aksun	bc220ddb82	sensors/vehicle_magnetometer: fix multi_mode check Signed-off-by: Serhat Aksun <serhat.aksun@maxwell-innovations.com>	2022-05-23 10:09:41 -04:00
Igor Misic	12a81979a8	px4io: replace safety_off state with safety button event (#19558 ) internal PX4IO safety_off state is removed and replaced with a normal safety button event. From this 'commit' commander is taking care of the PX4IO safety.	2022-05-23 06:54:37 +02:00
Igor Misic	08dcc72e1f	commander/safety: replace safety.msg with Safety class (#19558 )	2022-05-23 06:54:37 +02:00
Daniel Agar	b800600a6c	boards: px4_fmu-v2_fixeding disable fw_autotune_attitude_control to save flash	2022-05-22 12:32:24 -04:00
Daniel Agar	61d5a23445	Jenkinsfile: hardware test rack skip runing HITL airframes	2022-05-22 12:31:08 -04:00
Julian Oes	47bcee70c4	ROMFS: disable UAVCAN in HITL Without this, uavcan creates MixingOutput classes which then create empty actuator_outputs publications. This then prevents the motor output in HITL to be forwarded to the simulator via mavlink.	2022-05-21 17:53:20 -04:00
bresch	00e88a02c0	baro bias: improve innovation sequence monitoring The baro observation noise parameter is often over-estimated in order as a measure to mitigate temporary offsets in the readings due to wind gusts or poor pressure compensation tuning. The side effect is that the innovation sequence monitoring based on normalized innovation struggles to detect an offset in the state because the innovation isn't statistically significant. To counter this issue, a simpler check is added to trigger the process noise boost when the innovation has the same sign for a long period of time.	2022-05-21 22:37:04 +02:00
PX4 BuildBot	c3f10c2cb1	Update submodule sitl_gazebo to latest Sat May 21 12:38:49 UTC 2022 - sitl_gazebo in PX4/Firmware (`422be90140`): https://github.com/PX4/PX4-SITL_gazebo/commit/48440d7b5c78a21182415266334981f1163f4b2c - sitl_gazebo current upstream: https://github.com/PX4/PX4-SITL_gazebo/commit/5eb5df80450a412076bfc24e7dd343839020f056 - Changes: https://github.com/PX4/PX4-SITL_gazebo/compare/48440d7b5c78a21182415266334981f1163f4b2c...5eb5df80450a412076bfc24e7dd343839020f056 5eb5df8 2022-04-28 stmoon - ROS2_PLUGIN set OFF by default c8f4d5b 2022-04-28 stmoon - update the motor failure plugin for ros2	2022-05-21 14:58:42 -04:00
Daniel Agar	db4545e442	Update world_magnetic_model to latest Sat May 21 11:14:10 UTC 2022	2022-05-21 14:57:55 -04:00
mcsauder	6b0788ff46	Standardize variable naming and formatting across vehicle attitude controller files.	2022-05-21 12:22:18 -04:00
Junwoo Hwang	422be90140	Update Issue and Pull Request Templates 1. Change the paragraph headings to proper Markdown headings (easier to link / structure the Markdown) 2. Move PULL_REQUEST_TEMPLATE into .github folder 3. Change description in Issue template and remove outdated DevGuide Repository information 4. Add a bug emoji to bug report isue template 5. Support automatically adding labels 'bug-report' and 'feature-request' to easily sort / filter appropriate issues in Github	2022-05-20 21:01:14 -04:00
Julian Oes	d08e5eae2d	sitl_gazebo: update submodule This fixes the issue where HITL doesn't connect over USB.	2022-05-20 21:00:33 -04:00
Daniel Agar	c40631c136	commander: handle RC loss timeout if manual_control_setpoint stops publishing (#19680 ) Co-authored-by: Matthias Grob <maetugr@gmail.com>	2022-05-20 19:37:30 -04:00
David Sidrane	2bdeb36ea3	px4_fmu-v6x:Add Rev 4 Sensors	2022-05-20 15:52:32 -04:00
David Sidrane	9d0e57230a	boards: new px4_fmu-v6c board support (#19544 )	2022-05-20 15:49:06 -04:00