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

GCS_MAVLink: use AP_VisualOdom to handle vision-position-estimate messages

Browse Source
This commit is contained in:
Randy Mackay 2020-03-26 13:39:44 +09:00
parent aa720b0ae6
commit 728e8b45a9
2 changed files with 10 additions and 79 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/GCS_MAVLink/GCS.h
View File

@ -740,14 +740,6 @@ private:
const float pitch, const float pitch,
const float yaw, const float yaw,
const uint16_t payload_size); const uint16_t payload_size);
void log_vision_position_estimate_data(const uint64_t usec,
const uint32_t corrected_msec,
const float x,
const float y,
const float z,
const float roll,
const float pitch,
const float yaw);
void lock_channel(const mavlink_channel_t chan, bool lock); void lock_channel(const mavlink_channel_t chan, bool lock);

81
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -2977,52 +2977,12 @@ void GCS_MAVLINK::handle_common_vision_position_estimate_data(const uint64_t use
{ {
// correct offboard timestamp to be in local ms since boot // correct offboard timestamp to be in local ms since boot
uint32_t timestamp_ms = correct_offboard_timestamp_usec_to_ms(usec, payload_size); uint32_t timestamp_ms = correct_offboard_timestamp_usec_to_ms(usec, payload_size);
// sensor assumed to be at 0,0,0 body-frame; need parameters for this?
// or a new message
const Vector3f sensor_offset = {};
const Vector3f pos = {
x,
y,
z
};
Quaternion attitude;
attitude.from_euler(roll, pitch, yaw); // from_vector312?
const float posErr = 0; // parameter required?
const float angErr = 0; // parameter required?
const uint32_t reset_timestamp_ms = 0; // no data available
AP::ahrs().writeExtNavData(sensor_offset, AP_VisualOdom *visual_odom = AP::visualodom();
pos, if (visual_odom == nullptr) {
attitude, return;
posErr, }
angErr, visual_odom->handle_vision_position_estimate(usec, timestamp_ms, x, y, z, roll, pitch, yaw);
timestamp_ms,
reset_timestamp_ms);
log_vision_position_estimate_data(usec, timestamp_ms, x, y, z, roll, pitch, yaw);
}
void GCS_MAVLINK::log_vision_position_estimate_data(const uint64_t usec,
const uint32_t corrected_msec,
const float x,
const float y,
const float z,
const float roll,
const float pitch,
const float yaw)
{
AP::logger().Write("VISP", "TimeUS,RemTimeUS,CTimeMS,PX,PY,PZ,Roll,Pitch,Yaw",
"sssmmmddh", "FFC000000", "QQIffffff",
(uint64_t)AP_HAL::micros64(),
(uint64_t)usec,
corrected_msec,
(double)x,
(double)y,
(double)z,
(double)(roll * RAD_TO_DEG),
(double)(pitch * RAD_TO_DEG),
(double)(yaw * RAD_TO_DEG));
} }
void GCS_MAVLINK::handle_att_pos_mocap(const mavlink_message_t &msg) void GCS_MAVLINK::handle_att_pos_mocap(const mavlink_message_t &msg)
@ -3030,35 +2990,14 @@ void GCS_MAVLINK::handle_att_pos_mocap(const mavlink_message_t &msg)
mavlink_att_pos_mocap_t m; mavlink_att_pos_mocap_t m;
mavlink_msg_att_pos_mocap_decode(&msg, &m); mavlink_msg_att_pos_mocap_decode(&msg, &m);
// sensor assumed to be at 0,0,0 body-frame; need parameters for this?
const Vector3f sensor_offset = {};
const Vector3f pos = {
m.x,
m.y,
m.z
};
Quaternion attitude = Quaternion(m.q);
const float posErr = 0; // parameter required?
const float angErr = 0; // parameter required?
// correct offboard timestamp to be in local ms since boot // correct offboard timestamp to be in local ms since boot
uint32_t timestamp_ms = correct_offboard_timestamp_usec_to_ms(m.time_usec, PAYLOAD_SIZE(chan, ATT_POS_MOCAP)); uint32_t timestamp_ms = correct_offboard_timestamp_usec_to_ms(m.time_usec, PAYLOAD_SIZE(chan, ATT_POS_MOCAP));
const uint32_t reset_timestamp_ms = 0; // no data available
AP::ahrs().writeExtNavData(sensor_offset,
pos,
attitude,
posErr,
angErr,
timestamp_ms,
reset_timestamp_ms);
// calculate euler orientation for logging AP_VisualOdom *visual_odom = AP::visualodom();
float roll; if (visual_odom == nullptr) {
float pitch; return;
float yaw; }
attitude.to_euler(roll, pitch, yaw); visual_odom->handle_vision_position_estimate(m.time_usec, timestamp_ms, m.x, m.y, m.z, m.q);
log_vision_position_estimate_data(m.time_usec, timestamp_ms, 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)