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

AP_InertialSensor: move GYR & ACC logging functions to Logging.cpp

This commit is contained in:
Josh Henderson 2021-04-29 23:28:37 -04:00 committed by Peter Barker
parent 2cd0099bbc
commit 2b20dcf128
4 changed files with 76 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
View File

@ -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);

4
libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
View File

@ -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
};

32
libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp
View File

@ -3,6 +3,38 @@
#include <AP_Logger/AP_Logger.h>
// 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
{

38
libraries/AP_InertialSensor/LogStructure.h
View File

@ -3,11 +3,45 @@
#include <AP_Logger/LogStructure.h>
#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), \