diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp index a1f2ca55e8..b70539bb71 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp @@ -306,17 +306,7 @@ void AP_InertialSensor_Backend::log_gyro_raw(uint8_t instance, const uint64_t sa return; } if (should_log_imu_raw()) { - uint64_t now = AP_HAL::micros64(); - const struct log_GYR pkt{ - LOG_PACKET_HEADER_INIT(LOG_GYR_MSG), - time_us : now, - instance : instance, - sample_us : sample_us?sample_us:now, - GyrX : gyro.x, - GyrY : gyro.y, - GyrZ : gyro.z - }; - logger->WriteBlock(&pkt, sizeof(pkt)); + Write_GYR(instance, sample_us, gyro); } else { if (!_imu.batchsampler.doing_sensor_rate_logging()) { _imu.batchsampler.sample(instance, AP_InertialSensor::IMU_SENSOR_TYPE_GYRO, sample_us, gyro); @@ -454,17 +444,7 @@ void AP_InertialSensor_Backend::log_accel_raw(uint8_t instance, const uint64_t s return; } if (should_log_imu_raw()) { - uint64_t now = AP_HAL::micros64(); - const struct log_ACC pkt { - LOG_PACKET_HEADER_INIT(LOG_ACC_MSG), - time_us : now, - instance : instance, - sample_us : sample_us?sample_us:now, - AccX : accel.x, - AccY : accel.y, - AccZ : accel.z - }; - logger->WriteBlock(&pkt, sizeof(pkt)); + Write_ACC(instance, sample_us, accel); } else { if (!_imu.batchsampler.doing_sensor_rate_logging()) { _imu.batchsampler.sample(instance, AP_InertialSensor::IMU_SENSOR_TYPE_ACCEL, sample_us, accel); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h index 0c976d4f7f..9c5d44f8d8 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h @@ -342,4 +342,8 @@ private: void log_accel_raw(uint8_t instance, const uint64_t sample_us, const Vector3f &accel); void log_gyro_raw(uint8_t instance, const uint64_t sample_us, const Vector3f &gryo); + // logging + void Write_ACC(const uint8_t instance, const uint64_t sample_us, const Vector3f &accel) const; // Write ACC data packet: raw accel data + void Write_GYR(const uint8_t instance, const uint64_t sample_us, const Vector3f &gyro) const; // Write GYR data packet: raw gyro data + }; diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp index 606f8435e9..0c6bbb6e6d 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp @@ -3,6 +3,38 @@ #include +// Write ACC data packet: raw accel data +void AP_InertialSensor_Backend::Write_ACC(const uint8_t instance, const uint64_t sample_us, const Vector3f &accel) const +{ + const uint64_t now = AP_HAL::micros64(); + const struct log_ACC pkt { + LOG_PACKET_HEADER_INIT(LOG_ACC_MSG), + time_us : now, + instance : instance, + sample_us : sample_us?sample_us:now, + AccX : accel.x, + AccY : accel.y, + AccZ : accel.z + }; + AP::logger().WriteBlock(&pkt, sizeof(pkt)); +} + +// Write GYR data packet: raw gyro data +void AP_InertialSensor_Backend::Write_GYR(const uint8_t instance, const uint64_t sample_us, const Vector3f &gyro) const +{ + const uint64_t now = AP_HAL::micros64(); + const struct log_GYR pkt{ + LOG_PACKET_HEADER_INIT(LOG_GYR_MSG), + time_us : now, + instance : instance, + sample_us : sample_us?sample_us:now, + GyrX : gyro.x, + GyrY : gyro.y, + GyrZ : gyro.z + }; + AP::logger().WriteBlock(&pkt, sizeof(pkt)); +} + // Write IMU data packet: raw accel/gyro data void AP_InertialSensor::Write_IMU_instance(const uint64_t time_us, const uint8_t imu_instance) const { diff --git a/libraries/AP_InertialSensor/LogStructure.h b/libraries/AP_InertialSensor/LogStructure.h index a28616ea23..e672ba38cc 100644 --- a/libraries/AP_InertialSensor/LogStructure.h +++ b/libraries/AP_InertialSensor/LogStructure.h @@ -3,11 +3,45 @@ #include #define LOG_IDS_FROM_INERTIALSENSOR \ + LOG_ACC_MSG, \ + LOG_GYR_MSG, \ LOG_IMU_MSG, \ LOG_ISBH_MSG, \ LOG_ISBD_MSG, \ LOG_VIBE_MSG +// @LoggerMessage: ACC +// @Description: IMU accelerometer data +// @Field: TimeUS: Time since system startup +// @Field: I: accelerometer sensor instance number +// @Field: SampleUS: time since system startup this sample was taken +// @Field: AccX: acceleration along X axis +// @Field: AccY: acceleration along Y axis +// @Field: AccZ: acceleration along Z axis +struct PACKED log_ACC { + LOG_PACKET_HEADER; + uint64_t time_us; + uint8_t instance; + uint64_t sample_us; + float AccX, AccY, AccZ; +}; + +// @LoggerMessage: GYR +// @Description: IMU gyroscope data +// @Field: TimeUS: Time since system startup +// @Field: I: gyroscope sensor instance number +// @Field: SampleUS: time since system startup this sample was taken +// @Field: GyrX: measured rotation rate about X axis +// @Field: GyrY: measured rotation rate about Y axis +// @Field: GyrZ: measured rotation rate about Z axis +struct PACKED log_GYR { + LOG_PACKET_HEADER; + uint64_t time_us; + uint8_t instance; + uint64_t sample_us; + float GyrX, GyrY, GyrZ; +}; + // @LoggerMessage: IMU // @Description: Inertial Measurement Unit data // @Field: TimeUS: Time since system startup @@ -78,6 +112,10 @@ struct PACKED log_Vibe { }; #define LOG_STRUCTURE_FROM_INERTIALSENSOR \ + { LOG_ACC_MSG, sizeof(log_ACC), \ + "ACC", "QBQfff", "TimeUS,I,SampleUS,AccX,AccY,AccZ", "s#sooo", "F-F000" }, \ + { LOG_GYR_MSG, sizeof(log_GYR), \ + "GYR", "QBQfff", "TimeUS,I,SampleUS,GyrX,GyrY,GyrZ", "s#sEEE", "F-F000" }, \ { LOG_IMU_MSG, sizeof(log_IMU), \ "IMU", "QBffffffIIfBBHH", "TimeUS,I,GyrX,GyrY,GyrZ,AccX,AccY,AccZ,EG,EA,T,GH,AH,GHz,AHz", "s#EEEooo--O--zz", "F-000000-----00" }, \ { LOG_VIBE_MSG, sizeof(log_Vibe), \