mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-24 00:35:39 +03:00
134 lines
4.2 KiB
C
134 lines
4.2 KiB
C
/*
|
|
* 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 "string.h"
|
|
|
|
#include <platform.h>
|
|
#include "build/build_config.h"
|
|
|
|
#include "common/maths.h"
|
|
#include "common/utils.h"
|
|
#include "common/filter.h"
|
|
|
|
#include "drivers/adc.h"
|
|
#include "drivers/system.h"
|
|
|
|
#include "config/parameter_group.h"
|
|
#include "config/parameter_group_ids.h"
|
|
#include "config/config_reset.h"
|
|
|
|
#include "sensors/current.h"
|
|
#include "sensors/esc_sensor.h"
|
|
|
|
#define ADCVREF 3300 // in mV
|
|
|
|
#define IBAT_LPF_FREQ 0.4f
|
|
static biquadFilter_t adciBatFilter;
|
|
|
|
#ifndef CURRENT_METER_SCALE_DEFAULT
|
|
#define CURRENT_METER_SCALE_DEFAULT 400 // for Allegro ACS758LCB-100U (40mV/A)
|
|
#endif
|
|
|
|
#ifndef CURRENT_METER_OFFSET_DEFAULT
|
|
#define CURRENT_METER_OFFSET_DEFAULT 0
|
|
#endif
|
|
|
|
PG_REGISTER_ARRAY_WITH_RESET_FN(currentMeterADCOrVirtualConfig_t, MAX_ADC_OR_VIRTUAL_CURRENT_METERS, currentMeterADCOrVirtualConfig, PG_CURRENT_SENSOR_ADC_OR_VIRTUAL_CONFIG, 0);
|
|
|
|
void pgResetFn_currentMeterADCOrVirtualConfig(currentMeterADCOrVirtualConfig_t *instance)
|
|
{
|
|
for (int i = 0; i < MAX_ADC_OR_VIRTUAL_CURRENT_METERS; i++) {
|
|
if (i == CURRENT_METER_ADC) {
|
|
RESET_CONFIG(currentMeterADCOrVirtualConfig_t, &instance[i],
|
|
.scale = CURRENT_METER_SCALE_DEFAULT,
|
|
.offset = CURRENT_METER_OFFSET_DEFAULT,
|
|
);
|
|
}
|
|
}
|
|
}
|
|
|
|
static int32_t currentMeterADCToCentiamps(const uint16_t src)
|
|
{
|
|
int32_t millivolts;
|
|
|
|
const currentMeterADCOrVirtualConfig_t *config = currentMeterADCOrVirtualConfig(CURRENT_SENSOR_ADC);
|
|
|
|
millivolts = ((uint32_t)src * ADCVREF) / 4096;
|
|
millivolts -= config->offset;
|
|
|
|
return (millivolts * 1000) / (int32_t)config->scale; // current in 0.01A steps
|
|
}
|
|
|
|
void updateCurrentDrawn(currentMeter_t *state, int32_t lastUpdateAt)
|
|
{
|
|
state->mAhDrawnF = state->mAhDrawnF + (state->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));
|
|
|
|
updateCurrentDrawn(state, lastUpdateAt);
|
|
}
|
|
|
|
void currentUpdateVirtualMeter(currentMeter_t *state, int32_t lastUpdateAt, bool armed, bool throttleLowAndMotorStop, int32_t throttleOffset)
|
|
{
|
|
state->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;
|
|
}
|
|
updateCurrentDrawn(state, lastUpdateAt);
|
|
}
|
|
|
|
void currentUpdateESCMeter(currentMeter_t *state, int32_t lastUpdateAt)
|
|
{
|
|
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;
|
|
#endif
|
|
}
|
|
|
|
void resetCurrentMeterState(currentMeter_t *state)
|
|
{
|
|
state->amperage = 0;
|
|
state->amperageLatest = 0;
|
|
}
|
|
|
|
void currentMeterADCInit(void)
|
|
{
|
|
biquadFilterInitLPF(&adciBatFilter, IBAT_LPF_FREQ, 50000); //50HZ Update
|
|
}
|