mirror of https://github.com/ArduPilot/ardupilot
AP_Logger: log which RC channels are being overridden
This commit is contained in:
parent
891cc94577
commit
f30ad1d2dc
|
@ -488,7 +488,7 @@ private:
|
||||||
bool _armed;
|
bool _armed;
|
||||||
|
|
||||||
// state to help us not log unneccesary RCIN values:
|
// state to help us not log unneccesary RCIN values:
|
||||||
bool seen_nonzero_rcin15_or_rcin16;
|
bool should_log_rcin2;
|
||||||
|
|
||||||
void Write_Compass_instance(uint64_t time_us, uint8_t mag_instance);
|
void Write_Compass_instance(uint64_t time_us, uint8_t mag_instance);
|
||||||
|
|
||||||
|
|
|
@ -149,20 +149,28 @@ void AP_Logger::Write_RCIN(void)
|
||||||
};
|
};
|
||||||
WriteBlock(&pkt, sizeof(pkt));
|
WriteBlock(&pkt, sizeof(pkt));
|
||||||
|
|
||||||
|
const uint16_t override_mask = rc().get_override_mask();
|
||||||
|
|
||||||
// don't waste logging bandwidth if we haven't seen non-zero
|
// don't waste logging bandwidth if we haven't seen non-zero
|
||||||
// channels 15/16:
|
// channels 15/16:
|
||||||
if (!seen_nonzero_rcin15_or_rcin16) {
|
if (!should_log_rcin2) {
|
||||||
if (!values[14] && !values[15]) {
|
if (values[14] || values[15]) {
|
||||||
return;
|
should_log_rcin2 = true;
|
||||||
|
} else if (override_mask != 0) {
|
||||||
|
should_log_rcin2 = true;
|
||||||
}
|
}
|
||||||
seen_nonzero_rcin15_or_rcin16 = true;
|
}
|
||||||
|
|
||||||
|
if (!should_log_rcin2) {
|
||||||
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
const struct log_RCIN2 pkt2{
|
const struct log_RCIN2 pkt2{
|
||||||
LOG_PACKET_HEADER_INIT(LOG_RCIN2_MSG),
|
LOG_PACKET_HEADER_INIT(LOG_RCIN2_MSG),
|
||||||
time_us : AP_HAL::micros64(),
|
time_us : AP_HAL::micros64(),
|
||||||
chan15 : values[14],
|
chan15 : values[14],
|
||||||
chan16 : values[15]
|
chan16 : values[15],
|
||||||
|
override_mask : override_mask,
|
||||||
};
|
};
|
||||||
WriteBlock(&pkt2, sizeof(pkt2));
|
WriteBlock(&pkt2, sizeof(pkt2));
|
||||||
}
|
}
|
||||||
|
|
|
@ -247,6 +247,7 @@ struct PACKED log_RCIN2 {
|
||||||
uint64_t time_us;
|
uint64_t time_us;
|
||||||
uint16_t chan15;
|
uint16_t chan15;
|
||||||
uint16_t chan16;
|
uint16_t chan16;
|
||||||
|
uint16_t override_mask;
|
||||||
};
|
};
|
||||||
|
|
||||||
struct PACKED log_RCOUT {
|
struct PACKED log_RCOUT {
|
||||||
|
@ -1223,7 +1224,7 @@ LOG_STRUCTURE_FROM_GPS \
|
||||||
{ LOG_RCIN_MSG, sizeof(log_RCIN), \
|
{ LOG_RCIN_MSG, sizeof(log_RCIN), \
|
||||||
"RCIN", "QHHHHHHHHHHHHHH", "TimeUS,C1,C2,C3,C4,C5,C6,C7,C8,C9,C10,C11,C12,C13,C14", "sYYYYYYYYYYYYYY", "F--------------" }, \
|
"RCIN", "QHHHHHHHHHHHHHH", "TimeUS,C1,C2,C3,C4,C5,C6,C7,C8,C9,C10,C11,C12,C13,C14", "sYYYYYYYYYYYYYY", "F--------------" }, \
|
||||||
{ LOG_RCIN2_MSG, sizeof(log_RCIN2), \
|
{ LOG_RCIN2_MSG, sizeof(log_RCIN2), \
|
||||||
"RCI2", "QHH", "TimeUS,C15,C16", "sYY", "F--" }, \
|
"RCI2", "QHHH", "TimeUS,C15,C16,OMask", "sYY-", "F---" }, \
|
||||||
{ LOG_RCOUT_MSG, sizeof(log_RCOUT), \
|
{ LOG_RCOUT_MSG, sizeof(log_RCOUT), \
|
||||||
"RCOU", "QHHHHHHHHHHHHHH", "TimeUS,C1,C2,C3,C4,C5,C6,C7,C8,C9,C10,C11,C12,C13,C14", "sYYYYYYYYYYYYYY", "F--------------" }, \
|
"RCOU", "QHHHHHHHHHHHHHH", "TimeUS,C1,C2,C3,C4,C5,C6,C7,C8,C9,C10,C11,C12,C13,C14", "sYYYYYYYYYYYYYY", "F--------------" }, \
|
||||||
{ LOG_RSSI_MSG, sizeof(log_RSSI), \
|
{ LOG_RSSI_MSG, sizeof(log_RSSI), \
|
||||||
|
|
Loading…
Reference in New Issue