diff --git a/src/main/build/version.h b/src/main/build/version.h index ed00a47c31..562af0c0d2 100644 --- a/src/main/build/version.h +++ b/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) diff --git a/src/main/interface/cli.c b/src/main/interface/cli.c index f79a8f6697..80bac75d60 100644 --- a/src/main/interface/cli.c +++ b/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,7 +373,10 @@ 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); @@ -378,7 +384,11 @@ static void cliPrintf(const char *format, ...) { } -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: diff --git a/src/main/interface/cli.h b/src/main/interface/cli.h index 41796d0c19..3a3d5e6273 100644 --- a/src/main/interface/cli.h +++ b/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 diff --git a/src/main/interface/cli_debug_print.h b/src/main/interface/cli_debug_print.h new file mode 100644 index 0000000000..afe0329b4b --- /dev/null +++ b/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 . + */ + +// 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 diff --git a/src/main/interface/msp.c b/src/main/interface/msp.c index 8392f3ed67..b3aebb427d 100644 --- a/src/main/interface/msp.c +++ b/src/main/interface/msp.c @@ -745,7 +745,7 @@ 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) { @@ -753,12 +753,16 @@ bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst) { } 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])); diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 95e0d370e8..60d6a62df3 100644 --- a/src/main/sensors/gyro.c +++ b/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 \ No newline at end of file +#endif