void Copter::update_precland() { int32_t height_above_ground_cm = current_loc.alt; // use range finder altitude if it is valid, else try to get terrain alt if (rangefinder_alt_ok()) { height_above_ground_cm = rangefinder_state.alt_cm; } else if (terrain_use()) { if (!current_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, height_above_ground_cm)) { height_above_ground_cm = current_loc.alt; } } copter.precland.update(height_above_ground_cm, rangefinder_alt_ok()); }
void Sub::update_precland() { int32_t height_above_ground_cm = current_loc.alt; // use range finder altitude if it is valid, else try to get terrain alt if (rangefinder_alt_ok()) { height_above_ground_cm = rangefinder_state.alt_cm; } else if (terrain_use()) { current_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, height_above_ground_cm); } sub.precland.update(height_above_ground_cm); // log output Log_Write_Precland(); }
void Copter::land_run_vertical_control(bool pause_descent) { bool navigating = pos_control->is_active_xy(); #if PRECISION_LANDING == ENABLED bool doing_precision_landing = !ap.land_repo_active && precland.target_acquired() && navigating; #else bool doing_precision_landing = false; #endif // compute desired velocity const float precland_acceptable_error = 15.0f; const float precland_min_descent_speed = 10.0f; int32_t alt_above_ground = mode_land.get_alt_above_ground(); float cmb_rate = 0; if (!pause_descent) { float max_land_descent_velocity; if (g.land_speed_high > 0) { max_land_descent_velocity = -g.land_speed_high; } else { max_land_descent_velocity = pos_control->get_speed_down(); } // Don't speed up for landing. max_land_descent_velocity = MIN(max_land_descent_velocity, -abs(g.land_speed)); // Compute a vertical velocity demand such that the vehicle approaches g2.land_alt_low. Without the below constraint, this would cause the vehicle to hover at g2.land_alt_low. cmb_rate = AC_AttitudeControl::sqrt_controller(MAX(g2.land_alt_low,100)-alt_above_ground, pos_control->get_pos_z_p().kP(), pos_control->get_accel_z(), G_Dt); // Constrain the demanded vertical velocity so that it is between the configured maximum descent speed and the configured minimum descent speed. cmb_rate = constrain_float(cmb_rate, max_land_descent_velocity, -abs(g.land_speed)); if (doing_precision_landing && rangefinder_alt_ok() && rangefinder_state.alt_cm > 35.0f && rangefinder_state.alt_cm < 200.0f) { float max_descent_speed = abs(g.land_speed)/2.0f; float land_slowdown = MAX(0.0f, pos_control->get_horizontal_error()*(max_descent_speed/precland_acceptable_error)); cmb_rate = MIN(-precland_min_descent_speed, -max_descent_speed+land_slowdown); } } // update altitude target and call position controller pos_control->set_alt_target_from_climb_rate_ff(cmb_rate, G_Dt, true); pos_control->update_z_controller(); }
void Copter::land_run_vertical_control(bool pause_descent) { bool navigating = pos_control.is_active_xy(); #if PRECISION_LANDING == ENABLED bool doing_precision_landing = !ap.land_repo_active && precland.target_acquired() && navigating; #else bool doing_precision_landing = false; #endif // compute desired velocity const float precland_acceptable_error = 25.0f; const float precland_min_descent_speed = -10.0f; int32_t alt_above_ground; if (rangefinder_alt_ok()) { alt_above_ground = rangefinder_state.alt_cm_filt.get(); } else { if (!navigating || !current_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, alt_above_ground)) { current_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_HOME, alt_above_ground); } } float cmb_rate = 0; if (!pause_descent) { cmb_rate = AC_AttitudeControl::sqrt_controller(LAND_START_ALT-alt_above_ground, g.p_alt_hold.kP(), pos_control.get_accel_z()); cmb_rate = constrain_float(cmb_rate, pos_control.get_speed_down(), -abs(g.land_speed)); if (doing_precision_landing && alt_above_ground < 300.0f) { float land_slowdown = MAX(0.0f, pos_control.get_horizontal_error()*(abs(g.land_speed)/precland_acceptable_error)); cmb_rate = MIN(precland_min_descent_speed, cmb_rate+land_slowdown); } } // record desired climb rate for logging desired_climb_rate = cmb_rate; // update altitude target and call position controller pos_control.set_alt_target_from_climb_rate_ff(cmb_rate, G_Dt, true); pos_control.update_z_controller(); }
// althold_run - runs the althold controller // should be called at 100hz or more void Copter::althold_run() { AltHoldModeState althold_state; float takeoff_climb_rate = 0.0f; // initialize vertical speeds and acceleration pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max); pos_control.set_accel_z(g.pilot_accel_z); // apply SIMPLE mode transform to pilot inputs update_simple_mode(); // get pilot desired lean angles float target_roll, target_pitch; get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, attitude_control.get_althold_lean_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, -g.pilot_velocity_z_max, g.pilot_velocity_z_max); #if FRAME_CONFIG == HELI_FRAME // helicopters are held on the ground until rotor speed runup has finished bool takeoff_triggered = (ap.land_complete && (channel_throttle->get_control_in() > get_takeoff_trigger_throttle()) && motors.rotor_runup_complete()); #else bool takeoff_triggered = (ap.land_complete && (channel_throttle->get_control_in() > get_takeoff_trigger_throttle()) && motors.spool_up_complete()); #endif // Alt Hold State Machine Determination if (!motors.armed() || !motors.get_interlock()) { althold_state = AltHold_MotorStopped; } else if (!ap.auto_armed){ althold_state = AltHold_NotAutoArmed; } else if (takeoff_state.running || takeoff_triggered){ althold_state = AltHold_Takeoff; } else if (ap.land_complete){ althold_state = AltHold_Landed; } else { althold_state = AltHold_Flying; } // Alt Hold State Machine switch (althold_state) { case AltHold_MotorStopped: motors.set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN); #if FRAME_CONFIG == HELI_FRAME // helicopters are capable of flying even with the motor stopped, therefore we will attempt to keep flying // call attitude controller attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); // force descent rate and call position controller pos_control.set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false); pos_control.update_z_controller(); #else // Multicopters do not stabilize roll/pitch/yaw when motor are stopped attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->get_control_in())-throttle_average); #endif break; case AltHold_NotAutoArmed: motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); #if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw attitude_control.set_yaw_target_to_current_heading(); attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); attitude_control.set_throttle_out(0,false,g.throttle_filt); #else // Multicopters do not stabilize roll/pitch/yaw when not auto-armed (i.e. on the ground, pilot has never raised throttle) attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); #endif pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->get_control_in())-throttle_average); break; case AltHold_Takeoff: // 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); // set motors to full range motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // call attitude controller attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); // 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: #if FRAME_CONFIG == HELI_FRAME attitude_control.set_yaw_target_to_current_heading(); #endif // call attitude controller attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); attitude_control.set_throttle_out(get_throttle_pre_takeoff(channel_throttle->get_control_in()),false,g.throttle_filt); // set motors to spin-when-armed if throttle at zero, otherwise full range if (ap.throttle_zero) { motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); } else { motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); } pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->get_control_in())-throttle_average); break; case AltHold_Flying: motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // call attitude controller attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(target_roll, target_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); } // call position controller pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false); pos_control.update_z_controller(); break; } }
// althold_run - runs the althold controller // should be called at 100hz or more void Copter::althold_run() { AltHoldModeState althold_state; float takeoff_climb_rate = 0.0f; // initialize vertical speeds and acceleration pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max); pos_control.set_accel_z(g.pilot_accel_z); // apply SIMPLE mode transform to pilot inputs update_simple_mode(); // get pilot desired lean angles float target_roll, target_pitch; get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, attitude_control.get_althold_lean_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, -g.pilot_velocity_z_max, g.pilot_velocity_z_max); #if FRAME_CONFIG == HELI_FRAME // helicopters are held on the ground until rotor speed runup has finished bool takeoff_triggered = (ap.land_complete && (target_climb_rate > 0.0f) && motors.rotor_runup_complete()); #else bool takeoff_triggered = ap.land_complete && (target_climb_rate > 0.0f); #endif // Alt Hold State Machine Determination if (!motors.armed() || !motors.get_interlock()) { althold_state = AltHold_MotorStopped; } else if (takeoff_state.running || takeoff_triggered) { althold_state = AltHold_Takeoff; } else if (!ap.auto_armed || ap.land_complete) { althold_state = AltHold_Landed; } else { althold_state = AltHold_Flying; } // 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, get_smoothing_gain()); #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.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 AltHold_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); // call attitude controller attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); // 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: // 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_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, 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 AltHold_Flying: motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // call attitude controller attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_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); } // 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 Sub::poshold_run() { uint32_t tnow = AP_HAL::millis(); // if not armed set throttle to zero and exit immediately if (!motors.armed()) { 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(motors.get_throttle_hover()); return; } // set motors to full range motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // run loiter controller wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler); /////////////////////// // update xy outputs // float pilot_lateral = channel_lateral->norm_input(); float pilot_forward = channel_forward->norm_input(); float lateral_out = 0; float forward_out = 0; // Allow pilot to reposition the sub if (fabsf(pilot_lateral) > 0.1 || fabsf(pilot_forward) > 0.1) { lateral_out = pilot_lateral; forward_out = pilot_forward; wp_nav.init_loiter_target(); // initialize target to current position after repositioning } else { translate_wpnav_rp(lateral_out, forward_out); } motors.set_lateral(lateral_out); motors.set_forward(forward_out); ///////////////////// // Update attitude // // get pilot's desired yaw rate float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); // convert pilot input to lean angles // To-Do: convert get_pilot_desired_lean_angles to return angles as floats float target_roll, target_pitch; get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, aparm.angle_max); // update attitude controller targets if (!is_zero(target_yaw_rate)) { // call attitude controller with rate yaw determined by pilot input attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); last_pilot_heading = ahrs.yaw_sensor; last_pilot_yaw_input_ms = tnow; // time when pilot last changed heading } else { // hold current heading // this check is required to prevent bounce back after very fast yaw maneuvers // the inertia of the vehicle causes the heading to move slightly past the point when pilot input actually stopped if (tnow < last_pilot_yaw_input_ms + 250) { // give 250ms to slow down, then set target heading target_yaw_rate = 0; // Stop rotation on yaw axis // call attitude controller with target yaw rate = 0 to decelerate on yaw axis attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); last_pilot_heading = ahrs.yaw_sensor; // update heading to hold } else { // call attitude controller holding absolute absolute bearing attitude_control.input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, last_pilot_heading, true, get_smoothing_gain()); } } /////////////////// // Update z axis // // 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, -g.pilot_velocity_z_max, g.pilot_velocity_z_max); // 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); } // call z axis position controller if (ap.at_bottom) { pos_control.relax_alt_hold_controllers(motors.get_throttle_hover()); // clear velocity and position targets, and integrator pos_control.set_alt_target(inertial_nav.get_altitude() + 10.0f); // set target to 10 cm above bottom } else { pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false); } pos_control.update_z_controller(); }
// circle_run - runs the circle flight mode // should be called at 100hz or more void Copter::circle_run() { float target_yaw_rate = 0; float target_climb_rate = 0; // initialize speeds and accelerations pos_control.set_speed_xy(wp_nav.get_speed_xy()); pos_control.set_accel_xy(wp_nav.get_wp_acceleration()); pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max); 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 || ap.land_complete || !motors.get_interlock()) { // To-Do: add some initialisation of position controllers #if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw // call attitude controller attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain()); attitude_control.set_throttle_out(0,false,g.throttle_filt); #else motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); // multicopters do not stabilize roll/pitch/yaw when disarmed attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); #endif pos_control.set_alt_target_to_current_alt(); return; } // process pilot inputs if (!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)) { circle_pilot_yaw_override = true; } // 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(); } } // set motors to full range motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // run circle controller circle_nav.update(); // call attitude controller if (circle_pilot_yaw_override) { attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(circle_nav.get_roll(), circle_nav.get_pitch(), target_yaw_rate, get_smoothing_gain()); }else{ attitude_control.input_euler_angle_roll_pitch_yaw(circle_nav.get_roll(), circle_nav.get_pitch(), circle_nav.get_yaw(),true, 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); } // update altitude target and call position controller pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt, false); pos_control.update_z_controller(); }
// circle_run - runs the circle flight mode // should be called at 100hz or more void Sub::circle_run() { float target_yaw_rate = 0; float target_climb_rate = 0; // update parameters, to allow changing at runtime 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 armed set throttle to zero and exit immediately if (!motors.armed()) { // To-Do: add some initialisation of position controllers motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); // Sub vehicles do not stabilize roll/pitch/yaw when disarmed attitude_control.set_throttle_out(0,true,g.throttle_filt); attitude_control.relax_attitude_controllers(); pos_control.set_alt_target_to_current_alt(); return; } // process pilot inputs // 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)) { circle_pilot_yaw_override = true; } // get pilot desired climb rate target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in()); // set motors to full range motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); // run circle controller circle_nav.update(); /////////////////////// // update xy outputs // float lateral_out, forward_out; translate_circle_nav_rp(lateral_out, forward_out); // Send to forward/lateral outputs motors.set_lateral(lateral_out); motors.set_forward(forward_out); // call attitude controller if (circle_pilot_yaw_override) { attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate); } else { attitude_control.input_euler_angle_roll_pitch_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), circle_nav.get_yaw(), true); } // 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); } // update altitude target and call position controller pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt, false); pos_control.update_z_controller(); }
// loiter_run - runs the loiter controller // should be called at 100hz or more void Copter::loiter_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(-g.pilot_velocity_z_max, g.pilot_velocity_z_max); pos_control.set_accel_z(g.pilot_accel_z); // process pilot inputs unless we are in radio failsafe if (!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, -g.pilot_velocity_z_max, g.pilot_velocity_z_max); } 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 (!ap.auto_armed) { loiter_state = Loiter_NotAutoArmed; } else if (takeoff_state.running || (ap.land_complete && (channel_throttle->get_control_in() > get_takeoff_trigger_throttle()))){ loiter_state = Loiter_Takeoff; } else if (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 // helicopters are capable of flying even with the motor stopped, therefore we will attempt to keep flying // 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); // force descent rate and call position controller pos_control.set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false); pos_control.update_z_controller(); #else wp_nav.init_loiter_target(); // multicopters do not stabilize roll/pitch/yaw when motors are stopped attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->get_control_in())-throttle_average); #endif break; case Loiter_NotAutoArmed: motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); wp_nav.init_loiter_target(); #if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain()); attitude_control.set_throttle_out(0,false,g.throttle_filt); #else // Multicopters do not stabilize roll/pitch/yaw when not auto-armed (i.e. on the ground, pilot has never raised throttle) attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); #endif pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->get_control_in())-throttle_average); break; case Loiter_Takeoff: 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); // set motors to full range motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // 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); // 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: wp_nav.init_loiter_target(); // call attitude controller attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(0, 0, 0, get_smoothing_gain()); // move throttle to between minimum and non-takeoff-throttle to keep us on the ground attitude_control.set_throttle_out(get_throttle_pre_takeoff(channel_throttle->get_control_in()),false,g.throttle_filt); // if throttle zero reset attitude and exit immediately if (ap.throttle_zero) { motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); } else { motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); } pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->get_control_in())-throttle_average); break; case Loiter_Flying: // set motors to full range motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // 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); // 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); } // 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; } }
// althold_run - runs the althold controller // should be called at 100hz or more void Sub::althold_run() { uint32_t tnow = AP_HAL::millis(); // initialize vertical speeds 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); if (!motors.armed()) { motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); // Sub vehicles do not stabilize roll/pitch/yaw when not auto-armed (i.e. on the ground, pilot has never raised throttle) attitude_control.set_throttle_out(0,true,g.throttle_filt); attitude_control.relax_attitude_controllers(); pos_control.relax_alt_hold_controllers(motors.get_throttle_hover()); last_pilot_heading = ahrs.yaw_sensor; return; } motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); // get pilot desired lean angles float target_roll, target_pitch; // Check if set_attitude_target_no_gps is valid if (tnow - sub.set_attitude_target_no_gps.last_message_ms < 5000) { float target_yaw; Quaternion( set_attitude_target_no_gps.packet.q ).to_euler( target_roll, target_pitch, target_yaw ); target_roll = degrees(target_roll); target_pitch = degrees(target_pitch); target_yaw = degrees(target_yaw); attitude_control.input_euler_angle_roll_pitch_yaw(target_roll * 1e2f, target_pitch * 1e2f, target_yaw * 1e2f, true); return; } get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, attitude_control.get_althold_lean_angle_max()); // get pilot's desired yaw rate float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); // call attitude controller if (!is_zero(target_yaw_rate)) { // call attitude controller with rate yaw determined by pilot input attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); last_pilot_heading = ahrs.yaw_sensor; last_pilot_yaw_input_ms = tnow; // time when pilot last changed heading } else { // hold current heading // this check is required to prevent bounce back after very fast yaw maneuvers // the inertia of the vehicle causes the heading to move slightly past the point when pilot input actually stopped if (tnow < last_pilot_yaw_input_ms + 250) { // give 250ms to slow down, then set target heading target_yaw_rate = 0; // Stop rotation on yaw axis // call attitude controller with target yaw rate = 0 to decelerate on yaw axis attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); last_pilot_heading = ahrs.yaw_sensor; // update heading to hold } else { // call attitude controller holding absolute absolute bearing attitude_control.input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, last_pilot_heading, true); } } // Hold actual position until zero derivative is detected static bool engageStopZ = true; // Get last user velocity direction to check for zero derivative points static bool lastVelocityZWasNegative = false; if (fabsf(channel_throttle->norm_input()-0.5f) > 0.05f) { // Throttle input above 5% // output pilot's throttle attitude_control.set_throttle_out(channel_throttle->norm_input(), false, g.throttle_filt); // reset z targets to current values pos_control.relax_alt_hold_controllers(); engageStopZ = true; lastVelocityZWasNegative = is_negative(inertial_nav.get_velocity_z()); } else { // hold z if (ap.at_bottom) { pos_control.relax_alt_hold_controllers(); // clear velocity and position targets pos_control.set_alt_target(inertial_nav.get_altitude() + 10.0f); // set target to 10 cm above bottom } else if (rangefinder_alt_ok()) { // if rangefinder is ok, use surface tracking float target_climb_rate = get_surface_tracking_climb_rate(0, pos_control.get_alt_target(), G_Dt); pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false); } // Detects a zero derivative // When detected, move the altitude set point to the actual position // This will avoid any problem related to joystick delays // or smaller input signals if(engageStopZ && (lastVelocityZWasNegative ^ is_negative(inertial_nav.get_velocity_z()))) { engageStopZ = false; pos_control.relax_alt_hold_controllers(); } pos_control.update_z_controller(); } motors.set_forward(channel_forward->norm_input()); motors.set_lateral(channel_lateral->norm_input()); }
// update_land_detector - checks if we have landed and updates the ap.land_complete flag // called at MAIN_LOOP_RATE void Copter::update_land_detector() { // land detector can not use the following sensors because they are unreliable during landing // barometer altitude : ground effect can cause errors larger than 4m // EKF vertical velocity or altitude : poor barometer and large acceleration from ground impact // earth frame angle or angle error : landing on an uneven surface will force the airframe to match the ground angle // gyro output : on uneven surface the airframe may rock back an forth after landing // range finder : tend to be problematic at very short distances // input throttle : in slow land the input throttle may be only slightly less than hover if (!motors.armed()) { // if disarmed, always landed. set_land_complete(true); } else if (ap.land_complete) { #if FRAME_CONFIG == HELI_FRAME // if rotor speed and collective pitch are high then clear landing flag if (motors.get_throttle() > get_non_takeoff_throttle() && !motors.limit.throttle_lower && motors.rotor_runup_complete()) { #else // if throttle output is high then clear landing flag if (motors.get_throttle() > get_non_takeoff_throttle()) { #endif set_land_complete(false); } } else { #if FRAME_CONFIG == HELI_FRAME // check that collective pitch is on lower limit (should be constrained by LAND_COL_MIN) bool motor_at_lower_limit = motors.limit.throttle_lower; #else // check that the average throttle output is near minimum (less than 12.5% hover throttle) bool motor_at_lower_limit = motors.limit.throttle_lower && attitude_control.is_throttle_mix_min(); #endif // check that the airframe is not accelerating (not falling or breaking after fast forward flight) bool accel_stationary = (land_accel_ef_filter.get().length() <= LAND_DETECTOR_ACCEL_MAX); // check that vertical speed is within 1m/s of zero bool descent_rate_low = fabsf(inertial_nav.get_velocity_z()) < 100; // if we have a healthy rangefinder only allow landing detection below 2 meters bool rangefinder_check = (!rangefinder_alt_ok() || rangefinder_state.alt_cm_filt.get() < LAND_RANGEFINDER_MIN_ALT_CM); if (motor_at_lower_limit && accel_stationary && descent_rate_low && rangefinder_check) { // landed criteria met - increment the counter and check if we've triggered if( land_detector_count < ((float)LAND_DETECTOR_TRIGGER_SEC)*scheduler.get_loop_rate_hz()) { land_detector_count++; } else { set_land_complete(true); } } else { // we've sensed movement up or down so reset land_detector land_detector_count = 0; } } set_land_complete_maybe(ap.land_complete || (land_detector_count >= LAND_DETECTOR_MAYBE_TRIGGER_SEC*scheduler.get_loop_rate_hz())); } // set land_complete flag and disarm motors if disarm-on-land is configured void Copter::set_land_complete(bool b) { // if no change, exit immediately if( ap.land_complete == b ) return; land_detector_count = 0; if(b){ Log_Write_Event(DATA_LAND_COMPLETE); } else { Log_Write_Event(DATA_NOT_LANDED); } ap.land_complete = b; // trigger disarm-on-land if configured bool disarm_on_land_configured = (g.throttle_behavior & THR_BEHAVE_DISARM_ON_LAND_DETECT) != 0; bool mode_disarms_on_land = mode_allows_arming(control_mode,false) && !mode_has_manual_throttle(control_mode); if (ap.land_complete && motors.armed() && disarm_on_land_configured && mode_disarms_on_land) { init_disarm_motors(); } } // set land complete maybe flag void Copter::set_land_complete_maybe(bool b) { // if no change, exit immediately if (ap.land_complete_maybe == b) return; if (b) { Log_Write_Event(DATA_LAND_COMPLETE_MAYBE); } ap.land_complete_maybe = b; } // update_throttle_thr_mix - sets motors throttle_low_comp value depending upon vehicle state // low values favour pilot/autopilot throttle over attitude control, high values favour attitude control over throttle // has no effect when throttle is above hover throttle void Copter::update_throttle_thr_mix() { #if FRAME_CONFIG != HELI_FRAME // if disarmed or landed prioritise throttle if(!motors.armed() || ap.land_complete) { attitude_control.set_throttle_mix_min(); return; } if (mode_has_manual_throttle(control_mode)) { // manual throttle if(channel_throttle->get_control_in() <= 0) { attitude_control.set_throttle_mix_min(); } else { attitude_control.set_throttle_mix_mid(); } } else { // autopilot controlled throttle // check for aggressive flight requests - requested roll or pitch angle below 15 degrees const Vector3f angle_target = attitude_control.get_att_target_euler_cd(); bool large_angle_request = (norm(angle_target.x, angle_target.y) > LAND_CHECK_LARGE_ANGLE_CD); // check for large external disturbance - angle error over 30 degrees const float angle_error = attitude_control.get_att_error_angle_deg(); bool large_angle_error = (angle_error > LAND_CHECK_ANGLE_ERROR_DEG); // check for large acceleration - falling or high turbulence Vector3f accel_ef = ahrs.get_accel_ef_blended(); accel_ef.z += GRAVITY_MSS; bool accel_moving = (accel_ef.length() > LAND_CHECK_ACCEL_MOVING); // check for requested decent bool descent_not_demanded = pos_control.get_desired_velocity().z >= 0.0f; if ( large_angle_request || large_angle_error || accel_moving || descent_not_demanded) { attitude_control.set_throttle_mix_max(); } else { attitude_control.set_throttle_mix_min(); } } #endif }
// loiter_run - runs the loiter controller // should be called at 100hz or more void Copter::loiter_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(-g.pilot_velocity_z_max, g.pilot_velocity_z_max); pos_control.set_accel_z(g.pilot_accel_z); // process pilot inputs unless we are in radio failsafe if (!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, -g.pilot_velocity_z_max, g.pilot_velocity_z_max); } 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(); } #if FRAME_CONFIG == HELI_FRAME // helicopters are held on the ground until rotor speed runup has finished bool takeoff_triggered = (ap.land_complete && (target_climb_rate > 0.0f) && motors.rotor_runup_complete()); #else bool takeoff_triggered = ap.land_complete && (target_climb_rate > 0.0f); #endif // Loiter State Machine Determination if (!motors.armed() || !motors.get_interlock()) { loiter_state = Loiter_MotorStopped; } else if (takeoff_state.running || takeoff_triggered) { 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); // 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); // 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 (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); } // 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(-g.pilot_velocity_z_max, g.pilot_velocity_z_max); 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(get_throttle_pre_takeoff(channel_throttle->get_control_in())-throttle_average); 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, -g.pilot_velocity_z_max, g.pilot_velocity_z_max); // get takeoff adjusted pilot and takeoff climb rates takeoff_get_climb_rates(target_climb_rate, takeoff_climb_rate); // check for take-off if (ap.land_complete && (takeoff_state.running || channel_throttle->get_control_in() > get_takeoff_trigger_throttle())) { 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) { // if throttle zero reset attitude and exit immediately if (ap.throttle_zero) { 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(); // move throttle to between minimum and non-takeoff-throttle to keep us on the ground attitude_control.set_throttle_out(get_throttle_pre_takeoff(channel_throttle->get_control_in()),false,g.throttle_filt); pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->get_control_in())-throttle_average); 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); // 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); } // 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(); } }
// althold_run - runs the althold controller // should be called at 100hz or more void Sub::althold_run() { uint32_t tnow = AP_HAL::millis(); // initialize vertical speeds and acceleration pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max); pos_control.set_accel_z(g.pilot_accel_z); if (!motors.armed() || !motors.get_interlock()) { motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); // Sub vehicles do not stabilize roll/pitch/yaw when not auto-armed (i.e. on the ground, pilot has never raised throttle) attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); pos_control.relax_alt_hold_controllers(motors.get_throttle_hover()); last_pilot_heading = ahrs.yaw_sensor; return; } motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // apply SIMPLE mode transform to pilot inputs update_simple_mode(); // get pilot desired lean angles float target_roll, target_pitch; get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, attitude_control.get_althold_lean_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, -g.pilot_velocity_z_max, g.pilot_velocity_z_max); // call attitude controller if (!is_zero(target_yaw_rate)) { // call attitude controller with rate yaw determined by pilot input attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); last_pilot_heading = ahrs.yaw_sensor; last_pilot_yaw_input_ms = tnow; // time when pilot last changed heading } else { // hold current heading // this check is required to prevent bounce back after very fast yaw maneuvers // the inertia of the vehicle causes the heading to move slightly past the point when pilot input actually stopped if (tnow < last_pilot_yaw_input_ms + 250) { // give 250ms to slow down, then set target heading target_yaw_rate = 0; // Stop rotation on yaw axis // call attitude controller with target yaw rate = 0 to decelerate on yaw axis attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); last_pilot_heading = ahrs.yaw_sensor; // update heading to hold } else { // call attitude controller holding absolute absolute bearing attitude_control.input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, last_pilot_heading, true, 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); } // call z axis position controller if (ap.at_bottom) { pos_control.relax_alt_hold_controllers(motors.get_throttle_hover()); // clear velocity and position targets, and integrator pos_control.set_alt_target(inertial_nav.get_altitude() + 10.0f); // set target to 10 cm above bottom } else { pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false); } pos_control.update_z_controller(); //control_in is range 0-1000 //radio_in is raw pwm value motors.set_forward(channel_forward->norm_input()); motors.set_lateral(channel_lateral->norm_input()); }
// sport_run - runs the sport controller // should be called at 100hz or more void Copter::sport_run() { SportModeState sport_state; float takeoff_climb_rate = 0.0f; // initialize vertical speed and acceleration pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max); 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; int32_t roll_angle = wrap_180_cd(ahrs.roll_sensor); 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(ahrs.pitch_sensor); target_pitch_rate -= constrain_int32(pitch_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE) * g.acro_balance_pitch; 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, -g.pilot_velocity_z_max, g.pilot_velocity_z_max); #if FRAME_CONFIG == HELI_FRAME // helicopters are held on the ground until rotor speed runup has finished bool takeoff_triggered = (ap.land_complete && (target_climb_rate > 0.0f) && motors.rotor_runup_complete()); #else bool takeoff_triggered = ap.land_complete && (target_climb_rate > 0.0f); #endif // State Machine Determination if (!motors.armed() || !motors.get_interlock()) { sport_state = Sport_MotorStopped; } else if (takeoff_state.running || takeoff_triggered) { 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); // 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 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); } // call position controller pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false); pos_control.update_z_controller(); break; } }