mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-02 14:08:45 -04:00
Copter: Body frame ACRO upgrade
This commit is contained in:
Leonard Hall
Randy Mackay
parent
6dd446ac79
commit
64ce9b017d
@ -1466,7 +1466,7 @@ void update_yaw_mode(void)
|
||||
case YAW_ACRO:
|
||||
// pilot controlled yaw using rate controller
|
||||
if(g.axis_enabled) {
|
||||
get_yaw_rate_stabilized_ef(g.rc_4.control_in);
|
||||
get_yaw_rate_stabilized_bf(g.rc_4.control_in);
|
||||
}else{
|
||||
get_acro_yaw(g.rc_4.control_in);
|
||||
}
|
||||
@ -1625,8 +1625,8 @@ void update_roll_pitch_mode(void)
|
||||
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
if(g.axis_enabled) {
|
||||
get_roll_rate_stabilized_ef(g.rc_1.control_in);
|
||||
get_pitch_rate_stabilized_ef(g.rc_2.control_in);
|
||||
get_roll_rate_stabilized_bf(g.rc_1.control_in);
|
||||
get_pitch_rate_stabilized_bf(g.rc_2.control_in);
|
||||
}else{
|
||||
// ACRO does not get SIMPLE mode ability
|
||||
if (motors.flybar_mode == 1) {
|
||||
@ -1639,8 +1639,8 @@ void update_roll_pitch_mode(void)
|
||||
}
|
||||
#else // !HELI_FRAME
|
||||
if(g.axis_enabled) {
|
||||
get_roll_rate_stabilized_ef(g.rc_1.control_in);
|
||||
get_pitch_rate_stabilized_ef(g.rc_2.control_in);
|
||||
get_roll_rate_stabilized_bf(g.rc_1.control_in);
|
||||
get_pitch_rate_stabilized_bf(g.rc_2.control_in);
|
||||
}else{
|
||||
// ACRO does not get SIMPLE mode ability
|
||||
get_acro_roll(g.rc_1.control_in);
|
||||
|
||||
@ -99,6 +99,161 @@ 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
|
||||
static void
|
||||
get_roll_rate_stabilized_bf(int32_t stick_angle)
|
||||
{
|
||||
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 FRAME_CONFIG == HELI_FRAME
|
||||
if (!motors.motor_runup_complete) {
|
||||
angle_error = 0;
|
||||
}
|
||||
#else
|
||||
if (!motors.armed() || g.rc_3.servo_out == 0) {
|
||||
angle_error = 0;
|
||||
}
|
||||
#endif // HELI_FRAME
|
||||
|
||||
// set body frame targets for rate controller
|
||||
set_roll_rate_target(roll_axis + correction_rate, BODY_FRAME);
|
||||
|
||||
// Calculate trainer mode earth frame rate command
|
||||
int32_t roll_angle = wrap_180_cd(ahrs.roll_sensor);
|
||||
|
||||
int32_t target_rate = 0;
|
||||
|
||||
if (g.acro_trainer_enabled) {
|
||||
if (roll_angle > 4500){
|
||||
target_rate = g.pi_stabilize_roll.get_p(4500-roll_angle);
|
||||
}else if (roll_angle < -4500) {
|
||||
target_rate = g.pi_stabilize_roll.get_p(-4500-roll_angle);
|
||||
}
|
||||
}
|
||||
roll_angle = constrain_int32(roll_angle, -3000, 3000);
|
||||
target_rate -= (roll_angle * g.acro_balance_roll)/100;
|
||||
|
||||
// add earth frame targets for rate controller
|
||||
set_roll_rate_target(target_rate, BODY_EARTH_FRAME);
|
||||
}
|
||||
|
||||
// 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;
|
||||
|
||||
// scale pitch leveling by stick input
|
||||
|
||||
if (!g.acro_trainer_enabled) {
|
||||
pitch_axis = (float)pitch_axis*constrain_float((1-fabsf(stick_angle/4500.0)),0,1)*cos_pitch_x;
|
||||
}
|
||||
|
||||
// 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);
|
||||
|
||||
// Calculate integrated body frame rate error
|
||||
angle_error += (pitch_axis - (omega.y * DEGX100)) * G_Dt;
|
||||
|
||||
// don't let angle error grow too large
|
||||
angle_error = constrain_float(angle_error, - MAX_PITCH_OVERSHOOT, MAX_PITCH_OVERSHOOT);
|
||||
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
if (!motors.motor_runup_complete) {
|
||||
angle_error = 0;
|
||||
}
|
||||
#else
|
||||
if (!motors.armed() || g.rc_3.servo_out == 0) {
|
||||
angle_error = 0;
|
||||
}
|
||||
#endif // HELI_FRAME
|
||||
|
||||
// set body frame targets for rate controller
|
||||
set_pitch_rate_target(pitch_axis + correction_rate, BODY_FRAME);
|
||||
|
||||
// Calculate trainer mode earth frame rate command
|
||||
int32_t pitch_angle = wrap_180_cd(ahrs.pitch_sensor);
|
||||
|
||||
int32_t 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, -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
|
||||
static void
|
||||
get_yaw_rate_stabilized_bf(int32_t stick_angle)
|
||||
{
|
||||
static float angle_error = 0;
|
||||
|
||||
// scale yaw leveling by stick input
|
||||
|
||||
if (!g.acro_trainer_enabled) {
|
||||
nav_yaw = (float)nav_yaw*constrain_float((1-fabsf(stick_angle/4500.0)),0,1)*cos_pitch_x;
|
||||
}
|
||||
|
||||
// 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);
|
||||
|
||||
// Calculate integrated body frame rate error
|
||||
angle_error += (nav_yaw - (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);
|
||||
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
if (!motors.motor_runup_complete) {
|
||||
angle_error = 0;
|
||||
}
|
||||
#else
|
||||
if (!motors.armed() || g.rc_3.servo_out == 0) {
|
||||
angle_error = 0;
|
||||
}
|
||||
#endif // HELI_FRAME
|
||||
|
||||
// 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
|
||||
static void
|
||||
get_roll_rate_stabilized_ef(int32_t stick_angle)
|
||||
@ -237,9 +392,14 @@ update_rate_contoller_targets()
|
||||
{
|
||||
if( rate_targets_frame == EARTH_FRAME ) {
|
||||
// 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 * 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;
|
||||
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 * 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;
|
||||
}else if( rate_targets_frame == BODY_EARTH_FRAME ) {
|
||||
// add converted earth frame rates to body frame rates
|
||||
roll_axis = roll_rate_target_ef - sin_pitch * yaw_rate_target_ef;
|
||||
pitch_axis = cos_roll_x * pitch_rate_target_ef + sin_roll * cos_pitch_x * yaw_rate_target_ef;
|
||||
nav_yaw = cos_pitch_x * cos_roll_x * yaw_rate_target_ef - sin_roll * pitch_rate_target_ef;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -565,8 +565,9 @@
|
||||
|
||||
// definitions for earth frame and body frame
|
||||
// used to specify frame to rate controllers
|
||||
#define EARTH_FRAME 0
|
||||
#define BODY_FRAME 1
|
||||
#define EARTH_FRAME 0
|
||||
#define BODY_FRAME 1
|
||||
#define BODY_EARTH_FRAME 2
|
||||
|
||||
|
||||
// Flight mode roll, pitch, yaw, throttle and navigation definitions
|
||||
|
||||
@ -364,9 +364,9 @@ static void set_mode(uint8_t mode)
|
||||
set_nav_mode(NAV_NONE);
|
||||
// reset acro axis targets to current attitude
|
||||
if(g.axis_enabled){
|
||||
roll_axis = ahrs.roll_sensor;
|
||||
pitch_axis = ahrs.pitch_sensor;
|
||||
nav_yaw = ahrs.yaw_sensor;
|
||||
roll_axis = 0;
|
||||
pitch_axis = 0;
|
||||
nav_yaw = 0;
|
||||
}
|
||||
break;
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user