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

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 // provide 0 to cut motors
void AC_AttitudeControl::set_throttle_out(int16_t throttle_out, bool apply_angle_boost) 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); _motor_throttle = get_angle_boost(throttle_out);
}else{ }else{
_motor_throttle = throttle_out; _motor_throttle = throttle_out;
// clear angle_boost for logging purposes // clear angle_boost for logging purposes
//angle_boost = 0; _angle_boost = 0;
} }
// update compass with throttle value // 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 // get_angle_boost - returns a throttle including compensation for roll/pitch angle
// throttle value should be 0 ~ 1000 // 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; float temp = _cos_pitch * _cos_roll;
int16_t throttle_out; 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 // apply scale and constrain throttle
// To-Do: move throttle_min and throttle_max into the AP_Vehicles class? // 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 // record angle boost for logging
// To-Do: expose this so it can be logged? _angle_boost = throttle_out - throttle_pwm;
//angle_boost = throttle_out - throttle_pwm;
return throttle_out; return throttle_out;
} }

25
libraries/AC_AttitudeControl/AC_AttitudeControl.h
View File

@ -55,7 +55,12 @@ public:
_motor_pitch(motor_pitch), _motor_pitch(motor_pitch),
_motor_yaw(motor_yaw), _motor_yaw(motor_yaw),
_motor_throttle(motor_throttle), _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); AP_Param::setup_object_defaults(this, var_info);
} }
@ -150,6 +155,9 @@ public:
// provide 0 to cut motors // provide 0 to cut motors
void set_throttle_out(int16_t throttle_pwm, bool apply_angle_boost); 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 // helper functions
// //
@ -181,7 +189,7 @@ private:
// //
// get_angle_boost - calculate total body frame throttle required to produce the given earth frame throttle // 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 // logging
@ -224,12 +232,13 @@ private:
// internal variables // internal variables
// To-Do: make rate targets a typedef instead of Vector3f? // To-Do: make rate targets a typedef instead of Vector3f?
float _dt; // time delta in seconds float _dt; // time delta in seconds
Vector3f _angle_ef_target; // angle controller earth-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_stab_ef_target; // stabilized rate controller earth-frame rate targets
Vector3f _rate_ef_target; // rate controller earth-frame 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_target; // stabilized rate controller body-frame rate targets
Vector3f _rate_stab_bf_error; // stabilized rate controller body-frame angle errors Vector3f _rate_stab_bf_error; // stabilized rate controller body-frame angle errors
Vector3f _rate_bf_target; // rate controller body-frame targets Vector3f _rate_bf_target; // rate controller body-frame targets
int16_t _angle_boost; // used only for logging
// precalculated values for efficiency saves // precalculated values for efficiency saves
// To-Do: could these be changed to references and passed into the constructor? // To-Do: could these be changed to references and passed into the constructor?