5
0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-09 17:38:32 -04:00

DataFlash: added RPM logging

This commit is contained in:
Andrew Tridgell 2015-08-07 20:34:14 +10:00 committed by Randy Mackay
parent 2cf93e828d
commit 10b8192463
2 changed files with 25 additions and 2 deletions
libraries/DataFlash

View File

@ -17,6 +17,7 @@
#include <AP_Mission/AP_Mission.h>
#include "../AP_Airspeed/AP_Airspeed.h"
#include "../AP_BattMonitor/AP_BattMonitor.h"
#include "../AP_RPM/AP_RPM.h"
#include <stdint.h>
#include "DataFlash_Backend.h"
@ -96,6 +97,7 @@ public:
void Log_Write_Mission_Cmd(const AP_Mission &mission,
const AP_Mission::Mission_Command &cmd);
void Log_Write_Origin(uint8_t origin_type, const Location &loc);
void Log_Write_RPM(const AP_RPM &rpm_sensor);
// This structure provides information on the internal member data of a PID for logging purposes
struct PID_Info {
@ -600,6 +602,13 @@ struct PACKED log_ORGN {
int32_t altitude;
};
struct PACKED log_RPM {
LOG_PACKET_HEADER;
uint64_t time_us;
float rpm1;
float rpm2;
};
/*
Format characters in the format string for binary log messages
b : int8_t
@ -751,7 +760,9 @@ Format characters in the format string for binary log messages
{ LOG_IMUDT3_MSG, sizeof(log_IMUDT), \
"IMT3","Qffffffff","TimeUS,DelT,DelvT,DelAX,DelAY,DelAZ,DelVX,DelVY,DelVZ" }, \
{ LOG_ORGN_MSG, sizeof(log_ORGN), \
"ORGN","QBLLe","TimeUS,Type,Lat,Lng,Alt" }
"ORGN","QBLLe","TimeUS,Type,Lat,Lng,Alt" }, \
{ LOG_RPM_MSG, sizeof(log_RPM), \
"RPM", "Qff", "TimeUS,rpm1,rpm2" }
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_75
#define LOG_COMMON_STRUCTURES LOG_BASE_STRUCTURES, LOG_EXTRA_STRUCTURES
@ -825,7 +836,8 @@ enum LogMessages {
LOG_IMUDT_MSG,
LOG_IMUDT2_MSG,
LOG_IMUDT3_MSG,
LOG_ORGN_MSG
LOG_ORGN_MSG,
LOG_RPM_MSG
};
enum LogOriginType {

View File

@ -1493,3 +1493,14 @@ void DataFlash_Class::Log_Write_Origin(uint8_t origin_type, const Location &loc)
};
WriteBlock(&pkt, sizeof(pkt));
}
void DataFlash_Class::Log_Write_RPM(const AP_RPM &rpm_sensor)
{
struct log_RPM pkt = {
LOG_PACKET_HEADER_INIT(LOG_RPM_MSG),
time_us : hal.scheduler->micros64(),
rpm1 : rpm_sensor.get_rpm(0),
rpm2 : rpm_sensor.get_rpm(1)
};
WriteBlock(&pkt, sizeof(pkt));
}