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_AttControl: implement angle_boost

Browse Source
This commit is contained in:
Randy Mackay 2014-01-06 12:28:35 +09:00 committed by Andrew Tridgell
parent 77c38f4de4
commit 89b7e6b1c8
2 changed files with 23 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

@ -367,12 +367,12 @@ float AC_AttitudeControl::rate_bf_to_motor_yaw(float rate_target_cds)
// provide 0 to cut motors
void AC_AttitudeControl::set_throttle_out(int16_t throttle_out, bool apply_angle_boost)
{
if( apply_angle_boost ) {
if (apply_angle_boost) {
_motor_throttle = get_angle_boost(throttle_out);
}else{
_motor_throttle = throttle_out;
// clear angle_boost for logging purposes
//angle_boost = 0;
_angle_boost = 0;
}
// update compass with throttle value
@ -382,7 +382,7 @@ void AC_AttitudeControl::set_throttle_out(int16_t throttle_out, bool apply_angle
// get_angle_boost - returns a throttle including compensation for roll/pitch angle
// throttle value should be 0 ~ 1000
int16_t AC_AttitudeControl::get_angle_boost(int16_t throttle_pwm) const
int16_t AC_AttitudeControl::get_angle_boost(int16_t throttle_pwm)
{
float temp = _cos_pitch * _cos_roll;
int16_t throttle_out;
@ -394,11 +394,10 @@ int16_t AC_AttitudeControl::get_angle_boost(int16_t throttle_pwm) const
// apply scale and constrain throttle
// To-Do: move throttle_min and throttle_max into the AP_Vehicles class?
//throttle_out = constrain_float((float)(throttle-g.throttle_min) * temp + g.throttle_min, g.throttle_min, 1000);
throttle_out = constrain_float((float)(throttle_pwm-_motors.throttle_min()) * temp + _motors.throttle_min(), _motors.throttle_min(), 1000);
// to allow logging of angle boost
// To-Do: expose this so it can be logged?
//angle_boost = throttle_out - throttle_pwm;
// record angle boost for logging
_angle_boost = throttle_out - throttle_pwm;
return throttle_out;
}

25
libraries/AC_AttitudeControl/AC_AttitudeControl.h
View File

@ -55,7 +55,12 @@ public:
_motor_pitch(motor_pitch),
_motor_yaw(motor_yaw),
_motor_throttle(motor_throttle),
_dt(AC_ATTITUDE_100HZ_DT)
_dt(AC_ATTITUDE_100HZ_DT),
_angle_boost(0),
_cos_roll(1.0),
_cos_pitch(1.0),
_sin_roll(0.0),
_sin_pitch(0.0)
{
AP_Param::setup_object_defaults(this, var_info);
}
@ -150,6 +155,9 @@ public:
// provide 0 to cut motors
void set_throttle_out(int16_t throttle_pwm, bool apply_angle_boost);
// angle_boost - accessor for angle boost so it can be logged
int16_t angle_boost() const { return _angle_boost; }
//
// helper functions
//
@ -181,7 +189,7 @@ private:
//
// get_angle_boost - calculate total body frame throttle required to produce the given earth frame throttle
int16_t get_angle_boost(int16_t throttle_pwm) const;
int16_t get_angle_boost(int16_t throttle_pwm);
//
// logging
@ -224,12 +232,13 @@ private:
// internal variables
// To-Do: make rate targets a typedef instead of Vector3f?
float _dt; // time delta in seconds
Vector3f _angle_ef_target; // angle controller earth-frame targets
Vector3f _rate_stab_ef_target; // stabilized rate controller earth-frame rate targets
Vector3f _rate_ef_target; // rate controller earth-frame targets
Vector3f _rate_stab_bf_target; // stabilized rate controller body-frame rate targets
Vector3f _rate_stab_bf_error; // stabilized rate controller body-frame angle errors
Vector3f _rate_bf_target; // rate controller body-frame targets
Vector3f _angle_ef_target; // angle controller earth-frame targets
Vector3f _rate_stab_ef_target; // stabilized rate controller earth-frame rate targets
Vector3f _rate_ef_target; // rate controller earth-frame targets
Vector3f _rate_stab_bf_target; // stabilized rate controller body-frame rate targets
Vector3f _rate_stab_bf_error; // stabilized rate controller body-frame angle errors
Vector3f _rate_bf_target; // rate controller body-frame targets
int16_t _angle_boost; // used only for logging
// precalculated values for efficiency saves
// To-Do: could these be changed to references and passed into the constructor?