Merge branch 'master' into emuboost2

src/main/build/version.h

@@ -25,7 +25,7 @@
 #define FC_FIRMWARE_NAME            "EmuFlight"
 #define FC_VERSION_MAJOR            0  // increment when a major release is made (big new feature, etc)
 #define FC_VERSION_MINOR            4  // increment when a minor release is made (small new feature, change etc)
-#define FC_VERSION_PATCH_LEVEL      1 // increment when a bug is fixed
+#define FC_VERSION_PATCH_LEVEL      2 // increment when a bug is fixed
 
 #define FC_VERSION_STRING STR(FC_VERSION_MAJOR) "." STR(FC_VERSION_MINOR) "." STR(FC_VERSION_PATCH_LEVEL)
 

src/main/interface/cli.c

@@ -283,7 +283,10 @@ static void backupAndResetConfigs(void) {
     resetConfigs();
 }
 
-static void cliPrint(const char *str) {
+void cliPrint(const char *str) {
+    if (!cliMode) {
+        return;
+    }
     while (*str) {
         if(cliSmartMode) {
             //no carriage returns. Those are dumb.
@@ -299,14 +302,14 @@ static void cliPrint(const char *str) {
     bufWriterFlush(cliWriter);
 }
 
-static void cliPrintLinefeed(void) {
+void cliPrintLinefeed(void) {
     cliPrint("\r\n");
     if(cliSmartMode) {
         bufWriterFlush(cliWriter);
     }
 }
 
-static void cliPrintLine(const char *str) {
+void cliPrintLine(const char *str) {
     cliPrint(str);
     cliPrintLinefeed();
 }
@@ -370,15 +373,22 @@ static bool cliDefaultPrintLinef(uint8_t dumpMask, bool equalsDefault, const cha
     }
 }
 
-static void cliPrintf(const char *format, ...) {
+void cliPrintf(const char *format, ...) {
+    if (!cliMode) {
+        return;
+    }
     va_list va;
     va_start(va, format);
     cliPrintfva(format, va);
     va_end(va);
 }
 
 
-static void cliPrintLinef(const char *format, ...) {
+void cliPrintLinef(const char *format, ...) {
+    if (!cliMode) {
+        return;
+    }
+
     va_list va;
     va_start(va, format);
     cliPrintfva(format, va);
@@ -2395,7 +2405,7 @@ void cliRxBind(char *cmdline) {
     case RX_SPI_REDPINE:
     case RX_SPI_SFHSS:
         cc2500SpiBind();
-        cliPrint("Binding...");
+        cliPrintf("Binding...");
         break;
 #endif
     default:

src/main/interface/cli.h

@@ -32,3 +32,11 @@ void cliInit(const struct serialConfig_s *serialConfig);
 void cliProcess(void);
 struct serialPort_s;
 void cliEnter(struct serialPort_s *serialPort);
+
+#ifdef USE_CLI_DEBUG_PRINT
+void cliPrint(const char *str);
+void cliPrintLinefeed(void);
+void cliPrintLine(const char *str);
+void cliPrintf(const char *format, ...);
+void cliPrintLinef(const char *format, ...);
+#endif

src/main/interface/cli_debug_print.h

@@ -0,0 +1,49 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+// Provides: cliPrintDebug... functions for displaying debugging information in the CLI
+//
+// Usage: Make sure USE_CLI_DEBUG_PRINT is defined
+//        Include this header in your code
+//        Add cliDebugPrint... statements as needed in your code
+//        Use the CLI to see the output of the debugging statements
+//
+// Cautions: Be sure to include rate limiting logic to your debug printing
+//           if needed otherwise you can flood the output.
+//
+//           Be sure to reverse the Usage steps above to remove the debugging
+//           elements before submitting final code.
+
+#include "platform.h"
+
+#ifdef USE_CLI_DEBUG_PRINT
+
+#include "interface/cli.h"
+
+// Commands to print debugging information to the CLI
+#define cliDebugPrintLinefeed cliPrintLinefeed
+#define cliDebugPrintLinef cliPrintLinef
+#define cliDebugPrintLine cliPrintLine
+#define cliDebugPrintf cliPrintf
+#define cliDebugPrint cliPrint
+
+#else
+#error "Do not #include cli_debug_print.h unless you intend to do debugging and also define USE_CLI_DEBUG_PRINT"
+#endif

src/main/interface/msp.c

@@ -745,20 +745,24 @@ bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst) {
     case MSP_RAW_IMU: {
         // Hack scale due to choice of units for sensor data in multiwii
         uint8_t scale = 1;
-#ifndef USE_GYRO_IMUF9001
+//#ifndef USE_GYRO_IMUF9001
         if (acc.dev.acc_1G > 512 * 4) {
             scale = 8;
         } else if (acc.dev.acc_1G > 512 * 2) {
             scale = 4;
         } else if (acc.dev.acc_1G >= 512) {
             scale = 2;
         }
-#endif //USE_GYRO_IMUF9001
+//#endif //USE_GYRO_IMUF9001
         for (int i = 0; i < 3; i++) {
             sbufWriteU16(dst, lrintf(acc.accADC[i] / scale));
         }
         for (int i = 0; i < 3; i++) {
+#ifdef USE_GYRO_IMUF9001
+            sbufWriteU16(dst, gyroRateDps(i) * scale);
+#else
             sbufWriteU16(dst, gyroRateDps(i));
+#endif
         }
         for (int i = 0; i < 3; i++) {
             sbufWriteU16(dst, lrintf(mag.magADC[i]));

src/main/sensors/gyro.c

@@ -1141,11 +1141,6 @@ static FAST_CODE_NOINLINE void gyroUpdateSensor(gyroSensor_t* gyroSensor, timeUs
     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
         // NOTE: this branch optimized for when there is no gyro debugging, ensure it is kept in step with non-optimized branch
         DEBUG_SET(DEBUG_GYRO_SCALED, axis, lrintf(gyroSensor->gyroDev.gyroADCf[axis]));
-        if (!gyroSensor->overflowDetected) {
-            // integrate using trapezium rule to avoid bias
-            accumulatedMeasurements[axis] += 0.5f * (gyroPrevious[axis] + gyro.gyroADCf[axis]) * gyro.targetLooptime;
-            gyroPrevious[axis] = gyroSensor->gyroDev.gyroADCf[axis];
-        }
     }
     if (!isGyroSensorCalibrationComplete(gyroSensor)) {
         performGyroCalibration(gyroSensor, gyroConfig()->gyroMovementCalibrationThreshold);
@@ -1298,7 +1293,7 @@ FAST_CODE_NOINLINE void gyroUpdate(timeUs_t currentTimeUs) {
     if (!overflowDetected) {
         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
             // integrate using trapezium rule to avoid bias
-            accumulatedMeasurements[axis] += 0.5f * (gyroPrevious[axis] + gyro.gyroADCf[axis]) * gyro.targetLooptime;
+            accumulatedMeasurements[axis] += (gyroPrevious[axis] + gyro.gyroADCf[axis]);
             gyroPrevious[axis] = gyro.gyroADCf[axis];
         }
         accumulatedMeasurementCount++;
@@ -1307,11 +1302,11 @@ FAST_CODE_NOINLINE void gyroUpdate(timeUs_t currentTimeUs) {
 
 bool gyroGetAverage(quaternion *vAverage) {
     if (accumulatedMeasurementCount) {
-        const timeUs_t accumulatedMeasurementTimeUs = accumulatedMeasurementCount * gyro.targetLooptime;
+        const timeUs_t accumulatedMeasurementTimeUs = accumulatedMeasurementCount;
         vAverage->w = 0;
-        vAverage->x = DEGREES_TO_RADIANS(accumulatedMeasurements[X] / accumulatedMeasurementTimeUs);
-        vAverage->y = DEGREES_TO_RADIANS(accumulatedMeasurements[Y] / accumulatedMeasurementTimeUs);
-        vAverage->z = DEGREES_TO_RADIANS(accumulatedMeasurements[Z] / accumulatedMeasurementTimeUs);
+        vAverage->x = 0.5f * DEGREES_TO_RADIANS(accumulatedMeasurements[X] / accumulatedMeasurementTimeUs);
+        vAverage->y = 0.5f * DEGREES_TO_RADIANS(accumulatedMeasurements[Y] / accumulatedMeasurementTimeUs);
+        vAverage->z = 0.5f * DEGREES_TO_RADIANS(accumulatedMeasurements[Z] / accumulatedMeasurementTimeUs);
         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
             accumulatedMeasurements[axis] = 0.0f;
         }
@@ -1413,4 +1408,4 @@ void initYawSpinRecovery(int maxYawRate)
     yawSpinRecoveryEnabled = enabledFlag;
     yawSpinRecoveryThreshold = threshold;
 }
-#endif
+#endif