Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-21 15:23:57 -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
/* Check for automatic takeoff conditions being met using the following sequence:
* 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)
{
#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) {
// no auto takeoff without GPS lock
return false;
}
if (g_gps->ground_speed_cm < g.takeoff_throttle_min_speed*100.0f) {
// we haven't reached the minimum ground speed
return false;
// Check for launch acceleration or timer started. NOTE: relies on TECS 50Hz processing
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) {
float xaccel = ins.get_accel().x;
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;
if (!launchTimerStarted) {
launchTimerStarted = true;
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)));
}
if ((now - last_tkoff_arm_time) <= 2500000)
{
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)))
{
gcs_send_text_fmt(PSTR("Triggered AUTO, GPSspd = %.1f"), g_gps->ground_speed*100.0f);
launchCountStarted = false;
last_tkoff_arm_time = 0;
return true;
}
else
{
launchCountStarted = true;
return false;
}
}
else
{
gcs_send_text_fmt(PSTR("Timeout AUTO"));
launchCountStarted = false;
last_tkoff_arm_time = 0;
return false;
}
}
launchCountStarted = false;
last_tkoff_arm_time = 0;
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)));
}
// Only perform velocity check if not timed out
if ((now - last_tkoff_arm_time) > 2500) {
gcs_send_text_fmt(PSTR("Timeout AUTO"));
goto no_launch;
}
// Check aircraft attitude for bad launch
if (ahrs.pitch_sensor <= -3000 ||
ahrs.pitch_sensor >= 4500 ||
abs(ahrs.roll_sensor) > 3000) {
gcs_send_text_fmt(PSTR("Bad Launch AUTO"));
goto no_launch;
}
// 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)) &&
((now - last_tkoff_arm_time) >= min(uint16_t(g.takeoff_throttle_delay)*100,2500))) {
gcs_send_text_fmt(PSTR("Triggered AUTO, GPSspd = %.1f"), g_gps->ground_speed_cm*0.01f);
launchTimerStarted = false;
last_tkoff_arm_time = 0;
return true;
}
// we're not launching yet, but the timer is still going
return false;
no_launch:
launchTimerStarted = false;
last_tkoff_arm_time = 0;
return false;
#endif
}