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

26
libraries/GCS_MAVLink/GCS_DeviceOp.cpp
View File

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