diff --git a/APMrover2/GCS_Mavlink.cpp b/APMrover2/GCS_Mavlink.cpp index 1db8dff0b6..fc8fa135af 100644 --- a/APMrover2/GCS_Mavlink.cpp +++ b/APMrover2/GCS_Mavlink.cpp @@ -1415,6 +1415,15 @@ AP_ServoRelayEvents *GCS_MAVLINK_Rover::get_servorelayevents() const return &rover.ServoRelayEvents; } +AP_AdvancedFailsafe *GCS_MAVLINK_Rover::get_advanced_failsafe() const +{ +#if ADVANCED_FAILSAFE == ENABLED + return &rover.g2.afs; +#else + return nullptr; +#endif +} + Compass *GCS_MAVLINK_Rover::get_compass() const { return &rover.compass; diff --git a/APMrover2/GCS_Mavlink.h b/APMrover2/GCS_Mavlink.h index 49225501a9..bcf8193afc 100644 --- a/APMrover2/GCS_Mavlink.h +++ b/APMrover2/GCS_Mavlink.h @@ -22,6 +22,7 @@ protected: AP_Camera *get_camera() const override; AP_ServoRelayEvents *get_servorelayevents() const override; AP_GPS *get_gps() const override; + AP_AdvancedFailsafe *get_advanced_failsafe() const override; uint8_t sysid_my_gcs() const override;