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

AC_AttitudeControl: added inverted flight support

Browse Source 
for helis
This commit is contained in:
Andrew Tridgell 2017-08-28 09:48:36 +10:00
parent 18f04abfe8
commit f2efea4e1d
3 changed files with 37 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AC_AttitudeControl/AC_AttitudeControl.h
View File

@ -117,10 +117,10 @@ public:
void input_quaternion(Quaternion attitude_desired_quat, float smoothing_gain);
// Command an euler roll and pitch angle and an euler yaw rate with angular velocity feedforward and smoothing
void input_euler_angle_roll_pitch_euler_rate_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds, float smoothing_gain);
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, float smoothing_gain);
// Command an euler roll, pitch and yaw angle with angular velocity feedforward and smoothing
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, float smoothing_gain);
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, float smoothing_gain);
// Command an euler roll, pitch, and yaw rate with angular velocity feedforward and smoothing
void input_euler_rate_roll_pitch_yaw(float euler_roll_rate_cds, float euler_pitch_rate_cds, float euler_yaw_rate_cds);
@ -263,6 +263,9 @@ public:
// 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) {};
// enable inverted flight on backends that support it
virtual void set_inverted_flight(bool inverted) {}
// User settable parameters
static const struct AP_Param::GroupInfo var_info[];
@ -394,6 +397,9 @@ protected:
void control_monitor_filter_pid(float value, float &rms_P);
void control_monitor_update(void);
// true in inverted flight mode
bool _inverted_flight;
public:
// log a CTRL message
void control_monitor_log(void);

18
libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp
View File

@ -437,3 +437,21 @@ void AC_AttitudeControl_Heli::set_throttle_out(float throttle_in, bool apply_ang
// Clear angle_boost for logging purposes
_angle_boost = 0.0f;
}
// Command an euler roll and pitch angle and an euler yaw rate with angular velocity feedforward and smoothing
void AC_AttitudeControl_Heli::input_euler_angle_roll_pitch_euler_rate_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds, float smoothing_gain)
{
if (_inverted_flight) {
euler_roll_angle_cd = wrap_180_cd(euler_roll_angle_cd + 18000);
}
AC_AttitudeControl::input_euler_angle_roll_pitch_euler_rate_yaw(euler_roll_angle_cd, euler_pitch_angle_cd, euler_yaw_rate_cds, smoothing_gain);
}
// Command an euler roll, pitch and yaw angle with angular velocity feedforward and smoothing
void AC_AttitudeControl_Heli::input_euler_angle_roll_pitch_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_angle_cd, bool slew_yaw, float smoothing_gain)
{
if (_inverted_flight) {
euler_roll_angle_cd = wrap_180_cd(euler_roll_angle_cd + 18000);
}
AC_AttitudeControl::input_euler_angle_roll_pitch_yaw(euler_roll_angle_cd, euler_pitch_angle_cd, euler_yaw_angle_cd, slew_yaw, smoothing_gain);
}

11
libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h
View File

@ -95,6 +95,17 @@ public:
// Set output throttle
void set_throttle_out(float throttle_in, bool apply_angle_boost, float filt_cutoff) override;
// Command an euler roll and pitch angle and an euler yaw rate with angular velocity feedforward and smoothing
void input_euler_angle_roll_pitch_euler_rate_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds, float smoothing_gain) override;
// Command an euler roll, pitch and yaw angle with angular velocity feedforward and smoothing
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, float smoothing_gain) override;
// enable/disable inverted flight
void set_inverted_flight(bool inverted) override {
_inverted_flight = inverted;
}
// user settable parameters
static const struct AP_Param::GroupInfo var_info[];