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

AP_Logger: add VISP message

This commit is contained in:
Randy Mackay 2020-03-26 13:00:30 +09:00
parent 63320f140a
commit f18be824ea
3 changed files with 48 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
libraries/AP_Logger/AP_Logger.h
View File

@ -281,6 +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);
void Write_AOA_SSA(AP_AHRS &ahrs);
void Write_Beacon(AP_Beacon &beacon);
void Write_Proximity(AP_Proximity &proximity);

18
libraries/AP_Logger/LogFile.cpp
View File

@ -914,6 +914,24 @@ void AP_Logger::Write_VisualOdom(float time_delta, const Vector3f &angle_delta,
WriteBlock(&pkt_visualodom, sizeof(log_VisualOdom));
}
// 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)
{
const struct log_VisualPosition pkt_visualpos {
LOG_PACKET_HEADER_INIT(LOG_VISUALPOS_MSG),
time_us : AP_HAL::micros64(),
remote_time_us : remote_time_us,
time_ms : time_ms,
pos_x : x,
pos_y : y,
pos_z : z,
roll : roll,
pitch : pitch,
yaw : yaw
};
WriteBlock(&pkt_visualpos, sizeof(log_VisualPosition));
}
// Write AOA and SSA
void AP_Logger::Write_AOA_SSA(AP_AHRS &ahrs)
{

29
libraries/AP_Logger/LogStructure.h
View File

@ -650,6 +650,20 @@ struct PACKED log_VisualOdom {
float confidence;
};
// visual position data
struct PACKED log_VisualPosition {
LOG_PACKET_HEADER;
uint64_t time_us;
uint64_t remote_time_us;
uint32_t time_ms;
float pos_x;
float pos_y;
float pos_z;
float roll; // degrees
float pitch; // degrees
float yaw; // degrees
};
struct PACKED log_ekfBodyOdomDebug {
LOG_PACKET_HEADER;
uint64_t time_us;
@ -1874,6 +1888,18 @@ struct PACKED log_Arm_Disarm {
// @Field: Clip1: Number of clipping events on 2nd accelerometer
// @Field: Clip2: Number of clipping events on 3rd accelerometer
// @LoggerMessage: VISP
// @Description: Vision Position
// @Field: TimeUS: System time
// @Field: RemTimeUS: Remote system time
// @Field: CTimeMS: Corrected system time
// @Field: PX: Position X-axis (North-South)
// @Field: PY: Position Y-axis (East-West)
// @Field: PZ: Position Z-axis (Down-Up)
// @Field: Roll: Roll lean angle
// @Field: Pitch: Pitch lean angle
// @Field: Yaw: Yaw angle
// messages for all boards
#define LOG_BASE_STRUCTURES \
{ LOG_FORMAT_MSG, sizeof(log_Format), \
@ -2094,6 +2120,8 @@ struct PACKED log_Arm_Disarm {
"MAV", "QBHHHBH", "TimeUS,chan,txp,rxp,rxdp,flags,ss", "s#----s", "F-000-C" }, \
{ 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", "QQIffffff", "TimeUS,RemTimeUS,CTimeMS,PX,PY,PZ,Roll,Pitch,Yaw", "sssmmmddh", "FFC000000" }, \
{ LOG_OPTFLOW_MSG, sizeof(log_Optflow), \
"OF", "QBffff", "TimeUS,Qual,flowX,flowY,bodyX,bodyY", "s-EEnn", "F-0000" }, \
{ LOG_WHEELENCODER_MSG, sizeof(log_WheelEncoder), \
@ -2239,6 +2267,7 @@ enum LogMessages : uint8_t {
LOG_RATE_MSG,
LOG_RALLY_MSG,
LOG_VISUALODOM_MSG,
LOG_VISUALPOS_MSG,
LOG_AOA_SSA_MSG,
LOG_BEACON_MSG,
LOG_PROXIMITY_MSG,