Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

APM_Control: use a ahrs reference, not pointer 

saves pointer check
This commit is contained in:
Andrew Tridgell 2013-08-14 14:56:18 +10:00
parent a98d7bd050
commit c6e37aaec3
6 changed files with 24 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/APM_Control/AP_PitchController.cpp
View File

@ -106,11 +106,10 @@ int32_t AP_PitchController::_get_rate_out(float desired_rate, float scaler, bool
}
_last_t = tnow;
if(_ahrs == NULL) return 0;
float delta_time = (float)dt * 0.001f;
// Get body rate vector (radians/sec)
float omega_y = _ahrs->get_gyro().y;
float omega_y = _ahrs.get_gyro().y;
// Calculate the pitch rate error (deg/sec) and scale
float rate_error = (desired_rate - ToDeg(omega_y)) * scaler;
@ -143,7 +142,7 @@ int32_t AP_PitchController::_get_rate_out(float desired_rate, float scaler, bool
// Calculate equivalent gains so that values for K_P and K_I can be taken across from the old PID law
// No conversion is required for K_D
float kp_ff = max((_K_P - _K_I * _tau) * _tau - _K_D , 0) / _ahrs->get_EAS2TAS();
float kp_ff = max((_K_P - _K_I * _tau) * _tau - _K_D , 0) / _ahrs.get_EAS2TAS();
// Calculate the demanded control surface deflection
// Note the scaler is applied again. We want a 1/speed scaler applied to the feed-forward
@ -180,7 +179,7 @@ int32_t AP_PitchController::get_rate_out(float desired_rate, float scaler)
float AP_PitchController::_get_coordination_rate_offset(float &aspeed, bool &inverted) const
{
float rate_offset;
float bank_angle = _ahrs->roll;
float bank_angle = _ahrs.roll;
// limit bank angle between +- 80 deg if right way up
if (fabsf(bank_angle) < radians(90)) {
@ -194,11 +193,11 @@ float AP_PitchController::_get_coordination_rate_offset(float &aspeed, bool &inv
bank_angle = constrain_float(bank_angle,-radians(180),-radians(100));
}
}
if (!_ahrs->airspeed_estimate(&aspeed)) {
if (!_ahrs.airspeed_estimate(&aspeed)) {
// If no airspeed available use average of min and max
aspeed = 0.5f*(float(aparm.airspeed_min) + float(aparm.airspeed_max));
}
if (abs(_ahrs->pitch_sensor) > 7000) {
if (abs(_ahrs.pitch_sensor) > 7000) {
// don't do turn coordination handling when at very high pitch angles
rate_offset = 0;
} else {

9
libraries/APM_Control/AP_PitchController.h
View File

@ -10,14 +10,13 @@
class AP_PitchController {
public:
AP_PitchController(const AP_SpdHgtControl::AircraftParameters &parms) :
aparm(parms)
AP_PitchController(AP_AHRS &ahrs, const AP_SpdHgtControl::AircraftParameters &parms) :
aparm(parms),
_ahrs(ahrs)
{
AP_Param::setup_object_defaults(this, var_info);
}
void set_ahrs(AP_AHRS *ahrs) { _ahrs = ahrs; }
int32_t get_rate_out(float desired_rate, float scaler);
int32_t get_servo_out(int32_t angle_err, float scaler, bool disable_integrator);
float get_coordination_rate_offset(void) const;
@ -44,7 +43,7 @@ private:
int32_t _get_rate_out(float desired_rate, float scaler, bool disable_integrator, float aspeed);
float _get_coordination_rate_offset(float &aspeed, bool &inverted) const;
AP_AHRS *_ahrs;
AP_AHRS &_ahrs;
};

12
libraries/APM_Control/AP_RollController.cpp
View File

@ -82,16 +82,10 @@ int32_t AP_RollController::_get_rate_out(float desired_rate, float scaler, bool
}
_last_t = tnow;
if (_ahrs == NULL) {
// can't control without a reference
return 0;
}
// Calculate equivalent gains so that values for K_P and K_I can be taken across from the old PID law
// No conversion is required for K_D
float ki_rate = _K_I * _tau;
float kp_ff = max((_K_P - _K_I * _tau) * _tau - _K_D , 0)/_ahrs->get_EAS2TAS();
float kp_ff = max((_K_P - _K_I * _tau) * _tau - _K_D , 0)/_ahrs.get_EAS2TAS();
float delta_time = (float)dt * 0.001f;
// Limit the demanded roll rate
@ -102,14 +96,14 @@ int32_t AP_RollController::_get_rate_out(float desired_rate, float scaler, bool
}
// Get body rate vector (radians/sec)
float omega_x = _ahrs->get_gyro().x;
float omega_x = _ahrs.get_gyro().x;
// Calculate the roll rate error (deg/sec) and apply gain scaler
float rate_error = (desired_rate - ToDeg(omega_x)) * scaler;
// Get an airspeed estimate - default to zero if none available
float aspeed;
if (!_ahrs->airspeed_estimate(&aspeed)) {
if (!_ahrs.airspeed_estimate(&aspeed)) {
aspeed = 0.0f;
}

9
libraries/APM_Control/AP_RollController.h
View File

@ -10,14 +10,13 @@
class AP_RollController {
public:
AP_RollController(const AP_SpdHgtControl::AircraftParameters &parms) :
aparm(parms)
AP_RollController(AP_AHRS &ahrs, const AP_SpdHgtControl::AircraftParameters &parms) :
aparm(parms),
_ahrs(ahrs)
{
AP_Param::setup_object_defaults(this, var_info);
}
void set_ahrs(AP_AHRS *ahrs) { _ahrs = ahrs; }
int32_t get_rate_out(float desired_rate, float scaler);
int32_t get_servo_out(int32_t angle_err, float scaler, bool disable_integrator);
@ -40,7 +39,7 @@ private:
int32_t _get_rate_out(float desired_rate, float scaler, bool disable_integrator);
AP_AHRS *_ahrs;
AP_AHRS &_ahrs;
};

11
libraries/APM_Control/AP_YawController.cpp
View File

@ -71,9 +71,6 @@ int32_t AP_YawController::get_servo_out(float scaler, bool disable_integrator)
}
_last_t = tnow;
if(_ins == NULL) { // can't control without a reference
return 0;
}
int16_t aspd_min = aparm.airspeed_min;
if (aspd_min < 1) {
@ -85,22 +82,22 @@ int32_t AP_YawController::get_servo_out(float scaler, bool disable_integrator)
// Calculate yaw rate required to keep up with a constant height coordinated turn
float aspeed;
float rate_offset;
float bank_angle = _ahrs->roll;
float bank_angle = _ahrs.roll;
// limit bank angle between +- 80 deg if right way up
if (fabsf(bank_angle) < 1.5707964f) {
bank_angle = constrain_float(bank_angle,-1.3962634f,1.3962634f);
}
if (!_ahrs->airspeed_estimate(&aspeed)) {
if (!_ahrs.airspeed_estimate(&aspeed)) {
// If no airspeed available use average of min and max
aspeed = 0.5f*(float(aspd_min) + float(aparm.airspeed_max));
}
rate_offset = (GRAVITY_MSS / max(aspeed , float(aspd_min))) * tanf(bank_angle) * cosf(bank_angle) * _K_FF;
// Get body rate vector (radians/sec)
float omega_z = _ahrs->get_gyro().z;
float omega_z = _ahrs.get_gyro().z;
// Get the accln vector (m/s^2)
float accel_y = _ins->get_accel().y;
float accel_y = _ahrs.get_ins()->get_accel().y;
// Subtract the steady turn component of rate from the measured rate
// to calculate the rate relative to the turn requirement in degrees/sec

14
libraries/APM_Control/AP_YawController.h
View File

@ -10,17 +10,13 @@
class AP_YawController {
public:
AP_YawController(const AP_SpdHgtControl::AircraftParameters &parms) :
aparm(parms)
AP_YawController(AP_AHRS &ahrs, const AP_SpdHgtControl::AircraftParameters &parms) :
aparm(parms),
_ahrs(ahrs)
{
AP_Param::setup_object_defaults(this, var_info);
}
void set_ahrs(AP_AHRS *ahrs) {
_ahrs = ahrs;
_ins = _ahrs->get_ins();
}
int32_t get_servo_out(float scaler, bool disable_integrator);
void reset_I();
@ -43,9 +39,7 @@ private:
float _integrator;
AP_AHRS *_ahrs;
AP_InertialSensor *_ins;
AP_AHRS &_ahrs;
};
#endif // __AP_YAW_CONTROLLER_H__