mirror of
https://github.com/RobTillaart/Arduino.git
synced 2024-10-03 18:09:02 -04:00
74 lines
1.4 KiB
C++
74 lines
1.4 KiB
C++
//
|
|
// FILE: runningAngle.cpp
|
|
// AUTHOR: Rob Tillaart
|
|
// VERSION: 0.1.4
|
|
// PURPOSE: Library to average angles by means of low pass filtering with wrapping.
|
|
// URL: https://github.com/RobTillaart/runningAngle
|
|
// RELATED: https://github.com/RobTillaart/AverageAngle
|
|
//
|
|
|
|
// HISTORY:
|
|
// 0.1.0 2020-10-28 initial version (based on code of Edgar Bonet)
|
|
// 0.1.1 2021-01-07 Arduino-CI + unit test
|
|
// 0.1.2 2021-05-27 Arduino-lint
|
|
// 0.1.3 2021-12-28 update library.json, readme, license, minor edits
|
|
// 0.1.4 2022-05-29 add GRADIANS support.
|
|
|
|
|
|
#include "runningAngle.h"
|
|
|
|
|
|
runningAngle::runningAngle(const enum AngleType type)
|
|
{
|
|
_type = type;
|
|
reset();
|
|
}
|
|
|
|
|
|
void runningAngle::reset()
|
|
{
|
|
_average = 0;
|
|
_weight = 0.80;
|
|
_reset = true;
|
|
}
|
|
|
|
|
|
float runningAngle::add(float angle)
|
|
{
|
|
if (_reset)
|
|
{
|
|
_average = angle;
|
|
_reset = false;
|
|
}
|
|
else
|
|
{
|
|
_average = wrap(_average + _weight * wrap(angle - _average));
|
|
}
|
|
return _average;
|
|
}
|
|
|
|
|
|
float runningAngle::wrap(float angle)
|
|
{
|
|
if (_type == DEGREES)
|
|
{
|
|
while (angle < -180) angle += 360;
|
|
while (angle >= 180) angle -= 360;
|
|
}
|
|
else if (_type == RADIANS)
|
|
{
|
|
while (angle < -PI) angle += TWO_PI;
|
|
while (angle >= PI) angle -= TWO_PI;
|
|
}
|
|
else // GRADIANS
|
|
{
|
|
while (angle < -200) angle += 400;
|
|
while (angle >= 200) angle -= 400;
|
|
}
|
|
return angle;
|
|
}
|
|
|
|
|
|
// -- END OF FILE --
|
|
|