mirror of https://github.com/ArduPilot/ardupilot
Plane: move mode acro functions to mode acro
This commit is contained in:
Iampete1
Andrew Tridgell
parent
9097f51698
commit
f6b0c3775c
|
|
@ -368,205 +368,6 @@ void ModeTraining::run()
|
|||
plane.stabilize_yaw();
|
||||
}
|
||||
|
||||
|
||||
void ModeAcro::run()
|
||||
{
|
||||
if (plane.g.acro_locking == 2 && plane.g.acro_yaw_rate > 0 &&
|
||||
plane.yawController.rate_control_enabled()) {
|
||||
// we can do 3D acro locking
|
||||
stabilize_quaternion();
|
||||
return;
|
||||
}
|
||||
|
||||
// Normal acro
|
||||
stabilize();
|
||||
}
|
||||
|
||||
/*
|
||||
this is the ACRO mode stabilization function. It does rate
|
||||
stabilization on roll and pitch axes
|
||||
*/
|
||||
void ModeAcro::stabilize()
|
||||
{
|
||||
const float speed_scaler = plane.get_speed_scaler();
|
||||
const float rexpo = plane.roll_in_expo(true);
|
||||
const float pexpo = plane.pitch_in_expo(true);
|
||||
float roll_rate = (rexpo/SERVO_MAX) * plane.g.acro_roll_rate;
|
||||
float pitch_rate = (pexpo/SERVO_MAX) * plane.g.acro_pitch_rate;
|
||||
|
||||
IGNORE_RETURN(ahrs.get_quaternion(acro_state.q));
|
||||
|
||||
/*
|
||||
check for special roll handling near the pitch poles
|
||||
*/
|
||||
if (plane.g.acro_locking && is_zero(roll_rate)) {
|
||||
/*
|
||||
we have no roll stick input, so we will enter "roll locked"
|
||||
mode, and hold the roll we had when the stick was released
|
||||
*/
|
||||
if (!acro_state.locked_roll) {
|
||||
acro_state.locked_roll = true;
|
||||
acro_state.locked_roll_err = 0;
|
||||
} else {
|
||||
acro_state.locked_roll_err += ahrs.get_gyro().x * plane.G_Dt;
|
||||
}
|
||||
int32_t roll_error_cd = -ToDeg(acro_state.locked_roll_err)*100;
|
||||
plane.nav_roll_cd = ahrs.roll_sensor + roll_error_cd;
|
||||
// try to reduce the integrated angular error to zero. We set
|
||||
// 'stabilze' to true, which disables the roll integrator
|
||||
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, plane.rollController.get_servo_out(roll_error_cd,
|
||||
speed_scaler,
|
||||
true, false));
|
||||
} else {
|
||||
/*
|
||||
aileron stick is non-zero, use pure rate control until the
|
||||
user releases the stick
|
||||
*/
|
||||
acro_state.locked_roll = false;
|
||||
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, plane.rollController.get_rate_out(roll_rate, speed_scaler));
|
||||
}
|
||||
|
||||
if (plane.g.acro_locking && is_zero(pitch_rate)) {
|
||||
/*
|
||||
user has zero pitch stick input, so we lock pitch at the
|
||||
point they release the stick
|
||||
*/
|
||||
if (!acro_state.locked_pitch) {
|
||||
acro_state.locked_pitch = true;
|
||||
acro_state.locked_pitch_cd = ahrs.pitch_sensor;
|
||||
}
|
||||
// try to hold the locked pitch. Note that we have the pitch
|
||||
// integrator enabled, which helps with inverted flight
|
||||
plane.nav_pitch_cd = acro_state.locked_pitch_cd;
|
||||
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, plane.pitchController.get_servo_out(plane.nav_pitch_cd - ahrs.pitch_sensor,
|
||||
speed_scaler,
|
||||
false, false));
|
||||
} else {
|
||||
/*
|
||||
user has non-zero pitch input, use a pure rate controller
|
||||
*/
|
||||
acro_state.locked_pitch = false;
|
||||
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, plane.pitchController.get_rate_out(pitch_rate, speed_scaler));
|
||||
}
|
||||
|
||||
plane.steering_control.steering = plane.rudder_input();
|
||||
|
||||
if (plane.g.acro_yaw_rate > 0 && plane.yawController.rate_control_enabled()) {
|
||||
// user has asked for yaw rate control with yaw rate scaled by ACRO_YAW_RATE
|
||||
const float rudd_expo = plane.rudder_in_expo(true);
|
||||
const float yaw_rate = (rudd_expo/SERVO_MAX) * plane.g.acro_yaw_rate;
|
||||
plane.steering_control.steering = plane.steering_control.rudder = plane.yawController.get_rate_out(yaw_rate, speed_scaler, false);
|
||||
} else if (plane.g2.flight_options & FlightOptions::ACRO_YAW_DAMPER) {
|
||||
// use yaw controller
|
||||
plane.calc_nav_yaw_coordinated();
|
||||
} else {
|
||||
/*
|
||||
manual rudder
|
||||
*/
|
||||
plane.steering_control.rudder = plane.steering_control.steering;
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
quaternion based acro stabilization with continuous locking. Enabled with ACRO_LOCKING=2
|
||||
*/
|
||||
void ModeAcro::stabilize_quaternion()
|
||||
{
|
||||
const float speed_scaler = plane.get_speed_scaler();
|
||||
auto &q = acro_state.q;
|
||||
const float rexpo = plane.roll_in_expo(true);
|
||||
const float pexpo = plane.pitch_in_expo(true);
|
||||
const float yexpo = plane.rudder_in_expo(true);
|
||||
|
||||
// get pilot desired rates
|
||||
float roll_rate = (rexpo/SERVO_MAX) * plane.g.acro_roll_rate;
|
||||
float pitch_rate = (pexpo/SERVO_MAX) * plane.g.acro_pitch_rate;
|
||||
float yaw_rate = (yexpo/SERVO_MAX) * plane.g.acro_yaw_rate;
|
||||
bool roll_active = !is_zero(roll_rate);
|
||||
bool pitch_active = !is_zero(pitch_rate);
|
||||
bool yaw_active = !is_zero(yaw_rate);
|
||||
|
||||
// integrate target attitude
|
||||
Vector3f r{ float(radians(roll_rate)), float(radians(pitch_rate)), float(radians(yaw_rate)) };
|
||||
r *= plane.G_Dt;
|
||||
q.rotate_fast(r);
|
||||
q.normalize();
|
||||
|
||||
// fill in target roll/pitch for GCS/logs
|
||||
plane.nav_roll_cd = degrees(q.get_euler_roll())*100;
|
||||
plane.nav_pitch_cd = degrees(q.get_euler_pitch())*100;
|
||||
|
||||
// get AHRS attitude
|
||||
Quaternion ahrs_q;
|
||||
IGNORE_RETURN(ahrs.get_quaternion(ahrs_q));
|
||||
|
||||
// zero target if not flying, no stick input and zero throttle
|
||||
if (is_zero(plane.get_throttle_input()) &&
|
||||
!plane.is_flying() &&
|
||||
is_zero(roll_rate) &&
|
||||
is_zero(pitch_rate) &&
|
||||
is_zero(yaw_rate)) {
|
||||
// cope with sitting on the ground with neutral sticks, no throttle
|
||||
q = ahrs_q;
|
||||
}
|
||||
|
||||
// get error in attitude
|
||||
Quaternion error_quat = ahrs_q.inverse() * q;
|
||||
Vector3f error_angle1;
|
||||
error_quat.to_axis_angle(error_angle1);
|
||||
|
||||
// don't let too much error build up, limit to 0.2s
|
||||
const float max_error_t = 0.2;
|
||||
float max_err_roll_rad = radians(plane.g.acro_roll_rate*max_error_t);
|
||||
float max_err_pitch_rad = radians(plane.g.acro_pitch_rate*max_error_t);
|
||||
float max_err_yaw_rad = radians(plane.g.acro_yaw_rate*max_error_t);
|
||||
|
||||
if (!roll_active && acro_state.roll_active_last) {
|
||||
max_err_roll_rad = 0;
|
||||
}
|
||||
if (!pitch_active && acro_state.pitch_active_last) {
|
||||
max_err_pitch_rad = 0;
|
||||
}
|
||||
if (!yaw_active && acro_state.yaw_active_last) {
|
||||
max_err_yaw_rad = 0;
|
||||
}
|
||||
|
||||
Vector3f desired_rates = error_angle1;
|
||||
desired_rates.x = constrain_float(desired_rates.x, -max_err_roll_rad, max_err_roll_rad);
|
||||
desired_rates.y = constrain_float(desired_rates.y, -max_err_pitch_rad, max_err_pitch_rad);
|
||||
desired_rates.z = constrain_float(desired_rates.z, -max_err_yaw_rad, max_err_yaw_rad);
|
||||
|
||||
// correct target based on max error
|
||||
q.rotate_fast(desired_rates - error_angle1);
|
||||
q.normalize();
|
||||
|
||||
// convert to desired body rates
|
||||
desired_rates.x /= plane.rollController.tau();
|
||||
desired_rates.y /= plane.pitchController.tau();
|
||||
desired_rates.z /= plane.pitchController.tau(); // no yaw tau parameter, use pitch
|
||||
|
||||
desired_rates *= degrees(1.0);
|
||||
|
||||
if (roll_active) {
|
||||
desired_rates.x = roll_rate;
|
||||
}
|
||||
if (pitch_active) {
|
||||
desired_rates.y = pitch_rate;
|
||||
}
|
||||
if (yaw_active) {
|
||||
desired_rates.z = yaw_rate;
|
||||
}
|
||||
|
||||
// call to rate controllers
|
||||
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, plane.rollController.get_rate_out(desired_rates.x, speed_scaler));
|
||||
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, plane.pitchController.get_rate_out(desired_rates.y, speed_scaler));
|
||||
plane.steering_control.steering = plane.steering_control.rudder = plane.yawController.get_rate_out(desired_rates.z, speed_scaler, false);
|
||||
|
||||
acro_state.roll_active_last = roll_active;
|
||||
acro_state.pitch_active_last = pitch_active;
|
||||
acro_state.yaw_active_last = yaw_active;
|
||||
}
|
||||
|
||||
/*
|
||||
main stabilization function for all 3 axes
|
||||
*/
|
||||
|
|
|
|||
|
|
@ -24,3 +24,200 @@ void ModeAcro::update()
|
|||
}
|
||||
}
|
||||
|
||||
void ModeAcro::run()
|
||||
{
|
||||
if (plane.g.acro_locking == 2 && plane.g.acro_yaw_rate > 0 &&
|
||||
plane.yawController.rate_control_enabled()) {
|
||||
// we can do 3D acro locking
|
||||
stabilize_quaternion();
|
||||
return;
|
||||
}
|
||||
|
||||
// Normal acro
|
||||
stabilize();
|
||||
}
|
||||
|
||||
/*
|
||||
this is the ACRO mode stabilization function. It does rate
|
||||
stabilization on roll and pitch axes
|
||||
*/
|
||||
void ModeAcro::stabilize()
|
||||
{
|
||||
const float speed_scaler = plane.get_speed_scaler();
|
||||
const float rexpo = plane.roll_in_expo(true);
|
||||
const float pexpo = plane.pitch_in_expo(true);
|
||||
float roll_rate = (rexpo/SERVO_MAX) * plane.g.acro_roll_rate;
|
||||
float pitch_rate = (pexpo/SERVO_MAX) * plane.g.acro_pitch_rate;
|
||||
|
||||
IGNORE_RETURN(ahrs.get_quaternion(acro_state.q));
|
||||
|
||||
/*
|
||||
check for special roll handling near the pitch poles
|
||||
*/
|
||||
if (plane.g.acro_locking && is_zero(roll_rate)) {
|
||||
/*
|
||||
we have no roll stick input, so we will enter "roll locked"
|
||||
mode, and hold the roll we had when the stick was released
|
||||
*/
|
||||
if (!acro_state.locked_roll) {
|
||||
acro_state.locked_roll = true;
|
||||
acro_state.locked_roll_err = 0;
|
||||
} else {
|
||||
acro_state.locked_roll_err += ahrs.get_gyro().x * plane.G_Dt;
|
||||
}
|
||||
int32_t roll_error_cd = -ToDeg(acro_state.locked_roll_err)*100;
|
||||
plane.nav_roll_cd = ahrs.roll_sensor + roll_error_cd;
|
||||
// try to reduce the integrated angular error to zero. We set
|
||||
// 'stabilze' to true, which disables the roll integrator
|
||||
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, plane.rollController.get_servo_out(roll_error_cd,
|
||||
speed_scaler,
|
||||
true, false));
|
||||
} else {
|
||||
/*
|
||||
aileron stick is non-zero, use pure rate control until the
|
||||
user releases the stick
|
||||
*/
|
||||
acro_state.locked_roll = false;
|
||||
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, plane.rollController.get_rate_out(roll_rate, speed_scaler));
|
||||
}
|
||||
|
||||
if (plane.g.acro_locking && is_zero(pitch_rate)) {
|
||||
/*
|
||||
user has zero pitch stick input, so we lock pitch at the
|
||||
point they release the stick
|
||||
*/
|
||||
if (!acro_state.locked_pitch) {
|
||||
acro_state.locked_pitch = true;
|
||||
acro_state.locked_pitch_cd = ahrs.pitch_sensor;
|
||||
}
|
||||
// try to hold the locked pitch. Note that we have the pitch
|
||||
// integrator enabled, which helps with inverted flight
|
||||
plane.nav_pitch_cd = acro_state.locked_pitch_cd;
|
||||
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, plane.pitchController.get_servo_out(plane.nav_pitch_cd - ahrs.pitch_sensor,
|
||||
speed_scaler,
|
||||
false, false));
|
||||
} else {
|
||||
/*
|
||||
user has non-zero pitch input, use a pure rate controller
|
||||
*/
|
||||
acro_state.locked_pitch = false;
|
||||
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, plane.pitchController.get_rate_out(pitch_rate, speed_scaler));
|
||||
}
|
||||
|
||||
plane.steering_control.steering = plane.rudder_input();
|
||||
|
||||
if (plane.g.acro_yaw_rate > 0 && plane.yawController.rate_control_enabled()) {
|
||||
// user has asked for yaw rate control with yaw rate scaled by ACRO_YAW_RATE
|
||||
const float rudd_expo = plane.rudder_in_expo(true);
|
||||
const float yaw_rate = (rudd_expo/SERVO_MAX) * plane.g.acro_yaw_rate;
|
||||
plane.steering_control.steering = plane.steering_control.rudder = plane.yawController.get_rate_out(yaw_rate, speed_scaler, false);
|
||||
} else if (plane.g2.flight_options & FlightOptions::ACRO_YAW_DAMPER) {
|
||||
// use yaw controller
|
||||
plane.calc_nav_yaw_coordinated();
|
||||
} else {
|
||||
/*
|
||||
manual rudder
|
||||
*/
|
||||
plane.steering_control.rudder = plane.steering_control.steering;
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
quaternion based acro stabilization with continuous locking. Enabled with ACRO_LOCKING=2
|
||||
*/
|
||||
void ModeAcro::stabilize_quaternion()
|
||||
{
|
||||
const float speed_scaler = plane.get_speed_scaler();
|
||||
auto &q = acro_state.q;
|
||||
const float rexpo = plane.roll_in_expo(true);
|
||||
const float pexpo = plane.pitch_in_expo(true);
|
||||
const float yexpo = plane.rudder_in_expo(true);
|
||||
|
||||
// get pilot desired rates
|
||||
float roll_rate = (rexpo/SERVO_MAX) * plane.g.acro_roll_rate;
|
||||
float pitch_rate = (pexpo/SERVO_MAX) * plane.g.acro_pitch_rate;
|
||||
float yaw_rate = (yexpo/SERVO_MAX) * plane.g.acro_yaw_rate;
|
||||
bool roll_active = !is_zero(roll_rate);
|
||||
bool pitch_active = !is_zero(pitch_rate);
|
||||
bool yaw_active = !is_zero(yaw_rate);
|
||||
|
||||
// integrate target attitude
|
||||
Vector3f r{ float(radians(roll_rate)), float(radians(pitch_rate)), float(radians(yaw_rate)) };
|
||||
r *= plane.G_Dt;
|
||||
q.rotate_fast(r);
|
||||
q.normalize();
|
||||
|
||||
// fill in target roll/pitch for GCS/logs
|
||||
plane.nav_roll_cd = degrees(q.get_euler_roll())*100;
|
||||
plane.nav_pitch_cd = degrees(q.get_euler_pitch())*100;
|
||||
|
||||
// get AHRS attitude
|
||||
Quaternion ahrs_q;
|
||||
IGNORE_RETURN(ahrs.get_quaternion(ahrs_q));
|
||||
|
||||
// zero target if not flying, no stick input and zero throttle
|
||||
if (is_zero(plane.get_throttle_input()) &&
|
||||
!plane.is_flying() &&
|
||||
is_zero(roll_rate) &&
|
||||
is_zero(pitch_rate) &&
|
||||
is_zero(yaw_rate)) {
|
||||
// cope with sitting on the ground with neutral sticks, no throttle
|
||||
q = ahrs_q;
|
||||
}
|
||||
|
||||
// get error in attitude
|
||||
Quaternion error_quat = ahrs_q.inverse() * q;
|
||||
Vector3f error_angle1;
|
||||
error_quat.to_axis_angle(error_angle1);
|
||||
|
||||
// don't let too much error build up, limit to 0.2s
|
||||
const float max_error_t = 0.2;
|
||||
float max_err_roll_rad = radians(plane.g.acro_roll_rate*max_error_t);
|
||||
float max_err_pitch_rad = radians(plane.g.acro_pitch_rate*max_error_t);
|
||||
float max_err_yaw_rad = radians(plane.g.acro_yaw_rate*max_error_t);
|
||||
|
||||
if (!roll_active && acro_state.roll_active_last) {
|
||||
max_err_roll_rad = 0;
|
||||
}
|
||||
if (!pitch_active && acro_state.pitch_active_last) {
|
||||
max_err_pitch_rad = 0;
|
||||
}
|
||||
if (!yaw_active && acro_state.yaw_active_last) {
|
||||
max_err_yaw_rad = 0;
|
||||
}
|
||||
|
||||
Vector3f desired_rates = error_angle1;
|
||||
desired_rates.x = constrain_float(desired_rates.x, -max_err_roll_rad, max_err_roll_rad);
|
||||
desired_rates.y = constrain_float(desired_rates.y, -max_err_pitch_rad, max_err_pitch_rad);
|
||||
desired_rates.z = constrain_float(desired_rates.z, -max_err_yaw_rad, max_err_yaw_rad);
|
||||
|
||||
// correct target based on max error
|
||||
q.rotate_fast(desired_rates - error_angle1);
|
||||
q.normalize();
|
||||
|
||||
// convert to desired body rates
|
||||
desired_rates.x /= plane.rollController.tau();
|
||||
desired_rates.y /= plane.pitchController.tau();
|
||||
desired_rates.z /= plane.pitchController.tau(); // no yaw tau parameter, use pitch
|
||||
|
||||
desired_rates *= degrees(1.0);
|
||||
|
||||
if (roll_active) {
|
||||
desired_rates.x = roll_rate;
|
||||
}
|
||||
if (pitch_active) {
|
||||
desired_rates.y = pitch_rate;
|
||||
}
|
||||
if (yaw_active) {
|
||||
desired_rates.z = yaw_rate;
|
||||
}
|
||||
|
||||
// call to rate controllers
|
||||
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, plane.rollController.get_rate_out(desired_rates.x, speed_scaler));
|
||||
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, plane.pitchController.get_rate_out(desired_rates.y, speed_scaler));
|
||||
plane.steering_control.steering = plane.steering_control.rudder = plane.yawController.get_rate_out(desired_rates.z, speed_scaler, false);
|
||||
|
||||
acro_state.roll_active_last = roll_active;
|
||||
acro_state.pitch_active_last = pitch_active;
|
||||
acro_state.yaw_active_last = yaw_active;
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue