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
-	px4_ioctl(_io_fd, PX4IO_HEATER_CONTROL, HEATER_MODE_DISABLED);
+	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
-	px4_ioctl(_io_fd, PX4IO_HEATER_CONTROL, HEATER_MODE_OFF);
+	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
-	px4_ioctl(_io_fd, PX4IO_HEATER_CONTROL, HEATER_MODE_OFF);
+
+	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,64 +143,170 @@ void Heater::heater_off()
 void Heater::heater_on()
 {
 #ifdef HEATER_PX4IO
-	px4_ioctl(_io_fd, PX4IO_HEATER_CONTROL, HEATER_MODE_ON);
+
+	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.
-	uint8_t number_of_imus = orb_group_count(ORB_ID(sensor_accel));
+	for (uint8_t i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
+		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.
-	for (uint8_t x = 0; x < number_of_imus; x++) {
-		_sensor_accel.device_id = 0;
+		if (sensor_accel_sub.get().timestamp != 0 && sensor_accel_sub.get().device_id != 0
+		    && PX4_ISFINITE(sensor_accel_sub.get().temperature)) {
 
-		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;
+			// If the correct ID is found, exit the for-loop with _sensor_accel_sub pointing to the correct instance.
+			if (sensor_accel_sub.get().device_id == (uint32_t)_param_sens_temp_id.get()) {
+				_sensor_accel_sub.ChangeInstance(i);
+				_sensor_device_id = sensor_accel_sub.get().device_id;
+				return true;
 			}
-
-			_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()) {
-			break;
 		}
 	}
 
-	// Exit the driver if the sensor ID does not match the desired sensor.
-	if (_sensor_accel.device_id != (uint32_t)_param_sens_temp_id.get()) {
-		request_stop();
-		PX4_ERR("Could not identify IMU sensor.");
-	}
+	return false;
 }
 
-int Heater::print_status()
+void Heater::Run()
 {
-	float _feedforward_value = _param_sens_imu_temp_ff.get();
+	if (should_exit()) {
+#if defined(HEATER_PX4IO)
 
-	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));
+		// 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 if (_sensor_accel_sub.update(&sensor_accel)) {
+		float temperature_delta {0.f};
+
+		// Update the current IMU sensor temperature if valid.
+		if (PX4_ISFINITE(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();
+		_integrator_value += temperature_delta * _param_sens_imu_temp_i.get();
+
+		if (fabs(_param_sens_imu_temp_i.get()) <= 0.0001) {
+			_integrator_value = 0.f;
+		}
+
+		// Guard against integrator wind up.
+		_integrator_value = math::constrain(_integrator_value, -0.25f, 0.25f);
+
+		_controller_time_on_usec = static_cast<int>((_param_sens_imu_temp_ff.get() + _proportional_value +
+					   _integrator_value) * static_cast<float>(_controller_period_usec));
+
+		_controller_time_on_usec = math::constrain(_controller_time_on_usec, 0, _controller_period_usec);
+
+		_heater_on = true;
+		heater_on();
+	}
+
+	// Schedule the next cycle.
+	if (_heater_on) {
+		ScheduleDelayed(_controller_time_on_usec);
+
+	} 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()
+{
+	// Exit the driver if the sensor ID does not match the desired sensor.
+	if (_param_sens_temp_id.get() == 0) {
+		PX4_ERR("Valid SENS_TEMP_ID required");
+		request_stop();
+		return PX4_ERROR;
+	}
+
+	ScheduleNow();
 	return PX4_OK;
 }
 
+int Heater::task_spawn(int argc, char *argv[])
+{
+	Heater *heater = new Heater();
+
+	if (!heater) {
+		PX4_ERR("driver allocation failed");
+		return PX4_ERROR;
+	}
+
+	_object.store(heater);
+	_task_id = task_id_is_work_queue;
+
+	heater->start();
+	return 0;
+}
+
 int Heater::print_usage(const char *reason)
 {
 	if (reason) {
@@ -216,101 +328,7 @@ This task can be started at boot from the startup scripts by setting SENS_EN_THE
 	return 0;
 }
 
-void Heater::Run()
-{
-	if (should_exit()) {
-		exit_and_cleanup();
-		return;
-	}
-
-	if (_heater_on) {
-		// Turn the heater off.
-		heater_off();
-		_heater_on = false;
-
-	} else {
-		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)) {
-			temperature_delta = _param_sens_imu_temp.get() - _sensor_accel.temperature;
-		}
-
-		_proportional_value = temperature_delta * _param_sens_imu_temp_p.get();
-		_integrator_value += temperature_delta * _param_sens_imu_temp_i.get();
-
-		if (fabs(_param_sens_imu_temp_i.get()) <= 0.0001) {
-			_integrator_value = 0.f;
-		}
-
-		// Guard against integrator wind up.
-		_integrator_value = math::constrain(_integrator_value, -0.25f, 0.25f);
-
-		_controller_time_on_usec = static_cast<int>((_param_sens_imu_temp_ff.get() + _proportional_value +
-						             _integrator_value) * static_cast<float>(_controller_period_usec));
-
-		_controller_time_on_usec = math::constrain(_controller_time_on_usec, 0, _controller_period_usec);
-
-		_heater_on = true;
-		heater_on();
-	}
-
-	// Schedule the next cycle.
-	if (_heater_on) {
-		ScheduleDelayed(_controller_time_on_usec);
-
-	} else {
-		ScheduleDelayed(_controller_period_usec - _controller_time_on_usec);
-	}
-}
-
-int Heater::start()
-{
-	update_params(true);
-	initialize_topics();
-
-	// Allow sufficient time for all additional sensors and processes to start.
-	ScheduleDelayed(100000);
-	return PX4_OK;
-}
-
-int Heater::task_spawn(int argc, char *argv[])
-{
-	Heater *heater = new Heater();
-
-	if (!heater) {
-		PX4_ERR("driver allocation failed");
-		return PX4_ERROR;
-	}
-
-	_object.store(heater);
-	_task_id = task_id_is_work_queue;
-
-	heater->start();
-	return 0;
-}
-
-void Heater::update_params(const bool force)
-{
-	// check for parameter updates
-	if (_parameter_update_sub.updated() || force) {
-		// clear update
-		parameter_update_s pupdate;
-		_parameter_update_sub.copy(&pupdate);
-
-		// update parameters from storage
-		ModuleParams::updateParams();
-	}
-}
-
-/**
- * Main entry point for the heater driver module
- */
-int heater_main(int argc, char *argv[])
+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();
 
-	/**
-	 * @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:
+private:
 
 	/** @brief Called once to initialize uORB topics. */
-	void initialize_topics();
-
-private:
+	bool initialize_topics();
 
 	/** @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);