diff --git a/docs/Battery.md b/docs/Battery.md new file mode 100644 index 0000000000..93812b3b56 --- /dev/null +++ b/docs/Battery.md @@ -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 +``` diff --git a/src/main/drivers/adc.c b/src/main/drivers/adc.c index be607d0039..57f8a03480 100755 --- a/src/main/drivers/adc.c +++ b/src/main/drivers/adc.c @@ -32,7 +32,7 @@ extern int16_t debug[4]; uint16_t adcGetChannel(uint8_t channel) { -#if 0 +#if DEBUG_ADC_CHANNELS if (adcConfig[0].enabled) { debug[0] = adcValues[adcConfig[0].dmaIndex]; } diff --git a/src/main/drivers/adc_stm32f10x.c b/src/main/drivers/adc_stm32f10x.c index 9242c1c73f..2bb617d4e0 100644 --- a/src/main/drivers/adc_stm32f10x.c +++ b/src/main/drivers/adc_stm32f10x.c @@ -33,22 +33,24 @@ // Driver for STM32F103CB onboard ADC // +// Naze32 // Battery Voltage (VBAT) is connected to PA4 (ADC1_IN4) with 10k:1k divider // RSSI ADC uses CH2 (PA1, ADC1_IN1) // Current ADC uses CH8 (PB1, ADC1_IN9) // // 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 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) { +#ifdef CC3D + UNUSED(init); +#endif + ADC_InitTypeDef adc; DMA_InitTypeDef dma; GPIO_InitTypeDef GPIO_InitStructure; @@ -59,10 +61,19 @@ void adcInit(drv_adc_config_t *init) memset(&adcConfig, 0, sizeof(adcConfig)); GPIO_StructInit(&GPIO_InitStructure); - GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4; 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) + GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4; adcConfig[ADC_BATTERY].adcChannel = ADC_Channel_4; adcConfig[ADC_BATTERY].dmaIndex = configuredAdcChannels++; adcConfig[ADC_BATTERY].enabled = true; @@ -109,6 +120,7 @@ void adcInit(drv_adc_config_t *init) adcConfig[ADC_CURRENT].enabled = true; adcConfig[ADC_CURRENT].sampleTime = ADC_SampleTime_239Cycles5; } +#endif // !CC3D RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE); RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1, ENABLE); @@ -156,4 +168,3 @@ void adcInit(drv_adc_config_t *init) ADC_SoftwareStartConvCmd(ADC1, ENABLE); } -#endif diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index ac4c6015ee..044b8547e1 100755 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -326,6 +326,11 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init) continue; #endif +#ifdef CC3D + if (init->useVbat && timerIndex == PWM5) { + continue; + } +#endif // hacks to allow current functionality if (type == MAP_TO_PWM_INPUT && !init->useParallelPWM) type = 0; diff --git a/src/main/drivers/pwm_mapping.h b/src/main/drivers/pwm_mapping.h index e6766ed489..a504b9f7cc 100755 --- a/src/main/drivers/pwm_mapping.h +++ b/src/main/drivers/pwm_mapping.h @@ -42,6 +42,7 @@ typedef struct drv_pwm_config_t { #ifdef STM32F10X bool useUART2; #endif + bool useVbat; bool useSoftSerial; bool useLEDStrip; bool useServos; diff --git a/src/main/main.c b/src/main/main.c index 457dd9e5f2..95a68a70d1 100755 --- a/src/main/main.c +++ b/src/main/main.c @@ -197,6 +197,7 @@ void init(void) } #endif + pwm_params.useVbat = feature(FEATURE_VBAT); pwm_params.useSoftSerial = feature(FEATURE_SOFTSERIAL); pwm_params.useParallelPWM = feature(FEATURE_RX_PARALLEL_PWM); pwm_params.useRSSIADC = feature(FEATURE_RSSI_ADC);