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

RangeFinder: Add up to 10 RangeFinder logs

This commit is contained in:
Tatsuya Yamaguchi 2019-05-30 09:28:17 +09:00 committed by Peter Barker
parent be9df7f600
commit 18a43601cd
2 changed files with 20 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_Logger/LogStructure.h
View File

@ -801,12 +801,10 @@ struct PACKED log_Mode {
struct PACKED log_RFND { struct PACKED log_RFND {
LOG_PACKET_HEADER; LOG_PACKET_HEADER;
uint64_t time_us; uint64_t time_us;
uint16_t dist1; uint8_t instance;
uint8_t status1; uint16_t dist;
uint8_t orient1; uint8_t status;
uint16_t dist2; uint8_t orient;
uint8_t status2;
uint8_t orient2;
}; };
/* /*
@ -1329,7 +1327,7 @@ struct PACKED log_Arm_Disarm {
{ LOG_MODE_MSG, sizeof(log_Mode), \ { LOG_MODE_MSG, sizeof(log_Mode), \
"MODE", "QMBB", "TimeUS,Mode,ModeNum,Rsn", "s---", "F---" }, \ "MODE", "QMBB", "TimeUS,Mode,ModeNum,Rsn", "s---", "F---" }, \
{ LOG_RFND_MSG, sizeof(log_RFND), \ { LOG_RFND_MSG, sizeof(log_RFND), \
"RFND", "QCBBCBB", "TimeUS,Dist1,Stat1,Orient1,Dist2,Stat2,Orient2", "sm--m--", "FB--B--" }, \ "RFND", "QBCBB", "TimeUS,Instance,Dist,Stat,Orient", "s#m--", "F-B--" }, \
{ LOG_MAV_STATS, sizeof(log_MAV_Stats), \ { LOG_MAV_STATS, sizeof(log_MAV_Stats), \
"DMS", "IIIIIBBBBBBBBB", "TimeMS,N,Dp,RT,RS,Fa,Fmn,Fmx,Pa,Pmn,Pmx,Sa,Smn,Smx", "s-------------", "C-------------" }, \ "DMS", "IIIIIBBBBBBBBB", "TimeMS,N,Dp,RT,RS,Fa,Fmn,Fmx,Pa,Pmn,Pmx,Sa,Smn,Smx", "s-------------", "C-------------" }, \
{ LOG_BEACON_MSG, sizeof(log_Beacon), \ { LOG_BEACON_MSG, sizeof(log_Beacon), \

31
libraries/AP_RangeFinder/RangeFinder.cpp
View File

@ -675,23 +675,22 @@ void RangeFinder::Log_RFND()
return; return;
} }
const AP_RangeFinder_Backend *s0 = get_backend(0); for (uint8_t i=0; i<RANGEFINDER_MAX_INSTANCES; i++) {
const AP_RangeFinder_Backend *s1 = get_backend(1); const AP_RangeFinder_Backend *s = get_backend(i);
if (s0 == nullptr && s1 == nullptr) { if (s == nullptr) {
return; continue;
} }
struct log_RFND pkt = { const struct log_RFND pkt = {
LOG_PACKET_HEADER_INIT((uint8_t)(LOG_RFND_MSG)), LOG_PACKET_HEADER_INIT(LOG_RFND_MSG),
time_us : AP_HAL::micros64(), time_us : AP_HAL::micros64(),
dist1 : s0 ? s0->distance_cm() : (uint16_t)0, instance : i,
status1 : s0 ? (uint8_t)s0->status() : (uint8_t)0, dist : s->distance_cm(),
orient1 : s0 ? s0->orientation() : ROTATION_NONE, status : (uint8_t)s->status(),
dist2 : s1 ? s1->distance_cm() : (uint16_t)0, orient : s->orientation(),
status2 : s1 ? (uint8_t)s1->status() : (uint8_t)0, };
orient2 : s1 ? s1->orientation() : ROTATION_NONE, AP::logger().WriteBlock(&pkt, sizeof(pkt));
}; }
AP::logger().WriteBlock(&pkt, sizeof(pkt));
} }
RangeFinder *RangeFinder::_singleton; RangeFinder *RangeFinder::_singleton;