// drift_run - runs the drift controller // should be called at 100hz or more void Copter::drift_run() { static float breaker = 0.0f; static float roll_input = 0.0f; float target_roll, target_pitch; float target_yaw_rate; int16_t pilot_throttle_scaled; // if not armed or motor interlock not enabled or landed and throttle at zero, set throttle to zero and exit immediately if(!motors.armed() || !motors.get_interlock() || (ap.land_complete && ap.throttle_zero)) { attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); return; } // convert pilot input to lean angles get_pilot_desired_lean_angles(channel_roll->control_in, channel_pitch->control_in, target_roll, target_pitch, aparm.angle_max); // get pilot's desired throttle pilot_throttle_scaled = get_pilot_desired_throttle(channel_throttle->control_in); // Grab inertial velocity const Vector3f& vel = inertial_nav.get_velocity(); // rotate roll, pitch input from north facing to vehicle's perspective float roll_vel = vel.y * ahrs.cos_yaw() - vel.x * ahrs.sin_yaw(); // body roll vel float pitch_vel = vel.y * ahrs.sin_yaw() + vel.x * ahrs.cos_yaw(); // body pitch vel // gain sceduling for Yaw float pitch_vel2 = min(fabsf(pitch_vel), 2000); target_yaw_rate = ((float)target_roll/1.0f) * (1.0f - (pitch_vel2 / 5000.0f)) * g.acro_yaw_p; roll_vel = constrain_float(roll_vel, -DRIFT_SPEEDLIMIT, DRIFT_SPEEDLIMIT); pitch_vel = constrain_float(pitch_vel, -DRIFT_SPEEDLIMIT, DRIFT_SPEEDLIMIT); roll_input = roll_input * .96f + (float)channel_yaw->control_in * .04f; //convert user input into desired roll velocity float roll_vel_error = roll_vel - (roll_input / DRIFT_SPEEDGAIN); // Roll velocity is feed into roll acceleration to minimize slip target_roll = roll_vel_error * -DRIFT_SPEEDGAIN; target_roll = constrain_int16(target_roll, -4500, 4500); // If we let go of sticks, bring us to a stop if(is_zero(target_pitch)) { // .14/ (.03 * 100) = 4.6 seconds till full breaking breaker += .03f; breaker = min(breaker, DRIFT_SPEEDGAIN); target_pitch = pitch_vel * breaker; } else { breaker = 0.0f; } // call attitude controller attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); // output pilot's throttle with angle boost attitude_control.set_throttle_out(get_throttle_assist(vel.z, pilot_throttle_scaled), true, g.throttle_filt); }
// drift_run - runs the drift controller // should be called at 100hz or more void Copter::ModeDrift::run() { static float braker = 0.0f; static float roll_input = 0.0f; float target_roll, target_pitch; float target_yaw_rate; float pilot_throttle_scaled; // if landed and throttle at zero, set throttle to zero and exit immediately if (!motors->armed() || !motors->get_interlock() || (ap.land_complete && ap.throttle_zero)) { zero_throttle_and_relax_ac(); return; } // clear landing flag above zero throttle if (!ap.throttle_zero) { set_land_complete(false); } // 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 throttle pilot_throttle_scaled = get_pilot_desired_throttle(channel_throttle->get_control_in()); // Grab inertial velocity const Vector3f& vel = inertial_nav.get_velocity(); // rotate roll, pitch input from north facing to vehicle's perspective float roll_vel = vel.y * ahrs.cos_yaw() - vel.x * ahrs.sin_yaw(); // body roll vel float pitch_vel = vel.y * ahrs.sin_yaw() + vel.x * ahrs.cos_yaw(); // body pitch vel // gain sceduling for Yaw float pitch_vel2 = MIN(fabsf(pitch_vel), 2000); target_yaw_rate = ((float)target_roll/1.0f) * (1.0f - (pitch_vel2 / 5000.0f)) * g.acro_yaw_p; roll_vel = constrain_float(roll_vel, -DRIFT_SPEEDLIMIT, DRIFT_SPEEDLIMIT); pitch_vel = constrain_float(pitch_vel, -DRIFT_SPEEDLIMIT, DRIFT_SPEEDLIMIT); roll_input = roll_input * .96f + (float)channel_yaw->get_control_in() * .04f; //convert user input into desired roll velocity float roll_vel_error = roll_vel - (roll_input / DRIFT_SPEEDGAIN); // Roll velocity is feed into roll acceleration to minimize slip target_roll = roll_vel_error * -DRIFT_SPEEDGAIN; target_roll = constrain_float(target_roll, -4500.0f, 4500.0f); // If we let go of sticks, bring us to a stop if(is_zero(target_pitch)){ // .14/ (.03 * 100) = 4.6 seconds till full braking braker += .03f; braker = MIN(braker, DRIFT_SPEEDGAIN); target_pitch = pitch_vel * braker; }else{ braker = 0.0f; } // 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); // output pilot's throttle with angle boost attitude_control->set_throttle_out(get_throttle_assist(vel.z, pilot_throttle_scaled), true, g.throttle_filt); }