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

AP_InertialSensor: Privatize Logging

This commit is contained in:
Josh Henderson 2021-01-22 00:31:09 -05:00 committed by Peter Barker
parent 3225ca8853
commit 9792202810
4 changed files with 201 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -281,6 +281,10 @@ public:
// indicate which bit in LOG_BITMASK indicates raw logging enabled
void set_log_raw_bit(uint32_t log_raw_bit) { _log_raw_bit = log_raw_bit; }
// Logging Functions
void Write_IMU() const;
void Write_Vibration() const;
// calculate vibration levels and check for accelerometer clipping (called by a backends)
void calc_vibration_and_clipping(uint8_t instance, const Vector3f &accel, float dt);
@ -403,6 +407,10 @@ public:
bool should_log(uint8_t instance, IMU_SENSOR_TYPE type);
void push_data_to_log();
// Logging functions
bool Write_ISBH(const float sample_rate_hz) const;
bool Write_ISBD() const;
uint64_t measurement_started_us;
bool initialised : 1;
@ -460,6 +468,9 @@ private:
// save gyro calibration values to eeprom
void _save_gyro_calibration();
// Logging function
void Write_IMU_instance(const uint64_t time_us, const uint8_t imu_instance) const;
// backend objects
AP_InertialSensor_Backend *_backends[INS_MAX_BACKENDS];

98
libraries/AP_InertialSensor/AP_InertialSensor_Logging.cpp Normal file
View File

@ -0,0 +1,98 @@
#include "AP_InertialSensor.h"
#include "AP_InertialSensor_Backend.h"
#include <AP_Logger/AP_Logger.h>
// 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
{
const Vector3f &gyro = get_gyro(imu_instance);
const Vector3f &accel = get_accel(imu_instance);
const struct log_IMU pkt{
LOG_PACKET_HEADER_INIT(LOG_IMU_MSG),
time_us : time_us,
instance: imu_instance,
gyro_x : gyro.x,
gyro_y : gyro.y,
gyro_z : gyro.z,
accel_x : accel.x,
accel_y : accel.y,
accel_z : accel.z,
gyro_error : get_gyro_error_count(imu_instance),
accel_error : get_accel_error_count(imu_instance),
temperature : get_temperature(imu_instance),
gyro_health : (uint8_t)get_gyro_health(imu_instance),
accel_health : (uint8_t)get_accel_health(imu_instance),
gyro_rate : get_gyro_rate_hz(imu_instance),
accel_rate : get_accel_rate_hz(imu_instance),
};
AP::logger().WriteBlock(&pkt, sizeof(pkt));
}
// Write IMU data packet for all instances
void AP_InertialSensor::Write_IMU() const
{
const uint64_t time_us = AP_HAL::micros64();
uint8_t n = MAX(get_accel_count(), get_gyro_count());
for (uint8_t i=0; i<n; i++) {
Write_IMU_instance(time_us, i);
}
}
// Write VIBE data packet for all instances
void AP_InertialSensor::Write_Vibration() const
{
const uint64_t time_us = AP_HAL::micros64();
for (uint8_t i = 0; i < INS_MAX_INSTANCES; i++) {
if (!use_accel(i)) {
continue;
}
const Vector3f vibration = 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 : get_accel_clip_count(i)
};
AP::logger().WriteBlock(&pkt, sizeof(pkt));
}
}
// Write information about a series of IMU readings to log:
bool AP_InertialSensor::BatchSampler::Write_ISBH(const float sample_rate_hz) const
{
const struct log_ISBH pkt{
LOG_PACKET_HEADER_INIT(LOG_ISBH_MSG),
time_us : AP_HAL::micros64(),
seqno : isb_seqnum,
sensor_type : (uint8_t)type,
instance : instance,
multiplier : multiplier,
sample_count : (uint16_t)_required_count,
sample_us : measurement_started_us,
sample_rate_hz : sample_rate_hz,
};
return AP::logger().WriteBlock_first_succeed(&pkt, sizeof(pkt));
}
// Write a series of IMU readings to log:
bool AP_InertialSensor::BatchSampler::Write_ISBD() const
{
struct log_ISBD pkt = {
LOG_PACKET_HEADER_INIT(LOG_ISBD_MSG),
time_us : AP_HAL::micros64(),
isb_seqno : isb_seqnum,
seqno : (uint16_t) (data_read_offset/samples_per_msg)
};
memcpy(pkt.x, &data_x[data_read_offset], sizeof(pkt.x));
memcpy(pkt.y, &data_y[data_read_offset], sizeof(pkt.y));
memcpy(pkt.z, &data_z[data_read_offset], sizeof(pkt.z));
return AP::logger().WriteBlock_first_succeed(&pkt, sizeof(pkt));
}

17
libraries/AP_InertialSensor/BatchSampler.cpp
View File

@ -209,27 +209,18 @@ void AP_InertialSensor::BatchSampler::push_data_to_log()
}
break;
}
if (!logger->Write_ISBH(isb_seqnum,
type,
instance,
multiplier,
_required_count,
measurement_started_us,
sample_rate)) {
if (!Write_ISBH(sample_rate)) {
// buffer full?
return;
}
isbh_sent = true;
}
// pack and send a data packet:
if (!logger->Write_ISBD(isb_seqnum,
data_read_offset/samples_per_msg,
&data_x[data_read_offset],
&data_y[data_read_offset],
&data_z[data_read_offset])) {
// pack a nd send a data packet:
if (!Write_ISBD()) {
// maybe later?!
return;
}
data_read_offset += samples_per_msg;
last_sent_ms = AP_HAL::millis();
if (data_read_offset >= _required_count) {

88
libraries/AP_InertialSensor/LogStructure.h Normal file
View File

@ -0,0 +1,88 @@
#pragma once
#include <AP_Logger/LogStructure.h>
#define LOG_IDS_FROM_INERTIALSENSOR \
LOG_IMU_MSG, \
LOG_ISBH_MSG, \
LOG_ISBD_MSG, \
LOG_VIBE_MSG
// @LoggerMessage: IMU
// @Description: Inertial Measurement Unit data
// @Field: TimeUS: Time since system startup
// @Field: I: IMU sensor instance number
// @Field: GyrX: measured rotation rate about X axis
// @Field: GyrY: measured rotation rate about Y axis
// @Field: GyrZ: measured rotation rate about Z axis
// @Field: AccX: acceleration along X axis
// @Field: AccY: acceleration along Y axis
// @Field: AccZ: acceleration along Z axis
// @Field: EG: gyroscope error count
// @Field: EA: accelerometer error count
// @Field: T: IMU temperature
// @Field: GH: gyroscope health
// @Field: AH: accelerometer health
// @Field: GHz: gyroscope measurement rate
// @Field: AHz: accelerometer measurement rate
struct PACKED log_IMU {
LOG_PACKET_HEADER;
uint64_t time_us;
uint8_t instance;
float gyro_x, gyro_y, gyro_z;
float accel_x, accel_y, accel_z;
uint32_t gyro_error, accel_error;
float temperature;
uint8_t gyro_health, accel_health;
uint16_t gyro_rate, accel_rate;
};
struct PACKED log_ISBH {
LOG_PACKET_HEADER;
uint64_t time_us;
uint16_t seqno;
uint8_t sensor_type; // e.g. GYRO or ACCEL
uint8_t instance;
uint16_t multiplier;
uint16_t sample_count;
uint64_t sample_us;
float sample_rate_hz;
};
static_assert(sizeof(log_ISBH) < 256, "log_ISBH is over-size");
struct PACKED log_ISBD {
LOG_PACKET_HEADER;
uint64_t time_us;
uint16_t isb_seqno;
uint16_t seqno; // seqno within isb_seqno
int16_t x[32];
int16_t y[32];
int16_t z[32];
};
static_assert(sizeof(log_ISBD) < 256, "log_ISBD is over-size");
// @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: Clip: Number of clipping events on 1st accelerometer
struct PACKED log_Vibe {
LOG_PACKET_HEADER;
uint64_t time_us;
uint8_t imu;
float vibe_x, vibe_y, vibe_z;
uint32_t clipping;
};
#define LOG_STRUCTURE_FROM_INERTIALSENSOR \
{ 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), \
"VIBE", "QBfffI", "TimeUS,IMU,VibeX,VibeY,VibeZ,Clip", "s#----", "F-----" }, \
{ LOG_ISBH_MSG, sizeof(log_ISBH), \
"ISBH", "QHBBHHQf", "TimeUS,N,type,instance,mul,smp_cnt,SampleUS,smp_rate", "s-----sz", "F-----F-" }, \
{ LOG_ISBD_MSG, sizeof(log_ISBD), \
"ISBD", "QHHaaa", "TimeUS,N,seqno,x,y,z", "s--ooo", "F--???" },