Minor cleanup/error checking, static_casts, print_status() additions, and formatting in the heater driver.

src/drivers/drv_io_heater.h

@@ -47,11 +47,11 @@
  * ioctl() definitions
  */
 
-#define IO_HEATER_DEVICE_PATH	"/dev/px4io"
+#define IO_HEATER_DEVICE_PATH "/dev/px4io"
 
-#define _IO_HEATER_BASE		(0x2e00)
+#define _IO_HEATER_BASE       (0x2e00)
 
-#define PX4IO_HEATER_CONTROL		_PX4_IOC(_IO_HEATER_BASE, 0)
+#define PX4IO_HEATER_CONTROL  _PX4_IOC(_IO_HEATER_BASE, 0)
 
 /* ... to IOX_SET_VALUE + 8 */
 

src/drivers/heater/heater.cpp

@@ -74,13 +74,11 @@ Heater::Heater() :
 
 #endif
 
-	// Initialize heater to off state
 	heater_enable();
 }
 
 Heater::~Heater()
 {
-	// Reset heater to off state
 	heater_disable();
 
 #ifdef HEATER_PX4IO
@@ -88,18 +86,25 @@ Heater::~Heater()
 #endif
 }
 
-void Heater::heater_enable()
+int Heater::custom_command(int argc, char *argv[])
 {
-#ifdef HEATER_PX4IO
-	px4_ioctl(_io_fd, PX4IO_HEATER_CONTROL, HEATER_MODE_OFF);
-#endif
-#ifdef HEATER_GPIO
-	px4_arch_configgpio(GPIO_HEATER_OUTPUT);
-#endif
+	// Check if the driver is running.
+	if (!is_running()) {
+		PX4_INFO("not running");
+		return PX4_ERROR;
+	}
+
+	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);
 #endif
@@ -108,13 +113,14 @@ void Heater::heater_disable()
 #endif
 }
 
-void Heater::heater_on()
+void Heater::heater_enable()
 {
+	// Initialize heater to off state.
 #ifdef HEATER_PX4IO
-	px4_ioctl(_io_fd, PX4IO_HEATER_CONTROL, HEATER_MODE_ON);
+	px4_ioctl(_io_fd, PX4IO_HEATER_CONTROL, HEATER_MODE_OFF);
 #endif
 #ifdef HEATER_GPIO
-	px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, 1);
+	px4_arch_configgpio(GPIO_HEATER_OUTPUT);
 #endif
 }
 
@@ -128,15 +134,86 @@ void Heater::heater_off()
 #endif
 }
 
-int Heater::custom_command(int argc, char *argv[])
+void Heater::heater_on()
 {
-	// Check if the driver is running.
-	if (!is_running()) {
-		PX4_INFO("not running");
-		return PX4_ERROR;
+#ifdef HEATER_PX4IO
+	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()
+{
+	// Get the total number of accelerometer instances.
+	uint8_t number_of_imus = orb_group_count(ORB_ID(sensor_accel));
+
+	// 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;
+
+		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()) {
+			break;
+		}
 	}
 
-	return print_usage("Unrecognized command.");
+	// 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.");
+	}
+}
+
+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()
@@ -156,28 +233,28 @@ void Heater::Run()
 
 		_sensor_accel_sub.update(&_sensor_accel);
 
-		// Obtain the current IMU sensor temperature.
-		_sensor_temperature = _sensor_accel.temperature;
+		float temperature_delta {0.f};
 
-		// Calculate the temperature delta between the setpoint and reported temperature.
-		float temperature_delta = _param_sens_imu_temp.get() - _sensor_temperature;
+		// Update the current IMU sensor temperature if valid.
+		if (!isnan(_sensor_accel.temperature)) {
+			temperature_delta = _param_sens_imu_temp.get() - _sensor_accel.temperature;
+		}
 
-		// Modulate the heater time on with a feedforward/PI controller.
 		_proportional_value = temperature_delta * _param_sens_imu_temp_p.get();
 		_integrator_value += temperature_delta * _param_sens_imu_temp_i.get();
 
-		// Constrain the integrator value to no more than 25% of the duty cycle.
-		_integrator_value = math::constrain(_integrator_value, -0.25f, 0.25f);
+		if (fabs(_param_sens_imu_temp_i.get()) <= 0.0001) {
+			_integrator_value = 0.f;
+		}
 
-		// Calculate the duty cycle. This is a value between 0 and 1.
-		float duty = _proportional_value + _integrator_value;
+		// Guard against integrator wind up.
+		_integrator_value = math::constrain(_integrator_value, -0.25f, 0.25f);
 
-		_controller_time_on_usec = (int)(duty * (float)_controller_period_usec);
+		_controller_time_on_usec = static_cast<int>((_param_sens_imu_temp_ff.get() + _proportional_value +
+						             _integrator_value) * static_cast<float>(_controller_period_usec));
 
-		// Constrain the heater time within the allowable duty cycle.
 		_controller_time_on_usec = math::constrain(_controller_time_on_usec, 0, _controller_period_usec);
 
-		// Turn the heater on.
 		_heater_on = true;
 		heater_on();
 	}
@@ -191,52 +268,13 @@ void Heater::Run()
 	}
 }
 
-void Heater::initialize_topics()
-{
-	// Get the total number of accelerometer instances.
-	uint8_t number_of_imus = orb_group_count(ORB_ID(sensor_accel));
-
-	// Check each instance for the correct ID.
-	for (uint8_t x = 0; x < number_of_imus; x++) {
-		_sensor_accel_sub = uORB::Subscription{ORB_ID(sensor_accel), x};
-
-		if (!_sensor_accel_sub.advertised()) {
-			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()) {
-			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.");
-	}
-}
-
-int Heater::print_status()
-{
-	PX4_INFO("Sensor ID: %d - Temperature: %3.3fC, Setpoint: %3.2fC, Heater State: %s",
-		 _sensor_accel.device_id,
-		 (double)_sensor_temperature,
-		 (double)_param_sens_imu_temp.get(),
-		 _heater_on ? "On" : "Off");
-
-	return PX4_OK;
-}
-
 int Heater::start()
 {
 	update_params(true);
 	initialize_topics();
 
-	ScheduleNow();
-
+	// Allow sufficient time for all additional sensors and processes to start.
+	ScheduleDelayed(100000);
 	return PX4_OK;
 }
 
@@ -253,7 +291,6 @@ int Heater::task_spawn(int argc, char *argv[])
 	_task_id = task_id_is_work_queue;
 
 	heater->start();
-
 	return 0;
 }
 
@@ -270,27 +307,6 @@ void Heater::update_params(const bool force)
 	}
 }
 
-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;
-}
-
 /**
  * Main entry point for the heater driver module
  */

src/drivers/heater/heater.h

@@ -104,6 +104,9 @@ public:
 	 */
 	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.
@@ -118,12 +121,6 @@ public:
 	 */
 	float proportional(char *argv[]);
 
-	/**
-	 * @brief Reports the heater target sensor.
-	 * @return Returns the id of the target sensor
-	 */
-	uint32_t sensor_id();
-
 	/**
 	 * @brief Initiates the heater driver work queue, starts a new background task,
 	 *        and fails if it is already running.
@@ -146,18 +143,12 @@ public:
 
 protected:
 
-	/**
-	 * @brief Called once to initialize uORB topics.
-	 */
+	/** @brief Called once to initialize uORB topics. */
 	void initialize_topics();
 
 private:
 
-	/**
-	 * @brief Calculates the heater element on/off time, carries out
-	 *        closed loop feedback and feedforward temperature control,
-	 *        and schedules the next cycle.
-	 */
+	/** @brief Calculates the heater element on/off time and schedules the next cycle. */
 	void Run() override;
 
 	/**
@@ -166,27 +157,19 @@ private:
 	 */
 	void update_params(const bool force = false);
 
-	/**
-	 * @brief Enables / configures the heater (either by GPIO or PX4IO)
-	 */
+	/** Enables / configures the heater (either by GPIO or PX4IO). */
 	void heater_enable();
 
-	/**
-	 * @brief Disnables the heater (either by GPIO or PX4IO)
-	 */
+	/** Disnables the heater (either by GPIO or PX4IO). */
 	void heater_disable();
 
-	/**
-	 * @brief Turns the heater on (either by GPIO or PX4IO)
-	 */
+	/** Turns the heater on (either by GPIO or PX4IO). */
 	void heater_on();
 
-	/**
-	 * @brief Turns the heater off (either by GPIO or PX4IO)
-	 */
+	/** Turns the heater off (either by GPIO or PX4IO). */
 	void heater_off();
 
-	/** Work queue struct for the RTOS scheduler. */
+	/** Work queue struct for the scheduler. */
 	static struct work_s _work;
 
 	/** File descriptor for PX4IO for heater ioctl's */
@@ -194,28 +177,24 @@ private:
 	int _io_fd;
 #endif
 
-	int _controller_period_usec = CONTROLLER_PERIOD_DEFAULT;
+	bool _heater_on = false;
 
+	int _controller_period_usec = CONTROLLER_PERIOD_DEFAULT;
 	int _controller_time_on_usec = 0;
 
-	bool _heater_on = false;
-
-	float _integrator_value = 0.0f;
+	float _integrator_value   = 0.0f;
+	float _proportional_value = 0.0f;
 
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 
-	float _proportional_value = 0.0f;
-
 	uORB::Subscription _sensor_accel_sub{ORB_ID(sensor_accel)};
 	sensor_accel_s _sensor_accel{};
 
-	float _sensor_temperature = 0.0f;
-
-	/** @note Declare local parameters using defined parameters. */
 	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::SENS_IMU_TEMP_FF>) _param_sens_imu_temp_ff,
 		(ParamFloat<px4::params::SENS_IMU_TEMP_I>)  _param_sens_imu_temp_i,
 		(ParamFloat<px4::params::SENS_IMU_TEMP_P>)  _param_sens_imu_temp_p,
-		(ParamInt<px4::params::SENS_TEMP_ID>) _param_sens_temp_id,
-		(ParamFloat<px4::params::SENS_IMU_TEMP>) _param_sens_imu_temp
+		(ParamInt<px4::params::SENS_TEMP_ID>)       _param_sens_temp_id,
+		(ParamFloat<px4::params::SENS_IMU_TEMP>)    _param_sens_imu_temp
 	)
 };

src/drivers/heater/heater_params.c

@@ -60,6 +60,18 @@ PARAM_DEFINE_INT32(SENS_TEMP_ID, 0);
  */
 PARAM_DEFINE_FLOAT(SENS_IMU_TEMP, 55.0f);
 
+/**
+ * IMU heater controller feedforward value.
+ *
+ * @category system
+ * @group Sensors
+ * @unit %
+ * @min 0
+ * @max 1.0
+ * @decimal 3
+ */
+PARAM_DEFINE_FLOAT(SENS_IMU_TEMP_FF, 0.05f);
+
 /**
  * IMU heater controller integrator gain value.
  *