diff --git a/ArduCopter/AP_State.pde b/ArduCopter/AP_State.pde index 1abfda3b8b..2902352f9d 100644 --- a/ArduCopter/AP_State.pde +++ b/ArduCopter/AP_State.pde @@ -104,6 +104,8 @@ void set_land_complete(bool b) if(b){ Log_Write_Event(DATA_LAND_COMPLETE); + }else{ + Log_Write_Event(DATA_NOT_LANDED); } ap.land_complete = b; } diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 80f2496a28..04f9e41a87 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -378,7 +378,7 @@ static union { uint8_t rc_override_active : 1; // 14 // true if rc control are overwritten by ground station uint8_t do_flip : 1; // 15 // Used to enable flip code uint8_t takeoff_complete : 1; // 16 - uint8_t land_complete : 1; // 17 + uint8_t land_complete : 1; // 17 // true if we have detected a landing uint8_t compass_status : 1; // 18 uint8_t gps_status : 1; // 19 }; @@ -1035,6 +1035,9 @@ static void fifty_hz_loop() // ------------------------- update_throttle_mode(); + // check if we've landed + update_land_detector(); + #if TOY_EDF == ENABLED edf_toy(); #endif @@ -1458,6 +1461,10 @@ void update_yaw_mode(void) switch(yaw_mode) { case YAW_HOLD: + // if we are landed reset yaw target to current heading + if (ap.land_complete) { + nav_yaw = ahrs.yaw_sensor; + } // heading hold at heading held in nav_yaw but allow input from pilot get_yaw_rate_stabilized_ef(g.rc_4.control_in); break; @@ -1472,9 +1479,14 @@ void update_yaw_mode(void) break; case YAW_LOOK_AT_NEXT_WP: - // point towards next waypoint (no pilot input accepted) - // we don't use wp_bearing because we don't want the copter to turn too much during flight - nav_yaw = get_yaw_slew(nav_yaw, original_wp_bearing, AUTO_YAW_SLEW_RATE); + // if we are landed reset yaw target to current heading + if (ap.land_complete) { + nav_yaw = ahrs.yaw_sensor; + }else{ + // point towards next waypoint (no pilot input accepted) + // we don't use wp_bearing because we don't want the copter to turn too much during flight + nav_yaw = get_yaw_slew(nav_yaw, original_wp_bearing, AUTO_YAW_SLEW_RATE); + } get_stabilize_yaw(nav_yaw); // if there is any pilot input, switch to YAW_HOLD mode for the next iteration @@ -1484,6 +1496,10 @@ void update_yaw_mode(void) break; case YAW_LOOK_AT_LOCATION: + // if we are landed reset yaw target to current heading + if (ap.land_complete) { + nav_yaw = ahrs.yaw_sensor; + } // point towards a location held in yaw_look_at_WP get_look_at_yaw(); @@ -1494,6 +1510,10 @@ void update_yaw_mode(void) break; case YAW_CIRCLE: + // if we are landed reset yaw target to current heading + if (ap.land_complete) { + nav_yaw = ahrs.yaw_sensor; + } // points toward the center of the circle or does a panorama get_circle_yaw(); @@ -1504,8 +1524,13 @@ void update_yaw_mode(void) break; case YAW_LOOK_AT_HOME: - // keep heading always pointing at home with no pilot input allowed - nav_yaw = get_yaw_slew(nav_yaw, home_bearing, AUTO_YAW_SLEW_RATE); + // if we are landed reset yaw target to current heading + if (ap.land_complete) { + nav_yaw = ahrs.yaw_sensor; + }else{ + // keep heading always pointing at home with no pilot input allowed + nav_yaw = get_yaw_slew(nav_yaw, home_bearing, AUTO_YAW_SLEW_RATE); + } get_stabilize_yaw(nav_yaw); // if there is any pilot input, switch to YAW_HOLD mode for the next iteration @@ -1515,28 +1540,47 @@ void update_yaw_mode(void) break; case YAW_LOOK_AT_HEADING: - // keep heading pointing in the direction held in yaw_look_at_heading with no pilot input allowed - nav_yaw = get_yaw_slew(nav_yaw, yaw_look_at_heading, yaw_look_at_heading_slew); + // if we are landed reset yaw target to current heading + if (ap.land_complete) { + nav_yaw = ahrs.yaw_sensor; + }else{ + // keep heading pointing in the direction held in yaw_look_at_heading with no pilot input allowed + nav_yaw = get_yaw_slew(nav_yaw, yaw_look_at_heading, yaw_look_at_heading_slew); + } get_stabilize_yaw(nav_yaw); break; case YAW_LOOK_AHEAD: + // if we are landed reset yaw target to current heading + if (ap.land_complete) { + nav_yaw = ahrs.yaw_sensor; + } // Commanded Yaw to automatically look ahead. get_look_ahead_yaw(g.rc_4.control_in); break; #if TOY_LOOKUP == TOY_EXTERNAL_MIXER case YAW_TOY: - // update to allow external roll/yaw mixing - // keep heading always pointing at home with no pilot input allowed - nav_yaw = get_yaw_slew(nav_yaw, home_bearing, AUTO_YAW_SLEW_RATE); + // if we are landed reset yaw target to current heading + if (ap.land_complete) { + nav_yaw = ahrs.yaw_sensor; + }else{ + // update to allow external roll/yaw mixing + // keep heading always pointing at home with no pilot input allowed + nav_yaw = get_yaw_slew(nav_yaw, home_bearing, AUTO_YAW_SLEW_RATE); + } get_stabilize_yaw(nav_yaw); break; #endif case YAW_RESETTOARMEDYAW: - // changes yaw to be same as when quad was armed - nav_yaw = get_yaw_slew(nav_yaw, initial_armed_bearing, AUTO_YAW_SLEW_RATE); + // if we are landed reset yaw target to current heading + if (ap.land_complete) { + nav_yaw = ahrs.yaw_sensor; + }else{ + // changes yaw to be same as when quad was armed + nav_yaw = get_yaw_slew(nav_yaw, initial_armed_bearing, AUTO_YAW_SLEW_RATE); + } get_stabilize_yaw(nav_yaw); // if there is any pilot input, switch to YAW_HOLD mode for the next iteration @@ -1706,12 +1750,18 @@ void update_roll_pitch_mode(void) control_roll = g.rc_1.control_in; control_pitch = g.rc_2.control_in; - // update loiter target from user controls - max velocity is 5.0 m/s - wp_nav.move_loiter_target(control_roll, control_pitch,0.01f); + // if landed simply keep the copter level + if (ap.land_complete) { + nav_roll = 0; + nav_pitch = 0; + }else{ + // update loiter target from user controls + wp_nav.move_loiter_target(control_roll, control_pitch,0.01f); - // copy latest output from nav controller to stabilize controller - nav_roll = wp_nav.get_desired_roll(); - nav_pitch = wp_nav.get_desired_pitch(); + // copy latest output from nav controller to stabilize controller + nav_roll = wp_nav.get_desired_roll(); + nav_pitch = wp_nav.get_desired_pitch(); + } get_stabilize_roll(nav_roll); get_stabilize_pitch(nav_pitch); break; @@ -1806,8 +1856,7 @@ bool set_throttle_mode( uint8_t new_throttle_mode ) break; case THROTTLE_LAND: - set_land_complete(false); // mark landing as incomplete - land_detector = 0; // A counter that goes up if our climb rate stalls out. + reset_land_detector(); // initialise land detector controller_desired_alt = get_initial_alt_hold(current_loc.alt, climb_rate); // reset controller desired altitude to current altitude throttle_initialised = true; break; diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index 4842b3acab..d8618cdfe8 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -891,7 +891,7 @@ get_throttle_accel(int16_t z_target_accel) i = g.pid_throttle_accel.get_integrator(); // update i term as long as we haven't breached the limits or the I term will certainly reduce - if ((!motors.limit.throttle_lower && !motors.limit.throttle_lower) || (i>0&&z_accel_error<0) || (i<0&&z_accel_error>0)) { + if ((!motors.limit.throttle_lower && !motors.limit.throttle_upper) || (i>0&&z_accel_error<0) || (i<0&&z_accel_error>0)) { i = g.pid_throttle_accel.get_i(z_accel_error, .01f); } @@ -1150,25 +1150,46 @@ get_throttle_land() }else{ get_throttle_rate_stabilized(-abs(g.land_speed)); - // detect whether we have landed by watching for minimum throttle and now movement - if (abs(climb_rate) < 20 && (g.rc_3.servo_out <= get_angle_boost(g.throttle_min) || g.pid_throttle_accel.get_integrator() <= -150)) { - if( land_detector < LAND_DETECTOR_TRIGGER ) { - land_detector++; - }else{ - set_land_complete(true); - if( g.rc_3.control_in == 0 || ap.failsafe_radio ) { - init_disarm_motors(); - } - } - }else{ - // we've sensed movement up or down so decrease land_detector - if (land_detector > 0 ) { - land_detector--; - } + // disarm when the landing detector says we've landed and throttle is at min (or we're in failsafe so we have no pilot thorottle input) + if( ap.land_complete && (g.rc_3.control_in == 0 || ap.failsafe_radio) ) { + init_disarm_motors(); } } } +// reset_land_detector - initialises land detector +static void reset_land_detector() +{ + set_land_complete(false); + land_detector = 0; +} + +// update_land_detector - checks if we have landed and updates the ap.land_complete flag +// returns true if we have landed +static bool update_land_detector() +{ + // detect whether we have landed by watching for low climb rate and minimum throttle + if (abs(climb_rate) < 20 && motors.limit.throttle_lower) { + // run throttle controller if accel based throttle controller is enabled and active (active means it has been given a target) + if( land_detector < LAND_DETECTOR_TRIGGER ) { + land_detector++; + }else{ + if(!ap.land_complete) { + set_land_complete(true); + } + } + }else{ + // we've sensed movement up or down so reset land_detector + land_detector = 0; + if(ap.land_complete) { + set_land_complete(false); + } + } + + // return current state of landing + return ap.land_complete; +} + // get_throttle_surface_tracking - hold copter at the desired distance above the ground // updates accel based throttle controller targets static void diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index a5c9bd95c5..32450d8d56 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -307,6 +307,7 @@ enum ap_message { #define DATA_AUTO_ARMED 15 #define DATA_TAKEOFF 16 #define DATA_LAND_COMPLETE 18 +#define DATA_NOT_LANDED 28 #define DATA_LOST_GPS 19 #define DATA_BEGIN_FLIP 21 #define DATA_END_FLIP 22 diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index 19ee14b133..20a18da74d 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -159,6 +159,10 @@ static void update_nav_mode() break; case NAV_LOITER: + // reset target if we are still on the ground + if (ap.land_complete) { + wp_nav.init_loiter_target(inertial_nav.get_position(),inertial_nav.get_velocity()); + } // call loiter controller wp_nav.update_loiter(); break; diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index 0177168f8a..19b8f653aa 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -347,10 +347,6 @@ static bool set_mode(uint8_t mode) // boolean to record if flight mode could be set bool success = false; - // if we change modes, we must clear landed flag - // To-Do: this should be initialised in one of the flight modes - set_land_complete(false); - // report the GPS and Motor arming status // To-Do: this should be initialised somewhere else related to the LEDs led_mode = NORMAL_LEDS;