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