src/main/flight/mixer_profile.c

   1 #include <stdbool.h>
   2 #include <stdint.h>
   3 #include <string.h>
   4
   5 #include "config/feature.h"
   6 #include "config/parameter_group.h"
   7 #include "config/parameter_group_ids.h"
   8 #include "config/config_reset.h"
   9
  10 #include "drivers/pwm_output.h"
  11 #include "drivers/pwm_mapping.h"
  12 #include "drivers/time.h"
  13 #include "flight/mixer.h"
  14 #include "common/axis.h"
  15 #include "flight/pid.h"
  16 #include "flight/servos.h"
  17 #include "flight/failsafe.h"
  18 #include "navigation/navigation.h"
  19 #include "navigation/navigation_private.h"
  20
  21 #include "fc/fc_core.h"
  22 #include "fc/config.h"
  23 #include "fc/runtime_config.h"
  24 #include "fc/settings.h"
  25 #include "fc/rc_modes.h"
  26 #include "fc/cli.h"
  27
  28 #include "programming/logic_condition.h"
  29 #include "navigation/navigation.h"
  30
  31 #include "common/log.h"
  32
  33 mixerConfig_t currentMixerConfig;
  34 int currentMixerProfileIndex;
  35 bool isMixerTransitionMixing;
  36 bool isMixerTransitionMixing_requested;
  37 mixerProfileAT_t mixerProfileAT;
  38 int nextMixerProfileIndex;
  39
  40 PG_REGISTER_ARRAY_WITH_RESET_FN(mixerProfile_t, MAX_MIXER_PROFILE_COUNT, mixerProfiles, PG_MIXER_PROFILE, 1);
  41
  42 void pgResetFn_mixerProfiles(mixerProfile_t *instance)
  43 {
  44     for (int i = 0; i < MAX_MIXER_PROFILE_COUNT; i++)
  45     {
  46         RESET_CONFIG(mixerProfile_t, &instance[i],
  47                      .mixer_config = {
  48                          .motorDirectionInverted = SETTING_MOTOR_DIRECTION_INVERTED_DEFAULT,
  49                          .platformType = SETTING_PLATFORM_TYPE_DEFAULT,
  50                          .hasFlaps = SETTING_HAS_FLAPS_DEFAULT,
  51                          .appliedMixerPreset = SETTING_MODEL_PREVIEW_TYPE_DEFAULT, // This flag is not available in CLI and used by Configurator only
  52                          .motorstopOnLow = SETTING_MOTORSTOP_ON_LOW_DEFAULT,
  53                          .PIDProfileLinking = SETTING_MIXER_PID_PROFILE_LINKING_DEFAULT,
  54                          .automated_switch = SETTING_MIXER_AUTOMATED_SWITCH_DEFAULT,
  55                          .switchTransitionTimer =  SETTING_MIXER_SWITCH_TRANS_TIMER_DEFAULT,
  56                          .tailsitterOrientationOffset = SETTING_TAILSITTER_ORIENTATION_OFFSET_DEFAULT,
  57                      });
  58         for (int j = 0; j < MAX_SUPPORTED_MOTORS; j++)
  59         {
  60             RESET_CONFIG(motorMixer_t, &instance[i].MotorMixers[j],
  61                          .throttle = 0,
  62                          .roll = 0,
  63                          .pitch = 0,
  64                          .yaw = 0);
  65         }
  66         for (int j = 0; j < MAX_SERVO_RULES; j++)
  67         {
  68             RESET_CONFIG(servoMixer_t, &instance[i].ServoMixers[j],
  69                          .targetChannel = 0,
  70                          .inputSource = 0,
  71                          .rate = 0,
  72                          .speed = 0
  73 #ifdef USE_PROGRAMMING_FRAMEWORK
  74                          ,
  75                          .conditionId = -1,
  76 #endif
  77             );
  78         }
  79     }
  80 }
  81
  82 void activateMixerConfig(void){
  83     currentMixerProfileIndex = getConfigMixerProfile();
  84     currentMixerConfig = *mixerConfig();
  85     nextMixerProfileIndex = (currentMixerProfileIndex + 1) % MAX_MIXER_PROFILE_COUNT;
  86 }
  87
  88 void mixerConfigInit(void)
  89 {
  90     activateMixerConfig();
  91     servosInit();
  92     mixerUpdateStateFlags();
  93     mixerInit();
  94     if (currentMixerConfig.PIDProfileLinking)
  95     {
  96         // LOG_INFO(PWM, "mixer switch pidInit");
  97         setConfigProfile(getConfigMixerProfile());
  98         pidInit();
  99         pidInitFilters();
 100         pidResetErrorAccumulators(); //should be safer to reset error accumulators
 101         schedulePidGainsUpdate();
 102         navigationUsePIDs(); // set navigation pid gains
 103     }
 104 }
 105
 106 void setMixerProfileAT(void)
 107 {
 108     mixerProfileAT.transitionStartTime = millis();
 109     mixerProfileAT.transitionTransEndTime = mixerProfileAT.transitionStartTime + (timeMs_t)currentMixerConfig.switchTransitionTimer * 100;
 110 }
 111
 112 bool platformTypeConfigured(flyingPlatformType_e platformType)
 113 {
 114     if (!isModeActivationConditionPresent(BOXMIXERPROFILE)){
 115         return false;
 116     }
 117     return mixerConfigByIndex(nextMixerProfileIndex)->platformType == platformType;
 118 }
 119
 120 bool checkMixerATRequired(mixerProfileATRequest_e required_action)
 121 {
 122     //return false if mixerAT condition is not required or setting is not valid
 123     if ((!STATE(AIRPLANE)) && (!STATE(MULTIROTOR)))
 124     {
 125         return false;
 126     }
 127     if (!isModeActivationConditionPresent(BOXMIXERPROFILE))
 128     {
 129         return false;
 130     }
 131
 132     if(currentMixerConfig.automated_switch){
 133         if ((required_action == MIXERAT_REQUEST_RTH) && STATE(MULTIROTOR))
 134         {
 135             return true;
 136         }
 137         if ((required_action == MIXERAT_REQUEST_LAND) && STATE(AIRPLANE))
 138         {
 139             return true;
 140         }
 141     }
 142     return false;
 143 }
 144
 145 bool mixerATUpdateState(mixerProfileATRequest_e required_action)
 146 {
 147     //return true if mixerAT is done or not required
 148     bool reprocessState;
 149     do
 150     {
 151         reprocessState=false;
 152         if (required_action==MIXERAT_REQUEST_ABORT){
 153             isMixerTransitionMixing_requested = false;
 154             mixerProfileAT.phase = MIXERAT_PHASE_IDLE;
 155             return true;
 156         }
 157         switch (mixerProfileAT.phase){
 158         case MIXERAT_PHASE_IDLE:
 159             //check if mixerAT is required
 160             if (checkMixerATRequired(required_action)){
 161                 mixerProfileAT.phase=MIXERAT_PHASE_TRANSITION_INITIALIZE;
 162                 reprocessState = true;
 163             }
 164             break;
 165         case MIXERAT_PHASE_TRANSITION_INITIALIZE:
 166             // LOG_INFO(PWM, "MIXERAT_PHASE_IDLE");
 167             setMixerProfileAT();
 168             mixerProfileAT.phase = MIXERAT_PHASE_TRANSITIONING;
 169             reprocessState = true;
 170             break;
 171         case MIXERAT_PHASE_TRANSITIONING:
 172             isMixerTransitionMixing_requested = true;
 173             if (millis() > mixerProfileAT.transitionTransEndTime){
 174                 isMixerTransitionMixing_requested = false;
 175                 outputProfileHotSwitch(nextMixerProfileIndex);
 176                 mixerProfileAT.phase = MIXERAT_PHASE_IDLE;
 177                 reprocessState = true;
 178                 //transition is done
 179             }
 180             return false;
 181             break;
 182         default:
 183             break;
 184         }
 185     }
 186     while (reprocessState);
 187     return true;
 188 }
 189
 190 bool checkMixerProfileHotSwitchAvalibility(void)
 191 {
 192     if (MAX_MIXER_PROFILE_COUNT != 2)
 193     {
 194         return false;
 195     }
 196     return true;
 197 }
 198
 199 void outputProfileUpdateTask(timeUs_t currentTimeUs)
 200 {
 201     UNUSED(currentTimeUs);
 202     if(cliMode) return;
 203     bool mixerAT_inuse = mixerProfileAT.phase != MIXERAT_PHASE_IDLE;
 204     // transition mode input for servo mix and motor mix
 205     if (!FLIGHT_MODE(FAILSAFE_MODE) && (!mixerAT_inuse))
 206     {
 207         if (isModeActivationConditionPresent(BOXMIXERPROFILE)){
 208             outputProfileHotSwitch(IS_RC_MODE_ACTIVE(BOXMIXERPROFILE) == 0 ? 0 : 1);
 209         }
 210         isMixerTransitionMixing_requested = IS_RC_MODE_ACTIVE(BOXMIXERTRANSITION);
 211     }
 212     isMixerTransitionMixing = isMixerTransitionMixing_requested && ((posControl.navState == NAV_STATE_IDLE) || mixerAT_inuse ||(posControl.navState == NAV_STATE_ALTHOLD_IN_PROGRESS));
 213 }
 214
 215 // switch mixerprofile without reboot
 216 bool outputProfileHotSwitch(int profile_index)
 217 {
 218     static bool allow_hot_switch = true;
 219     // LOG_INFO(PWM, "OutputProfileHotSwitch");
 220     if (!allow_hot_switch)
 221     {
 222         return false;
 223     }
 224     if (currentMixerProfileIndex == profile_index)
 225     {
 226         return false;
 227     }
 228     if (profile_index < 0 || profile_index >= MAX_MIXER_PROFILE_COUNT)
 229     { // sanity check
 230         // LOG_INFO(PWM, "invalid mixer profile index");
 231         return false;
 232     }
 233     if (areSensorsCalibrating())
 234     { // it seems like switching before sensors calibration complete will cause pid stops to respond, especially in D
 235         return false;
 236     }
 237     if (!checkMixerProfileHotSwitchAvalibility())
 238     {
 239         // LOG_INFO(PWM, "mixer switch failed, checkMixerProfileHotSwitchAvalibility");
 240         return false;
 241     }
 242     if  ((posControl.navState != NAV_STATE_IDLE) && (posControl.navState != NAV_STATE_MIXERAT_IN_PROGRESS))
 243     {
 244         // LOG_INFO(PWM, "mixer switch failed, navState != NAV_STATE_IDLE");
 245         return false;
 246     }
 247     if (!setConfigMixerProfile(profile_index))
 248     {
 249         // LOG_INFO(PWM, "mixer switch failed to set config");
 250         return false;
 251     }
 252     mixerConfigInit();
 253     return true;
 254 }