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

GCS_MAVLink: fix VISP logging 

log VISP messages for the ATT_POS_MOCAP mavlink message as well
This commit is contained in:
Vince Kurtz 2018-04-10 17:07:24 -04:00 committed by Randy Mackay
parent 9480572075
commit 2f81747112
2 changed files with 27 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/GCS_MAVLink/GCS.h
View File

@ -457,6 +457,13 @@ private:
const float roll, const float roll,
const float pitch, const float pitch,
const float yaw); const float yaw);
void _log_vision_position_estimate_data(const uint64_t usec,
const float x,
const float y,
const float z,
const float roll,
const float pitch,
const float yaw);
void push_deferred_messages(); void push_deferred_messages();
void lock_channel(mavlink_channel_t chan, bool lock); void lock_channel(mavlink_channel_t chan, bool lock);

22
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -1950,10 +1950,20 @@ void GCS_MAVLINK::_handle_common_vision_position_estimate_data(const uint64_t us
timestamp_ms, timestamp_ms,
reset_timestamp_ms); reset_timestamp_ms);
// log data _log_vision_position_estimate_data(usec, x, y, z, roll, pitch, yaw);
}
void GCS_MAVLINK::_log_vision_position_estimate_data(const uint64_t usec,
const float x,
const float y,
const float z,
const float roll,
const float pitch,
const float yaw)
{
DataFlash_Class::instance()->Log_Write("VISP", "TimeUS,PX,PY,PZ,Roll,Pitch,Yaw", DataFlash_Class::instance()->Log_Write("VISP", "TimeUS,PX,PY,PZ,Roll,Pitch,Yaw",
"smmmrrr", "F000000", "Qffffff", "smmmrrr", "F000000", "Qffffff",
(uint64_t)timestamp_ms * 1000, (uint64_t)usec,
(double)x, (double)x,
(double)y, (double)y,
(double)z, (double)z,
@ -1987,6 +1997,14 @@ void GCS_MAVLINK::handle_att_pos_mocap(mavlink_message_t *msg)
angErr, angErr,
timestamp_ms, timestamp_ms,
reset_timestamp_ms); reset_timestamp_ms);
// calculate euler orientation for logging
float roll;
float pitch;
float yaw;
attitude.to_euler(roll, pitch, yaw);
_log_vision_position_estimate_data(m.time_usec, m.x, m.y, m.z, roll, pitch, yaw);
} }
void GCS_MAVLINK::handle_command_ack(const mavlink_message_t* msg) void GCS_MAVLINK::handle_command_ack(const mavlink_message_t* msg)