diff --git a/Blimp/Blimp.h b/Blimp/Blimp.h index bcc3a87727..c07a35e8b8 100644 --- a/Blimp/Blimp.h +++ b/Blimp/Blimp.h @@ -212,8 +212,6 @@ private: // Failsafe struct { - uint32_t last_heartbeat_ms; // the time when the last HEARTBEAT message arrived from a GCS - used for triggering gcs failsafe - int8_t radio_counter; // number of iterations with throttle below throttle_fs_value uint8_t radio : 1; // A status flag for the radio failsafe diff --git a/Blimp/GCS_Mavlink.cpp b/Blimp/GCS_Mavlink.cpp index ab8acc01d3..0b00656c2b 100644 --- a/Blimp/GCS_Mavlink.cpp +++ b/Blimp/GCS_Mavlink.cpp @@ -477,13 +477,6 @@ void GCS_MAVLINK_Blimp::send_banner() send_text(MAV_SEVERITY_INFO, "Frame: %s", blimp.get_frame_string()); } -// a RC override message is considered to be a 'heartbeat' from the ground station for failsafe purposes -void GCS_MAVLINK_Blimp::handle_rc_channels_override(const mavlink_message_t &msg) -{ - blimp.failsafe.last_heartbeat_ms = AP_HAL::millis(); - GCS_MAVLINK::handle_rc_channels_override(msg); -} - MAV_RESULT GCS_MAVLINK_Blimp::_handle_command_preflight_calibration(const mavlink_command_long_t &packet) { return GCS_MAVLINK::_handle_command_preflight_calibration(packet); @@ -638,15 +631,6 @@ void GCS_MAVLINK_Blimp::handleMessage(const mavlink_message_t &msg) { switch (msg.msgid) { - case MAVLINK_MSG_ID_HEARTBEAT: { // MAV ID: 0 - // We keep track of the last time we received a heartbeat from our GCS for failsafe purposes - if (msg.sysid != blimp.g.sysid_my_gcs) { - break; - } - blimp.failsafe.last_heartbeat_ms = AP_HAL::millis(); - break; - } - // #if MODE_GUIDED_ENABLED == ENABLED // case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET: // MAV ID: 82 // { diff --git a/Blimp/GCS_Mavlink.h b/Blimp/GCS_Mavlink.h index 14b7b1f7a2..97b4aedb85 100644 --- a/Blimp/GCS_Mavlink.h +++ b/Blimp/GCS_Mavlink.h @@ -49,7 +49,6 @@ private: void handleMessage(const mavlink_message_t &msg) override; bool handle_guided_request(AP_Mission::Mission_Command &cmd) override; void handle_change_alt_request(AP_Mission::Mission_Command &cmd) override; - void handle_rc_channels_override(const mavlink_message_t &msg) override; bool try_send_message(enum ap_message id) override; void packetReceived(const mavlink_status_t &status, diff --git a/Blimp/events.cpp b/Blimp/events.cpp index 6f77c54c91..579fbb706e 100644 --- a/Blimp/events.cpp +++ b/Blimp/events.cpp @@ -91,13 +91,18 @@ void Blimp::handle_battery_failsafe(const char *type_str, const int8_t action) void Blimp::failsafe_gcs_check() { // Bypass GCS failsafe checks if disabled or GCS never connected - if (g.failsafe_gcs == FS_GCS_DISABLED || failsafe.last_heartbeat_ms == 0) { + if (g.failsafe_gcs == FS_GCS_DISABLED) { return; } + const uint32_t gcs_last_seen_ms = gcs().sysid_myggcs_last_seen_time_ms(); + if (gcs_last_seen_ms == 0) { + return; + } + // calc time since last gcs update // note: this only looks at the heartbeat from the device id set by g.sysid_my_gcs - const uint32_t last_gcs_update_ms = millis() - failsafe.last_heartbeat_ms; + const uint32_t last_gcs_update_ms = millis() - gcs_last_seen_ms; const uint32_t gcs_timeout_ms = uint32_t(constrain_float(g2.fs_gcs_timeout * 1000.0f, 0.0f, UINT32_MAX)); // Determine which event to trigger