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

Plane: Auto Takeoff Check updates

Browse Source 
Adds additional launch angle protection and protection against early
exit from auto causing unexpected motor start on re-entry
This commit is contained in:
Paul Riseborough 2013-08-02 17:39:18 +10:00 committed by Andrew Tridgell
parent 4d2bfe1078
commit 43cf0736a3
1 changed files with 67 additions and 68 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

135
ArduPlane/Attitude.pde
View File

@ -434,85 +434,84 @@ static void throttle_slew_limit(int16_t last_throttle)
} }
/* /* Check for automatic takeoff conditions being met using the following sequence:
check for automatic takeoff conditions being met * 1) Check for adequate GPS lock - if not return false
* 2) Check the gravity compensated longitudinal acceleration against the threshold and start the timer if true
* 3) Wait until the timer has reached the specified value (increments of 0.1 sec) and then check the GPS speed against the threshold
* 4) If the GPS speed is above the threshold and the attitude is within limits then return true and reset the timer
* 5) If the GPS speed and attitude within limits has not been achieved after 2.5 seconds, return false and reset the timer
* 6) If the time lapsed since the last timecheck is greater than 0.2 seconds, return false and reset the timer
* NOTE : This function relies on the TECS 50Hz processing for its acceleration measure.
*/ */
static bool auto_takeoff_check(void) static bool auto_takeoff_check(void)
{ {
#if 1 // this is a more advanced check that relies on TECS
uint32_t now = hal.scheduler->millis();
static bool launchTimerStarted;
static uint32_t last_tkoff_arm_time;
static uint32_t last_check_ms;
// Reset states if process has been interrupted
if (last_check_ms && (now - last_check_ms) > 200) {
gcs_send_text_fmt(PSTR("Timer Interrupted AUTO"));
launchTimerStarted = false;
last_tkoff_arm_time = 0;
last_check_ms = now;
return false;
}
last_check_ms = now;
// Check for bad GPS
if (g_gps == NULL || g_gps->status() != GPS::GPS_OK_FIX_3D) { if (g_gps == NULL || g_gps->status() != GPS::GPS_OK_FIX_3D) {
// no auto takeoff without GPS lock // no auto takeoff without GPS lock
return false; return false;
} }
if (g_gps->ground_speed_cm < g.takeoff_throttle_min_speed*100.0f) {
// we haven't reached the minimum ground speed // Check for launch acceleration or timer started. NOTE: relies on TECS 50Hz processing
return false; if (!launchTimerStarted &&
g.takeoff_throttle_min_accel != 0.0 &&
SpdHgt_Controller->get_VXdot() < g.takeoff_throttle_min_accel) {
goto no_launch;
} }
if (g.takeoff_throttle_min_accel > 0.0f) { if (!launchTimerStarted) {
float xaccel = ins.get_accel().x; launchTimerStarted = true;
if (ahrs.pitch_sensor > -3000 &&
ahrs.pitch_sensor < 4500 &&
abs(ahrs.roll_sensor) < 3000 &&
xaccel >= g.takeoff_throttle_min_accel) {
// trigger with minimum acceleration when flat
// Thanks to Chris Miser for this suggestion
gcs_send_text_fmt(PSTR("Triggered AUTO xaccel=%.1f"), xaccel);
return true;
}
return false;
}
// we're good for takeoff
return true;
#else
// this is a more advanced check that relies on TECS
uint32_t now = hal.scheduler->micros();
static bool launchCountStarted;
static uint32_t last_tkoff_arm_time;
if (g_gps == NULL || g_gps->status() != GPS::GPS_OK_FIX_3D)
{
// no auto takeoff without GPS lock
return false;
}
if (SpdHgt_Controller->get_VXdot() >= g.takeoff_throttle_min_accel || g.takeoff_throttle_min_accel == 0.0 || launchCountStarted)
{
if (!launchCountStarted)
{
launchCountStarted = true;
last_tkoff_arm_time = now; last_tkoff_arm_time = now;
gcs_send_text_fmt(PSTR("Armed AUTO, xaccel = %.1f m/s/s, waiting %.1f sec"), SpdHgt_Controller->get_VXdot(), 0.1f*float(min(g.takeoff_throttle_delay,15))); gcs_send_text_fmt(PSTR("Armed AUTO, xaccel = %.1f m/s/s, waiting %.1f sec"),
} SpdHgt_Controller->get_VXdot(), 0.1f*float(min(g.takeoff_throttle_delay,25)));
if ((now - last_tkoff_arm_time) <= 2500000) }
{
// Only perform velocity check if not timed out
if ((g_gps->ground_speed > g.takeoff_throttle_min_speed*100.0f || g.takeoff_throttle_min_speed == 0.0) && ((now -last_tkoff_arm_time) >= min(uint32_t(g.takeoff_throttle_delay*100000),1500000))) if ((now - last_tkoff_arm_time) > 2500) {
{ gcs_send_text_fmt(PSTR("Timeout AUTO"));
gcs_send_text_fmt(PSTR("Triggered AUTO, GPSspd = %.1f"), g_gps->ground_speed*100.0f); goto no_launch;
launchCountStarted = false; }
last_tkoff_arm_time = 0;
return true; // Check aircraft attitude for bad launch
} if (ahrs.pitch_sensor <= -3000 ||
else ahrs.pitch_sensor >= 4500 ||
{ abs(ahrs.roll_sensor) > 3000) {
launchCountStarted = true; gcs_send_text_fmt(PSTR("Bad Launch AUTO"));
return false; goto no_launch;
} }
}
else // Check ground speed and time delay
{ if (((g_gps->ground_speed_cm > g.takeoff_throttle_min_speed*100.0f || g.takeoff_throttle_min_speed == 0.0)) &&
gcs_send_text_fmt(PSTR("Timeout AUTO")); ((now - last_tkoff_arm_time) >= min(uint16_t(g.takeoff_throttle_delay)*100,2500))) {
launchCountStarted = false; gcs_send_text_fmt(PSTR("Triggered AUTO, GPSspd = %.1f"), g_gps->ground_speed_cm*0.01f);
last_tkoff_arm_time = 0; launchTimerStarted = false;
return false; last_tkoff_arm_time = 0;
} return true;
} }
launchCountStarted = false;
last_tkoff_arm_time = 0; // we're not launching yet, but the timer is still going
return false;
no_launch:
launchTimerStarted = false;
last_tkoff_arm_time = 0;
return false; return false;
#endif
} }