Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-05 07:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Logger: add Write_OA

Browse Source
This commit is contained in:
Randy Mackay 2019-06-08 13:59:04 +09:00
parent beb9ecbdcb
commit c7b1ad3443
3 changed files with 29 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
libraries/AP_Logger/AP_Logger.h
View File

@ -274,6 +274,7 @@ public:
void Write_Beacon(AP_Beacon &beacon); void Write_Beacon(AP_Beacon &beacon);
void Write_Proximity(AP_Proximity &proximity); void Write_Proximity(AP_Proximity &proximity);
void Write_SRTL(bool active, uint16_t num_points, uint16_t max_points, uint8_t action, const Vector3f& point); void Write_SRTL(bool active, uint16_t num_points, uint16_t max_points, uint8_t action, const Vector3f& point);
void Write_OA(uint8_t algorithm, const Location& final_dest, const Location& oa_dest);
void Write(const char *name, const char *labels, const char *fmt, ...); void Write(const char *name, const char *labels, const char *fmt, ...);
void Write(const char *name, const char *labels, const char *units, const char *mults, const char *fmt, ...); void Write(const char *name, const char *labels, const char *units, const char *mults, const char *fmt, ...);

14
libraries/AP_Logger/LogFile.cpp
View File

@ -1694,3 +1694,17 @@ void AP_Logger::Write_SRTL(bool active, uint16_t num_points, uint16_t max_points
}; };
WriteBlock(&pkt_srtl, sizeof(pkt_srtl)); WriteBlock(&pkt_srtl, sizeof(pkt_srtl));
} }
void AP_Logger::Write_OA(uint8_t algorithm, const Location& final_dest, const Location& oa_dest)
{
struct log_OA pkt = {
LOG_PACKET_HEADER_INIT(LOG_OA_MSG),
time_us : AP_HAL::micros64(),
algorithm : algorithm,
final_lat : final_dest.lat,
final_lng : final_dest.lng,
oa_lat : oa_dest.lat,
oa_lng : oa_dest.lng,
};
WriteBlock(&pkt, sizeof(pkt));
}

15
libraries/AP_Logger/LogStructure.h
View File

@ -1125,6 +1125,16 @@ struct PACKED log_SRTL {
float D; float D;
}; };
struct PACKED log_OA {
LOG_PACKET_HEADER;
uint64_t time_us;
uint8_t algorithm;
int32_t final_lat;
int32_t final_lng;
int32_t oa_lat;
int32_t oa_lng;
};
struct PACKED log_DSTL { struct PACKED log_DSTL {
LOG_PACKET_HEADER; LOG_PACKET_HEADER;
uint64_t time_us; uint64_t time_us;
@ -1337,7 +1347,9 @@ struct PACKED log_Arm_Disarm {
{ LOG_PERFORMANCE_MSG, sizeof(log_Performance), \ { LOG_PERFORMANCE_MSG, sizeof(log_Performance), \
"PM", "QHHIIHIII", "TimeUS,NLon,NLoop,MaxT,Mem,Load,IntErr,SPICnt,I2CCnt", "s---b%--", "F---0A--" }, \ "PM", "QHHIIHIII", "TimeUS,NLon,NLoop,MaxT,Mem,Load,IntErr,SPICnt,I2CCnt", "s---b%--", "F---0A--" }, \
{ LOG_SRTL_MSG, sizeof(log_SRTL), \ { LOG_SRTL_MSG, sizeof(log_SRTL), \
"SRTL", "QBHHBfff", "TimeUS,Active,NumPts,MaxPts,Action,N,E,D", "s----mmm", "F----000" } "SRTL", "QBHHBfff", "TimeUS,Active,NumPts,MaxPts,Action,N,E,D", "s----mmm", "F----000" }, \
{ LOG_OA_MSG, sizeof(log_OA), \
"OA","QBLLLL","TimeUS,Algo,DLat,DLng,OALat,OALng", "s-----", "F-GGGG" }
// messages for more advanced boards // messages for more advanced boards
#define LOG_EXTRA_STRUCTURES \ #define LOG_EXTRA_STRUCTURES \
@ -1681,6 +1693,7 @@ enum LogMessages : uint8_t {
LOG_ERROR_MSG, LOG_ERROR_MSG,
LOG_ADSB_MSG, LOG_ADSB_MSG,
LOG_ARM_DISARM_MSG, LOG_ARM_DISARM_MSG,
LOG_OA_MSG,
_LOG_LAST_MSG_ _LOG_LAST_MSG_
}; };