src/main/tracker/rvosd.c

   1 /*
   2  * This file is part of u360gts, aka amv-open360tracker 32bits:
   3  * https://github.com/raul-ortega/amv-open360tracker-32bits
   4  *
   5  * The code below is an adaptation by Raúl Ortega of the original code written by Samuel Brucksch:
   6  * https://github.com/SamuelBrucksch/open360tracker
   7  *
   8  * u360gts is free software: you can redistribute it and/or modify
   9  * it under the terms of the GNU General Public License as published by
  10  * the Free Software Foundation, either version 3 of the License, or
  11  * (at your option) any later version.
  12  *
  13  * u360gts is distributed in the hope that it will be useful,
  14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  16  * GNU General Public License for more details.
  17  *
  18  * You should have received a copy of the GNU General Public License
  19  * along with u360gts.  If not, see <http://www.gnu.org/licenses/>.
  20  *
  21  */
  22
  23 /*
  24 This code is written by Samuel Brucksch, and improved by Raul Ortega
  25
  26  it will decode the RVOSD telemetry protocol and will convert it into values the open360tracker can understand
  27
  28  $1,1,00040291,00.000000,N,000.000000,W,+00000,+00000,000,0000,0000,000,+000,000,089,089,089,089,1160,0000,00004,0004,00,000,0026,*00
  29  $1,1,00194723,41.572332,N,001.973692,E,+00312,+00312,254,0021,0000,005,+000,220,000,000,000,000,1156,0002,00050,0161,06,000,0033,0,*01
  30
  31  It is sent 25 times/S, 115200, 8N1.
  32
  33  $
  34  Validity (1 valid, 0 invalid)
  35  Units (1 metric, 0 imperial)
  36  HHMMSSmm (hour|minutes|seconds|tenth of seconds)
  37  ddmm.mmmm,
  38  N/S (Latitude)
  39  dddmm.mmmm,
  40  E/W (Longitude)
  41  +/- altitude (relative)
  42  +/- altitude (absolute)
  43  Roll (0 to 255)
  44  Pitch (0 to 4096)
  45  Airspeed
  46  GroundSpeed
  47  +/- Variometer
  48  Heading (0 to 360)
  49  Rudder
  50  Elevator
  51  Aileron
  52  Throttle (mS / 100)
  53  Main battery voltage
  54  Aux battery voltage (V * 100)
  55  Current
  56  mAh
  57  Sats
  58  RSSI
  59  Temp
  60  *checksum
  61
  62
  63  Checksum is everything XORed from "$" to "temperature".
  64  */
  65 #include "config.h"
  66 #include "telemetry.h"
  67
  68 uint8_t dot_found = 0;
  69
  70 uint16_t checksum_calculation = 0;
  71 uint16_t checksum_read = 0;
  72
  73 //data needed to buffer
  74 int16_t alttemp;
  75 int16_t alt;
  76 int8_t altsign;
  77
  78 uint16_t rvosd_headingtemp;
  79 uint16_t rvosd_heading=0;
  80
  81 int8_t latsign;
  82 int8_t lonsign;
  83 uint32_t rvosd_lat_bp;
  84 uint32_t rvosd_lon_bp;
  85 uint32_t rvosd_lat_ap;
  86 uint32_t rvosd_lon_ap;
  87 uint8_t satstemp;
  88 int32_t lat = 0;
  89 int32_t lon = 0;
  90 uint8_t sats = 0;
  91
  92 uint8_t rvosd_data_index = 0;
  93 uint8_t frame_started = 0;
  94 uint8_t checksum_started = 0;
  95 uint8_t metric = 0;
  96 uint8_t checksum_index = 0;
  97
  98 void rvosd_encodeTargetData(uint8_t c) {
  99   if (c == '$' && !frame_started){
 100     frame_started = true;
 101     checksum_started = false;
 102     checksum_calculation = 0;
 103     checksum_read = 0;
 104     rvosd_lat_bp = 0;
 105     rvosd_lat_ap = 0;
 106     rvosd_lon_bp = 0;
 107     rvosd_lon_ap = 0;
 108     latsign = 0;
 109     lonsign = 0;
 110     alttemp = 0;
 111     satstemp = 0;
 112     altsign = 0;
 113     checksum_index = 0;
 114     rvosd_data_index=0;
 115     dot_found=false;
 116     rvosd_headingtemp=0;
 117     return;
 118   }
 119   else if (c == 'R' && frame_started){
 120     checksum_calculation ^= c;
 121     return;
 122   }
 123   else if (c == 'V' && frame_started){
 124     checksum_calculation ^= c;
 125     return;
 126   }
 127   else if (c == '*' && frame_started){
 128     checksum_started = true;
 129     return;
 130   }
 131   else if (checksum_index == 2){
 132     if (checksum_read == checksum_calculation){
 133       telemetry_lat = ((int32_t)rvosd_lat_bp*100000+(int32_t)(rvosd_lat_ap/10))*(int32_t)latsign;
 134       telemetry_lon = ((int32_t)rvosd_lon_bp*100000+(int32_t)(rvosd_lon_ap/10))*(int32_t)lonsign;
 135       telemetry_sats=satstemp;
 136       telemetry_alt=altsign*alttemp;
 137       rvosd_heading=rvosd_headingtemp;
 138       // data is ready
 139       gotFix = true;
 140       gotAlt = true;
 141     }
 142     else{
 143       //needed?
 144         telemetry_failed_cs++;
 145     }
 146     frame_started = false;
 147   }
 148
 149   if (frame_started){
 150     if (checksum_started){
 151       checksum_index++;
 152       checksum_read *= 16;
 153       if (c >= '0' && c <= '9'){
 154         checksum_read += (c - '0');
 155       }
 156       else{
 157         checksum_read += (c-'A'+10);
 158       }
 159     }
 160     else{
 161       //we get data
 162       checksum_calculation ^= c;
 163     }
 164   }
 165   else{
 166     return;
 167   }
 168
 169   if (c == ','){
 170     rvosd_data_index++;
 171     dot_found=false;
 172     return;
 173   }
 174
 175   switch (rvosd_data_index) {
 176   case 0:
 177     //frame valid?
 178     if ((c - '0') == 0){
 179       //frame invalid, skip...
 180       frame_started = false;
 181       return;
 182     }
 183     else if ((c - '0') == 1){
 184       //frame valid, do nothing
 185     }
 186     break;
 187   case 1:
 188     //units, 0 = imperial, 1 = metric
 189     if (c - '0' == 0){
 190       metric = false;
 191     }
 192     else if (c - '0' == 1){
 193       metric = true;
 194     }
 195     break;
 196   case 3:
 197     //lat
 198     if (c == '.'){
 199       dot_found = true;
 200       return;
 201     }
 202     if (!dot_found){
 203       rvosd_lat_bp *= 10;
 204       rvosd_lat_bp += c-'0';
 205     }
 206     else{
 207       rvosd_lat_ap *= 10;
 208       rvosd_lat_ap += c-'0';
 209     }
 210     break;
 211   case 4:
 212     //N/S
 213     if (c == 'N'){
 214       latsign = 1;
 215     }
 216     else if (c == 'S'){
 217       latsign = -1;
 218     }
 219     break;
 220   case 5:
 221     //lon
 222     if (c == '.'){
 223       dot_found = true;
 224       return;
 225     }
 226     if (!dot_found){
 227       rvosd_lon_bp *= 10;
 228       rvosd_lon_bp += c-'0';
 229     }
 230     else{
 231       rvosd_lon_ap *= 10;
 232       rvosd_lon_ap += c-'0';
 233     }
 234     break;
 235   case 6:
 236     //E/W
 237     if (c == 'E'){
 238       lonsign = 1;
 239     }
 240     else if (c == 'W'){
 241       lonsign = -1;
 242     }
 243     break;
 244   case 8:
 245     //absolute alt;
 246     if (c == '.'){
 247       dot_found = true;
 248       return;
 249     }
 250     else if (c == '-'){
 251       altsign = -1;
 252       return;
 253     }
 254     else if (c == '+'){
 255       altsign = 1;
 256       return;
 257     }
 258     if (!dot_found){
 259       alttemp *= 10;
 260       alttemp += c- '0';
 261     }
 262     else{
 263       //no after comma i think
 264     }
 265     break;
 266     case 14:
 267       //Heading;
 268       rvosd_headingtemp *= 10;
 269       rvosd_headingtemp += c- '0';
 270     break;
 271   case 23:
 272     satstemp *= 10;
 273     satstemp += c - '0';
 274     break;
 275   default:
 276     break;
 277   }
 278 }
 279
 280
 281