trying to avoid I term build up on an unarmed copter prepping to take off in Auto mode.

git-svn-id: https://arducopter.googlecode.com/svn/trunk@3292 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2011-09-07 04:44:01 +00:00
parent ff7821c87a
commit c152e364a8
3 changed files with 20 additions and 14 deletions

View File

@ -1199,7 +1199,7 @@ void update_throttle_mode(void)
// fall through
case THROTTLE_AUTO:
if(invalid_throttle){
if(invalid_throttle && motor_auto_armed == true){
// get the AP throttle
nav_throttle = get_nav_throttle(altitude_error, 200); //150 = target speed of 1.5m/s

View File

@ -14,8 +14,21 @@ static void init_commands()
clear_command_queue();
}
// forces the loading of a new command
// queue is emptied after a new command is processed
static void clear_command_queue(){
next_command.id = NO_COMMAND;
}
static void init_auto()
{
#if HIL_MODE != HIL_MODE_ATTITUDE
// read Baro pressure at ground -
// this resets Baro for more accuracy
//-----------------------------------
init_barometer();
#endif
//if (g.waypoint_index == g.waypoint_total) {
// Serial.println("ia_f");
// do_RTL();
@ -26,12 +39,6 @@ static void init_auto()
init_commands();
}
// forces the loading of a new command
// queue is emptied after a new command is processed
static void clear_command_queue(){
next_command.id = NO_COMMAND;
}
// Getters
// -------
static struct Location get_command_with_index(int i)

View File

@ -218,21 +218,22 @@ static void do_RTL(void)
static void do_takeoff()
{
wp_control = LOITER_MODE;
wp_control = LOITER_MODE;
// Start with current location
Location temp = current_loc;
Location temp = current_loc;
// next_command.alt is a relative altitude!!!
if (next_command.options & WP_OPTION_ALT_RELATIVE) {
temp.alt = next_command.alt + home.alt;
temp.alt = next_command.alt + home.alt;
//Serial.printf("rel alt: %ld",temp.alt);
} else {
temp.alt = next_command.alt;
temp.alt = next_command.alt;
//Serial.printf("abs alt: %ld",temp.alt);
}
takeoff_complete = false; // set flag to use g_gps ground course during TO. IMU will be doing yaw drift correction
takeoff_complete = false;
// set flag to use g_gps ground course during TO. IMU will be doing yaw drift correction
// Set our waypoint
set_next_WP(&temp);
@ -334,8 +335,6 @@ static bool verify_takeoff()
return false;
}
//Serial.printf("vt c_alt:%ld, n_alt:%ld\n", current_loc.alt, next_WP.alt);
if (current_loc.alt > next_WP.alt){
//Serial.println("Y");
takeoff_complete = true;