mirror of https://github.com/ArduPilot/ardupilot
Copter: remove unused Attitude.pde functions
This commit is contained in:
Randy Mackay
Andrew Tridgell
parent
0a2adbac1b
commit
b29a963fd6
|
|
@ -472,105 +472,6 @@ void set_yaw_rate_target( int32_t desired_rate, uint8_t earth_or_body_frame ) {
|
|||
}
|
||||
}
|
||||
|
||||
#if FRAME_CONFIG != HELI_FRAME
|
||||
static int16_t
|
||||
get_rate_roll(int32_t target_rate)
|
||||
{
|
||||
int32_t p,i,d; // used to capture pid values for logging
|
||||
int32_t current_rate; // this iteration's rate
|
||||
int32_t rate_error; // simply target_rate - current_rate
|
||||
int32_t output; // output from pid controller
|
||||
|
||||
// get current rate
|
||||
current_rate = (omega.x * DEGX100);
|
||||
|
||||
// call pid controller
|
||||
rate_error = target_rate - current_rate;
|
||||
p = g.pid_rate_roll.get_p(rate_error);
|
||||
|
||||
// get i term
|
||||
i = g.pid_rate_roll.get_integrator();
|
||||
|
||||
// update i term as long as we haven't breached the limits or the I term will certainly reduce
|
||||
if (!motors.limit.roll_pitch || ((i>0&&rate_error<0)||(i<0&&rate_error>0))) {
|
||||
i = g.pid_rate_roll.get_i(rate_error, G_Dt);
|
||||
}
|
||||
|
||||
d = g.pid_rate_roll.get_d(rate_error, G_Dt);
|
||||
output = p + i + d;
|
||||
|
||||
// constrain output
|
||||
output = constrain_int32(output, -5000, 5000);
|
||||
|
||||
// output control
|
||||
return output;
|
||||
}
|
||||
|
||||
static int16_t
|
||||
get_rate_pitch(int32_t target_rate)
|
||||
{
|
||||
int32_t p,i,d; // used to capture pid values for logging
|
||||
int32_t current_rate; // this iteration's rate
|
||||
int32_t rate_error; // simply target_rate - current_rate
|
||||
int32_t output; // output from pid controller
|
||||
|
||||
// get current rate
|
||||
current_rate = (omega.y * DEGX100);
|
||||
|
||||
// call pid controller
|
||||
rate_error = target_rate - current_rate;
|
||||
p = g.pid_rate_pitch.get_p(rate_error);
|
||||
|
||||
// get i term
|
||||
i = g.pid_rate_pitch.get_integrator();
|
||||
|
||||
// update i term as long as we haven't breached the limits or the I term will certainly reduce
|
||||
if (!motors.limit.roll_pitch || ((i>0&&rate_error<0)||(i<0&&rate_error>0))) {
|
||||
i = g.pid_rate_pitch.get_i(rate_error, G_Dt);
|
||||
}
|
||||
|
||||
d = g.pid_rate_pitch.get_d(rate_error, G_Dt);
|
||||
output = p + i + d;
|
||||
|
||||
// constrain output
|
||||
output = constrain_int32(output, -5000, 5000);
|
||||
|
||||
// output control
|
||||
return output;
|
||||
}
|
||||
|
||||
static int16_t
|
||||
get_rate_yaw(int32_t target_rate)
|
||||
{
|
||||
int32_t p,i,d; // used to capture pid values for logging
|
||||
int32_t rate_error;
|
||||
int32_t output;
|
||||
|
||||
// rate control
|
||||
rate_error = target_rate - (omega.z * DEGX100);
|
||||
|
||||
// separately calculate p, i, d values for logging
|
||||
p = g.pid_rate_yaw.get_p(rate_error);
|
||||
|
||||
// get i term
|
||||
i = g.pid_rate_yaw.get_integrator();
|
||||
|
||||
// update i term as long as we haven't breached the limits or the I term will certainly reduce
|
||||
if (!motors.limit.yaw || ((i>0&&rate_error<0)||(i<0&&rate_error>0))) {
|
||||
i = g.pid_rate_yaw.get_i(rate_error, G_Dt);
|
||||
}
|
||||
|
||||
// get d value
|
||||
d = g.pid_rate_yaw.get_d(rate_error, G_Dt);
|
||||
|
||||
output = p+i+d;
|
||||
output = constrain_int32(output, -4500, 4500);
|
||||
|
||||
// constrain output
|
||||
return output;
|
||||
}
|
||||
#endif // !HELI_FRAME
|
||||
|
||||
// calculate modified roll/pitch depending upon optical flow calculated position
|
||||
static int32_t
|
||||
get_of_roll(int32_t input_roll)
|
||||
|
|
@ -815,49 +716,6 @@ set_throttle_takeoff()
|
|||
motors.slow_start(true);
|
||||
}
|
||||
|
||||
// get_throttle_accel - accelerometer based throttle controller
|
||||
// returns an actual throttle output (0 ~ 1000) to be sent to the motors
|
||||
static int16_t
|
||||
get_throttle_accel(int16_t z_target_accel)
|
||||
{
|
||||
static float z_accel_error = 0; // The acceleration error in cm.
|
||||
static uint32_t last_call_ms = 0; // the last time this controller was called
|
||||
int32_t p,i,d; // used to capture pid values for logging
|
||||
int16_t output;
|
||||
float z_accel_meas;
|
||||
uint32_t now = millis();
|
||||
|
||||
// Calculate Earth Frame Z acceleration
|
||||
z_accel_meas = -(ahrs.get_accel_ef().z + GRAVITY_MSS) * 100;
|
||||
|
||||
// reset target altitude if this controller has just been engaged
|
||||
if( now - last_call_ms > 100 ) {
|
||||
// Reset Filter
|
||||
z_accel_error = 0;
|
||||
} else {
|
||||
// calculate accel error and Filter with fc = 2 Hz
|
||||
z_accel_error = z_accel_error + 0.11164f * (constrain_float(z_target_accel - z_accel_meas, -32000, 32000) - z_accel_error);
|
||||
}
|
||||
last_call_ms = now;
|
||||
|
||||
// separately calculate p, i, d values for logging
|
||||
p = g.pid_throttle_accel.get_p(z_accel_error);
|
||||
|
||||
// get i term
|
||||
i = g.pid_throttle_accel.get_integrator();
|
||||
|
||||
// update i term as long as we haven't breached the limits or the I term will certainly reduce
|
||||
if ((!motors.limit.throttle_lower && !motors.limit.throttle_upper) || (i>0&&z_accel_error<0) || (i<0&&z_accel_error>0)) {
|
||||
i = g.pid_throttle_accel.get_i(z_accel_error, .01f);
|
||||
}
|
||||
|
||||
d = g.pid_throttle_accel.get_d(z_accel_error, .01f);
|
||||
|
||||
output = constrain_float(p+i+d+g.throttle_cruise, g.throttle_min, g.throttle_max);
|
||||
|
||||
return output;
|
||||
}
|
||||
|
||||
// get_pilot_desired_throttle - transform pilot's throttle input to make cruise throttle mid stick
|
||||
// used only for manual throttle modes
|
||||
// returns throttle output 0 to 1000
|
||||
|
|
|
|||
Loading…
Reference in New Issue