DataFlash: clear format sent mask when backend starts new log
This commit is contained in:
parent
2876f1467c
commit
ac1484f60e
@ -265,6 +265,19 @@ void DataFlash_Class::Log_Write_MessageF(const char *fmt, ...)
|
|||||||
Log_Write_Message(msg);
|
Log_Write_Message(msg);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void DataFlash_Class::backend_starting_new_log(const DataFlash_Backend *backend)
|
||||||
|
{
|
||||||
|
for (uint8_t i=0; i<_next_backend; i++) {
|
||||||
|
if (backends[i] == backend) { // pointer comparison!
|
||||||
|
// reset sent masks
|
||||||
|
for (struct log_write_fmt *f = log_write_fmts; f; f=f->next) {
|
||||||
|
f->sent_mask &= ~(1<<i);
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
#define FOR_EACH_BACKEND(methodcall) \
|
#define FOR_EACH_BACKEND(methodcall) \
|
||||||
do { \
|
do { \
|
||||||
for (uint8_t i=0; i<_next_backend; i++) { \
|
for (uint8_t i=0; i<_next_backend; i++) { \
|
||||||
|
@ -272,7 +272,9 @@ private:
|
|||||||
void Log_Write_EKF2(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled);
|
void Log_Write_EKF2(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled);
|
||||||
void Log_Write_EKF3(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled);
|
void Log_Write_EKF3(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled);
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
void backend_starting_new_log(const DataFlash_Backend *backend);
|
||||||
|
|
||||||
private:
|
private:
|
||||||
static DataFlash_Class *_instance;
|
static DataFlash_Class *_instance;
|
||||||
|
|
||||||
|
@ -53,6 +53,7 @@ void DataFlash_Backend::periodic_tasks()
|
|||||||
void DataFlash_Backend::start_new_log_reset_variables()
|
void DataFlash_Backend::start_new_log_reset_variables()
|
||||||
{
|
{
|
||||||
_startup_messagewriter->reset();
|
_startup_messagewriter->reset();
|
||||||
|
_front.backend_starting_new_log(this);
|
||||||
}
|
}
|
||||||
|
|
||||||
void DataFlash_Backend::internal_error() {
|
void DataFlash_Backend::internal_error() {
|
||||||
|
@ -584,15 +584,14 @@ void DataFlash_Block::ListAvailableLogs(AP_HAL::BetterStream *port)
|
|||||||
|
|
||||||
// This function starts a new log file in the DataFlash, and writes
|
// This function starts a new log file in the DataFlash, and writes
|
||||||
// the format of supported messages in the log
|
// the format of supported messages in the log
|
||||||
|
// This function is ONLY called from the vehicle code.
|
||||||
|
// DataFlash_MAVLink, for example, will NOT call this function if the
|
||||||
|
// remote end disconnects and reconnects!
|
||||||
void DataFlash_Class::StartNewLog(void)
|
void DataFlash_Class::StartNewLog(void)
|
||||||
{
|
{
|
||||||
for (uint8_t i=0; i<_next_backend; i++) {
|
for (uint8_t i=0; i<_next_backend; i++) {
|
||||||
backends[i]->start_new_log();
|
backends[i]->start_new_log();
|
||||||
}
|
}
|
||||||
// reset sent masks
|
|
||||||
for (struct log_write_fmt *f = log_write_fmts; f; f=f->next) {
|
|
||||||
f->sent_mask = 0;
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
|
Loading…
Reference in New Issue
Block a user