Add code to disable throttle if we are on the ground and in FBW_B or higher

Add code to disable throttle if we are on the ground and in FBW_B or higher.  We believe we are on the ground if speed < 5 and alt < 5.  Also check that we are not trying to perform a takeoff.
This commit is contained in:
Doug Weibel 2011-09-27 21:14:42 -06:00
parent 016dd0bcc7
commit aac8eac0cb
2 changed files with 12 additions and 0 deletions

View File

@ -302,6 +302,17 @@ static void set_servos(void)
#else #else
// convert 0 to 100% into PWM // convert 0 to 100% into PWM
g.channel_throttle.servo_out = constrain(g.channel_throttle.servo_out, g.throttle_min.get(), g.throttle_max.get()); g.channel_throttle.servo_out = constrain(g.channel_throttle.servo_out, g.throttle_min.get(), g.throttle_max.get());
if (g.airspeed_enabled == true) {
// We want to supress the throttle if we think we are on the ground and in an autopilot controlled throttle mode.
if((control_mode==Circle || control_mode>=FBW_B) && current_loc.alt-home.alt < 500 && airspeed< 500) {
if(!(control_mode==Auto && takeoff_complete == false) g.channel_throttle.servo_out = 0;
}
} else {
if((control_mode==Circle || control_mode>=FBW_B) && current_loc.alt-home.alt < 500 && g_gps->ground_speed < 500) {
if(!(control_mode==Auto && takeoff_complete == false) g.channel_throttle.servo_out = 0;
}
}
#endif #endif
g.channel_throttle.calc_pwm(); g.channel_throttle.calc_pwm();

View File

@ -222,6 +222,7 @@ static void do_takeoff()
next_WP.lat = home.lat + 1000; // so we don't have bad calcs next_WP.lat = home.lat + 1000; // so we don't have bad calcs
next_WP.lng = home.lng + 1000; // so we don't have bad calcs next_WP.lng = home.lng + 1000; // so we don't have bad calcs
takeoff_complete = false; // set flag to use gps ground course during TO. IMU will be doing yaw drift correction takeoff_complete = false; // set flag to use gps ground course during TO. IMU will be doing yaw drift correction
// Flag also used to override "on the ground" throttle disable
} }
static void do_nav_wp() static void do_nav_wp()