|
|
|
@ -2216,15 +2216,15 @@ void GCS_MAVLINK::log_vision_position_estimate_data(const uint64_t usec,
@@ -2216,15 +2216,15 @@ void GCS_MAVLINK::log_vision_position_estimate_data(const uint64_t usec,
|
|
|
|
|
const float yaw) |
|
|
|
|
{ |
|
|
|
|
DataFlash_Class::instance()->Log_Write("VISP", "TimeUS,RemTimeUS,PX,PY,PZ,Roll,Pitch,Yaw", |
|
|
|
|
"ssmmmrrr", "FF000000", "QQffffff", |
|
|
|
|
"ssmmmddh", "FF000000", "QQffffff", |
|
|
|
|
(uint64_t)AP_HAL::micros64(), |
|
|
|
|
(uint64_t)usec, |
|
|
|
|
(double)x, |
|
|
|
|
(double)y, |
|
|
|
|
(double)z, |
|
|
|
|
(double)roll, |
|
|
|
|
(double)pitch, |
|
|
|
|
(double)yaw); |
|
|
|
|
(double)(roll * RAD_TO_DEG), |
|
|
|
|
(double)(pitch * RAD_TO_DEG), |
|
|
|
|
(double)(yaw * RAD_TO_DEG)); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void GCS_MAVLINK::handle_att_pos_mocap(mavlink_message_t *msg) |
|
|
|
|