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

AC_AttitudeControl: Move set_throttle_out to _Multi and _Heli

Browse Source
This commit is contained in:
Leonard Hall 2016-03-21 21:27:39 +10:30 committed by Randy Mackay
parent a1b573ed0a
commit 35ef761deb
6 changed files with 27 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

@ -600,20 +600,6 @@ void AC_AttitudeControl::accel_limiting(bool enable_limits)
} }
} }
void AC_AttitudeControl::set_throttle_out(float throttle_in, bool apply_angle_boost, float filter_cutoff)
{
_throttle_in = throttle_in;
_throttle_in_filt.apply(throttle_in, _dt);
_motors.set_throttle_filter_cutoff(filter_cutoff);
if (apply_angle_boost) {
_motors.set_throttle(get_boosted_throttle(throttle_in));
}else{
_motors.set_throttle(throttle_in);
// Clear angle_boost for logging purposes
_angle_boost = 0.0f;
}
}
void AC_AttitudeControl::set_throttle_out_unstabilized(float throttle_in, bool reset_attitude_control, float filter_cutoff) void AC_AttitudeControl::set_throttle_out_unstabilized(float throttle_in, bool reset_attitude_control, float filter_cutoff)
{ {
_throttle_in = throttle_in; _throttle_in = throttle_in;

2
libraries/AC_AttitudeControl/AC_AttitudeControl.h
View File

@ -191,7 +191,7 @@ public:
void accel_limiting(bool enable_or_disable); void accel_limiting(bool enable_or_disable);
// Set output throttle // Set output throttle
void set_throttle_out(float throttle_in, bool apply_angle_boost, float filt_cutoff); virtual void set_throttle_out(float throttle_in, bool apply_angle_boost, float filt_cutoff) = 0;
// Set output throttle and disable stabilization // Set output throttle and disable stabilization
void set_throttle_out_unstabilized(float throttle_in, bool reset_attitude_control, float filt_cutoff); void set_throttle_out_unstabilized(float throttle_in, bool reset_attitude_control, float filt_cutoff);

11
libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp
View File

@ -404,11 +404,12 @@ float AC_AttitudeControl_Heli::rate_bf_to_motor_yaw(float rate_target_rads)
// throttle functions // throttle functions
// //
// returns a throttle including compensation for roll/pitch angle void AC_AttitudeControl_Heli::set_throttle_out(float throttle_in, bool apply_angle_boost, float filter_cutoff)
// throttle value should be 0 ~ 1000
float AC_AttitudeControl_Heli::get_boosted_throttle(float throttle_in)
{ {
// no angle boost for trad helis _throttle_in = throttle_in;
_throttle_in_filt.apply(throttle_in, _dt);
_motors.set_throttle_filter_cutoff(filter_cutoff);
_motors.set_throttle(throttle_in);
// Clear angle_boost for logging purposes
_angle_boost = 0.0f; _angle_boost = 0.0f;
return throttle_in;
} }

3
libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.h
View File

@ -95,6 +95,9 @@ public:
// set_hover_roll_scalar - scales Hover Roll Trim parameter. To be used by vehicle code according to vehicle condition. // set_hover_roll_scalar - scales Hover Roll Trim parameter. To be used by vehicle code according to vehicle condition.
void set_hover_roll_trim_scalar(float scalar) {_hover_roll_trim_scalar = constrain_float(scalar, 0.0f, 1.0f);} void set_hover_roll_trim_scalar(float scalar) {_hover_roll_trim_scalar = constrain_float(scalar, 0.0f, 1.0f);}
// Set output throttle
void set_throttle_out(float throttle_in, bool apply_angle_boost, float filt_cutoff) override;
// user settable parameters // user settable parameters
static const struct AP_Param::GroupInfo var_info[]; static const struct AP_Param::GroupInfo var_info[];

14
libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp
View File

@ -147,6 +147,20 @@ float AC_AttitudeControl_Multi::get_althold_lean_angle_max() const
return ToDeg(acos(constrain_float(_throttle_in_filt.get()/(0.9f * thr_max), 0.0f, 1.0f))) * 100.0f; return ToDeg(acos(constrain_float(_throttle_in_filt.get()/(0.9f * thr_max), 0.0f, 1.0f))) * 100.0f;
} }
void AC_AttitudeControl_Multi::set_throttle_out(float throttle_in, bool apply_angle_boost, float filter_cutoff)
{
_throttle_in = throttle_in;
_throttle_in_filt.apply(throttle_in, _dt);
_motors.set_throttle_filter_cutoff(filter_cutoff);
if (apply_angle_boost) {
_motors.set_throttle(get_boosted_throttle(throttle_in));
}else{
_motors.set_throttle(throttle_in);
// Clear angle_boost for logging purposes
_angle_boost = 0.0f;
}
}
// returns a throttle including compensation for roll/pitch angle // returns a throttle including compensation for roll/pitch angle
// throttle value should be 0 ~ 1 // throttle value should be 0 ~ 1
float AC_AttitudeControl_Multi::get_boosted_throttle(float throttle_in) float AC_AttitudeControl_Multi::get_boosted_throttle(float throttle_in)

3
libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h
View File

@ -55,6 +55,9 @@ public:
// get lean angle max for pilot input that prioritises altitude hold over lean angle // get lean angle max for pilot input that prioritises altitude hold over lean angle
float get_althold_lean_angle_max() const; float get_althold_lean_angle_max() const;
// Set output throttle
void set_throttle_out(float throttle_in, bool apply_angle_boost, float filt_cutoff) override;
// calculate total body frame throttle required to produce the given earth frame throttle // calculate total body frame throttle required to produce the given earth frame throttle
float get_boosted_throttle(float throttle_in); float get_boosted_throttle(float throttle_in);