Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: move acro stabilization into mode acro

Browse Source
This commit is contained in:
Iampete1 2023-02-12 13:45:25 +00:00 committed by Andrew Tridgell
parent 6bb0096b9d
commit 50eaa1cc54
5 changed files with 87 additions and 75 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

113
ArduPlane/Attitude.cpp
View File

@ -369,30 +369,37 @@ void ModeTraining::run()
} }
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 this is the ACRO mode stabilization function. It does rate
stabilization on roll and pitch axes stabilization on roll and pitch axes
*/ */
void Plane::stabilize_acro() void ModeAcro::stabilize()
{ {
if (g.acro_locking == 2 && g.acro_yaw_rate > 0 && const float speed_scaler = plane.get_speed_scaler();
yawController.rate_control_enabled()) { const float rexpo = plane.roll_in_expo(true);
// we can do 3D acro locking const float pexpo = plane.pitch_in_expo(true);
stabilize_acro_quaternion(); float roll_rate = (rexpo/SERVO_MAX) * plane.g.acro_roll_rate;
return; float pitch_rate = (pexpo/SERVO_MAX) * plane.g.acro_pitch_rate;
}
const float speed_scaler = get_speed_scaler();
const float rexpo = roll_in_expo(true);
const float pexpo = pitch_in_expo(true);
float roll_rate = (rexpo/SERVO_MAX) * g.acro_roll_rate;
float pitch_rate = (pexpo/SERVO_MAX) * g.acro_pitch_rate;
IGNORE_RETURN(plane.ahrs.get_quaternion(plane.acro_state.q)); IGNORE_RETURN(ahrs.get_quaternion(acro_state.q));
/* /*
check for special roll handling near the pitch poles check for special roll handling near the pitch poles
*/ */
if (g.acro_locking && is_zero(roll_rate)) { if (plane.g.acro_locking && is_zero(roll_rate)) {
/* /*
we have no roll stick input, so we will enter "roll locked" we have no roll stick input, so we will enter "roll locked"
mode, and hold the roll we had when the stick was released mode, and hold the roll we had when the stick was released
@ -401,13 +408,13 @@ void Plane::stabilize_acro()
acro_state.locked_roll = true; acro_state.locked_roll = true;
acro_state.locked_roll_err = 0; acro_state.locked_roll_err = 0;
} else { } else {
acro_state.locked_roll_err += ahrs.get_gyro().x * G_Dt; acro_state.locked_roll_err += ahrs.get_gyro().x * plane.G_Dt;
} }
int32_t roll_error_cd = -ToDeg(acro_state.locked_roll_err)*100; int32_t roll_error_cd = -ToDeg(acro_state.locked_roll_err)*100;
nav_roll_cd = ahrs.roll_sensor + roll_error_cd; plane.nav_roll_cd = ahrs.roll_sensor + roll_error_cd;
// try to reduce the integrated angular error to zero. We set // try to reduce the integrated angular error to zero. We set
// 'stabilze' to true, which disables the roll integrator // 'stabilze' to true, which disables the roll integrator
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, rollController.get_servo_out(roll_error_cd, SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, plane.rollController.get_servo_out(roll_error_cd,
speed_scaler, speed_scaler,
true, false)); true, false));
} else { } else {
@ -416,10 +423,10 @@ void Plane::stabilize_acro()
user releases the stick user releases the stick
*/ */
acro_state.locked_roll = false; acro_state.locked_roll = false;
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, rollController.get_rate_out(roll_rate, speed_scaler)); SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, plane.rollController.get_rate_out(roll_rate, speed_scaler));
} }
if (g.acro_locking && is_zero(pitch_rate)) { if (plane.g.acro_locking && is_zero(pitch_rate)) {
/* /*
user has zero pitch stick input, so we lock pitch at the user has zero pitch stick input, so we lock pitch at the
point they release the stick point they release the stick
@ -430,8 +437,8 @@ void Plane::stabilize_acro()
} }
// try to hold the locked pitch. Note that we have the pitch // try to hold the locked pitch. Note that we have the pitch
// integrator enabled, which helps with inverted flight // integrator enabled, which helps with inverted flight
nav_pitch_cd = acro_state.locked_pitch_cd; plane.nav_pitch_cd = acro_state.locked_pitch_cd;
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, pitchController.get_servo_out(nav_pitch_cd - ahrs.pitch_sensor, SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, plane.pitchController.get_servo_out(plane.nav_pitch_cd - ahrs.pitch_sensor,
speed_scaler, speed_scaler,
false, false)); false, false));
} else { } else {
@ -439,63 +446,63 @@ void Plane::stabilize_acro()
user has non-zero pitch input, use a pure rate controller user has non-zero pitch input, use a pure rate controller
*/ */
acro_state.locked_pitch = false; acro_state.locked_pitch = false;
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, pitchController.get_rate_out(pitch_rate, speed_scaler)); SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, plane.pitchController.get_rate_out(pitch_rate, speed_scaler));
} }
steering_control.steering = rudder_input(); plane.steering_control.steering = plane.rudder_input();
if (g.acro_yaw_rate > 0 && yawController.rate_control_enabled()) { 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 // user has asked for yaw rate control with yaw rate scaled by ACRO_YAW_RATE
const float rudd_expo = rudder_in_expo(true); const float rudd_expo = plane.rudder_in_expo(true);
const float yaw_rate = (rudd_expo/SERVO_MAX) * g.acro_yaw_rate; const float yaw_rate = (rudd_expo/SERVO_MAX) * plane.g.acro_yaw_rate;
steering_control.steering = steering_control.rudder = yawController.get_rate_out(yaw_rate, speed_scaler, false); 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) { } else if (plane.g2.flight_options & FlightOptions::ACRO_YAW_DAMPER) {
// use yaw controller // use yaw controller
calc_nav_yaw_coordinated(); plane.calc_nav_yaw_coordinated();
} else { } else {
/* /*
manual rudder manual rudder
*/ */
steering_control.rudder = steering_control.steering; plane.steering_control.rudder = plane.steering_control.steering;
} }
} }
/* /*
quaternion based acro stabilization with continuous locking. Enabled with ACRO_LOCKING=2 quaternion based acro stabilization with continuous locking. Enabled with ACRO_LOCKING=2
*/ */
void Plane::stabilize_acro_quaternion() void ModeAcro::stabilize_quaternion()
{ {
const float speed_scaler = get_speed_scaler(); const float speed_scaler = plane.get_speed_scaler();
auto &q = acro_state.q; auto &q = acro_state.q;
const float rexpo = roll_in_expo(true); const float rexpo = plane.roll_in_expo(true);
const float pexpo = pitch_in_expo(true); const float pexpo = plane.pitch_in_expo(true);
const float yexpo = rudder_in_expo(true); const float yexpo = plane.rudder_in_expo(true);
// get pilot desired rates // get pilot desired rates
float roll_rate = (rexpo/SERVO_MAX) * g.acro_roll_rate; float roll_rate = (rexpo/SERVO_MAX) * plane.g.acro_roll_rate;
float pitch_rate = (pexpo/SERVO_MAX) * g.acro_pitch_rate; float pitch_rate = (pexpo/SERVO_MAX) * plane.g.acro_pitch_rate;
float yaw_rate = (yexpo/SERVO_MAX) * g.acro_yaw_rate; float yaw_rate = (yexpo/SERVO_MAX) * plane.g.acro_yaw_rate;
bool roll_active = !is_zero(roll_rate); bool roll_active = !is_zero(roll_rate);
bool pitch_active = !is_zero(pitch_rate); bool pitch_active = !is_zero(pitch_rate);
bool yaw_active = !is_zero(yaw_rate); bool yaw_active = !is_zero(yaw_rate);
// integrate target attitude // integrate target attitude
Vector3f r{ float(radians(roll_rate)), float(radians(pitch_rate)), float(radians(yaw_rate)) }; Vector3f r{ float(radians(roll_rate)), float(radians(pitch_rate)), float(radians(yaw_rate)) };
r *= G_Dt; r *= plane.G_Dt;
q.rotate_fast(r); q.rotate_fast(r);
q.normalize(); q.normalize();
// fill in target roll/pitch for GCS/logs // fill in target roll/pitch for GCS/logs
nav_roll_cd = degrees(q.get_euler_roll())*100; plane.nav_roll_cd = degrees(q.get_euler_roll())*100;
nav_pitch_cd = degrees(q.get_euler_pitch())*100; plane.nav_pitch_cd = degrees(q.get_euler_pitch())*100;
// get AHRS attitude // get AHRS attitude
Quaternion ahrs_q; Quaternion ahrs_q;
IGNORE_RETURN(ahrs.get_quaternion(ahrs_q)); IGNORE_RETURN(ahrs.get_quaternion(ahrs_q));
// zero target if not flying, no stick input and zero throttle // zero target if not flying, no stick input and zero throttle
if (is_zero(get_throttle_input()) && if (is_zero(plane.get_throttle_input()) &&
!is_flying() && !plane.is_flying() &&
is_zero(roll_rate) && is_zero(roll_rate) &&
is_zero(pitch_rate) && is_zero(pitch_rate) &&
is_zero(yaw_rate)) { is_zero(yaw_rate)) {
@ -510,9 +517,9 @@ void Plane::stabilize_acro_quaternion()
// don't let too much error build up, limit to 0.2s // don't let too much error build up, limit to 0.2s
const float max_error_t = 0.2; const float max_error_t = 0.2;
float max_err_roll_rad = radians(g.acro_roll_rate*max_error_t); float max_err_roll_rad = radians(plane.g.acro_roll_rate*max_error_t);
float max_err_pitch_rad = radians(g.acro_pitch_rate*max_error_t); float max_err_pitch_rad = radians(plane.g.acro_pitch_rate*max_error_t);
float max_err_yaw_rad = radians(g.acro_yaw_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) { if (!roll_active && acro_state.roll_active_last) {
max_err_roll_rad = 0; max_err_roll_rad = 0;
@ -534,9 +541,9 @@ void Plane::stabilize_acro_quaternion()
q.normalize(); q.normalize();
// convert to desired body rates // convert to desired body rates
desired_rates.x /= rollController.tau(); desired_rates.x /= plane.rollController.tau();
desired_rates.y /= pitchController.tau(); desired_rates.y /= plane.pitchController.tau();
desired_rates.z /= pitchController.tau(); // no yaw tau parameter, use pitch desired_rates.z /= plane.pitchController.tau(); // no yaw tau parameter, use pitch
desired_rates *= degrees(1.0); desired_rates *= degrees(1.0);
@ -551,9 +558,9 @@ void Plane::stabilize_acro_quaternion()
} }
// call to rate controllers // call to rate controllers
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, rollController.get_rate_out(desired_rates.x, speed_scaler)); 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, pitchController.get_rate_out(desired_rates.y, speed_scaler)); SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, plane.pitchController.get_rate_out(desired_rates.y, speed_scaler));
steering_control.steering = steering_control.rudder = yawController.get_rate_out(desired_rates.z, speed_scaler, false); 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.roll_active_last = roll_active;
acro_state.pitch_active_last = pitch_active; acro_state.pitch_active_last = pitch_active;
@ -614,7 +621,7 @@ void Plane::stabilize()
} }
#endif #endif
} else if (control_mode == &mode_acro) { } else if (control_mode == &mode_acro) {
stabilize_acro(); plane.control_mode->run();
} else if (control_mode == &mode_stabilize) { } else if (control_mode == &mode_stabilize) {
stabilize_roll(); stabilize_roll();
stabilize_pitch(); stabilize_pitch();
@ -629,7 +636,7 @@ void Plane::stabilize()
// we also stabilize using fixed wing surfaces // we also stabilize using fixed wing surfaces
if (plane.control_mode->mode_number() == Mode::Number::QACRO) { if (plane.control_mode->mode_number() == Mode::Number::QACRO) {
stabilize_acro(); plane.mode_acro.run();
} else { } else {
stabilize_roll(); stabilize_roll();
stabilize_pitch(); stabilize_pitch();

14
ArduPlane/Plane.h
View File

@ -392,18 +392,6 @@ private:
FUNCTOR_BIND_MEMBER(&Plane::handle_battery_failsafe, void, const char*, const int8_t), FUNCTOR_BIND_MEMBER(&Plane::handle_battery_failsafe, void, const char*, const int8_t),
_failsafe_priorities}; _failsafe_priorities};
// ACRO controller state
struct {
bool locked_roll;
bool locked_pitch;
float locked_roll_err;
int32_t locked_pitch_cd;
Quaternion q;
bool roll_active_last;
bool pitch_active_last;
bool yaw_active_last;
} acro_state;
struct { struct {
uint32_t last_tkoff_arm_time; uint32_t last_tkoff_arm_time;
uint32_t last_check_ms; uint32_t last_check_ms;
@ -868,8 +856,6 @@ private:
void stabilize_stick_mixing_direct(); void stabilize_stick_mixing_direct();
void stabilize_stick_mixing_fbw(); void stabilize_stick_mixing_fbw();
void stabilize_yaw(); void stabilize_yaw();
void stabilize_acro();
void stabilize_acro_quaternion();
void calc_nav_yaw_coordinated(); void calc_nav_yaw_coordinated();
void calc_nav_yaw_course(void); void calc_nav_yaw_course(void);
void calc_nav_yaw_ground(void); void calc_nav_yaw_ground(void);

19
ArduPlane/mode.h
View File

@ -150,6 +150,7 @@ protected:
class ModeAcro : public Mode class ModeAcro : public Mode
{ {
friend class ModeQAcro;
public: public:
Mode::Number mode_number() const override { return Mode::Number::ACRO; } Mode::Number mode_number() const override { return Mode::Number::ACRO; }
@ -159,8 +160,26 @@ public:
// methods that affect movement of the vehicle in this mode // methods that affect movement of the vehicle in this mode
void update() override; void update() override;
void run() override;
void stabilize();
void stabilize_quaternion();
protected: protected:
// ACRO controller state
struct {
bool locked_roll;
bool locked_pitch;
float locked_roll_err;
int32_t locked_pitch_cd;
Quaternion q;
bool roll_active_last;
bool pitch_active_last;
bool yaw_active_last;
} acro_state;
bool _enter() override; bool _enter() override;
}; };

14
ArduPlane/mode_acro.cpp
View File

@ -3,22 +3,22 @@
bool ModeAcro::_enter() bool ModeAcro::_enter()
{ {
plane.acro_state.locked_roll = false; acro_state.locked_roll = false;
plane.acro_state.locked_pitch = false; acro_state.locked_pitch = false;
IGNORE_RETURN(ahrs.get_quaternion(plane.acro_state.q)); IGNORE_RETURN(ahrs.get_quaternion(acro_state.q));
return true; return true;
} }
void ModeAcro::update() void ModeAcro::update()
{ {
// handle locked/unlocked control // handle locked/unlocked control
if (plane.acro_state.locked_roll) { if (acro_state.locked_roll) {
plane.nav_roll_cd = plane.acro_state.locked_roll_err; plane.nav_roll_cd = acro_state.locked_roll_err;
} else { } else {
plane.nav_roll_cd = ahrs.roll_sensor; plane.nav_roll_cd = ahrs.roll_sensor;
} }
if (plane.acro_state.locked_pitch) { if (acro_state.locked_pitch) {
plane.nav_pitch_cd = plane.acro_state.locked_pitch_cd; plane.nav_pitch_cd = acro_state.locked_pitch_cd;
} else { } else {
plane.nav_pitch_cd = ahrs.pitch_sensor; plane.nav_pitch_cd = ahrs.pitch_sensor;
} }

2
ArduPlane/mode_qacro.cpp
View File

@ -12,7 +12,7 @@ bool ModeQAcro::_enter()
// disable yaw rate time contant to mantain old behaviour // disable yaw rate time contant to mantain old behaviour
quadplane.disable_yaw_rate_time_constant(); quadplane.disable_yaw_rate_time_constant();
IGNORE_RETURN(ahrs.get_quaternion(plane.acro_state.q)); IGNORE_RETURN(ahrs.get_quaternion(plane.mode_acro.acro_state.q));
return true; return true;
} }