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

@@ -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;
+		return ret;
 	}
 
-	/* 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");
-	}
-
-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++;
 

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);
 

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);
 

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 */

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);
 	}
 }