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; _last_t = tnow;
if(_ahrs == NULL) return 0;
float delta_time = (float)dt * 0.001f; float delta_time = (float)dt * 0.001f;
// Get body rate vector (radians/sec) // 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 // Calculate the pitch rate error (deg/sec) and scale
float rate_error = (desired_rate - ToDeg(omega_y)) * scaler; 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 // 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 // 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 // Calculate the demanded control surface deflection
// Note the scaler is applied again. We want a 1/speed scaler applied to the feed-forward // 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 AP_PitchController::_get_coordination_rate_offset(float &aspeed, bool &inverted) const
{ {
float rate_offset; float rate_offset;
float bank_angle = _ahrs->roll; float bank_angle = _ahrs.roll;
// limit bank angle between +- 80 deg if right way up // limit bank angle between +- 80 deg if right way up
if (fabsf(bank_angle) < radians(90)) { 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)); 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 // If no airspeed available use average of min and max
aspeed = 0.5f*(float(aparm.airspeed_min) + float(aparm.airspeed_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 // don't do turn coordination handling when at very high pitch angles
rate_offset = 0; rate_offset = 0;
} else { } else {

9
libraries/APM_Control/AP_PitchController.h
View File

@ -10,14 +10,13 @@
class AP_PitchController { class AP_PitchController {
public: public:
AP_PitchController(const AP_SpdHgtControl::AircraftParameters &parms) : AP_PitchController(AP_AHRS &ahrs, const AP_SpdHgtControl::AircraftParameters &parms) :
aparm(parms) aparm(parms),
_ahrs(ahrs)
{ {
AP_Param::setup_object_defaults(this, var_info); 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_rate_out(float desired_rate, float scaler);
int32_t get_servo_out(int32_t angle_err, float scaler, bool disable_integrator); int32_t get_servo_out(int32_t angle_err, float scaler, bool disable_integrator);
float get_coordination_rate_offset(void) const; 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); 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; 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; _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 // 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 // No conversion is required for K_D
float ki_rate = _K_I * _tau; 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; float delta_time = (float)dt * 0.001f;
// Limit the demanded roll rate // 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) // 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 // Calculate the roll rate error (deg/sec) and apply gain scaler
float rate_error = (desired_rate - ToDeg(omega_x)) * scaler; float rate_error = (desired_rate - ToDeg(omega_x)) * scaler;
// Get an airspeed estimate - default to zero if none available // Get an airspeed estimate - default to zero if none available
float aspeed; float aspeed;
if (!_ahrs->airspeed_estimate(&aspeed)) { if (!_ahrs.airspeed_estimate(&aspeed)) {
aspeed = 0.0f; aspeed = 0.0f;
} }

9
libraries/APM_Control/AP_RollController.h
View File

@ -10,14 +10,13 @@
class AP_RollController { class AP_RollController {
public: public:
AP_RollController(const AP_SpdHgtControl::AircraftParameters &parms) : AP_RollController(AP_AHRS &ahrs, const AP_SpdHgtControl::AircraftParameters &parms) :
aparm(parms) aparm(parms),
_ahrs(ahrs)
{ {
AP_Param::setup_object_defaults(this, var_info); 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_rate_out(float desired_rate, float scaler);
int32_t get_servo_out(int32_t angle_err, float scaler, bool disable_integrator); 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); 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; _last_t = tnow;
if(_ins == NULL) { // can't control without a reference
return 0;
}
int16_t aspd_min = aparm.airspeed_min; int16_t aspd_min = aparm.airspeed_min;
if (aspd_min < 1) { 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 // Calculate yaw rate required to keep up with a constant height coordinated turn
float aspeed; float aspeed;
float rate_offset; float rate_offset;
float bank_angle = _ahrs->roll; float bank_angle = _ahrs.roll;
// limit bank angle between +- 80 deg if right way up // limit bank angle between +- 80 deg if right way up
if (fabsf(bank_angle) < 1.5707964f) { if (fabsf(bank_angle) < 1.5707964f) {
bank_angle = constrain_float(bank_angle,-1.3962634f,1.3962634f); 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 // If no airspeed available use average of min and max
aspeed = 0.5f*(float(aspd_min) + float(aparm.airspeed_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; rate_offset = (GRAVITY_MSS / max(aspeed , float(aspd_min))) * tanf(bank_angle) * cosf(bank_angle) * _K_FF;
// Get body rate vector (radians/sec) // 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) // 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 // Subtract the steady turn component of rate from the measured rate
// to calculate the rate relative to the turn requirement in degrees/sec // 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 { class AP_YawController {
public: public:
AP_YawController(const AP_SpdHgtControl::AircraftParameters &parms) : AP_YawController(AP_AHRS &ahrs, const AP_SpdHgtControl::AircraftParameters &parms) :
aparm(parms) aparm(parms),
_ahrs(ahrs)
{ {
AP_Param::setup_object_defaults(this, var_info); 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); int32_t get_servo_out(float scaler, bool disable_integrator);
void reset_I(); void reset_I();
@ -43,9 +39,7 @@ private:
float _integrator; float _integrator;
AP_AHRS *_ahrs; AP_AHRS &_ahrs;
AP_InertialSensor *_ins;
}; };
#endif // __AP_YAW_CONTROLLER_H__ #endif // __AP_YAW_CONTROLLER_H__