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

CF/BF - add support for reading all voltage and current meters via MSP. 

Refactored current meter API and state, now it's more closely aligned
with the the voltage meter API.
This commit is contained in:
Hydra 2017-03-17 19:42:28 +00:00 committed by Dominic Clifton
parent b0c49caa3d
commit 2f99749003
8 changed files with 460 additions and 174 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
src/main/fc/fc_msp.c
View File

@ -923,26 +923,33 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
sbufWriteU8(dst, (uint8_t)getBatteryState());
break;
}
/*
case MSP_VOLTAGE_METERS:
// TODO write out voltage, once for each meter id we support
for (int i = 0; i < MAX_VOLTAGE_METERS; i++) {
// write out id and voltage meter values, once for each meter we support
for (int i = 0; i < supportedVoltageMeterCount; i++) {
uint16_t voltage = getVoltageMeter(i)->vbat;
sbufWriteU8(dst, (uint8_t)constrain(voltage, 0, 255));
voltageMeter_t meter;
uint8_t id = (uint8_t)voltageMeterIds[i];
voltageMeterRead(id, &meter);
sbufWriteU8(dst, id);
sbufWriteU8(dst, (uint8_t)constrain(meter.filtered, 0, 255));
}
break;
case MSP_CURRENT_METERS:
// TODO write out amperage meter for each meter id we support
for (int i = 0; i < MAX_AMPERAGE_METERS; i++) {
amperageMeter_t *meter = getAmperageMeter(i);
// write out amperage, once for each current meter.
sbufWriteU16(dst, (uint16_t)constrain(meter->amperage * 10, 0, 0xFFFF)); // send amperage in 0.001 A steps. Negative range is truncated to zero
sbufWriteU32(dst, meter->mAhDrawn);
// write out id and current meter values, once for each meter we support
for (int i = 0; i < supportedCurrentMeterCount; i++) {
currentMeter_t meter;
uint8_t id = (uint8_t)currentMeterIds[i];
currentMeterRead(id, &meter);
sbufWriteU8(dst, id);
sbufWriteU16(dst, (uint16_t)constrain(meter.mAhDrawn, 0, 0xFFFF)); // milliamp hours drawn from battery
sbufWriteU16(dst, (uint16_t)constrain(meter.amperage * 10, 0, 0xFFFF)); // send amperage in 0.001 A steps (mA). Negative range is truncated to zero
}
break;
*/
case MSP_VOLTAGE_METER_CONFIG:
// by using a sensor type and a sub-frame length it's possible to configure any type of voltage meter,
// e.g. an i2c/spi/can sensor or any sensor not built directly into the FC such as ESC/RX/SPort/SBus that has

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

@ -279,12 +279,20 @@ void batteryInit(void)
// current
//
consumptionState = BATTERY_OK;
resetCurrentMeterState(&currentMeter);
currentMeterReset(&currentMeter);
switch(batteryConfig()->currentMeterSource) {
case CURRENT_METER_ADC:
currentMeterADCInit();
break;
case CURRENT_METER_VIRTUAL:
currentMeterVirtualInit();
break;
case CURRENT_METER_ESC:
currentMeterESCInit();
break;
default:
break;
}
@ -309,13 +317,14 @@ static void batteryUpdateConsumptionState(void)
void batteryUpdateCurrentMeter(int32_t lastUpdateAt, bool armed)
{
if (batteryCellCount == 0) {
resetCurrentMeterState(&currentMeter);
currentMeterReset(&currentMeter);
return;
}
switch(batteryConfig()->currentMeterSource) {
case CURRENT_METER_ADC:
currentUpdateADCMeter(&currentMeter, lastUpdateAt);
currentMeterADCRefresh(lastUpdateAt);
currentMeterADCRead(&currentMeter);
break;
case CURRENT_METER_VIRTUAL: {
@ -323,21 +332,23 @@ void batteryUpdateCurrentMeter(int32_t lastUpdateAt, bool armed)
bool throttleLowAndMotorStop = (throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP));
int32_t throttleOffset = (int32_t)rcCommand[THROTTLE] - 1000;
currentUpdateVirtualMeter(&currentMeter, lastUpdateAt, armed, throttleLowAndMotorStop, throttleOffset);
currentMeterVirtualRefresh(lastUpdateAt, armed, throttleLowAndMotorStop, throttleOffset);
currentMeterVirtualRead(&currentMeter);
break;
}
case CURRENT_METER_ESC:
#ifdef USE_ESC_SENSOR
if (feature(FEATURE_ESC_SENSOR)) {
currentUpdateESCMeter(&currentMeter, lastUpdateAt);
currentMeterESCRefresh(lastUpdateAt);
currentMeterESCReadCombined(&currentMeter);
}
#endif
break;
default:
// case CURRENT_METER_NONE:
// resetCurrentMeterState(&currentMeter);
case CURRENT_METER_NONE:
currentMeterReset(&currentMeter);
break;
}
}

189
src/main/sensors/current.c
View File

@ -36,6 +36,43 @@
#include "sensors/current.h"
#include "sensors/esc_sensor.h"
const uint8_t currentMeterIds[] = {
CURRENT_METER_ID_VBAT_1,
CURRENT_METER_ID_VIRTUAL_1,
#ifdef USE_ESC_SENSOR
CURRENT_METER_ID_ESC_COMBINED_1,
CURRENT_METER_ID_ESC_MOTOR_1,
CURRENT_METER_ID_ESC_MOTOR_2,
CURRENT_METER_ID_ESC_MOTOR_3,
CURRENT_METER_ID_ESC_MOTOR_4,
CURRENT_METER_ID_ESC_MOTOR_5,
CURRENT_METER_ID_ESC_MOTOR_6,
CURRENT_METER_ID_ESC_MOTOR_7,
CURRENT_METER_ID_ESC_MOTOR_8,
CURRENT_METER_ID_ESC_MOTOR_9,
CURRENT_METER_ID_ESC_MOTOR_10,
CURRENT_METER_ID_ESC_MOTOR_11,
CURRENT_METER_ID_ESC_MOTOR_12,
#endif
};
const uint8_t supportedCurrentMeterCount = ARRAYLEN(currentMeterIds);
//
// ADC/Virtual/ESC shared
//
void currentMeterReset(currentMeter_t *meter)
{
meter->amperage = 0;
meter->amperageLatest = 0;
meter->mAhDrawn = 0;
}
//
// ADC/Virtual shared
//
#define ADCVREF 3300 // in mV
#define IBAT_LPF_FREQ 0.4f
@ -75,60 +112,150 @@ static int32_t currentMeterADCToCentiamps(const uint16_t src)
return (millivolts * 1000) / (int32_t)config->scale; // current in 0.01A steps
}
void updateCurrentDrawn(currentMeter_t *state, int32_t lastUpdateAt)
static void updateCurrentmAhDrawnState(currentMeterMAhDrawnState_t *state, int32_t amperageLatest, int32_t lastUpdateAt)
{
state->mAhDrawnF = state->mAhDrawnF + (state->amperageLatest * lastUpdateAt / (100.0f * 1000 * 3600));
state->mAhDrawnF = state->mAhDrawnF + (amperageLatest * lastUpdateAt / (100.0f * 1000 * 3600));
state->mAhDrawn = state->mAhDrawnF;
}
void currentUpdateADCMeter(currentMeter_t *state, int32_t lastUpdateAt)
{
uint16_t iBatSample = adcGetChannel(ADC_CURRENT);
state->amperageLatest = currentMeterADCToCentiamps(iBatSample);
state->amperage = currentMeterADCToCentiamps(biquadFilterApply(&adciBatFilter, iBatSample));
//
// ADC
//
updateCurrentDrawn(state, lastUpdateAt);
currentMeterADCState_t currentMeterADCState;
void currentMeterADCInit(void)
{
memset(&currentMeterADCState, 0, sizeof(currentMeterADCState_t));
biquadFilterInitLPF(&adciBatFilter, IBAT_LPF_FREQ, 50000); //50HZ Update
}
void currentUpdateVirtualMeter(currentMeter_t *state, int32_t lastUpdateAt, bool armed, bool throttleLowAndMotorStop, int32_t throttleOffset)
void currentMeterADCRefresh(int32_t lastUpdateAt)
{
state->amperage = (int32_t)currentMeterADCOrVirtualConfig(CURRENT_SENSOR_VIRTUAL)->offset;
uint16_t iBatSample = adcGetChannel(ADC_CURRENT);
currentMeterADCState.amperageLatest = currentMeterADCToCentiamps(iBatSample);
currentMeterADCState.amperage = currentMeterADCToCentiamps(biquadFilterApply(&adciBatFilter, iBatSample));
updateCurrentmAhDrawnState(&currentMeterADCState.mahDrawnState, currentMeterADCState.amperageLatest, lastUpdateAt);
}
void currentMeterADCRead(currentMeter_t *meter)
{
meter->amperageLatest = currentMeterADCState.amperageLatest;
meter->amperage = currentMeterADCState.amperage;
meter->mAhDrawn = currentMeterADCState.mahDrawnState.mAhDrawn;
}
//
// VIRTUAL
//
currentMeterVirtualState_t currentMeterVirtualState;
void currentMeterVirtualInit(void)
{
memset(&currentMeterVirtualState, 0, sizeof(currentMeterVirtualState_t));
}
void currentMeterVirtualRefresh(int32_t lastUpdateAt, bool armed, bool throttleLowAndMotorStop, int32_t throttleOffset)
{
currentMeterVirtualState.amperage = (int32_t)currentMeterADCOrVirtualConfig(CURRENT_SENSOR_VIRTUAL)->offset;
if (armed) {
if (throttleLowAndMotorStop) {
throttleOffset = 0;
}
int throttleFactor = throttleOffset + (throttleOffset * throttleOffset / 50); // FIXME magic number 50, 50hz?
state->amperageLatest = state->amperage += throttleFactor * (int32_t)currentMeterADCOrVirtualConfig(CURRENT_SENSOR_VIRTUAL)->scale / 1000;
currentMeterVirtualState.amperage += throttleFactor * (int32_t)currentMeterADCOrVirtualConfig(CURRENT_SENSOR_VIRTUAL)->scale / 1000;
}
updateCurrentDrawn(state, lastUpdateAt);
updateCurrentmAhDrawnState(&currentMeterVirtualState.mahDrawnState, currentMeterVirtualState.amperage, lastUpdateAt);
}
void currentUpdateESCMeter(currentMeter_t *state, int32_t lastUpdateAt)
void currentMeterVirtualRead(currentMeter_t *meter)
{
UNUSED(lastUpdateAt);
#ifndef USE_ESC_SENSOR
UNUSED(state);
#else
escSensorData_t *escData = getEscSensorData(ESC_SENSOR_COMBINED);
if (escData->dataAge <= ESC_BATTERY_AGE_MAX) {
state->amperageLatest = escData->current;
state->mAhDrawn = escData->consumption;
} else {
state->amperageLatest = 0;
state->mAhDrawn = 0;
}
state->amperage = state->amperageLatest;
meter->amperageLatest = currentMeterVirtualState.amperage;
meter->amperage = currentMeterVirtualState.amperage;
meter->mAhDrawn = currentMeterVirtualState.mahDrawnState.mAhDrawn;
}
//
// ESC
//
#ifdef USE_ESC_SENSOR
currentMeterESCState_t currentMeterESCState;
#endif
void currentMeterESCInit(void)
{
#ifdef USE_ESC_SENSOR
memset(&currentMeterESCState, 0, sizeof(currentMeterESCState_t));
#endif
}
void resetCurrentMeterState(currentMeter_t *state)
void currentMeterESCRefresh(int32_t lastUpdateAt)
{
state->amperage = 0;
state->amperageLatest = 0;
UNUSED(lastUpdateAt);
#ifdef USE_ESC_SENSOR
escSensorData_t *escData = getEscSensorData(ESC_SENSOR_COMBINED);
if (escData->dataAge <= ESC_BATTERY_AGE_MAX) {
currentMeterESCState.amperage = escData->current;
currentMeterESCState.mAhDrawn = escData->consumption;
} else {
currentMeterESCState.amperage = 0;
currentMeterESCState.mAhDrawn = 0;
}
#endif
}
void currentMeterADCInit(void)
void currentMeterESCReadCombined(currentMeter_t *meter)
{
biquadFilterInitLPF(&adciBatFilter, IBAT_LPF_FREQ, 50000); //50HZ Update
#ifdef USE_ESC_SENSOR
meter->amperageLatest = currentMeterESCState.amperage;
meter->amperage = currentMeterESCState.amperage;
meter->mAhDrawn = currentMeterESCState.mAhDrawn;
#else
currentMeterReset(meter);
#endif
}
void currentMeterESCReadMotor(uint8_t motorNumber, currentMeter_t *meter)
{
#ifndef USE_ESC_SENSOR
UNUSED(motorNumber);
currentMeterReset(meter);
#else
escSensorData_t *escData = getEscSensorData(motorNumber);
if (escData->dataAge <= ESC_BATTERY_AGE_MAX) {
meter->amperage = escData->current;
meter->amperageLatest = escData->current;
meter->mAhDrawn = escData->consumption;
return;
}
#endif
}
//
// API for current meters using IDs
//
void currentMeterRead(currentMeterId_e id, currentMeter_t *meter)
{
if (id == CURRENT_METER_ID_VBAT_1) {
currentMeterADCRead(meter);
} else if (id == CURRENT_METER_ID_VIRTUAL_1) {
currentMeterVirtualRead(meter);
}
#ifdef USE_ESC_SENSOR
if (id == CURRENT_METER_ID_ESC_COMBINED_1) {
currentMeterESCReadCombined(meter);
} else
if (id >= CURRENT_METER_ID_ESC_MOTOR_1 && id <= CURRENT_METER_ID_ESC_MOTOR_20 ) {
int motor = id - CURRENT_METER_ID_ESC_MOTOR_1;
currentMeterESCReadMotor(motor, meter);
} else
#endif
{
currentMeterReset(meter);
}
}

92
src/main/sensors/current.h
View File

@ -17,6 +17,7 @@
#pragma once
#include "current_ids.h"
typedef enum {
CURRENT_METER_NONE = 0,
@ -26,54 +27,10 @@ typedef enum {
CURRENT_METER_MAX = CURRENT_METER_ESC
} currentMeterSource_e;
//
// fixed ids, current can be measured at many different places, these identifiers are the ones we support or would consider supporting.
//
typedef enum {
CURRENT_METER_ID_NONE = 0,
CURRENT_METER_ID_VBAT_1 = 10, // 10-19 for battery meters
CURRENT_METER_ID_VBAT_2,
//..
CURRENT_METER_ID_VBAT_10 = 19,
CURRENT_METER_ID_5V_1 = 20, // 20-29 for 5V meters
CURRENT_METER_ID_5V_2,
//..
CURRENT_METER_ID_5V_10 = 29,
CURRENT_METER_ID_9V_1 = 30, // 30-39 for 9V meters
CURRENT_METER_ID_9V_2,
//..
CURRENT_METER_ID_9V_10 = 39,
CURRENT_METER_ID_12V_1 = 40, // 40-49 for 12V meters
CURRENT_METER_ID_12V_2,
//..
CURRENT_METER_ID_12V_10 = 49,
CURRENT_METER_ID_ESC_COMBINED_1 = 50, // 50-59 for ESC combined (it's doubtful an FC would ever expose 51-59 however)
// ...
CURRENT_METER_ID_ESC_COMBINED_10 = 59,
CURRENT_METER_ID_ESC_MOTOR_1 = 60, // 60-79 for ESC motors (20 motors)
CURRENT_METER_ID_ESC_MOTOR_2,
CURRENT_METER_ID_ESC_MOTOR_3,
CURRENT_METER_ID_ESC_MOTOR_4,
CURRENT_METER_ID_ESC_MOTOR_5,
CURRENT_METER_ID_ESC_MOTOR_6,
CURRENT_METER_ID_ESC_MOTOR_7,
CURRENT_METER_ID_ESC_MOTOR_8,
//...
CURRENT_METER_ID_ESC_MOTOR_20 = 79,
} currentMeterId_e;
typedef struct currentMeter_s {
int32_t amperage; // current read by current sensor in centiampere (1/100th A)
int32_t amperageLatest; // current read by current sensor in centiampere (1/100th A) (unfiltered)
int32_t mAhDrawn; // milliampere hours drawn from the battery since start
float mAhDrawnF;
} currentMeter_t;
// NOTE: currentMeterConfig is only used by physical and virtual current meters, not ESC based current meters.
@ -86,6 +43,27 @@ typedef enum {
// WARNING - do not mix usage of CURRENT_SENSOR_* and CURRENT_METER_*, they are separate concerns.
typedef struct currentMeterMAhDrawnState_s {
int32_t mAhDrawn; // milliampere hours drawn from the battery since start
float mAhDrawnF;
} currentMeterMAhDrawnState_t;
typedef struct currentMeterADCState_s {
currentMeterMAhDrawnState_t mahDrawnState;
int32_t amperage; // current read by current sensor in centiampere (1/100th A)
int32_t amperageLatest; // current read by current sensor in centiampere (1/100th A) (unfiltered)
} currentMeterADCState_t;
typedef struct currentMeterVirtualState_s {
currentMeterMAhDrawnState_t mahDrawnState;
int32_t amperage; // current read by current sensor in centiampere (1/100th A)
} currentMeterVirtualState_t;
typedef struct currentMeterESCState_s {
int32_t mAhDrawn; // milliampere hours drawn from the battery since start
int32_t amperage; // current read by current sensor in centiampere (1/100th A)
} currentMeterESCState_t;
typedef struct currentMeterADCOrVirtualConfig_s {
int16_t scale; // scale the current sensor output voltage to milliamps. Value in 1/10th mV/A
uint16_t offset; // offset of the current sensor in millivolt steps
@ -93,10 +71,28 @@ typedef struct currentMeterADCOrVirtualConfig_s {
PG_DECLARE_ARRAY(currentMeterADCOrVirtualConfig_t, MAX_ADC_OR_VIRTUAL_CURRENT_METERS, currentMeterADCOrVirtualConfig);
//
// Main API
//
void currentMeterReset(currentMeter_t *meter);
void currentMeterADCInit(void);
void currentMeterADCRefresh(int32_t lastUpdateAt);
void currentMeterADCRead(currentMeter_t *meter);
void currentUpdateADCMeter(currentMeter_t *state, int32_t lastUpdateAt);
void currentUpdateESCMeter(currentMeter_t *state, int32_t lastUpdateAt);
void currentUpdateVirtualMeter(currentMeter_t *state, int32_t lastUpdateAt, bool armed, bool throttleLowAndMotorStop, int32_t throttleOffset);
void currentMeterVirtualInit(void);
void currentMeterVirtualRefresh(int32_t lastUpdateAt, bool armed, bool throttleLowAndMotorStop, int32_t throttleOffset);
void currentMeterVirtualRead(currentMeter_t *meter);
void resetCurrentMeterState(currentMeter_t *state);
void currentMeterESCInit(void);
void currentMeterESCRefresh(int32_t lastUpdateAt);
void currentMeterESCReadCombined(currentMeter_t *meter);
void currentMeterESCReadMotor(uint8_t motorNumber, currentMeter_t *meter);
//
// API for reading current meters by id.
//
extern const uint8_t supportedCurrentMeterCount;
extern const uint8_t currentMeterIds[];
void currentMeterRead(currentMeterId_e id, currentMeter_t *currentMeter);

69
src/main/sensors/current_ids.h Normal file
View File

@ -0,0 +1,69 @@
/*
* 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
//
// fixed ids, current can be measured at many different places, these identifiers are the ones we support or would consider supporting.
//
typedef enum {
CURRENT_METER_ID_NONE = 0,
CURRENT_METER_ID_VBAT_1 = 10, // 10-19 for battery meters
CURRENT_METER_ID_VBAT_2,
//..
CURRENT_METER_ID_VBAT_10 = 19,
CURRENT_METER_ID_5V_1 = 20, // 20-29 for 5V meters
CURRENT_METER_ID_5V_2,
//..
CURRENT_METER_ID_5V_10 = 29,
CURRENT_METER_ID_9V_1 = 30, // 30-39 for 9V meters
CURRENT_METER_ID_9V_2,
//..
CURRENT_METER_ID_9V_10 = 39,
CURRENT_METER_ID_12V_1 = 40, // 40-49 for 12V meters
CURRENT_METER_ID_12V_2,
//..
CURRENT_METER_ID_12V_10 = 49,
CURRENT_METER_ID_ESC_COMBINED_1 = 50, // 50-59 for ESC combined (it's doubtful an FC would ever expose 51-59 however)
// ...
CURRENT_METER_ID_ESC_COMBINED_10 = 59,
CURRENT_METER_ID_ESC_MOTOR_1 = 60, // 60-79 for ESC motors (20 motors)
CURRENT_METER_ID_ESC_MOTOR_2,
CURRENT_METER_ID_ESC_MOTOR_3,
CURRENT_METER_ID_ESC_MOTOR_4,
CURRENT_METER_ID_ESC_MOTOR_5,
CURRENT_METER_ID_ESC_MOTOR_6,
CURRENT_METER_ID_ESC_MOTOR_7,
CURRENT_METER_ID_ESC_MOTOR_8,
CURRENT_METER_ID_ESC_MOTOR_9,
CURRENT_METER_ID_ESC_MOTOR_10,
CURRENT_METER_ID_ESC_MOTOR_11,
CURRENT_METER_ID_ESC_MOTOR_12,
//...
CURRENT_METER_ID_ESC_MOTOR_20 = 79,
CURRENT_METER_ID_VIRTUAL_1 = 80, // 80-89 for virtual meters
CURRENT_METER_ID_VIRTUAL_2,
} currentMeterId_e;

92
src/main/sensors/voltage.c
View File

@ -37,6 +37,49 @@
#include "sensors/voltage.h"
#include "sensors/esc_sensor.h"
const uint8_t voltageMeterIds[] = {
VOLTAGE_METER_ID_VBAT_1,
#ifdef ADC_POWER_12V
VOLTAGE_METER_ID_12V_1,
#endif
#ifdef ADC_POWER_9V
VOLTAGE_METER_ID_9V_1,
#endif
#ifdef ADC_POWER_5V
VOLTAGE_METER_ID_5V_1,
#endif
#ifdef USE_ESC_SENSOR
VOLTAGE_METER_ID_ESC_COMBINED_1,
VOLTAGE_METER_ID_ESC_MOTOR_1,
VOLTAGE_METER_ID_ESC_MOTOR_2,
VOLTAGE_METER_ID_ESC_MOTOR_3,
VOLTAGE_METER_ID_ESC_MOTOR_4,
VOLTAGE_METER_ID_ESC_MOTOR_5,
VOLTAGE_METER_ID_ESC_MOTOR_6,
VOLTAGE_METER_ID_ESC_MOTOR_7,
VOLTAGE_METER_ID_ESC_MOTOR_8,
VOLTAGE_METER_ID_ESC_MOTOR_9,
VOLTAGE_METER_ID_ESC_MOTOR_10,
VOLTAGE_METER_ID_ESC_MOTOR_11,
VOLTAGE_METER_ID_ESC_MOTOR_12,
#endif
};
const uint8_t supportedVoltageMeterCount = ARRAYLEN(voltageMeterIds);
//
// ADC/ESC shared
//
void voltageMeterReset(voltageMeter_t *meter)
{
meter->filtered = 0;
meter->unfiltered = 0;
}
//
// ADC
//
#ifndef VBAT_SCALE_DEFAULT
#define VBAT_SCALE_DEFAULT 110
#endif
@ -49,16 +92,6 @@
#define VBAT_RESDIVMULTIPLIER_DEFAULT 1
#endif
#ifdef USE_ESC_SENSOR
typedef struct voltageMeterESCState_s {
uint16_t voltageFiltered; // battery voltage in 0.1V steps (filtered)
uint16_t voltageUnfiltered; // battery voltage in 0.1V steps (unfiltered)
biquadFilter_t filter;
} voltageMeterESCState_t;
static voltageMeterESCState_t voltageMeterESCState;
#endif
typedef struct voltageMeterADCState_s {
uint16_t voltageFiltered; // battery voltage in 0.1V steps (filtered)
uint16_t voltageUnfiltered; // battery voltage in 0.1V steps (unfiltered)
@ -67,7 +100,6 @@ typedef struct voltageMeterADCState_s {
extern voltageMeterADCState_t voltageMeterADCStates[MAX_VOLTAGE_SENSOR_ADC];
voltageMeterADCState_t voltageMeterADCStates[MAX_VOLTAGE_SENSOR_ADC];
voltageMeterADCState_t *getVoltageMeterADC(uint8_t index)
@ -148,11 +180,19 @@ void voltageMeterADCInit(void)
}
}
void voltageMeterReset(voltageMeter_t *meter)
{
meter->filtered = 0;
meter->unfiltered = 0;
}
//
// ESC
//
#ifdef USE_ESC_SENSOR
typedef struct voltageMeterESCState_s {
uint16_t voltageFiltered; // battery voltage in 0.1V steps (filtered)
uint16_t voltageUnfiltered; // battery voltage in 0.1V steps (unfiltered)
biquadFilter_t filter;
} voltageMeterESCState_t;
static voltageMeterESCState_t voltageMeterESCState;
#endif
#define VBAT_LPF_FREQ 0.4f
@ -196,36 +236,40 @@ void voltageMeterESCReadCombined(voltageMeter_t *voltageMeter)
#endif
}
void voltageReadMeter(voltageMeterId_e id, voltageMeter_t *voltageMeter)
//
// API for voltage meters using IDs
//
void voltageMeterRead(voltageMeterId_e id, voltageMeter_t *meter)
{
if (id == VOLTAGE_METER_ID_VBAT_1) {
voltageMeterADCRead(VOLTAGE_SENSOR_ADC_VBAT, voltageMeter);
voltageMeterADCRead(VOLTAGE_SENSOR_ADC_VBAT, meter);
} else
#ifdef ADC_POWER_12V
if (id == VOLTAGE_METER_ID_12V_1) {
voltageMeterADCRead(VOLTAGE_SENSOR_ADC_12V, voltageMeter);
voltageMeterADCRead(VOLTAGE_SENSOR_ADC_12V, meter);
} else
#endif
#ifdef ADC_POWER_9V
if (id == VOLTAGE_METER_ID_9V_1) {
voltageMeterADCRead(VOLTAGE_SENSOR_ADC_9V, voltageMeter);
voltageMeterADCRead(VOLTAGE_SENSOR_ADC_9V, meter);
} else
#endif
#ifdef ADC_POWER_5V
if (id == VOLTAGE_METER_ID_5V_1) {
voltageMeterADCRead(VOLTAGE_SENSOR_ADC_5V, voltageMeter);
voltageMeterADCRead(VOLTAGE_SENSOR_ADC_5V, meter);
} else
#endif
#ifdef USE_ESC_SENSOR
if (id == VOLTAGE_METER_ID_ESC_COMBINED_1) {
voltageMeterESCReadCombined(voltageMeter);
voltageMeterESCReadCombined(meter);
} else
if (id >= VOLTAGE_METER_ID_ESC_MOTOR_1 && id <= VOLTAGE_METER_ID_ESC_MOTOR_20 ) {
int motor = id - VOLTAGE_METER_ID_ESC_MOTOR_1;
voltageMeterESCReadMotor(motor, voltageMeter);
voltageMeterESCReadMotor(motor, meter);
} else
#endif
{
voltageMeterReset(voltageMeter);
voltageMeterReset(meter);
}
}

65
src/main/sensors/voltage.h
View File

@ -17,6 +17,8 @@
#pragma once
#include "voltage_ids.h"
//
// meters
//
@ -32,56 +34,6 @@ typedef struct voltageMeter_s {
uint16_t unfiltered; // voltage in 0.1V steps
} voltageMeter_t;
//
// fixed ids, voltage can be measured at many different places, these identifiers are the ones we support or would consider supporting.
//
typedef enum {
VOLTAGE_METER_ID_NONE = 0,
VOLTAGE_METER_ID_VBAT_1 = 10, // 10-19 for battery meters
VOLTAGE_METER_ID_VBAT_2,
//..
VOLTAGE_METER_ID_VBAT_10 = 19,
VOLTAGE_METER_ID_5V_1 = 20, // 20-29 for 5V meters
VOLTAGE_METER_ID_5V_2,
//..
VOLTAGE_METER_ID_5V_10 = 29,
VOLTAGE_METER_ID_9V_1 = 30, // 30-39 for 9V meters
VOLTAGE_METER_ID_9V_2,
//..
VOLTAGE_METER_ID_9V_10 = 39,
VOLTAGE_METER_ID_12V_1 = 40, // 40-49 for 12V meters
VOLTAGE_METER_ID_12V_2,
//..
VOLTAGE_METER_ID_12V_10 = 49,
VOLTAGE_METER_ID_ESC_COMBINED_1 = 50, // 50-59 for ESC combined (it's doubtful an FC would ever expose 51-59 however)
// ...
VOLTAGE_METER_ID_ESC_COMBINED_10 = 59,
VOLTAGE_METER_ID_ESC_MOTOR_1 = 60, // 60-79 for ESC motors (20 motors)
VOLTAGE_METER_ID_ESC_MOTOR_2,
VOLTAGE_METER_ID_ESC_MOTOR_3,
VOLTAGE_METER_ID_ESC_MOTOR_4,
VOLTAGE_METER_ID_ESC_MOTOR_5,
VOLTAGE_METER_ID_ESC_MOTOR_6,
VOLTAGE_METER_ID_ESC_MOTOR_7,
VOLTAGE_METER_ID_ESC_MOTOR_8,
//...
VOLTAGE_METER_ID_ESC_MOTOR_20 = 79,
VOLTAGE_METER_ID_CELL_1 = 80, // 80-119 for cell meters (40 cells)
VOLTAGE_METER_ID_CELL_2,
//...
VOLTAGE_METER_ID_CELL_40 = 119,
} voltageMeterId_e;
//
// sensors
//
@ -121,6 +73,11 @@ typedef struct voltageSensorADCConfig_s {
PG_DECLARE_ARRAY(voltageSensorADCConfig_t, MAX_VOLTAGE_SENSOR_ADC, voltageSensorADCConfig);
//
// Main API
//
void voltageMeterReset(voltageMeter_t *voltageMeter);
void voltageMeterADCInit(void);
void voltageMeterADCRefresh(void);
void voltageMeterADCRead(voltageSensorADC_e adcChannel, voltageMeter_t *voltageMeter);
@ -130,6 +87,10 @@ void voltageMeterESCRefresh(void);
void voltageMeterESCReadCombined(voltageMeter_t *voltageMeter);
void voltageMeterESCReadMotor(uint8_t motor, voltageMeter_t *voltageMeter);
void voltageMeterReset(voltageMeter_t *voltageMeter);
void voltageReadMeter(voltageMeterId_e id, voltageMeter_t *voltageMeter);
//
// API for reading current meters by id.
//
extern const uint8_t supportedVoltageMeterCount;
extern const uint8_t voltageMeterIds[];
void voltageMeterRead(voltageMeterId_e id, voltageMeter_t *voltageMeter);

71
src/main/sensors/voltage_ids.h Normal file
View File

@ -0,0 +1,71 @@
/*
* 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
//
// fixed ids, voltage can be measured at many different places, these identifiers are the ones we support or would consider supporting.
//
typedef enum {
VOLTAGE_METER_ID_NONE = 0,
VOLTAGE_METER_ID_VBAT_1 = 10, // 10-19 for battery meters
VOLTAGE_METER_ID_VBAT_2,
//..
VOLTAGE_METER_ID_VBAT_10 = 19,
VOLTAGE_METER_ID_5V_1 = 20, // 20-29 for 5V meters
VOLTAGE_METER_ID_5V_2,
//..
VOLTAGE_METER_ID_5V_10 = 29,
VOLTAGE_METER_ID_9V_1 = 30, // 30-39 for 9V meters
VOLTAGE_METER_ID_9V_2,
//..
VOLTAGE_METER_ID_9V_10 = 39,
VOLTAGE_METER_ID_12V_1 = 40, // 40-49 for 12V meters
VOLTAGE_METER_ID_12V_2,
//..
VOLTAGE_METER_ID_12V_10 = 49,
VOLTAGE_METER_ID_ESC_COMBINED_1 = 50, // 50-59 for ESC combined (it's doubtful an FC would ever expose 51-59 however)
// ...
VOLTAGE_METER_ID_ESC_COMBINED_10 = 59,
VOLTAGE_METER_ID_ESC_MOTOR_1 = 60, // 60-79 for ESC motors (20 motors)
VOLTAGE_METER_ID_ESC_MOTOR_2,
VOLTAGE_METER_ID_ESC_MOTOR_3,
VOLTAGE_METER_ID_ESC_MOTOR_4,
VOLTAGE_METER_ID_ESC_MOTOR_5,
VOLTAGE_METER_ID_ESC_MOTOR_6,
VOLTAGE_METER_ID_ESC_MOTOR_7,
VOLTAGE_METER_ID_ESC_MOTOR_8,
VOLTAGE_METER_ID_ESC_MOTOR_9,
VOLTAGE_METER_ID_ESC_MOTOR_10,
VOLTAGE_METER_ID_ESC_MOTOR_11,
VOLTAGE_METER_ID_ESC_MOTOR_12,
//...
VOLTAGE_METER_ID_ESC_MOTOR_20 = 79,
VOLTAGE_METER_ID_CELL_1 = 80, // 80-119 for cell meters (40 cells)
VOLTAGE_METER_ID_CELL_2,
//...
VOLTAGE_METER_ID_CELL_40 = 119,
} voltageMeterId_e;