rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Fixed current updates and review comments

This commit is contained in:
Bas Delfos 2016-11-10 23:39:34 +01:00
parent 16178a0662
commit 9fe84c0ff2
5 changed files with 61 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/main/drivers/pwm_output.h
View File

@ -52,6 +52,10 @@ typedef enum {
#define PWM_BRUSHED_TIMER_MHZ 24
#endif
#ifdef USE_DSHOT
#define MAX_DSHOT_MOTORS 4
#endif
#define MOTOR_DMA_BUFFER_SIZE 18 /* resolution + frame reset (2us) */
typedef struct {

4
src/main/fc/fc_tasks.c
View File

@ -103,7 +103,7 @@ static void taskUpdateBattery(uint32_t currentTime)
{
#ifdef USE_ADC
static uint32_t vbatLastServiced = 0;
if (feature(FEATURE_VBAT)) {
if (feature(FEATURE_VBAT) || feature(FEATURE_ESC_TELEMETRY)) {
if (cmp32(currentTime, vbatLastServiced) >= VBATINTERVAL) {
vbatLastServiced = currentTime;
updateBattery();
@ -112,7 +112,7 @@ static void taskUpdateBattery(uint32_t currentTime)
#endif
static uint32_t ibatLastServiced = 0;
if (feature(FEATURE_CURRENT_METER)) {
if (feature(FEATURE_CURRENT_METER) || feature(FEATURE_ESC_TELEMETRY)) {
const int32_t ibatTimeSinceLastServiced = cmp32(currentTime, ibatLastServiced);
if (ibatTimeSinceLastServiced >= IBATINTERVAL) {

48
src/main/sensors/battery.c
View File

@ -42,6 +42,8 @@
#include "rx/rx.h"
#include "common/utils.h"
#define VBATT_LPF_FREQ 0.4f
// Battery monitoring stuff
@ -67,25 +69,28 @@ static uint16_t batteryAdcToVoltage(uint16_t src)
static void updateBatteryVoltage(void)
{
#ifdef USE_ESC_TELEMETRY
if (batteryConfig->batteryMeterType == BATTERY_SENSOR_ESC) {
vbat = getEscTelemetryVbat();
}
else {
static biquadFilter_t vbatFilter;
static bool vbatFilterIsInitialised;
#else
// store the battery voltage with some other recent battery voltage readings
uint16_t vbatSample = vbatLatestADC = adcGetChannel(ADC_BATTERY);
static biquadFilter_t vbatFilter;
static bool vbatFilterIsInitialised;
if (debugMode == DEBUG_BATTERY) debug[0] = vbatSample;
// store the battery voltage with some other recent battery voltage readings
uint16_t vbatSample = vbatLatestADC = adcGetChannel(ADC_BATTERY);
if (!vbatFilterIsInitialised) {
biquadFilterInitLPF(&vbatFilter, VBATT_LPF_FREQ, 50000); //50HZ Update
vbatFilterIsInitialised = true;
}
vbatSample = biquadFilterApply(&vbatFilter, vbatSample);
vbat = batteryAdcToVoltage(vbatSample);
if (debugMode == DEBUG_BATTERY) debug[0] = vbatSample;
if (!vbatFilterIsInitialised) {
biquadFilterInitLPF(&vbatFilter, VBATT_LPF_FREQ, 50000); //50HZ Update
vbatFilterIsInitialised = true;
}
vbatSample = biquadFilterApply(&vbatFilter, vbatSample);
vbat = batteryAdcToVoltage(vbatSample);
#endif
if (debugMode == DEBUG_BATTERY) debug[1] = vbat;
}
@ -184,8 +189,14 @@ static int32_t currentSensorToCentiamps(uint16_t src)
void updateCurrentMeter(int32_t lastUpdateAt, rxConfig_t *rxConfig, uint16_t deadband3d_throttle)
{
static int32_t amperageRaw = 0;
#ifdef USE_ESC_TELEMETRY
UNUSED(lastUpdateAt);
#else
static int64_t mAhdrawnRaw = 0;
#endif
static int32_t amperageRaw = 0;
int32_t throttleOffset = (int32_t)rcCommand[THROTTLE] - 1000;
int32_t throttleFactor = 0;
@ -208,18 +219,21 @@ void updateCurrentMeter(int32_t lastUpdateAt, rxConfig_t *rxConfig, uint16_t dea
case CURRENT_SENSOR_NONE:
amperage = 0;
break;
#ifdef USE_ESC_TELEMETRY
case CURRENT_SENSOR_ESC:
amperage = getEscTelemetryCurrent();
break;
#endif
}
#ifdef USE_ESC_TELEMETRY
if (batteryConfig->currentMeterType == CURRENT_SENSOR_ESC) {
mAhDrawn = getEscTelemetryConsumption();
}
else {
mAhdrawnRaw += (amperage * lastUpdateAt) / 1000;
mAhDrawn = mAhdrawnRaw / (3600 * 100);
}
#else
mAhdrawnRaw += (amperage * lastUpdateAt) / 1000;
mAhDrawn = mAhdrawnRaw / (3600 * 100);
#endif
}
float calculateVbatPidCompensation(void) {

44
src/main/telemetry/esc_telemetry.c
View File

@ -58,11 +58,11 @@ set debug_mode = DEBUG_ESC_TELEMETRY in cli
typedef struct {
bool skipped;
uint8_t temperature;
uint16_t voltage;
uint16_t current;
uint16_t consumption;
uint16_t rpm;
int16_t temperature;
int16_t voltage;
int16_t current;
int16_t consumption;
int16_t rpm;
} esc_telemetry_t;
typedef enum {
@ -93,9 +93,9 @@ static uint8_t escTelemetryMotor = 0; // motor index 0 - 3
static bool escTelemetryEnabled = false;
static escTlmTriggerState_t escTelemetryTriggerState = ESC_TLM_TRIGGER_WAIT;
static uint16_t escVbat = 0;
static uint16_t escCurrent = 0;
static uint16_t escConsumption = 0;
static int16_t escVbat = 0;
static int16_t escCurrent = 0;
static int16_t escConsumption = 0;
static void escTelemetryDataReceive(uint16_t c);
static uint8_t update_crc8(uint8_t crc, uint8_t crc_seed);
@ -107,17 +107,17 @@ bool isEscTelemetryActive(void)
return escTelemetryEnabled;
}
uint16_t getEscTelemetryVbat(void)
int16_t getEscTelemetryVbat(void)
{
return escVbat / 10;
}
uint16_t getEscTelemetryCurrent(void)
int16_t getEscTelemetryCurrent(void)
{
return escCurrent;
}
uint16_t getEscTelemetryConsumption(void)
int16_t getEscTelemetryConsumption(void)
{
return escConsumption;
}
@ -199,7 +199,7 @@ uint8_t escTelemetryFrameStatus(void)
void escTelemetryProcess(uint32_t currentTime)
{
UNUSED(currentTime);
uint32_t currentTimeMs = currentTime / 1000;
if (!escTelemetryEnabled) {
return;
@ -215,7 +215,7 @@ void escTelemetryProcess(uint32_t currentTime)
// Ready for starting requesting telemetry
escTelemetryTriggerState = ESC_TLM_TRIGGER_READY;
escTelemetryMotor = 0;
escTriggerTimestamp = millis();
escTriggerTimestamp = currentTimeMs;
}
else if (escTelemetryTriggerState == ESC_TLM_TRIGGER_READY)
{
@ -226,7 +226,7 @@ void escTelemetryProcess(uint32_t currentTime)
escTelemetryTriggerState = ESC_TLM_TRIGGER_PENDING;
}
if (escTriggerTimestamp + ESC_REQUEST_TIMEOUT < millis())
if (escTriggerTimestamp + ESC_REQUEST_TIMEOUT < currentTimeMs)
{
// ESC did not repond in time, skip to next motor
escTelemetryData[escTelemetryMotor].skipped = true;
@ -244,10 +244,10 @@ void escTelemetryProcess(uint32_t currentTime)
// Wait until all ESCs are processed
if (firstCycleComplete)
{
uint8_t i;
int i;
escCurrent = 0;
escConsumption = 0;
for (i = 0; i < 4; i++) // Motor count for Dshot limited to 4
for (i = 0; i < MAX_DSHOT_MOTORS; i++) // Motor count for Dshot limited to 4
{
if (!escTelemetryData[i].skipped)
{
@ -269,7 +269,7 @@ void escTelemetryProcess(uint32_t currentTime)
static void selectNextMotor(void)
{
escTelemetryMotor++;
if (escTelemetryMotor >= 4) { // Motor count for Dshot limited to 4
if (escTelemetryMotor >= MAX_DSHOT_MOTORS) { // Motor count for Dshot limited to 4
escTelemetryMotor = 0;
firstCycleComplete = true;
}
@ -280,19 +280,17 @@ static void selectNextMotor(void)
static uint8_t update_crc8(uint8_t crc, uint8_t crc_seed)
{
uint8_t crc_u, i;
crc_u = crc;
uint8_t crc_u = crc;
crc_u ^= crc_seed;
for ( i=0; i<8; i++) crc_u = ( crc_u & 0x80 ) ? 0x7 ^ ( crc_u << 1 ) : ( crc_u << 1 );
for (int i=0; i<8; i++) crc_u = ( crc_u & 0x80 ) ? 0x7 ^ ( crc_u << 1 ) : ( crc_u << 1 );
return (crc_u);
}
static uint8_t get_crc8(uint8_t *Buf, uint8_t BufLen)
{
uint8_t crc = 0, i;
for( i=0; i<BufLen; i++) crc = update_crc8(Buf[i], crc);
uint8_t crc = 0;
for(int i=0; i<BufLen; i++) crc = update_crc8(Buf[i], crc);
return (crc);
}

6
src/main/telemetry/esc_telemetry.h
View File

@ -3,8 +3,8 @@
uint8_t escTelemetryFrameStatus(void);
bool escTelemetryInit(void);
bool isEscTelemetryActive(void);
uint16_t getEscTelemetryVbat(void);
uint16_t getEscTelemetryCurrent(void);
uint16_t getEscTelemetryConsumption(void);
int16_t getEscTelemetryVbat(void);
int16_t getEscTelemetryCurrent(void);
int16_t getEscTelemetryConsumption(void);
void escTelemetryProcess(uint32_t currentTime);