lib/rc: don't share decode buffer between protocols

commander: don't play failsafe tune if disarmed
vscode: do not disable autocomplete on enter
2026-06-01 03:20:05 +08:00 · 2021-09-16 14:40:26 -04:00 · 2021-09-16 09:17:00 -04:00 · 2021-09-16 09:11:52 -04:00 · 2021-09-16 13:59:27 +02:00 · 2021-09-16 13:59:27 +02:00
576 changed files with 16982 additions and 13233 deletions
@@ -9,10 +9,10 @@ pipeline {
        script {
          def build_nodes = [:]
          def docker_images = [
-            armhf: "px4io/px4-dev-armhf:2021-05-04",
-            arm64: "px4io/px4-dev-aarch64:2021-05-04",
-            base: "px4io/px4-dev-base-bionic:2021-05-04",
-            nuttx: "px4io/px4-dev-nuttx-focal:2021-05-04",
+            armhf: "px4io/px4-dev-armhf:2021-09-08",
+            arm64: "px4io/px4-dev-aarch64:2021-09-08",
+            base: "px4io/px4-dev-base-bionic:2021-09-08",
+            nuttx: "px4io/px4-dev-nuttx-focal:2021-09-08",
          ]

          def armhf_builds = [
@@ -127,7 +127,7 @@ pipeline {
    // TODO: actually upload artifacts to S3
    // stage('S3 Upload') {
    //   agent {
-    //     docker { image 'px4io/px4-dev-base-focal:2021-05-04' }
+    //     docker { image 'px4io/px4-dev-base-focal:2021-09-08' }
    //   }
    //   options {
    //         skipDefaultCheckout()
@@ -12,7 +12,7 @@ pipeline {
            stage("build cubepilot_cubeorange_test") {
              agent {
                docker {
-                  image 'px4io/px4-dev-nuttx-focal:2021-05-04'
+                  image 'px4io/px4-dev-nuttx-focal:2021-09-08'
                  args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
                }
              }
@@ -63,7 +63,7 @@ pipeline {
                    sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "SYS_BL_UPDATE" --value "1"'       // update bootloader
                    checkStatus()
                    quickCalibrate()
-                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "px4io status" || true'
+                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "px4io status"'
                  }
                }
                stage("print topics") {
@@ -71,7 +71,11 @@ pipeline {
                    printTopics()
                  }
                }
-
+              }
+              post {
+                always {
+                    resetBoard()
+                }
              }
            } // stage test
          }
@@ -82,7 +86,7 @@ pipeline {
            stage("build cuav_x7pro_test") {
              agent {
                docker {
-                  image 'px4io/px4-dev-nuttx-focal:2021-05-04'
+                  image 'px4io/px4-dev-nuttx-focal:2021-09-08'
                  args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
                }
              }
@@ -139,7 +143,11 @@ pipeline {
                    printTopics()
                  }
                }
-
+              }
+              post {
+                always {
+                    resetBoard()
+                }
              }
            } // stage test
          }
@@ -150,7 +158,7 @@ pipeline {
            stage("build px4_fmu-v3_test") {
              agent {
                docker {
-                  image 'px4io/px4-dev-nuttx-focal:2021-05-04'
+                  image 'px4io/px4-dev-nuttx-focal:2021-09-08'
                  args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
                }
              }
@@ -199,8 +207,7 @@ pipeline {
                    sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "SYS_BL_UPDATE" --value "1"'       // update bootloader
                    checkStatus()
                    quickCalibrate()
-                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener px4io_status"'
-                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "px4io status" || true'
+                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "px4io status"'
                  }
                }
                stage("print topics") {
@@ -208,7 +215,11 @@ pipeline {
                    printTopics()
                  }
                }
-
+              }
+              post {
+                always {
+                    resetBoard()
+                }
              }
            } // stage test
          }
@@ -219,7 +230,7 @@ pipeline {
            stage("build px4_fmu-v4_test") {
              agent {
                docker {
-                  image 'px4io/px4-dev-nuttx-focal:2021-05-04'
+                  image 'px4io/px4-dev-nuttx-focal:2021-09-08'
                  args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
                }
              }
@@ -275,7 +286,11 @@ pipeline {
                    printTopics()
                  }
                }
-
+              }
+              post {
+                always {
+                    resetBoard()
+                }
              }
            } // stage test
          }
@@ -286,7 +301,7 @@ pipeline {
            stage("build px4_fmu-v4pro_test") {
              agent {
                docker {
-                  image 'px4io/px4-dev-nuttx-focal:2021-05-04'
+                  image 'px4io/px4-dev-nuttx-focal:2021-09-08'
                  args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
                }
              }
@@ -335,7 +350,7 @@ pipeline {
                    sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "SYS_BL_UPDATE" --value "1"'       // update bootloader
                    checkStatus()
                    quickCalibrate()
-                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "px4io status" || true'
+                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "px4io status"'
                  }
                }
                stage("print topics") {
@@ -343,7 +358,11 @@ pipeline {
                    printTopics()
                  }
                }
-
+              }
+              post {
+                always {
+                    resetBoard()
+                }
              }
            } // stage test
          }
@@ -354,7 +373,7 @@ pipeline {
            stage("build px4_fmu-v5_debug") {
              agent {
                docker {
-                  image 'px4io/px4-dev-nuttx-focal:2021-05-04'
+                  image 'px4io/px4-dev-nuttx-focal:2021-09-08'
                  args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
                }
              }
@@ -392,14 +411,14 @@ pipeline {
                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param save" || true'
                    sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/px4_fmu-v5_debug/px4_fmu-v5_bootloader.elf'
                    // flash board and watch bootup
-                    sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/px4_fmu-v5_debug/px4_fmu-v5_debug.elf && ./Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-*` --baudrate 57600'
+                    sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/px4_fmu-v5_debug/px4_fmu-v5_debug.elf && ./Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-*` --baudrate 57600 || true'
                  }
                }
                stage("tests") {
                  steps {
                    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 "work_queue status"'
-                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb_tests" || true'
+                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb_tests"'
                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb_tests latency_test" || true'
                  }
                }
@@ -411,6 +430,7 @@ pipeline {
                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set IMU_GYRO_RATEMAX 200" || true' // limit cpu usage
                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set MAV_0_CONFIG 0" || true'       // limit cpu usage
                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set MAV_1_CONFIG 0" || true'       // limit cpu usage
+                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SDLOG_MODE -1" || true'        // limit cpu usage
                    checkStatus()
                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "px4io status" || true'
                  }
@@ -420,7 +440,11 @@ pipeline {
                    printTopics()
                  }
                }
-
+              }
+              post {
+                always {
+                    resetBoard()
+                }
              }
            } // stage test
          }
@@ -431,7 +455,7 @@ pipeline {
            stage("build px4_fmu-v5_optimized") {
              agent {
                docker {
-                  image 'px4io/px4-dev-nuttx-focal:2021-05-04'
+                  image 'px4io/px4-dev-nuttx-focal:2021-09-08'
                  args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
                }
              }
@@ -488,7 +512,11 @@ pipeline {
                    printTopics()
                  }
                }
-
+              }
+              post {
+                always {
+                    resetBoard()
+                }
              }
            } // stage test
          }
@@ -499,7 +527,7 @@ pipeline {
            stage("build px4_fmu-v5_stackcheck") {
              agent {
                docker {
-                  image 'px4io/px4-dev-nuttx-focal:2021-05-04'
+                  image 'px4io/px4-dev-nuttx-focal:2021-09-08'
                  args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
                }
              }
@@ -544,7 +572,7 @@ pipeline {
                  steps {
                    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 "work_queue status"'
-                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb_tests" || true'
+                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb_tests"'
                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb_tests latency_test" || true'
                  }
                }
@@ -556,6 +584,7 @@ pipeline {
                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set IMU_GYRO_RATEMAX 200" || true' // limit cpu usage
                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set MAV_0_CONFIG 0" || true'       // limit cpu usage
                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set MAV_1_CONFIG 0" || true'       // limit cpu usage
+                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SDLOG_MODE -1" || true'        // limit cpu usage
                    checkStatus()
                    sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "px4io status" || true'
                  }
@@ -565,7 +594,11 @@ pipeline {
                    printTopics()
                  }
                }
-
+              }
+              post {
+                always {
+                    resetBoard()
+                }
              }
            } // stage test
          }
@@ -576,7 +609,7 @@ pipeline {
            stage("build px4_fmu-v5_test") {
              agent {
                docker {
-                  image 'px4io/px4-dev-nuttx-focal:2021-05-04'
+                  image 'px4io/px4-dev-nuttx-focal:2021-09-08'
                  args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
                }
              }
@@ -633,85 +666,89 @@ pipeline {
                    printTopics()
                  }
                }
-
-              }
-            } // stage test
-          }
-        }
-
-        stage("modalai_fc-v1_test") {
-          stages {
-            stage("build modalai_fc-v1_test") {
-              agent {
-                docker {
-                  image 'px4io/px4-dev-nuttx-focal:2021-05-04'
-                  args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
-                }
-              }
-              steps {
-                checkout scm
-                sh 'export'
-                sh 'make distclean'
-                sh 'ccache -s'
-                sh 'git fetch --tags'
-                sh 'make modalai_fc-v1_test'
-                sh 'make modalai_fc-v1_test bootloader_elf'
-                sh 'ccache -s'
-                stash includes: 'build/*/*.elf, platforms/nuttx/Debug/upload_jlink_gdb.sh, Tools/HIL/*.py', name: 'modalai_fc-v1_test'
              }
              post {
                always {
-                    sh 'make distclean'
+                    resetBoard()
                }
              }
-            } // stage build
-            stage("hardware") {
-              agent {
-                label 'modalai_fc-v1'
-              }
-              stages {
-                stage("flash") {
-                  steps {
-                    sh 'export'
-                    sh 'find /dev/serial'
-                    unstash 'modalai_fc-v1_test'
-                    sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/modalai_fc-v1_test/modalai_fc-v1_bootloader.elf'
-                    // flash board and watch bootup
-                    sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/modalai_fc-v1_test/modalai_fc-v1_test.elf && ./Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-*` --baudrate 57600'
-                  }
-                }
-                stage("tests") {
-                  steps {
-                    runTests()
-                  }
-                }
-                stage("status") {
-                  steps {
-                    // configure
-                    resetParameters()
-                    sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "SYS_AUTOSTART" --value "4001"'    // generic quadcopter
-                    sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "SYS_BL_UPDATE" --value "1"'       // update bootloader
-                    checkStatus()
-                    quickCalibrate()
-                  }
-                }
-                stage("print topics") {
-                  steps {
-                    printTopics()
-                  }
-                }
-
-              }
            } // stage test
          }
        }

+        // stage("modalai_fc-v1_test") {
+        //   stages {
+        //     stage("build modalai_fc-v1_test") {
+        //       agent {
+        //         docker {
+        //           image 'px4io/px4-dev-nuttx-focal:2021-09-08'
+        //           args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
+        //         }
+        //       }
+        //       steps {
+        //         checkout scm
+        //         sh 'export'
+        //         sh 'make distclean'
+        //         sh 'ccache -s'
+        //         sh 'git fetch --tags'
+        //         sh 'make modalai_fc-v1_test'
+        //         sh 'make modalai_fc-v1_test bootloader_elf'
+        //         sh 'ccache -s'
+        //         stash includes: 'build/*/*.elf, platforms/nuttx/Debug/upload_jlink_gdb.sh, Tools/HIL/*.py', name: 'modalai_fc-v1_test'
+        //       }
+        //       post {
+        //         always {
+        //             sh 'make distclean'
+        //         }
+        //       }
+        //     } // stage build
+        //     stage("hardware") {
+        //       agent {
+        //         label 'modalai_fc-v1'
+        //       }
+        //       stages {
+        //         stage("flash") {
+        //           steps {
+        //             sh 'export'
+        //             sh 'find /dev/serial'
+        //             unstash 'modalai_fc-v1_test'
+        //             sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/modalai_fc-v1_test/modalai_fc-v1_bootloader.elf'
+        //             // flash board and watch bootup
+        //             sh './platforms/nuttx/Debug/upload_jlink_gdb.sh build/modalai_fc-v1_test/modalai_fc-v1_test.elf && ./Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-*` --baudrate 57600'
+        //           }
+        //         }
+        //         stage("tests") {
+        //           steps {
+        //             runTests()
+        //           }
+        //         }
+        //         stage("status") {
+        //           steps {
+        //             // configure
+        //             resetParameters()
+        //             sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "SYS_AUTOSTART" --value "4001"'    // generic quadcopter
+        //             sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "SYS_BL_UPDATE" --value "1"'       // update bootloader
+        //             checkStatus()
+        //             quickCalibrate()
+        //           }
+        //         }
+        //         stage("print topics") {
+        //           steps {
+        //             printTopics()
+        //           }
+        //         }
+
+        //       }
+        //     } // stage test
+        //   }
+        // }
+
        stage("nxp_fmuk66-v3_test") {
          stages {
            stage("build nxp_fmuk66-v3_test") {
              agent {
                docker {
-                  image 'px4io/px4-dev-nuttx-focal:2021-05-04'
+                  image 'px4io/px4-dev-nuttx-focal:2021-09-08'
                  args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
                }
              }
@@ -756,6 +793,7 @@ pipeline {
                  steps {
                    // configure
                    resetParameters()
+                    sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "IMU_GYRO_RATEMAX" --value "400"'
                    sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "SYS_AUTOSTART" --value "4001"'    // generic quadcopter
                    sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "SYS_BL_UPDATE" --value "1"'       // update bootloader
                    checkStatus()
@@ -767,7 +805,11 @@ pipeline {
                    printTopics()
                  }
                }
-
+              }
+              post {
+                always {
+                    resetBoard()
+                }
              }
            } // stage test
          }
@@ -800,12 +842,13 @@ void quickCalibrate() {
 void checkStatus() {
  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 "param status"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param show"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param show SYS*"'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "reboot" || true' // reboot to apply
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param show"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param show SYS*"'

  // run logger
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "logger on"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "logger on" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "sleep 1"' // sleep before continuing
  // status commands
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "cat /proc/meminfo"'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "cat /proc/uptime"'
@@ -817,8 +860,9 @@ void checkStatus() {
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "free"'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "gps status"'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener cpuload; top once; listener cpuload"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "logger status"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "logger status" || true'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ls /"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ls /bin"'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ls /dev"'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ls /etc"'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ls /obj"'
@@ -827,7 +871,7 @@ void checkStatus() {
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "mavlink status"'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "mount"'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "mtd status"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param show"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param show" || true'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param status"'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "perf latency"'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "perf"'
@@ -835,7 +879,6 @@ void checkStatus() {
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "pwm info -d /dev/pwm_output0"'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "pwm info -d /dev/pwm_output1" || true'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "pwm_out status"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "sd_bench -r 2"'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "sensors status"'
  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 "uavcan status" || true'
@@ -844,7 +887,7 @@ void checkStatus() {
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ver all"'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "work_queue status"'
  // stop logger
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "logger off"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "logger off" || true'
 }

 void resetParameters() {
@@ -862,12 +905,12 @@ void runTests() {
  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 "work_queue status"'

-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "commander_tests"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "commander_tests" || true'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "controllib_test"'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "lightware_laser_test"'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "mavlink_tests" || true' // TODO
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb_tests"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb_tests latency_test"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb_tests latency_test" || true'

  sh './Tools/HIL/run_tests.py --device `find /dev/serial -name *usb-*`'

@@ -877,7 +920,24 @@ void runTests() {
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "mtd rwtest"'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "mtd erase"'

+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "sd_bench"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "sd_bench -u"'
+
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "sensors stop"' // ignore irrelevant sensor timeouts during microbenchmarks
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "microbench all"'
+
+  // test rebooting multiple times
+  resetParameters()
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SYS_AUTOSTART 4001" || true'
+  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'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param show CBRK*; param show SYS*"' // check that CBRK_BUZZER and SYS_AUTOSTART haven't been lost
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "reboot" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param show CBRK*; param show SYS*"' // check that CBRK_BUZZER and SYS_AUTOSTART haven't been lost
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "reboot" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param show CBRK*; param show SYS*"' // check that CBRK_BUZZER and SYS_AUTOSTART haven't been lost
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "reboot" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param show CBRK*; param show SYS*"' // check that CBRK_BUZZER and SYS_AUTOSTART haven't been lost
 }

 void printTopics() {
@@ -885,91 +945,127 @@ void printTopics() {
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb status"'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb top -1 -a"'

-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener actuator_armed"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener actuator_controls_0"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener actuator_controls_1"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener actuator_controls_2"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener actuator_controls_3"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener actuator_outputs"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener adc_report"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener airspeed_validated"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener airspeed_wind"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener battery_status"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener commander_state"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener cpuload"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener distance_sensor"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener ekf2_timestamps"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener ekf_gps_drift"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener esc_status"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_attitude"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_baro_bias"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_event_flags"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_global_position"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_innovation_test_ratios"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_innovation_variances"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_innovations"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_local_position"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_odometry"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_optical_flow_vel"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_selector_status"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_sensor_bias"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_states"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_status"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_status_flags"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_wind"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener event"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener input_rc"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener led_control"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener log_message"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener logger_status"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener manual_control_setpoint"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener mavlink_log"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener mission"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener multirotor_motor_limits"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener optical_flow"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener parameter_update"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener position_controller_landing_status"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener position_setpoint_triplet"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener radio_status"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener rate_ctrl_status"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener safety"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_accel"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_accel_fifo"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_baro"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_combined"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_gyro"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_gyro_fft"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_gyro_fifo"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_mag"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_preflight_mag"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_selection"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensors_status_imu"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener system_power"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener task_stack_info"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener telemetry_status"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener test_motor"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener trajectory_setpoint"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener tune_control"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_acceleration"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_air_data"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_angular_acceleration"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_angular_velocity"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_attitude"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_attitude_setpoint"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_command"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_command_ack"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_control_mode"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_global_position"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_imu"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_imu_status"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_land_detected"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_local_position"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_local_position_setpoint"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_magnetometer"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_odometry"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_rates_setpoint"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_status"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_status_flags"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vtol_vehicle_status"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener yaw_estimator_status"'
+  // these are for casually inspecting the system, output failure doesn't matter
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener actuator_armed" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener actuator_controls_0" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener actuator_controls_1" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener actuator_controls_2" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener actuator_controls_3" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener actuator_outputs" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener adc_report" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener airspeed_validated" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener airspeed_wind" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener battery_status" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener commander_state" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener cpuload" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener distance_sensor" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener ekf2_timestamps" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener ekf_gps_drift" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener esc_status" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_attitude" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_baro_bias" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_event_flags" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_global_position" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_innovation_test_ratios" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_innovation_variances" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_innovations" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_local_position" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_odometry" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_optical_flow_vel" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_selector_status" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_sensor_bias" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_states" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_status" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_status_flags" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_wind" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener event" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener heater_status" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener input_rc" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener led_control" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener log_message" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener logger_status" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener manual_control_setpoint" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener mavlink_log" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener mission" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener multirotor_motor_limits" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener optical_flow" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener parameter_update" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener position_controller_landing_status" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener position_setpoint_triplet" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener radio_status" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener rate_ctrl_status" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener safety" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_accel" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_accel_fifo" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_baro" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_combined" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_gyro" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_gyro_fft" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_gyro_fifo" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_mag" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_preflight_mag" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensor_selection" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener sensors_status_imu" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener system_power" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener task_stack_info" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener telemetry_status" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener test_motor" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener trajectory_setpoint" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener tune_control" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_acceleration" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_air_data" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_angular_acceleration" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_angular_velocity" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_attitude" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_attitude_setpoint" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_command" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_command_ack" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_control_mode" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_global_position" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_imu" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_imu_status" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_land_detected" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_local_position" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_local_position_setpoint" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_magnetometer" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_odometry" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_rates_setpoint" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_status" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vehicle_status_flags" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener vtol_vehicle_status" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener yaw_estimator_status" || true'
+}
+
+void resetBoard() {
+  resetParameters()
+
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SYS_AUTOSTART 0" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set IMU_GYRO_RATEMAX 200" || true' // limit cpu usage
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set MAV_0_CONFIG 0" || true'       // limit cpu usage
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set MAV_1_CONFIG 0" || true'       // limit cpu usage
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param set SDLOG_MODE -1" || true'        // limit cpu usage
+  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"'
+  // 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 "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 "mount -t vfat /dev/mmcsd0 /fs/microsd"'
+  // 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"'
 }
@@ -2,7 +2,7 @@
 // https://github.com/microsoft/vscode-dev-containers/tree/v0.134.0/containers/cpp
 {
 	"name": "px4-dev-nuttx",
-	"image": "px4io/px4-dev-nuttx-focal:2021-05-04",
+	"image": "px4io/px4-dev-nuttx-focal:2021-09-08",

 	"runArgs": [ "--cap-add=SYS_PTRACE", "--security-opt", "seccomp=unconfined" ],

@@ -29,7 +29,7 @@ jobs:
          "parameters_metadata",
        ]
    container:
-      image: px4io/px4-dev-nuttx-focal:2021-05-04
+      image: px4io/px4-dev-nuttx-focal:2021-09-08
      options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined
    steps:
    - uses: actions/checkout@v1
@@ -11,7 +11,7 @@ on:
 jobs:
  build:
    runs-on: ubuntu-latest
-    container: px4io/px4-dev-clang:2021-05-04
+    container: px4io/px4-dev-clang:2021-09-08
    steps:
    - uses: actions/checkout@v1
      with:
@@ -11,7 +11,7 @@ on:
 jobs:
  build:
    runs-on: ubuntu-latest
-    container: px4io/px4-dev-armhf:2021-05-04
+    container: px4io/px4-dev-armhf:2021-09-08
    strategy:
      matrix:
        config: [
@@ -11,7 +11,7 @@ on:
 jobs:
  build:
    runs-on: ubuntu-latest
-    container: px4io/px4-dev-aarch64:2021-05-04
+    container: px4io/px4-dev-aarch64:2021-09-08
    strategy:
      matrix:
        config: [
@@ -11,7 +11,7 @@ on:
 jobs:
  build:
    runs-on: ubuntu-latest
-    container: px4io/px4-dev-nuttx-focal:2021-05-04
+    container: px4io/px4-dev-nuttx-focal:2021-09-08
    strategy:
      matrix:
        config: [
@@ -11,15 +11,16 @@ on:
 jobs:
  build:
    runs-on: ubuntu-latest
-    container: px4io/px4-dev-nuttx-focal:2021-05-04
+    container: px4io/px4-dev-nuttx-focal:2021-09-08
    strategy:
      matrix:
        config: [
          ark_can-flow_default,
+          ark_can-gps_default,
          cuav_can-gps-v1_default,
          freefly_can-rtk-gps_default,
          holybro_can-gps-v1_default,
-          #nxp_ucans32k146_default,
+          nxp_ucans32k146_default,
          px4_fmu-v4_cannode
          ]
    steps:
@@ -23,7 +23,7 @@ jobs:
    needs: enumerate_targets
    strategy:
      matrix: ${{fromJson(needs.enumerate_targets.outputs.matrix)}}
-    container: px4io/px4-dev-${{ matrix.container }}:2021-05-04
+    container: px4io/px4-dev-${{ matrix.container }}:2021-09-08
    steps:
    - uses: actions/checkout@v1
      with:
@@ -5,7 +5,7 @@ on: pull_request
 jobs:
  unit_tests:
    runs-on: ubuntu-latest
-    container: px4io/px4-dev-base-focal:2021-05-04
+    container: px4io/px4-dev-base-focal:2021-09-08
    steps:
    - uses: actions/checkout@v2.3.1
      with:
@@ -5,7 +5,7 @@ on: push
 jobs:
  unit_tests:
    runs-on: ubuntu-latest
-    container: px4io/px4-dev-base-focal:2021-05-04
+    container: px4io/px4-dev-base-focal:2021-09-08
    env:
      GIT_COMMITTER_EMAIL: bot@px4.io
      GIT_COMMITTER_NAME: PX4BuildBot
@@ -25,7 +25,7 @@ jobs:
          #- {vehicle: "tiltrotor",     mission: "VTOL_mission_1",  build_type: "RelWithDebInfo"}

    container:
-      image: px4io/px4-dev-ros-melodic:2021-05-04
+      image: px4io/px4-dev-ros-melodic:2021-09-08
      options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined
    steps:
    - uses: actions/checkout@v1
@@ -20,7 +20,7 @@ jobs:
          #- {test_file: "mavros_posix_tests_offboard_rpyrt_ctl.test", vehicle: "iris", build_type: "RelWithDebInfo"}

    container:
-      image: px4io/px4-dev-ros-melodic:2021-05-04
+      image: px4io/px4-dev-ros-melodic:2021-09-08
      options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined
    steps:
    - uses: actions/checkout@v1
@@ -11,7 +11,7 @@ jobs:

  airframe:
    runs-on: ubuntu-latest
-    container: px4io/px4-dev-base-focal:2021-05-04
+    container: px4io/px4-dev-base-focal:2021-09-08
    steps:
    - uses: actions/checkout@v1
      with:
@@ -37,7 +37,7 @@ jobs:

  module:
    runs-on: ubuntu-latest
-    container: px4io/px4-dev-base-focal:2021-05-04
+    container: px4io/px4-dev-base-focal:2021-09-08
    steps:
    - uses: actions/checkout@v1
      with:
@@ -52,7 +52,7 @@ jobs:

  parameter:
    runs-on: ubuntu-latest
-    container: px4io/px4-dev-base-focal:2021-05-04
+    container: px4io/px4-dev-base-focal:2021-09-08
    steps:
    - uses: actions/checkout@v1
      with:
@@ -76,7 +76,7 @@ jobs:

  events:
    runs-on: ubuntu-latest
-    container: px4io/px4-dev-base-focal:2021-05-04
+    container: px4io/px4-dev-base-focal:2021-09-08
    steps:
    - uses: actions/checkout@v1
      with:
@@ -103,7 +103,7 @@ jobs:

  uorb_graph:
    runs-on: ubuntu-latest
-    container: px4io/px4-dev-nuttx-focal:2021-05-04
+    container: px4io/px4-dev-nuttx-focal:2021-09-08
    steps:
    - uses: actions/checkout@v1
      with:
@@ -118,7 +118,7 @@ jobs:

  micrortps_agent:
    runs-on: ubuntu-latest
-    container: px4io/px4-dev-base-focal:2021-05-04
+    container: px4io/px4-dev-base-focal:2021-09-08
    steps:
    - uses: actions/checkout@v1
      with:
@@ -132,7 +132,7 @@ jobs:

  ROS_msgs:
    runs-on: ubuntu-latest
-    container: px4io/px4-dev-base-focal:2021-05-04
+    container: px4io/px4-dev-base-focal:2021-09-08
    steps:
    - uses: actions/checkout@v1
      with:
@@ -145,7 +145,7 @@ jobs:

  ROS2_bridge:
    runs-on: ubuntu-latest
-    container: px4io/px4-dev-base-focal:2021-05-04
+    container: px4io/px4-dev-base-focal:2021-09-08
    steps:
    - uses: actions/checkout@v1
      with:
@@ -154,4 +154,4 @@ jobs:
    - name: PX4 ROS2 bridge
      run: |
        git clone https://github.com/PX4/px4_ros_com.git
-        ./msg/tools/uorb_to_ros_rtps_ids.py -i msg/tools/uorb_rtps_message_ids.yaml -o px4_ros_com/templates/uorb_rtps_message_ids.yaml
+        ./msg/tools/uorb_to_ros_urtps_topics.py -i msg/tools/urtps_bridge_topics.yaml -o px4_ros_com/templates/urtps_bridge_topics.yaml
@@ -21,7 +21,7 @@ jobs:
          - {latitude:  "47.397742", longitude:    "8.545594", altitude: "488", build_type: "Coverage",         model: "standard_vtol" } # Zurich

    container:
-      image: px4io/px4-dev-simulation-focal:2021-05-04
+      image: px4io/px4-dev-simulation-focal:2021-09-08
      options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined
    steps:
    - uses: actions/checkout@v1
@@ -29,11 +29,11 @@
 [submodule "platforms/nuttx/NuttX/nuttx"]
 	path = platforms/nuttx/NuttX/nuttx
 	url = https://github.com/PX4/NuttX.git
-	branch = px4_firmware_nuttx-10.0.0+
+	branch = px4_firmware_nuttx-10.1.0+
 [submodule "platforms/nuttx/NuttX/apps"]
 	path = platforms/nuttx/NuttX/apps
 	url = https://github.com/PX4/NuttX-apps.git
-	branch = px4_firmware_nuttx-10.0.0+
+	branch = px4_firmware_nuttx-10.1.0+
 [submodule "Tools/flightgear_bridge"]
 	path = Tools/flightgear_bridge
 	url = https://github.com/PX4/PX4-FlightGear-Bridge.git
@@ -65,4 +65,4 @@
 	url = https://github.com/mavlink/libevents.git
 [submodule "Tools/simulation-ignition"]
 	path = Tools/simulation-ignition
-	url = https://github.com/Auterion/px4-simulation-ignition.git
+	url = https://github.com/Auterion/px4-simulation-ignition.git
@@ -31,7 +31,6 @@
                "esc"
        ],
        "debug.toolBarLocation": "docked",
-        "editor.acceptSuggestionOnEnter": "off",
        "editor.defaultFormatter": "chiehyu.vscode-astyle",
        "editor.dragAndDrop": false,
        "editor.insertSpaces": false,
@@ -143,13 +143,13 @@ pipeline {
          }
        }

-        stage('msg files') {
+        stage('msg file docs') {
          agent {
            docker { image 'px4io/px4-dev-base-focal:2021-05-04' }
          }
          steps {
-            sh './msg/tools/generate_msg_docs.py -d /tmp/msg_docs'
-            dir('/tmp') {
+            sh 'mkdir -p build/msg_docs; ./msg/tools/generate_msg_docs.py -d build/msg_docs'
+            dir('build') {
              archiveArtifacts(artifacts: 'msg_docs/*.md')
              stash includes: 'msg_docs/*.md', name: 'msg_documentation'
            }
@@ -323,7 +323,7 @@ pipeline {
            withCredentials([usernamePassword(credentialsId: 'px4buildbot_github_personal_token', passwordVariable: 'GIT_PASS', usernameVariable: 'GIT_USER')]) {
              sh("git clone https://${GIT_USER}:${GIT_PASS}@github.com/PX4/px4_ros_com.git -b ${BRANCH_NAME}")
              // deploy uORB RTPS ID map
-              sh('./msg/tools/uorb_to_ros_rtps_ids.py -i msg/tools/uorb_rtps_message_ids.yaml -o px4_ros_com/templates/uorb_rtps_message_ids.yaml')
+              sh('./msg/tools/uorb_to_ros_urtps_topics.py -i msg/tools/urtps_bridge_topics.yaml -o px4_ros_com/templates/urtps_bridge_topics.yaml')
              sh('cd px4_ros_com; git status; git add .; git commit -a -m "Update uORB RTPS ID map `date`" || true')
              sh('cd px4_ros_com; git push origin ${BRANCH_NAME} || true')
              // deploy uORB RTPS required tools
@@ -481,6 +481,7 @@ submodulesupdate:
 	@git submodule update --quiet --init --recursive --jobs 4 || true
 	@git submodule sync --recursive
 	@git submodule update --init --recursive --jobs 4
+	@git fetch --all --tags --recurse-submodules=yes --jobs=4

 gazeboclean:
 	@rm -rf ~/.gazebo/*
@@ -490,7 +491,7 @@ distclean: gazeboclean
 	@rm -rf "$(SRC_DIR)/build"
 	@git clean --force -X "$(SRC_DIR)/msg/" "$(SRC_DIR)/platforms/" "$(SRC_DIR)/posix-configs/" "$(SRC_DIR)/ROMFS/" "$(SRC_DIR)/src/" "$(SRC_DIR)/test/" "$(SRC_DIR)/Tools/"

-# Help / Error
+# Help / Error / Misc
 # --------------------------------------------------------------------

 # All other targets are handled by PX4_MAKE. Add a rule here to avoid printing an error.
@@ -524,3 +525,18 @@ check_px4: $(call make_list,nuttx,"px4") \

 check_nxp: $(call make_list,nuttx,"nxp") \
 	sizes
+
+ifneq ($(ROS2_WS_DIR),)
+  ROS2_WS_DIR := $(basename ${ROS2_WS_DIR})
+else
+  ROS2_WS_DIR := ~/colcon_ws
+endif
+
+update_ros2_bridge:
+	@Tools/update_px4_ros2_bridge.sh --ws_dir ${ROS2_WS_DIR} --all
+
+update_px4_ros_com:
+	@Tools/update_px4_ros2_bridge.sh --ws_dir ${ROS2_WS_DIR} --px4_ros_com
+
+update_px4_msgs:
+	@Tools/update_px4_ros2_bridge.sh --ws_dir ${ROS2_WS_DIR} --px4_msgs
@@ -120,6 +120,7 @@ add_custom_command(
 	COMMAND ${CMAKE_COMMAND} -E touch ${romfs_extract_stamp}
 	WORKING_DIRECTORY ${romfs_gen_root_dir}
 	DEPENDS ${romfs_tar_file}
+	VERBATIM
 	)

 add_custom_command(
@@ -15,10 +15,6 @@ set +e
 #
 #------------------------------------------------------------------------------
 set R /
-#
-# Mount the procfs.
-#
-mount -t procfs /proc

 #
 # Start CDC/ACM serial driver.
@@ -34,9 +34,10 @@
 add_subdirectory(airframes)

 px4_add_romfs_files(
-	px4-rc.params
-	px4-rc.simulator
 	px4-rc.mavlink
+	px4-rc.params
+	px4-rc.rtps
+	px4-rc.simulator
 	rc.replay
 	rcS
 )
@@ -0,0 +1,4 @@
+#!/bin/sh
+# shellcheck disable=SC2154
+
+micrortps_client start -t UDP -r $((2019+2*px4_instance)) -s $((2020+2*px4_instance))
@@ -21,8 +21,6 @@ then
 fi

 # initialize script variables
-set AUX_MODE                    none
-set AUX_BANK2			none
 set IO_PRESENT                  no
 set MAV_TYPE                    none
 set MIXER                       none
@@ -228,8 +226,7 @@ navigator start
 # Try to start the micrortps_client with UDP transport if module exists
 if px4-micrortps_client status > /dev/null 2>&1
 then
-	# shellcheck disable=SC2154
-	micrortps_client start -t UDP -r $((2019+2*px4_instance)) -s $((2020+2*px4_instance))
+	. px4-rc.rtps
 fi

 if param greater -s MNT_MODE_IN -1
@@ -19,6 +19,7 @@
 # @maintainer Lorenz Meier <lorenz@px4.io>
 #
 # @board bitcraze_crazyflie exclude
+# @board px4_fmu-v2 exclude
 #

 . ${R}etc/init.d/rc.mc_defaults
@@ -23,6 +23,7 @@
 # @maintainer William Peale <develop707@gmail.com>
 #
 # @board bitcraze_crazyflie exclude
+# @board px4_fmu-v2 exclude
 #

 set VEHICLE_TYPE mc
@@ -6,6 +6,7 @@
 # @class Copter
 #
 # @maintainer Lorenz Meier <lorenz@px4.io>
+# @board px4_fmu-v2 exclude
 #

 . ${R}etc/init.d/rc.mc_defaults
@@ -20,6 +20,7 @@
 # @maintainer Lorenz Meier <lorenz@px4.io>
 #
 # @board bitcraze_crazyflie exclude
+# @board px4_fmu-v2 exclude
 #

 . ${R}etc/init.d/rc.mc_defaults
@@ -29,12 +29,6 @@ then
        fi
 fi

-# USE_IO is set to 'no' for all boards w/o px4io driver or SYS_USE_IO disabled
-if [ $USE_IO = no -a $AUX_BANK2 = none ]
-then
-	set AUX_MODE none
-fi
-
 #
 # Set the default output mode if none was set.
 #
@@ -76,15 +70,6 @@ then
 		fi
 	fi

-	if [ $OUTPUT_MODE = $OUTPUT_CMD ]
-	then
-		if ! $OUTPUT_CMD mode_$FMU_MODE
-		then
-			echo "$OUTPUT_CMD start failed"
-			tune_control play error
-		fi
-	fi
-
 	if [ $OUTPUT_MODE = uavcan_esc ]
 	then
 		if param compare UAVCAN_ENABLE 0
@@ -98,6 +83,15 @@ then
 		. ${R}etc/init.d/rc.io
 	fi

+	if [ $OUTPUT_MODE = $OUTPUT_CMD -o $OUTPUT_MODE = io ]
+	then
+		if ! $OUTPUT_CMD start
+		then
+			echo "$OUTPUT_CMD start failed"
+			tune_control play error
+		fi
+	fi
+
 	#
 	# Start IO for RC input if needed.
 	#
@@ -161,7 +155,7 @@ else
 	fi
 fi

-if [ $MIXER_AUX != none -a $AUX_MODE != none ]
+if [ $MIXER_AUX != none ]
 then
 	#
 	# Load aux mixer.
@@ -179,34 +173,26 @@ then

 	if [ $MIXER_AUX_FILE != none ]
 	then
-		# Start the output module
-		if $OUTPUT_CMD mode_${AUX_MODE}
+		# Append aux mixer to main device.
+		if param greater SYS_HITL 0
 		then
-			# Append aux mixer to main device.
-			if param greater SYS_HITL 0
+			if mixer append ${OUTPUT_DEV} ${MIXER_AUX_FILE}
 			then
-				if mixer append ${OUTPUT_DEV} ${MIXER_AUX_FILE}
-				then
-					echo "INFO  [init] Mixer: ${MIXER_AUX_FILE} appended to ${OUTPUT_DEV}"
-				else
-					echo "ERROR  [init] Failed appending mixer: ${MIXER_AUX_FILE}"
-				fi
-			fi
-			if [ -e $OUTPUT_AUX_DEV -a $OUTPUT_MODE != hil ]
-			then
-				if mixer load ${OUTPUT_AUX_DEV} ${MIXER_AUX_FILE}
-				then
-					echo "INFO  [init] Mixer: ${MIXER_AUX_FILE} on ${OUTPUT_AUX_DEV}"
-				else
-					echo "ERROR  [init] Failed loading mixer: ${MIXER_AUX_FILE}"
-				fi
+				echo "INFO  [init] Mixer: ${MIXER_AUX_FILE} appended to ${OUTPUT_DEV}"
 			else
-				echo "INFO  [init] setting PWM_AUX_OUT none"
-				set PWM_AUX_OUT none
+				echo "ERROR  [init] Failed appending mixer: ${MIXER_AUX_FILE}"
+			fi
+		fi
+		if [ -e $OUTPUT_AUX_DEV -a $OUTPUT_MODE != hil ]
+		then
+			if mixer load ${OUTPUT_AUX_DEV} ${MIXER_AUX_FILE}
+			then
+				echo "INFO  [init] Mixer: ${MIXER_AUX_FILE} on ${OUTPUT_AUX_DEV}"
+			else
+				echo "ERROR  [init] Failed loading mixer: ${MIXER_AUX_FILE}"
 			fi
 		else
-			echo "ERROR: Could not start: pwm_out mode_pwm"
-			tune_control play -t 18 # tune 18 = PROG_PX4IO_ERR
+			echo "INFO  [init] setting PWM_AUX_OUT none"
 			set PWM_AUX_OUT none
 		fi

@@ -228,36 +214,6 @@ fi

 param set PWM_AUX_OUT ${PWM_AUX_OUT}

-if [ $MIXER_AUX != none -a $AUX_MODE = none -a -e $OUTPUT_AUX_DEV ]
-then
-	#
-	# Load aux mixer.
-	#
-	if [ -f ${SDCARD_MIXERS_PATH}/${MIXER_AUX}.aux.mix ]
-	then
-		set MIXER_AUX_FILE ${SDCARD_MIXERS_PATH}/${MIXER_AUX}.aux.mix
-	else
-
-		if [ -f /etc/mixers/${MIXER_AUX}.aux.mix ]
-		then
-			set MIXER_AUX_FILE /etc/mixers/${MIXER_AUX}.aux.mix
-		fi
-	fi
-
-	if mixer load ${OUTPUT_AUX_DEV} ${MIXER_AUX_FILE}
-	then
-		echo "INFO  [init] Mixer: ${MIXER_AUX_FILE} on ${OUTPUT_AUX_DEV}"
-
-		# Set PWM_AUX output frequency.
-		if [ $PWM_AUX_RATE != none ]
-		then
-			pwm rate -c ${PWM_AUX_OUT} -r ${PWM_AUX_RATE} -d ${OUTPUT_AUX_DEV}
-		fi
-	else
-		echo "ERROR  [init] Failed loading mixer: ${MIXER_AUX_FILE}"
-	fi
-fi
-
 if [ $OUTPUT_MODE = pwm_out -o $OUTPUT_MODE = io ]
 then
 	if [ $PWM_OUT != none ]
@@ -127,6 +127,34 @@ then
 	fi
 fi

+# Eagle Tree airspeed sensor external I2C
+if param compare -s SENS_EN_ETSASPD 1
+then
+	ets_airspeed start -X
+fi
+
+# Sensirion SDP3X differential pressure sensor external I2C
+if param compare -s SENS_EN_SDP3X 1
+then
+	if ! sdp3x_airspeed start -X
+	then
+		# try another common address
+		sdp3x_airspeed start -X -a 0x22
+	fi
+fi
+
+# TE MS4525 differential pressure sensor external I2C
+if param compare -s SENS_EN_MS4525 1
+then
+	ms4525_airspeed start -X
+fi
+
+# TE MS5525 differential pressure sensor external I2C
+if param compare -s SENS_EN_MS5525 1
+then
+	ms5525_airspeed start -X
+fi
+
 # probe for optional external I2C devices
 if param compare SENS_EXT_I2C_PRB 1
 then
@@ -143,30 +171,6 @@ then

 	# start last (wait for possible icm20948 passthrough mode)
 	ak09916 -X -q start
-
-	# differential pressure sensors
-	if [ ${VEHICLE_TYPE} = fw -o ${VEHICLE_TYPE} = vtol ]
-	then
-		# Always try to start the airspeed sensors
-		# even if their usage might be disabled
-		sdp3x_airspeed start -X -q
-		sdp3x_airspeed start -X -a 0x22 -q
-
-		# Pixhawk 2.1 has a MS5611 on I2C which gets wrongly
-		# detected as MS5525 because the chip manufacturer was so
-		# clever to assign the same I2C address and skip a WHO_AM_I
-		# register.
-		if [ $BOARD_FMUV3 = 21 ]
-		then
-			ms5525_airspeed start -X -b 2 -q
-		else
-			ms5525_airspeed start -X -q
-		fi
-
-		ms4525_airspeed start -X -q
-
-		ets_airspeed start -X -q
-	fi
 fi

 ###############################################################################
@@ -22,10 +22,8 @@ set +e
 #
 set R /
 set AUTOCNF no
-set AUX_MODE pwm
 set FCONFIG /fs/microsd/etc/config.txt
 set FEXTRAS /fs/microsd/etc/extras.txt
-set FMU_MODE pwm
 set FRC /fs/microsd/etc/rc.txt
 set IOFW "/etc/extras/px4_io-v2_default.bin"
 set IO_PRESENT no
@@ -51,11 +49,6 @@ set STARTUP_TUNE 1
 set USE_IO no
 set VEHICLE_TYPE none

-#
-# Mount the procfs.
-#
-mount -t procfs /proc
-
 #
 # Start CDC/ACM serial driver.
 #
@@ -69,8 +62,43 @@ ver all
 #
 # Try to mount the microSD card.
 #
+set SDCARD_AVAILABLE no
 # REBOOTWORK this needs to start after the flight control loop.
 if mount -t vfat /dev/mmcsd0 /fs/microsd
+then
+	set PX4_INIT_TEST_FILE "/fs/microsd/.px4_init_test_file"
+	date >> $PX4_INIT_TEST_FILE
+
+	if [ -f $PX4_INIT_TEST_FILE ]
+	then
+		cat $PX4_INIT_TEST_FILE
+		rm $PX4_INIT_TEST_FILE
+
+		if [ ! -f $PX4_INIT_TEST_FILE ]
+		then
+			set SDCARD_AVAILABLE yes
+		fi
+	fi
+fi
+
+if [ $SDCARD_AVAILABLE = no ]
+then
+	set STARTUP_TUNE 14 # tune 14 = SD_INIT
+	if mkfatfs -F 32 /dev/mmcsd0
+	then
+		if mount -t vfat /dev/mmcsd0 /fs/microsd
+		then
+			set SDCARD_AVAILABLE yes
+			echo "INFO [init] card formatted"
+
+		else
+			set STARTUP_TUNE 15 # tune 15 = SD_ERROR
+			echo "ERROR [init] format failed"
+		fi
+	fi
+fi
+
+if [ $SDCARD_AVAILABLE = yes ]
 then
 	if hardfault_log check
 	then
@@ -81,20 +109,8 @@ then
 			hardfault_log reset
 		fi
 	fi
-else
-	# tune SD_INIT
-	set STARTUP_TUNE 14 # tune 14 = SD_INIT
-	if mkfatfs /dev/mmcsd0
-	then
-		if mount -t vfat /dev/mmcsd0 /fs/microsd
-		then
-			echo "INFO [init] card formatted"
-		else
-			set STARTUP_TUNE 15 # tune 15 = SD_ERROR
-			echo "ERROR [init] format failed"
-		fi
-	fi
 fi
+unset SDCARD_AVAILABLE

 #
 # Look for an init script on the microSD card.
@@ -128,7 +144,7 @@ else
 	then
 		param reset_all
 	fi
-	if ver hwtypecmp V5X00 V5X90 V5Xa0
+	if ver hwcmp PX4_FMU_V5X PX4_FMU_V6X
 	then
 		netman update -i eth0
 	fi
@@ -358,52 +374,9 @@ else
 		commander start
 	fi

-	# Sensors on the PWM interface bank.
-	if param compare -s SENS_EN_LL40LS 1
-	then
-		# Clear pins 5 and 6.
-		set FMU_MODE pwm4
-		set AUX_MODE pwm4
-	fi
-
-
-	# Check if ATS is enabled
-	if param compare FD_EXT_ATS_EN 1
-	then
-		# Clear pins 5 and 6.
-		set FMU_MODE pwm4
-		set AUX_MODE pwm4
-	fi

 	if param greater -s TRIG_MODE 0
 	then
-		if param compare TRIG_PINS_EX 0
-		then
-			# We ONLY support trigger on pins 5+6 or 7+8 when simultanously using AUX for actuator output.
-			if param compare TRIG_PINS 56
-			then
-				# clear pins 5 and 6
-				set FMU_MODE pwm4
-				set AUX_MODE pwm4
-			else
-				if param compare TRIG_PINS 78
-				then
-					# clear pins 7 and 8
-					set FMU_MODE pwm6
-					set AUX_MODE pwm6
-				else
-					set FMU_MODE none
-					set AUX_MODE none
-				fi
-			fi
-		else
-			if param compare TRIG_PINS_EX 12288
-			then
-				set FMU_MODE pwm12
-				set AUX_MODE pwm12
-			fi
-		fi
-
 		camera_trigger start
 		camera_feedback start
 	fi
@@ -431,14 +404,7 @@ else
 		rc_input start $RC_INPUT_ARGS
 	fi

-	#
-	# Configure vehicle type specific parameters.
-	# Note: rc.vehicle_setup is the entry point for rc.interface,
-	#       rc.fw_apps, rc.mc_apps, rc.rover_apps, and rc.vtol_apps.
-	#
-	. ${R}etc/init.d/rc.vehicle_setup
-
-	# Camera capture driver
+	# Camera capture driver (before pwm_out)
 	if param greater -s CAM_CAP_FBACK 0
 	then
 		if camera_capture start
@@ -447,6 +413,13 @@ else
 		fi
 	fi

+	#
+	# Configure vehicle type specific parameters.
+	# Note: rc.vehicle_setup is the entry point for rc.interface,
+	#       rc.fw_apps, rc.mc_apps, rc.rover_apps, and rc.vtol_apps.
+	#
+	. ${R}etc/init.d/rc.vehicle_setup
+
 	#
 	# Start the navigator.
 	#
@@ -546,10 +519,8 @@ fi
 #
 unset R
 unset AUTOCNF
-unset AUX_MODE
 unset FCONFIG
 unset FEXTRAS
-unset FMU_MODE
 unset FRC
 unset IO_PRESENT
 unset IOFW
@@ -1,5 +1,6 @@
 Aileron/rudder/elevator/throttle mixer for PX4FMU
 ==================================================
+# @board px4_fmu-v2 exclude

 Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0
 (roll), 1 (pitch) and 3 (thrust).
@@ -1,5 +1,6 @@
 TF-G2 autogyro mixer
 ==================================================
+# @board px4_fmu-v2 exclude

 Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0
 (roll), 1 (pitch) and 3 (thrust).
@@ -1,5 +1,6 @@
 Viper Delta-wing mixer
 =================================
+# @board px4_fmu-v2 exclude

 Designed for Viper.

@@ -1,4 +1,5 @@
 # CruiseAder Claire Main Multirotor mixer for PX4FMU
+# @board px4_fmu-v2 exclude

 #

@@ -1,5 +1,6 @@
 Delta-wing mixer for PX4FMU
 ===========================
+# @board px4_fmu-v2 exclude

 This file defines mixers suitable for controlling a delta wing aircraft using
 PX4FMU. The configuration assumes the elevon servos are connected to PX4FMU
@@ -1,4 +1,5 @@
 # Direct mixer
+# @board px4_fmu-v2 exclude

 A: 0
 A: 1
@@ -1,3 +1,4 @@
 # Octo coaxial with wide arms
+# @board px4_fmu-v2 exclude

 R: 8cw
@@ -1,5 +1,6 @@
 Delta-wing VTOL mixer
 =====================
+# @board px4_fmu-v2 exclude

 This file defines mixers suitable for controlling a delta wing VTOL aircraft using
 PX4FMU. The configuration assumes the elevon servos are connected to PX4FMU
@@ -1,5 +1,6 @@
 Tailsitter duo mixer
 ============================
+# @board px4_fmu-v2 exclude

 This file defines a mixer for a generic duo tailsitter VTOL (eg TBS Caipirinha tailsitter edition). This vehicle
 has two motors in total, one attached to each wing. It also has two elevons which
@@ -30,8 +30,11 @@ def print_line(line):
    if "FAILED" in line:
        line = line.replace("FAILED", f"{COLOR_RED}FAILED{COLOR_RESET}", 1)

-    current_time = datetime.datetime.now()
-    print('[{0}] {1}'.format(current_time.isoformat(timespec='milliseconds'), line), end='')
+    if "\n" in line:
+        current_time = datetime.datetime.now()
+        print('[{0}] {1}'.format(current_time.isoformat(timespec='milliseconds'), line), end='')
+    else:
+        print('{0}'.format(line), end='')

 def monitor_firmware_upload(port, baudrate):
    ser = serial.Serial(port, baudrate, bytesize=serial.EIGHTBITS, parity=serial.PARITY_NONE, stopbits=serial.STOPBITS_ONE, timeout=1, xonxoff=True, rtscts=False, dsrdtr=False)
@@ -40,16 +43,21 @@ def monitor_firmware_upload(port, baudrate):
    timeout_start = time.time()
    timeout_newline = time.time()

+    return_code = 0
+
    while True:
        serial_line = ser.readline().decode("ascii", errors='ignore')

        if len(serial_line) > 0:
+            if "ERROR" in serial_line:
+                return_code = -1
+
            print_line(serial_line)

        if "NuttShell (NSH)" in serial_line:
-            sys.exit(0)
+            sys.exit(return_code)
        elif "nsh>" in serial_line:
-            sys.exit(0)
+            sys.exit(return_code)

        if time.time() > timeout_start + timeout:
            print("Error, timeout")
@@ -35,7 +35,7 @@ def do_param_set_cmd(port, baudrate, param_name, param_value):
    ser = serial.Serial(port, baudrate, bytesize=serial.EIGHTBITS, parity=serial.PARITY_NONE, stopbits=serial.STOPBITS_ONE, timeout=0.1, xonxoff=True, rtscts=False, dsrdtr=False)

    timeout_start = time.time()
-    timeout = 10  # 10 seconds
+    timeout = 30  # 30 seconds

    # wait for nsh prompt
    while True:
@@ -30,14 +30,18 @@ def print_line(line):
    if "FAILED" in line:
        line = line.replace("FAILED", f"{COLOR_RED}FAILED{COLOR_RESET}", 1)

-    current_time = datetime.datetime.now()
-    print('[{0}] {1}'.format(current_time.isoformat(timespec='milliseconds'), line), end='')
+    if "\n" in line:
+        current_time = datetime.datetime.now()
+        print('[{0}] {1}'.format(current_time.isoformat(timespec='milliseconds'), line), end='')
+    else:
+        print('{0}'.format(line), end='')
+

 def do_nsh_cmd(port, baudrate, cmd):
-    ser = serial.Serial(port, baudrate, bytesize=serial.EIGHTBITS, parity=serial.PARITY_NONE, stopbits=serial.STOPBITS_ONE, timeout=0.1, xonxoff=True, rtscts=False, dsrdtr=False)
+    ser = serial.Serial(port, baudrate, bytesize=serial.EIGHTBITS, parity=serial.PARITY_NONE, stopbits=serial.STOPBITS_ONE, timeout=0.2, xonxoff=True, rtscts=False, dsrdtr=False)

    timeout_start = time.time()
-    timeout = 10  # 10 seconds
+    timeout = 30  # 30 seconds

    # wait for nsh prompt
    while True:
@@ -90,13 +94,19 @@ def do_nsh_cmd(port, baudrate, cmd):
    timeout_start = time.time()
    timeout = 180 # 3 minutes

+    return_code = 0
+
    while True:
        serial_line = ser.readline().decode("ascii", errors='ignore')

        if success_cmd in serial_line:
+            sys.exit(return_code)
            break
        else:
            if len(serial_line) > 0:
+                if "ERROR " in serial_line:
+                    return_code = -1
+
                print_line(serial_line)

            if "nsh>" in serial_line:
@@ -32,14 +32,17 @@ def print_line(line):
    if "FAILED" in line:
        line = line.replace("FAILED", f"{COLOR_RED}FAILED{COLOR_RESET}", 1)

-    current_time = datetime.datetime.now()
-    print('[{0}] {1}'.format(current_time.isoformat(timespec='milliseconds'), line), end='')
+    if "\n" in line:
+        current_time = datetime.datetime.now()
+        print('[{0}] {1}'.format(current_time.isoformat(timespec='milliseconds'), line), end='')
+    else:
+        print('{0}'.format(line), end='')

 def do_test(port, baudrate, test_name):
    ser = serial.Serial(port, baudrate, bytesize=serial.EIGHTBITS, parity=serial.PARITY_NONE, stopbits=serial.STOPBITS_ONE, timeout=0.2, xonxoff=True, rtscts=False, dsrdtr=False)

    timeout_start = time.time()
-    timeout = 10  # 10 seconds
+    timeout = 30  # 30 seconds

    # wait for nsh prompt
    while True:
@@ -4,16 +4,16 @@ if [ -z ${PX4_DOCKER_REPO+x} ]; then
 	echo "guessing PX4_DOCKER_REPO based on input";
 	if [[ $@ =~ .*px4_fmu.* ]]; then
 		# nuttx-px4fmu-v{1,2,3,4,5}
-		PX4_DOCKER_REPO="px4io/px4-dev-nuttx-focal:2021-05-04"
+		PX4_DOCKER_REPO="px4io/px4-dev-nuttx-focal:2021-09-08"
 	elif [[ $@ =~ .*navio2.* ]] || [[ $@ =~ .*raspberry.* ]] || [[ $@ =~ .*beaglebone.* ]] || [[ $@ =~ .*pilotpi.default ]]; then
 		# beaglebone_blue_default, emlid_navio2_default, px4_raspberrypi_default, scumaker_pilotpi_default
-		PX4_DOCKER_REPO="px4io/px4-dev-armhf:2021-02-04"
+		PX4_DOCKER_REPO="px4io/px4-dev-armhf:2021-08-18"
 	elif [[ $@ =~ .*pilotpi.arm64 ]]; then
 		# scumaker_pilotpi_arm64
 		PX4_DOCKER_REPO="px4io/px4-dev-aarch64:latest"
 	elif [[ $@ =~ .*navio2.* ]] || [[ $@ =~ .*raspberry.* ]] || [[ $@ =~ .*bebop.* ]]; then
 		# posix_rpi_cross, posix_bebop_default
-		PX4_DOCKER_REPO="px4io/px4-dev-armhf:2021-02-04"
+		PX4_DOCKER_REPO="px4io/px4-dev-armhf:2021-08-18"
 	elif [[ $@ =~ .*clang.* ]] || [[ $@ =~ .*scan-build.* ]]; then
 		# clang tools
 		PX4_DOCKER_REPO="px4io/px4-dev-clang:2021-02-04"
@@ -27,7 +27,7 @@ fi

 # otherwise default to nuttx
 if [ -z ${PX4_DOCKER_REPO+x} ]; then
-	PX4_DOCKER_REPO="px4io/px4-dev-nuttx-focal:2021-05-04"
+	PX4_DOCKER_REPO="px4io/px4-dev-nuttx-focal:2021-09-08"
 fi

 # docker hygiene
@@ -86,8 +86,15 @@ sleep 1

 source ${src_path}/Tools/setup_gazebo.bash ${src_path} ${src_path}/build/${target}

+# To use gazebo_ros ROS2 plugins
+if [[ -n "$ROS_VERSION" ]] && [ "$ROS_VERSION" == "2" ]; then
+	ros_args="-s libgazebo_ros_init.so -s libgazebo_ros_factory.so"
+else
+	ros_args=""
+fi
+
 echo "Starting gazebo"
-gzserver ${src_path}/Tools/sitl_gazebo/worlds/${world}.world --verbose &
+gzserver ${src_path}/Tools/sitl_gazebo/worlds/${world}.world --verbose $ros_args &
 sleep 5

 n=0
@@ -13,6 +13,7 @@ import datetime
 from timeit import default_timer as timer
 os.environ['MAVLINK20'] = '1' # The commands require mavlink 2
 from argparse import ArgumentParser
+import signal

 try:
    from pymavlink import mavutil
@@ -24,6 +25,8 @@ except ImportError as e:
    print("")
    sys.exit(1)

+class LoggingCompleted(Exception):
+    pass

 class MavlinkLogStreaming():
    '''Streams log data via MAVLink.
@@ -49,6 +52,7 @@ class MavlinkLogStreaming():
        self.logging_started = False
        self.num_dropouts = 0
        self.target_component = 1
+        self.got_sig_int = False

    def debug(self, s, level=1):
        '''write some debug text'''
@@ -67,13 +71,24 @@ class MavlinkLogStreaming():
                mavutil.mavlink.MAV_CMD_LOGGING_STOP, 0,
                0, 0, 0, 0, 0, 0, 0)

+    def _int_handler(self, sig, frame):
+        self.got_sig_int = True
+
    def read_messages(self):
        ''' main loop reading messages '''
        measure_time_start = timer()
        measured_data = 0

        next_heartbeat_time = timer()
+        old_handler = signal.signal(signal.SIGINT, self._int_handler)
+
        while True:
+            if self.got_sig_int:
+                signal.signal(signal.SIGINT, old_handler)
+                self.got_sig_int = False
+                print('\nStopping log...')
+                self.stop_log()
+                # Continue reading until we get an ACK

            # handle heartbeat sending
            heartbeat_time = timer()
@@ -120,6 +135,9 @@ class MavlinkLogStreaming():
                        print('Logging started. Waiting for Header...')
                    else:
                        raise Exception('Logging start failed', m.result)
+                elif m.command == mavutil.mavlink.MAV_CMD_LOGGING_STOP and \
+                        m.result == mavutil.mavlink.MAV_RESULT_ACCEPTED:
+                    raise LoggingCompleted()
                return None, 0, 0

            # m is either 'LOGGING_DATA_ACKED' or 'LOGGING_DATA':
@@ -137,7 +155,8 @@ class MavlinkLogStreaming():

                if m.get_type() == 'LOGGING_DATA':
                    if not self.got_header_section:
-                        print('Header received in {:0.2f}s'.format(timer()-self.start_time))
+                        print('Header received in {:0.2f}s (size: {:.1f} KB)'.format(
+                              timer()-self.start_time, self.file.tell()/1024))
                        self.logging_started = True
                        self.got_header_section = True
                self.last_sequence = m.sequence
@@ -256,14 +275,10 @@ def main():
        print('Starting log...')
        mav_log_streaming.start_log()
        mav_log_streaming.read_messages()
-
-        print('Stopping log')
-        mav_log_streaming.stop_log()
-
    except KeyboardInterrupt:
-        print('Stopping log')
-        mav_log_streaming.stop_log()
-
+        print('Aborting')
+    except LoggingCompleted:
+        print('Done')

 if __name__ == '__main__':
    main()
@@ -221,7 +221,8 @@ class SourceParser(object):
                            else:
                                raise Exception("Could not extract event ID from {:}".format(args_split[0]))
                            event.name = event_name
-                            event.message = args_split[2][1:-1]
+                            # unescape \x, to treat the string the same as the C++ compiler
+                            event.message = args_split[2][1:-1].encode("utf-8").decode('unicode_escape')
                    elif call in ['reporter.healthFailure', 'reporter.armingCheckFailure']:
                        assert len(args_split) == num_args + 5, \
                            "Unexpected Number of arguments for: {:}, {:}".format(args_split, num_args)
@@ -682,7 +682,28 @@ class uploader(object):

        return True

-    def send_reboot(self):
+    def send_protocol_splitter_format(self, data):
+        # Header Structure:
+        #      bits:   1 2 3 4 5 6 7 8
+        # header[0] - |     Magic     | (='S')
+        # header[1] - |T|   LenH      | (T - 0: mavlink; 1: rtps)
+        # header[2] - |     LenL      |
+        # header[3] - |   Checksum    |
+
+        MAGIC = 83
+
+        len_bytes = len(data).to_bytes(2, "big")
+        LEN_H = len_bytes[0] & 127
+        LEN_L = len_bytes[1] & 255
+        CHECKSUM = MAGIC ^ LEN_H ^ LEN_L
+
+        header_ints = [MAGIC, LEN_H, LEN_L, CHECKSUM]
+        header_bytes = struct.pack("{}B".format(len(header_ints)), *header_ints)
+
+        self.__send(header_bytes)
+        self.__send(data)
+
+    def send_reboot(self, use_protocol_splitter_format=False):
        if (not self.__next_baud_flightstack()):
            return False

@@ -693,15 +714,19 @@ class uploader(object):
            print("If the board does not respond, unplug and re-plug the USB connector.", file=sys.stderr)

        try:
+            send_fct = self.__send
+            if use_protocol_splitter_format:
+                send_fct = self.send_protocol_splitter_format
+
            # try MAVLINK command first
            self.port.flush()
-            self.__send(uploader.MAVLINK_REBOOT_ID1)
-            self.__send(uploader.MAVLINK_REBOOT_ID0)
+            send_fct(uploader.MAVLINK_REBOOT_ID1)
+            send_fct(uploader.MAVLINK_REBOOT_ID0)
            # then try reboot via NSH
-            self.__send(uploader.NSH_INIT)
-            self.__send(uploader.NSH_REBOOT_BL)
-            self.__send(uploader.NSH_INIT)
-            self.__send(uploader.NSH_REBOOT)
+            send_fct(uploader.NSH_INIT)
+            send_fct(uploader.NSH_REBOOT_BL)
+            send_fct(uploader.NSH_INIT)
+            send_fct(uploader.NSH_REBOOT)
            self.port.flush()
            self.port.baudrate = self.baudrate_bootloader
        except Exception:
@@ -726,9 +751,13 @@ def main():
    parser.add_argument('--baud-flightstack', action="store", default="57600", help="Comma-separated list of baud rate of the serial port (default is 57600) when communicating with flight stack (Mavlink or NSH), only required for true serial ports.")
    parser.add_argument('--force', action='store_true', default=False, help='Override board type check, or silicon errata checks and continue loading')
    parser.add_argument('--boot-delay', type=int, default=None, help='minimum boot delay to store in flash')
+    parser.add_argument('--use-protocol-splitter-format', action='store_true', help='use protocol splitter format for reboot')
    parser.add_argument('firmware', action="store", help="Firmware file to be uploaded")
    args = parser.parse_args()

+    if args.use_protocol_splitter_format:
+        print("Using protocol splitter format to reboot pixhawk!")
+
    # warn people about ModemManager which interferes badly with Pixhawk
    if os.path.exists("/usr/sbin/ModemManager"):
        print("==========================================================================================================")
@@ -830,7 +859,7 @@ def main():

                    except Exception:

-                        if not up.send_reboot():
+                        if not up.send_reboot(args.use_protocol_splitter_format):
                            break

                        # wait for the reboot, without we might run into Serial I/O Error 5
@@ -1,5 +1,7 @@
 #! /usr/bin/env bash

+set -e
+
 ## Bash script to setup PX4 development environment on Ubuntu LTS (20.04, 18.04, 16.04).
 ## Can also be used in docker.
 ##
@@ -193,7 +195,7 @@ if [[ $INSTALL_SIM == "true" ]]; then
 		java_version=11
 		gazebo_version=9
 	elif [[ "${UBUNTU_RELEASE}" == "20.04" ]]; then
-		java_version=14
+		java_version=13
 		gazebo_version=11
 	else
 		java_version=14
@@ -56,6 +56,13 @@ else
    follow_mode=""
 fi

+# To use gazebo_ros ROS2 plugins
+if [[ -n "$ROS_VERSION" ]] && [ "$ROS_VERSION" == "2" ]; then
+	ros_args="-s libgazebo_ros_init.so -s libgazebo_ros_factory.so"
+else
+	ros_args=""
+fi
+
 if [ "$program" == "jmavsim" ]; then
 	jmavsim_pid=`ps aux | grep java | grep "\-jar jmavsim_run.jar" | awk '{ print $2 }'`
 	if [ -n "$jmavsim_pid" ]; then
@@ -133,7 +140,7 @@ elif [ "$program" == "gazebo" ] && [ ! -n "$no_sim" ]; then
 				world_path="$PX4_SITL_WORLD"
 			fi
 		fi
-		gzserver $verbose $world_path &
+		gzserver $verbose $world_path $ros_args &
 		SIM_PID=$!

 		# Check all paths in ${GAZEBO_MODEL_PATH} for specified model
@@ -179,8 +186,13 @@ elif [ "$program" == "gazebo" ] && [ ! -n "$no_sim" ]; then
 	fi
 elif [ "$program" == "ignition" ] && [ -z "$no_sim" ]; then
 	echo "Ignition Gazebo"
+	if [[ -n "$HEADLESS" ]]; then
+		ignition_headless="-s"
+	else
+		ignition_headless=""
+	fi
 	source "$src_path/Tools/setup_ignition.bash" "${src_path}" "${build_path}"
-	ign gazebo -r "${src_path}/Tools/simulation-ignition/worlds/ignition.world"&
+	ign gazebo ${verbose} ${ignition_headless} -r "${src_path}/Tools/simulation-ignition/worlds/${model}.world"&
 elif [ "$program" == "flightgear" ] && [ -z "$no_sim" ]; then
 	echo "FG setup"
 	cd "${src_path}/Tools/flightgear_bridge/"
@@ -0,0 +1,135 @@
+#!/usr/bin/env bash
+set -e
+
+agent_template_files_updated=0
+code_generator_files_updated=0
+
+# parse help argument
+if [[ $1 == "-h" ]] || [[ $1 == "--help" ]]; then
+  echo -e "Usage: update_px4_ros2_bridge.bash [options...] \t This script allows to update px4_ros_com and px4_msgs in a specified directory." >&2
+  echo
+  echo -e "\t--ws_dir \t\t Location of the ament/colcon workspace. Default: $HOME/colcon_ws."
+  echo -e "\t--px4_ros_com \t\t Updates px4_ros_com microRTPS agent code generation and templates."
+  echo -e "\t--px4_msgs \t\t Updates px4_msgs messages definition files."
+  echo -e "\t--all \t\t Updates both px4_ros_com and px4_msgs."
+  echo
+  exit 0
+fi
+
+# parse the arguments
+while [ $# -gt 0 ]; do
+  if [[ $1 == *"--"* ]]; then
+    v="${1/--/}"
+    if [ ! -z $2 ]; then
+      declare $v="$2"
+    else
+      declare $v=1
+    fi
+  fi
+  shift
+done
+
+# get script directory
+SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )"
+
+# get PX4-Autopilot directory
+PX4_DIR=$(cd "$(dirname "$SCRIPT_DIR")" && pwd)
+
+function compare_and_update () {
+  cmp -s "$1" "$2"
+  if [ $? -eq 1 ]; then
+    cp "$1" "$2"
+    return 0
+  fi
+
+  return 1;
+}
+
+# update microRTPS agent code generators / helpers
+function update_agent_code {
+  declare -a templates=( \
+    "microRTPS_agent.cpp.em" \
+    "microRTPS_timesync.cpp.em" \
+    "microRTPS_timesync.h.em" \
+    "microRTPS_transport.cpp" \
+    "microRTPS_transport.h" \
+    "Publisher.cpp.em" \
+    "Publisher.h.em" \
+    "Subscriber.cpp.em" \
+    "Subscriber.h.em" \
+    "RtpsTopics.cpp.em" \
+    "RtpsTopics.h.em" \
+    )
+
+  for file in ${templates[@]}; do
+    compare_and_update "$PX4_DIR/msg/templates/urtps/$file" "$ws_dir/src/px4_ros_com/templates/$file" \
+      && echo -e "--\t\t- '$ws_dir/src/px4_ros_com/templates/$file' updated" && ((agent_template_files_updated+=1))
+  done
+  if [ $agent_template_files_updated -eq 0 ]; then
+    echo -e "--\t\t- No template files updated"
+  elif [ $agent_template_files_updated -eq 1 ]; then
+    echo -e "--\t\t  - 1 template file updated"
+  else
+    echo -e "--\t\t  - $agent_template_files_updated template files updated"
+  fi
+}
+
+# update microRTPS agent code templates
+function update_agent_templates {
+  declare -a code_generators=( \
+    "uorb_rtps_classifier.py" \
+    "generate_microRTPS_bridge.py" \
+    "px_generate_uorb_topic_files.py" \
+    )
+  for file in ${code_generators[@]}; do
+    compare_and_update "$PX4_DIR/msg/tools/$file $ws_dir/src/px4_ros_com/scripts/$file" \
+      && echo -e "--\t\t- '$ws_dir/src/px4_ros_com/scripts/$file' updated" && ((code_generator_files_updated+=1))
+  done
+  if [ $code_generator_files_updated -eq 0 ]; then
+    echo -e "--\t\t- No code generators / helpers files updated"
+  elif [ $code_generator_files_updated -eq 1 ]; then
+    echo -e "--\t\t  - 1 code generator / helper file updated"
+  else
+    echo -e "--\t\t  - $code_generator_files_updated code generator / helper files updated"
+  fi
+}
+
+# update px4_ros_com files
+function update_px4_ros_com {
+  python3 $PX4_DIR/msg/tools/uorb_to_ros_urtps_topics.py -i $PX4_DIR/msg/tools/urtps_bridge_topics.yaml -o $ws_dir/src/px4_ros_com/templates/urtps_bridge_topics.yaml
+  echo -e "---------------          \033[1mmicroRTPS agent code generation and templates update\033[0m          ----------------"
+  echo "-------------------------------------------------------------------------------------------------------"
+  update_agent_code
+  update_agent_templates
+  return 0
+}
+
+# function to update px4_msgs
+function update_px4_msgs {
+  find "$ws_dir/src/px4_msgs/msg/" -maxdepth 1 -type f -delete
+  python3 $PX4_DIR/msg/tools/uorb_to_ros_msgs.py $PX4_DIR/msg/ $ws_dir/src/px4_msgs/msg/
+}
+
+# decisor
+echo "-------------------------------------------------------------------------------------------------------"
+if [ -d "${ws_dir}" ]; then
+  ws_dir=$(cd "$ws_dir" && pwd)
+  if [ ! -z ${all} ]; then
+    update_px4_ros_com
+    echo "-------------------------------------------------------------------------------------------------------"
+    update_px4_msgs
+  elif [ ! -z ${px4_ros_com} ]; then
+    update_px4_ros_com
+    echo "-------------------------------------------------------------------------------------------------------"
+  elif [ ! -z ${px4_msgs} ]; then
+    update_px4_msgs
+  fi
+  echo -e "--------------------------------          \033[0;32mUpdate successful!\033[0m          ---------------------------------"
+  echo "-------------------------------------------------------------------------------------------------------"
+  exit 0
+else
+  echo -e "-- \033[0;31mWorkspace directory doesn't exist...\033[0m"
+  echo -e "----------------------------------          \033[0;31mUpdate failed!\033[0m          -----------------------------------"
+  echo "-------------------------------------------------------------------------------------------------------"
+  exit $ERRCODE
+fi
@@ -19,6 +19,7 @@
 # CONFIG_NSH_DISABLE_GET is not set
 # CONFIG_NSH_DISABLE_ITEF is not set
 # CONFIG_NSH_DISABLE_LOOPS is not set
+# CONFIG_NSH_DISABLE_MKFATFS is not set
 # CONFIG_NSH_DISABLE_SEMICOLON is not set
 # CONFIG_NSH_DISABLE_TIME is not set
 # CONFIG_STM32_CCMEXCLUDE is not set
@@ -90,7 +91,6 @@ CONFIG_MTD_BYTE_WRITE=y
 CONFIG_MTD_PARTITION=y
 CONFIG_MTD_RAMTRON=y
 CONFIG_NAME_MAX=40
-CONFIG_NFILE_DESCRIPTORS=12
 CONFIG_NSH_ARCHINIT=y
 CONFIG_NSH_ARCHROMFS=y
 CONFIG_NSH_ARGCAT=y
@@ -99,9 +99,6 @@ CONFIG_NSH_CMDPARMS=y
 CONFIG_NSH_CROMFSETC=y
 CONFIG_NSH_DISABLE_IFCONFIG=y
 CONFIG_NSH_DISABLE_IFUPDOWN=y
-CONFIG_NSH_DISABLE_MB=y
-CONFIG_NSH_DISABLE_MH=y
-CONFIG_NSH_DISABLE_MW=y
 CONFIG_NSH_DISABLE_TELNETD=y
 CONFIG_NSH_LINELEN=128
 CONFIG_NSH_MAXARGUMENTS=15
@@ -95,7 +95,6 @@
 /* AUX PWMs
 */
 #define DIRECT_PWM_OUTPUT_CHANNELS	8
-#define DIRECT_INPUT_TIMER_CHANNELS	8

 /* USB OTG FS
 *
@@ -153,7 +152,6 @@
 #define BOARD_ADC_PERIPH_5V_OC  (0)
 #define BOARD_ADC_HIPOWER_5V_OC (0)

-#define BOARD_HAS_PWM	DIRECT_PWM_OUTPUT_CHANNELS

 /* This board provides a DMA pool and APIs */
 #define BOARD_DMA_ALLOC_POOL_SIZE 5120
@@ -36,7 +36,7 @@ CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_STRERROR=y
 CONFIG_LIB_BOARDCTL=y
-CONFIG_MAX_TASKS=0
+CONFIG_FS_PROCFS_MAX_TASKS=0
 CONFIG_MM_REGIONS=2
 CONFIG_NAME_MAX=0
 CONFIG_NUNGET_CHARS=0
@@ -15,6 +15,7 @@
 # CONFIG_NSH_DISABLE_GET is not set
 # CONFIG_NSH_DISABLE_ITEF is not set
 # CONFIG_NSH_DISABLE_LOOPS is not set
+# CONFIG_NSH_DISABLE_MKFATFS is not set
 # CONFIG_NSH_DISABLE_SEMICOLON is not set
 # CONFIG_NSH_DISABLE_TIME is not set
 # CONFIG_STM32_DMACAPABLE is not set
@@ -70,7 +71,6 @@ CONFIG_MTD_BYTE_WRITE=y
 CONFIG_MTD_PARTITION=y
 CONFIG_MTD_RAMTRON=y
 CONFIG_NAME_MAX=40
-CONFIG_NFILE_DESCRIPTORS=12
 CONFIG_NSH_ARCHINIT=y
 CONFIG_NSH_ARCHROMFS=y
 CONFIG_NSH_ARGCAT=y
@@ -79,9 +79,6 @@ CONFIG_NSH_CMDPARMS=y
 CONFIG_NSH_CROMFSETC=y
 CONFIG_NSH_DISABLE_IFCONFIG=y
 CONFIG_NSH_DISABLE_IFUPDOWN=y
-CONFIG_NSH_DISABLE_MB=y
-CONFIG_NSH_DISABLE_MH=y
-CONFIG_NSH_DISABLE_MW=y
 CONFIG_NSH_DISABLE_TELNETD=y
 CONFIG_NSH_LINELEN=128
 CONFIG_NSH_MAXARGUMENTS=15
@@ -22,8 +22,8 @@ px4_add_board(
 		#load_mon
 		#sensors
 	SYSTEMCMDS
-		i2cdetect
-		led_control
+		#i2cdetect
+		#led_control
 		param
 		perf
 		reboot
@@ -36,7 +36,7 @@ CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_STRERROR=y
 CONFIG_LIB_BOARDCTL=y
-CONFIG_MAX_TASKS=0
+CONFIG_FS_PROCFS_MAX_TASKS=0
 CONFIG_MM_REGIONS=2
 CONFIG_NAME_MAX=0
 CONFIG_NUNGET_CHARS=0
@@ -15,6 +15,7 @@
 # CONFIG_NSH_DISABLE_GET is not set
 # CONFIG_NSH_DISABLE_ITEF is not set
 # CONFIG_NSH_DISABLE_LOOPS is not set
+# CONFIG_NSH_DISABLE_MKFATFS is not set
 # CONFIG_NSH_DISABLE_SEMICOLON is not set
 # CONFIG_NSH_DISABLE_TIME is not set
 # CONFIG_STM32_DMACAPABLE is not set
@@ -70,7 +71,6 @@ CONFIG_MTD_BYTE_WRITE=y
 CONFIG_MTD_PARTITION=y
 CONFIG_MTD_RAMTRON=y
 CONFIG_NAME_MAX=40
-CONFIG_NFILE_DESCRIPTORS=12
 CONFIG_NSH_ARCHINIT=y
 CONFIG_NSH_ARCHROMFS=y
 CONFIG_NSH_ARGCAT=y
@@ -79,9 +79,6 @@ CONFIG_NSH_CMDPARMS=y
 CONFIG_NSH_CROMFSETC=y
 CONFIG_NSH_DISABLE_IFCONFIG=y
 CONFIG_NSH_DISABLE_IFUPDOWN=y
-CONFIG_NSH_DISABLE_MB=y
-CONFIG_NSH_DISABLE_MH=y
-CONFIG_NSH_DISABLE_MW=y
 CONFIG_NSH_DISABLE_TELNETD=y
 CONFIG_NSH_LINELEN=128
 CONFIG_NSH_MAXARGUMENTS=15
@@ -7,7 +7,6 @@ px4_add_board(
 	DRIVERS
 		adc/board_adc
 		barometer/maiertek/mpc2520
-		camera_capture
 		gps
 		#heater
 		imu/invensense/icm20602
@@ -20,6 +20,7 @@
 # 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"
@@ -71,6 +72,7 @@ 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
@@ -83,7 +85,6 @@ CONFIG_IDLETHREAD_STACKSIZE=750
 CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_STRERROR=y
-CONFIG_MAX_TASKS=64
 CONFIG_MEMSET_64BIT=y
 CONFIG_MEMSET_OPTSPEED=y
 CONFIG_MMCSD=y
@@ -95,7 +96,6 @@ CONFIG_MTD_BYTE_WRITE=y
 CONFIG_MTD_PARTITION=y
 CONFIG_MTD_RAMTRON=y
 CONFIG_NAME_MAX=40
-CONFIG_NFILE_DESCRIPTORS=12
 CONFIG_NSH_ARCHINIT=y
 CONFIG_NSH_ARCHROMFS=y
 CONFIG_NSH_ARGCAT=y
@@ -104,9 +104,6 @@ CONFIG_NSH_CMDPARMS=y
 CONFIG_NSH_CROMFSETC=y
 CONFIG_NSH_DISABLE_IFCONFIG=y
 CONFIG_NSH_DISABLE_IFUPDOWN=y
-CONFIG_NSH_DISABLE_MB=y
-CONFIG_NSH_DISABLE_MH=y
-CONFIG_NSH_DISABLE_MW=y
 CONFIG_NSH_DISABLE_TELNETD=y
 CONFIG_NSH_LINELEN=128
 CONFIG_NSH_MAXARGUMENTS=15
@@ -91,7 +91,7 @@ CONFIG_IOB_NBUFFERS=24
 CONFIG_IOB_THROTTLE=0
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_STRERROR=y
-CONFIG_MAX_TASKS=64
+CONFIG_FS_PROCFS_MAX_TASKS=64
 CONFIG_MEMSET_64BIT=y
 CONFIG_MEMSET_OPTSPEED=y
 CONFIG_MMCSD=y
@@ -115,7 +115,6 @@ CONFIG_NET_ARP_SEND=y
 CONFIG_NET_BROADCAST=y
 CONFIG_NET_ICMP=y
 CONFIG_NET_ICMP_SOCKET=y
-CONFIG_NET_SOCKOPTS=y
 CONFIG_NET_SOLINGER=y
 CONFIG_NET_TCP=y
 CONFIG_NET_TCPBACKLOG=y
@@ -123,16 +122,12 @@ CONFIG_NET_TCP_WRITE_BUFFERS=y
 CONFIG_NET_UDP=y
 CONFIG_NET_UDP_CHECKSUMS=y
 CONFIG_NET_UDP_WRITE_BUFFERS=y
-CONFIG_NFILE_DESCRIPTORS=12
 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_MB=y
-CONFIG_NSH_DISABLE_MH=y
-CONFIG_NSH_DISABLE_MW=y
 CONFIG_NSH_LINELEN=128
 CONFIG_NSH_MAXARGUMENTS=15
 CONFIG_NSH_NESTDEPTH=8
@@ -143,7 +138,6 @@ CONFIG_NSH_STRERROR=y
 CONFIG_NSH_TELNET=y
 CONFIG_NSH_TELNET_LOGIN=y
 CONFIG_NSH_VARS=y
-CONFIG_OTG_ID_GPIO_DISABLE=y
 CONFIG_PIPES=y
 CONFIG_PREALLOC_TIMERS=50
 CONFIG_PRIORITY_INHERITANCE=y
@@ -101,7 +101,6 @@
 /* PWM
 */
 #define DIRECT_PWM_OUTPUT_CHANNELS  9
-#define DIRECT_INPUT_TIMER_CHANNELS  9

 #define BOARD_CAPTURE_GPIO /* PD14 */  (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN14)

@@ -139,7 +138,6 @@
 #define BOARD_ADC_PERIPH_5V_OC  (!px4_arch_gpioread(GPIO_nVDD_5V_PERIPH_OC))
 #define BOARD_ADC_HIPOWER_5V_OC (!px4_arch_gpioread(GPIO_nVDD_5V_HIPOWER_OC))

-#define BOARD_HAS_PWM	DIRECT_PWM_OUTPUT_CHANNELS

 /* This board provides a DMA pool and APIs */
 #define BOARD_DMA_ALLOC_POOL_SIZE 5120
@@ -16,7 +16,6 @@ px4_add_board(
 		#barometer # all available barometer drivers
 		barometer/bmp280
 		batt_smbus
-		camera_capture
 		camera_trigger
 		differential_pressure # all available differential pressure drivers
 		distance_sensor # all available distance sensor drivers
@@ -18,6 +18,7 @@
 # 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_SPI_CALLBACK is not set
@@ -86,7 +87,6 @@ CONFIG_MTD=y
 CONFIG_MTD_AT24XX=y
 CONFIG_MTD_BYTE_WRITE=y
 CONFIG_MTD_PARTITION=y
-CONFIG_NFILE_DESCRIPTORS=12
 CONFIG_NSH_ARCHINIT=y
 CONFIG_NSH_ARCHROMFS=y
 CONFIG_NSH_ARGCAT=y
@@ -95,9 +95,6 @@ CONFIG_NSH_CMDPARMS=y
 CONFIG_NSH_CROMFSETC=y
 CONFIG_NSH_DISABLE_IFCONFIG=y
 CONFIG_NSH_DISABLE_IFUPDOWN=y
-CONFIG_NSH_DISABLE_MB=y
-CONFIG_NSH_DISABLE_MH=y
-CONFIG_NSH_DISABLE_MW=y
 CONFIG_NSH_DISABLE_TELNETD=y
 CONFIG_NSH_FATDEVNO=0
 CONFIG_NSH_LINELEN=128
@@ -153,7 +153,6 @@
 #define HRT_TIMER		8	/* use timer8 for the HRT */
 #define HRT_TIMER_CHANNEL	1	/* use capture/compare channel */

-#define BOARD_HAS_PWM	DIRECT_PWM_OUTPUT_CHANNELS

 #define BOARD_ENABLE_CONSOLE_BUFFER
 #define BOARD_CONSOLE_BUFFER_SIZE (1024*3)
@@ -17,6 +17,7 @@
 # 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_SPI_CALLBACK is not set
@@ -84,7 +85,6 @@ CONFIG_MTD=y
 CONFIG_MTD_AT24XX=y
 CONFIG_MTD_BYTE_WRITE=y
 CONFIG_MTD_PARTITION=y
-CONFIG_NFILE_DESCRIPTORS=12
 CONFIG_NSH_ARCHINIT=y
 CONFIG_NSH_ARCHROMFS=y
 CONFIG_NSH_ARGCAT=y
@@ -94,9 +94,6 @@ CONFIG_NSH_CROMFSETC=y
 CONFIG_NSH_DISABLE_EXPORT=y
 CONFIG_NSH_DISABLE_IFCONFIG=y
 CONFIG_NSH_DISABLE_IFUPDOWN=y
-CONFIG_NSH_DISABLE_MB=y
-CONFIG_NSH_DISABLE_MH=y
-CONFIG_NSH_DISABLE_MW=y
 CONFIG_NSH_DISABLE_TELNETD=y
 CONFIG_NSH_FATDEVNO=0
 CONFIG_NSH_LINELEN=128
@@ -154,7 +154,6 @@
 #define HRT_TIMER		8	/* use timer8 for the HRT */
 #define HRT_TIMER_CHANNEL	1	/* use capture/compare channel */

-#define BOARD_HAS_PWM	DIRECT_PWM_OUTPUT_CHANNELS

 #define BOARD_ENABLE_CONSOLE_BUFFER
 #define BOARD_CONSOLE_BUFFER_SIZE (1024*3)
@@ -1,35 +0,0 @@
-include (${CMAKE_CURRENT_LIST_DIR}/uavcan_board_identity)
-
-add_definitions(-DUSE_S_RGB_LED_DMA)
-
-px4_add_board(
-	PLATFORM nuttx
-	TOOLCHAIN arm-none-eabi
-	ARCHITECTURE cortex-m4
-	CONSTRAINED_MEMORY
-	ROMFSROOT cannode
-	UAVCAN_INTERFACES 1
-	DRIVERS
-		barometer/ms5611
-		bootloaders
-		gps
-		lights/neopixel
-		magnetometer/rm3100
-		safety_button
-		tone_alarm
-		uavcannode
-	MODULES
-		load_mon
-	SYSTEMCMDS
-		i2cdetect
-		led_control
-		param
-		perf
-		reboot
-		top
-		topic_listener
-		tune_control
-		uorb
-		ver
-		work_queue
-)
@@ -36,7 +36,7 @@ CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_STRERROR=y
 CONFIG_LIB_BOARDCTL=y
-CONFIG_MAX_TASKS=0
+CONFIG_FS_PROCFS_MAX_TASKS=0
 CONFIG_MM_REGIONS=2
 CONFIG_NAME_MAX=0
 CONFIG_NUNGET_CHARS=0
@@ -15,6 +15,7 @@
 # CONFIG_NSH_DISABLE_GET is not set
 # CONFIG_NSH_DISABLE_ITEF is not set
 # CONFIG_NSH_DISABLE_LOOPS is not set
+# CONFIG_NSH_DISABLE_MKFATFS is not set
 # CONFIG_NSH_DISABLE_SEMICOLON is not set
 # CONFIG_NSH_DISABLE_TIME is not set
 # CONFIG_STM32_DMACAPABLE is not set
@@ -70,7 +71,6 @@ CONFIG_MTD_BYTE_WRITE=y
 CONFIG_MTD_PARTITION=y
 CONFIG_MTD_RAMTRON=y
 CONFIG_NAME_MAX=40
-CONFIG_NFILE_DESCRIPTORS=12
 CONFIG_NSH_ARCHINIT=y
 CONFIG_NSH_ARCHROMFS=y
 CONFIG_NSH_ARGCAT=y
@@ -79,9 +79,6 @@ CONFIG_NSH_CMDPARMS=y
 CONFIG_NSH_CROMFSETC=y
 CONFIG_NSH_DISABLE_IFCONFIG=y
 CONFIG_NSH_DISABLE_IFUPDOWN=y
-CONFIG_NSH_DISABLE_MB=y
-CONFIG_NSH_DISABLE_MH=y
-CONFIG_NSH_DISABLE_MW=y
 CONFIG_NSH_DISABLE_TELNETD=y
 CONFIG_NSH_LINELEN=128
 CONFIG_NSH_MAXARGUMENTS=15
@@ -140,7 +137,6 @@ CONFIG_STM32_RTC_MAGIC_REG=1
 CONFIG_STM32_RTC_MAGIC_TIME_SET=0xfacefeef
 CONFIG_STM32_SERIALBRK_BSDCOMPAT=y
 CONFIG_STM32_SERIAL_DISABLE_REORDERING=y
-CONFIG_STM32_JTAG_SW_ENABLE=y
 CONFIG_STM32_SPI1=y
 CONFIG_STM32_TIM8=y
 CONFIG_STM32_USART1=y
@@ -100,7 +100,7 @@ px4_add_board(
 		led_control
 		mft
 		mixer
-		motor_ramp
+		#motor_ramp
 		motor_test
 		mtd
 		nshterm
@@ -108,9 +108,9 @@ px4_add_board(
 		perf
 		pwm
 		reboot
-		reflect
-		sd_bench
-		serial_test
+		#reflect
+		#sd_bench
+		#serial_test
 		system_time
 		top
 		topic_listener
@@ -51,10 +51,9 @@ CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_STRERROR=y
 CONFIG_LIB_BOARDCTL=y
-CONFIG_MAX_TASKS=8
+CONFIG_FS_PROCFS_MAX_TASKS=8
 CONFIG_MEMSET_64BIT=y
 CONFIG_MEMSET_OPTSPEED=y
-CONFIG_NFILE_DESCRIPTORS=5
 CONFIG_PREALLOC_TIMERS=50
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAM_SIZE=245760
@@ -83,7 +82,7 @@ CONFIG_SYSTEM_CDCACM=y
 CONFIG_TASK_NAME_SIZE=24
 CONFIG_TTY_SIGINT=y
 CONFIG_TTY_SIGINT_CHAR=0x03
-CONFIG_TTY_SIGSTP=y
+CONFIG_TTY_SIGTSTP=y
 CONFIG_USART6_RXBUFSIZE=600
 CONFIG_USART6_TXBUFSIZE=300
 CONFIG_USBDEV=y
@@ -19,6 +19,7 @@
 # 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"
@@ -82,7 +83,7 @@ CONFIG_IDLETHREAD_STACKSIZE=750
 CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_STRERROR=y
-CONFIG_MAX_TASKS=64
+CONFIG_FS_PROCFS_MAX_TASKS=64
 CONFIG_MEMSET_64BIT=y
 CONFIG_MEMSET_OPTSPEED=y
 CONFIG_MMCSD=y
@@ -95,7 +96,6 @@ CONFIG_MTD_PARTITION=y
 CONFIG_MTD_PROGMEM=y
 CONFIG_MTD_RAMTRON=y
 CONFIG_NAME_MAX=40
-CONFIG_NFILE_DESCRIPTORS=12
 CONFIG_NSH_ARCHINIT=y
 CONFIG_NSH_ARCHROMFS=y
 CONFIG_NSH_ARGCAT=y
@@ -104,9 +104,6 @@ CONFIG_NSH_CMDPARMS=y
 CONFIG_NSH_CROMFSETC=y
 CONFIG_NSH_DISABLE_IFCONFIG=y
 CONFIG_NSH_DISABLE_IFUPDOWN=y
-CONFIG_NSH_DISABLE_MB=y
-CONFIG_NSH_DISABLE_MH=y
-CONFIG_NSH_DISABLE_MW=y
 CONFIG_NSH_DISABLE_TELNETD=y
 CONFIG_NSH_LINELEN=128
 CONFIG_NSH_MAXARGUMENTS=15
@@ -214,7 +211,7 @@ CONFIG_SYSTEM_CDCACM=y
 CONFIG_SYSTEM_NSH=y
 CONFIG_TASK_NAME_SIZE=24
 CONFIG_TTY_SIGINT=y
-CONFIG_TTY_SIGSTP=y
+CONFIG_TTY_SIGTSTP=y
 CONFIG_UART4_BAUD=57600
 CONFIG_UART4_RXBUFSIZE=600
 CONFIG_UART4_TXBUFSIZE=1500
@@ -199,7 +199,6 @@
 /* This board provides the board_on_reset interface */
 #define BOARD_HAS_ON_RESET 1

-#define BOARD_HAS_PWM  DIRECT_PWM_OUTPUT_CHANNELS

 #define BOARD_ENABLE_CONSOLE_BUFFER

@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020, 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
@@ -65,7 +65,6 @@ __EXPORT int board_app_initialize(uintptr_t arg)

 void board_late_initialize(void)
 {
-	px4_platform_console_init();
 	sercon_main(0, NULL);
 }

@@ -60,6 +60,8 @@
 #include <px4_platform/gpio.h>
 #include <px4_platform/board_dma_alloc.h>

+#include <mpu.h>
+
 __BEGIN_DECLS
 extern void led_init(void);
 extern void led_on(int led);
@@ -127,6 +129,16 @@ __EXPORT void board_on_reset(int status)
 ************************************************************************************/
 __EXPORT void stm32_boardinitialize(void)
 {
+	// clear all existing MPU configuration from bootloader
+	for (int region = 0; region < CONFIG_ARM_MPU_NREGIONS; region++) {
+		putreg32(region, MPU_RNR);
+		putreg32(0, MPU_RBAR);
+		putreg32(0, MPU_RASR);
+
+		// save
+		putreg32(0, MPU_CTRL);
+	}
+
 	/* Reset PWM first thing */
 	board_on_reset(-1);

@@ -101,7 +101,7 @@ px4_add_board(
 		mft
 		microbench
 		mixer
-		motor_ramp
+		#motor_ramp
 		motor_test
 		mtd
 		nshterm
@@ -109,10 +109,10 @@ px4_add_board(
 		perf
 		pwm
 		reboot
-		reflect
+		#reflect
 		sd_bench
 		serial_test
-		system_time
+		#system_time
 		tests # tests and test runner
 		top
 		topic_listener
@@ -101,7 +101,7 @@ px4_add_board(
 		led_control
 		mft
 		mixer
-		motor_ramp
+		#motor_ramp
 		motor_test
 		mtd
 		nshterm
@@ -109,10 +109,10 @@ px4_add_board(
 		perf
 		pwm
 		reboot
-		reflect
+		#reflect
 		sd_bench
-		serial_test
-		system_time
+		#serial_test
+		#system_time
 		top
 		topic_listener
 		tune_control
@@ -15,6 +15,8 @@ param set-default BAT2_A_PER_V 24
 # Enable IMU thermal control
 param set-default SENS_EN_THERMAL 1

+param set-default SENS_TEMP_ID 6946850
+
 # Set Camera trigger pins to 13/14
 param set-default TRIG_PINS_EX 12288

@@ -51,10 +51,9 @@ CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_STRERROR=y
 CONFIG_LIB_BOARDCTL=y
-CONFIG_MAX_TASKS=8
+CONFIG_FS_PROCFS_MAX_TASKS=8
 CONFIG_MEMSET_64BIT=y
 CONFIG_MEMSET_OPTSPEED=y
-CONFIG_NFILE_DESCRIPTORS=5
 CONFIG_PREALLOC_TIMERS=50
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAM_SIZE=245760
@@ -83,7 +82,7 @@ CONFIG_SYSTEM_CDCACM=y
 CONFIG_TASK_NAME_SIZE=24
 CONFIG_TTY_SIGINT=y
 CONFIG_TTY_SIGINT_CHAR=0x03
-CONFIG_TTY_SIGSTP=y
+CONFIG_TTY_SIGTSTP=y
 CONFIG_USART6_RXBUFSIZE=600
 CONFIG_USART6_TXBUFSIZE=300
 CONFIG_USBDEV=y
@@ -19,6 +19,7 @@
 # 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"
@@ -82,7 +83,7 @@ CONFIG_IDLETHREAD_STACKSIZE=750
 CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_STRERROR=y
-CONFIG_MAX_TASKS=64
+CONFIG_FS_PROCFS_MAX_TASKS=64
 CONFIG_MEMSET_64BIT=y
 CONFIG_MEMSET_OPTSPEED=y
 CONFIG_MMCSD=y
@@ -95,7 +96,6 @@ CONFIG_MTD_PARTITION=y
 CONFIG_MTD_PROGMEM=y
 CONFIG_MTD_RAMTRON=y
 CONFIG_NAME_MAX=40
-CONFIG_NFILE_DESCRIPTORS=12
 CONFIG_NSH_ARCHINIT=y
 CONFIG_NSH_ARCHROMFS=y
 CONFIG_NSH_ARGCAT=y
@@ -104,9 +104,6 @@ CONFIG_NSH_CMDPARMS=y
 CONFIG_NSH_CROMFSETC=y
 CONFIG_NSH_DISABLE_IFCONFIG=y
 CONFIG_NSH_DISABLE_IFUPDOWN=y
-CONFIG_NSH_DISABLE_MB=y
-CONFIG_NSH_DISABLE_MH=y
-CONFIG_NSH_DISABLE_MW=y
 CONFIG_NSH_DISABLE_TELNETD=y
 CONFIG_NSH_LINELEN=128
 CONFIG_NSH_MAXARGUMENTS=15
@@ -214,7 +211,7 @@ CONFIG_SYSTEM_CDCACM=y
 CONFIG_SYSTEM_NSH=y
 CONFIG_TASK_NAME_SIZE=24
 CONFIG_TTY_SIGINT=y
-CONFIG_TTY_SIGSTP=y
+CONFIG_TTY_SIGTSTP=y
 CONFIG_UART4_BAUD=57600
 CONFIG_UART4_RXBUFSIZE=600
 CONFIG_UART4_TXBUFSIZE=1500
@@ -199,7 +199,6 @@
 /* This board provides the board_on_reset interface */
 #define BOARD_HAS_ON_RESET 1

-#define BOARD_HAS_PWM  DIRECT_PWM_OUTPUT_CHANNELS

 #define BOARD_ENABLE_CONSOLE_BUFFER

@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020, 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
@@ -65,7 +65,6 @@ __EXPORT int board_app_initialize(uintptr_t arg)

 void board_late_initialize(void)
 {
-	px4_platform_console_init();
 	sercon_main(0, NULL);
 }

@@ -60,6 +60,8 @@
 #include <px4_platform/gpio.h>
 #include <px4_platform/board_dma_alloc.h>

+#include <mpu.h>
+
 __BEGIN_DECLS
 extern void led_init(void);
 extern void led_on(int led);
@@ -127,6 +129,16 @@ __EXPORT void board_on_reset(int status)
 ************************************************************************************/
 __EXPORT void stm32_boardinitialize(void)
 {
+	// clear all existing MPU configuration from bootloader
+	for (int region = 0; region < CONFIG_ARM_MPU_NREGIONS; region++) {
+		putreg32(region, MPU_RNR);
+		putreg32(0, MPU_RBAR);
+		putreg32(0, MPU_RASR);
+
+		// save
+		putreg32(0, MPU_CTRL);
+	}
+
 	/* Reset PWM first thing */
 	board_on_reset(-1);

@@ -102,7 +102,7 @@ px4_add_board(
 		mft
 		microbench
 		mixer
-		motor_ramp
+		#motor_ramp
 		motor_test
 		mtd
 		nshterm
@@ -110,10 +110,10 @@ px4_add_board(
 		perf
 		pwm
 		reboot
-		reflect
+		#reflect
 		sd_bench
 		serial_test
-		system_time
+		#system_time
 		tests # tests and test runner
 		top
 		topic_listener
@@ -12,7 +12,7 @@ px4_add_board(
 		TEL2:/dev/ttyS1
 		GPS1:/dev/ttyS2
 		# PX4IO:/dev/ttyS3
-		TEL3:/dev/ttyS4
+		# TEL3:/dev/ttyS4  # connected to ADS-B receiver
 		GPS2:/dev/ttyS5
 	DRIVERS
 		adc/ads1115
@@ -12,4 +12,6 @@ param set-default BAT2_A_PER_V 17
 # Disable IMU thermal control
 param set-default SENS_EN_THERMAL 0

+param set-default -s SENS_TEMP_ID 2621474
+
 set IOFW "/etc/extras/cubepilot_io-v2_default.bin"
@@ -5,3 +5,9 @@

 # Start MAVLink on the USB port
 mavlink start -d /dev/ttyACM0
+
+# Start ADS-B receiver mavlink connection if console not present
+if [ ! -e /dev/console ]
+then
+	mavlink start -d /dev/ttyS4 -b 57600 -m minimal
+fi
@@ -51,10 +51,9 @@ CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_STRERROR=y
 CONFIG_LIB_BOARDCTL=y
-CONFIG_MAX_TASKS=8
+CONFIG_FS_PROCFS_MAX_TASKS=8
 CONFIG_MEMSET_64BIT=y
 CONFIG_MEMSET_OPTSPEED=y
-CONFIG_NFILE_DESCRIPTORS=5
 CONFIG_PREALLOC_TIMERS=50
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_RAM_SIZE=245760
@@ -83,7 +82,7 @@ CONFIG_SYSTEM_CDCACM=y
 CONFIG_TASK_NAME_SIZE=24
 CONFIG_TTY_SIGINT=y
 CONFIG_TTY_SIGINT_CHAR=0x03
-CONFIG_TTY_SIGSTP=y
+CONFIG_TTY_SIGTSTP=y
 CONFIG_USART3_RXBUFSIZE=600
 CONFIG_USART3_TXBUFSIZE=300
 CONFIG_USBDEV=y
@@ -20,6 +20,7 @@
 # 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"
@@ -71,6 +72,7 @@ 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
@@ -83,7 +85,6 @@ CONFIG_IDLETHREAD_STACKSIZE=750
 CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_STRERROR=y
-CONFIG_MAX_TASKS=64
 CONFIG_MEMSET_64BIT=y
 CONFIG_MEMSET_OPTSPEED=y
 CONFIG_MMCSD=y
@@ -96,7 +97,6 @@ CONFIG_MTD_PARTITION=y
 CONFIG_MTD_PROGMEM=y
 CONFIG_MTD_RAMTRON=y
 CONFIG_NAME_MAX=40
-CONFIG_NFILE_DESCRIPTORS=12
 CONFIG_NSH_ARCHINIT=y
 CONFIG_NSH_ARCHROMFS=y
 CONFIG_NSH_ARGCAT=y
@@ -105,9 +105,6 @@ CONFIG_NSH_CMDPARMS=y
 CONFIG_NSH_CROMFSETC=y
 CONFIG_NSH_DISABLE_IFCONFIG=y
 CONFIG_NSH_DISABLE_IFUPDOWN=y
-CONFIG_NSH_DISABLE_MB=y
-CONFIG_NSH_DISABLE_MH=y
-CONFIG_NSH_DISABLE_MW=y
 CONFIG_NSH_DISABLE_TELNETD=y
 CONFIG_NSH_LINELEN=128
 CONFIG_NSH_MAXARGUMENTS=15
@@ -205,7 +202,7 @@ CONFIG_SYSTEM_NSH=y
 CONFIG_TASK_NAME_SIZE=24
 CONFIG_TTY_SIGINT=y
 CONFIG_TTY_SIGINT_CHAR=0x03
-CONFIG_TTY_SIGSTP=y
+CONFIG_TTY_SIGTSTP=y
 CONFIG_UART4_BAUD=57600
 CONFIG_UART4_RXBUFSIZE=600
 CONFIG_UART4_TXBUFSIZE=1500
@@ -19,6 +19,7 @@
 # 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"
@@ -70,6 +71,7 @@ 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
@@ -82,7 +84,6 @@ CONFIG_IDLETHREAD_STACKSIZE=750
 CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_STRERROR=y
-CONFIG_MAX_TASKS=64
 CONFIG_MEMSET_64BIT=y
 CONFIG_MEMSET_OPTSPEED=y
 CONFIG_MMCSD=y
@@ -95,7 +96,6 @@ CONFIG_MTD_PARTITION=y
 CONFIG_MTD_PROGMEM=y
 CONFIG_MTD_RAMTRON=y
 CONFIG_NAME_MAX=40
-CONFIG_NFILE_DESCRIPTORS=12
 CONFIG_NSH_ARCHINIT=y
 CONFIG_NSH_ARCHROMFS=y
 CONFIG_NSH_ARGCAT=y
@@ -104,9 +104,6 @@ CONFIG_NSH_CMDPARMS=y
 CONFIG_NSH_CROMFSETC=y
 CONFIG_NSH_DISABLE_IFCONFIG=y
 CONFIG_NSH_DISABLE_IFUPDOWN=y
-CONFIG_NSH_DISABLE_MB=y
-CONFIG_NSH_DISABLE_MH=y
-CONFIG_NSH_DISABLE_MW=y
 CONFIG_NSH_DISABLE_TELNETD=y
 CONFIG_NSH_LINELEN=128
 CONFIG_NSH_MAXARGUMENTS=15
@@ -203,7 +200,7 @@ CONFIG_SYSTEM_CDCACM=y
 CONFIG_SYSTEM_NSH=y
 CONFIG_TASK_NAME_SIZE=24
 CONFIG_TTY_SIGINT=y
-CONFIG_TTY_SIGSTP=y
+CONFIG_TTY_SIGTSTP=y
 CONFIG_UART4_BAUD=57600
 CONFIG_UART4_RXBUFSIZE=600
 CONFIG_UART4_TXBUFSIZE=1500
@@ -62,7 +62,7 @@
 #define GPIO_nLED_AMBER        /* PE12 */  (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN12)

 #define BOARD_HAS_CONTROL_STATUS_LEDS      1
-#define BOARD_ARMED_STATE_LED  LED_AMBER
+#define BOARD_ARMED_LED  LED_AMBER

 /* ADC channels */
 #define PX4_ADC_GPIO  \
@@ -148,7 +148,6 @@

 #define BOARD_HAS_STATIC_MANIFEST 1

-#define BOARD_HAS_PWM  DIRECT_PWM_OUTPUT_CHANNELS

 #define BOARD_DSHOT_MOTOR_ASSIGNMENT {3, 2, 1, 0, 4, 5};

@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020, 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
@@ -65,7 +65,6 @@ __EXPORT int board_app_initialize(uintptr_t arg)

 void board_late_initialize(void)
 {
-	px4_platform_console_init();
 	sercon_main(0, NULL);
 }

@@ -60,6 +60,8 @@
 #include <px4_platform/gpio.h>
 #include <px4_platform/board_dma_alloc.h>

+#include <mpu.h>
+
 __BEGIN_DECLS
 extern void led_init(void);
 extern void led_on(int led);
@@ -118,6 +120,16 @@ __EXPORT void board_on_reset(int status)
 ************************************************************************************/
 __EXPORT void stm32_boardinitialize(void)
 {
+	// clear all existing MPU configuration from bootloader
+	for (int region = 0; region < CONFIG_ARM_MPU_NREGIONS; region++) {
+		putreg32(region, MPU_RNR);
+		putreg32(0, MPU_RBAR);
+		putreg32(0, MPU_RASR);
+
+		// save
+		putreg32(0, MPU_CTRL);
+	}
+
 	/* Reset PWM first thing */
 	board_on_reset(-1);

@@ -44,9 +44,9 @@ px4_add_board(
 		pwm_out_sim
 		pwm_out
 		px4io
-		roboclaw
+		#roboclaw
 		rpm
-		smart_battery/batmon
+		#smart_battery/batmon
 		telemetry # all available telemetry drivers
 		test_ppm
 		tone_alarm
@@ -99,7 +99,7 @@ px4_add_board(
 		mft
 		microbench
 		mixer
-		motor_ramp
+		#motor_ramp
 		motor_test
 		mtd
 		nshterm
@@ -107,10 +107,10 @@ px4_add_board(
 		perf
 		pwm
 		reboot
-		reflect
+		#reflect
 		sd_bench
 		#serial_test
-		system_time
+		#system_time
 		tests # tests and test runner
 		top
 		topic_listener
--- a/Show More
+++ b/Show More