ardupilot/libraries/AP_Common/AP_Loop.cpp

58 lines
1.5 KiB
C++

/*
* AP_Loop.pde
* Copyright (C) James Goppert 2010 <james.goppert@gmail.com>
*
* This file is free software: you can redistribute it and/or modify it
* under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "AP_Loop.h"
namespace apo
{
Loop::Loop(float frequency, void (*fptr)(void *), void * data) :
_fptr(fptr),
_data(data),
_period(1.0e6/frequency),
_timeStamp(micros()),
_subLoops(),
_load(0),
_dt(0)
{
}
void Loop::update()
{
// quick exit if not ready
if (micros() - _timeStamp < _period) return;
// update time stamp
uint32_t timeStamp0 = _timeStamp;
_timeStamp = micros();
_dt = (_timeStamp - timeStamp0)/1.0e6;
// update sub loops
for (int i=0; i<_subLoops.getSize(); i++) _subLoops[i]->update();
// callback function
if (_fptr) _fptr(_data);
// calculated load with a low pass filter
_load = 0.9*_load + 10*(float(micros()-_timeStamp)/(_timeStamp-timeStamp0));
}
} // apo
// vim:ts=4:sw=4:expandtab