From 4f1c01de7f7fda8623311b3f5bdb1d66fc3448de Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Mon, 28 May 2018 07:09:30 +0200
Subject: [PATCH] fmu params: add MOT_ORDERING to adjust the motor ordering

Useful for 4-in-1 ESCs such as the Hobbywing XRotor Micro 40A 4in1
where the FC can be directly plugged on top.
---
 src/drivers/px4fmu/fmu.cpp         | 71 +++++++++++++++++++++++++++---
 src/drivers/px4fmu/px4fmu_params.c | 20 +++++++++
 2 files changed, 86 insertions(+), 5 deletions(-)

diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index 06028b3382..eafa09cc7c 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -210,6 +210,11 @@ private:
 		"ST24"
 	};
 
+	enum class MotorOrdering : int32_t {
+		PX4 = 0,
+		Betaflight = 1
+	};
+
 	hrt_abstime _rc_scan_begin = 0;
 	bool _rc_scan_locked = false;
 	bool _report_lock = true;
@@ -273,9 +278,10 @@ private:
 	orb_advert_t		_to_safety;
 	orb_advert_t      _to_mixer_status; 	///< mixer status flags
 
-	float _mot_t_max;	// maximum rise time for motor (slew rate limiting)
-	float _thr_mdl_fac;	// thrust to pwm modelling factor
-	bool _airmode; 		// multicopter air-mode
+	float _mot_t_max;	///< maximum rise time for motor (slew rate limiting)
+	float _thr_mdl_fac;	///< thrust to pwm modelling factor
+	bool _airmode; 		///< multicopter air-mode
+	MotorOrdering _motor_ordering;
 
 	perf_counter_t	_perf_control_latency;
 
@@ -334,6 +340,12 @@ private:
 	void rc_io_invert(bool invert);
 	void safety_check_button(void);
 	void flash_safety_button(void);
+
+	/**
+	 * Reorder PWM outputs according to _motor_ordering
+	 * @param values PWM values to reorder
+	 */
+	inline void reorder_outputs(uint16_t values[MAX_ACTUATORS]);
 };
 
 #if defined(BOARD_HAS_FMU_GPIO)
@@ -387,6 +399,7 @@ PX4FMU::PX4FMU(bool run_as_task) :
 	_mot_t_max(0.0f),
 	_thr_mdl_fac(0.0f),
 	_airmode(false),
+	_motor_ordering(MotorOrdering::PX4),
 	_perf_control_latency(perf_alloc(PC_ELAPSED, "fmu control latency"))
 {
 	for (unsigned i = 0; i < _max_actuators; i++) {
@@ -1324,6 +1337,9 @@ PX4FMU::cycle()
 					}
 				}
 
+				/* apply _motor_ordering */
+				reorder_outputs(pwm_limited);
+
 				/* output to the servos */
 				if (_pwm_initialized) {
 					for (size_t i = 0; i < mixed_num_outputs; i++) {
@@ -1805,6 +1821,13 @@ void PX4FMU::update_params()
 		_airmode = val > 0;
 		PX4_DEBUG("%s: %d", "MC_AIRMODE", _airmode);
 	}
+
+	// motor ordering
+	param_handle = param_find("MOT_ORDERING");
+
+	if (param_handle != PARAM_INVALID) {
+		param_get(param_handle, (int32_t *)&_motor_ordering);
+	}
 }
 
 
@@ -2602,7 +2625,7 @@ ssize_t
 PX4FMU::write(file *filp, const char *buffer, size_t len)
 {
 	unsigned count = len / 2;
-	uint16_t values[len];
+	uint16_t values[MAX_ACTUATORS];
 
 #if BOARD_HAS_PWM == 0
 	return 0;
@@ -2613,10 +2636,20 @@ PX4FMU::write(file *filp, const char *buffer, size_t len)
 		count = BOARD_HAS_PWM;
 	}
 
+	if (count > MAX_ACTUATORS) {
+		count = MAX_ACTUATORS;
+	}
+
 	// allow for misaligned values
 	memcpy(values, buffer, count * 2);
 
-	for (uint8_t i = 0; i < count; i++) {
+	for (unsigned i = count; i < _num_outputs; ++i) {
+		values[i] = PWM_IGNORE_THIS_CHANNEL;
+	}
+
+	reorder_outputs(values);
+
+	for (unsigned i = 0; i < _num_outputs; i++) {
 		if (values[i] != PWM_IGNORE_THIS_CHANNEL) {
 			up_pwm_servo_set(i, values[i]);
 		}
@@ -2625,6 +2658,34 @@ PX4FMU::write(file *filp, const char *buffer, size_t len)
 	return count * 2;
 }
 
+void
+PX4FMU::reorder_outputs(uint16_t values[MAX_ACTUATORS])
+{
+	if (MAX_ACTUATORS < 4) {
+		return;
+	}
+
+	if (_motor_ordering == MotorOrdering::Betaflight) {
+		/*
+		 * Betaflight default motor ordering:
+		 * 4     2
+		 *    ^
+		 * 3     1
+		 */
+		const uint16_t pwm_tmp[4] = {values[0], values[1], values[2], values[3] };
+		values[0] = pwm_tmp[3];
+		values[1] = pwm_tmp[0];
+		values[2] = pwm_tmp[1];
+		values[3] = pwm_tmp[2];
+	}
+
+	/* else: PX4, no need to reorder
+	 * 3     1
+	 *    ^
+	 * 2     4
+	 */
+}
+
 void
 PX4FMU::sensor_reset(int ms)
 {
diff --git a/src/drivers/px4fmu/px4fmu_params.c b/src/drivers/px4fmu/px4fmu_params.c
index 5eac095eea..7d3fe5fbb7 100644
--- a/src/drivers/px4fmu/px4fmu_params.c
+++ b/src/drivers/px4fmu/px4fmu_params.c
@@ -190,6 +190,26 @@ PARAM_DEFINE_FLOAT(THR_MDL_FAC, 0.0f);
  */
 PARAM_DEFINE_FLOAT(MOT_SLEW_MAX, 0.0f);
 
+/**
+ * Motor Ordering
+ *
+ * Determines the motor ordering. This can be used for example in combination with
+ * a 4-in-1 ESC that assumes a motor ordering which is different from PX4.
+ *
+ * ONLY supported for Quads.
+ * ONLY supported for fmu output (Pixracer or Omnibus F4).
+ *
+ * When changing this, make sure to test the motor response without props first.
+ *
+ * @value 0 PX4
+ * @value 1 Betaflight / Cleanflight
+ *
+ * @min 0
+ * @max 1
+ * @group PWM Outputs
+ */
+PARAM_DEFINE_INT32(MOT_ORDERING, 0);
+
 /**
  * Run the FMU as a task to reduce latency
  *