radio/src/templates.cpp

   1 /*
   2  * Copyright (C) OpenTX
   3  *
   4  * Based on code named
   5  *   th9x - http://code.google.com/p/th9x
   6  *   er9x - http://code.google.com/p/er9x
   7  *   gruvin9x - http://code.google.com/p/gruvin9x
   8  *
   9  * License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
  10  *
  11  * This program is free software; you can redistribute it and/or modify
  12  * it under the terms of the GNU General Public License version 2 as
  13  * published by the Free Software Foundation.
  14  *
  15  * This program is distributed in the hope that it will be useful,
  16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  18  * GNU General Public License for more details.
  19  */
  20
  21 /*
  22  *
  23  * ============================================================
  24  * Templates file
  25  *
  26  * eccpm
  27  * crow
  28  * throttle cut
  29  * flaperon
  30  * elevon
  31  * v-tail
  32  * throttle hold
  33  * Aileron Differential
  34  * Spoilers
  35  * Snap Roll
  36  * ELE->Flap
  37  * Flap->ELE
  38  *
  39  */
  40
  41 #include "opentx.h"
  42
  43 #if defined(CPUARM)
  44   #pragma message("Templates are not implemented on this board")
  45 #endif
  46
  47 MixData* setDest(uint8_t dch, uint8_t src, bool clear=false)
  48 {
  49   uint8_t i = 0;
  50   MixData * mix;
  51
  52   while (1) {
  53     mix = mixAddress(i);
  54     if (mix->srcRaw && mix->destCh <= dch) {
  55       if (clear && mix->destCh == dch) {
  56         deleteExpoMix(0, i);
  57       }
  58       else {
  59         if (++i==MAX_MIXERS) {
  60           // TODO should return null pointer but needs to be tested then
  61           mix = mixAddress(0);
  62           break;
  63         }
  64       }
  65     }
  66     else {
  67       break;
  68     }
  69   }
  70
  71   memmove(mix+1, mix, (MAX_MIXERS-(i+1))*sizeof(MixData) );
  72   memclear(mix, sizeof(MixData));
  73   mix->destCh = dch;
  74   mix->srcRaw = src;
  75   mix->weight = 100;
  76   return mix;
  77 }
  78
  79 void mixSetWeight(MixData* md, int8_t weight)
  80 {
  81   u_int8int16_t tmp;
  82   tmp.word=weight;
  83   MD_UNION_TO_WEIGHT(tmp,md);
  84   // MD_SETWEIGHT(md,weight);  doesn't matter here in code cost compiler optimizes this anyway
  85 }
  86
  87 #if defined(PCBTARANIS)
  88   #define TMPL_INPUT(x) (MIXSRC_FIRST_INPUT+channel_order(x)-1)
  89 #else
  90   #define clearInputs()
  91   #define defaultInputs()
  92   #define TMPL_INPUT(x) (MIXSRC_Rud+x-1)
  93 #endif
  94
  95 void clearMixes()
  96 {
  97   memset(g_model.mixData, 0, sizeof(g_model.mixData)); // clear all mixes
  98 }
  99
 100 void clearCurves()
 101 {
 102   memclear(g_model.curves, sizeof(g_model.curves) + sizeof(g_model.points)); // clear all curves
 103 }
 104
 105 #if defined(CURVES)
 106 void setCurve(uint8_t c, const pm_int8_t ar[])
 107 {
 108   int8_t * cv = curveAddress(c);
 109   for (uint8_t i=0; i<5; i++) {
 110     cv[i] = pgm_read_byte(&ar[i]);
 111   }
 112 }
 113 #endif
 114
 115 void setLogicalSwitch(uint8_t idx, uint8_t func, int8_t v1, int8_t v2)
 116 {
 117   LogicalSwitchData *cs = lswAddress(idx-1);
 118   cs->func = func;
 119   cs->v1   = v1;
 120   cs->v2   = v2;
 121 }
 122
 123 const pm_int8_t heli_ar1[] PROGMEM = {-100, 20, 30, 70, 90};
 124 const pm_int8_t heli_ar2[] PROGMEM = {80, 70, 60, 70, 100};
 125 const pm_int8_t heli_ar3[] PROGMEM = {100, 90, 80, 90, 100};
 126 const pm_int8_t heli_ar4[] PROGMEM = {-30, -15, 0, 50, 100};
 127 const pm_int8_t heli_ar5[] PROGMEM = {-100, -50, 0, 50, 100};
 128
 129 void applyTemplate(uint8_t idx)
 130 {
 131     MixData *md;
 132
 133     //CC(STK)   -> vSTK
 134     //ICC(vSTK) -> STK
 135 #define ICC(x) icc[(x)-1]
 136     uint8_t icc[4] = {0};
 137     for (uint8_t i=0; i<4; i++) { //generate inverse array
 138       for(uint8_t j=0; j<4; j++)
 139         if(CC(i+1)==j+MIXSRC_Rud) icc[j]=i;
 140     }
 141
 142     switch (idx) {
 143       case TMPL_CLEAR_MIXES:
 144       case TMPL_SIMPLE_4CH:
 145 #if defined(HELI) && defined(CURVES)
 146       case TMPL_HELI_SETUP:
 147 #endif
 148         clearMixes();
 149         break;
 150     }
 151
 152     switch (idx) {
 153       // Simple 4-Ch
 154       case TMPL_SIMPLE_4CH:
 155         defaultInputs();
 156         setDest(ICC(STK_RUD), TMPL_INPUT(STK_RUD));
 157         setDest(ICC(STK_ELE), TMPL_INPUT(STK_ELE));
 158         setDest(ICC(STK_THR), TMPL_INPUT(STK_THR));
 159         setDest(ICC(STK_AIL), TMPL_INPUT(STK_AIL));
 160         break;
 161
 162       // Sticky-T-Cut
 163       case TMPL_STI_THR_CUT:
 164         md=setDest(ICC(STK_THR), MIXSRC_MAX); mixSetWeight(md, -100);  md->swtch=SWSRC_SWC;  md->mltpx=MLTPX_REP;
 165         md=setDest(13, MIXSRC_CH14); // md->weight= 100; done by setDest anyway
 166         md=setDest(13, MIXSRC_MAX); mixSetWeight(md, -100);  md->swtch=SWSRC_SWB;  md->mltpx=MLTPX_REP;
 167         md=setDest(13, MIXSRC_MAX); /* md->weight= 100;*/  md->swtch=SWSRC_THR;  md->mltpx=MLTPX_REP;
 168         setLogicalSwitch(11, LS_FUNC_VNEG, STK_THR, -99);
 169         setLogicalSwitch(12, LS_FUNC_VPOS, MIXSRC_CH14, 0);
 170         break;
 171
 172       // V-Tail
 173       case TMPL_V_TAIL:
 174         defaultInputs();
 175         setDest(ICC(STK_RUD), TMPL_INPUT(STK_RUD), true);
 176         md=setDest(ICC(STK_RUD), TMPL_INPUT(STK_ELE)); mixSetWeight(md, -100);
 177         setDest(ICC(STK_ELE), TMPL_INPUT(STK_RUD), true);
 178         setDest(ICC(STK_ELE), TMPL_INPUT(STK_ELE));
 179         break;
 180
 181       // Elevon\\Delta
 182       case TMPL_ELEVON_DELTA:
 183         defaultInputs();
 184         setDest(ICC(STK_ELE), MIXSRC_Ele, true);
 185         setDest(ICC(STK_ELE), MIXSRC_Ail);
 186         setDest(ICC(STK_AIL), MIXSRC_Ele, true);
 187         md=setDest(ICC(STK_AIL), MIXSRC_Ail); mixSetWeight(md, -100);
 188         break;
 189
 190       // eCCPM
 191       case TMPL_ECCPM:
 192         md=setDest(ICC(STK_ELE), MIXSRC_Ele, true); md->weight= 72;
 193         md=setDest(ICC(STK_ELE), MIXSRC_Thr);  md->weight= 55;
 194         md=setDest(ICC(STK_AIL), MIXSRC_Ele, true);  mixSetWeight(md, -36);
 195         md=setDest(ICC(STK_AIL), MIXSRC_Ail);  md->weight= 62;
 196         md=setDest(ICC(STK_AIL), MIXSRC_Thr);  md->weight= 55;
 197         md=setDest(5, MIXSRC_Ele, true);       mixSetWeight(md, -36);
 198         md=setDest(5, MIXSRC_Ail);             mixSetWeight(md, -62);
 199         md=setDest(5, MIXSRC_Thr);             md->weight= 55;
 200         break;
 201
 202 #if defined(HELI) && defined(CURVES)
 203       // Heli Setup
 204       case TMPL_HELI_SETUP:
 205         clearCurves();
 206
 207         //Set up Mixes
 208         // 3 cyclic channels
 209         md=setDest(0, MIXSRC_CYC1); // md->weight=100;
 210         md=setDest(1, MIXSRC_CYC2); // md->weight=100;
 211         md=setDest(2, MIXSRC_CYC3); // md->weight=100;
 212
 213         // rudder
 214         md=setDest(3, MIXSRC_Rud); // md->weight=100;
 215
 216         // throttle
 217 #if defined(PCBTARANIS)
 218         // TODO
 219 #else
 220         md=setDest(4, MIXSRC_Thr); md->swtch=SWSRC_ID0; mixSetCurve(md, 0); md->carryTrim=TRIM_OFF;
 221         md=setDest(4, MIXSRC_Thr); md->swtch=SWSRC_ID1; mixSetCurve(md, 1); md->carryTrim=TRIM_OFF;
 222         md=setDest(4, MIXSRC_Thr); md->swtch=SWSRC_ID2; mixSetCurve(md, 2); md->carryTrim=TRIM_OFF;
 223 #endif
 224         md=setDest(4, MIXSRC_MAX); mixSetWeight(md, -100); md->swtch=SWSRC_THR;  md->mltpx=MLTPX_REP;
 225
 226         // gyro gain
 227         md=setDest(5, MIXSRC_MAX); md->weight= 30; md->swtch=-SWSRC_GEA;
 228         md=setDest(5, MIXSRC_MAX); mixSetWeight(md, -30); md->swtch= SWSRC_GEA;
 229
 230         // collective
 231 #if defined(PCBTARANIS)
 232         // TODO
 233 #else
 234         md=setDest(10, MIXSRC_Thr); /*md->weight= 100;*/ md->swtch=SWSRC_ID0; mixSetCurve(md, 3); md->carryTrim=TRIM_OFF;
 235         md=setDest(10, MIXSRC_Thr); /*md->weight= 100;*/ md->swtch=SWSRC_ID1; mixSetCurve(md, 4); md->carryTrim=TRIM_OFF;
 236         md=setDest(10, MIXSRC_Thr); /*md->weight= 100;*/ md->swtch=SWSRC_ID2; mixSetCurve(md, 5); md->carryTrim=TRIM_OFF;
 237 #endif
 238
 239         g_model.swashR.collectiveSource = MIXSRC_CH11;
 240         g_model.swashR.type = SWASH_TYPE_120;
 241
 242         // curves
 243         setCurve(0, heli_ar1);
 244         setCurve(1, heli_ar2);
 245         setCurve(2, heli_ar3);
 246         setCurve(3, heli_ar4);
 247         setCurve(4, heli_ar5);
 248         setCurve(5, heli_ar5);
 249         break;
 250 #endif
 251
 252       // Servo Test
 253       case TMPL_SERVO_TEST:
 254         md=setDest(MAX_OUTPUT_CHANNELS-1, MIXSRC_SW1, true); md->weight=110; md->mltpx=MLTPX_ADD; md->delayUp = 6; md->delayDown = 6; md->speedUp = 8; md->speedDown = 8;
 255         setLogicalSwitch(1, LS_FUNC_VNEG, MIXSRC_LAST_CH, 0);
 256         break;
 257
 258     default:
 259         break;
 260
 261     }
 262
 263     storageDirty(EE_MODEL);
 264 }