mirror of https://github.com/ArduPilot/ardupilot
AP_RPM: move logging of RPM into RPM library
This commit is contained in:
parent
23f2055330
commit
ab86dafda7
|
@ -189,9 +189,11 @@ void AP_RPM::update(void)
|
|||
}
|
||||
}
|
||||
|
||||
#if HAL_LOGGING_ENABLED
|
||||
if (enabled(0) || enabled(1)) {
|
||||
AP::logger().Write_RPM(*this);
|
||||
Log_RPM();
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
/*
|
||||
|
@ -259,6 +261,24 @@ bool AP_RPM::arming_checks(size_t buflen, char *buffer) const
|
|||
return true;
|
||||
}
|
||||
|
||||
#if HAL_LOGGING_ENABLED
|
||||
void AP_RPM::Log_RPM()
|
||||
{
|
||||
float rpm1 = -1, rpm2 = -1;
|
||||
|
||||
get_rpm(0, rpm1);
|
||||
get_rpm(1, rpm2);
|
||||
|
||||
const struct log_RPM pkt{
|
||||
LOG_PACKET_HEADER_INIT(LOG_RPM_MSG),
|
||||
time_us : AP_HAL::micros64(),
|
||||
rpm1 : rpm1,
|
||||
rpm2 : rpm2
|
||||
};
|
||||
AP::logger().WriteBlock(&pkt, sizeof(pkt));
|
||||
}
|
||||
#endif
|
||||
|
||||
// singleton instance
|
||||
AP_RPM *AP_RPM::_singleton;
|
||||
|
||||
|
|
|
@ -104,6 +104,8 @@ private:
|
|||
uint8_t num_instances;
|
||||
|
||||
void detect_instance(uint8_t instance);
|
||||
|
||||
void Log_RPM();
|
||||
};
|
||||
|
||||
namespace AP {
|
||||
|
|
|
@ -0,0 +1,22 @@
|
|||
#pragma once
|
||||
|
||||
#include <AP_Logger/LogStructure.h>
|
||||
|
||||
#define LOG_IDS_FROM_RPM \
|
||||
LOG_RPM_MSG
|
||||
|
||||
// @LoggerMessage: RPM
|
||||
// @Description: Data from RPM sensors
|
||||
// @Field: TimeUS: Time since system startup
|
||||
// @Field: rpm1: First sensor's data
|
||||
// @Field: rpm2: Second sensor's data
|
||||
struct PACKED log_RPM {
|
||||
LOG_PACKET_HEADER;
|
||||
uint64_t time_us;
|
||||
float rpm1;
|
||||
float rpm2;
|
||||
};
|
||||
|
||||
#define LOG_STRUCTURE_FROM_RPM \
|
||||
{ LOG_RPM_MSG, sizeof(log_RPM), \
|
||||
"RPM", "Qff", "TimeUS,rpm1,rpm2", "sqq", "F00" , true },
|
Loading…
Reference in New Issue