Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-05 23:43:58 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Logger: Privatize AP_InertialSensor Logging

Browse Source
This commit is contained in:
Josh Henderson 2021-02-09 14:05:03 -05:00 committed by Peter Barker
parent 9792202810
commit 859fc73ba3
4 changed files with 21 additions and 233 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

75
libraries/AP_Logger/AP_Logger.cpp
View File

@ -692,6 +692,20 @@ void AP_Logger::WriteBlock(const void *pBuffer, uint16_t size) {
FOR_EACH_BACKEND(WriteBlock(pBuffer, size));
}
// only the first backend write need succeed for us to be successful
bool AP_Logger::WriteBlock_first_succeed(const void *pBuffer, uint16_t size)
{
if (_next_backend == 0) {
return false;
}
for (uint8_t i=1; i<_next_backend; i++) {
backends[i]->WriteBlock(pBuffer, size);
}
return backends[0]->WriteBlock(pBuffer, size);
}
// write a replay block. This differs from other as it returns false if a backend doesn't
// have space for the msg
bool AP_Logger::WriteReplayBlock(uint8_t msg_id, const void *pBuffer, uint16_t size) {
@ -1298,67 +1312,6 @@ void AP_Logger::start_io_thread(void)
#undef FOR_EACH_BACKEND
// Write information about a series of IMU readings to log:
bool AP_Logger::Write_ISBH(const uint16_t seqno,
const AP_InertialSensor::IMU_SENSOR_TYPE sensor_type,
const uint8_t sensor_instance,
const uint16_t mult,
const uint16_t sample_count,
const uint64_t sample_us,
const float sample_rate_hz)
{
if (_next_backend == 0) {
return false;
}
const struct log_ISBH pkt{
LOG_PACKET_HEADER_INIT(LOG_ISBH_MSG),
time_us : AP_HAL::micros64(),
seqno : seqno,
sensor_type : (uint8_t)sensor_type,
instance : sensor_instance,
multiplier : mult,
sample_count : sample_count,
sample_us : sample_us,
sample_rate_hz : sample_rate_hz,
};
// only the first backend need succeed for us to be successful
for (uint8_t i=1; i<_next_backend; i++) {
backends[i]->WriteBlock(&pkt, sizeof(pkt));
}
return backends[0]->WriteBlock(&pkt, sizeof(pkt));
}
// Write a series of IMU readings to log:
bool AP_Logger::Write_ISBD(const uint16_t isb_seqno,
const uint16_t seqno,
const int16_t x[32],
const int16_t y[32],
const int16_t z[32])
{
if (_next_backend == 0) {
return false;
}
struct log_ISBD pkt = {
LOG_PACKET_HEADER_INIT(LOG_ISBD_MSG),
time_us : AP_HAL::micros64(),
isb_seqno : isb_seqno,
seqno : seqno
};
memcpy(pkt.x, x, sizeof(pkt.x));
memcpy(pkt.y, y, sizeof(pkt.y));
memcpy(pkt.z, z, sizeof(pkt.z));
// only the first backend need succeed for us to be successful
for (uint8_t i=1; i<_next_backend; i++) {
backends[i]->WriteBlock(&pkt, sizeof(pkt));
}
return backends[0]->WriteBlock(&pkt, sizeof(pkt));
}
// Wrote an event packet
void AP_Logger::Write_Event(LogEvent id)
{

21
libraries/AP_Logger/AP_Logger.h
View File

@ -69,7 +69,6 @@
#include <AP_AHRS/AP_AHRS_NavEKF.h>
#include <AP_Common/AP_Common.h>
#include <AP_Param/AP_Param.h>
#include <AP_InertialSensor/AP_InertialSensor.h>
#include <AP_Mission/AP_Mission.h>
#include <AP_RPM/AP_RPM.h>
#include <AP_Logger/LogStructure.h>
@ -77,7 +76,6 @@
#include <AP_Rally/AP_Rally.h>
#include <AP_Beacon/AP_Beacon.h>
#include <AP_Proximity/AP_Proximity.h>
#include <AP_InertialSensor/AP_InertialSensor_Backend.h>
#include <AP_Vehicle/ModeReason.h>
#include <stdint.h>
@ -265,6 +263,10 @@ public:
/* Write a block of data at current offset */
void WriteBlock(const void *pBuffer, uint16_t size);
/* Write block of data at current offset and return true if first backend succeeds*/
bool WriteBlock_first_succeed(const void *pBuffer, uint16_t size);
/* Write an *important* block of data at current offset */
void WriteCriticalBlock(const void *pBuffer, uint16_t size);
@ -289,20 +291,6 @@ public:
void Write_Event(LogEvent id);
void Write_Error(LogErrorSubsystem sub_system,
LogErrorCode error_code);
void Write_IMU();
bool Write_ISBH(uint16_t seqno,
AP_InertialSensor::IMU_SENSOR_TYPE sensor_type,
uint8_t instance,
uint16_t multiplier,
uint16_t sample_count,
uint64_t sample_us,
float sample_rate_hz);
bool Write_ISBD(uint16_t isb_seqno,
uint16_t seqno,
const int16_t x[32],
const int16_t y[32],
const int16_t z[32]);
void Write_Vibration();
void Write_RCIN(void);
void Write_RCOUT(void);
void Write_RSSI();
@ -509,7 +497,6 @@ private:
// state to help us not log unneccesary RCIN values:
bool seen_nonzero_rcin15_or_rcin16;
void Write_IMU_instance(uint64_t time_us, uint8_t imu_instance);
void Write_Compass_instance(uint64_t time_us, uint8_t mag_instance);
void backend_starting_new_log(const AP_Logger_Backend *backend);

62
libraries/AP_Logger/LogFile.cpp
View File

@ -207,68 +207,6 @@ void AP_Logger::Write_RSSI()
WriteBlock(&pkt, sizeof(pkt));
}
void AP_Logger::Write_IMU_instance(const uint64_t time_us, const uint8_t imu_instance)
{
const AP_InertialSensor &ins = AP::ins();
const Vector3f &gyro = ins.get_gyro(imu_instance);
const Vector3f &accel = ins.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 : ins.get_gyro_error_count(imu_instance),
accel_error : ins.get_accel_error_count(imu_instance),
temperature : ins.get_temperature(imu_instance),
gyro_health : (uint8_t)ins.get_gyro_health(imu_instance),
accel_health : (uint8_t)ins.get_accel_health(imu_instance),
gyro_rate : ins.get_gyro_rate_hz(imu_instance),
accel_rate : ins.get_accel_rate_hz(imu_instance),
};
WriteBlock(&pkt, sizeof(pkt));
}
// Write an raw accel/gyro data packet
void AP_Logger::Write_IMU()
{
const uint64_t time_us = AP_HAL::micros64();
const AP_InertialSensor &ins = AP::ins();
uint8_t n = MAX(ins.get_accel_count(), ins.get_gyro_count());
for (uint8_t i=0; i<n; i++) {
Write_IMU_instance(time_us, i);
}
}
void AP_Logger::Write_Vibration()
{
const AP_InertialSensor &ins = AP::ins();
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,
const MAV_RESULT result,
bool was_command_long)

96
libraries/AP_Logger/LogStructure.h
View File

@ -123,6 +123,7 @@ const struct MultiplierStructure log_Multipliers[] = {
#include <AP_GPS/LogStructure.h>
#include <AP_NavEKF/LogStructure.h>
#include <AP_BattMonitor/LogStructure.h>
#include <AP_InertialSensor/LogStructure.h>
#include <AP_AHRS/LogStructure.h>
#include <AP_Camera/LogStructure.h>
#include <AP_Baro/LogStructure.h>
@ -219,50 +220,6 @@ struct PACKED log_Message {
char msg[64];
};
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");
struct PACKED log_Vibe {
LOG_PACKET_HEADER;
uint64_t time_us;
uint8_t imu;
float vibe_x, vibe_y, vibe_z;
uint32_t clipping;
};
struct PACKED log_RCIN {
LOG_PACKET_HEADER;
uint64_t time_us;
@ -837,16 +794,6 @@ struct PACKED log_PSCZ {
// UNIT messages define units which can be referenced by FMTU messages
// FMTU messages associate types (e.g. centimeters/second/second) to FMT message fields
#define ISBH_LABELS "TimeUS,N,type,instance,mul,smp_cnt,SampleUS,smp_rate"
#define ISBH_FMT "QHBBHHQf"
#define ISBH_UNITS "s-----sz"
#define ISBH_MULTS "F-----F-"
#define ISBD_LABELS "TimeUS,N,seqno,x,y,z"
#define ISBD_FMT "QHHaaa"
#define ISBD_UNITS "s--ooo"
#define ISBD_MULTS "F--???"
#define PID_LABELS "TimeUS,Tar,Act,Err,P,I,D,FF,Dmod,SRate,Limit"
#define PID_FMT "QfffffffffB"
#define PID_UNITS "s----------"
@ -1036,24 +983,6 @@ struct PACKED log_PSCZ {
// @Field: GyrY: measured rotation rate about Y axis
// @Field: GyrZ: measured rotation rate about Z axis
// @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
// @LoggerMessage: LGR
// @Description: Landing gear information
// @Field: TimeUS: Time since system startup
@ -1385,15 +1314,6 @@ struct PACKED log_PSCZ {
// @Field: Id: character referenced by FMTU
// @Field: Label: Unit - SI where available
// @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
// @LoggerMessage: WENC
// @Description: Wheel encoder measurements
// @Field: TimeUS: Time since system startup
@ -1459,8 +1379,6 @@ struct PACKED log_PSCZ {
{ LOG_PARAMETER_MSG, sizeof(log_Parameter), \
"PARM", "QNf", "TimeUS,Name,Value", "s--", "F--" }, \
LOG_STRUCTURE_FROM_GPS \
{ 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_MESSAGE_MSG, sizeof(log_Message), \
"MSG", "QZ", "TimeUS,Message", "s-", "F-"}, \
{ LOG_RCIN_MSG, sizeof(log_RCIN), \
@ -1538,12 +1456,7 @@ LOG_STRUCTURE_FROM_CAMERA \
"PIDE", PID_FMT, PID_LABELS, PID_UNITS, PID_MULTS }, \
{ LOG_DSTL_MSG, sizeof(log_DSTL), \
"DSTL", "QBfLLeccfeffff", "TimeUS,Stg,THdg,Lat,Lng,Alt,XT,Travel,L1I,Loiter,Des,P,I,D", "s??DUm--------", "F??000--------" }, \
{ LOG_VIBE_MSG, sizeof(log_Vibe), \
"VIBE", "QBfffI", "TimeUS,IMU,VibeX,VibeY,VibeZ,Clip", "s#----", "F-----" }, \
{ LOG_ISBH_MSG, sizeof(log_ISBH), \
"ISBH",ISBH_FMT,ISBH_LABELS,ISBH_UNITS,ISBH_MULTS }, \
{ LOG_ISBD_MSG, sizeof(log_ISBD), \
"ISBD",ISBD_FMT,ISBD_LABELS, ISBD_UNITS, ISBD_MULTS }, \
LOG_STRUCTURE_FROM_INERTIALSENSOR \
LOG_STRUCTURE_FROM_DAL \
LOG_STRUCTURE_FROM_NAVEKF2 \
LOG_STRUCTURE_FROM_NAVEKF3 \
@ -1614,7 +1527,6 @@ enum LogMessages : uint8_t {
LOG_PARAMETER_MSG = 64,
LOG_IDS_FROM_NAVEKF2,
LOG_IDS_FROM_NAVEKF3,
LOG_IMU_MSG,
LOG_MESSAGE_MSG,
LOG_RCIN_MSG,
LOG_RCIN2_MSG,
@ -1645,6 +1557,7 @@ enum LogMessages : uint8_t {
LOG_FORMAT_MSG = 128, // this must remain #128
LOG_IDS_FROM_DAL,
LOG_IDS_FROM_INERTIALSENSOR,
LOG_ACC_MSG,
LOG_GYR_MSG,
@ -1656,7 +1569,6 @@ enum LogMessages : uint8_t {
LOG_PIDN_MSG,
LOG_PIDE_MSG,
LOG_DSTL_MSG,
LOG_VIBE_MSG,
LOG_RPM_MSG,
LOG_RFND_MSG,
LOG_MAV_STATS,
@ -1679,8 +1591,6 @@ enum LogMessages : uint8_t {
LOG_PROXIMITY_MSG,
LOG_DF_FILE_STATS,
LOG_SRTL_MSG,
LOG_ISBH_MSG,
LOG_ISBD_MSG,
LOG_PERFORMANCE_MSG,
LOG_OPTFLOW_MSG,
LOG_EVENT_MSG,