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

Plane: fixed climb rate for quadplanes in CRUISE

Browse Source 
the high loop rate of quadplanes led to less than 1cm/loop change in
height, which got truncated to zero. Adjusting height at 10Hz fixes
that.

Thanks to Marco for reporting this!
This commit is contained in:
Andrew Tridgell 2017-05-09 18:05:36 +10:00
parent f870508b06
commit 92f88e9b33
2 changed files with 32 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
ArduPlane/Plane.h
View File

@ -712,6 +712,12 @@ private:
// lookahead value for height error reporting
float lookahead;
#endif
// last input for FBWB/CRUISE height control
float last_elevator_input;
// last time we checked for pilot control of height
uint32_t last_elev_check_us;
} target_altitude {};
float relative_altitude = 0.0f;

43
ArduPlane/navigation.cpp
View File

@ -248,29 +248,38 @@ void Plane::update_cruise()
*/
void Plane::update_fbwb_speed_height(void)
{
static float last_elevator_input;
float elevator_input;
elevator_input = channel_pitch->get_control_in() / 4500.0f;
if (g.flybywire_elev_reverse) {
elevator_input = -elevator_input;
}
change_target_altitude(g.flybywire_climb_rate * elevator_input * perf.delta_us_fast_loop * 0.0001f);
if (is_zero(elevator_input) && !is_zero(last_elevator_input)) {
// the user has just released the elevator, lock in
// the current altitude
set_target_altitude_current();
}
uint32_t now = micros();
if (now - target_altitude.last_elev_check_us >= 100000) {
// we don't run this on every loop as it would give too small granularity on quadplanes at 300Hz, and
// give below 1cm altitude change, which would result in no climb or descent
float dt = (now - target_altitude.last_elev_check_us) * 1.0e-6;
dt = constrain_float(dt, 0.1, 0.15);
target_altitude.last_elev_check_us = now;
float elevator_input = channel_pitch->get_control_in() / 4500.0f;
if (g.flybywire_elev_reverse) {
elevator_input = -elevator_input;
}
int32_t alt_change_cm = g.flybywire_climb_rate * elevator_input * dt * 100;
change_target_altitude(alt_change_cm);
if (is_zero(elevator_input) && !is_zero(target_altitude.last_elevator_input)) {
// the user has just released the elevator, lock in
// the current altitude
set_target_altitude_current();
}
target_altitude.last_elevator_input = elevator_input;
}
// check for FBWB altitude limit
check_minimum_altitude();
altitude_error_cm = calc_altitude_error_cm();
last_elevator_input = elevator_input;
calc_throttle();
calc_nav_pitch();
}