Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-04 06:58:39 -04:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Peter Barker 2021-02-05 13:28:08 +11:00 committed by Andrew Tridgell
parent 09a9bd73a4
commit aa973c5245
5 changed files with 12 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduPlane/ArduPlane.cpp
View File

@ -252,7 +252,7 @@ void Plane::afs_fs_check(void)
#else
const bool fence_breached = false;
#endif
afs.check(failsafe.last_heartbeat_ms, fence_breached, failsafe.AFS_last_valid_rc_ms);
afs.check(fence_breached, failsafe.AFS_last_valid_rc_ms);
}
#endif

21
ArduPlane/GCS_Mavlink.cpp
View File

@ -1152,17 +1152,9 @@ void GCS_MAVLINK_Plane::handleMessage(const mavlink_message_t &msg)
manual_override(plane.channel_throttle, packet.z, 0, 1000, tnow);
manual_override(plane.channel_rudder, packet.r, 1000, 2000, tnow);
// a manual control message is considered to be a 'heartbeat' from the ground station for failsafe purposes
plane.failsafe.last_heartbeat_ms = tnow;
break;
}
case MAVLINK_MSG_ID_HEARTBEAT:
{
// We keep track of the last time we received a heartbeat from
// our GCS for failsafe purposes
if (msg.sysid != plane.g.sysid_my_gcs) break;
plane.failsafe.last_heartbeat_ms = AP_HAL::millis();
// a manual control message is considered to be a 'heartbeat'
// from the ground station for failsafe purposes
gcs().sysid_myggcs_seen(tnow);
break;
}
@ -1417,13 +1409,6 @@ void GCS_MAVLINK_Plane::handleMessage(const mavlink_message_t &msg)
} // end switch
} // end handle mavlink
// a RC override message is considered to be a 'heartbeat' from the ground station for failsafe purposes
void GCS_MAVLINK_Plane::handle_rc_channels_override(const mavlink_message_t &msg)
{
plane.failsafe.last_heartbeat_ms = AP_HAL::millis();
GCS_MAVLINK::handle_rc_channels_override(msg);
}
MAV_RESULT GCS_MAVLINK_Plane::handle_command_do_set_mission_current(const mavlink_command_long_t &packet)
{
const MAV_RESULT result = GCS_MAVLINK::handle_command_do_set_mission_current(packet);

1
ArduPlane/GCS_Mavlink.h
View File

@ -50,7 +50,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;
MAV_RESULT handle_command_int_do_reposition(const mavlink_command_int_t &packet);
MAV_RESULT handle_command_int_guided_slew_commands(const mavlink_command_int_t &packet);

3
ArduPlane/Plane.h
View File

@ -334,9 +334,6 @@ private:
// number of low throttle values
uint8_t throttle_counter;
// the time when the last HEARTBEAT message arrived from a GCS
uint32_t last_heartbeat_ms;
// A timer used to track how long we have been in a "short failsafe" condition due to loss of RC signal
uint32_t short_timer_ms;

15
ArduPlane/system.cpp
View File

@ -200,7 +200,7 @@ void Plane::startup_ground(void)
// reset last heartbeat time, so we don't trigger failsafe on slow
// startup
failsafe.last_heartbeat_ms = millis();
gcs().sysid_myggcs_seen(AP_HAL::millis());
// we don't want writes to the serial port to cause us to pause
// mid-flight, so set the serial ports non-blocking once we are
@ -297,7 +297,8 @@ bool Plane::set_mode_by_number(const Mode::Number new_mode_number, const ModeRea
void Plane::check_long_failsafe()
{
uint32_t tnow = millis();
const uint32_t gcs_last_seen_ms = gcs().sysid_myggcs_last_seen_time_ms();
const uint32_t tnow = millis();
// only act on changes
// -------------------
if (failsafe.state != FAILSAFE_LONG && failsafe.state != FAILSAFE_GCS && flight_stage != AP_Vehicle::FixedWing::FLIGHT_LAND) {
@ -310,13 +311,13 @@ void Plane::check_long_failsafe()
(tnow - radio_timeout_ms) > g.fs_timeout_long*1000) {
failsafe_long_on_event(FAILSAFE_LONG, ModeReason::RADIO_FAILSAFE);
} else if (g.gcs_heartbeat_fs_enabled == GCS_FAILSAFE_HB_AUTO && control_mode == &mode_auto &&
failsafe.last_heartbeat_ms != 0 &&
(tnow - failsafe.last_heartbeat_ms) > g.fs_timeout_long*1000) {
gcs_last_seen_ms != 0 &&
(tnow - gcs_last_seen_ms) > g.fs_timeout_long*1000) {
failsafe_long_on_event(FAILSAFE_GCS, ModeReason::GCS_FAILSAFE);
} else if ((g.gcs_heartbeat_fs_enabled == GCS_FAILSAFE_HEARTBEAT ||
g.gcs_heartbeat_fs_enabled == GCS_FAILSAFE_HB_RSSI) &&
failsafe.last_heartbeat_ms != 0 &&
(tnow - failsafe.last_heartbeat_ms) > g.fs_timeout_long*1000) {
gcs_last_seen_ms != 0 &&
(tnow - gcs_last_seen_ms) > g.fs_timeout_long*1000) {
failsafe_long_on_event(FAILSAFE_GCS, ModeReason::GCS_FAILSAFE);
} else if (g.gcs_heartbeat_fs_enabled == GCS_FAILSAFE_HB_RSSI &&
gcs().chan(0) != nullptr &&
@ -332,7 +333,7 @@ void Plane::check_long_failsafe()
}
// We do not change state but allow for user to change mode
if (failsafe.state == FAILSAFE_GCS &&
(tnow - failsafe.last_heartbeat_ms) < timeout_seconds*1000) {
(tnow - gcs_last_seen_ms) < timeout_seconds*1000) {
failsafe_long_off_event(ModeReason::GCS_FAILSAFE);
} else if (failsafe.state == FAILSAFE_LONG &&
!failsafe.rc_failsafe) {