mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-16 12:55:19 +03:00
CC3D - Add support for battery voltage monitoring on S5_IN.
See docs for further details.
This commit is contained in:
parent
f591732aa9
commit
41ff2e8d0e
6 changed files with 83 additions and 8 deletions
57
docs/Battery.md
Normal file
57
docs/Battery.md
Normal file
|
@ -0,0 +1,57 @@
|
||||||
|
# Battery Monitoring
|
||||||
|
|
||||||
|
Cleanflight has battery monitoring capability. Battery voltage of the main battery can be measured by the system and used
|
||||||
|
to trigger a low-battery warning buzzer, on-board status LED flashing and LED strip patterns.
|
||||||
|
|
||||||
|
Low battery warnings can:
|
||||||
|
|
||||||
|
* help to ensure that you have time to safely land the aircraft.
|
||||||
|
* help maintain the life and safety of your LiPo/LiFe batteries which should not be discharged below manufactures recommendations.
|
||||||
|
|
||||||
|
Minimum and maximum cell voltages can be set, these voltages are used to detect the amount of cells you are using.
|
||||||
|
|
||||||
|
Per-cell monitoring is not supported.
|
||||||
|
|
||||||
|
## Supported targets
|
||||||
|
|
||||||
|
All targets support battery voltage monitoring unless status.
|
||||||
|
|
||||||
|
## Connections
|
||||||
|
|
||||||
|
When dealing with batteries ALWAYS CHECK POLARITY!
|
||||||
|
|
||||||
|
Measure expected voltages first and then connect to flight controller, connecting to the flight controller with
|
||||||
|
incorrect voltage or reversed polarity will likely fry your flight controller.
|
||||||
|
|
||||||
|
### Naze32
|
||||||
|
|
||||||
|
The Naze32 has an on-board battery divider circuit, connect your main battery to the VBAT connector.
|
||||||
|
|
||||||
|
### CC3D
|
||||||
|
|
||||||
|
The CC3D has no battery divider, create one that gives you a 3.3v MAXIMUM output when your battery is
|
||||||
|
fully charged and connect the output from it to S5_IN/PA0/RC5.
|
||||||
|
|
||||||
|
S5_IN/PA0/RC5 is Pin 7 on the 8 pin connector, second to last pin, opposite end from the GND/+5/PPM signal input.
|
||||||
|
|
||||||
|
Note: When battery monitoring is enabled on the CC3D RC5 can no-longer be used for PWM input.
|
||||||
|
|
||||||
|
## Configuration
|
||||||
|
|
||||||
|
Enable the `VBAT` feature.
|
||||||
|
|
||||||
|
Configure min/max cell voltages using the following CLI commands:
|
||||||
|
|
||||||
|
`vbat_scale` - adjust this to match battery voltage to reported value.
|
||||||
|
|
||||||
|
`vbat_max_cell_voltage` - maximum voltage per cell, used for auto-detecting battery voltage in 0.1V units, i.e. 43 = 4.3V
|
||||||
|
|
||||||
|
`vbat_min_cell_voltage` - minimum voltage per cell, this triggers battery out alarms, in 0.1V units, i.e. 33 = 3.3V
|
||||||
|
|
||||||
|
e.g.
|
||||||
|
|
||||||
|
```
|
||||||
|
set vbat_scale = 110
|
||||||
|
set vbat_max_cell_voltage = 43
|
||||||
|
set vbat_min_cell_voltage = 33
|
||||||
|
```
|
|
@ -32,7 +32,7 @@ extern int16_t debug[4];
|
||||||
|
|
||||||
uint16_t adcGetChannel(uint8_t channel)
|
uint16_t adcGetChannel(uint8_t channel)
|
||||||
{
|
{
|
||||||
#if 0
|
#if DEBUG_ADC_CHANNELS
|
||||||
if (adcConfig[0].enabled) {
|
if (adcConfig[0].enabled) {
|
||||||
debug[0] = adcValues[adcConfig[0].dmaIndex];
|
debug[0] = adcValues[adcConfig[0].dmaIndex];
|
||||||
}
|
}
|
||||||
|
|
|
@ -33,22 +33,24 @@
|
||||||
|
|
||||||
// Driver for STM32F103CB onboard ADC
|
// Driver for STM32F103CB onboard ADC
|
||||||
//
|
//
|
||||||
|
// Naze32
|
||||||
// Battery Voltage (VBAT) is connected to PA4 (ADC1_IN4) with 10k:1k divider
|
// Battery Voltage (VBAT) is connected to PA4 (ADC1_IN4) with 10k:1k divider
|
||||||
// RSSI ADC uses CH2 (PA1, ADC1_IN1)
|
// RSSI ADC uses CH2 (PA1, ADC1_IN1)
|
||||||
// Current ADC uses CH8 (PB1, ADC1_IN9)
|
// Current ADC uses CH8 (PB1, ADC1_IN9)
|
||||||
//
|
//
|
||||||
// NAZE rev.5 hardware has PA5 (ADC1_IN5) on breakout pad on bottom of board
|
// NAZE rev.5 hardware has PA5 (ADC1_IN5) on breakout pad on bottom of board
|
||||||
|
//
|
||||||
|
// CC3D Only one ADC channel supported currently, for battery on S5_IN/PA0
|
||||||
|
|
||||||
extern adc_config_t adcConfig[ADC_CHANNEL_COUNT];
|
extern adc_config_t adcConfig[ADC_CHANNEL_COUNT];
|
||||||
extern volatile uint16_t adcValues[ADC_CHANNEL_COUNT];
|
extern volatile uint16_t adcValues[ADC_CHANNEL_COUNT];
|
||||||
|
|
||||||
#ifdef CC3D
|
|
||||||
void adcInit(drv_adc_config_t *init) {
|
|
||||||
UNUSED(init);
|
|
||||||
}
|
|
||||||
#else
|
|
||||||
void adcInit(drv_adc_config_t *init)
|
void adcInit(drv_adc_config_t *init)
|
||||||
{
|
{
|
||||||
|
#ifdef CC3D
|
||||||
|
UNUSED(init);
|
||||||
|
#endif
|
||||||
|
|
||||||
ADC_InitTypeDef adc;
|
ADC_InitTypeDef adc;
|
||||||
DMA_InitTypeDef dma;
|
DMA_InitTypeDef dma;
|
||||||
GPIO_InitTypeDef GPIO_InitStructure;
|
GPIO_InitTypeDef GPIO_InitStructure;
|
||||||
|
@ -59,10 +61,19 @@ void adcInit(drv_adc_config_t *init)
|
||||||
memset(&adcConfig, 0, sizeof(adcConfig));
|
memset(&adcConfig, 0, sizeof(adcConfig));
|
||||||
|
|
||||||
GPIO_StructInit(&GPIO_InitStructure);
|
GPIO_StructInit(&GPIO_InitStructure);
|
||||||
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4;
|
|
||||||
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AIN;
|
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AIN;
|
||||||
|
|
||||||
|
#ifdef CC3D
|
||||||
|
UNUSED(init);
|
||||||
|
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0;
|
||||||
|
adcConfig[ADC_BATTERY].adcChannel = ADC_Channel_0;
|
||||||
|
adcConfig[ADC_BATTERY].dmaIndex = configuredAdcChannels++;
|
||||||
|
adcConfig[ADC_BATTERY].enabled = true;
|
||||||
|
adcConfig[ADC_BATTERY].sampleTime = ADC_SampleTime_239Cycles5;
|
||||||
|
GPIO_Init(GPIOA, &GPIO_InitStructure);
|
||||||
|
#else
|
||||||
// configure always-present battery index (ADC4)
|
// configure always-present battery index (ADC4)
|
||||||
|
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4;
|
||||||
adcConfig[ADC_BATTERY].adcChannel = ADC_Channel_4;
|
adcConfig[ADC_BATTERY].adcChannel = ADC_Channel_4;
|
||||||
adcConfig[ADC_BATTERY].dmaIndex = configuredAdcChannels++;
|
adcConfig[ADC_BATTERY].dmaIndex = configuredAdcChannels++;
|
||||||
adcConfig[ADC_BATTERY].enabled = true;
|
adcConfig[ADC_BATTERY].enabled = true;
|
||||||
|
@ -109,6 +120,7 @@ void adcInit(drv_adc_config_t *init)
|
||||||
adcConfig[ADC_CURRENT].enabled = true;
|
adcConfig[ADC_CURRENT].enabled = true;
|
||||||
adcConfig[ADC_CURRENT].sampleTime = ADC_SampleTime_239Cycles5;
|
adcConfig[ADC_CURRENT].sampleTime = ADC_SampleTime_239Cycles5;
|
||||||
}
|
}
|
||||||
|
#endif // !CC3D
|
||||||
|
|
||||||
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE);
|
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE);
|
||||||
RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1, ENABLE);
|
RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1, ENABLE);
|
||||||
|
@ -156,4 +168,3 @@ void adcInit(drv_adc_config_t *init)
|
||||||
|
|
||||||
ADC_SoftwareStartConvCmd(ADC1, ENABLE);
|
ADC_SoftwareStartConvCmd(ADC1, ENABLE);
|
||||||
}
|
}
|
||||||
#endif
|
|
||||||
|
|
|
@ -326,6 +326,11 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init)
|
||||||
continue;
|
continue;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
#ifdef CC3D
|
||||||
|
if (init->useVbat && timerIndex == PWM5) {
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
#endif
|
||||||
// hacks to allow current functionality
|
// hacks to allow current functionality
|
||||||
if (type == MAP_TO_PWM_INPUT && !init->useParallelPWM)
|
if (type == MAP_TO_PWM_INPUT && !init->useParallelPWM)
|
||||||
type = 0;
|
type = 0;
|
||||||
|
|
|
@ -42,6 +42,7 @@ typedef struct drv_pwm_config_t {
|
||||||
#ifdef STM32F10X
|
#ifdef STM32F10X
|
||||||
bool useUART2;
|
bool useUART2;
|
||||||
#endif
|
#endif
|
||||||
|
bool useVbat;
|
||||||
bool useSoftSerial;
|
bool useSoftSerial;
|
||||||
bool useLEDStrip;
|
bool useLEDStrip;
|
||||||
bool useServos;
|
bool useServos;
|
||||||
|
|
|
@ -197,6 +197,7 @@ void init(void)
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
pwm_params.useVbat = feature(FEATURE_VBAT);
|
||||||
pwm_params.useSoftSerial = feature(FEATURE_SOFTSERIAL);
|
pwm_params.useSoftSerial = feature(FEATURE_SOFTSERIAL);
|
||||||
pwm_params.useParallelPWM = feature(FEATURE_RX_PARALLEL_PWM);
|
pwm_params.useParallelPWM = feature(FEATURE_RX_PARALLEL_PWM);
|
||||||
pwm_params.useRSSIADC = feature(FEATURE_RSSI_ADC);
|
pwm_params.useRSSIADC = feature(FEATURE_RSSI_ADC);
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue