mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-26 01:35:41 +03:00
49 lines
956 B
C
49 lines
956 B
C
#include <stdint.h>
|
|
#include <math.h>
|
|
|
|
#include "maths.h"
|
|
|
|
int constrain(int amt, int low, int high)
|
|
{
|
|
if (amt < low)
|
|
return low;
|
|
else if (amt > high)
|
|
return high;
|
|
else
|
|
return amt;
|
|
}
|
|
|
|
void devClear(stdev_t *dev)
|
|
{
|
|
dev->m_n = 0;
|
|
}
|
|
|
|
void devPush(stdev_t *dev, float x)
|
|
{
|
|
dev->m_n++;
|
|
if (dev->m_n == 1) {
|
|
dev->m_oldM = dev->m_newM = x;
|
|
dev->m_oldS = 0.0f;
|
|
} else {
|
|
dev->m_newM = dev->m_oldM + (x - dev->m_oldM) / dev->m_n;
|
|
dev->m_newS = dev->m_oldS + (x - dev->m_oldM) * (x - dev->m_newM);
|
|
dev->m_oldM = dev->m_newM;
|
|
dev->m_oldS = dev->m_newS;
|
|
}
|
|
}
|
|
|
|
float devVariance(stdev_t *dev)
|
|
{
|
|
return ((dev->m_n > 1) ? dev->m_newS / (dev->m_n - 1) : 0.0f);
|
|
}
|
|
|
|
float devStandardDeviation(stdev_t *dev)
|
|
{
|
|
return sqrtf(devVariance(dev));
|
|
}
|
|
|
|
float degreesToRadians(int16_t degrees)
|
|
{
|
|
return degrees * RAD;
|
|
}
|
|
|