drivers/device/nuttx/I2C: add transfer failure details

This should fix the CI issue where the test just hangs trying to
connect.

.github/workflows/compile_nuttx.yml

@@ -20,6 +20,7 @@ jobs:
           ark_can-flow,
           ark_can-gps,
           ark_can-rtk-gps,
+          ark_cannode,
           atl_mantis-edu,
           av_x-v1,
           bitcraze_crazyflie,

.github/workflows/sitl_tests.yml

@@ -29,9 +29,9 @@ jobs:
         token: ${{ secrets.ACCESS_TOKEN }}
 
     - name: Download MAVSDK
-      run: wget "https://github.com/mavlink/MAVSDK/releases/download/v$(cat test/mavsdk_tests/MAVSDK_VERSION)/mavsdk_$(cat test/mavsdk_tests/MAVSDK_VERSION)_ubuntu20.04_amd64.deb"
+      run: wget "https://github.com/mavlink/MAVSDK/releases/download/v$(cat test/mavsdk_tests/MAVSDK_VERSION)/libmavsdk-dev_$(cat test/mavsdk_tests/MAVSDK_VERSION)_ubuntu20.04_amd64.deb"
     - name: Install MAVSDK
-      run: dpkg -i "mavsdk_$(cat test/mavsdk_tests/MAVSDK_VERSION)_ubuntu20.04_amd64.deb"
+      run: dpkg -i "libmavsdk-dev_$(cat test/mavsdk_tests/MAVSDK_VERSION)_ubuntu20.04_amd64.deb"
 
     - name: Prepare ccache timestamp
       id: ccache_cache_timestamp

.vscode/cmake-variants.yaml

@@ -121,6 +121,16 @@ CONFIG:
       buildType: MinSizeRel
       settings:
         CONFIG: ark_can-rtk-gps_canbootloader
+    ark_cannode_default:
+      short: ark_cannode_default
+      buildType: MinSizeRel
+      settings:
+        CONFIG: ark_cannode_default
+    ark_cannode_canbootloader:
+      short: ark_cannode_canbootloader
+      buildType: MinSizeRel
+      settings:
+        CONFIG: ark_cannode_canbootloader
     atl_mantis-edu_default:
       short: atl_mantis-edu
       buildType: MinSizeRel

ROMFS/px4fmu_common/init.d-posix/airframes/1040_standard_vtol

@@ -71,7 +71,6 @@ param set-default MPC_XY_VEL_I_ACC 4
 param set-default MPC_XY_VEL_D_ACC 0.1
 
 param set-default NAV_ACC_RAD 5
-param set-default NAV_LOITER_RAD 80
 
 param set-default VT_FWD_THRUST_EN 4
 param set-default VT_F_TRANS_THR 0.75

ROMFS/px4fmu_common/init.d-posix/airframes/1041_tailsitter

@@ -70,7 +70,6 @@ param set-default MPC_XY_VEL_I_ACC 4
 param set-default MPC_XY_VEL_D_ACC 0.1
 
 param set-default NAV_ACC_RAD 5
-param set-default NAV_LOITER_RAD 80
 
 param set-default VT_FW_DIFTHR_EN 1
 param set-default VT_FW_DIFTHR_SC 0.5

ROMFS/px4fmu_common/init.d-posix/airframes/1042_tiltrotor

@@ -82,7 +82,6 @@ param set-default MPC_XY_VEL_I_ACC 4
 param set-default MPC_XY_VEL_D_ACC 0.1
 
 param set-default NAV_ACC_RAD 5
-param set-default NAV_LOITER_RAD 80
 
 param set-default VT_B_TRANS_DUR 8
 param set-default VT_FWD_THRUST_EN 4

ROMFS/px4fmu_common/init.d-posix/airframes/1043_standard_vtol_drop

@@ -38,7 +38,6 @@ param set-default MPC_XY_VEL_I_ACC 4
 param set-default MPC_XY_VEL_D_ACC 0.1
 
 param set-default NAV_ACC_RAD 5
-param set-default NAV_LOITER_RAD 80
 
 param set-default VT_FWD_THRUST_EN 4
 param set-default VT_F_TRANS_THR 0.75

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

@@ -49,7 +49,6 @@ param set-default MPC_Z_VEL_MAX_DN 1.5
 
 param set-default NAV_ACC_RAD 5
 param set-default NAV_DLL_ACT 2
-param set-default NAV_LOITER_RAD 80
 
 param set-default RTL_DESCEND_ALT 10
 param set-default RTL_RETURN_ALT 30

ROMFS/px4fmu_common/init.d/rc.sensors

@@ -83,16 +83,28 @@ then
 	teraranger start -X
 fi
 
-# Possible external pmw3901 optical flow sensor
+# paa3905 optical flow sensor (external SPI)
+if param greater -s SENS_EN_PAA3905 0
+then
+	paa3905 -S start
+fi
+
+# paw3902 optical flow sensor (external SPI)
+if param greater -s SENS_EN_PAW3902 0
+then
+	paw3902 -S start
+fi
+
+# pmw3901 optical flow sensor (external SPI)
 if param greater -s SENS_EN_PMW3901 0
 then
 	pmw3901 -S start
 fi
 
-# Possible external paw3902 optical flow sensor
-if param greater -s SENS_EN_PAW3902 0
+# Check for px4flow sensor
+if param compare -s SENS_EN_PX4FLOW 1
 then
-	paw3902 -S start
+	px4flow start -X
 fi
 
 # vl53l1x i2c distance sensor

ROMFS/px4fmu_common/init.d/rcS

@@ -492,12 +492,6 @@ else
 		gimbal start
 	fi
 
-	# Check for flow sensor
-	if param compare -s SENS_EN_PX4FLOW 1
-	then
-		px4flow start -X
-	fi
-
 	# Blacksheep telemetry
 	if param compare -s TEL_BST_EN 1
 	then

Tools/ecl_ekf/analysis/checks.py

@@ -55,7 +55,7 @@ def perform_imu_checks(
 
     # perform the vibration check
     imu_status['imu_vibration_check'] = 'Pass'
-    for imu_vibr_metric in ['imu_coning', 'imu_hfdang', 'imu_hfdvel']:
+    for imu_vibr_metric in ['imu_coning', 'imu_hfgyro', 'imu_hfaccel']:
         mean_metric = '{:s}_mean'.format(imu_vibr_metric)
         peak_metric = '{:s}_peak'.format(imu_vibr_metric)
         if imu_metrics[mean_metric] > check_levels['{:s}_warn'.format(mean_metric)] \

Tools/ecl_ekf/analysis/metrics.py

@@ -144,17 +144,27 @@ def calculate_imu_metrics(ulog: ULog, multi_instance, in_air_no_ground_effects:
         imu_metrics[result] = calculate_stat_from_signal(
             estimator_status_data, 'estimator_status', signal, in_air_no_ground_effects, np.median)
 
+
     # calculates peak and mean for IMU vibration checks
-    for signal, result in [('vibe[0]', 'imu_coning'),
-                           ('vibe[1]', 'imu_hfdang'),
-                           ('vibe[2]', 'imu_hfdvel')]:
-        peak = calculate_stat_from_signal(
-            estimator_status_data, 'estimator_status', signal, in_air_no_ground_effects, np.amax)
-        if peak > 0.0:
-            imu_metrics['{:s}_peak'.format(result)] = peak
-            imu_metrics['{:s}_mean'.format(result)] = calculate_stat_from_signal(
-                estimator_status_data, 'estimator_status', signal,
-                in_air_no_ground_effects, np.mean)
+    for imu_status_instance in range(4):
+        try:
+            vehicle_imu_status_data = ulog.get_dataset('vehicle_imu_status', imu_status_instance).data
+
+            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'),
+                                       ('gyro_vibration_metric', 'imu_hfgyro'),
+                                       ('accel_vibration_metric', 'imu_hfaccel')]:
+
+                    peak = calculate_stat_from_signal(vehicle_imu_status_data, 'vehicle_imu_status', signal, in_air_no_ground_effects, np.amax)
+
+                    if peak > 0.0:
+                        imu_metrics['{:s}_peak'.format(result)] = peak
+                        imu_metrics['{:s}_mean'.format(result)] = calculate_stat_from_signal(vehicle_imu_status_data, 'vehicle_imu_status', signal, in_air_no_ground_effects, np.mean)
+
+        except:
+            pass
+
 
     # IMU bias checks
     estimator_states_data = ulog.get_dataset('estimator_states', multi_instance).data

Tools/ecl_ekf/batch_process_metadata_ekf.py

@@ -48,7 +48,7 @@ for filename in os.listdir(metadata_directory):
 
 #        # print out the check levels
 #        print('\n'+'The following metadata loaded from '+filename+' were used'+'\n')
-#        val = population_data.get(filename, {}).get('imu_hfdang_mean')
+#        val = population_data.get(filename, {}).get('imu_hfgyro_mean')
 #        print(val)
 
 # Open pdf file for plotting
@@ -90,10 +90,10 @@ population_results = {
 'ofy_fail_pct_avg':[float('NaN'),'The mean percentage of innovation test fails for the Y axis optical flow sensor'],
 'imu_coning_max_avg':[float('NaN'),'The mean of the maximum in-flight values of the IMU delta angle coning vibration level (mrad)'],
 'imu_coning_mean_avg':[float('NaN'),'The mean of the mean in-flight value of the IMU delta angle coning vibration level (mrad)'],
-'imu_hfdang_max_avg':[float('NaN'),'The mean of the maximum in-flight values of the IMU high frequency delta angle vibration level (mrad)'],
-'imu_hfdang_mean_avg':[float('NaN'),'The mean of the mean in-flight value of the IMU delta high frequency delta angle vibration level (mrad)'],
-'imu_hfdvel_max_avg':[float('NaN'),'The mean of the maximum in-flight values of the IMU high frequency delta velocity vibration level (m/s)'],
-'imu_hfdvel_mean_avg':[float('NaN'),'The mean of the mean in-flight value of the IMU delta high frequency delta velocity vibration level (m/s)'],
+'imu_hfgyro_max_avg':[float('NaN'),'The mean of the maximum in-flight values of the IMU high frequency gyro vibration level (rad/s)'],
+'imu_hfgyro_mean_avg':[float('NaN'),'The mean of the mean in-flight value of the IMU delta high frequency gyro vibration level (rad/s)'],
+'imu_hfaccel_max_avg':[float('NaN'),'The mean of the maximum in-flight values of the IMU high frequency accel vibration level (m/s/s)'],
+'imu_hfaccel_mean_avg':[float('NaN'),'The mean of the mean in-flight value of the IMU delta high frequency accel vibration level (m/s/s)'],
 'obs_ang_median_avg':[float('NaN'),'The mean of the median in-flight value of the output observer angular tracking error magnitude (mrad)'],
 'obs_vel_median_avg':[float('NaN'),'The mean of the median in-flight value of the output observer velocity tracking error magnitude (m/s)'],
 'obs_pos_median_avg':[float('NaN'),'The mean of the median in-flight value of the output observer position tracking error magnitude (m)'],
@@ -360,54 +360,54 @@ if (len(result1) > 0 and len(result2) > 0):
     plt.close(8)
 
 # IMU high frequency delta angle vibration levels
-temp = np.asarray([population_data[k].get('imu_hfdang_peak') for k in found_keys])
+temp = np.asarray([population_data[k].get('imu_hfgyro_peak') for k in found_keys])
 result1 = 1000.0 * temp[np.isfinite(temp)]
-temp = np.asarray([population_data[k].get('imu_hfdang_mean') for k in found_keys])
+temp = np.asarray([population_data[k].get('imu_hfgyro_mean') for k in found_keys])
 result2 = 1000.0 * temp[np.isfinite(temp)]
 
 if (len(result1) > 0 and len(result2) > 0):
-    population_results['imu_hfdang_max_avg'][0] = np.mean(result1)
-    population_results['imu_hfdang_mean_avg'][0] = np.mean(result2)
+    population_results['imu_hfgyro_max_avg'][0] = np.mean(result1)
+    population_results['imu_hfgyro_mean_avg'][0] = np.mean(result2)
 
     plt.figure(9,figsize=(20,13))
 
     plt.subplot(2,1,1)
     plt.hist(result1)
-    plt.title("Gaussian Histogram - IMU HF Delta Angle Vibration Peak")
-    plt.xlabel("imu_hfdang_max (mrad)")
+    plt.title("Gaussian Histogram - IMU HF Gyroscope Vibration Peak")
+    plt.xlabel("imu_hfgyro_max (rad/s)")
     plt.ylabel("Frequency")
 
     plt.subplot(2,1,2)
     plt.hist(result2)
-    plt.title("Gaussian Histogram - IMU HF Delta Angle Vibration Mean")
-    plt.xlabel("imu_hfdang_mean (mrad)")
+    plt.title("Gaussian Histogram - IMU HF Gyroscope Vibration Mean")
+    plt.xlabel("imu_hfgyro_mean (rad/s)")
     plt.ylabel("Frequency")
 
     pp.savefig()
     plt.close(9)
 
-# IMU high frequency delta velocity vibration levels
-temp = np.asarray([population_data[k].get('imu_hfdvel_peak') for k in found_keys])
+# IMU high frequency accel vibration levels
+temp = np.asarray([population_data[k].get('imu_hfaccel_peak') for k in found_keys])
 result1 = temp[np.isfinite(temp)]
-temp = np.asarray([population_data[k].get('imu_hfdvel_mean') for k in found_keys])
+temp = np.asarray([population_data[k].get('imu_hfaccel_mean') for k in found_keys])
 result2 = temp[np.isfinite(temp)]
 
 if (len(result1) > 0 and len(result2) > 0):
-    population_results['imu_hfdvel_max_avg'][0] = np.mean(result1)
-    population_results['imu_hfdvel_mean_avg'][0] = np.mean(result2)
+    population_results['imu_hfaccel_max_avg'][0] = np.mean(result1)
+    population_results['imu_hfaccel_mean_avg'][0] = np.mean(result2)
 
     plt.figure(10,figsize=(20,13))
 
     plt.subplot(2,1,1)
     plt.hist(result1)
-    plt.title("Gaussian Histogram - IMU HF Delta Velocity Vibration Peak")
-    plt.xlabel("imu_hfdvel_max (m/s)")
+    plt.title("Gaussian Histogram - IMU HF Accelerometer Vibration Peak")
+    plt.xlabel("imu_hfaccel_max (m/s/s)")
     plt.ylabel("Frequency")
 
     plt.subplot(2,1,2)
     plt.hist(result2)
-    plt.title("Gaussian Histogram - IMU HF Delta Velocity Vibration Mean")
-    plt.xlabel("imu_hfdvel_mean (m/s)")
+    plt.title("Gaussian Histogram - IMU HF Accelerometer Vibration Mean")
+    plt.xlabel("imu_hfaccel_mean (m/s/s)")
     plt.ylabel("Frequency")
 
     pp.savefig()
@@ -535,12 +535,12 @@ single_log_results = {
 'hgt_sensor_status':['Pass','Height sensor check summary. This sensor data can be sourced from either Baro, GPS, range fidner or external vision system. A Fail result indicates a significant error that caused a significant reduction in vehicle navigation performance was detected. A Warning result indicates that error levels higher than normal were detected but these errors did not significantly impact navigation performance. A Pass result indicates that no amonalies were detected and no further investigation is required'],
 'hgt_test_max':[float('NaN'),'The maximum in-flight value of the height sensor innovation consistency test ratio.'],
 'hgt_test_mean':[float('NaN'),'The mean in-flight value of the height sensor innovation consistency test ratio.'],
-'imu_coning_mean':[float('NaN'),'Mean in-flight value of the IMU delta angle coning vibration metric (rad)'],
-'imu_coning_peak':[float('NaN'),'Peak in-flight value of the IMU delta angle coning vibration metric (rad)'],
-'imu_hfdang_mean':[float('NaN'),'Mean in-flight value of the IMU delta angle high frequency vibration metric (rad)'],
-'imu_hfdang_peak':[float('NaN'),'Peak in-flight value of the IMU delta angle high frequency vibration metric (rad)'],
-'imu_hfdvel_mean':[float('NaN'),'Mean in-flight value of the IMU delta velocity high frequency vibration metric (m/s)'],
-'imu_hfdvel_peak':[float('NaN'),'Peak in-flight value of the IMU delta velocity high frequency vibration metric (m/s)'],
+'imu_coning_mean':[float('NaN'),'Mean in-flight value of the IMU delta angle coning vibration metric (rad^2)'],
+'imu_coning_peak':[float('NaN'),'Peak in-flight value of the IMU delta angle coning vibration metric (rad^2)'],
+'imu_hfgyro_mean':[float('NaN'),'Mean in-flight value of the IMU gyro high frequency vibration metric (rad/s)'],
+'imu_hfgyro_peak':[float('NaN'),'Peak in-flight value of the IMU gyro high frequency vibration metric (rad/s)'],
+'imu_hfaccel_mean':[float('NaN'),'Mean in-flight value of the IMU accel high frequency vibration metric (m/s/s)'],
+'imu_hfaccel_peak':[float('NaN'),'Peak in-flight value of the IMU accel high frequency vibration metric (m/s/s)'],
 'imu_sensor_status':['Pass','IMU sensor check summary. A Fail result indicates a significant error that caused a significant reduction in vehicle navigation performance was detected. A Warning result indicates that error levels higher than normal were detected but these errors did not significantly impact navigation performance. A Pass result indicates that no amonalies were detected and no further investigation is required'],
 'in_air_transition_time':[float('NaN'),'The time in seconds measured from startup that the EKF transtioned into in-air mode. Set to a nan if a transition event is not detected.'],
 'mag_percentage_amber':[float('NaN'),'The percentage of in-flight consolidated magnetic field sensor innovation consistency test values > 0.5.'],

Tools/ecl_ekf/check_level_dict.csv

@@ -21,10 +21,10 @@ hagl_amber_warn_pct,5.0
 tas_amber_warn_pct,5.0
 imu_coning_peak_warn,1.8E-5
 imu_coning_mean_warn,3.6E-6
-imu_hfdang_peak_warn,3.0E-3
-imu_hfdang_mean_warn,6.0E-4
-imu_hfdvel_peak_warn,9.0E-2
-imu_hfdvel_mean_warn,1.8E-2
+imu_hfgyro_peak_warn,12
+imu_hfgyro_mean_warn,2.4
+imu_hfaccel_peak_warn,360
+imu_hfaccel_mean_warn,72
 obs_ang_err_median_warn,8.0E-3
 obs_vel_err_median_warn,0.05
 obs_pos_err_median_warn,0.15

Tools/ecl_ekf/check_table.csv

@@ -49,12 +49,12 @@ hagl_test_mean, The mean in-flight value of the height above ground sensor innov
 ofx_fail_percentage, The percentage of in-flight recorded failure events for the optical flow sensor X-axis innovation consistency test.
 ofy_fail_percentage, The percentage of in-flight recorded failure events for the optical flow sensor Y-axis innovation consistency test.
 filter_faults_max, Largest recorded value of the filter internal fault bitmask. Should always be zero.
-imu_coning_peak, Peak in-flight value of the IMU delta angle coning vibration metric (rad)
-imu_coning_mean, Mean in-flight value of the IMU delta angle coning vibration metric (rad)
-imu_hfdang_peak, Peak in-flight value of the IMU delta angle high frequency vibration metric (rad)
-imu_hfdang_mean, Mean in-flight value of the IMU delta angle high frequency vibration metric (rad)
-imu_hfdvel_peak, Peak in-flight value of the IMU delta velocity high frequency vibration metric (m/s)
-imu_hfdvel_mean, Mean in-flight value of the IMU delta velocity high frequency vibration metric (m/s)
+imu_coning_peak, Peak in-flight value of the IMU delta angle coning vibration metric (rad^2)
+imu_coning_mean, Mean in-flight value of the IMU delta angle coning vibration metric (rad^2)
+imu_hfgyro_peak, Peak in-flight value of the IMU accel high frequency vibration metric (rad/s)
+imu_hfgyro_mean, Mean in-flight value of the IMU accel high frequency vibration metric (rad/s)
+imu_hfaccel_peak, Peak in-flight value of the IMU accel high frequency vibration metric (m/s/s)
+imu_hfaccel_mean, Mean in-flight value of the IMU accel high frequency vibration metric (m/s/s)
 output_obs_ang_err_median, Median in-flight value of the output observer angular error (rad)
 output_obs_vel_err_median, Median in-flight value of the output observer velocity error (m/s)
 output_obs_pos_err_median, Median in-flight value of the output observer position error (m)

Tools/ecl_ekf/drag_fusion_tuning/drag_fusion_symbolic.py

@@ -0,0 +1,55 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+"""
+    Copyright (c) 2022 PX4 Development Team
+    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: frag_fusion_symbolic.py
+Author: Mathieu Bresciani <mathieu@auterion.com>
+License: BSD 3-Clause
+Description:
+"""
+
+from sympy import *
+
+V = Symbol("V", real=True)
+rho = Symbol("rho", real=True)
+rho_n = Symbol("rho_n", real=True)
+Mcoef = Symbol("Mcoef", real=True)
+Bcoef = Symbol("Bcoef", real=True)
+a = Symbol("a", real=True)
+
+f1 = 0.5 * rho / Bcoef * V**2 + rho_n * Mcoef * V - a
+
+print("If Bcoef > 0 and Mcoef > 0")
+print("V =")
+res_V = solve(f1, V)
+res_V = res_V[0]
+pprint(res_V)
+print("a_pred =")
+pprint(solve(f1, a))

Tools/ecl_ekf/drag_fusion_tuning/drag_replay.py

@@ -0,0 +1,202 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+"""
+    Copyright (c) 2022 PX4 Development Team
+    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: drag_replay.py
+Author: Mathieu Bresciani <mathieu@auterion.com>
+License: BSD 3-Clause
+Description:
+    Find the best ballistic and momentum drag coefficients for wind estimation
+    using EKF2 replay data.
+    NOTE: this script currently assumes no wind.
+"""
+
+import matplotlib.pylab as plt
+from pyulog import ULog
+from pyulog.px4 import PX4ULog
+import numpy as np
+import quaternion
+from scipy import optimize
+
+def getAllData(logfile):
+    log = ULog(logfile)
+
+    v_local = np.matrix([getData(log, 'vehicle_local_position', 'vx'),
+              getData(log, 'vehicle_local_position', 'vy'),
+              getData(log, 'vehicle_local_position', 'vz')])
+
+    t_v_local = ms2s(getData(log, 'vehicle_local_position', 'timestamp'))
+
+    accel = np.matrix([getData(log, 'sensor_combined', 'accelerometer_m_s2[0]'),
+              getData(log, 'sensor_combined', 'accelerometer_m_s2[1]'),
+              getData(log, 'sensor_combined', 'accelerometer_m_s2[2]')])
+    t_accel = ms2s(getData(log, 'sensor_combined', 'timestamp'))
+
+    q = np.matrix([getData(log, 'vehicle_attitude', 'q[0]'),
+              getData(log, 'vehicle_attitude', 'q[1]'),
+              getData(log, 'vehicle_attitude', 'q[2]'),
+              getData(log, 'vehicle_attitude', 'q[3]')])
+    t_q = ms2s(getData(log, 'vehicle_attitude', 'timestamp'))
+
+    dist_bottom = getData(log, 'vehicle_local_position', 'dist_bottom')
+    t_dist_bottom = ms2s(getData(log, 'vehicle_local_position', 'timestamp'))
+
+    (t_aligned, v_body_aligned, accel_aligned) = alignData(t_v_local, v_local, t_accel, accel, t_q, q, t_dist_bottom, dist_bottom)
+
+    t_aligned -= t_aligned[0]
+
+    return (t_aligned, v_body_aligned, accel_aligned)
+
+def alignData(t_v, v_local, t_accel, accel, t_q, q, t_dist_bottom, dist_bottom):
+    len_accel = len(t_accel)
+    len_q = len(t_q)
+    len_db = len(t_dist_bottom)
+    i_a = 0
+    i_q = 0
+    i_db = 0
+    v_body_aligned = np.empty((3,0))
+    accel_aligned = np.empty((3,0))
+    t_aligned = []
+
+    for i_v in range(len(t_v)):
+        t = t_v[i_v]
+        accel_sum = np.zeros((3,1))
+        accel_count = 0
+        while t_accel[i_a] < t and i_a < len_accel-1:
+            accel_sum += accel[:, i_a] # Integrate accel samples between 2 velocity samples
+            accel_count += 1
+            i_a += 1
+        while t_q[i_q] < t and i_q < len_q-1:
+            i_q += 1
+        while t_dist_bottom[i_db] < t and i_db < len_db-1:
+            i_db += 1
+
+        # Only use in air data
+        if dist_bottom[i_db] < 1.0 or accel_count == 0:
+            continue
+
+        qk = np.quaternion(q[0, i_q],q[1, i_q],q[2, i_q],q[3, i_q])
+        q_vl = np.quaternion(0, v_local[0, i_v], v_local[1, i_v], v_local[2, i_v])
+        q_vb = qk.conjugate() * q_vl * qk # Get velocity in body frame
+        vb = quaternion.as_float_array(q_vb)[1:4]
+
+        v_body_aligned = np.append(v_body_aligned, [[vb[0]], [vb[1]], [vb[2]]], axis=1)
+        accel_aligned = np.append(accel_aligned, accel_sum / accel_count, axis=1)
+        t_aligned.append(t)
+
+    return (t_aligned, v_body_aligned, np.asarray(accel_aligned))
+
+def getData(log, topic_name, variable_name, instance=0):
+    variable_data = np.array([])
+    for elem in log.data_list:
+        if elem.name == topic_name:
+            if instance == elem.multi_id:
+                variable_data = elem.data[variable_name]
+                break
+
+    return variable_data
+
+def ms2s(time_ms):
+    return time_ms * 1e-6
+
+def run(logfile):
+    (t, v_body, a_body) = getAllData(logfile)
+
+    rho = 1.15 # air densitiy
+    rho15 = 1.225 # air density at 15 degC
+
+    # x[0]: momentum drag, scales with v
+    # x[1]: inverse of ballistic coefficient (X body axis), scales with v^2
+    # x[2]: inverse of ballistic coefficient (Y body axis), scales with v^2
+    predict_acc_x = lambda x: -v_body[0] * x[0] - 0.5 * rho * v_body[0]**2 * np.sign(v_body[0]) * x[1]
+    predict_acc_y = lambda x: -v_body[1] * x[0] - 0.5 * rho * v_body[1]**2 * np.sign(v_body[1]) * x[2]
+
+    J = lambda x: np.sum(np.power(abs(a_body[0]-predict_acc_x(x)), 2.0) + np.power(abs(a_body[1]-predict_acc_y(x)), 2.0)) # cost function
+
+    x0 = [0.15, 1/100, 1/100] # initial conditions
+    res = optimize.minimize(J, x0, method='nelder-mead', bounds=[(0,1),(0,10),(0,10)], options={'disp': True})
+
+    # Convert results to parameters
+    innov_var = J(res.x) / (len(v_body[0]) + len(v_body[1]))
+    mcoef = res.x[0] / np.sqrt(rho / rho15)
+
+    bcoef_x = 0.0
+    bcoef_y = 0.0
+
+    if res.x[1] > 1/200:
+        bcoef_x = 1/res.x[1]
+
+    if res.x[2] > 1/200:
+        bcoef_y = 1/res.x[2]
+
+    print(f"param set EKF2_BCOEF_X {bcoef_x:.1f}")
+    print(f"param set EKF2_BCOEF_Y {bcoef_y:.1f}")
+    print(f"param set EKF2_MCOEF {mcoef:.2f}")
+    print(f"/!\EXPERIMENTAL param set EKF2_DRAG_NOISE {innov_var:.2f}")
+
+    # Plot data
+    plt.figure(1)
+    plt.suptitle(logfile.split('/')[-1])
+    ax1 = plt.subplot(2, 1, 1)
+    ax1.plot(t, v_body[0])
+    ax1.plot(t, v_body[1])
+    ax1.set_xlabel("time (s)")
+    ax1.set_ylabel("velocity (m/s)")
+    ax1.legend(["forward", "right"])
+
+    ax2 = plt.subplot(2, 1, 2, sharex=ax1)
+    ax2.set_title(f"BCoef_x = {bcoef_x:.1f}, BCoef_y = {bcoef_y:.1f}, MCoef = {mcoef:.4f}", loc="right")
+    ax2.plot(t, a_body[0])
+    ax2.plot(t, a_body[1])
+    ax2.plot(t, predict_acc_x(res.x))
+    ax2.plot(t, predict_acc_y(res.x))
+    ax2.set_xlabel("time (s)")
+    ax2.set_ylabel("acceleration (m/s^2)")
+    ax2.legend(["meas_forward", "meas_right", "predicted_forward", "predicted_right"])
+    plt.show()
+
+if __name__ == '__main__':
+    import os
+    import argparse
+
+    # Get the path of this script (without file name)
+    script_path = os.path.split(os.path.realpath(__file__))[0]
+
+    # Parse arguments
+    parser = argparse.ArgumentParser(
+        description='Estimate mag biases from ULog file')
+
+    # Provide parameter file path and name
+    parser.add_argument('logfile', help='Full ulog file path, name and extension', type=str)
+    args = parser.parse_args()
+
+    logfile = os.path.abspath(args.logfile) # Convert to absolute path
+
+    run(logfile)

Tools/ecl_ekf/drag_fusion_tuning/readme.md

@@ -0,0 +1,32 @@
+# PX4 Drag fusion parameter tuning algorithm
+In PX4, drag fusion can be enabled in order to estimate the wind when flying a multirotor, assuming that the body vertical acceleration is produced by the rotors and that the lateral forces are produced by drag.
+The model assumes a combination of:
+1. momentum drag: created by the rotors changing the direction of the incoming air, linear to the relative airspeed. Parameter `EKF2_MCOEF`
+2. bluff body drag: created by the wetted area of the aircraft, quadratic to the relative airspeed. Parameters `EKF2_BCOEF_X` and `EKF2_BCOEF_Y`
+
+The python script was created to automate the tuning of the aforementioned parameters using flight test data.
+
+## How to use this script
+First, a flight log with enough information is required in order to accurately estimate the parameters.
+The best way to do this is to fly the drone in altitude mode, accelerate to a moderate-high speed and let the drone slow-down by its own drag.
+Repeat the same maneuver in all directions and several times to obtain a good dataset.
+
+/!\ NOTE: the current state of this script assumes no wind. Some modifications are required to estimate both the wind and the parameters at the same time.
+
+Then, install the required python packages:
+```
+pip install -r requirements.txt
+```
+and run the script and give it the log file as an argument:
+```
+python drag_replay.py <logfilename.ulg>
+```
+
+The estimated parameters are displayed in the console and the fit quality is shown in a plot:
+```
+param set EKF2_BCOEF_X 0.0
+param set EKF2_BCOEF_Y 62.1
+param set EKF2_MCOEF 0.16
+/!\EXPERIMENTAL param set EKF2_DRAG_NOISE 0.31
+```
+![DeepinScreenshot_matplotlib_20220329100027](https://user-images.githubusercontent.com/14822839/160563024-efddd100-d7db-46f7-8676-cf4296e9f737.png)

Tools/ecl_ekf/drag_fusion_tuning/requirements.txt

@@ -0,0 +1,6 @@
+matplotlib==3.5.1
+numpy==1.22.2
+pyulog==0.9.0
+quaternion==3.5.2.post4
+scipy==1.8.0
+sympy==1.10.1

Tools/ecl_ekf/plotting/pdf_report.py

@@ -250,18 +250,32 @@ def create_pdf_report(ulog: ULog, multi_instance: int, output_plot_filename: str
         data_plot.save()
         data_plot.close()
 
+
         # Plot the EKF IMU vibration metrics
-        scaled_estimator_status = {'vibe[0]': 1000. * estimator_status['vibe[0]'],
-                                   'vibe[1]': 1000. * estimator_status['vibe[1]'],
-                                   'vibe[2]': estimator_status['vibe[2]']
-                                   }
-        data_plot = CheckFlagsPlot(
-            status_time, scaled_estimator_status, [['vibe[0]'], ['vibe[1]'], ['vibe[2]']],
-            x_label='time (sec)', y_labels=['Del Ang Coning (mrad)', 'HF Del Ang (mrad)',
-                                            'HF Del Vel (m/s)'], plot_title='IMU Vibration Metrics',
-            pdf_handle=pdf_pages, annotate=True)
-        data_plot.save()
-        data_plot.close()
+        for imu_status_instance in range(4):
+            try:
+                vehicle_imu_status_data = ulog.get_dataset('vehicle_imu_status', imu_status_instance).data
+
+                imu_status_time = 1e-6 * vehicle_imu_status_data['timestamp']
+
+                if vehicle_imu_status_data['accel_device_id'][0] == estimator_status['accel_device_id'][0]:
+
+                    scaled_estimator_status = {'delta_angle_coning_metric': 1000. * vehicle_imu_status_data['delta_angle_coning_metric'],
+                                               'gyro_vibration_metric': vehicle_imu_status_data['gyro_vibration_metric'],
+                                               'accel_vibration_metric': vehicle_imu_status_data['accel_vibration_metric']
+                                              }
+                    data_plot = CheckFlagsPlot(
+                        imu_status_time, scaled_estimator_status, [['delta_angle_coning_metric'], ['gyro_vibration_metric'], ['accel_vibration_metric']],
+                        x_label='time (sec)',
+                        y_labels=['Del Ang Coning (mrad^2)', 'HF Gyro (rad/s)', 'HF accel (m/s/s)'],
+                        plot_title='IMU Vibration Metrics',
+                        pdf_handle=pdf_pages, annotate=True)
+                    data_plot.save()
+                    data_plot.close()
+
+            except:
+                pass
+
 
         # Plot the EKF output observer tracking errors
         scaled_innovations = {

boards/ark/cannode/canbootloader.px4board

@@ -0,0 +1,6 @@
+CONFIG_BOARD_TOOLCHAIN="arm-none-eabi"
+CONFIG_BOARD_ARCHITECTURE="cortex-m4"
+CONFIG_BOARD_ROMFSROOT=""
+CONFIG_BOARD_CONSTRAINED_MEMORY=y
+CONFIG_DRIVERS_BOOTLOADERS=y
+CONFIG_NSH_BUILTIN_APPS=y

boards/ark/cannode/default.px4board

@@ -0,0 +1,37 @@
+CONFIG_BOARD_TOOLCHAIN="arm-none-eabi"
+CONFIG_BOARD_ARCHITECTURE="cortex-m4"
+CONFIG_BOARD_ROMFSROOT="cannode"
+CONFIG_BOARD_CONSTRAINED_FLASH=y
+#CONFIG_BOARD_NO_HELP=y
+CONFIG_BOARD_CONSTRAINED_MEMORY=y
+CONFIG_BOARD_EXTERNAL_METADATA=y
+CONFIG_COMMON_BAROMETERS=y
+CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
+CONFIG_COMMON_DISTANCE_SENSOR=y
+CONFIG_COMMON_HYGROMETERS=y
+CONFIG_COMMON_MAGNETOMETER=y
+CONFIG_COMMON_LIGHT=y
+CONFIG_DRIVERS_BATT_SMBUS=y
+CONFIG_DRIVERS_BOOTLOADERS=y
+CONFIG_DRIVERS_GPS=y
+CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=y
+CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y
+CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
+CONFIG_DRIVERS_IRLOCK=y
+CONFIG_DRIVERS_PWM_OUT=y
+CONFIG_BOARD_UAVCAN_INTERFACES=1
+CONFIG_DRIVERS_UAVCANNODE=y
+CONFIG_MODULES_CONTROL_ALLOCATOR=y
+#CONFIG_MODULES_SENSORS=y
+CONFIG_NSH_BUILTIN_APPS=y
+CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
+CONFIG_SYSTEMCMDS_I2CDETECT=y
+CONFIG_SYSTEMCMDS_LED_CONTROL=y
+CONFIG_SYSTEMCMDS_MIXER=y
+CONFIG_SYSTEMCMDS_UORB=y
+CONFIG_SYSTEMCMDS_PARAM=y
+CONFIG_SYSTEMCMDS_TOP=y
+CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
+CONFIG_SYSTEMCMDS_PWM=y
+CONFIG_SYSTEMCMDS_REBOOT=y
+CONFIG_SYSTEMCMDS_WORK_QUEUE=y

boards/ark/cannode/firmware.prototype

@@ -0,0 +1,13 @@
+{
+    "board_id": 83,
+    "magic": "PX4FWv1",
+    "description": "Firmware for the ARK CANnode board",
+    "image": "",
+    "build_time": 0,
+    "summary": "ARKCANNODE",
+    "version": "0.1",
+    "image_size": 0,
+    "image_maxsize": 2080768,
+    "git_identity": "",
+    "board_revision": 0
+}

boards/ark/cannode/init/rc.board_defaults

@@ -0,0 +1,7 @@
+#!/bin/sh
+#
+# board specific defaults
+#------------------------------------------------------------------------------
+pwm_out start
+
+control_allocator start

boards/ark/cannode/init/rc.board_sensors

@@ -0,0 +1,118 @@
+#!/bin/sh
+#
+# board sensors init
+#------------------------------------------------------------------------------
+icm42688p -R 0 -s start
+
+if param compare -s SENS_EN_BATT 1
+then
+	batt_smbus start -X
+fi
+
+# Lidar-Lite on I2C
+if param compare -s SENS_EN_LL40LS 2
+then
+	ll40ls start -X
+fi
+
+# mappydot lidar sensor
+if param compare -s SENS_EN_MPDT 1
+then
+	mappydot start -X
+fi
+
+# mb12xx sonar sensor
+if param greater -s SENS_EN_MB12XX 0
+then
+	mb12xx start -X
+fi
+
+# Lightware i2c lidar sensor
+if param greater -s SENS_EN_SF1XX 0
+then
+	lightware_laser_i2c start -X
+fi
+
+# vl53l1x i2c distance sensor
+if param compare -s SENS_EN_VL53L1X 1
+then
+	vl53l1x start -X
+fi
+
+# ADIS16448 spi external IMU
+if param compare -s SENS_EN_ADIS164X 1
+then
+	if param compare -s SENS_OR_ADIS164X 0
+	then
+		adis16448 -S start
+	fi
+	if param compare -s SENS_OR_ADIS164X 4
+	then
+		adis16448 -S start -R 4
+	fi
+fi
+
+# Eagle Tree airspeed sensor external I2C
+if param compare -s SENS_EN_ETSASPD 1
+then
+	ets_airspeed start -X
+fi
+
+# Sensirion SDP3X differential pressure sensor external I2C
+if param compare -s SENS_EN_SDP3X 1
+then
+	if ! sdp3x_airspeed start -X
+	then
+		# try another common address
+		sdp3x_airspeed start -X -a 0x22
+	fi
+fi
+
+# SHT3x temperature and hygrometer sensor, external I2C
+if param compare -s SENS_EN_SHT3X 1
+then
+	sht3x start -X
+	sht3x start -X -a 0x45
+fi
+
+# TE MS4525 differential pressure sensor external I2C
+if param compare -s SENS_EN_MS4525 1
+then
+	ms4525_airspeed start -X
+fi
+
+# TE MS5525 differential pressure sensor external I2C
+if param compare -s SENS_EN_MS5525 1
+then
+	ms5525_airspeed start -X
+fi
+
+# IR-LOCK sensor external I2C
+if param compare -s SENS_EN_IRLOCK 1
+then
+	irlock start -X
+fi
+
+# SPL06 sensor external I2C
+if param compare -s SENS_EN_SPL06 1
+then
+	spl06 -X start
+	spl06 -X -a 0x77 start
+fi
+
+gps start -d /dev/ttyS0 -p ubx
+
+# probe for optional external I2C devices
+icm20948_i2c_passthrough -X -q start
+
+# compasses
+hmc5883 -T -X -q start
+ist8308 -X -q start
+ist8310 -X -q start
+lis2mdl -X -q start
+lis3mdl -X -q start
+qmc5883l -X -q start
+rm3100 -X -q start
+
+# start last (wait for possible icm20948 passthrough mode)
+ak09916 -X -q start

boards/ark/cannode/nuttx-config/canbootloader/defconfig

@@ -0,0 +1,61 @@
+#
+# This file is autogenerated: PLEASE DO NOT EDIT IT.
+#
+# You can use "make menuconfig" to make any modifications to the installed .config file.
+# You can then do "make savedefconfig" to generate a new defconfig file that includes your
+# modifications.
+#
+CONFIG_ARCH="arm"
+CONFIG_ARCH_BOARD_CUSTOM=y
+CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/ark/cannode/nuttx-config"
+CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y
+CONFIG_ARCH_BOARD_CUSTOM_NAME="px4"
+CONFIG_ARCH_CHIP="stm32"
+CONFIG_ARCH_CHIP_STM32=y
+CONFIG_ARCH_CHIP_STM32F412CE=y
+CONFIG_ARCH_INTERRUPTSTACK=4096
+CONFIG_ARMV7M_MEMCPY=y
+CONFIG_ARMV7M_USEBASEPRI=y
+CONFIG_BINFMT_DISABLE=y
+CONFIG_BOARD_LOOPSPERMSEC=16717
+CONFIG_C99_BOOL8=y
+CONFIG_CLOCK_MONOTONIC=y
+CONFIG_DEBUG_FULLOPT=y
+CONFIG_DEBUG_SYMBOLS=y
+CONFIG_DEFAULT_SMALL=y
+CONFIG_DISABLE_MOUNTPOINT=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=4096
+CONFIG_LIBC_FLOATINGPOINT=y
+CONFIG_LIBC_LONG_LONG=y
+CONFIG_LIBC_STRERROR=y
+CONFIG_LIB_BOARDCTL=y
+CONFIG_FS_PROCFS_MAX_TASKS=0
+CONFIG_MM_REGIONS=2
+CONFIG_NAME_MAX=0
+CONFIG_NUNGET_CHARS=0
+CONFIG_PREALLOC_TIMERS=0
+CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAM_SIZE=262144
+CONFIG_RAM_START=0x20010000
+CONFIG_RAW_BINARY=y
+CONFIG_SDCLONE_DISABLE=y
+CONFIG_SIG_DEFAULT=y
+CONFIG_SIG_SIGALRM_ACTION=y
+CONFIG_SIG_SIGUSR1_ACTION=y
+CONFIG_SIG_SIGUSR2_ACTION=y
+CONFIG_STACK_COLORATION=y
+CONFIG_START_DAY=30
+CONFIG_START_MONTH=11
+CONFIG_STDIO_DISABLE_BUFFERING=y
+CONFIG_STM32_FLASH_CONFIG_G=y
+CONFIG_STM32_NOEXT_VECTORS=y
+CONFIG_TASK_NAME_SIZE=0
+CONFIG_USEC_PER_TICK=1000
+CONFIG_USERMAIN_STACKSIZE=4096

boards/ark/cannode/nuttx-config/include/board.h

@@ -0,0 +1,149 @@
+/************************************************************************************
+ * configs/px4fmu/include/board.h
+ * include/arch/board/board.h
+ *
+ *   Copyright (C) 2009 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.
+ *
+ ************************************************************************************/
+#include "board_dma_map.h"
+
+#ifndef __ARCH_BOARD_BOARD_H
+#define __ARCH_BOARD_BOARD_H
+
+#include <nuttx/config.h>
+#ifndef __ASSEMBLY__
+# include <stdint.h>
+#endif
+
+#include <stm32.h>
+
+/* HSI - 8 MHz RC factory-trimmed
+ * LSI - 32 KHz RC
+ * HSE - 8  MHz Crystal
+ * LSE - not installed
+ */
+#define STM32_BOARD_USEHSE      1
+#define STM32_BOARD_XTAL        8000000
+#define STM32_HSE_FREQUENCY     STM32_BOARD_XTAL
+
+#define STM32_HSI_FREQUENCY     16000000ul
+#define STM32_LSI_FREQUENCY     32000
+
+/* Main PLL Configuration */
+#define STM32_PLLCFG_PLLM       RCC_PLLCFG_PLLM(8)
+#define STM32_PLLCFG_PLLN       RCC_PLLCFG_PLLN(384)
+#define STM32_PLLCFG_PLLP       RCC_PLLCFG_PLLP_4
+#define STM32_PLLCFG_PLLQ       RCC_PLLCFG_PLLQ(8)
+#define STM32_PLLCFG_PLLR       RCC_PLLCFG_PLLR(2)
+
+#define STM32_RCC_PLLI2SCFGR_PLLI2SM RCC_PLLI2SCFGR_PLLI2SM(16)
+#define STM32_RCC_PLLI2SCFGR_PLLI2SN RCC_PLLI2SCFGR_PLLI2SN(192)
+#define STM32_RCC_PLLI2SCFGR_PLLI2SQ RCC_PLLI2SCFGR_PLLI2SQ(2)
+#define STM32_RCC_PLLI2SCFGR_PLLI2SR RCC_PLLI2SCFGR_PLLI2SR(2)
+#define STM32_RCC_PLLI2SCFGR_PLLI2SSRC RCC_PLLI2SCFGR_PLLI2SSRC(0) /* HSE or HSI depending on PLLSRC of PLLCFGR*/
+
+#define STM32_RCC_DCKCFGR2_CK48MSEL RCC_DCKCFGR2_CK48MSEL_PLL
+#define STM32_RCC_DCKCFGR2_FMPI2C1SEL RCC_DCKCFGR2_FMPI2C1SEL_APB
+#define STM32_RCC_DCKCFGR2_SDIOSEL RCC_DCKCFGR2_SDIOSEL_48MHZ
+
+#define STM32_SYSCLK_FREQUENCY  96000000ul
+
+/* AHB clock (HCLK) is SYSCLK (96MHz) */
+#define STM32_RCC_CFGR_HPRE     RCC_CFGR_HPRE_SYSCLK      /* HCLK  = SYSCLK / 1 */
+#define STM32_HCLK_FREQUENCY    STM32_SYSCLK_FREQUENCY
+#define STM32_BOARD_HCLK        STM32_HCLK_FREQUENCY      /* Same as above, to satisfy compiler */
+
+/* APB1 clock (PCLK1) is HCLK/2 (48MHz) */
+#define STM32_RCC_CFGR_PPRE1    RCC_CFGR_PPRE1_HCLKd2     /* PCLK1 = HCLK / 2 */
+#define STM32_PCLK1_FREQUENCY   (STM32_HCLK_FREQUENCY/2)
+
+/* Timers driven from APB1 will be twice PCLK1 (see page 112 of reference manual) */
+#define STM32_APB1_TIM2_CLKIN   (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM3_CLKIN   (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM4_CLKIN   (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM5_CLKIN   (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM6_CLKIN   (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM7_CLKIN   (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM12_CLKIN  (2*STM32_PCLK1_FREQUENCY)
+
+/* APB2 clock (PCLK2) is HCLK (96MHz) */
+#define STM32_RCC_CFGR_PPRE2    RCC_CFGR_PPRE2_HCLK       /* PCLK2 = HCLK */
+#define STM32_PCLK2_FREQUENCY   (STM32_HCLK_FREQUENCY)
+
+/* Timers driven from APB2 will be PCLK2 since no prescale division */
+#define STM32_APB2_TIM1_CLKIN   (STM32_PCLK2_FREQUENCY)
+#define STM32_APB2_TIM8_CLKIN   (STM32_PCLK2_FREQUENCY)
+#define STM32_APB2_TIM9_CLKIN   (STM32_PCLK2_FREQUENCY)
+#define STM32_APB2_TIM10_CLKIN  (STM32_PCLK2_FREQUENCY)
+#define STM32_APB2_TIM11_CLKIN  (STM32_PCLK2_FREQUENCY)
+
+/* Timer Frequencies, if APBx is set to 1, frequency is same to APBx otherwise frequency is 2xAPBx. */
+#define BOARD_TIM2_FREQUENCY    (2 * STM32_PCLK1_FREQUENCY)
+#define BOARD_TIM3_FREQUENCY    (2 * STM32_PCLK1_FREQUENCY)
+#define BOARD_TIM4_FREQUENCY    (2 * STM32_PCLK1_FREQUENCY)
+#define BOARD_TIM5_FREQUENCY    (2 * STM32_PCLK1_FREQUENCY)
+#define BOARD_TIM6_FREQUENCY    (2 * STM32_PCLK1_FREQUENCY)
+#define BOARD_TIM7_FREQUENCY    (2 * STM32_PCLK1_FREQUENCY)
+#define BOARD_TIM8_FREQUENCY    (2 * STM32_PCLK2_FREQUENCY)
+
+/* Alternate function pin selections ************************************************/
+
+/* UARTs */
+#define GPIO_USART1_RX	GPIO_USART1_RX_2
+#define GPIO_USART1_TX	GPIO_USART1_TX_3
+
+#define GPIO_USART2_RX	GPIO_USART2_RX_1
+#define GPIO_USART2_TX	GPIO_USART2_TX_1
+
+/* CAN */
+#define GPIO_CAN1_RX	GPIO_CAN1_RX_1
+#define GPIO_CAN1_TX	GPIO_CAN1_TX_1
+
+/* SPI */
+#define GPIO_SPI1_MISO  GPIO_SPI1_MISO_1
+#define GPIO_SPI1_MOSI  GPIO_SPI1_MOSI_1
+#define GPIO_SPI1_SCK   GPIO_SPI1_SCK_1
+
+#define GPIO_SPI2_MISO  GPIO_SPI2_MISO_1 /* PB14 */
+#define GPIO_SPI2_MOSI  GPIO_SPI2_MOSI_1 /* PB15 */
+#define GPIO_SPI2_SCK   GPIO_SPI2_SCK_2  /* PB13 */
+
+/* I2C */
+#define GPIO_MCU_I2C1_SCL
+#define GPIO_MCU_I2C1_SDA
+
+#define GPIO_I2C1_SCL	GPIO_I2C1_SCL_1
+#define GPIO_I2C1_SDA	GPIO_I2C1_SDA_2
+
+#define GPIO_I2C1_SCL_GPIO                  (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTB | GPIO_PIN6)
+#define GPIO_I2C1_SDA_GPIO                  (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTB | GPIO_PIN7)
+
+#endif /* __ARCH_BOARD_BOARD_H */

boards/ark/cannode/nuttx-config/include/board_dma_map.h

@@ -0,0 +1,44 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+// DMA1 Channel/Stream Selections
+//--------------------------------------------//---------------------------//----------------
+#define DMACHAN_SPI2_RX    DMAMAP_SPI2_RX   // DMA1, Stream 3, Channel 0
+#define DMACHAN_SPI2_TX    DMAMAP_SPI2_TX   // DMA1, Stream 4, Channel 0
+
+//  DMA2 Channel/Stream Selections
+//--------------------------------------------//---------------------------//----------------
+#define DMACHAN_SPI1_RX    DMAMAP_SPI1_RX_2   // DMA2, Stream 2, Channel 3
+#define DMACHAN_SPI1_TX    DMAMAP_SPI1_TX_2   // DMA2, Stream 5, Channel 3

boards/ark/cannode/nuttx-config/nsh/defconfig

@@ -0,0 +1,161 @@
+#
+# 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_OS_API is not set
+# CONFIG_DISABLE_PSEUDOFS_OPERATIONS 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_STM32_DMACAPABLE is not set
+CONFIG_ARCH="arm"
+CONFIG_ARCH_BOARD_CUSTOM=y
+CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/ark/cannode/nuttx-config"
+CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y
+CONFIG_ARCH_BOARD_CUSTOM_NAME="px4"
+CONFIG_ARCH_CHIP="stm32"
+CONFIG_ARCH_CHIP_STM32=y
+CONFIG_ARCH_CHIP_STM32F412CE=y
+CONFIG_ARCH_INTERRUPTSTACK=512
+CONFIG_ARCH_STACKDUMP=y
+CONFIG_ARMV7M_MEMCPY=y
+CONFIG_ARMV7M_USEBASEPRI=y
+CONFIG_BOARDCTL_RESET=y
+CONFIG_BOARD_LOOPSPERMSEC=16717
+CONFIG_BOARD_RESET_ON_ASSERT=2
+CONFIG_BUILTIN=y
+CONFIG_C99_BOOL8=y
+CONFIG_CLOCK_MONOTONIC=y
+CONFIG_DEBUG_FULLOPT=y
+CONFIG_DEBUG_HARDFAULT_ALERT=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_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_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_LIBC_FLOATINGPOINT=y
+CONFIG_LIBC_LONG_LONG=y
+CONFIG_LIBC_STRERROR=y
+CONFIG_MEMSET_64BIT=y
+CONFIG_MEMSET_OPTSPEED=y
+CONFIG_MM_REGIONS=2
+CONFIG_MTD=y
+CONFIG_MTD_BYTE_WRITE=y
+CONFIG_MTD_PARTITION=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_DISABLE_IFCONFIG=y
+CONFIG_NSH_DISABLE_IFUPDOWN=y
+CONFIG_NSH_DISABLE_TELNETD=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_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=262144
+CONFIG_RAM_START=0x20000000
+CONFIG_RAW_BINARY=y
+CONFIG_RTC_DATETIME=y
+CONFIG_SCHED_ATEXIT=y
+CONFIG_SCHED_HPWORK=y
+CONFIG_SCHED_HPWORKPRIORITY=254
+CONFIG_SCHED_HPWORKSTACKSIZE=3000
+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_SEM_NNESTPRIO=8
+CONFIG_SEM_PREALLOCHOLDERS=0
+CONFIG_SERIAL_TERMIOS=y
+CONFIG_SIG_DEFAULT=y
+CONFIG_SIG_SIGALRM_ACTION=y
+CONFIG_SIG_SIGUSR1_ACTION=y
+CONFIG_SIG_SIGUSR2_ACTION=y
+CONFIG_SIG_SIGWORK=4
+CONFIG_STACK_COLORATION=y
+CONFIG_START_DAY=30
+CONFIG_START_MONTH=11
+CONFIG_STDIO_BUFFER_SIZE=32
+CONFIG_STM32_ADC1=y
+CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG=y
+CONFIG_STM32_DMA1=y
+CONFIG_STM32_DMA2=y
+CONFIG_STM32_FLASH_CONFIG_G=y
+CONFIG_STM32_FLASH_PREFETCH=y
+CONFIG_STM32_FLOWCONTROL_BROKEN=y
+CONFIG_STM32_I2C1=y
+CONFIG_STM32_JTAG_SW_ENABLE=y
+CONFIG_STM32_PWR=y
+CONFIG_STM32_RTC=y
+CONFIG_STM32_RTC_HSECLOCK=y
+CONFIG_STM32_RTC_MAGIC=0xfacefeee
+CONFIG_STM32_RTC_MAGIC_REG=1
+CONFIG_STM32_RTC_MAGIC_TIME_SET=0xfacefeef
+CONFIG_STM32_SERIALBRK_BSDCOMPAT=y
+CONFIG_STM32_SERIAL_DISABLE_REORDERING=y
+CONFIG_STM32_SPI1=y
+CONFIG_STM32_SPI1_DMA=y
+CONFIG_STM32_SPI1_DMA_BUFFER=2048
+CONFIG_STM32_SPI2=y
+CONFIG_STM32_SPI2_DMA=y
+CONFIG_STM32_SPI2_DMA_BUFFER=2048
+CONFIG_STM32_SPI_DMA=y
+CONFIG_STM32_USART1=y
+CONFIG_STM32_USART2=y
+CONFIG_STM32_USART_BREAKS=y
+CONFIG_STM32_WWDG=y
+CONFIG_SYSTEM_NSH=y
+CONFIG_TASK_NAME_SIZE=24
+CONFIG_USART1_BAUD=57600
+CONFIG_USART1_RXBUFSIZE=600
+CONFIG_USART1_TXBUFSIZE=1100
+CONFIG_USART2_BAUD=57600
+CONFIG_USART2_RXBUFSIZE=600
+CONFIG_USART2_SERIAL_CONSOLE=y
+CONFIG_USART2_TXBUFSIZE=1100
+CONFIG_USEC_PER_TICK=1000
+CONFIG_USERMAIN_STACKSIZE=2624
+CONFIG_USER_ENTRYPOINT="nsh_main"

boards/ark/cannode/nuttx-config/scripts/canbootloader_script.ld

@@ -0,0 +1,134 @@
+/****************************************************************************
+ * nuttx-config/scripts/canbootloader_script.ld
+ *
+ *   Copyright (C) 2015 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <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 STM32F412 has 512Kb of FLASH beginning at address 0x0800:0000 and
+ * 256Kb of SRAM. SRAM is split up into three blocks:
+ *
+ * 1) 112Kb of SRAM beginning at address 0x2000:0000
+ * 2)  16Kb of SRAM beginning at address 0x2001:c000
+ * 3)  64Kb of SRAM beginning at address 0x2002:0000
+ * 4)  64Kb of TCM SRAM beginning at address 0x1000:0000
+ *
+ * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000
+ * where the code expects to begin execution by jumping to the entry point in
+ * the 0x0800:0000 address range.
+ *
+ * The first 0x10000 of flash is reserved for the bootloader.
+ */
+
+MEMORY
+{
+    flash (rx)   : ORIGIN = 0x08000000, LENGTH = 32K
+    sram (rwx)   : ORIGIN = 0x20000000, LENGTH = 192K
+}
+
+OUTPUT_ARCH(arm)
+
+ENTRY(__start)		/* treat __start as the anchor for dead code stripping */
+EXTERN(_vectors)	/* force the vectors to be included in the output */
+
+/*
+ * Ensure that abort() is present in the final object.  The exception handling
+ * code pulled in by libgcc.a requires it (and that code cannot be easily avoided).
+ */
+EXTERN(abort)
+SECTIONS
+{
+	.text : {
+		_stext = ABSOLUTE(.);
+		*(.vectors)
+		*(.text .text.*)
+		*(.fixup)
+		*(.gnu.warning)
+		*(.rodata .rodata.*)
+		*(.gnu.linkonce.t.*)
+		*(.got)
+		*(.gcc_except_table)
+		*(.gnu.linkonce.r.*)
+		_etext = ABSOLUTE(.);
+	} > flash
+
+	/*
+	 * Init functions (static constructors and the like)
+	 */
+        .init_section : {
+                _sinit = ABSOLUTE(.);
+                KEEP(*(.init_array .init_array.*))
+                _einit = ABSOLUTE(.);
+        } > flash
+
+	.ARM.extab : {
+		*(.ARM.extab*)
+	} > flash
+
+	__exidx_start = ABSOLUTE(.);
+	.ARM.exidx : {
+		*(.ARM.exidx*)
+	} > flash
+	__exidx_end = ABSOLUTE(.);
+
+	_eronly = ABSOLUTE(.);
+
+	.data : {
+		_sdata = ABSOLUTE(.);
+		*(.data .data.*)
+		*(.gnu.linkonce.d.*)
+		CONSTRUCTORS
+		_edata = ABSOLUTE(.);
+	} > sram AT > flash
+
+	.bss : {
+		_sbss = ABSOLUTE(.);
+		*(.bss .bss.*)
+		*(.gnu.linkonce.b.*)
+		*(COMMON)
+		. = ALIGN(4);
+		_ebss = ABSOLUTE(.);
+	} > sram
+
+	/* Stabs debugging sections. */
+	.stab 0 : { *(.stab) }
+	.stabstr 0 : { *(.stabstr) }
+	.stab.excl 0 : { *(.stab.excl) }
+	.stab.exclstr 0 : { *(.stab.exclstr) }
+	.stab.index 0 : { *(.stab.index) }
+	.stab.indexstr 0 : { *(.stab.indexstr) }
+	.comment 0 : { *(.comment) }
+	.debug_abbrev 0 : { *(.debug_abbrev) }
+	.debug_info 0 : { *(.debug_info) }
+	.debug_line 0 : { *(.debug_line) }
+	.debug_pubnames 0 : { *(.debug_pubnames) }
+	.debug_aranges 0 : { *(.debug_aranges) }
+}

boards/ark/cannode/nuttx-config/scripts/script.ld

@@ -0,0 +1,146 @@
+/****************************************************************************
+ * scripts/ld.script
+ *
+ *   Copyright (C) 2011 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 STM32F412CG has 1Mb of FLASH beginning at address 0x0800:0000 and
+ * 256Kb of SRAM. SRAM is split up into three blocks:
+ *
+ * 1) 112Kb of SRAM beginning at address 0x2000:0000
+ * 2)  16Kb of SRAM beginning at address 0x2001:c000
+ * 3)  64Kb of SRAM beginning at address 0x2002:0000
+ * 4)  64Kb of TCM SRAM beginning at address 0x1000:0000
+ *
+ * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000
+ * where the code expects to begin execution by jumping to the entry point in
+ * the 0x0800:0000 address range.
+ *
+ * The first 0x10000 of flash is reserved for the bootloader.
+ */
+
+MEMORY
+{
+    flash (rx)   : ORIGIN = 0x08010000, LENGTH = 928K
+    sram (rwx)   : ORIGIN = 0x20000000, LENGTH = 192K
+}
+
+OUTPUT_ARCH(arm)
+
+ENTRY(__start)		/* treat __start as the anchor for dead code stripping */
+EXTERN(_vectors)	/* force the vectors to be included in the output */
+
+/*
+ * Ensure that abort() is present in the final object.  The exception handling
+ * code pulled in by libgcc.a requires it (and that code cannot be easily avoided).
+ */
+EXTERN(abort)
+EXTERN(_bootdelay_signature)
+
+SECTIONS
+{
+	.text : {
+		_stext = ABSOLUTE(.);
+		*(.vectors)
+		. = ALIGN(8);
+		/*
+		* This section positions the app_descriptor_t used
+		* by the make_can_boot_descriptor.py tool to set
+		* the application image's descriptor so that the
+		* uavcan bootloader has the ability to validate the
+		* image crc, size etc
+		*/
+		KEEP(*(.app_descriptor))
+		*(.text .text.*)
+		*(.fixup)
+		*(.gnu.warning)
+		*(.rodata .rodata.*)
+		*(.gnu.linkonce.t.*)
+		*(.got)
+		*(.gcc_except_table)
+		*(.gnu.linkonce.r.*)
+		_etext = ABSOLUTE(.);
+	} > flash
+
+	/*
+	 * Init functions (static constructors and the like)
+	 */
+        .init_section : {
+                _sinit = ABSOLUTE(.);
+                KEEP(*(.init_array .init_array.*))
+                _einit = ABSOLUTE(.);
+        } > flash
+
+
+	.ARM.extab : {
+		*(.ARM.extab*)
+	} > flash
+
+	__exidx_start = ABSOLUTE(.);
+	.ARM.exidx : {
+		*(.ARM.exidx*)
+	} > flash
+	__exidx_end = ABSOLUTE(.);
+
+	_eronly = ABSOLUTE(.);
+
+	.data : {
+		_sdata = ABSOLUTE(.);
+		*(.data .data.*)
+		*(.gnu.linkonce.d.*)
+		CONSTRUCTORS
+		_edata = ABSOLUTE(.);
+	} > sram AT > flash
+
+	.bss : {
+		_sbss = ABSOLUTE(.);
+		*(.bss .bss.*)
+		*(.gnu.linkonce.b.*)
+		*(COMMON)
+		. = ALIGN(4);
+		_ebss = ABSOLUTE(.);
+	} > sram
+
+	/* Stabs debugging sections. */
+	.stab 0 : { *(.stab) }
+	.stabstr 0 : { *(.stabstr) }
+	.stab.excl 0 : { *(.stab.excl) }
+	.stab.exclstr 0 : { *(.stab.exclstr) }
+	.stab.index 0 : { *(.stab.index) }
+	.stab.indexstr 0 : { *(.stab.indexstr) }
+	.comment 0 : { *(.comment) }
+	.debug_abbrev 0 : { *(.debug_abbrev) }
+	.debug_info 0 : { *(.debug_info) }
+	.debug_line 0 : { *(.debug_line) }
+	.debug_pubnames 0 : { *(.debug_pubnames) }
+	.debug_aranges 0 : { *(.debug_aranges) }
+}

boards/ark/cannode/src/CMakeLists.txt

@@ -0,0 +1,68 @@
+############################################################################
+#
+#   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+if("${PX4_BOARD_LABEL}" STREQUAL  "canbootloader")
+
+	add_library(drivers_board
+		boot_config.h
+		boot.c
+		led.c
+		led.h
+	)
+	target_link_libraries(drivers_board
+		PRIVATE
+			nuttx_arch
+			nuttx_drivers
+			canbootloader
+	)
+	target_include_directories(drivers_board PRIVATE ${PX4_SOURCE_DIR}/platforms/nuttx/src/canbootloader)
+
+else()
+	add_library(drivers_board
+		can.c
+		i2c.cpp
+		init.c
+		led.c
+		spi.cpp
+		timer_config.cpp
+	)
+
+	target_link_libraries(drivers_board
+		PRIVATE
+			arch_spi
+			drivers__led # drv_led_start
+			nuttx_arch
+			nuttx_drivers
+			px4_layer
+	)
+endif()

boards/ark/cannode/src/board_config.h

@@ -0,0 +1,114 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file board_config.h
+ *
+ * board internal definitions
+ */
+
+#pragma once
+
+#include <px4_platform_common/px4_config.h>
+#include <nuttx/compiler.h>
+#include <stdint.h>
+
+/* CAN  Silent mode control */
+#define GPIO_CAN1_SILENT_S0   /* PC14  */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN14)
+
+/* CAN termination software control */
+#define GPIO_CAN1_TERMINATION /* PC15 */  (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN15)
+#define GPIO_CAN_TERM                    GPIO_CAN1_TERMINATION
+
+/* Boot config */
+#define GPIO_BOOT_CONFIG      /* PH1 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTH|GPIO_PIN1|GPIO_EXTI)
+
+/* ICM42688p FSYNC */
+#define GPIO_42688P_FSYNC     /* PB8  */  (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN8)
+
+/* LEDs are driven with open drain to support Anode to 5V or 3.3V */
+#define GPIO_TIM1_CH1         /* PA8  */  (GPIO_TIM1_CH1_1|GPIO_OPENDRAIN|GPIO_SPEED_2MHz)
+#define GPIO_TIM1_CH2         /* PA9  */  (GPIO_TIM1_CH2_1|GPIO_OPENDRAIN|GPIO_SPEED_2MHz)
+#define GPIO_TIM1_CH3         /* PA10 */  (GPIO_TIM1_CH3_1|GPIO_OPENDRAIN|GPIO_SPEED_2MHz)
+
+/* PWM Outputs */
+#define DIRECT_PWM_OUTPUT_CHANNELS  6 // Actually 8
+
+#define GPIO_TIM2_CH1_RESET   /* PA0  */  (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN0)
+#define GPIO_TIM2_CH2_RESET   /* PA1  */  (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN1)
+#define GPIO_TIM2_CH3_RESET   /* PB10 */  (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10)
+#define GPIO_TIM3_CH1_RESET   /* PB4  */  (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN4)
+#define GPIO_TIM3_CH2_RESET   /* PB5  */  (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN5)
+#define GPIO_TIM3_CH3_RESET   /* PB0  */  (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN0)
+#define GPIO_TIM3_CH4_RESET   /* PB1  */  (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN1)
+#define GPIO_TIM4_CH4_RESET   /* PB7  */  (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN7)
+
+#define GPIO_I2C1_SCL_RESET   /* PB6  */  (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN6)
+#define GPIO_I2C1_SDA_RESET   /* PB9  */  (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN9)
+
+#define GPIO_USART1_RX_GPIO   /* PB3  */  (GPIO_INPUT|GPIO_PULLUP|GPIO_SPEED_50MHz|GPIO_PORTB|GPIO_PIN3)
+#define GPIO_USART1_TX_GPIO   /* PA15  */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_PULLUP|GPIO_SPEED_50MHz|GPIO_PORTA|GPIO_PIN15)
+
+#define GPIO_USART2_RX_GPIO   /* PA3  */  (GPIO_INPUT|GPIO_PULLUP|GPIO_SPEED_50MHz|GPIO_PORTA|GPIO_PIN3)
+#define GPIO_USART2_TX_GPIO   /* PA2  */  (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_PULLUP|GPIO_SPEED_50MHz|GPIO_PORTA|GPIO_PIN2)
+
+#define FLASH_BASED_PARAMS
+
+/* High-resolution timer */
+#define HRT_TIMER                    8  /* use timer 8 for the HRT */
+#define HRT_TIMER_CHANNEL            1  /* use capture/compare channel 1 */
+
+#define PX4_GPIO_INIT_LIST { \
+		GPIO_CAN1_SILENT_S0,              \
+		GPIO_CAN1_TERMINATION,            \
+		GPIO_42688P_FSYNC,                \
+		GPIO_CAN1_TX,                     \
+		GPIO_CAN1_RX,                     \
+		GPIO_I2C1_SCL_RESET,              \
+		GPIO_I2C1_SDA_RESET,              \
+	}
+
+__BEGIN_DECLS
+
+#define BOARD_HAS_N_S_RGB_LED       1
+#define BOARD_MAX_LEDS              BOARD_HAS_N_S_RGB_LED
+
+#ifndef __ASSEMBLY__
+
+extern void stm32_spiinitialize(void);
+
+#include <px4_platform_common/board_common.h>
+
+#endif /* __ASSEMBLY__ */
+
+__END_DECLS

boards/ark/cannode/src/boot.c

@@ -0,0 +1,188 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *       Author: Ben Dyer <ben_dyer@mac.com>
+ *               Pavel Kirienko <pavel.kirienko@zubax.com>
+ *               David Sidrane <david_s5@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 PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <px4_config.h>
+#include <stdint.h>
+#include "boot_config.h"
+#include "board.h"
+
+#include <debug.h>
+#include <string.h>
+#include <arch/board/board.h>
+
+#include <nuttx/board.h>
+#include "led.h"
+
+/************************************************************************************
+ * Name: stm32_boardinitialize
+ *
+ * Description:
+ *   All STM32 architectures must provide the following entry point.  This entry point
+ *   is called early in the initialization -- after all memory has been configured
+ *   and mapped but before any devices have been initialized.
+ *
+ ************************************************************************************/
+
+__EXPORT void stm32_boardinitialize(void)
+{
+	putreg32(getreg32(STM32_RCC_APB1ENR) | RCC_APB1ENR_CAN1EN, STM32_RCC_APB1ENR);
+	stm32_configgpio(GPIO_CAN1_RX);
+	stm32_configgpio(GPIO_CAN1_TX);
+	stm32_configgpio(GPIO_CAN1_SILENT_S0);
+	stm32_configgpio(GPIO_CAN1_TERMINATION);
+	putreg32(getreg32(STM32_RCC_APB1RSTR) | RCC_APB1RSTR_CAN1RST, STM32_RCC_APB1RSTR);
+	putreg32(getreg32(STM32_RCC_APB1RSTR) & ~RCC_APB1RSTR_CAN1RST, STM32_RCC_APB1RSTR);
+
+#if defined(OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO)
+	stm32_configgpio(GPIO_GETNODEINFO_JUMPER);
+#endif
+
+}
+
+/************************************************************************************
+ * Name: board_deinitialize
+ *
+ * Description:
+ *   This function is called by the bootloader code prior to booting
+ *   the application. Is should place the HW into an benign initialized state.
+ *
+ ************************************************************************************/
+
+void board_deinitialize(void)
+{
+	putreg32(getreg32(STM32_RCC_APB1RSTR) | RCC_APB1RSTR_CAN1RST, STM32_RCC_APB1RSTR);
+}
+
+/****************************************************************************
+ * Name: board_get_product_name
+ *
+ * Description:
+ *   Called to retrieve the product name. The returned value is a assumed
+ *   to be written to a pascal style string that will be length prefixed
+ *   and not null terminated
+ *
+ * Input Parameters:
+ *    product_name - A pointer to a buffer to write the name.
+ *    maxlen       - The maximum number of charter that can be written
+ *
+ * Returned Value:
+ *   The length of characters written to the buffer.
+ *
+ ****************************************************************************/
+
+uint8_t board_get_product_name(uint8_t *product_name, size_t maxlen)
+{
+	DEBUGASSERT(maxlen > UAVCAN_STRLEN(HW_UAVCAN_NAME));
+	memcpy(product_name, HW_UAVCAN_NAME, UAVCAN_STRLEN(HW_UAVCAN_NAME));
+	return UAVCAN_STRLEN(HW_UAVCAN_NAME);
+}
+
+/****************************************************************************
+ * Name: board_get_hardware_version
+ *
+ * Description:
+ *   Called to retrieve the hardware version information. The function
+ *   will first initialize the the callers struct to all zeros.
+ *
+ * Input Parameters:
+ *    hw_version - A pointer to a uavcan_hardwareversion_t.
+ *
+ * Returned Value:
+ *   Length of the unique_id
+ *
+ ****************************************************************************/
+
+size_t board_get_hardware_version(uavcan_HardwareVersion_t *hw_version)
+{
+	memset(hw_version, 0, sizeof(uavcan_HardwareVersion_t));
+
+	hw_version->major = HW_VERSION_MAJOR;
+	hw_version->minor = HW_VERSION_MINOR;
+
+	return board_get_mfguid(*(mfguid_t *) hw_version->unique_id);
+}
+
+/****************************************************************************
+ * Name: board_indicate
+ *
+ * Description:
+ *   Provides User feedback to indicate the state of the bootloader
+ *   on board specific  hardware.
+ *
+ * Input Parameters:
+ *    indication - A member of the uiindication_t
+ *
+ * Returned Value:
+ *   None
+ *
+ ****************************************************************************/
+#define led(n, code, r , g , b, h) {.red = (r),.green = (g), .blue = (b),.hz = (h)}
+
+typedef begin_packed_struct struct led_t {
+	uint8_t red;
+	uint8_t green;
+	uint8_t blue;
+	uint8_t hz;
+} end_packed_struct led_t;
+
+static const  led_t i2l[] = {
+
+	led(0, off,                             0,     0,     0,     0),
+	led(1, reset,                          128,   128,   128,   30),
+	led(2, autobaud_start,                  0,   128,     0,     1),
+	led(3, autobaud_end,                    0,   128,     0,     2),
+	led(4, allocation_start,                0,     0,    64,     2),
+	led(5, allocation_end,                  0,   128,    64,     3),
+	led(6, fw_update_start,                32,   128,    64,     3),
+	led(7, fw_update_erase_fail,           32,   128,    32,     3),
+	led(8, fw_update_invalid_response,     64,     0,     0,     1),
+	led(9, fw_update_timeout,              64,     0,     0,     2),
+	led(a, fw_update_invalid_crc,          64,     0,     0,     4),
+	led(b, jump_to_app,                     0,   128,     0,    10),
+
+};
+
+void board_indicate(uiindication_t indication)
+{
+	rgb_led(i2l[indication].red,
+		i2l[indication].green,
+		i2l[indication].blue,
+		i2l[indication].hz);
+}

boards/ark/cannode/src/boot_config.h

@@ -0,0 +1,130 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/*
+ * @file boot_config.h
+ *
+ * bootloader definitions that configures the behavior and options
+ * of the Boot loader
+ * This file is relies on the parent folder's boot_config.h file and defines
+ * different usages of the hardware for bootloading
+ */
+
+#pragma once
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+/* Bring in the board_config.h definitions
+ * todo:make this be pulled in from a targed's build
+ * files in nuttx*/
+
+#include "board_config.h"
+#include "uavcan.h"
+#include <nuttx/compiler.h>
+
+#include <stdint.h>
+
+#include <stm32_flash.h>
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+#define OPT_PREFERRED_NODE_ID ANY_NODE_ID
+
+//todo:wrap OPT_x in in ifdefs for command line definitions
+#define OPT_TBOOT_MS                    5000
+#define OPT_NODE_STATUS_RATE_MS         800
+#define OPT_NODE_INFO_RATE_MS           50
+#define OPT_BL_NUMBER_TIMERS            7
+
+/*
+ *  This Option set is set to 1 ensure a provider of firmware has an
+ *  opportunity update the node's firmware.
+ *  This Option is the default policy and can be overridden by
+ *  a jumper
+ *  When this Policy is set, the node will ignore tboot and
+ *  wait indefinitely for a GetNodeInfo request before booting.
+ *
+ *  OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO_INVERT is used to allow
+ *  the polarity of the jumper to be True Active
+ *
+ *  wait  OPT_WAIT_FOR_GETNODEINFO  OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO
+ *                                                 Jumper
+ *   yes           1                       0         x
+ *   yes           1                       1       Active
+ *   no            1                       1       Not Active
+ *   no            0                       0         X
+ *   yes           0                       1       Active
+ *   no            0                       1       Not Active
+ *
+ */
+#define OPT_WAIT_FOR_GETNODEINFO                    0
+#define OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO        1
+#define OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO_INVERT 0
+
+#define OPT_ENABLE_WD           1
+
+#define OPT_RESTART_TIMEOUT_MS          20000
+
+/* Reserved for the Booloader */
+#define OPT_BOOTLOADER_SIZE_IN_K        (1024*64)
+
+/* Reserved for the application out of the total
+ * system flash minus the BOOTLOADER_SIZE_IN_K
+ */
+#define OPT_APPLICATION_RESERVER_IN_K    0
+
+#define OPT_APPLICATION_IMAGE_OFFSET    OPT_BOOTLOADER_SIZE_IN_K
+#define OPT_APPLICATION_IMAGE_LENGTH    (FLASH_SIZE-(OPT_BOOTLOADER_SIZE_IN_K+OPT_APPLICATION_RESERVER_IN_K))
+
+
+#define FLASH_BASE              STM32_FLASH_BASE
+#define FLASH_SIZE              STM32_FLASH_SIZE
+
+#define APPLICATION_LOAD_ADDRESS (FLASH_BASE + OPT_APPLICATION_IMAGE_OFFSET)
+#define APPLICATION_SIZE (FLASH_SIZE-OPT_APPLICATION_IMAGE_OFFSET)
+#define APPLICATION_LAST_8BIT_ADDRRESS  ((uint8_t *)((APPLICATION_LOAD_ADDRESS+APPLICATION_SIZE)-sizeof(uint8_t)))
+#define APPLICATION_LAST_32BIT_ADDRRESS ((uint32_t *)((APPLICATION_LOAD_ADDRESS+APPLICATION_SIZE)-sizeof(uint32_t)))
+#define APPLICATION_LAST_64BIT_ADDRRESS ((uint64_t *)((APPLICATION_LOAD_ADDRESS+APPLICATION_SIZE)-sizeof(uint64_t)))
+
+/* If this board uses big flash that have large sectors */
+
+#define OPT_USE_YIELD
+
+/* Bootloader Option*****************************************************************
+ *
+ */
+#define GPIO_GETNODEINFO_JUMPER (GPIO_BOOT_CONFIG & ~GPIO_EXTI)

boards/ark/cannode/src/can.c

@@ -0,0 +1,130 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2021 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file can.c
+ *
+ * Board-specific CAN functions.
+ */
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <px4_platform_common/px4_config.h>
+
+#include <errno.h>
+#include <debug.h>
+
+#include <nuttx/can/can.h>
+#include <arch/board/board.h>
+
+#include "chip.h"
+#include "arm_arch.h"
+
+#include "stm32.h"
+#include "stm32_can.h"
+#include "board_config.h"
+
+#ifdef CONFIG_CAN
+
+/************************************************************************************
+ * Pre-processor Definitions
+ ************************************************************************************/
+/* Configuration ********************************************************************/
+
+#if defined(CONFIG_STM32_CAN1) && defined(CONFIG_STM32_CAN2)
+#  warning "Both CAN1 and CAN2 are enabled.  Assuming only CAN1."
+#  undef CONFIG_STM32_CAN2
+#endif
+
+#ifdef CONFIG_STM32_CAN1
+#  define CAN_PORT 1
+#else
+#  define CAN_PORT 2
+#endif
+
+/************************************************************************************
+ * Private Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Public Functions
+ ************************************************************************************/
+int can_devinit(void);
+
+/************************************************************************************
+ * Name: can_devinit
+ *
+ * Description:
+ *   All STM32 architectures must provide the following interface to work with
+ *   examples/can.
+ *
+ ************************************************************************************/
+
+int can_devinit(void)
+{
+	static bool initialized = false;
+	struct can_dev_s *can;
+	int ret;
+
+	/* Check if we have already initialized */
+
+	if (!initialized) {
+		/* Call stm32_caninitialize() to get an instance of the CAN interface */
+
+		can = stm32_caninitialize(CAN_PORT);
+
+		if (can == NULL) {
+			canerr("ERROR:  Failed to get CAN interface\n");
+			return -ENODEV;
+		}
+
+		/* Register the CAN driver at "/dev/can0" */
+
+		ret = can_register("/dev/can0", can);
+
+		if (ret < 0) {
+			canerr("ERROR: can_register failed: %d\n", ret);
+			return ret;
+		}
+
+		/* Now we are initialized */
+
+		initialized = true;
+	}
+
+	return OK;
+}
+
+#endif

boards/ark/cannode/src/i2c.cpp

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

boards/ark/cannode/src/init.c

@@ -0,0 +1,188 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file init.c
+ *
+ * board specific early startup code.  This file implements the
+ * board_app_initialize() function that is called early by nsh during startup.
+ *
+ * Code here is run before the rcS script is invoked; it should start required
+ * subsystems and perform board-specific initialization.
+ */
+
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/tasks.h>
+
+#include <stdbool.h>
+#include <stdio.h>
+#include <string.h>
+#include <debug.h>
+#include <errno.h>
+#include <syslog.h>
+
+#include <nuttx/board.h>
+
+#include <stm32.h>
+#include "board_config.h"
+#include "led.h"
+#include <stm32_uart.h>
+
+#include <arch/board/board.h>
+
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_board_led.h>
+#include <drivers/drv_watchdog.h>
+
+#include <systemlib/px4_macros.h>
+
+#include <px4_platform_common/init.h>
+#include <px4_platform/gpio.h>
+
+#include <drivers/drv_pwm_output.h>
+#include <px4_arch/io_timer.h>
+
+# if defined(FLASH_BASED_PARAMS)
+#  include <parameters/flashparams/flashfs.h>
+#endif
+
+/************************************************************************************
+ * 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)
+{
+	// Configure the GPIO pins to outputs and keep them low.
+	for (int i = 0; i < DIRECT_PWM_OUTPUT_CHANNELS; ++i) {
+		px4_arch_configgpio(io_timer_channel_get_gpio_output(i));
+	}
+
+	/*
+	 * On resets invoked from system (not boot) insure we establish a low
+	 * output state (discharge the pins) on PWM pins before they become inputs.
+	 */
+
+	if (status >= 0) {
+		up_mdelay(400);
+	}
+}
+
+/************************************************************************************
+ * 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)
+{
+	// Reset all PWM to Low outputs.
+	board_on_reset(-1);
+
+	watchdog_init();
+
+	/* configure pins */
+	const uint32_t gpio[] = PX4_GPIO_INIT_LIST;
+	px4_gpio_init(gpio, arraySize(gpio));
+
+	// Configure SPI all interfaces GPIO & enable power.
+	stm32_spiinitialize();
+}
+
+/****************************************************************************
+ * Name: board_app_initialize
+ *
+ * Description:
+ *   Perform application specific initialization.  This function is never
+ *   called directly from application code, but only indirectly via the
+ *   (non-standard) boardctl() interface using the command BOARDIOC_INIT.
+ *
+ * Input Parameters:
+ *   arg - The boardctl() argument is passed to the board_app_initialize()
+ *         implementation without modification.  The argument has no
+ *         meaning to NuttX; the meaning of the argument is a contract
+ *         between the board-specific initalization logic and the the
+ *         matching application logic.  The value cold be such things as a
+ *         mode enumeration value, a set of DIP switch switch settings, a
+ *         pointer to configuration data read from a file or serial FLASH,
+ *         or whatever you would like to do with it.  Every implementation
+ *         should accept zero/NULL as a default configuration.
+ *
+ * Returned Value:
+ *   Zero (OK) is returned on success; a negated errno value is returned on
+ *   any failure to indicate the nature of the failure.
+ *
+ ****************************************************************************/
+
+__EXPORT int board_app_initialize(uintptr_t arg)
+{
+	px4_platform_init();
+
+#if defined(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
+
+#if defined(FLASH_BASED_PARAMS)
+	static sector_descriptor_t params_sector_map[] = {
+		{2, 16 * 1024, 0x08008000},
+		{3, 16 * 1024, 0x0800C000},
+		{0, 0, 0},
+	};
+
+	/* Initialize the flashfs layer to use heap allocated memory */
+	int result = parameter_flashfs_init(params_sector_map, NULL, 0);
+
+	if (result != OK) {
+		syslog(LOG_ERR, "[boot] FAILED to init params in FLASH %d\n", result);
+	}
+
+#endif // FLASH_BASED_PARAMS
+
+	/* Configure the HW based on the manifest */
+
+	px4_platform_configure();
+
+	return OK;
+}

boards/ark/cannode/src/led.c

@@ -0,0 +1,124 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file led.c
+ *
+ * LED backend.
+ */
+
+#include <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>
+
+#include "led.h"
+
+#define TMR_BASE        STM32_TIM1_BASE
+#define TMR_FREQUENCY   STM32_APB2_TIM1_CLKIN
+#define TMR_REG(o)      (TMR_BASE+(o))
+
+void rgb_led(int r, int g, int b, int freqs)
+{
+
+	long fosc = TMR_FREQUENCY;
+	long prescale = 2048;
+	long p1s = fosc / prescale;
+	long p0p5s  = p1s / 2;
+	uint16_t val;
+	static bool once = 0;
+
+	if (!once) {
+		once = 1;
+
+		/* Enabel Clock to Block */
+		modifyreg32(STM32_RCC_APB2ENR, 0, RCC_APB2ENR_TIM1EN);
+
+		/* Reload */
+
+		val = getreg16(TMR_REG(STM32_BTIM_EGR_OFFSET));
+		val |= ATIM_EGR_UG;
+		putreg16(val, TMR_REG(STM32_BTIM_EGR_OFFSET));
+
+		/* Set Prescaler STM32_TIM_SETCLOCK */
+
+		putreg16(prescale, TMR_REG(STM32_BTIM_PSC_OFFSET));
+
+		/* Enable STM32_TIM_SETMODE*/
+
+		putreg16(ATIM_CR1_CEN | ATIM_CR1_ARPE, TMR_REG(STM32_BTIM_CR1_OFFSET));
+
+
+		putreg16((ATIM_CCMR_MODE_PWM1 << ATIM_CCMR1_OC1M_SHIFT) | ATIM_CCMR1_OC1PE |
+			 (ATIM_CCMR_MODE_PWM1 << ATIM_CCMR1_OC2M_SHIFT) | ATIM_CCMR1_OC2PE, TMR_REG(STM32_GTIM_CCMR1_OFFSET));
+		putreg16((ATIM_CCMR_MODE_PWM1 << ATIM_CCMR2_OC3M_SHIFT) | ATIM_CCMR2_OC3PE, TMR_REG(STM32_GTIM_CCMR2_OFFSET));
+		putreg16(ATIM_CCER_CC3E | ATIM_CCER_CC3P |
+			 ATIM_CCER_CC2E | ATIM_CCER_CC2P |
+			 ATIM_CCER_CC1E | ATIM_CCER_CC1P, TMR_REG(STM32_GTIM_CCER_OFFSET));
+
+
+		stm32_configgpio(GPIO_TIM1_CH1);
+		stm32_configgpio(GPIO_TIM1_CH2);
+		stm32_configgpio(GPIO_TIM1_CH3);
+
+		/* master output enable = on */
+		putreg16(ATIM_BDTR_MOE, (TMR_REG(STM32_ATIM_BDTR_OFFSET)));
+	}
+
+	long p  = freqs == 0 ? p1s : p1s / freqs;
+	putreg32(p, TMR_REG(STM32_BTIM_ARR_OFFSET));
+
+	p  = freqs == 0 ? p1s + 1 : p0p5s / freqs;
+
+	putreg32((r * p) / 255, TMR_REG(STM32_GTIM_CCR1_OFFSET));
+	putreg32((g * p) / 255, TMR_REG(STM32_GTIM_CCR2_OFFSET));
+	putreg32((b * p) / 255, TMR_REG(STM32_GTIM_CCR3_OFFSET));
+
+	val = getreg16(TMR_REG(STM32_BTIM_CR1_OFFSET));
+
+	if (freqs == 0) {
+		val &= ~ATIM_CR1_CEN;
+
+	} else {
+		val |= ATIM_CR1_CEN;
+	}
+
+	putreg16(val, TMR_REG(STM32_BTIM_CR1_OFFSET));
+
+}

boards/ark/cannode/src/led.h

@@ -0,0 +1,37 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2015 PX4 Development Team. All rights reserved.
+ *   Author: David Sidrane<david_s5@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 PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+__BEGIN_DECLS
+void rgb_led(int r, int g, int b, int freqs);
+__END_DECLS

boards/ark/cannode/src/spi.cpp

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

boards/ark/cannode/src/timer_config.cpp

@@ -0,0 +1,54 @@
+/****************************************************************************
+ *
+ *   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>
+
+constexpr io_timers_t io_timers[MAX_IO_TIMERS] = {
+	initIOTimer(Timer::Timer2),
+	initIOTimer(Timer::Timer3),
+	//initIOTimer(Timer::Timer4),
+};
+
+constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = {
+	initIOTimerChannel(io_timers, {Timer::Timer2, Timer::Channel1}, {GPIO::PortA, GPIO::Pin0}),
+	initIOTimerChannel(io_timers, {Timer::Timer2, Timer::Channel2}, {GPIO::PortA, GPIO::Pin1}),
+	initIOTimerChannel(io_timers, {Timer::Timer2, Timer::Channel3}, {GPIO::PortB, GPIO::Pin10}),
+	initIOTimerChannel(io_timers, {Timer::Timer3, Timer::Channel1}, {GPIO::PortB, GPIO::Pin4}),
+	initIOTimerChannel(io_timers, {Timer::Timer3, Timer::Channel2}, {GPIO::PortB, GPIO::Pin5}),
+	initIOTimerChannel(io_timers, {Timer::Timer3, Timer::Channel3}, {GPIO::PortB, GPIO::Pin0}),
+	//initIOTimerChannel(io_timers, {Timer::Timer3, Timer::Channel4}, {GPIO::PortB, GPIO::Pin1}),
+	//initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel2}, {GPIO::PortB, GPIO::Pin7}),
+};
+
+constexpr io_timers_channel_mapping_t io_timers_channel_mapping =
+	initIOTimerChannelMapping(io_timers, timer_io_channels);

boards/ark/cannode/uavcan_board_identity

@@ -0,0 +1,17 @@
+# UAVCAN boot loadable Module ID
+set(uavcanblid_sw_version_major 0)
+set(uavcanblid_sw_version_minor 1)
+add_definitions(
+	-DAPP_VERSION_MAJOR=${uavcanblid_sw_version_major}
+	-DAPP_VERSION_MINOR=${uavcanblid_sw_version_minor}
+)
+
+set(uavcanblid_hw_version_major 0)
+set(uavcanblid_hw_version_minor 83)
+set(uavcanblid_name "\"org.ark.cannode\"")
+
+add_definitions(
+	-DHW_UAVCAN_NAME=${uavcanblid_name}
+	-DHW_VERSION_MAJOR=${uavcanblid_hw_version_major}
+	-DHW_VERSION_MINOR=${uavcanblid_hw_version_minor}
+)

boards/cubepilot/io-v2/src/board_config.h

@@ -70,29 +70,19 @@
 
 /* LEDS  **********************************************************************/
 
-#define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN14)
-#define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN15)
-#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN13)
-#define GPIO_LED4 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN11)
+#define GPIO_LED_AMBER  (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN15) // IO-LED_AMBER
+#define GPIO_LED_SAFETY (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN13) // IO-LED_SAFETY
+#define GPIO_LED_GREEN  (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN11) // IO-LED_POWER_BREATHING
 
-#define GPIO_HEATER_OFF (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN14)
+#define LED_AMBER(on_true)  stm32_gpiowrite(GPIO_LED_AMBER, !(on_true))
+#define LED_SAFETY(on_true) stm32_gpiowrite(GPIO_LED_SAFETY, !(on_true))
+#define LED_GREEN(on_true)   stm32_gpiowrite(GPIO_LED_GREEN, (on_true))
 
-#define GPIO_PC14 (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTC|GPIO_PIN14)
-#define GPIO_PC15 (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTC|GPIO_PIN15)
 
-/*  PixHawk 1:
- *      PC14 Floating
- *      PC15 Floating
- *
- *  PixHawk 2:
- *      PC14 3.3v
- *      PC15 GND
- */
+/* HEATER */
+#define GPIO_HEATER_OUTPUT   /* PB14 */ (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN14)
+#define HEATER_OUTPUT_EN(on_true)       stm32_gpiowrite(GPIO_HEATER_OUTPUT, (on_true))
 
-#define GPIO_SENSE_PC14_DN (GPIO_INPUT|GPIO_CNF_INPULLDWN|GPIO_MODE_INPUT|GPIO_PORTC|GPIO_PIN14)
-#define GPIO_SENSE_PC15_UP (GPIO_INPUT|GPIO_CNF_INPULLUP|GPIO_MODE_INPUT|GPIO_PORTC|GPIO_PIN15)
-# define SENSE_PH1 0b10 /* Floating pulled as set */
-# define SENSE_PH2 0b01 /* Driven as tied */
 
 #define GPIO_USART1_RX_SPEKTRUM		(GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN10)
 
@@ -150,18 +140,6 @@
 #define HRT_PPM_CHANNEL		1	/* use capture/compare channel 1 PA8 */
 #define GPIO_PPM_IN		(GPIO_ALT|GPIO_CNF_INPULLUP|GPIO_PORTA|GPIO_PIN8)
 
-/* LED definitions ******************************************************************/
-/* PX4 has two LEDs that we will encode as: */
-
-#define LED_STARTED       0  /* LED? */
-#define LED_HEAPALLOCATE  1  /* LED? */
-#define LED_IRQSENABLED   2  /* LED? + LED? */
-#define LED_STACKCREATED  3  /* LED? */
-#define LED_INIRQ         4  /* LED? + LED? */
-#define LED_SIGNAL        5  /* LED? + LED? */
-#define LED_ASSERTION     6  /* LED? + LED? + LED? */
-#define LED_PANIC         7  /* N/C  + N/C  + N/C + LED? */
-
 #define BOARD_NUM_IO_TIMERS 3
 
 #include <px4_platform_common/board_common.h>

boards/cubepilot/io-v2/src/init.c

@@ -32,7 +32,7 @@
  ****************************************************************************/
 
 /**
- * @file px4iov2_init.c
+ * @file init.c
  *
  * PX4FMU-specific early startup code.  This file implements the
  * stm32_boardinitialize() function that is called during cpu startup.
@@ -84,38 +84,14 @@
 
 __EXPORT void stm32_boardinitialize(void)
 {
-
 	/* configure GPIOs */
 
-	/* Set up for sensing HW */
-
-	stm32_configgpio(GPIO_SENSE_PC14_DN);
-	stm32_configgpio(GPIO_SENSE_PC15_UP);
+	stm32_configgpio(GPIO_HEATER_OUTPUT);
 
 	/* LEDS - default to off */
-	stm32_configgpio(GPIO_LED1);
-	stm32_configgpio(GPIO_LED2);
-	stm32_configgpio(GPIO_LED3);
-	stm32_configgpio(GPIO_LED4);
-
-	/*  PixHawk 1:
-	 *      PC14 Floating
-	 *      PC15 Floating
-	 *
-	 *  PixHawk 2:
-	 *      PC14 3.3v
-	 *      PC15 GND
-	 */
-
-	uint8_t sense = stm32_gpioread(GPIO_SENSE_PC15_UP) << 1  | stm32_gpioread(GPIO_SENSE_PC14_DN);
-
-	if (sense == SENSE_PH2) {
-		stm32_configgpio(GPIO_HEATER_OFF);
-	}
-
-	stm32_configgpio(GPIO_PC14);
-	stm32_configgpio(GPIO_PC15);
-
+	stm32_configgpio(GPIO_LED_AMBER);
+	stm32_configgpio(GPIO_LED_SAFETY);
+	stm32_configgpio(GPIO_LED_GREEN);
 
 	stm32_configgpio(GPIO_BTN_SAFETY);
 
@@ -163,4 +139,7 @@ __EXPORT void stm32_boardinitialize(void)
 
 	stm32_gpiowrite(GPIO_PWM8, true);
 	stm32_configgpio(GPIO_PWM8);
+
+	/* disable heater */
+	HEATER_OUTPUT_EN(false);
 }

boards/matek/gnss-m9n-f4/init/rc.board_sensors

@@ -3,10 +3,10 @@
 # Matek M9NF4 CAN specific board sensors init
 #------------------------------------------------------------------------------
 
-icm20602 -s start
+icm20602 -s -R 10 start
 
-rm3100 -b 2 -s start
+rm3100 -b 2 -s -R 12 start
 
 dps310 -a 118 -X start
 
-sensors start
+sensors start

boards/px4/fmu-v2/multicopter.px4board

@@ -1,6 +1,4 @@
 CONFIG_SYSTEMCMDS_BL_UPDATE=n
 CONFIG_DRIVERS_DISTANCE_SENSOR_LIGHTWARE_LASER_I2C=y
-CONFIG_DRIVERS_DISTANCE_SENSOR_LIGHTWARE_LASER_SERIAL=y
 CONFIG_DRIVERS_DISTANCE_SENSOR_LL40LS=y
-CONFIG_MODULES_LOAD_MON=y
 CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y

boards/px4/fmu-v5/default.px4board

@@ -12,10 +12,10 @@ CONFIG_DRIVERS_CAMERA_CAPTURE=y
 CONFIG_DRIVERS_CAMERA_TRIGGER=y
 CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
 CONFIG_COMMON_DISTANCE_SENSOR=y
-CONFIG_COMMON_HYGROMETERS=y
 CONFIG_DRIVERS_DSHOT=y
 CONFIG_DRIVERS_GPS=y
 CONFIG_DRIVERS_HEATER=y
+CONFIG_COMMON_HYGROMETERS=y
 CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=y
 CONFIG_DRIVERS_IMU_BOSCH_BMI055=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y
@@ -58,6 +58,7 @@ 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
@@ -81,7 +82,6 @@ CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=y
-CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y

boards/px4/fmu-v5/rtps.px4board

@@ -1,4 +1,4 @@
 CONFIG_DRIVERS_HEATER=n
 CONFIG_DRIVERS_OSD=n
-CONFIG_MODULES_MICRORTPS_BRIDGE=y
 CONFIG_MODULES_MICRODDS_CLIENT=y
+CONFIG_MODULES_MICRORTPS_BRIDGE=y

boards/px4/fmu-v5/stackcheck.px4board

@@ -34,6 +34,8 @@ CONFIG_MODULES_SIH=n
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=n
 CONFIG_MODULES_UUV_ATT_CONTROL=n
 CONFIG_MODULES_UUV_POS_CONTROL=n
+CONFIG_SYSTEMCMDS_MOTOR_TEST=n
+CONFIG_SYSTEMCMDS_REFLECT=n
 CONFIG_SYSTEMCMDS_SERIAL_TEST=n
 CONFIG_BOARD_CONSTRAINED_FLASH=y
 CONFIG_BOARD_TESTING=y

boards/px4/fmu-v5/uavcanv0periph.px4board

@@ -24,6 +24,7 @@ CONFIG_SYSTEMCMDS_BL_UPDATE=n
 CONFIG_SYSTEMCMDS_DUMPFILE=n
 CONFIG_SYSTEMCMDS_GPIO=n
 CONFIG_SYSTEMCMDS_REFLECT=n
+CONFIG_SYSTEMCMDS_SD_BENCH=n
 CONFIG_SYSTEMCMDS_SD_STRESS=n
 CONFIG_SYSTEMCMDS_SERIAL_TEST=n
 CONFIG_SYSTEMCMDS_TOPIC_LISTENER=n

boards/px4/fmu-v5x/default.px4board

@@ -60,6 +60,7 @@ 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
@@ -81,7 +82,6 @@ CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
-CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y

boards/px4/fmu-v5x/rtps.px4board

@@ -1,8 +1,12 @@
-CONFIG_EXAMPLES_FAKE_GPS=n
 CONFIG_COMMON_TELEMETRY=n
+CONFIG_DRIVERS_OSD=n
+CONFIG_EXAMPLES_FAKE_GPS=n
+CONFIG_SYSTEMCMDS_ACTUATOR_TEST=n
 CONFIG_SYSTEMCMDS_DUMPFILE=n
+CONFIG_SYSTEMCMDS_MOTOR_TEST=n
 CONFIG_SYSTEMCMDS_REFLECT=n
 CONFIG_SYSTEMCMDS_SD_BENCH=n
 CONFIG_SYSTEMCMDS_SD_STRESS=n
+CONFIG_SYSTEMCMDS_SERIAL_TEST=n
 CONFIG_MODULES_MICRODDS_CLIENT=y
 CONFIG_MODULES_MICRORTPS_BRIDGE=y

boards/px4/fmu-v6x/default.px4board

@@ -87,7 +87,6 @@ CONFIG_SYSTEMCMDS_PARAM=y
 CONFIG_SYSTEMCMDS_PERF=y
 CONFIG_SYSTEMCMDS_PWM=y
 CONFIG_SYSTEMCMDS_REBOOT=y
-CONFIG_SYSTEMCMDS_SD_BENCH=y
 CONFIG_SYSTEMCMDS_SYSTEM_TIME=y
 CONFIG_SYSTEMCMDS_TOP=y
 CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y

boards/px4/io-v2/src/board_config.h

@@ -70,12 +70,10 @@
 
 /* LEDS  **********************************************************************/
 
-#define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN14)
-#define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN15)
-#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN13)
-#define GPIO_LED4 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN11)
-
-#define GPIO_HEATER_OFF (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN14)
+#define GPIO_LED_BLUE   (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN14) // IO-LED_BLUE or IMU_TEMPERATURE_CONTROL on Pixhawk 2.1
+#define GPIO_LED_AMBER  (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN15) // IO-LED_AMBER
+#define GPIO_LED_SAFETY (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN13) // IO-LED_SAFETY
+#define GPIO_LED_GREEN  (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN11) // IO-LED_POWER_BREATHING
 
 #define GPIO_PC14 (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTC|GPIO_PIN14)
 #define GPIO_PC15 (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTC|GPIO_PIN15)
@@ -88,12 +86,24 @@
  *      PC14 3.3v
  *      PC15 GND
  */
-
 #define GPIO_SENSE_PC14_DN (GPIO_INPUT|GPIO_CNF_INPULLDWN|GPIO_MODE_INPUT|GPIO_PORTC|GPIO_PIN14)
 #define GPIO_SENSE_PC15_UP (GPIO_INPUT|GPIO_CNF_INPULLUP|GPIO_MODE_INPUT|GPIO_PORTC|GPIO_PIN15)
 # define SENSE_PH1 0b10 /* Floating pulled as set */
 # define SENSE_PH2 0b01 /* Driven as tied */
 
+#define SENSE_PIXHAWK2() (((stm32_gpioread(GPIO_SENSE_PC15_UP) << 1 | stm32_gpioread(GPIO_SENSE_PC14_DN)) == SENSE_PH2) ? 1 : 0)
+
+#define LED_BLUE(on_true)   (SENSE_PIXHAWK2() ? (void)0 : stm32_gpiowrite(GPIO_LED_BLUE, !(on_true)))
+#define LED_AMBER(on_true)  stm32_gpiowrite(GPIO_LED_AMBER, !(on_true))
+#define LED_SAFETY(on_true) stm32_gpiowrite(GPIO_LED_SAFETY, !(on_true))
+#define LED_GREEN(on_true)   stm32_gpiowrite(GPIO_LED_GREEN, (on_true))
+
+
+/* HEATER */
+#define GPIO_HEATER_OUTPUT   /* PB14 */ (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN14)
+#define HEATER_OUTPUT_EN(on_true)       (SENSE_PIXHAWK2() ? stm32_gpiowrite(GPIO_HEATER_OUTPUT, !(on_true)) : (void)0)
+
+
 #define GPIO_USART1_RX_SPEKTRUM		(GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN10)
 
 /* Safety switch button *******************************************************/
@@ -150,18 +160,6 @@
 #define HRT_PPM_CHANNEL		1	/* use capture/compare channel 1 PA8 */
 #define GPIO_PPM_IN		(GPIO_ALT|GPIO_CNF_INPULLUP|GPIO_PORTA|GPIO_PIN8)
 
-/* LED definitions ******************************************************************/
-/* PX4 has two LEDs that we will encode as: */
-
-#define LED_STARTED       0  /* LED? */
-#define LED_HEAPALLOCATE  1  /* LED? */
-#define LED_IRQSENABLED   2  /* LED? + LED? */
-#define LED_STACKCREATED  3  /* LED? */
-#define LED_INIRQ         4  /* LED? + LED? */
-#define LED_SIGNAL        5  /* LED? + LED? */
-#define LED_ASSERTION     6  /* LED? + LED? + LED? */
-#define LED_PANIC         7  /* N/C  + N/C  + N/C + LED? */
-
 #define BOARD_NUM_IO_TIMERS 3
 
 #include <px4_platform_common/board_common.h>

boards/px4/io-v2/src/init.c

@@ -32,7 +32,7 @@
  ****************************************************************************/
 
 /**
- * @file px4iov2_init.c
+ * @file init.c
  *
  * PX4FMU-specific early startup code.  This file implements the
  * stm32_boardinitialize() function that is called during cpu startup.
@@ -84,39 +84,33 @@
 
 __EXPORT void stm32_boardinitialize(void)
 {
-
 	/* configure GPIOs */
 
 	/* Set up for sensing HW */
-
 	stm32_configgpio(GPIO_SENSE_PC14_DN);
 	stm32_configgpio(GPIO_SENSE_PC15_UP);
 
-	/* LEDS - default to off */
-	stm32_configgpio(GPIO_LED1);
-	stm32_configgpio(GPIO_LED2);
-	stm32_configgpio(GPIO_LED3);
-	stm32_configgpio(GPIO_LED4);
+	/* some boards such as Pixhawk 2.1 made
+	the unfortunate choice to combine the blue led channel with
+	the IMU heater. We need a software hack to fix the hardware hack
+	by allowing to disable the LED / heater.
+	*/
+	if (SENSE_PIXHAWK2()) {
+		stm32_configgpio(GPIO_HEATER_OUTPUT);
 
-	/*  PixHawk 1:
-	 *      PC14 Floating
-	 *      PC15 Floating
-	 *
-	 *  PixHawk 2:
-	 *      PC14 3.3v
-	 *      PC15 GND
-	 */
-
-	uint8_t sense = stm32_gpioread(GPIO_SENSE_PC15_UP) << 1  | stm32_gpioread(GPIO_SENSE_PC14_DN);
-
-	if (sense == SENSE_PH2) {
-		stm32_configgpio(GPIO_HEATER_OFF);
+	} else {
+		stm32_configgpio(GPIO_LED_BLUE);
 	}
 
 	stm32_configgpio(GPIO_PC14);
 	stm32_configgpio(GPIO_PC15);
 
 
+	/* LEDS - default to off */
+	stm32_configgpio(GPIO_LED_AMBER);
+	stm32_configgpio(GPIO_LED_SAFETY);
+	stm32_configgpio(GPIO_LED_GREEN);
+
 	stm32_configgpio(GPIO_BTN_SAFETY);
 
 	/* spektrum power enable is active high - enable it by default */
@@ -163,4 +157,7 @@ __EXPORT void stm32_boardinitialize(void)
 
 	stm32_gpiowrite(GPIO_PWM8, true);
 	stm32_configgpio(GPIO_PWM8);
+
+	/* disable heater */
+	HEATER_OUTPUT_EN(false);
 }

msg/estimator_innovations.msg

@@ -31,6 +31,7 @@ float32[2] drag		# drag specific force innovation (m/sec**2) and innovation vari
 float32 airspeed	# airspeed innovation (m/sec) and innovation variance ((m/sec)**2)
 float32 beta		# synthetic sideslip innovation (rad) and innovation variance (rad**2)
 float32 hagl		# height of ground innovation (m) and innovation variance (m**2)
+float32 hagl_rate	# height of ground rate innovation (m/s) and innovation variance ((m/s)**2)
 
 # The innovation test ratios are scalar values. In case the field is a vector,
 # the test ratio will be put in the first component of the vector.

msg/estimator_status_flags.msg

@@ -35,6 +35,7 @@ bool cs_gps_yaw_fault         # 27 - true when the GNSS heading has been declare
 bool cs_rng_fault             # 28 - true when the range finder has been declared faulty and is no longer being used
 bool cs_inertial_dead_reckoning # 29 - true if we are no longer fusing measurements that constrain horizontal velocity drift
 bool cs_wind_dead_reckoning     # 30 - true if we are navigationg reliant on wind relative measurements
+bool cs_rng_kin_consistent      # 31 - true when the range finder kinematic consistency check is passing
 
 # fault status
 uint32 fault_status_changes   # number of filter fault status (fs) changes

msg/sensor_combined.msg

@@ -18,3 +18,6 @@ uint8 CLIPPING_X = 1
 uint8 CLIPPING_Y = 2
 uint8 CLIPPING_Z = 4
 uint8 accelerometer_clipping            # bitfield indicating if there was any accelerometer clipping (per axis) during the sampling period
+
+uint8 accel_calibration_count  	# Calibration changed counter. Monotonically increases whenever accelermeter calibration changes.
+uint8 gyro_calibration_count   	# Calibration changed counter. Monotonically increases whenever rate gyro calibration changes.

msg/templates/uorb/uORBTopics.cpp.em

@@ -8,7 +8,7 @@
 @#
 @# Context:
 @#  - msgs (List) list of all msg files
-@#  - multi_topics (List) list of all multi-topic names
+@#  - topics (List) list of all topic names
 @###############################################
 /****************************************************************************
  *
@@ -48,17 +48,17 @@
 @{
 msg_names = [mn.replace(".msg", "") for mn in msgs]
 msgs_count = len(msg_names)
-msg_names_all = list(set(msg_names + multi_topics)) # set() filters duplicates
-msg_names_all.sort()
-msgs_count_all = len(msg_names_all)
+topics_all = topics
+topics_all.sort()
+topics_count_all = len(topics_all)
 }@
 @[for msg_name in msg_names]@
 #include <uORB/topics/@(msg_name).h>
 @[end for]
 
 const constexpr struct orb_metadata *const uorb_topics_list[ORB_TOPICS_COUNT] = {
-@[for idx, msg_name in enumerate(msg_names_all, 1)]@
-	ORB_ID(@(msg_name))@[if idx != msgs_count_all], @[end if]
+@[for idx, topic_name in enumerate(topics_all, 1)]@
+	ORB_ID(@(topic_name))@[if idx != topics_count_all], @[end if]
 @[end for]
 };
 

msg/templates/uorb/uORBTopics.hpp.em

@@ -8,7 +8,7 @@
 @#
 @# Context:
 @#  - msgs (List) list of all msg files
-@#  - multi_topics (List) list of all multi-topic names
+@#  - topics (List) list of all topic names
 @###############################################
 /****************************************************************************
  *
@@ -46,9 +46,9 @@
 @{
 msg_names = [mn.replace(".msg", "") for mn in msgs]
 msgs_count = len(msg_names)
-msg_names_all = list(set(msg_names + multi_topics)) # set() filters duplicates
-msg_names_all.sort()
-msgs_count_all = len(msg_names_all)
+topics_all = topics
+topics_all.sort()
+topics_count_all = len(topics_all)
 }@
 
 #pragma once
@@ -57,7 +57,7 @@ msgs_count_all = len(msg_names_all)
 
 #include <uORB/uORB.h>
 
-static constexpr size_t ORB_TOPICS_COUNT{@(msgs_count_all)};
+static constexpr size_t ORB_TOPICS_COUNT{@(topics_count_all)};
 static constexpr size_t orb_topics_count() { return ORB_TOPICS_COUNT; }
 
 /*
@@ -66,7 +66,7 @@ static constexpr size_t orb_topics_count() { return ORB_TOPICS_COUNT; }
 extern const struct orb_metadata *const *orb_get_topics() __EXPORT;
 
 enum class ORB_ID : uint8_t {
-@[for idx, msg_name in enumerate(msg_names_all)]@
+@[for idx, msg_name in enumerate(topics_all)]@
 	@(msg_name) = @(idx),
 @[end for]
 	INVALID

msg/templates/uorb_microcdr/dds_topics.h.em

@@ -7,7 +7,7 @@
 @#
 @# Context:
 @#  - msgs (List) list of all RTPS messages
-@#  - multi_topics (List) list of all multi-topic names
+@#  - topics (List) list of all topic names
 @#  - spec (msggen.MsgSpec) Parsed specification of the .msg file
 @###############################################
 @{
@@ -43,11 +43,11 @@ struct SendTopicsSubs {
 	uORB::Subscription @(topic)_sub{ORB_ID(@(topic))};
 	uxrObjectId @(topic)_data_writer;
 @[    end for]@
-	
+
 	uxrSession* session;
 
 	uint32_t num_payload_sent{};
-	
+
 	bool init(uxrSession* session_, uxrStreamId stream_id, uxrObjectId participant_id);
 	void update(uxrStreamId stream_id);
 };
@@ -140,7 +140,7 @@ struct RcvTopicsPubs {
 	uxrSession* session;
 
 	uint32_t num_payload_received{};
-	
+
 	bool init(uxrSession* session_, uxrStreamId stream_id, uxrStreamId input_stream, uxrObjectId participant_id);
 };
 

msg/templates/uorb_microcdr/microRTPS_client.cpp.em

@@ -7,7 +7,7 @@
 @#
 @# Context:
 @#  - msgs (List) list of all RTPS messages
-@#  - multi_topics (List) list of all multi-topic names
+@#  - topics (List) list of all topic names
 @#  - spec (msggen.MsgSpec) Parsed specification of the .msg file
 @###############################################
 @{

msg/tools/px_generate_uorb_topic_files.py

@@ -98,9 +98,11 @@ class MsgScope:
     RECEIVE = 2
 
 
-def get_multi_topics(filename):
+def get_topics(filename, msg_name):
     """
-    Get TOPICS names from a "# TOPICS" line
+    Get TOPICS names from a "# TOPICS" line. If there are no multi topics defined,
+    set topic name same as the message name, since the user doesn't expect any new
+    custom topic names.
     """
     ofile = open(filename, 'r')
     text = ofile.read()
@@ -111,6 +113,10 @@ def get_multi_topics(filename):
             topic_names_str = topic_names_str.replace(TOPICS_TOKEN, "")
             result.extend(topic_names_str.split(" "))
     ofile.close()
+
+    if len(result) == 0:
+        result.append(msg_name)
+
     return result
 
 
@@ -147,15 +153,16 @@ def generate_output_from_file(format_idx, filename, outputdir, package, template
         print("[ERROR] uORB topic files generator:\n\tgenerate_output_from_file:\t'timestamp' field in " + spec.short_name +
               " msg definition is not of type uint64 but rather of type " + field_name_and_type.get('timestamp') + "!")
         exit(1)
-    topics = get_multi_topics(filename)
+
+    # Get topics used for the message
+    topics = get_topics(filename, spec.short_name)
+
     if includepath:
         search_path = genmsg.command_line.includepath_to_dict(includepath)
     else:
         search_path = {}
     genmsg.msg_loader.load_depends(msg_context, spec, search_path)
     md5sum = genmsg.gentools.compute_md5(msg_context, spec)
-    if len(topics) == 0:
-        topics.append(spec.short_name)
     em_globals = {
         "file_name_in": filename,
         "md5sum": md5sum,
@@ -288,15 +295,16 @@ def get_em_globals(filename_msg, alias, package, includepath, msgs, fastrtps_ver
         package, os.path.basename(filename_msg))
     spec = genmsg.msg_loader.load_msg_from_file(
         msg_context, filename_msg, full_type_name)
-    topics = get_multi_topics(filename_msg)
+
+    # Get topics used for the message
+    topics = get_topics(filename_msg, spec.short_name)
+
     if includepath:
         search_path = genmsg.command_line.includepath_to_dict(includepath)
     else:
         search_path = {}
     genmsg.msg_loader.load_depends(msg_context, spec, search_path)
     md5sum = genmsg.gentools.compute_md5(msg_context, spec)
-    if len(topics) == 0:
-        topics.append(spec.short_name)
     em_globals = {
         "file_name_in": filename_msg,
         "md5sum": md5sum,
@@ -452,24 +460,31 @@ def convert_dir_save(format_idx, inputdir, outputdir, package, templatedir, temp
 def generate_topics_list_file(msgdir, outputdir, template_filename, templatedir):
     # generate cpp file with topics list
     msgs = get_msgs_list(msgdir)
-    multi_topics = []
+    topics = []
     for msg in msgs:
         msg_filename = os.path.join(msgdir, msg)
-        multi_topics.extend(get_multi_topics(msg_filename))
-    tl_globals = {"msgs": msgs, "multi_topics": multi_topics}
+        topics.extend(get_topics(msg_filename, msg))
+    tl_globals = {"msgs": msgs, "topics": topics}
     tl_template_file = os.path.join(templatedir, template_filename)
     tl_out_file = os.path.join(outputdir, template_filename.replace(".em", ""))
     generate_by_template(tl_out_file, tl_template_file, tl_globals)
 
 
 def generate_topics_list_file_from_files(files, outputdir, template_filename, templatedir):
-    # generate cpp file with topics list
-    filenames = [os.path.basename(
-        p) for p in files if os.path.basename(p).endswith(".msg")]
-    multi_topics = []
-    for msg_filename in files:
-        multi_topics.extend(get_multi_topics(msg_filename))
-    tl_globals = {"msgs": filenames, "multi_topics": multi_topics}
+    # Get message file names ending with .msg only
+    msg_filenames = [p for p in files if os.path.basename(p).endswith(".msg")]
+
+    # Get topics used in messages
+    topics = []
+    for msg_filename in msg_filenames:
+        msg_name = os.path.basename(msg_filename).replace('.msg', '')
+        topics.extend(get_topics(msg_filename, msg_name))
+
+    # Get only the message file name for "msgs" component
+    msg_basenames = [os.path.basename(p) for p in msg_filenames]
+
+    # Set the Template dictionary settings
+    tl_globals = {"msgs": msg_basenames, "topics": topics}
     tl_template_file = os.path.join(templatedir, template_filename)
     tl_out_file = os.path.join(outputdir, template_filename.replace(".em", ""))
     generate_by_template(tl_out_file, tl_template_file, tl_globals)
@@ -529,8 +544,10 @@ if __name__ == "__main__":
             generate_output_from_file(
                 generate_idx, f, args.temporarydir, args.package, args.templatedir, INCL_DEFAULT)
 
+        # Generate topics list header and source file
         if os.path.isfile(os.path.join(args.templatedir, TOPICS_LIST_TEMPLATE_FILE[generate_idx])):
             generate_topics_list_file_from_files(args.file, args.outputdir, TOPICS_LIST_TEMPLATE_FILE[generate_idx], args.templatedir)
+
         copy_changed(args.temporarydir, args.outputdir, args.prefix, args.quiet)
     elif args.dir is not None:
         convert_dir_save(

msg/vehicle_command.msg

@@ -155,6 +155,12 @@ uint8 FAILURE_TYPE_SLOW = 5
 uint8 FAILURE_TYPE_DELAYED = 6
 uint8 FAILURE_TYPE_INTERMITTENT = 7
 
+# used as param1 in DO_CHANGE_SPEED command
+uint8 SPEED_TYPE_AIRSPEED = 0
+uint8 SPEED_TYPE_GROUNDSPEED = 1
+uint8 SPEED_TYPE_CLIMB_SPEED = 2
+uint8 SPEED_TYPE_DESCEND_SPEED = 3
+
 # used as param1 in ARM_DISARM command
 int8 ARMING_ACTION_DISARM = 0
 int8 ARMING_ACTION_ARM = 1

msg/vehicle_imu_status.msg

@@ -14,9 +14,9 @@ float32 gyro_rate_hz
 float32 accel_raw_rate_hz       # full raw sensor sample rate (Hz)
 float32 gyro_raw_rate_hz        # full raw sensor sample rate (Hz)
 
-float32 accel_vibration_metric  # high frequency vibration level in the IMU delta velocity data (m/s)
-float32 gyro_vibration_metric   # high frequency vibration level in the IMU delta velocity data (m/s)
-float32 gyro_coning_vibration   # Level of coning vibration in the IMU delta angles (rad^2)
+float32 accel_vibration_metric  # high frequency vibration level in the accelerometer data (m/s/s)
+float32 gyro_vibration_metric   # high frequency vibration level in the gyro data (rad/s)
+float32 delta_angle_coning_metric # average IMU delta angle coning correction (rad^2)
 
 float32[3] mean_accel           # average accelerometer readings since last publication
 float32[3] mean_gyro            # average gyroscope readings since last publication

platforms/common/work_queue/hrt_queue.c

@@ -123,7 +123,7 @@ int hrt_work_queue(struct work_s *work, worker_t worker, void *arg, uint32_t del
 	work->qtime  = hrt_absolute_time(); /* Time work queued */
 	//PX4_INFO("hrt work_queue adding work delay=%u time=%lu", delay, work->qtime);
 
-	dq_addlast((dq_entry_t *)work, &wqueue->q);
+	dq_addlast(&work->dq, &wqueue->q);
 
 	if (px4_getpid() != wqueue->pid) { /* only need to wake up if called from a different thread */
 #ifdef __PX4_QURT

platforms/common/work_queue/hrt_thread.c

@@ -155,7 +155,7 @@ static void hrt_work_process()
 		if (elapsed >= work->delay) {
 			/* Remove the ready-to-execute work from the list */
 
-			(void)dq_rem((struct dq_entry_s *) & (work->dq), &(wqueue->q));
+			(void)dq_rem((dq_entry_t *)&work->dq, &wqueue->q);
 			//PX4_INFO("Dequeued work=%p", work);
 
 			/* Extract the work description from the entry (in case the work

platforms/common/work_queue/hrt_work_cancel.c

@@ -103,7 +103,7 @@ void hrt_work_cancel(struct work_s *work)
 		 * mark as availalbe (i.e., the worker field is nullified).
 		 */
 
-		dq_rem((dq_entry_t *)work, &wqueue->q);
+		dq_rem(&work->dq, &wqueue->q);
 		work->worker = NULL;
 	}
 

platforms/posix/src/px4/common/drv_hrt.cpp

@@ -95,7 +95,8 @@ hrt_abstime hrt_absolute_time_offset()
 
 static void hrt_lock()
 {
-	px4_sem_wait(&_hrt_lock);
+	// loop as the wait may be interrupted by a signal
+	do {} while (px4_sem_wait(&_hrt_lock) != 0);
 }
 
 static void hrt_unlock()
@@ -333,7 +334,7 @@ hrt_call_reschedule()
 	// Remove the existing expiry and update with the new expiry
 	hrt_work_cancel(&_hrt_work);
 
-	hrt_work_queue(&_hrt_work, (worker_t)&hrt_tim_isr, nullptr, delay);
+	hrt_work_queue(&_hrt_work, &hrt_tim_isr, nullptr, delay);
 }
 
 static void

src/drivers/drv_sensor.h

@@ -130,12 +130,14 @@
 #define DRV_GYR_DEVTYPE_BMI088		0x66
 #define DRV_BARO_DEVTYPE_BMP388	0x67
 #define DRV_BARO_DEVTYPE_DPS310	0x68
+#define DRV_PWM_DEVTYPE_PCA9685     0x69
 #define DRV_ACC_DEVTYPE_BMI088		0x6a
 #define DRV_OSD_DEVTYPE_ATXXXX		0x6b
 #define DRV_FLOW_DEVTYPE_PMW3901	0x6c
 #define DRV_FLOW_DEVTYPE_PAW3902	0x6d
 #define DRV_FLOW_DEVTYPE_PX4FLOW	0x6e
-#define DRV_PWM_DEVTYPE_PCA9685     0x6f
+#define DRV_FLOW_DEVTYPE_PAA3905	0x6f
+
 
 #define DRV_DIST_DEVTYPE_LL40LS       0x70
 #define DRV_DIST_DEVTYPE_MAPPYDOT     0x71

src/drivers/dshot/DShot.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2019-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019-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
@@ -35,6 +35,8 @@
 
 #include <px4_arch/io_timer.h>
 
+#include <px4_platform_common/sem.hpp>
+
 char DShot::_telemetry_device[] {};
 px4::atomic_bool DShot::_request_telemetry_init{false};
 
@@ -487,6 +489,8 @@ bool DShot::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
 
 void DShot::Run()
 {
+	SmartLock lock_guard(_lock);
+
 	if (should_exit()) {
 		ScheduleClear();
 		_mixing_output.unregister();
@@ -640,21 +644,21 @@ void DShot::update_params()
 
 int DShot::ioctl(file *filp, int cmd, unsigned long arg)
 {
+	SmartLock lock_guard(_lock);
+
 	int ret = OK;
 
 	PX4_DEBUG("dshot ioctl cmd: %d, arg: %ld", cmd, arg);
 
-	lock();
-
 	switch (cmd) {
 	case MIXERIOCRESET:
-		_mixing_output.resetMixerThreadSafe();
+		_mixing_output.resetMixer();
 		break;
 
 	case MIXERIOCLOADBUF: {
 			const char *buf = (const char *)arg;
 			unsigned buflen = strlen(buf);
-			ret = _mixing_output.loadMixerThreadSafe(buf, buflen);
+			ret = _mixing_output.loadMixer(buf, buflen);
 
 			break;
 		}
@@ -664,8 +668,6 @@ int DShot::ioctl(file *filp, int cmd, unsigned long arg)
 		break;
 	}
 
-	unlock();
-
 	// if nobody wants it, let CDev have it
 	if (ret == -ENOTTY) {
 		ret = CDev::ioctl(filp, cmd, arg);

src/drivers/linux_pwm_out/linux_pwm_out.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2021-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -36,6 +36,8 @@
 #include <board_pwm_out.h>
 #include <drivers/drv_hrt.h>
 
+#include <px4_platform_common/sem.hpp>
+
 using namespace pwm_out;
 
 LinuxPWMOut::LinuxPWMOut() :
@@ -124,6 +126,8 @@ bool LinuxPWMOut::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS
 
 void LinuxPWMOut::Run()
 {
+	SmartLock lock_guard(_lock);
+
 	if (should_exit()) {
 		ScheduleClear();
 		_mixing_output.unregister();
@@ -301,21 +305,21 @@ void LinuxPWMOut::update_params()
 
 int LinuxPWMOut::ioctl(device::file_t *filp, int cmd, unsigned long arg)
 {
+	SmartLock lock_guard(_lock);
+
 	int ret = OK;
 
 	PX4_DEBUG("ioctl cmd: %d, arg: %ld", cmd, arg);
 
-	lock();
-
 	switch (cmd) {
 	case MIXERIOCRESET:
-		_mixing_output.resetMixerThreadSafe();
+		_mixing_output.resetMixer();
 		break;
 
 	case MIXERIOCLOADBUF: {
 			const char *buf = (const char *)arg;
 			unsigned buflen = strlen(buf);
-			ret = _mixing_output.loadMixerThreadSafe(buf, buflen);
+			ret = _mixing_output.loadMixer(buf, buflen);
 			update_params();
 			break;
 		}
@@ -325,8 +329,6 @@ int LinuxPWMOut::ioctl(device::file_t *filp, int cmd, unsigned long arg)
 		break;
 	}
 
-	unlock();
-
 	if (ret == -ENOTTY) {
 		ret = CDev::ioctl(filp, cmd, arg);
 	}

src/drivers/optical_flow/Kconfig

@@ -2,6 +2,7 @@ menu "Optical flow"
     menuconfig COMMON_OPTICAL_FLOW
         bool "Common Optical flow"
         default n
+        select DRIVERS_OPTICAL_FLOW_PAA3905
         select DRIVERS_OPTICAL_FLOW_PAW3902
         select DRIVERS_OPTICAL_FLOW_PMW3901
         select DRIVERS_OPTICAL_FLOW_PX4FLOW

src/drivers/optical_flow/paa3905/CMakeLists.txt

@@ -0,0 +1,46 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+px4_add_module(
+	MODULE drivers__optical_flow__paa3905
+	MAIN paa3905
+	SRCS
+		paa3905_main.cpp
+		PAA3905.cpp
+		PAA3905.hpp
+		PixArt_PAA3905_Registers.hpp
+	DEPENDS
+		conversion
+		drivers__device
+		px4_work_queue
+	)

src/drivers/optical_flow/paa3905/Kconfig

@@ -0,0 +1,5 @@
+menuconfig DRIVERS_OPTICAL_FLOW_PAA3905
+	bool "paa3905"
+	default n
+	---help---
+		Enable support for paa3905

src/drivers/optical_flow/paa3905/PAA3905.cpp

@@ -0,0 +1,609 @@
+/****************************************************************************
+ *
+ *   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 "PAA3905.hpp"
+
+static constexpr int16_t combine(uint8_t msb, uint8_t lsb)
+{
+	return (msb << 8u) | lsb;
+}
+
+PAA3905::PAA3905(const I2CSPIDriverConfig &config) :
+	SPI(config),
+	I2CSPIDriver(config),
+	_drdy_gpio(config.drdy_gpio)
+{
+	float yaw_rotation_degrees = (float)config.custom1;
+
+	if (yaw_rotation_degrees >= 0.f) {
+		PX4_INFO("using yaw rotation %.3f degrees (%.3f radians)",
+			 (double)yaw_rotation_degrees, (double)math::radians(yaw_rotation_degrees));
+
+		_rotation = matrix::Dcmf{matrix::Eulerf{0.f, 0.f, math::radians(yaw_rotation_degrees)}};
+
+	} else {
+		// otherwise use the parameter SENS_FLOW_ROT
+		param_t rot = param_find("SENS_FLOW_ROT");
+		int32_t val = 0;
+
+		if (param_get(rot, &val) == PX4_OK) {
+			_rotation = get_rot_matrix((enum Rotation)val);
+
+		} else {
+			_rotation.identity();
+		}
+	}
+}
+
+PAA3905::~PAA3905()
+{
+	// free perf counters
+	perf_free(_sample_perf);
+	perf_free(_interval_perf);
+	perf_free(_comms_errors);
+	perf_free(_false_motion_perf);
+	perf_free(_register_write_fail_perf);
+}
+
+int PAA3905::init()
+{
+	/* do SPI init (and probe) first */
+	if (SPI::init() != OK) {
+		return PX4_ERROR;
+	}
+
+	Configure();
+
+	_previous_collect_timestamp = hrt_absolute_time();
+
+	return PX4_OK;
+}
+
+int PAA3905::probe()
+{
+	const uint8_t Product_ID = RegisterRead(Register::Product_ID);
+
+	if (Product_ID != PRODUCT_ID) {
+		PX4_ERR("Product_ID: %X", Product_ID);
+		return PX4_ERROR;
+	}
+
+	const uint8_t Revision_ID = RegisterRead(Register::Revision_ID);
+
+	if (Revision_ID != REVISION_ID) {
+		PX4_ERR("Revision_ID: %X", Revision_ID);
+		return PX4_ERROR;
+	}
+
+	const uint8_t Inverse_Product_ID = RegisterRead(Register::Inverse_Product_ID);
+
+	if (Inverse_Product_ID != PRODUCT_ID_INVERSE) {
+		PX4_ERR("Inverse_Product_ID: %X", Inverse_Product_ID);
+		return PX4_ERROR;
+	}
+
+	return PX4_OK;
+}
+
+int PAA3905::DataReadyInterruptCallback(int irq, void *context, void *arg)
+{
+	static_cast<PAA3905 *>(arg)->DataReady();
+	return 0;
+}
+
+void PAA3905::DataReady()
+{
+	ScheduleNow();
+}
+
+bool PAA3905::DataReadyInterruptConfigure()
+{
+	if (_drdy_gpio == 0) {
+		return false;
+	}
+
+	// Setup data ready on falling edge
+	if (px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &DataReadyInterruptCallback, this) == 0) {
+		_data_ready_interrupt_enabled = true;
+		return true;
+	}
+
+	_data_ready_interrupt_enabled = false;
+	return false;
+}
+
+bool PAA3905::DataReadyInterruptDisable()
+{
+	if (_drdy_gpio == 0) {
+		return false;
+	}
+
+	_data_ready_interrupt_enabled = false;
+
+	return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0;
+}
+
+void PAA3905::exit_and_cleanup()
+{
+	DataReadyInterruptDisable();
+	I2CSPIDriverBase::exit_and_cleanup();
+}
+
+void PAA3905::Configure()
+{
+	DataReadyInterruptDisable();
+	ScheduleClear();
+
+	// Issue a soft reset
+	RegisterWrite(Register::Power_Up_Reset, 0x5A);
+	px4_usleep(1000);
+	_last_reset = hrt_absolute_time();
+
+	StandardDetectionSetting();
+	ModeAuto012();
+
+	CheckMode();
+
+	switch (_mode) {
+	case Mode::Bright:
+		_scheduled_interval_us = SAMPLE_INTERVAL_MODE_0;
+		break;
+
+	case Mode::LowLight:
+		_scheduled_interval_us = SAMPLE_INTERVAL_MODE_1;
+		break;
+
+	case Mode::SuperLowLight:
+		_scheduled_interval_us = SAMPLE_INTERVAL_MODE_2;
+		break;
+	}
+
+	EnableLed();
+
+	_discard_reading = 3;
+	_valid_count = 0;
+
+	if (DataReadyInterruptConfigure()) {
+		// backup schedule as a watchdog timeout
+		ScheduleDelayed(_scheduled_interval_us * 2);
+
+	} else {
+		ScheduleOnInterval(_scheduled_interval_us);
+	}
+}
+
+void PAA3905::CheckMode()
+{
+	// Read Register 0x15. Check Bit [7:6] for AMS mode and store it into a variable.
+	const uint8_t Observation = RegisterRead(Register::Observation);
+
+	// Bit [7:6] AMS mode
+	const uint8_t ams_mode = (Observation & (Bit7 | Bit6)) >> 5;
+
+	if (ams_mode == 0x0) {
+		// Mode 0
+		_mode = Mode::SuperLowLight;
+
+	} else if (ams_mode == 0x1) {
+		// Mode 1
+		_mode = Mode::LowLight;
+
+	} else if (ams_mode == 0x2) {
+		// Mode 2
+		_mode = Mode::SuperLowLight;
+	}
+}
+
+void PAA3905::StandardDetectionSetting()
+{
+	RegisterWriteVerified(0x7F, 0x00);
+	RegisterWriteVerified(0x51, 0xFF);
+	RegisterWriteVerified(0x4E, 0x2A);
+	RegisterWriteVerified(0x66, 0x3E);
+	RegisterWriteVerified(0x7F, 0x14);
+	RegisterWriteVerified(0x7E, 0x71);
+	RegisterWriteVerified(0x55, 0x00);
+	RegisterWriteVerified(0x59, 0x00);
+	RegisterWriteVerified(0x6F, 0x2C);
+	RegisterWriteVerified(0x7F, 0x05);
+	RegisterWriteVerified(0x4D, 0xAC);
+	RegisterWriteVerified(0x4E, 0x32);
+	RegisterWriteVerified(0x7F, 0x09);
+	RegisterWriteVerified(0x5C, 0xAF);
+	RegisterWriteVerified(0x5F, 0xAF);
+	RegisterWriteVerified(0x70, 0x08);
+	RegisterWriteVerified(0x71, 0x04);
+	RegisterWriteVerified(0x72, 0x06);
+	RegisterWriteVerified(0x74, 0x3C);
+	RegisterWriteVerified(0x75, 0x28);
+	RegisterWriteVerified(0x76, 0x20);
+	RegisterWriteVerified(0x4E, 0xBF);
+	RegisterWriteVerified(0x7F, 0x03);
+	RegisterWriteVerified(0x64, 0x14);
+	RegisterWriteVerified(0x65, 0x0A);
+	RegisterWriteVerified(0x66, 0x10);
+	RegisterWriteVerified(0x55, 0x3C);
+	RegisterWriteVerified(0x56, 0x28);
+	RegisterWriteVerified(0x57, 0x20);
+	RegisterWriteVerified(0x4A, 0x2D);
+
+	RegisterWriteVerified(0x4B, 0x2D);
+	RegisterWriteVerified(0x4E, 0x4B);
+	RegisterWriteVerified(0x69, 0xFA);
+	RegisterWriteVerified(0x7F, 0x05);
+	RegisterWriteVerified(0x69, 0x1F);
+	RegisterWriteVerified(0x47, 0x1F);
+	RegisterWriteVerified(0x48, 0x0C);
+	RegisterWriteVerified(0x5A, 0x20);
+	RegisterWriteVerified(0x75, 0x0F);
+	RegisterWriteVerified(0x4A, 0x0F);
+	RegisterWriteVerified(0x42, 0x02);
+	RegisterWriteVerified(0x45, 0x03);
+	RegisterWriteVerified(0x65, 0x00);
+	RegisterWriteVerified(0x67, 0x76);
+	RegisterWriteVerified(0x68, 0x76);
+	RegisterWriteVerified(0x6A, 0xC5);
+	RegisterWriteVerified(0x43, 0x00);
+	RegisterWriteVerified(0x7F, 0x06);
+	RegisterWriteVerified(0x4A, 0x18);
+	RegisterWriteVerified(0x4B, 0x0C);
+	RegisterWriteVerified(0x4C, 0x0C);
+	RegisterWriteVerified(0x4D, 0x0C);
+	RegisterWriteVerified(0x46, 0x0A);
+	RegisterWriteVerified(0x59, 0xCD);
+	RegisterWriteVerified(0x7F, 0x0A);
+	RegisterWriteVerified(0x4A, 0x2A);
+	RegisterWriteVerified(0x48, 0x96);
+	RegisterWriteVerified(0x52, 0xB4);
+	RegisterWriteVerified(0x7F, 0x00);
+	RegisterWriteVerified(0x5B, 0xA0);
+}
+
+void PAA3905::EnhancedDetectionMode()
+{
+	RegisterWriteVerified(0x7F, 0x00);
+	RegisterWriteVerified(0x51, 0xFF);
+	RegisterWriteVerified(0x4E, 0x2A);
+	RegisterWriteVerified(0x66, 0x26);
+	RegisterWriteVerified(0x7F, 0x14);
+	RegisterWriteVerified(0x7E, 0x71);
+	RegisterWriteVerified(0x55, 0x00);
+	RegisterWriteVerified(0x59, 0x00);
+	RegisterWriteVerified(0x6F, 0x2C);
+	RegisterWriteVerified(0x7F, 0x05);
+	RegisterWriteVerified(0x4D, 0xAC);
+	RegisterWriteVerified(0x4E, 0x65);
+	RegisterWriteVerified(0x7F, 0x09);
+	RegisterWriteVerified(0x5C, 0xAF);
+	RegisterWriteVerified(0x5F, 0xAF);
+	RegisterWriteVerified(0x70, 0x00);
+	RegisterWriteVerified(0x71, 0x00);
+	RegisterWriteVerified(0x72, 0x00);
+	RegisterWriteVerified(0x74, 0x14);
+	RegisterWriteVerified(0x75, 0x14);
+	RegisterWriteVerified(0x76, 0x06);
+	RegisterWriteVerified(0x4E, 0x8F);
+	RegisterWriteVerified(0x7F, 0x03);
+	RegisterWriteVerified(0x64, 0x00);
+	RegisterWriteVerified(0x65, 0x00);
+	RegisterWriteVerified(0x66, 0x00);
+	RegisterWriteVerified(0x55, 0x14);
+	RegisterWriteVerified(0x56, 0x14);
+	RegisterWriteVerified(0x57, 0x06);
+	RegisterWriteVerified(0x4A, 0x20);
+
+	RegisterWriteVerified(0x4B, 0x20);
+	RegisterWriteVerified(0x4E, 0x32);
+	RegisterWriteVerified(0x69, 0xFE);
+	RegisterWriteVerified(0x7F, 0x05);
+	RegisterWriteVerified(0x69, 0x14);
+	RegisterWriteVerified(0x47, 0x14);
+	RegisterWriteVerified(0x48, 0x1C);
+	RegisterWriteVerified(0x5A, 0x20);
+	RegisterWriteVerified(0x75, 0xE5);
+	RegisterWriteVerified(0x4A, 0x05);
+	RegisterWriteVerified(0x42, 0x04);
+	RegisterWriteVerified(0x45, 0x03);
+	RegisterWriteVerified(0x65, 0x00);
+	RegisterWriteVerified(0x67, 0x50);
+	RegisterWriteVerified(0x68, 0x50);
+	RegisterWriteVerified(0x6A, 0xC5);
+	RegisterWriteVerified(0x43, 0x00);
+	RegisterWriteVerified(0x7F, 0x06);
+	RegisterWriteVerified(0x4A, 0x1E);
+	RegisterWriteVerified(0x4B, 0x1E);
+	RegisterWriteVerified(0x4C, 0x34);
+	RegisterWriteVerified(0x4D, 0x34);
+	RegisterWriteVerified(0x46, 0x32);
+	RegisterWriteVerified(0x59, 0x0D);
+	RegisterWriteVerified(0x7F, 0x0A);
+	RegisterWriteVerified(0x4A, 0x2A);
+	RegisterWriteVerified(0x48, 0x96);
+	RegisterWriteVerified(0x52, 0xB4);
+	RegisterWriteVerified(0x7F, 0x00);
+	RegisterWriteVerified(0x5B, 0xA0);
+}
+
+void PAA3905::ModeAuto012()
+{
+	// Automatic switching between Mode 0, 1 and 2:
+	RegisterWriteVerified(0x7F, 0x08);
+	RegisterWriteVerified(0x68, 0x02);
+	RegisterWriteVerified(0x7F, 0x00);
+}
+
+void PAA3905::EnableLed()
+{
+	// Enable LED_N controls
+	RegisterWriteVerified(0x7F, 0x14);
+	RegisterWriteVerified(0x6F, 0x0C);
+	RegisterWriteVerified(0x7F, 0x00);
+}
+
+uint8_t PAA3905::RegisterRead(uint8_t reg, int retries)
+{
+	for (int i = 0; i < retries; i++) {
+		px4_udelay(TIME_us_TSRAD);
+		uint8_t cmd[2] {reg, 0};
+
+		if (transfer(&cmd[0], &cmd[0], sizeof(cmd)) == 0) {
+			return cmd[1];
+		}
+	}
+
+	perf_count(_comms_errors);
+	return 0;
+}
+
+void PAA3905::RegisterWrite(uint8_t reg, uint8_t data)
+{
+	uint8_t cmd[2];
+	cmd[0] = DIR_WRITE(reg);
+	cmd[1] = data;
+
+	if (transfer(&cmd[0], nullptr, sizeof(cmd)) != 0) {
+		perf_count(_comms_errors);
+	}
+}
+
+bool PAA3905::RegisterWriteVerified(uint8_t reg, uint8_t data, int retries)
+{
+	for (int i = 0; i < retries; i++) {
+		uint8_t cmd[2];
+		cmd[0] = DIR_WRITE(reg);
+		cmd[1] = data;
+		transfer(&cmd[0], nullptr, sizeof(cmd));
+		px4_udelay(TIME_us_TSWW);
+
+		// read back to verify
+		uint8_t data_read = RegisterRead(reg);
+
+		if (data_read == data) {
+			return true;
+		}
+
+		PX4_DEBUG("Register write failed 0x%02hhX: 0x%02hhX (actual value 0x%02hhX)", reg, data, data_read);
+	}
+
+	perf_count(_register_write_fail_perf);
+
+	return false;
+}
+
+void PAA3905::RunImpl()
+{
+	// backup schedule
+	if (_data_ready_interrupt_enabled) {
+		ScheduleDelayed(_scheduled_interval_us * 2);
+	}
+
+	// force reset if there hasn't been valid data for an extended period (sensor could be in a bad state)
+	static constexpr hrt_abstime RESET_TIMEOUT_US = 5_s;
+
+	if ((hrt_elapsed_time(&_last_good_publish) > RESET_TIMEOUT_US) && (hrt_elapsed_time(&_last_reset) > RESET_TIMEOUT_US)) {
+		Configure();
+		return;
+	}
+
+	perf_begin(_sample_perf);
+	perf_count(_interval_perf);
+
+	struct TransferBuffer {
+		uint8_t cmd = Register::Motion_Burst;
+		BURST_TRANSFER data{};
+	} buf{};
+	static_assert(sizeof(buf) == (14 + 1));
+
+	const hrt_abstime timestamp_sample = hrt_absolute_time();
+
+	if (transfer((uint8_t *)&buf, (uint8_t *)&buf, sizeof(buf)) != PX4_OK) {
+		perf_count(_comms_errors);
+		perf_end(_sample_perf);
+		return;
+	}
+
+	perf_end(_sample_perf);
+
+	const uint64_t dt_flow = timestamp_sample - _previous_collect_timestamp;
+
+	// update for next iteration
+	_previous_collect_timestamp = timestamp_sample;
+
+	if (_discard_reading > 0) {
+		_discard_reading--;
+		ResetAccumulatedData();
+		_valid_count = 0;
+		return;
+	}
+
+	CheckMode(); // update _mode variable
+
+	// check SQUAL & Shutter values
+	// To suppress false motion reports, discard Delta X and Delta Y values if the SQUAL and Shutter values meet the condition
+	// Bright Mode,          SQUAL < 0x19, Shutter ≥ 0x00FF80
+	// Low Light Mode,       SQUAL < 0x46, Shutter ≥ 0x00FF80
+	// Super Low Light Mode, SQUAL < 0x55, Shutter ≥ 0x025998
+	const uint32_t shutter = (buf.data.Shutter_Upper << 16) | (buf.data.Shutter_Middle << 8) | buf.data.Shutter_Lower;
+
+	bool data_valid = (buf.data.SQUAL > 0);
+
+	switch (_mode) {
+	case Mode::Bright:
+
+		// quality < 25 (0x19) and shutter >= 0x00FF80
+		if ((buf.data.SQUAL < 0x19) && (shutter >= 0x00FF80)) {
+			// false motion report, discarding
+			perf_count(_false_motion_perf);
+			data_valid = false;
+		}
+
+		break;
+
+	case Mode::LowLight:
+
+		// quality < 70 (0x46) and shutter >= 0x00FF80
+		if ((buf.data.SQUAL < 0x46) && (shutter >= 0x00FF80)) {
+			// false motion report, discarding
+			perf_count(_false_motion_perf);
+			data_valid = false;
+		}
+
+		break;
+
+	case Mode::SuperLowLight:
+
+		// quality < 85 (0x55) and shutter >= 0x025998
+		if ((buf.data.SQUAL < 0x55) && (shutter >= 0x025998)) {
+			// false motion report, discarding
+			perf_count(_false_motion_perf);
+			data_valid = false;
+		}
+
+		break;
+	}
+
+	if (data_valid) {
+		const int16_t delta_x_raw = combine(buf.data.Delta_X_H, buf.data.Delta_X_L);
+		const int16_t delta_y_raw = combine(buf.data.Delta_Y_H, buf.data.Delta_Y_L);
+
+		_flow_dt_sum_usec += dt_flow;
+		_flow_sum_x += delta_x_raw;
+		_flow_sum_y += delta_y_raw;
+		_flow_sample_counter++;
+		_flow_quality_sum += buf.data.SQUAL;
+
+		_valid_count++;
+
+	} else {
+		_valid_count = 0;
+		ResetAccumulatedData();
+		return;
+	}
+
+	// returns if the collect time has not been reached
+	if (_flow_dt_sum_usec < COLLECT_TIME) {
+		return;
+	}
+
+	optical_flow_s report{};
+	report.timestamp = timestamp_sample;
+	//report.device_id = get_device_id();
+
+	float pixel_flow_x_integral = (float)_flow_sum_x / 500.0f;	// proportional factor + convert from pixels to radians
+	float pixel_flow_y_integral = (float)_flow_sum_y / 500.0f;	// proportional factor + convert from pixels to radians
+
+	// rotate measurements in yaw from sensor frame to body frame
+	const matrix::Vector3f pixel_flow_rotated = _rotation * matrix::Vector3f{pixel_flow_x_integral, pixel_flow_y_integral, 0.f};
+	report.pixel_flow_x_integral = pixel_flow_rotated(0);
+	report.pixel_flow_y_integral = pixel_flow_rotated(1);
+
+	report.frame_count_since_last_readout = _flow_sample_counter; // number of frames
+	report.integration_timespan = _flow_dt_sum_usec;              // microseconds
+
+	report.quality = _flow_quality_sum / _flow_sample_counter;
+
+	// No gyro on this board
+	report.gyro_x_rate_integral = NAN;
+	report.gyro_y_rate_integral = NAN;
+	report.gyro_z_rate_integral = NAN;
+
+	// set (conservative) specs according to datasheet
+	report.max_flow_rate = 7.4f;        // Datasheet: 7.4 rad/s
+	report.min_ground_distance = 0.08f; // Datasheet: 80mm
+	report.max_ground_distance = 30.0f; // Datasheet: infinity
+
+
+	switch (_mode) {
+	case Mode::Bright:
+		report.mode = optical_flow_s::MODE_BRIGHT;
+		break;
+
+	case Mode::LowLight:
+		report.mode = optical_flow_s::MODE_LOWLIGHT;
+		break;
+
+	case Mode::SuperLowLight:
+		report.mode = optical_flow_s::MODE_SUPER_LOWLIGHT;
+		break;
+	}
+
+	report.timestamp = hrt_absolute_time();
+	_optical_flow_pub.publish(report);
+
+	if (report.quality > 10) {
+		_last_good_publish = report.timestamp;
+	}
+
+	ResetAccumulatedData();
+}
+
+void PAA3905::ResetAccumulatedData()
+{
+	// reset
+	_flow_dt_sum_usec = 0;
+	_flow_sum_x = 0;
+	_flow_sum_y = 0;
+	_flow_sample_counter = 0;
+	_flow_quality_sum = 0;
+}
+
+void PAA3905::print_status()
+{
+	I2CSPIDriverBase::print_status();
+
+	perf_print_counter(_sample_perf);
+	perf_print_counter(_interval_perf);
+	perf_print_counter(_comms_errors);
+	perf_print_counter(_false_motion_perf);
+	perf_print_counter(_register_write_fail_perf);
+}

src/drivers/optical_flow/paa3905/PAA3905.hpp

@@ -0,0 +1,136 @@
+/****************************************************************************
+ *
+ *   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 paa3905.hpp
+ *
+ * Driver for the Pixart paa3905 optical flow sensors connected via SPI.
+ */
+
+#pragma once
+
+#include "PixArt_PAA3905_Registers.hpp"
+
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/defines.h>
+#include <px4_platform_common/getopt.h>
+#include <px4_platform_common/i2c_spi_buses.h>
+#include <drivers/device/spi.h>
+#include <conversion/rotation.h>
+#include <lib/perf/perf_counter.h>
+#include <lib/parameters/param.h>
+#include <drivers/drv_hrt.h>
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/topics/optical_flow.h>
+
+using namespace time_literals;
+using namespace PixArt_PAA3905;
+
+#define DIR_WRITE(a) ((a) | (1 << 7))
+#define DIR_READ(a) ((a) & 0x7f)
+
+class PAA3905 : public device::SPI, public I2CSPIDriver<PAA3905>
+{
+public:
+	PAA3905(const I2CSPIDriverConfig &config);
+	virtual ~PAA3905();
+
+	static void print_usage();
+
+	int init() override;
+
+	void print_status() override;
+
+	void RunImpl();
+
+private:
+	void exit_and_cleanup() override;
+
+	int probe() override;
+
+	static int DataReadyInterruptCallback(int irq, void *context, void *arg);
+	void DataReady();
+	bool DataReadyInterruptConfigure();
+	bool DataReadyInterruptDisable();
+
+	uint8_t	RegisterRead(uint8_t reg, int retries = 2);
+	void RegisterWrite(uint8_t reg, uint8_t data);
+	bool RegisterWriteVerified(uint8_t reg, uint8_t data, int retries = 1);
+
+	void EnableLed();
+
+	void StandardDetectionSetting();
+	void EnhancedDetectionMode();
+	void ModeAuto012();
+
+	void CheckMode();
+
+	void Configure();
+
+	void ResetAccumulatedData();
+
+	uORB::PublicationMulti<optical_flow_s> _optical_flow_pub{ORB_ID(optical_flow)};
+
+	perf_counter_t	_sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")};
+	perf_counter_t	_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")};
+	perf_counter_t	_comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": com err")};
+	perf_counter_t	_false_motion_perf{perf_alloc(PC_COUNT, MODULE_NAME": false motion report")};
+	perf_counter_t	_register_write_fail_perf{perf_alloc(PC_COUNT, MODULE_NAME": verified register write failed")};
+
+	static constexpr uint64_t COLLECT_TIME{15000}; // 15 milliseconds, optical flow data publish rate
+
+	const spi_drdy_gpio_t _drdy_gpio;
+
+	uint64_t _previous_collect_timestamp{0};
+	uint64_t _flow_dt_sum_usec{0};
+	uint8_t _flow_sample_counter{0};
+	uint16_t _flow_quality_sum{0};
+
+	matrix::Dcmf	_rotation;
+
+	int             _discard_reading{3};
+
+	int		_flow_sum_x{0};
+	int		_flow_sum_y{0};
+
+	Mode		_mode{Mode::LowLight};
+
+	uint32_t _scheduled_interval_us{SAMPLE_INTERVAL_MODE_1};
+
+	int _valid_count{0};
+
+	bool _data_ready_interrupt_enabled{false};
+
+	hrt_abstime _last_good_publish{0};
+	hrt_abstime _last_reset{0};
+};

src/drivers/optical_flow/paa3905/PixArt_PAA3905_Registers.hpp

@@ -0,0 +1,117 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+namespace PixArt_PAA3905
+{
+// TODO: move to a central header
+static constexpr uint8_t Bit0 = (1 << 0);
+static constexpr uint8_t Bit1 = (1 << 1);
+static constexpr uint8_t Bit2 = (1 << 2);
+static constexpr uint8_t Bit3 = (1 << 3);
+static constexpr uint8_t Bit4 = (1 << 4);
+static constexpr uint8_t Bit5 = (1 << 5);
+static constexpr uint8_t Bit6 = (1 << 6);
+static constexpr uint8_t Bit7 = (1 << 7);
+
+static constexpr uint8_t PRODUCT_ID = 0xA2;
+static constexpr uint8_t REVISION_ID = 0x00;
+static constexpr uint8_t PRODUCT_ID_INVERSE = 0x5D;
+
+static constexpr uint32_t SAMPLE_INTERVAL_MODE_0{1000000 / 126};	// 126 fps
+static constexpr uint32_t SAMPLE_INTERVAL_MODE_1{1000000 / 126};	// 126 fps
+static constexpr uint32_t SAMPLE_INTERVAL_MODE_2{1000000 / 50};		// 50 fps
+
+static constexpr uint32_t SPI_SPEED = 2 * 1000 * 1000; // 2MHz SPI serial interface
+
+// Various time delay needed for paa3905
+static constexpr uint32_t TIME_us_TSWW  = 11; // actually 10.5us
+static constexpr uint32_t TIME_us_TSRAD = 2;
+
+enum Register : uint8_t {
+	Product_ID         = 0x00,
+	Revision_ID        = 0x01,
+	Motion             = 0x02,
+	Delta_X_L          = 0x03,
+	Delta_X_H          = 0x04,
+	Delta_Y_L          = 0x05,
+	Delta_Y_H          = 0x06,
+	Squal              = 0x07,
+	RawData_Sum        = 0x08,
+	Maximum_RawData    = 0x09,
+	Minimum_RawData    = 0x0A,
+	Shutter_Lower      = 0x0B,
+	Shutter_Middle     = 0x0C,
+	Shutter_Upper      = 0x0D,
+
+	Observation        = 0x15,
+	Motion_Burst       = 0x16,
+
+	Power_Up_Reset     = 0x3A,
+	Shutdown           = 0x3B,
+
+	Resolution	   = 0x4E,
+
+	Inverse_Product_ID = 0x5F,
+};
+
+// Observation
+enum Observation_Bit : uint8_t {
+	Reset = 0x5A,
+};
+
+enum class Mode {
+	Bright        = 0,
+	LowLight      = 1,
+	SuperLowLight = 2,
+};
+
+struct BURST_TRANSFER {
+	uint8_t Motion;
+	uint8_t Observation;
+	uint8_t Delta_X_L;
+	uint8_t Delta_X_H;
+	uint8_t Delta_Y_L;
+	uint8_t Delta_Y_H;
+	uint8_t Reserved;
+	uint8_t SQUAL;
+	uint8_t RawData_Sum;
+	uint8_t Maximum_RawData;
+	uint8_t Minimum_RawData;
+	uint8_t Shutter_Upper;
+	uint8_t Shutter_Middle;
+	uint8_t Shutter_Lower;
+};
+
+}

src/drivers/optical_flow/paa3905/paa3905_main.cpp

@@ -0,0 +1,84 @@
+/****************************************************************************
+ *
+ *   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 "PAA3905.hpp"
+#include <px4_platform_common/module.h>
+
+void PAA3905::print_usage()
+{
+	PRINT_MODULE_USAGE_NAME("paa3905", "driver");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true);
+	PRINT_MODULE_USAGE_PARAM_INT('Y', 0, 0, 359, "custom yaw rotation (degrees)", true);
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+}
+
+extern "C" __EXPORT int paa3905_main(int argc, char *argv[])
+{
+	int ch = 0;
+	using ThisDriver = PAA3905;
+	BusCLIArguments cli{false, true};
+	cli.custom1 = -1;
+	cli.spi_mode = SPIDEV_MODE0;
+	cli.default_spi_frequency = SPI_SPEED;
+
+	while ((ch = cli.getOpt(argc, argv, "Y:")) != EOF) {
+		switch (ch) {
+		case 'Y':
+			cli.custom1 = atoi(cli.optArg());
+			break;
+		}
+	}
+
+	const char *verb = cli.optArg();
+
+	if (!verb) {
+		ThisDriver::print_usage();
+		return -1;
+	}
+
+	BusInstanceIterator iterator(MODULE_NAME, cli, DRV_FLOW_DEVTYPE_PAA3905);
+
+	if (!strcmp(verb, "start")) {
+		return ThisDriver::module_start(cli, iterator);
+
+	} else if (!strcmp(verb, "stop")) {
+		return ThisDriver::module_stop(iterator);
+
+	} else if (!strcmp(verb, "status")) {
+		return ThisDriver::module_status(iterator);
+	}
+
+	ThisDriver::print_usage();
+	return -1;
+}

src/drivers/optical_flow/paa3905/parameters.c

@@ -0,0 +1,42 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * paa3905 Optical Flow
+ *
+ * @reboot_required true
+ *
+ * @boolean
+ * @group Sensors
+ */
+PARAM_DEFINE_INT32(SENS_EN_PAA3905, 0);

src/drivers/pca9685_pwm_out/PCA9685.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019-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
@@ -35,6 +35,8 @@
 #include <cmath>
 #include "PCA9685.h"
 
+#include <px4_platform_common/sem.hpp>
+
 using namespace drv_pca9685_pwm;
 
 PCA9685::PCA9685(int bus, int addr):
@@ -255,4 +257,4 @@ void PCA9685::triggerRestart()
 		PX4_DEBUG("triggerRestart: i2c::transfer returned %d", ret);
 		return;
 	}
-}
+}

src/drivers/pca9685_pwm_out/main.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2019 PX4 Development Team. All rights reserved.
+ *   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
@@ -49,6 +49,8 @@
 
 #include "PCA9685.h"
 
+#include <px4_platform_common/sem.hpp>
+
 #define PCA9685_DEFAULT_IICBUS  1
 #define PCA9685_DEFAULT_ADDRESS (0x40)
 
@@ -374,6 +376,8 @@ bool PCA9685Wrapper::updateOutputs(bool stop_motors, uint16_t *outputs, unsigned
 
 void PCA9685Wrapper::Run()
 {
+	SmartLock lock_guard(_lock);
+
 	if (should_exit()) {
 		ScheduleClear();
 		_mixing_output.unregister();
@@ -464,21 +468,20 @@ void PCA9685Wrapper::Run()
 
 int PCA9685Wrapper::ioctl(cdev::file_t *filep, int cmd, unsigned long arg)
 {
-	int ret = OK;
+	SmartLock lock_guard(_lock);
 
-	lock();
+	int ret = OK;
 
 	switch (cmd) {
 	case MIXERIOCRESET:
-		_mixing_output.resetMixerThreadSafe();
+		_mixing_output.resetMixer();
 
 		break;
 
 	case MIXERIOCLOADBUF: {
 			const char *buf = (const char *)arg;
 			unsigned buflen = strlen(buf);
-
-			ret = _mixing_output.loadMixerThreadSafe(buf, buflen);
+			ret = _mixing_output.loadMixer(buf, buflen);
 
 			break;
 		}
@@ -501,8 +504,6 @@ int PCA9685Wrapper::ioctl(cdev::file_t *filep, int cmd, unsigned long arg)
 		break;
 	}
 
-	unlock();
-
 	if (ret == -ENOTTY) {
 		ret = CDev::ioctl(filep, cmd, arg);
 	}

src/drivers/power_monitor/ina226/ina226.cpp

@@ -102,14 +102,19 @@ int INA226::read(uint8_t address, int16_t &data)
 {
 	// read desired little-endian value via I2C
 	uint16_t received_bytes;
-	const int ret = transfer(&address, 1, (uint8_t *)&received_bytes, sizeof(received_bytes));
+	int ret = PX4_ERROR;
 
-	if (ret == PX4_OK) {
-		data = swap16(received_bytes);
+	for (size_t i = 0; i < 3; i++) {
+		ret = transfer(&address, 1, (uint8_t *)&received_bytes, sizeof(received_bytes));
 
-	} else {
-		perf_count(_comms_errors);
-		PX4_DEBUG("i2c::transfer returned %d", ret);
+		if (ret == PX4_OK) {
+			data = swap16(received_bytes);
+			break;
+
+		} else {
+			perf_count(_comms_errors);
+			PX4_DEBUG("i2c::transfer returned %d", ret);
+		}
 	}
 
 	return ret;

src/drivers/pwm_out/PWMOut.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2021 PX4 Development Team. All rights reserved.
+ *   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
@@ -33,6 +33,8 @@
 
 #include "PWMOut.hpp"
 
+#include <px4_platform_common/sem.hpp>
+
 pthread_mutex_t pwm_out_module_mutex = PTHREAD_MUTEX_INITIALIZER;
 static px4::atomic<PWMOut *> _objects[PWM_OUT_MAX_INSTANCES] {};
 static px4::atomic_bool _require_arming[PWM_OUT_MAX_INSTANCES] {};
@@ -437,6 +439,8 @@ bool PWMOut::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
 
 void PWMOut::Run()
 {
+	SmartLock lock_guard(_lock);
+
 	if (should_exit()) {
 		ScheduleClear();
 		_mixing_output.unregister();
@@ -688,6 +692,8 @@ void PWMOut::update_params()
 
 int PWMOut::ioctl(device::file_t *filp, int cmd, unsigned long arg)
 {
+	SmartLock lock_guard(_lock);
+
 	int ret = pwm_ioctl(filp, cmd, arg);
 
 	/* if nobody wants it, let CDev have it */
@@ -704,8 +710,6 @@ int PWMOut::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg)
 
 	PX4_DEBUG("pwm_out%u: ioctl cmd: %d, arg: %ld", _instance, cmd, arg);
 
-	lock();
-
 	switch (cmd) {
 	case PWM_SERVO_ARM:
 		update_pwm_out_state(true);
@@ -912,14 +916,14 @@ int PWMOut::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg)
 		break;
 
 	case MIXERIOCRESET:
-		_mixing_output.resetMixerThreadSafe();
+		_mixing_output.resetMixer();
 
 		break;
 
 	case MIXERIOCLOADBUF: {
 			const char *buf = (const char *)arg;
 			unsigned buflen = strlen(buf);
-			ret = _mixing_output.loadMixerThreadSafe(buf, buflen);
+			ret = _mixing_output.loadMixer(buf, buflen);
 			update_params();
 
 			break;
@@ -930,8 +934,6 @@ int PWMOut::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg)
 		break;
 	}
 
-	unlock();
-
 	return ret;
 }
 

src/drivers/pwm_out_sim/PWMSim.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2018 PX4 Development Team. All rights reserved.
+ *   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
@@ -39,6 +39,8 @@
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/parameter_update.h>
 
+#include <px4_platform_common/sem.hpp>
+
 PWMSim::PWMSim(bool hil_mode_enabled) :
 	CDev(PWM_OUTPUT0_DEVICE_PATH),
 	OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default)
@@ -56,6 +58,8 @@ PWMSim::PWMSim(bool hil_mode_enabled) :
 void
 PWMSim::Run()
 {
+	SmartLock lock_guard(_lock);
+
 	if (should_exit()) {
 		ScheduleClear();
 		_mixing_output.unregister();
@@ -120,9 +124,9 @@ PWMSim::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigne
 int
 PWMSim::ioctl(device::file_t *filp, int cmd, unsigned long arg)
 {
-	int ret = OK;
+	SmartLock lock_guard(_lock);
 
-	lock();
+	int ret = OK;
 
 	switch (cmd) {
 	case PWM_SERVO_ARM:
@@ -231,24 +235,21 @@ PWMSim::ioctl(device::file_t *filp, int cmd, unsigned long arg)
 		break;
 
 	case MIXERIOCRESET:
-		_mixing_output.resetMixerThreadSafe();
+		_mixing_output.resetMixer();
 		break;
 
 	case MIXERIOCLOADBUF: {
 			const char *buf = (const char *)arg;
 			unsigned buflen = strlen(buf);
-			ret = _mixing_output.loadMixerThreadSafe(buf, buflen);
+			ret = _mixing_output.loadMixer(buf, buflen);
 			break;
 		}
 
-
 	default:
 		ret = -ENOTTY;
 		break;
 	}
 
-	unlock();
-
 	return ret;
 }
 

src/drivers/px4io/px4io.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2021 PX4 Development Team. All rights reserved.
+ *   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
@@ -202,6 +202,8 @@ private:
 
 	hrt_abstime             _last_status_publish{0};
 
+	uint16_t 		_rc_valid_update_count{0};
+
 	bool			_param_update_force{true};	///< force a parameter update
 	bool			_timer_rates_configured{false};
 
@@ -377,8 +379,6 @@ PX4IO::~PX4IO()
 bool PX4IO::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
 			  unsigned num_outputs, unsigned num_control_groups_updated)
 {
-	SmartLock lock_guard(_lock);
-
 	if (!_test_fmu_fail) {
 		/* output to the servos */
 		io_reg_set(PX4IO_PAGE_DIRECT_PWM, 0, outputs, num_outputs);
@@ -389,6 +389,8 @@ bool PX4IO::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
 
 int PX4IO::init()
 {
+	SmartLock lock_guard(_lock);
+
 	/* do regular cdev init */
 	int ret = CDev::init();
 
@@ -523,6 +525,8 @@ void PX4IO::updateFailsafe()
 
 void PX4IO::Run()
 {
+	SmartLock lock_guard(_lock);
+
 	if (should_exit()) {
 		ScheduleClear();
 		_mixing_output.unregister();
@@ -544,8 +548,6 @@ void PX4IO::Run()
 		_mixing_output.update();
 	}
 
-	SmartLock lock_guard(_lock);
-
 	if (hrt_elapsed_time(&_poll_last) >= 20_ms) {
 		/* run at 50 */
 		_poll_last = hrt_absolute_time();
@@ -1198,6 +1200,14 @@ int PX4IO::io_get_status()
 
 int PX4IO::io_publish_raw_rc()
 {
+	const uint16_t rc_valid_update_count = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_FRAME_COUNT);
+	const bool rc_updated = (rc_valid_update_count != _rc_valid_update_count);
+	_rc_valid_update_count = rc_valid_update_count;
+
+	if (!rc_updated) {
+		return 0;
+	}
+
 	input_rc_s input_rc{};
 	input_rc.timestamp_last_signal = hrt_absolute_time();
 
@@ -1306,8 +1316,7 @@ int PX4IO::io_publish_raw_rc()
 		input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_ST24;
 	}
 
-	if ((input_rc.channel_count > 0) && !input_rc.rc_lost && !input_rc.rc_failsafe
-	    && (input_rc.input_source != input_rc_s::RC_INPUT_SOURCE_UNKNOWN)) {
+	if (input_rc.input_source != input_rc_s::RC_INPUT_SOURCE_UNKNOWN) {
 
 		_to_input_rc.publish(input_rc);
 	}
@@ -1689,7 +1698,7 @@ int PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 
 	case MIXERIOCRESET:
 		PX4_DEBUG("MIXERIOCRESET");
-		_mixing_output.resetMixerThreadSafe();
+		_mixing_output.resetMixer();
 		break;
 
 	case MIXERIOCLOADBUF: {
@@ -1697,7 +1706,7 @@ int PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 
 			const char *buf = (const char *)arg;
 			unsigned buflen = strlen(buf);
-			ret = _mixing_output.loadMixerThreadSafe(buf, buflen);
+			ret = _mixing_output.loadMixer(buf, buflen);
 
 			break;
 		}