forked from Archive/PX4-Autopilot
Merge branch 'beta' into beta_merge4
This commit is contained in:
commit
85007d8952
|
@ -18,9 +18,12 @@ float32 airspeed_filtered
|
||||||
float32 airspeedDerivativeSp
|
float32 airspeedDerivativeSp
|
||||||
float32 airspeedDerivative
|
float32 airspeedDerivative
|
||||||
|
|
||||||
float32 totalEnergyRateSp
|
float32 totalEnergyError
|
||||||
float32 totalEnergyRate
|
float32 energyDistributionError
|
||||||
float32 energyDistributionRateSp
|
float32 totalEnergyRateError
|
||||||
float32 energyDistributionRate
|
float32 energyDistributionRateError
|
||||||
|
|
||||||
|
float32 throttle_sp
|
||||||
|
float32 pitch_sp
|
||||||
|
|
||||||
uint8 mode
|
uint8 mode
|
||||||
|
|
|
@ -309,12 +309,12 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM
|
||||||
// Calculate total energy values
|
// Calculate total energy values
|
||||||
_STE_error = _SPE_dem - _SPE_est + _SKE_dem - _SKE_est;
|
_STE_error = _SPE_dem - _SPE_est + _SKE_dem - _SKE_est;
|
||||||
float STEdot_dem = constrain((_SPEdot_dem + _SKEdot_dem), _STEdot_min, _STEdot_max);
|
float STEdot_dem = constrain((_SPEdot_dem + _SKEdot_dem), _STEdot_min, _STEdot_max);
|
||||||
float STEdot_error = STEdot_dem - _SPEdot - _SKEdot;
|
_STEdot_error = STEdot_dem - _SPEdot - _SKEdot;
|
||||||
|
|
||||||
// Apply 0.5 second first order filter to STEdot_error
|
// Apply 0.5 second first order filter to STEdot_error
|
||||||
// This is required to remove accelerometer noise from the measurement
|
// This is required to remove accelerometer noise from the measurement
|
||||||
STEdot_error = 0.2f * STEdot_error + 0.8f * _STEdotErrLast;
|
_STEdot_error = 0.2f * _STEdot_error + 0.8f * _STEdotErrLast;
|
||||||
_STEdotErrLast = STEdot_error;
|
_STEdotErrLast = _STEdot_error;
|
||||||
|
|
||||||
// Calculate throttle demand
|
// Calculate throttle demand
|
||||||
// If underspeed condition is set, then demand full throttle
|
// If underspeed condition is set, then demand full throttle
|
||||||
|
@ -342,7 +342,7 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM
|
||||||
}
|
}
|
||||||
|
|
||||||
// Calculate PD + FF throttle and constrain to avoid blow-up of the integrator later
|
// Calculate PD + FF throttle and constrain to avoid blow-up of the integrator later
|
||||||
_throttle_dem = (_STE_error + STEdot_error * _thrDamp) * K_STE2Thr + ff_throttle;
|
_throttle_dem = (_STE_error + _STEdot_error * _thrDamp) * K_STE2Thr + ff_throttle;
|
||||||
_throttle_dem = constrain(_throttle_dem, _THRminf, _THRmaxf);
|
_throttle_dem = constrain(_throttle_dem, _THRminf, _THRmaxf);
|
||||||
|
|
||||||
// Rate limit PD + FF throttle
|
// Rate limit PD + FF throttle
|
||||||
|
@ -438,11 +438,11 @@ void TECS::_update_pitch(void)
|
||||||
// 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);
|
_SEB_error = SEB_dem - (_SPE_est * SPE_weighting - _SKE_est * SKE_weighting);
|
||||||
float SEBdot_error = SEBdot_dem - (_SPEdot * SPE_weighting - _SKEdot * SKE_weighting);
|
_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 integ7_input = SEB_error * _integGain;
|
float integ7_input = _SEB_error * _integGain;
|
||||||
|
|
||||||
if (_pitch_dem_unc > _PITCHmaxf) {
|
if (_pitch_dem_unc > _PITCHmaxf) {
|
||||||
integ7_input = min(integ7_input, _PITCHmaxf - _pitch_dem_unc);
|
integ7_input = min(integ7_input, _PITCHmaxf - _pitch_dem_unc);
|
||||||
|
@ -460,7 +460,7 @@ void TECS::_update_pitch(void)
|
||||||
// demand equal to the minimum value (which is )set by the mission plan during this mode). Otherwise the
|
// demand equal to the minimum value (which is )set by the mission plan during this mode). Otherwise the
|
||||||
// integrator has to catch up before the nose can be raised to reduce speed during climbout.
|
// integrator has to catch up before the nose can be raised to reduce speed during climbout.
|
||||||
float gainInv = (_integ5_state * _timeConst * CONSTANTS_ONE_G);
|
float gainInv = (_integ5_state * _timeConst * CONSTANTS_ONE_G);
|
||||||
float temp = SEB_error + SEBdot_error * _ptchDamp + SEBdot_dem * _timeConst;
|
float temp = _SEB_error + _SEBdot_error * _ptchDamp + SEBdot_dem * _timeConst;
|
||||||
if (_climbOutDem)
|
if (_climbOutDem)
|
||||||
{
|
{
|
||||||
temp += _PITCHminf * gainInv;
|
temp += _PITCHminf * gainInv;
|
||||||
|
@ -598,18 +598,27 @@ void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, f
|
||||||
_tecs_state.mode = ECL_TECS_MODE_NORMAL;
|
_tecs_state.mode = ECL_TECS_MODE_NORMAL;
|
||||||
}
|
}
|
||||||
|
|
||||||
_tecs_state.hgt_dem = _hgt_dem_adj;
|
_tecs_state.altitude_sp = _hgt_dem_adj;
|
||||||
_tecs_state.hgt = _integ3_state;
|
_tecs_state.altitude_filtered = _integ3_state;
|
||||||
_tecs_state.dhgt_dem = _hgt_rate_dem;
|
_tecs_state.altitude_rate_sp = _hgt_rate_dem;
|
||||||
_tecs_state.dhgt = _integ2_state;
|
_tecs_state.altitude_rate = _integ2_state;
|
||||||
_tecs_state.spd_dem = _TAS_dem_adj;
|
|
||||||
_tecs_state.spd = _integ5_state;
|
_tecs_state.airspeed_sp = _TAS_dem_adj;
|
||||||
_tecs_state.dspd = _vel_dot;
|
_tecs_state.airspeed_rate_sp = _TAS_rate_dem;
|
||||||
_tecs_state.ithr = _integ6_state;
|
_tecs_state.airspeed_filtered = _integ5_state;
|
||||||
_tecs_state.iptch = _integ7_state;
|
_tecs_state.airspeed_rate = _vel_dot;
|
||||||
_tecs_state.thr = _throttle_dem;
|
|
||||||
_tecs_state.ptch = _pitch_dem;
|
_tecs_state.total_energy_error = _STE_error;
|
||||||
_tecs_state.dspd_dem = _TAS_rate_dem;
|
_tecs_state.energy_distribution_error = _SEB_error;
|
||||||
|
_tecs_state.total_energy_rate_error = _STEdot_error;
|
||||||
|
_tecs_state.energy_distribution_error = _SEBdot_error;
|
||||||
|
|
||||||
|
_tecs_state.energy_error_integ = _integ6_state;
|
||||||
|
_tecs_state.energy_distribution_error_integ = _integ7_state;
|
||||||
|
|
||||||
|
|
||||||
|
_tecs_state.throttle_sp = _throttle_dem;
|
||||||
|
_tecs_state.pitch_sp = _pitch_dem;
|
||||||
|
|
||||||
_update_pitch_throttle_last_usec = now;
|
_update_pitch_throttle_last_usec = now;
|
||||||
|
|
||||||
|
|
|
@ -79,6 +79,10 @@ public:
|
||||||
_SKE_est(0.0f),
|
_SKE_est(0.0f),
|
||||||
_SPEdot(0.0f),
|
_SPEdot(0.0f),
|
||||||
_SKEdot(0.0f),
|
_SKEdot(0.0f),
|
||||||
|
_STE_error(0.0f),
|
||||||
|
_STEdot_error(0.0f),
|
||||||
|
_SEB_error(0.0f),
|
||||||
|
_SEBdot_error(0.0f),
|
||||||
_airspeed_enabled(false),
|
_airspeed_enabled(false),
|
||||||
_states_initalized(false),
|
_states_initalized(false),
|
||||||
_in_air(false),
|
_in_air(false),
|
||||||
|
@ -137,18 +141,22 @@ public:
|
||||||
|
|
||||||
struct tecs_state {
|
struct tecs_state {
|
||||||
uint64_t timestamp;
|
uint64_t timestamp;
|
||||||
float hgt;
|
float altitude_filtered;
|
||||||
float dhgt;
|
float altitude_sp;
|
||||||
float hgt_dem;
|
float altitude_rate;
|
||||||
float dhgt_dem;
|
float altitude_rate_sp;
|
||||||
float spd_dem;
|
float airspeed_filtered;
|
||||||
float spd;
|
float airspeed_sp;
|
||||||
float dspd;
|
float airspeed_rate;
|
||||||
float ithr;
|
float airspeed_rate_sp;
|
||||||
float iptch;
|
float energy_error_integ;
|
||||||
float thr;
|
float energy_distribution_error_integ;
|
||||||
float ptch;
|
float total_energy_error;
|
||||||
float dspd_dem;
|
float total_energy_rate_error;
|
||||||
|
float energy_distribution_error;
|
||||||
|
float energy_distribution_rate_error;
|
||||||
|
float throttle_sp;
|
||||||
|
float pitch_sp;
|
||||||
enum ECL_TECS_MODE mode;
|
enum ECL_TECS_MODE mode;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
@ -376,6 +384,15 @@ private:
|
||||||
// Specific energy error quantities
|
// Specific energy error quantities
|
||||||
float _STE_error;
|
float _STE_error;
|
||||||
|
|
||||||
|
// Energy error rate
|
||||||
|
float _STEdot_error;
|
||||||
|
|
||||||
|
// Specific energy balance error
|
||||||
|
float _SEB_error;
|
||||||
|
|
||||||
|
// Specific energy balance error rate
|
||||||
|
float _SEBdot_error;
|
||||||
|
|
||||||
// Time since last update of main TECS loop (seconds)
|
// Time since last update of main TECS loop (seconds)
|
||||||
float _DT;
|
float _DT;
|
||||||
|
|
||||||
|
|
|
@ -1920,22 +1920,25 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
t.altitudeSp = s.hgt_dem;
|
t.altitudeSp = s.altitude_sp;
|
||||||
t.altitude_filtered = s.hgt;
|
t.altitude_filtered = s.altitude_filtered;
|
||||||
t.airspeedSp = s.spd_dem;
|
t.airspeedSp = s.airspeed_sp;
|
||||||
t.airspeed_filtered = s.spd;
|
t.airspeed_filtered = s.airspeed_filtered;
|
||||||
|
|
||||||
t.flightPathAngleSp = s.dhgt_dem;
|
t.flightPathAngleSp = s.altitude_rate_sp;
|
||||||
t.flightPathAngle = s.dhgt;
|
t.flightPathAngle = s.altitude_rate;
|
||||||
t.flightPathAngleFiltered = s.dhgt;
|
t.flightPathAngleFiltered = s.altitude_rate;
|
||||||
|
|
||||||
t.airspeedDerivativeSp = s.dspd_dem;
|
t.airspeedDerivativeSp = s.airspeed_rate_sp;
|
||||||
t.airspeedDerivative = s.dspd;
|
t.airspeedDerivative = s.airspeed_rate;
|
||||||
|
|
||||||
t.totalEnergyRateSp = s.thr;
|
t.totalEnergyError = s.total_energy_error;
|
||||||
t.totalEnergyRate = s.ithr;
|
t.totalEnergyRateError = s.total_energy_rate_error;
|
||||||
t.energyDistributionRateSp = s.ptch;
|
t.energyDistributionError = s.energy_distribution_error;
|
||||||
t.energyDistributionRate = s.iptch;
|
t.energyDistributionRateError = s.energy_distribution_rate_error;
|
||||||
|
|
||||||
|
t.throttle_sp = s.throttle_sp;
|
||||||
|
t.pitch_sp = s.pitch_sp;
|
||||||
|
|
||||||
if (_tecs_status_pub != nullptr) {
|
if (_tecs_status_pub != nullptr) {
|
||||||
orb_publish(ORB_ID(tecs_status), _tecs_status_pub, &t);
|
orb_publish(ORB_ID(tecs_status), _tecs_status_pub, &t);
|
||||||
|
|
|
@ -226,17 +226,17 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
|
||||||
* is running) */
|
* is running) */
|
||||||
limitOverride.applyOverride(*outputLimiterThrottle, *outputLimiterPitch);
|
limitOverride.applyOverride(*outputLimiterThrottle, *outputLimiterPitch);
|
||||||
|
|
||||||
/* Write part of the status message */
|
// /* Write part of the status message */
|
||||||
_status.flightPathAngleSp = flightPathAngleSp;
|
// _status.flightPathAngleSp = flightPathAngleSp;
|
||||||
_status.flightPathAngle = flightPathAngle;
|
// _status.flightPathAngle = flightPathAngle;
|
||||||
_status.flightPathAngleFiltered = flightPathAngleFiltered;
|
// _status.flightPathAngleFiltered = flightPathAngleFiltered;
|
||||||
_status.airspeedDerivativeSp = airspeedDerivativeSp;
|
// _status.airspeedDerivativeSp = airspeedDerivativeSp;
|
||||||
_status.airspeedDerivative = airspeedDerivative;
|
// _status.airspeedDerivative = airspeedDerivative;
|
||||||
_status.totalEnergyRateSp = totalEnergyRateSp;
|
// _status.totalEnergyRateSp = totalEnergyRateSp;
|
||||||
_status.totalEnergyRate = totalEnergyRate;
|
// _status.totalEnergyRate = totalEnergyRate;
|
||||||
_status.energyDistributionRateSp = energyDistributionRateSp;
|
// _status.energyDistributionRateSp = energyDistributionRateSp;
|
||||||
_status.energyDistributionRate = energyDistributionRate;
|
// _status.energyDistributionRate = energyDistributionRate;
|
||||||
_status.mode = mode;
|
// _status.mode = mode;
|
||||||
|
|
||||||
/** update control blocks **/
|
/** update control blocks **/
|
||||||
/* update total energy rate control block */
|
/* update total energy rate control block */
|
||||||
|
|
|
@ -1900,15 +1900,16 @@ int sdlog2_thread_main(int argc, char *argv[])
|
||||||
log_msg.body.log_TECS.altitudeFiltered = buf.tecs_status.altitude_filtered;
|
log_msg.body.log_TECS.altitudeFiltered = buf.tecs_status.altitude_filtered;
|
||||||
log_msg.body.log_TECS.flightPathAngleSp = buf.tecs_status.flightPathAngleSp;
|
log_msg.body.log_TECS.flightPathAngleSp = buf.tecs_status.flightPathAngleSp;
|
||||||
log_msg.body.log_TECS.flightPathAngle = buf.tecs_status.flightPathAngle;
|
log_msg.body.log_TECS.flightPathAngle = buf.tecs_status.flightPathAngle;
|
||||||
log_msg.body.log_TECS.flightPathAngleFiltered = buf.tecs_status.flightPathAngleFiltered;
|
|
||||||
log_msg.body.log_TECS.airspeedSp = buf.tecs_status.airspeedSp;
|
log_msg.body.log_TECS.airspeedSp = buf.tecs_status.airspeedSp;
|
||||||
log_msg.body.log_TECS.airspeedFiltered = buf.tecs_status.airspeed_filtered;
|
log_msg.body.log_TECS.airspeedFiltered = buf.tecs_status.airspeed_filtered;
|
||||||
log_msg.body.log_TECS.airspeedDerivativeSp = buf.tecs_status.airspeedDerivativeSp;
|
log_msg.body.log_TECS.airspeedDerivativeSp = buf.tecs_status.airspeedDerivativeSp;
|
||||||
log_msg.body.log_TECS.airspeedDerivative = buf.tecs_status.airspeedDerivative;
|
log_msg.body.log_TECS.airspeedDerivative = buf.tecs_status.airspeedDerivative;
|
||||||
log_msg.body.log_TECS.totalEnergyRateSp = buf.tecs_status.totalEnergyRateSp;
|
log_msg.body.log_TECS.totalEnergyError = buf.tecs_status.totalEnergyError;
|
||||||
log_msg.body.log_TECS.totalEnergyRate = buf.tecs_status.totalEnergyRate;
|
log_msg.body.log_TECS.energyDistributionError = buf.tecs_status.energyDistributionError;
|
||||||
log_msg.body.log_TECS.energyDistributionRateSp = buf.tecs_status.energyDistributionRateSp;
|
log_msg.body.log_TECS.totalEnergyRateError = buf.tecs_status.totalEnergyRateError;
|
||||||
log_msg.body.log_TECS.energyDistributionRate = buf.tecs_status.energyDistributionRate;
|
log_msg.body.log_TECS.energyDistributionRateError = buf.tecs_status.energyDistributionRateError;
|
||||||
|
log_msg.body.log_TECS.throttle_sp = buf.tecs_status.throttle_sp;
|
||||||
|
log_msg.body.log_TECS.pitch_sp = buf.tecs_status.pitch_sp;
|
||||||
log_msg.body.log_TECS.mode = (uint8_t)buf.tecs_status.mode;
|
log_msg.body.log_TECS.mode = (uint8_t)buf.tecs_status.mode;
|
||||||
LOGBUFFER_WRITE_AND_COUNT(TECS);
|
LOGBUFFER_WRITE_AND_COUNT(TECS);
|
||||||
}
|
}
|
||||||
|
|
|
@ -363,16 +363,16 @@ struct log_TECS_s {
|
||||||
float altitudeFiltered;
|
float altitudeFiltered;
|
||||||
float flightPathAngleSp;
|
float flightPathAngleSp;
|
||||||
float flightPathAngle;
|
float flightPathAngle;
|
||||||
float flightPathAngleFiltered;
|
|
||||||
float airspeedSp;
|
float airspeedSp;
|
||||||
float airspeedFiltered;
|
float airspeedFiltered;
|
||||||
float airspeedDerivativeSp;
|
float airspeedDerivativeSp;
|
||||||
float airspeedDerivative;
|
float airspeedDerivative;
|
||||||
|
float totalEnergyError;
|
||||||
float totalEnergyRateSp;
|
float totalEnergyRateError;
|
||||||
float totalEnergyRate;
|
float energyDistributionError;
|
||||||
float energyDistributionRateSp;
|
float energyDistributionRateError;
|
||||||
float energyDistributionRate;
|
float throttle_sp;
|
||||||
|
float pitch_sp;
|
||||||
|
|
||||||
uint8_t mode;
|
uint8_t mode;
|
||||||
};
|
};
|
||||||
|
@ -545,7 +545,7 @@ static const struct log_format_s log_formats[] = {
|
||||||
LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
|
LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
|
||||||
LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
|
LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
|
||||||
LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
|
LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
|
||||||
LOG_FORMAT(TECS, "fffffffffffffB", "ASP,AF,FSP,F,FF,AsSP,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"),
|
LOG_FORMAT(TECS, "ffffffffffffffB", "ASP,AF,FSP,F,AsSP,AsF,AsDSP,AsD,EE,EDE,ERE,EDRE,Thr,Ptch,M"),
|
||||||
LOG_FORMAT(WIND, "ffff", "X,Y,CovX,CovY"),
|
LOG_FORMAT(WIND, "ffff", "X,Y,CovX,CovY"),
|
||||||
LOG_FORMAT(ENCD, "qfqf", "cnt0,vel0,cnt1,vel1"),
|
LOG_FORMAT(ENCD, "qfqf", "cnt0,vel0,cnt1,vel1"),
|
||||||
LOG_FORMAT(TSYN, "Q", "TimeOffset"),
|
LOG_FORMAT(TSYN, "Q", "TimeOffset"),
|
||||||
|
|
Loading…
Reference in New Issue