Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-06 16:03:58 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: reset loiter target when landed

Browse Source
This commit is contained in:
Randy Mackay 2013-07-26 14:15:27 +09:00
parent fe412437c2
commit c3daf78340
6 changed files with 113 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/AP_State.pde
View File

@ -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;
}

89
ArduCopter/ArduCopter.pde
View File

@ -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;

53
ArduCopter/Attitude.pde
View File

@ -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

1
ArduCopter/defines.h
View File

@ -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

4
ArduCopter/navigation.pde
View File

@ -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;

4
ArduCopter/system.pde
View File

@ -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;