/* handle DEVICE_OP_READ message */ void GCS_MAVLINK::handle_device_op_read(mavlink_message_t *msg) { mavlink_device_op_read_t packet; mavlink_msg_device_op_read_decode(msg, &packet); AP_HAL::OwnPtr<AP_HAL::Device> dev = nullptr; uint8_t retcode = 0; uint8_t data[sizeof(mavlink_device_op_read_reply_t::data)] {}; if (packet.bustype == DEVICE_OP_BUSTYPE_I2C) { dev = hal.i2c_mgr->get_device(packet.bus, packet.address); } else if (packet.bustype == DEVICE_OP_BUSTYPE_SPI) { dev = hal.spi->get_device(packet.busname); } else { retcode = 1; goto fail; } if (!dev) { retcode = 2; goto fail; } if (!dev->get_semaphore()->take(10)) { retcode = 3; goto fail; } if (!dev->read_registers(packet.regstart, data, packet.count)) { retcode = 4; dev->get_semaphore()->give(); goto fail; } dev->get_semaphore()->give(); mavlink_msg_device_op_read_reply_send( chan, packet.request_id, retcode, packet.regstart, packet.count, data); return; fail: mavlink_msg_device_op_read_reply_send( chan, packet.request_id, retcode, packet.regstart, 0, nullptr); }
/* handle DEVICE_OP_WRITE message */ void GCS_MAVLINK::handle_device_op_write(mavlink_message_t *msg) { mavlink_device_op_write_t packet; mavlink_msg_device_op_write_decode(msg, &packet); AP_HAL::OwnPtr<AP_HAL::Device> dev = nullptr; uint8_t retcode = 0; if (packet.bustype == DEVICE_OP_BUSTYPE_I2C) { dev = hal.i2c_mgr->get_device(packet.bus, packet.address); } else if (packet.bustype == DEVICE_OP_BUSTYPE_SPI) { dev = hal.spi->get_device(packet.busname); } else { retcode = 1; goto fail; } if (!dev) { retcode = 2; goto fail; } if (!dev->get_semaphore()->take(10)) { retcode = 3; goto fail; } for (uint8_t i=0; i<packet.count; i++) { if (!dev->write_register(packet.regstart+i, packet.data[i])) { retcode = 4; break; } } dev->get_semaphore()->give(); fail: mavlink_msg_device_op_write_reply_send( chan, packet.request_id, retcode); }
static void spi_init() { // SPI reads have flag 0x80 set spi_dev->set_read_flag(0x80); // run the SPI bus at low speed spi_dev->set_speed(AP_HAL::Device::SPEED_LOW); uint8_t whoami = 0; uint8_t v; spi_dev->write_register(0x6B, 0x01); spi_dev->write_register(0x6B, 0x01); hal.scheduler->delay(1); spi_dev->write_register(0x6A, 0x10); spi_dev->write_register(0x6B, 0x41); hal.scheduler->delay(1); spi_dev->write_register(0x6C, 0x3f); spi_dev->write_register(0xF5, 0x00); spi_dev->write_register(0x19, 0x09); spi_dev->write_register(0xEA, 0x00); spi_dev->write_register(0x6B, 0x01); hal.scheduler->delay(1); spi_dev->write_register(0x6A, 0x10); spi_dev->write_register(0x6B, 0x41); hal.scheduler->delay(1); spi_dev->write_register(0x6B, 0x01); hal.scheduler->delay(1); spi_dev->write_register(0x23, 0x00); spi_dev->write_register(0x6B, 0x41); hal.scheduler->delay(1); spi_dev->write_register(0x1D, 0xC0); spi_dev->write_register(0x6B, 0x01); hal.scheduler->delay(1); spi_dev->write_register(0x1A, 0xC0); spi_dev->write_register(0x6B, 0x41); hal.scheduler->delay(1); spi_dev->write_register(0x38, 0x01); spi_dev->read_registers(0x6A, &v, 1); printf("reg 0x6A=0x%02x\n", v); spi_dev->read_registers(0x6B, &v, 1); printf("reg 0x6B=0x%02x\n", v); hal.scheduler->delay(1); spi_dev->write_register(0x6A, 0x10); spi_dev->write_register(0x6B, 0x41); hal.scheduler->delay(1); spi_dev->write_register(0x6B, 0x01); hal.scheduler->delay(1); spi_dev->write_register(0x23, 0x00); spi_dev->write_register(0x6B, 0x41); hal.scheduler->delay(1); spi_dev->write_register(0x6B, 0x41); spi_dev->write_register(0x6C, 0x3f); spi_dev->write_register(0x6B, 0x41); spi_dev->read_registers(0x6A, &v, 1); printf("reg 0x6A=0x%02x\n", v); spi_dev->write_register(0x6B, 0x01); hal.scheduler->delay(1); spi_dev->write_register(0x6A, 0x10); spi_dev->write_register(0x6B, 0x41); hal.scheduler->delay(1); spi_dev->write_register(0x6B, 0x01); hal.scheduler->delay(1); spi_dev->write_register(0x23, 0x00); spi_dev->write_register(0x6B, 0x41); hal.scheduler->delay(1); spi_dev->read_registers(0x6A, &v, 1); printf("reg 0x6A=0x%02x\n", v); spi_dev->write_register(0x6B, 0x01); hal.scheduler->delay(1); spi_dev->write_register(0x6A, 0x10); spi_dev->write_register(0x6B, 0x41); hal.scheduler->delay(1); spi_dev->write_register(0x6B, 0x01); hal.scheduler->delay(1); spi_dev->write_register(0x23, 0x00); spi_dev->write_register(0x6B, 0x41); hal.scheduler->delay(1); spi_dev->write_register(0x6B, 0x41); spi_dev->write_register(0x6C, 0x3f); spi_dev->write_register(0x6B, 0x41); spi_dev->read_registers(0x6A, &v, 1); printf("reg 0x6A=0x%02x\n", v); spi_dev->write_register(0x6B, 0x01); hal.scheduler->delay(1); spi_dev->write_register(0x6A, 0x10); spi_dev->write_register(0x6B, 0x41); hal.scheduler->delay(1); spi_dev->write_register(0x6B, 0x01); hal.scheduler->delay(1); spi_dev->write_register(0x23, 0x00); spi_dev->write_register(0x6B, 0x41); // print the WHOAMI spi_dev->read_registers(MPUREG_WHOAMI, &whoami, 1); printf("20789 SPI whoami: 0x%02x\n", whoami); // wait for sensor to settle hal.scheduler->delay(100); // dump registers printf("ICM20789 registers\n"); #if 0 for (uint8_t reg = 0; reg<0x80; reg++) { v=0; spi_dev->read_registers(reg, &v, 1); printf("%02x:%02x ", (unsigned)reg, (unsigned)v); if ((reg+1) % 16 == 0) { printf("\n"); } } printf("\n"); #endif spi_dev->get_semaphore()->give(); }