mirror of https://github.com/ArduPilot/ardupilot
AP_ExternalAHRS: added EAHRS_LOG_RATE and common logging
common logging for all EAHRS backends
This commit is contained in:
parent
280bc3a285
commit
cb53e64293
|
@ -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;
|
||||
|
||||
|
@ -81,6 +82,13 @@ const AP_Param::GroupInfo AP_ExternalAHRS::var_info[] = {
|
|||
// @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
|
||||
|
|
|
@ -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 {
|
||||
|
|
|
@ -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",
|
||||
|
|
|
@ -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
|
||||
}
|
||||
|
||||
/*
|
||||
|
|
Loading…
Reference in New Issue