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

DF driver wrappers: advertise with actual data

Browse Source
This commit is contained in:
Julian Oes
2016-05-31 16:43:13 +02:00
committed by Lorenz Meier
parent c170657dff
commit b0b7832048
2 changed files with 13 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/platforms/posix/drivers/df_hmc5883_wrapper/df_hmc5883_wrapper.cpp
+4 -11
View File
@@ -143,16 +143,6 @@ DfHmc9250Wrapper::~DfHmc9250Wrapper()
int DfHmc9250Wrapper::start() int DfHmc9250Wrapper::start()
{ {
// TODO: don't publish garbage here
mag_report mag_report = {};
_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mag_report,
&_mag_orb_class_instance, ORB_PRIO_DEFAULT);
if (_mag_topic == nullptr) {
PX4_ERR("sensor_mag advert fail");
return -1;
}
/* Subscribe to param update topic. */ /* Subscribe to param update topic. */
if (_param_update_sub < 0) { if (_param_update_sub < 0) {
_param_update_sub = orb_subscribe(ORB_ID(parameter_update)); _param_update_sub = orb_subscribe(ORB_ID(parameter_update));
@@ -301,7 +291,10 @@ int DfHmc9250Wrapper::_publish(struct mag_sensor_data &data)
// TODO: when is this ever blocked? // TODO: when is this ever blocked?
if (!(m_pub_blocked)) { if (!(m_pub_blocked)) {
if (_mag_topic != nullptr) { if (_mag_topic == nullptr) {
_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mag_report,
&_mag_orb_class_instance, ORB_PRIO_HIGH);
} else {
orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report); orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report);
} }
src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp
+9 -12
View File
@@ -259,16 +259,7 @@ int DfMpu9250Wrapper::start()
return -1; return -1;
} }
if (_mag_enabled == true) { if (_mag_enabled) {
// TODO: don't publish garbage here
mag_report mag_report = {};
_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mag_report,
&_mag_orb_class_instance, ORB_PRIO_DEFAULT);
if (_mag_topic == nullptr) {
PX4_ERR("sensor_mag advert fail");
return -1;
}
} }
/* Subscribe to param update topic. */ /* Subscribe to param update topic. */
@@ -707,8 +698,14 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report); orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
} }
if ((_mag_topic != nullptr) && (_mag_enabled == true)) { if (_mag_enabled) {
orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report);
if (_mag_topic == nullptr) {
_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mag_report,
&_mag_orb_class_instance, ORB_PRIO_LOW);
} else {
orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report);
}
} }
/* Notify anyone waiting for data. */ /* Notify anyone waiting for data. */