mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-17 13:25:30 +03:00
Added ESC temperature / RPM tracking.
This commit is contained in:
mikeller
parent
40ec36f6c4
commit
8558db3cbb
4 changed files with 139 additions and 68 deletions
|
|
@ -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();
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
|
|
@ -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];
|
||||
}
|
||||
|
||||
int16_t getEscSensorCurrent(void)
|
||||
{
|
||||
return escCurrent;
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
int16_t getEscSensorConsumption(void)
|
||||
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;
|
||||
}
|
||||
|
||||
static void resetEscSensorData(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();
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#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);
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue