// rtl_descent_run - implements the final descent to the RTL_ALT // called by rtl_run at 100hz or more void Copter::ModeRTL::descent_run() { float target_roll = 0.0f; float target_pitch = 0.0f; float target_yaw_rate = 0.0f; // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) { zero_throttle_and_relax_ac(); // set target to current position loiter_nav->clear_pilot_desired_acceleration(); loiter_nav->init_target(); return; } // process pilot's input if (!copter.failsafe.radio) { if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && copter.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 (!copter.set_mode(LOITER, MODE_REASON_THROTTLE_LAND_ESCAPE)) { copter.set_mode(ALT_HOLD, MODE_REASON_THROTTLE_LAND_ESCAPE); } } if (g.land_repositioning) { // 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()); // record if pilot has overriden roll or pitch if (!is_zero(target_roll) || !is_zero(target_pitch)) { ap.land_repo_active = true; } } // 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 loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch, G_Dt); // run loiter controller loiter_nav->update(); // 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(loiter_nav->get_roll(), loiter_nav->get_pitch(), target_yaw_rate); // check if we've reached within 20cm of final altitude _state_complete = labs(rtl_path.descent_target.alt - copter.current_loc.alt) < 20; }
// 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(); }
// get_pilot_desired_angle_rates - transform pilot's roll pitch and yaw input into a desired lean angle rates // returns desired angle rates in centi-degrees-per-second void Copter::ModeAcro::get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int16_t yaw_in, float &roll_out, float &pitch_out, float &yaw_out) { float rate_limit; Vector3f rate_ef_level, rate_bf_level, rate_bf_request; AP_Vehicle::MultiCopter &aparm = copter.aparm; // apply circular limit to pitch and roll inputs float total_in = norm(pitch_in, roll_in); if (total_in > ROLL_PITCH_YAW_INPUT_MAX) { float ratio = (float)ROLL_PITCH_YAW_INPUT_MAX / total_in; roll_in *= ratio; pitch_in *= ratio; } // calculate roll, pitch rate requests if (g.acro_rp_expo <= 0) { rate_bf_request.x = roll_in * g.acro_rp_p; rate_bf_request.y = pitch_in * g.acro_rp_p; } else { // expo variables float rp_in, rp_in3, rp_out; // range check expo if (g.acro_rp_expo > 1.0f) { g.acro_rp_expo = 1.0f; } // roll expo rp_in = float(roll_in)/ROLL_PITCH_YAW_INPUT_MAX; rp_in3 = rp_in*rp_in*rp_in; rp_out = (g.acro_rp_expo * rp_in3) + ((1.0f - g.acro_rp_expo) * rp_in); rate_bf_request.x = ROLL_PITCH_YAW_INPUT_MAX * rp_out * g.acro_rp_p; // pitch expo rp_in = float(pitch_in)/ROLL_PITCH_YAW_INPUT_MAX; rp_in3 = rp_in*rp_in*rp_in; rp_out = (g.acro_rp_expo * rp_in3) + ((1.0f - g.acro_rp_expo) * rp_in); rate_bf_request.y = ROLL_PITCH_YAW_INPUT_MAX * rp_out * g.acro_rp_p; } // calculate yaw rate request rate_bf_request.z = get_pilot_desired_yaw_rate(yaw_in); // calculate earth frame rate corrections to pull the copter back to level while in ACRO mode if (g.acro_trainer != ACRO_TRAINER_DISABLED) { // 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); rate_ef_level.x = -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); rate_ef_level.y = -constrain_int32(pitch_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE) * g.acro_balance_pitch; // Calculate trainer mode earth frame rate command for yaw rate_ef_level.z = 0; // Calculate angle limiting earth frame rate commands if (g.acro_trainer == ACRO_TRAINER_LIMITED) { if (roll_angle > aparm.angle_max){ rate_ef_level.x -= g.acro_balance_roll*(roll_angle-aparm.angle_max); }else if (roll_angle < -aparm.angle_max) { rate_ef_level.x -= g.acro_balance_roll*(roll_angle+aparm.angle_max); } if (pitch_angle > aparm.angle_max){ rate_ef_level.y -= g.acro_balance_pitch*(pitch_angle-aparm.angle_max); }else if (pitch_angle < -aparm.angle_max) { rate_ef_level.y -= g.acro_balance_pitch*(pitch_angle+aparm.angle_max); } } // convert earth-frame level rates to body-frame level rates attitude_control->euler_rate_to_ang_vel(attitude_control->get_att_target_euler_cd()*radians(0.01f), rate_ef_level, rate_bf_level); // combine earth frame rate corrections with rate requests if (g.acro_trainer == ACRO_TRAINER_LIMITED) { rate_bf_request.x += rate_bf_level.x; rate_bf_request.y += rate_bf_level.y; rate_bf_request.z += rate_bf_level.z; }else{ float acro_level_mix = constrain_float(float(1-MAX(MAX(abs(roll_in), abs(pitch_in)), abs(yaw_in)))/4500.0, 0, 1)*ahrs.cos_pitch(); // Scale leveling rates by stick input rate_bf_level = rate_bf_level*acro_level_mix; // Calculate rate limit to prevent change of rate through inverted rate_limit = fabsf(fabsf(rate_bf_request.x)-fabsf(rate_bf_level.x)); rate_bf_request.x += rate_bf_level.x; rate_bf_request.x = constrain_float(rate_bf_request.x, -rate_limit, rate_limit); // Calculate rate limit to prevent change of rate through inverted rate_limit = fabsf(fabsf(rate_bf_request.y)-fabsf(rate_bf_level.y)); rate_bf_request.y += rate_bf_level.y; rate_bf_request.y = constrain_float(rate_bf_request.y, -rate_limit, rate_limit); // Calculate rate limit to prevent change of rate through inverted rate_limit = fabsf(fabsf(rate_bf_request.z)-fabsf(rate_bf_level.z)); rate_bf_request.z += rate_bf_level.z; rate_bf_request.z = constrain_float(rate_bf_request.z, -rate_limit, rate_limit); } } // hand back rate request roll_out = rate_bf_request.x; pitch_out = rate_bf_request.y; yaw_out = rate_bf_request.z; }
// heli_acro_run - runs the acro controller // should be called at 100hz or more void Copter::heli_acro_run() { float target_roll, target_pitch, target_yaw; 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(); if (!motors.has_flybar()){ // convert the input to the desired body frame rate get_pilot_desired_angle_rates(channel_roll->control_in, channel_pitch->control_in, channel_yaw->control_in, target_roll, target_pitch, target_yaw); // run attitude controller attitude_control.rate_bf_roll_pitch_yaw(target_roll, target_pitch, target_yaw); }else{ /* for fly-bar passthrough use control_in values with no deadzone. This gives true pass-through. */ float roll_in = channel_roll->pwm_to_angle_dz(0); float pitch_in = channel_pitch->pwm_to_angle_dz(0); float yaw_in; if (motors.supports_yaw_passthrough()) { // if the tail on a flybar heli has an external gyro then // also use no deadzone for the yaw control and // pass-through the input direct to output. yaw_in = channel_yaw->pwm_to_angle_dz(0); } else { // if there is no external gyro then run the usual // ACRO_YAW_P gain on the input control, including // deadzone yaw_in = get_pilot_desired_yaw_rate(channel_yaw->control_in); } // run attitude controller attitude_control.passthrough_bf_roll_pitch_rate_yaw(roll_in, pitch_in, yaw_in); } // get pilot's desired throttle pilot_throttle_scaled = input_manager.get_pilot_desired_collective(channel_throttle->control_in); // output pilot's throttle without angle boost attitude_control.set_throttle_out(pilot_throttle_scaled, false, g.throttle_filt); }
// 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())-motors.get_throttle_hover()); 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())-motors.get_throttle_hover()); return; }else{ // convert pilot input to lean angles get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, aparm.angle_max); // convert inertial nav earth-frame velocities to body-frame // To-Do: move this to AP_Math (or perhaps we already have a function to do this) vel_fw = vel.x*ahrs.cos_yaw() + vel.y*ahrs.sin_yaw(); vel_right = -vel.x*ahrs.sin_yaw() + vel.y*ahrs.cos_yaw(); // If not in LOITER, retrieve latest wind compensation lean angles related to current yaw if (poshold.roll_mode != POSHOLD_LOITER || poshold.pitch_mode != POSHOLD_LOITER) poshold_get_wind_comp_lean_angles(poshold.wind_comp_roll, poshold.wind_comp_pitch); // Roll state machine // Each state (aka mode) is responsible for: // 1. dealing with pilot input // 2. calculating the final roll output to the attitude controller // 3. checking if the state (aka mode) should be changed and if 'yes' perform any required initialisation for the new state switch (poshold.roll_mode) { case POSHOLD_PILOT_OVERRIDE: // update pilot desired roll angle using latest radio input // this filters the input so that it returns to zero no faster than the brake-rate poshold_update_pilot_lean_angle(poshold.pilot_roll, target_roll); // switch to BRAKE mode for next iteration if no pilot input if (is_zero(target_roll) && (fabsf(poshold.pilot_roll) < 2 * g.poshold_brake_rate)) { // initialise BRAKE mode poshold.roll_mode = POSHOLD_BRAKE; // Set brake roll mode poshold.brake_roll = 0; // initialise braking angle to zero poshold.brake_angle_max_roll = 0; // reset brake_angle_max so we can detect when vehicle begins to flatten out during braking poshold.brake_timeout_roll = POSHOLD_BRAKE_TIME_ESTIMATE_MAX; // number of cycles the brake will be applied, updated during braking mode. poshold.braking_time_updated_roll = false; // flag the braking time can be re-estimated } // final lean angle should be pilot input plus wind compensation poshold.roll = poshold.pilot_roll + poshold.wind_comp_roll; break; case POSHOLD_BRAKE: case POSHOLD_BRAKE_READY_TO_LOITER: // calculate brake_roll angle to counter-act velocity poshold_update_brake_angle_from_velocity(poshold.brake_roll, vel_right); // update braking time estimate if (!poshold.braking_time_updated_roll) { // check if brake angle is increasing if (abs(poshold.brake_roll) >= poshold.brake_angle_max_roll) { poshold.brake_angle_max_roll = abs(poshold.brake_roll); } else { // braking angle has started decreasing so re-estimate braking time poshold.brake_timeout_roll = 1+(uint16_t)(LOOP_RATE_FACTOR*15L*(int32_t)(abs(poshold.brake_roll))/(10L*(int32_t)g.poshold_brake_rate)); // the 1.2 (12/10) factor has to be tuned in flight, here it means 120% of the "normal" time. poshold.braking_time_updated_roll = true; } } // if velocity is very low reduce braking time to 0.5seconds if ((fabsf(vel_right) <= POSHOLD_SPEED_0) && (poshold.brake_timeout_roll > 50*LOOP_RATE_FACTOR)) { poshold.brake_timeout_roll = 50*LOOP_RATE_FACTOR; } // reduce braking timer if (poshold.brake_timeout_roll > 0) { poshold.brake_timeout_roll--; } else { // indicate that we are ready to move to Loiter. // Loiter will only actually be engaged once both roll_mode and pitch_mode are changed to POSHOLD_BRAKE_READY_TO_LOITER // logic for engaging loiter is handled below the roll and pitch mode switch statements poshold.roll_mode = POSHOLD_BRAKE_READY_TO_LOITER; } // final lean angle is braking angle + wind compensation angle poshold.roll = poshold.brake_roll + poshold.wind_comp_roll; // check for pilot input if (!is_zero(target_roll)) { // init transition to pilot override poshold_roll_controller_to_pilot_override(); } break; case POSHOLD_BRAKE_TO_LOITER: case POSHOLD_LOITER: // these modes are combined roll-pitch modes and are handled below break; case POSHOLD_CONTROLLER_TO_PILOT_OVERRIDE: // update pilot desired roll angle using latest radio input // this filters the input so that it returns to zero no faster than the brake-rate poshold_update_pilot_lean_angle(poshold.pilot_roll, target_roll); // count-down loiter to pilot timer if (poshold.controller_to_pilot_timer_roll > 0) { poshold.controller_to_pilot_timer_roll--; } else { // when timer runs out switch to full pilot override for next iteration poshold.roll_mode = POSHOLD_PILOT_OVERRIDE; } // calculate controller_to_pilot mix ratio controller_to_pilot_roll_mix = (float)poshold.controller_to_pilot_timer_roll / (float)POSHOLD_CONTROLLER_TO_PILOT_MIX_TIMER; // mix final loiter lean angle and pilot desired lean angles poshold.roll = poshold_mix_controls(controller_to_pilot_roll_mix, poshold.controller_final_roll, poshold.pilot_roll + poshold.wind_comp_roll); break; } // Pitch state machine // Each state (aka mode) is responsible for: // 1. dealing with pilot input // 2. calculating the final pitch output to the attitude contpitcher // 3. checking if the state (aka mode) should be changed and if 'yes' perform any required initialisation for the new state switch (poshold.pitch_mode) { case POSHOLD_PILOT_OVERRIDE: // update pilot desired pitch angle using latest radio input // this filters the input so that it returns to zero no faster than the brake-rate poshold_update_pilot_lean_angle(poshold.pilot_pitch, target_pitch); // switch to BRAKE mode for next iteration if no pilot input if (is_zero(target_pitch) && (fabsf(poshold.pilot_pitch) < 2 * g.poshold_brake_rate)) { // initialise BRAKE mode poshold.pitch_mode = POSHOLD_BRAKE; // set brake pitch mode poshold.brake_pitch = 0; // initialise braking angle to zero poshold.brake_angle_max_pitch = 0; // reset brake_angle_max so we can detect when vehicle begins to flatten out during braking poshold.brake_timeout_pitch = POSHOLD_BRAKE_TIME_ESTIMATE_MAX; // number of cycles the brake will be applied, updated during braking mode. poshold.braking_time_updated_pitch = false; // flag the braking time can be re-estimated } // final lean angle should be pilot input plus wind compensation poshold.pitch = poshold.pilot_pitch + poshold.wind_comp_pitch; break; case POSHOLD_BRAKE: case POSHOLD_BRAKE_READY_TO_LOITER: // calculate brake_pitch angle to counter-act velocity poshold_update_brake_angle_from_velocity(poshold.brake_pitch, -vel_fw); // update braking time estimate if (!poshold.braking_time_updated_pitch) { // check if brake angle is increasing if (abs(poshold.brake_pitch) >= poshold.brake_angle_max_pitch) { poshold.brake_angle_max_pitch = abs(poshold.brake_pitch); } else { // braking angle has started decreasing so re-estimate braking time poshold.brake_timeout_pitch = 1+(uint16_t)(LOOP_RATE_FACTOR*15L*(int32_t)(abs(poshold.brake_pitch))/(10L*(int32_t)g.poshold_brake_rate)); // the 1.2 (12/10) factor has to be tuned in flight, here it means 120% of the "normal" time. poshold.braking_time_updated_pitch = true; } } // if velocity is very low reduce braking time to 0.5seconds if ((fabsf(vel_fw) <= POSHOLD_SPEED_0) && (poshold.brake_timeout_pitch > 50*LOOP_RATE_FACTOR)) { poshold.brake_timeout_pitch = 50*LOOP_RATE_FACTOR; } // reduce braking timer if (poshold.brake_timeout_pitch > 0) { poshold.brake_timeout_pitch--; } else { // indicate that we are ready to move to Loiter. // Loiter will only actually be engaged once both pitch_mode and pitch_mode are changed to POSHOLD_BRAKE_READY_TO_LOITER // logic for engaging loiter is handled below the pitch and pitch mode switch statements poshold.pitch_mode = POSHOLD_BRAKE_READY_TO_LOITER; } // final lean angle is braking angle + wind compensation angle poshold.pitch = poshold.brake_pitch + poshold.wind_comp_pitch; // check for pilot input if (!is_zero(target_pitch)) { // init transition to pilot override poshold_pitch_controller_to_pilot_override(); } break; case POSHOLD_BRAKE_TO_LOITER: case POSHOLD_LOITER: // these modes are combined pitch-pitch modes and are handled below break; case POSHOLD_CONTROLLER_TO_PILOT_OVERRIDE: // update pilot desired pitch angle using latest radio input // this filters the input so that it returns to zero no faster than the brake-rate poshold_update_pilot_lean_angle(poshold.pilot_pitch, target_pitch); // count-down loiter to pilot timer if (poshold.controller_to_pilot_timer_pitch > 0) { poshold.controller_to_pilot_timer_pitch--; } else { // when timer runs out switch to full pilot override for next iteration poshold.pitch_mode = POSHOLD_PILOT_OVERRIDE; } // calculate controller_to_pilot mix ratio controller_to_pilot_pitch_mix = (float)poshold.controller_to_pilot_timer_pitch / (float)POSHOLD_CONTROLLER_TO_PILOT_MIX_TIMER; // mix final loiter lean angle and pilot desired lean angles poshold.pitch = poshold_mix_controls(controller_to_pilot_pitch_mix, poshold.controller_final_pitch, poshold.pilot_pitch + poshold.wind_comp_pitch); break; } // set motors to full range motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // // Shared roll & pitch states (POSHOLD_BRAKE_TO_LOITER and POSHOLD_LOITER) // // switch into LOITER mode when both roll and pitch are ready if (poshold.roll_mode == POSHOLD_BRAKE_READY_TO_LOITER && poshold.pitch_mode == POSHOLD_BRAKE_READY_TO_LOITER) { poshold.roll_mode = POSHOLD_BRAKE_TO_LOITER; poshold.pitch_mode = POSHOLD_BRAKE_TO_LOITER; poshold.brake_to_loiter_timer = POSHOLD_BRAKE_TO_LOITER_TIMER; // init loiter controller wp_nav.init_loiter_target(inertial_nav.get_position(), poshold.loiter_reset_I); // (false) to avoid I_term reset. In original code, velocity(0,0,0) was used instead of current velocity: wp_nav.init_loiter_target(inertial_nav.get_position(), Vector3f(0,0,0)); // at this stage, we are going to run update_loiter that will reset I_term once. From now, we ensure next time that we will enter loiter and update it, I_term won't be reset anymore poshold.loiter_reset_I = false; // set delay to start of wind compensation estimate updates poshold.wind_comp_start_timer = POSHOLD_WIND_COMP_START_TIMER; } // roll-mode is used as the combined roll+pitch mode when in BRAKE_TO_LOITER or LOITER modes if (poshold.roll_mode == POSHOLD_BRAKE_TO_LOITER || poshold.roll_mode == POSHOLD_LOITER) { // force pitch mode to be same as roll_mode just to keep it consistent (it's not actually used in these states) poshold.pitch_mode = poshold.roll_mode; // handle combined roll+pitch mode switch (poshold.roll_mode) { case POSHOLD_BRAKE_TO_LOITER: // reduce brake_to_loiter timer if (poshold.brake_to_loiter_timer > 0) { poshold.brake_to_loiter_timer--; } else { // progress to full loiter on next iteration poshold.roll_mode = POSHOLD_LOITER; poshold.pitch_mode = POSHOLD_LOITER; } // calculate percentage mix of loiter and brake control brake_to_loiter_mix = (float)poshold.brake_to_loiter_timer / (float)POSHOLD_BRAKE_TO_LOITER_TIMER; // calculate brake_roll and pitch angles to counter-act velocity poshold_update_brake_angle_from_velocity(poshold.brake_roll, vel_right); poshold_update_brake_angle_from_velocity(poshold.brake_pitch, -vel_fw); // run loiter controller wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler); // calculate final roll and pitch output by mixing loiter and brake controls poshold.roll = poshold_mix_controls(brake_to_loiter_mix, poshold.brake_roll + poshold.wind_comp_roll, wp_nav.get_roll()); poshold.pitch = poshold_mix_controls(brake_to_loiter_mix, poshold.brake_pitch + poshold.wind_comp_pitch, wp_nav.get_pitch()); // check for pilot input if (!is_zero(target_roll) || !is_zero(target_pitch)) { // if roll input switch to pilot override for roll if (!is_zero(target_roll)) { // init transition to pilot override poshold_roll_controller_to_pilot_override(); // switch pitch-mode to brake (but ready to go back to loiter anytime) // no need to reset poshold.brake_pitch here as wind comp has not been updated since last brake_pitch computation poshold.pitch_mode = POSHOLD_BRAKE_READY_TO_LOITER; } // if pitch input switch to pilot override for pitch if (!is_zero(target_pitch)) { // init transition to pilot override poshold_pitch_controller_to_pilot_override(); if (is_zero(target_roll)) { // switch roll-mode to brake (but ready to go back to loiter anytime) // no need to reset poshold.brake_roll here as wind comp has not been updated since last brake_roll computation poshold.roll_mode = POSHOLD_BRAKE_READY_TO_LOITER; } } } break; case POSHOLD_LOITER: // run loiter controller wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler); // set roll angle based on loiter controller outputs poshold.roll = wp_nav.get_roll(); poshold.pitch = wp_nav.get_pitch(); // update wind compensation estimate poshold_update_wind_comp_estimate(); // check for pilot input if (!is_zero(target_roll) || !is_zero(target_pitch)) { // if roll input switch to pilot override for roll if (!is_zero(target_roll)) { // init transition to pilot override poshold_roll_controller_to_pilot_override(); // switch pitch-mode to brake (but ready to go back to loiter anytime) poshold.pitch_mode = POSHOLD_BRAKE_READY_TO_LOITER; // reset brake_pitch because wind_comp is now different and should give the compensation of the whole previous loiter angle poshold.brake_pitch = 0; } // if pitch input switch to pilot override for pitch if (!is_zero(target_pitch)) { // init transition to pilot override poshold_pitch_controller_to_pilot_override(); // if roll not overriden switch roll-mode to brake (but be ready to go back to loiter any time) if (is_zero(target_roll)) { poshold.roll_mode = POSHOLD_BRAKE_READY_TO_LOITER; poshold.brake_roll = 0; } } } break; default: // do nothing for uncombined roll and pitch modes break; } } // constrain target pitch/roll angles poshold.roll = constrain_int16(poshold.roll, -aparm.angle_max, aparm.angle_max); poshold.pitch = constrain_int16(poshold.pitch, -aparm.angle_max, aparm.angle_max); // update attitude controller targets attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(poshold.roll, poshold.pitch, target_yaw_rate, get_smoothing_gain()); // adjust climb rate using rangefinder if (rangefinder_alt_ok()) { // if rangefinder is ok, use surface tracking target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control.get_alt_target(), G_Dt); } // 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(); } }
// 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)); } // 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 = copter.get_surface_tracking_climb_rate(target_climb_rate); // 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; } }
// guided_posvel_control_run - runs the guided spline controller // called from guided_run void Copter::ModeGuided::posvel_control_run() { // process pilot's yaw input float target_yaw_rate = 0; 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)) { auto_yaw.set_mode(AUTO_YAW_HOLD); } } // 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); // set velocity to zero and stop rotating if no updates received for 3 seconds uint32_t tnow = millis(); if (tnow - posvel_update_time_ms > GUIDED_POSVEL_TIMEOUT_MS) { guided_vel_target_cms.zero(); if (auto_yaw.mode() == AUTO_YAW_RATE) { auto_yaw.set_rate(0.0f); } } // calculate dt float dt = pos_control->time_since_last_xy_update(); // sanity check dt if (dt >= 0.2f) { dt = 0.0f; } // advance position target using velocity target guided_pos_target_cm += guided_vel_target_cms * dt; // send position and velocity targets to position controller pos_control->set_pos_target(guided_pos_target_cm); pos_control->set_desired_velocity_xy(guided_vel_target_cms.x, guided_vel_target_cms.y); // run position controllers pos_control->update_xy_controller(); 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(pos_control->get_roll(), pos_control->get_pitch(), target_yaw_rate); } else if (auto_yaw.mode() == AUTO_YAW_RATE) { // roll & pitch from position-velocity controller, yaw rate from mavlink command or mission item attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), auto_yaw.rate_cds()); } else { // roll, pitch from waypoint controller, yaw heading from GCS or auto_heading() attitude_control->input_euler_angle_roll_pitch_yaw(pos_control->get_roll(), pos_control->get_pitch(), auto_yaw.yaw(), true); } }
// heli_acro_run - runs the acro controller // should be called at 100hz or more void Copter::ModeAcro_Heli::run() { float target_roll, target_pitch, target_yaw; float 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()) { copter.heli_flags.init_targets_on_arming=true; attitude_control->set_attitude_target_to_current_attitude(); attitude_control->reset_rate_controller_I_terms(); } if(motors->armed() && copter.heli_flags.init_targets_on_arming) { attitude_control->set_attitude_target_to_current_attitude(); attitude_control->reset_rate_controller_I_terms(); if (motors->get_interlock()) { copter.heli_flags.init_targets_on_arming=false; } } // clear landing flag above zero throttle if (motors->armed() && motors->get_interlock() && motors->rotor_runup_complete() && !ap.throttle_zero) { set_land_complete(false); } if (!motors->has_flybar()){ // convert the input to the desired body frame rate get_pilot_desired_angle_rates(channel_roll->get_control_in(), channel_pitch->get_control_in(), channel_yaw->get_control_in(), target_roll, target_pitch, target_yaw); if (motors->supports_yaw_passthrough()) { // if the tail on a flybar heli has an external gyro then // also use no deadzone for the yaw control and // pass-through the input direct to output. target_yaw = channel_yaw->get_control_in_zero_dz(); } // run attitude controller attitude_control->input_rate_bf_roll_pitch_yaw(target_roll, target_pitch, target_yaw); }else{ /* for fly-bar passthrough use control_in values with no deadzone. This gives true pass-through. */ float roll_in = channel_roll->get_control_in_zero_dz(); float pitch_in = channel_pitch->get_control_in_zero_dz(); float yaw_in; if (motors->supports_yaw_passthrough()) { // if the tail on a flybar heli has an external gyro then // also use no deadzone for the yaw control and // pass-through the input direct to output. yaw_in = channel_yaw->get_control_in_zero_dz(); } else { // if there is no external gyro then run the usual // ACRO_YAW_P gain on the input control, including // deadzone yaw_in = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); } // run attitude controller attitude_control->passthrough_bf_roll_pitch_rate_yaw(roll_in, pitch_in, yaw_in); } // get pilot's desired throttle pilot_throttle_scaled = copter.input_manager.get_pilot_desired_collective(channel_throttle->get_control_in()); // output pilot's throttle without angle boost attitude_control->set_throttle_out(pilot_throttle_scaled, false, g.throttle_filt); }
void Copter::land_run_horizontal_control() { int16_t roll_control = 0, pitch_control = 0; float target_yaw_rate = 0; // 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, 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(); // 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->get_control_in()); } #if PRECISION_LANDING == ENABLED bool doing_precision_landing = !ap.land_repo_active && precland.target_acquired(); // run precision landing if (doing_precision_landing) { Vector2f target_pos, target_vel_rel; if (!precland.get_target_position_cm(target_pos)) { target_pos.x = inertial_nav.get_position().x; target_pos.y = inertial_nav.get_position().y; } if (!precland.get_target_velocity_relative_cms(target_vel_rel)) { target_vel_rel.x = -inertial_nav.get_velocity().x; target_vel_rel.y = -inertial_nav.get_velocity().y; } pos_control->set_xy_target(target_pos.x, target_pos.y); pos_control->override_vehicle_velocity_xy(-target_vel_rel); } #endif // process roll, pitch inputs wp_nav->set_pilot_desired_acceleration(roll_control, pitch_control); // run loiter controller wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler); int32_t nav_roll = wp_nav->get_roll(); int32_t nav_pitch = wp_nav->get_pitch(); if (g2.wp_navalt_min > 0) { // user has requested an altitude below which navigation // attitude is limited. This is used to prevent commanded roll // over on landing, which particularly affects helicopters if // there is any position estimate drift after touchdown. We // limit attitude to 7 degrees below this limit and linearly // interpolate for 1m above that int alt_above_ground = mode_land.get_alt_above_ground(); float attitude_limit_cd = linear_interpolate(700, aparm.angle_max, alt_above_ground, g2.wp_navalt_min*100U, (g2.wp_navalt_min+1)*100U); float total_angle_cd = norm(nav_roll, nav_pitch); if (total_angle_cd > attitude_limit_cd) { float ratio = attitude_limit_cd / total_angle_cd; nav_roll *= ratio; nav_pitch *= ratio; // tell position controller we are applying an external limit pos_control->set_limit_accel_xy(); } } // call attitude controller attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(nav_roll, nav_pitch, target_yaw_rate, get_smoothing_gain()); }
// 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 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); //hal.console->printf("1***g.pilot_v_z_max:%d\t g.pilot_a_z:%d\n", (int)g.pilot_velocity_z_max, (int)g.pilot_accel_z); // apply SIMPLE mode transform to pilot inputs update_simple_mode(); /*hal.console->printf("2***roll:%d**pitch:%d**throttle:%d**yaw:%d\n", \ channel_roll->get_control_in(),\ channel_pitch->get_control_in(),\ channel_throttle->get_control_in(),\ channel_yaw->get_control_in());*/ // 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; } /*hal.console->printf("3***motors.armed:%s**motor.interlock:%s**ap.auto_arm:%s**takeoff_running:%s**take_trigger:%s**land:%s \n",\ motors.armed()? "true":"false",\ motors.get_interlock()?"true":"false",\ ap.auto_armed?"true":"false",\ takeoff_state.running ?"true":"false",\ takeoff_triggered?"true":"false",\ ap.land_complete?"true":"false");*/ // 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; } }
// 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, 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(); // record if pilot has overriden roll or pitch if (roll_control != 0 || pitch_control != 0) { land_state.repo_active = true; } } // 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 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_state.pause && (millis()-land_state.start_ms < LAND_WITH_DELAY_MS)) { cmb_rate = 0; } else { land_state.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(); }
// stabilize_run - runs the main stabilize controller // should be called at 100hz or more void Copter::ModeStabilize_Heli::run() { float target_roll, target_pitch; float target_yaw_rate; float pilot_throttle_scaled; // 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, copter.aparm.angle_max, copter.aparm.angle_max); // get pilot's desired yaw rate target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); // get pilot's desired throttle pilot_throttle_scaled = copter.input_manager.get_pilot_desired_collective(channel_throttle->get_control_in()); // Tradheli should not reset roll, pitch, yaw targets when motors are not runup while flying, because // we may be in autorotation flight. 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()) { // Motors should be Stopped motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN); } else { // heli will not let the spool state progress to THROTTLE_UNLIMITED until motor interlock is enabled motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); } switch (motors->get_spool_state()) { case AP_Motors::SpoolState::SHUT_DOWN: // Motors Stopped attitude_control->set_yaw_target_to_current_heading(); attitude_control->reset_rate_controller_I_terms(); break; case AP_Motors::SpoolState::GROUND_IDLE: // Landed if (motors->init_targets_on_arming()) { attitude_control->set_yaw_target_to_current_heading(); attitude_control->reset_rate_controller_I_terms(); } break; case AP_Motors::SpoolState::THROTTLE_UNLIMITED: // clear landing flag above zero throttle if (!motors->limit.throttle_lower) { set_land_complete(false); } case AP_Motors::SpoolState::SPOOLING_UP: case AP_Motors::SpoolState::SPOOLING_DOWN: // do nothing break; } // call attitude controller attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); // 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 Copter::land_run_horizontal_control() { int16_t roll_control = 0, pitch_control = 0; float target_yaw_rate = 0; // 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, 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(); // 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->get_control_in()); } #if PRECISION_LANDING == ENABLED bool doing_precision_landing = !ap.land_repo_active && precland.target_acquired(); // run precision landing if (doing_precision_landing && precland_last_update_ms != precland.last_update_ms()) { Vector3f target_pos; precland.get_target_position(target_pos); pos_control.set_xy_target(target_pos.x, target_pos.y); pos_control.freeze_ff_xy(); precland_last_update_ms = precland.last_update_ms(); } #else bool doing_precision_landing = false; #endif // 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.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain()); }
// 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); #if AC_AVOID_ENABLED == ENABLED // apply avoidance avoid.adjust_roll_pitch(target_roll, target_pitch, aparm.angle_max); #endif // 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; } }
// 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, MODE_REASON_THROTTLE_LAND_ESCAPE); } 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->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()); } // if not auto armed or landed or motor interlock not enabled set throttle to zero and exit immediately if (!motors.armed() || !ap.auto_armed || ap.land_complete || !motors.get_interlock()) { #if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw // call attitude controller attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, 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(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); // pause before beginning land descent if(land_pause && millis()-land_start_time >= LAND_WITH_DELAY_MS) { land_pause = false; } land_run_vertical_control(land_pause); }
// sport_run - runs the sport controller // should be called at 100hz or more void Copter::sport_run() { SportModeState sport_state; float takeoff_climb_rate = 0.0f; // initialize vertical speed and acceleration pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max); pos_control.set_accel_z(g.pilot_accel_z); // apply SIMPLE mode transform update_simple_mode(); // get pilot's desired roll and pitch rates // calculate rate requests float target_roll_rate = channel_roll->get_control_in() * g.acro_rp_p; float target_pitch_rate = channel_pitch->get_control_in() * g.acro_rp_p; int32_t roll_angle = wrap_180_cd(ahrs.roll_sensor); target_roll_rate -= constrain_int32(roll_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE) * g.acro_balance_roll; // Calculate trainer mode earth frame rate command for pitch int32_t pitch_angle = wrap_180_cd(ahrs.pitch_sensor); target_pitch_rate -= constrain_int32(pitch_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE) * g.acro_balance_pitch; if (roll_angle > aparm.angle_max){ target_roll_rate -= g.acro_rp_p*(roll_angle-aparm.angle_max); }else if (roll_angle < -aparm.angle_max) { target_roll_rate -= g.acro_rp_p*(roll_angle+aparm.angle_max); } if (pitch_angle > aparm.angle_max){ target_pitch_rate -= g.acro_rp_p*(pitch_angle-aparm.angle_max); }else if (pitch_angle < -aparm.angle_max) { target_pitch_rate -= g.acro_rp_p*(pitch_angle+aparm.angle_max); } // get pilot's desired yaw rate float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); // get pilot desired climb rate float target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in()); target_climb_rate = constrain_float(target_climb_rate, -g.pilot_velocity_z_max, g.pilot_velocity_z_max); #if FRAME_CONFIG == HELI_FRAME // helicopters are held on the ground until rotor speed runup has finished bool takeoff_triggered = (ap.land_complete && (target_climb_rate > 0.0f) && motors.rotor_runup_complete()); #else bool takeoff_triggered = ap.land_complete && (target_climb_rate > 0.0f); #endif // State Machine Determination if (!motors.armed() || !motors.get_interlock()) { sport_state = Sport_MotorStopped; } else if (takeoff_state.running || takeoff_triggered) { sport_state = Sport_Takeoff; } else if (!ap.auto_armed || ap.land_complete) { sport_state = Sport_Landed; } else { sport_state = Sport_Flying; } // State Machine switch (sport_state) { case Sport_MotorStopped: motors.set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN); attitude_control.input_euler_rate_roll_pitch_yaw(target_roll_rate, target_pitch_rate, target_yaw_rate); #if FRAME_CONFIG == HELI_FRAME // force descent rate and call position controller pos_control.set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false); #else attitude_control.relax_attitude_controllers(); attitude_control.reset_rate_controller_I_terms(); attitude_control.set_yaw_target_to_current_heading(); pos_control.relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero #endif pos_control.update_z_controller(); break; case Sport_Takeoff: // set motors to full range motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // initiate take-off if (!takeoff_state.running) { takeoff_timer_start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f)); // indicate we are taking off set_land_complete(false); // clear i terms set_throttle_takeoff(); } // get take-off adjusted pilot and takeoff climb rates takeoff_get_climb_rates(target_climb_rate, takeoff_climb_rate); // call attitude controller attitude_control.input_euler_rate_roll_pitch_yaw(target_roll_rate, target_pitch_rate, target_yaw_rate); // call position controller pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false); pos_control.add_takeoff_climb_rate(takeoff_climb_rate, G_Dt); pos_control.update_z_controller(); break; case Sport_Landed: // set motors to spin-when-armed if throttle below deadzone, otherwise full range (but motors will only spin at min throttle) if (target_climb_rate < 0.0f) { motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); } else { motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); } attitude_control.reset_rate_controller_I_terms(); attitude_control.set_yaw_target_to_current_heading(); attitude_control.input_euler_rate_roll_pitch_yaw(target_roll_rate, target_pitch_rate, target_yaw_rate); pos_control.relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero pos_control.update_z_controller(); break; case Sport_Flying: motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // call attitude controller attitude_control.input_euler_rate_roll_pitch_yaw(target_roll_rate, target_pitch_rate, target_yaw_rate); // adjust climb rate using rangefinder if (rangefinder_alt_ok()) { // if rangefinder is ok, use surface tracking target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control.get_alt_target(), G_Dt); } // call position controller pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false); pos_control.update_z_controller(); break; } }