aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

DataFlash: add visual odometry sensor logging

Browse Source
This commit is contained in:
Randy Mackay 2017-04-04 11:47:04 +09:00
parent d2c89443df
commit 0644314499
3 changed files with 37 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
libraries/DataFlash/DataFlash.h
View File

@ -149,6 +149,7 @@ public:
const AC_AttitudeControl &attitude_control,
const AC_PosControl &pos_control);
void Log_Write_Rally(const AP_Rally &rally);
void Log_Write_VisualOdom(float time_delta, const Vector3f &angle_delta, const Vector3f &position_delta, float confidence);
void Log_Write(const char *name, const char *labels, const char *fmt, ...);

18
libraries/DataFlash/LogFile.cpp
View File

@ -2126,3 +2126,21 @@ void DataFlash_Class::Log_Write_Rally(const AP_Rally &rally)
}
}
}
// Write visual odometry sensor data
void DataFlash_Class::Log_Write_VisualOdom(float time_delta, const Vector3f &angle_delta, const Vector3f &position_delta, float confidence)
{
struct log_VisualOdom pkt_visualodom = {
LOG_PACKET_HEADER_INIT(LOG_VISUALODOM_MSG),
time_us : AP_HAL::micros64(),
time_delta : time_delta,
angle_delta_x : angle_delta.x,
angle_delta_y : angle_delta.y,
angle_delta_z : angle_delta.z,
position_delta_x : position_delta.x,
position_delta_y : position_delta.y,
position_delta_z : position_delta.z,
confidence : confidence
};
WriteBlock(&pkt_visualodom, sizeof(log_VisualOdom));
}

19
libraries/DataFlash/LogStructure.h
View File

@ -418,6 +418,20 @@ struct PACKED log_RngBcnDebug {
int16_t posD; // Down position of receiver rel to EKF origin (cm)
};
// visual odometry sensor data
struct PACKED log_VisualOdom {
LOG_PACKET_HEADER;
uint64_t time_us;
float time_delta;
float angle_delta_x;
float angle_delta_y;
float angle_delta_z;
float position_delta_x;
float position_delta_y;
float position_delta_z;
float confidence;
};
struct PACKED log_ekfBodyOdomDebug {
LOG_PACKET_HEADER;
uint64_t time_us;
@ -1036,7 +1050,9 @@ Format characters in the format string for binary log messages
{ LOG_RATE_MSG, sizeof(log_Rate), \
"RATE", "Qffffffffffff", "TimeUS,RDes,R,ROut,PDes,P,POut,YDes,Y,YOut,ADes,A,AOut" }, \
{ LOG_RALLY_MSG, sizeof(log_Rally), \
"RALY", "QBBLLh", "TimeUS,Tot,Seq,Lat,Lng,Alt" }
"RALY", "QBBLLh", "TimeUS,Tot,Seq,Lat,Lng,Alt" }, \
{ LOG_VISUALODOM_MSG, sizeof(log_VisualOdom), \
"VISO", "Qffffffff", "TimeUS,dt,AngDX,AngDY,AngDZ,PosDX,PosDY,PosDZ,conf" }
// #if SBP_HW_LOGGING
#define LOG_SBP_STRUCTURES \
@ -1166,6 +1182,7 @@ enum LogMessages {
LOG_GIMBAL3_MSG,
LOG_RATE_MSG,
LOG_RALLY_MSG,
LOG_VISUALODOM_MSG,
};
enum LogOriginType {