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

Rover: add FS_GCS_TIMEOUT

This commit is contained in:
Pierre Kancir 2023-01-29 23:49:35 +01:00 committed by Tom Pittenger
parent a6831805f1
commit 837ebd4491
4 changed files with 22 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
Rover/Parameters.cpp
View File

@ -683,6 +683,15 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
// @User: Advanced
AP_GROUPINFO("MANUAL_STR_EXPO", 55, ParametersG2, manual_steering_expo, 0),
// @Param: FS_GCS_TIMEOUT
// @DisplayName: GCS failsafe timeout
// @Description: Timeout before triggering the GCS failsafe
// @Units: s
// @Range: 2 120
// @Increment: 1
// @User: Standard
AP_GROUPINFO("FS_GCS_TIMEOUT", 56, ParametersG2, fs_gcs_timeout, 5),
AP_GROUPEND
};

3
Rover/Parameters.h
View File

@ -433,6 +433,9 @@ public:
// manual mode steering expo
AP_Float manual_steering_expo;
// FS GCS timeout trigger time
AP_Float fs_gcs_timeout;
};
extern const AP_Param::Info var_info[];

16
Rover/Rover.cpp
View File

@ -344,22 +344,24 @@ void Rover::ahrs_update()
*/
void Rover::gcs_failsafe_check(void)
{
if (!g.fs_gcs_enabled) {
if (g.fs_gcs_enabled == FS_GCS_DISABLED) {
// gcs failsafe disabled
return;
}
// check for updates from GCS within 2 seconds
const uint32_t gcs_last_seen_ms = gcs().sysid_myggcs_last_seen_time_ms();
bool do_failsafe = true;
if (gcs_last_seen_ms == 0) {
// we've never seen the GCS, so we never failsafe for not seeing it
do_failsafe = false;
} else if (millis() - gcs_last_seen_ms <= 2000) {
// we've never seen the GCS in the last couple of seconds, so all good
do_failsafe = false;
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() - gcs_last_seen_ms;
const uint32_t gcs_timeout_ms = uint32_t(constrain_float(g2.fs_gcs_timeout * 1000.0f, 0.0f, UINT32_MAX));
const bool do_failsafe = last_gcs_update_ms >= gcs_timeout_ms ? true : false;
failsafe_trigger(FAILSAFE_EVENT_GCS, "GCS", do_failsafe);
}

1
Rover/failsafe.cpp
View File

@ -78,6 +78,7 @@ void Rover::failsafe_trigger(uint8_t failsafe_type, const char* type_str, bool o
((failsafe_type == FAILSAFE_EVENT_THROTTLE && g.fs_throttle_enabled == FS_THR_ENABLED_CONTINUE_MISSION) ||
(failsafe_type == FAILSAFE_EVENT_GCS && g.fs_gcs_enabled == FS_GCS_ENABLED_CONTINUE_MISSION))) {
// continue with mission in auto mode
gcs().send_text(MAV_SEVERITY_WARNING, "Failsafe - Continuing Auto Mode");
} else {
switch (g.fs_action) {
case Failsafe_Action_None: