@@ -531,15 +531,37 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
531531
532532 case MSP_RC_TUNING :
533533 sbufWriteU8 (dst , 100 ); //rcRate8 kept for compatibity reasons, this setting is no longer used
534- sbufWriteU8 (dst , currentControlRateProfile -> rcExpo8 );
534+ sbufWriteU8 (dst , currentControlRateProfile -> stabilized . rcExpo8 );
535535 for (int i = 0 ; i < 3 ; i ++ ) {
536- sbufWriteU8 (dst , currentControlRateProfile -> rates [i ]); // R,P,Y see flight_dynamics_index_t
536+ sbufWriteU8 (dst , currentControlRateProfile -> stabilized .rates [i ]); // R,P,Y see flight_dynamics_index_t
537+ }
538+ sbufWriteU8 (dst , currentControlRateProfile -> throttle .dynPID );
539+ sbufWriteU8 (dst , currentControlRateProfile -> throttle .rcMid8 );
540+ sbufWriteU8 (dst , currentControlRateProfile -> throttle .rcExpo8 );
541+ sbufWriteU16 (dst , currentControlRateProfile -> throttle .pa_breakpoint );
542+ sbufWriteU8 (dst , currentControlRateProfile -> stabilized .rcYawExpo8 );
543+ break ;
544+
545+ case MSP2_INAV_RATE_PROFILE :
546+ // throttle
547+ sbufWriteU8 (dst , currentControlRateProfile -> throttle .rcMid8 );
548+ sbufWriteU8 (dst , currentControlRateProfile -> throttle .rcExpo8 );
549+ sbufWriteU8 (dst , currentControlRateProfile -> throttle .dynPID );
550+ sbufWriteU16 (dst , currentControlRateProfile -> throttle .pa_breakpoint );
551+
552+ // stabilized
553+ sbufWriteU8 (dst , currentControlRateProfile -> stabilized .rcExpo8 );
554+ sbufWriteU8 (dst , currentControlRateProfile -> stabilized .rcYawExpo8 );
555+ for (uint8_t i = 0 ; i < 3 ; ++ i ) {
556+ sbufWriteU8 (dst , currentControlRateProfile -> stabilized .rates [i ]); // R,P,Y see flight_dynamics_index_t
557+ }
558+
559+ // manual
560+ sbufWriteU8 (dst , currentControlRateProfile -> manual .rcExpo8 );
561+ sbufWriteU8 (dst , currentControlRateProfile -> manual .rcYawExpo8 );
562+ for (uint8_t i = 0 ; i < 3 ; ++ i ) {
563+ sbufWriteU8 (dst , currentControlRateProfile -> manual .rates [i ]); // R,P,Y see flight_dynamics_index_t
537564 }
538- sbufWriteU8 (dst , currentControlRateProfile -> dynThrPID );
539- sbufWriteU8 (dst , currentControlRateProfile -> thrMid8 );
540- sbufWriteU8 (dst , currentControlRateProfile -> thrExpo8 );
541- sbufWriteU16 (dst , currentControlRateProfile -> tpa_breakpoint );
542- sbufWriteU8 (dst , currentControlRateProfile -> rcYawExpo8 );
543565 break ;
544566
545567 case MSP_PID :
@@ -1348,23 +1370,23 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
13481370 if (dataSize >= 10 ) {
13491371 sbufReadU8 (src ); //Read rcRate8, kept for protocol compatibility reasons
13501372 // need to cast away const to set controlRateProfile
1351- ((controlRateConfig_t * )currentControlRateProfile )-> rcExpo8 = sbufReadU8 (src );
1373+ ((controlRateConfig_t * )currentControlRateProfile )-> stabilized . rcExpo8 = sbufReadU8 (src );
13521374 for (int i = 0 ; i < 3 ; i ++ ) {
13531375 rate = sbufReadU8 (src );
13541376 if (i == FD_YAW ) {
1355- ((controlRateConfig_t * )currentControlRateProfile )-> rates [i ] = constrain (rate , CONTROL_RATE_CONFIG_YAW_RATE_MIN , CONTROL_RATE_CONFIG_YAW_RATE_MAX );
1377+ ((controlRateConfig_t * )currentControlRateProfile )-> stabilized . rates [i ] = constrain (rate , CONTROL_RATE_CONFIG_YAW_RATE_MIN , CONTROL_RATE_CONFIG_YAW_RATE_MAX );
13561378 }
13571379 else {
1358- ((controlRateConfig_t * )currentControlRateProfile )-> rates [i ] = constrain (rate , CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN , CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX );
1380+ ((controlRateConfig_t * )currentControlRateProfile )-> stabilized . rates [i ] = constrain (rate , CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN , CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX );
13591381 }
13601382 }
13611383 rate = sbufReadU8 (src );
1362- ((controlRateConfig_t * )currentControlRateProfile )-> dynThrPID = MIN (rate , CONTROL_RATE_CONFIG_TPA_MAX );
1363- ((controlRateConfig_t * )currentControlRateProfile )-> thrMid8 = sbufReadU8 (src );
1364- ((controlRateConfig_t * )currentControlRateProfile )-> thrExpo8 = sbufReadU8 (src );
1365- ((controlRateConfig_t * )currentControlRateProfile )-> tpa_breakpoint = sbufReadU16 (src );
1384+ ((controlRateConfig_t * )currentControlRateProfile )-> throttle . dynPID = MIN (rate , CONTROL_RATE_CONFIG_TPA_MAX );
1385+ ((controlRateConfig_t * )currentControlRateProfile )-> throttle . rcMid8 = sbufReadU8 (src );
1386+ ((controlRateConfig_t * )currentControlRateProfile )-> throttle . rcExpo8 = sbufReadU8 (src );
1387+ ((controlRateConfig_t * )currentControlRateProfile )-> throttle . pa_breakpoint = sbufReadU16 (src );
13661388 if (dataSize >= 11 ) {
1367- ((controlRateConfig_t * )currentControlRateProfile )-> rcYawExpo8 = sbufReadU8 (src );
1389+ ((controlRateConfig_t * )currentControlRateProfile )-> stabilized . rcYawExpo8 = sbufReadU8 (src );
13681390 }
13691391
13701392 schedulePidGainsUpdate ();
@@ -1373,6 +1395,45 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
13731395 }
13741396 break ;
13751397
1398+ case MSP2_INAV_SET_RATE_PROFILE :
1399+ if (dataSize == 15 ) {
1400+ controlRateConfig_t * currentControlRateProfile_p = (controlRateConfig_t * )currentControlRateProfile ; // need to cast away const to set controlRateProfile
1401+
1402+ // throttle
1403+ currentControlRateProfile_p -> throttle .rcMid8 = sbufReadU8 (src );
1404+ currentControlRateProfile_p -> throttle .rcExpo8 = sbufReadU8 (src );
1405+ currentControlRateProfile_p -> throttle .dynPID = sbufReadU8 (src );
1406+ currentControlRateProfile_p -> throttle .pa_breakpoint = sbufReadU16 (src );
1407+
1408+ // stabilized
1409+ currentControlRateProfile_p -> stabilized .rcExpo8 = sbufReadU8 (src );
1410+ currentControlRateProfile_p -> stabilized .rcYawExpo8 = sbufReadU8 (src );
1411+ for (uint8_t i = 0 ; i < 3 ; ++ i ) {
1412+ rate = sbufReadU8 (src );
1413+ if (i == FD_YAW ) {
1414+ currentControlRateProfile_p -> stabilized .rates [i ] = constrain (rate , CONTROL_RATE_CONFIG_YAW_RATE_MIN , CONTROL_RATE_CONFIG_YAW_RATE_MAX );
1415+ } else {
1416+ currentControlRateProfile_p -> stabilized .rates [i ] = constrain (rate , CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN , CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX );
1417+ }
1418+ }
1419+
1420+ // manual
1421+ currentControlRateProfile_p -> manual .rcExpo8 = sbufReadU8 (src );
1422+ currentControlRateProfile_p -> manual .rcYawExpo8 = sbufReadU8 (src );
1423+ for (uint8_t i = 0 ; i < 3 ; ++ i ) {
1424+ rate = sbufReadU8 (src );
1425+ if (i == FD_YAW ) {
1426+ currentControlRateProfile_p -> manual .rates [i ] = constrain (rate , CONTROL_RATE_CONFIG_YAW_RATE_MIN , CONTROL_RATE_CONFIG_YAW_RATE_MAX );
1427+ } else {
1428+ currentControlRateProfile_p -> manual .rates [i ] = constrain (rate , CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN , CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX );
1429+ }
1430+ }
1431+
1432+ } else {
1433+ return MSP_RESULT_ERROR ;
1434+ }
1435+ break ;
1436+
13761437 case MSP_SET_MISC :
13771438 tmp = sbufReadU16 (src );
13781439 if (tmp < 1600 && tmp > 1400 )
0 commit comments