From 73b9123495e47d1cc9f384ab34ebf4adab385a21 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Wed, 12 Oct 2016 10:54:43 +1100
Subject: [PATCH] RC_Channel: added set_trim() API to SRV_Channels object

---
 libraries/RC_Channel/RC_Channel.cpp  | 14 +++---
 libraries/RC_Channel/RC_Channel.h    |  2 +-
 libraries/RC_Channel/SRV_Channel.cpp | 68 +++++++++++++++++++++-------
 libraries/RC_Channel/SRV_Channel.h   |  7 +++
 4 files changed, 66 insertions(+), 25 deletions(-)

diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp
index 68ade11a98..5ebdc584c1 100644
--- a/libraries/RC_Channel/RC_Channel.cpp
+++ b/libraries/RC_Channel/RC_Channel.cpp
@@ -582,28 +582,28 @@ bool RC_Channel::in_trim_dz()
 
   For range outputs the returned value is from 0 to 1
  */
-float RC_Channel::get_radio_out_normalised(void) const
+float RC_Channel::get_radio_out_normalised(uint16_t pwm) const
 {
     if (_radio_max <= _radio_min) {
         return 0;
     }
     float ret;
     if (_type_out == RC_CHANNEL_TYPE_RANGE) {
-        if (_radio_out <= _radio_min) {
+        if (pwm <= _radio_min) {
             ret = 0;
-        } else if (_radio_out >= _radio_max) {
+        } else if (pwm >= _radio_max) {
             ret = 1;
         } else {
-            ret = (_radio_out - _radio_min) / float(_radio_max - _radio_min);
+            ret = (pwm - _radio_min) / float(_radio_max - _radio_min);
         }
         if (_reverse == -1) {
             ret = 1 - ret;
         }
     } else {
-        if (_radio_out < _radio_trim) {
-            ret = -(_radio_trim - _radio_out) / float(_radio_trim - _radio_min);
+        if (pwm < _radio_trim) {
+            ret = -(_radio_trim - pwm) / float(_radio_trim - _radio_min);
         } else {
-            ret = (_radio_out - _radio_trim) / float(_radio_max - _radio_trim);
+            ret = (pwm - _radio_trim) / float(_radio_max - _radio_trim);
         }
         if (_reverse == -1) {
             ret = -ret;
diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h
index 28aed0779c..bb40a00aa4 100644
--- a/libraries/RC_Channel/RC_Channel.h
+++ b/libraries/RC_Channel/RC_Channel.h
@@ -155,7 +155,7 @@ public:
     
     // get the current radio_out value as a floating point number
     // normalised so that 1.0 is full output
-    float      get_radio_out_normalised() const;
+    float      get_radio_out_normalised(uint16_t pwm) const;
     
     bool min_max_configured()
     {
diff --git a/libraries/RC_Channel/SRV_Channel.cpp b/libraries/RC_Channel/SRV_Channel.cpp
index c6f73bce3f..88ee75719a 100644
--- a/libraries/RC_Channel/SRV_Channel.cpp
+++ b/libraries/RC_Channel/SRV_Channel.cpp
@@ -179,6 +179,33 @@ SRV_Channels::SRV_Channels(void)
 }
 
 
+// remap a PWM value from a channel in value
+uint16_t SRV_Channels::remap_pwm(uint8_t i, uint16_t pwm) const
+{
+    const RC_Channel *ch = RC_Channel::rc_channel(i);
+    if (ch == nullptr) {
+        return 0;
+    }
+    float v = ch->get_radio_out_normalised(pwm);
+    uint16_t radio_out;
+    if (ch->get_type_out() == RC_CHANNEL_TYPE_RANGE) {
+        if (reverse[i] == -1) {
+            v = 1 - v;
+        }
+        radio_out = servo_min[i] + v * (servo_max[i] - servo_min[i]);
+    } else {
+        if (reverse[i] == -1) {
+            v = -v;
+        }
+        if (v > 0) {
+            radio_out = servo_trim[i] + v * (servo_max[i] - servo_trim[i]);
+        } else {
+            radio_out = servo_trim[i] + v * (servo_trim[i] - servo_min[i]);
+        }
+    }
+    return radio_out;
+}
+
 /*
   remap radio_out values for first 4 servos using SERVO* parameters, if enabled
   This should be called with cork enabled in hal.rcout
@@ -193,24 +220,31 @@ void SRV_Channels::remap_servo_output(void)
         if (ch == nullptr) {
             continue;
         }
-        float v = ch->get_radio_out_normalised();
-        uint16_t radio_out;
-        if (ch->get_type_out() == RC_CHANNEL_TYPE_RANGE) {
-            if (reverse[i] == -1) {
-                v = 1 - v;
-            }
-            radio_out = servo_min[i] + v * (servo_max[i] - servo_min[i]);
-        } else {
-            if (reverse[i] == -1) {
-                v = -v;
-            }
-            if (v > 0) {
-                radio_out = servo_trim[i] + v * (servo_max[i] - servo_trim[i]);
-            } else {
-                radio_out = servo_trim[i] + v * (servo_trim[i] - servo_min[i]);
-            }
-        }
+        uint16_t radio_out = remap_pwm(i, ch->get_radio_out());
         ch->set_radio_out(radio_out);
         ch->output();
     }
 }
+
+
+/*
+  set trim values for output channels
+ */
+void SRV_Channels::set_trim(void)
+{
+    if (!enable) {
+        return;
+    }
+    for (uint8_t i=0; i<NUM_SERVO_RANGE_CHANNELS; i++) {
+        const RC_Channel *ch = RC_Channel::rc_channel(i);
+        if (ch == nullptr) {
+            continue;
+        }
+        if (ch->get_type_out() == RC_CHANNEL_TYPE_RANGE) {
+            // we don't trim range channels (like throttle)
+            continue;
+        }
+        uint16_t new_trim = remap_pwm(i, ch->get_radio_trim());
+        servo_trim[i].set_and_save(new_trim);
+    }
+}
diff --git a/libraries/RC_Channel/SRV_Channel.h b/libraries/RC_Channel/SRV_Channel.h
index fc21b17926..0142a129ce 100644
--- a/libraries/RC_Channel/SRV_Channel.h
+++ b/libraries/RC_Channel/SRV_Channel.h
@@ -16,6 +16,7 @@
 
 #include <AP_Common/AP_Common.h>
 #include <AP_Param/AP_Param.h>
+#include "RC_Channel.h"
 
 #define NUM_SERVO_RANGE_CHANNELS 4
 
@@ -32,6 +33,9 @@ public:
     // take current radio_out for first 4 channels and remap using
     // servo ranges if enabled
     void remap_servo_output(void);
+
+    // set and save trim values from current RC input trim
+    void set_trim(void);
     
 private:
     AP_Int8 enable;
@@ -43,4 +47,7 @@ private:
 
     // reversal, following convention that < 0 means reversed, >= 0 means normal
     AP_Int8 reverse[NUM_SERVO_RANGE_CHANNELS];
+
+    // remap a PWM value from a channel in value
+    uint16_t remap_pwm(uint8_t ch, uint16_t pwm) const;
 };