Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

GCS_MAVLink: allow non-register device access with devop 

useful for i2c devices such as LW20 lidar
This commit is contained in:
Andrew Tridgell 2019-07-19 14:52:59 +10:00
parent 75c5d6e1bf
commit 58b739db18
1 changed files with 25 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
libraries/GCS_MAVLink/GCS_DeviceOp.cpp
View File

@ -33,7 +33,9 @@ void GCS_MAVLINK::handle_device_op_read(const mavlink_message_t &msg)
AP_HAL::OwnPtr<AP_HAL::Device> dev = nullptr; AP_HAL::OwnPtr<AP_HAL::Device> dev = nullptr;
uint8_t retcode = 0; uint8_t retcode = 0;
uint8_t data[sizeof(mavlink_device_op_read_reply_t::data)] {}; uint8_t data[sizeof(mavlink_device_op_read_reply_t::data)] {};
bool ret = false;
uint8_t regstart = packet.regstart;
if (packet.bustype == DEVICE_OP_BUSTYPE_I2C) { if (packet.bustype == DEVICE_OP_BUSTYPE_I2C) {
dev = hal.i2c_mgr->get_device(packet.bus, packet.address); dev = hal.i2c_mgr->get_device(packet.bus, packet.address);
} else if (packet.bustype == DEVICE_OP_BUSTYPE_SPI) { } else if (packet.bustype == DEVICE_OP_BUSTYPE_SPI) {
@ -50,17 +52,24 @@ void GCS_MAVLINK::handle_device_op_read(const mavlink_message_t &msg)
retcode = 3; retcode = 3;
goto fail; goto fail;
} }
if (!dev->read_registers(packet.regstart, data, packet.count)) { if (regstart == 0xff) {
retcode = 4; // assume raw transfer, non-register interface
dev->get_semaphore()->give(); ret = dev->transfer(nullptr, 0, data, packet.count);
goto fail; // reply using register start 0 for display purposes
regstart = 0;
} else {
ret = dev->read_registers(packet.regstart, data, packet.count);
} }
dev->get_semaphore()->give(); dev->get_semaphore()->give();
if (!ret) {
retcode = 4;
goto fail;
}
mavlink_msg_device_op_read_reply_send( mavlink_msg_device_op_read_reply_send(
chan, chan,
packet.request_id, packet.request_id,
retcode, retcode,
packet.regstart, regstart,
packet.count, packet.count,
data); data);
return; return;
@ -101,10 +110,17 @@ void GCS_MAVLINK::handle_device_op_write(const mavlink_message_t &msg)
retcode = 3; retcode = 3;
goto fail; goto fail;
} }
for (uint8_t i=0; i<packet.count; i++) { if (packet.regstart == 0xff) {
if (!dev->write_register(packet.regstart+i, packet.data[i])) { // assume raw transfer, non-register interface
if (!dev->transfer(packet.data, packet.count, nullptr, 0)) {
retcode = 4; retcode = 4;
break; }
} else {
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(); dev->get_semaphore()->give();