mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-20 07:53:35 +08:00
Fixed up init sequence of all sensors - we can publish in interrupt context, but not advertise! All advertisements now contain valid data
This commit is contained in:
@@ -153,6 +153,7 @@ private:
|
||||
float _accel_range_scale;
|
||||
float _accel_range_m_s2;
|
||||
orb_advert_t _accel_topic;
|
||||
int _class_instance;
|
||||
|
||||
unsigned _current_lowpass;
|
||||
unsigned _current_range;
|
||||
@@ -238,6 +239,7 @@ BMA180::BMA180(int bus, spi_dev_e device) :
|
||||
_accel_range_scale(0.0f),
|
||||
_accel_range_m_s2(0.0f),
|
||||
_accel_topic(-1),
|
||||
_class_instance(-1),
|
||||
_current_lowpass(0),
|
||||
_current_range(0),
|
||||
_sample_perf(perf_alloc(PC_ELAPSED, "bma180_read"))
|
||||
@@ -282,11 +284,6 @@ BMA180::init()
|
||||
if (_reports == nullptr)
|
||||
goto out;
|
||||
|
||||
/* advertise sensor topic */
|
||||
struct accel_report zero_report;
|
||||
memset(&zero_report, 0, sizeof(zero_report));
|
||||
_accel_topic = orb_advertise(ORB_ID(sensor_accel), &zero_report);
|
||||
|
||||
/* perform soft reset (p48) */
|
||||
write_reg(ADDR_RESET, SOFT_RESET);
|
||||
|
||||
@@ -322,6 +319,19 @@ BMA180::init()
|
||||
ret = ERROR;
|
||||
}
|
||||
|
||||
_class_instance = register_class_devname(ACCEL_DEVICE_PATH);
|
||||
|
||||
/* advertise sensor topic, measure manually to initialize valid report */
|
||||
measure();
|
||||
|
||||
if (_class_instance == CLASS_DEVICE_PRIMARY) {
|
||||
struct accel_report arp;
|
||||
_reports->get(&arp);
|
||||
|
||||
/* measurement will have generated a report, publish */
|
||||
_accel_topic = orb_advertise(ORB_ID(sensor_accel), &arp);
|
||||
}
|
||||
|
||||
out:
|
||||
return ret;
|
||||
}
|
||||
@@ -723,7 +733,7 @@ BMA180::measure()
|
||||
poll_notify(POLLIN);
|
||||
|
||||
/* publish for subscribers */
|
||||
if !(_pub_blocked)
|
||||
if (_accel_topic > 0 && !(_pub_blocked))
|
||||
orb_publish(ORB_ID(sensor_accel), _accel_topic, &report);
|
||||
|
||||
/* stop the perf counter */
|
||||
|
||||
Reference in New Issue
Block a user