From 74bc6870ed2a987de9edd987b84080c905b048ab Mon Sep 17 00:00:00 2001
From: Mark Sauder <mcsauder@gmail.com>
Date: Tue, 16 Apr 2019 00:29:53 -0600
Subject: [PATCH] simulator: uorb and initialization cleanup (#11825)
* Cherry pick the vehicle_imu PR #9756 src/modules/simulator directory work to submit as standalone PR.
* Modify the accelsim init method mag_report usage to match usage in measure() and mag_measure() methods in the class.
* Incorporate review comments in the accelsim.cpp init() method and also make the same modifications in gyrosim.cpp.
* Delete unneeded mag_report initialization from accelsim init() method.
* Deprecate unneeded measure() call from accelsim.cpp init() and gyrosim.cpp init().
---
src/modules/simulator/accelsim/accelsim.cpp | 94 ++++++-------------
.../airspeedsim/meas_airspeed_sim.cpp | 8 +-
src/modules/simulator/barosim/baro.cpp | 12 +--
src/modules/simulator/gyrosim/gyrosim.cpp | 62 ++----------
src/modules/simulator/simulator_mavlink.cpp | 6 +-
5 files changed, 43 insertions(+), 139 deletions(-)
diff --git a/src/modules/simulator/accelsim/accelsim.cpp b/src/modules/simulator/accelsim/accelsim.cpp
index b412ecc653..a3f1b592f2 100644
--- a/src/modules/simulator/accelsim/accelsim.cpp
+++ b/src/modules/simulator/accelsim/accelsim.cpp
@@ -125,9 +125,9 @@ private:
float _mag_range_scale;
unsigned _mag_samplerate;
- orb_advert_t _accel_topic;
- int _accel_orb_class_instance;
- int _accel_class_instance;
+ orb_advert_t _accel_topic{nullptr};
+ int _accel_orb_class_instance{-1};
+ int _accel_class_instance{-1};
unsigned _accel_read;
unsigned _mag_read;
@@ -224,9 +224,9 @@ protected:
private:
ACCELSIM *_parent;
- orb_advert_t _mag_topic;
- int _mag_orb_class_instance;
- int _mag_class_instance;
+ orb_advert_t _mag_topic{nullptr};
+ int _mag_orb_class_instance{-1};
+ int _mag_class_instance{-1};
virtual void _measure();
@@ -250,9 +250,6 @@ ACCELSIM::ACCELSIM(const char *path, enum Rotation rotation) :
_mag_range_ga(0.0f),
_mag_range_scale(0.0f),
_mag_samplerate(0),
- _accel_topic(nullptr),
- _accel_orb_class_instance(-1),
- _accel_class_instance(-1),
_accel_read(0),
_mag_read(0),
_accel_sample_perf(perf_alloc(PC_ELAPSED, "sim_accel_read")),
@@ -318,65 +315,38 @@ ACCELSIM::~ACCELSIM()
int
ACCELSIM::init()
{
- int ret = -1;
-
- struct mag_report mrp = {};
- sensor_accel_s arp = {};
-
/* do SIM init first */
- if (VirtDevObj::init() != 0) {
+ int ret = VirtDevObj::init();
+
+ if (ret != PX4_OK) {
PX4_WARN("SIM init failed");
- goto out;
+ return ret;
}
/* allocate basic report buffers */
_accel_reports = new ringbuffer::RingBuffer(2, sizeof(sensor_accel_s));
if (_accel_reports == nullptr) {
- goto out;
+ PX4_WARN("_accel_reports creation failed");
+ return -ENOMEM;
}
- _mag_reports = new ringbuffer::RingBuffer(2, sizeof(mag_report));
+ _mag_reports = new ringbuffer::RingBuffer(2, sizeof(sensor_mag_s));
if (_mag_reports == nullptr) {
- goto out;
+ PX4_WARN("_mag_reports creation failed");
+ return -ENOMEM;
}
/* do init for the mag device node */
ret = _mag->init();
- if (ret != OK) {
+ if (ret != PX4_OK) {
PX4_WARN("MAG init failed");
- goto out;
- }
-
- /* fill report structures */
- _measure();
-
- /* advertise sensor topic, measure manually to initialize valid report */
- _mag_reports->get(&mrp);
-
- /* measurement will have generated a report, publish */
- _mag->_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mrp,
- &_mag->_mag_orb_class_instance, ORB_PRIO_LOW);
-
- if (_mag->_mag_topic == nullptr) {
- PX4_WARN("ADVERT ERR");
- }
-
- /* advertise sensor topic, measure manually to initialize valid report */
- _accel_reports->get(&arp);
-
- /* measurement will have generated a report, publish */
- _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp,
- &_accel_orb_class_instance, ORB_PRIO_DEFAULT);
-
- if (_accel_topic == nullptr) {
- PX4_WARN("ADVERT ERR");
+ return ret;
}
-out:
- return ret;
+ return PX4_OK;
}
int
@@ -743,9 +713,7 @@ ACCELSIM::_measure()
* 74 from all measurements centers them around zero.
*/
-
accel_report.timestamp = hrt_absolute_time();
-
accel_report.device_id = 1310728;
// use the temperature from the last mag reading
@@ -769,19 +737,18 @@ ACCELSIM::_measure()
accel_report.y = raw_accel_report.y;
accel_report.z = raw_accel_report.z;
+ //accel_report.integral_dt = 0;
+ //accel_report.x_integral = 0.0f;
+ //accel_report.y_integral = 0.0f;
+ //accel_report.z_integral = 0.0f;
+
+ accel_report.temperature = 25.0f;
+
accel_report.scaling = _accel_range_scale;
_accel_reports->force(&accel_report);
- if (!(m_pub_blocked)) {
- /* publish it */
-
- // The first call to measure() is from init() and _accel_topic is not
- // yet initialized
- if (_accel_topic != nullptr) {
- orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
- }
- }
+ orb_publish_auto(ORB_ID(sensor_accel), &_accel_topic, &accel_report, &_accel_orb_class_instance, ORB_PRIO_DEFAULT);
_accel_read++;
@@ -804,8 +771,6 @@ ACCELSIM::mag_measure()
} raw_mag_report;
#pragma pack(pop)
- mag_report mag_report = {};
-
/* start the performance counter */
perf_begin(_mag_sample_perf);
@@ -832,7 +797,7 @@ ACCELSIM::mag_measure()
* 74 from all measurements centers them around zero.
*/
-
+ mag_report mag_report = {};
mag_report.timestamp = hrt_absolute_time();
mag_report.device_id = 196616;
mag_report.is_external = false;
@@ -863,10 +828,7 @@ ACCELSIM::mag_measure()
_mag_reports->force(&mag_report);
- if (!(m_pub_blocked)) {
- /* publish it */
- orb_publish(ORB_ID(sensor_mag), _mag->_mag_topic, &mag_report);
- }
+ orb_publish_auto(ORB_ID(sensor_mag), &_mag->_mag_topic, &mag_report, &_mag->_mag_orb_class_instance, ORB_PRIO_LOW);
_mag_read++;
diff --git a/src/modules/simulator/airspeedsim/meas_airspeed_sim.cpp b/src/modules/simulator/airspeedsim/meas_airspeed_sim.cpp
index 66e3618693..8209afc9ab 100644
--- a/src/modules/simulator/airspeedsim/meas_airspeed_sim.cpp
+++ b/src/modules/simulator/airspeedsim/meas_airspeed_sim.cpp
@@ -190,7 +190,7 @@ MEASAirspeedSim::collect()
float temperature = airspeed_report.temperature;
float diff_press_pa_raw = airspeed_report.diff_pressure * 100.0f; // convert from millibar to bar
- struct differential_pressure_s report;
+ differential_pressure_s report = {};
report.timestamp = hrt_absolute_time();
report.error_count = perf_event_count(_comms_errors);
@@ -198,10 +198,8 @@ MEASAirspeedSim::collect()
report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa_raw);
report.differential_pressure_raw_pa = diff_press_pa_raw;
- if (_airspeed_pub != nullptr) {
- /* publish it */
- orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report);
- }
+ int instance;
+ orb_publish_auto(ORB_ID(differential_pressure), &_airspeed_pub, &report, &instance, ORB_PRIO_DEFAULT);
new_report(report);
diff --git a/src/modules/simulator/barosim/baro.cpp b/src/modules/simulator/barosim/baro.cpp
index 7af81b18bb..86a370fbac 100644
--- a/src/modules/simulator/barosim/baro.cpp
+++ b/src/modules/simulator/barosim/baro.cpp
@@ -246,16 +246,8 @@ BAROSIM::collect()
/* fake device ID */
report.device_id = 478459;
- /* publish it */
- if (!(m_pub_blocked)) {
- if (_baro_topic != nullptr) {
- /* publish it */
- orb_publish(ORB_ID(sensor_baro), _baro_topic, &report);
-
- } else {
- PX4_WARN("BAROSIM::collect _baro_topic not initialized");
- }
- }
+ int instance;
+ orb_publish_auto(ORB_ID(sensor_baro), &_baro_topic, &report, &instance, ORB_PRIO_DEFAULT);
_reports->force(&report);
diff --git a/src/modules/simulator/gyrosim/gyrosim.cpp b/src/modules/simulator/gyrosim/gyrosim.cpp
index 63b2e7be9b..1b46283944 100644
--- a/src/modules/simulator/gyrosim/gyrosim.cpp
+++ b/src/modules/simulator/gyrosim/gyrosim.cpp
@@ -250,7 +250,6 @@ private:
uint8_t _regdata[108];
- bool _pub_blocked = true; // Don't publish until initialized
};
/**
@@ -358,19 +357,11 @@ GYROSIM::~GYROSIM()
int
GYROSIM::init()
{
- PX4_DEBUG("init");
- int ret = 1;
-
- sensor_accel_s arp = {};
-
- sensor_gyro_s grp = {};
-
- ret = VirtDevObj::init();
+ int ret = VirtDevObj::init();
if (ret != 0) {
PX4_WARN("Base class init failed");
- ret = 1;
- goto out;
+ return ret;
}
/* allocate basic report buffers */
@@ -378,19 +369,19 @@ GYROSIM::init()
if (_accel_reports == nullptr) {
PX4_WARN("_accel_reports creation failed");
- goto out;
+ return -ENOMEM;
}
_gyro_reports = new ringbuffer::RingBuffer(2, sizeof(sensor_gyro_s));
if (_gyro_reports == nullptr) {
PX4_WARN("_gyro_reports creation failed");
- goto out;
+ return -ENOMEM;
}
if (reset() != OK) {
PX4_WARN("reset failed");
- goto out;
+ return PX4_ERROR;
}
/* Initialize offsets and scales */
@@ -408,7 +399,6 @@ GYROSIM::init()
_gyro_scale.z_offset = 0;
_gyro_scale.z_scale = 1.0f;
-
/* do init for the gyro device node, keep it optional */
ret = _gyro->init();
@@ -425,37 +415,7 @@ GYROSIM::init()
return ret;
}
- // Do not call _gyro->start() because polling is done in accel
-
- _measure();
-
- /* advertise sensor topic, measure manually to initialize valid report */
- _accel_reports->get(&arp);
-
- /* measurement will have generated a report, publish */
- _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp,
- &_accel_orb_class_instance, ORB_PRIO_HIGH);
-
- if (_accel_topic == nullptr) {
- PX4_WARN("ADVERT FAIL");
-
- } else {
- _pub_blocked = false;
- }
-
-
- /* advertise sensor topic, measure manually to initialize valid report */
- _gyro_reports->get(&grp);
-
- _gyro->_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp,
- &_gyro->_gyro_orb_class_instance, ORB_PRIO_HIGH);
-
- if (_gyro->_gyro_topic == nullptr) {
- PX4_WARN("ADVERT FAIL");
- }
-
-out:
- return ret;
+ return PX4_OK;
}
int GYROSIM::reset()
@@ -915,17 +875,11 @@ GYROSIM::_measure()
_gyro_reports->force(&grb);
if (accel_notify) {
- if (!(_pub_blocked)) {
- /* publish it */
- orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
- }
+ orb_publish_auto(ORB_ID(sensor_accel), &_accel_topic, &arb, &_accel_orb_class_instance, ORB_PRIO_HIGH);
}
if (gyro_notify) {
- if (!(_pub_blocked)) {
- /* publish it */
- orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb);
- }
+ orb_publish_auto(ORB_ID(sensor_gyro), &_gyro->_gyro_topic, &grb, &_gyro->_gyro_orb_class_instance, ORB_PRIO_HIGH);
}
/* stop measuring */
diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp
index 293ded4e34..018fe0aa3b 100644
--- a/src/modules/simulator/simulator_mavlink.cpp
+++ b/src/modules/simulator/simulator_mavlink.cpp
@@ -452,8 +452,7 @@ void Simulator::handle_message_hil_state_quaternion(const mavlink_message_t *msg
// always publish ground truth attitude message
int hil_gpos_multi;
- orb_publish_auto(ORB_ID(vehicle_global_position_groundtruth), &_gpos_pub, &hil_gpos, &hil_gpos_multi,
- ORB_PRIO_HIGH);
+ orb_publish_auto(ORB_ID(vehicle_global_position_groundtruth), &_gpos_pub, &hil_gpos, &hil_gpos_multi, ORB_PRIO_HIGH);
}
/* local position */
@@ -502,8 +501,7 @@ void Simulator::handle_message_hil_state_quaternion(const mavlink_message_t *msg
// always publish ground truth attitude message
int hil_lpos_multi;
- orb_publish_auto(ORB_ID(vehicle_local_position_groundtruth), &_lpos_pub, &hil_lpos, &hil_lpos_multi,
- ORB_PRIO_HIGH);
+ orb_publish_auto(ORB_ID(vehicle_local_position_groundtruth), &_lpos_pub, &hil_lpos, &hil_lpos_multi, ORB_PRIO_HIGH);
}
}
--
GitLab