Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: consolidate mode state decisions 

bnsgeyer and rmackay9 contributed to these changes

make_safe_shut_down waits for spool down before disarming
remove use of attitude_control::set_throttle_out_unstabilized to consolidate logic between multicopters and tradhelis
This commit is contained in:
Leonard Hall 2019-02-28 18:03:23 +09:00 committed by Randy Mackay
parent d949c80d54
commit 38cc5a817f
20 changed files with 842 additions and 880 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
ArduCopter/defines.h
View File

@ -176,30 +176,6 @@ enum SmartRTLState {
SmartRTL_Land
};
// Alt_Hold states
enum AltHoldModeState {
AltHold_MotorStopped,
AltHold_Takeoff,
AltHold_Flying,
AltHold_Landed
};
// Loiter states
enum LoiterModeState {
Loiter_MotorStopped,
Loiter_Takeoff,
Loiter_Flying,
Loiter_Landed
};
// Sport states
enum SportModeState {
Sport_MotorStopped,
Sport_Takeoff,
Sport_Flying,
Sport_Landed
};
// Flip states
enum FlipState {
Flip_Start,

93
ArduCopter/mode.cpp
View File

@ -396,14 +396,43 @@ void Copter::Mode::zero_throttle_and_relax_ac(bool spool_up)
} else {
motors->set_desired_spool_state(AP_Motors::DESIRED_GROUND_IDLE);
}
#if FRAME_CONFIG == HELI_FRAME
// Helicopters always stabilize roll/pitch/yaw
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f);
attitude_control->set_throttle_out(0.0f, false, copter.g.throttle_filt);
#else
// multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control->set_throttle_out_unstabilized(0.0f, true, copter.g.throttle_filt);
#endif
}
void Copter::Mode::zero_throttle_and_hold_attitude()
{
// run attitude controller
attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, 0.0f, 0.0f);
attitude_control->set_throttle_out(0.0f, false, copter.g.throttle_filt);
}
void Copter::Mode::make_safe_shut_down()
{
// command aircraft to initiate the shutdown process
motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN);
switch (motors->get_spool_mode()) {
case AP_Motors::SHUT_DOWN:
case AP_Motors::GROUND_IDLE:
// relax controllers during idle states
attitude_control->reset_rate_controller_I_terms();
attitude_control->set_yaw_target_to_current_heading();
break;
default:
// while transitioning though active states continue to operate normally
break;
}
// we may need to move this out
wp_nav->wp_and_spline_init();
// we may need to move this out
loiter_nav->init_target();
pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero
pos_control->update_z_controller();
// we may need to move this out
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f);
}
/*
@ -596,6 +625,58 @@ float Copter::Mode::get_pilot_desired_throttle() const
return throttle_out;
}
Copter::Mode::AltHoldModeState Copter::Mode::get_alt_hold_state(float target_climb_rate_cms)
{
// Alt Hold State Machine Determination
if (!motors->armed()) {
// the aircraft should moved to a shut down state
motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN);
// transition through states as aircraft spools down
switch (motors->get_spool_mode()) {
case AP_Motors::SHUT_DOWN:
return AltHold_MotorStopped;
case AP_Motors::DESIRED_GROUND_IDLE:
return AltHold_Landed_Ground_Idle;
default:
return AltHold_Landed_Pre_Takeoff;
}
} else if (takeoff.running() || takeoff.triggered(target_climb_rate_cms)) {
// the aircraft is currently landed or taking off, asking for a positive climb rate and in THROTTLE_UNLIMITED
// the aircraft should progress through the take off procedure
return AltHold_Takeoff;
} else if (!ap.auto_armed || ap.land_complete) {
// the aircraft is armed and landed
if (target_climb_rate_cms < 0.0f && !ap.using_interlock) {
// the aircraft should move to a ground idle state
motors->set_desired_spool_state(AP_Motors::DESIRED_GROUND_IDLE);
} else {
// the aircraft should prepare for imminent take off
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
}
if (motors->get_spool_mode() == AP_Motors::GROUND_IDLE) {
// the aircraft is waiting in ground idle
return AltHold_Landed_Ground_Idle;
} else {
// the aircraft can leave the ground at any time
return AltHold_Landed_Pre_Takeoff;
}
} else {
// the aircraft is in a flying state
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
return AltHold_Flying;
}
}
// pass-through functions to reduce code churn on conversion;
// these are candidates for moving into the Mode base
// class.

12
ArduCopter/mode.h
View File

@ -116,6 +116,8 @@ protected:
// helper functions
void zero_throttle_and_relax_ac(bool spool_up = false);
void zero_throttle_and_hold_attitude();
void make_safe_shut_down();
// functions to control landing
// in modes that support landing
@ -126,6 +128,16 @@ protected:
// return expected input throttle setting to hover:
virtual float throttle_hover() const;
// Alt_Hold based flight mode states used in Alt_Hold, Loiter, and Sport
enum AltHoldModeState {
AltHold_MotorStopped,
AltHold_Takeoff,
AltHold_Landed_Ground_Idle,
AltHold_Landed_Pre_Takeoff,
AltHold_Flying
};
AltHoldModeState get_alt_hold_state(float target_climb_rate_cms);
// convenience references to avoid code churn in conversion:
Parameters &g;
ParametersG2 &g2;

35
ArduCopter/mode_acro.cpp
View File

@ -10,21 +10,34 @@
void Copter::ModeAcro::run()
{
// convert the input to the desired body frame rate
float target_roll, target_pitch, target_yaw;
get_pilot_desired_angle_rates(channel_roll->get_control_in(), channel_pitch->get_control_in(), channel_yaw->get_control_in(), target_roll, target_pitch, target_yaw);
// if not armed set throttle to zero and exit immediately
if (!motors->armed() || ap.throttle_zero || !motors->get_interlock()) {
zero_throttle_and_relax_ac();
return;
if (!motors->armed()) {
// Motors should be Stopped
motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN);
} else if (ap.throttle_zero) {
// Attempting to Land
motors->set_desired_spool_state(AP_Motors::DESIRED_GROUND_IDLE);
} else {
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
}
// clear landing flag
set_land_complete(false);
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// convert the input to the desired body frame rate
get_pilot_desired_angle_rates(channel_roll->get_control_in(), channel_pitch->get_control_in(), channel_yaw->get_control_in(), target_roll, target_pitch, target_yaw);
if (motors->get_spool_mode() == AP_Motors::SHUT_DOWN) {
// Motors Stopped
attitude_control->set_yaw_target_to_current_heading();
attitude_control->reset_rate_controller_I_terms();
} else if (motors->get_spool_mode() == AP_Motors::GROUND_IDLE) {
// Landed
attitude_control->set_yaw_target_to_current_heading();
attitude_control->reset_rate_controller_I_terms();
} else if (motors->get_spool_mode() == AP_Motors::THROTTLE_UNLIMITED) {
// clear landing flag above zero throttle
if (!motors->limit.throttle_lower) {
set_land_complete(false);
}
}
// run attitude controller
attitude_control->input_rate_bf_roll_pitch_yaw(target_roll, target_pitch, target_yaw);

78
ArduCopter/mode_althold.cpp
View File

@ -21,7 +21,6 @@ bool Copter::ModeAltHold::init(bool ignore_checks)
// should be called at 100hz or more
void Copter::ModeAltHold::run()
{
AltHoldModeState althold_state;
float takeoff_climb_rate = 0.0f;
// initialize vertical speeds and acceleration
@ -43,40 +42,30 @@ void Copter::ModeAltHold::run()
target_climb_rate = constrain_float(target_climb_rate, -get_pilot_speed_dn(), g.pilot_speed_up);
// Alt Hold State Machine Determination
if (!motors->armed() || !motors->get_interlock()) {
althold_state = AltHold_MotorStopped;
} else if (takeoff.running() || takeoff.triggered(target_climb_rate)) {
althold_state = AltHold_Takeoff;
} else if (!ap.auto_armed || ap.land_complete) {
althold_state = AltHold_Landed;
} else {
althold_state = AltHold_Flying;
}
AltHoldModeState althold_state = get_alt_hold_state(target_climb_rate);
// Alt Hold State Machine
switch (althold_state) {
case AltHold_MotorStopped:
motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN);
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
attitude_control->reset_rate_controller_I_terms();
attitude_control->set_yaw_target_to_current_heading();
#if FRAME_CONFIG == HELI_FRAME
// force descent rate and call position controller
pos_control->set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false);
if (ap.land_complete_maybe) {
pos_control->relax_alt_hold_controllers(0.0f);
}
#else
pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero
#endif
pos_control->update_z_controller();
break;
case AltHold_Landed_Ground_Idle:
attitude_control->reset_rate_controller_I_terms();
attitude_control->set_yaw_target_to_current_heading();
// FALLTHROUGH
case AltHold_Landed_Pre_Takeoff:
pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero
break;
case AltHold_Takeoff:
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// initiate take-off
if (!takeoff.running()) {
@ -93,38 +82,9 @@ void Copter::ModeAltHold::run()
// get avoidance adjusted climb rate
target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
// call position controller
// set position controller targets
pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
pos_control->add_takeoff_climb_rate(takeoff_climb_rate, G_Dt);
pos_control->update_z_controller();
break;
case AltHold_Landed:
#if FRAME_CONFIG == HELI_FRAME
// helicopters do not spool down when landed. Only when commanded to go to ground idle by pilot.
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
if (motors->init_targets_on_arming()) {
attitude_control->reset_rate_controller_I_terms();
attitude_control->set_yaw_target_to_current_heading();
}
#else
// set motors to spin-when-armed if throttle below deadzone, otherwise full range (but motors will only spin at min throttle)
if (target_climb_rate < 0.0f) {
motors->set_desired_spool_state(AP_Motors::DESIRED_GROUND_IDLE);
} else {
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
}
attitude_control->reset_rate_controller_I_terms();
attitude_control->set_yaw_target_to_current_heading();
#endif
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero
pos_control->update_z_controller();
break;
case AltHold_Flying:
@ -135,18 +95,20 @@ void Copter::ModeAltHold::run()
copter.avoid.adjust_roll_pitch(target_roll, target_pitch, copter.aparm.angle_max);
#endif
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
// adjust climb rate using rangefinder
target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt);
// get avoidance adjusted climb rate
target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);
// call position controller
pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
pos_control->update_z_controller();
break;
}
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
// call z-axis position controller
pos_control->update_z_controller();
}

48
ArduCopter/mode_auto.cpp
View File

@ -729,16 +729,6 @@ void Copter::ModeAuto::takeoff_run()
// called by auto_run at 100hz or more
void Copter::ModeAuto::wp_run()
{
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) {
// To-Do: reset waypoint origin to current location because copter is probably on the ground so we don't want it lurching left or right on take-off
// (of course it would be better if people just used take-off)
zero_throttle_and_relax_ac();
// clear i term when we're taking off
set_throttle_takeoff();
return;
}
// process pilot's yaw input
float target_yaw_rate = 0;
if (!copter.failsafe.radio) {
@ -749,6 +739,12 @@ void Copter::ModeAuto::wp_run()
}
}
// if not armed set throttle to zero and exit immediately
if (!motors->armed() || !ap.auto_armed || ap.land_complete) {
make_safe_shut_down();
return;
}
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
@ -772,13 +768,9 @@ void Copter::ModeAuto::wp_run()
// called by auto_run at 100hz or more
void Copter::ModeAuto::spline_run()
{
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) {
// To-Do: reset waypoint origin to current location because copter is probably on the ground so we don't want it lurching left or right on take-off
// (of course it would be better if people just used take-off)
zero_throttle_and_relax_ac();
// clear i term when we're taking off
set_throttle_takeoff();
// if not armed set throttle to zero and exit immediately
if (!motors->armed() || !ap.auto_armed || ap.land_complete) {
make_safe_shut_down();
return;
}
@ -815,12 +807,14 @@ void Copter::ModeAuto::spline_run()
// called by auto_run at 100hz or more
void Copter::ModeAuto::land_run()
{
// if not auto armed or landed or motor interlock not enabled set throttle to zero and exit immediately
if (!motors->armed() || !ap.auto_armed || ap.land_complete || !motors->get_interlock()) {
zero_throttle_and_relax_ac();
// set target to current position
loiter_nav->clear_pilot_desired_acceleration();
loiter_nav->init_target();
// disarm when the landing detector says we've landed
if (ap.land_complete) {
copter.init_disarm_motors();
}
// if not armed set throttle to zero and exit immediately
if (!motors->armed() || !ap.auto_armed || ap.land_complete) {
make_safe_shut_down();
return;
}
@ -872,9 +866,9 @@ void Copter::ModeAuto::nav_guided_run()
// called by auto_run at 100hz or more
void Copter::ModeAuto::loiter_run()
{
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if (!motors->armed() || !ap.auto_armed || ap.land_complete || !motors->get_interlock()) {
zero_throttle_and_relax_ac();
// if not armed set throttle to zero and exit immediately
if (!motors->armed() || !ap.auto_armed || ap.land_complete) {
make_safe_shut_down();
return;
}
@ -1515,7 +1509,7 @@ bool Copter::ModeAuto::verify_land()
case LandStateType_Descending:
// rely on THROTTLE_LAND mode to correctly update landing status
retval = ap.land_complete;
retval = ap.land_complete && (motors->get_spool_mode() == AP_Motors::GROUND_IDLE);
break;
default:

19
ArduCopter/mode_brake.cpp
View File

@ -31,27 +31,20 @@ bool Copter::ModeBrake::init(bool ignore_checks)
// should be called at 100hz or more
void Copter::ModeBrake::run()
{
// if not auto armed set throttle to zero and exit immediately
if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) {
wp_nav->init_brake_target(BRAKE_MODE_DECEL_RATE);
zero_throttle_and_relax_ac();
pos_control->relax_alt_hold_controllers(0.0f);
// if not armed set throttle to zero and exit immediately
if (!motors->armed() || !ap.auto_armed || ap.land_complete) {
make_safe_shut_down();
return;
}
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// relax stop target if we might be landed
if (ap.land_complete_maybe) {
loiter_nav->soften_for_landing();
}
// if landed immediately disarm
if (ap.land_complete) {
copter.init_disarm_motors();
}
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// run brake controller
wp_nav->update_brake();

46
ArduCopter/mode_circle.cpp
View File

@ -27,41 +27,30 @@ bool Copter::ModeCircle::init(bool ignore_checks)
// should be called at 100hz or more
void Copter::ModeCircle::run()
{
float target_yaw_rate = 0;
float target_climb_rate = 0;
// initialize speeds and accelerations
pos_control->set_max_speed_xy(wp_nav->get_default_speed_xy());
pos_control->set_max_accel_xy(wp_nav->get_wp_acceleration());
pos_control->set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
pos_control->set_max_accel_z(g.pilot_accel_z);
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if (!motors->armed() || !ap.auto_armed || ap.land_complete || !motors->get_interlock()) {
// To-Do: add some initialisation of position controllers
zero_throttle_and_relax_ac();
pos_control->set_alt_target_to_current_alt();
return;
// get pilot's desired yaw rate (or zero if in radio failsafe)
float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
if (!is_zero(target_yaw_rate)) {
pilot_yaw_override = true;
}
// process pilot inputs
if (!copter.failsafe.radio) {
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
if (!is_zero(target_yaw_rate)) {
pilot_yaw_override = true;
}
// get pilot desired climb rate (or zero if in radio failsafe)
float target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in());
// adjust climb rate using rangefinder
if (copter.rangefinder_alt_ok()) {
// if rangefinder is ok, use surface tracking
target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt);
}
// get pilot desired climb rate
target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in());
// check for pilot requested take-off
if (ap.land_complete && target_climb_rate > 0) {
// indicate we are taking off
set_land_complete(false);
// clear i term when we're taking off
set_throttle_takeoff();
}
// if not armed set throttle to zero and exit immediately
if (!motors->armed() || !ap.auto_armed || ap.land_complete) {
make_safe_shut_down();
return;
}
// set motors to full range
@ -81,9 +70,6 @@ void Copter::ModeCircle::run()
copter.circle_nav->get_yaw(), true);
}
// adjust climb rate using rangefinder
target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt);
// update altitude target and call position controller
// protects heli's from inflight motor interlock disable
if (motors->get_desired_spool_state() == AP_Motors::DESIRED_GROUND_IDLE && !ap.land_complete) {

42
ArduCopter/mode_drift.cpp
View File

@ -40,21 +40,9 @@ void Copter::ModeDrift::run()
{
static float braker = 0.0f;
static float roll_input = 0.0f;
float target_roll, target_pitch;
float target_yaw_rate;
// if landed and throttle at zero, set throttle to zero and exit immediately
if (!motors->armed() || !motors->get_interlock() || (ap.land_complete && ap.throttle_zero)) {
zero_throttle_and_relax_ac(copter.is_tradheli() && motors->get_interlock());
return;
}
// clear landing flag above zero throttle
if (!ap.throttle_zero) {
set_land_complete(false);
}
// convert pilot input to lean angles
float target_roll, target_pitch;
get_pilot_desired_lean_angles(target_roll, target_pitch, copter.aparm.angle_max, copter.aparm.angle_max);
// Grab inertial velocity
@ -66,7 +54,7 @@ void Copter::ModeDrift::run()
// gain sceduling for Yaw
float pitch_vel2 = MIN(fabsf(pitch_vel), 2000);
target_yaw_rate = ((float)target_roll/1.0f) * (1.0f - (pitch_vel2 / 5000.0f)) * g.acro_yaw_p;
float target_yaw_rate = ((float)target_roll/1.0f) * (1.0f - (pitch_vel2 / 5000.0f)) * g.acro_yaw_p;
roll_vel = constrain_float(roll_vel, -DRIFT_SPEEDLIMIT, DRIFT_SPEEDLIMIT);
pitch_vel = constrain_float(pitch_vel, -DRIFT_SPEEDLIMIT, DRIFT_SPEEDLIMIT);
@ -90,8 +78,30 @@ void Copter::ModeDrift::run()
braker = 0.0f;
}
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
if (!motors->armed()) {
// Motors should be Stopped
motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN);
} else if (ap.throttle_zero) {
// Attempting to Land
motors->set_desired_spool_state(AP_Motors::DESIRED_GROUND_IDLE);
} else {
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
}
if (motors->get_spool_mode() == AP_Motors::SHUT_DOWN) {
// Motors Stopped
attitude_control->set_yaw_target_to_current_heading();
attitude_control->reset_rate_controller_I_terms();
} else if (motors->get_spool_mode() == AP_Motors::GROUND_IDLE) {
// Landed
attitude_control->set_yaw_target_to_current_heading();
attitude_control->reset_rate_controller_I_terms();
} else if (motors->get_spool_mode() == AP_Motors::THROTTLE_UNLIMITED) {
// clear landing flag above zero throttle
if (!motors->limit.throttle_lower) {
set_land_complete(false);
}
}
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);

6
ArduCopter/mode_flip.cpp
View File

@ -102,6 +102,9 @@ void Copter::ModeFlip::run()
// get pilot's desired throttle
float throttle_out = get_pilot_desired_throttle();
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// get corrected angle based on direction and axis of rotation
// we flip the sign of flip_angle to minimize the code repetition
int32_t flip_angle;
@ -209,9 +212,6 @@ void Copter::ModeFlip::run()
break;
}
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// output pilot's throttle without angle boost
attitude_control->set_throttle_out(throttle_out, false, g.throttle_filt);
}

120
ArduCopter/mode_flowhold.cpp
View File

@ -217,7 +217,6 @@ void Copter::ModeFlowHold::flowhold_flow_to_angle(Vector2f &bf_angles, bool stic
// should be called at 100hz or more
void Copter::ModeFlowHold::run()
{
FlowHoldModeState flowhold_state;
float takeoff_climb_rate = 0.0f;
update_height_estimate();
@ -241,15 +240,8 @@ void Copter::ModeFlowHold::run()
// get pilot's desired yaw rate
float target_yaw_rate = copter.get_pilot_desired_yaw_rate(copter.channel_yaw->get_control_in());
if (!copter.motors->armed() || !copter.motors->get_interlock()) {
flowhold_state = FlowHold_MotorStopped;
} else if (takeoff.running() || takeoff.triggered(target_climb_rate)) {
flowhold_state = FlowHold_Takeoff;
} else if (!copter.ap.auto_armed || copter.ap.land_complete) {
flowhold_state = FlowHold_Landed;
} else {
flowhold_state = FlowHold_Flying;
}
// Flow Hold State Machine Determination
AltHoldModeState flowhold_state = get_alt_hold_state(target_climb_rate);
if (copter.optflow.healthy()) {
const float filter_constant = 0.95;
@ -258,47 +250,19 @@ void Copter::ModeFlowHold::run()
quality_filtered = 0;
}
Vector2f bf_angles;
// calculate alt-hold angles
int16_t roll_in = copter.channel_roll->get_control_in();
int16_t pitch_in = copter.channel_pitch->get_control_in();
float angle_max = copter.attitude_control->get_althold_lean_angle_max();
get_pilot_desired_lean_angles(bf_angles.x, bf_angles.y,angle_max, attitude_control->get_althold_lean_angle_max());
if (quality_filtered >= flow_min_quality &&
AP_HAL::millis() - copter.arm_time_ms > 3000) {
// don't use for first 3s when we are just taking off
Vector2f flow_angles;
flowhold_flow_to_angle(flow_angles, (roll_in != 0) || (pitch_in != 0));
flow_angles.x = constrain_float(flow_angles.x, -angle_max/2, angle_max/2);
flow_angles.y = constrain_float(flow_angles.y, -angle_max/2, angle_max/2);
bf_angles += flow_angles;
}
bf_angles.x = constrain_float(bf_angles.x, -angle_max, angle_max);
bf_angles.y = constrain_float(bf_angles.y, -angle_max, angle_max);
// Flow Hold State Machine
switch (flowhold_state) {
case FlowHold_MotorStopped:
case AltHold_MotorStopped:
copter.motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN);
copter.attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(bf_angles.x, bf_angles.y, target_yaw_rate);
#if FRAME_CONFIG == HELI_FRAME
// force descent rate and call position controller
copter.pos_control->set_alt_target_from_climb_rate(-abs(copter.g.land_speed), copter.G_Dt, false);
#else
copter.attitude_control->reset_rate_controller_I_terms();
copter.attitude_control->set_yaw_target_to_current_heading();
copter.pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero
#endif
flow_pi_xy.reset_I();
copter.pos_control->update_z_controller();
break;
case FlowHold_Takeoff:
case AltHold_Takeoff:
// set motors to full range
copter.motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
@ -317,56 +281,66 @@ void Copter::ModeFlowHold::run()
// get avoidance adjusted climb rate
target_climb_rate = copter.get_avoidance_adjusted_climbrate(target_climb_rate);
// call attitude controller
copter.attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(bf_angles.x, bf_angles.y, target_yaw_rate);
// call position controller
copter.pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, copter.G_Dt, false);
copter.pos_control->add_takeoff_climb_rate(takeoff_climb_rate, copter.G_Dt);
copter.pos_control->update_z_controller();
break;
case FlowHold_Landed:
#if FRAME_CONFIG == HELI_FRAME
// helicopters do not spool down when landed. Only when commanded to go to ground idle by pilot.
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
#else
// set motors to spin-when-armed if throttle below deadzone, otherwise full range (but motors will only spin at min throttle)
if (target_climb_rate < 0.0f) {
copter.motors->set_desired_spool_state(AP_Motors::DESIRED_GROUND_IDLE);
} else {
copter.motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
}
#endif
copter.attitude_control->reset_rate_controller_I_terms();
copter.attitude_control->set_yaw_target_to_current_heading();
copter.attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(bf_angles.x, bf_angles.y, target_yaw_rate);
copter.pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero
copter.pos_control->update_z_controller();
case AltHold_Landed_Ground_Idle:
attitude_control->reset_rate_controller_I_terms();
attitude_control->set_yaw_target_to_current_heading();
// FALLTHROUGH
case AltHold_Landed_Pre_Takeoff:
pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero
break;
case FlowHold_Flying:
case AltHold_Flying:
copter.motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
#if AC_AVOID_ENABLED == ENABLED
// apply avoidance
copter.avoid.adjust_roll_pitch(bf_angles.x, bf_angles.y, copter.aparm.angle_max);
#endif
// call attitude controller
copter.attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(bf_angles.x, bf_angles.y, target_yaw_rate);
// adjust climb rate using rangefinder
target_climb_rate = copter.get_surface_tracking_climb_rate(target_climb_rate, copter.pos_control->get_alt_target(), copter.G_Dt);
// get avoidance adjusted climb rate
target_climb_rate = copter.get_avoidance_adjusted_climbrate(target_climb_rate);
// call position controller
copter.pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, copter.G_Dt, false);
copter.pos_control->update_z_controller();
copter.pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
break;
}
// flowhold attitude target calculations
Vector2f bf_angles;
// calculate alt-hold angles
int16_t roll_in = copter.channel_roll->get_control_in();
int16_t pitch_in = copter.channel_pitch->get_control_in();
float angle_max = copter.attitude_control->get_althold_lean_angle_max();
get_pilot_desired_lean_angles(bf_angles.x, bf_angles.y, angle_max, attitude_control->get_althold_lean_angle_max());
if (quality_filtered >= flow_min_quality &&
AP_HAL::millis() - copter.arm_time_ms > 3000) {
// don't use for first 3s when we are just taking off
Vector2f flow_angles;
flowhold_flow_to_angle(flow_angles, (roll_in != 0) || (pitch_in != 0));
flow_angles.x = constrain_float(flow_angles.x, -angle_max/2, angle_max/2);
flow_angles.y = constrain_float(flow_angles.y, -angle_max/2, angle_max/2);
bf_angles += flow_angles;
}
bf_angles.x = constrain_float(bf_angles.x, -angle_max, angle_max);
bf_angles.y = constrain_float(bf_angles.y, -angle_max, angle_max);
#if AC_AVOID_ENABLED == ENABLED
// apply avoidance
copter.avoid.adjust_roll_pitch(bf_angles.x, bf_angles.y, copter.aparm.angle_max);
#endif
// call attitude controller
copter.attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(bf_angles.x, bf_angles.y, target_yaw_rate);
// call z-axis position controller
pos_control->update_z_controller();
}
/*

9
ArduCopter/mode_follow.cpp
View File

@ -26,12 +26,15 @@ bool Copter::ModeFollow::init(const bool ignore_checks)
void Copter::ModeFollow::run()
{
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) {
zero_throttle_and_relax_ac();
// if not armed set throttle to zero and exit immediately
if (!motors->armed() || !ap.auto_armed || ap.land_complete) {
make_safe_shut_down();
return;
}
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// re-use guided mode's velocity controller
// Note: this is safe from interference from GCSs and companion computer's whose guided mode
// position and velocity requests will be ignored while the vehicle is not in guided mode

84
ArduCopter/mode_guided.cpp
View File

@ -377,13 +377,9 @@ void Copter::ModeGuided::takeoff_run()
void Copter::Mode::auto_takeoff_run()
{
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) {
// initialise wpnav targets
wp_nav->shift_wp_origin_to_current_pos();
zero_throttle_and_relax_ac();
// clear i term when we're taking off
set_throttle_takeoff();
// if not armed set throttle to zero and exit immediately
if (!motors->armed() || !ap.auto_armed) {
make_safe_shut_down();
return;
}
@ -394,17 +390,12 @@ void Copter::Mode::auto_takeoff_run()
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
}
#if FRAME_CONFIG == HELI_FRAME
// aircraft stays in landed state until rotor speed runup has finished
if (motors->get_spool_mode() == AP_Motors::THROTTLE_UNLIMITED) {
set_land_complete(false);
} else {
// initialise wpnav targets
wp_nav->shift_wp_origin_to_current_pos();
}
#else
set_land_complete(false);
#endif
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
@ -423,12 +414,6 @@ void Copter::Mode::auto_takeoff_run()
// called from guided_run
void Copter::ModeGuided::pos_control_run()
{
// if not auto armed or motors not enabled set throttle to zero and exit immediately
if (!motors->armed() || !ap.auto_armed || !motors->get_interlock() || ap.land_complete) {
zero_throttle_and_relax_ac();
return;
}
// process pilot's yaw input
float target_yaw_rate = 0;
if (!copter.failsafe.radio) {
@ -439,6 +424,12 @@ void Copter::ModeGuided::pos_control_run()
}
}
// if not armed set throttle to zero and exit immediately
if (!motors->armed() || !ap.auto_armed || ap.land_complete) {
make_safe_shut_down();
return;
}
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
@ -465,14 +456,6 @@ void Copter::ModeGuided::pos_control_run()
// called from guided_run
void Copter::ModeGuided::vel_control_run()
{
// if not auto armed or motors not enabled set throttle to zero and exit immediately
if (!motors->armed() || !ap.auto_armed || !motors->get_interlock() || ap.land_complete) {
// initialise velocity controller
pos_control->init_vel_controller_xyz();
zero_throttle_and_relax_ac();
return;
}
// process pilot's yaw input
float target_yaw_rate = 0;
if (!copter.failsafe.radio) {
@ -483,6 +466,12 @@ void Copter::ModeGuided::vel_control_run()
}
}
// if not armed set throttle to zero and exit immediately
if (!motors->armed() || !ap.auto_armed || ap.land_complete) {
make_safe_shut_down();
return;
}
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
@ -519,15 +508,6 @@ void Copter::ModeGuided::vel_control_run()
// called from guided_run
void Copter::ModeGuided::posvel_control_run()
{
// if not auto armed or motors not enabled set throttle to zero and exit immediately
if (!motors->armed() || !ap.auto_armed || !motors->get_interlock() || ap.land_complete) {
// set target position and velocity to current position and velocity
pos_control->set_pos_target(inertial_nav.get_position());
pos_control->set_desired_velocity(Vector3f(0,0,0));
zero_throttle_and_relax_ac();
return;
}
// process pilot's yaw input
float target_yaw_rate = 0;
@ -539,6 +519,12 @@ void Copter::ModeGuided::posvel_control_run()
}
}
// if not armed set throttle to zero and exit immediately
if (!motors->armed() || !ap.auto_armed || ap.land_complete) {
make_safe_shut_down();
return;
}
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
@ -587,16 +573,6 @@ void Copter::ModeGuided::posvel_control_run()
// called from guided_run
void Copter::ModeGuided::angle_control_run()
{
// if not auto armed or motors not enabled set throttle to zero and exit immediately
if (!motors->armed() || !ap.auto_armed || !motors->get_interlock() || (ap.land_complete && guided_angle_state.climb_rate_cms <= 0.0f)) {
#if FRAME_CONFIG == HELI_FRAME
attitude_control->set_yaw_target_to_current_heading();
#endif
zero_throttle_and_relax_ac();
pos_control->relax_alt_hold_controllers(0.0f);
return;
}
// constrain desired lean angles
float roll_in = guided_angle_state.roll_cd;
float pitch_in = guided_angle_state.pitch_cd;
@ -627,6 +603,24 @@ void Copter::ModeGuided::angle_control_run()
yaw_rate_in = 0.0f;
}
// if not armed set throttle to zero and exit immediately
if (!motors->armed() || !ap.auto_armed || (ap.land_complete && (guided_angle_state.climb_rate_cms <= 0.0f))) {
make_safe_shut_down();
return;
}
// TODO: use get_alt_hold_state
// landed with positive desired climb rate, takeoff
if (ap.land_complete && (guided_angle_state.climb_rate_cms > 0.0f)) {
zero_throttle_and_relax_ac();
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
if (motors->get_spool_mode() == AP_Motors::THROTTLE_UNLIMITED) {
set_land_complete(false);
set_throttle_takeoff();
}
return;
}
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);

80
ArduCopter/mode_land.cpp
View File

@ -53,29 +53,26 @@ void Copter::ModeLand::run()
// should be called at 100hz or more
void Copter::ModeLand::gps_run()
{
// if not auto armed or landed or motor interlock not enabled set throttle to zero and exit immediately
if (!motors->armed() || !ap.auto_armed || ap.land_complete || !motors->get_interlock()) {
zero_throttle_and_relax_ac();
loiter_nav->clear_pilot_desired_acceleration();
loiter_nav->init_target();
// disarm when the landing detector says we've landed
if (ap.land_complete) {
copter.init_disarm_motors();
}
// disarm when the landing detector says we've landed
if (ap.land_complete) {
copter.init_disarm_motors();
// Land State Machine Determination
if (!motors->armed() || !ap.auto_armed || ap.land_complete) {
make_safe_shut_down();
} else {
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// pause before beginning land descent
if(land_pause && millis()-land_start_time >= LAND_WITH_DELAY_MS) {
land_pause = false;
}
return;
land_run_horizontal_control();
land_run_vertical_control(land_pause);
}
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// pause before beginning land descent
if (land_pause && millis()-land_start_time >= LAND_WITH_DELAY_MS) {
land_pause = false;
}
land_run_horizontal_control();
land_run_vertical_control(land_pause);
}
// land_nogps_run - runs the land controller
@ -106,37 +103,28 @@ void Copter::ModeLand::nogps_run()
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
}
// if not auto armed or landed or motor interlock not enabled set throttle to zero and exit immediately
if (!motors->armed() || !ap.auto_armed || ap.land_complete || !motors->get_interlock()) {
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
attitude_control->set_throttle_out(0,false,g.throttle_filt);
#else
motors->set_desired_spool_state(AP_Motors::DESIRED_GROUND_IDLE);
// multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control->set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
// disarm when the landing detector says we've landed
if (ap.land_complete) {
copter.init_disarm_motors();
}
return;
// disarm when the landing detector says we've landed
if (ap.land_complete) {
copter.init_disarm_motors();
}
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// Land State Machine Determination
if (!motors->armed() || !ap.auto_armed || ap.land_complete) {
make_safe_shut_down();
} else {
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// pause before beginning land descent
if(land_pause && millis()-land_start_time >= LAND_WITH_DELAY_MS) {
land_pause = false;
}
land_run_vertical_control(land_pause);
}
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
// pause before beginning land descent
if (land_pause && millis()-land_start_time >= LAND_WITH_DELAY_MS) {
land_pause = false;
}
land_run_vertical_control(land_pause);
}
// do_not_use_GPS - forces land-mode to not use the GPS but instead rely on pilot input for roll and pitch

57
ArduCopter/mode_loiter.cpp
View File

@ -74,8 +74,6 @@ void Copter::ModeLoiter::precision_loiter_xy()
// should be called at 100hz or more
void Copter::ModeLoiter::run()
{
LoiterModeState loiter_state;
float target_roll, target_pitch;
float target_yaw_rate = 0.0f;
float target_climb_rate = 0.0f;
@ -113,42 +111,22 @@ void Copter::ModeLoiter::run()
}
// Loiter State Machine Determination
if (!motors->armed() || !motors->get_interlock()) {
loiter_state = Loiter_MotorStopped;
} else if (takeoff.running() || takeoff.triggered(target_climb_rate)) {
loiter_state = Loiter_Takeoff;
} else if (!ap.auto_armed || ap.land_complete) {
loiter_state = Loiter_Landed;
} else {
loiter_state = Loiter_Flying;
}
AltHoldModeState loiter_state = get_alt_hold_state(target_climb_rate);
// Loiter State Machine
switch (loiter_state) {
case Loiter_MotorStopped:
case AltHold_MotorStopped:
motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN);
#if FRAME_CONFIG == HELI_FRAME
// force descent rate and call position controller
pos_control->set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false);
if (ap.land_complete_maybe) {
pos_control->relax_alt_hold_controllers(0.0f);
}
#else
loiter_nav->init_target();
attitude_control->reset_rate_controller_I_terms();
attitude_control->set_yaw_target_to_current_heading();
pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero
#endif
loiter_nav->update();
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(loiter_nav->get_roll(), loiter_nav->get_pitch(), target_yaw_rate);
loiter_nav->init_target();
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate);
pos_control->update_z_controller();
break;
case Loiter_Takeoff:
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
case AltHold_Takeoff:
// initiate take-off
if (!takeoff.running()) {
@ -177,27 +155,21 @@ void Copter::ModeLoiter::run()
pos_control->update_z_controller();
break;
case Loiter_Landed:
#if FRAME_CONFIG == HELI_FRAME
// helicopters do not spool down when landed. Only when commanded to go to ground idle by pilot.
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
#else
// set motors to spin-when-armed if throttle below deadzone, otherwise full range (but motors will only spin at min throttle)
if (target_climb_rate < 0.0f) {
motors->set_desired_spool_state(AP_Motors::DESIRED_GROUND_IDLE);
} else {
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
}
#endif
loiter_nav->init_target();
case AltHold_Landed_Ground_Idle:
attitude_control->reset_rate_controller_I_terms();
attitude_control->set_yaw_target_to_current_heading();
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0);
// FALLTHROUGH
case AltHold_Landed_Pre_Takeoff:
loiter_nav->init_target();
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f);
pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero
pos_control->update_z_controller();
break;
case Loiter_Flying:
case AltHold_Flying:
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
@ -220,7 +192,6 @@ void Copter::ModeLoiter::run()
// get avoidance adjusted climb rate
target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);
// update altitude target and call position controller
pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
pos_control->update_z_controller();
break;

809
ArduCopter/mode_poshold.cpp
View File

@ -23,6 +23,14 @@
#define POSHOLD_STICK_RELEASE_SMOOTH_ANGLE 1800 // max angle required (in centi-degrees) after which the smooth stick release effect is applied
#define POSHOLD_WIND_COMP_ESTIMATE_SPEED_MAX 10 // wind compensation estimates will only run when velocity is at or below this speed in cm/s
// PosHold states
enum PosHoldModeState {
PosHold_MotorStopped,
PosHold_Takeoff,
PosHold_Flying,
PosHold_Landed
};
// mission state enumeration
enum poshold_rp_mode {
POSHOLD_PILOT_OVERRIDE=0, // pilot is controlling this axis (i.e. roll or pitch)
@ -34,8 +42,8 @@ enum poshold_rp_mode {
};
static struct {
poshold_rp_mode roll_mode : 3; // roll mode: pilot override, brake or loiter
poshold_rp_mode pitch_mode : 3; // pitch mode: pilot override, brake or loiter
poshold_rp_mode roll_mode : 3; // roll mode: pilot override, brake or loiter
poshold_rp_mode pitch_mode : 3; // pitch mode: pilot override, brake or loiter
uint8_t braking_time_updated_roll : 1; // true once we have re-estimated the braking time. This is done once as the vehicle begins to flatten out after braking
uint8_t braking_time_updated_pitch : 1; // true once we have re-estimated the braking time. This is done once as the vehicle begins to flatten out after braking
@ -95,16 +103,16 @@ bool Copter::ModePosHold::init(bool ignore_checks)
// if landed begin in loiter mode
poshold.roll_mode = POSHOLD_LOITER;
poshold.pitch_mode = POSHOLD_LOITER;
// set target to current position
// only init here as we can switch to PosHold in flight with a velocity <> 0 that will be used as _last_vel in PosControl and never updated again as we inhibit Reset_I
loiter_nav->clear_pilot_desired_acceleration();
loiter_nav->init_target();
}else{
// if not landed start in pilot override to avoid hard twitch
poshold.roll_mode = POSHOLD_PILOT_OVERRIDE;
poshold.pitch_mode = POSHOLD_PILOT_OVERRIDE;
}
// initialise loiter
loiter_nav->clear_pilot_desired_acceleration();
loiter_nav->init_target();
// initialise wind_comp each time PosHold is switched on
poshold.wind_comp_ef.zero();
poshold.wind_comp_roll = 0;
@ -118,14 +126,10 @@ bool Copter::ModePosHold::init(bool ignore_checks)
// should be called at 100hz or more
void Copter::ModePosHold::run()
{
float target_roll, target_pitch; // pilot's roll and pitch angle inputs
float target_yaw_rate = 0; // pilot desired yaw rate in centi-degrees/sec
float target_climb_rate = 0; // pilot desired climb rate in centimeters/sec
float takeoff_climb_rate = 0.0f; // takeoff induced climb rate
float takeoff_climb_rate = 0.0f;
float brake_to_loiter_mix; // mix of brake and loiter controls. 0 = fully brake controls, 1 = fully loiter controls
float controller_to_pilot_roll_mix; // mix of controller and pilot controls. 0 = fully last controller controls, 1 = fully pilot controls
float controller_to_pilot_pitch_mix; // mix of controller and pilot controls. 0 = fully last controller controls, 1 = fully pilot controls
float vel_fw, vel_right; // vehicle's current velocity in body-frame forward and right directions
const Vector3f& vel = inertial_nav.get_velocity();
// initialize vertical speeds and acceleration
@ -133,406 +137,437 @@ void Copter::ModePosHold::run()
pos_control->set_max_accel_z(g.pilot_accel_z);
loiter_nav->clear_pilot_desired_acceleration();
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) {
loiter_nav->init_target();
zero_throttle_and_relax_ac(copter.is_tradheli() && motors->get_interlock());
pos_control->relax_alt_hold_controllers(0.0f);
return;
}
// apply SIMPLE mode transform to pilot inputs
update_simple_mode();
// process pilot inputs
if (!copter.failsafe.radio) {
// apply SIMPLE mode transform to pilot inputs
update_simple_mode();
// convert pilot input to lean angles
float target_roll, target_pitch;
get_pilot_desired_lean_angles(target_roll, target_pitch, copter.aparm.angle_max, attitude_control->get_althold_lean_angle_max());
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
// get pilot's desired yaw rate
float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
// get pilot desired climb rate (for alt-hold mode and take-off)
target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in());
target_climb_rate = constrain_float(target_climb_rate, -get_pilot_speed_dn(), g.pilot_speed_up);
// get takeoff adjusted pilot and takeoff climb rates
takeoff.get_climb_rates(target_climb_rate, takeoff_climb_rate);
// check for take-off
#if FRAME_CONFIG == HELI_FRAME
// helicopters are held on the ground until rotor speed runup has finished
if (ap.land_complete && (takeoff.running() || (target_climb_rate > 0.0f && motors->rotor_runup_complete()))) {
#else
if (ap.land_complete && (takeoff.running() || target_climb_rate > 0.0f)) {
#endif
if (!takeoff.running()) {
takeoff.start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f));
}
// indicate we are taking off
set_land_complete(false);
// clear i term when we're taking off
set_throttle_takeoff();
}
}
// get pilot desired climb rate (for alt-hold mode and take-off)
float target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in());
target_climb_rate = constrain_float(target_climb_rate, -get_pilot_speed_dn(), g.pilot_speed_up);
// relax loiter target if we might be landed
if (ap.land_complete_maybe) {
loiter_nav->soften_for_landing();
}
// if landed initialise loiter targets, set throttle to zero and exit
if (ap.land_complete) {
#if FRAME_CONFIG == HELI_FRAME
// helicopters do not spool down when landed. Only when commanded to go to ground idle by pilot.
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
#else
// set motors to spin-when-armed if throttle below deadzone, otherwise full range (but motors will only spin at min throttle)
if (target_climb_rate < 0.0f) {
motors->set_desired_spool_state(AP_Motors::DESIRED_GROUND_IDLE);
} else {
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
}
#endif
loiter_nav->init_target();
// Pos Hold State Machine Determination
AltHoldModeState poshold_state = get_alt_hold_state(target_climb_rate);
// state machine
switch (poshold_state) {
case AltHold_MotorStopped:
attitude_control->reset_rate_controller_I_terms();
attitude_control->set_yaw_target_to_current_heading();
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0);
pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero
pos_control->update_z_controller();
return;
}else{
// convert pilot input to lean angles
get_pilot_desired_lean_angles(target_roll, target_pitch, copter.aparm.angle_max, attitude_control->get_althold_lean_angle_max());
loiter_nav->init_target();
loiter_nav->update();
// convert inertial nav earth-frame velocities to body-frame
// To-Do: move this to AP_Math (or perhaps we already have a function to do this)
vel_fw = vel.x*ahrs.cos_yaw() + vel.y*ahrs.sin_yaw();
vel_right = -vel.x*ahrs.sin_yaw() + vel.y*ahrs.cos_yaw();
// If not in LOITER, retrieve latest wind compensation lean angles related to current yaw
if (poshold.roll_mode != POSHOLD_LOITER || poshold.pitch_mode != POSHOLD_LOITER)
poshold_get_wind_comp_lean_angles(poshold.wind_comp_roll, poshold.wind_comp_pitch);
// set poshold state to pilot override
poshold.roll_mode = POSHOLD_PILOT_OVERRIDE;
poshold.pitch_mode = POSHOLD_PILOT_OVERRIDE;
break;
// Roll state machine
// Each state (aka mode) is responsible for:
// 1. dealing with pilot input
// 2. calculating the final roll output to the attitude controller
// 3. checking if the state (aka mode) should be changed and if 'yes' perform any required initialisation for the new state
switch (poshold.roll_mode) {
case AltHold_Takeoff:
case POSHOLD_PILOT_OVERRIDE:
// update pilot desired roll angle using latest radio input
// this filters the input so that it returns to zero no faster than the brake-rate
poshold_update_pilot_lean_angle(poshold.pilot_roll, target_roll);
// switch to BRAKE mode for next iteration if no pilot input
if (is_zero(target_roll) && (fabsf(poshold.pilot_roll) < 2 * g.poshold_brake_rate)) {
// initialise BRAKE mode
poshold.roll_mode = POSHOLD_BRAKE; // Set brake roll mode
poshold.brake_roll = 0; // initialise braking angle to zero
poshold.brake_angle_max_roll = 0; // reset brake_angle_max so we can detect when vehicle begins to flatten out during braking
poshold.brake_timeout_roll = POSHOLD_BRAKE_TIME_ESTIMATE_MAX; // number of cycles the brake will be applied, updated during braking mode.
poshold.braking_time_updated_roll = false; // flag the braking time can be re-estimated
}
// final lean angle should be pilot input plus wind compensation
poshold.roll = poshold.pilot_roll + poshold.wind_comp_roll;
break;
case POSHOLD_BRAKE:
case POSHOLD_BRAKE_READY_TO_LOITER:
// calculate brake_roll angle to counter-act velocity
poshold_update_brake_angle_from_velocity(poshold.brake_roll, vel_right);
// update braking time estimate
if (!poshold.braking_time_updated_roll) {
// check if brake angle is increasing
if (abs(poshold.brake_roll) >= poshold.brake_angle_max_roll) {
poshold.brake_angle_max_roll = abs(poshold.brake_roll);
} else {
// braking angle has started decreasing so re-estimate braking time
poshold.brake_timeout_roll = 1+(uint16_t)(LOOP_RATE_FACTOR*15L*(int32_t)(abs(poshold.brake_roll))/(10L*(int32_t)g.poshold_brake_rate)); // the 1.2 (12/10) factor has to be tuned in flight, here it means 120% of the "normal" time.
poshold.braking_time_updated_roll = true;
}
}
// if velocity is very low reduce braking time to 0.5seconds
if ((fabsf(vel_right) <= POSHOLD_SPEED_0) && (poshold.brake_timeout_roll > 50*LOOP_RATE_FACTOR)) {
poshold.brake_timeout_roll = 50*LOOP_RATE_FACTOR;
}
// reduce braking timer
if (poshold.brake_timeout_roll > 0) {
poshold.brake_timeout_roll--;
} else {
// indicate that we are ready to move to Loiter.
// Loiter will only actually be engaged once both roll_mode and pitch_mode are changed to POSHOLD_BRAKE_READY_TO_LOITER
// logic for engaging loiter is handled below the roll and pitch mode switch statements
poshold.roll_mode = POSHOLD_BRAKE_READY_TO_LOITER;
}
// final lean angle is braking angle + wind compensation angle
poshold.roll = poshold.brake_roll + poshold.wind_comp_roll;
// check for pilot input
if (!is_zero(target_roll)) {
// init transition to pilot override
poshold_roll_controller_to_pilot_override();
}
break;
case POSHOLD_BRAKE_TO_LOITER:
case POSHOLD_LOITER:
// these modes are combined roll-pitch modes and are handled below
break;
case POSHOLD_CONTROLLER_TO_PILOT_OVERRIDE:
// update pilot desired roll angle using latest radio input
// this filters the input so that it returns to zero no faster than the brake-rate
poshold_update_pilot_lean_angle(poshold.pilot_roll, target_roll);
// count-down loiter to pilot timer
if (poshold.controller_to_pilot_timer_roll > 0) {
poshold.controller_to_pilot_timer_roll--;
} else {
// when timer runs out switch to full pilot override for next iteration
poshold.roll_mode = POSHOLD_PILOT_OVERRIDE;
}
// calculate controller_to_pilot mix ratio
controller_to_pilot_roll_mix = (float)poshold.controller_to_pilot_timer_roll / (float)POSHOLD_CONTROLLER_TO_PILOT_MIX_TIMER;
// mix final loiter lean angle and pilot desired lean angles
poshold.roll = poshold_mix_controls(controller_to_pilot_roll_mix, poshold.controller_final_roll, poshold.pilot_roll + poshold.wind_comp_roll);
break;
// initiate take-off
if (!takeoff.running()) {
takeoff.start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f));
// indicate we are taking off
set_land_complete(false);
// clear i terms
set_throttle_takeoff();
}
// Pitch state machine
// Each state (aka mode) is responsible for:
// 1. dealing with pilot input
// 2. calculating the final pitch output to the attitude contpitcher
// 3. checking if the state (aka mode) should be changed and if 'yes' perform any required initialisation for the new state
switch (poshold.pitch_mode) {
case POSHOLD_PILOT_OVERRIDE:
// update pilot desired pitch angle using latest radio input
// this filters the input so that it returns to zero no faster than the brake-rate
poshold_update_pilot_lean_angle(poshold.pilot_pitch, target_pitch);
// switch to BRAKE mode for next iteration if no pilot input
if (is_zero(target_pitch) && (fabsf(poshold.pilot_pitch) < 2 * g.poshold_brake_rate)) {
// initialise BRAKE mode
poshold.pitch_mode = POSHOLD_BRAKE; // set brake pitch mode
poshold.brake_pitch = 0; // initialise braking angle to zero
poshold.brake_angle_max_pitch = 0; // reset brake_angle_max so we can detect when vehicle begins to flatten out during braking
poshold.brake_timeout_pitch = POSHOLD_BRAKE_TIME_ESTIMATE_MAX; // number of cycles the brake will be applied, updated during braking mode.
poshold.braking_time_updated_pitch = false; // flag the braking time can be re-estimated
}
// final lean angle should be pilot input plus wind compensation
poshold.pitch = poshold.pilot_pitch + poshold.wind_comp_pitch;
break;
case POSHOLD_BRAKE:
case POSHOLD_BRAKE_READY_TO_LOITER:
// calculate brake_pitch angle to counter-act velocity
poshold_update_brake_angle_from_velocity(poshold.brake_pitch, -vel_fw);
// update braking time estimate
if (!poshold.braking_time_updated_pitch) {
// check if brake angle is increasing
if (abs(poshold.brake_pitch) >= poshold.brake_angle_max_pitch) {
poshold.brake_angle_max_pitch = abs(poshold.brake_pitch);
} else {
// braking angle has started decreasing so re-estimate braking time
poshold.brake_timeout_pitch = 1+(uint16_t)(LOOP_RATE_FACTOR*15L*(int32_t)(abs(poshold.brake_pitch))/(10L*(int32_t)g.poshold_brake_rate)); // the 1.2 (12/10) factor has to be tuned in flight, here it means 120% of the "normal" time.
poshold.braking_time_updated_pitch = true;
}
}
// if velocity is very low reduce braking time to 0.5seconds
if ((fabsf(vel_fw) <= POSHOLD_SPEED_0) && (poshold.brake_timeout_pitch > 50*LOOP_RATE_FACTOR)) {
poshold.brake_timeout_pitch = 50*LOOP_RATE_FACTOR;
}
// reduce braking timer
if (poshold.brake_timeout_pitch > 0) {
poshold.brake_timeout_pitch--;
} else {
// indicate that we are ready to move to Loiter.
// Loiter will only actually be engaged once both pitch_mode and pitch_mode are changed to POSHOLD_BRAKE_READY_TO_LOITER
// logic for engaging loiter is handled below the pitch and pitch mode switch statements
poshold.pitch_mode = POSHOLD_BRAKE_READY_TO_LOITER;
}
// final lean angle is braking angle + wind compensation angle
poshold.pitch = poshold.brake_pitch + poshold.wind_comp_pitch;
// check for pilot input
if (!is_zero(target_pitch)) {
// init transition to pilot override
poshold_pitch_controller_to_pilot_override();
}
break;
case POSHOLD_BRAKE_TO_LOITER:
case POSHOLD_LOITER:
// these modes are combined pitch-pitch modes and are handled below
break;
case POSHOLD_CONTROLLER_TO_PILOT_OVERRIDE:
// update pilot desired pitch angle using latest radio input
// this filters the input so that it returns to zero no faster than the brake-rate
poshold_update_pilot_lean_angle(poshold.pilot_pitch, target_pitch);
// count-down loiter to pilot timer
if (poshold.controller_to_pilot_timer_pitch > 0) {
poshold.controller_to_pilot_timer_pitch--;
} else {
// when timer runs out switch to full pilot override for next iteration
poshold.pitch_mode = POSHOLD_PILOT_OVERRIDE;
}
// calculate controller_to_pilot mix ratio
controller_to_pilot_pitch_mix = (float)poshold.controller_to_pilot_timer_pitch / (float)POSHOLD_CONTROLLER_TO_PILOT_MIX_TIMER;
// mix final loiter lean angle and pilot desired lean angles
poshold.pitch = poshold_mix_controls(controller_to_pilot_pitch_mix, poshold.controller_final_pitch, poshold.pilot_pitch + poshold.wind_comp_pitch);
break;
}
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
//
// Shared roll & pitch states (POSHOLD_BRAKE_TO_LOITER and POSHOLD_LOITER)
//
// switch into LOITER mode when both roll and pitch are ready
if (poshold.roll_mode == POSHOLD_BRAKE_READY_TO_LOITER && poshold.pitch_mode == POSHOLD_BRAKE_READY_TO_LOITER) {
poshold.roll_mode = POSHOLD_BRAKE_TO_LOITER;
poshold.pitch_mode = POSHOLD_BRAKE_TO_LOITER;
poshold.brake_to_loiter_timer = POSHOLD_BRAKE_TO_LOITER_TIMER;
// init loiter controller
loiter_nav->init_target(inertial_nav.get_position());
// set delay to start of wind compensation estimate updates
poshold.wind_comp_start_timer = POSHOLD_WIND_COMP_START_TIMER;
}
// roll-mode is used as the combined roll+pitch mode when in BRAKE_TO_LOITER or LOITER modes
if (poshold.roll_mode == POSHOLD_BRAKE_TO_LOITER || poshold.roll_mode == POSHOLD_LOITER) {
// force pitch mode to be same as roll_mode just to keep it consistent (it's not actually used in these states)
poshold.pitch_mode = poshold.roll_mode;
// handle combined roll+pitch mode
switch (poshold.roll_mode) {
case POSHOLD_BRAKE_TO_LOITER:
// reduce brake_to_loiter timer
if (poshold.brake_to_loiter_timer > 0) {
poshold.brake_to_loiter_timer--;
} else {
// progress to full loiter on next iteration
poshold.roll_mode = POSHOLD_LOITER;
poshold.pitch_mode = POSHOLD_LOITER;
}
// calculate percentage mix of loiter and brake control
brake_to_loiter_mix = (float)poshold.brake_to_loiter_timer / (float)POSHOLD_BRAKE_TO_LOITER_TIMER;
// calculate brake_roll and pitch angles to counter-act velocity
poshold_update_brake_angle_from_velocity(poshold.brake_roll, vel_right);
poshold_update_brake_angle_from_velocity(poshold.brake_pitch, -vel_fw);
// run loiter controller
loiter_nav->update();
// calculate final roll and pitch output by mixing loiter and brake controls
poshold.roll = poshold_mix_controls(brake_to_loiter_mix, poshold.brake_roll + poshold.wind_comp_roll, loiter_nav->get_roll());
poshold.pitch = poshold_mix_controls(brake_to_loiter_mix, poshold.brake_pitch + poshold.wind_comp_pitch, loiter_nav->get_pitch());
// check for pilot input
if (!is_zero(target_roll) || !is_zero(target_pitch)) {
// if roll input switch to pilot override for roll
if (!is_zero(target_roll)) {
// init transition to pilot override
poshold_roll_controller_to_pilot_override();
// switch pitch-mode to brake (but ready to go back to loiter anytime)
// no need to reset poshold.brake_pitch here as wind comp has not been updated since last brake_pitch computation
poshold.pitch_mode = POSHOLD_BRAKE_READY_TO_LOITER;
}
// if pitch input switch to pilot override for pitch
if (!is_zero(target_pitch)) {
// init transition to pilot override
poshold_pitch_controller_to_pilot_override();
if (is_zero(target_roll)) {
// switch roll-mode to brake (but ready to go back to loiter anytime)
// no need to reset poshold.brake_roll here as wind comp has not been updated since last brake_roll computation
poshold.roll_mode = POSHOLD_BRAKE_READY_TO_LOITER;
}
}
}
break;
case POSHOLD_LOITER:
// run loiter controller
loiter_nav->update();
// set roll angle based on loiter controller outputs
poshold.roll = loiter_nav->get_roll();
poshold.pitch = loiter_nav->get_pitch();
// update wind compensation estimate
poshold_update_wind_comp_estimate();
// check for pilot input
if (!is_zero(target_roll) || !is_zero(target_pitch)) {
// if roll input switch to pilot override for roll
if (!is_zero(target_roll)) {
// init transition to pilot override
poshold_roll_controller_to_pilot_override();
// switch pitch-mode to brake (but ready to go back to loiter anytime)
poshold.pitch_mode = POSHOLD_BRAKE_READY_TO_LOITER;
// reset brake_pitch because wind_comp is now different and should give the compensation of the whole previous loiter angle
poshold.brake_pitch = 0;
}
// if pitch input switch to pilot override for pitch
if (!is_zero(target_pitch)) {
// init transition to pilot override
poshold_pitch_controller_to_pilot_override();
// if roll not overridden switch roll-mode to brake (but be ready to go back to loiter any time)
if (is_zero(target_roll)) {
poshold.roll_mode = POSHOLD_BRAKE_READY_TO_LOITER;
poshold.brake_roll = 0;
}
}
}
break;
default:
// do nothing for uncombined roll and pitch modes
break;
}
}
// constrain target pitch/roll angles
float angle_max = copter.aparm.angle_max;
poshold.roll = constrain_int16(poshold.roll, -angle_max, angle_max);
poshold.pitch = constrain_int16(poshold.pitch, -angle_max, angle_max);
// update attitude controller targets
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(poshold.roll, poshold.pitch, target_yaw_rate);
// adjust climb rate using rangefinder
target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt);
// get take-off adjusted pilot and takeoff climb rates
takeoff.get_climb_rates(target_climb_rate, takeoff_climb_rate);
// get avoidance adjusted climb rate
target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);
// update altitude target and call position controller
// init and update loiter although pilot is controlling lean angles
loiter_nav->init_target();
loiter_nav->update();
// set position controller targets
pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
pos_control->add_takeoff_climb_rate(takeoff_climb_rate, G_Dt);
pos_control->update_z_controller();
// set poshold state to pilot override
poshold.roll_mode = POSHOLD_PILOT_OVERRIDE;
poshold.pitch_mode = POSHOLD_PILOT_OVERRIDE;
break;
case AltHold_Landed_Ground_Idle:
loiter_nav->init_target();
loiter_nav->update();
attitude_control->reset_rate_controller_I_terms();
attitude_control->set_yaw_target_to_current_heading();
// FALLTHROUGH
case AltHold_Landed_Pre_Takeoff:
pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero
// set poshold state to pilot override
poshold.roll_mode = POSHOLD_PILOT_OVERRIDE;
poshold.pitch_mode = POSHOLD_PILOT_OVERRIDE;
break;
case AltHold_Flying:
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
#if AC_AVOID_ENABLED == ENABLED
// apply avoidance
copter.avoid.adjust_roll_pitch(target_roll, target_pitch, copter.aparm.angle_max);
#endif
// adjust climb rate using rangefinder
if (copter.rangefinder_alt_ok()) {
// if rangefinder is ok, use surface tracking
target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt);
}
// get avoidance adjusted climb rate
target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);
pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
break;
}
// poshold specific behaviour to calculate desired roll, pitch angles
// convert inertial nav earth-frame velocities to body-frame
// To-Do: move this to AP_Math (or perhaps we already have a function to do this)
float vel_fw = vel.x*ahrs.cos_yaw() + vel.y*ahrs.sin_yaw();
float vel_right = -vel.x*ahrs.sin_yaw() + vel.y*ahrs.cos_yaw();
// If not in LOITER, retrieve latest wind compensation lean angles related to current yaw
if (poshold.roll_mode != POSHOLD_LOITER || poshold.pitch_mode != POSHOLD_LOITER) {
poshold_get_wind_comp_lean_angles(poshold.wind_comp_roll, poshold.wind_comp_pitch);
}
// Roll state machine
// Each state (aka mode) is responsible for:
// 1. dealing with pilot input
// 2. calculating the final roll output to the attitude controller
// 3. checking if the state (aka mode) should be changed and if 'yes' perform any required initialisation for the new state
switch (poshold.roll_mode) {
case POSHOLD_PILOT_OVERRIDE:
// update pilot desired roll angle using latest radio input
// this filters the input so that it returns to zero no faster than the brake-rate
poshold_update_pilot_lean_angle(poshold.pilot_roll, target_roll);
// switch to BRAKE mode for next iteration if no pilot input
if (is_zero(target_roll) && (fabsf(poshold.pilot_roll) < 2 * g.poshold_brake_rate)) {
// initialise BRAKE mode
poshold.roll_mode = POSHOLD_BRAKE; // Set brake roll mode
poshold.brake_roll = 0; // initialise braking angle to zero
poshold.brake_angle_max_roll = 0; // reset brake_angle_max so we can detect when vehicle begins to flatten out during braking
poshold.brake_timeout_roll = POSHOLD_BRAKE_TIME_ESTIMATE_MAX; // number of cycles the brake will be applied, updated during braking mode.
poshold.braking_time_updated_roll = false; // flag the braking time can be re-estimated
}
// final lean angle should be pilot input plus wind compensation
poshold.roll = poshold.pilot_roll + poshold.wind_comp_roll;
break;
case POSHOLD_BRAKE:
case POSHOLD_BRAKE_READY_TO_LOITER:
// calculate brake_roll angle to counter-act velocity
poshold_update_brake_angle_from_velocity(poshold.brake_roll, vel_right);
// update braking time estimate
if (!poshold.braking_time_updated_roll) {
// check if brake angle is increasing
if (abs(poshold.brake_roll) >= poshold.brake_angle_max_roll) {
poshold.brake_angle_max_roll = abs(poshold.brake_roll);
} else {
// braking angle has started decreasing so re-estimate braking time
poshold.brake_timeout_roll = 1+(uint16_t)(LOOP_RATE_FACTOR*15L*(int32_t)(abs(poshold.brake_roll))/(10L*(int32_t)g.poshold_brake_rate)); // the 1.2 (12/10) factor has to be tuned in flight, here it means 120% of the "normal" time.
poshold.braking_time_updated_roll = true;
}
}
// if velocity is very low reduce braking time to 0.5seconds
if ((fabsf(vel_right) <= POSHOLD_SPEED_0) && (poshold.brake_timeout_roll > 50*LOOP_RATE_FACTOR)) {
poshold.brake_timeout_roll = 50*LOOP_RATE_FACTOR;
}
// reduce braking timer
if (poshold.brake_timeout_roll > 0) {
poshold.brake_timeout_roll--;
} else {
// indicate that we are ready to move to Loiter.
// Loiter will only actually be engaged once both roll_mode and pitch_mode are changed to POSHOLD_BRAKE_READY_TO_LOITER
// logic for engaging loiter is handled below the roll and pitch mode switch statements
poshold.roll_mode = POSHOLD_BRAKE_READY_TO_LOITER;
}
// final lean angle is braking angle + wind compensation angle
poshold.roll = poshold.brake_roll + poshold.wind_comp_roll;
// check for pilot input
if (!is_zero(target_roll)) {
// init transition to pilot override
poshold_roll_controller_to_pilot_override();
}
break;
case POSHOLD_BRAKE_TO_LOITER:
case POSHOLD_LOITER:
// these modes are combined roll-pitch modes and are handled below
break;
case POSHOLD_CONTROLLER_TO_PILOT_OVERRIDE:
// update pilot desired roll angle using latest radio input
// this filters the input so that it returns to zero no faster than the brake-rate
poshold_update_pilot_lean_angle(poshold.pilot_roll, target_roll);
// count-down loiter to pilot timer
if (poshold.controller_to_pilot_timer_roll > 0) {
poshold.controller_to_pilot_timer_roll--;
} else {
// when timer runs out switch to full pilot override for next iteration
poshold.roll_mode = POSHOLD_PILOT_OVERRIDE;
}
// calculate controller_to_pilot mix ratio
controller_to_pilot_roll_mix = (float)poshold.controller_to_pilot_timer_roll / (float)POSHOLD_CONTROLLER_TO_PILOT_MIX_TIMER;
// mix final loiter lean angle and pilot desired lean angles
poshold.roll = poshold_mix_controls(controller_to_pilot_roll_mix, poshold.controller_final_roll, poshold.pilot_roll + poshold.wind_comp_roll);
break;
}
// Pitch state machine
// Each state (aka mode) is responsible for:
// 1. dealing with pilot input
// 2. calculating the final pitch output to the attitude contpitcher
// 3. checking if the state (aka mode) should be changed and if 'yes' perform any required initialisation for the new state
switch (poshold.pitch_mode) {
case POSHOLD_PILOT_OVERRIDE:
// update pilot desired pitch angle using latest radio input
// this filters the input so that it returns to zero no faster than the brake-rate
poshold_update_pilot_lean_angle(poshold.pilot_pitch, target_pitch);
// switch to BRAKE mode for next iteration if no pilot input
if (is_zero(target_pitch) && (fabsf(poshold.pilot_pitch) < 2 * g.poshold_brake_rate)) {
// initialise BRAKE mode
poshold.pitch_mode = POSHOLD_BRAKE; // set brake pitch mode
poshold.brake_pitch = 0; // initialise braking angle to zero
poshold.brake_angle_max_pitch = 0; // reset brake_angle_max so we can detect when vehicle begins to flatten out during braking
poshold.brake_timeout_pitch = POSHOLD_BRAKE_TIME_ESTIMATE_MAX; // number of cycles the brake will be applied, updated during braking mode.
poshold.braking_time_updated_pitch = false; // flag the braking time can be re-estimated
}
// final lean angle should be pilot input plus wind compensation
poshold.pitch = poshold.pilot_pitch + poshold.wind_comp_pitch;
break;
case POSHOLD_BRAKE:
case POSHOLD_BRAKE_READY_TO_LOITER:
// calculate brake_pitch angle to counter-act velocity
poshold_update_brake_angle_from_velocity(poshold.brake_pitch, -vel_fw);
// update braking time estimate
if (!poshold.braking_time_updated_pitch) {
// check if brake angle is increasing
if (abs(poshold.brake_pitch) >= poshold.brake_angle_max_pitch) {
poshold.brake_angle_max_pitch = abs(poshold.brake_pitch);
} else {
// braking angle has started decreasing so re-estimate braking time
poshold.brake_timeout_pitch = 1+(uint16_t)(LOOP_RATE_FACTOR*15L*(int32_t)(abs(poshold.brake_pitch))/(10L*(int32_t)g.poshold_brake_rate)); // the 1.2 (12/10) factor has to be tuned in flight, here it means 120% of the "normal" time.
poshold.braking_time_updated_pitch = true;
}
}
// if velocity is very low reduce braking time to 0.5seconds
if ((fabsf(vel_fw) <= POSHOLD_SPEED_0) && (poshold.brake_timeout_pitch > 50*LOOP_RATE_FACTOR)) {
poshold.brake_timeout_pitch = 50*LOOP_RATE_FACTOR;
}
// reduce braking timer
if (poshold.brake_timeout_pitch > 0) {
poshold.brake_timeout_pitch--;
} else {
// indicate that we are ready to move to Loiter.
// Loiter will only actually be engaged once both pitch_mode and pitch_mode are changed to POSHOLD_BRAKE_READY_TO_LOITER
// logic for engaging loiter is handled below the pitch and pitch mode switch statements
poshold.pitch_mode = POSHOLD_BRAKE_READY_TO_LOITER;
}
// final lean angle is braking angle + wind compensation angle
poshold.pitch = poshold.brake_pitch + poshold.wind_comp_pitch;
// check for pilot input
if (!is_zero(target_pitch)) {
// init transition to pilot override
poshold_pitch_controller_to_pilot_override();
}
break;
case POSHOLD_BRAKE_TO_LOITER:
case POSHOLD_LOITER:
// these modes are combined pitch-pitch modes and are handled below
break;
case POSHOLD_CONTROLLER_TO_PILOT_OVERRIDE:
// update pilot desired pitch angle using latest radio input
// this filters the input so that it returns to zero no faster than the brake-rate
poshold_update_pilot_lean_angle(poshold.pilot_pitch, target_pitch);
// count-down loiter to pilot timer
if (poshold.controller_to_pilot_timer_pitch > 0) {
poshold.controller_to_pilot_timer_pitch--;
} else {
// when timer runs out switch to full pilot override for next iteration
poshold.pitch_mode = POSHOLD_PILOT_OVERRIDE;
}
// calculate controller_to_pilot mix ratio
controller_to_pilot_pitch_mix = (float)poshold.controller_to_pilot_timer_pitch / (float)POSHOLD_CONTROLLER_TO_PILOT_MIX_TIMER;
// mix final loiter lean angle and pilot desired lean angles
poshold.pitch = poshold_mix_controls(controller_to_pilot_pitch_mix, poshold.controller_final_pitch, poshold.pilot_pitch + poshold.wind_comp_pitch);
break;
}
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
//
// Shared roll & pitch states (POSHOLD_BRAKE_TO_LOITER and POSHOLD_LOITER)
//
// switch into LOITER mode when both roll and pitch are ready
if (poshold.roll_mode == POSHOLD_BRAKE_READY_TO_LOITER && poshold.pitch_mode == POSHOLD_BRAKE_READY_TO_LOITER) {
poshold.roll_mode = POSHOLD_BRAKE_TO_LOITER;
poshold.pitch_mode = POSHOLD_BRAKE_TO_LOITER;
poshold.brake_to_loiter_timer = POSHOLD_BRAKE_TO_LOITER_TIMER;
// init loiter controller
loiter_nav->init_target(inertial_nav.get_position());
// set delay to start of wind compensation estimate updates
poshold.wind_comp_start_timer = POSHOLD_WIND_COMP_START_TIMER;
}
// roll-mode is used as the combined roll+pitch mode when in BRAKE_TO_LOITER or LOITER modes
if (poshold.roll_mode == POSHOLD_BRAKE_TO_LOITER || poshold.roll_mode == POSHOLD_LOITER) {
// force pitch mode to be same as roll_mode just to keep it consistent (it's not actually used in these states)
poshold.pitch_mode = poshold.roll_mode;
// handle combined roll+pitch mode
switch (poshold.roll_mode) {
case POSHOLD_BRAKE_TO_LOITER:
// reduce brake_to_loiter timer
if (poshold.brake_to_loiter_timer > 0) {
poshold.brake_to_loiter_timer--;
} else {
// progress to full loiter on next iteration
poshold.roll_mode = POSHOLD_LOITER;
poshold.pitch_mode = POSHOLD_LOITER;
}
// calculate percentage mix of loiter and brake control
brake_to_loiter_mix = (float)poshold.brake_to_loiter_timer / (float)POSHOLD_BRAKE_TO_LOITER_TIMER;
// calculate brake_roll and pitch angles to counter-act velocity
poshold_update_brake_angle_from_velocity(poshold.brake_roll, vel_right);
poshold_update_brake_angle_from_velocity(poshold.brake_pitch, -vel_fw);
// run loiter controller
loiter_nav->update();
// calculate final roll and pitch output by mixing loiter and brake controls
poshold.roll = poshold_mix_controls(brake_to_loiter_mix, poshold.brake_roll + poshold.wind_comp_roll, loiter_nav->get_roll());
poshold.pitch = poshold_mix_controls(brake_to_loiter_mix, poshold.brake_pitch + poshold.wind_comp_pitch, loiter_nav->get_pitch());
// check for pilot input
if (!is_zero(target_roll) || !is_zero(target_pitch)) {
// if roll input switch to pilot override for roll
if (!is_zero(target_roll)) {
// init transition to pilot override
poshold_roll_controller_to_pilot_override();
// switch pitch-mode to brake (but ready to go back to loiter anytime)
// no need to reset poshold.brake_pitch here as wind comp has not been updated since last brake_pitch computation
poshold.pitch_mode = POSHOLD_BRAKE_READY_TO_LOITER;
}
// if pitch input switch to pilot override for pitch
if (!is_zero(target_pitch)) {
// init transition to pilot override
poshold_pitch_controller_to_pilot_override();
if (is_zero(target_roll)) {
// switch roll-mode to brake (but ready to go back to loiter anytime)
// no need to reset poshold.brake_roll here as wind comp has not been updated since last brake_roll computation
poshold.roll_mode = POSHOLD_BRAKE_READY_TO_LOITER;
}
}
}
break;
case POSHOLD_LOITER:
// run loiter controller
loiter_nav->update();
// set roll angle based on loiter controller outputs
poshold.roll = loiter_nav->get_roll();
poshold.pitch = loiter_nav->get_pitch();
// update wind compensation estimate
poshold_update_wind_comp_estimate();
// check for pilot input
if (!is_zero(target_roll) || !is_zero(target_pitch)) {
// if roll input switch to pilot override for roll
if (!is_zero(target_roll)) {
// init transition to pilot override
poshold_roll_controller_to_pilot_override();
// switch pitch-mode to brake (but ready to go back to loiter anytime)
poshold.pitch_mode = POSHOLD_BRAKE_READY_TO_LOITER;
// reset brake_pitch because wind_comp is now different and should give the compensation of the whole previous loiter angle
poshold.brake_pitch = 0;
}
// if pitch input switch to pilot override for pitch
if (!is_zero(target_pitch)) {
// init transition to pilot override
poshold_pitch_controller_to_pilot_override();
// if roll not overriden switch roll-mode to brake (but be ready to go back to loiter any time)
if (is_zero(target_roll)) {
poshold.roll_mode = POSHOLD_BRAKE_READY_TO_LOITER;
poshold.brake_roll = 0;
}
// if roll not overridden switch roll-mode to brake (but be ready to go back to loiter any time)
}
}
break;
default:
// do nothing for uncombined roll and pitch modes
break;
}
}
// constrain target pitch/roll angles
float angle_max = copter.aparm.angle_max;
poshold.roll = constrain_int16(poshold.roll, -angle_max, angle_max);
poshold.pitch = constrain_int16(poshold.pitch, -angle_max, angle_max);
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(poshold.roll, poshold.pitch, target_yaw_rate);
// call z-axis position controller
pos_control->update_z_controller();
}
// poshold_update_pilot_lean_angle - update the pilot's filtered lean angle with the latest raw input received

47
ArduCopter/mode_rtl.cpp
View File

@ -127,10 +127,9 @@ void Copter::ModeRTL::return_start()
// called by rtl_run at 100hz or more
void Copter::ModeRTL::climb_return_run()
{
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) {
zero_throttle_and_relax_ac();
// To-Do: re-initialise wpnav targets
// if not armed set throttle to zero and exit immediately
if (!motors->armed() || !ap.auto_armed || ap.land_complete) {
make_safe_shut_down();
return;
}
@ -185,10 +184,9 @@ void Copter::ModeRTL::loiterathome_start()
// called by rtl_run at 100hz or more
void Copter::ModeRTL::loiterathome_run()
{
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) {
zero_throttle_and_relax_ac();
// To-Do: re-initialise wpnav targets
// if not armed set throttle to zero and exit immediately
if (!motors->armed() || !ap.auto_armed || ap.land_complete) {
make_safe_shut_down();
return;
}
@ -258,12 +256,9 @@ void Copter::ModeRTL::descent_run()
float target_pitch = 0.0f;
float target_yaw_rate = 0.0f;
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) {
zero_throttle_and_relax_ac();
// set target to current position
loiter_nav->clear_pilot_desired_acceleration();
loiter_nav->init_target();
// if not armed set throttle to zero and exit immediately
if (!motors->armed() || !ap.auto_armed || ap.land_complete) {
make_safe_shut_down();
return;
}
@ -358,20 +353,17 @@ bool Copter::ModeRTL::landing_gear_should_be_deployed() const
// called by rtl_run at 100hz or more
void Copter::ModeRTL::land_run(bool disarm_on_land)
{
// if not auto armed or landing completed or motor interlock not enabled set throttle to zero and exit immediately
if (!motors->armed() || !ap.auto_armed || ap.land_complete || !motors->get_interlock()) {
zero_throttle_and_relax_ac();
// set target to current position
loiter_nav->clear_pilot_desired_acceleration();
loiter_nav->init_target();
// check if we've completed this stage of RTL
_state_complete = ap.land_complete;
// disarm when the landing detector says we've landed
if (ap.land_complete && disarm_on_land) {
copter.init_disarm_motors();
}
// disarm when the landing detector says we've landed
if (disarm_on_land && ap.land_complete) {
copter.init_disarm_motors();
}
// check if we've completed this stage of RTL
_state_complete = ap.land_complete;
// if not armed set throttle to zero and exit immediately
if (!motors->armed() || !ap.auto_armed || ap.land_complete) {
make_safe_shut_down();
return;
}
@ -380,9 +372,6 @@ void Copter::ModeRTL::land_run(bool disarm_on_land)
land_run_horizontal_control();
land_run_vertical_control();
// check if we've completed this stage of RTL
_state_complete = ap.land_complete;
}
void Copter::ModeRTL::build_path(bool terrain_following_allowed)

63
ArduCopter/mode_sport.cpp
View File

@ -26,7 +26,6 @@ bool Copter::ModeSport::init(bool ignore_checks)
// should be called at 100hz or more
void Copter::ModeSport::run()
{
SportModeState sport_state;
float takeoff_climb_rate = 0.0f;
// initialize vertical speed and acceleration
@ -73,39 +72,20 @@ void Copter::ModeSport::run()
float target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in());
target_climb_rate = constrain_float(target_climb_rate, -get_pilot_speed_dn(), g.pilot_speed_up);
// State Machine Determination
if (!motors->armed() || !motors->get_interlock()) {
sport_state = Sport_MotorStopped;
} else if (takeoff.running() || takeoff.triggered(target_climb_rate)) {
sport_state = Sport_Takeoff;
} else if (!ap.auto_armed || ap.land_complete) {
sport_state = Sport_Landed;
} else {
sport_state = Sport_Flying;
}
// Sport State Machine Determination
AltHoldModeState sport_state = get_alt_hold_state(target_climb_rate);
// State Machine
switch (sport_state) {
case Sport_MotorStopped:
case AltHold_MotorStopped:
motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN);
attitude_control->input_euler_rate_roll_pitch_yaw(target_roll_rate, target_pitch_rate, target_yaw_rate);
#if FRAME_CONFIG == HELI_FRAME
// force descent rate and call position controller
pos_control->set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false);
#else
attitude_control->relax_attitude_controllers();
attitude_control->reset_rate_controller_I_terms();
attitude_control->set_yaw_target_to_current_heading();
pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero
#endif
pos_control->update_z_controller();
break;
case Sport_Takeoff:
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
case AltHold_Takeoff:
// initiate take-off
if (!takeoff.running()) {
@ -122,38 +102,23 @@ void Copter::ModeSport::run()
// get avoidance adjusted climb rate
target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);
// call attitude controller
attitude_control->input_euler_rate_roll_pitch_yaw(target_roll_rate, target_pitch_rate, target_yaw_rate);
// call position controller
pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
pos_control->add_takeoff_climb_rate(takeoff_climb_rate, G_Dt);
pos_control->update_z_controller();
break;
case Sport_Landed:
#if FRAME_CONFIG == HELI_FRAME
// helicopters do not spool down when landed. Only when commanded to go to ground idle by pilot.
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
#else
// set motors to spin-when-armed if throttle below deadzone, otherwise full range (but motors will only spin at min throttle)
if (target_climb_rate < 0.0f) {
motors->set_desired_spool_state(AP_Motors::DESIRED_GROUND_IDLE);
} else {
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
}
#endif
case AltHold_Landed_Ground_Idle:
attitude_control->reset_rate_controller_I_terms();
attitude_control->set_yaw_target_to_current_heading();
attitude_control->input_euler_rate_roll_pitch_yaw(target_roll_rate, target_pitch_rate, target_yaw_rate);
// FALLTHROUGH
case AltHold_Landed_Pre_Takeoff:
pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero
pos_control->update_z_controller();
break;
case Sport_Flying:
case AltHold_Flying:
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// call attitude controller
attitude_control->input_euler_rate_roll_pitch_yaw(target_roll_rate, target_pitch_rate, target_yaw_rate);
// adjust climb rate using rangefinder
target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt);
@ -161,11 +126,15 @@ void Copter::ModeSport::run()
// get avoidance adjusted climb rate
target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate);
// call position controller
pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
pos_control->update_z_controller();
break;
}
// call attitude controller
attitude_control->input_euler_rate_roll_pitch_yaw(target_roll_rate, target_pitch_rate, target_yaw_rate);
// call z-axis position controller
pos_control->update_z_controller();
}
#endif

44
ArduCopter/mode_stabilize.cpp
View File

@ -8,28 +8,40 @@
// should be called at 100hz or more
void Copter::ModeStabilize::run()
{
float target_roll, target_pitch;
float target_yaw_rate;
// if not armed set throttle to zero and exit immediately
if (!motors->armed() || ap.throttle_zero || !motors->get_interlock()) {
zero_throttle_and_relax_ac();
return;
}
// clear landing flag
set_land_complete(false);
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// apply SIMPLE mode transform to pilot inputs
// apply simple mode transform to pilot inputs
update_simple_mode();
// convert pilot input to lean angles
float target_roll, target_pitch;
get_pilot_desired_lean_angles(target_roll, target_pitch, copter.aparm.angle_max, copter.aparm.angle_max);
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
if (!motors->armed()) {
// Motors should be Stopped
motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN);
} else if (ap.throttle_zero) {
// Attempting to Land
motors->set_desired_spool_state(AP_Motors::DESIRED_GROUND_IDLE);
} else {
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
}
if (motors->get_spool_mode() == AP_Motors::SHUT_DOWN) {
// Motors Stopped
attitude_control->set_yaw_target_to_current_heading();
attitude_control->reset_rate_controller_I_terms();
} else if (motors->get_spool_mode() == AP_Motors::GROUND_IDLE) {
// Landed
attitude_control->set_yaw_target_to_current_heading();
attitude_control->reset_rate_controller_I_terms();
} else if (motors->get_spool_mode() == AP_Motors::THROTTLE_UNLIMITED) {
// clear landing flag above zero throttle
if (!motors->limit.throttle_lower) {
set_land_complete(false);
}
}
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);

6
ArduCopter/mode_throw.cpp
View File

@ -35,7 +35,7 @@ void Copter::ModeThrow::run()
*/
// Don't enter THROW mode if interlock will prevent motors running
if (!motors->armed() && motors->get_interlock()) {
if (!motors->armed()) {
// state machine entry is always from a disarmed state
stage = Throw_Disarmed;
@ -116,7 +116,7 @@ void Copter::ModeThrow::run()
}
// demand zero throttle (motors will be stopped anyway) and continually reset the attitude controller
attitude_control->set_throttle_out_unstabilized(0,true,g.throttle_filt);
attitude_control->set_throttle_out(0,true,g.throttle_filt);
break;
case Throw_Detecting:
@ -129,7 +129,7 @@ void Copter::ModeThrow::run()
}
// Hold throttle at zero during the throw and continually reset the attitude controller
attitude_control->set_throttle_out_unstabilized(0,true,g.throttle_filt);
attitude_control->set_throttle_out(0,true,g.throttle_filt);
// Play the waiting for throw tone sequence to alert the user
AP_Notify::flags.waiting_for_throw = true;