aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

DataFlash: move proximity log from Copter (NFC)

Browse Source
This commit is contained in:
khancyr 2017-07-14 18:00:13 +01:00 committed by Francisco Ferreira
parent 0710018a42
commit 603621f2c4
3 changed files with 69 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/DataFlash/DataFlash.h
View File

@ -21,6 +21,7 @@
#include <AP_Motors/AP_Motors.h>
#include <AP_Rally/AP_Rally.h>
#include <AP_Beacon/AP_Beacon.h>
#include <AP_Proximity/AP_Proximity.h>
#include <stdint.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
@ -152,6 +153,7 @@ public:
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_Proximity(AP_Proximity &proximity);
void Log_Write(const char *name, const char *labels, const char *fmt, ...);

45
libraries/DataFlash/LogFile.cpp
View File

@ -1925,3 +1925,48 @@ void DataFlash_Class::Log_Write_Beacon(AP_Beacon &beacon)
};
WriteBlock(&pkt_beacon, sizeof(pkt_beacon));
}
// Write proximity sensor distances
void DataFlash_Class::Log_Write_Proximity(AP_Proximity &proximity)
{
// exit immediately if not enabled
if (proximity.get_status() == AP_Proximity::Proximity_NotConnected) {
return;
}
float sector_distance[8] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
proximity.get_horizontal_distance(0.0f, sector_distance[0]);
proximity.get_horizontal_distance(45.0f, sector_distance[1]);
proximity.get_horizontal_distance(90.0f, sector_distance[2]);
proximity.get_horizontal_distance(135.0f, sector_distance[3]);
proximity.get_horizontal_distance(180.0f, sector_distance[4]);
proximity.get_horizontal_distance(225.0f, sector_distance[5]);
proximity.get_horizontal_distance(270.0f, sector_distance[6]);
proximity.get_horizontal_distance(315.0f, sector_distance[7]);
float dist_up;
if (!proximity.get_upward_distance(dist_up)) {
dist_up = 0.0f;
}
float close_ang = 0.0f, close_dist = 0.0f;
proximity.get_closest_object(close_ang, close_dist);
struct log_Proximity pkt_proximity = {
LOG_PACKET_HEADER_INIT(LOG_PROXIMITY_MSG),
time_us : AP_HAL::micros64(),
health : (uint8_t)proximity.get_status(),
dist0 : sector_distance[0],
dist45 : sector_distance[1],
dist90 : sector_distance[2],
dist135 : sector_distance[3],
dist180 : sector_distance[4],
dist225 : sector_distance[5],
dist270 : sector_distance[6],
dist315 : sector_distance[7],
distup : dist_up,
closest_angle : close_ang,
closest_dist : close_dist
};
WriteBlock(&pkt_proximity, sizeof(pkt_proximity));
}

23
libraries/DataFlash/LogStructure.h
View File

@ -855,6 +855,24 @@ struct PACKED log_Beacon {
float posz;
};
// proximity sensor logging
struct PACKED log_Proximity {
LOG_PACKET_HEADER;
uint64_t time_us;
uint8_t health;
float dist0;
float dist45;
float dist90;
float dist135;
float dist180;
float dist225;
float dist270;
float dist315;
float distup;
float closest_angle;
float closest_dist;
};
// #endif // SBP_HW_LOGGING
#define ACC_LABELS "TimeUS,SampleUS,AccX,AccY,AccZ"
@ -982,7 +1000,9 @@ Format characters in the format string for binary log messages
{ 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" }, \
{ LOG_BEACON_MSG, sizeof(log_Beacon), \
"BCN", "QBBfffffff", "TimeUS,Health,Cnt,D0,D1,D2,D3,PosX,PosY,PosZ" }
"BCN", "QBBfffffff", "TimeUS,Health,Cnt,D0,D1,D2,D3,PosX,PosY,PosZ" }, \
{ LOG_PROXIMITY_MSG, sizeof(log_Proximity), \
"PRX", "QBfffffffffff", "TimeUS,Health,D0,D45,D90,D135,D180,D225,D270,D315,DUp,CAn,CDis" }
// messages for more advanced boards
#define LOG_EXTRA_STRUCTURES \
@ -1274,6 +1294,7 @@ enum LogMessages {
LOG_VISUALODOM_MSG,
LOG_AOA_SSA_MSG,
LOG_BEACON_MSG,
LOG_PROXIMITY_MSG,
};
enum LogOriginType {