From 6b2f5970eb27bb39f2a84782acbbf55cdc4a0fa2 Mon Sep 17 00:00:00 2001
From: Dennis Shtatnov <densht@gmail.com>
Date: Sun, 2 Apr 2017 15:52:53 -0400
Subject: [PATCH] FMU get initial values of parameters

---
 src/drivers/px4fmu/fmu.cpp | 74 +++++++++++++++++++++-----------------
 1 file changed, 41 insertions(+), 33 deletions(-)

diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index 524a5f739c..1c1d92bb8a 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -271,6 +271,7 @@ private:
 	void		publish_pwm_outputs(uint16_t *values, size_t numvalues);
 	void		update_pwm_out_state(bool on);
 	void		pwm_output_set(unsigned i, unsigned value);
+	void		update_params();
 
 	struct GPIOConfig {
 		uint32_t	input;
@@ -1081,8 +1082,9 @@ PX4FMU::cycle()
 #  endif
 #endif
 
-			param_find("MOT_SLEW_MAX");
-			param_find("THR_MDL_FAC");
+
+			// Getting initial parameter values
+			this->update_params();
 
 			for (unsigned i = 0; i < _max_actuators; i++) {
 				char pname[16];
@@ -1425,37 +1427,7 @@ PX4FMU::cycle()
 		orb_check(_param_sub, &updated);
 
 		if (updated) {
-			parameter_update_s pupdate;
-			orb_copy(ORB_ID(parameter_update), _param_sub, &pupdate);
-
-			update_pwm_rev_mask();
-			update_pwm_trims();
-
-			int32_t dsm_bind_val;
-			param_t param_handle;
-
-			/* see if bind parameter has been set, and reset it to -1 */
-			param_get(param_handle = param_find("RC_DSM_BIND"), &dsm_bind_val);
-
-			if (dsm_bind_val > -1) {
-				dsm_bind_ioctl(dsm_bind_val);
-				dsm_bind_val = -1;
-				param_set(param_handle, &dsm_bind_val);
-			}
-
-			// maximum motor slew rate parameter
-			param_handle = param_find("MOT_SLEW_MAX");
-
-			if (param_handle != PARAM_INVALID) {
-				param_get(param_handle, &_mot_t_max);
-			}
-
-			// thrust to pwm modelling factor
-			param_handle = param_find("THR_MDL_FAC");
-
-			if (param_handle != PARAM_INVALID) {
-				param_get(param_handle, &_thr_mdl_fac);
-			}
+			this->update_params();
 		}
 
 		/* update ADC sampling */
@@ -1735,6 +1707,42 @@ PX4FMU::cycle()
 	}
 }
 
+void PX4FMU::update_params()
+{
+	parameter_update_s pupdate;
+	orb_copy(ORB_ID(parameter_update), _param_sub, &pupdate);
+
+	update_pwm_rev_mask();
+	update_pwm_trims();
+
+	int32_t dsm_bind_val;
+	param_t param_handle;
+
+	/* see if bind parameter has been set, and reset it to -1 */
+	param_get(param_handle = param_find("RC_DSM_BIND"), &dsm_bind_val);
+
+	if (dsm_bind_val > -1) {
+		dsm_bind_ioctl(dsm_bind_val);
+		dsm_bind_val = -1;
+		param_set(param_handle, &dsm_bind_val);
+	}
+
+	// maximum motor slew rate parameter
+	param_handle = param_find("MOT_SLEW_MAX");
+
+	if (param_handle != PARAM_INVALID) {
+		param_get(param_handle, &_mot_t_max);
+	}
+
+	// thrust to pwm modelling factor
+	param_handle = param_find("THR_MDL_FAC");
+
+	if (param_handle != PARAM_INVALID) {
+		param_get(param_handle, &_thr_mdl_fac);
+	}
+}
+
+
 void PX4FMU::stop()
 {
 	/* Signal that we want to stop the task or work.