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

Copter: Acro flight mode from Leonard

This commit is contained in:
Randy Mackay 2014-01-25 12:00:07 +09:00 committed by Andrew Tridgell
parent 4003b4da9b
commit 32b8f80510
2 changed files with 141 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

140
ArduCopter/control_acro.pde Normal file
View File

@ -0,0 +1,140 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
* control_stabilize.pde - init and run calls for acro flight mode
*/
// acro_init - initialise acro controller
static bool acro_init(bool ignore_checks)
{
return true;
}
// acro_run - runs the acro controller
// should be called at 100hz or more
static void acro_run()
{
int16_t target_roll, target_pitch, target_yaw;
Vector3f rate_target; // for roll, pitch, yaw body-frame rate targets
// convert the input to the desired body frame rate
get_pilot_desired_angle_rates(g.rc_1.control_in, g.rc_2.control_in, g.rc_4.control_in, target_roll, target_pitch, target_yaw);
attitude_control.ratebf_rpy(target_roll, target_pitch, target_yaw);
// To-Do: handle helicopter
// pilot controlled yaw using rate controller
//get_yaw_rate_stabilized_bf(pilot_yaw);
// call get_acro_level_rates() here?
// To-Do: convert body-frame stabilized angles to earth frame angles and update controll_roll, pitch and yaw?
// body-frame rate controller is run directly from 100hz loop
}
// get_pilot_desired_angle_rates - transform pilot's roll pitch and yaw input into a desired lean angle rates
// returns desired angle rates in centi-degrees-per-second
static void get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int16_t yaw_in, int16_t &roll_out, int16_t &pitch_out, int16_t &yaw_out)
{
// Calculate trainer mode earth frame rate command for roll
int32_t target_rate;
Vector3f rate_ef_level, rate_bf_level, rate_bf_request;
// calculate rate requests
rate_bf_request.x = roll_in * g.acro_rp_p;
rate_bf_request.y = pitch_in * g.acro_rp_p;
rate_bf_request.z = yaw_in * g.acro_yaw_p;
// todo: add acceleration slew
// calculate earth frame rate corrections to pull the copter back to level while in ACRO mode
acro_level_mix = constrain_float(1-max(max(abs(roll_in), abs(pitch_in)), abs(yaw_in))/4500.0, 0, 1)*cos_pitch_x;
// Calculate trainer mode earth frame rate command for roll
int32_t roll_angle = wrap_180_cd(ahrs.roll_sensor);
target_rate = 0;
if (g.acro_trainer == ACRO_TRAINER_LIMITED) {
if (roll_angle > aparm.angle_max){
target_rate = g.pi_stabilize_roll.get_p(aparm.angle_max-roll_angle);
}else if (roll_angle < -aparm.angle_max) {
target_rate = g.pi_stabilize_roll.get_p(-aparm.angle_max-roll_angle);
}
}
roll_angle = constrain_int32(roll_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE);
rate_ef_level.x -= roll_angle * g.acro_balance_roll;
// Calculate trainer mode earth frame rate command for pitch
int32_t pitch_angle = wrap_180_cd(ahrs.pitch_sensor);
target_rate = 0;
if (g.acro_trainer == ACRO_TRAINER_LIMITED) {
if (pitch_angle > aparm.angle_max){
target_rate = g.pi_stabilize_pitch.get_p(aparm.angle_max-pitch_angle);
}else if (pitch_angle < -aparm.angle_max) {
target_rate = g.pi_stabilize_pitch.get_p(-aparm.angle_max-pitch_angle);
}
}
pitch_angle = constrain_int32(pitch_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE);
rate_ef_level.y -= pitch_angle * g.acro_balance_pitch;
// Calculate trainer mode earth frame rate command for yaw
rate_ef_level.z = 0;
// convert earth-frame level rates to body-frame level rates
attitude_control.rate_ef_targets_to_bf(rate_ef_level, rate_bf_level);
// combine earth frame rate corrections with rate requests
// combine bf roll leveling with requested bf roll rate
if (g.acro_trainer == ACRO_TRAINER_LIMITED) {
rate_bf_request.x += rate_bf_level.x;
}else{
// Scale pitch leveling by stick input
rate_bf_level.x = (float)rate_bf_level.x*acro_level_mix;
// Calculate rate limit to prevent change of rate through inverted
int32_t rate_limit = labs(labs(rate_bf_request.x)-labs(rate_bf_level.x));
rate_bf_request.x += acro_roll_rate;
rate_bf_request.x = constrain_int32(rate_bf_request.x, -rate_limit, rate_limit);
}
// combine bf pitch leveling with requested bf pitch rate
if (g.acro_trainer == ACRO_TRAINER_LIMITED) {
rate_bf_request.y += rate_bf_level.y;
}else{
// Scale pitch leveling by stick input
rate_bf_level.y = (float)rate_bf_level.y*acro_level_mix;
// Calculate rate limit to prevent change of rate through inverted
int32_t rate_limit = labs(labs(rate_bf_request.y)-labs(rate_bf_level.y));
rate_bf_request.y += acro_roll_rate;
rate_bf_request.y = constrain_int32(rate_bf_request.y, -rate_limit, rate_limit);
}
// combine bf yaw leveling with requested bf yaw rate
if (g.acro_trainer == ACRO_TRAINER_LIMITED) {
rate_bf_request.z += rate_bf_level.z;
}else{
// Scale pitch leveling by stick input
rate_bf_level.z = (float)rate_bf_level.z*acro_level_mix;
// Calculate rate limit to prevent change of rate through inverted
int32_t rate_limit = labs(labs(rate_bf_request.z)-labs(rate_bf_level.z));
rate_bf_request.z += acro_roll_rate;
rate_bf_request.z = constrain_int32(rate_bf_request.z, -rate_limit, rate_limit);
}
// hand back rate request
roll_out = rate_bf_request.x;
pitch_out = rate_bf_request.y;
yaw_out = rate_bf_request.z;
}

42
ArduCopter/control_stabilize.pde
View File

@ -1,49 +1,9 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
* control_stabilize.pde - init and run calls for acro, stabilize, althold, drift and sport flight modes
* control_stabilize.pde - init and run calls for stabilize, althold, drift and sport flight modes
*/
// acro_init - initialise acro controller
static bool acro_init(bool ignore_checks)
{
return true;
}
// acro_run - runs the acro controller
// should be called at 100hz or more
static void acro_run()
{
Vector3f rate_target; // for roll, pitch, yaw body-frame rate targets
// convert the input to the desired body frame rate
rate_target.x = g.rc_1.control_in * g.acro_rp_p;
rate_target.y = g.rc_2.control_in * g.acro_rp_p;
rate_target.z = g.rc_4.control_in * g.acro_yaw_p;
// To-Do: handle acro trainer here?
// To-Do: handle helicopter
acro_level_mix = constrain_float(1-max(max(abs(g.rc_1.control_in), abs(g.rc_2.control_in)), abs(g.rc_4.control_in))/4500.0, 0, 1)*cos_pitch_x;
// set targets for body frame rate controller
attitude_control.rate_stab_bf_targets(rate_target);
// convert stabilize rates to regular rates
attitude_control.rate_stab_bf_to_rate_bf_roll();
attitude_control.rate_stab_bf_to_rate_bf_pitch();
attitude_control.rate_stab_bf_to_rate_bf_yaw();
// pilot controlled yaw using rate controller
//get_yaw_rate_stabilized_bf(pilot_yaw);
// call get_acro_level_rates() here?
// To-Do: convert body-frame stabilized angles to earth frame angles and update controll_roll, pitch and yaw?
// body-frame rate controller is run directly from 100hz loop
}
// stabilize_init - initialise stabilize controller
static bool stabilize_init(bool ignore_checks)
{