Skip to content

Commit

Permalink
VectorNav: rename dataflash log message names
Browse files Browse the repository at this point in the history
  • Loading branch information
weavVN committed Sep 10, 2024
1 parent c40422c commit 65943ad
Show file tree
Hide file tree
Showing 2 changed files with 17 additions and 17 deletions.
30 changes: 15 additions & 15 deletions libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -484,19 +484,19 @@ const char* AP_ExternalAHRS_VectorNav::get_name() const
}

// Input data struct for EAHA logging message, used by both AHRS mode and INS mode
struct AP_ExternalAHRS_VectorNav::EAHA {
struct AP_ExternalAHRS_VectorNav::VNAT {
uint64_t timeUs;
float quat[4];
float ypr[3];
float yprU[3];
};


void AP_ExternalAHRS_VectorNav::write_eaha(const EAHA& data_to_log) const {
void AP_ExternalAHRS_VectorNav::write_vnat(const VNAT& data_to_log) const {

#if HAL_LOGGING_ENABLED
// @LoggerMessage: EAHA
// @Description: External AHRS Attitude data
// @LoggerMessage: VNAT
// @Description: VectorNav Attitude data
// @Field: TimeUS: Time since system startup
// @Field: Q1: Attitude quaternion 1
// @Field: Q2: Attitude quaternion 2
Expand All @@ -509,7 +509,7 @@ void AP_ExternalAHRS_VectorNav::write_eaha(const EAHA& data_to_log) const {
// @Field: PU: Pitch uncertainty
// @Field: RU: Roll uncertainty

AP::logger().WriteStreaming("EAHA", "TimeUS,Q1,Q2,Q3,Q4,Yaw,Pitch,Roll,YU,PU,RU",
AP::logger().WriteStreaming("VNAT", "TimeUS,Q1,Q2,Q3,Q4,Yaw,Pitch,Roll,YU,PU,RU",
"s----dddddd", "F0000000000",
"Qffffffffff",
data_to_log.timeUs,
Expand Down Expand Up @@ -573,8 +573,8 @@ void AP_ExternalAHRS_VectorNav::process_imu_packet(const uint8_t *b)
}

#if HAL_LOGGING_ENABLED
// @LoggerMessage: EAHI
// @Description: External AHRS IMU data
// @LoggerMessage: VNIM
// @Description: VectorNav IMU data
// @Field: TimeUS: Time since system startup
// @Field: Temp: Temprature
// @Field: Pres: Pressure
Expand All @@ -588,7 +588,7 @@ void AP_ExternalAHRS_VectorNav::process_imu_packet(const uint8_t *b)
// @Field: GY: Rotation rate Y-axis
// @Field: GZ: Rotation rate Z-axis

AP::logger().WriteStreaming("EAHI", "TimeUS,Temp,Pres,MX,MY,MZ,AX,AY,AZ,GX,GY,GZ",
AP::logger().WriteStreaming("VNIM", "TimeUS,Temp,Pres,MX,MY,MZ,AX,AY,AZ,GX,GY,GZ",
"sdPGGGoooEEE", "F00000000000",
"Qfffffffffff",
AP_HAL::micros64(),
Expand All @@ -610,13 +610,13 @@ void AP_ExternalAHRS_VectorNav::process_ahrs_ekf_packet(const uint8_t *b) {
state.have_quaternion = true;

#if HAL_LOGGING_ENABLED
EAHA data_to_log;
VNAT data_to_log;
data_to_log.timeUs = AP_HAL::micros64();
memcpy(data_to_log.quat, pkt.quaternion, sizeof(pkt.quaternion));
memcpy(data_to_log.ypr, pkt.ypr, sizeof(pkt.ypr));
memcpy(data_to_log.yprU, pkt.yprU, sizeof(pkt.yprU));

write_eaha(data_to_log);
write_vnat(data_to_log);
#endif // HAL_LOGGING_ENABLED
}

Expand All @@ -638,16 +638,16 @@ void AP_ExternalAHRS_VectorNav::process_ins_ekf_packet(const uint8_t *b) {
state.have_location = true;

#if HAL_LOGGING_ENABLED
EAHA data_to_log;
VNAT data_to_log;
auto now = AP_HAL::micros64();
data_to_log.timeUs = now;
memcpy(data_to_log.quat, pkt.quaternion, sizeof(pkt.quaternion));
memcpy(data_to_log.ypr, pkt.ypr, sizeof(pkt.ypr));
memcpy(data_to_log.yprU, pkt.yprU, sizeof(pkt.yprU));
write_eaha(data_to_log);
write_vnat(data_to_log);

// @LoggerMessage: EAHK
// @Description: External AHRS INS Kalman Filter data
// @LoggerMessage: VNKF
// @Description: VectorNav INS Kalman Filter data
// @Field: TimeUS: Time since system startup
// @Field: InsStatus: VectorNav INS health status
// @Field: Lat: Latitude
Expand All @@ -659,7 +659,7 @@ void AP_ExternalAHRS_VectorNav::process_ins_ekf_packet(const uint8_t *b) {
// @Field: PosU: Filter estimated position uncertainty
// @Field: VelU: Filter estimated Velocity uncertainty

AP::logger().WriteStreaming("EAHK", "TimeUS,InsStatus,Lat,Lon,Alt,VelN,VelE,VelD,PosU,VelU",
AP::logger().WriteStreaming("VNKF", "TimeUS,InsStatus,Lat,Lon,Alt,VelN,VelE,VelD,PosU,VelU",
"s-ddmnnndn", "F000000000",
"QHdddfffff",
now,
Expand Down
4 changes: 2 additions & 2 deletions libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.h
Original file line number Diff line number Diff line change
Expand Up @@ -65,8 +65,8 @@ class AP_ExternalAHRS_VectorNav : public AP_ExternalAHRS_backend {

void run_command(const char *fmt, ...);

struct EAHA;
void write_eaha(const EAHA& data_to_log) const;
struct VNAT;
void write_vnat(const VNAT& data_to_log) const;
void process_imu_packet(const uint8_t *b);
void process_ahrs_ekf_packet(const uint8_t *b);
void process_ins_ekf_packet(const uint8_t *b);
Expand Down

0 comments on commit 65943ad

Please sign in to comment.