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

AP_Logger: replace Write_OA with Write_OABendyRuler and Write_OADijkstra

Browse Source
This commit is contained in:
Randy Mackay 2019-08-05 15:22:05 +09:00
parent f5a307fb13
commit b666b172c4
3 changed files with 49 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
libraries/AP_Logger/AP_Logger.h
View File

@ -273,7 +273,8 @@ public:
void Write_Beacon(AP_Beacon &beacon);
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_OA(uint8_t algorithm, const Location& final_dest, const Location& oa_dest);
void Write_OABendyRuler(bool active, float target_yaw, float margin, const Location &final_dest, const Location &oa_dest);
void Write_OADijkstra(uint8_t state, uint8_t curr_point, uint8_t tot_points, 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 *units, const char *mults, const char *fmt, ...);

29
libraries/AP_Logger/LogFile.cpp
View File

@ -971,16 +971,35 @@ void AP_Logger::Write_SRTL(bool active, uint16_t num_points, uint16_t max_points
WriteBlock(&pkt_srtl, sizeof(pkt_srtl));
}
void AP_Logger::Write_OA(uint8_t algorithm, const Location& final_dest, const Location& oa_dest)
void AP_Logger::Write_OABendyRuler(bool active, float target_yaw, float margin, const Location &final_dest, const Location &oa_dest)
{
struct log_OA pkt = {
LOG_PACKET_HEADER_INIT(LOG_OA_MSG),
struct log_OABendyRuler pkt = {
LOG_PACKET_HEADER_INIT(LOG_OA_BENDYRULER_MSG),
time_us : AP_HAL::micros64(),
algorithm : algorithm,
active : active,
target_yaw : (uint16_t)wrap_360(target_yaw),
yaw : (uint16_t)wrap_360(AP::ahrs().yaw_sensor * 0.01f),
margin : margin,
final_lat : final_dest.lat,
final_lng : final_dest.lng,
oa_lat : oa_dest.lat,
oa_lng : oa_dest.lng,
oa_lng : oa_dest.lng
};
WriteBlock(&pkt, sizeof(pkt));
}
void AP_Logger::Write_OADijkstra(uint8_t state, uint8_t curr_point, uint8_t tot_points, const Location &final_dest, const Location &oa_dest)
{
struct log_OADijkstra pkt = {
LOG_PACKET_HEADER_INIT(LOG_OA_DIJKSTRA_MSG),
time_us : AP_HAL::micros64(),
state : state,
curr_point : curr_point,
tot_points : tot_points,
final_lat : final_dest.lat,
final_lng : final_dest.lng,
oa_lat : oa_dest.lat,
oa_lng : oa_dest.lng
};
WriteBlock(&pkt, sizeof(pkt));
}

28
libraries/AP_Logger/LogStructure.h
View File

@ -1130,10 +1130,25 @@ struct PACKED log_SRTL {
float D;
};
struct PACKED log_OA {
struct PACKED log_OABendyRuler {
LOG_PACKET_HEADER;
uint64_t time_us;
uint8_t algorithm;
uint8_t active;
uint16_t target_yaw;
uint16_t yaw;
float margin;
int32_t final_lat;
int32_t final_lng;
int32_t oa_lat;
int32_t oa_lng;
};
struct PACKED log_OADijkstra {
LOG_PACKET_HEADER;
uint64_t time_us;
uint8_t state;
uint8_t curr_point;
uint8_t tot_points;
int32_t final_lat;
int32_t final_lng;
int32_t oa_lat;
@ -1353,8 +1368,10 @@ struct PACKED log_Arm_Disarm {
"PM", "QHHIIHIIII", "TimeUS,NLon,NLoop,MaxT,Mem,Load,IntErr,IntErrCnt,SPICnt,I2CCnt", "s---b%----", "F---0A----" }, \
{ LOG_SRTL_MSG, sizeof(log_SRTL), \
"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" }
{ LOG_OA_BENDYRULER_MSG, sizeof(log_OABendyRuler), \
"OABR","QBHHfLLLL","TimeUS,Active,DesYaw,Yaw,Mar,DLat,DLng,OALat,OALng", "sbddmDUDU", "F----GGGG" }, \
{ LOG_OA_DIJKSTRA_MSG, sizeof(log_OADijkstra), \
"OADJ","QBBBLLLL","TimeUS,State,CurrPoint,TotPoints,DLat,DLng,OALat,OALng", "sbbbDUDU", "F---GGGG" }
// messages for more advanced boards
#define LOG_EXTRA_STRUCTURES \
@ -1726,7 +1743,8 @@ enum LogMessages : uint8_t {
LOG_ERROR_MSG,
LOG_ADSB_MSG,
LOG_ARM_DISARM_MSG,
LOG_OA_MSG,
LOG_OA_BENDYRULER_MSG,
LOG_OA_DIJKSTRA_MSG,
_LOG_LAST_MSG_
};