Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

ArduSub: move handling of last-seen-SYSID_MYGCS up to GCS base class

This commit is contained in:
Peter Barker 2021-02-05 13:28:08 +11:00 committed by Andrew Tridgell
parent aa973c5245
commit 2e23822b27
4 changed files with 11 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
ArduSub/GCS_Mavlink.cpp
View File

@ -529,15 +529,6 @@ void GCS_MAVLINK_Sub::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 != sub.g.sysid_my_gcs) {
break;
}
sub.failsafe.last_heartbeat_ms = AP_HAL::millis();
break;
}
case MAVLINK_MSG_ID_MANUAL_CONTROL: { // MAV ID: 69
if (msg.sysid != sub.g.sysid_my_gcs) {
break; // Only accept control from our gcs
@ -552,8 +543,9 @@ void GCS_MAVLINK_Sub::handleMessage(const mavlink_message_t &msg)
sub.transform_manual_control_to_rc_override(packet.x,packet.y,packet.z,packet.r,packet.buttons);
sub.failsafe.last_pilot_input_ms = AP_HAL::millis();
// a RC override message is considered to be a 'heartbeat' from the ground station for failsafe purposes
sub.failsafe.last_heartbeat_ms = AP_HAL::millis();
// a RC override message is considered to be a 'heartbeat'
// from the ground station for failsafe purposes
gcs().sysid_myggcs_seen(AP_HAL::millis());
break;
}
@ -787,13 +779,6 @@ uint64_t GCS_MAVLINK_Sub::capabilities() const
);
}
// a RC override message is considered to be a 'heartbeat' from the ground station for failsafe purposes
void GCS_MAVLINK_Sub::handle_rc_channels_override(const mavlink_message_t &msg)
{
sub.failsafe.last_heartbeat_ms = AP_HAL::millis();
GCS_MAVLINK::handle_rc_channels_override(msg);
}
MAV_RESULT GCS_MAVLINK_Sub::handle_flight_termination(const mavlink_command_long_t &packet) {
if (packet.param1 > 0.5f) {
sub.arming.disarm(AP_Arming::Method::TERMINATION);

1
ArduSub/GCS_Mavlink.h
View File

@ -44,7 +44,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;
bool send_info(void);

1
ArduSub/Sub.h
View File

@ -222,7 +222,6 @@ private:
struct {
uint32_t last_leak_warn_ms; // last time a leak warning was sent to gcs
uint32_t last_gcs_warn_ms;
uint32_t last_heartbeat_ms; // the time when the last HEARTBEAT message arrived from a GCS - used for triggering gcs failsafe
uint32_t last_pilot_input_ms; // last time we received pilot input in the form of MANUAL_CONTROL or RC_CHANNELS_OVERRIDE messages
uint32_t terrain_first_failure_ms; // the first time terrain data access failed - used to calculate the duration of the failure
uint32_t terrain_last_failure_ms; // the most recent time terrain data access failed

10
ArduSub/failsafe.cpp
View File

@ -308,14 +308,20 @@ void Sub::failsafe_gcs_check()
{
// return immediately if we have never had contact with a gcs, or if gcs failsafe action is disabled
// this also checks to see if we have a GCS failsafe active, if we do, then must continue to process the logic for recovery from this state.
if (failsafe.last_heartbeat_ms == 0 || (!g.failsafe_gcs && g.failsafe_gcs == FS_GCS_DISABLED)) {
if (!g.failsafe_gcs && 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) {
// we've never seen a GCS, so we don't failsafe if we stop seeing it
return;
}
uint32_t tnow = AP_HAL::millis();
// Check if we have gotten a GCS heartbeat recently (GCS sysid must match SYSID_MYGCS parameter)
if (tnow - failsafe.last_heartbeat_ms < FS_GCS_TIMEOUT_MS) {
if (tnow - gcs_last_seen_ms < FS_GCS_TIMEOUT_MS) {
// Log event if we are recovering from previous gcs failsafe
if (failsafe.gcs) {
AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_GCS, LogErrorCode::FAILSAFE_RESOLVED);