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

AC_AttitudeControl: extensive renaming and recommenting

Browse Source
This commit is contained in:
Jonathan Challinger 2015-11-24 19:28:42 -08:00 committed by Randy Mackay
parent b906767a45
commit 1afab89991
3 changed files with 511 additions and 660 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

794
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

File diff suppressed because it is too large Load Diff

299
libraries/AC_AttitudeControl/AC_AttitudeControl.h
View File

@ -15,16 +15,17 @@
#include <AC_PID/AC_PID.h>
#include <AC_PID/AC_P.h>
// To-Do: change the name or move to AP_Math?
#define AC_ATTITUDE_CONTROL_DEGX100 5729.57795f // constant to convert from radians to centi-degrees
// TODO: change the name or move to AP_Math? eliminate in favor of degrees(100)?
#define AC_ATTITUDE_CONTROL_DEGX100 5729.57795f // constant to convert from radians to centidegrees
#define AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN_RADSS radians(360.0f) // minimum body-frame acceleration limit for the stability controller (for roll and pitch axis)
#define AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX_RADSS radians(720.0f) // maximum body-frame acceleration limit for the stability controller (for roll and pitch axis)
#define AC_ATTITUDE_ACCEL_Y_CONTROLLER_MIN_RADSS radians(90.0f) // minimum body-frame acceleration limit for the stability controller (for yaw axis)
#define AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS radians(360.0f) // maximum body-frame acceleration limit for the stability controller (for yaw axis)
// BUG: SLEW_YAW's behavior does not match its parameter documentation
#define AC_ATTITUDE_CONTROL_SLEW_YAW_DEFAULT_CDS 1000 // constraint on yaw angle error in degrees. This should lead to maximum turn rate of 10deg/sed * Stab Rate P so by default will be 45deg/sec.
#define AC_ATTITUDE_CONTROL_ACCEL_RP_MAX_DEFAULT_CDSS 110000.0f // default maximum acceleration for roll/pitch axis in centi-degrees/sec/sec
#define AC_ATTITUDE_CONTROL_ACCEL_Y_MAX_DEFAULT_CDSS 27000.0f // default maximum acceleration for yaw axis in centi-degrees/sec/sec
#define AC_ATTITUDE_CONTROL_ACCEL_RP_MAX_DEFAULT_CDSS 110000.0f // default maximum acceleration for roll/pitch axis in centidegrees/sec/sec
#define AC_ATTITUDE_CONTROL_ACCEL_Y_MAX_DEFAULT_CDSS 27000.0f // default maximum acceleration for yaw axis in centidegrees/sec/sec
#define AC_ATTITUDE_RATE_CONTROLLER_TIMEOUT 1.0f // body-frame rate controller timeout in seconds
#define AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX 5000.0f // body-frame rate controller maximum output (for roll-pitch axis)
@ -68,222 +69,255 @@ public:
{
AP_Param::setup_object_defaults(this, var_info);
// initialise flags
_flags.limit_angle_to_rate_request = true;
_att_ctrl_use_accel_limit = true;
}
// empty destructor to suppress compiler warning
// Empty destructor to suppress compiler warning
virtual ~AC_AttitudeControl() {}
//
// initialisation functions
//
// set_dt - sets time delta in seconds for all controllers (i.e. 100hz = 0.01, 400hz = 0.0025)
// Set_dt - sets time delta in seconds for all controllers (i.e. 100hz = 0.01, 400hz = 0.0025)
void set_dt(float delta_sec);
// get_accel_roll_max - gets the roll acceleration limit in centidegrees/s/s
// Gets the roll acceleration limit in centidegrees/s/s
float get_accel_roll_max() { return _accel_roll_max; }
// set_accel_roll_max - sets the roll acceleration limit in centidegrees/s/s
// Sets the roll acceleration limit in centidegrees/s/s
void set_accel_roll_max(float accel_roll_max) { _accel_roll_max = accel_roll_max; }
// save_accel_roll_max - sets and saves the roll acceleration limit in centidegrees/s/s
// Sets and saves the roll acceleration limit in centidegrees/s/s
void save_accel_roll_max(float accel_roll_max) { _accel_roll_max = accel_roll_max; _accel_roll_max.save(); }
// get_accel_pitch_max - gets the pitch acceleration limit in centidegrees/s/s
// Sets the pitch acceleration limit in centidegrees/s/s
float get_accel_pitch_max() { return _accel_pitch_max; }
// set_accel_pitch_max - sets the pitch acceleration limit in centidegrees/s/s
// Sets the pitch acceleration limit in centidegrees/s/s
void set_accel_pitch_max(float accel_pitch_max) { _accel_pitch_max = accel_pitch_max; }
// save_accel_pitch_max - sets and saves the pitch acceleration limit in centidegrees/s/s
// Sets and saves the pitch acceleration limit in centidegrees/s/s
void save_accel_pitch_max(float accel_pitch_max) { _accel_pitch_max = accel_pitch_max; _accel_pitch_max.save(); }
// get_accel_yaw_max - gets the yaw acceleration limit in centidegrees/s/s
// Gets the yaw acceleration limit in centidegrees/s/s
float get_accel_yaw_max() { return _accel_yaw_max; }
// set_accel_yaw_max - sets the yaw acceleration limit in centidegrees/s/s
// Sets the yaw acceleration limit in centidegrees/s/s
void set_accel_yaw_max(float accel_yaw_max) { _accel_yaw_max = accel_yaw_max; }
// save_accel_yaw_max - sets and saves the yaw acceleration limit in centidegrees/s/s
// Sets and saves the yaw acceleration limit in centidegrees/s/s
void save_accel_yaw_max(float accel_yaw_max) { _accel_yaw_max = accel_yaw_max; _accel_yaw_max.save(); }
// relax_bf_rate_controller - ensure body-frame rate controller has zero errors to relax rate controller output
// Ensure body-frame rate controller has zero errors to relax rate controller output
void relax_bf_rate_controller();
// set_yaw_target_to_current_heading - sets yaw target to current heading
void set_yaw_target_to_current_heading() { _angle_ef_target_rad.z = _ahrs.yaw; }
// Sets yaw target to vehicle heading
void set_yaw_target_to_current_heading() { _att_target_euler_rad.z = _ahrs.yaw; }
// shifts earth frame yaw target by yaw_shift_cd. yaw_shift_cd should be in centi-degrees and is added to the current target heading
// Shifts earth frame yaw target by yaw_shift_cd. yaw_shift_cd should be in centidegrees and is added to the current target heading
void shift_ef_yaw_target(float yaw_shift_cd);
//
// methods to be called by upper controllers to request and implement a desired attitude
//
// Command an euler roll and pitch angle and an euler yaw rate with angular velocity feedforward and smoothing
void euler_angle_roll_pitch_euler_rate_yaw_smooth(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds, float smoothing_gain);
// angle_ef_roll_pitch_rate_ef_yaw_smooth - attempts to maintain a roll and pitch angle and yaw rate (all earth frame) while smoothing the attitude based on the feel parameter
// smoothing_gain : a number from 1 to 50 with 1 being sluggish and 50 being very crisp
void angle_ef_roll_pitch_rate_ef_yaw_smooth(float roll_angle_ef_cd, float pitch_angle_ef_cd, float yaw_rate_ef_cds, float smoothing_gain);
// Command an euler roll and pitch angle and an euler yaw rate
void euler_angle_roll_pitch_euler_rate_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds);
// angle_ef_roll_pitch_rate_ef_yaw - attempts to maintain a roll and pitch angle and yaw rate (all earth frame)
void angle_ef_roll_pitch_rate_ef_yaw(float roll_angle_ef_cd, float pitch_angle_ef_cd, float yaw_rate_ef_cds);
// Command an euler roll, pitch and yaw angle
void euler_angle_roll_pitch_yaw(float euler_roll_angle_cd, float euler_pitch_angle_ef_cd, float euler_yaw_angle_cd, bool slew_yaw);
// angle_ef_roll_pitch_yaw - attempts to maintain a roll, pitch and yaw angle (all earth frame)
// if yaw_slew is true then target yaw movement will be gradually moved to the new target based on the YAW_SLEW parameter
void angle_ef_roll_pitch_yaw(float roll_angle_ef_cd, float pitch_angle_ef_cd, float yaw_angle_ef_cd, bool slew_yaw);
// Command an euler roll, pitch, and yaw rate
void euler_rate_roll_pitch_yaw(float euler_roll_rate_cds, float euler_pitch_rate_cds, float euler_yaw_rate_cds);
// rate_ef_roll_pitch_yaw - attempts to maintain a roll, pitch and yaw rate (all earth frame)
void rate_ef_roll_pitch_yaw(float roll_rate_ef_cds, float pitch_rate_ef_cds, float yaw_rate_ef_cds);
// rate_bf_roll_pitch_yaw - attempts to maintain a roll, pitch and yaw rate (all body frame)
// Command an angular velocity
virtual void rate_bf_roll_pitch_yaw(float roll_rate_bf_cds, float pitch_rate_bf_cds, float yaw_rate_bf_cds);
//
// rate_controller_run - run lowest level body-frame rate controller and send outputs to the motors
// should be called at 100hz or more
//
// Run angular velocity controller and send outputs to the motors
virtual void rate_controller_run();
// converts a 321-intrinsic euler angle derivative to an angular velocity vector
// Convert a 321-intrinsic euler angle derivative to an angular velocity vector
void euler_derivative_to_ang_vel(const Vector3f& euler_rad, const Vector3f& euler_dot_rads, Vector3f& ang_vel_rads);
// converts an angular velocity vector to a 321-intrinsic euler angle derivative
// returns false if the vehicle is pitched all the way up or all the way down
// Convert an angular velocity vector to a 321-intrinsic euler angle derivative
// Returns false if the vehicle is pitched 90 degrees up or down
bool ang_vel_to_euler_derivative(const Vector3f& euler_rad, const Vector3f& ang_vel_rads, Vector3f& euler_dot_rads);
//
// public accessor functions
//
// Configures whether the attitude controller should limit the rate demand to constrain angular acceleration
void limit_angle_to_rate_request(bool limit_request) { _att_ctrl_use_accel_limit = limit_request; }
// limit_angle_to_rate_request
void limit_angle_to_rate_request(bool limit_request) { _flags.limit_angle_to_rate_request = limit_request; }
// Return 321-intrinsic euler angles in centidegrees representing the rotation from NED earth frame to the
// attitude controller's reference attitude.
Vector3f angle_ef_targets() const { return _att_target_euler_rad*degrees(100.0f); }
// angle_ef_targets - returns angle controller earth-frame targets (for reporting)
// convert from radians to centi-degrees on public interface
Vector3f angle_ef_targets() const { return _angle_ef_target_rad*degrees(100.0f); }
// Return a rotation vector in centidegrees representing the rotation from vehicle body frame to the
// attitude controller's reference attitude.
Vector3f angle_bf_error() const { return _att_error_rot_vec_rad*degrees(100.0f); }
// angle_bf_error - returns angle controller body-frame errors (for stability checking)
// convert from radians to centi-degrees on public interface
Vector3f angle_bf_error() const { return _angle_bf_error_rad*degrees(100.0f); }
// Set x-axis angular velocity reference in centidegrees/s
void rate_bf_roll_target(float rate_cds) { _ang_vel_target_rads.x = radians(rate_cds*0.01f); }
// rate_bf_targets - sets rate controller body-frame targets
// convert from centi-degrees on public interface to radians
void rate_bf_roll_target(float rate_cds) { _rate_bf_target_rads.x = radians(rate_cds*0.01f); }
void rate_bf_pitch_target(float rate_cds) { _rate_bf_target_rads.y = radians(rate_cds*0.01f); }
void rate_bf_yaw_target(float rate_cds) { _rate_bf_target_rads.z = radians(rate_cds*0.01f); }
// Set y-axis angular velocity reference in centidegrees/s
void rate_bf_pitch_target(float rate_cds) { _ang_vel_target_rads.y = radians(rate_cds*0.01f); }
// Maximum roll rate step size in centi-degrees/s that results in maximum output after 4 time steps
// Set z-axis angular velocity reference in centidegrees/s
void rate_bf_yaw_target(float rate_cds) { _ang_vel_target_rads.z = radians(rate_cds*0.01f); }
// Return roll rate step size in centidegrees/s that results in maximum output after 4 time steps
float max_rate_step_bf_roll();
// Maximum pitch rate step size in centi-degrees/s that results in maximum output after 4 time steps
// Return pitch rate step size in centidegrees/s that results in maximum output after 4 time steps
float max_rate_step_bf_pitch();
// Maximum yaw rate step size in centi-degrees/s that results in maximum output after 4 time steps
// Return yaw rate step size in centidegrees/s that results in maximum output after 4 time steps
float max_rate_step_bf_yaw();
// Maximum roll step size in centi-degrees that results in maximum output after 4 time steps
// Return roll step size in centidegrees that results in maximum output after 4 time steps
float max_angle_step_bf_roll() { return max_rate_step_bf_roll()/_p_angle_roll.kP(); }
// Maximum pitch step size in centi-degrees that results in maximum output after 4 time steps
// Return pitch step size in centidegrees that results in maximum output after 4 time steps
float max_angle_step_bf_pitch() { return max_rate_step_bf_pitch()/_p_angle_pitch.kP(); }
// Maximum yaw step size in centi-degrees that results in maximum output after 4 time steps
// Return yaw step size in centidegrees that results in maximum output after 4 time steps
float max_angle_step_bf_yaw() { return max_rate_step_bf_yaw()/_p_angle_yaw.kP(); }
// rate_ef_targets - returns rate controller body-frame targets (for reporting)
// convert from radians/s to centi-degrees/s on public interface
Vector3f rate_bf_targets() const { return _rate_bf_target_rads*degrees(100.0f); }
// Return reference angular velocity used in the angular velocity controller
Vector3f rate_bf_targets() const { return _ang_vel_target_rads*degrees(100.0f); }
// bf_feedforward - enable or disable body-frame feed forward
// Enable or disable body-frame feed forward
void bf_feedforward(bool enable_or_disable) { _rate_bf_ff_enabled = enable_or_disable; }
// bf_feedforward - enable or disable body-frame feed forward and save
// Enable or disable body-frame feed forward and save
void bf_feedforward_save(bool enable_or_disable) { _rate_bf_ff_enabled.set_and_save(enable_or_disable); }
// get_bf_feedforward - return body-frame feed forward setting
// Return body-frame feed forward setting
bool get_bf_feedforward() { return _rate_bf_ff_enabled; }
// enable_bf_feedforward - enable or disable body-frame feed forward
// Enable or disable body-frame feed forward
void accel_limiting(bool enable_or_disable);
//
// throttle functions
//
// set_throttle_out - to be called by upper throttle controllers when they wish to provide throttle output directly to motors
// Set output throttle
void set_throttle_out(float throttle_in, bool apply_angle_boost, float filt_cutoff);
// outputs a throttle to all motors evenly with no stabilization
// Set output throttle and disable stabilization
void set_throttle_out_unstabilized(float throttle_in, bool reset_attitude_control, float filt_cutoff);
// angle_boost - accessor for angle boost so it can be logged
// Return throttle increase applied for tilt compensation
int16_t angle_boost() const { return _angle_boost; }
// get lean angle max for pilot input that prioritises altitude hold over lean angle
// Return tilt angle limit for pilot input that prioritises altitude hold over lean angle
virtual float get_althold_lean_angle_max() const = 0;
//
// helper functions
//
// lean_angle_max - maximum lean angle of the copter in centi-degrees
// Return configured tilt angle limit in centidegrees/s
int16_t lean_angle_max() const { return _aparm.angle_max; }
// sqrt_controller - response based on the sqrt of the error instead of the more common linear response
// Proportional controller with piecewise sqrt sections to constrain second derivative
static float sqrt_controller(float error, float p, float second_ord_lim);
// user settable parameters
// User settable parameters
static const struct AP_Param::GroupInfo var_info[];
protected:
// Update _att_target_euler_rad.x by integrating a 321-intrinsic euler roll angle derivative
void update_att_target_and_error_roll(float roll_rate_ef_rads, Vector3f &angle_ef_error_rad, float overshoot_max_rad);
// attitude control flags
struct AttControlFlags {
uint8_t limit_angle_to_rate_request : 1; // 1 if the earth frame angle controller is limiting it's maximum rate request
} _flags;
// Update _att_target_euler_rad.y by integrating a 321-intrinsic euler pitch angle derivative
void update_att_target_and_error_pitch(float pitch_rate_ef_rads, Vector3f &angle_ef_error_rad, float overshoot_max_rad);
// update_ef_roll_angle_and_error - update _angle_ef_target.x using an earth frame roll rate request
void update_ef_roll_angle_and_error(float roll_rate_ef_rads, Vector3f &angle_ef_error_rad, float overshoot_max_rad);
// Update _att_target_euler_rad.z by integrating a 321-intrinsic euler yaw angle derivative
void update_att_target_and_error_yaw(float yaw_rate_ef_rads, Vector3f &angle_ef_error_rad, float overshoot_max_rad);
// update_ef_pitch_angle_and_error - update _angle_ef_target.y using an earth frame pitch rate request
void update_ef_pitch_angle_and_error(float pitch_rate_ef_rads, Vector3f &angle_ef_error_rad, float overshoot_max_rad);
// update_ef_yaw_angle_and_error - update _angle_ef_target.z using an earth frame yaw rate request
void update_ef_yaw_angle_and_error(float yaw_rate_ef_rads, Vector3f &angle_ef_error_rad, float overshoot_max_rad);
// integrate_bf_rate_error_to_angle_errors - calculates body frame angle errors
// body-frame feed forward rates (radians/s) taken from _angle_bf_error
// angle errors in radians placed in _angle_bf_error
// Integrate vehicle rate into _att_error_rot_vec_rad
void integrate_bf_rate_error_to_angle_errors();
// update_rate_bf_targets - converts body-frame angle error to body-frame rate targets for roll, pitch and yaw axis
// targets rates in radians taken from _angle_bf_error
// results in radians/s put into _rate_bf_target
void update_rate_bf_targets();
// Update _ang_vel_target_rads using _att_error_rot_vec_rad
void update_ang_vel_target_from_att_error();
//
// body-frame rate controller
//
// rate_bf_to_motor_roll - ask the rate controller to calculate the motor outputs to achieve the target body-frame rate (in radians/s) for roll, pitch and yaw
// Run the roll angular velocity PID controller and return the output
float rate_bf_to_motor_roll(float rate_target_rads);
// Run the pitch angular velocity PID controller and return the output
float rate_bf_to_motor_pitch(float rate_target_rads);
// Run the yaw angular velocity PID controller and return the output
virtual float rate_bf_to_motor_yaw(float rate_target_rads);
//
// throttle methods
//
// calculate total body frame throttle required to produce the given earth frame throttle
// Compute a throttle value that is adjusted for the tilt angle of the vehicle
virtual float get_boosted_throttle(float throttle_in) = 0;
// get_roll_trim - angle in centi-degrees to be added to roll angle. Used by helicopter to counter tail rotor thrust in hover
// Overloaded by AC_Attitude_Heli to return angle. Should be left to return zero for multirotors.
// Return angle in radians to be added to roll angle. Used by heli to counteract
// tail rotor thrust in hover. Overloaded by AC_Attitude_Heli to return angle.
virtual int16_t get_roll_trim_rad() { return 0;}
// Return the roll axis acceleration limit in radians/s/s
float get_accel_roll_max_radss() { return radians(_accel_roll_max*0.01f); }
// Return the pitch axis acceleration limit in radians/s/s
float get_accel_pitch_max_radss() { return radians(_accel_pitch_max*0.01f); }
// Return the yaw axis acceleration limit in radians/s/s
float get_accel_yaw_max_radss() { return radians(_accel_yaw_max*0.01f); }
// Return the yaw slew rate limit in radians/s
float get_slew_yaw_rads() { return radians(_slew_yaw*0.01f); }
// Return the tilt angle limit in radians
float get_tilt_limit_rad() { return radians(_aparm.angle_max*0.01f); }
// references to external libraries
// Maximum rate the yaw target can be updated in Loiter, RTL, Auto flight modes
// BUG: SLEW_YAW's behavior does not match its parameter documentation
AP_Float _slew_yaw;
// Maximum rotation acceleration for earth-frame roll axis
AP_Float _accel_roll_max;
// Maximum rotation acceleration for earth-frame pitch axis
AP_Float _accel_pitch_max;
// Maximum rotation acceleration for earth-frame yaw axis
AP_Float _accel_yaw_max;
// Enable/Disable body frame rate feed forward
AP_Int8 _rate_bf_ff_enabled;
// Intersampling period in seconds
float _dt;
// This represents a 321-intrinsic rotation from NED frame to the reference
// attitude used in the attitude controller. Formerly _angle_ef_target.
Vector3f _att_target_euler_rad;
// This represents an euler axis-angle rotation vector from the vehicle’s
// estimated attitude to the reference attitude used in the attitude
// controller. Formerly _angle_bf_error.
Vector3f _att_error_rot_vec_rad;
// This represents the angular velocity of the reference attitude used in
// the attitude controller as 321-intrinsic euler angle derivatives.
// Formerly _rate_ef_desired.
Vector3f _att_target_euler_deriv_rads;
// This represents the angular velocity of the reference attitude used in
// the attitude controller as an angular velocity vector in the reference
// attitude frame. Formerly _rate_bf_desired.
Vector3f _att_target_ang_vel_rads;
// This represents the reference angular velocity used in the angular
// velocity controller. Formerly _rate_bf_target.
Vector3f _ang_vel_target_rads;
// This represents the throttle increase applied for tilt compensation.
// Used only for logging.
int16_t _angle_boost;
// This provides hysteresis on the attitude controller switching used to account
// for deficiencies in the euler angle based attitude control.
float _acro_angle_switch_rad;
// Specifies whether the attitude controller should use the acceleration limit
bool _att_ctrl_use_accel_limit;
// Filtered throttle input - used to limit lean angle when throttle is saturated
LowPassFilterFloat _throttle_in_filt;
// References to external libraries
const AP_AHRS& _ahrs;
const AP_Vehicle::MultiCopter &_aparm;
AP_Motors& _motors;
@ -293,27 +327,6 @@ protected:
AC_PID& _pid_rate_roll;
AC_PID& _pid_rate_pitch;
AC_PID& _pid_rate_yaw;
// parameters
AP_Float _slew_yaw; // maximum rate the yaw target can be updated in Loiter, RTL, Auto flight modes
AP_Float _accel_roll_max; // maximum rotation acceleration for earth-frame roll axis
AP_Float _accel_pitch_max; // maximum rotation acceleration for earth-frame pitch axis
AP_Float _accel_yaw_max; // maximum rotation acceleration for earth-frame yaw axis
AP_Int8 _rate_bf_ff_enabled; // Enable/Disable body frame rate feed forward
// internal variables
// To-Do: make rate targets a typedef instead of Vector3f?
float _dt; // time delta in seconds
Vector3f _angle_ef_target_rad; // angle controller earth-frame targets
Vector3f _angle_bf_error_rad; // angle controller body-frame error
Vector3f _rate_bf_target_rads; // rate controller body-frame targets
Vector3f _rate_ef_desired_rads; // earth-frame feed forward rates
Vector3f _rate_bf_desired_rads; // body-frame feed forward rates
int16_t _angle_boost; // used only for logging
float _acro_angle_switch_rad;
// throttle based angle limits
LowPassFilterFloat _throttle_in_filt; // throttle input from pilot or alt hold controller
};
#define AC_ATTITUDE_CONTROL_LOG_FORMAT(msg) { msg, sizeof(AC_AttitudeControl::log_Attitude), \

50
libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp
View File

@ -42,55 +42,55 @@ void AC_AttitudeControl_Heli::passthrough_bf_roll_pitch_rate_yaw(float roll_pass
_flags_heli.flybar_passthrough = true;
// set bf rate targets to current body frame rates (i.e. relax and be ready for vehicle to switch out of acro)
_rate_bf_desired_rads.x = _ahrs.get_gyro().x;
_rate_bf_desired_rads.y = _ahrs.get_gyro().y;
_att_target_ang_vel_rads.x = _ahrs.get_gyro().x;
_att_target_ang_vel_rads.y = _ahrs.get_gyro().y;
// accel limit desired yaw rate
if (get_accel_yaw_max_radss() > 0.0f) {
float rate_change_limit_rads = get_accel_yaw_max_radss() * _dt;
float rate_change_rads = yaw_rate_bf_rads - _rate_bf_desired_rads.z;
float rate_change_rads = yaw_rate_bf_rads - _att_target_ang_vel_rads.z;
rate_change_rads = constrain_float(rate_change_rads, -rate_change_limit_rads, rate_change_limit_rads);
_rate_bf_desired_rads.z += rate_change_rads;
_att_target_ang_vel_rads.z += rate_change_rads;
} else {
_rate_bf_desired_rads.z = yaw_rate_bf_rads;
_att_target_ang_vel_rads.z = yaw_rate_bf_rads;
}
integrate_bf_rate_error_to_angle_errors();
_angle_bf_error_rad.x = 0;
_angle_bf_error_rad.y = 0;
_att_error_rot_vec_rad.x = 0;
_att_error_rot_vec_rad.y = 0;
// update our earth-frame angle targets
Vector3f angle_ef_error_rad;
// convert angle error rotation vector into 321-intrinsic euler angle difference
// NOTE: this results an an approximation linearized about the vehicle's attitude
if (ang_vel_to_euler_derivative(Vector3f(_ahrs.roll,_ahrs.pitch,_ahrs.yaw), _angle_bf_error_rad, angle_ef_error_rad)) {
_angle_ef_target_rad.x = wrap_PI(angle_ef_error_rad.x + _ahrs.roll);
_angle_ef_target_rad.y = wrap_PI(angle_ef_error_rad.y + _ahrs.pitch);
_angle_ef_target_rad.z = wrap_2PI(angle_ef_error_rad.z + _ahrs.yaw);
if (ang_vel_to_euler_derivative(Vector3f(_ahrs.roll,_ahrs.pitch,_ahrs.yaw), _att_error_rot_vec_rad, angle_ef_error_rad)) {
_att_target_euler_rad.x = wrap_PI(angle_ef_error_rad.x + _ahrs.roll);
_att_target_euler_rad.y = wrap_PI(angle_ef_error_rad.y + _ahrs.pitch);
_att_target_euler_rad.z = wrap_2PI(angle_ef_error_rad.z + _ahrs.yaw);
}
// handle flipping over pitch axis
if (_angle_ef_target_rad.y > M_PI/2.0f) {
_angle_ef_target_rad.x = wrap_PI(_angle_ef_target_rad.x + M_PI);
_angle_ef_target_rad.y = wrap_PI(M_PI - _angle_ef_target_rad.x);
_angle_ef_target_rad.z = wrap_2PI(_angle_ef_target_rad.z + M_PI);
if (_att_target_euler_rad.y > M_PI/2.0f) {
_att_target_euler_rad.x = wrap_PI(_att_target_euler_rad.x + M_PI);
_att_target_euler_rad.y = wrap_PI(M_PI - _att_target_euler_rad.x);
_att_target_euler_rad.z = wrap_2PI(_att_target_euler_rad.z + M_PI);
}
if (_angle_ef_target_rad.y < -M_PI/2.0f) {
_angle_ef_target_rad.x = wrap_PI(_angle_ef_target_rad.x + M_PI);
_angle_ef_target_rad.y = wrap_PI(-M_PI - _angle_ef_target_rad.x);
_angle_ef_target_rad.z = wrap_2PI(_angle_ef_target_rad.z + M_PI);
if (_att_target_euler_rad.y < -M_PI/2.0f) {
_att_target_euler_rad.x = wrap_PI(_att_target_euler_rad.x + M_PI);
_att_target_euler_rad.y = wrap_PI(-M_PI - _att_target_euler_rad.x);
_att_target_euler_rad.z = wrap_2PI(_att_target_euler_rad.z + M_PI);
}
// convert body-frame angle errors to body-frame rate targets
update_rate_bf_targets();
update_ang_vel_target_from_att_error();
// set body-frame roll/pitch rate target to current desired rates which are the vehicle's actual rates
_rate_bf_target_rads.x = _rate_bf_desired_rads.x;
_rate_bf_target_rads.y = _rate_bf_desired_rads.y;
_ang_vel_target_rads.x = _att_target_ang_vel_rads.x;
_ang_vel_target_rads.y = _att_target_ang_vel_rads.y;
// add desired target to yaw
_rate_bf_target_rads.z += _rate_bf_desired_rads.z;
_ang_vel_target_rads.z += _att_target_ang_vel_rads.z;
}
// subclass non-passthrough too, for external gyro, no flybar
@ -115,12 +115,12 @@ void AC_AttitudeControl_Heli::rate_controller_run()
_motors.set_roll(_passthrough_roll);
_motors.set_pitch(_passthrough_pitch);
} else {
rate_bf_to_motor_roll_pitch(_rate_bf_target_rads.x, _rate_bf_target_rads.y);
rate_bf_to_motor_roll_pitch(_ang_vel_target_rads.x, _ang_vel_target_rads.y);
}
if (_flags_heli.tail_passthrough) {
_motors.set_yaw(_passthrough_yaw);
} else {
_motors.set_yaw(rate_bf_to_motor_yaw(_rate_bf_target_rads.z));
_motors.set_yaw(rate_bf_to_motor_yaw(_ang_vel_target_rads.z));
}
}