mirror of https://github.com/ArduPilot/ardupilot
RangeFinder: Add up to 10 RangeFinder logs
This commit is contained in:
parent
be9df7f600
commit
18a43601cd
|
@ -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), \
|
||||
|
|
|
@ -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)),
|
||||
const struct log_RFND pkt = {
|
||||
LOG_PACKET_HEADER_INIT(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,
|
||||
instance : i,
|
||||
dist : s->distance_cm(),
|
||||
status : (uint8_t)s->status(),
|
||||
orient : s->orientation(),
|
||||
};
|
||||
AP::logger().WriteBlock(&pkt, sizeof(pkt));
|
||||
}
|
||||
}
|
||||
|
||||
RangeFinder *RangeFinder::_singleton;
|
||||
|
|
Loading…
Reference in New Issue