mirror of https://github.com/ArduPilot/ardupilot
uncrustify ArduPlane/geofence.pde
This commit is contained in:
parent
4517ca8124
commit
956b101189
|
@ -1,19 +1,19 @@
|
||||||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||||
/*
|
/*
|
||||||
geo-fencing support
|
* geo-fencing support
|
||||||
Andrew Tridgell, December 2011
|
* Andrew Tridgell, December 2011
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#if GEOFENCE_ENABLED == ENABLED
|
#if GEOFENCE_ENABLED == ENABLED
|
||||||
|
|
||||||
/*
|
/*
|
||||||
The state of geo-fencing. This structure is dynamically allocated
|
* The state of geo-fencing. This structure is dynamically allocated
|
||||||
the first time it is used. This means we only pay for the pointer
|
* the first time it is used. This means we only pay for the pointer
|
||||||
and not the structure on systems where geo-fencing is not being
|
* and not the structure on systems where geo-fencing is not being
|
||||||
used.
|
* used.
|
||||||
|
*
|
||||||
We store a copy of the boundary in memory as we need to access it
|
* We store a copy of the boundary in memory as we need to access it
|
||||||
very quickly at runtime
|
* very quickly at runtime
|
||||||
*/
|
*/
|
||||||
static struct geofence_state {
|
static struct geofence_state {
|
||||||
uint8_t num_points;
|
uint8_t num_points;
|
||||||
|
@ -29,7 +29,7 @@ static struct geofence_state {
|
||||||
|
|
||||||
|
|
||||||
/*
|
/*
|
||||||
fence boundaries fetch/store
|
* fence boundaries fetch/store
|
||||||
*/
|
*/
|
||||||
static Vector2l get_fence_point_with_index(unsigned i)
|
static Vector2l get_fence_point_with_index(unsigned i)
|
||||||
{
|
{
|
||||||
|
@ -71,7 +71,7 @@ static void set_fence_point_with_index(Vector2l &point, unsigned i)
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
allocate and fill the geofence state structure
|
* allocate and fill the geofence state structure
|
||||||
*/
|
*/
|
||||||
static void geofence_load(void)
|
static void geofence_load(void)
|
||||||
{
|
{
|
||||||
|
@ -116,7 +116,7 @@ failed:
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
return true if geo-fencing is enabled
|
* return true if geo-fencing is enabled
|
||||||
*/
|
*/
|
||||||
static bool geofence_enabled(void)
|
static bool geofence_enabled(void)
|
||||||
{
|
{
|
||||||
|
@ -137,7 +137,7 @@ static bool geofence_enabled(void)
|
||||||
|
|
||||||
|
|
||||||
/*
|
/*
|
||||||
return true if we have breached the geo-fence minimum altiude
|
* return true if we have breached the geo-fence minimum altiude
|
||||||
*/
|
*/
|
||||||
static bool geofence_check_minalt(void)
|
static bool geofence_check_minalt(void)
|
||||||
{
|
{
|
||||||
|
@ -151,7 +151,7 @@ static bool geofence_check_minalt(void)
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
return true if we have breached the geo-fence maximum altiude
|
* return true if we have breached the geo-fence maximum altiude
|
||||||
*/
|
*/
|
||||||
static bool geofence_check_maxalt(void)
|
static bool geofence_check_maxalt(void)
|
||||||
{
|
{
|
||||||
|
@ -166,7 +166,7 @@ static bool geofence_check_maxalt(void)
|
||||||
|
|
||||||
|
|
||||||
/*
|
/*
|
||||||
check if we have breached the geo-fence
|
* check if we have breached the geo-fence
|
||||||
*/
|
*/
|
||||||
static void geofence_check(bool altitude_check_only)
|
static void geofence_check(bool altitude_check_only)
|
||||||
{
|
{
|
||||||
|
@ -286,10 +286,10 @@ static void geofence_check(bool altitude_check_only)
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
return true if geofencing allows stick mixing. When we have
|
* return true if geofencing allows stick mixing. When we have
|
||||||
triggered failsafe and are in GUIDED mode then stick mixing is
|
* triggered failsafe and are in GUIDED mode then stick mixing is
|
||||||
disabled. Otherwise the aircraft may not be able to recover from
|
* disabled. Otherwise the aircraft may not be able to recover from
|
||||||
a breach of the geo-fence
|
* a breach of the geo-fence
|
||||||
*/
|
*/
|
||||||
static bool geofence_stickmixing(void) {
|
static bool geofence_stickmixing(void) {
|
||||||
if (geofence_enabled() &&
|
if (geofence_enabled() &&
|
||||||
|
@ -304,7 +304,7 @@ static bool geofence_stickmixing(void) {
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
|
*
|
||||||
*/
|
*/
|
||||||
static void geofence_send_status(mavlink_channel_t chan)
|
static void geofence_send_status(mavlink_channel_t chan)
|
||||||
{
|
{
|
||||||
|
@ -326,8 +326,13 @@ bool geofence_breached(void)
|
||||||
|
|
||||||
#else // GEOFENCE_ENABLED
|
#else // GEOFENCE_ENABLED
|
||||||
|
|
||||||
static void geofence_check(bool altitude_check_only) { }
|
static void geofence_check(bool altitude_check_only) {
|
||||||
static bool geofence_stickmixing(void) { return true; }
|
}
|
||||||
static bool geofence_enabled(void) { return false; }
|
static bool geofence_stickmixing(void) {
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
static bool geofence_enabled(void) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
#endif // GEOFENCE_ENABLED
|
#endif // GEOFENCE_ENABLED
|
||||||
|
|
Loading…
Reference in New Issue