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