Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: Acro trainer separated into limited and leveled

This commit is contained in:
Randy Mackay 2013-08-04 18:14:07 +09:00
parent e32342163d
commit 21e523b9ac
5 changed files with 47 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
ArduCopter/Attitude.pde
View File

@ -101,11 +101,19 @@ get_acro_yaw(int32_t target_rate)
static void static void
get_acro_level_rates() get_acro_level_rates()
{ {
// zero earth frame leveling if trainer is disabled
if (g.acro_trainer == ACRO_TRAINER_DISABLED) {
set_roll_rate_target(0, BODY_EARTH_FRAME);
set_pitch_rate_target(0, BODY_EARTH_FRAME);
set_yaw_rate_target(0, BODY_EARTH_FRAME);
return;
}
// Calculate trainer mode earth frame rate command for roll // Calculate trainer mode earth frame rate command for roll
int32_t roll_angle = wrap_180_cd(ahrs.roll_sensor); int32_t roll_angle = wrap_180_cd(ahrs.roll_sensor);
int32_t target_rate = 0; int32_t target_rate = 0;
if (g.acro_trainer_enabled) { if (g.acro_trainer == ACRO_TRAINER_LIMITED) {
if (roll_angle > 4500){ if (roll_angle > 4500){
target_rate = g.pi_stabilize_roll.get_p(4500-roll_angle); target_rate = g.pi_stabilize_roll.get_p(4500-roll_angle);
}else if (roll_angle < -4500) { }else if (roll_angle < -4500) {
@ -113,7 +121,7 @@ get_acro_level_rates()
} }
} }
roll_angle = constrain_int32(roll_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE); roll_angle = constrain_int32(roll_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE);
target_rate -= (roll_angle * g.acro_balance_roll)/100; target_rate -= roll_angle * g.acro_balance_roll;
// add earth frame targets for roll rate controller // add earth frame targets for roll rate controller
set_roll_rate_target(target_rate, BODY_EARTH_FRAME); set_roll_rate_target(target_rate, BODY_EARTH_FRAME);
@ -122,7 +130,7 @@ get_acro_level_rates()
int32_t pitch_angle = wrap_180_cd(ahrs.pitch_sensor); int32_t pitch_angle = wrap_180_cd(ahrs.pitch_sensor);
target_rate = 0; target_rate = 0;
if (g.acro_trainer_enabled) { if (g.acro_trainer == ACRO_TRAINER_LIMITED) {
if (pitch_angle > 4500){ if (pitch_angle > 4500){
target_rate = g.pi_stabilize_pitch.get_p(4500-pitch_angle); target_rate = g.pi_stabilize_pitch.get_p(4500-pitch_angle);
}else if (pitch_angle < -4500) { }else if (pitch_angle < -4500) {
@ -130,7 +138,7 @@ get_acro_level_rates()
} }
} }
pitch_angle = constrain_int32(pitch_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE); pitch_angle = constrain_int32(pitch_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE);
target_rate -= (pitch_angle * g.acro_balance_pitch)/100; target_rate -= pitch_angle * g.acro_balance_pitch;
// add earth frame targets for pitch rate controller // add earth frame targets for pitch rate controller
set_pitch_rate_target(target_rate, BODY_EARTH_FRAME); set_pitch_rate_target(target_rate, BODY_EARTH_FRAME);
@ -148,7 +156,9 @@ get_roll_rate_stabilized_bf(int32_t stick_angle)
// convert the input to the desired body frame roll rate // convert the input to the desired body frame roll rate
int32_t rate_request = stick_angle * g.acro_p; int32_t rate_request = stick_angle * g.acro_p;
if (!g.acro_trainer_enabled) { if (g.acro_trainer == ACRO_TRAINER_LIMITED) {
rate_request += acro_roll_rate;
}else{
// Scale pitch leveling by stick input // Scale pitch leveling by stick input
acro_roll_rate = (float)acro_roll_rate*acro_level_mix; acro_roll_rate = (float)acro_roll_rate*acro_level_mix;
@ -157,8 +167,6 @@ get_roll_rate_stabilized_bf(int32_t stick_angle)
rate_request += acro_roll_rate; rate_request += acro_roll_rate;
rate_request = constrain_int32(rate_request, -rate_limit, rate_limit); rate_request = constrain_int32(rate_request, -rate_limit, rate_limit);
} else {
rate_request += acro_roll_rate;
} }
// add automatic correction // add automatic correction
@ -187,7 +195,9 @@ get_pitch_rate_stabilized_bf(int32_t stick_angle)
// convert the input to the desired body frame pitch rate // convert the input to the desired body frame pitch rate
int32_t rate_request = stick_angle * g.acro_p; int32_t rate_request = stick_angle * g.acro_p;
if (!g.acro_trainer_enabled) { if (g.acro_trainer == ACRO_TRAINER_LIMITED) {
rate_request += acro_pitch_rate;
}else{
// Scale pitch leveling by stick input // Scale pitch leveling by stick input
acro_pitch_rate = (float)acro_pitch_rate*acro_level_mix; acro_pitch_rate = (float)acro_pitch_rate*acro_level_mix;
@ -196,8 +206,6 @@ get_pitch_rate_stabilized_bf(int32_t stick_angle)
rate_request += acro_pitch_rate; rate_request += acro_pitch_rate;
rate_request = constrain_int32(rate_request, -rate_limit, rate_limit); rate_request = constrain_int32(rate_request, -rate_limit, rate_limit);
} else {
rate_request += acro_pitch_rate;
} }
// add automatic correction // add automatic correction
@ -226,7 +234,9 @@ get_yaw_rate_stabilized_bf(int32_t stick_angle)
// convert the input to the desired body frame yaw rate // convert the input to the desired body frame yaw rate
int32_t rate_request = stick_angle * g.acro_p; int32_t rate_request = stick_angle * g.acro_p;
if (!g.acro_trainer_enabled) { if (g.acro_trainer == ACRO_TRAINER_LIMITED) {
rate_request += acro_yaw_rate;
}else{
// Scale pitch leveling by stick input // Scale pitch leveling by stick input
acro_yaw_rate = (float)acro_yaw_rate*acro_level_mix; acro_yaw_rate = (float)acro_yaw_rate*acro_level_mix;
@ -235,8 +245,6 @@ get_yaw_rate_stabilized_bf(int32_t stick_angle)
rate_request += acro_yaw_rate; rate_request += acro_yaw_rate;
rate_request = constrain_int32(rate_request, -rate_limit, rate_limit); rate_request = constrain_int32(rate_request, -rate_limit, rate_limit);
} else {
rate_request += acro_yaw_rate;
} }
// add automatic correction // add automatic correction
@ -263,14 +271,14 @@ get_roll_rate_stabilized_ef(int32_t stick_angle)
int32_t angle_error = 0; int32_t angle_error = 0;
// convert the input to the desired roll rate // convert the input to the desired roll rate
int32_t target_rate = stick_angle * g.acro_p - (acro_roll * g.acro_balance_roll)/100; int32_t target_rate = stick_angle * g.acro_p - (acro_roll * g.acro_balance_roll);
// convert the input to the desired roll rate // convert the input to the desired roll rate
acro_roll += target_rate * G_Dt; acro_roll += target_rate * G_Dt;
acro_roll = wrap_180_cd(acro_roll); acro_roll = wrap_180_cd(acro_roll);
// ensure that we don't reach gimbal lock // ensure that we don't reach gimbal lock
if (labs(acro_roll) > 4500 && g.acro_trainer_enabled) { if (labs(acro_roll) > 4500 && g.acro_trainer == ACRO_TRAINER_LIMITED) {
acro_roll = constrain_int32(acro_roll, -4500, 4500); acro_roll = constrain_int32(acro_roll, -4500, 4500);
angle_error = wrap_180_cd(acro_roll - ahrs.roll_sensor); angle_error = wrap_180_cd(acro_roll - ahrs.roll_sensor);
} else { } else {
@ -304,7 +312,7 @@ get_pitch_rate_stabilized_ef(int32_t stick_angle)
int32_t angle_error = 0; int32_t angle_error = 0;
// convert the input to the desired pitch rate // convert the input to the desired pitch rate
int32_t target_rate = stick_angle * g.acro_p - (acro_pitch * g.acro_balance_pitch)/100; int32_t target_rate = stick_angle * g.acro_p - (acro_pitch * g.acro_balance_pitch);
// convert the input to the desired pitch rate // convert the input to the desired pitch rate
acro_pitch += target_rate * G_Dt; acro_pitch += target_rate * G_Dt;

16
ArduCopter/Parameters.h
View File

@ -82,7 +82,7 @@ public:
k_param_rssi_pin, k_param_rssi_pin,
k_param_throttle_accel_enabled, // deprecated - remove k_param_throttle_accel_enabled, // deprecated - remove
k_param_wp_yaw_behavior, k_param_wp_yaw_behavior,
k_param_acro_trainer_enabled, k_param_acro_trainer,
k_param_pilot_velocity_z_max, k_param_pilot_velocity_z_max,
k_param_circle_rate, k_param_circle_rate,
k_param_sonar_gain, k_param_sonar_gain,
@ -251,9 +251,11 @@ public:
k_param_pid_throttle_rate, k_param_pid_throttle_rate,
k_param_pid_optflow_roll, k_param_pid_optflow_roll,
k_param_pid_optflow_pitch, k_param_pid_optflow_pitch,
k_param_acro_balance_roll, // scalar (not PID) k_param_acro_balance_roll_old, // 239 - remove
k_param_acro_balance_pitch, // scalar (not PID) k_param_acro_balance_pitch_old, // 240 - remove
k_param_pid_throttle_accel, // 241 k_param_pid_throttle_accel,
k_param_acro_balance_roll,
k_param_acro_balance_pitch, // 243
// 254,255: reserved // 254,255: reserved
}; };
@ -377,9 +379,9 @@ public:
// Acro parameters // Acro parameters
AP_Float acro_p; AP_Float acro_p;
AP_Int16 acro_balance_roll; AP_Float acro_balance_roll;
AP_Int16 acro_balance_pitch; AP_Float acro_balance_pitch;
AP_Int8 acro_trainer_enabled; AP_Int8 acro_trainer;
// PI/D controllers // PI/D controllers
AC_PID pid_rate_roll; AC_PID pid_rate_roll;

16
ArduCopter/Parameters.pde
View File

@ -531,25 +531,25 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Param: ACRO_BAL_ROLL // @Param: ACRO_BAL_ROLL
// @DisplayName: Acro Balance Roll // @DisplayName: Acro Balance Roll
// @Description: rate at which roll angle returns to level in acro mode // @Description: rate at which roll angle returns to level in acro mode
// @Range: 0 300 // @Range: 0 3
// @Increment: 1 // @Increment: 0.1
// @User: Advanced // @User: Advanced
GSCALAR(acro_balance_roll, "ACRO_BAL_ROLL", ACRO_BALANCE_ROLL), GSCALAR(acro_balance_roll, "ACRO_BAL_ROLL", ACRO_BALANCE_ROLL),
// @Param: ACRO_BAL_PITCH // @Param: ACRO_BAL_PITCH
// @DisplayName: Acro Balance Pitch // @DisplayName: Acro Balance Pitch
// @Description: rate at which pitch angle returns to level in acro mode // @Description: rate at which pitch angle returns to level in acro mode
// @Range: 0 300 // @Range: 0 3
// @Increment: 1 // @Increment: 0.1
// @User: Advanced // @User: Advanced
GSCALAR(acro_balance_pitch, "ACRO_BAL_PITCH", ACRO_BALANCE_PITCH), GSCALAR(acro_balance_pitch, "ACRO_BAL_PITCH", ACRO_BALANCE_PITCH),
// @Param: ACRO_TRAINER // @Param: ACRO_TRAINER
// @DisplayName: Acro Trainer Enabled // @DisplayName: Acro Trainer
// @Description: Set to 1 (Enabled) to make roll return to within 45 degrees of level automatically // @Description: Type of trainer used in acro mode
// @Values: 0:Disabled,1:Enabled // @Values: 0:Disabled,1:Leveling,2:Leveling and Limited
// @User: Advanced // @User: Advanced
GSCALAR(acro_trainer_enabled, "ACRO_TRAINER", ACRO_TRAINER_ENABLED), GSCALAR(acro_trainer, "ACRO_TRAINER", ACRO_TRAINER_LIMITED),
// @Param: LED_MODE // @Param: LED_MODE
// @DisplayName: Copter LED Mode // @DisplayName: Copter LED Mode

8
ArduCopter/config.h
View File

@ -869,15 +869,11 @@
#endif #endif
#ifndef ACRO_BALANCE_ROLL #ifndef ACRO_BALANCE_ROLL
#define ACRO_BALANCE_ROLL 200 #define ACRO_BALANCE_ROLL 1.0f
#endif #endif
#ifndef ACRO_BALANCE_PITCH #ifndef ACRO_BALANCE_PITCH
#define ACRO_BALANCE_PITCH 200 #define ACRO_BALANCE_PITCH 1.0f
#endif
#ifndef ACRO_TRAINER_ENABLED
#define ACRO_TRAINER_ENABLED ENABLED
#endif #endif
////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////

4
ArduCopter/defines.h
View File

@ -175,6 +175,10 @@
#define CH6_DECLINATION 38 // compass declination in radians #define CH6_DECLINATION 38 // compass declination in radians
#define CH6_CIRCLE_RATE 39 // circle turn rate in degrees (hard coded to about 45 degrees in either direction) #define CH6_CIRCLE_RATE 39 // circle turn rate in degrees (hard coded to about 45 degrees in either direction)
// Acro Trainer types
#define ACRO_TRAINER_DISABLED 0
#define ACRO_TRAINER_LEVELING 1
#define ACRO_TRAINER_LIMITED 2
// Commands - Note that APM now uses a subset of the MAVLink protocol // Commands - Note that APM now uses a subset of the MAVLink protocol
// commands. See enum MAV_CMD in the GCS_Mavlink library // commands. See enum MAV_CMD in the GCS_Mavlink library