mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-03-15 07:29:15 +01:00
Merge branch 'revo_battery' into testing
This commit is contained in:
commit
1b53a9c4e6
@ -55,15 +55,6 @@
|
||||
//
|
||||
#define SAMPLE_PERIOD_MS 500
|
||||
|
||||
//#define ENABLE_DEBUG_MSG
|
||||
|
||||
#ifdef ENABLE_DEBUG_MSG
|
||||
#define DEBUG_PORT PIOS_COM_GPS
|
||||
#define DEBUG_MSG(format, ...) PIOS_COM_SendFormattedString(DEBUG_PORT, format, ## __VA_ARGS__)
|
||||
#else
|
||||
#define DEBUG_MSG(format, ...)
|
||||
#endif
|
||||
|
||||
// Private types
|
||||
|
||||
// Private variables
|
||||
@ -75,12 +66,10 @@ static void onTimer(UAVObjEvent* ev);
|
||||
* Initialise the module, called on startup
|
||||
* \returns 0 on success or -1 if initialisation failed
|
||||
*/
|
||||
MODULE_INITCALL(BatteryInitialize, 0)
|
||||
|
||||
int32_t BatteryInitialize(void)
|
||||
{
|
||||
BatteryStateInitialze();
|
||||
BatterySettingsInitialize();
|
||||
FlightBatteryStateInitialize();
|
||||
FlightBatterySettingsInitialize();
|
||||
|
||||
static UAVObjEvent ev;
|
||||
|
||||
@ -90,46 +79,22 @@ int32_t BatteryInitialize(void)
|
||||
return 0;
|
||||
}
|
||||
|
||||
MODULE_INITCALL(BatteryInitialize, 0)
|
||||
|
||||
static void onTimer(UAVObjEvent* ev)
|
||||
{
|
||||
static portTickType lastSysTime;
|
||||
static bool firstRun = true;
|
||||
|
||||
static FlightBatteryStateData flightBatteryData;
|
||||
|
||||
if (firstRun) {
|
||||
#ifdef ENABLE_DEBUG_MSG
|
||||
PIOS_COM_ChangeBaud(DEBUG_PORT, 57600);
|
||||
#endif
|
||||
lastSysTime = xTaskGetTickCount();
|
||||
//FlightBatteryStateGet(&flightBatteryData);
|
||||
|
||||
firstRun = false;
|
||||
}
|
||||
|
||||
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_ERROR);
|
||||
|
||||
|
||||
portTickType thisSysTime;
|
||||
FlightBatterySettingsData batterySettings;
|
||||
static float dT = SAMPLE_PERIOD_MS / 1000;
|
||||
float Bob;
|
||||
float energyRemaining;
|
||||
|
||||
|
||||
// Check how long since last update
|
||||
thisSysTime = xTaskGetTickCount();
|
||||
if(thisSysTime > lastSysTime) // reuse dt in case of wraparound
|
||||
dT = (float)(thisSysTime - lastSysTime) / (float)(portTICK_RATE_MS * 1000.0f);
|
||||
//lastSysTime = thisSysTime;
|
||||
|
||||
FlightBatterySettingsGet(&batterySettings);
|
||||
|
||||
//calculate the battery parameters
|
||||
flightBatteryData.Voltage = ((float)PIOS_ADC_PinGet(2)) * batterySettings.SensorCalibrations[FLIGHTBATTERYSETTINGS_SENSORCALIBRATIONS_VOLTAGEFACTOR]; //in Volts
|
||||
flightBatteryData.Voltage = ((float)PIOS_ADC_PinGet(0)) * batterySettings.SensorCalibrations[FLIGHTBATTERYSETTINGS_SENSORCALIBRATIONS_VOLTAGEFACTOR]; //in Volts
|
||||
flightBatteryData.Current = ((float)PIOS_ADC_PinGet(1)) * batterySettings.SensorCalibrations[FLIGHTBATTERYSETTINGS_SENSORCALIBRATIONS_CURRENTFACTOR]; //in Amps
|
||||
Bob =dT; // FIXME: something funky happens if I don't do this... Andrew
|
||||
|
||||
flightBatteryData.ConsumedEnergy += (flightBatteryData.Current * 1000.0 * dT / 3600.0) ;//in mAh
|
||||
|
||||
if (flightBatteryData.Current > flightBatteryData.PeakCurrent)flightBatteryData.PeakCurrent = flightBatteryData.Current; //in Amps
|
||||
@ -162,7 +127,6 @@ Bob =dT; // FIXME: something funky happens if I don't do this... Andrew
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_WARNING);
|
||||
else AlarmsClear(SYSTEMALARMS_ALARM_BATTERY);
|
||||
}
|
||||
lastSysTime = thisSysTime;
|
||||
|
||||
FlightBatteryStateSet(&flightBatteryData);
|
||||
}
|
||||
|
@ -230,8 +230,25 @@ extern uint32_t pios_com_vcp_id;
|
||||
|
||||
//-------------------------
|
||||
// ADC
|
||||
// None.
|
||||
// PIOS_ADC_PinGet(0) = Current sensor
|
||||
// PIOS_ADC_PinGet(1) = Voltage sensor
|
||||
// PIOS_ADC_PinGet(4) = VREF
|
||||
// PIOS_ADC_PinGet(5) = Temperature sensor
|
||||
//-------------------------
|
||||
#define PIOS_DMA_PIN_CONFIG \
|
||||
{ \
|
||||
{GPIOC, GPIO_Pin_0, ADC_Channel_10}, \
|
||||
{GPIOC, GPIO_Pin_1, ADC_Channel_11}, \
|
||||
{NULL, 0, ADC_Channel_Vrefint}, /* Voltage reference */ \
|
||||
{NULL, 0, ADC_Channel_TempSensor} /* Temperature sensor */ \
|
||||
}
|
||||
|
||||
/* we have to do all this to satisfy the PIOS_ADC_MAX_SAMPLES define in pios_adc.h */
|
||||
/* which is annoying because this then determines the rate at which we generate buffer turnover events */
|
||||
/* the objective here is to get enough buffer space to support 100Hz averaging rate */
|
||||
#define PIOS_ADC_NUM_CHANNELS 4
|
||||
#define PIOS_ADC_MAX_OVERSAMPLING 2
|
||||
#define PIOS_ADC_USE_ADC2 0
|
||||
|
||||
//-------------------------
|
||||
// USB
|
||||
|
@ -76,9 +76,9 @@ struct pios_adc_dev {
|
||||
volatile uint8_t adc_oversample;
|
||||
uint8_t dma_block_size;
|
||||
uint16_t dma_half_buffer_size;
|
||||
int16_t fir_coeffs[PIOS_ADC_MAX_SAMPLES+1] __attribute__ ((aligned(4)));
|
||||
volatile int16_t raw_data_buffer[PIOS_ADC_MAX_SAMPLES] __attribute__ ((aligned(4))); // Double buffer that DMA just used
|
||||
float downsampled_buffer[PIOS_ADC_NUM_CHANNELS] __attribute__ ((aligned(4)));
|
||||
// int16_t fir_coeffs[PIOS_ADC_MAX_SAMPLES+1] __attribute__ ((aligned(4)));
|
||||
// volatile int16_t raw_data_buffer[PIOS_ADC_MAX_SAMPLES] __attribute__ ((aligned(4)));
|
||||
// float downsampled_buffer[PIOS_ADC_NUM_CHANNELS] __attribute__ ((aligned(4)));
|
||||
enum pios_adc_dev_magic magic;
|
||||
};
|
||||
|
||||
@ -108,17 +108,15 @@ struct adc_accumulator {
|
||||
};
|
||||
|
||||
#if defined(PIOS_INCLUDE_ADC)
|
||||
static struct dma_config config[] = PIOS_DMA_PIN_CONFIG;
|
||||
static const struct dma_config config[] = PIOS_DMA_PIN_CONFIG;
|
||||
#define PIOS_ADC_NUM_PINS (sizeof(config) / sizeof(config[0]))
|
||||
|
||||
static struct adc_accumulator accumulator[PIOS_ADC_NUM_PINS];
|
||||
|
||||
// Two buffers here for double buffering
|
||||
static uint16_t adc_raw_buffer[2][PIOS_ADC_MAX_SAMPLES][PIOS_ADC_NUM_PINS];
|
||||
#endif
|
||||
|
||||
#define PIOS_ADC_TIMER TIM3 /* might want this to come from the config */
|
||||
#define PIOS_LOWRATE_ADC ADC1
|
||||
|
||||
#if defined(PIOS_INCLUDE_ADC)
|
||||
static void
|
||||
init_pins(void)
|
||||
@ -130,6 +128,8 @@ init_pins(void)
|
||||
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AIN;
|
||||
|
||||
for (int32_t i = 0; i < PIOS_ADC_NUM_PINS; i++) {
|
||||
if (config[i].port == NULL)
|
||||
continue;
|
||||
GPIO_InitStructure.GPIO_Pin = config[i].pin;
|
||||
GPIO_Init(config[i].port, &GPIO_InitStructure);
|
||||
}
|
||||
@ -164,18 +164,17 @@ init_dma(void)
|
||||
DMA_DoubleBufferModeConfig(pios_adc_dev->cfg->dma.rx.channel, (uint32_t)&adc_raw_buffer[1], DMA_Memory_0);
|
||||
DMA_DoubleBufferModeCmd(pios_adc_dev->cfg->dma.rx.channel, ENABLE);
|
||||
DMA_ITConfig(pios_adc_dev->cfg->dma.rx.channel, DMA_IT_TC, ENABLE);
|
||||
//DMA_ITConfig(pios_adc_dev->cfg->dma.rx.channel, DMA_IT_HT, ENABLE);
|
||||
|
||||
/* enable DMA */
|
||||
DMA_Cmd(pios_adc_dev->cfg->dma.rx.channel, ENABLE);
|
||||
|
||||
/* Configure DMA interrupt */
|
||||
NVIC_InitTypeDef NVICInit = pios_adc_dev->cfg->dma.irq.init;
|
||||
NVICInit.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW;
|
||||
NVICInit.NVIC_IRQChannelSubPriority = 0;
|
||||
NVICInit.NVIC_IRQChannelCmd = ENABLE;
|
||||
NVIC_Init(&NVICInit);
|
||||
}
|
||||
|
||||
#if 0
|
||||
static void
|
||||
init_timer(void)
|
||||
{
|
||||
@ -196,16 +195,17 @@ init_timer(void)
|
||||
TIMInit.TIM_ClockDivision = TIM_CKD_DIV1; /* no additional divisor */
|
||||
TIM_TimeBaseInit(PIOS_ADC_TIMER, &TIMInit);
|
||||
|
||||
PIOS_COM_SendFormattedString(PIOS_COM_DEBUG, "TIM_Prescaler %d\r\n",TIMInit.TIM_Prescaler);
|
||||
|
||||
/* configure trigger output on reload */
|
||||
TIM_SelectOutputTrigger(PIOS_ADC_TIMER, TIM_TRGOSource_Update);
|
||||
TIM_Cmd(PIOS_ADC_TIMER, ENABLE);
|
||||
}
|
||||
#endif
|
||||
|
||||
static void
|
||||
init_adc(void)
|
||||
{
|
||||
RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1, ENABLE);
|
||||
|
||||
ADC_DeInit();
|
||||
|
||||
/* turn on VREFInt in case we need it */
|
||||
@ -215,7 +215,7 @@ init_adc(void)
|
||||
ADC_CommonInitTypeDef ADC_CommonInitStructure;
|
||||
ADC_CommonStructInit(&ADC_CommonInitStructure);
|
||||
ADC_CommonInitStructure.ADC_Mode = ADC_Mode_Independent;
|
||||
ADC_CommonInitStructure.ADC_Prescaler = ADC_Prescaler_Div2;
|
||||
ADC_CommonInitStructure.ADC_Prescaler = ADC_Prescaler_Div8;
|
||||
ADC_CommonInitStructure.ADC_DMAAccessMode = ADC_DMAAccessMode_Disabled;
|
||||
ADC_CommonInitStructure.ADC_TwoSamplingDelay = ADC_TwoSamplingDelay_5Cycles;
|
||||
ADC_CommonInit(&ADC_CommonInitStructure);
|
||||
@ -224,28 +224,29 @@ init_adc(void)
|
||||
ADC_StructInit(&ADC_InitStructure);
|
||||
ADC_InitStructure.ADC_Resolution = ADC_Resolution_12b;
|
||||
ADC_InitStructure.ADC_ScanConvMode = ENABLE;
|
||||
ADC_InitStructure.ADC_ContinuousConvMode = DISABLE;
|
||||
ADC_InitStructure.ADC_ExternalTrigConv = ADC_ExternalTrigConv_T3_TRGO;
|
||||
ADC_InitStructure.ADC_ExternalTrigConvEdge = ADC_ExternalTrigConvEdge_Rising;
|
||||
ADC_InitStructure.ADC_ContinuousConvMode = ENABLE;
|
||||
ADC_InitStructure.ADC_ExternalTrigConvEdge = ADC_ExternalTrigConvEdge_None;
|
||||
ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right;
|
||||
ADC_InitStructure.ADC_NbrOfConversion = ((PIOS_ADC_NUM_PINS)/* >> 1*/);
|
||||
ADC_Init(PIOS_LOWRATE_ADC, &ADC_InitStructure);
|
||||
ADC_Init(pios_adc_dev->cfg->adc_dev, &ADC_InitStructure);
|
||||
|
||||
/* Enable PIOS_LOWRATE_ADC->DMA request */
|
||||
ADC_DMACmd(PIOS_LOWRATE_ADC, ENABLE);
|
||||
/* Enable DMA request */
|
||||
ADC_DMACmd(pios_adc_dev->cfg->adc_dev, ENABLE);
|
||||
|
||||
/* Configure input scan */
|
||||
for (int32_t i = 0; i < PIOS_ADC_NUM_PINS; i++) {
|
||||
ADC_RegularChannelConfig(PIOS_LOWRATE_ADC,
|
||||
ADC_RegularChannelConfig(pios_adc_dev->cfg->adc_dev,
|
||||
config[i].channel,
|
||||
i+1,
|
||||
ADC_SampleTime_56Cycles); /* XXX this is totally arbitrary... */
|
||||
}
|
||||
|
||||
ADC_DMARequestAfterLastTransferCmd(PIOS_LOWRATE_ADC, ENABLE);
|
||||
ADC_DMARequestAfterLastTransferCmd(pios_adc_dev->cfg->adc_dev, ENABLE);
|
||||
|
||||
/* Finally start initial conversion */
|
||||
ADC_Cmd(PIOS_LOWRATE_ADC, ENABLE);
|
||||
ADC_Cmd(pios_adc_dev->cfg->adc_dev, ENABLE);
|
||||
ADC_ContinuousModeCmd(pios_adc_dev->cfg->adc_dev, ENABLE);
|
||||
ADC_SoftwareStartConv(pios_adc_dev->cfg->adc_dev);
|
||||
}
|
||||
#endif
|
||||
|
||||
@ -297,7 +298,7 @@ int32_t PIOS_ADC_Init(const struct pios_adc_cfg * cfg)
|
||||
#if defined(PIOS_INCLUDE_ADC)
|
||||
init_pins();
|
||||
init_dma();
|
||||
init_timer();
|
||||
//init_timer();
|
||||
init_adc();
|
||||
#endif
|
||||
|
||||
@ -319,16 +320,17 @@ void PIOS_ADC_Config(uint32_t oversampling)
|
||||
* @return ADC pin value averaged over the set of samples since the last reading.
|
||||
* @return -1 if pin doesn't exist
|
||||
*/
|
||||
int32_t last_conv_value;
|
||||
int32_t PIOS_ADC_PinGet(uint32_t pin)
|
||||
{
|
||||
#if defined(PIOS_INCLUDE_ADC)
|
||||
int32_t result;
|
||||
|
||||
|
||||
/* Check if pin exists */
|
||||
if (pin >= PIOS_ADC_NUM_PINS) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
|
||||
/* return accumulated result and clear accumulator */
|
||||
result = accumulator[pin].accumulator / (accumulator[pin].count ?: 1);
|
||||
accumulator[pin].accumulator = 0;
|
||||
@ -430,15 +432,15 @@ void accumulate(uint16_t *buffer, uint32_t count)
|
||||
// XXX should do something with this
|
||||
if (pios_adc_dev->data_queue) {
|
||||
static portBASE_TYPE xHigherPriorityTaskWoken;
|
||||
xQueueSendFromISR(pios_adc_dev->data_queue, pios_adc_dev->downsampled_buffer, &xHigherPriorityTaskWoken);
|
||||
// xQueueSendFromISR(pios_adc_dev->data_queue, pios_adc_dev->downsampled_buffer, &xHigherPriorityTaskWoken);
|
||||
portEND_SWITCHING_ISR(xHigherPriorityTaskWoken);
|
||||
}
|
||||
|
||||
#endif
|
||||
#endif
|
||||
|
||||
if(pios_adc_dev->callback_function)
|
||||
pios_adc_dev->callback_function(pios_adc_dev->downsampled_buffer);
|
||||
// if(pios_adc_dev->callback_function)
|
||||
// pios_adc_dev->callback_function(pios_adc_dev->downsampled_buffer);
|
||||
|
||||
}
|
||||
|
||||
|
@ -37,6 +37,7 @@
|
||||
#include <fifo_buffer.h>
|
||||
|
||||
struct pios_adc_cfg {
|
||||
ADC_TypeDef* adc_dev;
|
||||
struct stm32_dma dma;
|
||||
uint32_t half_flag;
|
||||
uint32_t full_flag;
|
||||
|
@ -50,6 +50,7 @@ FLASH_TOOL = OPENOCD
|
||||
|
||||
# List of modules to include
|
||||
MODULES = Sensors Attitude/revolution ManualControl Stabilization Actuator
|
||||
MODULES += Battery
|
||||
MODULES += Altitude/revolution GPS FirmwareIAP
|
||||
MODULES += AltitudeHold VtolPathFollower PathPlanner
|
||||
MODULES += CameraStab
|
||||
|
@ -39,7 +39,7 @@
|
||||
#define PIOS_INCLUDE_BL_HELPER
|
||||
|
||||
/* Enable/Disable PiOS Modules */
|
||||
//#define PIOS_INCLUDE_ADC
|
||||
#define PIOS_INCLUDE_ADC
|
||||
#define PIOS_INCLUDE_DELAY
|
||||
#define PIOS_INCLUDE_I2C
|
||||
#define PIOS_INCLUDE_IRQ
|
||||
|
@ -39,6 +39,43 @@
|
||||
/**
|
||||
* Sensor configurations
|
||||
*/
|
||||
|
||||
#if defined(PIOS_INCLUDE_ADC)
|
||||
#include "pios_adc_priv.h"
|
||||
void PIOS_ADC_DMC_irq_handler(void);
|
||||
void DMA2_Stream4_IRQHandler(void) __attribute__((alias("PIOS_ADC_DMC_irq_handler")));
|
||||
struct pios_adc_cfg pios_adc_cfg = {
|
||||
.adc_dev = ADC1,
|
||||
.dma = {
|
||||
.irq = {
|
||||
.flags = (DMA_FLAG_TCIF4 | DMA_FLAG_TEIF4 | DMA_FLAG_HTIF4),
|
||||
.init = {
|
||||
.NVIC_IRQChannel = DMA2_Stream4_IRQn,
|
||||
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW,
|
||||
.NVIC_IRQChannelSubPriority = 0,
|
||||
.NVIC_IRQChannelCmd = ENABLE,
|
||||
},
|
||||
},
|
||||
.rx = {
|
||||
.channel = DMA2_Stream4,
|
||||
.init = {
|
||||
.DMA_Channel = DMA_Channel_0,
|
||||
.DMA_PeripheralBaseAddr = (uint32_t) & ADC1->DR
|
||||
},
|
||||
}
|
||||
},
|
||||
.half_flag = DMA_IT_HTIF4,
|
||||
.full_flag = DMA_IT_TCIF4,
|
||||
|
||||
};
|
||||
void PIOS_ADC_DMC_irq_handler(void)
|
||||
{
|
||||
/* Call into the generic code to handle the IRQ for this specific device */
|
||||
PIOS_ADC_DMA_Handler();
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
#if defined(PIOS_INCLUDE_HMC5883)
|
||||
#include "pios_hmc5883.h"
|
||||
static const struct pios_exti_cfg pios_exti_hmc5883_cfg __exti_config = {
|
||||
@ -795,6 +832,10 @@ void PIOS_Board_Init(void) {
|
||||
|
||||
PIOS_DELAY_WaitmS(50);
|
||||
|
||||
#if defined(PIOS_INCLUDE_ADC)
|
||||
PIOS_ADC_Init(&pios_adc_cfg);
|
||||
#endif
|
||||
|
||||
#if defined(PIOS_INCLUDE_HMC5883)
|
||||
PIOS_HMC5883_Init(&pios_hmc5883_cfg);
|
||||
#endif
|
||||
|
Loading…
x
Reference in New Issue
Block a user