/**
* @brief Initialize the MPU6000 3-axis gyro sensor.
* @return 0 for success, -1 for failure
*/
int32_t PIOS_MPU6000_Init(uint32_t spi_id, uint32_t slave_num, const struct pios_mpu60x0_cfg *cfg)
{
pios_mpu6000_dev = PIOS_MPU6000_alloc();
if (pios_mpu6000_dev == NULL)
return -1;
pios_mpu6000_dev->spi_id = spi_id;
pios_mpu6000_dev->slave_num = slave_num;
pios_mpu6000_dev->cfg = cfg;
/* Configure the MPU6000 Sensor */
PIOS_SPI_SetClockSpeed(pios_mpu6000_dev->spi_id, 100000);
PIOS_MPU6000_Config(cfg);
PIOS_SPI_SetClockSpeed(pios_mpu6000_dev->spi_id, 3000000);
pios_mpu6000_dev->threadp = PIOS_Thread_Create(
PIOS_MPU6000_Task, "pios_mpu6000", MPU6000_TASK_STACK, NULL, MPU6000_TASK_PRIORITY);
PIOS_Assert(pios_mpu6000_dev->threadp != NULL);
/* Set up EXTI line */
PIOS_EXTI_Init(cfg->exti_cfg);
#if defined(PIOS_MPU6000_ACCEL)
PIOS_SENSORS_Register(PIOS_SENSOR_ACCEL, pios_mpu6000_dev->accel_queue);
#endif /* PIOS_MPU6000_ACCEL */
PIOS_SENSORS_Register(PIOS_SENSOR_GYRO, pios_mpu6000_dev->gyro_queue);
return 0;
}
/**
* @brief Initialize the HMC5883 magnetometer sensor.
* @return 0 on success
*/
int32_t PIOS_HMC5883_Init(uint32_t i2c_id, const struct pios_hmc5883_cfg *cfg)
{
dev = (struct hmc5883_dev *) PIOS_HMC5883_alloc();
if (dev == NULL)
return -1;
dev->cfg = cfg;
dev->i2c_id = i2c_id;
dev->orientation = cfg->Default_Orientation;
/* check if we are using an irq line */
if (cfg->exti_cfg != NULL) {
PIOS_EXTI_Init(cfg->exti_cfg);
dev->data_ready_sema = PIOS_Semaphore_Create();
PIOS_Assert(dev->data_ready_sema != NULL);
}
else {
dev->data_ready_sema = NULL;
}
if (PIOS_HMC5883_Config(cfg) != 0)
return -2;
PIOS_SENSORS_Register(PIOS_SENSOR_MAG, dev->queue);
dev->task = PIOS_Thread_Create(PIOS_HMC5883_Task, "pios_hmc5883", HMC5883_TASK_STACK_BYTES, NULL, HMC5883_TASK_PRIORITY);
PIOS_Assert(dev->task != NULL);
return 0;
}
/**
* @brief Initialize the MPU9150 3-axis gyro sensor.
* @return 0 for success, -1 for failure to allocate, -2 for failure to get irq
*/
int32_t PIOS_MPU9150_Init(uint32_t i2c_id, uint8_t i2c_addr, const struct pios_mpu60x0_cfg * cfg)
{
dev = PIOS_MPU9150_alloc();
if (dev == NULL)
return -1;
dev->i2c_id = i2c_id;
dev->i2c_addr = i2c_addr;
dev->cfg = cfg;
/* Configure the MPU9150 Sensor */
if (PIOS_MPU9150_Config(cfg) != 0)
return -2;
/* Set up EXTI line */
PIOS_EXTI_Init(cfg->exti_cfg);
// Wait 5 ms for data ready interrupt and make sure it happens
// twice
if ((PIOS_Semaphore_Take(dev->data_ready_sema, 5) != true) ||
(PIOS_Semaphore_Take(dev->data_ready_sema, 5) != true)) {
return -10;
}
dev->TaskHandle = PIOS_Thread_Create(
PIOS_MPU9150_Task, "pios_mpu9150", MPU9150_TASK_STACK_BYTES, NULL, MPU9150_TASK_PRIORITY);
PIOS_Assert(dev->TaskHandle != NULL);
PIOS_SENSORS_Register(PIOS_SENSOR_ACCEL, dev->accel_queue);
PIOS_SENSORS_Register(PIOS_SENSOR_GYRO, dev->gyro_queue);
PIOS_SENSORS_Register(PIOS_SENSOR_MAG, dev->mag_queue);
return 0;
}
/**
* @brief Initialize the HMC5883 magnetometer sensor.
* @return none
*/
int32_t PIOS_HMC5883_Init(uint32_t i2c_id, const struct pios_hmc5883_cfg *cfg)
{
dev = (struct hmc5883_dev *) PIOS_HMC5883_alloc();
if (dev == NULL)
return -1;
dev->cfg = cfg;
dev->i2c_id = i2c_id;
#ifdef PIOS_HMC5883_HAS_GPIOS
PIOS_EXTI_Init(cfg->exti_cfg);
#endif
if (PIOS_HMC5883_Config(cfg) != 0)
return -2;
PIOS_SENSORS_Register(PIOS_SENSOR_MAG, dev->queue);
int result = xTaskCreate(PIOS_HMC5883_Task, (const signed char *)"pios_hmc5883",
HMC5883_TASK_STACK, NULL, HMC5883_TASK_PRIORITY,
&dev->task);
PIOS_Assert(result == pdPASS);
dev->data_ready_sema = xSemaphoreCreateMutex();
return 0;
}
/**
* @brief Initialize the L3GD20 3-axis gyro sensor.
* @return 0 for success, -1 for failure to allocate, -2 for failure to get irq
*/
int32_t PIOS_L3GD20_Init(uint32_t spi_id, uint32_t slave_num, const struct pios_l3gd20_cfg *cfg)
{
pios_l3gd20_dev = PIOS_L3GD20_alloc();
if (pios_l3gd20_dev == NULL)
return -1;
pios_l3gd20_dev->spi_id = spi_id;
pios_l3gd20_dev->slave_num = slave_num;
pios_l3gd20_dev->cfg = cfg;
/* Configure the L3GD20 Sensor */
if(PIOS_L3GD20_Config(cfg) != 0)
return -2;
/* Set up EXTI */
PIOS_EXTI_Init(cfg->exti_cfg);
// An initial read is needed to get it running
struct pios_l3gd20_data data;
PIOS_L3GD20_ReadGyros(&data);
PIOS_SENSORS_Register(PIOS_SENSOR_GYRO, pios_l3gd20_dev->queue);
return 0;
}
/**
* @brief Initialize the MPU6050 3-axis gyro sensor.
* @return 0 for success, -1 for failure
*/
int32_t PIOS_MPU6050_Init(uint32_t i2c_id, uint8_t i2c_addr, const struct pios_mpu60x0_cfg *cfg)
{
pios_mpu6050_dev = PIOS_MPU6050_alloc();
if (pios_mpu6050_dev == NULL)
return -1;
pios_mpu6050_dev->i2c_id = i2c_id;
pios_mpu6050_dev->i2c_addr = i2c_addr;
pios_mpu6050_dev->cfg = cfg;
/* Configure the MPU6050 Sensor */
PIOS_MPU6050_Config(cfg);
int result = xTaskCreate(PIOS_MPU6050_Task, (const signed char *)"pios_mpu6050",
MPU6050_TASK_STACK, NULL, MPU6050_TASK_PRIORITY,
&pios_mpu6050_dev->TaskHandle);
PIOS_Assert(result == pdPASS);
/* Set up EXTI line */
PIOS_EXTI_Init(cfg->exti_cfg);
#if defined(PIOS_MPU6050_ACCEL)
PIOS_SENSORS_Register(PIOS_SENSOR_ACCEL, pios_mpu6050_dev->accel_queue);
#endif /* PIOS_MPU6050_ACCEL */
PIOS_SENSORS_Register(PIOS_SENSOR_GYRO, pios_mpu6050_dev->gyro_queue);
return 0;
}
/**
* @brief Initialise the HMC5843 sensor
*/
void PIOS_HMC5843_Init(void)
{
/* Enable DRDY GPIO clock */
RCC_APB2PeriphClockCmd(PIOS_HMC5843_DRDY_CLK | RCC_APB2Periph_AFIO, ENABLE);
PIOS_EXTI_Init(&pios_exti_hmc5843_cfg);
/* Configure the HMC5843 Sensor */
PIOS_HMC5843_ConfigTypeDef HMC5843_InitStructure;
HMC5843_InitStructure.M_ODR = PIOS_HMC5843_ODR_10;
HMC5843_InitStructure.Meas_Conf = PIOS_HMC5843_MEASCONF_NORMAL;
HMC5843_InitStructure.Gain = PIOS_HMC5843_GAIN_2;
HMC5843_InitStructure.Mode = PIOS_HMC5843_MODE_CONTINUOUS;
PIOS_HMC5843_Config(&HMC5843_InitStructure);
pios_hmc5843_data_ready = false;
}
/**
* @brief Initialize the MPU6000 3-axis gyro sensor.
* @return 0 for success, -1 for failure
*/
int32_t PIOS_MPU6000_Init(uint32_t spi_id, uint32_t slave_num, const struct pios_mpu6000_cfg *cfg)
{
dev = PIOS_MPU6000_alloc();
if (dev == NULL) {
return -1;
}
dev->spi_id = spi_id;
dev->slave_num = slave_num;
dev->cfg = cfg;
/* Configure the MPU6000 Sensor */
PIOS_SPI_SetClockSpeed(dev->spi_id, PIOS_SPI_PRESCALER_256);
PIOS_MPU6000_Config(cfg);
PIOS_SPI_SetClockSpeed(dev->spi_id, PIOS_SPI_PRESCALER_16);
/* Set up EXTI line */
PIOS_EXTI_Init(cfg->exti_cfg);
return 0;
}
/**
* @brief Initialize with good default settings
* @returns 0 for success, -1 for failure
*/
int32_t PIOS_BMA180_Init(uint32_t spi_id, uint32_t slave_num, const struct pios_bma180_cfg * cfg)
{
dev = PIOS_BMA180_alloc();
if(dev == NULL)
return -1;
dev->spi_id = spi_id;
dev->slave_num = slave_num;
dev->cfg = cfg;
if(PIOS_BMA180_Config() < 0)
return -1;
PIOS_DELAY_WaituS(50);
PIOS_EXTI_Init(dev->cfg->exti_cfg);
while(PIOS_BMA180_EnableIrq() != 0);
return 0;
}
void PIOS_Video_Init(const struct pios_video_cfg *cfg)
{
dev_cfg = cfg; // store config before enabling interrupt
configure_hsync_timers();
/* needed for HW hack */
const GPIO_InitTypeDef initStruct = {
.GPIO_Pin = GPIO_Pin_12,
.GPIO_Speed = GPIO_Speed_100MHz,
.GPIO_Mode = GPIO_Mode_IN,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_NOPULL
};
GPIO_Init(GPIOC, &initStruct);
/* SPI3 - MASKBUFFER */
GPIO_Init(cfg->mask.sclk.gpio, (GPIO_InitTypeDef *)&(cfg->mask.sclk.init));
GPIO_Init(cfg->mask.miso.gpio, (GPIO_InitTypeDef *)&(cfg->mask.miso.init));
/* SPI1 SLAVE FRAMEBUFFER */
GPIO_Init(cfg->level.sclk.gpio, (GPIO_InitTypeDef *)&(cfg->level.sclk.init));
GPIO_Init(cfg->level.miso.gpio, (GPIO_InitTypeDef *)&(cfg->level.miso.init));
if (cfg->mask.remap) {
GPIO_PinAFConfig(cfg->mask.sclk.gpio,
__builtin_ctz(cfg->mask.sclk.init.GPIO_Pin),
cfg->mask.remap);
GPIO_PinAFConfig(cfg->mask.miso.gpio,
__builtin_ctz(cfg->mask.miso.init.GPIO_Pin),
cfg->mask.remap);
}
if (cfg->level.remap) {
GPIO_PinAFConfig(cfg->level.sclk.gpio,
__builtin_ctz(cfg->level.sclk.init.GPIO_Pin),
cfg->level.remap);
GPIO_PinAFConfig(cfg->level.miso.gpio,
__builtin_ctz(cfg->level.miso.init.GPIO_Pin),
cfg->level.remap);
}
/* Initialize the SPI block */
SPI_Init(cfg->level.regs, (SPI_InitTypeDef *)&(cfg->level.init));
SPI_Init(cfg->mask.regs, (SPI_InitTypeDef *)&(cfg->mask.init));
/* Enable SPI */
SPI_Cmd(cfg->level.regs, ENABLE);
SPI_Cmd(cfg->mask.regs, ENABLE);
/* Configure DMA for SPI Tx SLAVE Maskbuffer */
DMA_Cmd(cfg->mask.dma.tx.channel, DISABLE);
DMA_Init(cfg->mask.dma.tx.channel, (DMA_InitTypeDef *)&(cfg->mask.dma.tx.init));
/* Configure DMA for SPI Tx SLAVE Framebuffer*/
DMA_Cmd(cfg->level.dma.tx.channel, DISABLE);
DMA_Init(cfg->level.dma.tx.channel, (DMA_InitTypeDef *)&(cfg->level.dma.tx.init));
/* Trigger interrupt when for half conversions too to indicate double buffer */
DMA_ITConfig(cfg->level.dma.tx.channel, DMA_IT_TC, ENABLE);
/* Configure and clear buffers */
draw_buffer_level = buffer0_level;
draw_buffer_mask = buffer0_mask;
disp_buffer_level = buffer1_level;
disp_buffer_mask = buffer1_mask;
memset(disp_buffer_mask, 0, GRAPHICS_WIDTH * GRAPHICS_HEIGHT);
memset(disp_buffer_level, 0, GRAPHICS_WIDTH * GRAPHICS_HEIGHT);
memset(draw_buffer_mask, 0, GRAPHICS_WIDTH * GRAPHICS_HEIGHT);
memset(draw_buffer_level, 0, GRAPHICS_WIDTH * GRAPHICS_HEIGHT);
/* Configure DMA interrupt */
NVIC_Init(&cfg->level.dma.irq.init);
/* Enable SPI interrupts to DMA */
SPI_I2S_DMACmd(cfg->mask.regs, SPI_I2S_DMAReq_Tx, ENABLE);
SPI_I2S_DMACmd(cfg->level.regs, SPI_I2S_DMAReq_Tx, ENABLE);
mask_dma = DMA1;
main_dma = DMA2;
main_stream = cfg->level.dma.tx.channel;
mask_stream = cfg->mask.dma.tx.channel;
/* Configure the Video Line interrupt */
PIOS_EXTI_Init(cfg->hsync);
PIOS_EXTI_Init(cfg->vsync);
// set levels to zero
PIOS_Servo_Set(0, 0);
PIOS_Servo_Set(1, 0);
}
void PIOS_Video_Init(const struct pios_video_cfg * cfg){
dev_cfg = cfg; // store config before enabling interrupt
if (cfg->mask.remap) {
GPIO_PinAFConfig(cfg->mask.sclk.gpio,
__builtin_ctz(cfg->mask.sclk.init.GPIO_Pin),
cfg->mask.remap);
GPIO_PinAFConfig(cfg->mask.mosi.gpio,
__builtin_ctz(cfg->mask.mosi.init.GPIO_Pin),
cfg->mask.remap);
}
if (cfg->level.remap)
{
GPIO_PinAFConfig(cfg->level.sclk.gpio,
__builtin_ctz(cfg->level.sclk.init.GPIO_Pin),
cfg->level.remap);
GPIO_PinAFConfig(cfg->level.miso.gpio,
__builtin_ctz(cfg->level.miso.init.GPIO_Pin),
cfg->level.remap);
}
/* SPI3 MASTER MASKBUFFER */
GPIO_Init(cfg->mask.sclk.gpio, (GPIO_InitTypeDef*)&(cfg->mask.sclk.init));
GPIO_Init(cfg->mask.mosi.gpio, (GPIO_InitTypeDef*)&(cfg->mask.mosi.init));
/* SPI1 SLAVE FRAMEBUFFER */
GPIO_Init(cfg->level.sclk.gpio, (GPIO_InitTypeDef*)&(cfg->level.sclk.init));
GPIO_Init(cfg->level.miso.gpio, (GPIO_InitTypeDef*)&(cfg->level.miso.init));
/* Initialize the SPI block */
SPI_Init(cfg->level.regs, (SPI_InitTypeDef*)&(cfg->level.init));
SPI_Init(cfg->mask.regs, (SPI_InitTypeDef*)&(cfg->mask.init));
/* Enable SPI */
SPI_Cmd(cfg->level.regs, ENABLE);
SPI_Cmd(cfg->mask.regs, ENABLE);
/* Configure DMA for SPI Tx MASTER */
DMA_Cmd(cfg->mask.dma.tx.channel, DISABLE);
DMA_Init(cfg->mask.dma.tx.channel, (DMA_InitTypeDef*)&(cfg->mask.dma.tx.init));
/* Configure DMA for SPI Tx SLAVE */
DMA_Cmd(cfg->level.dma.tx.channel, DISABLE);
DMA_Init(cfg->level.dma.tx.channel, (DMA_InitTypeDef*)&(cfg->level.dma.tx.init));
/* Trigger interrupt when for half conversions too to indicate double buffer */
DMA_ITConfig(cfg->mask.dma.tx.channel, DMA_IT_TC, ENABLE);
/*DMA_ClearFlag(cfg->mask.dma.tx.channel,DMA_FLAG_TCIF5);
DMA_ClearITPendingBit(cfg->mask.dma.tx.channel, DMA_IT_TCIF5);
DMA_ClearFlag(cfg->level.dma.tx.channel,DMA_FLAG_TCIF5);
DMA_ClearITPendingBit(cfg->level.dma.tx.channel, DMA_IT_TCIF5);
*/
/* Configure DMA interrupt */
NVIC_Init(&cfg->level.dma.irq.init);
NVIC_Init(&cfg->mask.dma.irq.init);
/* Enable SPI interrupts to DMA */
SPI_I2S_DMACmd(cfg->level.regs, SPI_I2S_DMAReq_Tx, ENABLE);
SPI_I2S_DMACmd(cfg->mask.regs, SPI_I2S_DMAReq_Tx, ENABLE);
/* Configure the Video Line interrupt */
PIOS_EXTI_Init(cfg->hsync);
PIOS_EXTI_Init(cfg->vsync);
draw_buffer_level = buffer0_level;
draw_buffer_mask = buffer0_mask;
disp_buffer_level = buffer1_level;
disp_buffer_mask = buffer1_mask;
}
