AP_Periph: gracefully handle PWM and Params during reboot and bot up as PWM safety on

Tools/AP_Periph/AP_Periph.cpp

@@ -281,7 +281,7 @@ void AP_Periph_FW::update()
 #endif
 
 #ifdef HAL_PERIPH_ENABLE_RC_OUT
-        SRV_Channels::enable_aux_servos();
+        rcout_init_1Hz();
 #endif
     }
 
@@ -319,6 +319,7 @@ void AP_Periph_FW::update()
 #endif
 }
 
+#ifdef HAL_PERIPH_LISTEN_FOR_SERIAL_UART_REBOOT_CMD_PORT
 // check for uploader.py reboot command
 void AP_Periph_FW::check_for_serial_reboot_cmd(const int8_t serial_index)
 {
@@ -362,16 +363,18 @@ void AP_Periph_FW::check_for_serial_reboot_cmd(const int8_t serial_index)
             }
             index[i]++;
             if (index[i] == reboot_string_len) {
-                // received reboot msg
-                reboot(true);
+                // received reboot msg. Trigger a reboot and stay in the bootloader
+                prepare_reboot();
+                hal.scheduler->reboot(true);
             }
         }
     }
 }
+#endif // HAL_PERIPH_LISTEN_FOR_SERIAL_UART_REBOOT_CMD_PORT
 
-// trigger a safe reboot where PWMs and params are gracefully disabled
-// This is copied from AP_Vehicle::reboot(bool hold_in_bootloader)
-void AP_Periph_FW::reboot(bool hold_in_bootloader)
+// prepare for a safe reboot where PWMs and params are gracefully disabled
+// This is copied from AP_Vehicle::reboot(bool hold_in_bootloader) minus the actual reboot
+void AP_Periph_FW::prepare_reboot()
 {
 #ifdef HAL_PERIPH_ENABLE_NOTIFY
         // Notify might want to blink some LEDs:
@@ -379,8 +382,10 @@ void AP_Periph_FW::reboot(bool hold_in_bootloader)
         notify.update();
 #endif
 
+#ifdef HAL_PERIPH_ENABLE_RC_OUT
         // force safety on
         hal.rcout->force_safety_on();
+#endif
 
         // flush pending parameter writes
         AP_Param::flush();
@@ -389,9 +394,8 @@ void AP_Periph_FW::reboot(bool hold_in_bootloader)
         hal.scheduler->register_delay_callback(nullptr, 5);
 
         // delay to give the ACK a chance to get out, the LEDs to flash,
-        // the IO board safety to be forced on, the parameters to flush, ...
-        hal.scheduler->delay(200);
-
-        hal.scheduler->reboot(hold_in_bootloader);
+        // the IO board safety to be forced on, the parameters to flush,
+        hal.scheduler->delay(40);
 }
+
 AP_HAL_MAIN();

Tools/AP_Periph/AP_Periph.h

@@ -46,9 +46,11 @@ public:
     void can_battery_update();
 
     void load_parameters();
+    void prepare_reboot();
 
+#ifdef HAL_PERIPH_LISTEN_FOR_SERIAL_UART_REBOOT_CMD_PORT
     void check_for_serial_reboot_cmd(const int8_t serial_index);
-    void reboot(bool hold_in_bootloader);
+#endif
 
     AP_SerialManager serial_manager;
 
@@ -133,6 +135,7 @@ public:
     SRV_Channels servo_channels;
 
     void rcout_init();
+    void rcout_init_1Hz();
     void rcout_esc(int16_t *rc, uint8_t num_channels);
     void rcout_srv(const uint8_t actuator_id, const float command_value);
     void rcout_update();

Tools/AP_Periph/can.cpp

@@ -389,6 +389,7 @@ static void handle_begin_firmware_update(CanardInstance* ins, CanardRxTransfer*
 
     // instant reboot, with backup register used to give bootloader
     // the node_id
+    periph.prepare_reboot();
 #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
     set_fast_reboot((rtc_boot_magic)(RTC_BOOT_CANBL | canardGetLocalNodeID(ins)));
     NVIC_SystemReset();
@@ -772,6 +773,7 @@ static void onTransferReceived(CanardInstance* ins,
     case UAVCAN_PROTOCOL_RESTARTNODE_ID:
         printf("RestartNode\n");
         hal.scheduler->delay(10);
+        periph.prepare_reboot();
 #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
         NVIC_SystemReset();
 #elif CONFIG_HAL_BOARD == HAL_BOARD_SITL

Tools/AP_Periph/rc_out.cpp

@@ -34,6 +34,9 @@ extern const AP_HAL::HAL &hal;
 
 void AP_Periph_FW::rcout_init()
 {
+    // start up with safety enabled. This disables the pwm output until we receive an packet from the rempte system
+    hal.rcout->force_safety_on();
+
     for (uint8_t i=0; i<HAL_PWM_COUNT; i++) {
         servo_channels.set_default_function(i, SRV_Channel::Aux_servo_function_t(SRV_Channel::k_rcin1 + i));
     }
@@ -44,6 +47,19 @@ void AP_Periph_FW::rcout_init()
     for (uint8_t i=0; i<SERVO_OUT_MOTOR_MAX; i++) {
         SRV_Channels::set_angle(SRV_Channels::get_motor_function(i), UAVCAN_ESC_MAX_VALUE);
     }
+
+    // run this once and at 1Hz to configure aux and esc ranges
+    rcout_init_1Hz();
+}
+
+void AP_Periph_FW::rcout_init_1Hz()
+{
+    // this runs at 1Hz to allow for run-time param changes
+    SRV_Channels::enable_aux_servos();
+
+    for (uint8_t i=0; i<SERVO_OUT_MOTOR_MAX; i++) {
+        servo_channels.set_esc_scaling_for(SRV_Channels::get_motor_function(i));
+    }
 }
 
 void AP_Periph_FW::rcout_esc(int16_t *rc, uint8_t num_channels)