/*
handle speed and height control in FBWB or CRUISE mode.
In this mode the elevator is used to change target altitude. The
throttle is used to change target airspeed or throttle
*/
void Plane::update_fbwb_speed_height(void)
{
static float last_elevator_input;
float elevator_input;
elevator_input = channel_pitch->control_in / 4500.0f;
if (g.flybywire_elev_reverse) {
elevator_input = -elevator_input;
}
change_target_altitude(g.flybywire_climb_rate * elevator_input * 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();
}
// 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();
}
/*
change target altitude by a proportion of the target altitude offset
(difference in height to next WP from previous WP). proportion
should be between 0 and 1.
When proportion is zero we have reached the destination. When
proportion is 1 we are at the starting waypoint.
Note that target_altitude is setup initially based on the
destination waypoint
*/
void Plane::set_target_altitude_proportion(const Location &loc, float proportion)
{
set_target_altitude_location(loc);
proportion = constrain_float(proportion, 0.0f, 1.0f);
change_target_altitude(-target_altitude.offset_cm*proportion);
//rebuild the glide slope if we are above it and supposed to be climbing
if(g.glide_slope_threshold > 0) {
if(target_altitude.offset_cm > 0 && calc_altitude_error_cm() < -100 * g.glide_slope_threshold) {
set_target_altitude_location(loc);
set_offset_altitude_location(loc);
change_target_altitude(-target_altitude.offset_cm*proportion);
//adjust the new target offset altitude to reflect that we are partially already done
if(proportion > 0.0f)
target_altitude.offset_cm = ((float)target_altitude.offset_cm)/proportion;
}
}
}
bool Plane::verify_change_alt()
{
if( (condition_rate>=0 && adjusted_altitude_cm() >= condition_value) ||
(condition_rate<=0 && adjusted_altitude_cm() <= condition_value)) {
condition_value = 0;
return true;
}
// condition_rate is climb rate in cm/s.
// We divide by 10 because this function is called at 10hz
change_target_altitude(condition_rate/10);
return false;
}
/*
process a DO_CHANGE_ALT request
*/
void Plane::do_change_alt(const AP_Mission::Mission_Command& cmd)
{
condition_rate = labs((int)cmd.content.location.lat); // climb rate in cm/s
condition_value = cmd.content.location.alt; // To-Do: ensure this altitude is an absolute altitude?
if (condition_value < adjusted_altitude_cm()) {
condition_rate = -condition_rate;
}
set_target_altitude_current_adjusted();
change_target_altitude(condition_rate/10);
next_WP_loc.alt = condition_value; // For future nav calculations
reset_offset_altitude();
setup_glide_slope();
}
/*
handle speed and height control in FBWB or CRUISE mode.
In this mode the elevator is used to change target altitude. The
throttle is used to change target airspeed or throttle
*/
void Plane::update_fbwb_speed_height(void)
{
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_fbwb_minimum_altitude();
altitude_error_cm = calc_altitude_error_cm();
calc_throttle();
calc_nav_pitch();
}
