2015-05-13 03:09:36 -03:00
|
|
|
#include "Plane.h"
|
2013-04-11 21:25:46 -03:00
|
|
|
|
|
|
|
// set the nav_controller pointer to the right controller
|
2015-05-13 03:09:36 -03:00
|
|
|
void Plane::set_nav_controller(void)
|
2013-04-11 21:25:46 -03:00
|
|
|
{
|
|
|
|
switch ((AP_Navigation::ControllerType)g.nav_controller.get()) {
|
2016-03-03 13:10:45 -04:00
|
|
|
|
|
|
|
default:
|
|
|
|
case AP_Navigation::CONTROLLER_DEFAULT:
|
2016-12-12 08:36:10 -04:00
|
|
|
// no break, fall through to L1 as default controller
|
2016-03-03 13:10:45 -04:00
|
|
|
|
2013-04-11 21:25:46 -03:00
|
|
|
case AP_Navigation::CONTROLLER_L1:
|
|
|
|
nav_controller = &L1_controller;
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
reset the total loiter angle
|
|
|
|
*/
|
2015-05-13 03:09:36 -03:00
|
|
|
void Plane::loiter_angle_reset(void)
|
2013-04-11 21:25:46 -03:00
|
|
|
{
|
2013-04-15 08:31:11 -03:00
|
|
|
loiter.sum_cd = 0;
|
|
|
|
loiter.total_cd = 0;
|
2017-02-21 07:56:32 -04:00
|
|
|
loiter.reached_target_alt = false;
|
|
|
|
loiter.unable_to_acheive_target_alt = false;
|
2013-04-11 21:25:46 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
update the total angle we have covered in a loiter. Used to support
|
|
|
|
commands to do N circles of loiter
|
|
|
|
*/
|
2015-05-13 03:09:36 -03:00
|
|
|
void Plane::loiter_angle_update(void)
|
2013-04-11 21:25:46 -03:00
|
|
|
{
|
2017-02-21 07:56:32 -04:00
|
|
|
static const int32_t lap_check_interval_cd = 3*36000;
|
|
|
|
|
|
|
|
const int32_t target_bearing_cd = nav_controller->target_bearing_cd();
|
2013-04-11 21:25:46 -03:00
|
|
|
int32_t loiter_delta_cd;
|
2020-07-31 06:12:33 -03:00
|
|
|
const bool reached_target = reached_loiter_target();
|
2017-02-21 07:56:32 -04:00
|
|
|
|
2020-07-31 06:12:33 -03:00
|
|
|
if (loiter.sum_cd == 0 && !reached_target) {
|
2016-03-10 00:43:28 -04:00
|
|
|
// we don't start summing until we are doing the real loiter
|
|
|
|
loiter_delta_cd = 0;
|
|
|
|
} else if (loiter.sum_cd == 0) {
|
2013-04-15 08:31:11 -03:00
|
|
|
// use 1 cd for initial delta
|
|
|
|
loiter_delta_cd = 1;
|
2017-02-21 07:56:32 -04:00
|
|
|
loiter.start_lap_alt_cm = current_loc.alt;
|
|
|
|
loiter.next_sum_lap_cd = lap_check_interval_cd;
|
2013-04-11 21:25:46 -03:00
|
|
|
} else {
|
|
|
|
loiter_delta_cd = target_bearing_cd - loiter.old_target_bearing_cd;
|
|
|
|
}
|
2017-02-21 07:56:32 -04:00
|
|
|
|
2013-04-11 21:25:46 -03:00
|
|
|
loiter.old_target_bearing_cd = target_bearing_cd;
|
|
|
|
loiter_delta_cd = wrap_180_cd(loiter_delta_cd);
|
2014-05-01 07:32:34 -03:00
|
|
|
loiter.sum_cd += loiter_delta_cd * loiter.direction;
|
2017-02-21 07:56:32 -04:00
|
|
|
|
2020-07-31 06:12:33 -03:00
|
|
|
bool reached_target_alt = false;
|
|
|
|
|
|
|
|
if (reached_target) {
|
|
|
|
// once we reach the position target we start checking the
|
|
|
|
// altitude target
|
|
|
|
bool terrain_status_ok = false;
|
|
|
|
#if AP_TERRAIN_AVAILABLE
|
|
|
|
/*
|
|
|
|
if doing terrain following then we check against terrain
|
|
|
|
target, fetch the terrain information
|
|
|
|
*/
|
|
|
|
float altitude_agl = 0;
|
|
|
|
if (target_altitude.terrain_following) {
|
|
|
|
if (terrain.status() == AP_Terrain::TerrainStatusOK &&
|
|
|
|
terrain.height_above_terrain(altitude_agl, true)) {
|
|
|
|
terrain_status_ok = true;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
if (terrain_status_ok &&
|
|
|
|
fabsf(altitude_agl - target_altitude.terrain_alt_cm*0.01) < 5) {
|
|
|
|
reached_target_alt = true;
|
|
|
|
} else
|
|
|
|
#endif
|
|
|
|
if (!terrain_status_ok && labs(current_loc.alt - target_altitude.amsl_cm) < 500) {
|
|
|
|
reached_target_alt = true;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
if (reached_target_alt) {
|
2017-02-21 07:56:32 -04:00
|
|
|
loiter.reached_target_alt = true;
|
|
|
|
loiter.unable_to_acheive_target_alt = false;
|
|
|
|
loiter.next_sum_lap_cd = loiter.sum_cd + lap_check_interval_cd;
|
|
|
|
|
|
|
|
} else if (!loiter.reached_target_alt && labs(loiter.sum_cd) >= loiter.next_sum_lap_cd) {
|
|
|
|
// check every few laps for scenario where up/downdrafts inhibit you from loitering up/down for too long
|
|
|
|
loiter.unable_to_acheive_target_alt = labs(current_loc.alt - loiter.start_lap_alt_cm) < 500;
|
|
|
|
loiter.start_lap_alt_cm = current_loc.alt;
|
|
|
|
loiter.next_sum_lap_cd += lap_check_interval_cd;
|
|
|
|
}
|
2013-04-11 21:25:46 -03:00
|
|
|
}
|
|
|
|
|
2011-09-08 22:29:39 -03:00
|
|
|
//****************************************************************
|
|
|
|
// Function that will calculate the desired direction to fly and distance
|
|
|
|
//****************************************************************
|
2015-05-13 03:09:36 -03:00
|
|
|
void Plane::navigate()
|
2011-09-08 22:29:39 -03:00
|
|
|
{
|
2013-04-11 21:25:46 -03:00
|
|
|
// allow change of nav controller mid-flight
|
|
|
|
set_nav_controller();
|
|
|
|
|
2012-08-16 21:50:15 -03:00
|
|
|
// do not navigate with corrupt data
|
|
|
|
// ---------------------------------
|
|
|
|
if (!have_position) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
2016-10-04 08:39:30 -03:00
|
|
|
if (next_WP_loc.lat == 0 && next_WP_loc.lng == 0) {
|
2012-08-16 21:50:15 -03:00
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
// waypoint distance from plane
|
|
|
|
// ----------------------------
|
2019-02-24 20:11:45 -04:00
|
|
|
auto_state.wp_distance = current_loc.get_distance(next_WP_loc);
|
2019-04-12 05:23:04 -03:00
|
|
|
auto_state.wp_proportion = current_loc.line_path_proportion(prev_WP_loc, next_WP_loc);
|
2016-01-08 20:12:03 -04:00
|
|
|
SpdHgt_Controller->set_path_proportion(auto_state.wp_proportion);
|
2012-08-16 21:50:15 -03:00
|
|
|
|
2013-04-11 21:25:46 -03:00
|
|
|
// update total loiter angle
|
|
|
|
loiter_angle_update();
|
2012-08-16 21:50:15 -03:00
|
|
|
|
2013-04-12 07:27:56 -03:00
|
|
|
// control mode specific updates to navigation demands
|
|
|
|
// ---------------------------------------------------
|
2020-08-19 05:19:01 -03:00
|
|
|
control_mode->navigate();
|
2011-09-08 22:29:39 -03:00
|
|
|
}
|
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
void Plane::calc_airspeed_errors()
|
2011-09-08 22:29:39 -03:00
|
|
|
{
|
2017-07-31 06:58:19 -03:00
|
|
|
float airspeed_measured = 0;
|
|
|
|
|
|
|
|
// we use the airspeed estimate function not direct sensor as TECS
|
|
|
|
// may be using synthetic airspeed
|
2020-01-06 20:45:27 -04:00
|
|
|
ahrs.airspeed_estimate(airspeed_measured);
|
2011-12-09 19:40:56 -04:00
|
|
|
|
2018-09-01 20:16:50 -03:00
|
|
|
// FBW_B/cruise airspeed target
|
2019-01-15 13:46:13 -04:00
|
|
|
if (!failsafe.rc_failsafe && (control_mode == &mode_fbwb || control_mode == &mode_cruise)) {
|
2018-12-15 01:43:39 -04:00
|
|
|
if (g2.flight_options & FlightOptions::CRUISE_TRIM_AIRSPEED) {
|
|
|
|
target_airspeed_cm = aparm.airspeed_cruise_cm;
|
|
|
|
} else if (g2.flight_options & FlightOptions::CRUISE_TRIM_THROTTLE) {
|
2018-09-01 21:01:32 -03:00
|
|
|
float control_min = 0.0f;
|
|
|
|
float control_mid = 0.0f;
|
|
|
|
const float control_max = channel_throttle->get_range();
|
2018-11-09 18:38:43 -04:00
|
|
|
const float control_in = get_throttle_input();
|
2018-09-01 21:01:32 -03:00
|
|
|
switch (channel_throttle->get_type()) {
|
|
|
|
case RC_Channel::RC_CHANNEL_TYPE_ANGLE:
|
|
|
|
control_min = -control_max;
|
|
|
|
break;
|
|
|
|
case RC_Channel::RC_CHANNEL_TYPE_RANGE:
|
|
|
|
control_mid = channel_throttle->get_control_mid();
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
if (control_in <= control_mid) {
|
|
|
|
target_airspeed_cm = linear_interpolate(aparm.airspeed_min * 100, aparm.airspeed_cruise_cm,
|
|
|
|
control_in,
|
|
|
|
control_min, control_mid);
|
|
|
|
} else {
|
|
|
|
target_airspeed_cm = linear_interpolate(aparm.airspeed_cruise_cm, aparm.airspeed_max * 100,
|
|
|
|
control_in,
|
|
|
|
control_mid, control_max);
|
|
|
|
}
|
|
|
|
} else {
|
|
|
|
target_airspeed_cm = ((int32_t)(aparm.airspeed_max - aparm.airspeed_min) *
|
2018-11-09 18:38:43 -04:00
|
|
|
get_throttle_input()) + ((int32_t)aparm.airspeed_min * 100);
|
2018-09-01 21:01:32 -03:00
|
|
|
}
|
2020-05-16 01:02:01 -03:00
|
|
|
#if OFFBOARD_GUIDED == ENABLED
|
|
|
|
} else if (control_mode == &mode_guided && !is_zero(guided_state.target_airspeed_cm)) {
|
|
|
|
// offboard airspeed demanded
|
|
|
|
uint32_t now = AP_HAL::millis();
|
|
|
|
float delta = 1e-3f * (now - guided_state.target_airspeed_time_ms);
|
|
|
|
guided_state.target_airspeed_time_ms = now;
|
|
|
|
float delta_amt = 100 * delta * guided_state.target_airspeed_accel;
|
|
|
|
target_airspeed_cm += delta_amt;
|
|
|
|
|
|
|
|
//target_airspeed_cm recalculated then clamped to between MIN airspeed and MAX airspeed by constrain_float
|
|
|
|
if (is_positive(guided_state.target_airspeed_accel)) {
|
|
|
|
target_airspeed_cm = constrain_float(MIN(guided_state.target_airspeed_cm, target_airspeed_cm), aparm.airspeed_min *100, aparm.airspeed_max *100);
|
|
|
|
} else {
|
|
|
|
target_airspeed_cm = constrain_float(MAX(guided_state.target_airspeed_cm, target_airspeed_cm), aparm.airspeed_min *100, aparm.airspeed_max *100);
|
|
|
|
}
|
2011-12-09 19:40:56 -04:00
|
|
|
|
2020-05-16 01:02:01 -03:00
|
|
|
#endif // OFFBOARD_GUIDED == ENABLED
|
2017-01-11 01:29:03 -04:00
|
|
|
} else if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND) {
|
2016-11-30 16:10:10 -04:00
|
|
|
// Landing airspeed target
|
2017-01-10 15:47:31 -04:00
|
|
|
target_airspeed_cm = landing.get_target_airspeed_cm();
|
2019-01-15 13:46:13 -04:00
|
|
|
} else if ((control_mode == &mode_auto) &&
|
2018-11-16 18:27:45 -04:00
|
|
|
(quadplane.options & QuadPlane::OPTION_MISSION_LAND_FW_APPROACH) &&
|
|
|
|
((vtol_approach_s.approach_stage == Landing_ApproachStage::APPROACH_LINE) ||
|
|
|
|
(vtol_approach_s.approach_stage == Landing_ApproachStage::VTOL_LANDING))) {
|
2020-07-30 17:16:08 -03:00
|
|
|
const float land_airspeed = SpdHgt_Controller->get_land_airspeed();
|
2018-11-16 18:27:45 -04:00
|
|
|
if (is_positive(land_airspeed)) {
|
2020-07-30 17:16:08 -03:00
|
|
|
target_airspeed_cm = land_airspeed * 100;
|
2018-11-16 18:27:45 -04:00
|
|
|
} else {
|
|
|
|
// fallover to normal airspeed
|
|
|
|
target_airspeed_cm = aparm.airspeed_cruise_cm;
|
|
|
|
}
|
2016-11-30 16:10:10 -04:00
|
|
|
} else {
|
|
|
|
// Normal airspeed target
|
|
|
|
target_airspeed_cm = aparm.airspeed_cruise_cm;
|
2016-04-13 13:12:26 -03:00
|
|
|
}
|
|
|
|
|
2011-12-09 19:40:56 -04:00
|
|
|
// Set target to current airspeed + ground speed undershoot,
|
|
|
|
// but only when this is faster than the target airspeed commanded
|
|
|
|
// above.
|
2018-01-03 00:45:41 -04:00
|
|
|
if (auto_throttle_mode &&
|
|
|
|
aparm.min_gndspeed_cm > 0 &&
|
2019-01-15 13:46:13 -04:00
|
|
|
control_mode != &mode_circle) {
|
2017-07-31 06:58:19 -03:00
|
|
|
int32_t min_gnd_target_airspeed = airspeed_measured*100 + groundspeed_undershoot;
|
|
|
|
if (min_gnd_target_airspeed > target_airspeed_cm) {
|
2012-08-07 03:05:51 -03:00
|
|
|
target_airspeed_cm = min_gnd_target_airspeed;
|
2017-07-31 06:58:19 -03:00
|
|
|
}
|
2011-12-09 19:40:56 -04:00
|
|
|
}
|
|
|
|
|
2020-05-16 01:02:01 -03:00
|
|
|
// when using the special GUIDED mode features for slew control, don't allow airspeed nudging as it doesn't play nicely.
|
|
|
|
#if OFFBOARD_GUIDED == ENABLED
|
|
|
|
if (control_mode == &mode_guided && !is_zero(guided_state.target_airspeed_cm) && (airspeed_nudge_cm != 0)) {
|
|
|
|
airspeed_nudge_cm = 0; //airspeed_nudge_cm forced to zero
|
|
|
|
}
|
|
|
|
#endif
|
|
|
|
|
2011-12-09 19:40:56 -04:00
|
|
|
// Bump up the target airspeed based on throttle nudging
|
2018-01-03 00:45:25 -04:00
|
|
|
if (throttle_allows_nudging && airspeed_nudge_cm > 0) {
|
2012-08-07 03:05:51 -03:00
|
|
|
target_airspeed_cm += airspeed_nudge_cm;
|
2011-12-09 19:40:56 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
// Apply airspeed limit
|
2016-08-07 21:48:36 -03:00
|
|
|
if (target_airspeed_cm > (aparm.airspeed_max * 100))
|
|
|
|
target_airspeed_cm = (aparm.airspeed_max * 100);
|
2011-12-09 19:40:56 -04:00
|
|
|
|
2014-03-19 17:57:27 -03:00
|
|
|
// use the TECS view of the target airspeed for reporting, to take
|
|
|
|
// account of the landing speed
|
2017-07-31 06:58:19 -03:00
|
|
|
airspeed_error = SpdHgt_Controller->get_target_airspeed() - airspeed_measured;
|
2011-12-09 19:40:56 -04:00
|
|
|
}
|
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
void Plane::calc_gndspeed_undershoot()
|
2011-12-09 19:40:56 -04:00
|
|
|
{
|
2018-11-16 17:51:12 -04:00
|
|
|
// Use the component of ground speed in the forward direction
|
|
|
|
// This prevents flyaway if wind takes plane backwards
|
2014-03-28 16:52:48 -03:00
|
|
|
if (gps.status() >= AP_GPS::GPS_OK_FIX_2D) {
|
2018-11-16 17:51:12 -04:00
|
|
|
Vector2f gndVel = ahrs.groundspeed_vector();
|
|
|
|
const Matrix3f &rotMat = ahrs.get_rotation_body_to_ned();
|
|
|
|
Vector2f yawVect = Vector2f(rotMat.a.x,rotMat.b.x);
|
2017-10-30 01:19:01 -03:00
|
|
|
if (!yawVect.is_zero()) {
|
|
|
|
yawVect.normalize();
|
|
|
|
float gndSpdFwd = yawVect * gndVel;
|
|
|
|
groundspeed_undershoot = (aparm.min_gndspeed_cm > 0) ? (aparm.min_gndspeed_cm - gndSpdFwd*100) : 0;
|
|
|
|
}
|
2018-01-09 20:19:47 -04:00
|
|
|
} else {
|
2018-11-16 17:51:12 -04:00
|
|
|
groundspeed_undershoot = 0;
|
2012-09-11 00:37:25 -03:00
|
|
|
}
|
2011-09-08 22:29:39 -03:00
|
|
|
}
|
|
|
|
|
2016-01-19 00:04:30 -04:00
|
|
|
void Plane::update_loiter(uint16_t radius)
|
2011-09-08 22:29:39 -03:00
|
|
|
{
|
2016-02-29 14:11:31 -04:00
|
|
|
if (radius <= 1) {
|
|
|
|
// if radius is <=1 then use the general loiter radius. if it's small, use default
|
2017-01-30 13:44:35 -04:00
|
|
|
radius = (abs(aparm.loiter_radius) <= 1) ? LOITER_RADIUS_DEFAULT : abs(aparm.loiter_radius);
|
2019-01-02 00:46:09 -04:00
|
|
|
if (next_WP_loc.loiter_ccw == 1) {
|
2016-04-11 18:09:08 -03:00
|
|
|
loiter.direction = -1;
|
|
|
|
} else {
|
2017-01-30 13:44:35 -04:00
|
|
|
loiter.direction = (aparm.loiter_radius < 0) ? -1 : 1;
|
2016-04-11 18:09:08 -03:00
|
|
|
}
|
2016-01-19 00:04:30 -04:00
|
|
|
}
|
2015-09-16 06:03:28 -03:00
|
|
|
|
2016-05-05 22:28:26 -03:00
|
|
|
if (loiter.start_time_ms != 0 &&
|
2016-09-30 19:35:58 -03:00
|
|
|
quadplane.guided_mode_enabled()) {
|
2016-05-11 02:57:41 -03:00
|
|
|
if (!auto_state.vtol_loiter) {
|
|
|
|
auto_state.vtol_loiter = true;
|
|
|
|
// reset loiter start time, so we don't consider the point
|
|
|
|
// reached till we get much closer
|
|
|
|
loiter.start_time_ms = 0;
|
|
|
|
quadplane.guided_start();
|
|
|
|
}
|
2017-10-30 02:24:32 -03:00
|
|
|
} else if ((loiter.start_time_ms == 0 &&
|
2019-01-15 13:46:13 -04:00
|
|
|
(control_mode == &mode_auto || control_mode == &mode_guided) &&
|
2017-11-21 15:41:45 -04:00
|
|
|
auto_state.crosstrack &&
|
2019-02-24 20:11:45 -04:00
|
|
|
current_loc.get_distance(next_WP_loc) > radius*3) ||
|
2019-01-15 13:46:13 -04:00
|
|
|
(control_mode == &mode_rtl && quadplane.available() && quadplane.rtl_mode == 1)) {
|
2017-10-30 02:24:32 -03:00
|
|
|
/*
|
|
|
|
if never reached loiter point and using crosstrack and somewhat far away from loiter point
|
|
|
|
navigate to it like in auto-mode for normal crosstrack behavior
|
|
|
|
|
|
|
|
we also use direct waypoint navigation if we are a quadplane
|
|
|
|
that is going to be switching to QRTL when it gets within
|
|
|
|
RTL_RADIUS
|
|
|
|
*/
|
2015-09-16 06:03:28 -03:00
|
|
|
nav_controller->update_waypoint(prev_WP_loc, next_WP_loc);
|
|
|
|
} else {
|
|
|
|
nav_controller->update_loiter(next_WP_loc, radius, loiter.direction);
|
|
|
|
}
|
|
|
|
|
|
|
|
if (loiter.start_time_ms == 0) {
|
2016-05-07 21:40:42 -03:00
|
|
|
if (reached_loiter_target() ||
|
2016-03-17 23:47:34 -03:00
|
|
|
auto_state.wp_proportion > 1) {
|
2015-09-16 06:03:28 -03:00
|
|
|
// we've reached the target, start the timer
|
|
|
|
loiter.start_time_ms = millis();
|
2020-09-22 14:00:39 -03:00
|
|
|
if (control_mode->is_guided_mode()) {
|
2015-12-21 13:48:30 -04:00
|
|
|
// starting a loiter in GUIDED means we just reached the target point
|
2017-07-07 22:55:12 -03:00
|
|
|
gcs().send_mission_item_reached_message(0);
|
2015-12-21 13:48:30 -04:00
|
|
|
}
|
2016-09-30 19:35:58 -03:00
|
|
|
if (quadplane.guided_mode_enabled()) {
|
2016-05-05 22:28:26 -03:00
|
|
|
quadplane.guided_start();
|
|
|
|
}
|
2015-09-16 06:03:28 -03:00
|
|
|
}
|
|
|
|
}
|
2011-09-08 22:29:39 -03:00
|
|
|
}
|
|
|
|
|
2013-07-13 07:05:53 -03:00
|
|
|
/*
|
|
|
|
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
|
|
|
|
*/
|
2015-05-13 03:09:36 -03:00
|
|
|
void Plane::update_fbwb_speed_height(void)
|
2013-07-13 07:05:53 -03:00
|
|
|
{
|
2017-05-09 05:05:36 -03:00
|
|
|
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;
|
2013-07-13 07:05:53 -03:00
|
|
|
|
2017-05-09 05:05:36 -03:00
|
|
|
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);
|
|
|
|
|
2019-08-22 21:17:14 -03:00
|
|
|
if (is_zero(elevator_input) && !is_zero(target_altitude.last_elevator_input)) {
|
2017-05-09 05:05:36 -03:00
|
|
|
// the user has just released the elevator, lock in
|
|
|
|
// the current altitude
|
|
|
|
set_target_altitude_current();
|
|
|
|
}
|
2019-08-22 21:17:14 -03:00
|
|
|
|
2020-09-23 05:16:45 -03:00
|
|
|
#if HAL_SOARING_ENABLED
|
2020-09-07 08:52:43 -03:00
|
|
|
if (g2.soaring_controller.is_active()) {
|
|
|
|
if (g2.soaring_controller.get_throttle_suppressed()) {
|
|
|
|
// we're in soaring mode with throttle suppressed
|
|
|
|
set_target_altitude_current();
|
|
|
|
} else {
|
2020-09-10 06:24:19 -03:00
|
|
|
// we're in soaring mode climbing back to altitude. Set target to SOAR_ALT_CUTOFF plus 10m to ensure we positively climb
|
|
|
|
// through SOAR_ALT_CUTOFF, thus triggering throttle suppression and return to glide.
|
2020-09-07 08:52:43 -03:00
|
|
|
target_altitude.amsl_cm = 100*plane.g2.soaring_controller.get_alt_cutoff() + 1000 + AP::ahrs().get_home().alt;
|
|
|
|
}
|
2019-08-22 21:17:14 -03:00
|
|
|
}
|
|
|
|
#endif
|
2017-05-09 05:05:36 -03:00
|
|
|
|
|
|
|
target_altitude.last_elevator_input = elevator_input;
|
2013-07-13 07:05:53 -03:00
|
|
|
}
|
|
|
|
|
2017-11-23 00:56:08 -04:00
|
|
|
check_fbwb_minimum_altitude();
|
2014-07-24 03:21:30 -03:00
|
|
|
|
|
|
|
altitude_error_cm = calc_altitude_error_cm();
|
2013-07-13 07:05:53 -03:00
|
|
|
|
|
|
|
calc_throttle();
|
|
|
|
calc_nav_pitch();
|
|
|
|
}
|
|
|
|
|
2014-06-04 20:35:09 -03:00
|
|
|
/*
|
|
|
|
calculate the turn angle for the next leg of the mission
|
|
|
|
*/
|
2015-05-13 03:09:36 -03:00
|
|
|
void Plane::setup_turn_angle(void)
|
2014-06-04 20:35:09 -03:00
|
|
|
{
|
|
|
|
int32_t next_ground_course_cd = mission.get_next_ground_course_cd(-1);
|
|
|
|
if (next_ground_course_cd == -1) {
|
|
|
|
// the mission library can't determine a turn angle, assume 90 degrees
|
|
|
|
auto_state.next_turn_angle = 90.0f;
|
|
|
|
} else {
|
|
|
|
// get the heading of the current leg
|
2019-04-05 10:02:44 -03:00
|
|
|
int32_t ground_course_cd = prev_WP_loc.get_bearing_to(next_WP_loc);
|
2014-06-04 20:35:09 -03:00
|
|
|
|
|
|
|
// work out the angle we need to turn through
|
|
|
|
auto_state.next_turn_angle = wrap_180_cd(next_ground_course_cd - ground_course_cd) * 0.01f;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2016-05-07 21:40:42 -03:00
|
|
|
/*
|
|
|
|
see if we have reached our loiter target
|
|
|
|
*/
|
|
|
|
bool Plane::reached_loiter_target(void)
|
|
|
|
{
|
|
|
|
if (quadplane.in_vtol_auto()) {
|
|
|
|
return auto_state.wp_distance < 3;
|
|
|
|
}
|
|
|
|
return nav_controller->reached_loiter_target();
|
|
|
|
}
|
|
|
|
|