fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-15 23:33:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

purge vehicle_vision_position alias

Browse Source
This commit is contained in:
TSC21
2018-09-18 09:52:33 +02:00
committed by Lorenz Meier
parent 8f23a073a4
commit 183a63cce9
7 changed files with 2294 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/simulator/simulator.h
+2 -4
View File
@@ -244,8 +244,7 @@ private:
_gyro_pub(nullptr),
_mag_pub(nullptr),
_flow_pub(nullptr),
_vision_position_pub(nullptr),
_vision_attitude_pub(nullptr),
_visual_odometry_pub(nullptr),
_dist_pub(nullptr),
_battery_pub(nullptr),
_param_sub(-1),
@@ -323,8 +322,7 @@ private:
orb_advert_t _gyro_pub;
orb_advert_t _mag_pub;
orb_advert_t _flow_pub;
orb_advert_t _vision_position_pub;
orb_advert_t _vision_attitude_pub;
orb_advert_t _visual_odometry_pub;
orb_advert_t _dist_pub;
orb_advert_t _battery_pub;
orb_advert_t _irlock_report_pub;
src/modules/simulator/simulator_mavlink.cpp
+8 -17
View File
@@ -46,6 +46,7 @@
#include <conversion/rotation.h>
#include <mathlib/mathlib.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_odometry.h>
#include <limits>
@@ -1128,28 +1129,18 @@ int Simulator::publish_ev_topic(mavlink_vision_position_estimate_t *ev_mavlink)
{
uint64_t timestamp = hrt_absolute_time();
struct vehicle_local_position_s vision_position = {};
struct vehicle_odometry_s visual_odometry = {};
vision_position.timestamp = timestamp;
vision_position.x = ev_mavlink->x;
vision_position.y = ev_mavlink->y;
vision_position.z = ev_mavlink->z;
vision_position.xy_valid = true;
vision_position.z_valid = true;
vision_position.v_xy_valid = true;
vision_position.v_z_valid = true;
struct vehicle_attitude_s vision_attitude = {};
vision_attitude.timestamp = timestamp;
visual_odometry.timestamp = timestamp;
visual_odometry.x = ev_mavlink->x;
visual_odometry.y = ev_mavlink->y;
visual_odometry.z = ev_mavlink->z;
matrix::Quatf q(matrix::Eulerf(ev_mavlink->roll, ev_mavlink->pitch, ev_mavlink->yaw));
q.copyTo(vision_attitude.q);
q.copyTo(visual_odometry.q);
int inst = 0;
orb_publish_auto(ORB_ID(vehicle_vision_position), &_vision_position_pub, &vision_position, &inst, ORB_PRIO_HIGH);
orb_publish_auto(ORB_ID(vehicle_vision_attitude), &_vision_attitude_pub, &vision_attitude, &inst, ORB_PRIO_HIGH);
orb_publish_auto(ORB_ID(vehicle_visual_odometry), &_visual_odometry_pub, &visual_odometry, &inst, ORB_PRIO_HIGH);
return OK;
}