mirror of https://github.com/ArduPilot/ardupilot
638 lines
32 KiB
C++
638 lines
32 KiB
C++
#pragma once
|
|
|
|
/// @file AC_AttitudeControl.h
|
|
/// @brief ArduCopter attitude control library
|
|
|
|
#include <AP_Common/AP_Common.h>
|
|
#include <AP_Param/AP_Param.h>
|
|
#include <AP_Math/AP_Math.h>
|
|
#include <AP_AHRS/AP_AHRS_View.h>
|
|
#include <AP_Motors/AP_Motors.h>
|
|
#include <AC_PID/AC_PID.h>
|
|
#include <AC_PID/AC_P.h>
|
|
#include <AP_Vehicle/AP_MultiCopter.h>
|
|
|
|
#define AC_ATTITUDE_CONTROL_ANGLE_P 4.5f // default angle P gain for roll, pitch and yaw
|
|
|
|
#define AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN_RADSS radians(40.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(10.0f) // minimum body-frame acceleration limit for the stability controller (for yaw axis)
|
|
#define AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS radians(120.0f) // maximum body-frame acceleration limit for the stability controller (for yaw axis)
|
|
#define AC_ATTITUDE_CONTROL_SLEW_YAW_DEFAULT_CDS 6000 // constraint on yaw angle error in degrees. This should lead to maximum turn rate of 10deg/sec * 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 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_RP_CONTROLLER_OUT_MAX 1.0f // body-frame rate controller maximum output (for roll-pitch axis)
|
|
#define AC_ATTITUDE_RATE_YAW_CONTROLLER_OUT_MAX 1.0f // body-frame rate controller maximum output (for yaw axis)
|
|
#define AC_ATTITUDE_RATE_RELAX_TC 0.16f // This is used to decay the rate I term to 5% in half a second.
|
|
|
|
#define AC_ATTITUDE_THRUST_ERROR_ANGLE radians(30.0f) // Thrust angle error above which yaw corrections are limited
|
|
#define AC_ATTITUDE_YAW_MAX_ERROR_ANGLE radians(45.0f) // Thrust angle error above which yaw corrections are limited
|
|
|
|
#define AC_ATTITUDE_CONTROL_RATE_BF_FF_DEFAULT 1 // body-frame rate feedforward enabled by default
|
|
|
|
#define AC_ATTITUDE_CONTROL_ANGLE_LIMIT_TC_DEFAULT 1.0f // Time constant used to limit lean angle so that vehicle does not lose altitude
|
|
#define AC_ATTITUDE_CONTROL_ANGLE_LIMIT_THROTTLE_MAX 0.8f // Max throttle used to limit lean angle so that vehicle does not lose altitude
|
|
|
|
#define AC_ATTITUDE_CONTROL_MIN_DEFAULT 0.1f // minimum throttle mix default
|
|
#define AC_ATTITUDE_CONTROL_MAN_DEFAULT 0.1f // manual throttle mix default
|
|
#define AC_ATTITUDE_CONTROL_MAX_DEFAULT 0.5f // maximum throttle mix default
|
|
#define AC_ATTITUDE_CONTROL_MIN_LIMIT 0.5f // min throttle mix upper limit
|
|
#define AC_ATTITUDE_CONTROL_MAN_LIMIT 4.0f // man throttle mix upper limit
|
|
#define AC_ATTITUDE_CONTROL_MAX 5.0f // maximum throttle mix default
|
|
|
|
#define AC_ATTITUDE_CONTROL_THR_MIX_DEFAULT 0.5f // ratio controlling the max throttle output during competing requests of low throttle from the pilot (or autopilot) and higher throttle for attitude control. Higher favours Attitude over pilot input
|
|
#define AC_ATTITUDE_CONTROL_THR_G_BOOST_THRESH 1.0f // default angle-p/pd throttle boost threshold
|
|
|
|
class AC_AttitudeControl {
|
|
public:
|
|
AC_AttitudeControl( AP_AHRS_View &ahrs,
|
|
const AP_MultiCopter &aparm,
|
|
AP_Motors& motors) :
|
|
_p_angle_roll(AC_ATTITUDE_CONTROL_ANGLE_P),
|
|
_p_angle_pitch(AC_ATTITUDE_CONTROL_ANGLE_P),
|
|
_p_angle_yaw(AC_ATTITUDE_CONTROL_ANGLE_P),
|
|
_angle_boost(0),
|
|
_use_sqrt_controller(true),
|
|
_throttle_rpy_mix_desired(AC_ATTITUDE_CONTROL_THR_MIX_DEFAULT),
|
|
_throttle_rpy_mix(AC_ATTITUDE_CONTROL_THR_MIX_DEFAULT),
|
|
_ahrs(ahrs),
|
|
_aparm(aparm),
|
|
_motors(motors)
|
|
{
|
|
_singleton = this;
|
|
AP_Param::setup_object_defaults(this, var_info);
|
|
}
|
|
|
|
static AC_AttitudeControl *get_singleton(void) {
|
|
return _singleton;
|
|
}
|
|
|
|
// Empty destructor to suppress compiler warning
|
|
virtual ~AC_AttitudeControl() {}
|
|
|
|
// set_dt / get_dt - dt is the time since the last time the attitude controllers were updated
|
|
// _dt should be set based on the time of the last IMU read used by these controllers
|
|
// the attitude controller should run updates for active controllers on each loop to ensure normal operation
|
|
void set_dt(float dt) { _dt = dt; }
|
|
float get_dt() const { return _dt; }
|
|
|
|
// pid accessors
|
|
AC_P& get_angle_roll_p() { return _p_angle_roll; }
|
|
AC_P& get_angle_pitch_p() { return _p_angle_pitch; }
|
|
AC_P& get_angle_yaw_p() { return _p_angle_yaw; }
|
|
virtual AC_PID& get_rate_roll_pid() = 0;
|
|
virtual AC_PID& get_rate_pitch_pid() = 0;
|
|
virtual AC_PID& get_rate_yaw_pid() = 0;
|
|
virtual const AC_PID& get_rate_roll_pid() const = 0;
|
|
virtual const AC_PID& get_rate_pitch_pid() const = 0;
|
|
virtual const AC_PID& get_rate_yaw_pid() const = 0;
|
|
|
|
// get the roll acceleration limit in centidegrees/s/s or radians/s/s
|
|
float get_accel_roll_max_cdss() const { return _accel_roll_max; }
|
|
float get_accel_roll_max_radss() const { return radians(_accel_roll_max * 0.01f); }
|
|
|
|
// Sets the roll acceleration limit in centidegrees/s/s
|
|
void set_accel_roll_max_cdss(float accel_roll_max) { _accel_roll_max.set(accel_roll_max); }
|
|
|
|
// Sets and saves the roll acceleration limit in centidegrees/s/s
|
|
void save_accel_roll_max_cdss(float accel_roll_max) { _accel_roll_max.set_and_save(accel_roll_max); }
|
|
|
|
// get the pitch acceleration limit in centidegrees/s/s or radians/s/s
|
|
float get_accel_pitch_max_cdss() const { return _accel_pitch_max; }
|
|
float get_accel_pitch_max_radss() const { return radians(_accel_pitch_max * 0.01f); }
|
|
|
|
// Sets the pitch acceleration limit in centidegrees/s/s
|
|
void set_accel_pitch_max_cdss(float accel_pitch_max) { _accel_pitch_max.set(accel_pitch_max); }
|
|
|
|
// Sets and saves the pitch acceleration limit in centidegrees/s/s
|
|
void save_accel_pitch_max_cdss(float accel_pitch_max) { _accel_pitch_max.set_and_save(accel_pitch_max); }
|
|
|
|
// get the yaw acceleration limit in centidegrees/s/s or radians/s/s
|
|
float get_accel_yaw_max_cdss() const { return _accel_yaw_max; }
|
|
float get_accel_yaw_max_radss() const { return radians(_accel_yaw_max * 0.01f); }
|
|
|
|
// Sets the yaw acceleration limit in centidegrees/s/s
|
|
void set_accel_yaw_max_cdss(float accel_yaw_max) { _accel_yaw_max.set(accel_yaw_max); }
|
|
|
|
// Sets and saves the yaw acceleration limit in centidegrees/s/s
|
|
void save_accel_yaw_max_cdss(float accel_yaw_max) { _accel_yaw_max.set_and_save(accel_yaw_max); }
|
|
|
|
// get the roll angular velocity limit in radians/s
|
|
float get_ang_vel_roll_max_rads() const { return radians(_ang_vel_roll_max); }
|
|
// get the roll angular velocity limit in degrees/s
|
|
float get_ang_vel_roll_max_degs() const { return _ang_vel_roll_max; }
|
|
|
|
// set the roll angular velocity limit in degrees/s
|
|
void set_ang_vel_roll_max_degs(float vel_roll_max) { _ang_vel_roll_max.set(vel_roll_max); }
|
|
|
|
// get the pitch angular velocity limit in radians/s
|
|
float get_ang_vel_pitch_max_rads() const { return radians(_ang_vel_pitch_max); }
|
|
// get the pitch angular velocity limit in degrees/s
|
|
float get_ang_vel_pitch_max_degs() const { return _ang_vel_pitch_max; }
|
|
|
|
// set the pitch angular velocity limit in degrees/s
|
|
void set_ang_vel_pitch_max_degs(float vel_pitch_max) { _ang_vel_pitch_max.set(vel_pitch_max); }
|
|
|
|
// get the yaw angular velocity limit in radians/s
|
|
float get_ang_vel_yaw_max_rads() const { return radians(_ang_vel_yaw_max); }
|
|
// get the yaw angular velocity limit in degrees/s
|
|
float get_ang_vel_yaw_max_degs() const { return _ang_vel_yaw_max; }
|
|
|
|
// set the yaw angular velocity limit in degrees/s
|
|
void set_ang_vel_yaw_max_degs(float vel_yaw_max) { _ang_vel_yaw_max.set(vel_yaw_max); }
|
|
|
|
// get the slew yaw rate limit in deg/s
|
|
float get_slew_yaw_max_degs() const;
|
|
|
|
// get the rate control input smoothing time constant
|
|
float get_input_tc() const { return _input_tc; }
|
|
|
|
// set the rate control input smoothing time constant
|
|
void set_input_tc(float input_tc) { _input_tc.set(constrain_float(input_tc, 0.0f, 1.0f)); }
|
|
|
|
// Ensure attitude controller have zero errors to relax rate controller output
|
|
void relax_attitude_controllers();
|
|
|
|
// Used by child class AC_AttitudeControl_TS to change behaviour for tailsitter quadplanes
|
|
virtual void relax_attitude_controllers(bool exclude_pitch) { relax_attitude_controllers(); }
|
|
|
|
// reset rate controller I terms
|
|
void reset_rate_controller_I_terms();
|
|
|
|
// reset rate controller I terms smoothly to zero in 0.5 seconds
|
|
void reset_rate_controller_I_terms_smoothly();
|
|
|
|
// Reduce attitude control gains while landed to stop ground resonance
|
|
void landed_gain_reduction(bool landed);
|
|
|
|
// Sets attitude target to vehicle attitude and sets all rates to zero
|
|
// If reset_rate is false rates are not reset to allow the rate controllers to run
|
|
void reset_target_and_rate(bool reset_rate = true);
|
|
|
|
// Sets yaw target to vehicle heading and sets yaw rate to zero
|
|
// If reset_rate is false rates are not reset to allow the rate controllers to run
|
|
void reset_yaw_target_and_rate(bool reset_rate = true);
|
|
|
|
// handle reset of attitude from EKF since the last iteration
|
|
void inertial_frame_reset();
|
|
|
|
// Command a Quaternion attitude with feedforward and smoothing
|
|
// attitude_desired_quat: is updated on each time_step (_dt) by the integral of the body frame angular velocity
|
|
virtual void input_quaternion(Quaternion& attitude_desired_quat, Vector3f ang_vel_body);
|
|
|
|
// Command an euler roll and pitch angle and an euler yaw rate with angular velocity feedforward and smoothing
|
|
virtual void input_euler_angle_roll_pitch_euler_rate_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds);
|
|
// Command an euler roll, pitch and yaw angle with angular velocity feedforward and smoothing
|
|
virtual void input_euler_angle_roll_pitch_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_angle_cd, bool slew_yaw);
|
|
|
|
// Command euler yaw rate and pitch angle with roll angle specified in body frame
|
|
// (implemented only in AC_AttitudeControl_TS for tailsitter quadplanes)
|
|
virtual void input_euler_rate_yaw_euler_angle_pitch_bf_roll(bool plane_controls, float euler_roll_angle_cd,
|
|
float euler_pitch_angle_cd, float euler_yaw_rate_cds) {}
|
|
|
|
// Command an euler roll, pitch, and yaw rate with angular velocity feedforward and smoothing
|
|
virtual void input_euler_rate_roll_pitch_yaw(float euler_roll_rate_cds, float euler_pitch_rate_cds, float euler_yaw_rate_cds);
|
|
|
|
// Command an angular velocity with angular velocity feedforward and smoothing
|
|
virtual void input_rate_bf_roll_pitch_yaw(float roll_rate_bf_cds, float pitch_rate_bf_cds, float yaw_rate_bf_cds);
|
|
|
|
// Command an angular velocity with angular velocity feedforward and smoothing
|
|
virtual void input_rate_bf_roll_pitch_yaw_2(float roll_rate_bf_cds, float pitch_rate_bf_cds, float yaw_rate_bf_cds);
|
|
|
|
// Command an angular velocity with angular velocity smoothing using rate loops only with integrated rate error stabilization
|
|
virtual void input_rate_bf_roll_pitch_yaw_3(float roll_rate_bf_cds, float pitch_rate_bf_cds, float yaw_rate_bf_cds);
|
|
|
|
// Command an angular step (i.e change) in body frame angle
|
|
virtual void input_angle_step_bf_roll_pitch_yaw(float roll_angle_step_bf_cd, float pitch_angle_step_bf_cd, float yaw_angle_step_bf_cd);
|
|
|
|
// Command an angular rate step (i.e change) in body frame rate
|
|
virtual void input_rate_step_bf_roll_pitch_yaw(float roll_rate_step_bf_cd, float pitch_rate_step_bf_cd, float yaw_rate_step_bf_cd);
|
|
|
|
// Command a thrust vector in the earth frame and a heading angle and/or rate
|
|
virtual void input_thrust_vector_rate_heading(const Vector3f& thrust_vector, float heading_rate_cds, bool slew_yaw = true);
|
|
virtual void input_thrust_vector_heading(const Vector3f& thrust_vector, float heading_angle_cd, float heading_rate_cds);
|
|
void input_thrust_vector_heading(const Vector3f& thrust_vector, float heading_cd) {input_thrust_vector_heading(thrust_vector, heading_cd, 0.0f);}
|
|
|
|
// Converts thrust vector and heading angle to quaternion rotation in the earth frame
|
|
Quaternion attitude_from_thrust_vector(Vector3f thrust_vector, float heading_angle) const;
|
|
|
|
// Run angular velocity controller and send outputs to the motors
|
|
virtual void rate_controller_run() = 0;
|
|
|
|
// optional variant to allow running with different dt
|
|
virtual void rate_controller_run_dt(float dt, const Vector3f& gyro) { rate_controller_run(); }
|
|
|
|
// Convert a 321-intrinsic euler angle derivative to an angular velocity vector
|
|
void euler_rate_to_ang_vel(const Quaternion& att, const Vector3f& euler_rate_rads, Vector3f& ang_vel_rads);
|
|
|
|
// 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_rate(const Quaternion& att, const Vector3f& ang_vel_rads, Vector3f& euler_rate_rads);
|
|
|
|
// Specifies whether the attitude controller should use the square root controller in the attitude correction.
|
|
// This is used during Autotune to ensure the P term is tuned without being influenced by the acceleration limit of the square root controller.
|
|
void use_sqrt_controller(bool use_sqrt_cont) { _use_sqrt_controller = use_sqrt_cont; }
|
|
|
|
// Return 321-intrinsic euler angles in centidegrees representing the rotation from NED earth frame to the
|
|
// attitude controller's target attitude.
|
|
// **NOTE** Using vector3f*deg(100) is more efficient than deg(vector3f)*100 or deg(vector3d*100) because it gives the
|
|
// same result with the fewest multiplications. Even though it may look like a bug, it is intentional. See issue 4895.
|
|
Vector3f get_att_target_euler_cd() const { return _euler_angle_target * degrees(100.0f); }
|
|
const Vector3f & get_att_target_euler_rad() const { return _euler_angle_target; }
|
|
|
|
// Return the body-to-NED target attitude used by the quadplane-specific attitude control input methods
|
|
Quaternion get_attitude_target_quat() const { return _attitude_target; }
|
|
|
|
// Return the angular velocity of the target (setpoint) [rad/s] in the target attitude frame
|
|
const Vector3f& get_attitude_target_ang_vel() const { return _ang_vel_target;}
|
|
|
|
// Return the angle between the target thrust vector and the current thrust vector.
|
|
float get_att_error_angle_deg() const { return degrees(_thrust_error_angle); }
|
|
|
|
// Set x-axis angular velocity in centidegrees/s
|
|
void rate_bf_roll_target(float rate_cds) { _ang_vel_body.x = radians(rate_cds * 0.01f); }
|
|
|
|
// Set y-axis angular velocity in centidegrees/s
|
|
void rate_bf_pitch_target(float rate_cds) { _ang_vel_body.y = radians(rate_cds * 0.01f); }
|
|
|
|
// Set z-axis angular velocity in centidegrees/s
|
|
void rate_bf_yaw_target(float rate_cds) { _ang_vel_body.z = radians(rate_cds * 0.01f); }
|
|
|
|
// Set x-axis system identification angular velocity in degrees/s
|
|
void rate_bf_roll_sysid(float rate) { _sysid_ang_vel_body.x = rate; }
|
|
|
|
// Set y-axis system identification angular velocity in degrees/s
|
|
void rate_bf_pitch_sysid(float rate) { _sysid_ang_vel_body.y = rate; }
|
|
|
|
// Set z-axis system identification angular velocity in degrees/s
|
|
void rate_bf_yaw_sysid(float rate) { _sysid_ang_vel_body.z = rate; }
|
|
|
|
// Set x-axis system identification actuator
|
|
void actuator_roll_sysid(float command) { _actuator_sysid.x = command; }
|
|
|
|
// Set y-axis system identification actuator
|
|
void actuator_pitch_sysid(float command) { _actuator_sysid.y = command; }
|
|
|
|
// Set z-axis system identification actuator
|
|
void actuator_yaw_sysid(float command) { _actuator_sysid.z = command; }
|
|
|
|
// Return roll rate step size in radians/s that results in maximum output after 4 time steps
|
|
float max_rate_step_bf_roll();
|
|
|
|
// Return pitch rate step size in radians/s that results in maximum output after 4 time steps
|
|
float max_rate_step_bf_pitch();
|
|
|
|
// Return yaw rate step size in radians/s that results in maximum output after 4 time steps
|
|
float max_rate_step_bf_yaw();
|
|
|
|
// Return roll step size in radians 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(); }
|
|
|
|
// Return pitch step size in radians 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(); }
|
|
|
|
// Return yaw step size in radians 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(); }
|
|
|
|
// Return angular velocity in radians used in the angular velocity controller
|
|
Vector3f rate_bf_targets() const { return _ang_vel_body + _sysid_ang_vel_body; }
|
|
|
|
// return the angular velocity of the target (setpoint) attitude rad/s
|
|
const Vector3f& get_rate_ef_targets() const { return _euler_rate_target; }
|
|
|
|
// Enable or disable body-frame feed forward
|
|
void bf_feedforward(bool enable_or_disable) { _rate_bf_ff_enabled.set(enable_or_disable); }
|
|
|
|
// 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); }
|
|
|
|
// Return body-frame feed forward setting
|
|
bool get_bf_feedforward() { return _rate_bf_ff_enabled; }
|
|
|
|
// Enable or disable body-frame feed forward
|
|
void accel_limiting(bool enable_or_disable);
|
|
|
|
// Update Alt_Hold angle maximum
|
|
virtual void update_althold_lean_angle_max(float throttle_in) = 0;
|
|
|
|
// Set output throttle
|
|
virtual void set_throttle_out(float throttle_in, bool apply_angle_boost, float filt_cutoff) = 0;
|
|
|
|
// get throttle passed into attitude controller (i.e. throttle_in provided to set_throttle_out)
|
|
float get_throttle_in() const { return _throttle_in; }
|
|
|
|
// Return throttle increase applied for tilt compensation
|
|
float angle_boost() const { return _angle_boost; }
|
|
|
|
// Return tilt angle limit for pilot input that prioritises altitude hold over lean angle
|
|
virtual float get_althold_lean_angle_max_cd() const;
|
|
|
|
// Return configured tilt angle limit in centidegrees
|
|
float lean_angle_max_cd() const { return _aparm.angle_max; }
|
|
|
|
// Return tilt angle in degrees
|
|
float lean_angle_deg() const { return degrees(_thrust_angle); }
|
|
|
|
// calculates the velocity correction from an angle error. The angular velocity has acceleration and
|
|
// deceleration limits including basic jerk limiting using smoothing_gain
|
|
static float input_shaping_angle(float error_angle, float input_tc, float accel_max, float target_ang_vel, float desired_ang_vel, float max_ang_vel, float dt);
|
|
static float input_shaping_angle(float error_angle, float input_tc, float accel_max, float target_ang_vel, float dt){ return input_shaping_angle(error_angle, input_tc, accel_max, target_ang_vel, 0.0f, 0.0f, dt); }
|
|
|
|
// Shapes the velocity request based on a rate time constant. The angular acceleration and deceleration is limited.
|
|
static float input_shaping_ang_vel(float target_ang_vel, float desired_ang_vel, float accel_max, float dt, float input_tc);
|
|
|
|
// calculates the expected angular velocity correction from an angle error based on the AC_AttitudeControl settings.
|
|
// This function can be used to predict the delay associated with angle requests.
|
|
void input_shaping_rate_predictor(const Vector2f &error_angle, Vector2f& target_ang_vel, float dt) const;
|
|
|
|
// translates body frame acceleration limits to the euler axis
|
|
void ang_vel_limit(Vector3f& euler_rad, float ang_vel_roll_max, float ang_vel_pitch_max, float ang_vel_yaw_max) const;
|
|
|
|
// translates body frame acceleration limits to the euler axis
|
|
Vector3f euler_accel_limit(const Quaternion &att, const Vector3f &euler_accel);
|
|
|
|
// Calculates the body frame angular velocities to follow the target attitude
|
|
void update_attitude_target();
|
|
|
|
// thrust_heading_rotation_angles - calculates two ordered rotations to move the attitude_body quaternion to the attitude_target quaternion.
|
|
// The maximum error in the yaw axis is limited based on the angle yaw P value and acceleration.
|
|
void thrust_heading_rotation_angles(Quaternion& attitude_target, const Quaternion& attitude_body, Vector3f& attitude_error, float& thrust_angle, float& thrust_error_angle) const;
|
|
|
|
// thrust_vector_rotation_angles - calculates two ordered rotations to move the attitude_body quaternion to the attitude_target quaternion.
|
|
// The first rotation corrects the thrust vector and the second rotation corrects the heading vector.
|
|
void thrust_vector_rotation_angles(const Quaternion& attitude_target, const Quaternion& attitude_body, Quaternion& thrust_vector_correction, Vector3f& attitude_error, float& thrust_angle, float& thrust_error_angle) const;
|
|
|
|
// sanity check parameters. should be called once before take-off
|
|
virtual void parameter_sanity_check() {}
|
|
|
|
// set the PID notch sample rates
|
|
virtual void set_notch_sample_rate(float sample_rate) {}
|
|
|
|
// return true if the rpy mix is at lowest value
|
|
virtual bool is_throttle_mix_min() const { return true; }
|
|
|
|
// control rpy throttle mix
|
|
virtual void set_throttle_mix_min() {}
|
|
virtual void set_throttle_mix_man() {}
|
|
virtual void set_throttle_mix_max(float ratio) {}
|
|
virtual void set_throttle_mix_value(float value) {}
|
|
virtual float get_throttle_mix(void) const { return 0; }
|
|
|
|
// enable use of flybass passthrough on heli
|
|
virtual void use_flybar_passthrough(bool passthrough, bool tail_passthrough) {}
|
|
|
|
// use_leaky_i - controls whether we use leaky i term for body-frame to motor output stage on heli
|
|
virtual void use_leaky_i(bool leaky_i) {}
|
|
|
|
// set_hover_roll_scalar - scales Hover Roll Trim parameter. To be used by vehicle code according to vehicle condition.
|
|
virtual void set_hover_roll_trim_scalar(float scalar) {}
|
|
|
|
// Return angle in centidegrees to be added to roll angle for hover collective learn. Used by heli to counteract
|
|
// tail rotor thrust in hover. Overloaded by AC_Attitude_Heli to return angle.
|
|
virtual float get_roll_trim_cd() { return 0;}
|
|
|
|
// passthrough_bf_roll_pitch_rate_yaw - roll and pitch are passed through directly, body-frame rate target for yaw
|
|
virtual void passthrough_bf_roll_pitch_rate_yaw(float roll_passthrough, float pitch_passthrough, float yaw_rate_bf_cds) {};
|
|
|
|
// provide feedback on whether arming would be a good idea right now:
|
|
bool pre_arm_checks(const char *param_prefix,
|
|
char *failure_msg,
|
|
const uint8_t failure_msg_len);
|
|
|
|
// enable inverted flight on backends that support it
|
|
virtual void set_inverted_flight(bool inverted) {}
|
|
|
|
// enable accessor for inverted flight flag on backends that support it
|
|
virtual bool get_inverted_flight() { return false;}
|
|
|
|
// get the slew rate value for roll, pitch and yaw, for oscillation detection in lua scripts
|
|
void get_rpy_srate(float &roll_srate, float &pitch_srate, float &yaw_srate);
|
|
|
|
// Sets the roll and pitch rate shaping time constant
|
|
void set_roll_pitch_rate_tc(float input_tc) { _rate_rp_tc = input_tc; }
|
|
|
|
// Sets the yaw rate shaping time constant
|
|
void set_yaw_rate_tc(float input_tc) { _rate_y_tc = input_tc; }
|
|
|
|
// setup a one loop angle P scale multiplier. This replaces any previous scale applied
|
|
// so should only be used when only one source of scaling is needed
|
|
void set_angle_P_scale(const Vector3f &angle_P_scale) { _angle_P_scale = angle_P_scale; }
|
|
|
|
// setup a one loop angle P scale multiplier, multiplying by any
|
|
// previously applied scale from this loop. This allows for more
|
|
// than one type of scale factor to be applied for different
|
|
// purposes
|
|
void set_angle_P_scale_mult(const Vector3f &angle_P_scale) { _angle_P_scale *= angle_P_scale; }
|
|
|
|
// get the value of the angle P scale that was used in the last loop
|
|
const Vector3f &get_last_angle_P_scale(void) const { return _angle_P_scale_used; }
|
|
|
|
// setup a one loop PD scale multiplier, multiplying by any
|
|
// previously applied scale from this loop. This allows for more
|
|
// than one type of scale factor to be applied for different
|
|
// purposes
|
|
void set_PD_scale_mult(const Vector3f &pd_scale) { _pd_scale *= pd_scale; }
|
|
|
|
// write RATE message
|
|
void Write_Rate(const AC_PosControl &pos_control) const;
|
|
|
|
// write ANG message
|
|
void Write_ANG() const;
|
|
|
|
// User settable parameters
|
|
static const struct AP_Param::GroupInfo var_info[];
|
|
|
|
static constexpr Vector3f VECTORF_111{1.0f,1.0f,1.0f};
|
|
|
|
protected:
|
|
|
|
// Update rate_target_ang_vel using attitude_error_rot_vec_rad
|
|
Vector3f update_ang_vel_target_from_att_error(const Vector3f &attitude_error_rot_vec_rad);
|
|
|
|
// 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 float get_roll_trim_rad() { return 0;}
|
|
|
|
// Return the yaw slew rate limit in radians/s
|
|
float get_slew_yaw_max_rads() const { return radians(get_slew_yaw_max_degs()); }
|
|
|
|
// Maximum rate the yaw target can be updated in Loiter, RTL, Auto flight modes
|
|
AP_Float _slew_yaw;
|
|
|
|
// Maximum angular velocity (in degrees/second) for earth-frame roll, pitch and yaw axis
|
|
AP_Float _ang_vel_roll_max;
|
|
AP_Float _ang_vel_pitch_max;
|
|
AP_Float _ang_vel_yaw_max;
|
|
|
|
// 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;
|
|
|
|
// Enable/Disable angle boost
|
|
AP_Int8 _angle_boost_enabled;
|
|
|
|
// angle controller P objects
|
|
AC_P _p_angle_roll;
|
|
AC_P _p_angle_pitch;
|
|
AC_P _p_angle_yaw;
|
|
|
|
// Angle limit time constant (to maintain altitude)
|
|
AP_Float _angle_limit_tc;
|
|
|
|
// rate controller input smoothing time constant
|
|
AP_Float _input_tc;
|
|
|
|
// Controller gain multiplyer to be used when landed
|
|
AP_Float _land_roll_mult;
|
|
AP_Float _land_pitch_mult;
|
|
AP_Float _land_yaw_mult;
|
|
|
|
// latest gyro value use by the rate_controller
|
|
Vector3f _rate_gyro;
|
|
// timestamp of the latest gyro value used by the rate controller
|
|
uint64_t _rate_gyro_time_us;
|
|
|
|
// Intersampling period in seconds
|
|
float _dt;
|
|
|
|
// This represents a 321-intrinsic rotation in NED frame to the target (setpoint)
|
|
// attitude used in the attitude controller, in radians.
|
|
Vector3f _euler_angle_target;
|
|
|
|
// This represents the angular velocity of the target (setpoint) attitude used in
|
|
// the attitude controller as 321-intrinsic euler angle derivatives, in radians per
|
|
// second.
|
|
Vector3f _euler_rate_target;
|
|
|
|
// This represents a quaternion rotation in NED frame to the target (setpoint)
|
|
// attitude used in the attitude controller.
|
|
Quaternion _attitude_target;
|
|
|
|
// This represents the angular velocity of the target (setpoint) attitude used in
|
|
// the attitude controller as an angular velocity vector, in radians per second in
|
|
// the target attitude frame.
|
|
Vector3f _ang_vel_target;
|
|
|
|
// This represents the angular velocity in radians per second in the body frame, used in the angular
|
|
// velocity controller.
|
|
Vector3f _ang_vel_body;
|
|
|
|
// This is the angular velocity in radians per second in the body frame, added to the output angular
|
|
// attitude controller by the System Identification Mode.
|
|
// It is reset to zero immediately after it is used.
|
|
Vector3f _sysid_ang_vel_body;
|
|
|
|
// This is the unitless value added to the output of the PID by the System Identification Mode.
|
|
// It is reset to zero immediately after it is used.
|
|
Vector3f _actuator_sysid;
|
|
|
|
// This represents a quaternion attitude error in the body frame, used for inertial frame reset handling.
|
|
Quaternion _attitude_ang_error;
|
|
|
|
// The angle between the target thrust vector and the current thrust vector.
|
|
float _thrust_angle;
|
|
|
|
// The angle between the target thrust vector and the current thrust vector.
|
|
float _thrust_error_angle;
|
|
|
|
// throttle provided as input to attitude controller. This does not include angle boost.
|
|
float _throttle_in = 0.0f;
|
|
|
|
// This represents the throttle increase applied for tilt compensation.
|
|
// Used only for logging.
|
|
float _angle_boost;
|
|
|
|
// Specifies whether the attitude controller should use the square root controller in the attitude correction.
|
|
// This is used during Autotune to ensure the P term is tuned without being influenced by the acceleration limit of the square root controller.
|
|
bool _use_sqrt_controller;
|
|
|
|
// Filtered Alt_Hold lean angle max - used to limit lean angle when throttle is saturated using Alt_Hold
|
|
float _althold_lean_angle_max = 0.0f;
|
|
|
|
// desired throttle_low_comp value, actual throttle_low_comp is slewed towards this value over 1~2 seconds
|
|
float _throttle_rpy_mix_desired;
|
|
|
|
// mix between throttle and hover throttle for 0 to 1 and ratio above hover throttle for >1
|
|
float _throttle_rpy_mix;
|
|
|
|
// Yaw feed forward percent to allow zero yaw actuator output during extreme roll and pitch corrections
|
|
float _feedforward_scalar = 1.0f;
|
|
|
|
// rate controller input smoothing time constant
|
|
float _rate_rp_tc;
|
|
float _rate_y_tc;
|
|
|
|
// angle P scaling vector for roll, pitch, yaw
|
|
Vector3f _angle_P_scale{1,1,1};
|
|
|
|
// angle scale used for last loop, used for logging and quadplane angle P scaling
|
|
Vector3f _angle_P_scale_used;
|
|
|
|
// PD scaling vector for roll, pitch, yaw
|
|
Vector3f _pd_scale{1,1,1};
|
|
|
|
// PD scale used for last loop, used for logging
|
|
Vector3f _pd_scale_used;
|
|
|
|
// ratio of normal gain to landed gain
|
|
float _landed_gain_ratio;
|
|
|
|
// References to external libraries
|
|
const AP_AHRS_View& _ahrs;
|
|
const AP_MultiCopter &_aparm;
|
|
AP_Motors& _motors;
|
|
|
|
static AC_AttitudeControl *_singleton;
|
|
|
|
protected:
|
|
/*
|
|
state of control monitoring
|
|
*/
|
|
struct {
|
|
float rms_roll_P;
|
|
float rms_roll_D;
|
|
float rms_pitch_P;
|
|
float rms_pitch_D;
|
|
float rms_yaw;
|
|
} _control_monitor;
|
|
|
|
// update state in ControlMonitor
|
|
void control_monitor_filter_pid(float value, float &rms_P);
|
|
void control_monitor_update(void);
|
|
|
|
public:
|
|
// log a CTRL message
|
|
void control_monitor_log(void) const;
|
|
|
|
// return current RMS controller filter for each axis
|
|
float control_monitor_rms_output_roll(void) const;
|
|
float control_monitor_rms_output_roll_P(void) const;
|
|
float control_monitor_rms_output_roll_D(void) const;
|
|
float control_monitor_rms_output_pitch_P(void) const;
|
|
float control_monitor_rms_output_pitch_D(void) const;
|
|
float control_monitor_rms_output_pitch(void) const;
|
|
float control_monitor_rms_output_yaw(void) const;
|
|
|
|
// structure for angle and/or rate target
|
|
enum class HeadingMode {
|
|
Angle_Only,
|
|
Angle_And_Rate,
|
|
Rate_Only
|
|
};
|
|
struct HeadingCommand {
|
|
float yaw_angle_cd;
|
|
float yaw_rate_cds;
|
|
HeadingMode heading_mode;
|
|
};
|
|
void input_thrust_vector_heading(const Vector3f& thrust_vector, HeadingCommand heading);
|
|
};
|