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

AP_TECS: use a ahrs reference

Browse Source 
saves a pointer access
This commit is contained in:
Andrew Tridgell 2013-08-14 14:58:49 +10:00
parent accfd46633
commit 3c967a9c71
2 changed files with 16 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
libraries/AP_TECS/AP_TECS.cpp
View File

@ -151,7 +151,7 @@ void AP_TECS::update_50hz(float hgt_afe)
_update_50hz_last_usec = now; _update_50hz_last_usec = now;
// Get height acceleration // Get height acceleration
float hgt_ddot_mea = -(_ahrs->get_accel_ef().z + GRAVITY_MSS); float hgt_ddot_mea = -(_ahrs.get_accel_ef().z + GRAVITY_MSS);
// Perform filter calculation using backwards Euler integration // Perform filter calculation using backwards Euler integration
// Coefficients selected to place all three filter poles at omega // Coefficients selected to place all three filter poles at omega
float omega2 = _hgtCompFiltOmega*_hgtCompFiltOmega; float omega2 = _hgtCompFiltOmega*_hgtCompFiltOmega;
@ -172,11 +172,11 @@ void AP_TECS::update_50hz(float hgt_afe)
// Update and average speed rate of change // Update and average speed rate of change
// Only required if airspeed is being measured and controlled // Only required if airspeed is being measured and controlled
float temp = 0; float temp = 0;
if (_ahrs->airspeed_sensor_enabled() && _ahrs->airspeed_estimate_true(&_EAS)) { if (_ahrs.airspeed_sensor_enabled() && _ahrs.airspeed_estimate_true(&_EAS)) {
// Get DCM // Get DCM
const Matrix3f &rotMat = _ahrs->get_dcm_matrix(); const Matrix3f &rotMat = _ahrs.get_dcm_matrix();
// Calculate speed rate of change // Calculate speed rate of change
temp = rotMat.c.x * GRAVITY_MSS + _ahrs->get_ins()->get_accel().x; temp = rotMat.c.x * GRAVITY_MSS + _ahrs.get_ins()->get_accel().x;
// take 5 point moving average // take 5 point moving average
_vel_dot = _vdot_filter.apply(temp); _vel_dot = _vdot_filter.apply(temp);
} else { } else {
@ -194,7 +194,7 @@ void AP_TECS::_update_speed(void)
// Convert equivalent airspeeds to true airspeeds // Convert equivalent airspeeds to true airspeeds
float EAS2TAS = _ahrs->get_EAS2TAS(); float EAS2TAS = _ahrs.get_EAS2TAS();
_TAS_dem = _EAS_dem * EAS2TAS; _TAS_dem = _EAS_dem * EAS2TAS;
_TASmax = aparm.airspeed_max * EAS2TAS; _TASmax = aparm.airspeed_max * EAS2TAS;
_TASmin = aparm.airspeed_min * EAS2TAS; _TASmin = aparm.airspeed_min * EAS2TAS;
@ -209,7 +209,7 @@ void AP_TECS::_update_speed(void)
// Get airspeed or default to halfway between min and max if // Get airspeed or default to halfway between min and max if
// airspeed is not being used and set speed rate to zero // airspeed is not being used and set speed rate to zero
if (!_ahrs->airspeed_sensor_enabled() || !_ahrs->airspeed_estimate(&_EAS)) { if (!_ahrs.airspeed_sensor_enabled() || !_ahrs.airspeed_estimate(&_EAS)) {
// If no airspeed available use average of min and max // If no airspeed available use average of min and max
_EAS = 0.5f * (aparm.airspeed_min + aparm.airspeed_max); _EAS = 0.5f * (aparm.airspeed_min + aparm.airspeed_max);
} }
@ -364,7 +364,7 @@ void AP_TECS::_update_throttle(void)
// Calculate feed-forward throttle // Calculate feed-forward throttle
float ff_throttle = 0; float ff_throttle = 0;
float nomThr = aparm.throttle_cruise * 0.01f; float nomThr = aparm.throttle_cruise * 0.01f;
const Matrix3f &rotMat = _ahrs->get_dcm_matrix(); const Matrix3f &rotMat = _ahrs.get_dcm_matrix();
// Use the demanded rate of change of total energy as the feed-forward demand, but add // Use the demanded rate of change of total energy as the feed-forward demand, but add
// additional component which scales with (1/cos(bank angle) - 1) to compensate for induced // additional component which scales with (1/cos(bank angle) - 1) to compensate for induced
// drag increase during turns. // drag increase during turns.
@ -406,7 +406,7 @@ void AP_TECS::_update_throttle(void)
// Sum the components. // Sum the components.
// Only use feed-forward component if airspeed is not being used // Only use feed-forward component if airspeed is not being used
if (_ahrs->airspeed_sensor_enabled()) { if (_ahrs.airspeed_sensor_enabled()) {
_throttle_dem = _throttle_dem + _integ6_state; _throttle_dem = _throttle_dem + _integ6_state;
} else { } else {
_throttle_dem = ff_throttle; _throttle_dem = ff_throttle;
@ -439,7 +439,7 @@ void AP_TECS::_update_throttle_option(int16_t throttle_nudge)
} }
// Calculate additional throttle for turn drag compensation including throttle nudging // Calculate additional throttle for turn drag compensation including throttle nudging
const Matrix3f &rotMat = _ahrs->get_dcm_matrix(); const Matrix3f &rotMat = _ahrs.get_dcm_matrix();
// Use the demanded rate of change of total energy as the feed-forward demand, but add // Use the demanded rate of change of total energy as the feed-forward demand, but add
// additional component which scales with (1/cos(bank angle) - 1) to compensate for induced // additional component which scales with (1/cos(bank angle) - 1) to compensate for induced
// drag increase during turns. // drag increase during turns.
@ -483,9 +483,9 @@ void AP_TECS::_update_pitch(void)
// or during takeoff/climbout where a minimum pitch angle is set to ensure height is gained. In this instance, if airspeed // or during takeoff/climbout where a minimum pitch angle is set to ensure height is gained. In this instance, if airspeed
// rises above the demanded value, the pitch angle will be increased by the TECS controller. // rises above the demanded value, the pitch angle will be increased by the TECS controller.
float SKE_weighting = constrain_float(_spdWeight, 0.0f, 2.0f); float SKE_weighting = constrain_float(_spdWeight, 0.0f, 2.0f);
if ( ( _underspeed || _climbOutDem ) && _ahrs->airspeed_sensor_enabled() ) { if ( ( _underspeed || _climbOutDem ) && _ahrs.airspeed_sensor_enabled() ) {
SKE_weighting = 2.0f; SKE_weighting = 2.0f;
} else if (!_ahrs->airspeed_sensor_enabled()) { } else if (!_ahrs.airspeed_sensor_enabled()) {
SKE_weighting = 0.0f; SKE_weighting = 0.0f;
} }
float SPE_weighting = 2.0f - SKE_weighting; float SPE_weighting = 2.0f - SKE_weighting;
@ -544,7 +544,7 @@ void AP_TECS::_initialise_states(int32_t ptchMinCO_cd, float hgt_afe)
_integ6_state = 0.0f; _integ6_state = 0.0f;
_integ7_state = 0.0f; _integ7_state = 0.0f;
_last_throttle_dem = aparm.throttle_cruise * 0.01f; _last_throttle_dem = aparm.throttle_cruise * 0.01f;
_last_pitch_dem = _ahrs->pitch; _last_pitch_dem = _ahrs.pitch;
_hgt_dem_adj_last = hgt_afe; _hgt_dem_adj_last = hgt_afe;
_hgt_dem_adj = _hgt_dem_adj_last; _hgt_dem_adj = _hgt_dem_adj_last;
_hgt_dem_prev = _hgt_dem_adj_last; _hgt_dem_prev = _hgt_dem_adj_last;
@ -621,7 +621,7 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm,
_update_energies(); _update_energies();
// Calculate throttle demand - use simple pitch to throttle if no airspeed sensor // Calculate throttle demand - use simple pitch to throttle if no airspeed sensor
if (_ahrs->airspeed_sensor_enabled()) { if (_ahrs.airspeed_sensor_enabled()) {
_update_throttle(); _update_throttle();
} else { } else {
_update_throttle_option(throttle_nudge); _update_throttle_option(throttle_nudge);

6
libraries/AP_TECS/AP_TECS.h
View File

@ -30,7 +30,7 @@
class AP_TECS : public AP_SpdHgtControl { class AP_TECS : public AP_SpdHgtControl {
public: public:
AP_TECS(AP_AHRS *ahrs, const AP_SpdHgtControl::AircraftParameters &parms) : AP_TECS(AP_AHRS &ahrs, const AP_SpdHgtControl::AircraftParameters &parms) :
_ahrs(ahrs), _ahrs(ahrs),
aparm(parms) aparm(parms)
{ {
@ -94,8 +94,8 @@ private:
// Last time update_pitch_throttle was called // Last time update_pitch_throttle was called
uint32_t _update_pitch_throttle_last_usec; uint32_t _update_pitch_throttle_last_usec;
// pointer to the AHRS object // reference to the AHRS object
AP_AHRS *_ahrs; AP_AHRS &_ahrs;
const AP_SpdHgtControl::AircraftParameters &aparm; const AP_SpdHgtControl::AircraftParameters &aparm;