diff --git a/Makefile b/Makefile index ed3c98bb5b..cceea251de 100755 --- a/Makefile +++ b/Makefile @@ -633,6 +633,7 @@ LDFLAGS = -lm \ -static \ -Wl,-gc-sections,-Map,$(TARGET_MAP) \ -Wl,-L$(LINKER_DIR) \ + -Wl,--cref \ -T$(LD_SCRIPT) ############################################################################### diff --git a/docs/Cli.md b/docs/Cli.md index 3fe6d7cb33..fc02b8dbbd 100644 --- a/docs/Cli.md +++ b/docs/Cli.md @@ -205,6 +205,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 | diff --git a/src/main/common/maths.c b/src/main/common/maths.c index 8f1ae62f79..c4a3701013 100644 --- a/src/main/common/maths.c +++ b/src/main/common/maths.c @@ -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) diff --git a/src/main/common/maths.h b/src/main/common/maths.h index 689d09fc69..1ea74491c8 100644 --- a/src/main/common/maths.h +++ b/src/main/common/maths.h @@ -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); diff --git a/src/main/config/config.c b/src/main/config/config.c index 6fe759e511..7921fdc301 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -390,6 +390,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; diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index 3c3a7435ee..6db1530152 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -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 diff --git a/src/main/drivers/gyro_sync.c b/src/main/drivers/gyro_sync.c index d252eef61f..90c7c851b0 100644 --- a/src/main/drivers/gyro_sync.c +++ b/src/main/drivers/gyro_sync.c @@ -89,20 +89,10 @@ void gyroUpdateSampleRate(uint8_t lpf) { #else if (lpf == INV_FILTER_256HZ_NOLPF2) { gyroSamplePeriod = 125; - - if(!sensors(SENSOR_ACC)) { - minLooptime = 625; // Max refresh 1,6khz - } else { - minLooptime = 1625; // Max refresh 615hz - } + minLooptime = 625; // Max refresh 1,6khz } else { gyroSamplePeriod = 1000; - - if(!sensors(SENSOR_ACC)) { - minLooptime = 1000; // Full sampling without ACC - } else { - minLooptime = 2000; - } + minLooptime = 1000; // Full sampling without ACC } #endif mpuDividerDrops = (minLooptime + gyroSamplePeriod -1 ) / gyroSamplePeriod - 1; diff --git a/src/main/drivers/serial.c b/src/main/drivers/serial.c index b827c6b0c2..e814b31fbe 100644 --- a/src/main/drivers/serial.c +++ b/src/main/drivers/serial.c @@ -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); +} diff --git a/src/main/drivers/serial.h b/src/main/drivers/serial.h index d197991f77..d7b3d928b8 100644 --- a/src/main/drivers/serial.h +++ b/src/main/drivers/serial.h @@ -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); diff --git a/src/main/drivers/serial_softserial.c b/src/main/drivers/serial_softserial.c index 7320284ded..fb540d4cee 100644 --- a/src/main/drivers/serial_softserial.c +++ b/src/main/drivers/serial_softserial.c @@ -465,7 +465,9 @@ const struct serialPortVTable softSerialVTable[] = { softSerialSetBaudRate, isSoftSerialTransmitBufferEmpty, softSerialSetMode, - } + .beginWrite = NULL, + .endWrite = NULL, + } }; #endif diff --git a/src/main/drivers/serial_uart.c b/src/main/drivers/serial_uart.c index 474ce65a41..964b00d70f 100644 --- a/src/main/drivers/serial_uart.c +++ b/src/main/drivers/serial_uart.c @@ -286,5 +286,7 @@ const struct serialPortVTable uartVTable[] = { uartSetBaudRate, isUartTransmitBufferEmpty, uartSetMode, + .beginWrite = NULL, + .endWrite = NULL, } }; diff --git a/src/main/drivers/serial_usb_vcp.c b/src/main/drivers/serial_usb_vcp.c index 2aab5ad9a7..1ed1f7795a 100644 --- a/src/main/drivers/serial_usb_vcp.c +++ b/src/main/drivers/serial_usb_vcp.c @@ -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) { diff --git a/src/main/drivers/serial_usb_vcp.h b/src/main/drivers/serial_usb_vcp.h index 536be82dcc..068073c2f0 100644 --- a/src/main/drivers/serial_usb_vcp.h +++ b/src/main/drivers/serial_usb_vcp.h @@ -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); diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index ff599d51eb..9c940901a6 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -20,6 +20,7 @@ #include #include #include +#include #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); +} diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h index c97ebd39ba..f7f4a6077c 100644 --- a/src/main/flight/imu.h +++ b/src/main/flight/imu.h @@ -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); diff --git a/src/main/flight/lowpass.c b/src/main/flight/lowpass.c index ea5f548d4c..bb1b71c9d9 100755 --- a/src/main/flight/lowpass.c +++ b/src/main/flight/lowpass.c @@ -20,6 +20,7 @@ #include #include +#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); diff --git a/src/main/flight/navigation.c b/src/main/flight/navigation.c index e460a2dc3c..7e2f4a7344 100644 --- a/src/main/flight/navigation.c +++ b/src/main/flight/navigation.c @@ -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; } diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 3863efbe05..7910887779 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -461,6 +461,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 }, diff --git a/src/main/main.c b/src/main/main.c index 2626224c91..c1a75f5b00 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -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 diff --git a/src/main/mw.c b/src/main/mw.c index 8adb0a8072..5b243123a2 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -20,6 +20,8 @@ #include #include +#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 { @@ -726,6 +730,13 @@ void filterRc(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 @@ -772,8 +783,8 @@ void loop(void) if (gyroSyncCheckUpdate() || (int32_t)(currentTime - (loopTime + GYRO_WATCHDOG_DELAY)) >= 0) { loopTime = currentTime + targetLooptime; + imuUpdate(¤tProfile->accelerometerTrims, masterConfig.acc_for_fast_looptime); - imuUpdate(¤tProfile->accelerometerTrims); // Measure loop rate just after reading the sensors currentTime = micros(); @@ -781,6 +792,18 @@ void loop(void) previousTime = currentTime; dT = (float)cycleTime * 0.000001f; +#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 filterApply7TapFIR(gyroADC); diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h index 9f23c49ccc..b58936f4a4 100644 --- a/src/main/target/CC3D/target.h +++ b/src/main/target/CC3D/target.h @@ -151,3 +151,5 @@ // USART3, PB11 (Flexport) #define BIND_PORT GPIOB #define BIND_PIN Pin_11 + +#define USE_QUATERNION diff --git a/src/test/unit/maths_unittest.cc b/src/test/unit/maths_unittest.cc index b1a93cc087..f6736dd718 100644 --- a/src/test/unit/maths_unittest.cc +++ b/src/test/unit/maths_unittest.cc @@ -20,6 +20,8 @@ #include +#include + #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