// returns true if the destination is within fence (used to reject waypoints outside the fence)
bool AC_Fence::check_destination_within_fence(const Location_Class& loc)
{
// Altitude fence check
if ((get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX)) {
int32_t alt_above_home_cm;
if (loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_HOME, alt_above_home_cm)) {
if ((alt_above_home_cm * 0.01f) > _alt_max) {
return false;
}
}
}
// Circular fence check
if ((get_enabled_fences() & AC_FENCE_TYPE_CIRCLE)) {
if ((get_distance_cm(_ahrs.get_home(), loc) * 0.01f) > _circle_radius) {
return false;
}
}
// polygon fence check
if ((get_enabled_fences() & AC_FENCE_TYPE_POLYGON) && _boundary_num_points > 0) {
// check ekf has a good location
Location temp_loc;
if (_inav.get_location(temp_loc)) {
const struct Location &ekf_origin = _inav.get_origin();
Vector2f position = location_diff(ekf_origin, loc) * 100.0f;
if (_poly_loader.boundary_breached(position, _boundary_num_points, _boundary, true)) {
return false;
}
}
}
return true;
}
// sets guided mode's target from a Location object
// returns false if destination could not be set (probably caused by missing terrain data)
// or if the fence is enabled and guided waypoint is outside the fence
bool Copter::guided_set_destination(const Location_Class& dest_loc)
{
// ensure we are in position control mode
if (guided_mode != Guided_WP) {
guided_pos_control_start();
}
#if AC_FENCE == ENABLED
// reject destination outside the fence.
// Note: there is a danger that a target specified as a terrain altitude might not be checked if the conversion to alt-above-home fails
int32_t alt_target_cm;
if (dest_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_HOME, alt_target_cm)) {
if (!fence.check_destination_within_fence(alt_target_cm*0.01f, get_distance_cm(ahrs.get_home(), dest_loc)*0.01f)) {
Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_DEST_OUTSIDE_FENCE);
// failure is propagated to GCS with NAK
return false;
}
}
#endif
if (!wp_nav.set_wp_destination(dest_loc)) {
// failure to set destination can only be because of missing terrain data
Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_SET_DESTINATION);
// failure is propagated to GCS with NAK
return false;
}
// log target
Log_Write_GuidedTarget(guided_mode, Vector3f(dest_loc.lat, dest_loc.lng, dest_loc.alt),Vector3f());
return true;
}
// check_position - returns true if gps position is acceptable, false if not
void GPS_Glitch::check_position()
{
uint32_t now = hal.scheduler->millis(); // current system time
float sane_dt; // time since last sane gps reading
float accel_based_distance; // movement based on max acceleration
Location curr_pos; // our current position estimate
Location gps_pos; // gps reported position
float distance_cm; // distance from gps to current position estimate in cm
bool all_ok; // true if the new gps position passes sanity checks
// exit immediately if we don't have gps lock
if (_gps == NULL || _gps->status() != GPS::GPS_OK_FIX_3D) {
_flags.glitching = true;
return;
}
// if not initialised or disabled update last good position and exit
if (!_flags.initialised || !_enabled) {
_last_good_update = now;
_last_good_lat = _gps->latitude;
_last_good_lon = _gps->longitude;
_last_good_vel.x = _gps->velocity_north();
_last_good_vel.y = _gps->velocity_east();
_flags.initialised = true;
_flags.glitching = false;
return;
}
// calculate time since last sane gps reading in ms
sane_dt = (now - _last_good_update) / 1000.0f;
// project forward our position from last known velocity
curr_pos.lat = _last_good_lat;
curr_pos.lng = _last_good_lon;
location_offset(curr_pos, _last_good_vel.x * sane_dt, _last_good_vel.y * sane_dt);
// calculate distance from recent gps position to current estimate
gps_pos.lat = _gps->latitude;
gps_pos.lng = _gps->longitude;
distance_cm = get_distance_cm(curr_pos, gps_pos);
// all ok if within a given hardcoded radius
if (distance_cm <= _radius_cm) {
all_ok = true;
}else{
// or if within the maximum distance we could have moved based on our acceleration
accel_based_distance = 0.5f * _accel_max_cmss * sane_dt * sane_dt;
all_ok = (distance_cm <= accel_based_distance);
}
// store updates to gps position
if (all_ok) {
// position is acceptable
_last_good_update = now;
_last_good_lat = _gps->latitude;
_last_good_lon = _gps->longitude;
_last_good_vel.x = _gps->velocity_north();
_last_good_vel.y = _gps->velocity_east();
}
// update glitching flag
_flags.glitching = !all_ok;
}
