From f9dd31b6e549b115b1e567234bed4a8086d3106f Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sat, 15 Oct 2016 21:41:04 +1100
Subject: [PATCH] RC_Channel: support automatic servo trimming

---
 libraries/RC_Channel/SRV_Channel.cpp | 43 ++++++++++++++++++++++++++++
 libraries/RC_Channel/SRV_Channel.h   | 15 ++++++++++
 2 files changed, 58 insertions(+)

diff --git a/libraries/RC_Channel/SRV_Channel.cpp b/libraries/RC_Channel/SRV_Channel.cpp
index b360ead189..e6f421652a 100644
--- a/libraries/RC_Channel/SRV_Channel.cpp
+++ b/libraries/RC_Channel/SRV_Channel.cpp
@@ -168,6 +168,13 @@ const AP_Param::GroupInfo SRV_Channels::var_info[] = {
     // @Values: -1:Reversed,1:Normal
     // @User: Advanced
     AP_GROUPINFO("4_REV",  17, SRV_Channels, reverse[3], 1),
+
+    // @Param: _AUTO_TRIM
+    // @DisplayName: Automatic servo trim
+    // @Description: This enables automatic servo trim in flight. Servos will be trimed in stabilized flight modes when the aircraft is close to level. Changes to servo trim will be saved every 10 seconds and will persist between flights.
+    // @Values: 0:Disable,1:Enable
+    // @User: Advanced
+    AP_GROUPINFO("_AUTO_TRIM",  18, SRV_Channels, auto_trim, 0),
     
     AP_GROUPEND
 };
@@ -269,3 +276,39 @@ void SRV_Channels::set_esc_scaling(uint8_t chnum)
         hal.rcout->set_esc_scaling(servo_min[chnum], servo_max[chnum]);
     }
 }
+
+/*
+  auto-adjust channel trim from an integrator value. Positive v means
+  adjust trim up. Negative means decrease
+ */
+void SRV_Channels::adjust_trim(uint8_t chnum, float v)
+{
+    if (reverse[chnum] == -1) {
+        v = -v;
+    }
+    uint16_t new_trim = servo_trim[chnum];
+    float trim_scaled = float(servo_trim[chnum] - servo_min[chnum]) / (servo_max[chnum] - servo_min[chnum]);
+    if (v > 0 && trim_scaled < 0.6f) {
+        new_trim++;
+    } else if (v < 0  && trim_scaled > 0.4f) {
+        new_trim--;
+    } else {
+        return;
+    }
+    servo_trim[chnum].set(new_trim);
+
+    trimmed_mask |= 1U<<chnum;
+}
+
+/*
+  save adjusted trims
+ */
+void SRV_Channels::save_trim(void)
+{
+    for (uint8_t i=0; i<NUM_SERVO_RANGE_CHANNELS; i++) {
+        if (trimmed_mask & (1U<<i)) {
+            servo_trim[i].set_and_save(servo_trim[i].get());
+        }
+    }
+    trimmed_mask = 0;
+}
diff --git a/libraries/RC_Channel/SRV_Channel.h b/libraries/RC_Channel/SRV_Channel.h
index 057b176ac3..2700cfd94e 100644
--- a/libraries/RC_Channel/SRV_Channel.h
+++ b/libraries/RC_Channel/SRV_Channel.h
@@ -39,12 +39,25 @@ public:
 
     // setup output ESC scaling for a channel
     void set_esc_scaling(uint8_t chnum);
+
+    // return true when enabled
+    bool enabled(void) const { return enable; }
+
+    // return true when auto_trim enabled
+    bool auto_trim_enabled(void) const { return auto_trim; }
+    
+    // adjust trim of a channel by a small increment
+    void adjust_trim(uint8_t ch, float v);
+
+    // save trims
+    void save_trim(void);
     
 private:
     AP_Int8 enable;
 
     int8_t esc_cal_chan;
     bool last_enable;
+    uint8_t trimmed_mask;
 
     // PWM values for min/max and trim
     AP_Int16 servo_min[NUM_SERVO_RANGE_CHANNELS];
@@ -54,6 +67,8 @@ private:
     // reversal, following convention that < 0 means reversed, >= 0 means normal
     AP_Int8 reverse[NUM_SERVO_RANGE_CHANNELS];
 
+    AP_Int8 auto_trim;
+
     // remap a PWM value from a channel in value
     uint16_t remap_pwm(uint8_t ch, uint16_t pwm) const;
 };