radio/src/maths.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 #if !defined(CPUARM)
  24 // #define CORRECT_NEGATIVE_SHIFTS
  25 // open.20.fsguruh; shift right operations do the rounding different for negative values compared to positive values
  26 // so all negative divisions round always further down, which give absolute values bigger compared to a usual division
  27 // this is noticable on the display, because instead of -100.0 -99.9 is shown; While in praxis I doublt somebody will notice a
  28 // difference this is more a mental thing. Maybe people are distracted, because the easy calculations are obviously wrong
  29 // this define would correct this, but costs 34 bytes code for stock version
  30
  31 // currently we set this to active always, because it might cause a fault about 1% compared positive and negative values
  32 // is done now in makefile
  33
  34 int16_t calc100to256_16Bits(int16_t x) // return x*2.56
  35 {
  36     // y = 2*x + x/2 +x/16-x/512-x/2048
  37     // 512 and 2048 are out of scope from int8 input --> forget it
  38 #ifdef CORRECT_NEGATIVE_SHIFTS
  39     int16_t res=(int16_t)x<<1;
  40     //int8_t  sign=(uint8_t) x>>7;
  41     int8_t sign=(x<0?1:0);
  42
  43     x-=sign;
  44     res+=(x>>1);
  45     res+=sign;
  46     res+=(x>>4);
  47     res+=sign;
  48     return res;
  49 #else
  50     return ((int16_t)x<<1)+(x>>1)+(x>>4);
  51 #endif
  52 }
  53
  54 int16_t calc100to256(int8_t x) // return x*2.56
  55 {
  56   return calc100to256_16Bits(x);
  57 }
  58
  59 int16_t calc100toRESX_16Bits(int16_t x) // return x*10.24
  60 {
  61 #ifdef CORRECT_NEGATIVE_SHIFTS
  62   int16_t res= ((int16_t)x*41)>>2;
  63   int8_t sign=(x<0?1:0);
  64   //int8_t  sign=(uint8_t) x>>7;
  65   x-=sign;
  66   res-=(x>>6);
  67   res-=sign;
  68   return res;
  69 #else
  70   // return (int16_t)x*10 + x/4 - x/64;
  71   return ((x*41)>>2) - (x>>6);
  72 #endif
  73 }
  74
  75 int16_t calc100toRESX(int8_t x) // return x*10.24
  76 {
  77   return calc100toRESX_16Bits(x);
  78 }
  79
  80 // return x*1.024
  81 int16_t calc1000toRESX(int16_t x) // improve calc time by Pat MacKenzie
  82 {
  83   // return x + x/32 - x/128 + x/512;
  84   int16_t y = x>>5;
  85   x+=y;
  86   y=y>>2;
  87   x-=y;
  88   return x+(y>>2);
  89 }
  90
  91 int16_t calcRESXto1000(int16_t x)  // return x/1.024
  92 {
  93 // *1000/1024 = x - x/32 + x/128
  94   return (x - (x>>5) + (x>>7));
  95 }
  96
  97 int8_t calcRESXto100(int16_t x)
  98 {
  99   return (x*25) >> 8;
 100 }
 101 #endif
 102 #if defined(HELI) || defined(FRSKY_HUB) || defined(CPUARM)
 103 uint16_t isqrt32(uint32_t n)
 104 {
 105   uint16_t c = 0x8000;
 106   uint16_t g = 0x8000;
 107
 108   for (;;) {
 109     if ((uint32_t)g*g > n)
 110       g ^= c;
 111     c >>= 1;
 112     if(c == 0)
 113       return g;
 114     g |= c;
 115   }
 116 }
 117 #endif
 118
 119 /*
 120   Division by 10 and rounding or fixed point arithmetic values
 121
 122   Examples:
 123     value -> result
 124     105 ->  11
 125     104 ->  10
 126    -205 -> -21
 127    -204 -> -20
 128 */
 129
 130 #if defined(FRSKY_HUB) && !defined(CPUARM)
 131 // convert latitude and longitude to 1/10^6 degrees
 132 void extractLatitudeLongitude(uint32_t * latitude, uint32_t * longitude)
 133 {
 134   div_t qr = div(telemetryData.hub.gpsLatitude_bp, 100);
 135   *latitude = ((uint32_t)(qr.quot) * 1000000) + (((uint32_t)(qr.rem) * 10000 + telemetryData.hub.gpsLatitude_ap) * 5) / 3;
 136
 137   qr = div(telemetryData.hub.gpsLongitude_bp, 100);
 138   *longitude = ((uint32_t)(qr.quot) * 1000000) + (((uint32_t)(qr.rem) * 10000 + telemetryData.hub.gpsLongitude_ap) * 5) / 3;
 139 }
 140
 141 #if __clang__
 142 // clang does not like packed member access at all. Since mavlink is a 3rd party library, ignore the errors
 143 #pragma clang diagnostic push
 144 #pragma clang diagnostic warning "-Waddress-of-packed-member"
 145 #endif
 146 void getGpsPilotPosition()
 147 {
 148   extractLatitudeLongitude(&telemetryData.hub.pilotLatitude, &telemetryData.hub.pilotLongitude);
 149   // distFromEarthAxis = cos(lat) * EARTH_RADIUS
 150   // 1 - x2/2 + x4/24
 151   uint32_t lat = telemetryData.hub.pilotLatitude / 10000;
 152   uint32_t angle2 = (lat*lat) / 10000;
 153   uint32_t angle4 = angle2 * angle2;
 154   telemetryData.hub.distFromEarthAxis = 139 * (((uint32_t)10000000-((angle2*(uint32_t)123370)/81)+(angle4/25)) / 12500);
 155   // TRACE("telemetryData.hub.distFromEarthAxis=%d", telemetryData.hub.distFromEarthAxis);
 156 }
 157
 158 void getGpsDistance()
 159 {
 160   uint32_t lat, lng;
 161
 162   extractLatitudeLongitude(&lat, &lng);
 163
 164   // printf("lat=%d (%d), long=%d (%d)\n", lat, abs(lat - telemetryData.hub.pilotLatitude), lng, abs(lng - telemetryData.hub.pilotLongitude));
 165
 166   uint32_t angle = (lat > telemetryData.hub.pilotLatitude) ? lat - telemetryData.hub.pilotLatitude : telemetryData.hub.pilotLatitude - lat;
 167   uint32_t dist = EARTH_RADIUS * angle / 1000000;
 168   uint32_t result = dist*dist;
 169
 170   angle = (lng > telemetryData.hub.pilotLongitude) ? lng - telemetryData.hub.pilotLongitude : telemetryData.hub.pilotLongitude - lng;
 171   dist = telemetryData.hub.distFromEarthAxis * angle / 1000000;
 172   result += dist*dist;
 173
 174   dist = abs(TELEMETRY_BARO_ALT_AVAILABLE() ? TELEMETRY_RELATIVE_BARO_ALT_BP : TELEMETRY_RELATIVE_GPS_ALT_BP);
 175   result += dist*dist;
 176
 177   telemetryData.hub.gpsDistance = isqrt32(result);
 178   if (telemetryData.hub.gpsDistance > telemetryData.hub.maxGpsDistance)
 179     telemetryData.hub.maxGpsDistance = telemetryData.hub.gpsDistance;
 180 }
 181 #endif
 182
 183 #if defined(CPUARM)
 184 // djb2 hash algorithm
 185 uint32_t hash(const void * ptr, uint32_t size)
 186 {
 187   const uint8_t * data = (const uint8_t *)ptr;
 188   uint32_t hash = 5381;
 189   for (uint32_t i=0; i<size; i++) {
 190     hash = ((hash << 5) + hash) + data[i]; /* hash * 33 + c */
 191   }
 192   return hash;
 193 }
 194 #endif