Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-27 02:04:00 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_TECS: removed log_tuning structure

Browse Source
This commit is contained in:
Andrew Tridgell 2016-05-13 16:11:34 +10:00
parent bc911d15d1
commit ff97f52555
2 changed files with 30 additions and 71 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

76
libraries/AP_TECS/AP_TECS.cpp
View File

@ -753,30 +753,28 @@ void AP_TECS::_update_pitch(void)
// A SKE_weighting of 0 provides 100% priority to height control. This is used when no airspeed measurement is available // A SKE_weighting of 0 provides 100% priority to height control. This is used when no airspeed measurement is available
// A SKE_weighting of 2 provides 100% priority to speed control. This is used when an underspeed condition is detected. In this instance, if airspeed // A SKE_weighting of 2 provides 100% priority to speed control. This is used when an underspeed condition is detected. 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 (!_ahrs.airspeed_sensor_enabled()) { if (!_ahrs.airspeed_sensor_enabled()) {
SKE_weighting = 0.0f; _SKE_weighting = 0.0f;
} else if ( _flags.underspeed || _flight_stage == AP_TECS::FLIGHT_TAKEOFF || _flight_stage == AP_TECS::FLIGHT_LAND_ABORT) { } else if ( _flags.underspeed || _flight_stage == AP_TECS::FLIGHT_TAKEOFF || _flight_stage == AP_TECS::FLIGHT_LAND_ABORT) {
SKE_weighting = 2.0f; _SKE_weighting = 2.0f;
} else if (_flags.is_doing_auto_land) { } else if (_flags.is_doing_auto_land) {
if (_spdWeightLand < 0) { if (_spdWeightLand < 0) {
// use sliding scale from normal weight down to zero at landing // use sliding scale from normal weight down to zero at landing
float scaled_weight = _spdWeight * (1.0f - constrain_float(_path_proportion,0,1)); float scaled_weight = _spdWeight * (1.0f - constrain_float(_path_proportion,0,1));
SKE_weighting = constrain_float(scaled_weight, 0.0f, 2.0f); _SKE_weighting = constrain_float(scaled_weight, 0.0f, 2.0f);
} else { } else {
SKE_weighting = constrain_float(_spdWeightLand, 0.0f, 2.0f); _SKE_weighting = constrain_float(_spdWeightLand, 0.0f, 2.0f);
} }
} }
log_tuning.speed_weight = SKE_weighting; float SPE_weighting = 2.0f - _SKE_weighting;
float SPE_weighting = 2.0f - SKE_weighting;
// Calculate Specific Energy Balance demand, and error // Calculate Specific Energy Balance demand, and error
float SEB_dem = _SPE_dem * SPE_weighting - _SKE_dem * SKE_weighting; float SEB_dem = _SPE_dem * SPE_weighting - _SKE_dem * _SKE_weighting;
float SEBdot_dem = _SPEdot_dem * SPE_weighting - _SKEdot_dem * SKE_weighting; float SEBdot_dem = _SPEdot_dem * SPE_weighting - _SKEdot_dem * _SKE_weighting;
float SEB_error = SEB_dem - (_SPE_est * SPE_weighting - _SKE_est * SKE_weighting); float SEB_error = SEB_dem - (_SPE_est * SPE_weighting - _SKE_est * _SKE_weighting);
float SEBdot_error = SEBdot_dem - (_SPEdot * SPE_weighting - _SKEdot * SKE_weighting); float SEBdot_error = SEBdot_dem - (_SPEdot * SPE_weighting - _SKEdot * _SKE_weighting);
// Calculate integrator state, constraining input if pitch limits are exceeded // Calculate integrator state, constraining input if pitch limits are exceeded
float integSEB_input = SEB_error * _get_i_gain(); float integSEB_input = SEB_error * _get_i_gain();
@ -1023,41 +1021,21 @@ void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm,
// Calculate pitch demand // Calculate pitch demand
_update_pitch(); _update_pitch();
// Write internal variables to the log_tuning structure. This // log to DataFlash
// structure will be logged in dataflash at 10Hz DataFlash_Class::instance()->Log_Write("TECS", "TimeUS,h,dh,hdem,dhdem,spdem,sp,dsp,ith,iph,th,ph,dspdem,w,f", "QfffffffffffffB",
log_tuning.hgt_dem = _hgt_dem_adj; now,
log_tuning.hgt = _height; _height,
log_tuning.dhgt_dem = _hgt_rate_dem; _climb_rate,
log_tuning.dhgt = _climb_rate; _hgt_dem_adj,
log_tuning.spd_dem = _TAS_dem_adj; _hgt_rate_dem,
log_tuning.spd = _TAS_state; _TAS_dem_adj,
log_tuning.dspd = _vel_dot; _TAS_state,
log_tuning.ithr = _integTHR_state; _vel_dot,
log_tuning.iptch = _integSEB_state; _integTHR_state,
log_tuning.thr = _throttle_dem; _integSEB_state,
log_tuning.ptch = _pitch_dem; _throttle_dem,
log_tuning.dspd_dem = _TAS_rate_dem; _pitch_dem,
log_tuning.flags = _flags_byte; _TAS_rate_dem,
log_tuning.time_us = AP_HAL::micros64(); _SKE_weighting,
} _flags_byte);
// log the contents of the log_tuning structure to dataflash
void AP_TECS::log_data(DataFlash_Class &dataflash)
{
dataflash.Log_Write("TECS", "TimeUS,h,dh,hdem,dhdem,spdem,sp,dsp,ith,iph,th,ph,dspdem,w,f", "QfffffffffffffB",
AP_HAL::micros64(),
log_tuning.hgt,
log_tuning.dhgt,
log_tuning.hgt_dem,
log_tuning.dhgt_dem,
log_tuning.spd_dem,
log_tuning.spd,
log_tuning.dspd,
log_tuning.ithr,
log_tuning.iptch,
log_tuning.thr,
log_tuning.ptch,
log_tuning.dspd_dem,
log_tuning.speed_weight,
log_tuning.flags);
} }

25
libraries/AP_TECS/AP_TECS.h
View File

@ -69,9 +69,6 @@ public:
return _vel_dot; return _vel_dot;
} }
// log data on internal state of the controller. Called at 10Hz
void log_data(DataFlash_Class &dataflash);
// return current target airspeed // return current target airspeed
float get_target_airspeed(void) const { float get_target_airspeed(void) const {
return _TAS_dem / _ahrs.get_EAS2TAS(); return _TAS_dem / _ahrs.get_EAS2TAS();
@ -110,25 +107,6 @@ public:
// this supports the TECS_* user settable parameters // this supports the TECS_* user settable parameters
static const struct AP_Param::GroupInfo var_info[]; static const struct AP_Param::GroupInfo var_info[];
struct PACKED log_TECS_Tuning {
LOG_PACKET_HEADER;
uint64_t time_us;
float hgt;
float dhgt;
float hgt_dem;
float dhgt_dem;
float spd_dem;
float spd;
float dspd;
float ithr;
float iptch;
float thr;
float ptch;
float dspd_dem;
float speed_weight;
uint8_t flags;
} log_tuning;
private: private:
// Last time update_50Hz was called // Last time update_50Hz was called
uint64_t _update_50hz_last_usec; uint64_t _update_50hz_last_usec;
@ -300,6 +278,9 @@ private:
float _SPEdot; float _SPEdot;
float _SKEdot; float _SKEdot;
// speed weight for logging
float _SKE_weighting;
// Specific energy error quantities // Specific energy error quantities
float _STE_error; float _STE_error;