diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index 1144618b54..8107c1046b 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -391,6 +391,77 @@ static CMS_Menu cmsx_menuFilterPerProfile = { .entries = cmsx_menuFilterPerProfileEntries, }; +#ifdef USE_COPY_PROFILE_CMS_MENU + +static uint8_t cmsx_dstPidProfile; +static uint8_t cmsx_dstControlRateProfile; + +static const char * const cmsx_ProfileNames[] = { + "-", + "1", + "2", + "3" +}; + +static OSD_TAB_t cmsx_PidProfileTable = { &cmsx_dstPidProfile, 3, cmsx_ProfileNames }; +static OSD_TAB_t cmsx_ControlRateProfileTable = { &cmsx_dstControlRateProfile, 3, cmsx_ProfileNames }; + +static long cmsx_menuCopyProfile_onEnter(void) +{ + cmsx_dstPidProfile = 0; + cmsx_dstControlRateProfile = 0; + + return 0; +} + +static long cmsx_CopyPidProfile(displayPort_t *pDisplay, const void *ptr) +{ + UNUSED(pDisplay); + UNUSED(ptr); + + if (cmsx_dstPidProfile > 0) { + copyPidProfile(cmsx_dstPidProfile - 1, getCurrentPidProfileIndex()); + } + + return 0; +} + +static long cmsx_CopyControlRateProfile(displayPort_t *pDisplay, const void *ptr) +{ + UNUSED(pDisplay); + UNUSED(ptr); + + if (cmsx_dstControlRateProfile > 0) { + copyControlRateProfile(cmsx_dstControlRateProfile - 1, getCurrentControlRateProfileIndex()); + } + + return 0; +} + +static OSD_Entry cmsx_menuCopyProfileEntries[] = +{ + { "-- COPY PROFILE --", OME_Label, NULL, NULL, 0}, + + { "CPY PID PROF TO", OME_TAB, NULL, &cmsx_PidProfileTable, 0 }, + { "COPY PP", OME_Funcall, cmsx_CopyPidProfile, NULL, 0 }, + { "CPY RATE PROF TO", OME_TAB, NULL, &cmsx_ControlRateProfileTable, 0 }, + { "COPY RP", OME_Funcall, cmsx_CopyControlRateProfile, NULL, 0 }, + + { "BACK", OME_Back, NULL, NULL, 0 }, + { NULL, OME_END, NULL, NULL, 0 } +}; + +CMS_Menu cmsx_menuCopyProfile = { + .GUARD_text = "XCPY", + .GUARD_type = OME_MENU, + .onEnter = cmsx_menuCopyProfile_onEnter, + .onExit = NULL, + .onGlobalExit = NULL, + .entries = cmsx_menuCopyProfileEntries, +}; + +#endif + static OSD_Entry cmsx_menuImuEntries[] = { { "-- IMU --", OME_Label, NULL, NULL, 0}, @@ -404,6 +475,9 @@ static OSD_Entry cmsx_menuImuEntries[] = {"RATE", OME_Submenu, cmsMenuChange, &cmsx_menuRateProfile, 0}, {"FILT GLB", OME_Submenu, cmsMenuChange, &cmsx_menuFilterGlobal, 0}, +#ifdef USE_COPY_PROFILE_CMS_MENU + {"COPY PROF", OME_Submenu, cmsMenuChange, &cmsx_menuCopyProfile, 0}, +#endif {"BACK", OME_Back, NULL, NULL, 0}, {NULL, OME_END, NULL, NULL, 0} @@ -417,4 +491,5 @@ CMS_Menu cmsx_menuImu = { .onGlobalExit = NULL, .entries = cmsx_menuImuEntries, }; + #endif // CMS diff --git a/src/main/fc/controlrate_profile.c b/src/main/fc/controlrate_profile.c index c89c9ea8e2..b8a4c65d52 100644 --- a/src/main/fc/controlrate_profile.c +++ b/src/main/fc/controlrate_profile.c @@ -71,3 +71,11 @@ void changeControlRateProfile(uint8_t controlRateProfileIndex) setControlRateProfile(controlRateProfileIndex); generateThrottleCurve(); } + +void copyControlRateProfile(const uint8_t dstControlRateProfileIndex, const uint8_t srcControlRateProfileIndex) { + if ((dstControlRateProfileIndex < CONTROL_RATE_PROFILE_COUNT-1 && srcControlRateProfileIndex < CONTROL_RATE_PROFILE_COUNT-1) + && dstControlRateProfileIndex != srcControlRateProfileIndex + ) { + memcpy(controlRateProfilesMutable(dstControlRateProfileIndex), controlRateProfilesMutable(srcControlRateProfileIndex), sizeof(controlRateConfig_t)); + } +} diff --git a/src/main/fc/controlrate_profile.h b/src/main/fc/controlrate_profile.h index 2911907077..2978d9110a 100644 --- a/src/main/fc/controlrate_profile.h +++ b/src/main/fc/controlrate_profile.h @@ -41,3 +41,5 @@ extern controlRateConfig_t *currentControlRateProfile; void setControlRateProfile(uint8_t controlRateProfileIndex); void changeControlRateProfile(uint8_t controlRateProfileIndex); + +void copyControlRateProfile(const uint8_t dstControlRateProfileIndex, const uint8_t srcControlRateProfileIndex); diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 8281750ecc..e1329859ca 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1314,6 +1314,18 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) } break; + case MSP_COPY_PROFILE: + value = sbufReadU8(src); // 0 = pid profile, 1 = control rate profile + uint8_t dstProfileIndex = sbufReadU8(src); + uint8_t srcProfileIndex = sbufReadU8(src); + if (value == 0) { + copyPidProfile(dstProfileIndex, srcProfileIndex); + } + else if (value == 1) { + copyControlRateProfile(dstProfileIndex, srcProfileIndex); + } + break; + #if defined(GPS) || defined(MAG) case MSP_SET_HEADING: magHold = sbufReadU16(src); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index d723d0041d..842d172094 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -524,3 +524,11 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an } } } + +void copyPidProfile(const uint8_t dstPidProfileIndex, const uint8_t srcPidProfileIndex) { + if ((dstPidProfileIndex < MAX_PROFILE_COUNT-1 && srcPidProfileIndex < MAX_PROFILE_COUNT-1) + && dstPidProfileIndex != srcPidProfileIndex + ) { + memcpy(pidProfilesMutable(dstPidProfileIndex), pidProfilesMutable(srcPidProfileIndex), sizeof(pidProfile_t)); + } +} diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index e487e9e8cb..2baa17b60f 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -134,5 +134,6 @@ void pidSetItermAccelerator(float newItermAccelerator); void pidInitFilters(const pidProfile_t *pidProfile); void pidInitConfig(const pidProfile_t *pidProfile); void pidInit(const pidProfile_t *pidProfile); +void copyPidProfile(const uint8_t dstPidProfileIndex, const uint8_t srcPidProfileIndex); #endif diff --git a/src/main/msp/msp_protocol.h b/src/main/msp/msp_protocol.h index 4deda31f51..21bc7d3faf 100644 --- a/src/main/msp/msp_protocol.h +++ b/src/main/msp/msp_protocol.h @@ -223,6 +223,8 @@ // External OSD displayport mode messages #define MSP_DISPLAYPORT 182 +#define MSP_COPY_PROFILE 183 + #define MSP_BEEPER_CONFIG 184 #define MSP_SET_BEEPER_CONFIG 185 diff --git a/src/main/target/common_fc_pre.h b/src/main/target/common_fc_pre.h index c104118700..a6828777f9 100644 --- a/src/main/target/common_fc_pre.h +++ b/src/main/target/common_fc_pre.h @@ -125,6 +125,7 @@ #define VTX_TRAMP #define USE_CAMERA_CONTROL #define USE_HUFFMAN +#define USE_COPY_PROFILE_CMS_MENU #ifdef USE_SERIALRX_SPEKTRUM #define USE_SPEKTRUM_BIND