diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index f090ae756c..dbdf6bf428 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -85,7 +85,8 @@ static void updateBatteryVoltage(void) #ifdef USE_ESC_SENSOR if (feature(FEATURE_ESC_SENSOR) && batteryConfig->batteryMeterType == BATTERY_SENSOR_ESC) { - vbatLatest = getEscSensorVbat(); + escSensorData_t escData = getEscSensorData(ESC_SENSOR_COMBINED); + vbatLatest = escData.stale ? 0 : escData.voltage / 10; if (debugMode == DEBUG_BATTERY) { debug[0] = -1; } @@ -293,9 +294,15 @@ void updateCurrentMeter(int32_t lastUpdateAt, rxConfig_t *rxConfig, uint16_t dea case CURRENT_SENSOR_ESC: #ifdef USE_ESC_SENSOR if (feature(FEATURE_ESC_SENSOR)) { - amperageLatest = getEscSensorCurrent(); + escSensorData_t escData = getEscSensorData(ESC_SENSOR_COMBINED); + if (!escData.stale) { + amperageLatest = escData.current; + mAhDrawn = escData.consumption; + } else { + amperageLatest = 0; + mAhDrawn = 0; + } amperage = amperageLatest; - mAhDrawn = getEscSensorConsumption(); updateConsumptionWarning(); } diff --git a/src/main/sensors/esc_sensor.c b/src/main/sensors/esc_sensor.c index af2c0aa11e..365161db59 100644 --- a/src/main/sensors/esc_sensor.c +++ b/src/main/sensors/esc_sensor.c @@ -1,3 +1,20 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it 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 is distributed in the hope that it 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 Cleanflight. If not, see . + */ + #include #include #include @@ -21,10 +38,10 @@ #include "sensors/battery.h" -#include "esc_sensor.h" - #include "build/debug.h" +#include "esc_sensor.h" + /* KISS ESC TELEMETRY PROTOCOL --------------------------- @@ -50,22 +67,15 @@ DEBUG INFORMATION set debug_mode = DEBUG_ESC_TELEMETRY in cli -0: current motor index requested -1: number of timeouts -2: voltage -3: current */ #ifdef USE_DSHOT - -typedef struct { - bool skipped; - int16_t temperature; - int16_t voltage; - int16_t current; - int16_t consumption; - int16_t rpm; -} esc_telemetry_t; +enum { + DEBUG_ESC_MOTOR_INDEX = 1, + DEBUG_ESC_NUM_TIMEOUTS = 2, + DEBUG_ESC_TEMPERATURE = 3, + DEBUG_ESC_RPM = 3 +}; typedef enum { ESC_SENSOR_FRAME_PENDING = 1 << 0, // 1 @@ -87,7 +97,7 @@ static bool tlmFrameDone = false; static uint8_t tlm[ESC_SENSOR_BUFFSIZE] = { 0, }; static uint8_t tlmFramePosition = 0; static serialPort_t *escSensorPort = NULL; -static esc_telemetry_t escSensorData[MAX_SUPPORTED_MOTORS]; +static escSensorData_t escSensorData[MAX_SUPPORTED_MOTORS]; static uint32_t escTriggerTimestamp = -1; static uint32_t escLastResponseTimestamp; static uint8_t timeoutRetryCount = 0; @@ -97,10 +107,6 @@ static uint8_t escSensorMotor = 0; // motor index static bool escSensorEnabled = false; static escSensorTriggerState_t escSensorTriggerState = ESC_SENSOR_TRIGGER_WAIT; -static int16_t escVbat = 0; -static int16_t escCurrent = 0; -static int16_t escConsumption = 0; - static void escSensorDataReceive(uint16_t c); static uint8_t update_crc8(uint8_t crc, uint8_t crc_seed); static uint8_t get_crc8(uint8_t *Buf, uint8_t BufLen); @@ -111,19 +117,51 @@ bool isEscSensorActive(void) return escSensorEnabled; } -int16_t getEscSensorVbat(void) +escSensorData_t getEscSensorData(uint8_t motorNumber) { - return escVbat / 10; + if (motorNumber < getMotorCount()) { + return escSensorData[motorNumber]; + } + + escSensorData_t combinedEscSensorData = { + .stale = true, + .temperature = 0, + .voltage = 0, + .current = 0, + .consumption = 0, + .rpm = 0 + }; + if (motorNumber == ESC_SENSOR_COMBINED) { + unsigned int activeSensors = 0; + for (int i = 0; i < getMotorCount(); i = i + 1) { + if (!escSensorData[i].stale) { + combinedEscSensorData.temperature = MAX(combinedEscSensorData.temperature, escSensorData[i].temperature); + combinedEscSensorData.voltage += escSensorData[i].voltage; + combinedEscSensorData.current += escSensorData[i].current; + combinedEscSensorData.consumption += escSensorData[i].consumption; + combinedEscSensorData.rpm += escSensorData[i].rpm; + activeSensors = activeSensors + 1; + } + } + + if (activeSensors > 0) { + combinedEscSensorData.stale = false; + combinedEscSensorData.voltage = combinedEscSensorData.voltage / activeSensors; + combinedEscSensorData.rpm = combinedEscSensorData.rpm / activeSensors; + + DEBUG_SET(DEBUG_ESC_SENSOR, DEBUG_ESC_TEMPERATURE, combinedEscSensorData.temperature); + DEBUG_SET(DEBUG_ESC_SENSOR, DEBUG_ESC_RPM, combinedEscSensorData.rpm); + } + } + + return combinedEscSensorData; } -int16_t getEscSensorCurrent(void) +static void resetEscSensorData(void) { - return escCurrent; -} - -int16_t getEscSensorConsumption(void) -{ - return escConsumption; + for (int i; i < MAX_SUPPORTED_MOTORS; i = i + 1) { + escSensorData[i].stale = true; + } } bool escSensorInit(void) @@ -142,10 +180,12 @@ bool escSensorInit(void) escSensorEnabled = true; } + resetEscSensorData(); + return escSensorPort != NULL; } -void freeEscSensorPort(void) +static void freeEscSensorPort(void) { closeSerialPort(escSensorPort); escSensorPort = NULL; @@ -170,7 +210,7 @@ static void escSensorDataReceive(uint16_t c) } } -uint8_t escSensorFrameStatus(void) +static uint8_t escSensorFrameStatus(void) { uint8_t frameStatus = ESC_SENSOR_FRAME_PENDING; uint16_t chksum, tlmsum; @@ -186,7 +226,7 @@ uint8_t escSensorFrameStatus(void) tlmsum = tlm[ESC_SENSOR_BUFFSIZE - 1]; // last byte contains CRC value if (chksum == tlmsum) { - escSensorData[escSensorMotor].skipped = false; + escSensorData[escSensorMotor].stale = false; escSensorData[escSensorMotor].temperature = tlm[0]; escSensorData[escSensorMotor].voltage = tlm[1] << 8 | tlm[2]; escSensorData[escSensorMotor].current = tlm[3] << 8 | tlm[4]; @@ -219,7 +259,7 @@ void escSensorProcess(timeUs_t currentTimeUs) escLastResponseTimestamp = escTriggerTimestamp; } else if (escSensorTriggerState == ESC_SENSOR_TRIGGER_READY) { - DEBUG_SET(DEBUG_ESC_SENSOR, 0, escSensorMotor+1); + DEBUG_SET(DEBUG_ESC_SENSOR, DEBUG_ESC_MOTOR_INDEX, escSensorMotor + 1); motorDmaOutput_t * const motor = getMotorDmaOutput(escSensorMotor); motor->requestTelemetry = true; @@ -235,35 +275,17 @@ void escSensorProcess(timeUs_t currentTimeUs) if (timeoutRetryCount == 4) { // Not responding after 3 times, skip motor - escSensorData[escSensorMotor].skipped = true; + escSensorData[escSensorMotor].stale = true; selectNextMotor(); } - DEBUG_SET(DEBUG_ESC_SENSOR, 1, ++totalRetryCount); + DEBUG_SET(DEBUG_ESC_SENSOR, DEBUG_ESC_NUM_TIMEOUTS, ++totalRetryCount); } // Get received frame status uint8_t state = escSensorFrameStatus(); if (state == ESC_SENSOR_FRAME_COMPLETE) { - // Wait until all ESCs are processed - if (escSensorMotor == getMotorCount()-1) { - escCurrent = 0; - escConsumption = 0; - escVbat = 0; - - for (int i = 0; i < getMotorCount(); i++) { - if (!escSensorData[i].skipped) { - escVbat = i > 0 ? ((escVbat + escSensorData[i].voltage) / 2) : escSensorData[i].voltage; - escCurrent = escCurrent + escSensorData[i].current; - escConsumption = escConsumption + escSensorData[i].consumption; - } - } - } - - DEBUG_SET(DEBUG_ESC_SENSOR, 2, escVbat); - DEBUG_SET(DEBUG_ESC_SENSOR, 3, escCurrent); - selectNextMotor(); escSensorTriggerState = ESC_SENSOR_TRIGGER_READY; escLastResponseTimestamp = currentTimeMs; @@ -274,8 +296,8 @@ void escSensorProcess(timeUs_t currentTimeUs) // ESCs did not respond for 10 seconds // Disable ESC telemetry and reset voltage and current to let the use know something is wrong freeEscSensorPort(); - escVbat = 0; - escCurrent = 0; + + resetEscSensorData(); } } diff --git a/src/main/sensors/esc_sensor.h b/src/main/sensors/esc_sensor.h index 4ea61ee13c..3d8aff416b 100644 --- a/src/main/sensors/esc_sensor.h +++ b/src/main/sensors/esc_sensor.h @@ -1,10 +1,37 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it 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 is distributed in the hope that it 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 Cleanflight. If not, see . + */ + #pragma once -uint8_t escSensorFrameStatus(void); -bool escSensorInit(void); -bool isEscSensorActive(void); -int16_t getEscSensorVbat(void); -int16_t getEscSensorCurrent(void); -int16_t getEscSensorConsumption(void); +#include "common/time.h" + +typedef struct { + bool stale; + int8_t temperature; + int16_t voltage; + int16_t current; + int16_t consumption; + int16_t rpm; +} escSensorData_t; + +bool escSensorInit(void); +void escSensorProcess(timeUs_t currentTime); + +#define ESC_SENSOR_COMBINED 255 + +escSensorData_t getEscSensorData(uint8_t motorNumber); -void escSensorProcess(uint32_t currentTime); diff --git a/src/main/telemetry/frsky.c b/src/main/telemetry/frsky.c index 5f1700bea7..5d8f1afcb0 100644 --- a/src/main/telemetry/frsky.c +++ b/src/main/telemetry/frsky.c @@ -30,6 +30,7 @@ #include "common/maths.h" #include "common/axis.h" +#include "common/utils.h" #include "config/feature.h" @@ -61,6 +62,10 @@ #include "telemetry/telemetry.h" #include "telemetry/frsky.h" +#ifdef USE_ESC_SENSOR +#include "sensors/esc_sensor.h" +#endif + static serialPort_t *frskyPort = NULL; static serialPortConfig_t *portConfig; @@ -195,23 +200,33 @@ static void sendGpsAltitude(void) static void sendThrottleOrBatterySizeAsRpm(rxConfig_t *rxConfig, uint16_t deadband3d_throttle) { - uint16_t throttleForRPM = rcCommand[THROTTLE] / BLADE_NUMBER_DIVIDER; sendDataHead(ID_RPM); +#ifdef USE_ESC_SENSOR + UNUSED(rxConfig); + UNUSED(deadband3d_throttle); + + escSensorData_t escData = getEscSensorData(ESC_SENSOR_COMBINED); + serialize16(escData.stale ? 0 : escData.rpm); +#else if (ARMING_FLAG(ARMED)) { throttleStatus_e throttleStatus = calculateThrottleStatus(rxConfig, deadband3d_throttle); + uint16_t throttleForRPM = rcCommand[THROTTLE] / BLADE_NUMBER_DIVIDER; if (throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP)) throttleForRPM = 0; serialize16(throttleForRPM); } else { serialize16((batteryConfig->batteryCapacity / BLADE_NUMBER_DIVIDER)); } - +#endif } static void sendTemperature1(void) { sendDataHead(ID_TEMPRATURE1); -#ifdef BARO +#if defined(USE_ESC_SENSOR) + escSensorData_t escData = getEscSensorData(ESC_SENSOR_COMBINED); + serialize16(escData.stale ? 0 : escData.temperature); +#elif defined(BARO) serialize16((baro.baroTemperature + 50)/ 100); //Airmamaf #else serialize16(telemTemperature1 / 10);