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

SITL: RPLidar: add support for GET_DEVICE_INFO command

This commit is contained in:
Peter Barker 2021-02-05 12:28:34 +11:00 committed by Randy Mackay
parent 76cf605e11
commit 742416c2aa
2 changed files with 29 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
libraries/SITL/SIM_PS_RPLidarA2.cpp
View File

@ -105,6 +105,30 @@ void PS_RPLidarA2::update_input()
set_inputstate(InputState::WAITING_FOR_PREAMBLE);
return;
}
case Command::GET_DEVICE_INFO: {
// consume the command:
memmove(_buffer, &_buffer[1], _buflen-1);
_buflen--;
send_response_descriptor(0x14, SendMode::SRSR, DataType::Unknown04);
// now send the device info:
struct PACKED _device_info {
uint8_t model;
uint8_t firmware_minor;
uint8_t firmware_major;
uint8_t hardware;
uint8_t serial[16];
} device_info;
device_info.model = device_info_model();
device_info.firmware_minor = 17;
device_info.firmware_major = 42;
device_info.hardware = 6;
const ssize_t ret = write_to_autopilot((const char*)&device_info, sizeof(device_info));
if (ret != sizeof(device_info)) {
abort();
}
set_inputstate(InputState::WAITING_FOR_PREAMBLE);
return;
}
case Command::FORCE_SCAN:
abort();
case Command::RESET:

5
libraries/SITL/SIM_PS_RPLidarA2.h
View File

@ -109,12 +109,14 @@ private:
SCAN = 0x20,
FORCE_SCAN = 0x21,
RESET = 0x40,
GET_DEVICE_INFO = 0x50,
GET_HEALTH = 0x52,
};
void move_preamble_in_buffer();
enum class DataType {
Unknown04 = 0x04, // uint8_t ?!
Unknown06 = 0x06, // uint8_t ?!
Unknown81 = 0x81, // uint8_t ?!
};
@ -132,6 +134,9 @@ private:
// the driver expects to see an "R" followed by 62 bytes more crap.
static const constexpr char *FIRMWARE_INFO = "R12345678901234567890123456789012345678901234567890123456789012";
uint8_t _firmware_info_offset;
// this will be pure-virtual in a notional RPLidar base class:
uint8_t device_info_model() const { return 0x28; }
};
};