From 64204de79f415c2ba1e68ea169f4c3d1883e81ee Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Tue, 25 Jun 2013 22:33:47 +0900
Subject: [PATCH] Copter: LAND only control horizontal position if we have GPS
lock
---
ArduCopter/commands_logic.pde | 19 ++++++++++++++-----
ArduCopter/system.pde | 2 +-
2 files changed, 15 insertions(+), 6 deletions(-)
diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde
index bd594a9f1..c45fcc1d0 100644
--- a/ArduCopter/commands_logic.pde
+++ b/ArduCopter/commands_logic.pde
@@ -323,9 +323,20 @@ static void do_land(const struct Location *cmd)
// set landing state
land_state = LAND_STATE_DESCENDING;
- // switch to loiter which restores horizontal control to pilot
- // To-Do: check that we are not in failsafe to ensure we don't process bad roll-pitch commands
- set_roll_pitch_mode(ROLL_PITCH_LOITER);
+ // if we have gps lock, attempt to hold horizontal position
+ if( ap.home_is_set && g_gps->status() == GPS::GPS_OK_FIX_3D ) {
+ // switch to loiter which restores horizontal control to pilot
+ // To-Do: check that we are not in failsafe to ensure we don't process bad roll-pitch commands
+ set_roll_pitch_mode(ROLL_PITCH_LOITER);
+ // switch into loiter nav mode
+ set_nav_mode(NAV_LOITER);
+ }else{
+ // no gps lock so give horizontal control to pilot
+ // To-Do: check that we are not in failsafe to ensure we don't process bad roll-pitch commands
+ set_roll_pitch_mode(ROLL_PITCH_STABLE);
+ // switch into loiter nav mode
+ set_nav_mode(NAV_NONE);
+ }
// hold yaw while landing
set_yaw_mode(YAW_HOLD);
@@ -333,8 +344,6 @@ static void do_land(const struct Location *cmd)
// set throttle mode to land
set_throttle_mode(THROTTLE_LAND);
- // switch into loiter nav mode
- set_nav_mode(NAV_LOITER);
}
}
diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde
index 9759405d4..63767f644 100644
--- a/ArduCopter/system.pde
+++ b/ArduCopter/system.pde
@@ -434,7 +434,7 @@ static void set_mode(uint8_t mode)
case LAND:
// To-Do: it is messy to set manual_attitude here because the do_land function is reponsible for setting the roll_pitch_mode
- if( g_gps->status() == GPS::GPS_OK_FIX_3D ) {
+ if( ap.home_is_set && g_gps->status() == GPS::GPS_OK_FIX_3D ) {
// switch to loiter if we have gps
ap.manual_attitude = false;
}else{
--
GitLab