From 9f14ace0fa0023c0f79b20dd1cb6b19644c7bca4 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 4 Dec 2016 12:14:17 +0100
Subject: [PATCH] IO: Allow control of the blue led state as function of a
 Pixhawk 2.1 heater

---
 src/modules/px4iofirmware/protocol.h  |  5 ++++
 src/modules/px4iofirmware/px4io.c     | 39 ++++++++++++++++++---------
 src/modules/px4iofirmware/registers.c |  3 ++-
 3 files changed, 34 insertions(+), 13 deletions(-)

diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index 9352799f66..717a89a753 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -244,6 +244,11 @@ enum {							/* DSM bind states */
 
 #define PX4IO_P_SETUP_MOTOR_SLEW_MAX 	24 	/* max motor slew rate */
 
+#define PX4IO_P_SETUP_THERMAL			25	/* thermal management */
+#define PX4IO_THERMAL_IGNORE			UINT16_MAX
+#define PX4IO_THERMAL_OFF				0
+#define PX4IO_THERMAL_FULL				10000
+
 /* autopilot control values, -10000..10000 */
 #define PX4IO_PAGE_CONTROLS			51	/**< actuator control groups, one after the other, 8 wide */
 #define PX4IO_P_CONTROLS_GROUP_0		(PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 0)	/**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */
diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c
index 49195ef6e2..5d6991a691 100644
--- a/src/modules/px4iofirmware/px4io.c
+++ b/src/modules/px4iofirmware/px4io.c
@@ -376,21 +376,36 @@ user_start(int argc, char *argv[])
 		controls_tick();
 		perf_end(controls_perf);
 
-		/*
-		  blink blue LED at 4Hz in normal operation. When in
-		  override blink 4x faster so the user can clearly see
-		  that override is happening. This helps when
-		  pre-flight testing the override system
+		/* some boards such as Pixhawk 2.1 made
+		   the unfortunate choice to combine the blue led channel with
+		   the IMU heater. We need a software hack to fix the hardware hack
+		   by allowing to disable the LED / heater.
 		 */
-		uint32_t heartbeat_period_us = 250 * 1000UL;
+		if (r_page_setup[PX4IO_P_SETUP_THERMAL] == PX4IO_THERMAL_IGNORE) {
+			/*
+			  blink blue LED at 4Hz in normal operation. When in
+			  override blink 4x faster so the user can clearly see
+			  that override is happening. This helps when
+			  pre-flight testing the override system
+			 */
+			uint32_t heartbeat_period_us = 250 * 1000UL;
 
-		if (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) {
-			heartbeat_period_us /= 4;
-		}
+			if (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) {
+				heartbeat_period_us /= 4;
+			}
 
-		if ((hrt_absolute_time() - last_heartbeat_time) > heartbeat_period_us) {
-			last_heartbeat_time = hrt_absolute_time();
-			heartbeat_blink();
+			if ((hrt_absolute_time() - last_heartbeat_time) > heartbeat_period_us) {
+				last_heartbeat_time = hrt_absolute_time();
+				heartbeat_blink();
+			}
+
+		} else if (r_page_setup[PX4IO_P_SETUP_THERMAL] < PX4IO_THERMAL_FULL) {
+			/* switch resistive heater off */
+			LED_BLUE(false);
+
+		} else {
+			/* switch resistive heater hard on */
+			LED_BLUE(true);
 		}
 
 		ring_blink();
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index 707b0d424f..ee1473bb27 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -181,7 +181,8 @@ volatile uint16_t	r_page_setup[] = {
 	[PX4IO_P_SETUP_SCALE_ROLL] = 10000,
 	[PX4IO_P_SETUP_SCALE_PITCH] = 10000,
 	[PX4IO_P_SETUP_SCALE_YAW] = 10000,
-	[PX4IO_P_SETUP_MOTOR_SLEW_MAX] = 0
+	[PX4IO_P_SETUP_MOTOR_SLEW_MAX] = 0,
+	[PX4IO_P_SETUP_THERMAL] = PX4IO_THERMAL_IGNORE
 };
 
 #ifdef CONFIG_ARCH_BOARD_PX4IO_V2