From 73362f325bf5254639bf27a738451def62d50322 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lorenz@px4.io>
Date: Mon, 15 Aug 2016 23:31:58 +0200
Subject: [PATCH] Update mapping

---
 src/drivers/tap_esc/drv_tap_esc.h |  3 +++
 src/drivers/tap_esc/tap_esc.cpp   | 44 ++++++++++++-------------------
 2 files changed, 20 insertions(+), 27 deletions(-)

diff --git a/src/drivers/tap_esc/drv_tap_esc.h b/src/drivers/tap_esc/drv_tap_esc.h
index 0a2d1a60ca..a77fae823a 100644
--- a/src/drivers/tap_esc/drv_tap_esc.h
+++ b/src/drivers/tap_esc/drv_tap_esc.h
@@ -74,7 +74,10 @@
  *   ....
  *
  */
+
+// Circular from back right in CCW direction
 #define ESC_POS {0, 1, 4, 3, 2, 5, 7, 8}
+// 0 is CW, 1 is CCW
 #define ESC_DIR {0, 1, 0, 1, 0, 1, 0, 1}
 
 #define RPMMAX 1900
diff --git a/src/drivers/tap_esc/tap_esc.cpp b/src/drivers/tap_esc/tap_esc.cpp
index fab0c91f3e..c5f1e4745e 100644
--- a/src/drivers/tap_esc/tap_esc.cpp
+++ b/src/drivers/tap_esc/tap_esc.cpp
@@ -630,31 +630,6 @@ TAP_ESC::cycle()
 
 		size_t num_outputs = _channels_count;
 
-		/*
-		// FIXME: don't know what this mode should be used for. It's hardcoded in initialization and never changed.
-		switch (_mode) {
-		case MODE_2PWM:
-			num_outputs = 2;
-			break;
-
-		case MODE_4PWM:
-			num_outputs = 4;
-			break;
-
-		case MODE_6PWM:
-			num_outputs = 6;
-			break;
-
-		case MODE_8PWM:
-			num_outputs = 8;
-			break;
-
-		default:
-			num_outputs = 0;
-			break;
-		}
-		*/
-
 		/* can we mix? */
 		if (_is_armed && _mixers != nullptr) {
 
@@ -723,8 +698,23 @@ TAP_ESC::cycle()
 		const unsigned esc_count = num_outputs;
 		float motor_out[TAP_ESC_MAX_MOTOR_NUM];
 
-		for (int i = 0; i < esc_count; ++i) {
-			motor_out[i] = _outputs.output[i];
+		// We need to remap from the system default to what PX4's normal
+		// scheme is
+		if (num_outputs == 6) {
+			motor_out[0] = _outputs.output[3];
+			motor_out[1] = _outputs.output[0];
+			motor_out[2] = _outputs.output[4];
+			motor_out[3] = _outputs.output[2];
+			motor_out[4] = _outputs.output[1];
+			motor_out[5] = _outputs.output[5];
+			motor_out[6] = RPMSTOPPED;
+			motor_out[7] = RPMSTOPPED;
+		} else {
+
+			// Use the system defaults
+			for (int i = 0; i < esc_count; ++i) {
+				motor_out[i] = _outputs.output[i];
+			}
 		}
 
 		send_esc_outputs(motor_out, esc_count);