/** * @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; }