rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-03-22 14:19:42 +01:00
Code Issues Releases Activity

Merge branch 'revo_battery' into testing

Browse Source
This commit is contained in:
James Cotton 2012-05-26 17:06:27 -05:00
commit 1b53a9c4e6
7 changed files with 98 additions and 72 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
flight/Modules/Battery/battery.c
View File

@ -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);
}

19
flight/PiOS/Boards/STM32F4xx_Revolution.h
View File

@ -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

58
flight/PiOS/STM32F4xx/pios_adc.c
View File

@ -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);
}

1
flight/PiOS/inc/pios_adc_priv.h
View File

@ -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;

1
flight/Revolution/Makefile
View File

@ -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

2
flight/Revolution/System/inc/pios_config.h
View File

@ -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

41
flight/Revolution/System/pios_board.c
View File

@ -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