Pr pwm trim

src/drivers/px4fmu/fmu.cpp

@@ -265,7 +265,6 @@ class PX4FMU : public device::CDev, public ModuleBase<PX4FMU>
 	uint16_t	_disarmed_pwm[_max_actuators];
 	uint16_t	_min_pwm[_max_actuators];
 	uint16_t	_max_pwm[_max_actuators];
-	uint16_t	_trim_pwm[_max_actuators];
 	uint16_t	_reverse_pwm_mask;
 	unsigned	_num_failsafe_set;
 	unsigned	_num_disarmed_set;
@@ -391,7 +390,6 @@ PX4FMU::PX4FMU(bool run_as_task) :
 	for (unsigned i = 0; i < _max_actuators; i++) {
 		_min_pwm[i] = PWM_DEFAULT_MIN;
 		_max_pwm[i] = PWM_DEFAULT_MAX;
-		_trim_pwm[i] = PWM_DEFAULT_TRIM;
 	}
 
 	_control_topics[0] = ORB_ID(actuator_controls_0);
@@ -2157,12 +2155,8 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 	case PWM_SERVO_GET_TRIM_PWM: {
 			struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
 
-			for (unsigned i = 0; i < _max_actuators; i++) {
-				pwm->values[i] = _trim_pwm[i];
-			}
+			pwm->channel_count = _mixers->get_trims((int16_t *)pwm->values);
 
-			pwm->channel_count = _max_actuators;
-			arg = (unsigned long)&pwm;
 			break;
 		}
 

src/lib/mixer/mixer.h

@@ -204,6 +204,13 @@ class __EXPORT Mixer
 	 */
 	virtual unsigned set_trim(float trim) = 0;
 
+	/**
+	 * @brief Get trim offset for this mixer
+	 *
+	 * @return the number of outputs this mixer feeds to
+	 */
+	virtual unsigned get_trim(float *trim) = 0;
+
 	/*
 	 * @brief      Sets the thrust factor used to calculate mapping from desired thrust to pwm.
 	 *
@@ -382,6 +389,13 @@ class __EXPORT MixerGroup : public Mixer
 		return 0;
 	}
 
+	unsigned get_trims(int16_t *values);
+
+	unsigned get_trim(float *trim)
+	{
+		return 0;
+	}
+
 	/**
 	 * @brief      Sets the thrust factor used to calculate mapping from desired thrust to pwm.
 	 *
@@ -432,6 +446,11 @@ class __EXPORT NullMixer : public Mixer
 		return 0;
 	}
 
+	unsigned get_trim(float *trim)
+	{
+		return 0;
+	}
+
 };
 
 /**
@@ -507,6 +526,8 @@ class __EXPORT SimpleMixer : public Mixer
 
 	unsigned set_trim(float trim);
 
+	unsigned get_trim(float *trim);
+
 protected:
 
 private:
@@ -617,6 +638,11 @@ class __EXPORT MultirotorMixer : public Mixer
 		return _rotor_count;
 	}
 
+	unsigned get_trim(float *trim)
+	{
+		return _rotor_count;
+	}
+
 	/**
 	 * @brief      Sets the thrust factor used to calculate mapping from desired thrust to pwm.
 	 *
@@ -732,6 +758,11 @@ class __EXPORT HelicopterMixer : public Mixer
 		return 4;
 	}
 
+	unsigned get_trim(float *trim)
+	{
+		return 0;
+	}
+
 private:
 	mixer_heli_s			_mixer_info;
 

src/lib/mixer/mixer_group.cpp

@@ -132,10 +132,10 @@ MixerGroup::set_trims(int16_t *values, unsigned n)
 		/* convert from integer to float */
 		float offset = (float)values[index] / 10000;
 
-		/* to be safe, clamp offset to range of [-100, 100] usec */
-		if (offset < -0.2f) { offset = -0.2f; }
+		/* to be safe, clamp offset to range of [-500, 500] usec */
+		if (offset < -1.0f) { offset = -1.0f; }
 
-		if (offset >  0.2f) { offset =  0.2f; }
+		if (offset >  1.0f) { offset =  1.0f; }
 
 		debug("set trim: %d, offset: %5.3f", values[index], (double)offset);
 		index += mixer->set_trim(offset);
@@ -145,6 +145,37 @@ MixerGroup::set_trims(int16_t *values, unsigned n)
 	return index;
 }
 
+/*
+ * get_trims() has no effect except for the SimpleMixer implementation for which get_trim()
+ * always returns the value one and sets the trim value.
+ * The only other existing implementation is MultirotorMixer, which ignores the trim value
+ * and returns _rotor_count.
+ */
+unsigned
+MixerGroup::get_trims(int16_t *values)
+{
+	Mixer	*mixer = _first;
+	unsigned index_mixer = 0;
+	unsigned index = 0;
+	float trim;
+
+	while (mixer != nullptr) {
+		trim = 0;
+		index_mixer += mixer->get_trim(&trim);
+
+		// MultirotorMixer returns the number of motors so we
+		// loop through index_mixer and set the same trim value for all motors
+		while (index < index_mixer) {
+			values[index] = trim * 10000;
+			index++;
+		}
+
+		mixer = mixer->_next;
+	}
+
+	return index;
+}
+
 void
 MixerGroup::set_thrust_factor(float val)
 {

src/lib/mixer/mixer_simple.cpp

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