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