fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

mtecs: do not calculate derivative in first iteration

Browse Source
This commit is contained in:
Thomas Gubler 2014-06-01 09:38:14 +02:00
parent 7716a1816d
commit db925db460
3 changed files with 11 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
View File

@ -861,6 +861,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* reset integrators */
if (_mTecs.getEnabled()) {
_mTecs.resetIntegrators();
_mTecs.resetDerivatives(_airspeed.true_airspeed_m_s);
}
}

6
src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
View File

@ -261,6 +261,12 @@ void mTecs::resetIntegrators()
_firstIterationAfterReset = true;
}
void mTecs::resetDerivatives(float airspeed)
{
_airspeedDerivative.setU(airspeed);
}
void mTecs::updateTimeMeasurement()
{
if (!_dtCalculated) {

4
src/modules/fw_pos_control_l1/mtecs/mTecs.h
View File

@ -139,6 +139,10 @@ public:
*/
void resetIntegrators();
/*
* Reset all derivative calculations
*/
void resetDerivatives(float airspeed);
/* Accessors */
bool getEnabled() {return _mTecsEnabled.get() > 0;}