112 lines
3.1 KiB
C
112 lines
3.1 KiB
C
|
#ifndef __MATHS_H
|
|||
|
|
#define __MATHS_H
|
|||
|
|
|
|||
|
|
#include "math.h"
|
|||
|
|
#include "main.h"
|
|||
|
|
|
|||
|
|
#define LIMIT_MIN_MAX(x,min,max) (x) = (((x)<=(min))?(min):(((x)>=(max))?(max):(x)))
|
|||
|
|
|
|||
|
|
|
|||
|
|
#ifndef sq
|
|||
|
|
#define sq(x) ((x)*(x))
|
|||
|
|
#endif
|
|||
|
|
|
|||
|
|
#define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
|
|||
|
|
|
|||
|
|
// Use floating point M_PI instead explicitly.
|
|||
|
|
#define M_PIf 3.14159265358979323846f
|
|||
|
|
#define M_LN2f 0.69314718055994530942f
|
|||
|
|
#define M_Ef 2.71828182845904523536f
|
|||
|
|
|
|||
|
|
#define DEG2RAD 0.017453293f /* <20><>ת<EFBFBD><D7AA><EFBFBD><EFBFBD> <20><>/180 */
|
|||
|
|
#define RAD2DEG 57.29578f /* <20><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA> 180/<2F><> */
|
|||
|
|
|
|||
|
|
#define RAD (M_PIf / 180.0f)
|
|||
|
|
|
|||
|
|
#define DEGREES_TO_CENTIDEGREES(angle) ((angle) * 100)
|
|||
|
|
#define CENTIDEGREES_TO_DEGREES(angle) ((angle) / 100)
|
|||
|
|
|
|||
|
|
#define CENTIDEGREES_TO_DECIDEGREES(angle) ((angle) / 10)
|
|||
|
|
#define DECIDEGREES_TO_CENTIDEGREES(angle) ((angle) * 10)
|
|||
|
|
|
|||
|
|
#define DEGREES_TO_DECIDEGREES(angle) ((angle) * 10)
|
|||
|
|
#define DECIDEGREES_TO_DEGREES(angle) ((angle) / 10)
|
|||
|
|
|
|||
|
|
#define DEGREES_PER_DEKADEGREE 10
|
|||
|
|
#define DEGREES_TO_DEKADEGREES(angle) ((angle) / DEGREES_PER_DEKADEGREE)
|
|||
|
|
#define DEKADEGREES_TO_DEGREES(angle) ((angle) * DEGREES_PER_DEKADEGREE)
|
|||
|
|
|
|||
|
|
#define DEGREES_TO_RADIANS(angle) ((angle) * RAD)
|
|||
|
|
#define RADIANS_TO_DEGREES(angle) ((angle) / RAD)
|
|||
|
|
#define DECIDEGREES_TO_RADIANS(angle) (((angle) / 10.0f) * RAD)
|
|||
|
|
#define RADIANS_TO_DECIDEGREES(angle) (((angle) * 10.0f) / RAD)
|
|||
|
|
|
|||
|
|
#define RADIANS_TO_CENTIDEGREES(angle) (((angle) * 100.0f) / RAD)
|
|||
|
|
#define CENTIDEGREES_TO_RADIANS(angle) (((angle) / 100.0f) * RAD)
|
|||
|
|
|
|||
|
|
#define MIN(a, b) (((a) < (b)) ? (a) : (b))
|
|||
|
|
#define MAX(a, b) (((a) > (b)) ? (a) : (b))
|
|||
|
|
|
|||
|
|
#define ABS(x) (((x) < 0) ? (-x) : (x))
|
|||
|
|
|
|||
|
|
|
|||
|
|
typedef enum {
|
|||
|
|
X = 0,
|
|||
|
|
Y,
|
|||
|
|
Z
|
|||
|
|
} axis_e;
|
|||
|
|
|
|||
|
|
typedef struct stdev_s
|
|||
|
|
{
|
|||
|
|
float m_oldM, m_newM, m_oldS, m_newS;
|
|||
|
|
int m_n;
|
|||
|
|
} stdev_t;
|
|||
|
|
|
|||
|
|
|
|||
|
|
// Floating point Euler angles.
|
|||
|
|
// Be carefull, could be either of degrees or radians.
|
|||
|
|
typedef struct fp_angles {
|
|||
|
|
float roll;
|
|||
|
|
float pitch;
|
|||
|
|
float yaw;
|
|||
|
|
} fp_angles_def;
|
|||
|
|
|
|||
|
|
|
|||
|
|
typedef union {
|
|||
|
|
float raw[3];
|
|||
|
|
fp_angles_def angles;
|
|||
|
|
} fp_angles_t;
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
typedef struct {
|
|||
|
|
float XtY[4];
|
|||
|
|
float XtX[4][4];
|
|||
|
|
} sensorCalibrationState_t;
|
|||
|
|
|
|||
|
|
|
|||
|
|
float constrainFloat(float data,float min,float max);
|
|||
|
|
int16_t constrainInt16_t(int16_t data,int16_t min,int16_t max);
|
|||
|
|
uint16_t constrainUint16_t(uint16_t data,uint16_t min,uint16_t max);
|
|||
|
|
|
|||
|
|
|
|||
|
|
void sensorCalibrationResetState(sensorCalibrationState_t * state);
|
|||
|
|
void sensorCalibrationPushSampleForOffsetCalculation(sensorCalibrationState_t * state, int32_t sample[3]);
|
|||
|
|
void sensorCalibrationSolveForOffset(sensorCalibrationState_t * state, float result[3]);
|
|||
|
|
float sin_approx(float x);
|
|||
|
|
float cos_approx(float x);
|
|||
|
|
float atan2_approx(float y, float x);
|
|||
|
|
float acos_approx(float x);
|
|||
|
|
void devClear(stdev_t *dev);
|
|||
|
|
void devPush(stdev_t *dev, float x);
|
|||
|
|
float devStandardDeviation(stdev_t *dev);
|
|||
|
|
void buildRotationMatrix(fp_angles_t *delta, float matrix[3][3]);
|
|||
|
|
float map(long x, long in_min, long in_max, float out_min, float out_max);
|
|||
|
|
int32_t applyDeadband(int32_t value, int32_t deadband);
|
|||
|
|
float applyDeadbandF(float value, float deadband);
|
|||
|
|
float mapFloat(float x, float in_min, float in_max, float out_min, float out_max);
|
|||
|
|
|
|||
|
|
#endif
|
|||
|
|
|
|||
|
|
|