From 2dd94b7f0f7a9d5a4e2c929ae2a6c2ec65be7604 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 23 May 2015 15:18:13 +0200
Subject: [PATCH] FMU driver: auto param update

---
 src/drivers/px4fmu/fmu.cpp         | 43 +++++++++++++++++-------------
 src/drivers/px4fmu/px4fmu_params.c | 12 ++++-----
 2 files changed, 31 insertions(+), 24 deletions(-)

diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index 54c955bb05..c757dcc29b 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -171,9 +171,10 @@ private:
 					 uint8_t control_group,
 					 uint8_t control_index,
 					 float &input);
-	void	subscribe();
+	void		subscribe();
 	int		set_pwm_rate(unsigned rate_map, unsigned default_rate, unsigned alt_rate);
 	int		pwm_ioctl(file *filp, int cmd, unsigned long arg);
+	void		update_pwm_rev_mask();
 
 	struct GPIOConfig {
 		uint32_t	input;
@@ -549,6 +550,26 @@ PX4FMU::subscribe()
 	}
 }
 
+void
+PX4FMU::update_pwm_rev_mask()
+{
+	_reverse_pwm_mask = 0;
+
+	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);
+		param_t param_h = param_find(pname);
+
+		if (param_h != PARAM_INVALID) {
+			param_get(param_h, &ival);
+			_reverse_pwm_mask |= ((int16_t)(ival != 0)) << i;
+		}
+	}
+}
+
 void
 PX4FMU::task_main()
 {
@@ -574,7 +595,7 @@ PX4FMU::task_main()
 	/* initialize PWM limit lib */
 	pwm_limit_init(&_pwm_limit);
 
-	log("starting");
+	update_pwm_rev_mask();
 
 	/* loop until killed */
 	while (!_task_should_exit) {
@@ -731,25 +752,11 @@ PX4FMU::task_main()
 		}
 
 		orb_check(_param_sub, &updated);
-
 		if (updated) {
 			parameter_update_s pupdate;
 			orb_copy(ORB_ID(parameter_update), _param_sub, &pupdate);
-			_reverse_pwm_mask = 0;
-
-			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);
-				param_t param_h = param_find(pname);
-
-				if (param_h != PARAM_INVALID) {
-					param_get(param_h, &ival);
-					_reverse_pwm_mask |= ((int16_t)(ival != 0)) << i;
-				}
-			}
+			
+			update_pwm_rev_mask();
 		}
 
 #ifdef HRT_PPM_CHANNEL
diff --git a/src/drivers/px4fmu/px4fmu_params.c b/src/drivers/px4fmu/px4fmu_params.c
index 461d7cb2f8..4377c22e77 100644
--- a/src/drivers/px4fmu/px4fmu_params.c
+++ b/src/drivers/px4fmu/px4fmu_params.c
@@ -51,7 +51,7 @@
  * @max 1
  * @group PWM Outputs
  */
-PARAM_DEFINE_INT32(PWM_AUX_REV1, 1);
+PARAM_DEFINE_INT32(PWM_AUX_REV1, 0);
 
 /**
  * Inverter for main output channel 2
@@ -62,7 +62,7 @@ PARAM_DEFINE_INT32(PWM_AUX_REV1, 1);
  * @max 1
  * @group PWM Outputs
  */
-PARAM_DEFINE_INT32(PWM_AUX_REV2, 1);
+PARAM_DEFINE_INT32(PWM_AUX_REV2, 0);
 
 /**
  * Inverter for main output channel 3
@@ -73,7 +73,7 @@ PARAM_DEFINE_INT32(PWM_AUX_REV2, 1);
  * @max 1
  * @group PWM Outputs
  */
-PARAM_DEFINE_INT32(PWM_AUX_REV3, 1);
+PARAM_DEFINE_INT32(PWM_AUX_REV3, 0);
 
 /**
  * Inverter for main output channel 4
@@ -84,7 +84,7 @@ PARAM_DEFINE_INT32(PWM_AUX_REV3, 1);
  * @max 1
  * @group PWM Outputs
  */
-PARAM_DEFINE_INT32(PWM_AUX_REV4, 1);
+PARAM_DEFINE_INT32(PWM_AUX_REV4, 0);
 
 /**
  * Inverter for main output channel 5
@@ -95,7 +95,7 @@ PARAM_DEFINE_INT32(PWM_AUX_REV4, 1);
  * @max 1
  * @group PWM Outputs
  */
-PARAM_DEFINE_INT32(PWM_AUX_REV5, 1);
+PARAM_DEFINE_INT32(PWM_AUX_REV5, 0);
 
 /**
  * Inverter for main output channel 6
@@ -106,4 +106,4 @@ PARAM_DEFINE_INT32(PWM_AUX_REV5, 1);
  * @max 1
  * @group PWM Outputs
  */
-PARAM_DEFINE_INT32(PWM_AUX_REV6, 1);
+PARAM_DEFINE_INT32(PWM_AUX_REV6, 0);