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

mocap_support: added support for mocap data on firmware

Browse Source
This commit is contained in:
TSC21
2015-06-13 17:03:31 +01:00
parent c92655b850
commit dccd4df7bc
13 changed files with 225 additions and 90 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_messages.cpp
+27 -26
View File
@@ -54,7 +54,7 @@
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_vicon_position.h>
#include <uORB/topics/att_pos_mocap.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/position_setpoint_triplet.h>
@@ -1187,64 +1187,65 @@ protected:
};
class MavlinkStreamViconPositionEstimate : public MavlinkStream
class MavlinkStreamAttPosMocap : public MavlinkStream
{
public:
const char *get_name() const
{
return MavlinkStreamViconPositionEstimate::get_name_static();
return MavlinkStreamAttPosMocap::get_name_static();
}
static const char *get_name_static()
{
return "VICON_POSITION_ESTIMATE";
return "ATT_POS_MOCAP";
}
uint8_t get_id()
{
return MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE;
return MAVLINK_MSG_ID_ATT_POS_MOCAP;
}
static MavlinkStream *new_instance(Mavlink *mavlink)
{
return new MavlinkStreamViconPositionEstimate(mavlink);
return new MavlinkStreamAttPosMocap(mavlink);
}
unsigned get_size()
{
return MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
return MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
}
private:
MavlinkOrbSubscription *_pos_sub;
uint64_t _pos_time;
MavlinkOrbSubscription *_mocap_sub;
uint64_t _mocap_time;
/* do not allow top copying this class */
MavlinkStreamViconPositionEstimate(MavlinkStreamViconPositionEstimate &);
MavlinkStreamViconPositionEstimate& operator = (const MavlinkStreamViconPositionEstimate &);
MavlinkStreamAttPosMocap(MavlinkStreamAttPosMocap &);
MavlinkStreamAttPosMocap& operator = (const MavlinkStreamAttPosMocap &);
protected:
explicit MavlinkStreamViconPositionEstimate(Mavlink *mavlink) : MavlinkStream(mavlink),
_pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position))),
_pos_time(0)
explicit MavlinkStreamAttPosMocap(Mavlink *mavlink) : MavlinkStream(mavlink),
_mocap_sub(_mavlink->add_orb_subscription(ORB_ID(att_pos_mocap))),
_mocap_time(0)
{}
void send(const hrt_abstime t)
{
struct vehicle_vicon_position_s pos;
struct att_pos_mocap_s mocap;
if (_pos_sub->update(&_pos_time, &pos)) {
mavlink_vicon_position_estimate_t msg;
if (_mocap_sub->update(&_mocap_time, &mocap)) {
mavlink_att_pos_mocap_t msg;
msg.usec = pos.timestamp;
msg.x = pos.x;
msg.y = pos.y;
msg.z = pos.z;
msg.roll = pos.roll;
msg.pitch = pos.pitch;
msg.yaw = pos.yaw;
msg.time_usec = mocap.timestamp_boot;
msg.q[0] = mocap.q[0];
msg.q[1] = mocap.q[1];
msg.q[2] = mocap.q[2];
msg.q[3] = mocap.q[3];
msg.x = mocap.x;
msg.y = mocap.y;
msg.z = mocap.z;
_mavlink->send_message(MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, &msg);
_mavlink->send_message(MAVLINK_MSG_ID_ATT_POS_MOCAP, &msg);
}
}
};
@@ -2277,7 +2278,7 @@ const StreamListItem *streams_list[] = {
new StreamListItem(&MavlinkStreamTimesync::new_instance, &MavlinkStreamTimesync::get_name_static),
new StreamListItem(&MavlinkStreamGlobalPositionInt::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static),
new StreamListItem(&MavlinkStreamLocalPositionNED::new_instance, &MavlinkStreamLocalPositionNED::get_name_static),
new StreamListItem(&MavlinkStreamViconPositionEstimate::new_instance, &MavlinkStreamViconPositionEstimate::get_name_static),
new StreamListItem(&MavlinkStreamAttPosMocap::new_instance, &MavlinkStreamAttPosMocap::get_name_static),
new StreamListItem(&MavlinkStreamGPSGlobalOrigin::new_instance, &MavlinkStreamGPSGlobalOrigin::get_name_static),
new StreamListItem(&MavlinkStreamServoOutputRaw<0>::new_instance, &MavlinkStreamServoOutputRaw<0>::get_name_static),
new StreamListItem(&MavlinkStreamServoOutputRaw<1>::new_instance, &MavlinkStreamServoOutputRaw<1>::get_name_static),
src/modules/mavlink/mavlink_receiver.cpp
+25 -18
View File
@@ -118,7 +118,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_rates_sp_pub(nullptr),
_force_sp_pub(nullptr),
_pos_sp_triplet_pub(nullptr),
_vicon_position_pub(nullptr),
_att_pos_mocap_pub(nullptr),
_vision_position_pub(nullptr),
_telemetry_status_pub(nullptr),
_rc_pub(nullptr),
@@ -169,8 +169,8 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
handle_message_set_mode(msg);
break;
case MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE:
handle_message_vicon_position_estimate(msg);
case MAVLINK_MSG_ID_ATT_POS_MOCAP:
handle_message_att_pos_mocap(msg);
break;
case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED:
@@ -556,27 +556,34 @@ MavlinkReceiver::handle_message_distance_sensor(mavlink_message_t *msg)
}
void
MavlinkReceiver::handle_message_vicon_position_estimate(mavlink_message_t *msg)
MavlinkReceiver::handle_message_att_pos_mocap(mavlink_message_t *msg)
{
mavlink_vicon_position_estimate_t pos;
mavlink_msg_vicon_position_estimate_decode(msg, &pos);
mavlink_att_pos_mocap_t mocap;
mavlink_msg_att_pos_mocap_decode(msg, &mocap);
struct vehicle_vicon_position_s vicon_position;
memset(&vicon_position, 0, sizeof(vicon_position));
struct att_pos_mocap_s att_pos_mocap;
memset(&att_pos_mocap, 0, sizeof(att_pos_mocap));
vicon_position.timestamp = hrt_absolute_time();
vicon_position.x = pos.x;
vicon_position.y = pos.y;
vicon_position.z = pos.z;
vicon_position.roll = pos.roll;
vicon_position.pitch = pos.pitch;
vicon_position.yaw = pos.yaw;
// Use the component ID to identify the mocap system
att_pos_mocap.id = msg->compid;
if (_vicon_position_pub == nullptr) {
_vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position);
att_pos_mocap.timestamp_boot = hrt_absolute_time(); // Monotonic time
att_pos_mocap.timestamp_computer = sync_stamp(mocap.time_usec); // Synced time
att_pos_mocap.q[0] = mocap.q[0];
att_pos_mocap.q[1] = mocap.q[1];
att_pos_mocap.q[2] = mocap.q[2];
att_pos_mocap.q[3] = mocap.q[3];
att_pos_mocap.x = mocap.x;
att_pos_mocap.y = mocap.y;
att_pos_mocap.z = mocap.z;
if (_att_pos_mocap_pub == nullptr) {
_att_pos_mocap_pub = orb_advertise(ORB_ID(att_pos_mocap), &att_pos_mocap);
} else {
orb_publish(ORB_ID(vehicle_vicon_position), _vicon_position_pub, &vicon_position);
orb_publish(ORB_ID(att_pos_mocap), _att_pos_mocap_pub, &att_pos_mocap);
}
}
src/modules/mavlink/mavlink_receiver.h
+3 -3
View File
@@ -58,7 +58,7 @@
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_global_velocity_setpoint.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_vicon_position.h>
#include <uORB/topics/att_pos_mocap.h>
#include <uORB/topics/vision_position_estimate.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
@@ -119,7 +119,7 @@ private:
void handle_message_optical_flow_rad(mavlink_message_t *msg);
void handle_message_hil_optical_flow(mavlink_message_t *msg);
void handle_message_set_mode(mavlink_message_t *msg);
void handle_message_vicon_position_estimate(mavlink_message_t *msg);
void handle_message_att_pos_mocap(mavlink_message_t *msg);
void handle_message_vision_position_estimate(mavlink_message_t *msg);
void handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg);
void handle_message_set_position_target_local_ned(mavlink_message_t *msg);
@@ -174,7 +174,7 @@ private:
orb_advert_t _rates_sp_pub;
orb_advert_t _force_sp_pub;
orb_advert_t _pos_sp_triplet_pub;
orb_advert_t _vicon_position_pub;
orb_advert_t _att_pos_mocap_pub;
orb_advert_t _vision_position_pub;
orb_advert_t _telemetry_status_pub;
orb_advert_t _rc_pub;