radio/src/curves.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 #include "opentx.h"
  22
  23 uint8_t s_curveChan;
  24
  25 #if defined(CPUARM)
  26 int8_t * curveEnd[MAX_CURVES];
  27 void loadCurves()
  28 {
  29   bool showWarning= false;
  30   int8_t * tmp = g_model.points;
  31   for (int i=0; i<MAX_CURVES; i++) {
  32     switch (g_model.curves[i].type) {
  33       case CURVE_TYPE_STANDARD:
  34         tmp += 5+g_model.curves[i].points;
  35         break;
  36       case CURVE_TYPE_CUSTOM:
  37         tmp += 8+2*g_model.curves[i].points;
  38         break;
  39       default:
  40         TRACE("Wrong curve type! Fixing...");
  41         g_model.curves[i].type = CURVE_TYPE_STANDARD;
  42         tmp += 5+g_model.curves[i].points;
  43         break;
  44     }
  45     // Older version did not check if we exceeded the array
  46     int8_t * maxend = &g_model.points[MAX_CURVE_POINTS - 2*(MAX_CURVES-i-1)];
  47     if (tmp > maxend) {
  48       tmp = maxend;
  49       g_model.curves[i].type=CURVE_TYPE_STANDARD;
  50       g_model.curves[i].points=-3;
  51       showWarning=true;
  52     }
  53     curveEnd[i] = tmp;
  54
  55   }
  56   if (showWarning) {
  57     POPUP_WARNING("Invalid curve data repaired");
  58     const char * w = "check your curves, logic switches";
  59     SET_WARNING_INFO(w, strlen(w), 0);
  60   }
  61 }
  62
  63 int8_t * curveAddress(uint8_t idx)
  64 {
  65   return idx==0 ? g_model.points : curveEnd[idx-1];
  66 }
  67
  68 bool moveCurve(uint8_t index, int8_t shift)
  69 {
  70   if (curveEnd[MAX_CURVES-1] + shift > g_model.points + sizeof(g_model.points)) {
  71     AUDIO_WARNING2();
  72     return false;
  73   }
  74
  75   int8_t * nextCrv = curveAddress(index+1);
  76   memmove(nextCrv+shift, nextCrv, 5*(MAX_CURVES-index-1)+curveEnd[MAX_CURVES-1]-curveEnd[index]);
  77   if (shift < 0) memclear(&g_model.points[MAX_CURVE_POINTS-1] + shift, -shift);
  78   while (index<MAX_CURVES) {
  79     curveEnd[index++] += shift;
  80   }
  81
  82   storageDirty(EE_MODEL);
  83   return true;
  84 }
  85
  86 int8_t getCurveX(int noPoints, int point)
  87 {
  88   return -100 + div_and_round((point*2000) / (noPoints-1), 10);
  89 }
  90
  91 void resetCustomCurveX(int8_t * points, int noPoints)
  92 {
  93   for (int i=0; i<noPoints-2; i++) {
  94     points[noPoints+i] = getCurveX(noPoints, i+1);
  95   }
  96 }
  97 #else
  98 int8_t * curveAddress(uint8_t idx)
  99 {
 100   return &g_model.points[idx==0 ? 0 : 5*idx+g_model.curves[idx-1]];
 101 }
 102
 103 CurveInfo curveInfo(uint8_t idx)
 104 {
 105   CurveInfo result;
 106   result.crv = curveAddress(idx);
 107   int8_t *next = curveAddress(idx+1);
 108   uint8_t size = next - result.crv;
 109   if ((size & 1) == 0) {
 110     result.points = (size / 2) + 1;
 111     result.custom = true;
 112   }
 113   else {
 114     result.points = size;
 115     result.custom = false;
 116   }
 117   return result;
 118 }
 119
 120 bool moveCurve(uint8_t index, int8_t shift, int8_t custom) // TODO move?
 121 {
 122   if (g_model.curves[MAX_CURVES-1] + shift > MAX_CURVE_POINTS-5*MAX_CURVES) {
 123     AUDIO_WARNING2();
 124     return false;
 125   }
 126
 127   int8_t * crv = curveAddress(index);
 128   if (shift < 0) {
 129     for (uint8_t i=0; i<custom; i++)
 130       crv[i] = crv[2*i];
 131   }
 132
 133   int8_t * nextCrv = curveAddress(index+1);
 134   memmove(nextCrv+shift, nextCrv, 5*(MAX_CURVES-index-1)+g_model.curves[MAX_CURVES-1]-g_model.curves[index]);
 135   if (shift < 0) memclear(&g_model.points[MAX_CURVE_POINTS-1] + shift, -shift);
 136   while (index<MAX_CURVES) {
 137     g_model.curves[index++] += shift;
 138   }
 139
 140   for (uint8_t i=0; i<custom-2; i++) {
 141     crv[custom + i] = -100 + ((200 * (i + 1) + custom / 2) / (custom - 1));
 142   }
 143
 144   storageDirty(EE_MODEL);
 145   return true;
 146 }
 147 #endif
 148
 149 #if defined(CPUARM)
 150 #define CUSTOM_POINT_X(points, count, idx) ((idx)==0 ? -100 : (((idx)==(count)-1) ? 100 : points[(count)+(idx)-1]))
 151 int32_t compute_tangent(CurveInfo * crv, int8_t * points, int i)
 152 {
 153   int32_t m=0;
 154     uint8_t num_points = crv->points + 5;
 155     #define MMULT 1024
 156     if (i == 0) {
 157       //linear interpolation between 1st 2 points
 158       //keep 3 decimal-places for m
 159       if (crv->type == CURVE_TYPE_CUSTOM) {
 160         int8_t x0 = CUSTOM_POINT_X(points, num_points, 0);
 161         int8_t x1 = CUSTOM_POINT_X(points, num_points, 1);
 162         if (x1 > x0) m = (MMULT * (points[1] - points[0])) / (x1 - x0);
 163       }
 164       else {
 165         int32_t delta = (2 * 100) / (num_points - 1);
 166         m = (MMULT * (points[1] - points[0])) / delta;
 167       }
 168     }
 169     else if (i == num_points - 1) {
 170       //linear interpolation between last 2 points
 171       //keep 3 decimal-places for m
 172       if (crv->type == CURVE_TYPE_CUSTOM) {
 173         int8_t x0 = CUSTOM_POINT_X(points, num_points, num_points-2);
 174         int8_t x1 = CUSTOM_POINT_X(points, num_points, num_points-1);
 175         if (x1 > x0) m = (MMULT * (points[num_points-1] - points[num_points-2])) / (x1 - x0);
 176       }
 177       else {
 178         int32_t delta = (2 * 100) / (num_points - 1);
 179         m = (MMULT * (points[num_points-1] - points[num_points-2])) / delta;
 180       }
 181     }
 182     else {
 183         //apply monotone rules from
 184         //http://en.wikipedia.org/wiki/Monotone_cubic_interpolation
 185         //1) compute slopes of secant lines
 186       int32_t d0=0, d1=0;
 187         if (crv->type == CURVE_TYPE_CUSTOM) {
 188           int8_t x0 = CUSTOM_POINT_X(points, num_points, i-1);
 189           int8_t x1 = CUSTOM_POINT_X(points, num_points, i);
 190           int8_t x2 = CUSTOM_POINT_X(points, num_points, i+1);
 191           if (x1 > x0) d0 = (MMULT * (points[i] - points[i-1])) / (x1 - x0);
 192           if (x2 > x1) d1 = (MMULT * (points[i+1] - points[i])) / (x2 - x1);
 193         }
 194         else {
 195           int32_t delta = (2 * 100) / (num_points - 1);
 196           d0 = (MMULT * (points[i] - points[i-1])) / (delta);
 197           d1 = (MMULT * (points[i+1] - points[i])) / (delta);
 198         }
 199         //2) compute initial average tangent
 200         m = (d0 + d1) / 2;
 201         //3 check for horizontal lines
 202         if (d0 == 0 || d1 == 0 || (d0 > 0 && d1 < 0) || (d0 < 0 && d1 > 0)) {
 203           m = 0;
 204         }
 205         else if (MMULT * m / d0 >  3 * MMULT) {
 206           m = 3 * d0;
 207         }
 208         else if (MMULT * m / d1 > 3 * MMULT) {
 209           m = 3 * d1;
 210         }
 211     }
 212     return m;
 213 }
 214
 215 /* The following is a hermite cubic spline.
 216    The basis functions can be found here:
 217    http://en.wikipedia.org/wiki/Cubic_Hermite_spline
 218    The tangents are computed via the 'cubic monotone' rules (allowing for local-maxima)
 219 */
 220 int16_t hermite_spline(int16_t x, uint8_t idx)
 221 {
 222   CurveInfo &crv = g_model.curves[idx];
 223   int8_t *points = curveAddress(idx);
 224   uint8_t count = crv.points+5;
 225   bool custom = (crv.type == CURVE_TYPE_CUSTOM);
 226
 227   if (x < -RESX)
 228     x = -RESX;
 229   else if (x > RESX)
 230     x = RESX;
 231
 232   for (int i=0; i<count-1; i++) {
 233     int32_t p0x, p3x;
 234     if (custom) {
 235       p0x = (i>0 ? calc100toRESX(points[count+i-1]) : -RESX);
 236       p3x = (i<count-2 ? calc100toRESX(points[count+i]) : RESX);
 237     }
 238     else {
 239       p0x = -RESX + (i*2*RESX)/(count-1);
 240       p3x = -RESX + ((i+1)*2*RESX)/(count-1);
 241     }
 242
 243     if (x >= p0x && x <= p3x) {
 244       int32_t p0y = calc100toRESX(points[i]);
 245       int32_t p3y = calc100toRESX(points[i+1]);
 246       int32_t m0 = compute_tangent(&crv, points, i);
 247       int32_t m3 = compute_tangent(&crv, points, i+1);
 248       int32_t y;
 249       int32_t h = p3x - p0x;
 250       int32_t t = (h > 0 ? (MMULT * (x - p0x)) / h : 0);
 251       int32_t t2 = t * t / MMULT;
 252       int32_t t3 = t2 * t / MMULT;
 253       int32_t h00 = 2*t3 - 3*t2 + MMULT;
 254       int32_t h10 = t3 - 2*t2 + t;
 255       int32_t h01 = -2*t3 + 3*t2;
 256       int32_t h11 = t3 - t2;
 257       y = p0y * h00 + h * (m0 * h10 / MMULT) + p3y * h01 + h * (m3 * h11 / MMULT);
 258       y /= MMULT;
 259       return y;
 260     }
 261   }
 262   return 0;
 263 }
 264 #endif
 265
 266 int intpol(int x, uint8_t idx) // -100, -75, -50, -25, 0 ,25 ,50, 75, 100
 267 {
 268 #if defined(CPUARM)
 269   CurveInfo & crv = g_model.curves[idx];
 270   int8_t * points = curveAddress(idx);
 271   uint8_t count = crv.points+5;
 272   bool custom = (crv.type == CURVE_TYPE_CUSTOM);
 273 #else
 274   CurveInfo crv = curveInfo(idx);
 275   int8_t * points = crv.crv;
 276   uint8_t count = crv.points;
 277   bool custom = crv.custom;
 278 #endif
 279   int16_t erg = 0;
 280
 281   x += RESXu;
 282
 283   if (x <= 0) {
 284     erg = (int16_t)points[0] * (RESX/4);
 285   }
 286   else if (x >= (RESX*2)) {
 287     erg = (int16_t)points[count-1] * (RESX/4);
 288   }
 289   else {
 290     uint16_t a=0, b=0;
 291     uint8_t i;
 292     if (custom) {
 293       for (i=0; i<count-1; i++) {
 294         a = b;
 295         b = (i==count-2 ? 2*RESX : RESX + calc100toRESX(points[count+i]));
 296         if ((uint16_t)x<=b) break;
 297       }
 298     }
 299     else {
 300       uint16_t d = (RESX * 2) / (count-1);
 301       i = (uint16_t)x / d;
 302       a = i * d;
 303       b = a + d;
 304     }
 305     erg = (int16_t)points[i]*(RESX/4) + ((int32_t)(x-a) * (points[i+1]-points[i]) * (RESX/4)) / ((b-a));
 306   }
 307
 308   return erg / 25; // 100*D5/RESX;
 309 }
 310
 311 #if defined(CPUARM)
 312 int applyCurve(int x, CurveRef & curve)
 313 {
 314   switch (curve.type) {
 315     case CURVE_REF_DIFF:
 316     {
 317       int curveParam = GET_GVAR_PREC1(curve.value, -100, 100, mixerCurrentFlightMode);
 318       if (curveParam > 0 && x < 0)
 319         x = (x * (1000 - curveParam)) / 1000;
 320       else if (curveParam < 0 && x > 0)
 321         x = (x * (1000 + curveParam)) / 1000;
 322       return x;
 323     }
 324
 325     case CURVE_REF_EXPO:
 326     {
 327       int curveParam = GET_GVAR_PREC1(curve.value, -100, 100, mixerCurrentFlightMode) / 10;
 328       return expo(x, curveParam);
 329     }
 330
 331     case CURVE_REF_FUNC:
 332       switch (curve.value) {
 333         case CURVE_X_GT0:
 334           if (x < 0) x = 0; //x|x>0
 335           return x;
 336         case CURVE_X_LT0:
 337           if (x > 0) x = 0; //x|x<0
 338           return x;
 339         case CURVE_ABS_X: // x|abs(x)
 340           return abs(x);
 341         case CURVE_F_GT0: //f|f>0
 342           return x > 0 ? RESX : 0;
 343         case CURVE_F_LT0: //f|f<0
 344           return x < 0 ? -RESX : 0;
 345         case CURVE_ABS_F: //f|abs(f)
 346           return x > 0 ? RESX : -RESX;
 347       }
 348       break;
 349
 350     case CURVE_REF_CUSTOM:
 351     {
 352       int curveParam = curve.value;
 353       if (curveParam < 0) {
 354         x = -x;
 355         curveParam = -curveParam;
 356       }
 357       if (curveParam > 0 && curveParam <= MAX_CURVES) {
 358         return applyCustomCurve(x, curveParam - 1);
 359       }
 360       break;
 361     }
 362   }
 363
 364   return x;
 365 }
 366
 367 int applyCustomCurve(int x, uint8_t idx)
 368 {
 369   if (idx >= MAX_CURVES)
 370     return 0;
 371
 372   CurveInfo & crv = g_model.curves[idx];
 373   if (crv.smooth)
 374     return hermite_spline(x, idx);
 375   else
 376     return intpol(x, idx);
 377 }
 378 #else
 379 int applyCurve(int x, int8_t idx)
 380 {
 381   /* already tried to have only one return at the end */
 382   switch(idx) {
 383     case CURVE_NONE:
 384       return x;
 385     case CURVE_X_GT0:
 386       if (x < 0) x = 0; //x|x>0
 387       return x;
 388     case CURVE_X_LT0:
 389       if (x > 0) x = 0; //x|x<0
 390       return x;
 391     case CURVE_ABS_X: // x|abs(x)
 392       return abs(x);
 393     case CURVE_F_GT0: //f|f>0
 394       return x > 0 ? RESX : 0;
 395     case CURVE_F_LT0: //f|f<0
 396       return x < 0 ? -RESX : 0;
 397     case CURVE_ABS_F: //f|abs(f)
 398       return x > 0 ? RESX : -RESX;
 399   }
 400   if (idx < 0) {
 401     x = -x;
 402     idx = -idx + CURVE_BASE - 1;
 403   }
 404   return applyCustomCurve(x, idx - CURVE_BASE);
 405 }
 406 #endif
 407
 408 point_t getPoint(uint8_t i)
 409 {
 410   point_t result = {0, 0};
 411 #if defined(CPUARM)
 412   CurveInfo & crv = g_model.curves[s_curveChan];
 413   int8_t * points = curveAddress(s_curveChan);
 414   bool custom = (crv.type == CURVE_TYPE_CUSTOM);
 415   uint8_t count = 5+crv.points;
 416 #else
 417   CurveInfo crv = curveInfo(s_curveChan);
 418   int8_t * points = crv.crv;
 419   bool custom = crv.custom;
 420   uint8_t count = crv.points;
 421 #endif
 422   if (i < count) {
 423     result.x = CURVE_CENTER_X-1-CURVE_SIDE_WIDTH + i*CURVE_SIDE_WIDTH*2/(count-1);
 424     result.y = CURVE_CENTER_Y - (points[i]) * (CURVE_SIDE_WIDTH-1) / 100;
 425     if (custom && i>0 && i<count-1) {
 426       result.x = CURVE_CENTER_X - 1 - CURVE_SIDE_WIDTH + (100 + (100 + points[count + i - 1]) * (2 * CURVE_SIDE_WIDTH)) / 200;
 427     }
 428   }
 429   return result;
 430 }
 431
 432 int applyCurrentCurve(int x)
 433 {
 434   return applyCustomCurve(x, s_curveChan);
 435 }