diff --git a/libraries/SRV_Channel/SRV_Channel.h b/libraries/SRV_Channel/SRV_Channel.h index 1648f1f5ac..4151bea78f 100644 --- a/libraries/SRV_Channel/SRV_Channel.h +++ b/libraries/SRV_Channel/SRV_Channel.h @@ -346,6 +346,9 @@ public: return i #include #include +#include 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<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; +} +