diff --git a/libraries/SITL/SIM_PS_RPLidarA2.cpp b/libraries/SITL/SIM_PS_RPLidarA2.cpp index 47c4248582..1da08f4262 100644 --- a/libraries/SITL/SIM_PS_RPLidarA2.cpp +++ b/libraries/SITL/SIM_PS_RPLidarA2.cpp @@ -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: diff --git a/libraries/SITL/SIM_PS_RPLidarA2.h b/libraries/SITL/SIM_PS_RPLidarA2.h index 677ca4ab69..cb8a4a4233 100644 --- a/libraries/SITL/SIM_PS_RPLidarA2.h +++ b/libraries/SITL/SIM_PS_RPLidarA2.h @@ -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; } }; };