From e375a270581e19d083718d893dfb9c8f31f3f842 Mon Sep 17 00:00:00 2001 From: rmackay9 Date: Mon, 1 Oct 2012 14:02:49 +0900 Subject: [PATCH] ArduCopter: prioritise rate controllers, rate controller targets converted to body frame --- ArduCopter/ArduCopter.pde | 68 ++++++++++++++++++++++----------- ArduCopter/Attitude.pde | 79 ++++++++++++++++++++++++++++++--------- ArduCopter/flip.pde | 6 ++- ArduCopter/toy.pde | 18 +++++---- 4 files changed, 124 insertions(+), 47 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 097328e3c8..1eaf0cf16c 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -567,6 +567,8 @@ static float cos_roll_x = 1; static float cos_pitch_x = 1; static float cos_yaw_x = 1; static float sin_yaw_y; +static float sin_roll; +static float sin_pitch; //////////////////////////////////////////////////////////////////////////////// // SIMPLE Mode @@ -575,6 +577,19 @@ static float sin_yaw_y; // or in SuperSimple mode when the copter leaves a 20m radius from home. static int32_t initial_simple_bearing; +//////////////////////////////////////////////////////////////////////////////// +// Rate contoller targets +//////////////////////////////////////////////////////////////////////////////// +static int32_t roll_rate_target_ef = 0; +static int32_t roll_rate_trim_ef = 0; // normally i term from stabilize controller +static int32_t pitch_rate_target_ef = 0; +static int32_t pitch_rate_trim_ef = 0; // normally i term from stabilize controller +static int32_t yaw_rate_target_ef = 0; +static int32_t yaw_rate_trim_ef = 0; // normally i term from stabilize controller +static int32_t roll_rate_target_bf = 0; // body frame roll rate target +static int32_t pitch_rate_target_bf = 0; // body frame pitch rate target +static int32_t yaw_rate_target_bf = 0; // body frame yaw rate target + //////////////////////////////////////////////////////////////////////////////// // ACRO Mode //////////////////////////////////////////////////////////////////////////////// @@ -1073,6 +1088,13 @@ static void fast_loop() // some space available gcs_send_message(MSG_RETRY_DEFERRED); + // run low level rate controllers that only require IMU data + run_rate_controllers(); + + // write out the servo PWM values + // ------------------------------ + set_servos_4(); + // Read radio // ---------- read_radio(); @@ -1100,9 +1122,8 @@ static void fast_loop() update_yaw_mode(); update_roll_pitch_mode(); - // write out the servo PWM values - // ------------------------------ - set_servos_4(); + // update targets to rate controllers + update_rate_contoller_targets(); // agmatthews - USERHOOKS #ifdef USERHOOK_FASTLOOP @@ -1564,7 +1585,7 @@ void update_yaw_mode(void) { switch(yaw_mode) { case YAW_ACRO: - g.rc_4.servo_out = get_acro_yaw(g.rc_4.control_in); + get_acro_yaw(g.rc_4.control_in); return; break; @@ -1575,12 +1596,12 @@ void update_yaw_mode(void) case YAW_HOLD: if(g.rc_4.control_in != 0) { - g.rc_4.servo_out = get_acro_yaw(g.rc_4.control_in); + get_acro_yaw(g.rc_4.control_in); yaw_stopped = false; yaw_timer = 150; }else if (!yaw_stopped) { - g.rc_4.servo_out = get_acro_yaw(0); + get_acro_yaw(0); yaw_timer--; if((yaw_timer == 0) || (fabs(omega.z) < .17)) { @@ -1593,21 +1614,21 @@ void update_yaw_mode(void) if(motors.armed() == false || ((g.rc_3.control_in == 0) && (control_mode <= ACRO) && !failsafe)) nav_yaw = ahrs.yaw_sensor; - g.rc_4.servo_out = get_stabilize_yaw(nav_yaw); + get_stabilize_yaw(nav_yaw); } return; break; case YAW_LOOK_AT_HOME: //nav_yaw updated in update_navigation() - g.rc_4.servo_out = get_stabilize_yaw(nav_yaw); + get_stabilize_yaw(nav_yaw); break; case YAW_AUTO: nav_yaw += constrain(wrap_180(auto_yaw - nav_yaw), -60, 60); // 40 deg a second //Serial.printf("nav_yaw %d ", nav_yaw); nav_yaw = wrap_360(nav_yaw); - g.rc_4.servo_out = get_stabilize_yaw(nav_yaw); + get_stabilize_yaw(nav_yaw); break; } } @@ -1634,8 +1655,8 @@ void update_roll_pitch_mode(void) pitch_axis = wrap_360(pitch_axis); // in this mode, nav_roll and nav_pitch = the iterm - g.rc_1.servo_out = get_stabilize_roll(roll_axis); - g.rc_2.servo_out = get_stabilize_pitch(pitch_axis); + get_stabilize_roll(roll_axis); + get_stabilize_pitch(pitch_axis); if (g.rc_3.control_in == 0) { roll_axis = 0; @@ -1649,12 +1670,12 @@ void update_roll_pitch_mode(void) g.rc_1.servo_out = g.rc_1.control_in; g.rc_2.servo_out = g.rc_2.control_in; } else { - g.rc_1.servo_out = get_acro_roll(g.rc_1.control_in); - g.rc_2.servo_out = get_acro_pitch(g.rc_2.control_in); + get_acro_roll(g.rc_1.control_in); + get_acro_pitch(g.rc_2.control_in); } #else - g.rc_1.servo_out = get_acro_roll(g.rc_1.control_in); - g.rc_2.servo_out = get_acro_pitch(g.rc_2.control_in); + get_acro_roll(g.rc_1.control_in); + get_acro_pitch(g.rc_2.control_in); #endif } break; @@ -1669,8 +1690,9 @@ void update_roll_pitch_mode(void) control_pitch = g.rc_2.control_in; // in this mode, nav_roll and nav_pitch = the iterm - g.rc_1.servo_out = get_stabilize_roll(control_roll); - g.rc_2.servo_out = get_stabilize_pitch(control_pitch); + get_stabilize_roll(control_roll); + get_stabilize_pitch(control_pitch); + break; case ROLL_PITCH_AUTO: @@ -1685,8 +1707,8 @@ void update_roll_pitch_mode(void) control_roll = g.rc_1.control_mix(nav_roll); control_pitch = g.rc_2.control_mix(nav_pitch); - g.rc_1.servo_out = get_stabilize_roll(control_roll); - g.rc_2.servo_out = get_stabilize_pitch(control_pitch); + get_stabilize_roll(control_roll); + get_stabilize_pitch(control_pitch); break; case ROLL_PITCH_STABLE_OF: @@ -1699,8 +1721,8 @@ void update_roll_pitch_mode(void) control_pitch = g.rc_2.control_in; // mix in user control with optical flow - g.rc_1.servo_out = get_stabilize_roll(get_of_roll(control_roll)); - g.rc_2.servo_out = get_stabilize_pitch(get_of_pitch(control_pitch)); + get_stabilize_roll(get_of_roll(control_roll)); + get_stabilize_pitch(get_of_pitch(control_pitch)); break; // THOR @@ -2127,6 +2149,10 @@ static void update_trig(void){ sin_yaw_y = yawvector.x; // 1y = north cos_yaw_x = yawvector.y; // 0x = north + // added to convert earth frame to body frame for rate controllers + sin_roll = sin(ahrs.roll); + sin_pitch = sin(ahrs.pitch); + //flat: // 0 ° = cos_yaw: 0.00, sin_yaw: 1.00, // 90° = cos_yaw: 1.00, sin_yaw: 0.00, diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index f8bec21911..19d7e221f1 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -1,6 +1,6 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -static int16_t +static void get_stabilize_roll(int32_t target_angle) { // angle error @@ -29,11 +29,13 @@ get_stabilize_roll(int32_t target_angle) i_stab = g.pi_stabilize_roll.get_integrator(); } - return get_rate_roll(target_rate) + i_stab; + // set targets for rate controller + roll_rate_target_ef = target_rate; + roll_rate_trim_ef = i_stab; #endif } -static int16_t +static void get_stabilize_pitch(int32_t target_angle) { // angle error @@ -60,17 +62,20 @@ get_stabilize_pitch(int32_t target_angle) }else{ i_stab = g.pi_stabilize_pitch.get_integrator(); } - return get_rate_pitch(target_rate) + i_stab; + + // set targets for rate controller + pitch_rate_target_ef = target_rate; + pitch_rate_trim_ef = i_stab; #endif } -static int16_t +static void get_stabilize_yaw(int32_t target_angle) { int32_t target_rate,i_term; int32_t angle_error; - int32_t output; + int32_t output = 0; // angle error angle_error = wrap_180(target_angle - ahrs.yaw_sensor); @@ -89,12 +94,12 @@ get_stabilize_yaw(int32_t target_angle) // do not use rate controllers for helicotpers with external gyros #if FRAME_CONFIG == HELI_FRAME if(!motors.ext_gyro_enabled) { - output = get_rate_yaw(target_rate) + i_term; + yaw_rate_target_ef = target_rate; + yaw_rate_trim_ef = i_term; }else{ + // TO-DO: fix this for helicopters which don't use rate controller output = constrain((target_rate + i_term), -4500, 4500); } -#else - output = get_rate_yaw(target_rate) + i_term; #endif #if LOGGING_ENABLED == ENABLED @@ -109,29 +114,39 @@ get_stabilize_yaw(int32_t target_angle) } #endif - // ensure output does not go beyond barries of what an int16_t can hold - return constrain(output,-32000,32000); + // set targets for rate controller + yaw_rate_target_ef = target_rate; + yaw_rate_trim_ef = i_term; } -static int16_t +static void get_acro_roll(int32_t target_rate) { target_rate = target_rate * g.acro_p; - return get_rate_roll(target_rate); + + // set targets for rate controller + roll_rate_target_ef = target_rate; + roll_rate_trim_ef = 0; } -static int16_t +static void get_acro_pitch(int32_t target_rate) { target_rate = target_rate * g.acro_p; - return get_rate_pitch(target_rate); + + // set targets for rate controller + pitch_rate_target_ef = target_rate; + pitch_rate_trim_ef = 0; } -static int16_t +static void get_acro_yaw(int32_t target_rate) { target_rate = g.pi_stabilize_yaw.get_p(target_rate); - return get_rate_yaw(target_rate); + + // set targets for rate controller + yaw_rate_target_ef = target_rate; + yaw_rate_trim_ef = 0; } /* @@ -208,6 +223,35 @@ get_acro_yaw(int32_t target_rate) * } */ + // update_rate_contoller_targets - converts earth frame rates to body frame rates for rate controllers + void + update_rate_contoller_targets() + { + static int16_t counter = 0; + // convert earth frame rates to body frame rates + 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; + yaw_rate_target_bf = cos_pitch_x * cos_roll_x * yaw_rate_target_ef + sin_roll * pitch_rate_target_ef; + + //counter++; + if( counter >= 100 ) { + counter = 0; + Serial.printf_P(PSTR("\nr:%ld\tp:%ld\ty:%ld\t"),ahrs.roll_sensor, ahrs.pitch_sensor, ahrs.yaw_sensor); + Serial.printf_P(PSTR("Rrate:%ld/%ld\tPrate:%ld/%ld\tYrate:%ld/%ld\n"),roll_rate_target_ef, roll_rate_target_bf, pitch_rate_target_ef, pitch_rate_target_bf, yaw_rate_target_ef, yaw_rate_target_bf); + } + } + +// run roll, pitch and yaw rate controllers and send output to motors +// targets for these controllers comes from stabilize controllers +void +run_rate_controllers() +{ + // call rate controllers + g.rc_1.servo_out = get_rate_roll(roll_rate_target_bf) + roll_rate_trim_ef; + g.rc_2.servo_out = get_rate_pitch(pitch_rate_target_bf) + pitch_rate_trim_ef; + g.rc_4.servo_out = get_rate_yaw(yaw_rate_target_bf) + yaw_rate_trim_ef; +} + static int16_t get_rate_roll(int32_t target_rate) { @@ -346,6 +390,7 @@ get_rate_yaw(int32_t target_rate) #else // output control: int16_t yaw_limit = 2200 + abs(g.rc_4.control_in); + // smoother Yaw control: return constrain(output, -yaw_limit, yaw_limit); #endif diff --git a/ArduCopter/flip.pde b/ArduCopter/flip.pde index 7154ce0adb..9aa20df2c0 100644 --- a/ArduCopter/flip.pde +++ b/ArduCopter/flip.pde @@ -27,7 +27,8 @@ void init_flip() void roll_flip() { // Pitch - g.rc_2.servo_out = get_stabilize_pitch(g.rc_2.control_in); + //g.rc_2.servo_out = get_stabilize_pitch(g.rc_2.control_in); + get_stabilize_pitch(g.rc_2.control_in); int32_t roll = ahrs.roll_sensor * flip_dir; @@ -55,7 +56,8 @@ void roll_flip() case 2: if (flip_timer < 100) { - g.rc_1.servo_out = get_stabilize_roll(g.rc_1.control_in); + //g.rc_1.servo_out = get_stabilize_roll(g.rc_1.control_in); + get_stabilize_roll(g.rc_1.control_in); g.rc_3.servo_out = g.rc_3.control_in + AAP_THR_INC; flip_timer++; }else{ diff --git a/ArduCopter/toy.pde b/ArduCopter/toy.pde index 5f1f96d900..e3723aeed7 100644 --- a/ArduCopter/toy.pde +++ b/ArduCopter/toy.pde @@ -109,12 +109,12 @@ void roll_pitch_toy() int16_t yaw_rate = g.rc_1.control_in / g.toy_yaw_rate; if(g.rc_1.control_in != 0) { // roll - g.rc_4.servo_out = get_acro_yaw(yaw_rate/2); + get_acro_yaw(yaw_rate/2); yaw_stopped = false; yaw_timer = 150; }else if (!yaw_stopped) { - g.rc_4.servo_out = get_acro_yaw(0); + get_acro_yaw(0); yaw_timer--; if((yaw_timer == 0) || (fabs(omega.z) < .17)) { @@ -125,7 +125,7 @@ void roll_pitch_toy() if(motors.armed() == false || g.rc_3.control_in == 0) nav_yaw = ahrs.yaw_sensor; - g.rc_4.servo_out = get_stabilize_yaw(nav_yaw); + get_stabilize_yaw(nav_yaw); } #endif @@ -167,13 +167,17 @@ void roll_pitch_toy() #if TOY_EDF == ENABLED // Output the attitude - g.rc_1.servo_out = get_stabilize_roll(roll_rate); - g.rc_2.servo_out = get_stabilize_pitch(g.rc_6.control_in); // use CH6 to trim pitch + //g.rc_1.servo_out = get_stabilize_roll(roll_rate); + //g.rc_2.servo_out = get_stabilize_pitch(g.rc_6.control_in); // use CH6 to trim pitch + get_stabilize_roll(roll_rate); + get_stabilize_pitch(g.rc_6.control_in); // use CH6 to trim pitch #else // Output the attitude - g.rc_1.servo_out = get_stabilize_roll(roll_rate); - g.rc_2.servo_out = get_stabilize_pitch(g.rc_2.control_in); + //g.rc_1.servo_out = get_stabilize_roll(roll_rate); + //g.rc_2.servo_out = get_stabilize_pitch(g.rc_2.control_in); + get_stabilize_roll(roll_rate); + get_stabilize_pitch(g.rc_2.control_in); #endif } \ No newline at end of file