aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Copter: soften takeoff

Browse Source
This commit is contained in:
Jonathan Challinger 2015-10-19 17:56:49 -07:00 committed by Randy Mackay
parent 75d2aed6e1
commit 082c1f77fb
2 changed files with 22 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduCopter/Copter.h
View File

@ -260,9 +260,9 @@ private:
struct {
bool running;
float speed;
float max_speed;
float alt_delta;
uint32_t start_ms;
uint32_t time_ms;
} takeoff_state;
RCMapper rcmap;

28
ArduCopter/takeoff.cpp
View File

@ -66,9 +66,9 @@ void Copter::takeoff_timer_start(float alt_cm)
// initialise takeoff state
takeoff_state.running = true;
takeoff_state.speed = speed;
takeoff_state.max_speed = speed;
takeoff_state.start_ms = millis();
takeoff_state.time_ms = (alt_cm/takeoff_state.speed) * 1.0e3f;
takeoff_state.alt_delta = alt_cm;
}
// stop takeoff
@ -90,21 +90,33 @@ void Copter::takeoff_get_climb_rates(float& pilot_climb_rate, float& takeoff_cli
return;
}
// check if timeout as expired
if ((millis()-takeoff_state.start_ms) >= takeoff_state.time_ms) {
float takeoff_minspeed = min(50.0f,takeoff_state.max_speed);
static const float takeoff_accel = 50.0f;
float time_elapsed = (millis()-takeoff_state.start_ms)*1.0e-3f;
float speed = min(time_elapsed*takeoff_accel+takeoff_minspeed, takeoff_state.max_speed);
float time_to_max_speed = (takeoff_state.max_speed-takeoff_minspeed)/takeoff_accel;
float height_gained;
if (time_elapsed <= time_to_max_speed) {
height_gained = 0.5f*takeoff_accel*sq(time_elapsed) + takeoff_minspeed*time_elapsed;
} else {
height_gained = 0.5f*takeoff_accel*sq(time_to_max_speed) + takeoff_minspeed*time_to_max_speed +
(time_elapsed-time_to_max_speed)*takeoff_state.max_speed;
}
// check if the takeoff is over
if (height_gained >= takeoff_state.alt_delta) {
takeoff_stop();
takeoff_climb_rate = 0.0f;
return;
}
// if takeoff climb rate is zero return
if (takeoff_state.speed <= 0.0f) {
if (speed <= 0.0f) {
takeoff_climb_rate = 0.0f;
return;
}
// default take-off climb rate to maximum speed
takeoff_climb_rate = takeoff_state.speed;
takeoff_climb_rate = speed;
// if pilot's commands descent
if (pilot_climb_rate < 0.0f) {