diff --git a/.ci/Jenkinsfile-hardware b/.ci/Jenkinsfile-hardware
index 5bd2a6effa9..a7836d2f644 100644
--- a/.ci/Jenkinsfile-hardware
+++ b/.ci/Jenkinsfile-hardware
@@ -20,10 +20,10 @@ pipeline {
             sh 'make distclean'
             sh 'ccache -z'
             sh 'git fetch --tags'
-            sh 'make px4_fmu-v2_default'
+            sh 'make px4_fmu-v2_test'
             sh 'make sizes'
             sh 'ccache -s'
-            stash includes: 'build/px4_fmu-v2_default/px4_fmu-v2_default.elf', name: 'px4_fmu-v2_test'
+            stash includes: 'build/px4_fmu-v2_test/px4_fmu-v2_test.elf', name: 'px4_fmu-v2_test'
           }
           post {
             always {
@@ -212,19 +212,30 @@ pipeline {
             label 'px4_fmu-v3'
           }
           steps {
-            script {
-              try {
-                sh 'export'
-                sh 'find /dev/serial'
-                unstash 'px4_fmu-v3_default'
-                sh 'platforms/nuttx/Debug/jlink_gdb_upload.sh build/px4_fmu-v3_default/px4_fmu-v3_default.elf'
-                sh './Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-FTDI_*` --baudrate 57600'
-                sh './Tools/HIL/run_tests.py --device `find /dev/serial -name *usb-FTDI_*`'
-              } catch (Exception err) {
-                  // always report passed for now
-                  currentBuild.result = 'SUCCESS'
-              }
-            } // script
+            sh 'export'
+            sh 'find /dev/serial'
+            unstash 'px4_fmu-v3_default'
+            // flash board and watch bootup
+            sh './platforms/nuttx/Debug/jlink_gdb_upload.sh build/px4_fmu-v3_default/px4_fmu-v3_default.elf'
+            sh './Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-FTDI_*` --baudrate 57600'
+            // commands
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "dataman status"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "free"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ls /dev"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ls /obj"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "mavlink status"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "mtd status"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "param show"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "param status"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "perf latency"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "perf"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ps"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "sensors status"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "top once"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "uorb status"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ver all"'
+            // run tests
+            sh './Tools/HIL/run_tests.py --device `find /dev/serial -name *usb-FTDI_*`'
           }
           options {
             timeout(time: 600, unit: 'SECONDS')
@@ -236,19 +247,30 @@ pipeline {
             label 'px4_fmu-v4'
           }
           steps {
-            script {
-              try {
-                sh 'export'
-                sh 'find /dev/serial'
-                unstash 'px4_fmu-v4_default'
-                sh 'platforms/nuttx/Debug/jlink_gdb_upload.sh build/px4_fmu-v4_default/px4_fmu-v4_default.elf'
-                sh './Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-FTDI_*` --baudrate 57600'
-                sh './Tools/HIL/run_tests.py --device `find /dev/serial -name *usb-FTDI_*`'
-              } catch (Exception err) {
-                  // always report passed for now
-                  currentBuild.result = 'SUCCESS'
-              }
-            } // script
+            sh 'export'
+            sh 'find /dev/serial'
+            unstash 'px4_fmu-v4_default'
+            // flash board and watch bootup
+            sh 'platforms/nuttx/Debug/jlink_gdb_upload.sh build/px4_fmu-v4_default/px4_fmu-v4_default.elf'
+            sh './Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-FTDI_*` --baudrate 57600'
+            // commands
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "dataman status"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "free"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ls /dev"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ls /obj"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "mavlink status"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "mtd status"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "param show"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "param status"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "perf latency"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "perf"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ps"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "sensors status"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "top once"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "uorb status"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ver all"'
+            // run tests
+            sh './Tools/HIL/run_tests.py --device `find /dev/serial -name *usb-FTDI_*`'
           }
           options {
             timeout(time: 600, unit: 'SECONDS')
@@ -260,19 +282,30 @@ pipeline {
             label 'px4_fmu-v4pro'
           }
           steps {
-            script {
-              try {
-                sh 'export'
-                sh 'find /dev/serial'
-                unstash 'px4_fmu-v4pro_default'
-                sh 'platforms/nuttx/Debug/jlink_gdb_upload.sh build/px4_fmu-v4pro_default/px4_fmu-v4pro_default.elf'
-                sh './Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-FTDI_*` --baudrate 57600'
-                sh './Tools/HIL/run_tests.py --device `find /dev/serial -name *usb-FTDI_*`'
-              } catch (Exception err) {
-                  // always report passed for now
-                  currentBuild.result = 'SUCCESS'
-              }
-            } // script
+            sh 'export'
+            sh 'find /dev/serial'
+            unstash 'px4_fmu-v4pro_default'
+            // flash board and watch bootup
+            sh 'platforms/nuttx/Debug/jlink_gdb_upload.sh build/px4_fmu-v4pro_default/px4_fmu-v4pro_default.elf'
+            sh './Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-FTDI_*` --baudrate 57600'
+            // commands
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "dataman status"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "free"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ls /dev"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ls /obj"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "mavlink status"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "mtd status"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "param show"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "param status"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "perf latency"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "perf"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ps"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "sensors status"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "top once"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "uorb status"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ver all"'
+            // run tests
+            sh './Tools/HIL/run_tests.py --device `find /dev/serial -name *usb-FTDI_*`'
           }
           options {
             timeout(time: 600, unit: 'SECONDS')
@@ -284,19 +317,30 @@ pipeline {
             label 'px4_fmu-v5'
           }
           steps {
-            script {
-              try {
-                sh 'export'
-                sh 'find /dev/serial'
-                unstash 'px4_fmu-v5_default'
-                sh './platforms/nuttx/Debug/jlink_gdb_upload.sh  build/px4_fmu-v5_default/px4_fmu-v5_default.elf'
-                sh './Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-FTDI_*` --baudrate 57600'
-                sh './Tools/HIL/run_tests.py --device `find /dev/serial -name *usb-FTDI_*`'
-              } catch (Exception err) {
-                  // always report passed for now
-                  currentBuild.result = 'SUCCESS'
-              }
-            } // script
+            sh 'export'
+            sh 'find /dev/serial'
+            unstash 'px4_fmu-v5_default'
+            // flash board and watch bootup
+            sh './platforms/nuttx/Debug/jlink_gdb_upload.sh  build/px4_fmu-v5_default/px4_fmu-v5_default.elf'
+            sh './Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-FTDI_*` --baudrate 57600'
+            // commands
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "dataman status"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "free"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ls /dev"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ls /obj"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "mavlink status"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "mtd status"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "param show"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "param status"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "perf latency"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "perf"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ps"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "sensors status"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "top once"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "uorb status"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ver all"'
+            // run tests
+            sh './Tools/HIL/run_tests.py --device `find /dev/serial -name *usb-FTDI_*`'
           }
           options {
             timeout(time: 600, unit: 'SECONDS')
@@ -308,19 +352,30 @@ pipeline {
             label 'pixhawk_4'
           }
           steps {
-            script {
-              try {
-                sh 'export'
-                sh 'find /dev/serial'
-                unstash 'px4_fmu-v5_default'
-                sh './platforms/nuttx/Debug/jlink_gdb_upload.sh  build/px4_fmu-v5_default/px4_fmu-v5_default.elf'
-                sh './Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-FTDI_*` --baudrate 57600'
-                sh './Tools/HIL/run_tests.py --device `find /dev/serial -name *usb-FTDI_*`'
-              } catch (Exception err) {
-                  // always report passed for now
-                  currentBuild.result = 'SUCCESS'
-              }
-            } // script
+            sh 'export'
+            sh 'find /dev/serial'
+            unstash 'px4_fmu-v5_default'
+            // flash board and watch bootup
+            sh './platforms/nuttx/Debug/jlink_gdb_upload.sh  build/px4_fmu-v5_default/px4_fmu-v5_default.elf'
+            sh './Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-FTDI_*` --baudrate 57600'
+            // commands
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "dataman status"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "free"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ls /dev"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ls /obj"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "mavlink status"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "mtd status"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "param show"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "param status"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "perf latency"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "perf"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ps"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "sensors status"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "top once"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "uorb status"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ver all"'
+            // run tests
+            sh './Tools/HIL/run_tests.py --device `find /dev/serial -name *usb-FTDI_*`'
           }
           options {
             timeout(time: 600, unit: 'SECONDS')
@@ -332,19 +387,30 @@ pipeline {
             label 'pixhawk_4_mini'
           }
           steps {
-            script {
-              try {
-                sh 'export'
-                sh 'find /dev/serial'
-                unstash 'px4_fmu-v5_default'
-                sh './platforms/nuttx/Debug/jlink_gdb_upload.sh  build/px4_fmu-v5_default/px4_fmu-v5_default.elf'
-                sh './Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-FTDI_*` --baudrate 57600'
-                sh './Tools/HIL/run_tests.py --device `find /dev/serial -name *usb-FTDI_*`'
-              } catch (Exception err) {
-                  // always report passed for now
-                  currentBuild.result = 'SUCCESS'
-              }
-            } // script
+            sh 'export'
+            sh 'find /dev/serial'
+            unstash 'px4_fmu-v5_default'
+            // flash board and watch bootup
+            sh './platforms/nuttx/Debug/jlink_gdb_upload.sh  build/px4_fmu-v5_default/px4_fmu-v5_default.elf'
+            sh './Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-FTDI_*` --baudrate 57600'
+            // commands
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "dataman status"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "free"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ls /dev"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ls /obj"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "mavlink status"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "mtd status"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "param show"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "param status"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "perf latency"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "perf"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ps"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "sensors status"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "top once"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "uorb status"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ver all"'
+            // run tests
+            sh './Tools/HIL/run_tests.py --device `find /dev/serial -name *usb-FTDI_*`'
           }
           options {
             timeout(time: 600, unit: 'SECONDS')
@@ -356,19 +422,30 @@ pipeline {
             label 'cuav_v5_nano'
           }
           steps {
-            script {
-              try {
-                sh 'export'
-                sh 'find /dev/serial'
-                unstash 'px4_fmu-v5_default'
-                sh './platforms/nuttx/Debug/jlink_gdb_upload.sh  build/px4_fmu-v5_default/px4_fmu-v5_default.elf'
-                sh './Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-FTDI_*` --baudrate 57600'
-                sh './Tools/HIL/run_tests.py --device `find /dev/serial -name *usb-FTDI_*`'
-              } catch (Exception err) {
-                  // always report passed for now
-                  currentBuild.result = 'SUCCESS'
-              }
-            } // script
+            sh 'export'
+            sh 'find /dev/serial'
+            unstash 'px4_fmu-v5_default'
+            // flash board and watch bootup
+            sh './platforms/nuttx/Debug/jlink_gdb_upload.sh  build/px4_fmu-v5_default/px4_fmu-v5_default.elf'
+            sh './Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-FTDI_*` --baudrate 57600'
+            // commands
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "dataman status"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "free"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ls /dev"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ls /obj"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "mavlink status"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "mtd status"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "param show"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "param status"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "perf latency"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "perf"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ps"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "sensors status"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "top once"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "uorb status"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ver all"'
+            // run tests
+            sh './Tools/HIL/run_tests.py --device `find /dev/serial -name *usb-FTDI_*`'
           }
           options {
             timeout(time: 600, unit: 'SECONDS')
@@ -380,19 +457,30 @@ pipeline {
             label 'cuav_v5_plus'
           }
           steps {
-            script {
-              try {
-                sh 'export'
-                sh 'find /dev/serial'
-                unstash 'px4_fmu-v5_default'
-                sh './platforms/nuttx/Debug/jlink_gdb_upload.sh  build/px4_fmu-v5_default/px4_fmu-v5_default.elf'
-                sh './Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-FTDI_*` --baudrate 57600'
-                sh './Tools/HIL/run_tests.py --device `find /dev/serial -name *usb-FTDI_*`'
-              } catch (Exception err) {
-                  // always report passed for now
-                  currentBuild.result = 'SUCCESS'
-              }
-            } // script
+            sh 'export'
+            sh 'find /dev/serial'
+            unstash 'px4_fmu-v5_default'
+            // flash board and watch bootup
+            sh './platforms/nuttx/Debug/jlink_gdb_upload.sh  build/px4_fmu-v5_default/px4_fmu-v5_default.elf'
+            sh './Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-FTDI_*` --baudrate 57600'
+            // commands
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "dataman status"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "free"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ls /dev"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ls /obj"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "mavlink status"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "mtd status"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "param show"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "param status"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "perf latency"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "perf"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ps"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "sensors status"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "top once"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "uorb status"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ver all"'
+            // run tests
+            sh './Tools/HIL/run_tests.py --device `find /dev/serial -name *usb-FTDI_*`'
           }
           options {
             timeout(time: 600, unit: 'SECONDS')
@@ -428,19 +516,30 @@ pipeline {
             label 'nxp_fmuk66-v3'
           }
           steps {
-            script {
-              try {
-                sh 'export'
-                sh 'find /dev/serial'
-                unstash 'nxp_fmuk66-v3_default'
-                sh './platforms/nuttx/Debug/jlink_gdb_upload.sh build/nxp_fmuk66-v3_default/nxp_fmuk66-v3_default.elf'
-                sh './Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-FTDI*` --baudrate 57600'
-                sh './Tools/HIL/run_tests.py --device `find /dev/serial -name *usb-FTDI_*`'
-              } catch (Exception err) {
-                  // always report passed for now
-                  currentBuild.result = 'SUCCESS'
-              }
-            } // script
+            sh 'export'
+            sh 'find /dev/serial'
+            unstash 'nxp_fmuk66-v3_default'
+            // flash board and watch bootup
+            sh './platforms/nuttx/Debug/jlink_gdb_upload.sh build/nxp_fmuk66-v3_default/nxp_fmuk66-v3_default.elf'
+            sh './Tools/HIL/monitor_firmware_upload.py --device `find /dev/serial -name *usb-FTDI_*` --baudrate 57600'
+            // commands
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "dataman status"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "free"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ls /dev"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ls /obj"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "mavlink status"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "mtd status"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "param show"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "param status"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "perf latency"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "perf"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ps"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "sensors status"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "top once"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "uorb status"'
+            sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ver all"'
+            // run tests
+            sh './Tools/HIL/run_tests.py --device `find /dev/serial -name *usb-FTDI_*`'
           }
           options {
             timeout(time: 600, unit: 'SECONDS')
diff --git a/Tools/HIL/monitor_firmware_upload.py b/Tools/HIL/monitor_firmware_upload.py
index ed4e78694a6..080f0679e8e 100755
--- a/Tools/HIL/monitor_firmware_upload.py
+++ b/Tools/HIL/monitor_firmware_upload.py
@@ -6,21 +6,36 @@ from subprocess import call, Popen
 from argparse import ArgumentParser
 import re
 
-def monitor_firmware_upload(port, baudrate):     
+def monitor_firmware_upload(port, baudrate):
     databits = serial.EIGHTBITS
     stopbits = serial.STOPBITS_ONE
     parity = serial.PARITY_NONE
-    ser = serial.Serial(port, baudrate, databits, parity, stopbits, 100)
+    ser = serial.Serial(port, baudrate, databits, parity, stopbits, timeout=10)
 
     finished = 0
 
+    timeout = 30  # 30 seconds
+    timeout_start = time.time()
+    timeout_newline = time.time()
+
     while finished == 0:
         serial_line = ser.readline()
-        print(serial_line.replace('\n','')) 
+        print(serial_line.replace('\n',''))
 
         if "NuttShell (NSH)" in serial_line:
             finished = 1
-        time.sleep(0.05)
+            break
+
+        if time.time() > timeout_start + timeout:
+            print("Error, timeout")
+            finished = 1
+            break
+
+        # newline every 30 seconds if still running
+        if time.time() - timeout_newline > 30:
+            ser.write('\n')
+            timeout_newline = time.time()
+
     ser.close()
 
 def main():
diff --git a/Tools/HIL/run_nsh_cmd.py b/Tools/HIL/run_nsh_cmd.py
new file mode 100755
index 00000000000..47f88da7e74
--- /dev/null
+++ b/Tools/HIL/run_nsh_cmd.py
@@ -0,0 +1,77 @@
+#! /usr/bin/python
+
+import serial, time
+import subprocess
+from subprocess import call, Popen
+from argparse import ArgumentParser
+import re
+
+def do_nsh_cmd(port, baudrate, cmd):
+    databits = serial.EIGHTBITS
+    stopbits = serial.STOPBITS_ONE
+    parity = serial.PARITY_NONE
+    ser = serial.Serial(port, baudrate, databits, parity, stopbits, timeout=10)
+
+    ser.write('\n\n')
+
+    finished = 0
+    success = False
+
+    timeout = 10  # 10 seconds
+    timeout_start = time.time()
+
+    while finished == 0:
+        serial_line = ser.readline()
+        print(serial_line.replace('\n',''))
+
+        if "nsh>" in serial_line:
+            finished = 1
+
+        if time.time() > timeout_start + timeout:
+            print("Error, timeout")
+            finished = 1
+            break
+
+    # run command
+    ser.write(cmd + '\n')
+    time.sleep(0.05)
+    ser.write('\n')
+
+    finished = 0
+    timeout = 30  # 30 seconds
+    timeout_start = time.time()
+    timeout_newline = time.time()
+
+    while finished == 0:
+        serial_line = ser.readline()
+        print(serial_line.replace('\n',''))
+
+        if cmd in serial_line:
+            continue
+        elif "nsh>" in serial_line:
+            finished = 1
+            break
+
+        if time.time() > timeout_start + timeout:
+            print("Error, timeout")
+            finished = 1
+            break
+
+        # newline every 10 seconds if still running
+        if time.time() - timeout_newline > 10:
+            ser.write('\n')
+            timeout_newline = time.time()
+
+    ser.close()
+
+def main():
+    parser = ArgumentParser(description=__doc__)
+    parser.add_argument('--device', "-d", nargs='?', default = None, help='')
+    parser.add_argument("--baudrate", "-b", dest="baudrate", type=int, help="Mavlink port baud rate (default=57600)", default=57600)
+    parser.add_argument("--cmd", "-c", dest="cmd", help="Command to run")
+    args = parser.parse_args()
+
+    do_nsh_cmd(args.device, args.baudrate, args.cmd)
+
+if __name__ == "__main__":
+   main()
diff --git a/Tools/HIL/run_tests.py b/Tools/HIL/run_tests.py
index bca87cd5770..f02c67c0ad4 100755
--- a/Tools/HIL/run_tests.py
+++ b/Tools/HIL/run_tests.py
@@ -10,22 +10,38 @@ def do_test(port, baudrate, test_name):
     databits = serial.EIGHTBITS
     stopbits = serial.STOPBITS_ONE
     parity = serial.PARITY_NONE
-    ser = serial.Serial(port, baudrate, databits, parity, stopbits, 100)
+    ser = serial.Serial(port, baudrate, databits, parity, stopbits, timeout=10)
+
     ser.write('\n\n')
 
     finished = 0
+    success = False
+
+    timeout = 10  # 10 seconds
+    timeout_start = time.time()
+
     while finished == 0:
         serial_line = ser.readline()
         print(serial_line.replace('\n',''))
 
         if "nsh>" in serial_line:
             finished = 1
-        time.sleep(0.05)
 
+        if time.time() > timeout_start + timeout:
+            print("Error, timeout")
+            finished = 1
+            break
+
+
+    # run test
     ser.write('tests ' + test_name + '\n')
+    time.sleep(0.05)
 
     finished = 0
-    success = False
+    timeout = 300  # 5 minutes
+    timeout_start = time.time()
+    timeout_newline = time.time()
+
     while finished == 0:
         serial_line = ser.readline()
         print(serial_line.replace('\n',''))
@@ -37,7 +53,17 @@ def do_test(port, baudrate, test_name):
             finished = 1
             success = False
 
-        time.sleep(0.05)
+        if time.time() > timeout_start + timeout:
+            print("Error, timeout")
+            print(test_name + " FAILED")
+            finished = 1
+            success = False
+            break
+
+        # newline every 30 seconds if still running
+        if time.time() - timeout_newline > 30:
+            ser.write('\n')
+            timeout_newline = time.time()
 
     ser.close()