From ab2d5952246f1422c4efd2222d35edfc94f4a580 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Mon, 16 Jul 2018 10:32:26 -0400
Subject: [PATCH] FMU PWM parameters respect instance for MAIN/AUX usage

---
 src/drivers/px4fmu/fmu.cpp | 42 +++++++++++++++++++++++++++-----------
 1 file changed, 30 insertions(+), 12 deletions(-)

diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index b7f8abc818..72d74997e0 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -515,8 +515,6 @@ PX4FMU::init()
 	/* initialize PWM limit lib */
 	pwm_limit_init(&_pwm_limit);
 
-	update_pwm_rev_mask();
-
 #ifdef RC_SERIAL_PORT
 
 #  ifdef RF_RADIO_POWER_CONTROL
@@ -537,12 +535,6 @@ PX4FMU::init()
 	// Getting initial parameter values
 	update_params();
 
-	for (unsigned i = 0; i < _max_actuators; i++) {
-		char pname[16];
-		sprintf(pname, "PWM_AUX_TRIM%d", i + 1);
-		param_find(pname);
-	}
-
 	return 0;
 }
 
@@ -926,15 +918,28 @@ PX4FMU::update_pwm_rev_mask()
 {
 	_reverse_pwm_mask = 0;
 
+	const char *pname_format;
+
+	if (_class_instance == CLASS_DEVICE_PRIMARY) {
+		pname_format = "PWM_MAIN_REV%d";
+
+	} else if (_class_instance == CLASS_DEVICE_SECONDARY) {
+		pname_format = "PWM_AUX_REV%d";
+
+	} else {
+		PX4_ERR("PWM REV only for MAIN and AUX");
+		return;
+	}
+
 	for (unsigned i = 0; i < _max_actuators; i++) {
 		char pname[16];
-		int32_t ival;
 
 		/* fill the channel reverse mask from parameters */
-		sprintf(pname, "PWM_AUX_REV%d", i + 1);
+		sprintf(pname, pname_format, i + 1);
 		param_t param_h = param_find(pname);
 
 		if (param_h != PARAM_INVALID) {
+			int32_t ival = 0;
 			param_get(param_h, &ival);
 			_reverse_pwm_mask |= ((int16_t)(ival != 0)) << i;
 		}
@@ -950,15 +955,28 @@ PX4FMU::update_pwm_trims()
 
 		int16_t values[_max_actuators] = {};
 
+		const char *pname_format;
+
+		if (_class_instance == CLASS_DEVICE_PRIMARY) {
+			pname_format = "PWM_MAIN_TRIM%d";
+
+		} else if (_class_instance == CLASS_DEVICE_SECONDARY) {
+			pname_format = "PWM_AUX_TRIM%d";
+
+		} else {
+			PX4_ERR("PWM TRIM only for MAIN and AUX");
+			return;
+		}
+
 		for (unsigned i = 0; i < _max_actuators; i++) {
 			char pname[16];
-			float pval;
 
 			/* fill the struct from parameters */
-			sprintf(pname, "PWM_AUX_TRIM%d", i + 1);
+			sprintf(pname, pname_format, i + 1);
 			param_t param_h = param_find(pname);
 
 			if (param_h != PARAM_INVALID) {
+				float pval = 0.0f;
 				param_get(param_h, &pval);
 				values[i] = (int16_t)(10000 * pval);
 				PX4_DEBUG("%s: %d", pname, values[i]);