Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-05 07:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: make takeoff less noisy with zero timeout

Browse Source
This commit is contained in:
Andrew Tridgell 2016-05-13 21:34:45 +10:00
parent e3107a7e10
commit 2a0476483b
2 changed files with 31 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
ArduPlane/Plane.h
View File

@ -406,6 +406,13 @@ private:
uint32_t lock_timer_ms; uint32_t lock_timer_ms;
} cruise_state; } cruise_state;
struct {
uint32_t last_tkoff_arm_time;
uint32_t last_check_ms;
uint32_t last_report_ms;
bool launchTimerStarted;
} takeoff_state;
// ground steering controller state // ground steering controller state
struct { struct {
// Direction held during phases of takeoff and landing centidegrees // Direction held during phases of takeoff and landing centidegrees

39
ArduPlane/takeoff.cpp
View File

@ -15,21 +15,18 @@ bool Plane::auto_takeoff_check(void)
{ {
// this is a more advanced check that relies on TECS // this is a more advanced check that relies on TECS
uint32_t now = millis(); uint32_t now = millis();
static bool launchTimerStarted;
static uint32_t last_tkoff_arm_time;
static uint32_t last_check_ms;
uint16_t wait_time_ms = MIN(uint16_t(g.takeoff_throttle_delay)*100,12700); uint16_t wait_time_ms = MIN(uint16_t(g.takeoff_throttle_delay)*100,12700);
// Reset states if process has been interrupted // Reset states if process has been interrupted
if (last_check_ms && (now - last_check_ms) > 200) { if (takeoff_state.last_check_ms && (now - takeoff_state.last_check_ms) > 200) {
gcs_send_text_fmt(MAV_SEVERITY_WARNING, "Timer interrupted AUTO"); gcs_send_text_fmt(MAV_SEVERITY_WARNING, "Timer interrupted AUTO");
launchTimerStarted = false; takeoff_state.launchTimerStarted = false;
last_tkoff_arm_time = 0; takeoff_state.last_tkoff_arm_time = 0;
last_check_ms = now; takeoff_state.last_check_ms = now;
return false; return false;
} }
last_check_ms = now; takeoff_state.last_check_ms = now;
// Check for bad GPS // Check for bad GPS
if (gps.status() < AP_GPS::GPS_OK_FIX_3D) { if (gps.status() < AP_GPS::GPS_OK_FIX_3D) {
@ -38,23 +35,29 @@ bool Plane::auto_takeoff_check(void)
} }
// Check for launch acceleration or timer started. NOTE: relies on TECS 50Hz processing // Check for launch acceleration or timer started. NOTE: relies on TECS 50Hz processing
if (!launchTimerStarted && if (!takeoff_state.launchTimerStarted &&
!is_zero(g.takeoff_throttle_min_accel) && !is_zero(g.takeoff_throttle_min_accel) &&
SpdHgt_Controller->get_VXdot() < g.takeoff_throttle_min_accel) { SpdHgt_Controller->get_VXdot() < g.takeoff_throttle_min_accel) {
goto no_launch; goto no_launch;
} }
// we've reached the acceleration threshold, so start the timer // we've reached the acceleration threshold, so start the timer
if (!launchTimerStarted) { if (!takeoff_state.launchTimerStarted) {
launchTimerStarted = true; takeoff_state.launchTimerStarted = true;
last_tkoff_arm_time = now; takeoff_state.last_tkoff_arm_time = now;
if (now - takeoff_state.last_report_ms > 2000) {
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Armed AUTO, xaccel = %.1f m/s/s, waiting %.1f sec", gcs_send_text_fmt(MAV_SEVERITY_INFO, "Armed AUTO, xaccel = %.1f m/s/s, waiting %.1f sec",
(double)SpdHgt_Controller->get_VXdot(), (double)(wait_time_ms*0.001f)); (double)SpdHgt_Controller->get_VXdot(), (double)(wait_time_ms*0.001f));
takeoff_state.last_report_ms = now;
}
} }
// Only perform velocity check if not timed out // Only perform velocity check if not timed out
if ((now - last_tkoff_arm_time) > wait_time_ms+100U) { if ((now - takeoff_state.last_tkoff_arm_time) > wait_time_ms+100U) {
if (now - takeoff_state.last_report_ms > 2000) {
gcs_send_text_fmt(MAV_SEVERITY_WARNING, "Timeout AUTO"); gcs_send_text_fmt(MAV_SEVERITY_WARNING, "Timeout AUTO");
takeoff_state.last_report_ms = now;
}
goto no_launch; goto no_launch;
} }
@ -68,10 +71,10 @@ bool Plane::auto_takeoff_check(void)
// Check ground speed and time delay // Check ground speed and time delay
if (((gps.ground_speed() > g.takeoff_throttle_min_speed || is_zero(g.takeoff_throttle_min_speed))) && if (((gps.ground_speed() > g.takeoff_throttle_min_speed || is_zero(g.takeoff_throttle_min_speed))) &&
((now - last_tkoff_arm_time) >= wait_time_ms)) { ((now - takeoff_state.last_tkoff_arm_time) >= wait_time_ms)) {
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Triggered AUTO. GPS speed = %.1f", (double)gps.ground_speed()); gcs_send_text_fmt(MAV_SEVERITY_INFO, "Triggered AUTO. GPS speed = %.1f", (double)gps.ground_speed());
launchTimerStarted = false; takeoff_state.launchTimerStarted = false;
last_tkoff_arm_time = 0; takeoff_state.last_tkoff_arm_time = 0;
steer_state.locked_course_err = 0; // use current heading without any error offset steer_state.locked_course_err = 0; // use current heading without any error offset
return true; return true;
} }
@ -80,8 +83,8 @@ bool Plane::auto_takeoff_check(void)
return false; return false;
no_launch: no_launch:
launchTimerStarted = false; takeoff_state.launchTimerStarted = false;
last_tkoff_arm_time = 0; takeoff_state.last_tkoff_arm_time = 0;
return false; return false;
} }