From b04844ca13477685e0e2c2559911fcea2f3baf3c Mon Sep 17 00:00:00 2001 From: Michael du Breuil Date: Sat, 3 Oct 2020 20:49:05 -0700 Subject: [PATCH] AP_Logger: Log all vibration instances --- libraries/AP_Logger/LogFile.cpp | 31 +++++++++++++++++------------- libraries/AP_Logger/LogStructure.h | 10 +++++----- 2 files changed, 23 insertions(+), 18 deletions(-) diff --git a/libraries/AP_Logger/LogFile.cpp b/libraries/AP_Logger/LogFile.cpp index 7b6273a98a..f9bd609b56 100644 --- a/libraries/AP_Logger/LogFile.cpp +++ b/libraries/AP_Logger/LogFile.cpp @@ -400,20 +400,25 @@ void AP_Logger::Write_IMUDT(uint64_t time_us, uint8_t imu_mask) void AP_Logger::Write_Vibration() { - uint64_t time_us = AP_HAL::micros64(); const AP_InertialSensor &ins = AP::ins(); - const Vector3f vibration = ins.get_vibration_levels(); - const 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)); + const uint64_t time_us = AP_HAL::micros64(); + for (uint8_t i = 0; i < INS_MAX_INSTANCES; i++) { + if (!ins.use_accel(i)) { + continue; + } + + const Vector3f vibration = ins.get_vibration_levels(i); + const struct log_Vibe pkt{ + LOG_PACKET_HEADER_INIT(LOG_VIBE_MSG), + time_us : time_us, + imu : i, + vibe_x : vibration.x, + vibe_y : vibration.y, + vibe_z : vibration.z, + clipping : ins.get_accel_clip_count(i) + }; + WriteBlock(&pkt, sizeof(pkt)); + } } void AP_Logger::Write_Command(const mavlink_command_int_t &packet, diff --git a/libraries/AP_Logger/LogStructure.h b/libraries/AP_Logger/LogStructure.h index 8d9552969c..aabff5633f 100644 --- a/libraries/AP_Logger/LogStructure.h +++ b/libraries/AP_Logger/LogStructure.h @@ -281,8 +281,9 @@ static_assert(sizeof(log_ISBD) < 256, "log_ISBD is over-size"); struct PACKED log_Vibe { LOG_PACKET_HEADER; uint64_t time_us; + uint8_t imu; float vibe_x, vibe_y, vibe_z; - uint32_t clipping_0, clipping_1, clipping_2; + uint32_t clipping; }; struct PACKED log_RCIN { @@ -2238,12 +2239,11 @@ struct PACKED log_PSC { // @LoggerMessage: VIBE // @Description: Processed (acceleration) vibration information // @Field: TimeUS: Time since system startup +// @Field: IMU: Vibration instance number // @Field: VibeX: Primary accelerometer filtered vibration, x-axis // @Field: VibeY: Primary accelerometer filtered vibration, y-axis // @Field: VibeZ: Primary accelerometer filtered vibration, z-axis -// @Field: Clip0: Number of clipping events on 1st accelerometer -// @Field: Clip1: Number of clipping events on 2nd accelerometer -// @Field: Clip2: Number of clipping events on 3rd accelerometer +// @Field: Clip: Number of clipping events on 1st accelerometer // @LoggerMessage: VISO // @Description: Visual Odometry @@ -2663,7 +2663,7 @@ struct PACKED log_PSC { { LOG_BAR3_MSG, sizeof(log_BARO), \ "BAR3", BARO_FMT, BARO_LABELS, BARO_UNITS, BARO_MULTS }, \ { LOG_VIBE_MSG, sizeof(log_Vibe), \ - "VIBE", "QfffIII", "TimeUS,VibeX,VibeY,VibeZ,Clip0,Clip1,Clip2", "s------", "F------" }, \ + "VIBE", "QBfffI", "TimeUS,IMU,VibeX,VibeY,VibeZ,Clip", "s#----", "F-----" }, \ { LOG_IMUDT_MSG, sizeof(log_IMUDT), \ "IMT",IMT_FMT,IMT_LABELS, IMT_UNITS, IMT_MULTS }, \ { LOG_IMUDT2_MSG, sizeof(log_IMUDT), \