From 64ce9b017d38171d2566a6b3751766c2d6e793a2 Mon Sep 17 00:00:00 2001
From: Leonard Hall <LeonardTHall@gmail.com>
Date: Sat, 6 Jul 2013 22:29:57 +0930
Subject: [PATCH] Copter: Body frame ACRO upgrade
---
ArduCopter/ArduCopter.pde | 10 +--
ArduCopter/Attitude.pde | 166 +++++++++++++++++++++++++++++++++++++-
ArduCopter/config.h | 5 +-
ArduCopter/system.pde | 6 +-
4 files changed, 174 insertions(+), 13 deletions(-)
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;