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

AP_Motors: add feedforward input to mixers

Browse Source
This commit is contained in:
Leonard Hall 2019-06-27 19:02:30 +09:30 committed by Randy Mackay
parent 979b54b33e
commit 0bb9db5139
8 changed files with 32 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AP_Motors/AP_Motors6DOF.cpp
View File

@ -291,9 +291,9 @@ void AP_Motors6DOF::output_armed_stabilizing()
float forward_thrust; // forward thrust input value, +/- 1.0
float lateral_thrust; // lateral thrust input value, +/- 1.0
roll_thrust = _roll_in;
pitch_thrust = _pitch_in;
yaw_thrust = _yaw_in;
roll_thrust = (_roll_in + _roll_in_ff);
pitch_thrust = (_pitch_in + _pitch_in_ff);
yaw_thrust = (_yaw_in + _yaw_in_ff);
throttle_thrust = get_throttle_bidirectional();
forward_thrust = _forward_in;
lateral_thrust = _lateral_in;
@ -396,9 +396,9 @@ void AP_Motors6DOF::output_armed_stabilizing_vectored()
float forward_thrust; // forward thrust input value, +/- 1.0
float lateral_thrust; // lateral thrust input value, +/- 1.0
roll_thrust = _roll_in;
pitch_thrust = _pitch_in;
yaw_thrust = _yaw_in;
roll_thrust = (_roll_in + _roll_in_ff);
pitch_thrust = (_pitch_in + _pitch_in_ff);
yaw_thrust = (_yaw_in + _yaw_in_ff);
throttle_thrust = get_throttle_bidirectional();
forward_thrust = _forward_in;
lateral_thrust = _lateral_in;
@ -480,9 +480,9 @@ void AP_Motors6DOF::output_armed_stabilizing_vectored_6dof()
float forward_thrust; // forward thrust input value, +/- 1.0
float lateral_thrust; // lateral thrust input value, +/- 1.0
roll_thrust = _roll_in;
pitch_thrust = _pitch_in;
yaw_thrust = _yaw_in;
roll_thrust = (_roll_in + _roll_in_ff);
pitch_thrust = (_pitch_in + _pitch_in_ff);
yaw_thrust = (_yaw_in + _yaw_in_ff);
throttle_thrust = get_throttle_bidirectional();
forward_thrust = _forward_in;
lateral_thrust = _lateral_in;

6
libraries/AP_Motors/AP_MotorsCoax.cpp
View File

@ -137,9 +137,9 @@ void AP_MotorsCoax::output_armed_stabilizing()
// apply voltage and air pressure compensation
const float compensation_gain = get_compensation_gain();
roll_thrust = _roll_in * compensation_gain;
pitch_thrust = _pitch_in * compensation_gain;
yaw_thrust = _yaw_in * compensation_gain;
roll_thrust = (_roll_in + _roll_in_ff) * compensation_gain;
pitch_thrust = (_pitch_in + _pitch_in_ff) * compensation_gain;
yaw_thrust = (_yaw_in + _yaw_in_ff) * compensation_gain;
throttle_thrust = get_throttle() * compensation_gain;
throttle_avg_max = _throttle_avg_max * compensation_gain;

6
libraries/AP_Motors/AP_MotorsMatrix.cpp
View File

@ -147,9 +147,9 @@ void AP_MotorsMatrix::output_armed_stabilizing()
// apply voltage and air pressure compensation
const float compensation_gain = get_compensation_gain(); // compensation for battery voltage and altitude
roll_thrust = _roll_in * compensation_gain;
pitch_thrust = _pitch_in * compensation_gain;
yaw_thrust = _yaw_in * compensation_gain;
roll_thrust = (_roll_in + _roll_in_ff) * compensation_gain;
pitch_thrust = (_pitch_in + _pitch_in_ff) * compensation_gain;
yaw_thrust = (_yaw_in + _yaw_in_ff) * compensation_gain;
throttle_thrust = get_throttle() * compensation_gain;
throttle_avg_max = _throttle_avg_max * compensation_gain;
throttle_thrust_max = _thrust_boost_ratio + (1.0f - _thrust_boost_ratio) * _throttle_thrust_max;

4
libraries/AP_Motors/AP_MotorsMatrixTS.cpp
View File

@ -75,8 +75,8 @@ void AP_MotorsMatrixTS::output_armed_stabilizing()
// apply voltage and air pressure compensation
const float compensation_gain = get_compensation_gain(); // compensation for battery voltage and altitude
roll_thrust = _roll_in * compensation_gain;
pitch_thrust = _pitch_in * compensation_gain;
roll_thrust = (_roll_in + _roll_in_ff) * compensation_gain;
pitch_thrust = (_roll_in + _roll_in_ff) * compensation_gain;
throttle_thrust = get_throttle() * compensation_gain;
// sanity check throttle is above zero and below current limited throttle

6
libraries/AP_Motors/AP_MotorsSingle.cpp
View File

@ -142,9 +142,9 @@ void AP_MotorsSingle::output_armed_stabilizing()
// apply voltage and air pressure compensation
const float compensation_gain = get_compensation_gain();
roll_thrust = _roll_in * compensation_gain;
pitch_thrust = _pitch_in * compensation_gain;
yaw_thrust = _yaw_in * compensation_gain;
roll_thrust = (_roll_in + _roll_in_ff) * compensation_gain;
pitch_thrust = (_pitch_in + _pitch_in_ff) * compensation_gain;
yaw_thrust = (_yaw_in + _yaw_in_ff) * compensation_gain;
throttle_thrust = get_throttle() * compensation_gain;
throttle_avg_max = _throttle_avg_max * compensation_gain;

6
libraries/AP_Motors/AP_MotorsTailsitter.cpp
View File

@ -137,9 +137,9 @@ void AP_MotorsTailsitter::output_armed_stabilizing()
// apply voltage and air pressure compensation
const float compensation_gain = get_compensation_gain();
roll_thrust = _roll_in * compensation_gain;
pitch_thrust = _pitch_in * compensation_gain;
yaw_thrust = _yaw_in * compensation_gain;
roll_thrust = (_roll_in + _roll_in_ff) * compensation_gain;
pitch_thrust = (_pitch_in + _pitch_in_ff) * compensation_gain;
yaw_thrust = (_yaw_in + _yaw_in_ff) * compensation_gain;
throttle_thrust = get_throttle() * compensation_gain;
// sanity check throttle is above zero and below current limited throttle

6
libraries/AP_Motors/AP_MotorsTri.cpp
View File

@ -147,9 +147,9 @@ void AP_MotorsTri::output_armed_stabilizing()
// apply voltage and air pressure compensation
const float compensation_gain = get_compensation_gain();
roll_thrust = _roll_in * compensation_gain;
pitch_thrust = _pitch_in * compensation_gain;
yaw_thrust = _yaw_in * compensation_gain * sinf(radians(_yaw_servo_angle_max_deg)); // we scale this so a thrust request of 1.0f will ask for full servo deflection at full rear throttle
roll_thrust = (_roll_in + _roll_in_ff) * compensation_gain;
pitch_thrust = (_pitch_in + _pitch_in_ff) * compensation_gain;
yaw_thrust = (_yaw_in + _yaw_in_ff) * compensation_gain * sinf(radians(_yaw_servo_angle_max_deg)); // we scale this so a thrust request of 1.0f will ask for full servo deflection at full rear throttle
throttle_thrust = get_throttle() * compensation_gain;
throttle_avg_max = _throttle_avg_max * compensation_gain;

6
libraries/AP_Motors/AP_Motors_Class.h
View File

@ -82,8 +82,11 @@ public:
// set_roll, set_pitch, set_yaw, set_throttle
void set_roll(float roll_in) { _roll_in = roll_in; }; // range -1 ~ +1
void set_roll_ff(float roll_in) { _roll_in_ff = roll_in; }; // range -1 ~ +1
void set_pitch(float pitch_in) { _pitch_in = pitch_in; }; // range -1 ~ +1
void set_pitch_ff(float pitch_in) { _pitch_in_ff = pitch_in; }; // range -1 ~ +1
void set_yaw(float yaw_in) { _yaw_in = yaw_in; }; // range -1 ~ +1
void set_yaw_ff(float yaw_in) { _yaw_in_ff = yaw_in; }; // range -1 ~ +1
void set_throttle(float throttle_in) { _throttle_in = throttle_in; }; // range 0 ~ 1
void set_throttle_avg_max(float throttle_avg_max) { _throttle_avg_max = constrain_float(throttle_avg_max, 0.0f, 1.0f); }; // range 0 ~ 1
void set_throttle_filter_cutoff(float filt_hz) { _throttle_filter.set_cutoff_frequency(filt_hz); }
@ -215,8 +218,11 @@ protected:
uint16_t _loop_rate; // rate in Hz at which output() function is called (normally 400hz)
uint16_t _speed_hz; // speed in hz to send updates to motors
float _roll_in; // desired roll control from attitude controllers, -1 ~ +1
float _roll_in_ff; // desired roll feed forward control from attitude controllers, -1 ~ +1
float _pitch_in; // desired pitch control from attitude controller, -1 ~ +1
float _pitch_in_ff; // desired pitch feed forward control from attitude controller, -1 ~ +1
float _yaw_in; // desired yaw control from attitude controller, -1 ~ +1
float _yaw_in_ff; // desired yaw feed forward control from attitude controller, -1 ~ +1
float _throttle_in; // last throttle input from set_throttle caller
float _forward_in; // last forward input from set_forward caller
float _lateral_in; // last lateral input from set_lateral caller