fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-18 15:19:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

simulator: add ODOMETRY Mavlink msg handler

Browse Source
This commit is contained in:
TSC21 2018-07-12 20:12:15 +01:00 committed by Lorenz Meier
parent cce36e69c8
commit 04dc6bc04a
2 changed files with 89 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/modules/simulator/simulator.h
View File

@ -1,6 +1,7 @@
/****************************************************************************
*
* Copyright (c) 2015 Mark Charlebois. All rights reserved.
* Copyright (c) 2018 PX4 Pro Dev Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -43,7 +44,9 @@
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_odometry.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/irlock_report.h>
@ -341,7 +344,8 @@ private:
// class methods
int publish_sensor_topics(mavlink_hil_sensor_t *imu);
int publish_flow_topic(mavlink_hil_optical_flow_t *flow);
int publish_ev_topic(mavlink_vision_position_estimate_t *ev_mavlink);
template<typename T>
int publish_odometry_topic(T *odom_mavlink);
int publish_distance_topic(mavlink_distance_sensor_t *dist);
#ifndef __PX4_QURT

99
src/modules/simulator/simulator_mavlink.cpp
View File

@ -2,6 +2,7 @@
*
* Copyright (c) 2015 Mark Charlebois. All rights reserved.
* Copyright (c) 2016 Anton Matosov. All rights reserved.
* Copyright (c) 2018 PX4 Pro Dev Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -45,8 +46,6 @@
#include <pthread.h>
#include <conversion/rotation.h>
#include <mathlib/mathlib.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_odometry.h>
#include <limits>
@ -393,10 +392,9 @@ void Simulator::handle_message(mavlink_message_t *msg, bool publish)
publish_flow_topic(&flow);
break;
case MAVLINK_MSG_ID_ODOMETRY:
case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE:
mavlink_vision_position_estimate_t ev;
mavlink_msg_vision_position_estimate_decode(msg, &ev);
publish_ev_topic(&ev);
publish_odometry_topic(msg);
break;
case MAVLINK_MSG_ID_DISTANCE_SENSOR:
@ -1125,22 +1123,93 @@ int Simulator::publish_flow_topic(mavlink_hil_optical_flow_t *flow_mavlink)
return OK;
}
int Simulator::publish_ev_topic(mavlink_vision_position_estimate_t *ev_mavlink)
template<typename T>
int Simulator::publish_odometry_topic(T *msg)
{
uint64_t timestamp = hrt_absolute_time();
struct vehicle_odometry_s visual_odometry = {};
struct vehicle_odometry_s odom = {};
visual_odometry.timestamp = timestamp;
visual_odometry.x = ev_mavlink->x;
visual_odometry.y = ev_mavlink->y;
visual_odometry.z = ev_mavlink->z;
odom.timestamp = timestamp;
matrix::Quatf q(matrix::Eulerf(ev_mavlink->roll, ev_mavlink->pitch, ev_mavlink->yaw));
q.copyTo(visual_odometry.q);
if (msg->msgid == MAVLINK_MSG_ID_ODOMETRY) {
mavlink_odometry_t odom_msg;
mavlink_msg_odometry_decode(msg, &odom_msg);
int inst = 0;
orb_publish_auto(ORB_ID(vehicle_visual_odometry), &_visual_odometry_pub, &visual_odometry, &inst, ORB_PRIO_HIGH);
/* Dcm rotation matrix from body frame to local NED frame */
matrix::Dcm<float> Rbl;
/* since odom.child_frame_id == MAV_FRAME_BODY_FRD, WRT to estimated vehicle body-fixed frame */
/* get quaternion from the msg quaternion itself and build DCM matrix from it */
/* No need to transform the covariance matrices since the non-diagonal values are all zero */
Rbl = matrix::Dcm<float>(matrix::Quatf(odom_msg.q)).I();
/* the linear velocities needs to be transformed to the local NED frame */
matrix::Vector3<float> linvel_local(Rbl * matrix::Vector3<float>(odom_msg.vx, odom_msg.vy, odom_msg.vz));
/* The position in the local NED frame */
odom.x = odom_msg.x;
odom.y = odom_msg.y;
odom.z = odom_msg.z;
/* The quaternion of the ODOMETRY msg represents a rotation from
* NED earth/local frame to XYZ body frame */
matrix::Quatf q(odom_msg.q[0], odom_msg.q[1], odom_msg.q[2], odom_msg.q[3]);
q.copyTo(odom.q);
/* The pose covariance URT */
for (size_t i = 0; i < 21; i++) {
odom.pose_covariance[i] = odom_msg.pose_covariance[i];
}
/* The velocity in the local NED frame */
odom.vx = linvel_local(0);
odom.vy = linvel_local(1);
odom.vz = linvel_local(2);
/* The angular velocity in the body-fixed frame */
odom.rollspeed = odom_msg.rollspeed;
odom.pitchspeed = odom_msg.pitchspeed;
odom.yawspeed = odom_msg.yawspeed;
/* The velocity covariance URT */
for (size_t i = 0; i < 21; i++) {
odom.velocity_covariance[i] = odom_msg.twist_covariance[i];
}
} else if (msg->msgid == MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE) {
mavlink_vision_position_estimate_t ev;
mavlink_msg_vision_position_estimate_decode(msg, &ev);
/* The position in the local NED frame */
odom.x = ev.x;
odom.y = ev.y;
odom.z = ev.z;
/* The euler angles of the VISUAL_POSITION_ESTIMATE msg represent a
* rotation from NED earth/local frame to XYZ body frame */
matrix::Quatf q(matrix::Eulerf(ev.roll, ev.pitch, ev.yaw));
q.copyTo(odom.q);
/* The pose covariance URT */
for (size_t i = 0; i < 21; i++) {
odom.pose_covariance[i] = ev.covariance[i];
}
/* The velocity in the local NED frame - unknown */
odom.vx = NAN;
odom.vy = NAN;
odom.vz = NAN;
/* The angular velocity in body-fixed frame - unknown */
odom.rollspeed = NAN;
odom.pitchspeed = NAN;
odom.yawspeed = NAN;
/* The velocity covariance URT - unknown */
odom.velocity_covariance[0] = NAN;
}
int instance_id = 0;
/** @note: frame_id == MAV_FRAME_VISION_NED) */
orb_publish_auto(ORB_ID(vehicle_visual_odometry), &_visual_odometry_pub, &odom, &instance_id, ORB_PRIO_HIGH);
return OK;
}