void gpio_config(struct gpio_cfg *cfg)
{
uint8_t port, pin, cfg_data;
port = cfg->port;
pin = cfg->pin;
*RCC_AHB1ENR |= (1 << port);
/* pupd */
cfg_data = cfg->pupd;
gpio_pupdr(port, pin, cfg_data);
/* mode type */
cfg_data = cfg->type;
gpio_moder(port, pin, cfg_data);
if (cfg_data == GPIO_MODER_IN)
return;
/* Alternative function */
if (cfg_data == GPIO_MODER_ALT) {
uint8_t func = cfg->func;
gpio_afr(port, pin, func);
}
/* Sets pin output type */
cfg_data = cfg->o_type;
gpio_otyper(port, pin, cfg_data);
/* Speed */
cfg_data = cfg->speed;
gpio_ospeedr(port, pin, cfg_data);
}
static void init_i2c2(void) {
*RCC_APB1ENR |= RCC_APB1ENR_I2C2EN; /* Enable I2C2 Clock */
*RCC_AHB1ENR |= RCC_AHB1ENR_GPIOBEN; /* Enable GPIOB Clock */
/* Set PB8 and PB9 to alternative function I2C
* See stm32f4_ref.pdf pg 141 and stm32f407.pdf pg 51 */
/* I2C2_SCL */
gpio_moder(GPIOB, I2C2_SCL, GPIO_MODER_ALT);
gpio_afr(GPIOB, I2C2_SCL, GPIO_AF_I2C);
gpio_otyper(GPIOB, I2C2_SCL, GPIO_OTYPER_OD);
gpio_pupdr(GPIOB, I2C2_SCL, GPIO_PUPDR_NONE);
gpio_ospeedr(GPIOB, I2C2_SCL, GPIO_OSPEEDR_50M);
/* I2C2_SDA */
gpio_moder(GPIOB, I2C2_SDA, GPIO_MODER_ALT);
gpio_afr(GPIOB, I2C2_SDA, GPIO_AF_I2C);
gpio_otyper(GPIOB, I2C2_SDA, GPIO_OTYPER_OD);
gpio_pupdr(GPIOB, I2C2_SDA, GPIO_PUPDR_NONE);
gpio_ospeedr(GPIOB, I2C2_SDA, GPIO_OSPEEDR_50M);
/* Configure peripheral */
*I2C_CR2(2) |= I2C_CR2_FREQ(42);
/* Set I2C to 300kHz */
*I2C_CCR(2) |= I2C_CCR_CCR(140);
*I2C_TRISE(2) = 43;
/* Enable */
*I2C_CR1(2) |= I2C_CR1_PE;
/* Pre-initialized */
//init_semaphore(&i2c2_semaphore);
i2c2.ready = 1;
}
void init_usart(void) {
usart_t *usart1 = get_usart(1);
*RCC_APB2ENR |= RCC_APB2ENR_USART1EN; /* Enable USART1 Clock */
*RCC_AHB1ENR |= RCC_AHB1ENR_GPIOBEN; /* Enable GPIOB Clock */
/* Set PB6 and PB7 to alternative function USART
* See stm32f4_ref.pdf pg 141 and stm32f407.pdf pg 51 */
/* PB6 */
gpio_moder(GPIOB, 6, GPIO_MODER_ALT);
gpio_afr(GPIOB, 6, GPIO_AF_USART13);
gpio_otyper(GPIOB, 6, GPIO_OTYPER_PP);
gpio_pupdr(GPIOB, 6, GPIO_PUPDR_NONE);
gpio_ospeedr(GPIOB, 6, GPIO_OSPEEDR_50M);
/* PB7 */
gpio_moder(GPIOB, 7, GPIO_MODER_ALT);
gpio_afr(GPIOB, 7, GPIO_AF_USART13);
gpio_otyper(GPIOB, 7, GPIO_OTYPER_PP);
gpio_pupdr(GPIOB, 7, GPIO_PUPDR_NONE);
gpio_ospeedr(GPIOB, 7, GPIO_OSPEEDR_50M);
/* Enable USART1 */
usart1->CR1 |= USART_CR1_UE;
/* 8 data bits */
usart1->CR1 &= ~(1 << 12);
/* 1 stop bit */
usart1->CR2 &= ~(3 << 12);
/** DMA set up **/
/* DMA2, Stream 2, Channel 4 is USART1_RX
* DMA2, Stream 7, Channel 4 is USART1_TX */
usart1->CR3 |= USART_CR3_DMAR | USART_CR3_DMAT;
/* Enable DMA2 clock */
*RCC_AHB1ENR |= RCC_AHB1ENR_DMA2EN;
*RCC_AHB1ENR |= RCC_AHB1ENR_DMA1EN;
/* Clear configuration registers enable bits and wait for them to be ready */
*DMA2_CR_S(2) &= ~(DMA_SxCR_EN);
*DMA2_CR_S(7) &= ~(DMA_SxCR_EN);
while ( (*DMA2_CR_S(2) & DMA_SxCR_EN) || (*DMA2_CR_S(7) & DMA_SxCR_EN) );
/* Select channel 4 */
*DMA2_CR_S(2) |= DMA_SxCR_CHSEL(4);
*DMA2_CR_S(7) |= DMA_SxCR_CHSEL(4);
/* Peripheral address - Both use USART data register */
*DMA2_PAR_S(2) = (uint32_t) &usart1->DR; /* RX */
*DMA2_PAR_S(7) = (uint32_t) &usart1->DR; /* TX */
/*
* Allocate buffer memory.
* This must be allocated because static data goes into
* CCMRAM on STM32F4 implementation, and the bus matrix
* does not connect CCMRAM to the DMA engine. Thus, it
* is allocated from user heap to ensure it is accessible
* by DMA
*/
usart_rx_buf = (char *) malloc(USART_DMA_MSIZE);
usart_tx_buf = (char *) malloc(USART_DMA_MSIZE);
if ((usart_rx_buf == NULL) || (usart_tx_buf == NULL)) {
panic();
}
else {
/* Clear buffers */
memset(usart_rx_buf, 0, USART_DMA_MSIZE);
memset(usart_tx_buf, 0, USART_DMA_MSIZE);
*DMA2_M0AR_S(2) = (uint32_t) usart_rx_buf;
*DMA2_M0AR_S(7) = (uint32_t) usart_tx_buf;
}
/* Number of data items to be transferred */
*DMA2_NDTR_S(2) = (uint16_t) USART_DMA_MSIZE;
/* FIFO setup */
*DMA2_FCR_S(7) |= DMA_SxFCR_FTH_4 | DMA_SxFCR_DMDIS;
/* Data direct, memory increment, high priority, memory burst */
*DMA2_CR_S(2) |= DMA_SxCR_DIR_PM | DMA_SxCR_MINC | DMA_SxCR_PL_HIGH | DMA_SxCR_CIRC;
*DMA2_CR_S(7) |= DMA_SxCR_DIR_MP | DMA_SxCR_MINC | DMA_SxCR_PL_HIGH | DMA_SxCR_MBURST_4;
/* Enable DMAs */
*DMA2_CR_S(2) |= DMA_SxCR_EN;
/** DMA End **/
/* Set baud rate */
usart1->BRR = usart_baud(115200);
/* Enable reciever and transmitter */
usart1->CR1 |= USART_CR1_RE;
usart1->CR1 |= USART_CR1_TE;
usart_ready = 1;
}
rd_t open_px4_mpu6000(void) {
/* Set up CS pin and set high */
*RCC_AHB1ENR |= RCC_AHB1ENR_GPIOBEN;
/* PB0 */
gpio_moder(GPIOB, 0, GPIO_MODER_OUT);
gpio_otyper(GPIOB, 0, GPIO_OTYPER_PP);
gpio_pupdr(GPIOB, 0, GPIO_PUPDR_NONE);
gpio_ospeedr(GPIOB, 0, GPIO_OSPEEDR_50M);
/* idle CS */
cs_high();
resource *new_r = create_new_resource();
if (!new_r) {
printk("OOPS: Could not allocate space for mpu6000 resource.\r\n");
return -1;
}
struct mpu6000 *env = (struct mpu6000 *) kmalloc(sizeof(struct mpu6000));
if (!env) {
printk("OOPS: Could not allocate space for mpu6000 resource.\r\n");
kfree(new_r);
return -1;
}
env->spi_port = &spi1;
if (!env->spi_port->ready) {
env->spi_port->init();
}
env->spi_dev.cs_high = &cs_high;
env->spi_dev.cs_low = &cs_low;
env->read_ctr = 0;
acquire(&spi1_semaphore);
/* Active mode, clock with gyro X reference */
uint8_t data = MPU6000_PWR_MGMT_1_CLK_PLLGYROX;
if (spi_write(env->spi_port, &env->spi_dev, MPU6000_PWR_MGMT_1, &data, 1) != 1) {
/* Unable to activate :( */
release(&spi1_semaphore);
kfree(env);
kfree(new_r);
return -1;
}
release(&spi1_semaphore);
/* Let clock settle */
usleep(1000);
acquire(&spi1_semaphore);
/* 100Hz LPF, Gyro range +- 500deg/s, Accel range +-4g */
uint8_t config[3] = {MPU6000_CONFIG_LPF_100HZ, MPU6000_GYRO_CONFIG_500DPS, MPU6000_ACCEL_CONFIG_4G};
if (spi_write(env->spi_port, &env->spi_dev, MPU6000_CONFIG, config, 3) != 3) {
data = MPU6000_PWR_MGMT_1_SLEEP; /* Sleep mode */
spi_write(env->spi_port, &env->spi_dev, MPU6000_PWR_MGMT_1, &data, 1);
release(&spi1_semaphore);
kfree(env);
kfree(new_r);
return -1;
}
release(&spi1_semaphore);
new_r->env = (void *) env;
new_r->reader = &px4_mpu6000_read;
new_r->writer = &px4_mpu6000_write;
new_r->closer = &px4_mpu6000_close;
new_r->sem = &spi1_semaphore;
return add_resource(curr_task->task, new_r);
}