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 {
LOG_PACKET_HEADER;
uint64_t time_us;
uint16_t dist1;
uint8_t status1;
uint8_t orient1;
uint16_t dist2;
uint8_t status2;
uint8_t orient2;
uint8_t instance;
uint16_t dist;
uint8_t status;
uint8_t orient;
};
/*
@ -1329,7 +1327,7 @@ struct PACKED log_Arm_Disarm {
{ LOG_MODE_MSG, sizeof(log_Mode), \
"MODE", "QMBB", "TimeUS,Mode,ModeNum,Rsn", "s---", "F---" }, \
{ 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), \
"DMS", "IIIIIBBBBBBBBB", "TimeMS,N,Dp,RT,RS,Fa,Fmn,Fmx,Pa,Pmn,Pmx,Sa,Smn,Smx", "s-------------", "C-------------" }, \
{ LOG_BEACON_MSG, sizeof(log_Beacon), \

31
libraries/AP_RangeFinder/RangeFinder.cpp
View File

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