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

Merge pull request #11772 from damosvil/use_calc_esc_rpm_to_erpm_to_rpm

Browse source 
Unify all eRPM to RPM calculations into one single function
This commit is contained in:
haslinghuis 2022-09-17 14:26:26 +02:00 • committed by GitHub
commit a90e2e5fb1
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
14 changed files with 65 additions and 43 deletions
Download patch file Download diff file Expand all files Collapse all files

8
src/main/cli/cli.c
View file

@ -165,7 +165,6 @@ bool cliMode = false;
#include "sensors/battery.h"
#include "sensors/boardalignment.h"
#include "sensors/compass.h"
#include "sensors/esc_sensor.h"
#include "sensors/gyro.h"
#include "sensors/gyro_init.h"
#include "sensors/sensors.h"
@ -6169,6 +6168,9 @@ static void cliDshotTelemetryInfo(const char *cmdName, char *cmdline)
#endif
for (uint8_t i = 0; i < getMotorCount(); i++) {
const uint16_t erpm = getDshotTelemetry(i);
const uint16_t rpm = erpmToRpm(erpm);
cliPrintf("%5d %c%c%c%c%c %6d %6d %6d",
i + 1,
((dshotTelemetryState.motorState[i].telemetryTypes & (1 << DSHOT_TELEMETRY_TYPE_eRPM)) ? 'R' : '-'),
@ -6176,9 +6178,7 @@ static void cliDshotTelemetryInfo(const char *cmdName, char *cmdline)
((dshotTelemetryState.motorState[i].telemetryTypes & (1 << DSHOT_TELEMETRY_TYPE_VOLTAGE)) ? 'V' : '-'),
((dshotTelemetryState.motorState[i].telemetryTypes & (1 << DSHOT_TELEMETRY_TYPE_CURRENT)) ? 'C' : '-'),
((dshotTelemetryState.motorState[i].telemetryTypes & (1 << DSHOT_TELEMETRY_TYPE_STATE_EVENTS)) ? 'S' : '-'),
dshotTelemetryState.motorState[i].telemetryData[DSHOT_TELEMETRY_TYPE_eRPM] * 100,
dshotTelemetryState.motorState[i].telemetryData[DSHOT_TELEMETRY_TYPE_eRPM] * 100 * 2 / motorConfig()->motorPoleCount,
dshotTelemetryState.motorState[i].telemetryData[DSHOT_TELEMETRY_TYPE_eRPM] * 100 * 2 / motorConfig()->motorPoleCount / 60);
erpm * 100, rpm, rpm / 60);
#ifdef USE_DSHOT_TELEMETRY_STATS
if (isDshotMotorTelemetryActive(i)) {

13
src/main/drivers/dshot.c
View file

@ -40,6 +40,8 @@
#include "drivers/dshot_command.h"
#include "drivers/nvic.h"
#include "flight/mixer.h"
#include "rx/rx.h"
#include "dshot.h"
@ -188,6 +190,17 @@ FAST_CODE void dshotUpdateTelemetryData(uint8_t motorIndex, dshotTelemetryType_t
}
}
uint32_t erpmToRpm(uint16_t erpm)
{
// rpm = (erpm * 100) / (motorConfig()->motorPoleCount / 2)
return (erpm * 200) / motorConfig()->motorPoleCount;
}
uint32_t getDshotAverageRpm(void)
{
return dshotTelemetryState.averageRpm;
}
#endif // USE_DSHOT_TELEMETRY
#ifdef USE_DSHOT_TELEMETRY_STATS

3
src/main/drivers/dshot.h
View file

@ -95,6 +95,7 @@ typedef struct dshotTelemetryState_s {
uint32_t readCount;
dshotTelemetryMotorState_t motorState[MAX_SUPPORTED_MOTORS];
uint32_t inputBuffer[MAX_GCR_EDGES];
uint32_t averageRpm;
} dshotTelemetryState_t;
extern dshotTelemetryState_t dshotTelemetryState;
@ -105,6 +106,8 @@ void updateDshotTelemetryQuality(dshotTelemetryQuality_t *qualityStats, bool pac
#endif
uint16_t getDshotTelemetry(uint8_t index);
uint32_t erpmToRpm(uint16_t erpm);
uint32_t getDshotAverageRpm(void);
bool isDshotMotorTelemetryActive(uint8_t motorIndex);
bool isDshotTelemetryActive(void);

15
src/main/drivers/dshot_bitbang.c
View file

@ -495,6 +495,9 @@ static bool bbMotorConfig(IO_t io, uint8_t motorIndex, motorPwmProtocolTypes_e p
static bool bbUpdateStart(void)
{
uint32_t totalErpm;
uint32_t totalMotorsWithErpmData;
#ifdef USE_DSHOT_TELEMETRY
if (useDshotTelemetry) {
#ifdef USE_DSHOT_TELEMETRY_STATS
@ -507,6 +510,9 @@ static bool bbUpdateStart(void)
return false;
}
totalErpm = 0;
totalMotorsWithErpmData = 0;
for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
dshotTelemetryType_t type;
#ifdef USE_DSHOT_CACHE_MGMT
@ -543,6 +549,10 @@ static bool bbUpdateStart(void)
dshotTelemetryState.readCount++;
if (value != DSHOT_TELEMETRY_INVALID) {
if (type == DSHOT_TELEMETRY_TYPE_eRPM) {
totalErpm += value;
totalMotorsWithErpmData++;
}
dshotUpdateTelemetryData(motorIndex, type, value);
if (motorIndex < 4) {
DEBUG_SET(DEBUG_DSHOT_RPM_TELEMETRY, motorIndex, value);
@ -554,6 +564,11 @@ static bool bbUpdateStart(void)
updateDshotTelemetryQuality(&dshotTelemetryQuality[motorIndex], value != DSHOT_TELEMETRY_INVALID, currentTimeMs);
#endif
}
// Calculate average when possible
if (totalMotorsWithErpmData > 0) {
dshotTelemetryState.averageRpm = erpmToRpm(totalErpm / totalMotorsWithErpmData);
}
}
#endif
for (int i = 0; i < usedMotorPorts; i++) {

10
src/main/drivers/pwm_output_dshot_shared.c
View file

@ -187,6 +187,8 @@ FAST_CODE_NOINLINE bool pwmStartDshotMotorUpdate(void)
#endif
const timeUs_t currentUs = micros();
dshotTelemetryType_t type;
uint32_t totalErpm = 0;
uint32_t totalMotorsWithErpmData = 0;
for (int i = 0; i < dshotPwmDevice.count; i++) {
timeDelta_t usSinceInput = cmpTimeUs(currentUs, inputStampUs);
@ -220,6 +222,10 @@ FAST_CODE_NOINLINE bool pwmStartDshotMotorUpdate(void)
bool validTelemetryPacket = false;
#endif
if (value != DSHOT_TELEMETRY_INVALID) {
if (type == DSHOT_TELEMETRY_TYPE_eRPM) {
totalErpm += value;
totalMotorsWithErpmData++;
}
dshotUpdateTelemetryData(value, type, value);
if (i < 4) {
DEBUG_SET(DEBUG_DSHOT_RPM_TELEMETRY, i, value);
@ -240,6 +246,10 @@ FAST_CODE_NOINLINE bool pwmStartDshotMotorUpdate(void)
}
pwmDshotSetDirectionOutput(&dmaMotors[i]);
}
if (totalMotorsWithErpmData > 0) {
dshotTelemetryState.averageRpm = erpmToRpm(totalErpm / totalMotorsWithErpmData);
}
inputStampUs = 0;
dshotEnableChannels(dshotPwmDevice.count);
return true;

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

@ -139,7 +139,6 @@
#include "sensors/battery.h"
#include "sensors/boardalignment.h"
#include "sensors/compass.h"
#include "sensors/esc_sensor.h"
#include "sensors/gyro.h"
#include "sensors/gyro_init.h"
#include "sensors/rangefinder.h"
@ -1211,7 +1210,7 @@ static bool mspProcessOutCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, sbuf_t
#ifdef USE_DSHOT_TELEMETRY
if (motorConfig()->dev.useDshotTelemetry) {
rpm = getDshotTelemetry(i) * 100 * 2 / motorConfig()->motorPoleCount;
rpm = erpmToRpm(getDshotTelemetry(i));
rpmDataAvailable = true;
invalidPct = 10000; // 100.00%
@ -1247,7 +1246,7 @@ static bool mspProcessOutCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, sbuf_t
if (featureIsEnabled(FEATURE_ESC_SENSOR)) {
escSensorData_t *escData = getEscSensorData(i);
if (!rpmDataAvailable) { // We want DSHOT telemetry RPM data (if available) to have precedence
rpm = calcEscRpm(escData->rpm);
rpm = erpmToRpm(escData->rpm);
rpmDataAvailable = true;
}
escTemperature = escData->temperature;

18
src/main/osd/osd.c
View file

@ -94,7 +94,6 @@
#include "sensors/acceleration.h"
#include "sensors/battery.h"
#include "sensors/esc_sensor.h"
#include "sensors/sensors.h"
#ifdef USE_HARDWARE_REVISION_DETECTION
@ -502,19 +501,14 @@ static void osdResetStats(void)
#if defined(USE_ESC_SENSOR) || defined(USE_DSHOT_TELEMETRY)
static int32_t getAverageEscRpm(void)
{
#ifdef USE_DSHOT_TELEMETRY
if (motorConfig()->dev.useDshotTelemetry) {
uint32_t rpm = 0;
for (int i = 0; i < getMotorCount(); i++) {
rpm += getDshotTelemetry(i);
}
rpm = rpm / getMotorCount();
return rpm * 100 * 2 / motorConfig()->motorPoleCount;
}
#endif
#ifdef USE_ESC_SENSOR
if (featureIsEnabled(FEATURE_ESC_SENSOR)) {
return calcEscRpm(osdEscDataCombined->rpm);
return erpmToRpm(osdEscDataCombined->rpm);
}
#endif
#ifdef USE_DSHOT_TELEMETRY
if (motorConfig()->dev.useDshotTelemetry) {
return getDshotAverageRpm();
}
#endif
return 0;

5
src/main/osd/osd_elements.c
View file

@ -159,7 +159,6 @@
#include "sensors/adcinternal.h"
#include "sensors/barometer.h"
#include "sensors/battery.h"
#include "sensors/esc_sensor.h"
#include "sensors/sensors.h"
#ifdef USE_GPS_PLUS_CODES
@ -259,12 +258,12 @@ static int getEscRpm(int i)
{
#ifdef USE_DSHOT_TELEMETRY
if (motorConfig()->dev.useDshotTelemetry) {
return 100.0f / (motorConfig()->motorPoleCount / 2.0f) * getDshotTelemetry(i);
return erpmToRpm(getDshotTelemetry(i));
}
#endif
#ifdef USE_ESC_SENSOR
if (featureIsEnabled(FEATURE_ESC_SENSOR)) {
return calcEscRpm(getEscSensorData(i)->rpm);
return erpmToRpm(getEscSensorData(i)->rpm);
}
#endif
return 0;

2
src/main/osd/osd_warnings.c
View file

@ -273,7 +273,7 @@ void renderOsdWarning(char *warningText, bool *blinking, uint8_t *displayAttr)
const char motorNumber = '1' + i;
// if everything is OK just display motor number else R, T or C
char warnFlag = motorNumber;
if (ARMING_FLAG(ARMED) && osdConfig()->esc_rpm_alarm != ESC_RPM_ALARM_OFF && calcEscRpm(escData->rpm) <= osdConfig()->esc_rpm_alarm) {
if (ARMING_FLAG(ARMED) && osdConfig()->esc_rpm_alarm != ESC_RPM_ALARM_OFF && erpmToRpm(escData->rpm) <= (uint32_t)osdConfig()->esc_rpm_alarm) {
warnFlag = 'R';
}
if (osdConfig()->esc_temp_alarm != ESC_TEMP_ALARM_OFF && escData->temperature >= osdConfig()->esc_temp_alarm) {

6
src/main/sensors/esc_sensor.c
View file

@ -271,7 +271,7 @@ static uint8_t decodeEscFrame(void)
frameStatus = ESC_SENSOR_FRAME_COMPLETE;
if (escSensorMotor < 4) {
DEBUG_SET(DEBUG_ESC_SENSOR_RPM, escSensorMotor, calcEscRpm(escSensorData[escSensorMotor].rpm) / 10); // output actual rpm/10 to fit in 16bit signed.
DEBUG_SET(DEBUG_ESC_SENSOR_RPM, escSensorMotor, erpmToRpm(escSensorData[escSensorMotor].rpm) / 10); // output actual rpm/10 to fit in 16bit signed.
DEBUG_SET(DEBUG_ESC_SENSOR_TMP, escSensorMotor, escSensorData[escSensorMotor].temperature);
}
} else {
@ -361,8 +361,4 @@ void escSensorProcess(timeUs_t currentTimeUs)
}
}
int calcEscRpm(int erpm)
{
return (erpm * 100) / (motorConfig()->motorPoleCount / 2);
}
#endif

2
src/main/sensors/esc_sensor.h
View file

@ -54,5 +54,3 @@ void startEscDataRead(uint8_t *frameBuffer, uint8_t frameLength);
uint8_t getNumberEscBytesRead(void);
uint8_t calculateCrc8(const uint8_t *Buf, const uint8_t BufLen);
int calcEscRpm(int erpm);

3
src/main/telemetry/frsky_hub.c
View file

@ -44,6 +44,7 @@
#include "drivers/sensor.h"
#include "drivers/serial.h"
#include "drivers/time.h"
#include "drivers/dshot.h"
#include "config/config.h"
#include "fc/rc_controls.h"
@ -189,7 +190,7 @@ static void sendThrottleOrBatterySizeAsRpm(void)
#if defined(USE_ESC_SENSOR_TELEMETRY)
escSensorData_t *escData = getEscSensorData(ESC_SENSOR_COMBINED);
if (escData) {
data = escData->dataAge < ESC_DATA_INVALID ? (calcEscRpm(escData->rpm) / 10) : 0;
data = escData->dataAge < ESC_DATA_INVALID ? (erpmToRpm(escData->rpm) / 10) : 0;
}
#else
if (ARMING_FLAG(ARMED)) {

5
src/main/telemetry/smartport.c
View file

@ -45,6 +45,7 @@
#include "drivers/compass/compass.h"
#include "drivers/sensor.h"
#include "drivers/time.h"
#include "drivers/dshot.h"
#include "config/config.h"
#include "fc/controlrate_profile.h"
@ -656,7 +657,7 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear
case FSSP_DATAID_RPM :
escData = getEscSensorData(ESC_SENSOR_COMBINED);
if (escData != NULL) {
smartPortSendPackage(id, calcEscRpm(escData->rpm));
smartPortSendPackage(id, erpmToRpm(escData->rpm));
*clearToSend = false;
}
break;
@ -670,7 +671,7 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear
case FSSP_DATAID_RPM8 :
escData = getEscSensorData(id - FSSP_DATAID_RPM1);
if (escData != NULL) {
smartPortSendPackage(id, calcEscRpm(escData->rpm));
smartPortSendPackage(id, erpmToRpm(escData->rpm));
*clearToSend = false;
}
break;

13
src/main/telemetry/srxl.c
View file

@ -193,16 +193,9 @@ uint16_t getMotorAveragePeriod(void)
#endif
#if defined(USE_DSHOT_TELEMETRY)
if (useDshotTelemetry) {
uint16_t motors = getMotorCount();
if (motors > 0) {
for (int motor = 0; motor < motors; motor++) {
rpm += getDshotTelemetry(motor);
}
rpm = 100.0f / (motorConfig()->motorPoleCount / 2.0f) * rpm; // convert erpm freq to RPM.
rpm /= motors; // Average combined rpm
}
// Calculate this way when no rpm from esc data
if (useDshotTelemetry && rpm == 0) {
rpm = getDshotAverageRpm();
}
#endif