fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Support topic groups in MAVLink subscription handling

Browse Source
This commit is contained in:
Lorenz Meier 2015-01-26 09:49:30 +01:00
parent 2f7a9eaf65
commit cbe3783d5e
5 changed files with 18 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/modules/mavlink/mavlink_main.cpp
View File

@ -904,20 +904,20 @@ Mavlink::send_statustext(unsigned char severity, const char *string)
mavlink_logbuffer_write(&_logbuffer, &logmsg);
}
MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic)
MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic, int instance)
{
/* check if already subscribed to this topic */
MavlinkOrbSubscription *sub;
LL_FOREACH(_subscriptions, sub) {
if (sub->get_topic() == topic) {
if (sub->get_topic() == topic && sub->get_instance() == instance) {
/* already subscribed */
return sub;
}
}
/* add new subscription */
MavlinkOrbSubscription *sub_new = new MavlinkOrbSubscription(topic);
MavlinkOrbSubscription *sub_new = new MavlinkOrbSubscription(topic, instance);
LL_APPEND(_subscriptions, sub_new);

2
src/modules/mavlink/mavlink_main.h
View File

@ -171,7 +171,7 @@ public:
void handle_message(const mavlink_message_t *msg);
MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic);
MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic, int instance=0);
int get_instance_id();

11
src/modules/mavlink/mavlink_messages.cpp
View File

@ -1342,14 +1342,7 @@ protected:
_act_sub(nullptr),
_act_time(0)
{
orb_id_t act_topics[] = {
ORB_ID(actuator_outputs_0),
ORB_ID(actuator_outputs_1),
ORB_ID(actuator_outputs_2),
ORB_ID(actuator_outputs_3)
};
_act_sub = _mavlink->add_orb_subscription(act_topics[N]);
_act_sub = _mavlink->add_orb_subscription(ORB_ID(actuator_outputs), N);
}
void send(const hrt_abstime t)
@ -1424,7 +1417,7 @@ protected:
_status_time(0),
_pos_sp_triplet_sub(_mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet))),
_pos_sp_triplet_time(0),
_act_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_outputs_0))),
_act_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_outputs))),
_act_time(0)
{}

11
src/modules/mavlink/mavlink_orb_subscription.cpp
View File

@ -46,10 +46,11 @@
#include "mavlink_orb_subscription.h"
MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic) :
MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic, int instance) :
next(nullptr),
_topic(topic),
_fd(orb_subscribe(_topic)),
_instance(instance),
_fd(orb_subscribe_multi(_topic, instance)),
_published(false)
{
}
@ -65,6 +66,12 @@ MavlinkOrbSubscription::get_topic() const
return _topic;
}
int
MavlinkOrbSubscription::get_instance() const
{
return _instance;
}
bool
MavlinkOrbSubscription::update(uint64_t *time, void* data)
{

4
src/modules/mavlink/mavlink_orb_subscription.h
View File

@ -50,7 +50,7 @@ class MavlinkOrbSubscription
public:
MavlinkOrbSubscription *next; ///< pointer to next subscription in list
MavlinkOrbSubscription(const orb_id_t topic);
MavlinkOrbSubscription(const orb_id_t topic, int instance);
~MavlinkOrbSubscription();
/**
@ -77,9 +77,11 @@ public:
*/
bool is_published();
orb_id_t get_topic() const;
int get_instance() const;
private:
const orb_id_t _topic; ///< topic metadata
const int _instance; ///< get topic instance
int _fd; ///< subscription handle
bool _published; ///< topic was ever published