Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

ArduCopter: bug fix in earthframe->body frame translation. 

Provided by Leonard Hall.

Also removed large unnecessary comments.
This commit is contained in:
rmackay9 2012-10-17 19:15:25 +09:00
parent 7d7de976c3
commit b944d9df26
1 changed files with 2 additions and 102 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

104
ArduCopter/Attitude.pde
View File

@ -178,80 +178,6 @@ get_yaw_rate_stabilized_ef(int32_t stick_angle)
set_yaw_rate_target(g.pi_stabilize_yaw.get_p(angle_error)+target_rate, EARTH_FRAME); set_yaw_rate_target(g.pi_stabilize_yaw.get_p(angle_error)+target_rate, EARTH_FRAME);
} }
/*
* static int16_t
* get_acro_yaw2(int32_t target_rate)
* {
* int32_t p,i,d; // used to capture pid values for logging
* int32_t rate_error; // current yaw rate error
* int32_t current_rate; // current real yaw rate
* int32_t decel_boost; // gain scheduling if we are overshooting
* int32_t output; // output to rate controller
*
* target_rate = g.pi_stabilize_yaw.get_p(target_rate);
* current_rate = omega.z * DEGX100;
* rate_error = target_rate - current_rate;
*
* //Gain Scheduling:
* //If the yaw input is to the right, but stick is moving to the middle
* //and actual rate is greater than the target rate then we are
* //going to overshoot the yaw target to the left side, so we should
* //strengthen the yaw output to slow down the yaw!
*
* #if (FRAME_CONFIG == HELI_FRAME || FRAME_CONFIG == TRI_FRAME)
* static int32_t last_target_rate = 0; // last iteration's target rate
* if ( target_rate > 0 && last_target_rate > target_rate && rate_error < 0 ){
* decel_boost = 1;
* } else if (target_rate < 0 && last_target_rate < target_rate && rate_error > 0 ){
* decel_boost = 1;
* } else if (target_rate == 0 && labs(current_rate) > 1000){
* decel_boost = 1;
* } else {
* decel_boost = 0;
* }
*
* last_target_rate = target_rate;
*
* #else
*
* decel_boost = 0;
*
* #endif
*
* // separately calculate p, i, d values for logging
* // we will use d=0, and hold i at it's last value
* // since manual inputs are never steady state
*
* p = g.pid_rate_yaw.get_p(rate_error);
* i = g.pid_rate_yaw.get_integrator();
* d = 0;
*
* if (decel_boost){
* p *= 2;
* }
*
* output = p+i+d;
*
* // output control:
* // constrain output
* output = constrain(output, -4500, 4500);
*
* #if LOGGING_ENABLED == ENABLED
* static int8_t log_counter = 0; // used to slow down logging of PID values to dataflash
* // log output if PID loggins is on and we are tuning the yaw
* if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_YAW_KP || g.radio_tuning == CH6_YAW_RATE_KP) ) {
* log_counter++;
* if( log_counter >= 10 ) { // (update rate / desired output rate) = (100hz / 10hz) = 10
* log_counter = 0;
* Log_Write_PID(CH6_YAW_RATE_KP, rate_error, p, i, d, output, tuning_value);
* }
* }
* #endif
*
* return output;
* }
*/
// set_roll_rate_target - to be called by upper controllers to set roll rate targets in the earth frame // set_roll_rate_target - to be called by upper controllers to set roll rate targets in the earth frame
void set_roll_rate_target( int32_t desired_rate, uint8_t earth_or_body_frame ) { void set_roll_rate_target( int32_t desired_rate, uint8_t earth_or_body_frame ) {
rate_targets_frame = earth_or_body_frame; rate_targets_frame = earth_or_body_frame;
@ -289,8 +215,8 @@ update_rate_contoller_targets()
if( rate_targets_frame == EARTH_FRAME ) { if( rate_targets_frame == EARTH_FRAME ) {
// convert earth frame rates to body frame rates // convert earth frame rates to body frame rates
roll_rate_target_bf = roll_rate_target_ef - sin_pitch * yaw_rate_target_ef; roll_rate_target_bf = roll_rate_target_ef - sin_pitch * yaw_rate_target_ef;
pitch_rate_target_bf = cos_roll_x * pitch_rate_target_ef + sin_roll * yaw_rate_target_ef; pitch_rate_target_bf = cos_roll_x * pitch_rate_target_ef + sin_roll * cos_pitch_x * yaw_rate_target_ef;
yaw_rate_target_bf = cos_pitch_x * cos_roll_x * yaw_rate_target_ef + sin_roll * pitch_rate_target_ef; yaw_rate_target_bf = cos_pitch_x * cos_roll_x * yaw_rate_target_ef - sin_roll * pitch_rate_target_ef;
} }
} }
@ -615,32 +541,6 @@ static void reset_stability_I(void)
* throttle control * throttle control
****************************************************************/ ****************************************************************/
/* Depricated
*
* static long
* //get_nav_yaw_offset(int yaw_input, int reset)
* {
* int32_t _yaw;
*
* if(reset == 0){
* // we are on the ground
* return ahrs.yaw_sensor;
*
* }else{
* // re-define nav_yaw if we have stick input
* if(yaw_input != 0){
* // set nav_yaw + or - the current location
* _yaw = yaw_input + ahrs.yaw_sensor;
* // we need to wrap our value so we can be 0 to 360 (*100)
* return wrap_360(_yaw);
* }else{
* // no stick input, lets not change nav_yaw
* return nav_yaw;
* }
* }
* }
*/
static int16_t get_angle_boost(int16_t value) static int16_t get_angle_boost(int16_t value)
{ {
float temp = cos_pitch_x * cos_roll_x; float temp = cos_pitch_x * cos_roll_x;