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

Merge remote-tracking branch 'upstream/master' into ros_messagelayer_merge2_attctrl_posctrl

Browse Source 
Conflicts:
	src/drivers/px4fmu/fmu.cpp
This commit is contained in:
Thomas Gubler
2015-02-01 11:06:47 +01:00
parent 7c958a2cca a2a244584e
commit 84ff3c671d
63 changed files with 807 additions and 484 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_main.cpp
+3 -3
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);
src/modules/mavlink/mavlink_main.h
+1 -1
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();
src/modules/mavlink/mavlink_messages.cpp
+2 -9
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)
{}
src/modules/mavlink/mavlink_orb_subscription.cpp
+9 -2
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)
{
src/modules/mavlink/mavlink_orb_subscription.h
+3 -1
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
src/modules/mavlink/mavlink_receiver.cpp
+11 -10
View File
@@ -1043,10 +1043,10 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
gyro.temperature = imu.temperature;
if (_gyro_pub < 0) {
_gyro_pub = orb_advertise(ORB_ID(sensor_gyro0), &gyro);
_gyro_pub = orb_advertise(ORB_ID(sensor_gyro), &gyro);
} else {
orb_publish(ORB_ID(sensor_gyro0), _gyro_pub, &gyro);
orb_publish(ORB_ID(sensor_gyro), _gyro_pub, &gyro);
}
}
@@ -1065,10 +1065,10 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
accel.temperature = imu.temperature;
if (_accel_pub < 0) {
_accel_pub = orb_advertise(ORB_ID(sensor_accel0), &accel);
_accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel);
} else {
orb_publish(ORB_ID(sensor_accel0), _accel_pub, &accel);
orb_publish(ORB_ID(sensor_accel), _accel_pub, &accel);
}
}
@@ -1086,10 +1086,11 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
mag.z = imu.zmag;
if (_mag_pub < 0) {
_mag_pub = orb_advertise(ORB_ID(sensor_mag0), &mag);
/* publish to the first mag topic */
_mag_pub = orb_advertise(ORB_ID(sensor_mag), &mag);
} else {
orb_publish(ORB_ID(sensor_mag0), _mag_pub, &mag);
orb_publish(ORB_ID(sensor_mag), _mag_pub, &mag);
}
}
@@ -1104,10 +1105,10 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
baro.temperature = imu.temperature;
if (_baro_pub < 0) {
_baro_pub = orb_advertise(ORB_ID(sensor_baro0), &baro);
_baro_pub = orb_advertise(ORB_ID(sensor_baro), &baro);
} else {
orb_publish(ORB_ID(sensor_baro0), _baro_pub, &baro);
orb_publish(ORB_ID(sensor_baro), _baro_pub, &baro);
}
}
@@ -1394,10 +1395,10 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
accel.temperature = 25.0f;
if (_accel_pub < 0) {
_accel_pub = orb_advertise(ORB_ID(sensor_accel0), &accel);
_accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel);
} else {
orb_publish(ORB_ID(sensor_accel0), _accel_pub, &accel);
orb_publish(ORB_ID(sensor_accel), _accel_pub, &accel);
}
}