Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Proximity: emit RPLidar detection information

Browse Source
This commit is contained in:
Peter Barker 2023-05-12 12:20:17 +10:00 committed by Randy Mackay
parent cb7f29cc6b
commit 77f7533f02
1 changed files with 5 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_Proximity/AP_Proximity_RPLidarA2.cpp
View File

@ -41,8 +41,8 @@
#define RP_DEBUG_LEVEL 0 #define RP_DEBUG_LEVEL 0
#if RP_DEBUG_LEVEL
#include <GCS_MAVLink/GCS.h> #include <GCS_MAVLink/GCS.h>
#if RP_DEBUG_LEVEL
#define Debug(level, fmt, args ...) do { if (level <= RP_DEBUG_LEVEL) { gcs().send_text(MAV_SEVERITY_INFO, fmt, ## args); } } while (0) #define Debug(level, fmt, args ...) do { if (level <= RP_DEBUG_LEVEL) { gcs().send_text(MAV_SEVERITY_INFO, fmt, ## args); } } while (0)
#else #else
#define Debug(level, fmt, args ...) #define Debug(level, fmt, args ...)
@ -313,18 +313,20 @@ void AP_Proximity_RPLidarA2::get_readings()
void AP_Proximity_RPLidarA2::parse_response_device_info() void AP_Proximity_RPLidarA2::parse_response_device_info()
{ {
Debug(1, "Received DEVICE_INFO"); Debug(1, "Received DEVICE_INFO");
const char *device_type = "UNKNOWN";
switch (_payload.device_info.model) { switch (_payload.device_info.model) {
case 0x18: case 0x18:
model = Model::A1; model = Model::A1;
device_type = "A1";
break; break;
case 0x28: case 0x28:
model = Model::A2; model = Model::A2;
device_type = "A2";
break; break;
default: default:
Debug(1, "Unknown device (%u)", _payload.device_info.model); Debug(1, "Unknown device (%u)", _payload.device_info.model);
} }
Debug(1, "firmware (%u.%u)", _payload.device_info.firmware_minor, _payload.device_info.firmware_major); GCS_SEND_TEXT(MAV_SEVERITY_INFO, "RPLidar %s hw=%u fw=%u.%u", device_type, _payload.device_info.hardware, _payload.device_info.firmware_minor, _payload.device_info.firmware_major);
Debug(1, "Hardware (%u)", _payload.device_info.hardware);
send_scan_mode_request(); send_scan_mode_request();
_state = State::AWAITING_RESPONSE; _state = State::AWAITING_RESPONSE;
} }