aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Copter: GPS ok only when not glitching

Browse Source 
Removed redundant checks to GPS_ok before setting flight mode to RTL
(this check is already performed inside the set_mode function)
Removed reset of home distance and bearing when GPS lock is lost, it now
remains at the last known value
This commit is contained in:
Randy Mackay 2013-11-11 22:24:18 +09:00
parent e4d8c673f3
commit 03831cdd28
4 changed files with 19 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
ArduCopter/events.pde
View File

@ -22,7 +22,7 @@ static void failsafe_radio_on_event()
}else if(g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_LAND) {
// if failsafe_throttle is 3 (i.e. FS_THR_ENABLED_ALWAYS_LAND) land immediately
set_mode(LAND);
}else if(GPS_ok() && home_distance > wp_nav.get_waypoint_radius()) {
}else if(home_distance > wp_nav.get_waypoint_radius()) {
if (!set_mode(RTL)) {
set_mode(LAND);
}
@ -34,7 +34,7 @@ static void failsafe_radio_on_event()
case AUTO:
// failsafe_throttle is 1 do RTL, 2 means continue with the mission
if (g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_RTL) {
if(GPS_ok() && home_distance > wp_nav.get_waypoint_radius()) {
if(home_distance > wp_nav.get_waypoint_radius()) {
if (!set_mode(RTL)) {
set_mode(LAND);
}
@ -57,7 +57,7 @@ static void failsafe_radio_on_event()
if(g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_LAND) {
// if failsafe_throttle is 3 (i.e. FS_THR_ENABLED_ALWAYS_LAND) land immediately
set_mode(LAND);
}else if(GPS_ok() && home_distance > wp_nav.get_waypoint_radius()) {
}else if(home_distance > wp_nav.get_waypoint_radius()) {
if (!set_mode(RTL)){
set_mode(LAND);
}
@ -105,7 +105,7 @@ static void failsafe_battery_event(void)
break;
case AUTO:
// set_mode to RTL or LAND
if (GPS_ok() && home_distance > wp_nav.get_waypoint_radius()) {
if (home_distance > wp_nav.get_waypoint_radius()) {
if (!set_mode(RTL)) {
set_mode(LAND);
}
@ -139,6 +139,11 @@ static void failsafe_gps_check()
// return immediately if gps failsafe is disabled
if( !g.failsafe_gps_enabled ) {
// if we have just disabled the gps failsafe, ensure the gps failsafe event is cleared
if (failsafe.gps) {
failsafe_gps_off_event();
set_failsafe_gps(false);
}
return;
}
@ -227,7 +232,7 @@ static void failsafe_gcs_check()
// if throttle is zero disarm motors
if (g.rc_3.control_in == 0) {
init_disarm_motors();
}else if(GPS_ok() && home_distance > wp_nav.get_waypoint_radius()) {
}else if(home_distance > wp_nav.get_waypoint_radius()) {
if (!set_mode(RTL)) {
set_mode(LAND);
}
@ -239,7 +244,7 @@ static void failsafe_gcs_check()
case AUTO:
// if g.failsafe_gcs is 1 do RTL, 2 means continue with the mission
if (g.failsafe_gcs == FS_GCS_ENABLED_ALWAYS_RTL) {
if (GPS_ok() && home_distance > wp_nav.get_waypoint_radius()) {
if (home_distance > wp_nav.get_waypoint_radius()) {
if (!set_mode(RTL)) {
set_mode(LAND);
}
@ -251,7 +256,7 @@ static void failsafe_gcs_check()
// if failsafe_throttle is 2 (i.e. FS_THR_ENABLED_CONTINUE_MISSION) no need to do anything
break;
default:
if(GPS_ok() && home_distance > wp_nav.get_waypoint_radius()) {
if(home_distance > wp_nav.get_waypoint_radius()) {
if (!set_mode(RTL)) {
set_mode(LAND);
}

18
ArduCopter/fence.pde
View File

@ -35,22 +35,14 @@ void fence_check()
if(manual_flight_mode(control_mode) && g.rc_3.control_in == 0 && !failsafe.radio && ((fence.get_breaches() & AC_FENCE_TYPE_ALT_MAX)== 0)){
init_disarm_motors();
}else{
// if we have a GPS
if (GPS_ok()) {
// if we are within 100m of the fence, RTL
if( fence.get_breach_distance(new_breaches) <= AC_FENCE_GIVE_UP_DISTANCE) {
if (!set_mode(RTL)) {
set_mode(LAND);
}
}else{
// if more than 100m outside the fence just force a land
// if we are within 100m of the fence, RTL
if (fence.get_breach_distance(new_breaches) <= AC_FENCE_GIVE_UP_DISTANCE) {
if (!set_mode(RTL)) {
set_mode(LAND);
}
}else{
// we have no GPS so LAND
if(control_mode != LAND) {
set_mode(LAND);
}
// if more than 100m outside the fence just force a land
set_mode(LAND);
}
}
}

5
ArduCopter/navigation.pde
View File

@ -42,15 +42,12 @@ static void calc_distance_and_bearing()
}
// calculate home distance and bearing
if( ap.home_is_set && (g_gps->status() == GPS::GPS_OK_FIX_3D || g_gps->status() == GPS::GPS_OK_FIX_2D)) {
if(GPS_ok()) {
home_distance = pythagorous2(curr.x, curr.y);
home_bearing = pv_get_bearing_cd(curr,Vector3f(0,0,0));
// update super simple bearing (if required) because it relies on home_bearing
update_super_simple_bearing(false);
}else{
home_distance = 0;
home_bearing = 0;
}
}

2
ArduCopter/system.pde
View File

@ -291,7 +291,7 @@ static void startup_ground(bool force_gyro_cal)
// returns true if the GPS is ok and home position is set
static bool GPS_ok()
{
if (g_gps != NULL && ap.home_is_set && g_gps->status() == GPS::GPS_OK_FIX_3D) {
if (g_gps != NULL && ap.home_is_set && g_gps->status() == GPS::GPS_OK_FIX_3D && !gps_glitch.glitching() && !failsafe.gps) {
return true;
}else{
return false;