src/main/tracker/telemetry.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 #include "config.h"
  24 #include "telemetry.h"
  25 #include "TinyGPS.h"
  26 #include "protocol_detection.h"
  27
  28 int32_t telemetry_lat = 0;
  29 int32_t telemetry_lon = 0;
  30 int16_t telemetry_alt = 0;
  31 int16_t telemetry_sats = 0;
  32 int32_t telemetry_time = 0;
  33 int32_t telemetry_date = 0;
  34 int16_t telemetry_age = 0;
  35
  36
  37 uint8_t telemetry_failed_cs = 0;
  38
  39 float telemetry_course = 0.0f;
  40 float telemetry_speed = 0.0f;
  41 float telemetry_declination = 0.0f;
  42 float telemetry_hdop = 0.0f;
  43
  44 float telemetry_pitch = 0.0f;
  45 float telemetry_roll = 0.0f;
  46 float telemetry_yaw = 0.0f;
  47
  48 uint8_t telemetry_provider = 0;
  49
  50 uint8_t telemetry_fixtype = 0;
  51
  52 uint8_t telemetry_fixes = 0;
  53 uint8_t telemetry_frequency = 0;
  54 uint32_t telemetry_millis = 0;
  55
  56 int32_t telemetry_home_lat;
  57 int32_t telemetry_home_lon;
  58 int16_t telemetry_home_alt;
  59
  60 uint8_t a;
  61
  62 uint8_t LOCAL_GPS;
  63
  64 uint16_t chars = 0;
  65
  66 uint8_t sentences = 0;
  67
  68 int32_t getTargetLat() {
  69   return telemetry_lat;
  70 }
  71
  72 int32_t getTargetLon() {
  73   return telemetry_lon;
  74 }
  75
  76 int16_t getTargetAlt(int16_t home_alt) {
  77   return telemetry_alt - home_alt;
  78 }
  79
  80 uint16_t getSats() {
  81   return telemetry_sats;
  82 }
  83
  84 void encodeTargetData(uint8_t c) {
  85
  86         uint16_t chars = 0;
  87         uint8_t sentences = 0;
  88
  89         protocolDetectionParser(c);
  90
  91         if(PROTOCOL(TP_MFD))
  92                 mfd_encodeTargetData(c);
  93         else if(PROTOCOL(TP_GPS_TELEMETRY))
  94                 gps_encodeTargetData(c);
  95         else if(PROTOCOL(TP_MAVLINK))
  96                 mavlink_encodeTargetData(c);
  97         else if(PROTOCOL(TP_RVOSD))
  98                 rvosd_encodeTargetData(c);
  99         else if(PROTOCOL(TP_FRSKY_D))
 100                 frskyd_encodeTargetData(c);
 101         else if(PROTOCOL(TP_FRSKY_X))
 102                 frskyx_encodeTargetData(c);
 103         else if(PROTOCOL(TP_LTM))
 104                 ltm_encodeTargetData(c);
 105         else if(PROTOCOL(TP_CROSSFIRE))
 106             crossfire_encodeTargetData(c);
 107         else if(PROTOCOL(TP_PITLAB))
 108                 pitlab_encodeTargetData(c);
 109
 110         if(forwardEnabled()){
 111             forwardTelemetry(c);
 112         }
 113 }
 114
 115 void gps_encodeTargetData(uint8_t c) {
 116   //if(PROTOCOL(TP_GPS_TELEMETRY)){
 117         TinyGPS_stats(&chars,&sentences,&telemetry_failed_cs);
 118   //}
 119   if (TinyGPS_encode(c)) {
 120     unsigned long fix_age;
 121     get_position(&telemetry_lat, &telemetry_lon, &fix_age);
 122     if (fix_age == GPS_INVALID_AGE) {
 123       gotFix = false;
 124     } else if (fix_age > 5000) {
 125       gotFix = false;
 126     } else {
 127       //TODO valid data
 128
 129       if (altitude() != GPS_INVALID_ALTITUDE) {
 130         telemetry_alt = (int16_t)(altitude() / 100);
 131         gotAlt = true;
 132       }
 133       //date and time
 134       get_datetime(&telemetry_date, &telemetry_time, &telemetry_age);
 135       //course (angle)
 136       telemetry_course = f_course();
 137       //speed
 138       telemetry_speed = f_speed_knots();
 139       //hdop
 140       telemetry_hdop = f_hdop();
 141
 142       telemetry_fixtype = f_fixtype();
 143
 144       if(!LOCAL_GPS){
 145         if (satellites() != GPS_INVALID_SATELLITES) {
 146                 telemetry_sats = (int16_t)satellites();
 147         }
 148       }
 149       if (get_sentence_type() != _GPS_SENTENCE_GPRMC)
 150           gotFix = true;
 151     }
 152   }
 153 }
 154
 155 //TODO check if this is the right conversion
 156 int32_t gpsToLong(int8_t neg, uint16_t bp, uint16_t ap) {
 157   // we want convert from frsky to millionth of degrees
 158   // 0302.7846 -> 03 + (02.7846/60) = 3.04641 -> 3046410
 159   // first the upper part
 160   uint32_t first = ((uint32_t)bp / 100) * 100000;
 161   uint32_t second = ((((uint32_t)bp % 100) * 100000) + ((uint32_t)ap * 10)) / 60;
 162
 163   // take sign into account
 164   return ((int32_t)(first + second) * (uint32_t)neg);
 165 }
 166
 167 void setTelemetryHome(int32_t lat, int32_t lon, int16_t alt){
 168         telemetry_home_lat = lat;
 169         telemetry_home_lon = lon;
 170         telemetry_home_alt = alt;
 171 }