aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

ArduCopter: fixed acro mode

Browse Source 
Changes included:
Removing earth frame roll_rate_trim, pitch_rate_trim and yaw_rate_trim.
Switch ACRO mode to use YAW_ACRO instead of YAW_HOLD.
Changed YAW_ACRO to use stabilize yaw when axis_enabled.
Reset ACRO roll, pitch and yaw targets to current attitude when first entering ACRO.
This commit is contained in:
rmackay9 2012-10-14 17:47:46 +09:00
parent ddef15b095
commit 254fff6acd
4 changed files with 91 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
ArduCopter/ArduCopter.pde
View File

@ -590,12 +590,10 @@ static int32_t initial_simple_bearing;
////////////////////////////////////////////////////////////////////////////////
// Rate contoller targets
////////////////////////////////////////////////////////////////////////////////
static uint8_t rate_targets_frame = EARTH_FRAME; // indicates whether rate targets provided in earth or body frame
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
@ -1594,7 +1592,16 @@ void update_yaw_mode(void)
{
switch(yaw_mode) {
case YAW_ACRO:
if(g.axis_enabled) {
nav_yaw += (float)g.rc_4.control_in * g.axis_lock_p;
nav_yaw = wrap_360(nav_yaw);
if (g.rc_3.control_in == 0) {
nav_yaw = ahrs.yaw_sensor;
}
get_stabilize_yaw(nav_yaw);
}else{
get_acro_yaw(g.rc_4.control_in);
}
return;
break;
@ -1605,12 +1612,12 @@ void update_yaw_mode(void)
case YAW_HOLD:
if(g.rc_4.control_in != 0) {
get_acro_yaw(g.rc_4.control_in);
get_stabilize_rate_yaw(g.rc_4.control_in);
yaw_stopped = false;
yaw_timer = 150;
}else if (!yaw_stopped) {
get_acro_yaw(0);
get_stabilize_rate_yaw(0);
yaw_timer--;
if((yaw_timer == 0) || (fabs(omega.z) < .17)) {
@ -1660,18 +1667,17 @@ void update_roll_pitch_mode(void)
roll_axis += (float)g.rc_1.control_in * g.axis_lock_p;
pitch_axis += (float)g.rc_2.control_in * g.axis_lock_p;
roll_axis = wrap_360(roll_axis);
pitch_axis = wrap_360(pitch_axis);
// in this mode, nav_roll and nav_pitch = the iterm
get_stabilize_roll(roll_axis);
get_stabilize_pitch(pitch_axis);
roll_axis = wrap_180(roll_axis);
pitch_axis = wrap_180(pitch_axis);
if (g.rc_3.control_in == 0) {
roll_axis = 0;
pitch_axis = 0;
}
// in this mode, nav_roll and nav_pitch = the iterm
get_stabilize_roll(roll_axis);
get_stabilize_pitch(pitch_axis);
}else{
// ACRO does not get SIMPLE mode ability
#if FRAME_CONFIG == HELI_FRAME

78
ArduCopter/Attitude.pde
View File

@ -30,8 +30,7 @@ get_stabilize_roll(int32_t target_angle)
}
// set targets for rate controller
roll_rate_target_ef = target_rate;
roll_rate_trim_ef = i_stab;
set_roll_rate_target(target_rate+i_stab, EARTH_FRAME);
#endif
}
@ -64,9 +63,7 @@ get_stabilize_pitch(int32_t target_angle)
}
// set targets for rate controller
pitch_rate_target_ef = target_rate;
pitch_rate_trim_ef = i_stab;
set_pitch_rate_target(target_rate + i_stab, EARTH_FRAME);
#endif
}
@ -111,8 +108,16 @@ get_stabilize_yaw(int32_t target_angle)
#endif
// set targets for rate controller
yaw_rate_target_ef = target_rate;
yaw_rate_trim_ef = i_term;
set_yaw_rate_target(target_rate+i_term, EARTH_FRAME);
}
static void
get_stabilize_rate_yaw(int32_t target_rate)
{
target_rate = g.pi_stabilize_yaw.get_p(target_rate);
// set targets for rate controller
set_yaw_rate_target(target_rate, EARTH_FRAME);
}
static void
@ -121,8 +126,7 @@ get_acro_roll(int32_t target_rate)
target_rate = target_rate * g.acro_p;
// set targets for rate controller
roll_rate_target_ef = target_rate;
roll_rate_trim_ef = 0;
set_roll_rate_target(target_rate, BODY_FRAME);
}
static void
@ -131,18 +135,16 @@ get_acro_pitch(int32_t target_rate)
target_rate = target_rate * g.acro_p;
// set targets for rate controller
pitch_rate_target_ef = target_rate;
pitch_rate_trim_ef = 0;
set_pitch_rate_target(target_rate, BODY_FRAME);
}
static void
get_acro_yaw(int32_t target_rate)
{
target_rate = g.pi_stabilize_yaw.get_p(target_rate);
target_rate = target_rate * g.acro_p;
// set targets for rate controller
yaw_rate_target_ef = target_rate;
yaw_rate_trim_ef = 0;
set_yaw_rate_target(target_rate, BODY_FRAME);
}
/*
@ -219,15 +221,47 @@ 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()
{
// set_roll_rate_target - to be called by upper controllers to set roll rate targets in the earth frame
void set_roll_rate_target( int32_t desired_rate, uint8_t earth_or_body_frame ) {
rate_targets_frame = earth_or_body_frame;
if( earth_or_body_frame == BODY_FRAME ) {
roll_rate_target_bf = desired_rate;
}else{
roll_rate_target_ef = desired_rate;
}
}
// set_pitch_rate_target - to be called by upper controllers to set pitch rate targets in the earth frame
void set_pitch_rate_target( int32_t desired_rate, uint8_t earth_or_body_frame ) {
rate_targets_frame = earth_or_body_frame;
if( earth_or_body_frame == BODY_FRAME ) {
pitch_rate_target_bf = desired_rate;
}else{
pitch_rate_target_ef = desired_rate;
}
}
// set_yaw_rate_target - to be called by upper controllers to set yaw rate targets in the earth frame
void set_yaw_rate_target( int32_t desired_rate, uint8_t earth_or_body_frame ) {
rate_targets_frame = earth_or_body_frame;
if( earth_or_body_frame == BODY_FRAME ) {
yaw_rate_target_bf = desired_rate;
}else{
yaw_rate_target_ef = desired_rate;
}
}
// update_rate_contoller_targets - converts earth frame rates to body frame rates for rate controllers
void
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 * yaw_rate_target_ef;
yaw_rate_target_bf = cos_pitch_x * cos_roll_x * yaw_rate_target_ef + sin_roll * pitch_rate_target_ef;
}
}
// run roll, pitch and yaw rate controllers and send output to motors
// targets for these controllers comes from stabilize controllers
@ -236,13 +270,13 @@ run_rate_controllers()
{
#if FRAME_CONFIG == HELI_FRAME // helicopters only use rate controllers for yaw and only when not using an external gyro
if(!motors.ext_gyro_enabled) {
g.rc_4.servo_out = get_rate_yaw(yaw_rate_target_bf) + yaw_rate_trim_ef;
g.rc_4.servo_out = get_rate_yaw(yaw_rate_target_bf);
}
#else
// 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;
g.rc_1.servo_out = get_rate_roll(roll_rate_target_bf);
g.rc_2.servo_out = get_rate_pitch(pitch_rate_target_bf);
g.rc_4.servo_out = get_rate_yaw(yaw_rate_target_bf);
#endif
}

5
ArduCopter/config.h
View File

@ -526,6 +526,11 @@
// Attitude Control
//
// definitions for earth frame and body frame
// used to specify frame to rate controllers
#define EARTH_FRAME 0
#define BODY_FRAME 1
// Alt Hold Mode
#ifndef ALT_HOLD_YAW
# define ALT_HOLD_YAW YAW_HOLD

8
ArduCopter/system.pde
View File

@ -456,9 +456,15 @@ static void set_mode(byte mode)
switch(control_mode)
{
case ACRO:
yaw_mode = YAW_HOLD;
yaw_mode = YAW_ACRO;
roll_pitch_mode = ROLL_PITCH_ACRO;
throttle_mode = THROTTLE_MANUAL;
// 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;
}
break;
case STABILIZE: