mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-11 02:18:29 -04:00
ArduCopter: prioritise rate controllers, rate controller targets converted to body frame
This commit is contained in:
rmackay9
parent
ea4f256f8e
commit
e375a27058
@ -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,
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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{
|
||||
|
||||
@ -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
|
||||
|
||||
}
|
||||
Loading…
Reference in New Issue
Block a user