Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

updated climb rate control 

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

53
ArduCopter/commands_logic.pde
View File

@ -199,7 +199,7 @@ static void do_RTL(void)
//so we know where we are navigating from
// --------------------------------------
next_WP = current_loc;
next_WP = current_loc;
// Loads WP from Memory
// --------------------
@ -265,19 +265,13 @@ static void do_land()
{
wp_control = LOITER_MODE;
//Serial.println("dlnd ");
// not really used right now, might be good for debugging
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
set_next_WP(&current_loc);
// Set a new target altitude
set_new_altitude(0);
}
static void do_loiter_unlimited()
@ -339,52 +333,43 @@ static bool verify_takeoff()
return false;
}
// 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 int32_t old_alt = 0;
static int16_t velocity_land = -1;
// land at .62 meter per second
next_WP.alt = original_alt - ((millis() - land_start) / 32); // condition_value = our initial
if (old_alt == 0)
// landing detector
if ((current_loc.alt - home.alt) > 300){
old_alt = current_loc.alt;
if (velocity_land == -1)
velocity_land = 2000;
if ((current_loc.alt - home.alt) < 300){
}else{
// 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
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
old_alt = current_loc.alt;
if ((current_loc.alt - home.alt) < 200){
// don't bank to hold position
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;
// reset manual_boost hack
manual_boost = 0;
// reset old_alt
old_alt = 0;
return false;
return true;
}
return false;
}
@ -394,7 +379,9 @@ static bool verify_nav_wp()
// Altitude checking
if(next_WP.options & MASK_OPTIONS_RELATIVE_ALT){
// 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
wp_verify_byte |= NAV_ALTITUDE;
}
@ -592,7 +579,7 @@ static bool verify_wait_delay()
static bool verify_change_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
if(current_loc.alt > next_WP.alt){
return true;