use trim values to set mixer:scaler.offset

clamp mixer output offset to [-.2,.2] ([-2000, 2000] in mixer file)

add 8 main PWM trim parameters

add long desc to parameters and bump minor parameter version

Tools/px4params/xmlout.py

@@ -25,7 +25,7 @@ class XMLOutput():
         xml_version = ET.SubElement(xml_parameters, "parameter_version_major")
         xml_version.text = "1"
         xml_version = ET.SubElement(xml_parameters, "parameter_version_minor")
-        xml_version.text = "9"
+        xml_version.text = "10"
         importtree = ET.parse(inject_xml_file_name)
         injectgroups = importtree.getroot().findall("group")
         for igroup in injectgroups:

src/drivers/px4io/px4io.cpp

@@ -1204,6 +1204,30 @@ PX4IO::task_main()
 
 				(void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_REVERSE, pwm_invert_mask);
 
+				// update trim values
+				struct pwm_output_values pwm_values;
+
+				memset(&pwm_values, 0, sizeof(pwm_values));
+				ret = io_reg_get(PX4IO_PAGE_CONTROL_TRIM_PWM, 0, (uint16_t *)pwm_values.values, _max_actuators);
+
+				for (unsigned i = 0; i < _max_actuators; i++) {
+					char pname[16];
+					int32_t ival;
+
+					/* fetch the trim values from parameters */
+					sprintf(pname, "PWM_MAIN_TRIM%u", i + 1);
+					param_t param_h = param_find(pname);
+
+					if (param_h != PARAM_INVALID) {
+
+						param_get(param_h, &ival);
+						pwm_values.values[i] = ival;
+					}
+				}
+
+				/* copy values to registers in IO */
+				ret = io_reg_set(PX4IO_PAGE_CONTROL_TRIM_PWM, 0, pwm_values.values, _max_actuators);
+
 				float param_val;
 				param_t parm_handle;
 

src/drivers/px4io/px4io_params.c

@@ -130,6 +130,94 @@ PARAM_DEFINE_INT32(PWM_MAIN_REV7, 0);
  */
 PARAM_DEFINE_INT32(PWM_MAIN_REV8, 0);
 
+/**
+ * Trim value for main output channel 1
+ *
+ * Set to neutral period in usec
+ *
+ * @min 1400
+ * @max 1600
+ * @group PWM Outputs
+ */
+PARAM_DEFINE_INT32(PWM_MAIN_TRIM1, 1500);
+
+/**
+ * Trim value for main output channel 2
+ *
+ * Set to neutral period in usec
+ *
+ * @min 1400
+ * @max 1600
+ * @group PWM Outputs
+ */
+PARAM_DEFINE_INT32(PWM_MAIN_TRIM2, 1500);
+
+/**
+ * Trim value for main output channel 3
+ *
+ * Set to neutral period in usec
+ *
+ * @min 1400
+ * @max 1600
+ * @group PWM Outputs
+ */
+PARAM_DEFINE_INT32(PWM_MAIN_TRIM3, 1500);
+
+/**
+ * Trim value for main output channel 4
+ *
+ * Set to neutral period in usec
+ *
+ * @min 1400
+ * @max 1600
+ * @group PWM Outputs
+ */
+PARAM_DEFINE_INT32(PWM_MAIN_TRIM4, 1500);
+
+/**
+ * Trim value for main output channel 5
+ *
+ * Set to neutral period in usec
+ *
+ * @min 1400
+ * @max 1600
+ * @group PWM Outputs
+ */
+PARAM_DEFINE_INT32(PWM_MAIN_TRIM5, 1500);
+
+/**
+ * Trim value for main output channel 6
+ *
+ * Set to neutral period in usec
+ *
+ * @min 1400
+ * @max 1600
+ * @group PWM Outputs
+ */
+PARAM_DEFINE_INT32(PWM_MAIN_TRIM6, 1500);
+
+/**
+ * Trim value for main output channel 7
+ *
+ * Set to neutral period in usec
+ *
+ * @min 1400
+ * @max 1600
+ * @group PWM Outputs
+ */
+PARAM_DEFINE_INT32(PWM_MAIN_TRIM7, 1500);
+
+/**
+ * Trim value for main output channel 8
+ *
+ * Set to neutral period in usec
+ *
+ * @min 1400
+ * @max 1600
+ * @group PWM Outputs
+ */
+PARAM_DEFINE_INT32(PWM_MAIN_TRIM8, 1500);
+
 /**
  * S.BUS out
  *

src/modules/px4iofirmware/mixer.cpp

@@ -212,6 +212,13 @@ mixer_tick(void)
 		r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FAILSAFE);
 	}
 
+	/*
+	 * Set simple mixer trim values
+	 * (there should be a "dirty" flag to indicate that r_page_servo_control_trim has changed)
+	 */
+	mixer_group.set_trims(r_page_servo_control_trim, PX4IO_SERVO_COUNT);
+
+
 	/*
 	 * Run the mixers.
 	 */

src/modules/systemlib/mixer/mixer.h

@@ -192,6 +192,7 @@ public:
 	 */
 	virtual void 			set_max_delta_out_once(float delta_out_max) {};
 
+	virtual unsigned set_trim(float trim) = 0;
 
 protected:
 	/** client-supplied callback used when fetching control values */
@@ -333,6 +334,17 @@ public:
 	 */
 	virtual void 			set_max_delta_out_once(float delta_out_max);
 
+	/*
+	 * Invoke the set_offset method of each mixer in the group
+	 * for each value in page r_page_servo_control_trim
+	 */
+	unsigned set_trims(uint16_t *v, unsigned n);
+
+	unsigned set_trim(float trim)
+	{
+		return 0;
+	}
+
 private:
 	Mixer				*_first;	/**< linked list of mixers */
 
@@ -369,6 +381,12 @@ public:
 
 	virtual unsigned		mix(float *outputs, unsigned space, uint16_t *status_reg);
 	virtual void			groups_required(uint32_t &groups);
+	virtual void 			set_offset(float trim) {};
+	unsigned set_trim(float trim)
+	{
+		return 0;
+	}
+
 };
 
 /**
@@ -445,6 +463,8 @@ public:
 	 */
 	int				check();
 
+	unsigned set_trim(float trim);
+
 protected:
 
 private:
@@ -551,6 +571,11 @@ public:
 	 */
 	virtual void 			set_max_delta_out_once(float delta_out_max) {_delta_out_max = delta_out_max;}
 
+	unsigned set_trim(float trim)
+	{
+		return _rotor_count;
+	}
+
 private:
 	float				_roll_scale;
 	float				_pitch_scale;

src/modules/systemlib/mixer/mixer_group.cpp

@@ -112,6 +112,28 @@ MixerGroup::mix(float *outputs, unsigned space, uint16_t *status_reg)
 	return index;
 }
 
+unsigned
+MixerGroup::set_trims(uint16_t *values, unsigned n)
+{
+	Mixer	*mixer = _first;
+	unsigned index = 0;
+
+	while ((mixer != nullptr) && (index < n)) {
+		/* hardwired assumption that PWM output range is [1000, 2000] usec */
+		float offset = ((float)values[index] - 1500) / 500;
+
+		/* to be safe, clamp offset to range of [-100, 100] usec */
+		if (offset < -0.2f) { offset = -0.2f; }
+
+		if (offset >  0.2f) { offset =  0.2f; }
+
+		index += mixer->set_trim(offset);
+		mixer = mixer->_next;
+	}
+
+	return index;
+}
+
 unsigned
 MixerGroup::count()
 {

src/modules/systemlib/mixer/mixer_simple.cpp

@@ -72,6 +72,12 @@ SimpleMixer::~SimpleMixer()
 	}
 }
 
+unsigned SimpleMixer::set_trim(float trim)
+{
+	_pinfo->output_scaler.offset = trim;
+	return 1;
+}
+
 int
 SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler)
 {

src/modules/systemlib/pwm_limit/pwm_limit.c

@@ -206,15 +206,15 @@ void pwm_limit_calc(const bool armed, const bool pre_armed, const unsigned num_c
 				control_value = -1.0f * control_value;
 			}
 
-			if (trim_pwm[i] == 0) {
-				effective_pwm[i] = control_value * (max_pwm[i] - min_pwm[i]) / 2 + (max_pwm[i] + min_pwm[i]) / 2;
+//			if (trim_pwm[i] == 0) {
+			effective_pwm[i] = control_value * (max_pwm[i] - min_pwm[i]) / 2 + (max_pwm[i] + min_pwm[i]) / 2;
 
-			} else if (control_value < 0) {
-				effective_pwm[i] = control_value * (trim_pwm[i] - min_pwm[i]) + trim_pwm[i];
-
-			} else {
-				effective_pwm[i] = control_value * (max_pwm[i] - trim_pwm[i]) + trim_pwm[i];
-			}
+//			} else if (control_value < 0) {
+//				effective_pwm[i] = control_value * (trim_pwm[i] - min_pwm[i]) + trim_pwm[i];
+//
+//			} else {
+//				effective_pwm[i] = control_value * (max_pwm[i] - trim_pwm[i]) + trim_pwm[i];
+//			}
 
 			/* last line of defense against invalid inputs */
 			if (effective_pwm[i] < min_pwm[i]) {