// rtl_returnhome_run - return home // called by rtl_run at 100hz or more void Copter::rtl_land_run() { // if not auto armed or landing completed or motor interlock not enabled set throttle to zero and exit immediately if (!motors.armed() || !ap.auto_armed || ap.land_complete || !motors.get_interlock()) { #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 // set target to current position wp_nav.init_loiter_target(); // disarm when the landing detector says we've landed if (ap.land_complete) { init_disarm_motors(); } // check if we've completed this stage of RTL rtl_state_complete = ap.land_complete; return; } // set motors to full range motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); land_run_horizontal_control(); land_run_vertical_control(); // check if we've completed this stage of RTL rtl_state_complete = ap.land_complete; }
// auto_takeoff_run - takeoff in auto mode // called by auto_run at 100hz or more void Copter::auto_takeoff_run() { // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately if(!ap.auto_armed || !motors.get_interlock()) { // initialise wpnav targets wp_nav.shift_wp_origin_to_current_pos(); #if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw // call attitude controller 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 disarmed // reset attitude control targets attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); #endif // clear i term when we're taking off set_throttle_takeoff(); return; } // process pilot's yaw input float target_yaw_rate = 0; if (!failsafe.radio) { // get pilot's desired yaw rate target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in); } // run waypoint controller wp_nav.update_wpnav(); // call z-axis position controller (wpnav should have already updated it's alt target) pos_control.update_z_controller(); // roll & pitch from waypoint controller, yaw rate from pilot attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate); }
// guided_takeoff_run - takeoff in guided mode // called by guided_run at 100hz or more void Copter::guided_takeoff_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()) { #if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw // call attitude controller 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 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 return; } // process pilot's yaw input float target_yaw_rate = 0; if (!failsafe.radio) { // get pilot's desired yaw rate target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); } // set motors to full range motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // run waypoint controller failsafe_terrain_set_status(wp_nav.update_wpnav()); // call z-axis position controller (wpnav should have already updated it's alt target) pos_control.update_z_controller(); // roll & pitch from waypoint controller, yaw rate from pilot attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate); }
// auto_loiter_run - loiter in AUTO flight mode // called by auto_run at 100hz or more void Copter::auto_loiter_run() { // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately if (!motors.armed() || !ap.auto_armed || ap.land_complete || !motors.get_interlock()) { #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 return; } // accept pilot input of yaw float target_yaw_rate = 0; if(!failsafe.radio) { target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); } // set motors to full range motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // run waypoint and z-axis position controller failsafe_terrain_set_status(wp_nav.update_wpnav()); pos_control.update_z_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()); }
// auto_loiter_run - loiter in AUTO flight mode // called by auto_run at 100hz or more void Copter::auto_loiter_run() { // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately if(!ap.auto_armed || ap.land_complete || !motors.get_interlock()) { #if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw // call attitude controller attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(0, 0, 0, get_smoothing_gain()); attitude_control.set_throttle_out(0,false,g.throttle_filt); #else // Multirotors do not stabilize roll/pitch/yaw when disarmed attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); #endif return; } // accept pilot input of yaw float target_yaw_rate = 0; if(!failsafe.radio) { target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in); } // run waypoint and z-axis postion controller wp_nav.update_wpnav(); pos_control.update_z_controller(); attitude_control.angle_ef_roll_pitch_rate_ef_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate); }
// land_run - runs the land controller // horizontal position controlled with loiter controller // should be called at 100hz or more void Copter::land_gps_run() { // if not auto armed or landed or motor interlock not enabled set throttle to zero and exit immediately if (!motors.armed() || !ap.auto_armed || ap.land_complete || !motors.get_interlock()) { #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 wp_nav.init_loiter_target(); // disarm when the landing detector says we've landed if (ap.land_complete) { init_disarm_motors(); } return; } // set motors to full range motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // pause before beginning land descent if(land_pause && millis()-land_start_time >= LAND_WITH_DELAY_MS) { land_pause = false; } land_run_horizontal_control(); land_run_vertical_control(land_pause); }
// rtl_climb_return_descent_run - implements the initial climb, return home and descent portions of RTL which all rely on the wp controller // called by rtl_run at 100hz or more void Copter::rtl_loiterathome_run() { // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) { #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 // reset attitude control targets attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); #endif // To-Do: re-initialise wpnav targets return; } // process pilot's yaw input float target_yaw_rate = 0; 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)) { set_auto_yaw_mode(AUTO_YAW_HOLD); } } // set motors to full range motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // run waypoint controller failsafe_terrain_set_status(wp_nav.update_wpnav()); // call z-axis position controller (wpnav should have already updated it's alt target) pos_control.update_z_controller(); // call attitude controller if (auto_yaw_mode == AUTO_YAW_HOLD) { // roll & pitch from waypoint controller, yaw rate from pilot attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain()); }else{ // roll, pitch from waypoint controller, yaw heading from auto_heading() attitude_control.input_euler_angle_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(),true, get_smoothing_gain()); } // check if we've completed this stage of RTL if ((millis() - rtl_loiter_start_time) >= (uint32_t)g.rtl_loiter_time.get()) { if (auto_yaw_mode == AUTO_YAW_RESETTOARMEDYAW) { // check if heading is within 2 degrees of heading when vehicle was armed if (labs(wrap_180_cd(ahrs.yaw_sensor-initial_armed_bearing)) <= 200) { rtl_state_complete = true; } } else { // we have loitered long enough rtl_state_complete = true; } } }
// 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; // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately if(!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.angle_ef_roll_pitch_rate_ef_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 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->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->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(); } } // run circle controller circle_nav.update(); // call attitude controller if (circle_pilot_yaw_override) { attitude_control.angle_ef_roll_pitch_rate_ef_yaw(circle_nav.get_roll(), circle_nav.get_pitch(), target_yaw_rate); }else{ attitude_control.angle_ef_roll_pitch_yaw(circle_nav.get_roll(), circle_nav.get_pitch(), circle_nav.get_yaw(),true); } // run altitude controller if (sonar_enabled && (sonar_alt_health >= SONAR_ALT_HEALTH_MAX)) { // if sonar 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(); }
// auto_land_run - lands in auto mode // called by auto_run at 100hz or more void Copter::auto_land_run() { int16_t roll_control = 0, pitch_control = 0; float target_yaw_rate = 0; // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately if(!ap.auto_armed || ap.land_complete) { #if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw // call attitude controller attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(0, 0, 0, get_smoothing_gain()); attitude_control.set_throttle_out(0,false,g.throttle_filt); #else // Multirotors do not stabilize roll/pitch/yaw when disarmed attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); #endif // set target to current position wp_nav.init_loiter_target(); return; } // relax loiter targets if we might be landed if (ap.land_complete_maybe) { wp_nav.loiter_soften_for_landing(); } // process pilot's input if (!failsafe.radio) { if (g.land_repositioning) { // apply SIMPLE mode transform to pilot inputs update_simple_mode(); // process pilot's roll and pitch input roll_control = channel_roll->control_in; pitch_control = channel_pitch->control_in; } // get pilot's desired yaw rate target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in); } // process roll, pitch inputs wp_nav.set_pilot_desired_acceleration(roll_control, pitch_control); // run loiter controller wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler); // call z-axis position controller float cmb_rate = get_land_descent_speed(); pos_control.set_alt_target_from_climb_rate(cmb_rate, G_Dt, true); pos_control.update_z_controller(); // record desired climb rate for logging desired_climb_rate = cmb_rate; // roll & pitch from waypoint controller, yaw rate from pilot attitude_control.angle_ef_roll_pitch_rate_ef_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate); }
// guided_vel_control_run - runs the guided velocity controller // called from guided_run void Copter::guided_vel_control_run() { // if not auto armed or motors not enabled set throttle to zero and exit immediately if (!motors.armed() || !ap.auto_armed || !motors.get_interlock() || ap.land_complete) { // initialise velocity controller pos_control.init_vel_controller_xyz(); #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 return; } // process pilot's yaw input float target_yaw_rate = 0; 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)) { set_auto_yaw_mode(AUTO_YAW_HOLD); } } // set motors to full range motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // set velocity to zero if no updates received for 3 seconds uint32_t tnow = millis(); if (tnow - vel_update_time_ms > GUIDED_POSVEL_TIMEOUT_MS && !pos_control.get_desired_velocity().is_zero()) { guided_set_desired_velocity_with_accel_and_fence_limits(Vector3f(0.0f,0.0f,0.0f)); } else { guided_set_desired_velocity_with_accel_and_fence_limits(guided_vel_target_cms); } // call velocity controller which includes z axis controller pos_control.update_vel_controller_xyz(ekfNavVelGainScaler); // call attitude controller if (auto_yaw_mode == AUTO_YAW_HOLD) { // roll & pitch from waypoint controller, yaw rate from pilot attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(pos_control.get_roll(), pos_control.get_pitch(), target_yaw_rate, get_smoothing_gain()); }else{ // roll, pitch from waypoint controller, yaw heading from auto_heading() attitude_control.input_euler_angle_roll_pitch_yaw(pos_control.get_roll(), pos_control.get_pitch(), get_auto_heading(), true, get_smoothing_gain()); } }
// rtl_descent_run - implements the final descent to the RTL_ALT // called by rtl_run at 100hz or more void Copter::rtl_descent_run() { int16_t roll_control = 0, pitch_control = 0; float target_yaw_rate = 0; // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately if(!ap.auto_armed || !motors.get_interlock()) { #if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw // call attitude controller attitude_control.angle_ef_roll_pitch_rate_ef_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 disarmed attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); #endif // set target to current position wp_nav.init_loiter_target(); return; } // process pilot's input if (!failsafe.radio) { if (g.land_repositioning) { // apply SIMPLE mode transform to pilot inputs update_simple_mode(); // process pilot's roll and pitch input roll_control = channel_roll->control_in; pitch_control = channel_pitch->control_in; } // get pilot's desired yaw rate target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in); } // process roll, pitch inputs wp_nav.set_pilot_desired_acceleration(roll_control, pitch_control); // run loiter controller wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler); // call z-axis position controller pos_control.set_alt_target_with_slew(pv_alt_above_origin(g.rtl_alt_final), G_Dt); pos_control.update_z_controller(); // roll & pitch from waypoint controller, yaw rate from pilot attitude_control.angle_ef_roll_pitch_rate_ef_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate); // check if we've reached within 20cm of final altitude rtl_state_complete = fabsf(pv_alt_above_origin(g.rtl_alt_final) - inertial_nav.get_altitude()) < 20.0f; }
// brake_run - runs the brake controller // should be called at 100hz or more void Copter::brake_run() { // if not auto armed set throttle to zero and exit immediately if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) { wp_nav.init_brake_target(BRAKE_MODE_DECEL_RATE); #if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw // call attitude controller 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 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.relax_alt_hold_controllers(get_throttle_pre_takeoff(0)-throttle_average); return; } // relax stop target if we might be landed if (ap.land_complete_maybe) { wp_nav.loiter_soften_for_landing(); } // if landed immediately disarm if (ap.land_complete) { init_disarm_motors(); } // set motors to full range motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // run brake controller wp_nav.update_brake(ekfGndSpdLimit, ekfNavVelGainScaler); // call attitude controller attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), 0.0f); // body-frame rate controller is run directly from 100hz loop // update altitude target and call position controller pos_control.set_alt_target_from_climb_rate_ff(0.0f, G_Dt, false); pos_control.update_z_controller(); if (brake_timeout_ms != 0 && millis()-brake_timeout_start >= brake_timeout_ms) { if (!set_mode(LOITER, MODE_REASON_BRAKE_TIMEOUT)) { set_mode(ALT_HOLD, MODE_REASON_BRAKE_TIMEOUT); } } }
// rtl_climb_return_run - implements the initial climb, return home and descent portions of RTL which all rely on the wp controller // called by rtl_run at 100hz or more void Copter::rtl_climb_return_run() { // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) { #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 // reset attitude control targets attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); #endif // To-Do: re-initialise wpnav targets return; } // process pilot's yaw input float target_yaw_rate = 0; 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)) { set_auto_yaw_mode(AUTO_YAW_HOLD); } } // set motors to full range motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // run waypoint controller failsafe_terrain_set_status(wp_nav.update_wpnav()); // call z-axis position controller (wpnav should have already updated it's alt target) pos_control.update_z_controller(); // call attitude controller if (auto_yaw_mode == AUTO_YAW_HOLD) { // roll & pitch from waypoint controller, yaw rate from pilot attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain()); }else{ // roll, pitch from waypoint controller, yaw heading from auto_heading() attitude_control.input_euler_angle_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(),true, get_smoothing_gain()); } // check if we've completed this stage of RTL rtl_state_complete = wp_nav.reached_wp_destination(); }
// land_run - runs the land controller // horizontal position controlled with loiter controller and precision landing void Copter::land_precision_run() { // if not auto armed or motors not enabled set throttle to zero and exit immediately if (!ap.auto_armed || !motors.get_interlock() || ap.land_complete) { // initialise velocity controller pos_control.init_vel_controller_xyz(); #if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw // call attitude controller attitude_control.angle_ef_roll_pitch_rate_ef_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 disarmed attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); #endif return; } // process pilot's yaw input float target_yaw_rate = 0.0f; if (!failsafe.radio) { // get pilot's desired yaw rate target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in); if (!is_zero(target_yaw_rate)) { set_auto_yaw_mode(AUTO_YAW_HOLD); } } // set desired velocity Vector3f desvel = precland.calc_desired_velocity(g.land_speed); pos_control.set_desired_velocity(desvel); // call velocity controller which includes z axis controller pos_control.update_vel_controller_xyz(ekfNavVelGainScaler); // call attitude controller // roll & pitch from waypoint controller, yaw rate from pilot attitude_control.angle_ef_roll_pitch_rate_ef_yaw(pos_control.get_roll(), pos_control.get_pitch(), target_yaw_rate); // any pilot roll input input switches out of precision landing to gps or pilot controlled land on next iteration if (!failsafe.radio && g.land_repositioning) { if ((channel_roll->control_in != 0) || (channel_pitch->control_in != 0)) { land_state.use_precision = false; if (!land_gps_init()) { land_nogps_init(); } } } }
// auto_spline_run - runs the auto spline controller // called by auto_run at 100hz or more void Copter::auto_spline_run() { // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) { // To-Do: reset waypoint origin to current location because copter is probably on the ground so we don't want it lurching left or right on take-off // (of course it would be better if people just used take-off) #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 // multicopters do not stabilize roll/pitch/yaw when disarmed attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); #endif // clear i term when we're taking off set_throttle_takeoff(); return; } // process pilot's yaw input float target_yaw_rate = 0; if (!failsafe.radio) { // get pilot's desired yaw rat target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); if (!is_zero(target_yaw_rate)) { set_auto_yaw_mode(AUTO_YAW_HOLD); } } // set motors to full range motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // run waypoint controller wp_nav.update_spline(); // call z-axis position controller (wpnav should have already updated it's alt target) pos_control.update_z_controller(); // call attitude controller if (auto_yaw_mode == AUTO_YAW_HOLD) { // roll & pitch from waypoint controller, yaw rate from pilot attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain()); }else{ // roll, pitch from waypoint controller, yaw heading from auto_heading() attitude_control.input_euler_angle_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(), true, get_smoothing_gain()); } }
// brake_run - runs the brake controller // should be called at 100hz or more void Copter::brake_run() { // if not auto armed set throttle to zero and exit immediately if(!ap.auto_armed) { wp_nav.init_brake_target(BRAKE_MODE_DECEL_RATE); #if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw // call attitude controller attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(0, 0, 0, get_smoothing_gain()); attitude_control.set_throttle_out(0,false,g.throttle_filt); #else // Multirotors do not stabilize roll/pitch/yaw when disarmed attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); #endif pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(0)-throttle_average); return; } // relax stop target if we might be landed if (ap.land_complete_maybe) { wp_nav.loiter_soften_for_landing(); } // if landed immediately disarm if (ap.land_complete) { init_disarm_motors(); } // run brake controller wp_nav.update_brake(ekfGndSpdLimit, ekfNavVelGainScaler); // call attitude controller attitude_control.angle_ef_roll_pitch_rate_ef_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), 0.0f); // body-frame rate controller is run directly from 100hz loop // update altitude target and call position controller pos_control.set_alt_target_from_climb_rate(0.0f, 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->control_in, channel_pitch->control_in); // get pilot's desired yaw rate target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in); // get pilot desired climb rate target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->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->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->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->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->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->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); // run altitude controller if (sonar_enabled && (sonar_alt_health >= SONAR_ALT_HEALTH_MAX)) { // if sonar 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; } }
void Copter::heli_stabilize_run_ruas() { float target_roll, target_pitch; float target_yaw_rate; int16_t pilot_throttle_scaled; // Tradheli should not reset roll, pitch, yaw targets when motors are not runup, because // we may be in autorotation flight. These should be reset only when transitioning from disarmed // to armed, because the pilot will have placed the helicopter down on the landing pad. This is so // that the servos move in a realistic fashion while disarmed for operational checks. // Also, unlike multicopters we do not set throttle (i.e. collective pitch) to zero so the swash servos move if(!motors.armed()) { heli_flags.init_targets_on_arming=true; attitude_control.set_yaw_target_to_current_heading(); } if(motors.armed() && heli_flags.init_targets_on_arming) { attitude_control.set_yaw_target_to_current_heading(); if (motors.rotor_speed_above_critical()) { heli_flags.init_targets_on_arming=false; } } // send RC inputs direct into motors library for use during manual passthrough for helicopter setup heli_radio_passthrough(); // apply SIMPLE mode transform to pilot inputs update_simple_mode(); // updates avoidnace logic avoidance_maneuver(); //RUAS //Allowing the pilot to have presidance in heli attitude control demos! The presidnace order can be changed as needed here: if(!failsafe.radio && (channel_roll->control_in >= 200 || channel_roll->control_in <= -200 || channel_pitch->control_in >= 200 ||channel_pitch->control_in <= -200)){ get_pilot_desired_lean_angles(channel_roll->control_in, channel_pitch->control_in, target_roll, target_pitch, aparm.angle_max); //If no pilot input and need to implement manouver }else if(do_avoid_maneuver){ get_pilot_desired_lean_angles(avoidance_roll_angle_cd, avoidance_pitch_angle_cd , target_roll, target_pitch, aparm.angle_max); //If no pilot input and no mpending crash, hold middlestick position }else{ get_pilot_desired_lean_angles(channel_roll->control_in, channel_pitch->control_in, target_roll, target_pitch, aparm.angle_max); } //the pilot takes presidnace in the yaw direction in the demos! if(!failsafe.radio && (channel_yaw->control_in >= 200 || channel_yaw->control_in <= -200)){ target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in); }else if(do_track_maneuver){ //point to air traffic target_yaw_rate = _trafic_angle * 100 * g.acro_yaw_p;//check this }else{ //point to current heading target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in); } // get pilot's desired throttle pilot_throttle_scaled = input_manager.get_pilot_desired_collective(channel_throttle->control_in); // call attitude controller attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); // output pilot's throttle - note that TradHeli does not used angle-boost attitude_control.set_throttle_out(pilot_throttle_scaled, false, g.throttle_filt); }
// stabilize_run - runs the main stabilize controller // should be called at 100hz or more void Copter::heli_stabilize_run() { float target_roll, target_pitch; float target_yaw_rate; int16_t pilot_throttle_scaled; // Tradheli should not reset roll, pitch, yaw targets when motors are not runup, because // we may be in autorotation flight. These should be reset only when transitioning from disarmed // to armed, because the pilot will have placed the helicopter down on the landing pad. This is so // that the servos move in a realistic fashion while disarmed for operational checks. // Also, unlike multicopters we do not set throttle (i.e. collective pitch) to zero so the swash servos move if(!motors.armed()) { heli_flags.init_targets_on_arming=true; attitude_control.set_yaw_target_to_current_heading(); } if(motors.armed() && heli_flags.init_targets_on_arming) { attitude_control.set_yaw_target_to_current_heading(); if (motors.rotor_speed_above_critical()) { heli_flags.init_targets_on_arming=false; } } // send RC inputs direct into motors library for use during manual passthrough for helicopter setup heli_radio_passthrough(); // apply SIMPLE mode transform to pilot inputs update_simple_mode(); // convert pilot input to lean angles // To-Do: convert get_pilot_desired_lean_angles to return angles as floats get_pilot_desired_lean_angles(channel_roll->control_in, channel_pitch->control_in, target_roll, target_pitch, aparm.angle_max); // get pilot's desired yaw rate target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in); // get pilot's desired throttle pilot_throttle_scaled = input_manager.get_pilot_desired_collective(channel_throttle->control_in); // call attitude controller attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); // output pilot's throttle - note that TradHeli does not used angle-boost attitude_control.set_throttle_out(pilot_throttle_scaled, false, g.throttle_filt); }
void Sub::surface_run() { float target_roll, target_pitch; float target_yaw_rate; // if not armed set throttle to zero and exit immediately if (!motors.armed()) { motors.output_min(); motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); return; } // Already at surface, hold depth at surface if (ap.at_surface) { set_mode(ALT_HOLD, MODE_REASON_SURFACE_COMPLETE); } // convert pilot input to lean angles // To-Do: convert get_pilot_desired_lean_angles to return angles as floats get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, aparm.angle_max); // get pilot's desired yaw rate target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); // call attitude controller attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); // set target climb rate float cmb_rate = constrain_float(abs(wp_nav.get_speed_up()), 1, pos_control.get_speed_up()); // 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(); // pilot has control for repositioning motors.set_forward(channel_forward->norm_input()); motors.set_lateral(channel_lateral->norm_input()); }
// auto_circle_run - circle in AUTO flight mode // called by auto_run at 100hz or more void Copter::auto_circle_run() { // call circle controller circle_nav.update(); // call z-axis position controller pos_control.update_z_controller(); // roll & pitch from waypoint controller, yaw rate from pilot attitude_control.input_euler_angle_roll_pitch_yaw(circle_nav.get_roll(), circle_nav.get_pitch(), circle_nav.get_yaw(),true, get_smoothing_gain()); }
// land_run - runs the land controller // horizontal position controlled with loiter controller // should be called at 100hz or more void Copter::land_gps_run() { int16_t roll_control = 0, pitch_control = 0; float target_yaw_rate = 0; // if not auto armed or landed or motor interlock not enabled set throttle to zero and exit immediately if(!ap.auto_armed || ap.land_complete || !motors.get_interlock()) { #if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw // call attitude controller attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(0, 0, 0, get_smoothing_gain()); attitude_control.set_throttle_out(0,false,g.throttle_filt); #else // Multirotors do not stabilize roll/pitch/yaw when disarmed attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); #endif wp_nav.init_loiter_target(); #if LAND_REQUIRE_MIN_THROTTLE_TO_DISARM == ENABLED // disarm when the landing detector says we've landed and throttle is at minimum if (ap.land_complete && (ap.throttle_zero || failsafe.radio)) { init_disarm_motors(); } #else // disarm when the landing detector says we've landed if (ap.land_complete) { init_disarm_motors(); } #endif return; } // relax loiter target if we might be landed if (ap.land_complete_maybe) { wp_nav.loiter_soften_for_landing(); } // process pilot inputs if (!failsafe.radio) { if (g.land_repositioning) { // apply SIMPLE mode transform to pilot inputs update_simple_mode(); // process pilot's roll and pitch input roll_control = channel_roll->control_in; pitch_control = channel_pitch->control_in; } // get pilot's desired yaw rate target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in); } // process roll, pitch inputs wp_nav.set_pilot_desired_acceleration(roll_control, pitch_control); // run loiter controller wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler); // call attitude controller attitude_control.angle_ef_roll_pitch_rate_ef_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate); // pause 4 seconds before beginning land descent float cmb_rate; if(land_pause && millis()-land_start_time < 4000) { cmb_rate = 0; } else { land_pause = false; cmb_rate = get_land_descent_speed(); } // 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(cmb_rate, G_Dt, true); pos_control.update_z_controller(); }
// land_nogps_run - runs the land controller // pilot controls roll and pitch angles // should be called at 100hz or more void Copter::land_nogps_run() { float target_roll = 0.0f, target_pitch = 0.0f; float target_yaw_rate = 0; // process pilot inputs if (!failsafe.radio) { if (g.land_repositioning) { // apply SIMPLE mode transform to pilot inputs update_simple_mode(); // get pilot desired lean angles get_pilot_desired_lean_angles(channel_roll->control_in, channel_pitch->control_in, target_roll, target_pitch); } // get pilot's desired yaw rate target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in); } // if not auto armed or landed or motor interlock not enabled set throttle to zero and exit immediately if(!ap.auto_armed || ap.land_complete || !motors.get_interlock()) { #if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw // call attitude controller attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); attitude_control.set_throttle_out(0,false,g.throttle_filt); #else // Multirotors do not stabilize roll/pitch/yaw when disarmed attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); #endif #if LAND_REQUIRE_MIN_THROTTLE_TO_DISARM == ENABLED // disarm when the landing detector says we've landed and throttle is at minimum if (ap.land_complete && (ap.throttle_zero || failsafe.radio)) { init_disarm_motors(); } #else // disarm when the landing detector says we've landed if (ap.land_complete) { init_disarm_motors(); } #endif return; } // call attitude controller attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); // pause 4 seconds before beginning land descent float cmb_rate; if(land_pause && millis()-land_start_time < LAND_WITH_DELAY_MS) { cmb_rate = 0; } else { land_pause = false; cmb_rate = get_land_descent_speed(); } // record desired climb rate for logging desired_climb_rate = cmb_rate; // call position controller pos_control.set_alt_target_from_climb_rate(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; } }
// rtl_returnhome_run - return home // called by rtl_run at 100hz or more void Copter::rtl_land_run() { int16_t roll_control = 0, pitch_control = 0; float target_yaw_rate = 0; // if not auto armed or landing completed or motor interlock not enabled set throttle to zero and exit immediately if (!motors.armed() || !ap.auto_armed || ap.land_complete || !motors.get_interlock()) { #if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw // call attitude controller 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 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 // set target to current position wp_nav.init_loiter_target(); #if LAND_REQUIRE_MIN_THROTTLE_TO_DISARM == ENABLED // disarm when the landing detector says we've landed and throttle is at minimum if (ap.land_complete && (ap.throttle_zero || failsafe.radio)) { init_disarm_motors(); } #else // disarm when the landing detector says we've landed if (ap.land_complete) { init_disarm_motors(); } #endif // check if we've completed this stage of RTL rtl_state_complete = ap.land_complete; return; } // relax loiter target if we might be landed if (ap.land_complete_maybe) { wp_nav.loiter_soften_for_landing(); } // process pilot's input if (!failsafe.radio) { if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){ Log_Write_Event(DATA_LAND_CANCELLED_BY_PILOT); // exit land if throttle is high if (!set_mode(LOITER, MODE_REASON_THROTTLE_LAND_ESCAPE)) { set_mode(ALT_HOLD, MODE_REASON_THROTTLE_LAND_ESCAPE); } } if (g.land_repositioning) { // apply SIMPLE mode transform to pilot inputs update_simple_mode(); // process pilot's roll and pitch input roll_control = channel_roll->control_in; pitch_control = channel_pitch->control_in; } // get pilot's desired yaw rate target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in); } // set motors to full range motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // process pilot's roll and pitch input wp_nav.set_pilot_desired_acceleration(roll_control, pitch_control); // run loiter controller wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler); // call z-axis position controller float cmb_rate = get_land_descent_speed(); pos_control.set_alt_target_from_climb_rate(cmb_rate, G_Dt, true); pos_control.update_z_controller(); // record desired climb rate for logging desired_climb_rate = cmb_rate; // roll & pitch from waypoint controller, yaw rate from pilot attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate); // check if we've completed this stage of RTL rtl_state_complete = ap.land_complete; }
// land_nogps_run - runs the land controller // pilot controls roll and pitch angles // should be called at 100hz or more void Copter::land_nogps_run() { float target_roll = 0.0f, target_pitch = 0.0f; float target_yaw_rate = 0; // process pilot inputs if (!failsafe.radio) { if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){ Log_Write_Event(DATA_LAND_CANCELLED_BY_PILOT); // exit land if throttle is high set_mode(ALT_HOLD); } if (g.land_repositioning) { // apply SIMPLE mode transform to pilot inputs update_simple_mode(); // get pilot desired lean angles get_pilot_desired_lean_angles(channel_roll->control_in, channel_pitch->control_in, target_roll, target_pitch, aparm.angle_max); } // get pilot's desired yaw rate target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in); } // if not auto armed or landed or motor interlock not enabled set throttle to zero and exit immediately if (!motors.armed() || !ap.auto_armed || ap.land_complete || !motors.get_interlock()) { #if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw // call attitude controller attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(target_roll, target_pitch, target_yaw_rate, 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 #if LAND_REQUIRE_MIN_THROTTLE_TO_DISARM == ENABLED // disarm when the landing detector says we've landed and throttle is at minimum if (ap.land_complete && (ap.throttle_zero || failsafe.radio)) { init_disarm_motors(); } #else // disarm when the landing detector says we've landed if (ap.land_complete) { init_disarm_motors(); } #endif return; } // 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()); // pause 4 seconds before beginning land descent float cmb_rate; if(land_pause && millis()-land_start_time < LAND_WITH_DELAY_MS) { cmb_rate = 0; } else { land_pause = false; cmb_rate = get_land_descent_speed(); } // record desired climb rate for logging desired_climb_rate = cmb_rate; // call position controller pos_control.set_alt_target_from_climb_rate(cmb_rate, G_Dt, true); pos_control.update_z_controller(); }
// rtl_descent_run - implements the final descent to the RTL_ALT // called by rtl_run at 100hz or more void Copter::rtl_descent_run() { int16_t roll_control = 0, pitch_control = 0; float target_yaw_rate = 0; // 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()) { #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 // set target to current position wp_nav.init_loiter_target(); return; } // process pilot's input if (!failsafe.radio) { if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){ Log_Write_Event(DATA_LAND_CANCELLED_BY_PILOT); // exit land if throttle is high if (!set_mode(LOITER, MODE_REASON_THROTTLE_LAND_ESCAPE)) { set_mode(ALT_HOLD, MODE_REASON_THROTTLE_LAND_ESCAPE); } } if (g.land_repositioning) { // apply SIMPLE mode transform to pilot inputs update_simple_mode(); // process pilot's roll and pitch input roll_control = channel_roll->get_control_in(); pitch_control = channel_pitch->get_control_in(); } // get pilot's desired yaw rate target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); } // set motors to full range motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // process roll, pitch inputs wp_nav.set_pilot_desired_acceleration(roll_control, pitch_control); // run loiter controller wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler); // call z-axis position controller pos_control.set_alt_target_with_slew(rtl_path.descent_target.alt, G_Dt); pos_control.update_z_controller(); // roll & pitch from waypoint controller, yaw rate from pilot attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain()); // check if we've reached within 20cm of final altitude rtl_state_complete = fabsf(rtl_path.descent_target.alt - current_loc.alt) < 20.0f; }
// land_run - runs the land controller // horizontal position controlled with loiter controller // should be called at 100hz or more void Copter::land_gps_run() { int16_t roll_control = 0, pitch_control = 0; float target_yaw_rate = 0; // if not auto armed or landed or motor interlock not enabled set throttle to zero and exit immediately if (!motors.armed() || !ap.auto_armed || ap.land_complete || !motors.get_interlock()) { #if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw // call attitude controller attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw_smooth(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 wp_nav.init_loiter_target(); #if LAND_REQUIRE_MIN_THROTTLE_TO_DISARM == ENABLED // disarm when the landing detector says we've landed and throttle is at minimum if (ap.land_complete && (ap.throttle_zero || failsafe.radio)) { init_disarm_motors(); } #else // disarm when the landing detector says we've landed if (ap.land_complete) { init_disarm_motors(); } #endif return; } // relax loiter target if we might be landed if (ap.land_complete_maybe) { wp_nav.loiter_soften_for_landing(); } // process pilot inputs if (!failsafe.radio) { if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){ Log_Write_Event(DATA_LAND_CANCELLED_BY_PILOT); // exit land if throttle is high if (!set_mode(LOITER)) { set_mode(ALT_HOLD); } } if (g.land_repositioning) { // apply SIMPLE mode transform to pilot inputs update_simple_mode(); // process pilot's roll and pitch input roll_control = channel_roll->control_in; pitch_control = channel_pitch->control_in; // record if pilot has overriden roll or pitch if (roll_control != 0 || pitch_control != 0) { ap.land_repo_active = true; } } // get pilot's desired yaw rate target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in); } // set motors to full range motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // process roll, pitch inputs wp_nav.set_pilot_desired_acceleration(roll_control, pitch_control); #if PRECISION_LANDING == ENABLED // run precision landing if (!ap.land_repo_active) { wp_nav.shift_loiter_target(precland.get_target_shift(wp_nav.get_loiter_target())); } #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); // pause 4 seconds before beginning land descent float cmb_rate; if(land_pause && millis()-land_start_time < 4000) { cmb_rate = 0; } else { land_pause = false; cmb_rate = get_land_descent_speed(); } // 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(cmb_rate, G_Dt, true); 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; // 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->control_in, channel_pitch->control_in); // get pilot's desired yaw rate target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in); // get pilot desired climb rate target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->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(!ap.auto_armed || !motors.get_interlock()) { loiter_state = Loiter_Disarmed; } else if (takeoff_state.running || (ap.land_complete && (channel_throttle->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_Disarmed: wp_nav.init_loiter_target(); #if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw // call attitude controller attitude_control.angle_ef_roll_pitch_rate_ef_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 disarmed attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); #endif // HELI_FRAME pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->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); // run loiter controller wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler); // call attitude controller attitude_control.angle_ef_roll_pitch_rate_ef_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(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(); #if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw // call attitude controller attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(0, 0, 0, get_smoothing_gain()); attitude_control.set_throttle_out(get_throttle_pre_takeoff(channel_throttle->control_in),false,g.throttle_filt); #else // multicopters do not stabilize roll/pitch/yaw when disarmed // move throttle to between minimum and non-takeoff-throttle to keep us on the ground attitude_control.set_throttle_out_unstabilized(get_throttle_pre_takeoff(channel_throttle->control_in),true,g.throttle_filt); #endif pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average); break; case Loiter_Flying: // run loiter controller wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler); // call attitude controller attitude_control.angle_ef_roll_pitch_rate_ef_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate); // run altitude controller if (sonar_enabled && (sonar_alt_health >= SONAR_ALT_HEALTH_MAX)) { // if sonar 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(); break; } }