Added #define for imu debug output (+16 squashed commit)
Squashed local commits: from : e4265d4a13f63f82d5cf55eea2c091622f96660b up to (inc): 72416dc74745fa8bae1aded79aa4b9ed0e389076
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity)
ProDrone
parent
c6f5b98a79
commit
45a4f11f92
1
Makefile
1
Makefile
|
|
@ -611,6 +611,7 @@ LDFLAGS = -lm \
|
|||
-static \
|
||||
-Wl,-gc-sections,-Map,$(TARGET_MAP) \
|
||||
-Wl,-L$(LINKER_DIR) \
|
||||
-Wl,--cref \
|
||||
-T$(LD_SCRIPT)
|
||||
|
||||
###############################################################################
|
||||
|
|
|
|||
|
|
@ -204,6 +204,7 @@ Re-apply any new defaults as desired.
|
|||
| `rx_max_usec` | Defines the longest pulse width value used when ensuring the channel value is valid. If the receiver gives a pulse value higher than this value then the channel will be marked as bad and will default to the value of `mid_rc`. | 100 | 3000 | 2115 | Profile | UINT16 |
|
||||
| `gimbal_mode` | When feature SERVO_TILT is enabled, this can be either 0=normal gimbal (default) or 1=tiltmix gimbal | 0 | 1 | 0 | Profile | UINT8 |
|
||||
| `acc_hardware` | This is used to suggest which accelerometer driver should load, or to force no accelerometer in case gyro-only flight is needed. Default (0) will attempt to auto-detect among enabled drivers. Otherwise, to force a particular device, set it to 2 for ADXL345, 3 for MPU6050 integrated accelerometer, 4 for MMA8452, 5 for BMA280, 6 for LSM303DLHC, 7 for SPI_MPU6000, 8 for SPI_MPU6500 or 1 to disable accelerometer alltogether - resulting in gyro-only operation. | 0 | 9 | 0 | Master | UINT8 |
|
||||
| `acc_for_fast_looptime` | This shortens accelerometer processing time by using 1 out of 9 samples. Intended use is in combination with fast looptimes. Default (0) - standard, one sample per cycle; (1) - 1 out of 9 samples. | 0 | 1 | 0 | Master | UINT8 |
|
||||
| `acc_lpf_factor` | This setting controls the Low Pass Filter factor for ACC. Increasing this value reduces ACC noise (visible in GUI), but would increase ACC lag time. Zero = no filter | 0 | 250 | 4 | Profile | UINT8 |
|
||||
| `accxy_deadband` | | 0 | 100 | 40 | Profile | UINT8 |
|
||||
| `accz_deadband` | | 0 | 100 | 40 | Profile | UINT8 |
|
||||
|
|
|
|||
|
|
@ -21,12 +21,15 @@
|
|||
#include "axis.h"
|
||||
#include "maths.h"
|
||||
|
||||
#if defined(FAST_MATH) || defined(VERY_FAST_MATH)
|
||||
#if defined(VERY_FAST_MATH)
|
||||
|
||||
// http://lolengine.net/blog/2011/12/21/better-function-approximations
|
||||
// Chebyshev http://stackoverflow.com/questions/345085/how-do-trigonometric-functions-work/345117#345117
|
||||
// Thanks for ledvinap for making such accuracy possible! See: https://github.com/cleanflight/cleanflight/issues/940#issuecomment-110323384
|
||||
// https://github.com/Crashpilot1000/HarakiriWebstore1/blob/master/src/mw.c#L1235
|
||||
#if defined(FAST_TRIGONOMETRY) || defined(EVEN_FASTER_TRIGONOMETRY)
|
||||
#if defined(EVEN_FASTER_TRIGONOMETRY)
|
||||
// sin_approx maximum absolute error = 2.305023e-06
|
||||
// cos_approx maximum absolute error = 2.857298e-06
|
||||
#define sinPolyCoef3 -1.666568107e-1f
|
||||
#define sinPolyCoef5 8.312366210e-3f
|
||||
#define sinPolyCoef7 -1.849218155e-4f
|
||||
|
|
@ -37,7 +40,6 @@
|
|||
#define sinPolyCoef7 -1.980661520e-4f // Double: -1.980661520135080504411629636078917643846e-4
|
||||
#define sinPolyCoef9 2.600054768e-6f // Double: 2.600054767890361277123254766503271638682e-6
|
||||
#endif
|
||||
|
||||
float sin_approx(float x)
|
||||
{
|
||||
int32_t xint = x;
|
||||
|
|
@ -54,6 +56,47 @@ float cos_approx(float x)
|
|||
{
|
||||
return sin_approx(x + (0.5f * M_PIf));
|
||||
}
|
||||
|
||||
// Initial implementation by Crashpilot1000 (https://github.com/Crashpilot1000/HarakiriWebstore1/blob/396715f73c6fcf859e0db0f34e12fe44bace6483/src/mw.c#L1292)
|
||||
// Polynomial coefficients by Andor (http://www.dsprelated.com/showthread/comp.dsp/21872-1.php) optimized by Ledvinap to save one multiplication
|
||||
// Max absolute error 0,000027 degree
|
||||
// atan2_approx maximum absolute error = 7.152557e-07 rads (4.098114e-05 degree)
|
||||
float atan2_approx(float y, float x)
|
||||
{
|
||||
#define atanPolyCoef1 3.14551665884836e-07f
|
||||
#define atanPolyCoef2 0.99997356613987f
|
||||
#define atanPolyCoef3 0.14744007058297684f
|
||||
#define atanPolyCoef4 0.3099814292351353f
|
||||
#define atanPolyCoef5 0.05030176425872175f
|
||||
#define atanPolyCoef6 0.1471039133652469f
|
||||
#define atanPolyCoef7 0.6444640676891548f
|
||||
|
||||
float res, absX, absY;
|
||||
absX = fabsf(x);
|
||||
absY = fabsf(y);
|
||||
res = MAX(absX, absY);
|
||||
if (res) res = MIN(absX, absY) / res;
|
||||
else res = 0.0f;
|
||||
res = -((((atanPolyCoef5 * res - atanPolyCoef4) * res - atanPolyCoef3) * res - atanPolyCoef2) * res - atanPolyCoef1) / ((atanPolyCoef7 * res + atanPolyCoef6) * res + 1.0f);
|
||||
if (absY > absX) res = (M_PIf / 2.0f) - res;
|
||||
if (x < 0) res = M_PIf - res;
|
||||
if (y < 0) res = -res;
|
||||
return res;
|
||||
}
|
||||
|
||||
// http://http.developer.nvidia.com/Cg/acos.html
|
||||
// Handbook of Mathematical Functions
|
||||
// M. Abramowitz and I.A. Stegun, Ed.
|
||||
// acos_approx maximum absolute error = 6.760856e-05 rads (3.873685e-03 degree)
|
||||
float acos_approx(float x)
|
||||
{
|
||||
float xa = fabsf(x);
|
||||
float result = sqrtf(1.0f - xa) * (1.5707288f + xa * (-0.2121144f + xa * (0.0742610f + (-0.0187293f * xa))));
|
||||
if (x < 0.0f)
|
||||
return M_PIf - result;
|
||||
else
|
||||
return result;
|
||||
}
|
||||
#endif
|
||||
|
||||
int32_t applyDeadband(int32_t value, int32_t deadband)
|
||||
|
|
|
|||
|
|
@ -22,8 +22,8 @@
|
|||
#endif
|
||||
|
||||
// Undefine this for use libc sinf/cosf. Keep this defined to use fast sin/cos approximations
|
||||
#define FAST_TRIGONOMETRY // order 9 approximation
|
||||
//#define EVEN_FASTER_TRIGONOMETRY // order 7 approximation
|
||||
#define FAST_MATH // order 9 approximation
|
||||
#define VERY_FAST_MATH // order 7 approximation
|
||||
|
||||
// Use floating point M_PI instead explicitly.
|
||||
#define M_PIf 3.14159265358979323846f
|
||||
|
|
@ -88,12 +88,18 @@ int32_t quickMedianFilter5(int32_t * v);
|
|||
int32_t quickMedianFilter7(int32_t * v);
|
||||
int32_t quickMedianFilter9(int32_t * v);
|
||||
|
||||
#if defined(FAST_TRIGONOMETRY) || defined(EVEN_FASTER_TRIGONOMETRY)
|
||||
#if defined(FAST_MATH) || defined(VERY_FAST_MATH)
|
||||
float sin_approx(float x);
|
||||
float cos_approx(float x);
|
||||
float atan2_approx(float y, float x);
|
||||
float acos_approx(float x);
|
||||
#define tan_approx(x) (sin_approx(x) / cos_approx(x))
|
||||
#else
|
||||
#define sin_approx(x) sinf(x)
|
||||
#define cos_approx(x) cosf(x)
|
||||
#define atan2_approx(y,x) atan2f(y,x)
|
||||
#define acos_approx(x) acosf(x)
|
||||
#define tan_approx(x) tanf(x)
|
||||
#endif
|
||||
|
||||
void arraySubInt32(int32_t *dest, int32_t *array1, int32_t *array2, int count);
|
||||
|
|
|
|||
|
|
@ -392,6 +392,7 @@ static void resetConf(void)
|
|||
masterConfig.boardAlignment.pitchDegrees = 0;
|
||||
masterConfig.boardAlignment.yawDegrees = 0;
|
||||
masterConfig.acc_hardware = ACC_DEFAULT; // default/autodetect
|
||||
masterConfig.acc_for_fast_looptime = 0;
|
||||
masterConfig.max_angle_inclination = 500; // 50 degrees
|
||||
masterConfig.yaw_control_direction = 1;
|
||||
masterConfig.gyroConfig.gyroMovementCalibrationThreshold = 32;
|
||||
|
|
|
|||
|
|
@ -46,6 +46,7 @@ typedef struct master_t {
|
|||
|
||||
int8_t yaw_control_direction; // change control direction of yaw (inverted, normal)
|
||||
uint8_t acc_hardware; // Which acc hardware to use on boards with more than one device
|
||||
uint8_t acc_for_fast_looptime; // shorten acc processing time by using 1 out of 9 samples. For combination with fast looptimes.
|
||||
uint16_t gyro_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen.
|
||||
uint16_t gyro_cmpf_factor; // Set the Gyro Weight for Gyro/Acc complementary filter. Increasing this value would reduce and delay Acc influence on the output of the filter.
|
||||
uint16_t gyro_cmpfm_factor; // Set the Gyro Weight for Gyro/Magnetometer complementary filter. Increasing this value would reduce and delay Magnetometer influence on the output of the filter
|
||||
|
|
|
|||
|
|
@ -65,3 +65,14 @@ void serialSetMode(serialPort_t *instance, portMode_t mode)
|
|||
instance->vTable->setMode(instance, mode);
|
||||
}
|
||||
|
||||
void serialBeginWrite(serialPort_t *instance)
|
||||
{
|
||||
if (instance->vTable->beginWrite)
|
||||
instance->vTable->beginWrite(instance);
|
||||
}
|
||||
|
||||
void serialEndWrite(serialPort_t *instance)
|
||||
{
|
||||
if (instance->vTable->endWrite)
|
||||
instance->vTable->endWrite(instance);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -72,6 +72,10 @@ struct serialPortVTable {
|
|||
bool (*isSerialTransmitBufferEmpty)(serialPort_t *instance);
|
||||
|
||||
void (*setMode)(serialPort_t *instance, portMode_t mode);
|
||||
|
||||
// Optional functions used to buffer large writes.
|
||||
void (*beginWrite)(serialPort_t *instance);
|
||||
void (*endWrite)(serialPort_t *instance);
|
||||
};
|
||||
|
||||
void serialWrite(serialPort_t *instance, uint8_t ch);
|
||||
|
|
@ -82,3 +86,6 @@ void serialSetMode(serialPort_t *instance, portMode_t mode);
|
|||
bool isSerialTransmitBufferEmpty(serialPort_t *instance);
|
||||
void serialPrint(serialPort_t *instance, const char *str);
|
||||
uint32_t serialGetBaudRate(serialPort_t *instance);
|
||||
|
||||
void serialBeginWrite(serialPort_t *instance);
|
||||
void serialEndWrite(serialPort_t *instance);
|
||||
|
|
|
|||
|
|
@ -465,7 +465,9 @@ const struct serialPortVTable softSerialVTable[] = {
|
|||
softSerialSetBaudRate,
|
||||
isSoftSerialTransmitBufferEmpty,
|
||||
softSerialSetMode,
|
||||
}
|
||||
.beginWrite = NULL,
|
||||
.endWrite = NULL,
|
||||
}
|
||||
};
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -286,5 +286,7 @@ const struct serialPortVTable uartVTable[] = {
|
|||
uartSetBaudRate,
|
||||
isUartTransmitBufferEmpty,
|
||||
uartSetMode,
|
||||
.beginWrite = NULL,
|
||||
.endWrite = NULL,
|
||||
}
|
||||
};
|
||||
|
|
|
|||
|
|
@ -27,6 +27,7 @@
|
|||
#include "usb_core.h"
|
||||
#include "usb_init.h"
|
||||
#include "hw_config.h"
|
||||
#include "common/utils.h"
|
||||
|
||||
#include "drivers/system.h"
|
||||
|
||||
|
|
@ -38,7 +39,7 @@
|
|||
|
||||
static vcpPort_t vcpPort;
|
||||
|
||||
void usbVcpSetBaudRate(serialPort_t *instance, uint32_t baudRate)
|
||||
static void usbVcpSetBaudRate(serialPort_t *instance, uint32_t baudRate)
|
||||
{
|
||||
UNUSED(instance);
|
||||
UNUSED(baudRate);
|
||||
|
|
@ -46,7 +47,7 @@ void usbVcpSetBaudRate(serialPort_t *instance, uint32_t baudRate)
|
|||
// TODO implement
|
||||
}
|
||||
|
||||
void usbVcpSetMode(serialPort_t *instance, portMode_t mode)
|
||||
static void usbVcpSetMode(serialPort_t *instance, portMode_t mode)
|
||||
{
|
||||
UNUSED(instance);
|
||||
UNUSED(mode);
|
||||
|
|
@ -54,20 +55,20 @@ void usbVcpSetMode(serialPort_t *instance, portMode_t mode)
|
|||
// TODO implement
|
||||
}
|
||||
|
||||
bool isUsbVcpTransmitBufferEmpty(serialPort_t *instance)
|
||||
static bool isUsbVcpTransmitBufferEmpty(serialPort_t *instance)
|
||||
{
|
||||
UNUSED(instance);
|
||||
return true;
|
||||
}
|
||||
|
||||
uint8_t usbVcpAvailable(serialPort_t *instance)
|
||||
static uint8_t usbVcpAvailable(serialPort_t *instance)
|
||||
{
|
||||
UNUSED(instance);
|
||||
|
||||
return receiveLength & 0xFF; // FIXME use uint32_t return type everywhere
|
||||
}
|
||||
|
||||
uint8_t usbVcpRead(serialPort_t *instance)
|
||||
static uint8_t usbVcpRead(serialPort_t *instance)
|
||||
{
|
||||
UNUSED(instance);
|
||||
|
||||
|
|
@ -82,24 +83,63 @@ uint8_t usbVcpRead(serialPort_t *instance)
|
|||
return buf[0];
|
||||
}
|
||||
|
||||
void usbVcpWrite(serialPort_t *instance, uint8_t c)
|
||||
static bool usbVcpFlush(vcpPort_t *port)
|
||||
{
|
||||
UNUSED(instance);
|
||||
uint8_t count = port->txAt;
|
||||
port->txAt = 0;
|
||||
|
||||
if (count == 0) {
|
||||
return true;
|
||||
}
|
||||
if (!usbIsConnected() || !usbIsConfigured()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
uint32_t txed;
|
||||
uint32_t start = millis();
|
||||
|
||||
if (!(usbIsConnected() && usbIsConfigured())) {
|
||||
return;
|
||||
}
|
||||
|
||||
do {
|
||||
txed = CDC_Send_DATA((uint8_t*)&c, 1);
|
||||
} while (txed < 1 && (millis() - start < USB_TIMEOUT));
|
||||
txed = CDC_Send_DATA(port->txBuf, count);
|
||||
} while (txed != count && (millis() - start < USB_TIMEOUT));
|
||||
|
||||
return txed == count;
|
||||
}
|
||||
|
||||
const struct serialPortVTable usbVTable[] = { { usbVcpWrite, usbVcpAvailable, usbVcpRead, usbVcpSetBaudRate, isUsbVcpTransmitBufferEmpty, usbVcpSetMode } };
|
||||
static void usbVcpWrite(serialPort_t *instance, uint8_t c)
|
||||
{
|
||||
vcpPort_t *port = container_of(instance, vcpPort_t, port);
|
||||
|
||||
port->txBuf[port->txAt++] = c;
|
||||
if (!port->buffering || port->txAt >= ARRAYLEN(port->txBuf)) {
|
||||
usbVcpFlush(port);
|
||||
}
|
||||
}
|
||||
|
||||
static void usbVcpBeginWrite(serialPort_t *instance)
|
||||
{
|
||||
vcpPort_t *port = container_of(instance, vcpPort_t, port);
|
||||
port->buffering = true;
|
||||
}
|
||||
|
||||
static void usbVcpEndWrite(serialPort_t *instance)
|
||||
{
|
||||
vcpPort_t *port = container_of(instance, vcpPort_t, port);
|
||||
port->buffering = false;
|
||||
usbVcpFlush(port);
|
||||
}
|
||||
|
||||
static const struct serialPortVTable usbVTable[] = {
|
||||
{
|
||||
.serialWrite = usbVcpWrite,
|
||||
.serialTotalBytesWaiting = usbVcpAvailable,
|
||||
.serialRead = usbVcpRead,
|
||||
.serialSetBaudRate = usbVcpSetBaudRate,
|
||||
.isSerialTransmitBufferEmpty = isUsbVcpTransmitBufferEmpty,
|
||||
.setMode = usbVcpSetMode,
|
||||
.beginWrite = usbVcpBeginWrite,
|
||||
.endWrite = usbVcpEndWrite,
|
||||
}
|
||||
};
|
||||
|
||||
serialPort_t *usbVcpOpen(void)
|
||||
{
|
||||
|
|
|
|||
|
|
@ -22,13 +22,11 @@
|
|||
typedef struct {
|
||||
serialPort_t port;
|
||||
|
||||
// Buffer used during bulk writes.
|
||||
uint8_t txBuf[20];
|
||||
uint8_t txAt;
|
||||
// Set if the port is in bulk write mode and can buffer.
|
||||
bool buffering;
|
||||
} vcpPort_t;
|
||||
|
||||
serialPort_t *usbVcpOpen(void);
|
||||
|
||||
uint8_t usbVcpAvailable(serialPort_t *instance);
|
||||
|
||||
uint8_t usbVcpRead(serialPort_t *instance);
|
||||
|
||||
void usbVcpWrite(serialPort_t *instance, uint8_t ch);
|
||||
void usbPrintStr(const char *str);
|
||||
|
|
|
|||
|
|
@ -20,6 +20,7 @@
|
|||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <math.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "common/maths.h"
|
||||
|
||||
|
|
@ -46,6 +47,8 @@
|
|||
|
||||
#include "config/runtime_config.h"
|
||||
|
||||
//#define DEBUG_IMU
|
||||
|
||||
int16_t accSmooth[XYZ_AXIS_COUNT];
|
||||
int32_t accSum[XYZ_AXIS_COUNT];
|
||||
|
||||
|
|
@ -63,11 +66,15 @@ float gyroScaleRad;
|
|||
|
||||
|
||||
rollAndPitchInclination_t inclination = { { 0, 0 } }; // absolute angle inclination in multiple of 0.1 degree 180 deg = 1800
|
||||
float anglerad[2] = { 0.0f, 0.0f }; // absolute angle inclination in radians
|
||||
static float anglerad[2] = { 0.0f, 0.0f }; // absolute angle inclination in radians
|
||||
|
||||
static imuRuntimeConfig_t *imuRuntimeConfig;
|
||||
static pidProfile_t *pidProfile;
|
||||
static accDeadband_t *accDeadband;
|
||||
static accProcessor_t accProc;
|
||||
|
||||
static void qAccProcessingStateMachine(rollAndPitchTrims_t *accelerometerTrims, uint8_t acc_for_fast_looptime);
|
||||
|
||||
|
||||
void imuConfigure(
|
||||
imuRuntimeConfig_t *initialImuRuntimeConfig,
|
||||
|
|
@ -84,13 +91,6 @@ void imuConfigure(
|
|||
throttleAngleScale = calculateThrottleAngleScale(throttle_correction_angle);
|
||||
}
|
||||
|
||||
void imuInit()
|
||||
{
|
||||
smallAngle = lrintf(acc_1G * cos_approx(degreesToRadians(imuRuntimeConfig->small_angle)));
|
||||
accVelScale = 9.80665f / acc_1G / 10000.0f;
|
||||
gyroScaleRad = gyro.scale * (M_PIf / 180.0f) * 0.000001f;
|
||||
}
|
||||
|
||||
float calculateThrottleAngleScale(uint16_t throttle_correction_angle)
|
||||
{
|
||||
return (1800.0f / M_PIf) * (900.0f / throttle_correction_angle);
|
||||
|
|
@ -130,50 +130,6 @@ void imuResetAccelerationSum(void)
|
|||
accTimeSum = 0;
|
||||
}
|
||||
|
||||
// rotate acc into Earth frame and calculate acceleration in it
|
||||
void imuCalculateAcceleration(uint32_t deltaT)
|
||||
{
|
||||
static int32_t accZoffset = 0;
|
||||
static float accz_smooth = 0;
|
||||
float dT;
|
||||
fp_angles_t rpy;
|
||||
t_fp_vector accel_ned;
|
||||
|
||||
// deltaT is measured in us ticks
|
||||
dT = (float)deltaT * 1e-6f;
|
||||
|
||||
// the accel values have to be rotated into the earth frame
|
||||
rpy.angles.roll = -(float)anglerad[AI_ROLL];
|
||||
rpy.angles.pitch = -(float)anglerad[AI_PITCH];
|
||||
rpy.angles.yaw = -(float)heading * RAD;
|
||||
|
||||
accel_ned.V.X = accSmooth[0];
|
||||
accel_ned.V.Y = accSmooth[1];
|
||||
accel_ned.V.Z = accSmooth[2];
|
||||
|
||||
rotateV(&accel_ned.V, &rpy);
|
||||
|
||||
if (imuRuntimeConfig->acc_unarmedcal == 1) {
|
||||
if (!ARMING_FLAG(ARMED)) {
|
||||
accZoffset -= accZoffset / 64;
|
||||
accZoffset += accel_ned.V.Z;
|
||||
}
|
||||
accel_ned.V.Z -= accZoffset / 64; // compensate for gravitation on z-axis
|
||||
} else
|
||||
accel_ned.V.Z -= acc_1G;
|
||||
|
||||
accz_smooth = accz_smooth + (dT / (fc_acc + dT)) * (accel_ned.V.Z - accz_smooth); // low pass filter
|
||||
|
||||
// apply Deadband to reduce integration drift and vibration influence
|
||||
accSum[X] += applyDeadband(lrintf(accel_ned.V.X), accDeadband->xy);
|
||||
accSum[Y] += applyDeadband(lrintf(accel_ned.V.Y), accDeadband->xy);
|
||||
accSum[Z] += applyDeadband(lrintf(accz_smooth), accDeadband->z);
|
||||
|
||||
// sum up Values for later integration to get velocity and distance
|
||||
accTimeSum += deltaT;
|
||||
accSumCount++;
|
||||
}
|
||||
|
||||
/*
|
||||
* Baseflight calculation by Luggi09 originates from arducopter
|
||||
* ============================================================
|
||||
|
|
@ -216,7 +172,7 @@ int16_t imuCalculateHeading(t_fp_vector *vec)
|
|||
float Yh = vec->A[Y] * cosineRoll - vec->A[Z] * sineRoll;
|
||||
//TODO: Replace this comment with an explanation of why Yh and Xh can never simultanoeusly be zero,
|
||||
// or handle the case in which they are and (atan2f(0, 0) is undefined.
|
||||
float hd = (atan2f(Yh, Xh) * 1800.0f / M_PIf + magneticDeclination) / 10.0f;
|
||||
float hd = (atan2_approx(Yh, Xh) * 1800.0f / M_PIf + magneticDeclination) / 10.0f;
|
||||
head = lrintf(hd);
|
||||
|
||||
// Arctan returns a value in the range -180 to 180 degrees. We 'normalize' negative angles to be positive.
|
||||
|
|
@ -226,84 +182,12 @@ int16_t imuCalculateHeading(t_fp_vector *vec)
|
|||
return head;
|
||||
}
|
||||
|
||||
static void imuCalculateEstimatedAttitude(void)
|
||||
{
|
||||
int32_t axis;
|
||||
int32_t accMag = 0;
|
||||
static t_fp_vector EstM;
|
||||
static t_fp_vector EstN = { .A = { 1.0f, 0.0f, 0.0f } };
|
||||
static float accLPF[3];
|
||||
static uint32_t previousT;
|
||||
uint32_t currentT = micros();
|
||||
uint32_t deltaT;
|
||||
float scale;
|
||||
fp_angles_t deltaGyroAngle;
|
||||
deltaT = currentT - previousT;
|
||||
scale = deltaT * gyroScaleRad;
|
||||
previousT = currentT;
|
||||
|
||||
// Initialization
|
||||
for (axis = 0; axis < 3; axis++) {
|
||||
deltaGyroAngle.raw[axis] = gyroADC[axis] * scale;
|
||||
if (imuRuntimeConfig->acc_lpf_factor > 0) {
|
||||
accLPF[axis] = accLPF[axis] * (1.0f - (1.0f / imuRuntimeConfig->acc_lpf_factor)) + accADC[axis] * (1.0f / imuRuntimeConfig->acc_lpf_factor);
|
||||
accSmooth[axis] = accLPF[axis];
|
||||
} else {
|
||||
accSmooth[axis] = accADC[axis];
|
||||
}
|
||||
accMag += (int32_t)accSmooth[axis] * accSmooth[axis];
|
||||
}
|
||||
accMag = accMag * 100 / ((int32_t)acc_1G * acc_1G);
|
||||
|
||||
rotateV(&EstG.V, &deltaGyroAngle);
|
||||
|
||||
// Apply complimentary filter (Gyro drift correction)
|
||||
// If accel magnitude >1.15G or <0.85G and ACC vector outside of the limit range => we neutralize the effect of accelerometers in the angle estimation.
|
||||
// To do that, we just skip filter, as EstV already rotated by Gyro
|
||||
|
||||
float invGyroComplimentaryFilterFactor = (1.0f / (imuRuntimeConfig->gyro_cmpf_factor + 1.0f));
|
||||
|
||||
if (72 < (uint16_t)accMag && (uint16_t)accMag < 133) {
|
||||
for (axis = 0; axis < 3; axis++)
|
||||
EstG.A[axis] = (EstG.A[axis] * imuRuntimeConfig->gyro_cmpf_factor + accSmooth[axis]) * invGyroComplimentaryFilterFactor;
|
||||
}
|
||||
|
||||
if (EstG.A[Z] > smallAngle) {
|
||||
ENABLE_STATE(SMALL_ANGLE);
|
||||
} else {
|
||||
DISABLE_STATE(SMALL_ANGLE);
|
||||
}
|
||||
|
||||
// Attitude of the estimated vector
|
||||
anglerad[AI_ROLL] = atan2f(EstG.V.Y, EstG.V.Z);
|
||||
anglerad[AI_PITCH] = atan2f(-EstG.V.X, sqrtf(EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z));
|
||||
inclination.values.rollDeciDegrees = lrintf(anglerad[AI_ROLL] * (1800.0f / M_PIf));
|
||||
inclination.values.pitchDeciDegrees = lrintf(anglerad[AI_PITCH] * (1800.0f / M_PIf));
|
||||
|
||||
if (sensors(SENSOR_MAG)) {
|
||||
rotateV(&EstM.V, &deltaGyroAngle);
|
||||
// FIXME what does the _M_ mean?
|
||||
float invGyroComplimentaryFilter_M_Factor = (1.0f / (imuRuntimeConfig->gyro_cmpfm_factor + 1.0f));
|
||||
for (axis = 0; axis < 3; axis++) {
|
||||
EstM.A[axis] = (EstM.A[axis] * imuRuntimeConfig->gyro_cmpfm_factor + magADC[axis]) * invGyroComplimentaryFilter_M_Factor;
|
||||
}
|
||||
heading = imuCalculateHeading(&EstM);
|
||||
} else {
|
||||
rotateV(&EstN.V, &deltaGyroAngle);
|
||||
normalizeV(&EstN.V, &EstN.V);
|
||||
heading = imuCalculateHeading(&EstN);
|
||||
}
|
||||
|
||||
imuCalculateAcceleration(deltaT); // rotate acc vector into earth frame
|
||||
}
|
||||
|
||||
void imuUpdate(rollAndPitchTrims_t *accelerometerTrims)
|
||||
void imuUpdate(rollAndPitchTrims_t *accelerometerTrims, uint8_t acc_for_fast_looptime)
|
||||
{
|
||||
gyroUpdate();
|
||||
|
||||
if (sensors(SENSOR_ACC)) {
|
||||
updateAccelerationReadings(accelerometerTrims); // TODO rename to accelerometerUpdate and rename many other 'Acceleration' references to be 'Accelerometer'
|
||||
imuCalculateEstimatedAttitude();
|
||||
qAccProcessingStateMachine(accelerometerTrims, acc_for_fast_looptime);
|
||||
} else {
|
||||
accADC[X] = 0;
|
||||
accADC[Y] = 0;
|
||||
|
|
@ -323,8 +207,554 @@ int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value)
|
|||
if (cosZ <= 0.015f) {
|
||||
return 0;
|
||||
}
|
||||
int angle = lrintf(acosf(cosZ) * throttleAngleScale);
|
||||
int angle = lrintf(acos_approx(cosZ) * throttleAngleScale);
|
||||
if (angle > 900)
|
||||
angle = 900;
|
||||
return lrintf(throttle_correction_value * sin_approx(angle / (900.0f * M_PIf / 2.0f)));
|
||||
}
|
||||
|
||||
// WITHOUT
|
||||
//arm - none - eabi - size . / obj / main / cleanflight_CC3D.elf
|
||||
//text data bss dec hex filename
|
||||
//116324 376 12640 129340 1f93c . / obj / main / cleanflight_CC3D.elf
|
||||
|
||||
//////////////////////////////////////////////////////////////////////
|
||||
// 4D Quaternion / 3D Vector Math
|
||||
//arm - none - eabi - size . / obj / main / cleanflight_CC3D.elf
|
||||
//text data bss dec hex filename
|
||||
//116284 364 12636 129284 1f904 . / obj / main / cleanflight_CC3D.elf
|
||||
|
||||
typedef struct v3_s
|
||||
{
|
||||
float x;
|
||||
float y;
|
||||
float z;
|
||||
} v3_t;
|
||||
|
||||
const v3_t V0 = { .x = 0.0f, .y = 0.0f, .z = 0.0f };
|
||||
const v3_t VX = { .x = 1.0f, .y = 0.0f, .z = 0.0f };
|
||||
const v3_t VY = { .x = 0.0f, .y = 1.0f, .z = 0.0f };
|
||||
const v3_t VZ = { .x = 0.0f, .y = 0.0f, .z = 1.0f };
|
||||
|
||||
typedef struct q4_s
|
||||
{
|
||||
float w;
|
||||
float x;
|
||||
float y;
|
||||
float z;
|
||||
} q4_t;
|
||||
|
||||
const q4_t Q0 = { .w = 1.0f, .x = 0.0f, .y = 0.0f, .z = 0.0f };
|
||||
|
||||
void MulQQ(const q4_t *a, const q4_t *b, q4_t *o)
|
||||
{
|
||||
q4_t r;
|
||||
r.w = a->w * b->w - a->x * b->x - a->y * b->y - a->z * b->z;
|
||||
r.x = a->w * b->x + a->z * b->y - a->y * b->z + a->x * b->w;
|
||||
r.y = a->w * b->y + a->x * b->z + a->y * b->w - a->z * b->x;
|
||||
r.z = a->y * b->x - a->x * b->y + a->w * b->z + a->z * b->w;
|
||||
*o = r;
|
||||
}
|
||||
|
||||
void MulQV(const q4_t *a, const v3_t *b, q4_t *o)
|
||||
{
|
||||
q4_t r;
|
||||
r.w = -a->x * b->x - a->y * b->y - a->z * b->z;
|
||||
r.x = a->w * b->x + a->z * b->y - a->y * b->z;
|
||||
r.y = a->w * b->y + a->x * b->z - a->z * b->x;
|
||||
r.z = a->y * b->x - a->x * b->y + a->w * b->z;
|
||||
*o = r;
|
||||
}
|
||||
|
||||
void MulQF(const q4_t *a, const float b, q4_t *o)
|
||||
{
|
||||
q4_t r;
|
||||
r.w = a->w * b;
|
||||
r.x = a->x * b;
|
||||
r.y = a->y * b;
|
||||
r.z = a->z * b;
|
||||
*o = r;
|
||||
}
|
||||
|
||||
void MulVF(const v3_t *a, const float b, v3_t *o)
|
||||
{
|
||||
v3_t r;
|
||||
r.x = a->x * b;
|
||||
r.y = a->y * b;
|
||||
r.z = a->z * b;
|
||||
*o = r;
|
||||
}
|
||||
|
||||
void SumQQ(const q4_t *a, const q4_t *b, q4_t *o)
|
||||
{
|
||||
q4_t r;
|
||||
r.w = a->w + b->w;
|
||||
r.x = a->x + b->x;
|
||||
r.y = a->y + b->y;
|
||||
r.z = a->z + b->z;
|
||||
*o = r;
|
||||
}
|
||||
|
||||
|
||||
void SumVV(const v3_t *a, const v3_t *b, v3_t *o)
|
||||
{
|
||||
v3_t r;
|
||||
r.x = a->x + b->x;
|
||||
r.y = a->y + b->y;
|
||||
r.z = a->z + b->z;
|
||||
*o = r;
|
||||
}
|
||||
|
||||
void SubQQ(const q4_t *a, const q4_t *b, q4_t *o)
|
||||
{
|
||||
q4_t r;
|
||||
r.w = a->w - b->w;
|
||||
r.x = a->x - b->x;
|
||||
r.y = a->y - b->y;
|
||||
r.z = a->z - b->z;
|
||||
*o = r;
|
||||
}
|
||||
|
||||
|
||||
void SubVV(const v3_t *a, const v3_t *b, v3_t *o)
|
||||
{
|
||||
v3_t r;
|
||||
r.x = a->x - b->x;
|
||||
r.y = a->y - b->y;
|
||||
r.z = a->z - b->z;
|
||||
*o = r;
|
||||
}
|
||||
|
||||
void CrossQQ(const q4_t *a, const q4_t *b, q4_t *o)
|
||||
{
|
||||
q4_t r;
|
||||
r.w = 0.0f;
|
||||
r.x = a->y * b->z - a->z * b->y;
|
||||
r.y = a->z * b->x - a->x * b->z;
|
||||
r.z = a->x * b->y - a->y * b->x;
|
||||
*o = r;
|
||||
}
|
||||
|
||||
void CrossVV(const v3_t *a, const v3_t *b, v3_t *o)
|
||||
{
|
||||
v3_t r;
|
||||
r.x = a->y * b->z - a->z * b->y;
|
||||
r.y = a->z * b->x - a->x * b->z;
|
||||
r.z = a->x * b->y - a->y * b->x;
|
||||
*o = r;
|
||||
}
|
||||
|
||||
float DotQQ(const q4_t *a, const q4_t *b)
|
||||
{
|
||||
return a->w * b->w + a->x * b->x + a->y * b->y + a->z * b->z;
|
||||
}
|
||||
|
||||
float DotVV(const v3_t *a, const v3_t *b)
|
||||
{
|
||||
return a->x * b->x + a->y * b->y + a->z * b->z;
|
||||
}
|
||||
|
||||
float Mag2Q(const q4_t *a) // magnitude squared
|
||||
{
|
||||
return a->w*a->w + a->x*a->x + a->y*a->y + a->z*a->z;
|
||||
}
|
||||
|
||||
#define MagQ(a) sqrtf(Mag2Q(a))
|
||||
|
||||
float Mag2V(const v3_t *a) // magnitude squared
|
||||
{
|
||||
return a->x*a->x + a->y*a->y + a->z*a->z; // TODO: optimize for unit vectors (m2 nearly 1.0)
|
||||
}
|
||||
|
||||
#define MagV(a) sqrtf(Mag2V(a))
|
||||
|
||||
void NormQ(const q4_t *a, q4_t *o)
|
||||
{
|
||||
q4_t r;
|
||||
MulQF(a, 1 / MagQ(a), &r);
|
||||
*o = r;
|
||||
}
|
||||
|
||||
void NormV(const v3_t *a, v3_t *o)
|
||||
{
|
||||
v3_t r;
|
||||
float m = MagV(a);
|
||||
MulVF(a, 1 / m, &r); // TODO: m nearly 0
|
||||
*o = r;
|
||||
}
|
||||
|
||||
void ConjQ(const q4_t *a, q4_t *o)
|
||||
{
|
||||
q4_t r;
|
||||
r.w = a->w;
|
||||
r.x = -a->x;
|
||||
r.y = -a->y;
|
||||
r.z = -a->z;
|
||||
*o = r;
|
||||
}
|
||||
|
||||
void RotateVQ(const v3_t *v, const q4_t *q, v3_t *o) //Vector rotated by a Quaternion(matches V^ = V * Matrix)
|
||||
{
|
||||
// v + 2 * r X(r X v + q.w*v) --https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation#Performance_comparisons
|
||||
// vector r is the three imaginary coefficients of quaternion q
|
||||
|
||||
v3_t r2_rv_vw;
|
||||
{
|
||||
// reverse signs to change direction of rotation
|
||||
v3_t r = { .x = -q->x, .y = -q->y, .z = -q->z };
|
||||
v3_t r2;
|
||||
SumVV(&r, &r, &r2);
|
||||
|
||||
v3_t rv_vw;
|
||||
{
|
||||
v3_t vw;
|
||||
MulVF(v, q->w, &vw);
|
||||
v3_t rv;
|
||||
CrossVV(&r, v, &rv);
|
||||
SumVV(&rv, &vw, &rv_vw);
|
||||
}
|
||||
CrossVV(&r2, &rv_vw, &r2_rv_vw);
|
||||
}
|
||||
SumVV(v, &r2_rv_vw, o);
|
||||
}
|
||||
|
||||
void quaternion_approx(const v3_t *w, q4_t *o) // (angle vector[rad]) --Small angle approximation
|
||||
{
|
||||
q4_t r;
|
||||
r.x = w->x / 2;
|
||||
r.y = w->y / 2;
|
||||
r.z = w->z / 2;
|
||||
r.w = 1.0f - (0.5f * ((r.x * r.x) + (r.y * r.y) + (r.z * r.z)));
|
||||
*o = r;
|
||||
}
|
||||
|
||||
#if 0
|
||||
void quaternion(const v3_t *w, q4_t *o) // (angle vector[rad]) --Large Rotation Quaternion
|
||||
{
|
||||
float m = MagV(w);
|
||||
if (m == 0.0f)
|
||||
{
|
||||
*o = Q0;
|
||||
}
|
||||
else
|
||||
{
|
||||
q4_t r;
|
||||
float t2 = m * 0.5f; // # rotation angle divided by 2
|
||||
float sm = sin(t2) / m; // # computation minimization
|
||||
r.x = w->x * sm;
|
||||
r.y = w->y * sm;
|
||||
r.z = w->z * sm;
|
||||
r.w = cos(t2);
|
||||
*o = r;
|
||||
}
|
||||
}
|
||||
#else
|
||||
# define quaternion(w,o) quaternion_approx(w,o) // I think we can get away with the approximation
|
||||
// TODO - try usining sin_approx, cos_approx
|
||||
#endif
|
||||
|
||||
typedef struct rpy_s
|
||||
{
|
||||
float r;
|
||||
float p;
|
||||
float y;
|
||||
} rpy_t;
|
||||
const rpy_t RPY0 = { .r = 0, .p = 0, .y = 0 };
|
||||
|
||||
void quaternion_from_rpy(const rpy_t *a, q4_t *o) // (degrees) yaw->pitch->roll order
|
||||
{
|
||||
float cr, sr, cp, sp, cy, sy;
|
||||
|
||||
{ float r2 = a->r * (RAD / 2); cr = cos_approx(r2); sr = sin_approx(r2); }
|
||||
{ float p2 = a->p * (RAD / 2); cp = cos_approx(p2); sp = sin_approx(p2); }
|
||||
{ float y2 = a->y * (RAD / 2); cy = cos_approx(y2); sy = sin_approx(y2); }
|
||||
|
||||
o->w = cr*cp*cy + sr*sp*sy;
|
||||
o->x = sr*cp*cy - cr*sp*sy;
|
||||
o->y = cr*sp*cy + sr*cp*sy;
|
||||
o->z = cr*cp*sy - sr*sp*cy;
|
||||
}
|
||||
|
||||
void quaternion_to_rpy(const q4_t *q, rpy_t *o) // (degrees) yaw->pitch->roll order
|
||||
{
|
||||
// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
|
||||
// Body Z - Y - X sequence
|
||||
|
||||
float q0 = q->w;
|
||||
float q1 = q->x;
|
||||
float q2 = q->y;
|
||||
float q3 = q->z;
|
||||
|
||||
float p0 = MAX(-1.0f, MIN(1.0f, 2 * (q0*q2 - q3*q1)));
|
||||
o->p = asin(p0);
|
||||
|
||||
if (ABS(ABS(o->p) - (90 * RAD)) < (0.5f*RAD)) // vertical
|
||||
{
|
||||
o->y = 2 * atan2_approx(q3, q0);
|
||||
o->r = 0.0f;
|
||||
}
|
||||
else
|
||||
{
|
||||
float r0 = 2 * (q0*q1 + q2*q3);
|
||||
float r1 = 1 - 2 * (q1*q1 + q2*q2);
|
||||
if ((r0 == 0) && (r1 == 0)) { o->r = 0.0f; } // atan(0,0)!
|
||||
else { o->r = atan2_approx(r0, r1); }
|
||||
|
||||
float y0 = 2 * (q0*q3 + q1*q2);
|
||||
float y1 = 1 - 2 * (q2*q2 + q3*q3);
|
||||
if ((y0 == 0) && (y1 == 0)) { o->y = 0.0f; } // atan(0,0)!
|
||||
else { o->y = atan2_approx(y0, y1); }
|
||||
}
|
||||
#ifdef NAZE
|
||||
o->y = -o->y; // yaw inversion hack for NAZE32
|
||||
#endif
|
||||
#ifdef SPARKY
|
||||
o->y = -o->y; // yaw inversion hack for SPARKY
|
||||
#endif
|
||||
}
|
||||
|
||||
void angle_vector(const q4_t *a, v3_t *o) // convert from quaternion to angle vector[rad]
|
||||
{
|
||||
q4_t a1;
|
||||
if (a->w < 0) { MulQF(a, -1, &a1); a = &a1; }
|
||||
|
||||
float t2 = acos_approx(MIN(1, a->w)); // TODO acos_approx??
|
||||
|
||||
if (ABS(t2) > (0.005f * RAD))
|
||||
{
|
||||
float s = sin_approx(t2) / (2 * t2);
|
||||
o->x = a->x / s;
|
||||
o->y = a->y / s;
|
||||
o->z = a->z / s;
|
||||
}
|
||||
else
|
||||
{
|
||||
*o = V0;
|
||||
}
|
||||
}
|
||||
|
||||
void nlerp_step(const q4_t *a, const q4_t *b, float max_th, q4_t *o) // max_th in radians (max_rate * update time)
|
||||
{
|
||||
float dot = MAX(-1, MIN(1, DotQQ(a, b)));
|
||||
float th = 2*acos_approx(ABS(dot)); // ABS -> change direction for shortest path
|
||||
|
||||
if (th <= (0.01f*RAD)) { *o = *b; } // tiny step
|
||||
else
|
||||
{
|
||||
float tb = MIN(1, ABS(max_th / th));
|
||||
float ta = 1-tb;
|
||||
if (dot < 0) { tb = -tb; } // change direction for shortest path
|
||||
|
||||
q4_t r, a1, b1;
|
||||
MulQF(a, ta, &a1);
|
||||
MulQF(b, tb, &b1);
|
||||
SumQQ(&a1, &b1, &r);
|
||||
NormQ(&r, o);
|
||||
}
|
||||
}
|
||||
|
||||
q4_t attitude_est_e_q;
|
||||
float acc_rad_scale; // adc -> G
|
||||
float gyro_rads_scale; // adc -> rad/s
|
||||
float cosSmallAngle;
|
||||
float acc_lpf_f0, acc_lpf_f1;
|
||||
v3_t gravity_lpf_b_v, acc_lpf_b_v;
|
||||
|
||||
void qimuInit()
|
||||
{
|
||||
cosSmallAngle = cos_approx(RAD*imuRuntimeConfig->small_angle);
|
||||
|
||||
acc_rad_scale = 1.0f / acc_1G;
|
||||
gyro_rads_scale = gyro.scale * RAD;
|
||||
|
||||
acc_lpf_f1 = (1.0f / imuRuntimeConfig->acc_lpf_factor);
|
||||
acc_lpf_f0 = 1.0f - acc_lpf_f1;
|
||||
gravity_lpf_b_v = VZ;
|
||||
acc_lpf_b_v = VZ;
|
||||
|
||||
quaternion_from_rpy(&RPY0, &attitude_est_e_q);
|
||||
accProc.state = ACCPROC_READ;
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////
|
||||
|
||||
static void qAccProcessingStateMachine(rollAndPitchTrims_t *accelerometerTrims, uint8_t acc_for_fast_looptime)
|
||||
{
|
||||
int axis;
|
||||
const float gyro_drift_factor = 0.00f;
|
||||
static v3_t gyro_drift_correction_b_v = { .x = 0.0f, .y = 0.0f, .z = 0.0f }; // rad/s
|
||||
|
||||
const float attitude_correction_factor = 0.001f;
|
||||
static v3_t attitude_correction_b_v = { .x = 0.0f, .y = 0.0f, .z = 0.0f }; // rad/s
|
||||
static v3_t acc_b_v, gyro_rate_b_v;
|
||||
|
||||
static int16_t normalize_counter = 0;
|
||||
static uint32_t previousT = 0;
|
||||
static uint32_t currentT;
|
||||
|
||||
// get time step.. TODO: this should really be fixed to division of MPU sample rate
|
||||
static float dT;
|
||||
|
||||
bool keepProcessing = !acc_for_fast_looptime; // (keepProcessing == true): causes all states to execute (for slow cycle times)
|
||||
|
||||
do {
|
||||
switch (accProc.state) {
|
||||
|
||||
case ACCPROC_READ:
|
||||
currentT = micros();
|
||||
dT = (currentT - previousT)*0.000001f;
|
||||
previousT = currentT;
|
||||
updateAccelerationReadings(accelerometerTrims); // TODO rename to accelerometerUpdate and rename many other 'Acceleration' references to be 'Accelerometer'
|
||||
accProc.state++;
|
||||
break;
|
||||
|
||||
case ACCPROC_CHUNK_1:
|
||||
for (axis = 0; axis < 3; axis++) {
|
||||
accSmooth[axis] = accADC[axis]; // TODO acc_lpf - or would this work better without it?
|
||||
((float *)&acc_b_v)[axis] = accSmooth[axis] * acc_rad_scale;
|
||||
((float *)&gyro_rate_b_v)[axis] = gyroADC[axis] * gyro_rads_scale;
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////
|
||||
// add in drift compensation
|
||||
SumVV(&gyro_rate_b_v, &gyro_drift_correction_b_v, &gyro_rate_b_v);
|
||||
|
||||
#ifdef DEBUG_IMU
|
||||
debug[0] = gyro_drift_correction_b_v.x * 10000;
|
||||
debug[1] = gyro_drift_correction_b_v.y * 10000;
|
||||
debug[2] = gyro_drift_correction_b_v.z * 10000;
|
||||
#endif
|
||||
|
||||
////////////////////////////////////////////////////////////////
|
||||
// add in attitude estimate correction, convert to degrees
|
||||
v3_t gyro_rotation_b_v;
|
||||
SumVV(&gyro_rate_b_v, &attitude_correction_b_v, &gyro_rotation_b_v);
|
||||
MulVF(&gyro_rotation_b_v, dT, &gyro_rotation_b_v);
|
||||
|
||||
////////////////////////////////////////////////////////////////
|
||||
// Update attitude estimate with gyro data
|
||||
// small angle approximation should be fine, but error does creep in at high rotational rates on multiple axes - Normalize periodically
|
||||
q4_t attitude_est_update_b_q;
|
||||
quaternion(&gyro_rotation_b_v, &attitude_est_update_b_q); // convert angle vector to quaternion
|
||||
MulQQ(&attitude_est_update_b_q, &attitude_est_e_q, &attitude_est_e_q); // and rotate estimate
|
||||
|
||||
v3_t gravity_b_v;
|
||||
// Calculate expected gravity(allows drift to be compensated on all 3 axis when possible)
|
||||
RotateVQ(&VZ, &attitude_est_e_q, &gravity_b_v);
|
||||
|
||||
// check small angle
|
||||
if (gravity_b_v.z > cosSmallAngle) {
|
||||
ENABLE_STATE(SMALL_ANGLE);
|
||||
} else {
|
||||
DISABLE_STATE(SMALL_ANGLE);
|
||||
}
|
||||
|
||||
// acc_lpf
|
||||
if (imuRuntimeConfig->acc_lpf_factor > 0) {
|
||||
v3_t a0, a1;
|
||||
MulVF(&acc_lpf_b_v, acc_lpf_f0, &a0);
|
||||
MulVF(&acc_b_v, acc_lpf_f1, &a1);
|
||||
SumVV(&a0, &a1, &acc_lpf_b_v);
|
||||
|
||||
MulVF(&gravity_lpf_b_v, acc_lpf_f0, &a0);
|
||||
MulVF(&gravity_b_v, acc_lpf_f1, &a1);
|
||||
SumVV(&a0, &a1, &gravity_lpf_b_v);
|
||||
} else {
|
||||
acc_lpf_b_v = acc_b_v;
|
||||
gravity_lpf_b_v = gravity_b_v;
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////
|
||||
// Calculate Correction
|
||||
float acc_m2 = Mag2V(&acc_b_v);
|
||||
#ifdef DEBUG_IMU
|
||||
debug[3] = acc_m2*1000;
|
||||
#endif
|
||||
if ((acc_m2 > 1.1025f) || (acc_m2 < 0.9025f)) {
|
||||
attitude_correction_b_v = V0;
|
||||
} else { // we're not accelerating
|
||||
// Cross product to determine error
|
||||
CrossVV(&acc_lpf_b_v, &gravity_lpf_b_v, &attitude_correction_b_v);
|
||||
MulVF(&attitude_correction_b_v, attitude_correction_factor/dT, &attitude_correction_b_v); // convert to rate for drift correction
|
||||
|
||||
if (gyro_drift_factor != 0.0f) {
|
||||
// conditionally update drift for valid axes (4.5 degree check)
|
||||
if (ABS(gravity_b_v.x) < 0.997f) {
|
||||
gyro_drift_correction_b_v.x = gyro_drift_correction_b_v.x + (attitude_correction_b_v.x*gyro_drift_factor);
|
||||
}
|
||||
if (ABS(gravity_b_v.y) < 0.997f) {
|
||||
gyro_drift_correction_b_v.y = gyro_drift_correction_b_v.y + (attitude_correction_b_v.y*gyro_drift_factor);
|
||||
}
|
||||
if (ABS(gravity_b_v.z) < 0.997f) {
|
||||
gyro_drift_correction_b_v.z = gyro_drift_correction_b_v.z + (attitude_correction_b_v.z*gyro_drift_factor);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// renormalize every couple of seconds
|
||||
if (++normalize_counter == 1000) {
|
||||
NormQ(&attitude_est_e_q, &attitude_est_e_q);
|
||||
normalize_counter = 0;
|
||||
}
|
||||
|
||||
// convert to cleanflight values
|
||||
// update inclination
|
||||
rpy_t rpy;
|
||||
quaternion_to_rpy(&attitude_est_e_q, &rpy);
|
||||
inclination.values.rollDeciDegrees = lrintf(rpy.r * (10 / RAD));
|
||||
inclination.values.pitchDeciDegrees = lrintf(rpy.p * (10 / RAD));
|
||||
heading = rpy.y * (1 / RAD);
|
||||
if (heading < 0) heading += 360;
|
||||
#ifdef DEBUG_IMU
|
||||
//uint32_t endT = micros();
|
||||
//debug[3] = endT - currentT;
|
||||
#endif
|
||||
|
||||
#if 0
|
||||
accProc.state++;
|
||||
break;
|
||||
|
||||
case ACCPROC_CHUNK_2:
|
||||
accProc.state++;
|
||||
break;
|
||||
|
||||
case ACCPROC_CHUNK_3:
|
||||
accProc.state++;
|
||||
break;
|
||||
|
||||
case ACCPROC_CHUNK_4:
|
||||
accProc.state++;
|
||||
break;
|
||||
|
||||
case ACCPROC_CHUNK_5:
|
||||
accProc.state++;
|
||||
break;
|
||||
|
||||
case ACCPROC_CHUNK_6:
|
||||
accProc.state++;
|
||||
break;
|
||||
|
||||
case ACCPROC_CHUNK_7:
|
||||
accProc.state = ACCPROC_COPY;
|
||||
break;
|
||||
|
||||
case ACCPROC_COPY:
|
||||
// assign deliverables (copy local to global)
|
||||
/*
|
||||
memcpy(&EstG, &fsmEstG, sizeof(t_fp_vector));
|
||||
for (axis = 0; axis < 3; axis++) {
|
||||
accSmooth[axis] = fsmAccSmooth[axis];
|
||||
}
|
||||
memcpy(&inclination, &fsmInclination, sizeof(rollAndPitchInclination_t));
|
||||
heading = fsmHeading;
|
||||
*/
|
||||
#endif
|
||||
keepProcessing = false;
|
||||
/* no break */
|
||||
|
||||
default:
|
||||
accProc.state = ACCPROC_READ;
|
||||
break;
|
||||
}
|
||||
} while (keepProcessing);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -18,13 +18,13 @@
|
|||
#pragma once
|
||||
|
||||
extern int16_t throttleAngleCorrection;
|
||||
extern uint32_t accTimeSum;
|
||||
extern int accSumCount;
|
||||
extern float accVelScale;
|
||||
extern t_fp_vector EstG;
|
||||
extern int16_t accSmooth[XYZ_AXIS_COUNT];
|
||||
extern int32_t accSum[XYZ_AXIS_COUNT];
|
||||
extern int16_t smallAngle;
|
||||
extern uint32_t accTimeSum; // altitudehold.c
|
||||
extern int accSumCount; // altitudehold.c
|
||||
extern float accVelScale; // altitudehold.c
|
||||
extern t_fp_vector EstG; // display.c
|
||||
extern int16_t accSmooth[XYZ_AXIS_COUNT]; // blackbox.c, display.c, serial_msp.c, frsky.c, smartport.c
|
||||
extern int32_t accSum[XYZ_AXIS_COUNT]; // altitudehold.c
|
||||
extern int16_t smallAngle; // display.c
|
||||
|
||||
typedef struct rollAndPitchInclination_s {
|
||||
// absolute angle inclination in multiple of 0.1 degree 180 deg = 1800
|
||||
|
|
@ -52,6 +52,22 @@ typedef struct imuRuntimeConfig_s {
|
|||
uint8_t small_angle;
|
||||
} imuRuntimeConfig_t;
|
||||
|
||||
typedef enum {
|
||||
ACCPROC_READ = 0,
|
||||
ACCPROC_CHUNK_1,
|
||||
ACCPROC_CHUNK_2,
|
||||
ACCPROC_CHUNK_3,
|
||||
ACCPROC_CHUNK_4,
|
||||
ACCPROC_CHUNK_5,
|
||||
ACCPROC_CHUNK_6,
|
||||
ACCPROC_CHUNK_7,
|
||||
ACCPROC_COPY
|
||||
} accProcessorState_e;
|
||||
|
||||
typedef struct accProcessor_s {
|
||||
accProcessorState_e state;
|
||||
} accProcessor_t;
|
||||
|
||||
void imuConfigure(
|
||||
imuRuntimeConfig_t *initialImuRuntimeConfig,
|
||||
pidProfile_t *initialPidProfile,
|
||||
|
|
@ -61,7 +77,6 @@ void imuConfigure(
|
|||
);
|
||||
|
||||
void calculateEstimatedAltitude(uint32_t currentTime);
|
||||
void imuUpdate(rollAndPitchTrims_t *accelerometerTrims);
|
||||
float calculateThrottleAngleScale(uint16_t throttle_correction_angle);
|
||||
int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value);
|
||||
float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff);
|
||||
|
|
@ -69,5 +84,4 @@ float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff);
|
|||
int16_t imuCalculateHeading(t_fp_vector *vec);
|
||||
|
||||
void imuResetAccelerationSum(void);
|
||||
|
||||
|
||||
void imuUpdate(rollAndPitchTrims_t *accelerometerTrims, uint8_t acc_for_fast_looptime);
|
||||
|
|
|
|||
|
|
@ -20,6 +20,7 @@
|
|||
#include <stdlib.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "common/maths.h"
|
||||
#include "flight/lowpass.h"
|
||||
|
||||
//#define DEBUG_LOWPASS
|
||||
|
|
@ -31,7 +32,7 @@ void generateLowpassCoeffs2(int16_t freq, lowpass_t *filter)
|
|||
|
||||
// generates coefficients for a 2nd-order butterworth low-pass filter
|
||||
float freqf = (float)freq*0.001f;
|
||||
float omega = tanf((float)M_PI*freqf/2.0f);
|
||||
float omega = tan_approx((float)M_PI*freqf/2.0f);
|
||||
float scaling = 1.0f / (omega*omega + 1.4142136f*omega + 1.0f);
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -456,7 +456,7 @@ static void GPS_distance_cm_bearing(int32_t *currentLat1, int32_t *currentLon1,
|
|||
float dLon = (float)(*destinationLon2 - *currentLon1) * GPS_scaleLonDown;
|
||||
*dist = sqrtf(sq(dLat) + sq(dLon)) * DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR_IN_HUNDREDS_OF_KILOMETERS;
|
||||
|
||||
*bearing = 9000.0f + atan2f(-dLat, dLon) * TAN_89_99_DEGREES; // Convert the output radians to 100xdeg
|
||||
*bearing = 9000.0f + atan2_approx(-dLat, dLon) * TAN_89_99_DEGREES; // Convert the output radians to 100xdeg
|
||||
if (*bearing < 0)
|
||||
*bearing += 36000;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -453,6 +453,7 @@ const clivalue_t valueTable[] = {
|
|||
#endif
|
||||
|
||||
{ "acc_hardware", VAR_UINT8 | MASTER_VALUE, &masterConfig.acc_hardware, 0, ACC_MAX },
|
||||
{ "acc_for_fast_looptime", VAR_UINT8 | MASTER_VALUE, &masterConfig.acc_for_fast_looptime, 0, 1 },
|
||||
{ "acc_lpf_factor", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].acc_lpf_factor, 0, 250 },
|
||||
{ "accxy_deadband", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].accDeadband.xy, 0, 100 },
|
||||
{ "accz_deadband", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].accDeadband.z, 0, 100 },
|
||||
|
|
|
|||
|
|
@ -115,13 +115,13 @@ void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfigura
|
|||
void rxInit(rxConfig_t *rxConfig);
|
||||
void gpsInit(serialConfig_t *serialConfig, gpsConfig_t *initialGpsConfig);
|
||||
void navigationInit(gpsProfile_t *initialGpsProfile, pidProfile_t *pidProfile);
|
||||
void imuInit(void);
|
||||
void displayInit(rxConfig_t *intialRxConfig);
|
||||
void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse);
|
||||
void loop(void);
|
||||
void spektrumBind(rxConfig_t *rxConfig);
|
||||
const sonarHardware_t *sonarGetHardwareConfiguration(batteryConfig_t *batteryConfig);
|
||||
void sonarInit(const sonarHardware_t *sonarHardware);
|
||||
void qimuInit(void);
|
||||
|
||||
#ifdef STM32F303xC
|
||||
// from system_stm32f30x.c
|
||||
|
|
@ -396,8 +396,7 @@ void init(void)
|
|||
compassInit();
|
||||
#endif
|
||||
|
||||
imuInit();
|
||||
|
||||
qimuInit();
|
||||
mspInit(&masterConfig.serialConfig);
|
||||
|
||||
#ifdef USE_CLI
|
||||
|
|
|
|||
|
|
@ -20,6 +20,8 @@
|
|||
#include <stdint.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "debug.h"
|
||||
|
||||
#include "platform.h"
|
||||
|
||||
#include "common/maths.h"
|
||||
|
|
@ -80,6 +82,8 @@
|
|||
#include "config/config_profile.h"
|
||||
#include "config/config_master.h"
|
||||
|
||||
#define DEBUG_CYCLE_TIME
|
||||
|
||||
// June 2013 V2.2-dev
|
||||
|
||||
enum {
|
||||
|
|
@ -687,6 +691,13 @@ void processRx(void)
|
|||
void loop(void)
|
||||
{
|
||||
static uint32_t loopTime;
|
||||
|
||||
#ifdef DEBUG_CYCLE_TIME
|
||||
static uint32_t minCycleTime = 0xffffffff;
|
||||
static uint32_t maxCycleTime = 0;
|
||||
static uint32_t clearTime = 0;
|
||||
#endif
|
||||
|
||||
#if defined(BARO) || defined(SONAR)
|
||||
static bool haveProcessedAnnexCodeOnce = false;
|
||||
#endif
|
||||
|
|
@ -732,13 +743,26 @@ void loop(void)
|
|||
if (masterConfig.looptime == 0 || (int32_t)(currentTime - loopTime) >= 0) {
|
||||
loopTime = currentTime + masterConfig.looptime;
|
||||
|
||||
imuUpdate(¤tProfile->accelerometerTrims);
|
||||
imuUpdate(¤tProfile->accelerometerTrims, masterConfig.acc_for_fast_looptime);
|
||||
|
||||
// Measure loop rate just after reading the sensors
|
||||
currentTime = micros();
|
||||
cycleTime = (int32_t)(currentTime - previousTime);
|
||||
previousTime = currentTime;
|
||||
|
||||
#ifdef DEBUG_CYCLE_TIME
|
||||
if (currentTime > clearTime) {
|
||||
clearTime = currentTime + (uint32_t)20000000;
|
||||
minCycleTime = 0xffffffff;
|
||||
maxCycleTime = 0;
|
||||
}
|
||||
if (cycleTime < minCycleTime) minCycleTime = cycleTime;
|
||||
if (cycleTime > maxCycleTime) maxCycleTime = cycleTime;
|
||||
debug[0] = cycleTime;
|
||||
debug[1] = minCycleTime;
|
||||
debug[2] = maxCycleTime;
|
||||
#endif
|
||||
|
||||
// Gyro Low Pass
|
||||
if (currentProfile->pidProfile.gyro_cut_hz) {
|
||||
int axis;
|
||||
|
|
|
|||
|
|
@ -55,17 +55,17 @@
|
|||
#define ACC_SPI_MPU6000_ALIGN CW270_DEG
|
||||
|
||||
// External I2C BARO
|
||||
#define BARO
|
||||
#define USE_BARO_MS5611
|
||||
#define USE_BARO_BMP085
|
||||
//#define BARO
|
||||
//#define USE_BARO_MS5611
|
||||
//#define USE_BARO_BMP085
|
||||
|
||||
// External I2C MAG
|
||||
#define MAG
|
||||
#define USE_MAG_HMC5883
|
||||
//#define MAG
|
||||
//#define USE_MAG_HMC5883
|
||||
|
||||
#define INVERTER
|
||||
#define BEEPER
|
||||
#define DISPLAY
|
||||
//#define DISPLAY
|
||||
|
||||
#define USE_VCP
|
||||
#define USE_USART1
|
||||
|
|
@ -104,15 +104,15 @@
|
|||
#define RSSI_ADC_GPIO_PIN GPIO_Pin_1
|
||||
#define RSSI_ADC_CHANNEL ADC_Channel_1
|
||||
|
||||
#define GPS
|
||||
#define LED_STRIP
|
||||
#define LED_STRIP_TIMER TIM3
|
||||
//#define GPS
|
||||
//#define LED_STRIP
|
||||
//#define LED_STRIP_TIMER TIM3
|
||||
|
||||
#define BLACKBOX
|
||||
#define TELEMETRY
|
||||
//#define TELEMETRY
|
||||
#define SERIAL_RX
|
||||
#define SONAR
|
||||
#define AUTOTUNE
|
||||
//#define SONAR
|
||||
//#define AUTOTUNE
|
||||
#define USE_SERVOS
|
||||
#define USE_CLI
|
||||
|
||||
|
|
@ -125,7 +125,9 @@
|
|||
#endif
|
||||
|
||||
|
||||
#define SPEKTRUM_BIND
|
||||
// USART3, PB11 (Flexport)
|
||||
#define BIND_PORT GPIOB
|
||||
#define BIND_PIN Pin_11
|
||||
//#define SPEKTRUM_BIND
|
||||
//// USART3, PB11 (Flexport)
|
||||
//#define BIND_PORT GPIOB
|
||||
//#define BIND_PIN Pin_11
|
||||
|
||||
#define USE_QUATERNION
|
||||
|
|
@ -20,6 +20,8 @@
|
|||
|
||||
#include <limits.h>
|
||||
|
||||
#include <math.h>
|
||||
|
||||
#define BARO
|
||||
|
||||
extern "C" {
|
||||
|
|
@ -145,3 +147,52 @@ TEST(MathsUnittest, TestRotateVectorAroundAxis)
|
|||
|
||||
expectVectorsAreEqual(&vector, &expected_result);
|
||||
}
|
||||
|
||||
#if defined(FAST_MATH) || defined(VERY_FAST_MATH)
|
||||
TEST(MathsUnittest, TestFastTrigonometrySinCos)
|
||||
{
|
||||
double sinError = 0;
|
||||
for (float x = -10 * M_PI; x < 10 * M_PI; x += M_PI / 300) {
|
||||
double approxResult = sin_approx(x);
|
||||
double libmResult = sinf(x);
|
||||
sinError = MAX(sinError, fabs(approxResult - libmResult));
|
||||
}
|
||||
printf("sin_approx maximum absolute error = %e\n", sinError);
|
||||
EXPECT_LE(sinError, 3e-6);
|
||||
|
||||
double cosError = 0;
|
||||
for (float x = -10 * M_PI; x < 10 * M_PI; x += M_PI / 300) {
|
||||
double approxResult = cos_approx(x);
|
||||
double libmResult = cosf(x);
|
||||
cosError = MAX(cosError, fabs(approxResult - libmResult));
|
||||
}
|
||||
printf("cos_approx maximum absolute error = %e\n", cosError);
|
||||
EXPECT_LE(cosError, 3e-6);
|
||||
}
|
||||
|
||||
TEST(MathsUnittest, TestFastTrigonometryATan2)
|
||||
{
|
||||
double error = 0;
|
||||
for (float x = -1.0f; x < 1.0f; x += 0.01) {
|
||||
for (float y = -1.0f; x < 1.0f; x += 0.001) {
|
||||
double approxResult = atan2_approx(y, x);
|
||||
double libmResult = atan2f(y, x);
|
||||
error = MAX(error, fabs(approxResult - libmResult));
|
||||
}
|
||||
}
|
||||
printf("atan2_approx maximum absolute error = %e rads (%e degree)\n", error, error / M_PI * 180.0f);
|
||||
EXPECT_LE(error, 1e-6);
|
||||
}
|
||||
|
||||
TEST(MathsUnittest, TestFastTrigonometryACos)
|
||||
{
|
||||
double error = 0;
|
||||
for (float x = -1.0f; x < 1.0f; x += 0.001) {
|
||||
double approxResult = acos_approx(x);
|
||||
double libmResult = acos(x);
|
||||
error = MAX(error, fabs(approxResult - libmResult));
|
||||
}
|
||||
printf("acos_approx maximum absolute error = %e rads (%e degree)\n", error, error / M_PI * 180.0f);
|
||||
EXPECT_LE(error, 1e-4);
|
||||
}
|
||||
#endif
|
||||
|
|
|
|||
Loading…
Reference in New Issue