rd_t open_buf_stream(char *buf) {
struct buf_stream *env = malloc(sizeof(struct buf_stream));
if (!env) {
printk("OOPS: Could not allocate space for buffer stream resource.\r\n");
return -1;
}
resource *new_r = create_new_resource();
if (!new_r) {
printk("OOPS: Unable to allocate space for buffer stream resource.\r\n");
free(env);
return -1;
}
env->buf = buf;
new_r->env = env;
new_r->writer = &buf_stream_write;
new_r->swriter = NULL;
new_r->reader = &buf_stream_read;
new_r->closer = &buf_stream_close;
new_r->sem = malloc(sizeof(semaphore));
if (new_r->sem) {
init_semaphore(new_r->sem);
}
else {
printk("OOPS: Unable to allocate memory for buffer stream semaphore.\r\n");
kfree(new_r);
free(env);
return -1;
}
return add_resource(curr_task->task, new_r);
}
rd_t open_sfe9dof_gyro(void) {
rd_t ret;
sfe9dof_gyro *gyro = kmalloc(sizeof(sfe9dof_gyro));
if (!gyro) {
ret = -1;
goto err;
}
resource *new_r = create_new_resource();
if(!new_r) {
ret = -1;
goto err_free_gyro;
}
/* We expect that spi1 was init'd in bootmain.c */
gyro->i2c_port = &i2c1;
gyro->addr_ctr = 0;
/* Address on I2C bus of gyro. */
gyro->device_addr = 0x68;
gyro->tmp_addr = 0x1D;
if (!(gyro->i2c_port->ready)) {
gyro->i2c_port->init();
}
acquire(&i2c1_semaphore);
/* Fire it up, fire it up/ When we finally turn it over, make a b-line towards the boat, or... */
uint8_t packet[2];
packet[0] = 0x15;
packet[1] = 0x07;
if (i2c_write(gyro->i2c_port, gyro->device_addr, packet, 2) != 2) {
ret = -1;
goto err_release_sem;
}
packet[0] = 0x16;
packet[1] = 0x18;
if (i2c_write(gyro->i2c_port, gyro->device_addr, packet, 2) != 2) {
ret = -1;
goto err_release_sem;
}
release(&i2c1_semaphore);
new_r->env = gyro;
new_r->writer = &sfe9dof_gyro_write;
new_r->reader = &sfe9dof_gyro_read;
new_r->closer = &sfe9dof_gyro_close;
new_r->read_sem = &i2c1_semaphore;
new_r->write_sem = &i2c1_semaphore;
ret = add_resource(curr_task, new_r);
if (ret < 0) {
goto err_free_new_r;
}
return ret;
err_release_sem:
release(&i2c1_semaphore);
err_free_new_r:
kfree(new_r);
err_free_gyro:
kfree(gyro);
err:
printk("OOPS: Unable to open 9DOF gyro,\r\n");
return ret;
}
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);
}
