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

AP_TECS: make AHRS attitude member variables private

This commit is contained in:
Peter Barker 2024-01-12 23:40:23 +11:00 committed by Peter Barker
parent ba68d0fcbe
commit 7f43facb37
1 changed files with 6 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_TECS/AP_TECS.cpp
View File

@ -856,7 +856,7 @@ void AP_TECS::_update_throttle_without_airspeed(int16_t throttle_nudge)
// so that the throttle mapping adjusts for the effect of pitch control errors // so that the throttle mapping adjusts for the effect of pitch control errors
_pitch_demand_lpf.apply(_pitch_dem, _DT); _pitch_demand_lpf.apply(_pitch_dem, _DT);
const float pitch_demand_hpf = _pitch_dem - _pitch_demand_lpf.get(); const float pitch_demand_hpf = _pitch_dem - _pitch_demand_lpf.get();
_pitch_measured_lpf.apply(_ahrs.pitch, _DT); _pitch_measured_lpf.apply(_ahrs.get_pitch(), _DT);
const float pitch_corrected_lpf = _pitch_measured_lpf.get() - radians(0.01f * (float)aparm.pitch_trim_cd); const float pitch_corrected_lpf = _pitch_measured_lpf.get() - radians(0.01f * (float)aparm.pitch_trim_cd);
const float pitch_blended = pitch_demand_hpf + pitch_corrected_lpf; const float pitch_blended = pitch_demand_hpf + pitch_corrected_lpf;
@ -1107,7 +1107,7 @@ void AP_TECS::_initialise_states(int32_t ptchMinCO_cd, float hgt_afe)
_integSEBdot = 0.0f; _integSEBdot = 0.0f;
_integKE = 0.0f; _integKE = 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.get_pitch();
_hgt_afe = hgt_afe; _hgt_afe = hgt_afe;
_hgt_dem_in_prev = hgt_afe; _hgt_dem_in_prev = hgt_afe;
_hgt_dem_lpf = hgt_afe; _hgt_dem_lpf = hgt_afe;
@ -1138,8 +1138,8 @@ void AP_TECS::_initialise_states(int32_t ptchMinCO_cd, float hgt_afe)
const float fc = 1.0f / (M_2PI * _timeConst); const float fc = 1.0f / (M_2PI * _timeConst);
_pitch_demand_lpf.set_cutoff_frequency(fc); _pitch_demand_lpf.set_cutoff_frequency(fc);
_pitch_measured_lpf.set_cutoff_frequency(fc); _pitch_measured_lpf.set_cutoff_frequency(fc);
_pitch_demand_lpf.reset(_ahrs.pitch); _pitch_demand_lpf.reset(_ahrs.get_pitch());
_pitch_measured_lpf.reset(_ahrs.pitch); _pitch_measured_lpf.reset(_ahrs.get_pitch());
} else if (_flight_stage == AP_FixedWing::FlightStage::TAKEOFF || _flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING) { } else if (_flight_stage == AP_FixedWing::FlightStage::TAKEOFF || _flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING) {
_PITCHminf = 0.000174533f * ptchMinCO_cd; _PITCHminf = 0.000174533f * ptchMinCO_cd;
@ -1159,8 +1159,8 @@ void AP_TECS::_initialise_states(int32_t ptchMinCO_cd, float hgt_afe)
_max_climb_scaler = 1.0f; _max_climb_scaler = 1.0f;
_max_sink_scaler = 1.0f; _max_sink_scaler = 1.0f;
_pitch_demand_lpf.reset(_ahrs.pitch); _pitch_demand_lpf.reset(_ahrs.get_pitch());
_pitch_measured_lpf.reset(_ahrs.pitch); _pitch_measured_lpf.reset(_ahrs.get_pitch());
} }
if (_flight_stage != AP_FixedWing::FlightStage::TAKEOFF && _flight_stage != AP_FixedWing::FlightStage::ABORT_LANDING) { if (_flight_stage != AP_FixedWing::FlightStage::TAKEOFF && _flight_stage != AP_FixedWing::FlightStage::ABORT_LANDING) {