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

Dataflash: add AP_Beacon logging

This commit is contained in:
karthik.desai 2017-04-18 16:43:03 +02:00 committed by Randy Mackay
parent 8f43b60247
commit 28a6ca7d75
3 changed files with 45 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/DataFlash/DataFlash.h
View File

@ -20,6 +20,7 @@
#include <DataFlash/LogStructure.h>
#include <AP_Motors/AP_Motors.h>
#include <AP_Rally/AP_Rally.h>
#include <AP_Beacon/AP_Beacon.h>
#include <stdint.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
@ -151,6 +152,7 @@ public:
void Log_Write_Rally(const AP_Rally &rally);
void Log_Write_VisualOdom(float time_delta, const Vector3f &angle_delta, const Vector3f &position_delta, float confidence);
void Log_Write_AOA_SSA(AP_AHRS &ahrs);
void Log_Write_Beacon(AP_Beacon &beacon);
void Log_Write(const char *name, const char *labels, const char *fmt, ...);

26
libraries/DataFlash/LogFile.cpp
View File

@ -2099,7 +2099,7 @@ void DataFlash_Class::Log_Write_RPM(const AP_RPM &rpm_sensor)
WriteBlock(&pkt, sizeof(pkt));
}
// Write an rate packet
// Write a rate packet
void DataFlash_Class::Log_Write_Rate(const AP_AHRS &ahrs,
const AP_Motors &motors,
const AC_AttitudeControl &attitude_control,
@ -2176,3 +2176,27 @@ void DataFlash_Class::Log_Write_AOA_SSA(AP_AHRS &ahrs)
WriteBlock(&aoa_ssa, sizeof(aoa_ssa));
}
// Write beacon sensor (position) data
void DataFlash_Class::Log_Write_Beacon(AP_Beacon &beacon)
{
// position
Vector3f pos;
float accuracy = 0.0f;
beacon.get_vehicle_position_ned(pos, accuracy);
struct log_Beacon pkt_beacon = {
LOG_PACKET_HEADER_INIT(LOG_BEACON_MSG),
time_us : AP_HAL::micros64(),
health : (uint8_t)beacon.healthy(),
count : (uint8_t)beacon.count(),
dist0 : beacon.beacon_distance(0),
dist1 : beacon.beacon_distance(1),
dist2 : beacon.beacon_distance(2),
dist3 : beacon.beacon_distance(3),
posx : pos.x,
posy : pos.y,
posz : pos.z
};
WriteBlock(&pkt_beacon, sizeof(pkt_beacon));
}

19
libraries/DataFlash/LogStructure.h
View File

@ -796,6 +796,20 @@ struct PACKED log_AOA_SSA {
float SSA;
};
struct PACKED log_Beacon {
LOG_PACKET_HEADER;
uint64_t time_us;
uint8_t health;
uint8_t count;
float dist0;
float dist1;
float dist2;
float dist3;
float posx;
float posy;
float posz;
};
// #endif // SBP_HW_LOGGING
#define ACC_LABELS "TimeUS,SampleUS,AccX,AccY,AccZ"
@ -914,7 +928,9 @@ Format characters in the format string for binary log messages
{ LOG_RFND_MSG, sizeof(log_RFND), \
"RFND", "QCBCB", "TimeUS,Dist1,Orient1,Dist2,Orient2" }, \
{ LOG_DF_MAV_STATS, sizeof(log_DF_MAV_Stats), \
"DMS", "IIIIIBBBBBBBBBB", "TimeMS,N,Dp,RT,RS,Er,Fa,Fmn,Fmx,Pa,Pmn,Pmx,Sa,Smn,Smx" }
"DMS", "IIIIIBBBBBBBBBB", "TimeMS,N,Dp,RT,RS,Er,Fa,Fmn,Fmx,Pa,Pmn,Pmx,Sa,Smn,Smx" }, \
{ LOG_BEACON_MSG, sizeof(log_Beacon), \
"BCN", "QBBfffffff", "TimeUS,Health,Cnt,D0,D1,D2,D3,PosX,PosY,PosZ" }
// messages for more advanced boards
#define LOG_EXTRA_STRUCTURES \
@ -1195,6 +1211,7 @@ enum LogMessages {
LOG_RALLY_MSG,
LOG_VISUALODOM_MSG,
LOG_AOA_SSA_MSG,
LOG_BEACON_MSG,
};
enum LogOriginType {