From ea83b67bd86ada93711327d780a0395512d127a0 Mon Sep 17 00:00:00 2001
From: Mark Whitehorn <kd0aij@gmail.com>
Date: Sun, 23 Oct 2016 10:07:09 -0600
Subject: [PATCH] add trim parameter handling
---
src/drivers/px4fmu/fmu.cpp | 41 ++++++++++++++++++++++++--------------
1 file changed, 26 insertions(+), 15 deletions(-)
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index be48dc61b7..c1953736b0 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -150,6 +150,8 @@ public:
hrt_abstime edge_time, uint32_t edge_state,
uint32_t overflow);
+ void update_pwm_trims();
+
private:
enum RC_SCAN {
RC_SCAN_PPM = 0,
@@ -756,6 +758,28 @@ PX4FMU::update_pwm_rev_mask()
}
}
+void
+PX4FMU::update_pwm_trims()
+{
+ struct pwm_output_values pwm;
+ for (unsigned i = 0; i < _max_actuators; i++) {
+ char pname[16];
+ int32_t ival;
+
+ /* fill the struct from parameters */
+ sprintf(pname, "PWM_AUX_TRIM%d", i + 1);
+ param_t param_h = param_find(pname);
+
+ if (param_h != PARAM_INVALID) {
+ param_get(param_h, &ival);
+ pwm.values[i] = ival;
+ }
+ }
+
+ /* copy the trim values to the mixer offsets */
+ _mixers->set_trims(pwm.values, _max_actuators);
+}
+
void
PX4FMU::publish_pwm_outputs(uint16_t *values, size_t numvalues)
{
@@ -1892,21 +1916,8 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
break;
}
- for (unsigned i = 0; i < pwm->channel_count; i++) {
- if (pwm->values[i] == 0) {
- /* allow 0 - turns the trim option off */
- _trim_pwm[i] = 0;
-
- } else if (pwm->values[i] < PWM_LOWEST_MAX) {
- _trim_pwm[i] = PWM_LOWEST_MAX;
-
- } else if (pwm->values[i] > PWM_HIGHEST_MAX) {
- _trim_pwm[i] = PWM_HIGHEST_MAX;
-
- } else {
- _trim_pwm[i] = pwm->values[i];
- }
- }
+ /* copy the trim values to the mixer offsets */
+ _mixers->set_trims(pwm->values, pwm->channel_count);
break;
}
--
GitLab