Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 10:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

updated climb rate control

Browse Source 
Land detector
This commit is contained in:
Jason Short 2012-01-10 23:33:49 -08:00
parent acd877abda
commit 3a5ffc252d
1 changed files with 20 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

51
ArduCopter/commands_logic.pde
View File

@ -265,19 +265,13 @@ static void do_land()
{ {
wp_control = LOITER_MODE; wp_control = LOITER_MODE;
//Serial.println("dlnd ");
// not really used right now, might be good for debugging
land_complete = false; land_complete = false;
// used to limit decent rate
land_start = millis();
// used to limit decent rate
original_alt = current_loc.alt;
// hold at our current location // hold at our current location
set_next_WP(&current_loc); set_next_WP(&current_loc);
// Set a new target altitude
set_new_altitude(0);
} }
static void do_loiter_unlimited() static void do_loiter_unlimited()
@ -339,52 +333,43 @@ static bool verify_takeoff()
return false; return false;
} }
// are we above our target altitude? // are we above our target altitude?
return (current_loc.alt > next_WP.alt); //return (current_loc.alt > next_WP.alt);
return (current_loc.alt > target_altitude);
} }
static bool verify_land() static bool verify_land()
{ {
static int32_t old_alt = 0; static int32_t old_alt = 0;
static int16_t velocity_land = -1; static int16_t velocity_land = -1;
// land at .62 meter per second // landing detector
next_WP.alt = original_alt - ((millis() - land_start) / 32); // condition_value = our initial if ((current_loc.alt - home.alt) > 300){
if (old_alt == 0)
old_alt = current_loc.alt; old_alt = current_loc.alt;
if (velocity_land == -1)
velocity_land = 2000; velocity_land = 2000;
}else{
if ((current_loc.alt - home.alt) < 300){
// a LP filter used to tell if we have landed // a LP filter used to tell if we have landed
// will drive to 0 if we are on the ground - maybe, the baro is noisy // will drive to 0 if we are on the ground - maybe, the baro is noisy
velocity_land = ((velocity_land * 7) + (old_alt - current_loc.alt)) / 8; int16_t delta = (old_alt - current_loc.alt) << 3;
velocity_land = ((velocity_land * 7) + delta ) / 8;
} }
//Serial.printf("velocity_land %d \n", velocity_land);
// remenber altitude for climb_rate // remenber altitude for climb_rate
old_alt = current_loc.alt; old_alt = current_loc.alt;
if ((current_loc.alt - home.alt) < 200){ if ((current_loc.alt - home.alt) < 200){
// don't bank to hold position // don't bank to hold position
wp_control = NO_NAV_MODE; wp_control = NO_NAV_MODE;
// Update by JLN for a safe AUTO landing
manual_boost = -10;
g.throttle_cruise += g.pi_alt_hold.get_integrator();
g.pi_alt_hold.reset_I();
g.pi_throttle.reset_I();
} }
if((current_loc.alt - home.alt) < 100 && velocity_land <= 100){ if((current_loc.alt - home.alt) < 100 && velocity_land <= 50){
land_complete = true; land_complete = true;
// reset manual_boost hack
manual_boost = 0;
// reset old_alt // reset old_alt
old_alt = 0; old_alt = 0;
return false; return true;
} }
return false; return false;
} }
@ -394,7 +379,9 @@ static bool verify_nav_wp()
// Altitude checking // Altitude checking
if(next_WP.options & MASK_OPTIONS_RELATIVE_ALT){ if(next_WP.options & MASK_OPTIONS_RELATIVE_ALT){
// we desire a certain minimum altitude // we desire a certain minimum altitude
if (current_loc.alt > next_WP.alt){ //if (current_loc.alt > next_WP.alt){
if (current_loc.alt > target_altitude){
// we have reached that altitude // we have reached that altitude
wp_verify_byte |= NAV_ALTITUDE; wp_verify_byte |= NAV_ALTITUDE;
} }
@ -592,7 +579,7 @@ static bool verify_wait_delay()
static bool verify_change_alt() static bool verify_change_alt()
{ {
//Serial.printf("change_alt, ca:%d, na:%d\n", (int)current_loc.alt, (int)next_WP.alt); //Serial.printf("change_alt, ca:%d, na:%d\n", (int)current_loc.alt, (int)next_WP.alt);
if (condition_start < next_WP.alt){ if ((int32_t)condition_start < next_WP.alt){
// we are going higer // we are going higer
if(current_loc.alt > next_WP.alt){ if(current_loc.alt > next_WP.alt){
return true; return true;