From 3bde706cb3cc663965ad0c38ad451e1528c5dea3 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Fri, 4 Apr 2025 16:49:00 +1300
Subject: [PATCH] cubepilot: enable heater for CubeOrange+

This adds everything required to use the heater on Orange+ but leaves it
disabled by default.
---
 boards/cubepilot/cubeorangeplus/default.px4board       | 1 +
 boards/cubepilot/cubeorangeplus/init/rc.board_defaults | 2 --
 boards/cubepilot/cubeorangeplus/init/rc.board_sensors  | 6 +++++-
 boards/cubepilot/cubeorangeplus/src/board_config.h     | 2 ++
 4 files changed, 8 insertions(+), 3 deletions(-)

diff --git a/boards/cubepilot/cubeorangeplus/default.px4board b/boards/cubepilot/cubeorangeplus/default.px4board
index c8b2bb60d6..ba6ed0b34f 100644
--- a/boards/cubepilot/cubeorangeplus/default.px4board
+++ b/boards/cubepilot/cubeorangeplus/default.px4board
@@ -32,6 +32,7 @@ CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_PWM_INPUT=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_PX4IO=y
+CONFIG_DRIVERS_HEATER=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
diff --git a/boards/cubepilot/cubeorangeplus/init/rc.board_defaults b/boards/cubepilot/cubeorangeplus/init/rc.board_defaults
index 7f961cd33e..9840a14497 100644
--- a/boards/cubepilot/cubeorangeplus/init/rc.board_defaults
+++ b/boards/cubepilot/cubeorangeplus/init/rc.board_defaults
@@ -12,6 +12,4 @@ param set-default BAT2_A_PER_V 17
 # Disable IMU thermal control
 param set-default SENS_EN_THERMAL 0
 
-param set-default -s SENS_TEMP_ID 2621474
-
 set IOFW "/etc/extras/cubepilot_io-v2_default.bin"
diff --git a/boards/cubepilot/cubeorangeplus/init/rc.board_sensors b/boards/cubepilot/cubeorangeplus/init/rc.board_sensors
index 7782e73d6a..8ef0bff121 100644
--- a/boards/cubepilot/cubeorangeplus/init/rc.board_sensors
+++ b/boards/cubepilot/cubeorangeplus/init/rc.board_sensors
@@ -17,6 +17,9 @@ ms5611 -s -b 4 start
 
 if icm42688p -s -b 4 -R 10 -q start -c 15
 then
+	# We need to use the temperature of the first isolated IMU for heater control.
+	param set-default SENS_TEMP_ID 2490402
+
 	if ! icm20948 -s -b 4 -R 10 -M -q start
 	then
 		icm42688p -s -b 4 -R 6 start -c 13
@@ -24,6 +27,8 @@ then
 else
 	icm45686 -s -b 4 -R 10 start -c 15
 	icm45686 -s -b 4 -R 6 start -c 13
+
+	param set-default SENS_TEMP_ID 3407906
 fi
 
 # SPI1, body-fixed
@@ -33,7 +38,6 @@ then
 	ak09916 start -I -R 13
 else
 	icm20649 -s -b 1 start
-
 fi
 
 ms5611 -s -b 1 start
diff --git a/boards/cubepilot/cubeorangeplus/src/board_config.h b/boards/cubepilot/cubeorangeplus/src/board_config.h
index 1646925c3a..a77723b491 100644
--- a/boards/cubepilot/cubeorangeplus/src/board_config.h
+++ b/boards/cubepilot/cubeorangeplus/src/board_config.h
@@ -68,6 +68,8 @@
 #define PX4IO_SERIAL_CLOCK             STM32_PCLK2_FREQUENCY
 #define PX4IO_SERIAL_BITRATE           1500000               /* 1.5Mbps -> max rate for IO */
 
+#define PX4IO_HEATER_ENABLED
+
 /* LEDs */
 #define GPIO_nLED_AMBER        /* PE12 */  (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN12)