cmake: git check submodules updates

Update Tools/gazebo_sitl_multiple_run.sh
Co-authored-by: JaeyoungLim <jalim@ethz.ch>
2026-05-28 05:00:05 +08:00 · 2021-10-01 14:51:23 -04:00 · 2021-10-01 16:09:32 +02:00 · 2021-10-01 16:09:32 +02:00 · 2021-10-01 10:58:25 +02:00 · 2021-09-29 23:55:00 -04:00
181 changed files with 4405 additions and 4602 deletions
@@ -825,7 +825,7 @@ pipeline {
  }
  options {
    buildDiscarder(logRotator(numToKeepStr: '30', artifactDaysToKeepStr: '60'))
-    timeout(time: 120, unit: 'MINUTES')
+    timeout(time: 180, unit: 'MINUTES')
    skipDefaultCheckout()
  }
 }
@@ -899,6 +899,9 @@ void resetParameters() {
 void runTests() {
  resetParameters()

+  sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "IMU_GYRO_CAL_EN" --value "0" || true' // disable during testing
+  sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "IMU_GYRO_FFT_EN" --value "0" || true' // disable during testing
+
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param save"'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "reboot" || true' // reboot to apply

@@ -1047,25 +1050,25 @@ void resetBoard() {
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "reboot" || true' // reboot to apply

  // check SD card
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "df -h"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ls /fs/microsd"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ls /proc/fs"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "cat /proc/fs/blocks"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "cat /proc/fs/mount"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "cat /proc/fs/usage"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "df -h" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ls /fs/microsd" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ls /proc/fs" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "cat /proc/fs/blocks" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "cat /proc/fs/mount" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "cat /proc/fs/usage" || true'
  // format
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "dataman stop"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "umount /fs/microsd"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "top once"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "dataman stop" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "umount /fs/microsd" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "top once" || true'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "mkfatfs -F 32 /dev/mmcsd0" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "top once"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "top once" || true'

-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "mount -t vfat /dev/mmcsd0 /fs/microsd"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "mount -t vfat /dev/mmcsd0 /fs/microsd" || true'
  // check SD card
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "df -h"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ls /fs/microsd"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ls /proc/fs"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "cat /proc/fs/blocks"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "cat /proc/fs/mount"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "cat /proc/fs/usage"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "df -h" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ls /fs/microsd" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ls /proc/fs" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "cat /proc/fs/blocks" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "cat /proc/fs/mount" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "cat /proc/fs/usage" || true'
 }
@@ -66,3 +66,11 @@
 [submodule "Tools/simulation-ignition"]
 	path = Tools/simulation-ignition
 	url = https://github.com/Auterion/px4-simulation-ignition.git
+[submodule "src/lib/crypto/libtomcrypt"]
+	path = src/lib/crypto/libtomcrypt
+	url = https://github.com/PX4/libtomcrypt.git
+	branch = px4
+[submodule "src/lib/crypto/libtommath"]
+	path = src/lib/crypto/libtommath
+	url = https://github.com/PX4/libtommath.git
+	branch = px4
@@ -110,14 +110,22 @@ include(px4_parse_function_args)
 #=============================================================================
 # git
 #
+find_package(Git QUIET)
 include(px4_git)

+# sync submodules
 execute_process(
-	COMMAND git describe --exclude ext/* --always --tags
-	OUTPUT_VARIABLE PX4_GIT_TAG
-	OUTPUT_STRIP_TRAILING_WHITESPACE
-	WORKING_DIRECTORY ${PX4_SOURCE_DIR}
-	)
+        COMMAND ${GIT_EXECUTABLE} submodule sync --recursive --quiet
+        OUTPUT_VARIABLE GIT_SUBMODULE_SYNC
+        WORKING_DIRECTORY ${PX4_SOURCE_DIR}
+)
+
+execute_process(
+        COMMAND ${GIT_EXECUTABLE} describe --exclude ext/* --always --tags
+        OUTPUT_VARIABLE PX4_GIT_TAG
+        OUTPUT_STRIP_TRAILING_WHITESPACE
+        WORKING_DIRECTORY ${PX4_SOURCE_DIR}
+)
 message(STATUS "PX4 version: ${PX4_GIT_TAG}")

 define_property(GLOBAL PROPERTY PX4_MODULE_LIBRARIES
@@ -120,8 +120,6 @@ then

 	param set SENS_BOARD_X_OFF 0.000001
 	param set SENS_DPRES_OFF 0.001
-
-	param set SYS_RESTART_TYPE 2
 fi

 param set-default BAT1_N_CELLS 4
@@ -42,7 +42,6 @@ px4_add_romfs_files(
 	rc.fw_apps
 	rc.fw_defaults
 	rc.interface
-	rc.io
 	rc.logging
 	rc.mc_apps
 	rc.mc_defaults
@@ -56,7 +56,6 @@ param set-default RTL_DESCEND_ALT 10
 param set-default RTL_RETURN_ALT 30

 param set-default SDLOG_DIRS_MAX 7
-param set-default SYS_RESTART_TYPE 2

 param set-default VT_F_TRANS_THR 0.75
 param set-default VT_MOT_ID 1234
@@ -48,7 +48,6 @@ param set-default CBRK_IO_SAFETY    22027
 param set-default CBRK_SUPPLY_CHK   894281

 # RC configuration
-param set-default RC_MAP_MODE_SW    5
 param set-default RC_MAP_PITCH      2
 param set-default RC_MAP_ROLL       1
 param set-default RC_MAP_THROTTLE   3
@@ -17,6 +17,10 @@ set OUTPUT_DEV none
 set OUTPUT_AUX_DEV /dev/pwm_output1
 set OUTPUT_EXTRA_DEV /dev/pwm_output0

+# set these before starting the modules
+param set PWM_AUX_OUT ${PWM_AUX_OUT}
+param set PWM_MAIN_OUT ${PWM_OUT}
+
 #
 # If mount (gimbal) control is enabled and output mode is AUX, set the aux
 # mixer to mount (override the airframe-specific MIXER_AUX setting).
@@ -78,9 +82,16 @@ then
 		fi
 	fi

-	if [ $OUTPUT_MODE = io ]
+	#
+	# Start IO for PWM output or RC input if needed.
+	#
+	if [ $IO_PRESENT = yes ]
 	then
-		. ${R}etc/init.d/rc.io
+		if ! px4io start
+		then
+			echo "PX4IO start failed"
+			tune_control play -t 18 # PROG_PX4IO_ERR
+		fi
 	fi

 	if [ $OUTPUT_MODE = $OUTPUT_CMD -o $OUTPUT_MODE = io ]
@@ -91,17 +102,6 @@ then
 			tune_control play error
 		fi
 	fi
-
-	#
-	# Start IO for RC input if needed.
-	#
-	if [ $IO_PRESENT = yes ]
-	then
-		if [ $OUTPUT_MODE != io ]
-		then
-			. ${R}etc/init.d/rc.io
-		fi
-	fi
 fi

 if [ $MIXER != none -a $MIXER != skip ]
@@ -212,8 +212,6 @@ then
 	fi
 fi

-param set PWM_AUX_OUT ${PWM_AUX_OUT}
-
 if [ $OUTPUT_MODE = pwm_out -o $OUTPUT_MODE = io ]
 then
 	if [ $PWM_OUT != none ]
@@ -226,8 +224,6 @@ then
 	fi
 fi

-param set PWM_MAIN_OUT ${PWM_OUT}
-
 if [ $EXTRA_MIXER_MODE != none ]
 then

@@ -1,23 +0,0 @@
-#!/bin/sh
-#
-# PX4IO interface init script.
-#
-
-# If $OUTPUT_MODE indicated Hardware-int-the-loop simulation, px4io should not publish actuator_outputs,
-# instead, pwm_out_sim will publish that uORB
-if [ $OUTPUT_MODE = hil ]
-then
-    set HIL_ARG $OUTPUT_MODE
-fi
-
-if [ $IO_PRESENT = yes ]
-then
-	if px4io start $HIL_ARG
-	then
-		# Allow PX4IO to recover from midair restarts.
-		px4io recovery
-	else
-		echo "PX4IO start failed"
-		tune_control play -t 18 # PROG_PX4IO_ERR
-	fi
-fi
@@ -13,6 +13,8 @@ param set-default MIS_YAW_TMT 10
 param set-default EKF2_ARSP_THR 10
 param set-default EKF2_FUSE_BETA 1

+param set-default HTE_VXY_THR 2.0
+
 param set-default MIS_DIST_WPS 5000

 param set-default MPC_ACC_HOR_MAX 2
@@ -70,14 +70,6 @@ then
 	set io_file /etc/extras/px4_io-v2_default.bin
 fi

-if px4io start
-then
-	echo "PX4IO OK"
-else
-	set unit_test_failure 1
-	set unit_test_failure_list "${unit_test_failure_list} px4io_start"
-fi
-
 if px4io checkcrc $io_file
 then
 	echo "PX4IO CRC OK"
@@ -104,6 +96,15 @@ else
 	fi
 fi

+if px4io start
+then
+	echo "PX4IO OK"
+else
+	set unit_test_failure 1
+	set unit_test_failure_list "${unit_test_failure_list} px4io_start"
+fi
+
+
 #
 # The presence of this file suggests we're running a mount stress test
 #
@@ -70,7 +70,7 @@ def do_nsh_cmd(port, baudrate, cmd):
    success_cmd = "cmd succeeded!"

    # wait for command echo
-    serial_cmd = '{0}; echo "{1}"\r\n'.format(cmd, success_cmd)
+    serial_cmd = '{0}; echo "{1}"; echo "{2}";\r\n'.format(cmd, success_cmd, success_cmd)
    ser.write(serial_cmd.encode("ascii"))
    ser.flush()
    while True:
@@ -142,9 +142,6 @@ class TestHardwareMethods(unittest.TestCase):
    def test_bson(self):
        self.assertTrue(do_test(self.TEST_DEVICE, self.TEST_BAUDRATE, "bson"))

-    def test_conv(self):
-        self.assertTrue(do_test(self.TEST_DEVICE, self.TEST_BAUDRATE, "conv"))
-
    def test_dataman(self):
        self.assertTrue(do_test(self.TEST_DEVICE, self.TEST_BAUDRATE, "dataman"))

@@ -24,4 +24,7 @@ exec find boards msg src platforms test \
    -path src/modules/micrortps_bridge/micro-CDR -prune -o \
    -path src/modules/micrortps_bridge/microRTPS_client -prune -o \
    -path test/mavsdk_tests/catch2 -prune -o \
+    -path src/lib/crypto/monocypher -prune -o \
+    -path src/lib/crypto/libtomcrypt -prune -o \
+    -path src/lib/crypto/libtommath -prune -o \
    -type f \( -name "*.c" -o -name "*.h" -o -name "*.cpp" -o -name "*.hpp" \) | grep $PATTERN
@@ -1,85 +0,0 @@
-#!/usr/bin/env bash
-
-function check_git_submodule {
-
-# The .git exists in a submodule if init and update have been done.
-if [[ -f $1"/.git" || -d $1"/.git" ]]; then
-
-	# always update within CI environment or configuring withing VSCode CMake where you can't interact
-	if [ "$CI" == "true" ] || [ -n "${VSCODE_PID+set}" ]; then
-		git submodule --quiet sync --recursive -- $1
-		git submodule --quiet update --init --recursive --jobs=8 -- $1  || true
-		git submodule --quiet update --init --recursive --jobs=8 -- $1
-		exit 0
-	fi
-
-	SUBMODULE_STATUS=$(git submodule summary "$1")
-	STATUSRETVAL=$(echo $SUBMODULE_STATUS | grep -A20 -i "$1")
-	if ! [[ -z "$STATUSRETVAL" ]]; then
-		echo -e "\033[31mChecked $1 submodule, ACTION REQUIRED:\033[0m"
-		echo ""
-		echo -e "Different commits:"
-		echo -e "$SUBMODULE_STATUS"
-		echo ""
-		echo ""
-		echo -e " *******************************************************************************"
-		echo -e " *   \033[31mIF YOU DID NOT CHANGE THIS FILE (OR YOU DON'T KNOW WHAT A SUBMODULE IS):\033[0m  *"
-		echo -e " *   \033[31mHit 'u' and <ENTER> to update ALL submodules and resolve this.\033[0m            *"
-		echo -e " *   (performs \033[94mgit submodule sync --recursive\033[0m                                  *"
-		echo -e " *    and \033[94mgit submodule update --init --recursive\033[0m )                            *"
-		echo -e " *******************************************************************************"
-		echo ""
-		echo ""
-		echo -e "   Only for EXPERTS:"
-		echo -e "   $1 submodule is not in the recommended version."
-		echo -e "   Hit 'y' and <ENTER> to continue the build with this version. Hit <ENTER> to resolve manually."
-		echo -e "   Use \033[94mgit add $1 && git commit -m 'Updated $1'\033[0m to choose this version (careful!)"
-		echo ""
-		read user_cmd
-		if [ "$user_cmd" == "y" ]; then
-			echo "Continuing build with manually overridden submodule.."
-		elif [ "$user_cmd" == "u" ]; then
-			git submodule sync --recursive -- $1
-			git submodule update --init --recursive -- $1 || true
-			git submodule update --init --recursive --force -- $1
-			echo "Submodule fixed, continuing build.."
-		else
-			echo "Build aborted."
-			exit 1
-		fi
-	fi
-else
-	git submodule --quiet sync --recursive --quiet -- $1
-	git submodule --quiet update --init --recursive -- $1  || true
-	git submodule --quiet update --init --recursive -- $1
-fi
-
-}
-
-# If called with a path then respect $GIT_SUBMODULES_ARE_EVIL but do normal processing
-if [ "$#" != "0" ]; then
-	# called with a path then process only that path but respect $GIT_SUBMODULES_ARE_EVIL
-	[ -n "$GIT_SUBMODULES_ARE_EVIL" ] && {
-		# GIT_SUBMODULES_ARE_EVIL is set, meaning user doesn't want submodules updated
-		exit 0
-	}
-
-	check_git_submodule $1
-
-else
-
-	[ -n "$GIT_SUBMODULES_ARE_EVIL" ] && {
-		# GIT_SUBMODULES_ARE_EVIL is set, meaning user doesn't want submodules updated
-		echo "GIT_SUBMODULES_ARE_EVIL is defined - Skipping All submodule checking!"
-		exit 0
-	}
-
-	submodules=$(git submodule status | awk '{ print $2 }')
-	for i in $submodules;
-	do
-		check_git_submodule $i
-	done
-
-fi
-
-exit 0
@@ -0,0 +1,62 @@
+#!/usr/bin/env python3
+
+from Crypto.PublicKey import RSA
+from Crypto.Cipher import PKCS1_OAEP
+from Crypto.Cipher import ChaCha20
+from Crypto.Hash import SHA256
+import binascii
+import argparse
+#from pathlib import Path
+import sys
+
+if __name__ == "__main__":
+
+    parser = argparse.ArgumentParser(description="""CLI tool to decrypt an ulog file\n""")
+    parser.add_argument("ulog_file", help=".ulog file", nargs='?', default=None)
+    parser.add_argument("ulog_key", help=".ulogk, encrypted key", nargs='?', default=None)
+    parser.add_argument("rsa_key", help=".pem format key for decrypting the ulog key", nargs='?', default=None)
+
+    args = parser.parse_args()
+
+    # Only generate a key pair, don't sign
+    if not args.ulog_file or not args.ulog_key or not args.rsa_key:
+        print('Need all arguments, the encrypted ulog file, the key and the key decryption key')
+        sys.exit(1);
+
+    # Read the private RSA key to decrypt the cahcha key
+    with open(args.rsa_key, 'rb') as f:
+        r = RSA.importKey(f.read(), passphrase='')
+
+    # Read the encrypted xchacha key and the nonce
+    with open(args.ulog_key, 'rb') as f:
+        ulog_key_header = f.read(22)
+
+        # Parse the header
+        try:
+            # magic
+            if not ulog_key_header.startswith(bytearray("ULogKey".encode())):
+                raise Exception()
+            # version
+            if ulog_key_header[7] != 1:
+                raise Exception()
+            # expected key exchange algorithm (RSA_OAEP)
+            if ulog_key_header[16] != 4:
+                raise Exception()
+            key_size = ulog_key_header[19] << 8 | ulog_key_header[18];
+            nonce_size = ulog_key_header[21] << 8 | ulog_key_header[20];
+            ulog_key_cipher = f.read(key_size)
+            nonce = f.read(nonce_size)
+        except:
+            print("Keyfile format error")
+            sys.exit(1);
+
+    # Decrypt the xchacha key
+    cipher_rsa = PKCS1_OAEP.new(r,SHA256)
+    ulog_key = cipher_rsa.decrypt(ulog_key_cipher)
+    #print(binascii.hexlify(ulog_key))
+
+    # Read and decrypt the .ulgc
+    cipher = ChaCha20.new(key=ulog_key, nonce=nonce)
+    with open(args.ulog_file, 'rb') as f:
+        with open(args.ulog_file.rstrip(args.ulog_file[-1]), 'wb') as out:
+            out.write(cipher.decrypt(f.read()))
@@ -126,7 +126,8 @@ else

 		m=0
 		while [ $m -lt ${target_number} ]; do
-			spawn_model ${target_vehicle} $n $target_x $target_y
+			export PX4_SIM_MODEL=${target_vehicle}${LABEL}
+			spawn_model ${target_vehicle}${LABEL} $n $target_x $target_y
 			m=$(($m + 1))
 			n=$(($n + 1))
 		done
@@ -138,6 +138,12 @@ serial_ports = {
        "default_baudrate": 1, # set default to an unusable value to detect that this serial port has not been configured
        },

+    # Pixhawk Payload Bus
+    "PPB": {
+        "label": "Pixhawk Payload Bus",
+        "index": 401,
+        "default_baudrate": 57600,
+        },

    }

@@ -0,0 +1,27 @@
+-----BEGIN RSA PRIVATE KEY-----
+MIIEowIBAAKCAQEAz6O47E+sN+Yb+h6zkukagzYtJ6ZKq344b5N6H2CDg6z9LmG/
+GRAfOeNstsz1hXyyOCcUQ9f+vKHofTN0k+fWKy95h1mwY5Vc509gRsmpD96pNp4+
+NLp3GFDjawOTqsfyLQ7zKwU0YKT3qeI3o20n2KNxnGuyXt0SK3Ph2530wwzWBR2s
+pNxpKvV8MCR7K4AbeuxodxKXKrLl0v9BHfUGpLZHpfoFfvChuK3eXK4si06tr6xt
+RmLh6f6hxcn0SNKAK+WpPsH8mN4DqayGt7UZ5XVqY4M/J3pe+PFtstI2ocDSX5Oc
+kOg+yonXytDH6I9Lt2CSXjZDcCM2+7WlBprAywIDAQABAoIBAGZE1nUV/NX/cXIt
+IvdN9q//xBfOUOLMpVFXSwQfTkdRsdXhcPUQOsERYd9bbeZUd5cusE2GGkKgYFki
+Od4LhzH4DRx8MWOrEnofX2UeODXHzoJHSI5B9Ry14n034shv+Lj2rxBWXOjo987l
+y8+jmMecIP4REWal7igWyHyZ/Q7/5UCCCH5HaA4jgMhMdrD3zdxrlPhrW/egjfKF
+Q+444qG5HzUqh7tOBxvf9Yu9FujHC9NxKDNXZZ3WBpwVQIVPCxexb4b8FBZ4syCH
+iPbSgyw91CVfMvpWCYqDe7ubw4QDLHzDIiyKFtvHmIOgPo74y8fi1YYZ+3dy5mpa
+UNdC11kCgYEA3whbNd71AVcSykRZ23AxDcjtUoiFXlpzSFes0QBGrmi2WTc04r8d
+5Zr1E8oohc/zPJnUnCh93BAAu+HJsg4wKCqOWu5qR3NWcFp4/4voG9hsg1hgWD8Z
+6fMMjoZRezSxadyeeaDStnfSkYyE2J8DPpkK5mjFG9IbrylGfApzDf0CgYEA7lTi
+j2NQ+HNMlp/oOtsRZHkQtHYutv5SltG4E4DlwDHyA2ZLR2xB+D2ce5VdPQvfHu8F
+ioMquDLeJbOPeikwko+RmpbmCsODjLueZHBmGwAfkuE0A5TZUwTi+W6iYbrXLK6E
+TgNMaS4AGeBbioo6cdnMnYp9jdm63zvefet6oGcCgYAwr4BJmCvfaQR/BsCeuDTd
+D3lOxOJoIFJ9/jWJQggr1kvH2dc/j/yUvGi3My/5VdWA6wuQMv6WZR/j43vF1HcK
+rY95pgWpJzI9QGKdVgsK2QmG+mm9mbisaxPYoNV0kaIQu8oUPtkAX9OlVglByCRL
+K9lHRqOQWSMV72qldRp8eQKBgHVCJkXN42SZtbDV8/ghGCmKtwFStCEsd43kmOBf
+pqos6JlrltYJGVv9VCQplLoYQSqDBwLjDf2aaVm7QngkE9XH9SdN3tik4PA4zvEz
+q8jVArPNQT4R2erSmKmIGTRkLMG7CzUmwk1taHdSvzcmUyL4uYc5QBSubxat6gWh
+a85AoGBAKgfnjjVoAWWvqEDLpfGPmE8lW+RaS7i5ff6QsSBx7uTEnHq6RNHuVnN
+et4pR87yIENeG8jMBiDCj8AGDtUNt9Ps9vWCPrf9HSOYoBUk+gZagU/9N+RBpuCM
+egoxtxIHM7HI+XIer+ZnZpVpgr+EoCaL7avx6k/susLQb7tqSBt1
+-----END RSA PRIVATE KEY-----
@@ -0,0 +1,38 @@
+
+0x30, 0x82, 0x1, 0x22, 0x30, 0xd, 0x6, 0x9, 
+0x2a, 0x86, 0x48, 0x86, 0xf7, 0xd, 0x1, 0x1, 
+0x1, 0x5, 0x0, 0x3, 0x82, 0x1, 0xf, 0x0, 
+0x30, 0x82, 0x1, 0xa, 0x2, 0x82, 0x1, 0x1, 
+0x0, 0xcf, 0xa3, 0xb8, 0xec, 0x4f, 0xac, 0x37, 
+0xe6, 0x1b, 0xfa, 0x1e, 0xb3, 0x92, 0xe9, 0x1a, 
+0x83, 0x36, 0x2d, 0x27, 0xa6, 0x4a, 0xab, 0x7e, 
+0x38, 0x6f, 0x93, 0x7a, 0x1f, 0x60, 0x83, 0x83, 
+0xac, 0xfd, 0x2e, 0x61, 0xbf, 0x19, 0x10, 0x1f, 
+0x39, 0xe3, 0x6c, 0xb6, 0xcc, 0xf5, 0x85, 0x7c, 
+0xb2, 0x38, 0x27, 0x14, 0x43, 0xd7, 0xfe, 0xbc, 
+0xa1, 0xe8, 0x7d, 0x33, 0x74, 0x93, 0xe7, 0xd6, 
+0x2b, 0x2f, 0x79, 0x87, 0x59, 0xb0, 0x63, 0x95, 
+0x5c, 0xe7, 0x4f, 0x60, 0x46, 0xc9, 0xa9, 0xf, 
+0xde, 0xa9, 0x36, 0x9e, 0x3e, 0x34, 0xba, 0x77, 
+0x18, 0x50, 0xe3, 0x6b, 0x3, 0x93, 0xaa, 0xc7, 
+0xf2, 0x2d, 0xe, 0xf3, 0x2b, 0x5, 0x34, 0x60, 
+0xa4, 0xf7, 0xa9, 0xe2, 0x37, 0xa3, 0x6d, 0x27, 
+0xd8, 0xa3, 0x71, 0x9c, 0x6b, 0xb2, 0x5e, 0xdd, 
+0x12, 0x2b, 0x73, 0xe1, 0xdb, 0x9d, 0xf4, 0xc3, 
+0xc, 0xd6, 0x5, 0x1d, 0xac, 0xa4, 0xdc, 0x69, 
+0x2a, 0xf5, 0x7c, 0x30, 0x24, 0x7b, 0x2b, 0x80, 
+0x1b, 0x7a, 0xec, 0x68, 0x77, 0x12, 0x97, 0x2a, 
+0xb2, 0xe5, 0xd2, 0xff, 0x41, 0x1d, 0xf5, 0x6, 
+0xa4, 0xb6, 0x47, 0xa5, 0xfa, 0x5, 0x7e, 0xf0, 
+0xa1, 0xb8, 0xad, 0xde, 0x5c, 0xae, 0x2c, 0x8b, 
+0x4e, 0xad, 0xaf, 0xac, 0x6d, 0x46, 0x62, 0xe1, 
+0xe9, 0xfe, 0xa1, 0xc5, 0xc9, 0xf4, 0x48, 0xd2, 
+0x80, 0x2b, 0xe5, 0xa9, 0x3e, 0xc1, 0xfc, 0x98, 
+0xde, 0x3, 0xa9, 0xac, 0x86, 0xb7, 0xb5, 0x19, 
+0xe5, 0x75, 0x6a, 0x63, 0x83, 0x3f, 0x27, 0x7a, 
+0x5e, 0xf8, 0xf1, 0x6d, 0xb2, 0xd2, 0x36, 0xa1, 
+0xc0, 0xd2, 0x5f, 0x93, 0x9c, 0x90, 0xe8, 0x3e, 
+0xca, 0x89, 0xd7, 0xca, 0xd0, 0xc7, 0xe8, 0x8f, 
+0x4b, 0xb7, 0x60, 0x92, 0x5e, 0x36, 0x43, 0x70, 
+0x23, 0x36, 0xfb, 0xb5, 0xa5, 0x6, 0x9a, 0xc0, 
+0xcb, 0x2, 0x3, 0x1, 0x0, 0x1, 
@@ -1,5 +1,7 @@
 include (${CMAKE_CURRENT_LIST_DIR}/uavcan_board_identity)

+add_definitions(-DCONSTRAINED_FLASH_NO_HELP="https://docs.px4.io/master/en/modules/modules_main.html")
+
 px4_add_board(
 	PLATFORM nuttx
 	TOOLCHAIN arm-none-eabi
@@ -25,7 +27,7 @@ px4_add_board(
 		#i2cdetect
 		#led_control
 		param
-		perf
+		#perf
 		reboot
 		top
 		topic_listener
@@ -53,7 +53,6 @@
 #include <fcntl.h>
 #include <mqueue.h>

-#include <drivers/drv_rc_input.h>
 #include <drivers/drv_board_led.h>

 #include <systemlib/err.h>
@@ -96,7 +95,7 @@ Syslink::Syslink() :
 	_fd(0),
 	_queue(2, sizeof(syslink_message_t)),
 	_writebuffer(16, sizeof(crtp_message_t)),
-	_rssi(RC_INPUT_RSSI_MAX),
+	_rssi(input_rc_s::RSSI_MAX),
 	_bstate(BAT_DISCHARGING)
 {
 	px4_sem_init(&memory_sem, 0, 0);
@@ -103,6 +103,8 @@

 #define GPIO_SENSOR_3V3_EN              /* PC15 */  (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN15)

+#define VDD_3V3_SENSORS_EN(on_true)       px4_arch_gpiowrite(GPIO_SENSOR_3V3_EN, (on_true))
+
 #define GPIO_MCU_I2C1_SCL_RESET         /* PB6  */  (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN6)
 #define GPIO_MCU_I2C1_SDA_RESET         /* PB7  */  (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN7)

@@ -135,6 +135,8 @@ stm32_boardinitialize(void)

 __EXPORT int board_app_initialize(uintptr_t arg)
 {
+	VDD_3V3_SENSORS_EN(true);
+
 	px4_platform_init();

 	if (OK == board_determine_hw_info()) {
@@ -265,7 +265,7 @@

 #define GPIO_SPI5_SCK    ADJ_SLEW_RATE(GPIO_SPI5_SCK_1) /* PF7  */
 #define GPIO_SPI5_MISO   GPIO_SPI5_MISO_1               /* PF8  */
-#define GPIO_SPI5_MOSI   GPIO_SPI5_MOSI_1               /* PF9  */
+#define GPIO_SPI5_MOSI   GPIO_SPI5_MOSI_2               /* PF9  */


 /* I2C */
@@ -159,8 +159,12 @@
 #define PIN_CAN0_TX      PIN_CAN0_TX_4      /* PTE5 */
 #define PIN_CAN0_RX      PIN_CAN0_RX_4      /* PTE4 */
 #define PIN_CAN0_STB     (GPIO_OUTPUT | PIN_PORTE | PIN11 )
+#define PIN_CAN0_ERRN    (GPIO_INPUT  | PIN_PORTA | PIN11 )
+#define PIN_CAN0_EN      (GPIO_HIGHDRIVE  | PIN_PORTA | PIN10 )
 #define PIN_CAN1_TX      PIN_CAN1_TX_1      /* PTA13 */
 #define PIN_CAN1_RX      PIN_CAN1_RX_1      /* PTA12 */
 #define PIN_CAN1_STB     (GPIO_OUTPUT | PIN_PORTE | PIN10 )
+#define PIN_CAN1_ERRN    (GPIO_PULLDOWN  | PIN_PORTE | PIN6 )
+#define PIN_CAN1_EN      (GPIO_OUTPUT  | PIN_PORTE | PIN2 )

 #endif  /* __BOARDS_ARM_RDDRONE_UAVCAN146_INCLUDE_BOARD_H */
@@ -75,8 +75,32 @@ __EXPORT void s32k1xx_board_initialize(void)
 	s32k1xx_pinconfig(BOARD_REVISION_DETECT_PIN);

 	if (s32k1xx_gpioread(BOARD_REVISION_DETECT_PIN)) {
-		/* STB high -> active CAN phy */
-		s32k1xx_pinconfig(PIN_CAN0_STB  | GPIO_OUTPUT_ONE);
+		/* Config Pins to do CAN tranceiver HW selftest */
+		s32k1xx_pinconfig(PIN_CAN0_ERRN);
+		s32k1xx_pinconfig(PIN_CAN0_STB);
+		s32k1xx_pinconfig(PIN_CAN0_EN);
+
+		/* EN high & STB high -> normal mode */
+		s32k1xx_gpiowrite(PIN_CAN0_STB, 1);
+		s32k1xx_gpiowrite(PIN_CAN0_EN, 1);
+		up_udelay(3000); // Wait for startup to normal mode
+
+		/* EN low & STB high -> listen only mode */
+		s32k1xx_gpiowrite(PIN_CAN0_STB, 1);
+		s32k1xx_gpiowrite(PIN_CAN0_EN, 0);
+		up_udelay(100); // t moch ERRN_N
+
+		/* Check for HW err and wait untill ERR has been cleared */
+		while (!s32k1xx_gpioread(PIN_CAN0_ERRN)) {
+			board_indicate(hardware_failure);
+			s32k1xx_gpiowrite(PIN_CAN0_EN, 1);
+			up_udelay(50);
+			s32k1xx_gpiowrite(PIN_CAN0_EN, 0);
+			up_udelay(50);
+		}
+
+		/* Enter normal-mode */
+		s32k1xx_gpiowrite(PIN_CAN0_EN, 1);

 	} else {
 		/* STB low -> active CAN phy */
@@ -208,6 +232,7 @@ static const  led_t i2l[] = {
 	led(9, fw_update_timeout,              31,    0,    0,     2),
 	led(a, fw_update_invalid_crc,          31,    0,    0,     4),
 	led(b, jump_to_app,                     0,   63,    0,    10),
+	led(c, hardware_failure,               31,    0,    0,    10),

 };

@@ -10,6 +10,8 @@ set(uavcanblid_hw_version_major 0)
 set(uavcanblid_hw_version_minor 34)
 set(uavcanblid_name "\"org.nxp.ucans32k146\"")

+set(uavcanbl_padding 8)
+
 add_definitions(
 	-DHW_UAVCAN_NAME=${uavcanblid_name}
 	-DHW_VERSION_MAJOR=${uavcanblid_hw_version_major}
@@ -79,6 +79,7 @@ SECTIONS
 		_bootdelay_signature = ABSOLUTE(.);
 		FILL(0xffecc2925d7d05c5)
 		. += 8;
+		KEEP(*(.app_descriptor))
 		*(.text .text.*)
 		*(.fixup)
 		*(.gnu.warning)
@@ -0,0 +1,150 @@
+
+px4_add_board(
+	PLATFORM nuttx
+	TOOLCHAIN arm-none-eabi
+	ARCHITECTURE cortex-m7
+	ROMFSROOT px4fmu_common
+	IO px4_io-v2_default
+	UAVCAN_INTERFACES 2
+	UAVCAN_TIMER_OVERRIDE 6
+        CRYPTO sw_crypto
+        KEYSTORE stub_keystore
+	SERIAL_PORTS
+		GPS1:/dev/ttyS0
+		TEL1:/dev/ttyS1
+		TEL2:/dev/ttyS2
+		TEL4:/dev/ttyS3
+	DRIVERS
+		adc/ads1115
+		adc/board_adc
+		barometer # all available barometer drivers
+		batt_smbus
+		camera_capture
+		camera_trigger
+		differential_pressure # all available differential pressure drivers
+		distance_sensor # all available distance sensor drivers
+		dshot
+		gps
+		heater
+		#imu # all available imu drivers
+		imu/analog_devices/adis16448
+		imu/bosch/bmi055
+		imu/invensense/icm20602
+		imu/invensense/icm20689
+		imu/invensense/icm20948 # required for ak09916 mag
+		irlock
+		lights # all available light drivers
+		lights/rgbled_pwm
+		magnetometer # all available magnetometer drivers
+		optical_flow # all available optical flow drivers
+		osd
+		pca9685
+		pca9685_pwm_out
+		power_monitor/ina226
+		#protocol_splitter
+		pwm_input
+		pwm_out_sim
+		pwm_out
+		px4io
+		rc_input
+		roboclaw
+		smart_battery/batmon
+		rpm
+		safety_button
+		telemetry # all available telemetry drivers
+		tone_alarm
+		uavcan
+	MODULES
+		airspeed_selector
+		attitude_estimator_q
+		battery_status
+		camera_feedback
+		commander
+		dataman
+		ekf2
+		esc_battery
+		events
+		flight_mode_manager
+		fw_att_control
+		fw_pos_control_l1
+		gyro_calibration
+		gyro_fft
+		land_detector
+		landing_target_estimator
+		load_mon
+		local_position_estimator
+		logger
+		mavlink
+		mc_att_control
+		mc_hover_thrust_estimator
+		mc_pos_control
+		mc_rate_control
+		#micrortps_bridge
+		navigator
+		rc_update
+		rover_pos_control
+		sensors
+		sih
+		temperature_compensation
+		uuv_att_control
+		uuv_pos_control
+		vmount
+		vtol_att_control
+	SYSTEMCMDS
+		bl_update
+		dmesg
+		dumpfile
+		esc_calib
+		gpio
+		hardfault_log
+		i2cdetect
+		led_control
+		mft
+		mixer
+		motor_ramp
+		motor_test
+		mtd
+		nshterm
+		param
+		perf
+		pwm
+		reboot
+		reflect
+		sd_bench
+		serial_test
+		system_time
+		top
+		topic_listener
+		tune_control
+		uorb
+		usb_connected
+		ver
+		work_queue
+	EXAMPLES
+		fake_gps
+		#fake_imu
+		#fake_magnetometer
+		#fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control
+		#hello
+		#hwtest # Hardware test
+		#matlab_csv_serial
+		#px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html
+		#px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html
+		#rover_steering_control # Rover example app
+		#uuv_example_app
+		#work_item
+	)
+
+# For testing crypto with the stub_keystore, set keys to the PX4 test keys,
+# for any real target using that keystore, these would probably be set as environment
+# variables in the build environment
+
+# Key 0: The firmware signature check key (public key), used by bootloaders
+# NOTE: for FMUv5 this is not really used, it is only for NuttX based bootloaders
+set(ENV{PUBLIC_KEY0} "${CMAKE_SOURCE_DIR}/Tools/test_keys/key0.pub")
+
+# Key 1: An RSA2048 public key, used currently for logfile symmetric key exchange
+set(ENV{PUBLIC_KEY1} "${CMAKE_SOURCE_DIR}/Tools/test_keys/rsa2048.pub")
+
+# FW Signing tool, using ed25519 (this uses by default the key in Tools/test_keys/test_keys.json)
+set(ENV{SIGNING_TOOL} "${CMAKE_SOURCE_DIR}/Tools/cryptotools.py")
@@ -0,0 +1,244 @@
+#
+# This file is autogenerated: PLEASE DO NOT EDIT IT.
+#
+# You can use "make menuconfig" to make any modifications to the installed .config file.
+# You can then do "make savedefconfig" to generate a new defconfig file that includes your
+# modifications.
+#
+# CONFIG_DISABLE_ENVIRON is not set
+# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set
+# CONFIG_FS_PROCFS_EXCLUDE_ENVIRON is not set
+# CONFIG_MMCSD_HAVE_CARDDETECT is not set
+# CONFIG_MMCSD_HAVE_WRITEPROTECT is not set
+# CONFIG_MMCSD_MMCSUPPORT is not set
+# CONFIG_MMCSD_SPI is not set
+# CONFIG_NSH_DISABLEBG is not set
+# CONFIG_NSH_DISABLESCRIPT is not set
+# CONFIG_NSH_DISABLE_DF is not set
+# CONFIG_NSH_DISABLE_EXEC is not set
+# CONFIG_NSH_DISABLE_EXIT is not set
+# CONFIG_NSH_DISABLE_GET is not set
+# CONFIG_NSH_DISABLE_ITEF is not set
+# CONFIG_NSH_DISABLE_LOOPS is not set
+# CONFIG_NSH_DISABLE_MKFATFS is not set
+# CONFIG_NSH_DISABLE_SEMICOLON is not set
+# CONFIG_NSH_DISABLE_TIME is not set
+CONFIG_ARCH="arm"
+CONFIG_ARCH_BOARD_CUSTOM=y
+CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config"
+CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y
+CONFIG_ARCH_BOARD_CUSTOM_NAME="px4"
+CONFIG_ARCH_CHIP="stm32f7"
+CONFIG_ARCH_CHIP_STM32F765II=y
+CONFIG_ARCH_CHIP_STM32F7=y
+CONFIG_ARCH_INTERRUPTSTACK=512
+CONFIG_ARCH_STACKDUMP=y
+CONFIG_ARMV7M_BASEPRI_WAR=y
+CONFIG_ARMV7M_DCACHE=y
+CONFIG_ARMV7M_DTCM=y
+CONFIG_ARMV7M_ICACHE=y
+CONFIG_ARMV7M_MEMCPY=y
+CONFIG_ARMV7M_USEBASEPRI=y
+CONFIG_BOARDCTL_RESET=y
+CONFIG_BOARD_CRASHDUMP=y
+CONFIG_BOARD_LOOPSPERMSEC=22114
+CONFIG_BOARD_RESET_ON_ASSERT=2
+CONFIG_BUILTIN=y
+CONFIG_C99_BOOL8=y
+CONFIG_CDCACM=y
+CONFIG_CDCACM_PRODUCTID=0x0032
+CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v5.x"
+CONFIG_CDCACM_RXBUFSIZE=600
+CONFIG_CDCACM_TXBUFSIZE=12000
+CONFIG_CDCACM_VENDORID=0x26ac
+CONFIG_CDCACM_VENDORSTR="3D Robotics"
+CONFIG_CLOCK_MONOTONIC=y
+CONFIG_CRYPTO=y
+CONFIG_CRYPTO_RANDOM_POOL=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_DISABLE_MQUEUE=y
+CONFIG_FAT_DMAMEMORY=y
+CONFIG_FAT_LCNAMES=y
+CONFIG_FAT_LFN=y
+CONFIG_FAT_LFN_ALIAS_HASH=y
+CONFIG_FDCLONE_STDIO=y
+CONFIG_FS_BINFS=y
+CONFIG_FS_CROMFS=y
+CONFIG_FS_FAT=y
+CONFIG_FS_FATTIME=y
+CONFIG_FS_PROCFS=y
+CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y
+CONFIG_FS_PROCFS_MAX_TASKS=64
+CONFIG_FS_PROCFS_REGISTER=y
+CONFIG_FS_ROMFS=y
+CONFIG_GRAN=y
+CONFIG_GRAN_INTR=y
+CONFIG_HAVE_CXX=y
+CONFIG_HAVE_CXXINITIALIZE=y
+CONFIG_I2C=y
+CONFIG_I2C_RESET=y
+CONFIG_IDLETHREAD_STACKSIZE=750
+CONFIG_LIBC_FLOATINGPOINT=y
+CONFIG_LIBC_LONG_LONG=y
+CONFIG_LIBC_STRERROR=y
+CONFIG_MEMSET_64BIT=y
+CONFIG_MEMSET_OPTSPEED=y
+CONFIG_MMCSD=y
+CONFIG_MMCSD_SDIO=y
+CONFIG_MMCSD_SDIOWAIT_WRCOMPLETE=y
+CONFIG_MM_REGIONS=3
+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_ARCHROMFS=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_OTG_ID_GPIO_DISABLE=y
+CONFIG_PIPES=y
+CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
+CONFIG_PTHREAD_MUTEX_ROBUST=y
+CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_SETSPEED=y
+CONFIG_RAMTRON_WRITEWAIT=y
+CONFIG_RAM_SIZE=245760
+CONFIG_RAM_START=0x20010000
+CONFIG_RAW_BINARY=y
+CONFIG_RTC_DATETIME=y
+CONFIG_SCHED_ATEXIT=y
+CONFIG_SCHED_HPWORK=y
+CONFIG_SCHED_HPWORKPRIORITY=249
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
+CONFIG_SCHED_INSTRUMENTATION=y
+CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
+CONFIG_SCHED_WAITPID=y
+CONFIG_SDCLONE_DISABLE=y
+CONFIG_SDMMC1_SDIO_MODE=y
+CONFIG_SDMMC1_SDIO_PULLUP=y
+CONFIG_SEM_NNESTPRIO=8
+CONFIG_SEM_PREALLOCHOLDERS=0
+CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
+CONFIG_SERIAL_TERMIOS=y
+CONFIG_SIG_DEFAULT=y
+CONFIG_SIG_SIGALRM_ACTION=y
+CONFIG_SIG_SIGUSR1_ACTION=y
+CONFIG_SIG_SIGUSR2_ACTION=y
+CONFIG_SIG_SIGWORK=4
+CONFIG_STACK_COLORATION=y
+CONFIG_START_DAY=30
+CONFIG_START_MONTH=11
+CONFIG_STDIO_BUFFER_SIZE=256
+CONFIG_STM32F7_ADC1=y
+CONFIG_STM32F7_BBSRAM=y
+CONFIG_STM32F7_BBSRAM_FILES=5
+CONFIG_STM32F7_BKPSRAM=y
+CONFIG_STM32F7_DMA1=y
+CONFIG_STM32F7_DMA2=y
+CONFIG_STM32F7_DMACAPABLE=y
+CONFIG_STM32F7_FLOWCONTROL_BROKEN=y
+CONFIG_STM32F7_I2C1=y
+CONFIG_STM32F7_I2C2=y
+CONFIG_STM32F7_I2C3=y
+CONFIG_STM32F7_I2C4=y
+CONFIG_STM32F7_I2C_DYNTIMEO=y
+CONFIG_STM32F7_I2C_DYNTIMEO_STARTSTOP=10
+CONFIG_STM32F7_OTGFS=y
+CONFIG_STM32F7_PROGMEM=y
+CONFIG_STM32F7_PWR=y
+CONFIG_STM32F7_RTC=y
+CONFIG_STM32F7_RTC_AUTO_LSECLOCK_START_DRV_CAPABILITY=y
+CONFIG_STM32F7_RTC_MAGIC_REG=1
+CONFIG_STM32F7_SAVE_CRASHDUMP=y
+CONFIG_STM32F7_SDMMC1=y
+CONFIG_STM32F7_SDMMC_DMA=y
+CONFIG_STM32F7_SERIALBRK_BSDCOMPAT=y
+CONFIG_STM32F7_SERIAL_DISABLE_REORDERING=y
+CONFIG_STM32F7_SPI1=y
+CONFIG_STM32F7_SPI1_DMA=y
+CONFIG_STM32F7_SPI1_DMA_BUFFER=1024
+CONFIG_STM32F7_SPI2=y
+CONFIG_STM32F7_SPI4=y
+CONFIG_STM32F7_SPI5=y
+CONFIG_STM32F7_SPI6=y
+CONFIG_STM32F7_SPI_DMA=y
+CONFIG_STM32F7_SPI_DMATHRESHOLD=8
+CONFIG_STM32F7_TIM10=y
+CONFIG_STM32F7_TIM11=y
+CONFIG_STM32F7_UART4=y
+CONFIG_STM32F7_UART7=y
+CONFIG_STM32F7_UART8=y
+CONFIG_STM32F7_USART1=y
+CONFIG_STM32F7_USART2=y
+CONFIG_STM32F7_USART3=y
+CONFIG_STM32F7_USART6=y
+CONFIG_STM32F7_USART_BREAKS=y
+CONFIG_STM32F7_USART_INVERT=y
+CONFIG_STM32F7_USART_SINGLEWIRE=y
+CONFIG_STM32F7_USART_SWAP=y
+CONFIG_STM32F7_WWDG=y
+CONFIG_SYSTEM_CDCACM=y
+CONFIG_SYSTEM_NSH=y
+CONFIG_TASK_NAME_SIZE=24
+CONFIG_UART4_BAUD=57600
+CONFIG_UART4_RXBUFSIZE=600
+CONFIG_UART4_RXDMA=y
+CONFIG_UART4_TXBUFSIZE=1500
+CONFIG_UART7_BAUD=57600
+CONFIG_UART7_RXBUFSIZE=600
+CONFIG_UART7_SERIAL_CONSOLE=y
+CONFIG_UART7_TXBUFSIZE=1500
+CONFIG_UART8_BAUD=57600
+CONFIG_UART8_RXBUFSIZE=600
+CONFIG_UART8_RXDMA=y
+CONFIG_UART8_TXBUFSIZE=1500
+CONFIG_USART1_BAUD=57600
+CONFIG_USART1_RXBUFSIZE=600
+CONFIG_USART1_TXBUFSIZE=1500
+CONFIG_USART2_BAUD=57600
+CONFIG_USART2_IFLOWCONTROL=y
+CONFIG_USART2_OFLOWCONTROL=y
+CONFIG_USART2_RXBUFSIZE=600
+CONFIG_USART2_RXDMA=y
+CONFIG_USART2_TXBUFSIZE=1500
+CONFIG_USART3_BAUD=57600
+CONFIG_USART3_IFLOWCONTROL=y
+CONFIG_USART3_OFLOWCONTROL=y
+CONFIG_USART3_RXBUFSIZE=600
+CONFIG_USART3_RXDMA=y
+CONFIG_USART3_TXBUFSIZE=3000
+CONFIG_USART3_TXDMA=y
+CONFIG_USART6_BAUD=57600
+CONFIG_USART6_RXBUFSIZE=600
+CONFIG_USART6_RXDMA=y
+CONFIG_USART6_TXBUFSIZE=1500
+CONFIG_USBDEV=y
+CONFIG_USBDEV_BUSPOWERED=y
+CONFIG_USBDEV_MAXPOWER=500
+CONFIG_USEC_PER_TICK=1000
+CONFIG_USERMAIN_STACKSIZE=2944
+CONFIG_USER_ENTRYPOINT="nsh_main"
@@ -41,6 +41,7 @@ px4_add_board(
 		pca9685_pwm_out
 		power_monitor/ina226
 		power_monitor/ina228
+		power_monitor/ina238
 		#protocol_splitter
 		pwm_input
 		pwm_out_sim
@@ -3,6 +3,7 @@
 # board specific defaults
 #------------------------------------------------------------------------------

+param set-default SENS_EN_INA238 0
 param set-default SENS_EN_INA228 0
 param set-default SENS_EN_INA226 1

@@ -19,6 +19,13 @@ then
 	ina228 -X -b 2 -t 2 -k start
 fi

+if param compare SENS_EN_INA238 1
+then
+	# Start Digital power monitors
+	ina238 -X -b 1 -t 1 -k start
+	ina238 -X -b 2 -t 2 -k start
+fi
+
 if ver hwtypecmp V5X90 V5X91 V5X92 V5Xa0 V5Xa1 V5Xa2
 then
 	#SKYNODE base fmu board orientation
@@ -42,6 +42,7 @@ px4_add_board(
 		pca9685_pwm_out
 		power_monitor/ina226
 		power_monitor/ina228
+		power_monitor/ina238
 		#protocol_splitter
 		pwm_out_sim
 		pwm_out
@@ -3,7 +3,8 @@
 # board specific defaults
 #------------------------------------------------------------------------------

+param set-default SENS_EN_INA238 0
 param set-default SENS_EN_INA228 0
 param set-default SENS_EN_INA226 1

-safety_button start
+safety_button start
@@ -18,6 +18,13 @@ then
 	ina228 -X -b 2 -t 2 -k start
 fi

+if param compare SENS_EN_INA238 1
+then
+	# Start Digital power monitors
+	ina238 -X -b 1 -t 1 -k start
+	ina238 -X -b 2 -t 2 -k start
+fi
+
 # Internal SPI BMI088
 bmi088 -A -R 4 -s start
 bmi088 -G -R 4 -s start
@@ -277,6 +277,7 @@ function(px4_add_board)

 	if(CRYPTO)
 		set(PX4_CRYPTO ${CRYPTO} CACHE STRING "PX4 crypto implementation" FORCE)
+		add_definitions(-DPX4_CRYPTO)
 	endif()

 	if(KEYSTORE)
@@ -62,30 +62,120 @@ function(px4_add_git_submodule)
 		REQUIRED TARGET PATH
 		ARGN ${ARGN})

-	set(REL_PATH)
+	option(GIT_SUBMODULE "Check submodules during build" ON)

-	if(IS_ABSOLUTE ${PATH})
-		file(RELATIVE_PATH REL_PATH ${PX4_SOURCE_DIR} ${PATH})
-	else()
-		file(RELATIVE_PATH REL_PATH ${PX4_SOURCE_DIR} ${CMAKE_CURRENT_SOURCE_DIR}/${PATH})
+	# ENV GIT_SUBMODULES_ARE_EVIL
+
+	# ENV TERM_PROGRAM="vscode"
+	# ENV VSCODE_GIT_ASKPASS_MAIN
+	# ENV VSCODE_GIT_ASKPASS_NODE
+	# ENV VSCODE_GIT_IPC_HANDLE
+	# ENV VSCODE_IPC_HOOK_CLI
+
+	# if CI or vscode then force submodule update?
+
+	if(DEFINED ENV{VSCODE_IPC_HOOK_CLI})
+		#message(STATUS "VSCODE_IPC_HOOK_CLI: $ENV{VSCODE_IPC_HOOK_CLI}")
+
+		#message(STATUS "TERM_PROGRAM: $ENV{TERM_PROGRAM}")
+
+		#message(STATUS "VSCODE_GIT_ASKPASS_MAIN: $ENV{VSCODE_GIT_ASKPASS_MAIN}")
+		#message(STATUS "VSCODE_GIT_ASKPASS_NODE: $ENV{VSCODE_GIT_ASKPASS_NODE}")
+		#message(STATUS "VSCODE_GIT_IPC_HANDLE: $ENV{VSCODE_GIT_IPC_HANDLE}")
 	endif()

-	execute_process(
-		COMMAND Tools/check_submodules.sh ${REL_PATH}
-		WORKING_DIRECTORY ${PX4_SOURCE_DIR}
-		)
+	if(GIT_FOUND AND GIT_SUBMODULE AND NOT DEFINED ENV{GIT_SUBMODULES_ARE_EVIL})

-	string(REPLACE "/" "_" NAME ${PATH})
-	string(REPLACE "." "_" NAME ${NAME})
+		set(REL_PATH)

-	add_custom_command(OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/git_init_${NAME}.stamp
-		COMMAND Tools/check_submodules.sh ${REL_PATH}
-		COMMAND ${CMAKE_COMMAND} -E touch ${CMAKE_CURRENT_BINARY_DIR}/git_init_${NAME}.stamp
-		DEPENDS ${PX4_SOURCE_DIR}/.gitmodules ${PATH}/.git
-		COMMENT "git submodule ${REL_PATH}"
-		WORKING_DIRECTORY ${PX4_SOURCE_DIR}
-		USES_TERMINAL
-		)
+		if(IS_ABSOLUTE ${PATH})
+			file(RELATIVE_PATH REL_PATH ${PX4_SOURCE_DIR} ${PATH})
+		else()
+			file(RELATIVE_PATH REL_PATH ${PX4_SOURCE_DIR} ${CMAKE_CURRENT_SOURCE_DIR}/${PATH})
+		endif()

-	add_custom_target(${TARGET} DEPENDS git_init_${NAME}.stamp)
+		if(NOT EXISTS ${PATH}/.git)
+			execute_process(
+				COMMAND ${GIT_EXECUTABLE} submodule update --init --recursive --jobs=4 -- ${REL_PATH}
+				OUTPUT_VARIABLE GIT_SUBMODULE_UPDATE
+				RESULT_VARIABLE GIT_SUBMODULE_UPDATE_RESULT
+				WORKING_DIRECTORY ${PX4_SOURCE_DIR}
+			)
+			if(NOT GIT_SUBMODULE_UPDATE_RESULT EQUAL "0")
+				message(FATAL_ERROR "git submodule update --init --recursive -- ${REL_PATH} failed: ${GIT_SUBMODULE_UPDATE}")
+			endif()
+
+		else()
+			execute_process(
+				COMMAND ${GIT_EXECUTABLE} submodule status -- ${REL_PATH}
+				OUTPUT_VARIABLE GIT_SUBMODULE_STATUS
+				RESULT_VARIABLE GIT_SUBMODULE_STATUS_RESULT
+				WORKING_DIRECTORY ${PX4_SOURCE_DIR}
+			)
+			#message(STATUS "git submodule status ${REL_PATH} result ${GIT_SUBMODULE_STATUS_RESULT} : ${GIT_SUBMODULE_STATUS}")
+
+			if(NOT GIT_SUBMODULE_STATUS_RESULT EQUAL "0")
+				message(FATAL_ERROR "git submodule status -- ${REL_PATH} failed: ${GIT_SUBMODULE_STATUS}")
+			else()
+				# submodule status characters
+				string(FIND "${GIT_SUBMODULE_STATUS}" "+" submodule_plus)
+				string(FIND "${GIT_SUBMODULE_STATUS}" "-" submodule_minus)
+				string(FIND "${GIT_SUBMODULE_STATUS}" "U" submodule_merge_conflicts)
+
+				# + commit does not match the SHA-1 found in the index
+				if("${submodule_plus}" EQUAL 0)
+					# GIT_SHA_HEAD: submodule git SHA on current HEAD
+					execute_process(
+						COMMAND ${GIT_EXECUTABLE} ls-tree -d HEAD ${REL_PATH} | awk '{print $3;}
+						OUTPUT_VARIABLE GIT_SHA_HEAD
+						WORKING_DIRECTORY ${PX4_SOURCE_DIR}
+					)
+
+					# GIT_SHA_PREV_HEAD: submodule git SHA on previous HEAD
+					execute_process(
+						COMMAND ${GIT_EXECUTABLE} ls-tree -d HEAD@{1} ${REL_PATH} | awk '{print $3;}
+						OUTPUT_VARIABLE GIT_SHA_PREV_HEAD
+						WORKING_DIRECTORY ${PX4_SOURCE_DIR}
+					)
+
+					#message(STATUS "GIT_SHA_HEAD: ${GIT_SHA_HEAD}")
+					#message(STATUS "GIT_SHA_PREV_HEAD: ${GIT_SHA_PREV_HEAD}")
+
+					# TODO: check if submodule is dirty?
+
+					if(${GIT_SHA_HEAD} MATCHES ${GIT_SHA_PREV_HEAD})
+						#message(STATUS "${rel_path} GIT SHA matched previous")
+						# submodule hasn't changed, assume it's safe to update
+						execute_process(
+							COMMAND ${GIT_EXECUTABLE} submodule update --init --recursive --jobs=4 -- ${REL_PATH}
+							OUTPUT_VARIABLE GIT_SUBMODULE_UPDATE
+							WORKING_DIRECTORY ${PX4_SOURCE_DIR}
+						)
+					else()
+						message(NOTICE "submodule ${REL_PATH} commit does not match the SHA-1 found in the index")
+					endif()
+				endif()
+
+				# - uninitialized
+				if("${submodule_minus}" EQUAL 0)
+					execute_process(
+						COMMAND ${GIT_EXECUTABLE} submodule update --init --recursive --jobs=4 -- ${REL_PATH}
+						OUTPUT_VARIABLE GIT_SUBMODULE_UPDATE
+						WORKING_DIRECTORY ${PX4_SOURCE_DIR}
+					)
+				endif()
+
+				# U merge conflicts
+				if("${submodule_merge_conflicts}" EQUAL 0)
+					message(NOTICE "submodule ${REL_PATH} has merge conflicts")
+				endif()
+			endif()
+		endif()
+
+
+		add_custom_target(${TARGET})
+
+		set_property(DIRECTORY APPEND PROPERTY CMAKE_CONFIGURE_DEPENDS ${PATH}/.git)
+
+	endif()
 endfunction()
@@ -4,4 +4,4 @@ uint64 timestamp_utc # UTC timestamp
 uint32 seq		# Image sequence number
 bool feedback	# Trigger feedback from camera

-# TOPICS camera_trigger camera_trigger_secondary
+# TOPICS camera_trigger
@@ -23,6 +23,7 @@ uint64 timestamp_last_signal		# last valid reception time

 uint8 channel_count			# number of channels actually being seen

+int8 RSSI_MAX = 100
 int32 rssi				# receive signal strength indicator (RSSI): < 0: Undefined, 0: no signal, 100: full reception

 bool rc_failsafe			# explicit failsafe flag: true on TX failure or TX out of range , false otherwise. Only the true state is reliable, as there are some (PPM) receivers on the market going into failsafe without telling us explicitly.
@@ -26,11 +26,4 @@ uint8 kill_switch                # throttle kill: _NORMAL_, KILL
 uint8 gear_switch                # landing gear switch: _DOWN_, UP
 uint8 transition_switch          # VTOL transition switch: _HOVER, FORWARD_FLIGHT

-# legacy "advanced" switch configuration (will be removed soon)
-uint8 mode_switch                # main mode 3 position switch (mandatory): _MANUAL_, ASSIST, AUTO
-uint8 man_switch                 # manual switch (only relevant for fixed wings, optional): _STABILIZED_, MANUAL
-uint8 acro_switch                # acro 2 position switch (optional): _MANUAL_, ACRO
-uint8 stab_switch                # stabilize switch (only relevant for fixed wings, optional): _MANUAL, STABILIZED
-uint8 posctl_switch              # position control 2 position switch (optional): _ALTCTL_, POSCTL
-
 uint32 switch_changes            # number of switch changes
@@ -6,49 +6,38 @@ float32 voltage_v		# Servo rail voltage in volts
 float32 rssi_v			# RSSI pin voltage in volts

 # PX4IO status flags (PX4IO_P_STATUS_FLAGS)
-bool status_outputs_armed
-bool status_override
-bool status_rc_ok
-bool status_rc_ppm
-bool status_rc_dsm
-bool status_rc_sbus
-bool status_fmu_ok
-bool status_raw_pwm
-bool status_mixer_ok
 bool status_arm_sync
-bool status_init_ok
 bool status_failsafe
-bool status_safety_off
 bool status_fmu_initialized
+bool status_fmu_ok
+bool status_init_ok
+bool status_outputs_armed
+bool status_raw_pwm
+bool status_rc_ok
+bool status_rc_dsm
+bool status_rc_ppm
+bool status_rc_sbus
 bool status_rc_st24
 bool status_rc_sumd
+bool status_safety_off

 # PX4IO alarms (PX4IO_P_STATUS_ALARMS)
-bool alarm_vbatt_low
-bool alarm_temperature
-bool alarm_servo_current
-bool alarm_acc_current
-bool alarm_fmu_lost
-bool alarm_rc_lost
 bool alarm_pwm_error
-bool alarm_vservo_fault
+bool alarm_rc_lost

 # PX4IO arming (PX4IO_P_SETUP_ARMING)
-bool arming_io_arm_ok
+bool arming_failsafe_custom
 bool arming_fmu_armed
 bool arming_fmu_prearmed
-bool arming_manual_override_ok
-bool arming_failsafe_custom
-bool arming_inair_restart_ok
-bool arming_always_pwm_enable
-bool arming_rc_handling_disabled
-bool arming_lockdown
 bool arming_force_failsafe
+bool arming_io_arm_ok
+bool arming_lockdown
 bool arming_termination_failsafe
-bool arming_override_immediate

+uint16[8] pwm
+uint16[8] pwm_disarmed
+uint16[8] pwm_failsafe

-int16[8] actuators
-uint16[8] servos
+uint16[8] pwm_rate_hz

 uint16[18] raw_inputs
@@ -4,41 +4,36 @@ uint8 FUNCTION_THROTTLE      = 0
 uint8 FUNCTION_ROLL          = 1
 uint8 FUNCTION_PITCH         = 2
 uint8 FUNCTION_YAW           = 3
-uint8 FUNCTION_MODE          = 4
-uint8 FUNCTION_RETURN        = 5
-uint8 FUNCTION_POSCTL        = 6
-uint8 FUNCTION_LOITER        = 7
-uint8 FUNCTION_OFFBOARD      = 8
-uint8 FUNCTION_ACRO          = 9
-uint8 FUNCTION_FLAPS         = 10
-uint8 FUNCTION_AUX_1         = 11
-uint8 FUNCTION_AUX_2         = 12
-uint8 FUNCTION_AUX_3         = 13
-uint8 FUNCTION_AUX_4         = 14
-uint8 FUNCTION_AUX_5         = 15
-uint8 FUNCTION_PARAM_1       = 16
-uint8 FUNCTION_PARAM_2       = 17
-uint8 FUNCTION_PARAM_3_5     = 18
-uint8 FUNCTION_KILLSWITCH    = 19
-uint8 FUNCTION_TRANSITION    = 20
-uint8 FUNCTION_GEAR          = 21
-uint8 FUNCTION_ARMSWITCH     = 22
-uint8 FUNCTION_STAB          = 23
-uint8 FUNCTION_AUX_6         = 24
-uint8 FUNCTION_MAN           = 25
-uint8 FUNCTION_FLTBTN_SLOT_1 = 26
-uint8 FUNCTION_FLTBTN_SLOT_2 = 27
-uint8 FUNCTION_FLTBTN_SLOT_3 = 28
-uint8 FUNCTION_FLTBTN_SLOT_4 = 39
-uint8 FUNCTION_FLTBTN_SLOT_5 = 30
-uint8 FUNCTION_FLTBTN_SLOT_6 = 31
+uint8 FUNCTION_RETURN        = 4
+uint8 FUNCTION_LOITER        = 5
+uint8 FUNCTION_OFFBOARD      = 6
+uint8 FUNCTION_FLAPS         = 7
+uint8 FUNCTION_AUX_1         = 8
+uint8 FUNCTION_AUX_2         = 9
+uint8 FUNCTION_AUX_3         = 10
+uint8 FUNCTION_AUX_4         = 11
+uint8 FUNCTION_AUX_5         = 12
+uint8 FUNCTION_AUX_6         = 13
+uint8 FUNCTION_PARAM_1       = 14
+uint8 FUNCTION_PARAM_2       = 15
+uint8 FUNCTION_PARAM_3_5     = 16
+uint8 FUNCTION_KILLSWITCH    = 17
+uint8 FUNCTION_TRANSITION    = 18
+uint8 FUNCTION_GEAR          = 19
+uint8 FUNCTION_ARMSWITCH     = 20
+uint8 FUNCTION_FLTBTN_SLOT_1 = 21
+uint8 FUNCTION_FLTBTN_SLOT_2 = 22
+uint8 FUNCTION_FLTBTN_SLOT_3 = 23
+uint8 FUNCTION_FLTBTN_SLOT_4 = 24
+uint8 FUNCTION_FLTBTN_SLOT_5 = 25
+uint8 FUNCTION_FLTBTN_SLOT_6 = 26

 uint8 FUNCTION_FLTBTN_SLOT_COUNT = 6

 uint64 timestamp_last_valid					# Timestamp of last valid RC signal
 float32[18] channels						# Scaled to -1..1 (throttle: 0..1)
 uint8 channel_count						# Number of valid channels
-int8[32] function						# Functions mapping
+int8[27] function						# Functions mapping
 uint8 rssi							# Receive signal strength index
 bool signal_lost						# Control signal lost, should be checked together with topic timeout
 uint32 frame_drop_count						# Number of dropped frames
@@ -1,5 +1,3 @@
 uint64 timestamp		# time since system start (microseconds)
 bool safety_switch_available		# Set to true if a safety switch is connected
 bool safety_off			# Set to true if safety is off
-bool override_available		# Set to true if external override system is connected
-bool override_enabled		# Set to true if override is engaged
@@ -1,7 +1,6 @@
 uint64 timestamp		# time since system start (microseconds)
 bool flag_armed			# synonym for actuator_armed.armed

-bool flag_external_manual_override_ok		# external override non-fatal for system. Only true for fixed wing
 bool flag_multicopter_position_control_enabled

 bool flag_control_manual_enabled		# true if manual input is mixed in
@@ -1,4 +1,5 @@
 # Fused local position in NED.
+# The coordinate system origin is the vehicle position at the time when the EKF2-module was started.

 uint64 timestamp			# time since system start (microseconds)
 uint64 timestamp_sample                 # the timestamp of the raw data (microseconds)
@@ -27,7 +27,6 @@ bool circuit_breaker_engaged_usb_check
 bool circuit_breaker_engaged_posfailure_check	# set to true when the position valid checks have been disabled
 bool circuit_breaker_vtol_fw_arming_check 	# set to true if for VTOLs arming in fixed-wing mode should be allowed

-bool offboard_control_signal_found_once
 bool offboard_control_signal_lost

 bool rc_signal_found_once
@@ -0,0 +1,219 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 Technology Innovation Institute. 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
+
+#ifdef PX4_CRYPTO
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include <px4_random.h>
+#include <px4_platform_common/crypto_algorithms.h>
+#include <px4_platform_common/sem.h>
+#include "crypto_backend_definitions.h"
+
+/*
+ * Crypto API interface class
+ */
+
+class PX4Crypto
+{
+public:
+	/*
+	 * Constructor & destructor
+	 */
+
+	PX4Crypto();
+	~PX4Crypto();
+
+	/*
+	 * Class function for crypto api initialization, called only once at
+	 * boot
+	 */
+
+	static void px4_crypto_init(void);
+
+	/*
+	 * Open crypto API for a specific algorithm
+	 * algorithm: The crypto algorithm to be used
+	 * returns true on success, false on failure
+	 */
+
+	bool open(px4_crypto_algorithm_t algorithm);
+
+	/*
+	 * Close the crypto API. Optional, it is also closed by destructing the
+	 * interface object
+	 */
+
+	void close();
+
+	/*
+	 * Keystore access functions
+	 */
+
+	/*
+	 * Generate a single random key for symmetric-key encryption
+	 *
+	 * idx: the index in keystore where the key will be stored
+	 * persistent: whether the key need to be stored persistently
+	 * returns true on success, false on failure
+	 */
+
+	bool generate_key(uint8_t idx,
+			  bool persistent);
+
+	/*
+	 * Generate a key pair for asymmetric-key encryption
+	 *
+	 * algorithm: the key type
+	 * key_size: size of the key in bytes
+	 * private_idx: the private key will be stored in this index in the keystore
+	 * public_idx: the public key will be stored in this index in the keystore
+	 * persistent: whether the keys need to be stored persistently
+	 * returns true on success, false on failure
+	*/
+
+	bool generate_keypair(size_t key_size,
+			      uint8_t private_idx,
+			      uint8_t public_idx,
+			      bool persistent);
+
+
+	/*
+	 * Re-create or set nonce.
+	 *
+	 * A nonce or intialization vector value for the selected algortithm is
+	 * automatically generated when the crypto session is opened. If needed, the
+	 * nonce can be set by this function.
+	 * If this is called with NULL pointer, a new nonce is automatically random
+	 * generated
+	 */
+
+	bool renew_nonce(const uint8_t *nonce, size_t nonce_size);
+
+
+	/*
+	 * Get current crypto session nonce
+	 *
+	 * This function returns the current nonce for the session
+	 * If the "nonce" is NULL, only nonse legth will be provided
+	 * nonce: pointer to the buffer where the nonce will be written
+	 * nonce_len: length of the current nonce vector for the session
+	 * returns true on success, false on failure
+	 */
+
+	bool get_nonce(uint8_t *nonce,
+		       size_t *nonce_len);
+
+	/*
+	 * Store a key into keystore
+	 *
+	 * encryption_idx: The key index in keystore to be used in decrypting and
+	 *   authenticating the key before storing
+	 * key: The pointer to the key
+	 * key_idx: Index where the key will be stored in keystore
+	*/
+
+
+	bool set_key(uint8_t encryption_idx,
+		     const uint8_t *key,
+		     uint8_t key_idx);
+
+	/*
+	 * Get a key from keystore. Key can be encrypted
+	 *
+	 * key_idx: Index of the requested key in the keystore
+	 * key: The provided buffer to the key. If NULL, the function only provides
+	 *          the length of the key.
+	 * key_len: input: the size of the provided "key" buffer.
+	            output: the actual size of the key
+	 * encryption_key_idx: The key index in keystore to be used for encrypting
+	 * returns true on success, false on failure
+	 *
+	 */
+
+	bool get_encrypted_key(uint8_t key_idx,
+			       uint8_t *key,
+			       size_t *key_len,
+			       uint8_t encryption_key_idx);
+
+	/*
+	 * PX4 Crypto API functions
+	 */
+
+	/*
+	 * Verify signature
+	 *
+	 * key_index: public key index in keystore
+	 * signature: pointer to the signature
+	 * message: pointer to the data to be verified
+	 * message_size: size of the message in bytes
+	*/
+
+	bool signature_check(uint8_t  key_index,
+			     const uint8_t *signature,
+			     const uint8_t *message,
+			     size_t message_size);
+
+
+	/*
+	 * Encrypt data. This always supports encryption in-place
+	 *
+	 * key_index: key index in keystore
+	 * message: pointer to the message
+	 * message_size: size of the message in bytes
+	 * cipher: pointer to a buffer for encrypted data
+	 * cipher_size: size of the buffer reserved for cipher and actual cipher length
+	 *   after the encryption
+	 * returns true on success, false on failure
+	 */
+
+	bool encrypt_data(uint8_t  key_index,
+			  const uint8_t *message,
+			  size_t message_size,
+			  uint8_t *cipher,
+			  size_t *cipher_size);
+
+	size_t get_min_blocksize(uint8_t key_idx);
+
+private:
+	crypto_session_handle_t _crypto_handle;
+	static px4_sem_t _lock;
+	static bool _initialized;
+	static void lock() { do {} while (px4_sem_wait(&PX4Crypto::_lock) != 0); }
+	static void unlock() { px4_sem_post(&PX4Crypto::_lock); }
+};
+
+#endif
@@ -71,6 +71,16 @@ void keystore_close(keystore_session_handle_t *handle);
 */
 size_t keystore_get_key(keystore_session_handle_t handle, uint8_t idx, uint8_t *key_buf, size_t key_buf_size);

+/*
+ * Store a key persistently into the keystore
+ * handle: a handle to an open keystore
+ * idx: key index in keystore
+ * key: pointer to the key
+ * key_size: size of the key
+ */
+bool keystore_put_key(keystore_session_handle_t handle, uint8_t idx, uint8_t *key, size_t key_size);
+
+
 /*
 * Architecture specific PX4 Crypto API functions
 */
@@ -96,6 +106,48 @@ crypto_session_handle_t crypto_open(px4_crypto_algorithm_t algorithm);

 void crypto_close(crypto_session_handle_t *handle);

+/*
+ * Generate a key
+ * handle: Open handle for the crypto session. The key will be generated for
+ *         the crypto algorithm used by this session
+ * idx: The key index, by which the key can be used
+ * persistent: if set to "true", the key will be stored into the keystore
+ */
+bool crypto_generate_key(crypto_session_handle_t handle,
+			 uint8_t idx,
+			 bool persistent);
+
+/*
+ * Get a key from keystore, possibly encrypted
+ *
+ * handle: an open crypto context; the returned key will be encrypted
+ *   according to this context
+ * key_idx: Index of the requested key in the keystore
+ * key: The provided buffer to the key
+ * max_len: the length of the provided key buffer. Returns the actual key size
+ * encryption_key_idx: The key index in keystore to be used for encrypting
+ * returns true on success, false on failure
+ */
+bool crypto_get_encrypted_key(crypto_session_handle_t handle,
+			      uint8_t key_idx,
+			      uint8_t *key,
+			      size_t *max_len,
+			      uint8_t encryption_key_idx);
+
+/*
+ * Get the generated nonce value
+ *
+ * handle: an open crypto context; the returned nonce is the one associsated
+ *   with this context/algorithm
+ * nonce: The provided buffer to the key. If NULL, only length is returned
+ * nonce_len: the length of the nonce value
+ * encryption_key_idx: The key index in keystore to be used for encrypting
+ * returns true on success, false on failure
+ */
+bool crypto_get_nonce(crypto_session_handle_t handle,
+		      uint8_t *nonce,
+		      size_t *nonce_len);
+
 /*
 * Perform signature check using an open session to crypto
 * handle: session handle, returned by open
@@ -110,6 +162,25 @@ bool crypto_signature_check(crypto_session_handle_t handle,
 			    const uint8_t *message,
 			    size_t message_size);

+bool crypto_encrypt_data(crypto_session_handle_t handle,
+			 uint8_t  key_index,
+			 const uint8_t *message,
+			 size_t message_size,
+			 uint8_t *cipher,
+			 size_t *cipher_size);
+
+/*
+ * Returns a minimum data block size on which the crypto operations can be
+ *   performed. Performing encryption on sizes which are not multiple of this
+ *   are valid, but the output will be padded to the multiple of this value
+ *   Input for decryption must be multiple of this value.
+ * handle: session handle, returned by open
+ * key_idx: key to be used for encryption/decryption
+ * returs the block size
+ */
+
+size_t crypto_get_min_blocksize(crypto_session_handle_t handle, uint8_t key_idx);
+
 #if defined(__cplusplus)
 } // extern "C"
 #endif
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2019-2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019-2021 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -77,16 +77,16 @@ static constexpr wq_config_t hp_default{"wq:hp_default", 1900, -18};

 static constexpr wq_config_t uavcan{"wq:uavcan", 3624, -19};

-static constexpr wq_config_t UART0{"wq:UART0", 1536, -21};
-static constexpr wq_config_t UART1{"wq:UART1", 1536, -22};
-static constexpr wq_config_t UART2{"wq:UART2", 1536, -23};
-static constexpr wq_config_t UART3{"wq:UART3", 1536, -24};
-static constexpr wq_config_t UART4{"wq:UART4", 1536, -25};
-static constexpr wq_config_t UART5{"wq:UART5", 1536, -26};
-static constexpr wq_config_t UART6{"wq:UART6", 1536, -27};
-static constexpr wq_config_t UART7{"wq:UART7", 1536, -28};
-static constexpr wq_config_t UART8{"wq:UART8", 1536, -29};
-static constexpr wq_config_t UART_UNKNOWN{"wq:UART_UNKNOWN", 1536, -30};
+static constexpr wq_config_t UART0{"wq:UART0", 1632, -21};
+static constexpr wq_config_t UART1{"wq:UART1", 1632, -22};
+static constexpr wq_config_t UART2{"wq:UART2", 1632, -23};
+static constexpr wq_config_t UART3{"wq:UART3", 1632, -24};
+static constexpr wq_config_t UART4{"wq:UART4", 1632, -25};
+static constexpr wq_config_t UART5{"wq:UART5", 1632, -26};
+static constexpr wq_config_t UART6{"wq:UART6", 1632, -27};
+static constexpr wq_config_t UART7{"wq:UART7", 1632, -28};
+static constexpr wq_config_t UART8{"wq:UART8", 1632, -29};
+static constexpr wq_config_t UART_UNKNOWN{"wq:UART_UNKNOWN", 1632, -30};

 static constexpr wq_config_t lp_default{"wq:lp_default", 1920, -50};

@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2021 Technology Innovation Institute. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -31,25 +31,17 @@
 *
 ****************************************************************************/

+#pragma once
+
+#include <stdint.h>
+#include <visibility.h>
+
 /*
- * @file rgbled_params.c
- *
- * Parameters defined by the RBG led driver
- *
- * @author Nate Weibley <nate.weibley@prioria.com>
+ * Random number generator provided by the operating system. This can be used
+ * by the crypto_arch if needed, or it can implement an own HW-specific rng
 */

-
-#include <px4_platform_common/px4_config.h>
-#include <parameters/param.h>
-
-/**
- * RGB Led brightness limit
- *
- * Set to 0 to disable, 1 for minimum brightness up to 15 (max)
- *
- * @min 0
- * @max 15
- * @group System
- */
-PARAM_DEFINE_INT32(LED_RGB_MAXBRT, 15);
+__BEGIN_DECLS
+size_t px4_get_secure_random(uint8_t *out,
+			     size_t outlen);
+__END_DECLS
@@ -37,6 +37,5 @@ typedef struct {
 	int handle;
 } keystore_session_handle_t;

-/* For the stub_keystore the handle is not used at the moment, so it is always valid */
-
-inline bool keystore_session_handle_valid(keystore_session_handle_t handle) {return handle.handle > 0;}
+static inline void keystore_session_handle_init(keystore_session_handle_t *handle) {handle->handle = 0;}
+static inline bool keystore_session_handle_valid(keystore_session_handle_t handle) {return handle.handle > 0;}
@@ -77,3 +77,8 @@ size_t keystore_get_key(keystore_session_handle_t handle, uint8_t idx, uint8_t *

 	return ret;
 }
+
+bool keystore_put_key(keystore_session_handle_t handle, uint8_t idx, uint8_t *key, size_t key_size)
+{
+	return false;
+}
@@ -41,8 +41,9 @@ target_link_libraries(crypto_backend

 target_link_libraries(crypto_backend
 	PRIVATE
+		px4_random
 		monocypher
+		libtomcrypt
 )

 target_include_directories(crypto_backend PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
-
@@ -42,7 +42,11 @@
 #include <stdbool.h>

 #include <px4_platform_common/crypto_backend.h>
+#include <px4_random.h>
 #include <lib/crypto/monocypher/src/optional/monocypher-ed25519.h>
+#include <tomcrypt.h>
+
+extern void libtomcrypt_init(void);

 /* room for 16 keys */
 #define KEY_CACHE_LEN 16
@@ -67,6 +71,11 @@ typedef struct {

 static volatile_key_t key_cache[KEY_CACHE_LEN];

+typedef struct {
+	uint8_t nonce[24];
+	uint64_t ctr;
+} chacha20_context_t;
+
 /* Clear key cache */
 static void clear_key_cache(void)
 {
@@ -126,19 +135,43 @@ void crypto_init()
 {
 	keystore_init();
 	clear_key_cache();
+	libtomcrypt_init();
 }

 crypto_session_handle_t crypto_open(px4_crypto_algorithm_t algorithm)
 {
 	crypto_session_handle_t ret;
-	ret.algorithm = algorithm;
 	ret.keystore_handle = keystore_open();

 	if (keystore_session_handle_valid(ret.keystore_handle)) {
+		ret.algorithm = algorithm;
 		ret.handle = ++crypto_open_count;

 	} else {
 		ret.handle = 0;
+		ret.context = NULL;
+		ret.algorithm = CRYPTO_NONE;
+		return ret;
+	}
+
+	switch (algorithm) {
+	case CRYPTO_XCHACHA20: {
+			chacha20_context_t *context = malloc(sizeof(chacha20_context_t));
+
+			if (!context) {
+				ret.handle = 0;
+				crypto_open_count--;
+
+			} else {
+				ret.context = context;
+				px4_get_secure_random(context->nonce, sizeof(context->nonce));
+				context->ctr = 0;
+			}
+		}
+		break;
+
+	default:
+		ret.context = NULL;
 	}

 	return ret;
@@ -146,9 +179,17 @@ crypto_session_handle_t crypto_open(px4_crypto_algorithm_t algorithm)

 void crypto_close(crypto_session_handle_t *handle)
 {
+
+	// Not open?
+	if (!crypto_session_handle_valid(*handle)) {
+		return;
+	}
+
 	crypto_open_count--;
 	handle->handle = 0;
 	keystore_close(&handle->keystore_handle);
+	free(handle->context);
+	handle->context = NULL;
 }

 bool crypto_signature_check(crypto_session_handle_t handle,
@@ -159,13 +200,13 @@ bool crypto_signature_check(crypto_session_handle_t handle,
 {
 	bool ret = false;
 	size_t keylen = 0;
-	const uint8_t *public_key;
+	const uint8_t *public_key = NULL;

 	if (crypto_session_handle_valid(handle)) {
 		public_key = crypto_get_key_ptr(handle.keystore_handle, key_index, &keylen);
 	}

-	if (keylen == 0) {
+	if (keylen == 0 || public_key == NULL) {
 		return false;
 	}

@@ -180,3 +221,212 @@ bool crypto_signature_check(crypto_session_handle_t handle,

 	return ret;
 }
+
+bool crypto_encrypt_data(crypto_session_handle_t handle,
+			 uint8_t  key_idx,
+			 const uint8_t *message,
+			 size_t message_size,
+			 uint8_t *cipher,
+			 size_t *cipher_size)
+{
+
+	bool ret = false;
+
+	if (!crypto_session_handle_valid(handle)) {
+		return ret;
+	}
+
+	switch (handle.algorithm) {
+	case CRYPTO_NONE:
+		if (*cipher_size >= message_size) {
+			/* In-place there is no copy needed */
+			if (message != cipher) {
+				memcpy(cipher, message, message_size);
+			}
+
+			*cipher_size = message_size;
+			ret = true;
+		}
+
+		break;
+
+	case CRYPTO_XCHACHA20: {
+			size_t key_sz;
+			uint8_t *key = (uint8_t *)crypto_get_key_ptr(handle.keystore_handle, key_idx, &key_sz);
+			chacha20_context_t *context = handle.context;
+
+			if (key_sz == 32) {
+				context->ctr = crypto_xchacha20_ctr(cipher, message, *cipher_size, key, context->nonce, context->ctr);
+				ret = true;
+			}
+		}
+		break;
+
+	case CRYPTO_RSA_OAEP: {
+			rsa_key key;
+			size_t key_sz;
+			unsigned long outlen = *cipher_size;
+			uint8_t *public_key = (uint8_t *)crypto_get_key_ptr(handle.keystore_handle, key_idx, &key_sz);
+			*cipher_size = 0;
+
+			if (public_key &&
+			    rsa_import(public_key, key_sz, &key) == CRYPT_OK) {
+				if (outlen >= ltc_mp.unsigned_size(key.N) &&
+				    pkcs_1_oaep_encode(message, message_size,
+						       NULL, 0,
+						       ltc_mp.count_bits(key.N),
+						       NULL,
+						       0, 0,
+						       cipher, &outlen) == CRYPT_OK &&
+				    ltc_mp.rsa_me(cipher, outlen, cipher, &outlen, PK_PUBLIC, &key) == CRYPT_OK) {
+					*cipher_size = outlen;
+					ret = true;
+
+				}
+
+				rsa_free(&key);
+			}
+		}
+		break;
+
+	default:
+		break;
+	}
+
+	return ret;
+}
+
+bool crypto_generate_key(crypto_session_handle_t handle,
+			 uint8_t idx, bool persistent)
+{
+	bool ret = false;
+
+	if (idx >= KEY_CACHE_LEN) {
+		return false;
+	}
+
+	switch (handle.algorithm) {
+	case CRYPTO_XCHACHA20:
+		if (key_cache[idx].key_size < 32) {
+			if (key_cache[idx].key_size > 0) {
+				SECMEM_FREE(key_cache[idx].key);
+				key_cache[idx].key_size = 0;
+			}
+
+			key_cache[idx].key = SECMEM_ALLOC(32);
+		}
+
+		if (key_cache[idx].key) {
+			key_cache[idx].key_size = 32;
+			px4_get_secure_random(key_cache[idx].key, 32);
+			ret = true;
+
+		} else {
+			key_cache[idx].key_size = 0;
+		}
+
+		break;
+
+	default:
+		break;
+	}
+
+	if (ret && persistent) {
+		keystore_put_key(handle.keystore_handle, idx, key_cache[idx].key, key_cache[idx].key_size);
+	}
+
+	return ret;
+}
+
+bool crypto_get_encrypted_key(crypto_session_handle_t handle,
+			      uint8_t key_idx,
+			      uint8_t *key,
+			      size_t *max_len,
+			      uint8_t encryption_key_idx)
+{
+	// Retrieve the plaintext key
+	bool ret = true;
+	size_t key_sz;
+	const uint8_t *plain_key = crypto_get_key_ptr(handle.keystore_handle, key_idx, &key_sz);
+
+	if (key_sz == 0) {
+		return false;
+	}
+
+	// Encrypt it
+	if (key != NULL) {
+		ret = crypto_encrypt_data(handle,
+					  encryption_key_idx,
+					  plain_key,
+					  key_sz,
+					  key,
+					  max_len);
+
+	} else {
+		// The key size, encrypted, is a multiple of minimum block size for the algorithm+key
+		size_t min_block = crypto_get_min_blocksize(handle, encryption_key_idx);
+		*max_len = key_sz / min_block * min_block;
+
+		if (key_sz % min_block) {
+			*max_len += min_block;
+		}
+	}
+
+	return ret;
+}
+
+
+bool crypto_get_nonce(crypto_session_handle_t handle,
+		      uint8_t *nonce,
+		      size_t *nonce_len)
+{
+	switch (handle.algorithm) {
+	case CRYPTO_XCHACHA20: {
+			chacha20_context_t *context = handle.context;
+
+			if (nonce != NULL && context != NULL) {
+				memcpy(nonce, context->nonce, sizeof(context->nonce));
+			}
+
+			*nonce_len = sizeof(context->nonce);
+		}
+		break;
+
+	default:
+		*nonce_len = 0;
+	}
+
+	return true;
+}
+
+size_t crypto_get_min_blocksize(crypto_session_handle_t handle, uint8_t key_idx)
+{
+	size_t ret;
+
+	switch (handle.algorithm) {
+	case CRYPTO_XCHACHA20:
+		ret = 64;
+		break;
+
+	case CRYPTO_RSA_OAEP: {
+			rsa_key enc_key;
+			unsigned pub_key_sz;
+			uint8_t *pub_key = (uint8_t *)crypto_get_key_ptr(handle.keystore_handle, key_idx, &pub_key_sz);
+
+			if (pub_key &&
+			    rsa_import(pub_key, pub_key_sz, &enc_key) == CRYPT_OK) {
+				ret = ltc_mp.unsigned_size(enc_key.N);
+				rsa_free(&enc_key);
+
+			} else {
+				ret = 0;
+			}
+		}
+		break;
+
+	default:
+		ret = 1;
+	}
+
+	return ret;
+}
@@ -39,8 +39,9 @@
 typedef struct {
 	int handle;
 	px4_crypto_algorithm_t algorithm;
-	uint8_t *nonce;
 	keystore_session_handle_t keystore_handle;
+	void *context;
 } crypto_session_handle_t;

-static inline bool crypto_session_handle_valid(crypto_session_handle_t handle) {return handle.handle > 0;}
+static inline void crypto_session_handle_init(crypto_session_handle_t *handle) {handle->handle = 0;}
+static inline bool crypto_session_handle_valid(crypto_session_handle_t handle) {return handle.handle > 0 && keystore_session_handle_valid(handle.keystore_handle);}
@@ -369,7 +369,9 @@ void uORB::DeviceMaster::showTop(char **topic_filter, int num_filters)

 		if (!quit) {

-			//update the stats
+			// update the stats
+			int total_size = 0;
+			int total_msgs = 0;
 			hrt_abstime current_time = hrt_absolute_time();
 			float dt = (current_time - start_time) / 1.e6f;
 			cur_node = first_node;
@@ -378,6 +380,10 @@ void uORB::DeviceMaster::showTop(char **topic_filter, int num_filters)
 				unsigned int num_msgs = cur_node->node->updates_available(cur_node->last_pub_msg_count);
 				cur_node->pub_msg_delta = roundf(num_msgs / dt);
 				cur_node->last_pub_msg_count += num_msgs;
+
+				total_size += cur_node->pub_msg_delta * cur_node->node->get_meta()->o_size;
+				total_msgs += cur_node->pub_msg_delta;
+
 				cur_node = cur_node->next;
 			}

@@ -388,7 +394,8 @@ void uORB::DeviceMaster::showTop(char **topic_filter, int num_filters)
 				PX4_INFO_RAW("\033[H"); // move cursor to top left corner
 			}

-			PX4_INFO_RAW(CLEAR_LINE "update: 1s, num topics: %i\n", num_topics);
+			PX4_INFO_RAW(CLEAR_LINE "update: 1s, topics: %i, total publications: %i, %.1f kB/s\n",
+				     num_topics, total_msgs, (double)(total_size / 1000.f));
 			PX4_INFO_RAW(CLEAR_LINE "%-*s INST #SUB RATE #Q SIZE\n", (int)max_topic_name_length - 2, "TOPIC NAME");
 			cur_node = first_node;

@@ -404,6 +411,7 @@ void uORB::DeviceMaster::showTop(char **topic_filter, int num_filters)
 				cur_node = cur_node->next;
 			}

+
 			if (!only_once) {
 				PX4_INFO_RAW("\033[0J"); // clear the rest of the screen
 			}
@@ -83,7 +83,7 @@ int uORBTest::UnitTest::pubsublatency_main()
 		if (fds[0].revents & POLLIN) {
 			orb_copy(ORB_ID(orb_test_medium), test_multi_sub, &t);

-			unsigned elt = (unsigned)hrt_elapsed_time_atomic(&t.timestamp);
+			unsigned elt = (unsigned)hrt_elapsed_time(&t.timestamp);
 			latency_integral += elt;
 			timings[i] = elt;

@@ -94,6 +94,7 @@ list(APPEND nuttx_libs
 	nuttx_fs
 	nuttx_mm
 	nuttx_sched
+	nuttx_crypto
 )

 if (CONFIG_NET)
@@ -319,6 +319,7 @@ add_nuttx_dir(sched sched y -D__KERNEL__)
 add_nuttx_dir(c libs/libc n "")
 add_nuttx_dir(xx libs/libxx n "")
 add_nuttx_dir(mm mm n "")
+add_nuttx_dir(crypto crypto y -D__KERNEL__)

 if(CONFIG_NET)
 	add_nuttx_dir(net net y -D__KERNEL__)
@@ -57,6 +57,7 @@ typedef enum {
 	fw_update_timeout,
 	fw_update_invalid_crc,
 	jump_to_app,
+	hardware_failure,
 } uiindication_t;

 #ifndef __ASSEMBLY__
@@ -48,6 +48,7 @@ if(NOT PX4_BOARD MATCHES "io-v2")
 		px4_manifest.cpp
 		px4_mtd.cpp
 		px4_24xxxx_mtd.c
+		px4_crypto.cpp
 	)
 	target_link_libraries(px4_layer
 		PRIVATE
@@ -58,7 +59,7 @@ if(NOT PX4_BOARD MATCHES "io-v2")
 			nuttx_sched
 			px4_work_queue
 			uORB
-		)
+	)
 else()
 	add_library(px4_layer ${PX4_SOURCE_DIR}/platforms/common/empty.c)
 endif()
@@ -66,3 +67,14 @@ add_dependencies(px4_layer prebuild_targets)

 add_subdirectory(gpio)
 add_subdirectory(srgbled)
+
+if (DEFINED PX4_CRYPTO)
+	add_library(px4_random nuttx_random.c)
+	add_dependencies(px4_random nuttx_context)
+	target_link_libraries(px4_random PRIVATE nuttx_crypto)
+
+	target_link_libraries(px4_layer
+		PUBLIC
+			crypto_backend
+	)
+endif()
@@ -1,14 +1,14 @@
 /****************************************************************************
 *
- *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2021 Technology Innovation Institute. 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. Redistribution of source code must retain the above copyright
+ * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
- * 2. Redistribution in binary form must reproduce the above copyright
+ * 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.
@@ -31,25 +31,16 @@
 *
 ****************************************************************************/

-/*
- * @file rgbled_ncp5623c_params.c
- *
- * Parameters defined by the RBG led driver
- *
- * @author CUAVcaijie <caijie@cuav.net>
- */
+#include <nuttx/random.h>

-
-#include <px4_platform_common/px4_config.h>
-#include <parameters/param.h>
-
-/**
- * RGB Led brightness limit
- *
- * Set to 0 to disable, 1 for minimum brightness up to 31 (max)
- *
- * @min 0
- * @max 31
- * @group System
- */
-PARAM_DEFINE_INT32(LED_RGB1_MAXBRT, 31);
+#if defined(CONFIG_CRYPTO_RANDOM_POOL)
+size_t px4_get_secure_random(uint8_t *out,
+			     size_t outlen)
+{
+	/* TODO: can getrandom fail?? */
+	arc4random_buf(out, outlen);
+	return outlen;
+}
+#else
+#error CONFIG_CRYPTO_RANDOM_POOL has to be defined
+#endif
@@ -0,0 +1,149 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 Technology Innovation Institute. 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 defined(PX4_CRYPTO)
+
+#include <px4_platform_common/crypto.h>
+#include <px4_platform_common/crypto_backend.h>
+
+extern "C" {
+#include <nuttx/random.h>
+}
+
+
+px4_sem_t PX4Crypto::_lock;
+bool PX4Crypto::_initialized = false;
+
+void PX4Crypto::px4_crypto_init()
+{
+	if (PX4Crypto::_initialized) {
+		return;
+	}
+
+	px4_sem_init(&PX4Crypto::_lock, 0, 1);
+
+	// Initialize nuttx random pool, if it is being used by crypto
+#ifdef CONFIG_CRYPTO_RANDOM_POOL
+	up_randompool_initialize();
+#endif
+
+	// initialize keystore functionality
+	keystore_init();
+
+	// initialize actual crypto algoritms
+	crypto_init();
+
+	PX4Crypto::_initialized = true;
+}
+
+PX4Crypto::PX4Crypto()
+{
+	// Initialize an empty handle
+	crypto_session_handle_init(&_crypto_handle);
+}
+
+PX4Crypto::~PX4Crypto()
+{
+	close();
+}
+
+bool PX4Crypto::open(px4_crypto_algorithm_t algorithm)
+{
+	bool ret = false;
+	lock();
+
+	// HW specific crypto already open? Just close before proceeding
+	if (crypto_session_handle_valid(_crypto_handle)) {
+		crypto_close(&_crypto_handle);
+	}
+
+	// Open the HW specific crypto handle
+	_crypto_handle = crypto_open(algorithm);
+
+	if (crypto_session_handle_valid(_crypto_handle)) {
+		ret = true;
+	}
+
+	unlock();
+
+	return ret;
+}
+
+void PX4Crypto::close()
+{
+	if (!crypto_session_handle_valid(_crypto_handle)) {
+		return;
+	}
+
+	lock();
+	crypto_close(&_crypto_handle);
+	unlock();
+}
+
+bool PX4Crypto::encrypt_data(uint8_t  key_index,
+			     const uint8_t *message,
+			     size_t message_size,
+			     uint8_t *cipher,
+			     size_t *cipher_size)
+{
+	return crypto_encrypt_data(_crypto_handle, key_index, message, message_size, cipher, cipher_size);
+}
+
+bool  PX4Crypto::generate_key(uint8_t idx,
+			      bool persistent)
+{
+	return crypto_generate_key(_crypto_handle, idx, persistent);
+}
+
+
+bool  PX4Crypto::get_nonce(uint8_t *nonce,
+			   size_t *nonce_len)
+{
+	return crypto_get_nonce(_crypto_handle, nonce, nonce_len);
+}
+
+
+bool  PX4Crypto::get_encrypted_key(uint8_t key_idx,
+				   uint8_t *key,
+				   size_t *key_len,
+				   uint8_t encryption_key_idx)
+{
+	return crypto_get_encrypted_key(_crypto_handle, key_idx, key, key_len, encryption_key_idx);
+}
+
+size_t PX4Crypto::get_min_blocksize(uint8_t key_idx)
+{
+	return crypto_get_min_blocksize(_crypto_handle, key_idx);
+}
+
+#endif
@@ -52,6 +52,10 @@
 # include <nuttx/i2c/i2c_master.h>
 #endif // CONFIG_I2C

+#if defined(PX4_CRYPTO)
+#include <px4_platform_common/crypto.h>
+#endif
+
 int px4_platform_init()
 {

@@ -70,6 +74,10 @@ int px4_platform_init()
 		close(fd_buf);
 	}

+#if defined(PX4_CRYPTO)
+	PX4Crypto::px4_crypto_init();
+#endif
+
 	hrt_init();

 	param_init();
@@ -586,21 +586,6 @@ abstime_to_ts(struct timespec *ts, hrt_abstime abstime)
 	ts->tv_nsec = abstime * 1000;
 }

-/**
- * Compare a time value with the current time as atomic operation.
- */
-hrt_abstime
-hrt_elapsed_time_atomic(const volatile hrt_abstime *then)
-{
-	irqstate_t flags = px4_enter_critical_section();
-
-	hrt_abstime delta = hrt_absolute_time() - *then;
-
-	px4_leave_critical_section(flags);
-
-	return delta;
-}
-
 /**
 * Store the absolute time in an interrupt-safe fashion
 */
@@ -87,10 +87,6 @@ __BEGIN_DECLS
 #define PX4_CPU_UUID_WORD32_FORMAT_SIZE         (PX4_CPU_UUID_WORD32_LENGTH-1+(2*PX4_CPU_UUID_BYTE_LENGTH)+1)
 #define PX4_CPU_MFGUID_FORMAT_SIZE              ((2*PX4_CPU_MFGUID_BYTE_LENGTH)+1)

-#define kinetis_bbsram_savepanic(fileno, context, length) (0) // todo:Not implemented yet
-
-#define px4_savepanic(fileno, context, length)   kinetis_bbsram_savepanic(fileno, context, length)
-
 /* bus_num is zero based on kinetis and must be translated from the legacy one based */

 #define PX4_BUS_OFFSET       1                  /* Kinetis buses are 0 based and adjustment is needed */
@@ -598,21 +598,6 @@ abstime_to_ts(struct timespec *ts, hrt_abstime abstime)
 	ts->tv_nsec = abstime * 1000;
 }

-/**
- * Compare a time value with the current time as atomic operation.
- */
-hrt_abstime
-hrt_elapsed_time_atomic(const volatile hrt_abstime *then)
-{
-	irqstate_t flags = px4_enter_critical_section();
-
-	hrt_abstime delta = hrt_absolute_time() - *then;
-
-	px4_leave_critical_section(flags);
-
-	return delta;
-}
-
 /**
 * Store the absolute time in an interrupt-safe fashion
 */
@@ -82,10 +82,6 @@ __BEGIN_DECLS
 #define PX4_CPU_UUID_WORD32_FORMAT_SIZE         (PX4_CPU_UUID_WORD32_LENGTH-1+(2*PX4_CPU_UUID_BYTE_LENGTH)+1)
 #define PX4_CPU_MFGUID_FORMAT_SIZE              ((2*PX4_CPU_MFGUID_BYTE_LENGTH)+1)

-#define imxrt_bbsram_savepanic(fileno, context, length) (0) // todo:Not implemented yet
-
-#define px4_savepanic(fileno, context, length)   imxrt_bbsram_savepanic(fileno, context, length)
-
 /* bus_num is 1 based on imx and must be translated from the legacy one based */

 #define PX4_BUS_OFFSET       0                  /* imxrt buses are 1 based no adjustment needed */
@@ -89,10 +89,6 @@ __BEGIN_DECLS
 #define PX4_CPU_UUID_WORD32_FORMAT_SIZE         (PX4_CPU_UUID_WORD32_LENGTH-1+(2*PX4_CPU_UUID_BYTE_LENGTH)+1)
 #define PX4_CPU_MFGUID_FORMAT_SIZE              ((2*PX4_CPU_MFGUID_BYTE_LENGTH)+1)

-#define s32k1xx_bbsram_savepanic(fileno, context, length) (0) // todo:Not implemented yet
-
-#define px4_savepanic(fileno, context, length)   s32k1xx_bbsram_savepanic(fileno, context, length)
-
 /* bus_num is zero based on s32k1xx and must be translated from the legacy one based */

 #define PX4_BUS_OFFSET       1                  /* s32k1xx buses are 0 based and adjustment is needed */
@@ -634,21 +634,6 @@ abstime_to_ts(struct timespec *ts, hrt_abstime abstime)
 	ts->tv_nsec = abstime * 1000;
 }

-/**
- * Compare a time value with the current time as atomic operation.
- */
-hrt_abstime
-hrt_elapsed_time_atomic(const volatile hrt_abstime *then)
-{
-	irqstate_t flags = px4_enter_critical_section();
-
-	hrt_abstime delta = hrt_absolute_time() - *then;
-
-	px4_leave_critical_section(flags);
-
-	return delta;
-}
-
 /**
 * Store the absolute time in an interrupt-safe fashion
 */
@@ -719,21 +719,6 @@ abstime_to_ts(struct timespec *ts, hrt_abstime abstime)
 	ts->tv_nsec = abstime * 1000;
 }

-/**
- * Compare a time value with the current time as atomic operation
- */
-hrt_abstime
-hrt_elapsed_time_atomic(const volatile hrt_abstime *then)
-{
-	irqstate_t flags = px4_enter_critical_section();
-
-	hrt_abstime delta = hrt_absolute_time() - *then;
-
-	px4_leave_critical_section(flags);
-
-	return delta;
-}
-
 /**
 * Store the absolute time in an interrupt-safe fashion
 */
@@ -318,7 +318,8 @@ ArchPX4IOSerial::_bus_exchange(IOPacket *_packet)
 			  DMA_SCR_PSIZE_8BITS   |
 			  DMA_SCR_MSIZE_8BITS   |
 			  DMA_SCR_PBURST_SINGLE |
-			  DMA_SCR_MBURST_SINGLE);
+			  DMA_SCR_MBURST_SINGLE |
+			  DMA_SCR_TRBUFF);
 	rxdmacfg.cfg2  = 0;


@@ -343,7 +344,8 @@ ArchPX4IOSerial::_bus_exchange(IOPacket *_packet)
 			  DMA_SCR_PSIZE_8BITS   |
 			  DMA_SCR_MSIZE_8BITS   |
 			  DMA_SCR_PBURST_SINGLE |
-			  DMA_SCR_MBURST_SINGLE);
+			  DMA_SCR_MBURST_SINGLE |
+			  DMA_SCR_TRBUFF);
 	txdmacfg.cfg2  = 0;

 	stm32_dmasetup(_tx_dma, &txdmacfg);
@@ -8,7 +8,6 @@ set(tests
 	bezier
 	bitset
 	bson
-	conv
 	dataman
 	file2
 	float
@@ -157,20 +157,6 @@ hrt_abstime ts_to_abstime(const struct timespec *ts)
 	return result;
 }

-/*
- * Compute the delta between a timestamp taken in the past
- * and now.
- *
- * This function is safe to use even if the timestamp is updated
- * by an interrupt during execution.
- */
-hrt_abstime hrt_elapsed_time_atomic(const volatile hrt_abstime *then)
-{
-	// This is not atomic as the value on the application layer of POSIX is limited.
-	hrt_abstime delta = hrt_absolute_time() - *then;
-	return delta;
-}
-
 /*
 * Store the absolute time in an interrupt-safe fashion.
 *
@@ -5,7 +5,6 @@

 param load
 param set CBRK_SUPPLY_CHK 894281
-param set SYS_RESTART_TYPE 0

 dataman start

@@ -5,7 +5,6 @@

 param load
 param set CBRK_SUPPLY_CHK 894281
-param set SYS_RESTART_TYPE 0

 dataman start

@@ -5,7 +5,6 @@

 param load
 param set CBRK_SUPPLY_CHK 894281
-param set SYS_RESTART_TYPE 0

 dataman start

@@ -9,7 +9,6 @@ param set BAT1_N_CELLS 3
 param set CBRK_SUPPLY_CHK 894281
 param set MAV_TYPE 22
 param set VT_TYPE 2
-param set SYS_RESTART_TYPE 0

 dataman start

@@ -5,7 +5,6 @@

 param load
 param set CBRK_SUPPLY_CHK 894281
-param set SYS_RESTART_TYPE 0

 dataman start

@@ -47,6 +47,9 @@ if(NOT "${PX4_BOARD_LABEL}" MATCHES  "canbootloader")
 	set(HW_MINOR ${uavcanblid_hw_version_minor})
 	set(SW_MAJOR ${uavcanblid_sw_version_major})
 	set(SW_MINOR ${uavcanblid_sw_version_minor})
+	if("${uavcanbl_padding}" STREQUAL "")
+		set(uavcanbl_padding 4)
+	endif()

 	math(EXPR HWBOARD_ID "(${HW_MAJOR} << 8) + ${HW_MINOR}")

@@ -68,7 +71,7 @@ if(NOT "${PX4_BOARD_LABEL}" MATCHES  "canbootloader")
 			${PX4_BINARY_DIR}/${uavcan_bl_image_name}
 			${PX4_BINARY_DIR}/deploy/${HWBOARD_ID}.bin
 		COMMAND
-			${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/make_can_boot_descriptor.py -v --use-git-hash ${PX4_BOARD}.bin ${uavcan_bl_image_name}
+			${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/make_can_boot_descriptor.py -v --padding ${uavcanbl_padding} --use-git-hash ${PX4_BOARD}.bin ${uavcan_bl_image_name}
 		COMMAND
 			 COMMAND ${CMAKE_COMMAND} -E make_directory deploy
 		COMMAND
@@ -120,18 +120,17 @@ class AppDescriptor(object):


 class FirmwareImage(object):
-    def __init__(self, path_or_file, mode="r"):
+    def __init__(self, path_or_file, mode="r", padding = 4):
+        self._padding = padding;
        if getattr(path_or_file, "read", None):
            self._file = path_or_file
            self._do_close = False
-            self._padding = 0
        else:
            if "b" not in mode:
                self._file = open(path_or_file, mode + "b")
            else:
                self._file = open(path_or_file, mode)
            self._do_close = True
-            self._padding = 4

        if "r" in mode:
            self._contents = BytesIO(self._file.read())
@@ -216,11 +215,20 @@ class FirmwareImage(object):
            prev_offset = self._contents.tell()
            self._contents.seek(0, os.SEEK_END)
            self._length = self._contents.tell()
-            if self._padding:
-                fill = self._padding - (self._length % self._padding)
-                if fill:
-                    self._length += fill
-                self._padding = fill
+            # Was Padding requested
+            if self._padding != 0:
+            # If so. then is the file already a multiple of the padding?
+                if 0 == (self._length % self._padding):
+                    # padding not needed
+                    self._padding = 0
+
+                # Still need padding
+                if self._padding:
+                    fill = self._padding - (self._length % self._padding)
+                    if fill:
+                        self._length += fill
+                    #report back the fill which is the padding
+                    self._padding = fill
            self._contents.seek(prev_offset)

        return self._length
@@ -285,6 +293,8 @@ if __name__ == "__main__":
                      metavar="IMAGE")
    parser.add_option("-v", "--verbose", dest="verbose", action="store_true",
                      help="show additional firmware information on stdout")
+    parser.add_option("-p", "--padding", dest="padding", type= int,  default=4,
+                      help="Padds the image to a multple of")

    options, args = parser.parse_args()
    if len(args) not in (0, 2):
@@ -315,8 +325,8 @@ if __name__ == "__main__":

    bootloader_size = int(options.bootloader_size)

-    with FirmwareImage(in_file, "rb") as in_image:
-        with FirmwareImage(out_file, "wb") as out_image:
+    with FirmwareImage(in_file, "rb", 0) as in_image:
+        with FirmwareImage(out_file, "wb", options.padding) as out_image:
            image = in_image.read()
            out_image.write(bootloader_image)
            out_image.write(image[bootloader_size:])
@@ -271,7 +271,10 @@ CameraTrigger::CameraTrigger() :
 	param_get(_p_distance, &_distance);
 	param_get(_p_mode, (int32_t *)&_trigger_mode);
 	param_get(_p_interface, (int32_t *)&_camera_interface_mode);
-	param_get(_p_cam_cap_fback, (int32_t *)&_cam_cap_fback);
+
+	if (_p_cam_cap_fback != PARAM_INVALID) {
+		param_get(_p_cam_cap_fback, (int32_t *)&_cam_cap_fback);
+	}

 	switch (_camera_interface_mode) {
 #ifdef __PX4_NUTTX
@@ -317,8 +320,6 @@ CameraTrigger::CameraTrigger() :
 	if (!_cam_cap_fback) {
 		_trigger_pub = orb_advertise(ORB_ID(camera_trigger), &trigger);

-	} else {
-		_trigger_pub = orb_advertise(ORB_ID(camera_trigger_secondary), &trigger);
 	}
 }

@@ -843,11 +844,10 @@ CameraTrigger::engage(void *arg)
 	trigger.feedback = false;
 	trigger.timestamp = hrt_absolute_time();

+	// Publish only if  _cam_cap_fback is disabled, otherwise, it is published over camera_capture driver
 	if (!trig->_cam_cap_fback) {
 		orb_publish(ORB_ID(camera_trigger), trig->_trigger_pub, &trigger);

-	} else {
-		orb_publish(ORB_ID(camera_trigger_secondary), trig->_trigger_pub, &trigger);
 	}

 	// increment frame count
@@ -72,7 +72,12 @@ SDP3X::init_sdp3x()
 int
 SDP3X::configure()
 {
-	int ret = write_command(SDP3X_CONT_MEAS_AVG_MODE);
+	int ret = write_command(SDP3X_CONT_MODE_STOP);
+
+	if (ret == PX4_OK) {
+		px4_udelay(500); // SDP3X is unresponsive for 500us after stop continuous measurement command
+		ret = write_command(SDP3X_CONT_MEAS_AVG_MODE);
+	}

 	if (ret != PX4_OK) {
 		perf_count(_comms_errors);
@@ -56,6 +56,7 @@
 #define SDP3X_RESET_ADDR		0x00
 #define SDP3X_RESET_CMD			0x06
 #define SDP3X_CONT_MEAS_AVG_MODE	0x3615
+#define SDP3X_CONT_MODE_STOP		0x3FF9

 #define SDP3X_SCALE_PRESSURE_SDP31	60
 #define SDP3X_SCALE_PRESSURE_SDP32	240
@@ -110,15 +110,6 @@ static inline hrt_abstime hrt_elapsed_time(const hrt_abstime *then)
 	return hrt_absolute_time() - *then;
 }

-/**
- * Compute the delta between a timestamp taken in the past
- * and now.
- *
- * This function is safe to use even if the timestamp is updated
- * by an interrupt during execution.
- */
-__EXPORT extern hrt_abstime hrt_elapsed_time_atomic(const volatile hrt_abstime *then);
-
 /**
 * Store the absolute time in an interrupt-safe fashion.
 *
@@ -215,9 +215,6 @@ typedef uint16_t	servo_position_t;
 /** force safety switch on (to enable use of safety switch) */
 #define PWM_SERVO_SET_FORCE_SAFETY_ON		_PX4_IOC(_PWM_SERVO_BASE, 28)

-/** setup OVERRIDE_IMMEDIATE behaviour on FMU fail */
-#define PWM_SERVO_SET_OVERRIDE_IMMEDIATE	_PX4_IOC(_PWM_SERVO_BASE, 32)
-
 /** set auxillary output mode */
 #define PWM_SERVO_ENTER_TEST_MODE  18
 #define PWM_SERVO_EXIT_TEST_MODE   19
@@ -1,80 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2012-2014 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 drv_rc_input.h
- *
- * R/C input interface.
- */
-
-#ifndef _DRV_RC_INPUT_H
-#define _DRV_RC_INPUT_H
-
-#include <stdint.h>
-#include <sys/ioctl.h>
-#include <uORB/topics/input_rc.h>
-
-#include "drv_orb_dev.h"
-
-/**
- * Path for the default R/C input device.
- *
- * Note that on systems with more than one R/C input path (e.g.
- * PX4FMU with PX4IO connected) there may be other devices that
- * respond to this protocol.
- *
- * Input data may be obtained by subscribing to the input_rc
- * object, or by poll/reading from the device.
- */
-#define RC_INPUT0_DEVICE_PATH	"/dev/input_rc0"
-
-/**
- * Maximum RSSI value
- */
-#define RC_INPUT_RSSI_MAX	100
-
-/**
- * Input signal type, value is a control position from zero to 100
- * percent.
- */
-typedef uint16_t		rc_input_t;
-
-#define _RC_INPUT_BASE		0x2b00
-
-/** Enable RSSI input via ADC */
-#define RC_INPUT_ENABLE_RSSI_ANALOG	_IOC(_RC_INPUT_BASE, 1)
-
-/** Enable RSSI input via PWM signal */
-#define RC_INPUT_ENABLE_RSSI_PWM	_IOC(_RC_INPUT_BASE, 2)
-
-#endif /* _DRV_RC_INPUT_H */
@@ -182,6 +182,7 @@
 #define DRV_DIST_DEVTYPE_GY_US42       0x9C

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

 #define DRV_GPS_DEVTYPE_ASHTECH 0xA0
@@ -242,9 +242,18 @@ int DShot::send_command_thread_safe(const dshot_command_t command, const int num
 	cmd.num_repetitions = num_repetitions;
 	_new_command.store(&cmd);

+	hrt_abstime timestamp_for_timeout = hrt_absolute_time();
+
 	// wait until main thread processed it
 	while (_new_command.load()) {
-		px4_usleep(1000);
+
+		if (hrt_elapsed_time(&timestamp_for_timeout) < 2_s) {
+			px4_usleep(1000);
+
+		} else {
+			_new_command.store(nullptr);
+			PX4_WARN("DShot command timeout!");
+		}
 	}

 	return 0;
@@ -48,9 +48,6 @@
 #include <px4_platform_common/getopt.h>
 #include <px4_platform_common/i2c_spi_buses.h>
 #include <px4_platform_common/module.h>
-#include <uORB/Subscription.hpp>
-#include <uORB/SubscriptionInterval.hpp>
-#include <uORB/topics/parameter_update.h>

 using namespace time_literals;

@@ -78,26 +75,20 @@ public:

 	void			RunImpl();

-protected:
-	void			print_status() override;
 private:
+	void			print_status() override;
+	int			send_led_enable(bool enable);
+	int			send_led_rgb();
+	int			get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b);

 	float			_brightness{1.0f};
-	float			_max_brightness{1.0f};

 	uint8_t			_r{0};
 	uint8_t			_g{0};
 	uint8_t			_b{0};
 	bool			_leds_enabled{true};

-	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
-
 	LedController		_led_controller;
-
-	int			send_led_enable(bool enable);
-	int			send_led_rgb();
-	int			get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b);
-	void			update_params();
 };

 RGBLED::RGBLED(const I2CSPIDriverConfig &config) :
@@ -119,8 +110,6 @@ RGBLED::init()
 	send_led_enable(false);
 	send_led_rgb();

-	update_params();
-
 	// kick off work queue
 	ScheduleNow();

@@ -166,19 +155,6 @@ RGBLED::print_status()
 void
 RGBLED::RunImpl()
 {
-	// check for parameter updates
-	if (_parameter_update_sub.updated()) {
-		// clear update
-		parameter_update_s pupdate;
-		_parameter_update_sub.copy(&pupdate);
-
-		// update parameters from storage
-		update_params();
-
-		// Immediately update to change brightness
-		send_led_rgb();
-	}
-
 	LedControlData led_control_data;

 	if (_led_controller.update(led_control_data) == 1) {
@@ -267,9 +243,9 @@ RGBLED::send_led_rgb()
 {
 	/* To scale from 0..255 -> 0..15 shift right by 4 bits */
 	const uint8_t msg[6] = {
-		SUB_ADDR_PWM0, static_cast<uint8_t>((_b >> 4) * _brightness * _max_brightness + 0.5f),
-		SUB_ADDR_PWM1, static_cast<uint8_t>((_g >> 4) * _brightness * _max_brightness + 0.5f),
-		SUB_ADDR_PWM2, static_cast<uint8_t>((_r >> 4) * _brightness * _max_brightness + 0.5f)
+		SUB_ADDR_PWM0, static_cast<uint8_t>((_b >> 4) * _brightness + 0.5f),
+		SUB_ADDR_PWM1, static_cast<uint8_t>((_g >> 4) * _brightness + 0.5f),
+		SUB_ADDR_PWM2, static_cast<uint8_t>((_r >> 4) * _brightness + 0.5f)
 	};
 	return transfer(msg, sizeof(msg), nullptr, 0);
 }
@@ -294,22 +270,6 @@ RGBLED::get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b)
 	return ret;
 }

-void
-RGBLED::update_params()
-{
-	int32_t maxbrt = 15;
-	param_get(param_find("LED_RGB_MAXBRT"), &maxbrt);
-	maxbrt = maxbrt > 15 ? 15 : maxbrt;
-	maxbrt = maxbrt <  0 ?  0 : maxbrt;
-
-	// A minimum of 2 "on" steps is required for breathe effect
-	if (maxbrt == 1) {
-		maxbrt = 2;
-	}
-
-	_max_brightness = maxbrt / 15.0f;
-}
-
 void
 RGBLED::print_usage()
 {
@@ -44,12 +44,9 @@

 #include <drivers/device/i2c.h>
 #include <lib/led/led.h>
-#include <lib/parameters/param.h>
 #include <px4_platform_common/getopt.h>
 #include <px4_platform_common/i2c_spi_buses.h>
 #include <px4_platform_common/module.h>
-#include <uORB/SubscriptionInterval.hpp>
-#include <uORB/topics/parameter_update.h>

 using namespace time_literals;

@@ -81,12 +78,10 @@ public:

 private:
 	int			send_led_rgb();
-	void			update_params();

 	int			write(uint8_t reg, uint8_t data);

 	float			_brightness{1.0f};
-	float			_max_brightness{1.0f};

 	uint8_t		_r{0};
 	uint8_t		_g{0};
@@ -95,8 +90,6 @@ private:
 	volatile bool		_should_run{true};
 	bool			_leds_enabled{true};

-	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
-
 	LedController		_led_controller;

 	uint8_t		_red{NCP5623_LED_PWM0};
@@ -154,8 +147,6 @@ RGBLED_NCP5623C::init()
 		return ret;
 	}

-	update_params();
-
 	_running = true;

 	ScheduleNow();
@@ -180,19 +171,6 @@ RGBLED_NCP5623C::probe()
 void
 RGBLED_NCP5623C::RunImpl()
 {
-	// check for parameter updates
-	if (_parameter_update_sub.updated()) {
-		// clear update
-		parameter_update_s pupdate;
-		_parameter_update_sub.copy(&pupdate);
-
-		// update parameters from storage
-		update_params();
-
-		// Immediately update to change brightness
-		send_led_rgb();
-	}
-
 	LedControlData led_control_data;

 	if (_led_controller.update(led_control_data) == 1) {
@@ -247,7 +225,7 @@ int
 RGBLED_NCP5623C::send_led_rgb()
 {
 	uint8_t msg[7] = {0x20, 0x70, 0x40, 0x70, 0x60, 0x70, 0x80};
-	uint8_t brightness = 0x1f * _max_brightness;
+	uint8_t brightness = UINT8_MAX;

 	msg[0] = NCP5623_LED_CURRENT | (brightness & 0x1f);
 	msg[2] = _red | (uint8_t(_r * _brightness) & 0x1f);
@@ -257,21 +235,6 @@ RGBLED_NCP5623C::send_led_rgb()
 	return transfer(&msg[0], 7, nullptr, 0);
 }

-void
-RGBLED_NCP5623C::update_params()
-{
-	int32_t maxbrt = 31;
-	param_get(param_find("LED_RGB1_MAXBRT"), &maxbrt);
-	maxbrt = maxbrt > 31 ? 31 : maxbrt;
-	maxbrt = maxbrt <  0 ?  0 : maxbrt;
-
-	if (maxbrt == 0) {
-		maxbrt = 1;
-	}
-
-	_max_brightness = maxbrt / 31.0f;
-}
-
 void
 RGBLED_NCP5623C::print_usage()
 {
@@ -176,7 +176,6 @@ void LSM9DS1_MAG::RunImpl()
 					// sensor Z is up (RHC), flip z for publication
 					// sensor X is aligned with -X of lsm9ds1 accel/gyro
 					x = (x == INT16_MIN) ? INT16_MAX : -x;
-					y = y;
 					z = (z == INT16_MIN) ? INT16_MAX : -z;

 					_px4_mag.update(now, x, y, z);
@@ -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.
+#
+############################################################################
+px4_add_module(
+	MODULE drivers__ina238
+	MAIN ina238
+	COMPILE_FLAGS
+		-Wno-cast-align # TODO: fix and enable
+	SRCS
+		ina238_main.cpp
+		ina238.cpp
+	DEPENDS
+		battery
+		px4_work_queue
+	)
@@ -0,0 +1,296 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * Driver for the I2C attached INA238
+ */
+
+#include "ina238.h"
+
+
+INA238::INA238(const I2CSPIDriverConfig &config, int battery_index) :
+	I2C(config),
+	ModuleParams(nullptr),
+	I2CSPIDriver(config),
+	_sample_perf(perf_alloc(PC_ELAPSED, "ina238_read")),
+	_comms_errors(perf_alloc(PC_COUNT, "ina238_com_err")),
+	_collection_errors(perf_alloc(PC_COUNT, "ina238_collection_err")),
+	_battery(battery_index, this, INA238_SAMPLE_INTERVAL_US)
+{
+	float fvalue = DEFAULT_MAX_CURRENT;
+	_max_current = fvalue;
+	param_t ph = param_find("INA238_CURRENT");
+
+	if (ph != PARAM_INVALID && param_get(ph, &fvalue) == PX4_OK) {
+		_max_current = fvalue;
+	}
+
+	_range = _max_current > (DEFAULT_MAX_CURRENT - 1.0f) ? INA238_ADCRANGE_HIGH : INA238_ADCRANGE_LOW;
+
+	fvalue = DEFAULT_SHUNT;
+	_rshunt = fvalue;
+	ph = param_find("INA238_SHUNT");
+
+	if (ph != PARAM_INVALID && param_get(ph, &fvalue) == PX4_OK) {
+		_rshunt = fvalue;
+	}
+
+	_current_lsb = _max_current / INA238_DN_MAX;
+
+	// We need to publish immediately, to guarantee that the first instance of the driver publishes to uORB instance 0
+	_battery.updateBatteryStatus(
+		hrt_absolute_time(),
+		0.0,
+		0.0,
+		false,
+		battery_status_s::BATTERY_SOURCE_POWER_MODULE,
+		0,
+		0.0
+	);
+}
+
+INA238::~INA238()
+{
+	/* free perf counters */
+	perf_free(_sample_perf);
+	perf_free(_comms_errors);
+	perf_free(_collection_errors);
+}
+
+int INA238::read(uint8_t address, uint16_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));
+
+	if (ret == PX4_OK) {
+		data = swap16(received_bytes);
+
+	} else {
+		perf_count(_comms_errors);
+		PX4_DEBUG("i2c::transfer returned %d", ret);
+	}
+
+	return ret;
+}
+
+int INA238::write(uint8_t address, uint16_t value)
+{
+	uint8_t data[3] = {address, ((uint8_t)((value & 0xff00) >> 8)), (uint8_t)(value & 0xff)};
+	return transfer(data, sizeof(data), nullptr, 0);
+}
+
+int INA238::init()
+{
+	int ret = PX4_ERROR;
+
+	/* do I2C init (and probe) first */
+	if (I2C::init() != PX4_OK) {
+		return ret;
+	}
+
+	write(INA238_REG_CONFIG, (uint16_t)(INA238_RST_RESET | _range));
+
+	uint16_t shunt_calibration = static_cast<uint16_t>(INA238_CONST * _current_lsb * _rshunt);
+
+	if (_range == INA238_ADCRANGE_LOW) {
+		shunt_calibration *= 4;
+	}
+
+	if (write(INA238_REG_SHUNTCAL, shunt_calibration) < 0) {
+		return -3;
+	}
+
+	// Set the CONFIG for max I
+	write(INA238_REG_CONFIG, (uint16_t) _range);
+
+	// Start ADC continous mode here
+	ret = write(INA238_REG_ADCCONFIG, (uint16_t)INA238_ADCCONFIG);
+
+	start();
+	_sensor_ok = true;
+
+	_initialized = ret == PX4_OK;
+	return ret;
+}
+
+int INA238::force_init()
+{
+	int ret = init();
+
+	start();
+
+	return ret;
+}
+
+int INA238::probe()
+{
+	uint16_t value{0};
+
+	if (read(INA238_MANUFACTURER_ID, value) != PX4_OK || value != INA238_MFG_ID_TI) {
+		PX4_DEBUG("probe mfgid %d", value);
+		return -1;
+	}
+
+	if (read(INA238_DEVICE_ID, value) != PX4_OK || (
+		    INA238_DEVICEID(value) != INA238_MFG_DIE
+	    )) {
+		PX4_DEBUG("probe die id %d", value);
+		return -1;
+	}
+
+	return PX4_OK;
+}
+
+
+int INA238::collect()
+{
+	perf_begin(_sample_perf);
+
+	if (_parameter_update_sub.updated()) {
+		// Read from topic to clear updated flag
+		parameter_update_s parameter_update;
+		_parameter_update_sub.copy(&parameter_update);
+
+		updateParams();
+	}
+
+	// read from the sensor
+	// Note: If the power module is connected backwards, then the values of _current will be negative but otherwise valid.
+	bool success{true};
+	int16_t bus_voltage{0};
+	int16_t current{0};
+
+	success = success && (read(INA238_REG_VSBUS, bus_voltage) == PX4_OK);
+	success = success && (read(INA238_REG_CURRENT, current) == PX4_OK);
+
+	if (!success) {
+		PX4_DEBUG("error reading from sensor");
+		bus_voltage = current = 0;
+	}
+
+	_actuators_sub.copy(&_actuator_controls);
+
+	_battery.updateBatteryStatus(
+		hrt_absolute_time(),
+		(float) bus_voltage * INA238_VSCALE,
+		(float) current * _current_lsb,
+		success,
+		battery_status_s::BATTERY_SOURCE_POWER_MODULE,
+		0,
+		_actuator_controls.control[actuator_controls_s::INDEX_THROTTLE]
+	);
+
+	perf_end(_sample_perf);
+
+	if (success) {
+		return PX4_OK;
+
+	} else {
+		return PX4_ERROR;
+	}
+}
+
+void INA238::start()
+{
+	ScheduleClear();
+
+	/* reset the report ring and state machine */
+	_collect_phase = false;
+
+	_measure_interval = INA238_CONVERSION_INTERVAL;
+
+	/* schedule a cycle to start things */
+	ScheduleDelayed(5);
+}
+
+void INA238::RunImpl()
+{
+	if (_initialized) {
+		if (_collect_phase) {
+			/* perform collection */
+			if (collect() != PX4_OK) {
+				perf_count(_collection_errors);
+				/* if error restart the measurement state machine */
+				start();
+				return;
+			}
+
+			/* next phase is measurement */
+			_collect_phase = true;
+
+			if (_measure_interval > INA238_CONVERSION_INTERVAL) {
+				/* schedule a fresh cycle call when we are ready to measure again */
+				ScheduleDelayed(_measure_interval - INA238_CONVERSION_INTERVAL);
+				return;
+			}
+		}
+
+		/* next phase is collection */
+		_collect_phase = true;
+
+		/* schedule a fresh cycle call when the measurement is done */
+		ScheduleDelayed(INA238_CONVERSION_INTERVAL);
+
+	} else {
+		_battery.updateBatteryStatus(
+			hrt_absolute_time(),
+			0.0f,
+			0.0f,
+			false,
+			battery_status_s::BATTERY_SOURCE_POWER_MODULE,
+			0,
+			0.0f
+		);
+
+		if (init() != PX4_OK) {
+			ScheduleDelayed(INA238_INIT_RETRY_INTERVAL_US);
+		}
+	}
+}
+
+void INA238::print_status()
+{
+	I2CSPIDriverBase::print_status();
+
+	if (_initialized) {
+		perf_print_counter(_sample_perf);
+		perf_print_counter(_comms_errors);
+
+		printf("poll interval:  %u \n", _measure_interval);
+
+	} else {
+		PX4_INFO("Device not initialized. Retrying every %d ms until battery is plugged in.",
+			 INA238_INIT_RETRY_INTERVAL_US / 1000);
+	}
+}
@@ -0,0 +1,370 @@
+/****************************************************************************
+ *
+ *   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
+
+
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/getopt.h>
+#include <drivers/device/i2c.h>
+#include <lib/perf/perf_counter.h>
+#include <battery/battery.h>
+#include <drivers/drv_hrt.h>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionInterval.hpp>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/parameter_update.h>
+#include <px4_platform_common/i2c_spi_buses.h>
+
+using namespace time_literals;
+
+/* Configuration Constants */
+#define INA238_BASEADDR 	            0x45 /* 7-bit address. 8-bit address is 0x45 */
+// If initialization is forced (with the -f flag on the command line), but it fails, the drive will try again to
+// connect to the INA238 every this many microseconds
+#define INA238_INIT_RETRY_INTERVAL_US       500000
+
+/* INA238 Registers addresses */
+#define INA238_REG_CONFIG                   (0x00)
+#define INA238_REG_ADCCONFIG                (0x01)
+#define INA238_REG_SHUNTCAL                 (0x02)
+#define INA238_REG_SHUNTTEMPCO              (0x03)
+#define INA238_REG_VSHUNT                   (0x04)
+#define INA238_REG_VSBUS                    (0x05)
+#define INA238_REG_DIETEMP                  (0x06)
+#define INA238_REG_CURRENT                  (0x07)
+#define INA238_REG_POWER                    (0x08)
+#define INA238_REG_ENERGY                   (0x09)
+#define INA238_REG_CHARGE                   (0x0a)
+#define INA238_REG_DIAG_ALRT                (0x0b)
+#define INA238_REG_SOVL                     (0x0c)
+#define INA238_REG_SUVL                     (0x0d)
+#define INA238_REG_BOVL                     (0x0e)
+#define INA238_REG_BUVL                     (0x0f)
+#define INA238_REG_TEMP_LIMIT               (0x10)
+#define INA238_REG_TPWR_LIMIT               (0x11)
+#define INA238_MANUFACTURER_ID              (0x3e)
+#define INA238_DEVICE_ID                    (0x3f)
+
+#define INA238_MFG_ID_TI                    (0x5449) // TI
+#define INA238_MFG_DIE                      (0x238) // INA237, INA238
+
+/* INA238 Configuration (CONFIG) 16-bit Register (Address = 0h) [reset = 0h] */
+#define INA238_ADCRANGE_SHIFTS              (4)
+#define INA238_ADCRANGE_MASK                (1 << INA238_ADCRANGE_SHIFTS)
+#define INA238_ADCRANGE_LOW                 (1 << INA238_ADCRANGE_SHIFTS) // ± 40.96 mV
+#define INA238_ADCRANGE_HIGH                (0 << INA238_ADCRANGE_SHIFTS) // ±163.84 mV
+#define INA238_TEMPCOMP_SHIFTS              (5)
+#define INA238_TEMPCOMP_MASK                (1 << INA238_TEMPCOMP_SHIFTS)
+#define INA238_TEMPCOMP_ENABLE              (1 << INA238_TEMPCOMP_SHIFTS)
+#define INA238_TEMPCOMP_DISABLE             (0 << INA238_TEMPCOMP_SHIFTS)
+
+#define INA238_CONVDLY_SHIFTS               (6)
+#define INA238_CONVDLY_MASK                 (0xff << INA238_CONVDLY_SHIFTS)
+#define INA238_CONVDLY2MS(n)                ((n)  << INA238_CONVDLY_SHIFTS)
+
+#define INA238_RSTACC_SHIFTS                (14)
+#define INA238_RSTACC_MASK                  (1 << INA238_RSTACC_SHIFTS)
+#define INA238_RSTACC_CLEAR                 (1 << INA238_RSTACC_SHIFTS)
+#define INA238_RSTACC_NORMAL                (0 << INA238_RSTACC_SHIFTS)
+
+#define INA238_RST_SHIFTS                   (15)
+#define INA238_RST_MASK                     (1 << INA238_RST_SHIFTS)
+#define INA238_RST_RESET                    (1 << INA238_RST_SHIFTS)
+#define INA238_RST_NORMAL                   (0 << INA238_RST_SHIFTS)
+
+/* INA238 ADC Configuration (ADC_CONFIG) 16-bit  Register (Address = 1h) [reset = FB68h] */
+#define INA238_MODE_SHIFTS                  (12)
+#define INA238_MODE_MASK                    (0xf << INA238_MODE_SHIFTS)
+#define INA238_MODE_SHUTDOWN_TRIG           (0 << INA238_MODE_SHIFTS)
+#define INA238_MODE_BUS_TRIG                (1 << INA238_MODE_SHIFTS)
+#define INA238_MODE_SHUNT_TRIG              (2 << INA238_MODE_SHIFTS)
+#define INA238_MODE_SHUNT_BUS_TRIG          (3 << INA238_MODE_SHIFTS)
+#define INA238_MODE_TEMP_TRIG               (4 << INA238_MODE_SHIFTS)
+#define INA238_MODE_TEMP_BUS_TRIG           (5 << INA238_MODE_SHIFTS)
+#define INA238_MODE_TEMP_SHUNT_TRIG         (6 << INA238_MODE_SHIFTS)
+#define INA238_MODE_TEMP_SHUNT_BUS_TRIG     (7 << INA238_MODE_SHIFTS)
+
+#define INA238_MODE_SHUTDOWN_CONT           (8 << INA238_MODE_SHIFTS)
+#define INA238_MODE_BUS_CONT                (9 << INA238_MODE_SHIFTS)
+#define INA238_MODE_SHUNT_CONT              (10 << INA238_MODE_SHIFTS)
+#define INA238_MODE_SHUNT_BUS_CONT          (11 << INA238_MODE_SHIFTS)
+#define INA238_MODE_TEMP_CONT               (12 << INA238_MODE_SHIFTS)
+#define INA238_MODE_TEMP_BUS_CONT           (13 << INA238_MODE_SHIFTS)
+#define INA238_MODE_TEMP_SHUNT_CONT         (14 << INA238_MODE_SHIFTS)
+#define INA238_MODE_TEMP_SHUNT_BUS_CONT     (15 << INA238_MODE_SHIFTS)
+
+#define INA238_VBUSCT_SHIFTS                (9)
+#define INA238_VBUSCT_MASK                  (7 << INA238_VBUSCT_SHIFTS)
+#define INA238_VBUSCT_50US                  (0 << INA238_VBUSCT_SHIFTS)
+#define INA238_VBUSCT_84US                  (1 << INA238_VBUSCT_SHIFTS)
+#define INA238_VBUSCT_150US                 (2 << INA238_VBUSCT_SHIFTS)
+#define INA238_VBUSCT_280US                 (3 << INA238_VBUSCT_SHIFTS)
+#define INA238_VBUSCT_540US                 (4 << INA238_VBUSCT_SHIFTS)
+#define INA238_VBUSCT_1052US                (5 << INA238_VBUSCT_SHIFTS)
+#define INA238_VBUSCT_2074US                (6 << INA238_VBUSCT_SHIFTS)
+#define INA238_VBUSCT_4170US                (7 << INA238_VBUSCT_SHIFTS)
+
+#define INA238_VSHCT_SHIFTS                 (6)
+#define INA238_VSHCT_MASK                   (7 << INA238_VSHCT_SHIFTS)
+#define INA238_VSHCT_50US                   (0 << INA238_VSHCT_SHIFTS)
+#define INA238_VSHCT_84US                   (1 << INA238_VSHCT_SHIFTS)
+#define INA238_VSHCT_150US                  (2 << INA238_VSHCT_SHIFTS)
+#define INA238_VSHCT_280US                  (3 << INA238_VSHCT_SHIFTS)
+#define INA238_VSHCT_540US                  (4 << INA238_VSHCT_SHIFTS)
+#define INA238_VSHCT_1052US                 (5 << INA238_VSHCT_SHIFTS)
+#define INA238_VSHCT_2074US                 (6 << INA238_VSHCT_SHIFTS)
+#define INA238_VSHCT_4170US                 (7 << INA238_VSHCT_SHIFTS)
+
+#define INA238_VTCT_SHIFTS                  (3)
+#define INA238_VTCT_MASK                    (7 << INA238_VTCT_SHIFTS)
+#define INA238_VTCT_50US                    (0 << INA238_VTCT_SHIFTS)
+#define INA238_VTCT_84US                    (1 << INA238_VTCT_SHIFTS)
+#define INA238_VTCT_150US                   (2 << INA238_VTCT_SHIFTS)
+#define INA238_VTCT_280US                   (3 << INA238_VTCT_SHIFTS)
+#define INA238_VTCT_540US                   (4 << INA238_VTCT_SHIFTS)
+#define INA238_VTCT_1052US                  (5 << INA238_VTCT_SHIFTS)
+#define INA238_VTCT_2074US                  (6 << INA238_VTCT_SHIFTS)
+#define INA238_VTCT_4170US                  (7 << INA238_VTCT_SHIFTS)
+
+#define INA238_AVERAGES_SHIFTS              (0)
+#define INA238_AVERAGES_MASK                (7 << INA238_AVERAGES_SHIFTS)
+#define INA238_AVERAGES_1                   (0 << INA238_AVERAGES_SHIFTS)
+#define INA238_AVERAGES_4                   (1 << INA238_AVERAGES_SHIFTS)
+#define INA238_AVERAGES_16                  (2 << INA238_AVERAGES_SHIFTS)
+#define INA238_AVERAGES_64                  (3 << INA238_AVERAGES_SHIFTS)
+#define INA238_AVERAGES_128                 (4 << INA238_AVERAGES_SHIFTS)
+#define INA238_AVERAGES_256                 (5 << INA238_AVERAGES_SHIFTS)
+#define INA238_AVERAGES_512                 (6 << INA238_AVERAGES_SHIFTS)
+#define INA238_AVERAGES_1024                (7 << INA238_AVERAGES_SHIFTS)
+
+#define INA238_ADCCONFIG (INA238_MODE_TEMP_SHUNT_BUS_CONT | INA238_VBUSCT_540US | INA238_VSHCT_540US | INA238_VTCT_540US |INA238_AVERAGES_64)
+
+/* INA238 Shunt Calibration (SHUNT_CAL) 16-bit Register (Address = 2h) [reset = 1000h] */
+
+#define INA238_CURRLSB_SHIFTS               (0)
+#define INA238_CURRLSB_MASK                 (0x7fff << INA238_CURRLSB_SHIFTS)
+
+/* INA238 Shunt Temperature Coefficient (SHUNT_TEMPCO) 16-bit Register (Address = 3h) [reset = 0h] */
+
+#define INA238_TEMPCO_SHIFTS                (0)
+#define INA238_TEMPCO_MASK                  (0x1fff << INA238_TEMPCO_SHIFTS)
+
+/* INA238 Shunt Voltage Measurement (VSHUNT) 24-bit Register (Address = 4h) [reset = 0h] */
+
+#define INA238_VSHUNT_SHIFTS                (4)
+#define INA238_VSHUNT_MASK                  (UINT32_C(0xffffff) << INA238_VSHUNT_SHIFTS)
+
+/* INA238 Bus Voltage Measurement (VBUS) 24-bit Register (Address = 5h) [reset = 0h] */
+
+#define INA238_VBUS_SHIFTS                  (4)
+#define INA238_VBUS_MASK                    (UINT32_C(0xffffff) << INA238_VBUS_SHIFTS)
+
+/* INA238 Temperature Measurement (DIETEMP) 16-bit Register (Address = 6h) [reset = 0h] */
+
+#define INA238_DIETEMP_SHIFTS               (0)
+#define INA238_DIETEMP_MASK                 (0xffff << INA238_DIETEMP_SHIFTS)
+
+/* INA238 Current Result (CURRENT) 24-bit Register (Address = 7h) [reset = 0h] */
+
+#define INA238_CURRENT_SHIFTS               (4)
+#define INA238_CURRENT_MASK                 (UINT32_C(0xffffff) << INA238_CURRENT_SHIFTS)
+
+/* INA238 Power Result (POWER) 24-bit Register (Address = 8h) [reset = 0h] */
+
+#define INA238_POWER_SHIFTS                 (0)
+#define INA238_POWER_MASK                   (UINT32_C(0xffffff) << INA238_POWER_SHIFTS)
+
+/* INA238 Energy Result (ENERGY) 40-bit Register (Address = 9h) [reset = 0h] */
+
+#define INA238_ENERGY_SHIFTS                 (0)
+#define INA238_ENERGY_MASK                   (UINT64_C(0xffffffffff) << INA238_ENERGY_SHIFTS)
+
+/* INA238 Energy Result (CHARGE) 40-bit Register (Address = Ah) [reset = 0h] */
+
+#define INA238_CHARGE_SHIFTS                (0)
+#define INA238_CHARGE_MASK                  (UINT64_C(0xffffffffff) << INA238_CHARGE_SHIFTS)
+
+
+/* INA238 Diagnostic Flags and Alert (DIAG_ALRT) 16-bit Register (Address = Bh) [reset = 0001h] */
+
+#define INA238_MEMSTAT                      (1 << 0)  // This bit is set to 0 if a checksum error is detected in the device trim memory space
+#define INA238_CNVRF                        (1 << 1)  // This bit is set to 1 if the conversion is completed. When ALATCH =1 this bit is cleared by reading the register or starting a new triggered conversion.
+#define INA238_POL                          (1 << 2)  // This bit is set to 1 if the power measurement exceeds the threshold limit in the power limit register.
+#define INA238_BUSUL                        (1 << 3)  // This bit is set to 1 if the bus voltage measurement falls below the threshold limit in the bus under-limit register.
+#define INA238_BUSOL                        (1 << 4)  // This bit is set to 1 if the bus voltage measurement exceeds the threshold limit in the bus over-limit register.
+#define INA238_SHNTUL                       (1 << 5)  // This bit is set to 1 if the shunt voltage measurement falls below the threshold limit in the shunt under-limit register
+#define INA238_SHNTOL                       (1 << 6)  // This bit is set to 1 if the shunt voltage measurement exceeds the threshold limit in the shunt over-limit register.
+#define INA238_TMPOL                        (1 << 7)  // This bit is set to 1 if the temperature measurement exceeds the threshold limit in the temperature over-limit register.
+#define INA238_MATHOF                       (1 << 9)  // This bit is set to 1 if an arithmetic operation resulted in an overflow error.
+#define INA238_CHARGEOF                     (1 << 10) // This bit indicates the health of the CHARGE register. If the 40 bit CHARGE register has overflowed this bit is set to 1.
+#define INA238_ENERGYOF                     (1 << 11) // This bit indicates the health of the ENERGY register. If the 40 bit ENERGY register has overflowed this bit is set to 1.
+#define INA238_APOL                         (1 << 12) // Alert Polarity bit sets the Alert pin polarity.
+#define INA238_SLOWALER                     (1 << 13) // ALERT function is asserted on the completed averaged value.  This gives the flexibility to delay the ALERT after the averaged value.
+#define INA238_CNVR                         (1 << 14) // Setting this bit high configures the Alert pin to be asserted when the Conversion Ready Flag (bit 1) is asserted, indicating that a conversion cycle has completed
+#define INA238_ALATCH                       (1 << 15) // When the Alert Latch Enable bit is set to Transparent mode, the Alert pin and Flag bit reset to the idle state when the fault has been
+// cleared. When the Alert Latch Enable bit is set to Latch mode, the Alert pin and Alert Flag bit remain active following a fault until
+// the DIAG_ALRT Register has been read.
+
+/* Shunt Overvoltage Threshold (SOVL) 16-bit Register (Address = Ch) [reset = 7FFFh] */
+
+#define INA238_SOVL_SHIFTS                   (0)
+#define INA238_SOVL_MASK                     (0xffff << INA238_SOVL_SHIFTS)
+
+/* Shunt Undervoltage Threshold (SUVL) 16-bit Register (Address = Dh) [reset = 8000h] */
+
+#define INA238_SUVL_SHIFTS                   (0)
+#define INA238_SUVL_MASK                     (0xffff << INA238_SUVL_SHIFTS)
+
+/* Bus Overvoltage Threshold (BOVL) 16-bit Register (Address = Eh) [reset = 7FFFh] */
+
+#define INA238_BOVL_SHIFTS                   (0)
+#define INA238_BOVL_MASK                     (0xffff << INA238_BOVL_SHIFTS)
+
+/* Bus Undervoltage Threshold (BUVL) 16-bit Register (Address = Fh) [reset = 0h] */
+
+#define INA238_BUVL_SHIFTS                   (0)
+#define INA238_BUVL_MASK                     (0xffff << INA238_BUVL_SHIFTS)
+
+/* Temperature Over-Limit Threshold (TEMP_LIMIT) 16-bit Register (Address = 10h) [reset = 7FFFh */
+
+#define INA238_TEMP_LIMIT_SHIFTS             (0)
+#define INA238_TEMP_LIMIT_MASK               (0xffff << INA238_TEMP_LIMIT_SHIFTS)
+
+/* Power Over-Limit Threshold (PWR_LIMIT) 16-bit Register (Address = 11h) [reset = FFFFh] */
+
+#define INA238_POWER_LIMIT_SHIFTS            (0)
+#define INA238_POWER_LIMIT_MASK              (0xffff << INA238_POWER_LIMIT_SHIFTS)
+
+/* Manufacturer ID (MANUFACTURER_ID) 16-bit Register (Address = 3Eh) [reset = 5449h] */
+
+/* Device ID (DEVICE_ID) 16-bit Register (Address = 3Fh) [reset = 2380h] */
+
+#define INA238_DEVICE_REV_ID_SHIFTS          (0)
+#define INA238_DEVICE_REV_ID_MASK            (0xf << INA238_DEVICE_REV_ID_SHIFTS)
+#define INA238_DEVICEREV_ID(v)               (((v) & INA238_DEVICE_REV_ID_MASK) >> INA238_DEVICE_REV_ID_SHIFTS)
+#define INA238_DEVICE_ID_SHIFTS              (4)
+#define INA238_DEVICE_ID_MASK                (0xfff << INA238_DEVICE_ID_SHIFTS)
+#define INA238_DEVICEID(v)                   (((v) & INA238_DEVICE_ID_MASK) >> INA238_DEVICE_ID_SHIFTS)
+
+#define INA238_SAMPLE_FREQUENCY_HZ           10
+#define INA238_SAMPLE_INTERVAL_US            (1_s / INA238_SAMPLE_FREQUENCY_HZ)
+#define INA238_CONVERSION_INTERVAL           (INA238_SAMPLE_INTERVAL_US - 7)
+#define INA238_DN_MAX                        32768.0f   /* 2^15 */
+#define INA238_CONST                         819.2e6f  /* is an internal fixed value used to ensure scaling is maintained properly  */
+#define INA238_VSCALE                        3.125e-03f  /* LSB of voltage is 3.1255 mV/LSB */
+
+
+#define DEFAULT_MAX_CURRENT                  327.68f    /* Amps */
+#define DEFAULT_SHUNT                        0.0003f   /* Shunt is 300 uOhm */
+
+#define swap16(w)                            __builtin_bswap16((w))
+#define swap32(d)                            __builtin_bswap32((d))
+#define swap64(q)                            __builtin_bswap64((q))
+
+class INA238 : public device::I2C, public ModuleParams, public I2CSPIDriver<INA238>
+{
+public:
+	INA238(const I2CSPIDriverConfig &config, int battery_index);
+	virtual ~INA238();
+
+	static I2CSPIDriverBase *instantiate(const I2CSPIDriverConfig &config, int runtime_instance);
+	static void print_usage();
+
+	void RunImpl();
+
+	int init() override;
+
+	/**
+	 * Tries to call the init() function. If it fails, then it will schedule to retry again in
+	 * INA238_INIT_RETRY_INTERVAL_US microseconds. It will keep retrying at this interval until initialization succeeds.
+	 *
+	 * @return PX4_OK if initialization succeeded on the first try. Negative value otherwise.
+	 */
+	int force_init();
+
+	/**
+	* Diagnostics - print some basic information about the driver.
+	*/
+	void print_status() override;
+
+protected:
+	int probe() override;
+
+private:
+	bool _sensor_ok{false};
+	unsigned int _measure_interval{0};
+	bool _collect_phase{false};
+	bool _initialized{false};
+
+	perf_counter_t _sample_perf;
+	perf_counter_t _comms_errors;
+	perf_counter_t _collection_errors;
+
+	// Configuration state, computed from params
+	float _max_current;
+	float _rshunt;
+	float _current_lsb;
+	int16_t _range;
+
+	actuator_controls_s _actuator_controls{};
+
+	Battery _battery;
+	uORB::Subscription _actuators_sub{ORB_ID(actuator_controls_0)};
+	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
+
+	int read(uint8_t address, uint16_t &data);
+	int write(uint8_t address, uint16_t data);
+
+	int read(uint8_t address, int16_t &data)
+	{
+		return read(address, (uint16_t &)data);
+	}
+
+	int write(uint8_t address, int16_t data)
+	{
+		return write(address, (uint16_t)data);
+	}
+
+	/**
+	* Initialise the automatic measurement state machine and start it.
+	*
+	* @note This function is called at open and error time.  It might make sense
+	*       to make it more aggressive about resetting the bus in case of errors.
+	*/
+	void start();
+
+	int collect();
+
+};
--- a/Show More
+++ b/Show More
Author	SHA1	Message	Date
Daniel Agar	a0a0a3e983	cmake: git check submodules updates	2021-10-01 14:51:23 -04:00
SungTae Moon	2adf3f959b	Update Tools/gazebo_sitl_multiple_run.sh Co-authored-by: JaeyoungLim <jalim@ethz.ch>	2021-10-01 16:09:32 +02:00
stmoon	bc5d55c4a4	fix the problem related to sitl multiple vehicle environment	2021-10-01 16:09:32 +02:00
marcojob	68a13326f0	mavlink: remove unused strncpy (#18300 )	2021-10-01 10:58:25 +02:00
David Sidrane	7c9903f610	stm32h7:px4io_serial use TRBUFF - includes NuttX with TRBUFF backports	2021-09-29 23:55:00 -04:00
David Sidrane	25a27d26e0	logger:log_writer_file Do not try to store ulog name on systems without hardfault logging	2021-09-29 23:51:25 -04:00
David Sidrane	f56701e72f	micro_hal:Remove px4_savepanic on arch without hardfault logging	2021-09-29 23:51:25 -04:00
David Sidrane	cb47203a9e	NuttX with deglitched GPIO backports	2021-09-29 23:50:11 -04:00
Daniel Agar	f3a904c109	Jenkins: HIL increase timeout - occasionally we have a considerable backlog	2021-09-29 20:37:07 -04:00
David Sidrane	63e29b821c	holybro_can-gps-v1:Ensure sensors are powered	2021-09-29 14:37:17 -04:00
David Sidrane	2abbf791b3	NuttX with deglitched GPIO backports	2021-09-29 14:37:17 -04:00
David Sidrane	70eca23468	ark_can-gps_debug:Fit in Flash w/o loosing tools	2021-09-29 14:37:17 -04:00
David Sidrane	20c1f763d2	px4_fmu-v4:CAN build needs .app_descriptor	2021-09-29 14:34:42 -04:00
David Sidrane	797bd9592b	mxp_ucans32k146:using padding of 8	2021-09-29 14:34:42 -04:00
David Sidrane	f4e9672fde	make_can_boot_descriptor:Add padding argument	2021-09-29 14:34:42 -04:00
Peter van der Perk	360c3781f3	UCANS32K146-01 Add CAN tranceiver self test	2021-09-29 09:36:14 -07:00
Daniel Agar	c39a21aa54	ekf2: selector remove special timeout condition - this mainly produces false positives when disarmed and bench testing	2021-09-29 09:59:08 -04:00
Paul Riseborough	ed7c5991eb	ekf2: Fix dimensional error in gyro and accel bias variance conversions	2021-09-29 09:58:41 -04:00
Hyungsub	5677ab042b	tap_esc: fix resource leak (#18296 )	2021-09-29 14:51:31 +02:00
Daniel Agar	03bd5e0f48	lib/led: move drivers/lights/rgbled LED_RGB_MAXBRT -> SYS_RGB_MAXBRT	2021-09-28 10:09:55 -04:00
Nicolas Martin	b24e5fc0af	clean remaining offboard_control_signal_found_once offboard_control_signal_found_once is not used any more	2021-09-28 09:25:04 -04:00
Nicolas Martin	417a50b2cc	fix typo in comment	2021-09-28 09:24:47 -04:00
Jukka Laitinen	6cae4c92e7	Add a header to logfile encryption key exchange file Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2021-09-27 17:45:07 +02:00
Jukka Laitinen	9a4ef709ca	Add parameters for configuring the logfile encryption SDLOG_ALGORITHM for selecting the crypto algorithm SDLOG_KEY for selecting the key in keystore to use with the algorithm. for symmetric algorithms which are currently supported, this is just a free slot to be used, the key is generated at logging start SDLOG_EXCH_KEY for selecting an RSA2048 key for encrypting the SDLOG_KEY Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2021-09-27 17:45:07 +02:00
Jukka Laitinen	1687cab56c	FMUv5: Add a build target for crypto interface testing, px4_fmu-v5_cryptotest To avoid messing with existing targets, and to automatically set the test keys for the keystore. Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2021-09-27 17:45:07 +02:00
Jukka Laitinen	ce76380051	Add a simple script to decrypt ulog files This now only decrypts xchacha20 encrypted logs, where keys are exchanged with rsa_oaep_sha256 and nonce appended to the end of the key Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2021-09-27 17:45:07 +02:00
Jukka Laitinen	9472f79307	Take crypto interfaces into use in logger, to encrypt ulog data - Generate a symmetric encryption key and a nonce value - Use a public key in keystore to encrypt the symmteric key - Write the encrypted key and the nonce value to disk into .ulgk, name matching with the encrypted log file - use quick stream encryption to crypt the .ulg file contents Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2021-09-27 17:45:07 +02:00
Jukka Laitinen	6b22fe99b7	Globally define -DPX4_CRYPTO when the board defines some crypto backend Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2021-09-27 17:45:07 +02:00
Jukka Laitinen	3db76d88fe	Add crypto key generation functions Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2021-09-27 17:45:07 +02:00
Jukka Laitinen	4c6779812d	Add keystore_put_key interface function for storing keys permanently Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2021-09-27 17:45:07 +02:00
Jukka Laitinen	fc2668cba1	Add encryption function for the crypto_backend and px4_crypto Take into use libtomcrypt library to add RSA and some other algorithms for SW ecnryption use case Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2021-09-27 17:45:07 +02:00
Jukka Laitinen	634d911b74	Add a test key for RSA2048 Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2021-09-27 17:45:07 +02:00
Jukka Laitinen	40063bd54b	Add libtomcrypt and libtommath submodules These provide e.g. rsa_oaep, which can be used for sw crypto Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2021-09-27 17:45:07 +02:00
Jukka Laitinen	be4f7cabf9	Add compilation of nuttx_crypto NuttX has lots of usable functionality for crypto, such as rng with entropy pool Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2021-09-27 17:45:07 +02:00
Jukka Laitinen	d36a91ceaf	Add generic px4 interface for crypto features Add common functions, implemented for nuttx, and link to architecture specific libraries Make a separate library to wrap nuttx random number generator as "os_random". Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2021-09-27 17:45:07 +02:00
Jukka Laitinen	aa61bc4942	Add functions for initializing empty crypto backend handles Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2021-09-27 17:45:07 +02:00
Julian Oes	9290ea974e	commander: allow force disarming Allows to do commander disarm -f to disarm no matter what.	2021-09-27 09:58:03 -04:00
Daniel Agar	333f539900	mavlink: add simple incoming message stats to status	2021-09-27 09:48:00 -04:00
PX4 BuildBot	feff564882	Update submodule jMAVSim to latest Mon Sep 27 00:38:51 UTC 2021 - jMAVSim in PX4/Firmware (`592885480e`): https://github.com/PX4/jMAVSim/commit/0a5375a70689aac53143768de9033034f3636022 - jMAVSim current upstream: https://github.com/PX4/jMAVSim/commit/0a5a8c6d9da05bee5f82faa25430213d97d396b0 - Changes: https://github.com/PX4/jMAVSim/compare/0a5375a70689aac53143768de9033034f3636022...0a5a8c6d9da05bee5f82faa25430213d97d396b0 0a5a8c6 2021-09-02 Julian Oes - Merge pull request #130 from PX4/pr-udp-connection 2590b24 2021-09-01 Julian Oes - UDPMavLinkPort: add debug output around wait 7255b28 2021-08-31 Julian Oes - src: enable UDP connection in client and host mode	2021-09-27 08:05:48 +02:00
Daniel Agar	592885480e	boards: ark_can-gps_debug disable perf to save flash	2021-09-26 12:29:11 -04:00
Daniel Agar	9a3fa6199b	uorb top report total publication rate	2021-09-26 12:29:11 -04:00
Julian Oes	e81214a22f	mavlink: avoid double trigger using sequence By adding the sequence number we can avoid double triggering due to command retransmissions. This is according to the mavlink spec for MAV_CMD_IMAGE_START_CAPTURE.	2021-09-25 20:18:07 -04:00
bresch	76615f8df3	VTOL: set HTE horizontal speed sensitivity threshold low	2021-09-25 20:07:28 -04:00
bresch	904ed57aba	HTE: decrease sensitivity with speed VTOL planes are getting lift from the wing when flying in MC mode at high speed. They (and some other drones) also get extra drag when climbing and descending at high speed, corrupting the hover thrust estimate. To avoid this, two speed thresholds (vertical and horizontal) are defined above which the sensitivity of the estimator is decreased by linearly increasing the observation noise.	2021-09-25 20:07:28 -04:00
achim	c67d943158	boards: mro_ctrl-zero-h7-oem board.h fix SPI5 (#18294 ) - fix spi pin missmatch PF9=GPIO_SPI5_MOSI_2	2021-09-25 20:05:18 -04:00
Daniel Agar	e4b0fa25a2	systemcmds/tests: remove px4io conv test	2021-09-25 19:15:05 -04:00
Beat Küng	1b0bfd6381	dataman: remove nonexisting SYS_RESTART_TYPE param	2021-09-25 19:15:05 -04:00
Daniel Agar	089c962d92	px4io: moving mixing to FMU side Using mixers on the IO side had a remote benefit of being able to override all control surfaces with a radio remote on a fixed wing. This ended up not being used that much and since the original design 10 years ago (2011) we have been able to convince ourselves that the overall system stability is at a level where this marginal benefit, which is not present on multicopters, is not worth the hazzle. Co-authored-by: Beat Küng <beat-kueng@gmx.net> Co-authored-by: Daniel Agar <daniel@agar.ca>	2021-09-25 19:15:05 -04:00
PX4 BuildBot	f772b0f45d	Update submodule sitl_gazebo to latest Sat Sep 25 12:38:57 UTC 2021 - sitl_gazebo in PX4/Firmware (e800e7c27663ae0a08ae1e377cbccd2272657ad0): https://github.com/PX4/PX4-SITL_gazebo/commit/53d606b32f5729cc70d59281085d639bd97b30d7 - sitl_gazebo current upstream: https://github.com/PX4/PX4-SITL_gazebo/commit/d8366bf2389eae6106d1dbfaac72ebfdf23a5d2d - Changes: https://github.com/PX4/PX4-SITL_gazebo/compare/53d606b32f5729cc70d59281085d639bd97b30d7...d8366bf2389eae6106d1dbfaac72ebfdf23a5d2d d8366bf 2021-09-19 Mohamed ElDewany - Fix gazebo plugin for laser sensor 37a3a64 2021-09-16 Julian Oes - camera: fix stream URI 2561ff0 2021-09-01 Julian Oes - mavlink_interface: enable HITL over UDP 9216dab 2021-09-01 Julian Oes - mavlink_interface: remove poll before writing 8e9eb3f 2021-09-01 Julian Oes - mavlink_interface: clean up switch case a009174 2021-09-01 Julian Oes - mavlink_interface: use flag from mavlink_interface 6f1243a 2021-09-01 Julian Oes - mavlink_interface: remove redundant inline keyword	2021-09-25 11:56:50 -04:00
David Sidrane	f01cab3be1	px4io:Ensure proper lifecycle of registration of cdev	2021-09-24 19:01:54 -04:00
Igor Mišić	9ad0e5b357	mavlink_receiver: updated sens flow parameters to be dynamically handle	2021-09-23 09:23:22 +02:00
Beat Küng	e73172b537	hrt: remove hrt_elapsed_time_atomic There's no protection on posix	2021-09-23 09:23:08 +02:00
Beat Küng	6f902e54f9	mavlink: correctly lock radio status - hrt_elapsed_time_atomic is not atomic on posix - other fields like _radio_status_mult need protection as well	2021-09-23 09:23:08 +02:00
Beat Küng	0bfb1a983b	px4iofirmware: avoid using hrt_elapsed_time_atomic It's only ever read and written in controls_tick() after startup	2021-09-23 09:23:08 +02:00
Beat Küng	eec94965f2	serial: add pixhawk payload bus option	2021-09-23 09:23:08 +02:00
Igor Mišić	682db4b8ac	dshot: timeout if commands are not processed	2021-09-23 09:22:49 +02:00
bresch	e53d2907d7	AutoSmoothVel: adjust controller constraints in emergency braking When engaging auto mode at high vertical speed, we don't want to cut the velocity trajectory setpoint in order to create a smooth deceleration.	2021-09-23 09:22:32 +02:00
bresch	316e0dfeb5	AutoLineSmoothVel: add emergency braking mode Currently only for the Z axis. If the current downward velocity is more than twice the maximum allowed value, the emergency braking mode is activated. In this mode, a higher vertical acceleration and jerk is used until the vehicle stops moving.	2021-09-23 09:22:32 +02:00
Igor Mišić	92696b589f	sdp3x: stop continuous measurement while reconfig	2021-09-23 09:22:09 +02:00
Igor Mišić	3a148bc777	camera_trigger: remove camera trigger secondary camera_trigger: publish trigger when capture feedback is not enabled	2021-09-23 09:21:56 +02:00
Hamish Willee	f41e5985e8	vehicle_local_position: clarify origin of system	2021-09-22 10:14:23 +02:00
Thomas Debrunner	43ece74fa0	ina238: Do not expose raw adc config to user, make specific to ina238	2021-09-22 10:02:17 +02:00
Thomas Debrunner	b8d0bb44c4	ina23X: Support for ina23X power monitors	2021-09-22 10:02:17 +02:00
Daniel Agar	cf5da66e9f	Jenkins: hardware disable gyro auto cal and FFT before running tests	2021-09-21 19:13:30 -04:00
Daniel Agar	87db18c1a0	Jenkins: hardware don't allow board cleanup commands to fail build	2021-09-21 19:13:30 -04:00
Daniel Agar	a92e44c90e	Tools/HIL/run_nsh_cmd.py: echo success cmd twice to avoid missing dropouts	2021-09-21 19:13:30 -04:00
Julian Oes	faca2b17d0	camera_trigger: only get param if available	2021-09-17 12:23:30 +02:00