diff --git a/.ci/Jenkinsfile-hardware b/.ci/Jenkinsfile-hardware
index 56465f64c2..8fefb6c766 100644
--- a/.ci/Jenkinsfile-hardware
+++ b/.ci/Jenkinsfile-hardware
@@ -702,7 +702,7 @@ 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 SYS*"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "reboot" || true' // reboot to apply
+  sh './Tools/HIL/reboot.py --device `find /dev/serial -name *usb-*`' // reboot to apply
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param show SYS*"'
 
   // run logger
@@ -762,7 +762,7 @@ void runTests() {
   sh './Tools/HIL/nsh_param_set.py --device `find /dev/serial -name *usb-*` --name "IMU_GYRO_FFT_EN" --value "0" || true' // disable during testing
 
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param save"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "reboot" || true' // reboot to apply
+  sh './Tools/HIL/reboot.py --device `find /dev/serial -name *usb-*`' // reboot to apply
 
   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"'
@@ -795,13 +795,13 @@ void runTests() {
   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/reboot.py --device `find /dev/serial -name *usb-*`'
   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/reboot.py --device `find /dev/serial -name *usb-*`'
   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/reboot.py --device `find /dev/serial -name *usb-*`'
   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/reboot.py --device `find /dev/serial -name *usb-*`'
   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
 }
 
@@ -909,7 +909,7 @@ void resetBoard() {
   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
+  sh './Tools/HIL/reboot.py --device `find /dev/serial -name *usb-*`' // reboot to apply
 
   // check SD card
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "df -h" || true'
diff --git a/Tools/HIL/reboot.py b/Tools/HIL/reboot.py
new file mode 100755
index 0000000000..670854207f
--- /dev/null
+++ b/Tools/HIL/reboot.py
@@ -0,0 +1,88 @@
+#! /usr/bin/env python3
+
+import serial, time
+import subprocess
+from subprocess import call, Popen
+from argparse import ArgumentParser
+import re
+import sys
+import datetime
+
+COLOR_RED    = "\x1b[31m"
+COLOR_GREEN  = "\x1b[32m"
+COLOR_YELLOW = "\x1b[33m"
+COLOR_WHITE  = "\x1b[37m"
+COLOR_RESET  = "\x1b[0m"
+
+def print_line(line):
+    if "WARNING" in line:
+        line = line.replace("WARNING", f"{COLOR_YELLOW}WARNING{COLOR_RESET}", 1)
+    elif "WARN" in line:
+        line = line.replace("WARN", f"{COLOR_YELLOW}WARN{COLOR_RESET}", 1)
+    elif "ERROR" in line:
+        line = line.replace("ERROR", f"{COLOR_RED}ERROR{COLOR_RESET}", 1)
+    elif "INFO" in line:
+        line = line.replace("INFO", f"{COLOR_WHITE}INFO{COLOR_RESET}", 1)
+
+    if "PASSED" in line:
+        line = line.replace("PASSED", f"{COLOR_GREEN}PASSED{COLOR_RESET}", 1)
+
+    if "FAILED" in line:
+        line = line.replace("FAILED", f"{COLOR_RED}FAILED{COLOR_RESET}", 1)
+
+    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)
+
+    # clear
+    ser.readlines()
+
+    ser.write("\nreboot\n".encode("ascii"))
+    ser.flush()
+
+    timeout_reboot_cmd = 30
+    timeout = 300  # 5 minutes
+    timeout_start = time.time()
+    timeout_newline = time.time()
+    time_success = 0
+
+    return_code = 0
+
+    while True:
+        if time.time() > timeout_start + timeout_reboot_cmd:
+            ser.write("reboot\n".encode("ascii"))
+
+        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:
+            time_success = time.time()
+
+        # wait at least 2 seconds after seeing prompt to catch potential errors
+        if time_success > 0 and time.time() > time_success + 2:
+            sys.exit(return_code)
+
+        if time.time() > timeout_start + timeout:
+            print("Error, timeout")
+            sys.exit(-1)
+
+def main():
+    parser = ArgumentParser(description=__doc__)
+    parser.add_argument('--device', "-d", nargs='?', default=None, help='', required=True)
+    parser.add_argument("--baudrate", "-b", dest="baudrate", type=int, help="Mavlink port baud rate (default=57600)", default=57600)
+    args = parser.parse_args()
+
+    monitor_firmware_upload(args.device, args.baudrate)
+
+if __name__ == "__main__":
+   main()