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

AP_ExternalAHRS: added EAHRS_LOG_RATE and common logging 

common logging for all EAHRS backends
This commit is contained in:
Andrew Tridgell 2023-12-02 19:32:26 +11:00 committed by Peter Barker
parent 280bc3a285
commit cb53e64293
4 changed files with 47 additions and 57 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
libraries/AP_ExternalAHRS/AP_ExternalAHRS.cpp
View File

@ -29,6 +29,7 @@
#include <GCS_MAVLink/GCS.h>
#include <AP_AHRS/AP_AHRS.h>
#include <AP_Logger/AP_Logger.h>
extern const AP_HAL::HAL &hal;
@ -80,6 +81,13 @@ const AP_Param::GroupInfo AP_ExternalAHRS::var_info[] = {
// @Bitmask: 0:GPS,1:IMU,2:Baro,3:Compass
// @User: Advanced
AP_GROUPINFO("_SENSORS", 4, AP_ExternalAHRS, sensors, 0xF),
// @Param: _LOG_RATE
// @DisplayName: AHRS logging rate
// @Description: Logging rate for EARHS devices
// @Units: Hz
// @User: Standard
AP_GROUPINFO("_LOG_RATE", 5, AP_ExternalAHRS, log_rate, 10),
AP_GROUPEND
};
@ -281,6 +289,39 @@ void AP_ExternalAHRS::update(void)
state.have_origin = true;
}
}
const uint32_t now_ms = AP_HAL::millis();
if (log_rate.get() > 0 && now_ms - last_log_ms >= uint32_t(1000U/log_rate.get())) {
last_log_ms = now_ms;
// @LoggerMessage: EAHR
// @Description: External AHRS data
// @Field: TimeUS: Time since system startup
// @Field: Roll: euler roll
// @Field: Pitch: euler pitch
// @Field: Yaw: euler yaw
// @Field: VN: velocity north
// @Field: VE: velocity east
// @Field: VD: velocity down
// @Field: Lat: latitude
// @Field: Lon: longitude
// @Field: Alt: altitude AMSL
// @Field: Flg: nav status flags
float roll, pitch, yaw;
state.quat.to_euler(roll, pitch, yaw);
nav_filter_status filterStatus {};
get_filter_status(filterStatus);
AP::logger().WriteStreaming("EAHR", "TimeUS,Roll,Pitch,Yaw,VN,VE,VD,Lat,Lon,Alt,Flg",
"sdddnnnDUm-",
"F000000GG0-",
"QffffffLLfI",
AP_HAL::micros64(),
degrees(roll), degrees(pitch), degrees(yaw),
state.velocity.x, state.velocity.y, state.velocity.z,
state.location.lat, state.location.lng, state.location.alt*0.01,
filterStatus.value);
}
}
// Get model/type name

3
libraries/AP_ExternalAHRS/AP_ExternalAHRS.h
View File

@ -176,6 +176,7 @@ private:
AP_Enum<DevType> devtype;
AP_Int16 rate;
AP_Int16 log_rate;
AP_Int16 options;
AP_Int16 sensors;
@ -190,6 +191,8 @@ private:
void set_default_sensors(uint16_t _sensors) {
sensors.set_default(_sensors);
}
uint32_t last_log_ms;
};
namespace AP {

28
libraries/AP_ExternalAHRS/AP_ExternalAHRS_InertialLabs.cpp
View File

@ -501,28 +501,6 @@ bool AP_ExternalAHRS_InertialLabs::check_uart()
// @LoggerMessage: ILB1
// @Description: InertialLabs AHRS data1
// @Field: TimeUS: Time since system startup
// @Field: Roll: euler roll
// @Field: Pitch: euler pitch
// @Field: Yaw: euler yaw
// @Field: VN: velocity north
// @Field: VE: velocity east
// @Field: VD: velocity down
// @Field: Lat: latitude
// @Field: Lon: longitude
// @Field: Alt: altitude AMSL
AP::logger().WriteStreaming("ILB1", "TimeUS,Roll,Pitch,Yaw,VN,VE,VD,Lat,Lon,Alt",
"sdddnnnDUm",
"F000000GG0",
"QffffffLLf",
now_us,
degrees(roll), degrees(pitch), degrees(yaw),
state.velocity.x, state.velocity.y, state.velocity.z,
state.location.lat, state.location.lng, state.location.alt*0.01);
// @LoggerMessage: ILB2
// @Description: InertialLabs AHRS data2
// @Field: TimeUS: Time since system startup
// @Field: PosVarN: position variance north
// @Field: PosVarE: position variance east
// @Field: PosVarD: position variance down
@ -530,7 +508,7 @@ bool AP_ExternalAHRS_InertialLabs::check_uart()
// @Field: VelVarE: velocity variance east
// @Field: VelVarD: velocity variance down
AP::logger().WriteStreaming("ILB2", "TimeUS,PosVarN,PosVarE,PosVarD,VelVarN,VelVarE,VelVarD",
AP::logger().WriteStreaming("ILB1", "TimeUS,PosVarN,PosVarE,PosVarD,VelVarN,VelVarE,VelVarD",
"smmmnnn",
"F000000",
"Qffffff",
@ -538,7 +516,7 @@ bool AP_ExternalAHRS_InertialLabs::check_uart()
state2.kf_pos_covariance.x, state2.kf_pos_covariance.x, state2.kf_pos_covariance.z,
state2.kf_vel_covariance.x, state2.kf_vel_covariance.x, state2.kf_vel_covariance.z);
// @LoggerMessage: ILB3
// @LoggerMessage: ILB2
// @Description: InertialLabs AHRS data3
// @Field: TimeUS: Time since system startup
// @Field: Stat1: unit status1
@ -553,7 +531,7 @@ bool AP_ExternalAHRS_InertialLabs::check_uart()
// @Field: WVE: Wind velocity east
// @Field: WVD: Wind velocity down
AP::logger().WriteStreaming("ILB3", "TimeUS,Stat1,Stat2,FType,SpStat,GI1,GI2,GJS,TAS,WVN,WVE,WVD",
AP::logger().WriteStreaming("ILB2", "TimeUS,Stat1,Stat2,FType,SpStat,GI1,GI2,GJS,TAS,WVN,WVE,WVD",
"s-----------",
"F-----------",
"QHHBBBBBffff",

32
libraries/AP_ExternalAHRS/AP_ExternalAHRS_VectorNav.cpp
View File

@ -511,38 +511,6 @@ void AP_ExternalAHRS_VectorNav::process_packet1(const uint8_t *b)
AP::ins().handle_external(ins);
}
#if HAL_LOGGING_ENABLED
// @LoggerMessage: EAH1
// @Description: External AHRS data
// @Field: TimeUS: Time since system startup
// @Field: Roll: euler roll
// @Field: Pitch: euler pitch
// @Field: Yaw: euler yaw
// @Field: VN: velocity north
// @Field: VE: velocity east
// @Field: VD: velocity down
// @Field: Lat: latitude
// @Field: Lon: longitude
// @Field: Alt: altitude AMSL
// @Field: UXY: uncertainty in XY position
// @Field: UV: uncertainty in velocity
// @Field: UR: uncertainty in roll
// @Field: UP: uncertainty in pitch
// @Field: UY: uncertainty in yaw
AP::logger().WriteStreaming("EAH1", "TimeUS,Roll,Pitch,Yaw,VN,VE,VD,Lat,Lon,Alt,UXY,UV,UR,UP,UY",
"sdddnnnDUmmnddd", "F000000GG000000",
"QffffffLLffffff",
AP_HAL::micros64(),
pkt1.ypr[2], pkt1.ypr[1], pkt1.ypr[0],
pkt1.velNED[0], pkt1.velNED[1], pkt1.velNED[2],
int32_t(pkt1.positionLLA[0]*1.0e7), int32_t(pkt1.positionLLA[1]*1.0e7),
float(pkt1.positionLLA[2]),
pkt1.posU, pkt1.velU,
pkt1.yprU[2], pkt1.yprU[1], pkt1.yprU[0]);
#endif // HAL_LOGGING_ENABLED
}
/*