zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

ArduCopter: prioritise rate controllers, rate controller targets converted to body frame

mission-4.1.18
rmackay9 13 years ago
parent
ea4f256f8e
commit
e375a27058
4 changed files with 124 additions and 47 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 68
      ArduCopter/ArduCopter.pde
  2. 79
      ArduCopter/Attitude.pde
  3. 6
      ArduCopter/flip.pde
  4. 18
      ArduCopter/toy.pde

68
ArduCopter/ArduCopter.pde
Unescape View File

@ -567,6 +567,8 @@ static float cos_roll_x = 1;
static float cos_pitch_x = 1; static float cos_pitch_x = 1;
static float cos_yaw_x = 1; static float cos_yaw_x = 1;
static float sin_yaw_y; static float sin_yaw_y;
static float sin_roll;
static float sin_pitch;
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
// SIMPLE Mode // SIMPLE Mode
@ -575,6 +577,19 @@ static float sin_yaw_y;
// or in SuperSimple mode when the copter leaves a 20m radius from home. // or in SuperSimple mode when the copter leaves a 20m radius from home.
static int32_t initial_simple_bearing; 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 // ACRO Mode
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
@ -1073,6 +1088,13 @@ static void fast_loop()
// some space available // some space available
gcs_send_message(MSG_RETRY_DEFERRED); 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
// ---------- // ----------
read_radio(); read_radio();
@ -1100,9 +1122,8 @@ static void fast_loop()
update_yaw_mode(); update_yaw_mode();
update_roll_pitch_mode(); update_roll_pitch_mode();
// write out the servo PWM values // update targets to rate controllers
// ------------------------------ update_rate_contoller_targets();
set_servos_4();
// agmatthews - USERHOOKS // agmatthews - USERHOOKS
#ifdef USERHOOK_FASTLOOP #ifdef USERHOOK_FASTLOOP
@ -1564,7 +1585,7 @@ void update_yaw_mode(void)
{ {
switch(yaw_mode) { switch(yaw_mode) {
case YAW_ACRO: 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; return;
break; break;
@ -1575,12 +1596,12 @@ void update_yaw_mode(void)
case YAW_HOLD: case YAW_HOLD:
if(g.rc_4.control_in != 0) { 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_stopped = false;
yaw_timer = 150; yaw_timer = 150;
}else if (!yaw_stopped) { }else if (!yaw_stopped) {
g.rc_4.servo_out = get_acro_yaw(0); get_acro_yaw(0);
yaw_timer--; yaw_timer--;
if((yaw_timer == 0) || (fabs(omega.z) < .17)) { 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)) if(motors.armed() == false || ((g.rc_3.control_in == 0) && (control_mode <= ACRO) && !failsafe))
nav_yaw = ahrs.yaw_sensor; nav_yaw = ahrs.yaw_sensor;
g.rc_4.servo_out = get_stabilize_yaw(nav_yaw); get_stabilize_yaw(nav_yaw);
} }
return; return;
break; break;
case YAW_LOOK_AT_HOME: case YAW_LOOK_AT_HOME:
//nav_yaw updated in update_navigation() //nav_yaw updated in update_navigation()
g.rc_4.servo_out = get_stabilize_yaw(nav_yaw); get_stabilize_yaw(nav_yaw);
break; break;
case YAW_AUTO: case YAW_AUTO:
nav_yaw += constrain(wrap_180(auto_yaw - nav_yaw), -60, 60); // 40 deg a second nav_yaw += constrain(wrap_180(auto_yaw - nav_yaw), -60, 60); // 40 deg a second
//Serial.printf("nav_yaw %d ", nav_yaw); //Serial.printf("nav_yaw %d ", nav_yaw);
nav_yaw = wrap_360(nav_yaw); nav_yaw = wrap_360(nav_yaw);
g.rc_4.servo_out = get_stabilize_yaw(nav_yaw); get_stabilize_yaw(nav_yaw);
break; break;
} }
} }
@ -1634,8 +1655,8 @@ void update_roll_pitch_mode(void)
pitch_axis = wrap_360(pitch_axis); pitch_axis = wrap_360(pitch_axis);
// in this mode, nav_roll and nav_pitch = the iterm // in this mode, nav_roll and nav_pitch = the iterm
g.rc_1.servo_out = get_stabilize_roll(roll_axis); get_stabilize_roll(roll_axis);
g.rc_2.servo_out = get_stabilize_pitch(pitch_axis); get_stabilize_pitch(pitch_axis);
if (g.rc_3.control_in == 0) { if (g.rc_3.control_in == 0) {
roll_axis = 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_1.servo_out = g.rc_1.control_in;
g.rc_2.servo_out = g.rc_2.control_in; g.rc_2.servo_out = g.rc_2.control_in;
} else { } else {
g.rc_1.servo_out = get_acro_roll(g.rc_1.control_in); get_acro_roll(g.rc_1.control_in);
g.rc_2.servo_out = get_acro_pitch(g.rc_2.control_in); get_acro_pitch(g.rc_2.control_in);
} }
#else #else
g.rc_1.servo_out = get_acro_roll(g.rc_1.control_in); get_acro_roll(g.rc_1.control_in);
g.rc_2.servo_out = get_acro_pitch(g.rc_2.control_in); get_acro_pitch(g.rc_2.control_in);
#endif #endif
} }
break; break;
@ -1669,8 +1690,9 @@ void update_roll_pitch_mode(void)
control_pitch = g.rc_2.control_in; control_pitch = g.rc_2.control_in;
// in this mode, nav_roll and nav_pitch = the iterm // in this mode, nav_roll and nav_pitch = the iterm
g.rc_1.servo_out = get_stabilize_roll(control_roll); get_stabilize_roll(control_roll);
g.rc_2.servo_out = get_stabilize_pitch(control_pitch); get_stabilize_pitch(control_pitch);
break; break;
case ROLL_PITCH_AUTO: case ROLL_PITCH_AUTO:
@ -1685,8 +1707,8 @@ void update_roll_pitch_mode(void)
control_roll = g.rc_1.control_mix(nav_roll); control_roll = g.rc_1.control_mix(nav_roll);
control_pitch = g.rc_2.control_mix(nav_pitch); control_pitch = g.rc_2.control_mix(nav_pitch);
g.rc_1.servo_out = get_stabilize_roll(control_roll); get_stabilize_roll(control_roll);
g.rc_2.servo_out = get_stabilize_pitch(control_pitch); get_stabilize_pitch(control_pitch);
break; break;
case ROLL_PITCH_STABLE_OF: case ROLL_PITCH_STABLE_OF:
@ -1699,8 +1721,8 @@ void update_roll_pitch_mode(void)
control_pitch = g.rc_2.control_in; control_pitch = g.rc_2.control_in;
// mix in user control with optical flow // mix in user control with optical flow
g.rc_1.servo_out = get_stabilize_roll(get_of_roll(control_roll)); get_stabilize_roll(get_of_roll(control_roll));
g.rc_2.servo_out = get_stabilize_pitch(get_of_pitch(control_pitch)); get_stabilize_pitch(get_of_pitch(control_pitch));
break; break;
// THOR // THOR
@ -2127,6 +2149,10 @@ static void update_trig(void){
sin_yaw_y = yawvector.x; // 1y = north sin_yaw_y = yawvector.x; // 1y = north
cos_yaw_x = yawvector.y; // 0x = 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: //flat:
// 0 ° = cos_yaw: 0.00, sin_yaw: 1.00, // 0 ° = cos_yaw: 0.00, sin_yaw: 1.00,
// 90° = cos_yaw: 1.00, sin_yaw: 0.00, // 90° = cos_yaw: 1.00, sin_yaw: 0.00,

79
ArduCopter/Attitude.pde
Unescape View File

@ -1,6 +1,6 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- /// -*- 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) get_stabilize_roll(int32_t target_angle)
{ {
// angle error // angle error
@ -29,11 +29,13 @@ get_stabilize_roll(int32_t target_angle)
i_stab = g.pi_stabilize_roll.get_integrator(); 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 #endif
} }
static int16_t static void
get_stabilize_pitch(int32_t target_angle) get_stabilize_pitch(int32_t target_angle)
{ {
// angle error // angle error
@ -60,17 +62,20 @@ get_stabilize_pitch(int32_t target_angle)
}else{ }else{
i_stab = g.pi_stabilize_pitch.get_integrator(); 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 #endif
} }
static int16_t static void
get_stabilize_yaw(int32_t target_angle) get_stabilize_yaw(int32_t target_angle)
{ {
int32_t target_rate,i_term; int32_t target_rate,i_term;
int32_t angle_error; int32_t angle_error;
int32_t output; int32_t output = 0;
// angle error // angle error
angle_error = wrap_180(target_angle - ahrs.yaw_sensor); 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 // do not use rate controllers for helicotpers with external gyros
#if FRAME_CONFIG == HELI_FRAME #if FRAME_CONFIG == HELI_FRAME
if(!motors.ext_gyro_enabled) { 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{ }else{
// TO-DO: fix this for helicopters which don't use rate controller
output = constrain((target_rate + i_term), -4500, 4500); output = constrain((target_rate + i_term), -4500, 4500);
} }
#else
output = get_rate_yaw(target_rate) + i_term;
#endif #endif
#if LOGGING_ENABLED == ENABLED #if LOGGING_ENABLED == ENABLED
@ -109,29 +114,39 @@ get_stabilize_yaw(int32_t target_angle)
} }
#endif #endif
// ensure output does not go beyond barries of what an int16_t can hold // set targets for rate controller
return constrain(output,-32000,32000); yaw_rate_target_ef = target_rate;
yaw_rate_trim_ef = i_term;
} }
static int16_t static void
get_acro_roll(int32_t target_rate) get_acro_roll(int32_t target_rate)
{ {
target_rate = target_rate * g.acro_p; 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) get_acro_pitch(int32_t target_rate)
{ {
target_rate = target_rate * g.acro_p; 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) get_acro_yaw(int32_t target_rate)
{ {
target_rate = g.pi_stabilize_yaw.get_p(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 static int16_t
get_rate_roll(int32_t target_rate) get_rate_roll(int32_t target_rate)
{ {
@ -346,6 +390,7 @@ get_rate_yaw(int32_t target_rate)
#else #else
// output control: // output control:
int16_t yaw_limit = 2200 + abs(g.rc_4.control_in); int16_t yaw_limit = 2200 + abs(g.rc_4.control_in);
// smoother Yaw control: // smoother Yaw control:
return constrain(output, -yaw_limit, yaw_limit); return constrain(output, -yaw_limit, yaw_limit);
#endif #endif

6
ArduCopter/flip.pde
Unescape View File

@ -27,7 +27,8 @@ void init_flip()
void roll_flip() void roll_flip()
{ {
// Pitch // 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; int32_t roll = ahrs.roll_sensor * flip_dir;
@ -55,7 +56,8 @@ void roll_flip()
case 2: case 2:
if (flip_timer < 100) { 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; g.rc_3.servo_out = g.rc_3.control_in + AAP_THR_INC;
flip_timer++; flip_timer++;
}else{ }else{

18
ArduCopter/toy.pde
Unescape View File

@ -109,12 +109,12 @@ void roll_pitch_toy()
int16_t yaw_rate = g.rc_1.control_in / g.toy_yaw_rate; int16_t yaw_rate = g.rc_1.control_in / g.toy_yaw_rate;
if(g.rc_1.control_in != 0) { // roll 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_stopped = false;
yaw_timer = 150; yaw_timer = 150;
}else if (!yaw_stopped) { }else if (!yaw_stopped) {
g.rc_4.servo_out = get_acro_yaw(0); get_acro_yaw(0);
yaw_timer--; yaw_timer--;
if((yaw_timer == 0) || (fabs(omega.z) < .17)) { 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) if(motors.armed() == false || g.rc_3.control_in == 0)
nav_yaw = ahrs.yaw_sensor; nav_yaw = ahrs.yaw_sensor;
g.rc_4.servo_out = get_stabilize_yaw(nav_yaw); get_stabilize_yaw(nav_yaw);
} }
#endif #endif
@ -167,13 +167,17 @@ void roll_pitch_toy()
#if TOY_EDF == ENABLED #if TOY_EDF == ENABLED
// Output the attitude // Output the attitude
g.rc_1.servo_out = get_stabilize_roll(roll_rate); //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_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 #else
// Output the attitude // Output the attitude
g.rc_1.servo_out = get_stabilize_roll(roll_rate); //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_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 #endif
} }
Write Preview
Loading…
Cancel
Save