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:
|
|
|
|
// fall through to L1 as default controller
|
|
|
|
|
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;
|
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
|
|
|
{
|
|
|
|
int32_t target_bearing_cd = nav_controller->target_bearing_cd();
|
|
|
|
int32_t loiter_delta_cd;
|
2016-05-07 21:40:42 -03:00
|
|
|
if (loiter.sum_cd == 0 && !reached_loiter_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;
|
2013-04-11 21:25:46 -03:00
|
|
|
} else {
|
|
|
|
loiter_delta_cd = target_bearing_cd - loiter.old_target_bearing_cd;
|
|
|
|
}
|
|
|
|
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;
|
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
|
|
|
|
// ----------------------------
|
2015-01-01 00:17:45 -04:00
|
|
|
auto_state.wp_distance = get_distance(current_loc, next_WP_loc);
|
|
|
|
auto_state.wp_proportion = location_path_proportion(current_loc,
|
|
|
|
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
|
|
|
|
// ---------------------------------------------------
|
2012-08-16 21:50:15 -03:00
|
|
|
update_navigation();
|
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
|
|
|
{
|
2016-04-13 13:13:49 -03:00
|
|
|
float airspeed_measured_cm = airspeed.get_airspeed_cm();
|
2012-07-15 22:21:50 -03:00
|
|
|
|
2011-12-09 19:40:56 -04:00
|
|
|
// Normal airspeed target
|
2012-08-07 03:05:51 -03:00
|
|
|
target_airspeed_cm = g.airspeed_cruise_cm;
|
2011-12-09 19:40:56 -04:00
|
|
|
|
|
|
|
// FBW_B airspeed target
|
2013-07-13 07:05:53 -03:00
|
|
|
if (control_mode == FLY_BY_WIRE_B ||
|
|
|
|
control_mode == CRUISE) {
|
2016-08-07 21:48:36 -03:00
|
|
|
target_airspeed_cm = ((int32_t)(aparm.airspeed_max -
|
|
|
|
aparm.airspeed_min) *
|
ArduPlane: Fix up after refactoring RC_Channel class
Further to refactor of RC_Channel class which included
adding get_xx set_xx methods, change reads and writes to the public members
to calls to get and set functionsss
old public member(int16_t) get function -> int16_t set function (int16_t)
(expression where c is an object of type RC_Channel)
c.radio_in c.get_radio_in() c.set_radio_in(v)
c.control_in c.get_control_in() c.set_control_in(v)
c.servo_out c.get_servo_out() c.set_servo_out(v)
c.pwm_out c.get_pwm_out() // use existing
c.radio_out c.get_radio_out() c.set_radio_out(v)
c.radio_max c.get_radio_max() c.set_radio_max(v)
c.radio_min c.get_radio_min() c.set_radio_min(v)
c.radio_trim c.get_radio_trim() c.set_radio_trim(v);
c.min_max_configured() // return true if min and max are configured
Because data members of RC_Channels are now private and so cannot be written directly
some overloads are provided in the Plane classes to provide the old functionality
new overload Plane::stick_mix_channel(RC_Channel *channel)
which forwards to the previously existing
void stick_mix_channel(RC_Channel *channel, int16_t &servo_out);
new overload Plane::channel_output_mixer(Rc_Channel* , RC_Channel*)const
which forwards to
(uint8_t mixing_type, int16_t & chan1, int16_t & chan2)const;
Rename functions
RC_Channel_aux::set_radio_trim(Aux_servo_function_t function)
to RC_Channel_aux::set_trim_to_radio_in_for(Aux_servo_function_t function)
RC_Channel_aux::set_servo_out(Aux_servo_function_t function, int16_t value)
to RC_Channel_aux::set_servo_out_for(Aux_servo_function_t function, int16_t value)
Rationale:
RC_Channel is a complicated class, which combines
several functionalities dealing with stick inputs
in pwm and logical units, logical and actual actuator
outputs, unit conversion etc, etc
The intent of this PR is to clarify existing use of
the class. At the basic level it should now be possible
to grep all places where private variable is set by
searching for the set_xx function.
(The wider purpose is to provide a more generic and
logically simpler method of output mixing. This is a small step)
2016-05-08 05:33:02 -03:00
|
|
|
channel_throttle->get_control_in()) +
|
2016-08-07 21:48:36 -03:00
|
|
|
((int32_t)aparm.airspeed_min * 100);
|
2011-12-09 19:40:56 -04:00
|
|
|
}
|
|
|
|
|
2016-04-13 13:12:26 -03:00
|
|
|
// Landing airspeed target
|
2016-05-02 21:52:07 -03:00
|
|
|
if (control_mode == AUTO) {
|
2016-04-13 13:12:26 -03:00
|
|
|
float land_airspeed = SpdHgt_Controller->get_land_airspeed();
|
|
|
|
switch (flight_stage) {
|
|
|
|
case AP_SpdHgtControl::FLIGHT_LAND_APPROACH:
|
|
|
|
if (land_airspeed >= 0) {
|
|
|
|
target_airspeed_cm = land_airspeed * 100;
|
|
|
|
}
|
|
|
|
break;
|
|
|
|
|
|
|
|
case AP_SpdHgtControl::FLIGHT_LAND_PREFLARE:
|
|
|
|
case AP_SpdHgtControl::FLIGHT_LAND_FINAL:
|
|
|
|
if (auto_state.land_pre_flare && aparm.land_pre_flare_airspeed > 0) {
|
|
|
|
// if we just preflared then continue using the pre-flare airspeed during final flare
|
|
|
|
target_airspeed_cm = aparm.land_pre_flare_airspeed * 100;
|
|
|
|
} else if (land_airspeed >= 0) {
|
|
|
|
target_airspeed_cm = land_airspeed * 100;
|
|
|
|
}
|
|
|
|
break;
|
|
|
|
|
|
|
|
default:
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
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.
|
2012-08-07 03:05:51 -03:00
|
|
|
if (control_mode >= FLY_BY_WIRE_B && (g.min_gndspeed_cm > 0)) {
|
2016-04-13 13:13:49 -03:00
|
|
|
int32_t min_gnd_target_airspeed = airspeed_measured_cm + groundspeed_undershoot;
|
2012-08-07 03:05:51 -03:00
|
|
|
if (min_gnd_target_airspeed > target_airspeed_cm)
|
|
|
|
target_airspeed_cm = min_gnd_target_airspeed;
|
2011-12-09 19:40:56 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
// Bump up the target airspeed based on throttle nudging
|
2012-08-07 03:05:51 -03:00
|
|
|
if (control_mode >= AUTO && airspeed_nudge_cm > 0) {
|
|
|
|
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
|
2016-05-10 16:44:16 -03:00
|
|
|
airspeed_error = SpdHgt_Controller->get_target_airspeed() - airspeed_measured_cm * 0.01f;
|
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
|
|
|
{
|
2013-04-27 03:27:37 -03: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) {
|
2013-04-27 03:27:37 -03:00
|
|
|
Vector2f gndVel = ahrs.groundspeed_vector();
|
2015-12-10 18:06:37 -04:00
|
|
|
const Matrix3f &rotMat = ahrs.get_rotation_body_to_ned();
|
2013-04-27 03:27:37 -03:00
|
|
|
Vector2f yawVect = Vector2f(rotMat.a.x,rotMat.b.x);
|
2013-05-05 02:03:05 -03:00
|
|
|
yawVect.normalize();
|
2013-04-27 03:27:37 -03:00
|
|
|
float gndSpdFwd = yawVect * gndVel;
|
|
|
|
groundspeed_undershoot = (g.min_gndspeed_cm > 0) ? (g.min_gndspeed_cm - gndSpdFwd*100) : 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
|
2016-04-17 20:33:06 -03:00
|
|
|
radius = (abs(g.loiter_radius) <= 1) ? LOITER_RADIUS_DEFAULT : abs(g.loiter_radius);
|
2016-04-11 18:09:08 -03:00
|
|
|
if (next_WP_loc.flags.loiter_ccw == 1) {
|
|
|
|
loiter.direction = -1;
|
|
|
|
} else {
|
|
|
|
loiter.direction = (g.loiter_radius < 0) ? -1 : 1;
|
|
|
|
}
|
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();
|
|
|
|
}
|
2016-05-05 22:28:26 -03:00
|
|
|
} else if (loiter.start_time_ms == 0 &&
|
2015-09-16 06:03:28 -03:00
|
|
|
control_mode == AUTO &&
|
|
|
|
!auto_state.no_crosstrack &&
|
2016-06-23 16:34:18 -03:00
|
|
|
get_distance(current_loc, next_WP_loc) > radius*3) {
|
2015-09-16 06:03:28 -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
|
|
|
|
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();
|
2016-08-12 17:27:48 -03:00
|
|
|
if (control_mode == GUIDED || control_mode == AVOID_ADSB) {
|
2015-12-21 13:48:30 -04:00
|
|
|
// starting a loiter in GUIDED means we just reached the target point
|
|
|
|
gcs_send_mission_item_reached_message(0);
|
|
|
|
}
|
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 CRUISE mode, locking heading to GPS course when we have
|
|
|
|
sufficient ground speed, and no aileron or rudder input
|
|
|
|
*/
|
2015-05-13 03:09:36 -03:00
|
|
|
void Plane::update_cruise()
|
2013-07-13 07:05:53 -03:00
|
|
|
{
|
|
|
|
if (!cruise_state.locked_heading &&
|
ArduPlane: Fix up after refactoring RC_Channel class
Further to refactor of RC_Channel class which included
adding get_xx set_xx methods, change reads and writes to the public members
to calls to get and set functionsss
old public member(int16_t) get function -> int16_t set function (int16_t)
(expression where c is an object of type RC_Channel)
c.radio_in c.get_radio_in() c.set_radio_in(v)
c.control_in c.get_control_in() c.set_control_in(v)
c.servo_out c.get_servo_out() c.set_servo_out(v)
c.pwm_out c.get_pwm_out() // use existing
c.radio_out c.get_radio_out() c.set_radio_out(v)
c.radio_max c.get_radio_max() c.set_radio_max(v)
c.radio_min c.get_radio_min() c.set_radio_min(v)
c.radio_trim c.get_radio_trim() c.set_radio_trim(v);
c.min_max_configured() // return true if min and max are configured
Because data members of RC_Channels are now private and so cannot be written directly
some overloads are provided in the Plane classes to provide the old functionality
new overload Plane::stick_mix_channel(RC_Channel *channel)
which forwards to the previously existing
void stick_mix_channel(RC_Channel *channel, int16_t &servo_out);
new overload Plane::channel_output_mixer(Rc_Channel* , RC_Channel*)const
which forwards to
(uint8_t mixing_type, int16_t & chan1, int16_t & chan2)const;
Rename functions
RC_Channel_aux::set_radio_trim(Aux_servo_function_t function)
to RC_Channel_aux::set_trim_to_radio_in_for(Aux_servo_function_t function)
RC_Channel_aux::set_servo_out(Aux_servo_function_t function, int16_t value)
to RC_Channel_aux::set_servo_out_for(Aux_servo_function_t function, int16_t value)
Rationale:
RC_Channel is a complicated class, which combines
several functionalities dealing with stick inputs
in pwm and logical units, logical and actual actuator
outputs, unit conversion etc, etc
The intent of this PR is to clarify existing use of
the class. At the basic level it should now be possible
to grep all places where private variable is set by
searching for the set_xx function.
(The wider purpose is to provide a more generic and
logically simpler method of output mixing. This is a small step)
2016-05-08 05:33:02 -03:00
|
|
|
channel_roll->get_control_in() == 0 &&
|
2015-04-16 10:30:32 -03:00
|
|
|
rudder_input == 0 &&
|
2014-03-28 16:52:48 -03:00
|
|
|
gps.status() >= AP_GPS::GPS_OK_FIX_2D &&
|
|
|
|
gps.ground_speed() >= 3 &&
|
2013-07-13 07:05:53 -03:00
|
|
|
cruise_state.lock_timer_ms == 0) {
|
|
|
|
// user wants to lock the heading - start the timer
|
2015-05-13 19:05:32 -03:00
|
|
|
cruise_state.lock_timer_ms = millis();
|
2013-07-13 07:05:53 -03:00
|
|
|
}
|
|
|
|
if (cruise_state.lock_timer_ms != 0 &&
|
2015-05-13 19:05:32 -03:00
|
|
|
(millis() - cruise_state.lock_timer_ms) > 500) {
|
2013-07-13 07:05:53 -03:00
|
|
|
// lock the heading after 0.5 seconds of zero heading input
|
|
|
|
// from user
|
|
|
|
cruise_state.locked_heading = true;
|
|
|
|
cruise_state.lock_timer_ms = 0;
|
2014-03-28 16:52:48 -03:00
|
|
|
cruise_state.locked_heading_cd = gps.ground_course_cd();
|
2014-03-16 01:53:10 -03:00
|
|
|
prev_WP_loc = current_loc;
|
2013-07-13 07:05:53 -03:00
|
|
|
}
|
|
|
|
if (cruise_state.locked_heading) {
|
2014-03-16 01:53:10 -03:00
|
|
|
next_WP_loc = prev_WP_loc;
|
2013-07-13 07:05:53 -03:00
|
|
|
// always look 1km ahead
|
2014-03-16 01:53:10 -03:00
|
|
|
location_update(next_WP_loc,
|
2013-07-13 07:05:53 -03:00
|
|
|
cruise_state.locked_heading_cd*0.01f,
|
2014-03-16 01:53:10 -03:00
|
|
|
get_distance(prev_WP_loc, current_loc) + 1000);
|
|
|
|
nav_controller->update_waypoint(prev_WP_loc, next_WP_loc);
|
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
|
|
|
{
|
|
|
|
static float last_elevator_input;
|
|
|
|
float elevator_input;
|
ArduPlane: Fix up after refactoring RC_Channel class
Further to refactor of RC_Channel class which included
adding get_xx set_xx methods, change reads and writes to the public members
to calls to get and set functionsss
old public member(int16_t) get function -> int16_t set function (int16_t)
(expression where c is an object of type RC_Channel)
c.radio_in c.get_radio_in() c.set_radio_in(v)
c.control_in c.get_control_in() c.set_control_in(v)
c.servo_out c.get_servo_out() c.set_servo_out(v)
c.pwm_out c.get_pwm_out() // use existing
c.radio_out c.get_radio_out() c.set_radio_out(v)
c.radio_max c.get_radio_max() c.set_radio_max(v)
c.radio_min c.get_radio_min() c.set_radio_min(v)
c.radio_trim c.get_radio_trim() c.set_radio_trim(v);
c.min_max_configured() // return true if min and max are configured
Because data members of RC_Channels are now private and so cannot be written directly
some overloads are provided in the Plane classes to provide the old functionality
new overload Plane::stick_mix_channel(RC_Channel *channel)
which forwards to the previously existing
void stick_mix_channel(RC_Channel *channel, int16_t &servo_out);
new overload Plane::channel_output_mixer(Rc_Channel* , RC_Channel*)const
which forwards to
(uint8_t mixing_type, int16_t & chan1, int16_t & chan2)const;
Rename functions
RC_Channel_aux::set_radio_trim(Aux_servo_function_t function)
to RC_Channel_aux::set_trim_to_radio_in_for(Aux_servo_function_t function)
RC_Channel_aux::set_servo_out(Aux_servo_function_t function, int16_t value)
to RC_Channel_aux::set_servo_out_for(Aux_servo_function_t function, int16_t value)
Rationale:
RC_Channel is a complicated class, which combines
several functionalities dealing with stick inputs
in pwm and logical units, logical and actual actuator
outputs, unit conversion etc, etc
The intent of this PR is to clarify existing use of
the class. At the basic level it should now be possible
to grep all places where private variable is set by
searching for the set_xx function.
(The wider purpose is to provide a more generic and
logically simpler method of output mixing. This is a small step)
2016-05-08 05:33:02 -03:00
|
|
|
elevator_input = channel_pitch->get_control_in() / 4500.0f;
|
2013-07-13 07:05:53 -03:00
|
|
|
|
|
|
|
if (g.flybywire_elev_reverse) {
|
|
|
|
elevator_input = -elevator_input;
|
|
|
|
}
|
|
|
|
|
2016-04-20 22:50:13 -03:00
|
|
|
change_target_altitude(g.flybywire_climb_rate * elevator_input * perf.delta_us_fast_loop * 0.0001f);
|
2013-07-13 07:05:53 -03:00
|
|
|
|
2015-05-04 23:34:27 -03:00
|
|
|
if (is_zero(elevator_input) && !is_zero(last_elevator_input)) {
|
2013-07-13 07:05:53 -03:00
|
|
|
// the user has just released the elevator, lock in
|
|
|
|
// the current altitude
|
2014-07-24 03:21:30 -03:00
|
|
|
set_target_altitude_current();
|
2013-07-13 07:05:53 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
// check for FBWB altitude limit
|
2014-07-24 03:21:30 -03:00
|
|
|
check_minimum_altitude();
|
|
|
|
|
|
|
|
altitude_error_cm = calc_altitude_error_cm();
|
2013-07-13 07:05:53 -03:00
|
|
|
|
|
|
|
last_elevator_input = elevator_input;
|
|
|
|
|
|
|
|
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
|
|
|
|
int32_t ground_course_cd = get_bearing_cd(prev_WP_loc, next_WP_loc);
|
|
|
|
|
|
|
|
// 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();
|
|
|
|
}
|
|
|
|
|