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

Copter: Acro add rate constrain and combine limit calc

mission-4.1.18
Leonard Hall 12 years ago committed by Randy Mackay
parent
65276d360b
commit
d2deaaa686
2 changed files with 106 additions and 72 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 5
      ArduCopter/ArduCopter.pde
  2. 173
      ArduCopter/Attitude.pde

5
ArduCopter/ArduCopter.pde
Unescape View File

@ -569,6 +569,7 @@ static float target_alt_for_reporting; // target altitude in cm for reporti @@ -569,6 +569,7 @@ static float target_alt_for_reporting; // target altitude in cm for reporti
// Used to control Axis lock
static int32_t roll_axis;
static int32_t pitch_axis;
static float acro_level_mix;
// Filters
#if FRAME_CONFIG == HELI_FRAME
@ -1670,8 +1671,10 @@ void update_roll_pitch_mode(void) @@ -1670,8 +1671,10 @@ void update_roll_pitch_mode(void)
#if FRAME_CONFIG == HELI_FRAME
if(g.axis_enabled) {
acro_level_mix = constrain_float(1-max(max(abs(g.rc_1.control_in), abs(g.rc_2.control_in)), abs(g.rc_4.control_in))/4500.0, 0, 1)*cos_pitch_x;
get_roll_rate_stabilized_bf(g.rc_1.control_in);
get_pitch_rate_stabilized_bf(g.rc_2.control_in);
get_acro_level_rates();
}else{
// ACRO does not get SIMPLE mode ability
if (motors.flybar_mode == 1) {
@ -1684,8 +1687,10 @@ void update_roll_pitch_mode(void) @@ -1684,8 +1687,10 @@ void update_roll_pitch_mode(void)
}
#else // !HELI_FRAME
if(g.axis_enabled) {
acro_level_mix = constrain_float(1-max(max(abs(g.rc_1.control_in), abs(g.rc_2.control_in)), abs(g.rc_4.control_in))/4500.0, 0, 1)*cos_pitch_x;
get_roll_rate_stabilized_bf(g.rc_1.control_in);
get_pitch_rate_stabilized_bf(g.rc_2.control_in);
get_acro_level_rates();
}else{
// ACRO does not get SIMPLE mode ability
get_acro_roll(g.rc_1.control_in);

173
ArduCopter/Attitude.pde
Unescape View File

@ -97,39 +97,12 @@ get_acro_yaw(int32_t target_rate) @@ -97,39 +97,12 @@ get_acro_yaw(int32_t target_rate)
set_yaw_rate_target(target_rate, BODY_FRAME);
}
// Roll with rate input and stabilized in the body frame
#define ACRO_LEVEL_MAX_ANGLE 3000
// Get ACRO level rates
static void
get_roll_rate_stabilized_bf(int32_t stick_angle)
get_acro_level_rates()
{
static float angle_error = 0;
// Scale pitch leveling by stick input
if (!g.acro_trainer_enabled) {
roll_axis = (float)roll_axis*constrain_float((1-fabsf(stick_angle/4500.0)),0,1)*cos_pitch_x;
}
// convert the input to the desired body frame roll rate
roll_axis += stick_angle * g.acro_p;
// add automatic correction
int32_t correction_rate = g.pi_stabilize_roll.get_p(angle_error);
// Calculate integrated body frame rate error
angle_error += (roll_axis - (omega.x * DEGX100)) * G_Dt;
// don't let angle error grow too large
angle_error = constrain_float(angle_error, - MAX_ROLL_OVERSHOOT, MAX_ROLL_OVERSHOOT);
if (!motors.armed() || g.rc_3.servo_out == 0) {
angle_error = 0;
}
// set body frame targets for rate controller
set_roll_rate_target(roll_axis + correction_rate, BODY_FRAME);
// Calculate trainer mode earth frame rate command
// Calculate trainer mode earth frame rate command for roll
int32_t roll_angle = wrap_180_cd(ahrs.roll_sensor);
int32_t target_rate = 0;
@ -141,61 +114,113 @@ get_roll_rate_stabilized_bf(int32_t stick_angle) @@ -141,61 +114,113 @@ get_roll_rate_stabilized_bf(int32_t stick_angle)
target_rate = g.pi_stabilize_roll.get_p(-4500-roll_angle);
}
}
roll_angle = constrain_int32(roll_angle, -3000, 3000);
roll_angle = constrain_int32(roll_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE);
target_rate -= (roll_angle * g.acro_balance_roll)/100;
// add earth frame targets for rate controller
// add earth frame targets for roll rate controller
set_roll_rate_target(target_rate, BODY_EARTH_FRAME);
// Calculate trainer mode earth frame rate command for pitch
int32_t pitch_angle = wrap_180_cd(ahrs.pitch_sensor);
target_rate = 0;
if (g.acro_trainer_enabled) {
if (pitch_angle > 4500){
target_rate = g.pi_stabilize_pitch.get_p(4500-pitch_angle);
}else if (pitch_angle < -4500) {
target_rate = g.pi_stabilize_pitch.get_p(-4500-pitch_angle);
}
}
pitch_angle = constrain_int32(pitch_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE);
target_rate -= (pitch_angle * g.acro_balance_pitch)/100;
// add earth frame targets for pitch rate controller
set_pitch_rate_target(target_rate, BODY_EARTH_FRAME);
// add earth frame targets for yaw rate controller
set_yaw_rate_target(0, BODY_EARTH_FRAME);
}
// Pitch with rate input and stabilized in the body frame
// Roll with rate input and stabilized in the body frame
static void
get_pitch_rate_stabilized_bf(int32_t stick_angle)
get_roll_rate_stabilized_bf(int32_t stick_angle)
{
static float angle_error = 0;
// scale pitch leveling by stick input
// convert the input to the desired body frame roll rate
int32_t rate_request = stick_angle * g.acro_p;
if (!g.acro_trainer_enabled) {
pitch_axis = (float)pitch_axis*constrain_float((1-fabsf(stick_angle/4500.0)),0,1)*cos_pitch_x;
// Scale pitch leveling by stick input
roll_axis = (float)roll_axis*acro_level_mix;
// Calculate rate limit to prevent change of rate through inverted
int32_t rate_limit = abs(abs(rate_request)-abs(roll_axis));
rate_request += roll_axis;
rate_request = constrain_int32(rate_request, -rate_limit, rate_limit);
} else {
rate_request += roll_axis;
}
// convert the input to the desired body frame pitch rate
pitch_axis += stick_angle * g.acro_p;
// add automatic correction
int32_t correction_rate = g.pi_stabilize_pitch.get_p(angle_error);
int32_t rate_correction = g.pi_stabilize_roll.get_p(angle_error);
// set body frame targets for rate controller
set_roll_rate_target(rate_request+rate_correction, BODY_FRAME);
// Calculate integrated body frame rate error
angle_error += (pitch_axis - (omega.y * DEGX100)) * G_Dt;
angle_error += (rate_request - (omega.x * DEGX100)) * G_Dt;
// don't let angle error grow too large
angle_error = constrain_float(angle_error, - MAX_PITCH_OVERSHOOT, MAX_PITCH_OVERSHOOT);
angle_error = constrain_float(angle_error, - MAX_ROLL_OVERSHOOT, MAX_ROLL_OVERSHOOT);
if (!motors.armed() || g.rc_3.servo_out == 0) {
angle_error = 0;
}
}
// Pitch with rate input and stabilized in the body frame
static void
get_pitch_rate_stabilized_bf(int32_t stick_angle)
{
static float angle_error = 0;
// convert the input to the desired body frame pitch rate
int32_t rate_request = stick_angle * g.acro_p;
if (!g.acro_trainer_enabled) {
// Scale pitch leveling by stick input
pitch_axis = (float)pitch_axis*acro_level_mix;
// Calculate rate limit to prevent change of rate through inverted
int32_t rate_limit = abs(abs(rate_request)-abs(pitch_axis));
rate_request += pitch_axis;
rate_request = constrain_int32(rate_request, -rate_limit, rate_limit);
} else {
rate_request += pitch_axis;
}
// add automatic correction
int32_t rate_correction = g.pi_stabilize_pitch.get_p(angle_error);
// set body frame targets for rate controller
set_pitch_rate_target(pitch_axis + correction_rate, BODY_FRAME);
set_pitch_rate_target(rate_request+rate_correction, BODY_FRAME);
// Calculate trainer mode earth frame rate command
int32_t pitch_angle = wrap_180_cd(ahrs.pitch_sensor);
// Calculate integrated body frame rate error
angle_error += (rate_request - (omega.y * DEGX100)) * G_Dt;
int32_t target_rate = 0;
// don't let angle error grow too large
angle_error = constrain_float(angle_error, - MAX_ROLL_OVERSHOOT, MAX_ROLL_OVERSHOOT);
if (g.acro_trainer_enabled) {
if (pitch_angle > 4500){
target_rate = g.pi_stabilize_pitch.get_p(4500-pitch_angle);
}else if (pitch_angle < -4500) {
target_rate = g.pi_stabilize_pitch.get_p(-4500-pitch_angle);
}
if (!motors.armed() || g.rc_3.servo_out == 0) {
angle_error = 0;
}
pitch_angle = constrain_int32(pitch_angle, -3000, 3000);
target_rate -= (pitch_angle * g.acro_balance_pitch)/100;
// add earth frame targets for rate controller
set_pitch_rate_target(target_rate, BODY_EARTH_FRAME);
}
// Yaw with rate input and stabilized in the body frame
@ -204,33 +229,37 @@ get_yaw_rate_stabilized_bf(int32_t stick_angle) @@ -204,33 +229,37 @@ get_yaw_rate_stabilized_bf(int32_t stick_angle)
{
static float angle_error = 0;
// scale yaw leveling by stick input
// convert the input to the desired body frame yaw rate
int32_t rate_request = stick_angle * g.acro_p;
if (!g.acro_trainer_enabled) {
nav_yaw = (float)nav_yaw*constrain_float((1-fabsf(stick_angle/4500.0)),0,1)*cos_pitch_x;
// Scale pitch leveling by stick input
nav_yaw = (float)nav_yaw*acro_level_mix;
// Calculate rate limit to prevent change of rate through inverted
int32_t rate_limit = abs(abs(rate_request)-abs(nav_yaw));
rate_request += nav_yaw;
rate_request = constrain_int32(rate_request, -rate_limit, rate_limit);
} else {
rate_request += nav_yaw;
}
// convert the input to the desired body frame yaw rate
nav_yaw += stick_angle * g.acro_p;
// add automatic correction
int32_t correction_rate = g.pi_stabilize_yaw.get_p(angle_error);
int32_t rate_correction = g.pi_stabilize_yaw.get_p(angle_error);
// set body frame targets for rate controller
set_yaw_rate_target(rate_request+rate_correction, BODY_FRAME);
// Calculate integrated body frame rate error
angle_error += (nav_yaw - (omega.z * DEGX100)) * G_Dt;
angle_error += (rate_request - (omega.z * DEGX100)) * G_Dt;
// don't let angle error grow too large
angle_error = constrain_float(angle_error, - MAX_YAW_OVERSHOOT, MAX_YAW_OVERSHOOT);
angle_error = constrain_float(angle_error, - MAX_ROLL_OVERSHOOT, MAX_ROLL_OVERSHOOT);
if (!motors.armed() || g.rc_3.servo_out == 0) {
angle_error = 0;
}
// set body frame targets for rate controller
set_yaw_rate_target(nav_yaw + correction_rate, BODY_FRAME);
// add earth frame targets for rate controller
set_yaw_rate_target(0, BODY_EARTH_FRAME);
}
// Roll with rate input and stabilized in the earth frame

Write Preview
Loading…
Cancel
Save