From b29a963fd6657d50c3633caabe74640ded0145d6 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 22 Jan 2014 11:42:29 +0900 Subject: [PATCH] Copter: remove unused Attitude.pde functions --- ArduCopter/Attitude.pde | 142 ---------------------------------------- 1 file changed, 142 deletions(-) diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index 8b481c0115..e7c12b71bb 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -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