// helper function to update position controller's desired velocity while respecting acceleration limits
void Copter::ModeGuided::set_desired_velocity_with_accel_and_fence_limits(const Vector3f& vel_des)
{
// get current desired velocity
Vector3f curr_vel_des = pos_control->get_desired_velocity();
// get change in desired velocity
Vector3f vel_delta = vel_des - curr_vel_des;
// limit xy change
float vel_delta_xy = safe_sqrt(sq(vel_delta.x)+sq(vel_delta.y));
float vel_delta_xy_max = G_Dt * pos_control->get_max_accel_xy();
float ratio_xy = 1.0f;
if (!is_zero(vel_delta_xy) && (vel_delta_xy > vel_delta_xy_max)) {
ratio_xy = vel_delta_xy_max / vel_delta_xy;
}
curr_vel_des.x += (vel_delta.x * ratio_xy);
curr_vel_des.y += (vel_delta.y * ratio_xy);
// limit z change
float vel_delta_z_max = G_Dt * pos_control->get_max_accel_z();
curr_vel_des.z += constrain_float(vel_delta.z, -vel_delta_z_max, vel_delta_z_max);
#if AC_AVOID_ENABLED
// limit the velocity to prevent fence violations
copter.avoid.adjust_velocity(pos_control->get_pos_xy_p().kP(), pos_control->get_max_accel_xy(), curr_vel_des, G_Dt);
// get avoidance adjusted climb rate
curr_vel_des.z = get_avoidance_adjusted_climbrate(curr_vel_des.z);
#endif
// update position controller with new target
pos_control->set_desired_velocity(curr_vel_des);
}
// guided_angle_control_run - runs the guided angle controller
// 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;
float total_in = norm(roll_in, pitch_in);
float angle_max = MIN(attitude_control->get_althold_lean_angle_max(), copter.aparm.angle_max);
if (total_in > angle_max) {
float ratio = angle_max / total_in;
roll_in *= ratio;
pitch_in *= ratio;
}
// wrap yaw request
float yaw_in = wrap_180_cd(guided_angle_state.yaw_cd);
float yaw_rate_in = wrap_180_cd(guided_angle_state.yaw_rate_cds);
// constrain climb rate
float climb_rate_cms = constrain_float(guided_angle_state.climb_rate_cms, -fabsf(wp_nav->get_default_speed_down()), wp_nav->get_default_speed_up());
// get avoidance adjusted climb rate
climb_rate_cms = get_avoidance_adjusted_climbrate(climb_rate_cms);
// check for timeout - set lean angles and climb rate to zero if no updates received for 3 seconds
uint32_t tnow = millis();
if (tnow - guided_angle_state.update_time_ms > GUIDED_ATTITUDE_TIMEOUT_MS) {
roll_in = 0.0f;
pitch_in = 0.0f;
climb_rate_cms = 0.0f;
yaw_rate_in = 0.0f;
}
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// call attitude controller
if (guided_angle_state.use_yaw_rate) {
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(roll_in, pitch_in, yaw_rate_in);
} else {
attitude_control->input_euler_angle_roll_pitch_yaw(roll_in, pitch_in, yaw_in, true);
}
// call position controller
pos_control->set_alt_target_from_climb_rate_ff(climb_rate_cms, G_Dt, false);
pos_control->update_z_controller();
}
// manual_control - process manual control
void Copter::ModeZigZag::manual_control()
{
float target_yaw_rate = 0.0f;
float target_climb_rate = 0.0f;
// process pilot inputs unless we are in radio failsafe
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
get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max());
// process pilot's roll and pitch input
loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch, G_Dt);
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
// get pilot desired climb rate
target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in());
// make sure the climb rate is in the given range, prevent floating point errors
target_climb_rate = constrain_float(target_climb_rate, -get_pilot_speed_dn(), g.pilot_speed_up);
} 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();
}
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// run loiter controller
loiter_nav->update();
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(loiter_nav->get_roll(), loiter_nav->get_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);
// update altitude target and call position controller
pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
// adjusts target up or down using a climb rate
pos_control->update_z_controller();
}
// loiter_run - runs the loiter controller
// should be called at 100hz or more
void Copter::ModeLoiter::run()
{
float target_roll, target_pitch;
float target_yaw_rate = 0.0f;
float target_climb_rate = 0.0f;
float takeoff_climb_rate = 0.0f;
// initialize vertical speed and 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);
// process pilot inputs unless we are in radio failsafe
if (!copter.failsafe.radio) {
// apply SIMPLE mode transform to pilot inputs
update_simple_mode();
// convert pilot input to lean angles
get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max());
// process pilot's roll and pitch input
loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch, G_Dt);
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
// get pilot desired climb rate
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);
} 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();
}
// relax loiter target if we might be landed
if (ap.land_complete_maybe) {
loiter_nav->soften_for_landing();
}
// Loiter State Machine Determination
AltHoldModeState loiter_state = get_alt_hold_state(target_climb_rate);
// Loiter State Machine
switch (loiter_state) {
case AltHold_MotorStopped:
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
loiter_nav->init_target();
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(loiter_nav->get_roll(), loiter_nav->get_pitch(), target_yaw_rate);
pos_control->update_z_controller();
break;
case AltHold_Takeoff:
// 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 term when we're taking off
set_throttle_takeoff();
}
// get takeoff 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);
// run loiter controller
loiter_nav->update();
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(loiter_nav->get_roll(), loiter_nav->get_pitch(), target_yaw_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->add_takeoff_climb_rate(takeoff_climb_rate, G_Dt);
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:
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 AltHold_Flying:
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
#if PRECISION_LANDING == ENABLED
if (do_precision_loiter()) {
precision_loiter_xy();
}
#endif
// run loiter controller
loiter_nav->update();
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(loiter_nav->get_roll(), loiter_nav->get_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);
pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
pos_control->update_z_controller();
break;
}
}
void Copter::ModeFollow::run()
{
// if not armed set throttle to zero and exit immediately
if (is_disarmed_or_landed()) {
make_safe_spool_down();
return;
}
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::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
// variables to be sent to velocity controller
Vector3f desired_velocity_neu_cms;
bool use_yaw = false;
float yaw_cd = 0.0f;
Vector3f dist_vec; // vector to lead vehicle
Vector3f dist_vec_offs; // vector to lead vehicle + offset
Vector3f vel_of_target; // velocity of lead vehicle
if (g2.follow.get_target_dist_and_vel_ned(dist_vec, dist_vec_offs, vel_of_target)) {
// convert dist_vec_offs to cm in NEU
const Vector3f dist_vec_offs_neu(dist_vec_offs.x * 100.0f, dist_vec_offs.y * 100.0f, -dist_vec_offs.z * 100.0f);
// calculate desired velocity vector in cm/s in NEU
const float kp = g2.follow.get_pos_p().kP();
desired_velocity_neu_cms.x = (vel_of_target.x * 100.0f) + (dist_vec_offs_neu.x * kp);
desired_velocity_neu_cms.y = (vel_of_target.y * 100.0f) + (dist_vec_offs_neu.y * kp);
desired_velocity_neu_cms.z = (-vel_of_target.z * 100.0f) + (dist_vec_offs_neu.z * kp);
// scale desired velocity to stay within horizontal speed limit
float desired_speed_xy = safe_sqrt(sq(desired_velocity_neu_cms.x) + sq(desired_velocity_neu_cms.y));
if (!is_zero(desired_speed_xy) && (desired_speed_xy > pos_control->get_max_speed_xy())) {
const float scalar_xy = pos_control->get_max_speed_xy() / desired_speed_xy;
desired_velocity_neu_cms.x *= scalar_xy;
desired_velocity_neu_cms.y *= scalar_xy;
desired_speed_xy = pos_control->get_max_speed_xy();
}
// limit desired velocity to be between maximum climb and descent rates
desired_velocity_neu_cms.z = constrain_float(desired_velocity_neu_cms.z, -fabsf(pos_control->get_max_speed_down()), pos_control->get_max_speed_up());
// unit vector towards target position (i.e. vector to lead vehicle + offset)
Vector3f dir_to_target_neu = dist_vec_offs_neu;
const float dir_to_target_neu_len = dir_to_target_neu.length();
if (!is_zero(dir_to_target_neu_len)) {
dir_to_target_neu /= dir_to_target_neu_len;
}
// create horizontal desired velocity vector (required for slow down calculations)
Vector2f desired_velocity_xy_cms(desired_velocity_neu_cms.x, desired_velocity_neu_cms.y);
// create horizontal unit vector towards target (required for slow down calculations)
Vector2f dir_to_target_xy(desired_velocity_xy_cms.x, desired_velocity_xy_cms.y);
if (!dir_to_target_xy.is_zero()) {
dir_to_target_xy.normalize();
}
// slow down horizontally as we approach target (use 1/2 of maximum deceleration for gentle slow down)
const float dist_to_target_xy = Vector2f(dist_vec_offs_neu.x, dist_vec_offs_neu.y).length();
copter.avoid.limit_velocity(pos_control->get_pos_xy_p().kP().get(), pos_control->get_max_accel_xy() * 0.5f, desired_velocity_xy_cms, dir_to_target_xy, dist_to_target_xy, copter.G_Dt);
// limit the horizontal velocity to prevent fence violations
copter.avoid.adjust_velocity(pos_control->get_pos_xy_p().kP().get(), pos_control->get_max_accel_xy(), desired_velocity_xy_cms, G_Dt);
// copy horizontal velocity limits back to 3d vector
desired_velocity_neu_cms.x = desired_velocity_xy_cms.x;
desired_velocity_neu_cms.y = desired_velocity_xy_cms.y;
// limit vertical desired_velocity_neu_cms to slow as we approach target (we use 1/2 of maximum deceleration for gentle slow down)
const float des_vel_z_max = copter.avoid.get_max_speed(pos_control->get_pos_z_p().kP().get(), pos_control->get_max_accel_z() * 0.5f, fabsf(dist_vec_offs_neu.z), copter.G_Dt);
desired_velocity_neu_cms.z = constrain_float(desired_velocity_neu_cms.z, -des_vel_z_max, des_vel_z_max);
// get avoidance adjusted climb rate
desired_velocity_neu_cms.z = get_avoidance_adjusted_climbrate(desired_velocity_neu_cms.z);
// calculate vehicle heading
switch (g2.follow.get_yaw_behave()) {
case AP_Follow::YAW_BEHAVE_FACE_LEAD_VEHICLE: {
const Vector3f dist_vec_xy(dist_vec.x, dist_vec.y, 0.0f);
if (dist_vec_xy.length() > 1.0f) {
yaw_cd = get_bearing_cd(Vector3f(), dist_vec_xy);
use_yaw = true;
}
break;
}
case AP_Follow::YAW_BEHAVE_SAME_AS_LEAD_VEHICLE: {
float target_hdg = 0.0f;
if (g2.follow.get_target_heading_deg(target_hdg)) {
yaw_cd = target_hdg * 100.0f;
use_yaw = true;
}
break;
}
case AP_Follow::YAW_BEHAVE_DIR_OF_FLIGHT: {
const Vector3f vel_vec(desired_velocity_neu_cms.x, desired_velocity_neu_cms.y, 0.0f);
if (vel_vec.length() > 100.0f) {
yaw_cd = get_bearing_cd(Vector3f(), vel_vec);
use_yaw = true;
}
break;
}
case AP_Follow::YAW_BEHAVE_NONE:
default:
// do nothing
break;
}
}
// log output at 10hz
uint32_t now = AP_HAL::millis();
bool log_request = false;
if ((now - last_log_ms >= 100) || (last_log_ms == 0)) {
log_request = true;
last_log_ms = now;
}
// re-use guided mode's velocity controller (takes NEU)
Copter::ModeGuided::set_velocity(desired_velocity_neu_cms, use_yaw, yaw_cd, false, 0.0f, false, log_request);
Copter::ModeGuided::run();
}
// sport_run - runs the sport controller
// 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
pos_control->set_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
pos_control->set_accel_z(g.pilot_accel_z);
// apply SIMPLE mode transform
update_simple_mode();
// get pilot's desired roll and pitch rates
// calculate rate requests
float target_roll_rate = channel_roll->get_control_in() * g.acro_rp_p;
float target_pitch_rate = channel_pitch->get_control_in() * g.acro_rp_p;
// get attitude targets
const Vector3f att_target = attitude_control->get_att_target_euler_cd();
// Calculate trainer mode earth frame rate command for roll
int32_t roll_angle = wrap_180_cd(att_target.x);
target_roll_rate -= constrain_int32(roll_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE) * g.acro_balance_roll;
// Calculate trainer mode earth frame rate command for pitch
int32_t pitch_angle = wrap_180_cd(att_target.y);
target_pitch_rate -= constrain_int32(pitch_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE) * g.acro_balance_pitch;
AP_Vehicle::MultiCopter &aparm = copter.aparm;
if (roll_angle > aparm.angle_max){
target_roll_rate -= g.acro_rp_p*(roll_angle-aparm.angle_max);
}else if (roll_angle < -aparm.angle_max) {
target_roll_rate -= g.acro_rp_p*(roll_angle+aparm.angle_max);
}
if (pitch_angle > aparm.angle_max){
target_pitch_rate -= g.acro_rp_p*(pitch_angle-aparm.angle_max);
}else if (pitch_angle < -aparm.angle_max) {
target_pitch_rate -= g.acro_rp_p*(pitch_angle+aparm.angle_max);
}
// 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
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_state.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;
}
// State Machine
switch (sport_state) {
case Sport_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);
// initiate take-off
if (!takeoff_state.running) {
takeoff_timer_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();
}
// 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);
// 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:
// 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_SPIN_WHEN_ARMED);
} 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();
attitude_control->input_euler_rate_roll_pitch_yaw(target_roll_rate, target_pitch_rate, 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 Sport_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);
// 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;
}
}
// loiter_run - runs the loiter controller
// should be called at 100hz or more
void Copter::ModeLoiter::run()
{
LoiterModeState loiter_state;
float target_yaw_rate = 0.0f;
float target_climb_rate = 0.0f;
float takeoff_climb_rate = 0.0f;
// initialize vertical speed and acceleration
pos_control->set_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
pos_control->set_accel_z(g.pilot_accel_z);
// process pilot inputs unless we are in radio failsafe
if (!copter.failsafe.radio) {
// apply SIMPLE mode transform to pilot inputs
update_simple_mode();
// process pilot's roll and pitch input
wp_nav->set_pilot_desired_acceleration(channel_roll->get_control_in(), channel_pitch->get_control_in());
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
// get pilot desired climb rate
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);
} else {
// clear out pilot desired acceleration in case radio failsafe event occurs and we do not switch to RTL for some reason
wp_nav->clear_pilot_desired_acceleration();
}
// relax loiter target if we might be landed
if (ap.land_complete_maybe) {
wp_nav->loiter_soften_for_landing();
}
// Loiter State Machine Determination
if (!motors->armed() || !motors->get_interlock()) {
loiter_state = Loiter_MotorStopped;
} else if (takeoff_state.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;
}
// Loiter State Machine
switch (loiter_state) {
case Loiter_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);
#else
wp_nav->init_loiter_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
wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain());
pos_control->update_z_controller();
break;
case Loiter_Takeoff:
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// initiate take-off
if (!takeoff_state.running) {
takeoff_timer_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 takeoff 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);
// run loiter controller
wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain());
// update altitude target and 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 Loiter_Landed:
// 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_SPIN_WHEN_ARMED);
} else {
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
}
wp_nav->init_loiter_target();
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, get_smoothing_gain());
pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero
pos_control->update_z_controller();
break;
case Loiter_Flying:
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
#if PRECISION_LANDING == ENABLED
if (do_precision_loiter()) {
precision_loiter_xy();
}
#endif
// run loiter controller
wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain());
// 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);
// 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;
}
}
// poshold_run - runs the PosHold controller
// should be called at 100hz or more
void Copter::poshold_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 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
pos_control->set_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
pos_control->set_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 || !motors->get_interlock()) {
motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
wp_nav->init_loiter_target();
attitude_control->set_throttle_out_unstabilized(0,true,g.throttle_filt);
pos_control->relax_alt_hold_controllers(0.0f);
return;
}
// process pilot inputs
if (!failsafe.radio) {
// apply SIMPLE mode transform to pilot inputs
update_simple_mode();
// get pilot's desired yaw rate
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_state.running || (target_climb_rate > 0.0f && motors->rotor_runup_complete()))) {
#else
if (ap.land_complete && (takeoff_state.running || target_climb_rate > 0.0f)) {
#endif
if (!takeoff_state.running) {
takeoff_timer_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();
}
}
// relax loiter target if we might be landed
if (ap.land_complete_maybe) {
wp_nav->loiter_soften_for_landing();
}
// if landed initialise loiter targets, set throttle to zero and exit
if (ap.land_complete) {
// 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_SPIN_WHEN_ARMED);
} else {
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
}
wp_nav->init_loiter_target();
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, get_smoothing_gain());
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(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, aparm.angle_max);
// 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);
// 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
wp_nav->init_loiter_target(inertial_nav.get_position(), poshold.loiter_reset_I); // (false) to avoid I_term reset. In original code, velocity(0,0,0) was used instead of current velocity: wp_nav->init_loiter_target(inertial_nav.get_position(), Vector3f(0,0,0));
// at this stage, we are going to run update_loiter that will reset I_term once. From now, we ensure next time that we will enter loiter and update it, I_term won't be reset anymore
poshold.loiter_reset_I = false;
// 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
wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
// 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, wp_nav->get_roll());
poshold.pitch = poshold_mix_controls(brake_to_loiter_mix, poshold.brake_pitch + poshold.wind_comp_pitch, wp_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
wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
// set roll angle based on loiter controller outputs
poshold.roll = wp_nav->get_roll();
poshold.pitch = wp_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;
}
}
}
break;
default:
// do nothing for uncombined roll and pitch modes
break;
}
}
// constrain target pitch/roll angles
poshold.roll = constrain_int16(poshold.roll, -aparm.angle_max, aparm.angle_max);
poshold.pitch = constrain_int16(poshold.pitch, -aparm.angle_max, aparm.angle_max);
// update attitude controller targets
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(poshold.roll, poshold.pitch, target_yaw_rate, get_smoothing_gain());
// adjust climb rate using rangefinder
if (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);
// update altitude target and 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();
}
}
// poshold_update_pilot_lean_angle - update the pilot's filtered lean angle with the latest raw input received
void Copter::poshold_update_pilot_lean_angle(float &lean_angle_filtered, float &lean_angle_raw)
{
// if raw input is large or reversing the vehicle's lean angle immediately set the fitlered angle to the new raw angle
if ((lean_angle_filtered > 0 && lean_angle_raw < 0) || (lean_angle_filtered < 0 && lean_angle_raw > 0) || (fabsf(lean_angle_raw) > POSHOLD_STICK_RELEASE_SMOOTH_ANGLE)) {
lean_angle_filtered = lean_angle_raw;
} else {
// lean_angle_raw must be pulling lean_angle_filtered towards zero, smooth the decrease
if (lean_angle_filtered > 0) {
// reduce the filtered lean angle at 5% or the brake rate (whichever is faster).
lean_angle_filtered -= MAX((float)lean_angle_filtered * POSHOLD_SMOOTH_RATE_FACTOR, MAX(1, g.poshold_brake_rate/LOOP_RATE_FACTOR));
// do not let the filtered angle fall below the pilot's input lean angle.
// the above line pulls the filtered angle down and the below line acts as a catch
lean_angle_filtered = MAX(lean_angle_filtered, lean_angle_raw);
}else{
lean_angle_filtered += MAX(-(float)lean_angle_filtered * POSHOLD_SMOOTH_RATE_FACTOR, MAX(1, g.poshold_brake_rate/LOOP_RATE_FACTOR));
lean_angle_filtered = MIN(lean_angle_filtered, lean_angle_raw);
}
}
}
// poshold_mix_controls - mixes two controls based on the mix_ratio
// mix_ratio of 1 = use first_control completely, 0 = use second_control completely, 0.5 = mix evenly
int16_t Copter::poshold_mix_controls(float mix_ratio, int16_t first_control, int16_t second_control)
{
mix_ratio = constrain_float(mix_ratio, 0.0f, 1.0f);
return (int16_t)((mix_ratio * first_control) + ((1.0f-mix_ratio)*second_control));
}
