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

AP_Beacon_Marvelmind: Added some debug messages

This commit is contained in:
Karthik Desai 2018-05-18 15:44:38 +02:00 committed by Francisco Ferreira
parent 64fdefe7cf
commit 372fba2152
1 changed files with 18 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
libraries/AP_Beacon/AP_Beacon_Marvelmind.cpp
View File

@ -30,6 +30,15 @@
extern const AP_HAL::HAL& hal;
#define MM_DEBUG_LEVEL 0
#if MM_DEBUG_LEVEL
#include <GCS_MAVLink/GCS.h>
#define Debug(level, fmt, args ...) do { if (level <= MM_DEBUG_LEVEL) { gcs().send_text(MAV_SEVERITY_INFO, fmt, ## args); } } while (0)
#else
#define Debug(level, fmt, args ...)
#endif
AP_Beacon_Marvelmind::AP_Beacon_Marvelmind(AP_Beacon &frontend, AP_SerialManager &serial_manager) :
AP_Beacon_Backend(frontend)
{
@ -128,6 +137,7 @@ void AP_Beacon_Marvelmind::process_beacons_positions_datagram()
const uint8_t n = input_buffer[5]; // number of beacons in packet
StationaryBeaconPosition *stationary_beacon;
if ((1 + n * 8) != input_buffer[4]) {
Debug(1, "beacon pos lo pkt size %d != %d", input_buffer[4], (1 + n * 8));
return; // incorrect size
}
for (uint8_t i = 0; i < n; i++) {
@ -157,6 +167,7 @@ void AP_Beacon_Marvelmind::process_beacons_positions_highres_datagram()
const uint8_t n = input_buffer[5]; // number of beacons in packet
StationaryBeaconPosition *stationary_beacon;
if ((1 + n * 14) != input_buffer[4]) {
Debug(1, "beacon pos hi pkt size %d != %d", input_buffer[4], (1 + n * 14));
return; // incorrect size
}
for (uint8_t i = 0; i < n; i++) {
@ -363,7 +374,7 @@ void AP_Beacon_Marvelmind::set_stationary_beacons_positions()
//TODO: Calculate Accuracy of the received signal. Marvelmind *advertises* +/- 2cms
set_vehicle_position(vehicle_position_NED__m, 0.02f);
Debug(2,
"Hedge is at N%f, E%f, D%f",
"Hedge is at N%.2f, E%.2f, D%.2f",
vehicle_position_NED__m[0],
vehicle_position_NED__m[1],
vehicle_position_NED__m[2]);
@ -375,6 +386,12 @@ void AP_Beacon_Marvelmind::set_stationary_beacons_positions()
hedge.positions_beacons.beacons[i].x__mm * 0.001f,
-hedge.positions_beacons.beacons[i].z__mm * 0.001f); //Transform Marvelmind ENU to Ardupilot NED
set_beacon_position(i, beacon_position_NED__m[i]);
Debug(2,
"Beacon %d is at N%.2f, E%.2f, D%.2f",
i,
beacon_position_NED__m[i][0],
beacon_position_NED__m[i][1],
beacon_position_NED__m[i][2]);
}
}
hedge.positions_beacons.updated = false;