SRV_Channel: added automatic parameter upgrade

libraries/SRV_Channel/SRV_Channel.h

@@ -346,6 +346,9 @@ public:
         return i<NUM_SERVO_CHANNELS?&channels[i]:nullptr;
     }
 
+    // upgrade RC* parameters into SERVO* parameters
+    static bool upgrade_parameters(const uint8_t old_keys[14], uint16_t aux_channel_mask, RCMapper *rcmap);
+    
 private:
     struct {
         bool k_throttle_reversible:1;

libraries/SRV_Channel/SRV_Channel_aux.cpp

@@ -20,6 +20,7 @@
 #include <AP_Math/AP_Math.h>
 #include <AP_HAL/AP_HAL.h>
 #include <RC_Channel/RC_Channel.h>
+#include <AP_RCMapper/AP_RCMapper.h>
 
 extern const AP_HAL::HAL& hal;
 
@@ -595,3 +596,117 @@ void SRV_Channels::constrain_pwm(SRV_Channel::Aux_servo_function_t function)
         }
     }
 }
+
+/*
+  upgrade RC* parameters into SERVO* parameters. This does the following:
+
+  - copies MIN/MAX/TRIM values from old RC parameters into new RC* parameters and SERVO* parameters. 
+  - copies RCn_FUNCTION to SERVOn_FUNCTION
+  - maps old RCn_REV to SERVOn_REVERSE and RCn_REVERSE
+
+  aux_channel_mask is a bitmask of which channels were RC_Channel_aux channels
+
+  Note that this code is highly dependent on the parameter indexing of
+  the old RC_Channel and RC_Channel_aux objects.
+
+  If rcmap is passed in then the vehicle code also wants functions for
+  the first 4 output channels to be remapped
+
+  We return true if an upgrade has been done. This allows the caller
+  to make any vehicle specific upgrades that may be needed
+*/
+bool SRV_Channels::upgrade_parameters(const uint8_t rc_keys[14], uint16_t aux_channel_mask, RCMapper *rcmap)
+{
+    // use SERVO16_FUNCTION as a marker to say that we have run the upgrade already
+    if (channels[15].function.configured_in_storage()) {
+        // upgrade already done
+        return false;
+    }
+    
+    // old system had 14 RC channels
+    for (uint8_t i=0; i<14; i++) {
+        uint8_t k = rc_keys[i];
+        if (k == 0) {
+            // missing parameter. Some vehicle types didn't have all parameters
+            continue;
+        }
+        SRV_Channel &srv_chan = channels[i];
+        RC_Channel &rc_chan = RC_Channels::channels[i];
+        const struct mapping {
+            uint8_t old_index;
+            AP_Param *new_srv_param;
+            AP_Param *new_rc_param;
+            enum ap_var_type type;
+            bool is_reverse;
+        } mapping[] = {
+            { 0, &srv_chan.servo_min,  &rc_chan.radio_min,  AP_PARAM_INT16, false },
+            { 1, &srv_chan.servo_trim, &rc_chan.radio_trim, AP_PARAM_INT16, false },
+            { 2, &srv_chan.servo_max,  &rc_chan.radio_max,  AP_PARAM_INT16, false },
+            { 3, &srv_chan.reversed,   &rc_chan.reversed,   AP_PARAM_INT8,  true },
+            { 1, &srv_chan.function,   nullptr,             AP_PARAM_INT8,  false },
+        };
+        bool is_aux = aux_channel_mask & (1U<<i);
+        
+        for (uint8_t j=0; j<ARRAY_SIZE(mapping); j++) {
+            const struct mapping &m = mapping[j];
+            AP_Param::ConversionInfo info;
+            AP_Int8 v8;
+            AP_Int16 v16;
+            AP_Param *v = m.type == AP_PARAM_INT16?(AP_Param*)&v16:(AP_Param*)&v8;
+            info.old_key = k;
+            info.type = m.type;
+            info.new_name = nullptr;
+            // if this was an aux channel we need to shift by 6 bits, but not for RCn_FUNCTION
+            info.old_group_element = (is_aux && m.new_rc_param)?(m.old_index<<6):m.old_index;
+            
+            if (!AP_Param::find_old_parameter(&info, v)) {
+                // the parameter wasn't set in the old eeprom
+                continue;
+            }
+
+            if (m.is_reverse) {
+                // special mapping from RCn_REV to RCn_REVERSED
+                v8.set(v8.get() == -1?1:0);
+            }
+            
+            if (!m.new_srv_param->configured_in_storage()) {
+                // not configured yet in new eeprom
+                if (m.type == AP_PARAM_INT16) {
+                    ((AP_Int16 *)m.new_srv_param)->set_and_save_ifchanged(v16.get());
+                } else {
+                    ((AP_Int8 *)m.new_srv_param)->set_and_save_ifchanged(v8.get());
+                }
+            }
+            if (m.new_rc_param && !m.new_rc_param->configured_in_storage()) {
+                // not configured yet in new eeprom
+                if (m.type == AP_PARAM_INT16) {
+                    ((AP_Int16 *)m.new_rc_param)->set_and_save_ifchanged(v16.get());
+                } else {
+                    ((AP_Int8 *)m.new_rc_param)->set_and_save_ifchanged(v8.get());
+                }
+            }
+        }
+    }
+
+    if (rcmap != nullptr) {
+        // we need to make the output functions from the rcmapped inputs
+        const int8_t func_map[4] = { channels[0].function.get(),
+                                     channels[1].function.get(),
+                                     channels[2].function.get(),
+                                     channels[3].function.get() };
+        const uint8_t map[4] = { rcmap->roll(), rcmap->pitch(), rcmap->throttle(), rcmap->yaw() };
+        for (uint8_t i=0; i<4; i++) {
+            uint8_t m = uint8_t(map[i]-1);
+            if (m != i && m < 4) {
+                channels[m].function.set_and_save_ifchanged(func_map[i]);
+            }
+        }
+    }
+
+    
+    // mark the upgrade as having been done
+    channels[15].function.set_and_save(channels[15].function.get());
+
+    return true;
+}
+