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

mavlink : use new vision estimate topic and fix stream name

Browse Source
This commit is contained in:
Kabir Mohammed
2017-02-15 21:48:33 +01:00
committed by Lorenz Meier
parent 294663854d
commit f43ee3a0f5
1 changed files with 19 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_messages.cpp
+19 -15
View File
@@ -86,7 +86,6 @@
#include <uORB/topics/vehicle_local_position_setpoint.h> #include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_rates_setpoint.h> #include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_status.h> #include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vision_position_estimate.h>
#include <uORB/topics/vtol_vehicle_status.h> #include <uORB/topics/vtol_vehicle_status.h>
#include <uORB/topics/wind_estimate.h> #include <uORB/topics/wind_estimate.h>
#include <uORB/topics/mount_orientation.h> #include <uORB/topics/mount_orientation.h>
@@ -1520,17 +1519,17 @@ protected:
} }
}; };
class MavlinkStreamVisionPositionNED : public MavlinkStream class MavlinkStreamVisionPositionEstimate : public MavlinkStream
{ {
public: public:
const char *get_name() const const char *get_name() const
{ {
return MavlinkStreamVisionPositionNED::get_name_static(); return MavlinkStreamVisionPositionEstimate::get_name_static();
} }
static const char *get_name_static() static const char *get_name_static()
{ {
return "VISION_POSITION_NED"; return "VISION_POSITION_ESTIMATE";
} }
static uint16_t get_id_static() static uint16_t get_id_static()
@@ -1545,7 +1544,7 @@ public:
static MavlinkStream *new_instance(Mavlink *mavlink) static MavlinkStream *new_instance(Mavlink *mavlink)
{ {
return new MavlinkStreamVisionPositionNED(mavlink); return new MavlinkStreamVisionPositionEstimate(mavlink);
} }
unsigned get_size() unsigned get_size()
@@ -1557,28 +1556,33 @@ private:
MavlinkOrbSubscription *_pos_sub; MavlinkOrbSubscription *_pos_sub;
uint64_t _pos_time; uint64_t _pos_time;
MavlinkOrbSubscription *_att_sub;
uint64_t _att_time;
/* do not allow top copying this class */ /* do not allow top copying this class */
MavlinkStreamVisionPositionNED(MavlinkStreamVisionPositionNED &); MavlinkStreamVisionPositionEstimate(MavlinkStreamVisionPositionEstimate &);
MavlinkStreamVisionPositionNED &operator = (const MavlinkStreamVisionPositionNED &); MavlinkStreamVisionPositionEstimate &operator = (const MavlinkStreamVisionPositionEstimate &);
protected: protected:
explicit MavlinkStreamVisionPositionNED(Mavlink *mavlink) : MavlinkStream(mavlink), explicit MavlinkStreamVisionPositionEstimate(Mavlink *mavlink) : MavlinkStream(mavlink),
_pos_sub(_mavlink->add_orb_subscription(ORB_ID(vision_position_estimate))), _pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_vision_position))),
_pos_time(0) _pos_time(0),
_att_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_vision_attitude))),
_att_time(0)
{} {}
void send(const hrt_abstime t) void send(const hrt_abstime t)
{ {
struct vision_position_estimate_s vpos; struct vehicle_local_position_s vpos = {};
memset(&vpos, 0, sizeof(vpos)); struct vehicle_attitude_s vatt = {};
if (_pos_sub->update(&_pos_time, &vpos)) { if (_pos_sub->update(&_pos_time, &vpos) || _att_sub->update(&_att_time, &vatt)) {
mavlink_vision_position_estimate_t vmsg; mavlink_vision_position_estimate_t vmsg;
vmsg.usec = vpos.timestamp; vmsg.usec = vpos.timestamp;
vmsg.x = vpos.x; vmsg.x = vpos.x;
vmsg.y = vpos.y; vmsg.y = vpos.y;
vmsg.z = vpos.z; vmsg.z = vpos.z;
math::Quaternion q(vpos.q); math::Quaternion q(vatt.q);
math::Vector<3> rpy = q.to_euler(); math::Vector<3> rpy = q.to_euler();
vmsg.roll = rpy(0); vmsg.roll = rpy(0);
vmsg.pitch = rpy(1); vmsg.pitch = rpy(1);
@@ -3934,7 +3938,7 @@ const StreamListItem *streams_list[] = {
new StreamListItem(&MavlinkStreamTimesync::new_instance, &MavlinkStreamTimesync::get_name_static, &MavlinkStreamTimesync::get_id_static), new StreamListItem(&MavlinkStreamTimesync::new_instance, &MavlinkStreamTimesync::get_name_static, &MavlinkStreamTimesync::get_id_static),
new StreamListItem(&MavlinkStreamGlobalPositionInt::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static, &MavlinkStreamGlobalPositionInt::get_id_static), new StreamListItem(&MavlinkStreamGlobalPositionInt::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static, &MavlinkStreamGlobalPositionInt::get_id_static),
new StreamListItem(&MavlinkStreamLocalPositionNED::new_instance, &MavlinkStreamLocalPositionNED::get_name_static, &MavlinkStreamLocalPositionNED::get_id_static), new StreamListItem(&MavlinkStreamLocalPositionNED::new_instance, &MavlinkStreamLocalPositionNED::get_name_static, &MavlinkStreamLocalPositionNED::get_id_static),
new StreamListItem(&MavlinkStreamVisionPositionNED::new_instance, &MavlinkStreamVisionPositionNED::get_name_static, &MavlinkStreamVisionPositionNED::get_id_static), new StreamListItem(&MavlinkStreamVisionPositionEstimate::new_instance, &MavlinkStreamVisionPositionEstimate::get_name_static, &MavlinkStreamVisionPositionEstimate::get_id_static),
new StreamListItem(&MavlinkStreamLocalPositionNEDCOV::new_instance, &MavlinkStreamLocalPositionNEDCOV::get_name_static, &MavlinkStreamLocalPositionNEDCOV::get_id_static), new StreamListItem(&MavlinkStreamLocalPositionNEDCOV::new_instance, &MavlinkStreamLocalPositionNEDCOV::get_name_static, &MavlinkStreamLocalPositionNEDCOV::get_id_static),
new StreamListItem(&MavlinkStreamEstimatorStatus::new_instance, &MavlinkStreamEstimatorStatus::get_name_static, &MavlinkStreamEstimatorStatus::get_id_static), new StreamListItem(&MavlinkStreamEstimatorStatus::new_instance, &MavlinkStreamEstimatorStatus::get_name_static, &MavlinkStreamEstimatorStatus::get_id_static),
new StreamListItem(&MavlinkStreamAttPosMocap::new_instance, &MavlinkStreamAttPosMocap::get_name_static, &MavlinkStreamAttPosMocap::get_id_static), new StreamListItem(&MavlinkStreamAttPosMocap::new_instance, &MavlinkStreamAttPosMocap::get_name_static, &MavlinkStreamAttPosMocap::get_id_static),