Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-21 07:15:18 +03:00
Code Issues Wiki Activity

Added the parameters for RPM filtering to MSP.

Browse source
This commit is contained in:
mikeller 2019-09-05 01:08:10 +12:00 • committed by Michael Keller
parent 2553a4a1f9
commit 3f0f3ab685
1 changed files with 29 additions and 6 deletions
Download patch file Download diff file Expand all files Collapse all files

35
src/main/msp/msp.c
View file

@ -76,12 +76,13 @@
#include "fc/rc_modes.h" #include "fc/rc_modes.h"
#include "fc/runtime_config.h" #include "fc/runtime_config.h"
#include "flight/position.h"
#include "flight/failsafe.h" #include "flight/failsafe.h"
#include "flight/gps_rescue.h" #include "flight/gps_rescue.h"
#include "flight/imu.h" #include "flight/imu.h"
#include "flight/mixer.h" #include "flight/mixer.h"
#include "flight/pid.h" #include "flight/pid.h"
#include "flight/position.h"
#include "flight/rpm_filter.h"
#include "flight/servos.h" #include "flight/servos.h"
#include "io/asyncfatfs/asyncfatfs.h" #include "io/asyncfatfs/asyncfatfs.h"
@ -1017,7 +1018,7 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
bool rpmDataAvailable = false; bool rpmDataAvailable = false;
#ifdef USE_DSHOT_TELEMETRY #ifdef USE_DSHOT_TELEMETRY
if (useDshotTelemetry) { if (motorConfig()->dev.useDshotTelemetry) {
rpm = (int)getDshotTelemetry(i) * 100 * 2 / motorConfig()->motorPoleCount; rpm = (int)getDshotTelemetry(i) * 100 * 2 / motorConfig()->motorPoleCount;
rpmDataAvailable = true; rpmDataAvailable = true;
invalidPct = 10000; // 100.00% invalidPct = 10000; // 100.00%
@ -1186,7 +1187,7 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
sbufWriteU8(dst, getMotorCount()); sbufWriteU8(dst, getMotorCount());
sbufWriteU8(dst, motorConfig()->motorPoleCount); sbufWriteU8(dst, motorConfig()->motorPoleCount);
#ifdef USE_DSHOT_TELEMETRY #ifdef USE_DSHOT_TELEMETRY
sbufWriteU8(dst, useDshotTelemetry); // DSHOT telemetry is enabled sbufWriteU8(dst, motorConfig()->dev.useDshotTelemetry);
#else #else
sbufWriteU8(dst, 0); sbufWriteU8(dst, 0);
#endif #endif
@ -1556,8 +1557,8 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
sbufWriteU16(dst, 0); sbufWriteU16(dst, 0);
sbufWriteU16(dst, 0); sbufWriteU16(dst, 0);
#endif #endif
#if defined(USE_GYRO_DATA_ANALYSE)
// Added in MSP API 1.42 // Added in MSP API 1.42
#if defined(USE_GYRO_DATA_ANALYSE)
sbufWriteU8(dst, gyroConfig()->dyn_notch_range); sbufWriteU8(dst, gyroConfig()->dyn_notch_range);
sbufWriteU8(dst, gyroConfig()->dyn_notch_width_percent); sbufWriteU8(dst, gyroConfig()->dyn_notch_width_percent);
sbufWriteU16(dst, gyroConfig()->dyn_notch_q); sbufWriteU16(dst, gyroConfig()->dyn_notch_q);
@ -1568,6 +1569,15 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
sbufWriteU16(dst, 0); sbufWriteU16(dst, 0);
sbufWriteU16(dst, 0); sbufWriteU16(dst, 0);
#endif #endif
#if defined(USE_RPM_FILTER)
sbufWriteU8(dst, rpmFilterConfig()->gyro_rpm_notch_harmonics);
sbufWriteU8(dst, rpmFilterConfig()->gyro_rpm_notch_min);
#else
sbufWriteU8(dst, 0);
sbufWriteU8(dst, 0);
#endif
break; break;
case MSP_PID_ADVANCED: case MSP_PID_ADVANCED:
sbufWriteU16(dst, 0); sbufWriteU16(dst, 0);
@ -2118,8 +2128,13 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
motorConfigMutable()->mincommand = sbufReadU16(src); motorConfigMutable()->mincommand = sbufReadU16(src);
// version 1.42 // version 1.42
if (sbufBytesRemaining(src)) { if (sbufBytesRemaining(src) >= 2) {
motorConfigMutable()->motorPoleCount = sbufReadU8(src); motorConfigMutable()->motorPoleCount = sbufReadU8(src);
#if defined(USE_DSHOT_TELEMETRY)
motorConfigMutable()->dev.useDshotTelemetry = sbufReadU8(src);
#else
sbufReadU8(src);
#endif
} }
break; break;
@ -2336,7 +2351,7 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
sbufReadU16(src); sbufReadU16(src);
#endif #endif
} }
if (sbufBytesRemaining(src) >= 6) { if (sbufBytesRemaining(src) >= 8) {
// Added in MSP API 1.42 // Added in MSP API 1.42
#if defined(USE_GYRO_DATA_ANALYSE) #if defined(USE_GYRO_DATA_ANALYSE)
gyroConfigMutable()->dyn_notch_range = sbufReadU8(src); gyroConfigMutable()->dyn_notch_range = sbufReadU8(src);
@ -2349,6 +2364,14 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
sbufReadU16(src); sbufReadU16(src);
sbufReadU16(src); sbufReadU16(src);
#endif #endif
#if defined(USE_RPM_FILTER)
rpmFilterConfigMutable()->gyro_rpm_notch_harmonics = sbufReadU8(src);
rpmFilterConfigMutable()->gyro_rpm_notch_min = sbufReadU8(src);
#else
sbufReadU8(src);
sbufReadU8(src);
#endif
} }
// reinitialize the gyro filters with the new values // reinitialize the gyro filters with the new values