// 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(); }
// circle_run - runs the circle flight mode // should be called at 100hz or more void Copter::ModeCircle::run() { // initialize speeds and accelerations pos_control->set_max_speed_xy(wp_nav->get_default_speed_xy()); pos_control->set_max_accel_xy(wp_nav->get_wp_acceleration()); pos_control->set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up); pos_control->set_max_accel_z(g.pilot_accel_z); // get pilot's desired yaw rate (or zero if in radio failsafe) float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); if (!is_zero(target_yaw_rate)) { pilot_yaw_override = true; } // get pilot desired climb rate (or zero if in radio failsafe) float target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in()); // adjust climb rate using rangefinder if (copter.rangefinder_alt_ok()) { // if rangefinder is ok, use surface tracking target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt); } // if not armed set throttle to zero and exit immediately if (is_disarmed_or_landed()) { make_safe_spool_down(); return; } // set motors to full range motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); // run circle controller copter.circle_nav->update(); // call attitude controller if (pilot_yaw_override) { attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(copter.circle_nav->get_roll(), copter.circle_nav->get_pitch(), target_yaw_rate); } else { attitude_control->input_euler_angle_roll_pitch_yaw(copter.circle_nav->get_roll(), copter.circle_nav->get_pitch(), copter.circle_nav->get_yaw(), true); } // update altitude target and call position controller // protects heli's from inflight motor interlock disable if (motors->get_desired_spool_state() == AP_Motors::DesiredSpoolState::GROUND_IDLE && !ap.land_complete) { pos_control->set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false); } else { pos_control->set_alt_target_from_climb_rate(target_climb_rate, G_Dt, false); } pos_control->update_z_controller(); }
// manual_control - process manual control void Copter::ModeZigZag::manual_control() { float target_yaw_rate = 0.0f; float target_climb_rate = 0.0f; // process pilot inputs unless we are in radio failsafe if (!copter.failsafe.radio) { float target_roll, target_pitch; // apply SIMPLE mode transform to pilot inputs update_simple_mode(); // convert pilot input to lean angles get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max()); // process pilot's roll and pitch input loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch, G_Dt); // get pilot's desired yaw rate target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); // get pilot desired climb rate target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in()); // make sure the climb rate is in the given range, prevent floating point errors target_climb_rate = constrain_float(target_climb_rate, -get_pilot_speed_dn(), g.pilot_speed_up); } else { // clear out pilot desired acceleration in case radio failsafe event occurs and we // do not switch to RTL for some reason loiter_nav->clear_pilot_desired_acceleration(); } // set motors to full range motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // run loiter controller loiter_nav->update(); // call attitude controller attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(loiter_nav->get_roll(), loiter_nav->get_pitch(), target_yaw_rate); // adjust climb rate using rangefinder target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt); // get avoidance adjusted climb rate target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate); // update altitude target and call position controller pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false); // adjusts target up or down using a climb rate pos_control->update_z_controller(); }
// loiter_run - runs the loiter controller // should be called at 100hz or more void Copter::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; } }
// sport_run - runs the sport controller // should be called at 100hz or more void Copter::sport_run() { float target_roll_rate, target_pitch_rate, target_yaw_rate; float target_climb_rate = 0; 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); // if not armed or throttle at zero, set throttle to zero and exit immediately if (!motors.armed() || ap.throttle_zero || !motors.get_interlock()) { motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); 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); return; } // apply SIMPLE mode transform update_simple_mode(); // get pilot's desired roll and pitch rates // calculate rate requests target_roll_rate = channel_roll->control_in * g.acro_rp_p; target_pitch_rate = channel_pitch->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 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); // 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->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(); } // reset target lean angles and heading while landed if (ap.land_complete) { 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); } // 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); pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average); }else{ // set motors to full range 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); // call throttle 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); } // 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(); } }
// loiter_run - runs the loiter controller // should be called at 100hz or more void Copter::ModeLoiter::run() { float target_roll, target_pitch; float target_yaw_rate = 0.0f; float target_climb_rate = 0.0f; float takeoff_climb_rate = 0.0f; // initialize vertical speed and acceleration pos_control->set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up); pos_control->set_max_accel_z(g.pilot_accel_z); // process pilot inputs unless we are in radio failsafe if (!copter.failsafe.radio) { // apply SIMPLE mode transform to pilot inputs update_simple_mode(); // convert pilot input to lean angles get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max()); // process pilot's roll and pitch input loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch, G_Dt); // get pilot's desired yaw rate target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); // get pilot desired climb rate target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in()); target_climb_rate = constrain_float(target_climb_rate, -get_pilot_speed_dn(), g.pilot_speed_up); } else { // clear out pilot desired acceleration in case radio failsafe event occurs and we do not switch to RTL for some reason loiter_nav->clear_pilot_desired_acceleration(); } // relax loiter target if we might be landed if (ap.land_complete_maybe) { loiter_nav->soften_for_landing(); } // Loiter State Machine Determination AltHoldModeState loiter_state = get_alt_hold_state(target_climb_rate); // Loiter State Machine switch (loiter_state) { case AltHold_MotorStopped: attitude_control->reset_rate_controller_I_terms(); attitude_control->set_yaw_target_to_current_heading(); pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero loiter_nav->init_target(); attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(loiter_nav->get_roll(), loiter_nav->get_pitch(), target_yaw_rate); pos_control->update_z_controller(); break; case AltHold_Takeoff: // initiate take-off if (!takeoff.running()) { takeoff.start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f)); // indicate we are taking off set_land_complete(false); // clear i term when we're taking off set_throttle_takeoff(); } // get takeoff adjusted pilot and takeoff climb rates takeoff.get_climb_rates(target_climb_rate, takeoff_climb_rate); // get avoidance adjusted climb rate target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate); // run loiter controller loiter_nav->update(); // call attitude controller attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(loiter_nav->get_roll(), loiter_nav->get_pitch(), target_yaw_rate); // update altitude target and call position controller pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false); pos_control->add_takeoff_climb_rate(takeoff_climb_rate, G_Dt); pos_control->update_z_controller(); break; case AltHold_Landed_Ground_Idle: attitude_control->reset_rate_controller_I_terms(); attitude_control->set_yaw_target_to_current_heading(); // FALLTHROUGH case AltHold_Landed_Pre_Takeoff: loiter_nav->init_target(); attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f); pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero pos_control->update_z_controller(); break; case AltHold_Flying: // set motors to full range motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); #if PRECISION_LANDING == ENABLED if (do_precision_loiter()) { precision_loiter_xy(); } #endif // run loiter controller loiter_nav->update(); // call attitude controller attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(loiter_nav->get_roll(), loiter_nav->get_pitch(), target_yaw_rate); // adjust climb rate using rangefinder target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt); // get avoidance adjusted climb rate target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate); pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false); pos_control->update_z_controller(); break; } }
// sport_run - runs the sport controller // should be called at 100hz or more void Copter::ModeSport::run() { SportModeState sport_state; float takeoff_climb_rate = 0.0f; // initialize vertical speed and acceleration pos_control->set_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up); pos_control->set_accel_z(g.pilot_accel_z); // apply SIMPLE mode transform update_simple_mode(); // get pilot's desired roll and pitch rates // calculate rate requests float target_roll_rate = channel_roll->get_control_in() * g.acro_rp_p; float target_pitch_rate = channel_pitch->get_control_in() * g.acro_rp_p; // get attitude targets const Vector3f att_target = attitude_control->get_att_target_euler_cd(); // Calculate trainer mode earth frame rate command for roll int32_t roll_angle = wrap_180_cd(att_target.x); target_roll_rate -= constrain_int32(roll_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE) * g.acro_balance_roll; // Calculate trainer mode earth frame rate command for pitch int32_t pitch_angle = wrap_180_cd(att_target.y); target_pitch_rate -= constrain_int32(pitch_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE) * g.acro_balance_pitch; AP_Vehicle::MultiCopter &aparm = copter.aparm; if (roll_angle > aparm.angle_max){ target_roll_rate -= g.acro_rp_p*(roll_angle-aparm.angle_max); }else if (roll_angle < -aparm.angle_max) { target_roll_rate -= g.acro_rp_p*(roll_angle+aparm.angle_max); } if (pitch_angle > aparm.angle_max){ target_pitch_rate -= g.acro_rp_p*(pitch_angle-aparm.angle_max); }else if (pitch_angle < -aparm.angle_max) { target_pitch_rate -= g.acro_rp_p*(pitch_angle+aparm.angle_max); } // get pilot's desired yaw rate float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); // get pilot desired climb rate float target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in()); target_climb_rate = constrain_float(target_climb_rate, -get_pilot_speed_dn(), g.pilot_speed_up); // State Machine Determination if (!motors->armed() || !motors->get_interlock()) { sport_state = Sport_MotorStopped; } else if (takeoff_state.running || takeoff_triggered(target_climb_rate)) { sport_state = Sport_Takeoff; } else if (!ap.auto_armed || ap.land_complete) { sport_state = Sport_Landed; } else { sport_state = Sport_Flying; } // State Machine switch (sport_state) { case Sport_MotorStopped: motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN); attitude_control->input_euler_rate_roll_pitch_yaw(target_roll_rate, target_pitch_rate, target_yaw_rate); #if FRAME_CONFIG == HELI_FRAME // force descent rate and call position controller pos_control->set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false); #else attitude_control->relax_attitude_controllers(); attitude_control->reset_rate_controller_I_terms(); attitude_control->set_yaw_target_to_current_heading(); pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero #endif pos_control->update_z_controller(); break; case Sport_Takeoff: // set motors to full range motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // initiate take-off if (!takeoff_state.running) { takeoff_timer_start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f)); // indicate we are taking off set_land_complete(false); // clear i terms set_throttle_takeoff(); } // get take-off adjusted pilot and takeoff climb rates takeoff_get_climb_rates(target_climb_rate, takeoff_climb_rate); // get avoidance adjusted climb rate target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate); // call attitude controller attitude_control->input_euler_rate_roll_pitch_yaw(target_roll_rate, target_pitch_rate, target_yaw_rate); // call position controller pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false); pos_control->add_takeoff_climb_rate(takeoff_climb_rate, G_Dt); pos_control->update_z_controller(); break; case Sport_Landed: // set motors to spin-when-armed if throttle below deadzone, otherwise full range (but motors will only spin at min throttle) if (target_climb_rate < 0.0f) { motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); } else { motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); } attitude_control->reset_rate_controller_I_terms(); attitude_control->set_yaw_target_to_current_heading(); attitude_control->input_euler_rate_roll_pitch_yaw(target_roll_rate, target_pitch_rate, target_yaw_rate); pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero pos_control->update_z_controller(); break; case Sport_Flying: motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // call attitude controller attitude_control->input_euler_rate_roll_pitch_yaw(target_roll_rate, target_pitch_rate, target_yaw_rate); // adjust climb rate using rangefinder target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt); // get avoidance adjusted climb rate target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate); // call position controller pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false); pos_control->update_z_controller(); break; } }
// 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(); }
// 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()); }
// arm_checks - perform final checks before arming // always called just before arming. Return true if ok to arm // has side-effect that logging is started bool Copter::arm_checks(bool display_failure, bool arming_from_gcs) { #if LOGGING_ENABLED == ENABLED // start dataflash start_logging(); #endif // check accels and gyro are healthy if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_INS)) { //check if accelerometers have calibrated and require reboot if (ins.accel_cal_requires_reboot()) { if (display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL, "PreArm: Accelerometers calibrated requires reboot"); } return false; } if (!ins.get_accel_health_all()) { if (display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Accelerometers not healthy"); } return false; } if (!ins.get_gyro_health_all()) { if (display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Gyros not healthy"); } return false; } // get ekf attitude (if bad, it's usually the gyro biases) if (!pre_arm_ekf_attitude_check()) { if (display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: gyros still settling"); } return false; } } // always check if inertial nav has started and is ready if (!ahrs.healthy()) { if (display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Waiting for Nav Checks"); } return false; } if (compass.is_calibrating()) { if (display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Compass calibration running"); } return false; } //check if compass has calibrated and requires reboot if (compass.compass_cal_requires_reboot()) { if (display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL, "PreArm: Compass calibrated requires reboot"); } return false; } // always check if the current mode allows arming if (!mode_allows_arming(control_mode, arming_from_gcs)) { if (display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Mode not armable"); } return false; } // always check gps if (!pre_arm_gps_checks(display_failure)) { return false; } // if we are using motor interlock switch and it's enabled, fail to arm // skip check in Throw mode which takes control of the motor interlock if (ap.using_interlock && motors.get_interlock()) { gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Motor Interlock Enabled"); return false; } // if we are not using Emergency Stop switch option, force Estop false to ensure motors // can run normally if (!check_if_auxsw_mode_used(AUXSW_MOTOR_ESTOP)){ set_motor_emergency_stop(false); // if we are using motor Estop switch, it must not be in Estop position } else if (check_if_auxsw_mode_used(AUXSW_MOTOR_ESTOP) && ap.motor_emergency_stop){ gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Motor Emergency Stopped"); return false; } // succeed if arming checks are disabled if (g.arming_check == ARMING_CHECK_NONE) { return true; } // baro checks if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_BARO)) { // baro health check if (!barometer.all_healthy()) { if (display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Barometer not healthy"); } return false; } // Check baro & inav alt are within 1m if EKF is operating in an absolute position mode. // Do not check if intending to operate in a ground relative height mode as EKF will output a ground relative height // that may differ from the baro height due to baro drift. nav_filter_status filt_status = inertial_nav.get_filter_status(); bool using_baro_ref = (!filt_status.flags.pred_horiz_pos_rel && filt_status.flags.pred_horiz_pos_abs); if (using_baro_ref && (fabsf(inertial_nav.get_altitude() - baro_alt) > PREARM_MAX_ALT_DISPARITY_CM)) { if (display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Altitude disparity"); } return false; } } #if AC_FENCE == ENABLED // check vehicle is within fence if (!fence.pre_arm_check()) { if (display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: check fence"); } return false; } #endif // check lean angle if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_INS)) { if (degrees(acosf(ahrs.cos_roll()*ahrs.cos_pitch()))*100.0f > aparm.angle_max) { if (display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Leaning"); } return false; } } // check battery voltage if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_VOLTAGE)) { if (failsafe.battery || (!ap.usb_connected && battery.exhausted(g.fs_batt_voltage, g.fs_batt_mah))) { if (display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Check Battery"); } return false; } } // check for missing terrain data if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_PARAMETERS)) { if (!pre_arm_terrain_check(display_failure)) { return false; } } // check adsb if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_PARAMETERS)) { if (failsafe.adsb) { if (display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: ADSB threat detected"); } return false; } } // check throttle if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_RC)) { // check throttle is not too low - must be above failsafe throttle if (g.failsafe_throttle != FS_THR_DISABLED && channel_throttle->get_radio_in() < g.failsafe_throttle_value) { if (display_failure) { #if FRAME_CONFIG == HELI_FRAME gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Collective below Failsafe"); #else gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Throttle below Failsafe"); #endif } return false; } // check throttle is not too high - skips checks if arming from GCS in Guided if (!(arming_from_gcs && (control_mode == GUIDED || control_mode == GUIDED_NOGPS))) { // above top of deadband is too always high if (get_pilot_desired_climb_rate(channel_throttle->get_control_in()) > 0.0f) { if (display_failure) { #if FRAME_CONFIG == HELI_FRAME gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Collective too high"); #else gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Throttle too high"); #endif } return false; } // in manual modes throttle must be at zero if ((mode_has_manual_throttle(control_mode) || control_mode == DRIFT) && channel_throttle->get_control_in() > 0) { if (display_failure) { #if FRAME_CONFIG == HELI_FRAME gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Collective too high"); #else gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Throttle too high"); #endif } return false; } } } // check if safety switch has been pushed if (hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED) { if (display_failure) { gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Safety Switch"); } return false; } // if we've gotten this far all is ok return true; }
// 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(); }
// althold_run - runs the althold controller // should be called at 100hz or more void Copter::althold_run() { float target_roll, target_pitch; float target_yaw_rate; float target_climb_rate; float takeoff_climb_rate = 0.0f; // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately if(!ap.auto_armed || !motors.get_interlock()) { 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); return; } // apply SIMPLE mode transform to pilot inputs update_simple_mode(); // get pilot desired 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); // 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); // 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->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(); } // reset target lean angles and heading while landed if (ap.land_complete) { attitude_control.set_throttle_out_unstabilized(get_throttle_pre_takeoff(channel_throttle->control_in),true,g.throttle_filt); pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(channel_throttle->control_in)-throttle_average); }else{ // call attitude controller attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); // body-frame rate controller is run directly from 100hz loop // call throttle controller if (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); } // 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(); } }
// loiter_run - runs the loiter controller // should be called at 100hz or more void Copter::ModeLoiter::run() { LoiterModeState loiter_state; float target_yaw_rate = 0.0f; float target_climb_rate = 0.0f; float takeoff_climb_rate = 0.0f; // initialize vertical speed and acceleration pos_control->set_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up); pos_control->set_accel_z(g.pilot_accel_z); // process pilot inputs unless we are in radio failsafe if (!copter.failsafe.radio) { // apply SIMPLE mode transform to pilot inputs update_simple_mode(); // process pilot's roll and pitch input wp_nav->set_pilot_desired_acceleration(channel_roll->get_control_in(), channel_pitch->get_control_in()); // get pilot's desired yaw rate target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); // get pilot desired climb rate target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in()); target_climb_rate = constrain_float(target_climb_rate, -get_pilot_speed_dn(), g.pilot_speed_up); } else { // clear out pilot desired acceleration in case radio failsafe event occurs and we do not switch to RTL for some reason wp_nav->clear_pilot_desired_acceleration(); } // relax loiter target if we might be landed if (ap.land_complete_maybe) { wp_nav->loiter_soften_for_landing(); } // Loiter State Machine Determination if (!motors->armed() || !motors->get_interlock()) { loiter_state = Loiter_MotorStopped; } else if (takeoff_state.running || takeoff_triggered(target_climb_rate)) { loiter_state = Loiter_Takeoff; } else if (!ap.auto_armed || ap.land_complete) { loiter_state = Loiter_Landed; } else { loiter_state = Loiter_Flying; } // Loiter State Machine switch (loiter_state) { case Loiter_MotorStopped: motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN); #if FRAME_CONFIG == HELI_FRAME // force descent rate and call position controller pos_control->set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false); #else wp_nav->init_loiter_target(); attitude_control->reset_rate_controller_I_terms(); attitude_control->set_yaw_target_to_current_heading(); pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero #endif wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler); attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain()); pos_control->update_z_controller(); break; case Loiter_Takeoff: // set motors to full range motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // initiate take-off if (!takeoff_state.running) { takeoff_timer_start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f)); // indicate we are taking off set_land_complete(false); // clear i term when we're taking off set_throttle_takeoff(); } // get takeoff adjusted pilot and takeoff climb rates takeoff_get_climb_rates(target_climb_rate, takeoff_climb_rate); // get avoidance adjusted climb rate target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate); // run loiter controller wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler); // call attitude controller attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain()); // update altitude target and call position controller pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false); pos_control->add_takeoff_climb_rate(takeoff_climb_rate, G_Dt); pos_control->update_z_controller(); break; case Loiter_Landed: // set motors to spin-when-armed if throttle below deadzone, otherwise full range (but motors will only spin at min throttle) if (target_climb_rate < 0.0f) { motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); } else { motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); } wp_nav->init_loiter_target(); attitude_control->reset_rate_controller_I_terms(); attitude_control->set_yaw_target_to_current_heading(); attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain()); pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero pos_control->update_z_controller(); break; case Loiter_Flying: // set motors to full range motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); #if PRECISION_LANDING == ENABLED if (do_precision_loiter()) { precision_loiter_xy(); } #endif // run loiter controller wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler); // call attitude controller attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain()); // adjust climb rate using rangefinder if (copter.rangefinder_alt_ok()) { // if rangefinder is ok, use surface tracking target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt); } // get avoidance adjusted climb rate target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate); // update altitude target and call position controller pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false); pos_control->update_z_controller(); break; } }
// 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; // 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->control_in, channel_pitch->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->control_in); // get pilot desired climb rate float 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); #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->control_in > get_takeoff_trigger_throttle()) && motors.rotor_runup_complete()); #else bool takeoff_triggered = (ap.land_complete && (channel_throttle->control_in > get_takeoff_trigger_throttle())); #endif // Alt Hold State Machine Determination if(!ap.auto_armed || !motors.get_interlock()) { althold_state = AltHold_Disarmed; } 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_Disarmed: #if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw attitude_control.set_yaw_target_to_current_heading(); // 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 // Multicopter 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 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); // call attitude controller attitude_control.angle_ef_roll_pitch_rate_ef_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 // Helicopters always stabilize roll/pitch/yaw attitude_control.set_yaw_target_to_current_heading(); // 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(get_throttle_pre_takeoff(channel_throttle->control_in),false,g.throttle_filt); #else // Multicopter do not stabilize roll/pitch/yaw when disarmed 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 AltHold_Flying: // call attitude controller attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); // call throttle 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); } // 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 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; } }
// 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; } }
// 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(); }
// 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; } }
// circle_run - runs the circle flight mode // should be called at 100hz or more void Copter::ModeCircle::run() { float target_yaw_rate = 0; float target_climb_rate = 0; // initialize speeds and accelerations pos_control->set_speed_xy(wp_nav->get_speed_xy()); pos_control->set_accel_xy(wp_nav->get_wp_acceleration()); pos_control->set_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up); pos_control->set_accel_z(g.pilot_accel_z); // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately if (!motors->armed() || !ap.auto_armed || ap.land_complete || !motors->get_interlock()) { // To-Do: add some initialisation of position controllers zero_throttle_and_relax_ac(); pos_control->set_alt_target_to_current_alt(); return; } // process pilot inputs if (!copter.failsafe.radio) { // get pilot's desired yaw rate target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); if (!is_zero(target_yaw_rate)) { pilot_yaw_override = true; } // get pilot desired climb rate 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 copter.circle_nav->update(); // call attitude controller if (pilot_yaw_override) { attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(copter.circle_nav->get_roll(), copter.circle_nav->get_pitch(), target_yaw_rate, get_smoothing_gain()); }else{ attitude_control->input_euler_angle_roll_pitch_yaw(copter.circle_nav->get_roll(), copter.circle_nav->get_pitch(), copter.circle_nav->get_yaw(),true, get_smoothing_gain()); } // adjust climb rate using rangefinder if (copter.rangefinder_alt_ok()) { // if rangefinder is ok, use surface tracking target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt); } // 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(); }