Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-22 08:38:36 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Logger: log ext nav pos err and ang err

Browse Source
This commit is contained in:
chobits 2020-06-04 15:54:19 +08:00 committed by Randy Mackay
parent 5d271d1e04
commit 5e5a0d2111
3 changed files with 9 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_Logger/AP_Logger.h
View File

@ -281,7 +281,7 @@ public:
uint8_t sequence,
const RallyLocation &rally_point);
void Write_VisualOdom(float time_delta, const Vector3f &angle_delta, const Vector3f &position_delta, float confidence);
void Write_VisualPosition(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw, uint8_t reset_counter);
void Write_VisualPosition(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw, float pos_err, float ang_err, uint8_t reset_counter);
void Write_VisualVelocity(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, float vel_err, uint8_t reset_counter);
void Write_AOA_SSA(AP_AHRS &ahrs);
void Write_Beacon(AP_Beacon &beacon);

4
libraries/AP_Logger/LogFile.cpp
View File

@ -911,7 +911,7 @@ void AP_Logger::Write_VisualOdom(float time_delta, const Vector3f &angle_delta,
}
// Write visual position sensor data. x,y,z are in meters, angles are in degrees
void AP_Logger::Write_VisualPosition(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw, uint8_t reset_counter)
void AP_Logger::Write_VisualPosition(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw, float pos_err, float ang_err, uint8_t reset_counter)
{
const struct log_VisualPosition pkt_visualpos {
LOG_PACKET_HEADER_INIT(LOG_VISUALPOS_MSG),
@ -924,6 +924,8 @@ void AP_Logger::Write_VisualPosition(uint64_t remote_time_us, uint32_t time_ms,
roll : roll,
pitch : pitch,
yaw : yaw,
pos_err : pos_err,
ang_err : ang_err,
reset_counter : reset_counter
};
WriteBlock(&pkt_visualpos, sizeof(log_VisualPosition));

6
libraries/AP_Logger/LogStructure.h
View File

@ -625,6 +625,8 @@ struct PACKED log_VisualPosition {
float roll; // degrees
float pitch; // degrees
float yaw; // degrees
float pos_err;
float ang_err;
uint8_t reset_counter;
};
@ -2134,6 +2136,8 @@ struct PACKED log_Arm_Disarm {
// @Field: Roll: Roll lean angle
// @Field: Pitch: Pitch lean angle
// @Field: Yaw: Yaw angle
// @Field: PosErr: Position estimate error
// @Field: AngErr: Attitude estimate error
// @Field: ResetCnt: Position reset counter
// @LoggerMessage: VISV
@ -2504,7 +2508,7 @@ struct PACKED log_Arm_Disarm {
{ LOG_VISUALODOM_MSG, sizeof(log_VisualOdom), \
"VISO", "Qffffffff", "TimeUS,dt,AngDX,AngDY,AngDZ,PosDX,PosDY,PosDZ,conf", "ssrrrmmm-", "FF000000-" }, \
{ LOG_VISUALPOS_MSG, sizeof(log_VisualPosition), \
"VISP", "QQIffffffb", "TimeUS,RemTimeUS,CTimeMS,PX,PY,PZ,Roll,Pitch,Yaw,ResetCnt", "sssmmmddh-", "FFC000000-" }, \
"VISP", "QQIffffffffb", "TimeUS,RemTimeUS,CTimeMS,PX,PY,PZ,Roll,Pitch,Yaw,PosErr,AngErr,ResetCnt", "sssmmmddhmd-", "FFC00000000-" }, \
{ LOG_VISUALVEL_MSG, sizeof(log_VisualVelocity), \
"VISV", "QQIffffb", "TimeUS,RemTimeUS,CTimeMS,VX,VY,VZ,VErr,ResetCnt", "sssnnnn-", "FFC0000-" }, \
{ LOG_OPTFLOW_MSG, sizeof(log_Optflow), \