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

Plane: Remove unneeded state from geofence

This commit is contained in:
Michael du Breuil 2019-09-16 13:06:45 -07:00 committed by Andrew Tridgell
parent 8f6ca7bb8f
commit bfcd1da485
2 changed files with 12 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduPlane/Plane.h
View File

@ -903,8 +903,8 @@ private:
void failsafe_short_off_event(mode_reason_t reason); void failsafe_short_off_event(mode_reason_t reason);
void failsafe_long_off_event(mode_reason_t reason); void failsafe_long_off_event(mode_reason_t reason);
void handle_battery_failsafe(const char* type_str, const int8_t action); void handle_battery_failsafe(const char* type_str, const int8_t action);
uint8_t max_fencepoints(void); uint8_t max_fencepoints(void) const;
Vector2l get_fence_point_with_index(unsigned i); Vector2l get_fence_point_with_index(uint8_t i) const;
void set_fence_point_with_index(const Vector2l &point, unsigned i); void set_fence_point_with_index(const Vector2l &point, unsigned i);
void geofence_load(void); void geofence_load(void);
bool geofence_present(void); bool geofence_present(void);

28
ArduPlane/geofence.cpp
View File

@ -25,8 +25,6 @@ static struct GeofenceState {
bool boundary_uptodate; bool boundary_uptodate;
bool fence_triggered; bool fence_triggered;
bool is_pwm_enabled; //true if above FENCE_ENABLE_PWM threshold bool is_pwm_enabled; //true if above FENCE_ENABLE_PWM threshold
bool previous_is_pwm_enabled; //true if above FENCE_ENALBE_PWM threshold
// last time we checked
bool is_enabled; bool is_enabled;
GeofenceEnableReason enable_reason; GeofenceEnableReason enable_reason;
bool floor_enabled; //typically used for landing bool floor_enabled; //typically used for landing
@ -46,7 +44,7 @@ static const StorageAccess fence_storage(StorageManager::StorageFence);
/* /*
maximum number of fencepoints maximum number of fencepoints
*/ */
uint8_t Plane::max_fencepoints(void) uint8_t Plane::max_fencepoints(void) const
{ {
return MIN(255U, fence_storage.size() / sizeof(Vector2l)); return MIN(255U, fence_storage.size() / sizeof(Vector2l));
} }
@ -54,19 +52,15 @@ uint8_t Plane::max_fencepoints(void)
/* /*
* fence boundaries fetch/store * fence boundaries fetch/store
*/ */
Vector2l Plane::get_fence_point_with_index(unsigned i) Vector2l Plane::get_fence_point_with_index(uint8_t i) const
{ {
Vector2l ret; if (i > (uint8_t)g.fence_total || i >= max_fencepoints()) {
if (i > (unsigned)g.fence_total || i >= max_fencepoints()) {
return Vector2l(0,0); return Vector2l(0,0);
} }
// read fence point // read fence point
ret.x = fence_storage.read_uint32(i * sizeof(Vector2l)); return Vector2l(fence_storage.read_uint32(i * sizeof(Vector2l)),
ret.y = fence_storage.read_uint32(i * sizeof(Vector2l) + sizeof(int32_t)); fence_storage.read_uint32(i * sizeof(Vector2l) + sizeof(int32_t)));
return ret;
} }
// save a fence point // save a fence point
@ -90,8 +84,6 @@ void Plane::set_fence_point_with_index(const Vector2l &point, unsigned i)
*/ */
void Plane::geofence_load(void) void Plane::geofence_load(void)
{ {
uint8_t i;
if (geofence_state == nullptr) { if (geofence_state == nullptr) {
uint16_t boundary_size = sizeof(Vector2l) * max_fencepoints(); uint16_t boundary_size = sizeof(Vector2l) * max_fencepoints();
if (hal.util->available_memory() < 100 + boundary_size + sizeof(struct GeofenceState)) { if (hal.util->available_memory() < 100 + boundary_size + sizeof(struct GeofenceState)) {
@ -122,10 +114,10 @@ void Plane::geofence_load(void)
return; return;
} }
for (i=0; i<g.fence_total; i++) { for (uint8_t i = 0; i<g.fence_total; i++) {
geofence_state->boundary[i] = get_fence_point_with_index(i); geofence_state->boundary[i] = get_fence_point_with_index(i);
} }
geofence_state->num_points = i; geofence_state->num_points = g.fence_total;
if (!Polygon_complete(&geofence_state->boundary[1], geofence_state->num_points-1)) { if (!Polygon_complete(&geofence_state->boundary[1], geofence_state->num_points-1)) {
geofence_disable_and_send_error_msg("pt[1] and pt[total-1] must match"); geofence_disable_and_send_error_msg("pt[1] and pt[total-1] must match");
@ -193,11 +185,9 @@ void Plane::geofence_update_pwm_enabled_state()
return; return;
} }
geofence_state->previous_is_pwm_enabled = geofence_state->is_pwm_enabled; if (geofence_state->is_pwm_enabled != is_pwm_enabled) {
geofence_set_enabled(is_pwm_enabled, PWM_TOGGLED);
geofence_state->is_pwm_enabled = is_pwm_enabled; geofence_state->is_pwm_enabled = is_pwm_enabled;
if (geofence_state->is_pwm_enabled != geofence_state->previous_is_pwm_enabled) {
geofence_set_enabled(geofence_state->is_pwm_enabled, PWM_TOGGLED);
} }
} }