rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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) 2015-07-10 15:02:38 +10:00 committed by ProDrone
parent c6f5b98a79
commit 45a4f11f92
21 changed files with 823 additions and 188 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
Makefile
View File

@ -611,6 +611,7 @@ LDFLAGS = -lm \
-static \ -static \
-Wl,-gc-sections,-Map,$(TARGET_MAP) \ -Wl,-gc-sections,-Map,$(TARGET_MAP) \
-Wl,-L$(LINKER_DIR) \ -Wl,-L$(LINKER_DIR) \
-Wl,--cref \
-T$(LD_SCRIPT) -T$(LD_SCRIPT)
############################################################################### ###############################################################################

1
docs/Cli.md
View File

@ -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 | | `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 | | `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_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 | | `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 | | `accxy_deadband` | | 0 | 100 | 40 | Profile | UINT8 |
| `accz_deadband` | | 0 | 100 | 40 | Profile | UINT8 | | `accz_deadband` | | 0 | 100 | 40 | Profile | UINT8 |

49
src/main/common/maths.c
View File

@ -21,12 +21,15 @@
#include "axis.h" #include "axis.h"
#include "maths.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 // http://lolengine.net/blog/2011/12/21/better-function-approximations
// Chebyshev http://stackoverflow.com/questions/345085/how-do-trigonometric-functions-work/345117#345117 // 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 // 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 // https://github.com/Crashpilot1000/HarakiriWebstore1/blob/master/src/mw.c#L1235
#if defined(FAST_TRIGONOMETRY) || defined(EVEN_FASTER_TRIGONOMETRY) // sin_approx maximum absolute error = 2.305023e-06
#if defined(EVEN_FASTER_TRIGONOMETRY) // cos_approx maximum absolute error = 2.857298e-06
#define sinPolyCoef3 -1.666568107e-1f #define sinPolyCoef3 -1.666568107e-1f
#define sinPolyCoef5 8.312366210e-3f #define sinPolyCoef5 8.312366210e-3f
#define sinPolyCoef7 -1.849218155e-4f #define sinPolyCoef7 -1.849218155e-4f
@ -37,7 +40,6 @@
#define sinPolyCoef7 -1.980661520e-4f // Double: -1.980661520135080504411629636078917643846e-4 #define sinPolyCoef7 -1.980661520e-4f // Double: -1.980661520135080504411629636078917643846e-4
#define sinPolyCoef9 2.600054768e-6f // Double: 2.600054767890361277123254766503271638682e-6 #define sinPolyCoef9 2.600054768e-6f // Double: 2.600054767890361277123254766503271638682e-6
#endif #endif
float sin_approx(float x) float sin_approx(float x)
{ {
int32_t xint = x; int32_t xint = x;
@ -54,6 +56,47 @@ float cos_approx(float x)
{ {
return sin_approx(x + (0.5f * M_PIf)); 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 #endif
int32_t applyDeadband(int32_t value, int32_t deadband) int32_t applyDeadband(int32_t value, int32_t deadband)

12
src/main/common/maths.h
View File

@ -22,8 +22,8 @@
#endif #endif
// Undefine this for use libc sinf/cosf. Keep this defined to use fast sin/cos approximations // Undefine this for use libc sinf/cosf. Keep this defined to use fast sin/cos approximations
#define FAST_TRIGONOMETRY // order 9 approximation #define FAST_MATH // order 9 approximation
//#define EVEN_FASTER_TRIGONOMETRY // order 7 approximation #define VERY_FAST_MATH // order 7 approximation
// Use floating point M_PI instead explicitly. // Use floating point M_PI instead explicitly.
#define M_PIf 3.14159265358979323846f #define M_PIf 3.14159265358979323846f
@ -88,12 +88,18 @@ int32_t quickMedianFilter5(int32_t * v);
int32_t quickMedianFilter7(int32_t * v); int32_t quickMedianFilter7(int32_t * v);
int32_t quickMedianFilter9(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 sin_approx(float x);
float cos_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 #else
#define sin_approx(x) sinf(x) #define sin_approx(x) sinf(x)
#define cos_approx(x) cosf(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 #endif
void arraySubInt32(int32_t *dest, int32_t *array1, int32_t *array2, int count); void arraySubInt32(int32_t *dest, int32_t *array1, int32_t *array2, int count);

1
src/main/config/config.c
View File

@ -392,6 +392,7 @@ static void resetConf(void)
masterConfig.boardAlignment.pitchDegrees = 0; masterConfig.boardAlignment.pitchDegrees = 0;
masterConfig.boardAlignment.yawDegrees = 0; masterConfig.boardAlignment.yawDegrees = 0;
masterConfig.acc_hardware = ACC_DEFAULT; // default/autodetect masterConfig.acc_hardware = ACC_DEFAULT; // default/autodetect
masterConfig.acc_for_fast_looptime = 0;
masterConfig.max_angle_inclination = 500; // 50 degrees masterConfig.max_angle_inclination = 500; // 50 degrees
masterConfig.yaw_control_direction = 1; masterConfig.yaw_control_direction = 1;
masterConfig.gyroConfig.gyroMovementCalibrationThreshold = 32; masterConfig.gyroConfig.gyroMovementCalibrationThreshold = 32;

1
src/main/config/config_master.h
View File

@ -46,6 +46,7 @@ typedef struct master_t {
int8_t yaw_control_direction; // change control direction of yaw (inverted, normal) 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_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_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_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 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

11
src/main/drivers/serial.c
View File

@ -65,3 +65,14 @@ void serialSetMode(serialPort_t *instance, portMode_t mode)
instance->vTable->setMode(instance, 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);
}

7
src/main/drivers/serial.h
View File

@ -72,6 +72,10 @@ struct serialPortVTable {
bool (*isSerialTransmitBufferEmpty)(serialPort_t *instance); bool (*isSerialTransmitBufferEmpty)(serialPort_t *instance);
void (*setMode)(serialPort_t *instance, portMode_t mode); 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); 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); bool isSerialTransmitBufferEmpty(serialPort_t *instance);
void serialPrint(serialPort_t *instance, const char *str); void serialPrint(serialPort_t *instance, const char *str);
uint32_t serialGetBaudRate(serialPort_t *instance); uint32_t serialGetBaudRate(serialPort_t *instance);
void serialBeginWrite(serialPort_t *instance);
void serialEndWrite(serialPort_t *instance);

4
src/main/drivers/serial_softserial.c
View File

@ -465,7 +465,9 @@ const struct serialPortVTable softSerialVTable[] = {
softSerialSetBaudRate, softSerialSetBaudRate,
isSoftSerialTransmitBufferEmpty, isSoftSerialTransmitBufferEmpty,
softSerialSetMode, softSerialSetMode,
} .beginWrite = NULL,
.endWrite = NULL,
}
}; };
#endif #endif

2
src/main/drivers/serial_uart.c
View File

@ -286,5 +286,7 @@ const struct serialPortVTable uartVTable[] = {
uartSetBaudRate, uartSetBaudRate,
isUartTransmitBufferEmpty, isUartTransmitBufferEmpty,
uartSetMode, uartSetMode,
.beginWrite = NULL,
.endWrite = NULL,
} }
}; };

68
src/main/drivers/serial_usb_vcp.c
View File

@ -27,6 +27,7 @@
#include "usb_core.h" #include "usb_core.h"
#include "usb_init.h" #include "usb_init.h"
#include "hw_config.h" #include "hw_config.h"
#include "common/utils.h"
#include "drivers/system.h" #include "drivers/system.h"
@ -38,7 +39,7 @@
static vcpPort_t vcpPort; static vcpPort_t vcpPort;
void usbVcpSetBaudRate(serialPort_t *instance, uint32_t baudRate) static void usbVcpSetBaudRate(serialPort_t *instance, uint32_t baudRate)
{ {
UNUSED(instance); UNUSED(instance);
UNUSED(baudRate); UNUSED(baudRate);
@ -46,7 +47,7 @@ void usbVcpSetBaudRate(serialPort_t *instance, uint32_t baudRate)
// TODO implement // TODO implement
} }
void usbVcpSetMode(serialPort_t *instance, portMode_t mode) static void usbVcpSetMode(serialPort_t *instance, portMode_t mode)
{ {
UNUSED(instance); UNUSED(instance);
UNUSED(mode); UNUSED(mode);
@ -54,20 +55,20 @@ void usbVcpSetMode(serialPort_t *instance, portMode_t mode)
// TODO implement // TODO implement
} }
bool isUsbVcpTransmitBufferEmpty(serialPort_t *instance) static bool isUsbVcpTransmitBufferEmpty(serialPort_t *instance)
{ {
UNUSED(instance); UNUSED(instance);
return true; return true;
} }
uint8_t usbVcpAvailable(serialPort_t *instance) static uint8_t usbVcpAvailable(serialPort_t *instance)
{ {
UNUSED(instance); UNUSED(instance);
return receiveLength & 0xFF; // FIXME use uint32_t return type everywhere 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); UNUSED(instance);
@ -82,24 +83,63 @@ uint8_t usbVcpRead(serialPort_t *instance)
return buf[0]; 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 txed;
uint32_t start = millis(); uint32_t start = millis();
if (!(usbIsConnected() && usbIsConfigured())) {
return;
}
do { do {
txed = CDC_Send_DATA((uint8_t*)&c, 1); txed = CDC_Send_DATA(port->txBuf, count);
} while (txed < 1 && (millis() - start < USB_TIMEOUT)); } 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) serialPort_t *usbVcpOpen(void)
{ {

12
src/main/drivers/serial_usb_vcp.h
View File

@ -22,13 +22,11 @@
typedef struct { typedef struct {
serialPort_t port; 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; } vcpPort_t;
serialPort_t *usbVcpOpen(void); 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);

686
src/main/flight/imu.c
View File

@ -20,6 +20,7 @@
#include <stdbool.h> #include <stdbool.h>
#include <stdint.h> #include <stdint.h>
#include <math.h> #include <math.h>
#include <string.h>
#include "common/maths.h" #include "common/maths.h"
@ -46,6 +47,8 @@
#include "config/runtime_config.h" #include "config/runtime_config.h"
//#define DEBUG_IMU
int16_t accSmooth[XYZ_AXIS_COUNT]; int16_t accSmooth[XYZ_AXIS_COUNT];
int32_t accSum[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 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 imuRuntimeConfig_t *imuRuntimeConfig;
static pidProfile_t *pidProfile; static pidProfile_t *pidProfile;
static accDeadband_t *accDeadband; static accDeadband_t *accDeadband;
static accProcessor_t accProc;
static void qAccProcessingStateMachine(rollAndPitchTrims_t *accelerometerTrims, uint8_t acc_for_fast_looptime);
void imuConfigure( void imuConfigure(
imuRuntimeConfig_t *initialImuRuntimeConfig, imuRuntimeConfig_t *initialImuRuntimeConfig,
@ -84,13 +91,6 @@ void imuConfigure(
throttleAngleScale = calculateThrottleAngleScale(throttle_correction_angle); 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) float calculateThrottleAngleScale(uint16_t throttle_correction_angle)
{ {
return (1800.0f / M_PIf) * (900.0f / throttle_correction_angle); return (1800.0f / M_PIf) * (900.0f / throttle_correction_angle);
@ -130,50 +130,6 @@ void imuResetAccelerationSum(void)
accTimeSum = 0; 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 * 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; 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, //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. // 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); head = lrintf(hd);
// Arctan returns a value in the range -180 to 180 degrees. We 'normalize' negative angles to be positive. // 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; return head;
} }
static void imuCalculateEstimatedAttitude(void) void imuUpdate(rollAndPitchTrims_t *accelerometerTrims, uint8_t acc_for_fast_looptime)
{
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)
{ {
gyroUpdate(); gyroUpdate();
if (sensors(SENSOR_ACC)) { if (sensors(SENSOR_ACC)) {
updateAccelerationReadings(accelerometerTrims); // TODO rename to accelerometerUpdate and rename many other 'Acceleration' references to be 'Accelerometer' qAccProcessingStateMachine(accelerometerTrims, acc_for_fast_looptime);
imuCalculateEstimatedAttitude();
} else { } else {
accADC[X] = 0; accADC[X] = 0;
accADC[Y] = 0; accADC[Y] = 0;
@ -323,8 +207,554 @@ int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value)
if (cosZ <= 0.015f) { if (cosZ <= 0.015f) {
return 0; return 0;
} }
int angle = lrintf(acosf(cosZ) * throttleAngleScale); int angle = lrintf(acos_approx(cosZ) * throttleAngleScale);
if (angle > 900) if (angle > 900)
angle = 900; angle = 900;
return lrintf(throttle_correction_value * sin_approx(angle / (900.0f * M_PIf / 2.0f))); 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);
}

34
src/main/flight/imu.h
View File

@ -18,13 +18,13 @@
#pragma once #pragma once
extern int16_t throttleAngleCorrection; extern int16_t throttleAngleCorrection;
extern uint32_t accTimeSum; extern uint32_t accTimeSum; // altitudehold.c
extern int accSumCount; extern int accSumCount; // altitudehold.c
extern float accVelScale; extern float accVelScale; // altitudehold.c
extern t_fp_vector EstG; extern t_fp_vector EstG; // display.c
extern int16_t accSmooth[XYZ_AXIS_COUNT]; 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]; extern int32_t accSum[XYZ_AXIS_COUNT]; // altitudehold.c
extern int16_t smallAngle; extern int16_t smallAngle; // display.c
typedef struct rollAndPitchInclination_s { typedef struct rollAndPitchInclination_s {
// absolute angle inclination in multiple of 0.1 degree 180 deg = 1800 // absolute angle inclination in multiple of 0.1 degree 180 deg = 1800
@ -52,6 +52,22 @@ typedef struct imuRuntimeConfig_s {
uint8_t small_angle; uint8_t small_angle;
} imuRuntimeConfig_t; } 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( void imuConfigure(
imuRuntimeConfig_t *initialImuRuntimeConfig, imuRuntimeConfig_t *initialImuRuntimeConfig,
pidProfile_t *initialPidProfile, pidProfile_t *initialPidProfile,
@ -61,7 +77,6 @@ void imuConfigure(
); );
void calculateEstimatedAltitude(uint32_t currentTime); void calculateEstimatedAltitude(uint32_t currentTime);
void imuUpdate(rollAndPitchTrims_t *accelerometerTrims);
float calculateThrottleAngleScale(uint16_t throttle_correction_angle); float calculateThrottleAngleScale(uint16_t throttle_correction_angle);
int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value); int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value);
float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff); float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff);
@ -69,5 +84,4 @@ float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff);
int16_t imuCalculateHeading(t_fp_vector *vec); int16_t imuCalculateHeading(t_fp_vector *vec);
void imuResetAccelerationSum(void); void imuResetAccelerationSum(void);
void imuUpdate(rollAndPitchTrims_t *accelerometerTrims, uint8_t acc_for_fast_looptime);

3
src/main/flight/lowpass.c
View File

@ -20,6 +20,7 @@
#include <stdlib.h> #include <stdlib.h>
#include <math.h> #include <math.h>
#include "common/maths.h"
#include "flight/lowpass.h" #include "flight/lowpass.h"
//#define DEBUG_LOWPASS //#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 // generates coefficients for a 2nd-order butterworth low-pass filter
float freqf = (float)freq*0.001f; 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); float scaling = 1.0f / (omega*omega + 1.4142136f*omega + 1.0f);

2
src/main/flight/navigation.c
View File

@ -456,7 +456,7 @@ static void GPS_distance_cm_bearing(int32_t *currentLat1, int32_t *currentLon1,
float dLon = (float)(*destinationLon2 - *currentLon1) * GPS_scaleLonDown; float dLon = (float)(*destinationLon2 - *currentLon1) * GPS_scaleLonDown;
*dist = sqrtf(sq(dLat) + sq(dLon)) * DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR_IN_HUNDREDS_OF_KILOMETERS; *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) if (*bearing < 0)
*bearing += 36000; *bearing += 36000;
} }

1
src/main/io/serial_cli.c
View File

@ -453,6 +453,7 @@ const clivalue_t valueTable[] = {
#endif #endif
{ "acc_hardware", VAR_UINT8 | MASTER_VALUE, &masterConfig.acc_hardware, 0, ACC_MAX }, { "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 }, { "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 }, { "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 }, { "accz_deadband", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].accDeadband.z, 0, 100 },

5
src/main/main.c
View File

@ -115,13 +115,13 @@ void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfigura
void rxInit(rxConfig_t *rxConfig); void rxInit(rxConfig_t *rxConfig);
void gpsInit(serialConfig_t *serialConfig, gpsConfig_t *initialGpsConfig); void gpsInit(serialConfig_t *serialConfig, gpsConfig_t *initialGpsConfig);
void navigationInit(gpsProfile_t *initialGpsProfile, pidProfile_t *pidProfile); void navigationInit(gpsProfile_t *initialGpsProfile, pidProfile_t *pidProfile);
void imuInit(void);
void displayInit(rxConfig_t *intialRxConfig); void displayInit(rxConfig_t *intialRxConfig);
void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse); void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse);
void loop(void); void loop(void);
void spektrumBind(rxConfig_t *rxConfig); void spektrumBind(rxConfig_t *rxConfig);
const sonarHardware_t *sonarGetHardwareConfiguration(batteryConfig_t *batteryConfig); const sonarHardware_t *sonarGetHardwareConfiguration(batteryConfig_t *batteryConfig);
void sonarInit(const sonarHardware_t *sonarHardware); void sonarInit(const sonarHardware_t *sonarHardware);
void qimuInit(void);
#ifdef STM32F303xC #ifdef STM32F303xC
// from system_stm32f30x.c // from system_stm32f30x.c
@ -396,8 +396,7 @@ void init(void)
compassInit(); compassInit();
#endif #endif
imuInit(); qimuInit();
mspInit(&masterConfig.serialConfig); mspInit(&masterConfig.serialConfig);
#ifdef USE_CLI #ifdef USE_CLI

26
src/main/mw.c
View File

@ -20,6 +20,8 @@
#include <stdint.h> #include <stdint.h>
#include <math.h> #include <math.h>
#include "debug.h"
#include "platform.h" #include "platform.h"
#include "common/maths.h" #include "common/maths.h"
@ -80,6 +82,8 @@
#include "config/config_profile.h" #include "config/config_profile.h"
#include "config/config_master.h" #include "config/config_master.h"
#define DEBUG_CYCLE_TIME
// June 2013 V2.2-dev // June 2013 V2.2-dev
enum { enum {
@ -687,6 +691,13 @@ void processRx(void)
void loop(void) void loop(void)
{ {
static uint32_t loopTime; 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) #if defined(BARO) || defined(SONAR)
static bool haveProcessedAnnexCodeOnce = false; static bool haveProcessedAnnexCodeOnce = false;
#endif #endif
@ -732,13 +743,26 @@ void loop(void)
if (masterConfig.looptime == 0 || (int32_t)(currentTime - loopTime) >= 0) { if (masterConfig.looptime == 0 || (int32_t)(currentTime - loopTime) >= 0) {
loopTime = currentTime + masterConfig.looptime; loopTime = currentTime + masterConfig.looptime;
imuUpdate(&currentProfile->accelerometerTrims); imuUpdate(&currentProfile->accelerometerTrims, masterConfig.acc_for_fast_looptime);
// Measure loop rate just after reading the sensors // Measure loop rate just after reading the sensors
currentTime = micros(); currentTime = micros();
cycleTime = (int32_t)(currentTime - previousTime); cycleTime = (int32_t)(currentTime - previousTime);
previousTime = currentTime; 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 // Gyro Low Pass
if (currentProfile->pidProfile.gyro_cut_hz) { if (currentProfile->pidProfile.gyro_cut_hz) {
int axis; int axis;

34
src/main/target/CC3D/target.h
View File

@ -55,17 +55,17 @@
#define ACC_SPI_MPU6000_ALIGN CW270_DEG #define ACC_SPI_MPU6000_ALIGN CW270_DEG
// External I2C BARO // External I2C BARO
#define BARO //#define BARO
#define USE_BARO_MS5611 //#define USE_BARO_MS5611
#define USE_BARO_BMP085 //#define USE_BARO_BMP085
// External I2C MAG // External I2C MAG
#define MAG //#define MAG
#define USE_MAG_HMC5883 //#define USE_MAG_HMC5883
#define INVERTER #define INVERTER
#define BEEPER #define BEEPER
#define DISPLAY //#define DISPLAY
#define USE_VCP #define USE_VCP
#define USE_USART1 #define USE_USART1
@ -104,15 +104,15 @@
#define RSSI_ADC_GPIO_PIN GPIO_Pin_1 #define RSSI_ADC_GPIO_PIN GPIO_Pin_1
#define RSSI_ADC_CHANNEL ADC_Channel_1 #define RSSI_ADC_CHANNEL ADC_Channel_1
#define GPS //#define GPS
#define LED_STRIP //#define LED_STRIP
#define LED_STRIP_TIMER TIM3 //#define LED_STRIP_TIMER TIM3
#define BLACKBOX #define BLACKBOX
#define TELEMETRY //#define TELEMETRY
#define SERIAL_RX #define SERIAL_RX
#define SONAR //#define SONAR
#define AUTOTUNE //#define AUTOTUNE
#define USE_SERVOS #define USE_SERVOS
#define USE_CLI #define USE_CLI
@ -125,7 +125,9 @@
#endif #endif
#define SPEKTRUM_BIND //#define SPEKTRUM_BIND
// USART3, PB11 (Flexport) //// USART3, PB11 (Flexport)
#define BIND_PORT GPIOB //#define BIND_PORT GPIOB
#define BIND_PIN Pin_11 //#define BIND_PIN Pin_11
#define USE_QUATERNION

51
src/test/unit/maths_unittest.cc
View File

@ -20,6 +20,8 @@
#include <limits.h> #include <limits.h>
#include <math.h>
#define BARO #define BARO
extern "C" { extern "C" {
@ -145,3 +147,52 @@ TEST(MathsUnittest, TestRotateVectorAroundAxis)
expectVectorsAreEqual(&vector, &expected_result); 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