void Plane::update_alt()
{
barometer.update();
if (should_log(MASK_LOG_IMU)) {
Log_Write_Baro();
}
// calculate the sink rate.
float sink_rate;
Vector3f vel;
if (ahrs.get_velocity_NED(vel)) {
sink_rate = vel.z;
} else if (gps.status() >= AP_GPS::GPS_OK_FIX_3D && gps.have_vertical_velocity()) {
sink_rate = gps.velocity().z;
} else {
sink_rate = -barometer.get_climb_rate();
}
// low pass the sink rate to take some of the noise out
auto_state.sink_rate = 0.8f * auto_state.sink_rate + 0.2f*sink_rate;
geofence_check(true);
update_flight_stage();
if (auto_throttle_mode && !throttle_suppressed) {
bool is_doing_auto_land = false;
float distance_beyond_land_wp = 0;
switch (flight_stage) {
case AP_SpdHgtControl::FLIGHT_LAND_APPROACH:
case AP_SpdHgtControl::FLIGHT_LAND_PREFLARE:
case AP_SpdHgtControl::FLIGHT_LAND_FINAL:
is_doing_auto_land = true;
if (location_passed_point(current_loc, prev_WP_loc, next_WP_loc)) {
distance_beyond_land_wp = get_distance(current_loc, next_WP_loc);
}
break;
default:
break;
}
SpdHgt_Controller->update_pitch_throttle(relative_target_altitude_cm(),
target_airspeed_cm,
flight_stage,
is_doing_auto_land,
distance_beyond_land_wp,
get_takeoff_pitch_min_cd(),
throttle_nudge,
tecs_hgt_afe(),
aerodynamic_load_factor);
if (should_log(MASK_LOG_TECS)) {
Log_Write_TECS_Tuning();
}
}
}
void Plane::update_alt()
{
barometer.update();
// calculate the sink rate.
float sink_rate;
Vector3f vel;
if (ahrs.get_velocity_NED(vel)) {
sink_rate = vel.z;
} else if (gps.status() >= AP_GPS::GPS_OK_FIX_3D && gps.have_vertical_velocity()) {
sink_rate = gps.velocity().z;
} else {
sink_rate = -barometer.get_climb_rate();
}
// low pass the sink rate to take some of the noise out
auto_state.sink_rate = 0.8f * auto_state.sink_rate + 0.2f*sink_rate;
geofence_check(true);
update_flight_stage();
if (auto_throttle_mode && !throttle_suppressed) {
float distance_beyond_land_wp = 0;
if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND && location_passed_point(current_loc, prev_WP_loc, next_WP_loc)) {
distance_beyond_land_wp = get_distance(current_loc, next_WP_loc);
}
bool soaring_active = false;
#if SOARING_ENABLED == ENABLED
if (g2.soaring_controller.is_active() && g2.soaring_controller.get_throttle_suppressed()) {
soaring_active = true;
}
#endif
SpdHgt_Controller->update_pitch_throttle(relative_target_altitude_cm(),
target_airspeed_cm,
flight_stage,
distance_beyond_land_wp,
get_takeoff_pitch_min_cd(),
throttle_nudge,
tecs_hgt_afe(),
aerodynamic_load_factor,
soaring_active);
}
}
void Plane::update_alt()
{
barometer.update();
if (should_log(MASK_LOG_IMU)) {
Log_Write_Baro();
}
// calculate the sink rate.
float sink_rate;
Vector3f vel;
if (ahrs.get_velocity_NED(vel)) {
sink_rate = vel.z;
} else if (gps.status() >= AP_GPS::GPS_OK_FIX_3D && gps.have_vertical_velocity()) {
sink_rate = gps.velocity().z;
} else {
sink_rate = -barometer.get_climb_rate();
}
// low pass the sink rate to take some of the noise out
auto_state.sink_rate = 0.8f * auto_state.sink_rate + 0.2f*sink_rate;
geofence_check(true);
update_flight_stage();
if (auto_throttle_mode && !throttle_suppressed) {
float distance_beyond_land_wp = 0;
if (auto_state.land_in_progress && location_passed_point(current_loc, prev_WP_loc, next_WP_loc)) {
distance_beyond_land_wp = get_distance(current_loc, next_WP_loc);
}
SpdHgt_Controller->update_pitch_throttle(relative_target_altitude_cm(),
target_airspeed_cm,
flight_stage,
auto_state.land_in_progress,
distance_beyond_land_wp,
get_takeoff_pitch_min_cd(),
throttle_nudge,
tecs_hgt_afe(),
aerodynamic_load_factor);
}
}
/*
calculate desired pitch angle during takeoff, setting nav_pitch_cd
*/
void Plane::takeoff_calc_pitch(void)
{
if (auto_state.highest_airspeed < g.takeoff_rotate_speed) {
// we have not reached rotate speed, use a target pitch of 5
// degrees. This should be enough to get the tail off the
// ground, while making it unlikely that overshoot in the
// pitch controller will cause a prop strike
nav_pitch_cd = 500;
return;
}
if (ahrs.airspeed_sensor_enabled()) {
int16_t takeoff_pitch_min_cd = get_takeoff_pitch_min_cd();
calc_nav_pitch();
if (nav_pitch_cd < takeoff_pitch_min_cd) {
nav_pitch_cd = takeoff_pitch_min_cd;
}
} else {
nav_pitch_cd = ((gps.ground_speed()*100) / (float)aparm.airspeed_cruise_cm) * auto_state.takeoff_pitch_cd;
nav_pitch_cd = constrain_int32(nav_pitch_cd, 500, auto_state.takeoff_pitch_cd);
}
}
