mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-26 01:33:56 -04:00
geofence: send fence status messages on fence events
this ensures the fence status goes out as soon as possible, which means the messages arrive in a sane order on the GCS
This commit is contained in:
parent
86b86c1bdf
commit
c14d702912
@ -107,6 +107,7 @@ static void geofence_load(void)
|
|||||||
geofence_state->fence_triggered = false;
|
geofence_state->fence_triggered = false;
|
||||||
|
|
||||||
gcs_send_text_P(SEVERITY_LOW,PSTR("geo-fence loaded"));
|
gcs_send_text_P(SEVERITY_LOW,PSTR("geo-fence loaded"));
|
||||||
|
gcs_send_message(MSG_FENCE_STATUS);
|
||||||
return;
|
return;
|
||||||
|
|
||||||
failed:
|
failed:
|
||||||
@ -227,6 +228,7 @@ static void geofence_check(bool altitude_check_only)
|
|||||||
#if FENCE_TRIGGERED_PIN > 0
|
#if FENCE_TRIGGERED_PIN > 0
|
||||||
digitalWrite(FENCE_TRIGGERED_PIN, LOW);
|
digitalWrite(FENCE_TRIGGERED_PIN, LOW);
|
||||||
#endif
|
#endif
|
||||||
|
gcs_send_message(MSG_FENCE_STATUS);
|
||||||
}
|
}
|
||||||
// we're inside, all is good with the world
|
// we're inside, all is good with the world
|
||||||
return;
|
return;
|
||||||
@ -251,6 +253,7 @@ static void geofence_check(bool altitude_check_only)
|
|||||||
#endif
|
#endif
|
||||||
|
|
||||||
gcs_send_text_P(SEVERITY_LOW,PSTR("geo-fence triggered"));
|
gcs_send_text_P(SEVERITY_LOW,PSTR("geo-fence triggered"));
|
||||||
|
gcs_send_message(MSG_FENCE_STATUS);
|
||||||
|
|
||||||
// see what action the user wants
|
// see what action the user wants
|
||||||
switch (g.fence_action) {
|
switch (g.fence_action) {
|
||||||
|
Loading…
Reference in New Issue
Block a user