diff --git a/ArduCopter/AP_State.pde b/ArduCopter/AP_State.pde index 63ccaa975e..fcba1c36c3 100644 --- a/ArduCopter/AP_State.pde +++ b/ArduCopter/AP_State.pde @@ -110,6 +110,21 @@ void set_land_complete(bool b) // --------------------------------------------- +// set land complete maybe flag +void set_land_complete_maybe(bool b) +{ + // if no change, exit immediately + if (ap.land_complete_maybe == b) + return; + + if (b) { + Log_Write_Event(DATA_LAND_COMPLETE_MAYBE); + } + ap.land_complete_maybe = b; +} + +// --------------------------------------------- + void set_pre_arm_check(bool b) { if(ap.pre_arm_check != b) { diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 17ac08e5f1..5dae5d8f4b 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -389,6 +389,7 @@ static union { uint8_t compass_mot : 1; // 15 // true if we are currently performing compassmot calibration uint8_t motor_test : 1; // 16 // true if we are currently performing the motors test uint8_t initialised : 1; // 17 // true once the init_ardupilot function has completed. Extended status to GCS is not sent until this completes + uint8_t land_complete_maybe : 1; // 18 // true if we may have landed (less strict version of land_complete) }; uint32_t value; } ap; diff --git a/ArduCopter/control_land.pde b/ArduCopter/control_land.pde index 7299e8fe58..14399be58c 100644 --- a/ArduCopter/control_land.pde +++ b/ArduCopter/control_land.pde @@ -1,7 +1,7 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- // counter to verify landings -static uint16_t land_detector; +static uint16_t land_detector = LAND_DETECTOR_TRIGGER; // we assume we are landed static bool land_with_gps; static uint32_t land_start_time; @@ -192,9 +192,10 @@ static float get_throttle_land() } } +// land_maybe_complete - return true if we may have landed (used to reset loiter targets during landing) static bool land_maybe_complete() { - return (ap.land_complete || land_detector > LAND_DETECTOR_MAYBE_TRIGGER); + return (ap.land_complete || ap.land_complete_maybe); } // update_land_detector - checks if we have landed and updates the ap.land_complete flag @@ -210,7 +211,7 @@ static void update_land_detector() land_detector++; }else{ set_land_complete(true); - land_detector = 0; + land_detector = LAND_DETECTOR_TRIGGER; } } } else { @@ -221,6 +222,9 @@ static void update_land_detector() set_land_complete(false); } } + + // set land maybe flag + set_land_complete_maybe(land_detector >= LAND_DETECTOR_MAYBE_TRIGGER); } // land_do_not_use_GPS - forces land-mode to not use the GPS but instead rely on pilot input for roll and pitch diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index d4855f68f1..7b82a46cd8 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -267,6 +267,7 @@ enum FlipState { #define DATA_DISARMED 11 #define DATA_AUTO_ARMED 15 #define DATA_TAKEOFF 16 +#define DATA_LAND_COMPLETE_MAYBE 17 #define DATA_LAND_COMPLETE 18 #define DATA_NOT_LANDED 28 #define DATA_LOST_GPS 19 diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index b7aee6b994..ac29fd4809 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -615,6 +615,7 @@ static void init_disarm_motors() // we are not in the air set_land_complete(true); + set_land_complete_maybe(true); // reset the mission mission.reset(); diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index 317bb91a71..4c3d3282c4 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -322,6 +322,7 @@ static void startup_ground(bool force_gyro_cal) // set landed flag set_land_complete(true); + set_land_complete_maybe(true); } // returns true if the GPS is ok and home position is set