diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 77c40a78d5..3aa4bb634c 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -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); diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index f351345acf..b217619df0 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -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; } } diff --git a/ArduCopter/config.h b/ArduCopter/config.h index beff8c60c3..4c96cbc112 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -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 diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index b59d714eb6..1ed5547ecb 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -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;