diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 3b9ea8acba..2e2a38e6ab 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -309,7 +309,9 @@ protected: void handle_common_message(mavlink_message_t *msg); void handle_set_gps_global_origin(const mavlink_message_t *msg); void handle_setup_signing(const mavlink_message_t *msg); - MAV_RESULT handle_preflight_reboot(const mavlink_command_long_t &packet, bool disable_overrides); + virtual bool should_disable_overrides_on_reboot() const { return true; } + virtual bool should_zero_rc_outputs_on_reboot() const { return false; } + MAV_RESULT handle_preflight_reboot(const mavlink_command_long_t &packet); MAV_RESULT handle_rc_bind(const mavlink_command_long_t &packet); virtual MAV_RESULT handle_flight_termination(const mavlink_command_long_t &packet); @@ -556,6 +558,8 @@ private: struct Location global_position_current_loc; void send_global_position_int(); + + void zero_rc_outputs(); }; /// @class GCS diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index f1f2632d4a..22b0d44881 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -1769,6 +1769,18 @@ void GCS_MAVLINK::send_vfr_hud() vfr_hud_climbrate()); } +void GCS_MAVLINK::zero_rc_outputs() +{ + // Send an invalid signal to the motors to prevent spinning due to neutral (1500) pwm pulse being cut short + // For that matter, send an invalid signal to all channels to prevent undesired/unexpected behavior + SRV_Channels::cork(); + for (int i=0; iwrite(i, 1); + } + SRV_Channels::push(); +} + /* handle a MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN command @@ -1777,10 +1789,10 @@ void GCS_MAVLINK::send_vfr_hud() motors. That can be dangerous when a preflight reboot is done with the pilot close to the aircraft and can also damage the aircraft */ -MAV_RESULT GCS_MAVLINK::handle_preflight_reboot(const mavlink_command_long_t &packet, bool disable_overrides) +MAV_RESULT GCS_MAVLINK::handle_preflight_reboot(const mavlink_command_long_t &packet) { if (is_equal(packet.param1,1.0f) || is_equal(packet.param1,3.0f)) { - if (disable_overrides) { + if (should_disable_overrides_on_reboot()) { #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN // disable overrides while rebooting int px4io_fd = open("/dev/px4io", 0); @@ -1797,8 +1809,17 @@ MAV_RESULT GCS_MAVLINK::handle_preflight_reboot(const mavlink_command_long_t &pa } #endif } + if (should_zero_rc_outputs_on_reboot()) { + reboot_zero_rc_outputs(); + } // send ack before we reboot mavlink_msg_command_ack_send(chan, packet.command, MAV_RESULT_ACCEPTED); + // Notify might want to blink some LEDs: + AP_Notify *notify = AP_Notify::instance(); + if (notify) { + AP_Notify::flags.firmware_update = 1; + notify->update(); + } // force safety on hal.rcout->force_safety_on(); hal.rcout->force_safety_no_wait(); @@ -2665,6 +2686,10 @@ MAV_RESULT GCS_MAVLINK::handle_command_long_message(mavlink_command_long_t &pack result = handle_command_do_send_banner(packet); break; + case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN: + result = handle_preflight_reboot(packet); + break; + case MAV_CMD_DO_START_MAG_CAL: case MAV_CMD_DO_ACCEPT_MAG_CAL: case MAV_CMD_DO_CANCEL_MAG_CAL: {