drivers/heater: add logging and minor improvements

- new heater_status logging message
 - run continously at low rate until configured sensor is found
 - fix px4io fd bugs (fd open/close/ioctl must be same thread)

msg/CMakeLists.txt

@@ -68,6 +68,7 @@ set(msg_files
 	geofence_result.msg
 	gps_dump.msg
 	gps_inject_data.msg
+	heater_status.msg
 	home_position.msg
 	hover_thrust_estimate.msg
 	input_rc.msg
@@ -127,11 +128,11 @@ set(msg_files
 	sensor_selection.msg
 	sensors_status_imu.msg
 	system_power.msg
+	takeoff_status.msg
 	task_stack_info.msg
 	tecs_status.msg
 	telemetry_status.msg
 	test_motor.msg
-	takeoff_status.msg
 	timesync.msg
 	timesync_status.msg
 	trajectory_bezier.msg

msg/heater_status.msg

@@ -0,0 +1,19 @@
+uint64 timestamp	# time since system start (microseconds)
+
+uint32 device_id
+
+bool heater_on
+
+float32 temperature_sensor
+float32 temperature_target
+
+uint32 controller_period_usec
+uint32 controller_time_on_usec
+
+float32 proportional_value
+float32 integrator_value
+float32 feed_forward_value
+
+uint8 MODE_GPIO  = 1
+uint8 MODE_PX4IO = 2
+uint8 mode

msg/tools/uorb_rtps_message_ids.yaml

@@ -313,6 +313,8 @@ rtps:
     id: 148
   - msg: takeoff_status
     id: 149
+  - msg: heater_status
+    id: 150
   ########## multi topics: begin ##########
   - msg: actuator_controls_0
     id: 170

src/drivers/heater/heater.cpp

@@ -73,17 +73,11 @@ Heater::Heater() :
 	}
 
 #endif
-
-	heater_enable();
 }
 
 Heater::~Heater()
 {
 	heater_disable();
-
-#ifdef HEATER_PX4IO
-	px4_close(_io_fd);
-#endif
 }
 
 int Heater::custom_command(int argc, char *argv[])
@@ -97,28 +91,35 @@ int Heater::custom_command(int argc, char *argv[])
 	return print_usage("Unrecognized command.");
 }
 
-uint32_t Heater::get_sensor_id()
-{
-	return _sensor_accel.device_id;
-}
-
 void Heater::heater_disable()
 {
 	// Reset heater to off state.
 #ifdef HEATER_PX4IO
+	if (_io_fd >= 0) {
 		px4_ioctl(_io_fd, PX4IO_HEATER_CONTROL, HEATER_MODE_DISABLED);
+	}
+
 #endif
+
 #ifdef HEATER_GPIO
 	px4_arch_configgpio(GPIO_HEATER_OUTPUT);
 #endif
 }
 
-void Heater::heater_enable()
+void Heater::heater_initialize()
 {
 	// Initialize heater to off state.
 #ifdef HEATER_PX4IO
+	if (_io_fd < 0) {
+		_io_fd = px4_open(IO_HEATER_DEVICE_PATH, O_RDWR);
+	}
+
+	if (_io_fd >= 0) {
 		px4_ioctl(_io_fd, PX4IO_HEATER_CONTROL, HEATER_MODE_OFF);
+	}
+
 #endif
+
 #ifdef HEATER_GPIO
 	px4_arch_configgpio(GPIO_HEATER_OUTPUT);
 #endif
@@ -127,8 +128,13 @@ void Heater::heater_enable()
 void Heater::heater_off()
 {
 #ifdef HEATER_PX4IO
+
+	if (_io_fd >= 0) {
 		px4_ioctl(_io_fd, PX4IO_HEATER_CONTROL, HEATER_MODE_OFF);
+	}
+
 #endif
+
 #ifdef HEATER_GPIO
 	px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, 0);
 #endif
@@ -137,107 +143,88 @@ void Heater::heater_off()
 void Heater::heater_on()
 {
 #ifdef HEATER_PX4IO
+
+	if (_io_fd >= 0) {
 		px4_ioctl(_io_fd, PX4IO_HEATER_CONTROL, HEATER_MODE_ON);
+	}
+
 #endif
+
 #ifdef HEATER_GPIO
 	px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, 1);
 #endif
 }
 
-void Heater::initialize_topics()
+bool Heater::initialize_topics()
 {
-	// Get the total number of accelerometer instances.
+	for (uint8_t i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
-	uint8_t number_of_imus = orb_group_count(ORB_ID(sensor_accel));
+		uORB::SubscriptionData<sensor_accel_s> sensor_accel_sub{ORB_ID(sensor_accel), i};
 
-	// Get the total number of accelerometer instances and check each instance for the correct ID.
+		if (sensor_accel_sub.get().timestamp != 0 && sensor_accel_sub.get().device_id != 0
-	for (uint8_t x = 0; x < number_of_imus; x++) {
+		    && PX4_ISFINITE(sensor_accel_sub.get().temperature)) {
-		_sensor_accel.device_id = 0;
-
-		while (_sensor_accel.device_id == 0) {
-			_sensor_accel_sub = uORB::Subscription{ORB_ID(sensor_accel), x};
-
-			if (!_sensor_accel_sub.advertised()) {
-				px4_usleep(100);
-				continue;
-			}
-
-			_sensor_accel_sub.copy(&_sensor_accel);
-		}
 
 			// If the correct ID is found, exit the for-loop with _sensor_accel_sub pointing to the correct instance.
-		if (_sensor_accel.device_id == (uint32_t)_param_sens_temp_id.get()) {
+			if (sensor_accel_sub.get().device_id == (uint32_t)_param_sens_temp_id.get()) {
-			break;
+				_sensor_accel_sub.ChangeInstance(i);
+				_sensor_device_id = sensor_accel_sub.get().device_id;
+				return true;
+			}
 		}
 	}
 
-	// Exit the driver if the sensor ID does not match the desired sensor.
+	return false;
-	if (_sensor_accel.device_id != (uint32_t)_param_sens_temp_id.get()) {
-		request_stop();
-		PX4_ERR("Could not identify IMU sensor.");
-	}
-}
-
-int Heater::print_status()
-{
-	float _feedforward_value = _param_sens_imu_temp_ff.get();
-
-	PX4_INFO("Sensor ID: %d,\tSetpoint: %3.2fC,\t Sensor Temperature: %3.2fC,\tDuty Cycle (usec): %d",
-		 _sensor_accel.device_id,
-		 static_cast<double>(_param_sens_imu_temp.get()),
-		 static_cast<double>(_sensor_accel.temperature),
-		 _controller_period_usec);
-	PX4_INFO("Feed Forward control effort: %3.2f%%,\tProportional control effort: %3.2f%%,\tIntegrator control effort: %3.3f%%,\t Heater cycle: %3.2f%%",
-		 static_cast<double>(_feedforward_value * 100),
-		 static_cast<double>(_proportional_value * 100),
-		 static_cast<double>(_integrator_value * 100),
-		 static_cast<double>(static_cast<float>(_controller_time_on_usec) / static_cast<float>(_controller_period_usec) * 100));
-
-	return PX4_OK;
-}
-
-int Heater::print_usage(const char *reason)
-{
-	if (reason) {
-		printf("%s\n\n", reason);
-	}
-
-	PRINT_MODULE_DESCRIPTION(
-		R"DESCR_STR(
-### Description
-Background process running periodically on the LP work queue to regulate IMU temperature at a setpoint.
-
-This task can be started at boot from the startup scripts by setting SENS_EN_THERMAL or via CLI.
-)DESCR_STR");
-
-	PRINT_MODULE_USAGE_NAME("heater", "system");
-	PRINT_MODULE_USAGE_COMMAND("start");
-	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
-
-	return 0;
 }
 
 void Heater::Run()
 {
 	if (should_exit()) {
+#if defined(HEATER_PX4IO)
+
+		// must be closed from wq thread
+		if (_io_fd >= 0) {
+			px4_close(_io_fd);
+		}
+
+#endif
 		exit_and_cleanup();
 		return;
 	}
 
+	// check for parameter updates
+	if (_parameter_update_sub.updated()) {
+		// clear update
+		parameter_update_s pupdate;
+		_parameter_update_sub.copy(&pupdate);
+
+		// update parameters from storage
+		ModuleParams::updateParams();
+	}
+
+	if (_sensor_device_id == 0) {
+		if (initialize_topics()) {
+			heater_initialize();
+
+		} else {
+			// if sensor still not found try again in 1 second
+			ScheduleDelayed(1_s);
+			return;
+		}
+	}
+
+	sensor_accel_s sensor_accel;
+
 	if (_heater_on) {
 		// Turn the heater off.
 		heater_off();
 		_heater_on = false;
 
-	} else {
+	} else if (_sensor_accel_sub.update(&sensor_accel)) {
-		update_params(false);
-
-		_sensor_accel_sub.update(&_sensor_accel);
-
 		float temperature_delta {0.f};
 
 		// Update the current IMU sensor temperature if valid.
-		if (!isnan(_sensor_accel.temperature)) {
+		if (PX4_ISFINITE(sensor_accel.temperature)) {
-			temperature_delta = _param_sens_imu_temp.get() - _sensor_accel.temperature;
+			temperature_delta = _param_sens_imu_temp.get() - sensor_accel.temperature;
+			_temperature_last = sensor_accel.temperature;
 		}
 
 		_proportional_value = temperature_delta * _param_sens_imu_temp_p.get();
@@ -266,15 +253,41 @@ void Heater::Run()
 	} else {
 		ScheduleDelayed(_controller_period_usec - _controller_time_on_usec);
 	}
+
+
+	// publish status
+	heater_status_s status{};
+	status.heater_on = _heater_on;
+	status.device_id = _sensor_device_id;
+	status.temperature_sensor = _temperature_last;
+	status.temperature_target = _param_sens_imu_temp.get();
+	status.controller_period_usec = _controller_period_usec;
+	status.controller_time_on_usec = _controller_time_on_usec;
+	status.proportional_value = _proportional_value;
+	status.integrator_value = _integrator_value;
+	status.feed_forward_value = _param_sens_imu_temp_ff.get();
+
+#ifdef HEATER_PX4IO
+	status.mode |= heater_status_s::MODE_PX4IO;
+#endif
+#ifdef HEATER_GPIO
+	status.mode |= heater_status_s::MODE_GPIO;
+#endif
+
+	status.timestamp = hrt_absolute_time();
+	_heater_status_pub.publish(status);
 }
 
 int Heater::start()
 {
-	update_params(true);
+	// Exit the driver if the sensor ID does not match the desired sensor.
-	initialize_topics();
+	if (_param_sens_temp_id.get() == 0) {
+		PX4_ERR("Valid SENS_TEMP_ID required");
+		request_stop();
+		return PX4_ERROR;
+	}
 
-	// Allow sufficient time for all additional sensors and processes to start.
+	ScheduleNow();
-	ScheduleDelayed(100000);
 	return PX4_OK;
 }
 
@@ -294,23 +307,28 @@ int Heater::task_spawn(int argc, char *argv[])
 	return 0;
 }
 
-void Heater::update_params(const bool force)
+int Heater::print_usage(const char *reason)
 {
-	// check for parameter updates
+	if (reason) {
-	if (_parameter_update_sub.updated() || force) {
+		printf("%s\n\n", reason);
-		// clear update
-		parameter_update_s pupdate;
-		_parameter_update_sub.copy(&pupdate);
-
-		// update parameters from storage
-		ModuleParams::updateParams();
-	}
 	}
 
-/**
+	PRINT_MODULE_DESCRIPTION(
- * Main entry point for the heater driver module
+		R"DESCR_STR(
- */
+### Description
-int heater_main(int argc, char *argv[])
+Background process running periodically on the LP work queue to regulate IMU temperature at a setpoint.
+
+This task can be started at boot from the startup scripts by setting SENS_EN_THERMAL or via CLI.
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("heater", "system");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+
+	return 0;
+}
+
+extern "C" __EXPORT int heater_main(int argc, char *argv[])
 {
 	return Heater::main(argc, argv);
 }

src/drivers/heater/heater.h

@@ -47,7 +47,9 @@
 #include <px4_platform_common/module.h>
 #include <px4_platform_common/module_params.h>
 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#include <uORB/Publication.hpp>
 #include <uORB/SubscriptionInterval.hpp>
+#include <uORB/topics/heater_status.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/sensor_accel.h>
 
@@ -57,13 +59,6 @@ using namespace time_literals;
 
 #define CONTROLLER_PERIOD_DEFAULT 100000
 
-/**
- * @brief IMU Heater Controller driver used to maintain consistent
- *        temparature at the IMU.
- */
-extern "C" __EXPORT int heater_main(int argc, char *argv[]);
-
-
 class Heater : public ModuleBase<Heater>, public ModuleParams, public px4::ScheduledWorkItem
 {
 public:
@@ -97,30 +92,6 @@ public:
 	 */
 	static int task_spawn(int argc, char *argv[]);
 
-	/**
-	 * @brief Sets and/or reports the heater controller time period value in microseconds.
-	 * @param argv Pointer to the input argument array.
-	 * @return Returns 0 iff successful, -1 otherwise.
-	 */
-	int controller_period(char *argv[]);
-
-	/** @brief Returns the id of the target sensor. */
-	uint32_t get_sensor_id();
-
-	/**
-	 * @brief Sets and/or reports the heater controller integrator gain value.
-	 * @param argv Pointer to the input argument array.
-	 * @return Returns the heater integrator gain value iff successful, 0.0f otherwise.
-	 */
-	float integrator(char *argv[]);
-
-	/**
-	 * @brief Sets and/or reports the heater controller proportional gain value.
-	 * @param argv Pointer to the input argument array.
-	 * @return Returns the heater proportional gain value iff successful, 0.0f otherwise.
-	 */
-	float proportional(char *argv[]);
-
 	/**
 	 * @brief Initiates the heater driver work queue, starts a new background task,
 	 *        and fails if it is already running.
@@ -128,25 +99,10 @@ public:
 	 */
 	int start();
 
-	/**
+private:
-	 * @brief Reports curent status and diagnostic information about the heater driver.
-	 * @return Returns 0 iff successful, -1 otherwise.
-	 */
-	int print_status();
-
-	/**
-	 * @brief Sets and/or reports the heater target temperature.
-	 * @param argv Pointer to the input argument array.
-	 * @return Returns the heater target temperature value iff successful, -1.0f otherwise.
-	 */
-	float temperature_setpoint(char *argv[]);
-
-protected:
 
 	/** @brief Called once to initialize uORB topics. */
-	void initialize_topics();
+	bool initialize_topics();
-
-private:
 
 	/** @brief Calculates the heater element on/off time and schedules the next cycle. */
 	void Run() override;
@@ -158,7 +114,7 @@ private:
 	void update_params(const bool force = false);
 
 	/** Enables / configures the heater (either by GPIO or PX4IO). */
-	void heater_enable();
+	void heater_initialize();
 
 	/** Disnables the heater (either by GPIO or PX4IO). */
 	void heater_disable();
@@ -174,7 +130,7 @@ private:
 
 	/** File descriptor for PX4IO for heater ioctl's */
 #if defined(HEATER_PX4IO)
-	int _io_fd;
+	int _io_fd_ {-1};
 #endif
 
 	bool _heater_on = false;
@@ -185,10 +141,15 @@ private:
 	float _integrator_value   = 0.0f;
 	float _proportional_value = 0.0f;
 
+	uORB::Publication<heater_status_s> _heater_status_pub{ORB_ID(heater_status)};
+
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 
 	uORB::Subscription _sensor_accel_sub{ORB_ID(sensor_accel)};
-	sensor_accel_s _sensor_accel{};
+
+	uint32_t _sensor_device_id{0};
+
+	float _temperature_last{NAN};
 
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::SENS_IMU_TEMP_FF>) _param_sens_imu_temp_ff,

src/modules/logger/logged_topics.cpp

@@ -62,6 +62,7 @@ void LoggedTopics::add_default_topics()
 	add_topic("cpuload");
 	add_topic("esc_status", 250);
 	add_topic("generator_status");
+	add_topic("heater_status");
 	add_topic("home_position");
 	add_topic("hover_thrust_estimate", 100);
 	add_topic("input_rc", 500);