src/main/cms/cms_menu_mixer_servo.c

   1 /* This file is part of Cleanflight.
   2  *
   3  * Cleanflight is free software: you can redistribute it and/or modify
   4  * it under the terms of the GNU General Public License as published by
   5  * the Free Software Foundation, either version 3 of the License, or
   6  * (at your option) any later version.
   7  *
   8  * Cleanflight is distributed in the hope that it will be useful,
   9  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  11  * GNU General Public License for more details.
  12  *
  13  * You should have received a copy of the GNU General Public License
  14  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  15  */
  16
  17 //
  18 // Mixer and servo menu
  19 //
  20
  21 #include <stdbool.h>
  22 #include <stdint.h>
  23 #include <string.h>
  24 #include <ctype.h>
  25
  26 #include "platform.h"
  27
  28 #ifdef USE_CMS
  29
  30 #include "common/utils.h"
  31
  32 #include "build/version.h"
  33
  34 #include "flight/mixer_profile.h"
  35 #include "flight/mixer.h"
  36 #include "flight/servos.h"
  37
  38 #include "cms/cms.h"
  39 #include "cms/cms_types.h"
  40 #include "cms_menu_mixer_servo.h"
  41
  42 #include "fc/runtime_config.h"
  43 #include "fc/settings.h"
  44 #include "fc/config.h"
  45
  46 static uint8_t currentMotorMixerIndex = 0;
  47 static uint8_t tmpcurrentMotorMixerIndex = 1;
  48 static uint16_t tmpMotorMixerThrottle;
  49 static int16_t tmpMotorMixerRoll;
  50 static int16_t tmpMotorMixerYaw;
  51 static int16_t tmpMotorMixerPitch;
  52 static uint8_t currentServoMixerIndex = 0;
  53 static uint8_t tmpcurrentServoMixerIndex = 1;
  54 static servoMixer_t tmpServoMixer;
  55 static uint8_t currentServoIndex = 0;
  56 static uint8_t oldServoIndex = 0;
  57 static servoParam_t tmpServoParam;
  58
  59 static void loadServoSettings(void)
  60 {
  61     tmpServoParam.middle = servoParams(currentServoIndex)->middle;
  62     tmpServoParam.min = servoParams(currentServoIndex)->min;
  63     tmpServoParam.max = servoParams(currentServoIndex)->max;
  64     tmpServoParam.rate = servoParams(currentServoIndex)->rate;
  65 }
  66
  67 static void saveServoSettings(uint8_t idx)
  68 {
  69     servoParamsMutable(idx)->middle = tmpServoParam.middle;
  70     servoParamsMutable(idx)->min = tmpServoParam.min;
  71     servoParamsMutable(idx)->max = tmpServoParam.max;
  72     servoParamsMutable(idx)->rate = tmpServoParam.rate;
  73 }
  74
  75 static long cmsx_menuServo_onEnter(const OSD_Entry *from)
  76 {
  77     UNUSED(from);
  78
  79     loadServoSettings();
  80
  81     return 0;
  82 }
  83
  84 static long cmsx_menuServo_onExit(const OSD_Entry *from)
  85 {
  86     UNUSED(from);
  87
  88     saveServoSettings(currentServoIndex);
  89
  90     return 0;
  91 }
  92
  93 static long cmsx_menuServoIndexOnChange(displayPort_t *displayPort, const void *ptr)
  94 {
  95     UNUSED(displayPort);
  96     UNUSED(ptr);
  97
  98     saveServoSettings(oldServoIndex);
  99     loadServoSettings();
 100     oldServoIndex = currentServoIndex;
 101
 102     return 0;
 103 }
 104
 105 static const OSD_Entry cmsx_menuServoEntries[] =
 106 {
 107        OSD_LABEL_ENTRY("-- SERVOS --"),
 108        OSD_UINT8_CALLBACK_ENTRY("SERVO", cmsx_menuServoIndexOnChange, (&(const OSD_UINT8_t){ &currentServoIndex, 0, MAX_SUPPORTED_SERVOS - 1, 1})),
 109        OSD_INT16_DYN_ENTRY("MID", (&(const OSD_INT16_t){&tmpServoParam.middle, 500, 2500, 1})),
 110        OSD_INT16_DYN_ENTRY("MIN", (&(const OSD_INT16_t){&tmpServoParam.min, 500, 2500, 1})),
 111        OSD_INT16_DYN_ENTRY("MAX", (&(const OSD_INT16_t){&tmpServoParam.max, 500, 2500, 1})),
 112        OSD_INT8_DYN_ENTRY("RATE", (&(const OSD_INT8_t){&tmpServoParam.rate, -125, 125, 1})),
 113        OSD_BACK_AND_END_ENTRY
 114 };
 115
 116 const CMS_Menu cmsx_menuServo = {
 117 #ifdef CMS_MENU_DEBUG
 118     .GUARD_text = "MENUSERVO",
 119     .GUARD_type = OME_MENU,
 120 #endif
 121     .onEnter = cmsx_menuServo_onEnter,
 122     .onExit = cmsx_menuServo_onExit,
 123     .onGlobalExit = NULL,
 124     .entries = cmsx_menuServoEntries
 125 };
 126
 127 #define SERVO_MIXER_INPUT_CMS_NAMES_COUNT 23
 128 const char * const servoMixerInputCmsNames[SERVO_MIXER_INPUT_CMS_NAMES_COUNT] = {
 129         "STAB ROLL",
 130         "STAB PITCH",
 131         "STAB YAW",
 132         "STAB THROTTLE",
 133         "RC ROLL",
 134         "RC PITCH",
 135         "RC YAW",
 136         "RC THROTTLE",
 137         "RC CHAN 5",
 138         "RC CHAN 6",
 139         "RC CHAN 7",
 140         "RC CHAN 8",
 141         "GIMB PITCH",
 142         "GIMB ROLL",
 143         "FLAPS",
 144         "RC CHAN 9",
 145         "RC CHAN 10",
 146         "RC CHAN 11",
 147         "RC CHAN 12",
 148         "RC CHAN 13",
 149         "RC CHAN 14",
 150         "RC CHAN 15",
 151         "RC CHAN 16"
 152 };
 153
 154 static void loadServoMixerSettings(void)
 155 {
 156     tmpServoMixer.targetChannel = customServoMixers(currentServoMixerIndex)->targetChannel;
 157     tmpServoMixer.inputSource = customServoMixers(currentServoMixerIndex)->inputSource;
 158     tmpServoMixer.rate = customServoMixers(currentServoMixerIndex)->rate;
 159     tmpServoMixer.speed = customServoMixers(currentServoMixerIndex)->speed;
 160 }
 161
 162 static void saveServoMixerSettings(void)
 163 {
 164     customServoMixersMutable(currentServoMixerIndex)->targetChannel = tmpServoMixer.targetChannel;
 165     customServoMixersMutable(currentServoMixerIndex)->inputSource = tmpServoMixer.inputSource;
 166     customServoMixersMutable(currentServoMixerIndex)->rate = tmpServoMixer.rate;
 167     customServoMixersMutable(currentServoMixerIndex)->speed = tmpServoMixer.speed;
 168
 169 }
 170
 171 static long cmsx_menuServoMixer_onEnter(const OSD_Entry *from)
 172 {
 173     UNUSED(from);
 174
 175     loadServoMixerSettings();
 176
 177     return 0;
 178 }
 179
 180 static long cmsx_menuServoMixer_onExit(const OSD_Entry *from)
 181 {
 182     UNUSED(from);
 183
 184     saveServoMixerSettings();
 185
 186     return 0;
 187 }
 188
 189 static long cmsx_menuServoMixerIndexOnChange(displayPort_t *displayPort, const void *ptr)
 190 {
 191     UNUSED(displayPort);
 192     UNUSED(ptr);
 193
 194     saveServoMixerSettings();
 195     currentServoMixerIndex = tmpcurrentServoMixerIndex - 1;
 196     loadServoMixerSettings();
 197
 198     return 0;
 199 }
 200
 201 static const OSD_Entry cmsx_menuServoMixerEntries[] =
 202 {
 203        OSD_LABEL_ENTRY("-- SERVO MIXER --"),
 204        OSD_UINT8_CALLBACK_ENTRY("SERVO MIX", cmsx_menuServoMixerIndexOnChange, (&(const OSD_UINT8_t){ &tmpcurrentServoMixerIndex, 1, MAX_SERVO_RULES, 1})),
 205        OSD_UINT8_DYN_ENTRY("SERVO", (&(const OSD_UINT8_t){ &tmpServoMixer.targetChannel, 0, MAX_SUPPORTED_SERVOS, 1})),
 206        OSD_TAB_DYN_ENTRY("INPUT", (&(const OSD_TAB_t){ &tmpServoMixer.inputSource, SERVO_MIXER_INPUT_CMS_NAMES_COUNT - 1, servoMixerInputCmsNames})),
 207        OSD_INT16_DYN_ENTRY("WEIGHT", (&(const OSD_INT16_t){&tmpServoMixer.rate, -1000, 1000, 1})),
 208        OSD_UINT8_DYN_ENTRY("SPEED", (&(const OSD_UINT8_t){&tmpServoMixer.speed, 0, 255, 1})),
 209        OSD_BACK_AND_END_ENTRY
 210 };
 211
 212 const CMS_Menu cmsx_menuServoMixer = {
 213 #ifdef CMS_MENU_DEBUG
 214     .GUARD_text = "MENUSERVOMIXER",
 215     .GUARD_type = OME_MENU,
 216 #endif
 217     .onEnter = cmsx_menuServoMixer_onEnter,
 218     .onExit = cmsx_menuServoMixer_onExit,
 219     .onGlobalExit = NULL,
 220     .entries = cmsx_menuServoMixerEntries
 221 };
 222
 223 static void loadMotorMixerSettings(void)
 224 {
 225     tmpMotorMixerThrottle = primaryMotorMixer(currentMotorMixerIndex)->throttle * 1000;
 226     tmpMotorMixerRoll = primaryMotorMixer(currentMotorMixerIndex)->roll * 1000;
 227     tmpMotorMixerPitch = primaryMotorMixer(currentMotorMixerIndex)->pitch * 1000;
 228     tmpMotorMixerYaw = primaryMotorMixer(currentMotorMixerIndex)->yaw * 1000;
 229 }
 230
 231 static void saveMotorMixerSettings(void)
 232 {
 233     primaryMotorMixerMutable(currentMotorMixerIndex)->throttle = tmpMotorMixerThrottle / 1000.0f;
 234     primaryMotorMixerMutable(currentMotorMixerIndex)->roll = tmpMotorMixerRoll / 1000.0f;
 235     primaryMotorMixerMutable(currentMotorMixerIndex)->pitch = tmpMotorMixerPitch / 1000.0f;
 236     primaryMotorMixerMutable(currentMotorMixerIndex)->yaw = tmpMotorMixerYaw / 1000.0f;
 237 }
 238
 239 static long cmsx_menuMotorMixer_onEnter(const OSD_Entry *from)
 240 {
 241     UNUSED(from);
 242
 243     loadMotorMixerSettings();
 244
 245     return 0;
 246 }
 247
 248 static long cmsx_menuMotorMixer_onExit(const OSD_Entry *self)
 249 {
 250     UNUSED(self);
 251
 252     saveMotorMixerSettings();
 253
 254     return 0;
 255 }
 256
 257 static long cmsx_menuMotorMixerIndexOnChange(displayPort_t *displayPort, const void *ptr)
 258 {
 259     UNUSED(displayPort);
 260     UNUSED(ptr);
 261
 262     saveMotorMixerSettings();
 263     currentMotorMixerIndex = tmpcurrentMotorMixerIndex - 1;
 264     loadMotorMixerSettings();
 265
 266     return 0;
 267 }
 268
 269 static const OSD_Entry cmsx_motorMixerEntries[] =
 270 {
 271         OSD_LABEL_ENTRY("-- MOTOR MIXER --"),
 272         OSD_UINT8_CALLBACK_ENTRY ("MOTOR", cmsx_menuMotorMixerIndexOnChange, (&(const OSD_UINT8_t){ &tmpcurrentMotorMixerIndex, 1, MAX_SUPPORTED_MOTORS, 1})),
 273         OSD_UINT16_DYN_ENTRY("THROTTLE", (&(const OSD_UINT16_t){ &tmpMotorMixerThrottle, 0, 1000, 1 })),
 274         OSD_INT16_DYN_ENTRY("ROLL", (&(const OSD_INT16_t){ &tmpMotorMixerRoll, -2000, 2000, 1 })),
 275         OSD_INT16_DYN_ENTRY("PITCH", (&(const OSD_INT16_t){ &tmpMotorMixerPitch, -2000, 2000, 1 })),
 276         OSD_INT16_DYN_ENTRY("YAW", (&(const OSD_INT16_t){ &tmpMotorMixerYaw, -2000, 2000, 1 })),
 277         OSD_BACK_AND_END_ENTRY
 278 };
 279
 280 const CMS_Menu cmsx_menuMotorMixer = {
 281 #ifdef CMS_MENU_DEBUG
 282     .GUARD_text = "MENUMMOTORMIXER",
 283     .GUARD_type = OME_MENU,
 284 #endif
 285     .onEnter = cmsx_menuMotorMixer_onEnter,
 286     .onExit = cmsx_menuMotorMixer_onExit,
 287     .onGlobalExit = NULL,
 288     .entries = cmsx_motorMixerEntries
 289 };
 290
 291 static const OSD_Entry cmsx_mixerServoEntries[] =
 292 {
 293         OSD_LABEL_ENTRY("-- MIXER AND SERVOS --"),
 294         OSD_SETTING_ENTRY("PLATFORM", SETTING_PLATFORM_TYPE),
 295         OSD_SETTING_ENTRY("HAS FLAPS", SETTING_HAS_FLAPS),
 296         OSD_SUBMENU_ENTRY("MOTOR MIXER", &cmsx_menuMotorMixer),
 297         OSD_SUBMENU_ENTRY("SERVO MIXER", &cmsx_menuServoMixer),
 298         OSD_SUBMENU_ENTRY("SERVOS", &cmsx_menuServo),
 299         OSD_BACK_AND_END_ENTRY
 300 };
 301
 302 const CMS_Menu cmsx_menuMixerServo = {
 303 #ifdef CMS_MENU_DEBUG
 304     .GUARD_text = "MENUMIXERSERVO",
 305     .GUARD_type = OME_MENU,
 306 #endif
 307     .onEnter = NULL,
 308     .onExit = NULL,
 309     .onGlobalExit = NULL,
 310     .entries = cmsx_mixerServoEntries
 311 };
 312
 313 #endif