Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

DataFlash: add Log_Write_Vibration

This commit is contained in:
Randy Mackay 2015-06-11 22:26:45 +09:00
parent 1289208244
commit f0f262eb04
2 changed files with 31 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/DataFlash/DataFlash.h
View File

@ -67,6 +67,7 @@ public:
void Log_Write_Parameter(const char *name, float value); void Log_Write_Parameter(const char *name, float value);
void Log_Write_GPS(const AP_GPS &gps, uint8_t instance, int32_t relative_alt); void Log_Write_GPS(const AP_GPS &gps, uint8_t instance, int32_t relative_alt);
void Log_Write_IMU(const AP_InertialSensor &ins); void Log_Write_IMU(const AP_InertialSensor &ins);
void Log_Write_Vibration(const AP_InertialSensor &ins);
void Log_Write_RCIN(void); void Log_Write_RCIN(void);
void Log_Write_RCOUT(void); void Log_Write_RCOUT(void);
void Log_Write_Baro(AP_Baro &baro); void Log_Write_Baro(AP_Baro &baro);
@ -226,6 +227,13 @@ struct PACKED log_IMU {
uint8_t gyro_health, accel_health; uint8_t gyro_health, accel_health;
}; };
struct PACKED log_Vibe {
LOG_PACKET_HEADER;
uint64_t time_us;
float vibe_x, vibe_y, vibe_z;
uint32_t clipping_0, clipping_1, clipping_2;
};
struct PACKED log_RCIN { struct PACKED log_RCIN {
LOG_PACKET_HEADER; LOG_PACKET_HEADER;
uint64_t time_us; uint64_t time_us;
@ -707,7 +715,10 @@ Format characters in the format string for binary log messages
{ LOG_PIDA_MSG, sizeof(log_PID), \ { LOG_PIDA_MSG, sizeof(log_PID), \
"PIDA", "Qffffff", "TimeUS,Des,P,I,D,FF,AFF" }, \ "PIDA", "Qffffff", "TimeUS,Des,P,I,D,FF,AFF" }, \
{ LOG_BAR2_MSG, sizeof(log_BARO), \ { LOG_BAR2_MSG, sizeof(log_BARO), \
"BAR2", "Qffcf", "TimeUS,Alt,Press,Temp,CRt" } "BAR2", "Qffcf", "TimeUS,Alt,Press,Temp,CRt" }, \
{ LOG_VIBE_MSG, sizeof(log_Vibe), \
"VIBE", "QfffIII", "TimeUS,VibeX,VibeY,VibeZ,Clip0,Clip1,Clip2" }
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_75 #if HAL_CPU_CLASS >= HAL_CPU_CLASS_75
#define LOG_COMMON_STRUCTURES LOG_BASE_STRUCTURES, LOG_EXTRA_STRUCTURES #define LOG_COMMON_STRUCTURES LOG_BASE_STRUCTURES, LOG_EXTRA_STRUCTURES
@ -773,6 +784,7 @@ Format characters in the format string for binary log messages
#define LOG_PIDP_MSG 180 #define LOG_PIDP_MSG 180
#define LOG_PIDY_MSG 181 #define LOG_PIDY_MSG 181
#define LOG_PIDA_MSG 182 #define LOG_PIDA_MSG 182
#define LOG_VIBE_MSG 183
// message types 200 to 210 reversed for GPS driver use // message types 200 to 210 reversed for GPS driver use
// message types 211 to 220 reversed for autotune use // message types 211 to 220 reversed for autotune use

18
libraries/DataFlash/LogFile.cpp
View File

@ -9,6 +9,7 @@
#include <AP_AHRS.h> #include <AP_AHRS.h>
#include "../AP_BattMonitor/AP_BattMonitor.h" #include "../AP_BattMonitor/AP_BattMonitor.h"
#include <AP_Compass.h> #include <AP_Compass.h>
#include <stdio.h>
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
@ -868,6 +869,23 @@ void DataFlash_Class::Log_Write_IMU(const AP_InertialSensor &ins)
#endif #endif
} }
void DataFlash_Class::Log_Write_Vibration(const AP_InertialSensor &ins)
{
uint64_t time_us = hal.scheduler->micros64();
Vector3f vibration = ins.get_vibration_levels();
struct log_Vibe pkt = {
LOG_PACKET_HEADER_INIT(LOG_VIBE_MSG),
time_us : time_us,
vibe_x : vibration.x,
vibe_y : vibration.y,
vibe_z : vibration.z,
clipping_0 : ins.get_accel_clip_count(0),
clipping_1 : ins.get_accel_clip_count(1),
clipping_2 : ins.get_accel_clip_count(2)
};
WriteBlock(&pkt, sizeof(pkt));
}
// Write a text message to the log // Write a text message to the log
void DataFlash_Class::Log_Write_Message(const char *message) void DataFlash_Class::Log_Write_Message(const char *message)
{ {