2015-05-29 23:12:49 -03:00
|
|
|
#include "Copter.h"
|
|
|
|
|
2019-02-25 10:32:52 -04:00
|
|
|
#if MODE_LOITER_ENABLED == ENABLED
|
|
|
|
|
2014-01-24 02:47:42 -04:00
|
|
|
/*
|
2016-07-25 15:45:29 -03:00
|
|
|
* Init and run calls for loiter flight mode
|
2014-01-24 02:47:42 -04:00
|
|
|
*/
|
|
|
|
|
|
|
|
// loiter_init - initialise loiter controller
|
2019-05-09 23:18:49 -03:00
|
|
|
bool ModeLoiter::init(bool ignore_checks)
|
2014-01-24 02:47:42 -04:00
|
|
|
{
|
2019-02-28 20:52:28 -04:00
|
|
|
if (!copter.failsafe.radio) {
|
|
|
|
float target_roll, target_pitch;
|
|
|
|
// apply SIMPLE mode transform to pilot inputs
|
|
|
|
update_simple_mode();
|
|
|
|
|
|
|
|
// convert pilot input to lean angles
|
2021-09-06 06:44:18 -03:00
|
|
|
get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max_cd());
|
2019-02-28 20:52:28 -04:00
|
|
|
|
|
|
|
// process pilot's roll and pitch input
|
2021-05-11 01:42:02 -03:00
|
|
|
loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch);
|
2019-02-28 20:52:28 -04:00
|
|
|
} else {
|
|
|
|
// clear out pilot desired acceleration in case radio failsafe event occurs and we do not switch to RTL for some reason
|
|
|
|
loiter_nav->clear_pilot_desired_acceleration();
|
|
|
|
}
|
|
|
|
loiter_nav->init_target();
|
|
|
|
|
2021-05-19 11:07:38 -03:00
|
|
|
// initialise the vertical position controller
|
2019-02-28 20:52:28 -04:00
|
|
|
if (!pos_control->is_active_z()) {
|
2021-05-11 01:42:02 -03:00
|
|
|
pos_control->init_z_controller();
|
2019-02-28 20:52:28 -04:00
|
|
|
}
|
|
|
|
|
2021-07-08 01:17:41 -03:00
|
|
|
// set vertical speed and acceleration limits
|
|
|
|
pos_control->set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
|
|
|
|
pos_control->set_correction_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
|
|
|
|
|
2019-02-28 20:52:28 -04:00
|
|
|
return true;
|
2014-01-24 02:47:42 -04:00
|
|
|
}
|
|
|
|
|
2016-12-05 06:06:08 -04:00
|
|
|
#if PRECISION_LANDING == ENABLED
|
2019-05-09 23:18:49 -03:00
|
|
|
bool ModeLoiter::do_precision_loiter()
|
2016-11-13 22:46:47 -04:00
|
|
|
{
|
2016-11-16 21:40:49 -04:00
|
|
|
if (!_precision_loiter_enabled) {
|
|
|
|
return false;
|
|
|
|
}
|
2019-05-09 23:18:49 -03:00
|
|
|
if (copter.ap.land_complete_maybe) {
|
2016-11-13 22:46:47 -04:00
|
|
|
return false; // don't move on the ground
|
|
|
|
}
|
|
|
|
// if the pilot *really* wants to move the vehicle, let them....
|
2018-03-27 23:13:37 -03:00
|
|
|
if (loiter_nav->get_pilot_desired_acceleration().length() > 50.0f) {
|
2016-11-13 22:46:47 -04:00
|
|
|
return false;
|
|
|
|
}
|
2018-02-07 22:21:09 -04:00
|
|
|
if (!copter.precland.target_acquired()) {
|
2016-11-13 22:46:47 -04:00
|
|
|
return false; // we don't have a good vector
|
|
|
|
}
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
2019-05-09 23:18:49 -03:00
|
|
|
void ModeLoiter::precision_loiter_xy()
|
2016-11-13 22:46:47 -04:00
|
|
|
{
|
2018-03-27 23:13:37 -03:00
|
|
|
loiter_nav->clear_pilot_desired_acceleration();
|
2016-11-13 22:46:47 -04:00
|
|
|
Vector2f target_pos, target_vel_rel;
|
2018-02-07 22:21:09 -04:00
|
|
|
if (!copter.precland.get_target_position_cm(target_pos)) {
|
2016-11-13 22:46:47 -04:00
|
|
|
target_pos.x = inertial_nav.get_position().x;
|
|
|
|
target_pos.y = inertial_nav.get_position().y;
|
|
|
|
}
|
2018-02-07 22:21:09 -04:00
|
|
|
if (!copter.precland.get_target_velocity_relative_cms(target_vel_rel)) {
|
2016-11-13 22:46:47 -04:00
|
|
|
target_vel_rel.x = -inertial_nav.get_velocity().x;
|
|
|
|
target_vel_rel.y = -inertial_nav.get_velocity().y;
|
|
|
|
}
|
2021-05-11 01:42:02 -03:00
|
|
|
pos_control->set_pos_target_xy_cm(target_pos.x, target_pos.y);
|
2017-01-09 03:31:26 -04:00
|
|
|
pos_control->override_vehicle_velocity_xy(-target_vel_rel);
|
2016-11-13 22:46:47 -04:00
|
|
|
}
|
2016-12-05 06:06:08 -04:00
|
|
|
#endif
|
2016-11-13 22:46:47 -04:00
|
|
|
|
2014-01-24 02:47:42 -04:00
|
|
|
// loiter_run - runs the loiter controller
|
|
|
|
// should be called at 100hz or more
|
2019-05-09 23:18:49 -03:00
|
|
|
void ModeLoiter::run()
|
2014-01-24 02:47:42 -04:00
|
|
|
{
|
2017-11-10 11:23:18 -04:00
|
|
|
float target_roll, target_pitch;
|
2015-06-26 09:42:08 -03:00
|
|
|
float target_yaw_rate = 0.0f;
|
|
|
|
float target_climb_rate = 0.0f;
|
2014-01-24 02:47:42 -04:00
|
|
|
|
2021-05-19 11:07:38 -03:00
|
|
|
// set vertical speed and acceleration limits
|
2021-05-11 01:42:02 -03:00
|
|
|
pos_control->set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
|
2015-10-19 21:05:10 -03:00
|
|
|
|
2015-06-24 22:31:06 -03:00
|
|
|
// process pilot inputs unless we are in radio failsafe
|
2018-02-07 22:21:09 -04:00
|
|
|
if (!copter.failsafe.radio) {
|
2014-01-24 02:47:42 -04:00
|
|
|
// apply SIMPLE mode transform to pilot inputs
|
|
|
|
update_simple_mode();
|
|
|
|
|
2017-11-10 11:23:18 -04:00
|
|
|
// convert pilot input to lean angles
|
2021-09-06 06:44:18 -03:00
|
|
|
get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max_cd());
|
2017-11-10 11:23:18 -04:00
|
|
|
|
2014-01-24 02:47:42 -04:00
|
|
|
// process pilot's roll and pitch input
|
2021-05-11 01:42:02 -03:00
|
|
|
loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch);
|
2014-01-24 02:47:42 -04:00
|
|
|
|
|
|
|
// get pilot's desired yaw rate
|
ArduCopter: 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:46:59 -03:00
|
|
|
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
|
2014-01-24 02:47:42 -04:00
|
|
|
|
|
|
|
// get pilot desired climb rate
|
ArduCopter: 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:46:59 -03:00
|
|
|
target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in());
|
2017-11-08 09:25:53 -04:00
|
|
|
target_climb_rate = constrain_float(target_climb_rate, -get_pilot_speed_dn(), g.pilot_speed_up);
|
2014-04-25 02:38:11 -03:00
|
|
|
} else {
|
|
|
|
// clear out pilot desired acceleration in case radio failsafe event occurs and we do not switch to RTL for some reason
|
2018-03-27 23:13:37 -03:00
|
|
|
loiter_nav->clear_pilot_desired_acceleration();
|
2014-01-24 02:47:42 -04:00
|
|
|
}
|
|
|
|
|
2014-08-29 03:25:37 -03:00
|
|
|
// relax loiter target if we might be landed
|
2019-05-09 23:18:49 -03:00
|
|
|
if (copter.ap.land_complete_maybe) {
|
2018-03-27 23:13:37 -03:00
|
|
|
loiter_nav->soften_for_landing();
|
2014-08-29 03:25:37 -03:00
|
|
|
}
|
|
|
|
|
2015-06-24 22:31:06 -03:00
|
|
|
// Loiter State Machine Determination
|
2019-02-28 05:03:23 -04:00
|
|
|
AltHoldModeState loiter_state = get_alt_hold_state(target_climb_rate);
|
2015-06-24 22:31:06 -03:00
|
|
|
|
|
|
|
// Loiter State Machine
|
|
|
|
switch (loiter_state) {
|
|
|
|
|
2019-02-28 05:03:23 -04:00
|
|
|
case AltHold_MotorStopped:
|
2017-01-09 03:31:26 -04:00
|
|
|
attitude_control->reset_rate_controller_I_terms();
|
2021-05-24 10:42:19 -03:00
|
|
|
attitude_control->reset_yaw_target_and_rate();
|
2021-05-19 11:07:38 -03:00
|
|
|
pos_control->relax_z_controller(0.0f); // forces throttle output to decay to zero
|
2019-02-28 05:03:23 -04:00
|
|
|
loiter_nav->init_target();
|
2021-04-13 02:39:53 -03:00
|
|
|
attitude_control->input_thrust_vector_rate_heading(loiter_nav->get_thrust_vector(), target_yaw_rate);
|
2015-11-11 23:19:15 -04:00
|
|
|
break;
|
|
|
|
|
2019-02-28 05:03:23 -04:00
|
|
|
case AltHold_Takeoff:
|
2016-07-12 09:45:13 -03:00
|
|
|
// initiate take-off
|
2018-03-16 03:22:14 -03:00
|
|
|
if (!takeoff.running()) {
|
|
|
|
takeoff.start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f));
|
2015-06-24 22:31:06 -03:00
|
|
|
}
|
|
|
|
|
2017-01-05 02:24:02 -04:00
|
|
|
// get avoidance adjusted climb rate
|
|
|
|
target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);
|
|
|
|
|
2021-05-19 11:07:38 -03:00
|
|
|
// set position controller targets adjusted for pilot input
|
2021-05-13 02:40:05 -03:00
|
|
|
takeoff.do_pilot_takeoff(target_climb_rate);
|
|
|
|
|
2015-06-24 22:31:06 -03:00
|
|
|
// run loiter controller
|
2018-09-05 03:47:50 -03:00
|
|
|
loiter_nav->update();
|
2015-06-24 22:31:06 -03:00
|
|
|
|
|
|
|
// call attitude controller
|
2021-04-13 02:39:53 -03:00
|
|
|
attitude_control->input_thrust_vector_rate_heading(loiter_nav->get_thrust_vector(), target_yaw_rate);
|
2015-06-24 22:31:06 -03:00
|
|
|
break;
|
|
|
|
|
2019-02-28 05:03:23 -04:00
|
|
|
case AltHold_Landed_Ground_Idle:
|
2021-05-24 10:42:19 -03:00
|
|
|
attitude_control->reset_yaw_target_and_rate();
|
2020-01-09 05:12:06 -04:00
|
|
|
FALLTHROUGH;
|
2019-02-28 05:03:23 -04:00
|
|
|
|
|
|
|
case AltHold_Landed_Pre_Takeoff:
|
2020-12-20 17:40:58 -04:00
|
|
|
attitude_control->reset_rate_controller_I_terms_smoothly();
|
2019-02-28 05:03:23 -04:00
|
|
|
loiter_nav->init_target();
|
2021-05-24 11:06:49 -03:00
|
|
|
attitude_control->input_thrust_vector_rate_heading(loiter_nav->get_thrust_vector(), target_yaw_rate);
|
2021-05-19 11:07:38 -03:00
|
|
|
pos_control->relax_z_controller(0.0f); // forces throttle output to decay to zero
|
2015-06-24 22:31:06 -03:00
|
|
|
break;
|
|
|
|
|
2019-02-28 05:03:23 -04:00
|
|
|
case AltHold_Flying:
|
2016-01-17 23:53:08 -04:00
|
|
|
// set motors to full range
|
2019-04-09 09:16:58 -03:00
|
|
|
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
|
2016-01-17 23:53:08 -04:00
|
|
|
|
2016-12-05 06:06:08 -04:00
|
|
|
#if PRECISION_LANDING == ENABLED
|
2016-11-13 22:46:47 -04:00
|
|
|
if (do_precision_loiter()) {
|
|
|
|
precision_loiter_xy();
|
|
|
|
}
|
2016-12-05 06:06:08 -04:00
|
|
|
#endif
|
2016-11-13 22:46:47 -04:00
|
|
|
|
2014-01-24 02:47:42 -04:00
|
|
|
// run loiter controller
|
2018-09-05 03:47:50 -03:00
|
|
|
loiter_nav->update();
|
2014-01-24 02:47:42 -04:00
|
|
|
|
|
|
|
// call attitude controller
|
2021-04-13 02:39:53 -03:00
|
|
|
attitude_control->input_thrust_vector_rate_heading(loiter_nav->get_thrust_vector(), target_yaw_rate);
|
2014-01-24 02:47:42 -04:00
|
|
|
|
2017-01-05 02:24:02 -04:00
|
|
|
// get avoidance adjusted climb rate
|
|
|
|
target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);
|
|
|
|
|
2021-08-31 01:17:59 -03:00
|
|
|
// update the vertical offset based on the surface measurement
|
|
|
|
copter.surface_tracking.update_surface_offset();
|
|
|
|
|
|
|
|
// Send the commanded climb rate to the position controller
|
|
|
|
pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate);
|
2015-06-24 22:31:06 -03:00
|
|
|
break;
|
2014-01-24 02:47:42 -04:00
|
|
|
}
|
2021-05-13 02:40:05 -03:00
|
|
|
|
2021-05-19 11:07:38 -03:00
|
|
|
// run the vertical position controller and set output throttle
|
2021-05-13 02:40:05 -03:00
|
|
|
pos_control->update_z_controller();
|
2015-06-26 09:42:08 -03:00
|
|
|
}
|
2018-02-07 22:21:09 -04:00
|
|
|
|
2019-05-09 23:18:49 -03:00
|
|
|
uint32_t ModeLoiter::wp_distance() const
|
2018-02-07 22:21:09 -04:00
|
|
|
{
|
2018-03-27 23:13:37 -03:00
|
|
|
return loiter_nav->get_distance_to_target();
|
2018-02-07 22:21:09 -04:00
|
|
|
}
|
|
|
|
|
2019-05-09 23:18:49 -03:00
|
|
|
int32_t ModeLoiter::wp_bearing() const
|
2018-02-07 22:21:09 -04:00
|
|
|
{
|
2018-03-27 23:13:37 -03:00
|
|
|
return loiter_nav->get_bearing_to_target();
|
2018-02-07 22:21:09 -04:00
|
|
|
}
|
2019-02-25 10:32:52 -04:00
|
|
|
|
|
|
|
#endif
|