mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-15 20:35:33 +03:00
cffdfb782c
full gyro scale is used now and a new pid with float calculations was added based on PIDrewrite eeprom size was also increased from 1kB to 2kB Conflicts: src/config.c src/drivers/accgyro_l3g4200d.c src/drivers/accgyro_mpu3050.c src/drivers/accgyro_mpu6050.c src/flight_imu.c src/mw.c src/mw.h src/serial_cli.c src/serial_msp.c src/utils.c src/utils.h
59 lines
1.1 KiB
C
59 lines
1.1 KiB
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;
|
|
}
|
|
|
|
float constrainf(float amt, float low, float 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;
|
|
}
|
|
|