src/main/telemetry/ghst.c

   1 /*
   2  * This file is part of Cleanflight and Betaflight.
   3  *
   4  * Cleanflight and Betaflight are free software. You can redistribute
   5  * this software and/or modify this software under the terms of the
   6  * GNU General Public License as published by the Free Software
   7  * Foundation, either version 3 of the License, or (at your option)
   8  * any later version.
   9  *
  10  * Cleanflight and Betaflight are distributed in the hope that they
  11  * will be useful, but WITHOUT ANY WARRANTY; without even the implied
  12  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  * See the GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this software.
  17  *
  18  * If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 #include <stdbool.h>
  22 #include <stdint.h>
  23 #include <string.h>
  24
  25 #include "platform.h"
  26
  27 #ifdef USE_TELEMETRY_GHST
  28
  29 #include "build/atomic.h"
  30 #include "build/build_config.h"
  31 #include "build/version.h"
  32
  33 #include "config/feature.h"
  34 #include "pg/pg.h"
  35 #include "pg/pg_ids.h"
  36
  37 #include "common/crc.h"
  38 #include "common/maths.h"
  39 #include "common/printf.h"
  40 #include "common/streambuf.h"
  41 #include "common/utils.h"
  42
  43 #include "cms/cms.h"
  44
  45 #include "drivers/nvic.h"
  46
  47 #include "config/config.h"
  48 #include "fc/rc_modes.h"
  49 #include "fc/runtime_config.h"
  50
  51 #include "flight/imu.h"
  52 #include "flight/position.h"
  53
  54 #include "io/gps.h"
  55 #include "io/serial.h"
  56
  57 #include "rx/ghst.h"
  58 #include "rx/ghst_protocol.h"
  59
  60 #include "sensors/battery.h"
  61 #include "sensors/sensors.h"
  62
  63 #include "telemetry/telemetry.h"
  64 #include "telemetry/msp_shared.h"
  65
  66 #include "telemetry/ghst.h"
  67
  68 #define GHST_CYCLETIME_US                   100000      // 10x/sec
  69 #define GHST_FRAME_PACK_PAYLOAD_SIZE        10
  70 #define GHST_FRAME_GPS_PAYLOAD_SIZE         10
  71 #define GHST_FRAME_LENGTH_CRC               1
  72 #define GHST_FRAME_LENGTH_TYPE              1
  73
  74 static bool ghstTelemetryEnabled;
  75 static uint8_t ghstFrame[GHST_FRAME_SIZE_MAX];
  76
  77 static void ghstInitializeFrame(sbuf_t *dst)
  78 {
  79     dst->ptr = ghstFrame;
  80     dst->end = ARRAYEND(ghstFrame);
  81
  82     sbufWriteU8(dst, GHST_ADDR_RX);
  83 }
  84
  85 static void ghstFinalize(sbuf_t *dst)
  86 {
  87     crc8_dvb_s2_sbuf_append(dst, &ghstFrame[2]); // start at byte 2, since CRC does not include device address and frame length
  88     sbufSwitchToReader(dst, ghstFrame);
  89     // write the telemetry frame to the receiver.
  90     ghstRxWriteTelemetryData(sbufPtr(dst), sbufBytesRemaining(dst));
  91 }
  92
  93 // Battery (Pack) status
  94 void ghstFramePackTelemetry(sbuf_t *dst)
  95 {
  96     // use sbufWrite since CRC does not include frame length
  97     sbufWriteU8(dst, GHST_FRAME_PACK_PAYLOAD_SIZE + GHST_FRAME_LENGTH_CRC + GHST_FRAME_LENGTH_TYPE);
  98     sbufWriteU8(dst, 0x23);                     // GHST_DL_PACK_STAT
  99
 100     if (telemetryConfig()->report_cell_voltage) {
 101         sbufWriteU16(dst, getBatteryAverageCellVoltage());      // units of 10mV
 102     } else {
 103         sbufWriteU16(dst, getBatteryVoltage());
 104     }
 105     sbufWriteU16(dst, getAmperage());                           // units of 10mA
 106
 107     sbufWriteU16(dst, getMAhDrawn() / 10);                      // units of 10mAh (range of 0-655.36Ah)
 108
 109     sbufWriteU8(dst, 0x00);                     // Rx Voltage, units of 100mV (not passed from BF, added in Ghost Rx)
 110
 111     sbufWriteU8(dst, 0x00);                     // tbd1
 112     sbufWriteU8(dst, 0x00);                     // tbd2
 113     sbufWriteU8(dst, 0x00);                     // tbd3
 114 }
 115
 116 // GPS data, primary, positional data
 117 void ghstFrameGpsPrimaryTelemetry(sbuf_t *dst)
 118 {
 119     // use sbufWrite since CRC does not include frame length
 120     sbufWriteU8(dst, GHST_FRAME_GPS_PAYLOAD_SIZE + GHST_FRAME_LENGTH_CRC + GHST_FRAME_LENGTH_TYPE);
 121     sbufWriteU8(dst, GHST_DL_GPS_PRIMARY);
 122
 123     sbufWriteU32(dst, gpsSol.llh.lat);
 124     sbufWriteU32(dst, gpsSol.llh.lon);
 125
 126     int32_t altitudeCm = gpsSol.llh.altCm;      // gps Altitude (absolute)
 127     if (!STATE(GPS_FIX)) {
 128         altitudeCm = 0;
 129     }
 130
 131     const int16_t altitude = altitudeCm / 100;
 132     sbufWriteU16(dst, altitude);
 133 }
 134
 135 // GPS data, secondary, auxiliary data
 136 void ghstFrameGpsSecondaryTelemetry(sbuf_t *dst)
 137 {
 138     // use sbufWrite since CRC does not include frame length
 139     sbufWriteU8(dst, GHST_FRAME_GPS_PAYLOAD_SIZE + GHST_FRAME_LENGTH_CRC + GHST_FRAME_LENGTH_TYPE);
 140     sbufWriteU8(dst, GHST_DL_GPS_SECONDARY);
 141
 142     sbufWriteU16(dst, gpsSol.groundSpeed);      // speed in 0.1m/s
 143     sbufWriteU16(dst, gpsSol.groundCourse);     // degrees * 10
 144     sbufWriteU8(dst, gpsSol.numSat);
 145
 146     sbufWriteU16(dst, (uint16_t) (GPS_distanceToHome / 10));    // use units of 10m to increase range of U16 to 655.36km
 147     sbufWriteU16(dst, GPS_directionToHome);
 148
 149     uint8_t gpsFlags = 0;
 150     if (STATE(GPS_FIX)) {
 151         gpsFlags |= GPS_FLAGS_FIX;
 152     }
 153     if (STATE(GPS_FIX_HOME)) {
 154         gpsFlags |= GPS_FLAGS_FIX_HOME;
 155     }
 156     sbufWriteU8(dst, gpsFlags);
 157 }
 158
 159 // Mag, Baro (and Vario) data
 160 void ghstFrameMagBaro(sbuf_t *dst)
 161 {
 162     int16_t vario = 0;
 163     int16_t altitude = 0;
 164     int16_t yaw = 0;
 165     uint8_t flags = 0;
 166
 167 #ifdef USE_VARIO
 168     if (sensors(SENSOR_VARIO) && telemetryIsSensorEnabled(SENSOR_VARIO)) {
 169         vario = getEstimatedVario();       // vario, cm/s
 170         flags |= MISC_FLAGS_VARIO;
 171     }
 172 #endif
 173
 174 #ifdef USE_BARO
 175     if (sensors(SENSOR_BARO) && telemetryIsSensorEnabled(SENSOR_ALTITUDE)) {
 176         flags |= MISC_FLAGS_BAROALT;
 177         altitude = (constrain(getEstimatedAltitudeCm(), -32000 * 100, 32000 * 100) / 100);
 178     }
 179 #endif
 180
 181 #ifdef USE_MAG
 182     if (sensors(SENSOR_MAG) && telemetryIsSensorEnabled(SENSOR_HEADING)) {
 183         flags |= MISC_FLAGS_MAGHEAD;
 184         yaw = attitude.values.yaw;
 185     }
 186 #endif
 187
 188     // use sbufWrite since CRC does not include frame length
 189     sbufWriteU8(dst, GHST_FRAME_GPS_PAYLOAD_SIZE + GHST_FRAME_LENGTH_CRC + GHST_FRAME_LENGTH_TYPE);
 190     sbufWriteU8(dst, GHST_DL_MAGBARO);
 191
 192     sbufWriteU16(dst, yaw);                 // magHeading, deci-degrees
 193     sbufWriteU16(dst, altitude);            // baroAltitude, m
 194     sbufWriteU8(dst, vario);                // cm/s
 195
 196     sbufWriteU16(dst, 0);
 197     sbufWriteU16(dst, 0);
 198
 199     sbufWriteU8(dst, flags);
 200 }
 201
 202 // schedule array to decide how often each type of frame is sent
 203 typedef enum {
 204     GHST_FRAME_START_INDEX = 0,
 205     GHST_FRAME_PACK_INDEX = GHST_FRAME_START_INDEX, // Battery (Pack) data
 206     GHST_FRAME_GPS_PRIMARY_INDEX,                   // GPS, primary values (Lat, Long, Alt)
 207     GHST_FRAME_GPS_SECONDARY_INDEX,                 // GPS, secondary values (Sat Count, HDOP, etc.)
 208     GHST_FRAME_MAGBARO_INDEX,                       // Magnetometer/Baro values
 209     GHST_SCHEDULE_COUNT_MAX
 210 } ghstFrameTypeIndex_e;
 211
 212 static uint8_t ghstScheduleCount;
 213 static uint8_t ghstSchedule[GHST_SCHEDULE_COUNT_MAX];
 214
 215 static void processGhst(void)
 216 {
 217     static uint8_t ghstScheduleIndex = 0;
 218
 219     const uint8_t currentSchedule = ghstSchedule[ghstScheduleIndex];
 220
 221     sbuf_t ghstPayloadBuf;
 222     sbuf_t *dst = &ghstPayloadBuf;
 223
 224     if (currentSchedule & BIT(GHST_FRAME_PACK_INDEX)) {
 225         ghstInitializeFrame(dst);
 226         ghstFramePackTelemetry(dst);
 227         ghstFinalize(dst);
 228     }
 229
 230     if (currentSchedule & BIT(GHST_FRAME_GPS_PRIMARY_INDEX)) {
 231         ghstInitializeFrame(dst);
 232         ghstFrameGpsPrimaryTelemetry(dst);
 233         ghstFinalize(dst);
 234     }
 235
 236     if (currentSchedule & BIT(GHST_FRAME_GPS_SECONDARY_INDEX)) {
 237         ghstInitializeFrame(dst);
 238         ghstFrameGpsSecondaryTelemetry(dst);
 239         ghstFinalize(dst);
 240     }
 241
 242     if (currentSchedule & BIT(GHST_FRAME_MAGBARO_INDEX)) {
 243         ghstInitializeFrame(dst);
 244         ghstFrameMagBaro(dst);
 245         ghstFinalize(dst);
 246     }
 247
 248     ghstScheduleIndex = (ghstScheduleIndex + 1) % ghstScheduleCount;
 249 }
 250
 251 void initGhstTelemetry(void)
 252 {
 253     // If the GHST Rx driver is active, since tx and rx share the same pin, assume telemetry is enabled.
 254     ghstTelemetryEnabled = ghstRxIsActive();
 255
 256     if (!ghstTelemetryEnabled) {
 257         return;
 258     }
 259
 260     int index = 0;
 261     if ((isBatteryVoltageConfigured() && telemetryIsSensorEnabled(SENSOR_VOLTAGE))
 262         || (isAmperageConfigured() && telemetryIsSensorEnabled(SENSOR_CURRENT | SENSOR_FUEL))) {
 263         ghstSchedule[index++] = BIT(GHST_FRAME_PACK_INDEX);
 264     }
 265
 266 #ifdef USE_GPS
 267     if (featureIsEnabled(FEATURE_GPS)
 268        && telemetryIsSensorEnabled(SENSOR_ALTITUDE | SENSOR_LAT_LONG)) {
 269         ghstSchedule[index++] = BIT(GHST_FRAME_GPS_PRIMARY_INDEX);
 270     }
 271
 272     if (featureIsEnabled(FEATURE_GPS)
 273        && telemetryIsSensorEnabled(SENSOR_GROUND_SPEED | SENSOR_HEADING)) {
 274         ghstSchedule[index++] = BIT(GHST_FRAME_GPS_SECONDARY_INDEX);
 275      }
 276 #endif
 277
 278 #if defined(USE_BARO) || defined(USE_MAG) || defined(USE_VARIO)
 279     if ((sensors(SENSOR_BARO) && telemetryIsSensorEnabled(SENSOR_ALTITUDE))
 280         || (sensors(SENSOR_MAG) && telemetryIsSensorEnabled(SENSOR_HEADING))
 281         || (sensors(SENSOR_VARIO) && telemetryIsSensorEnabled(SENSOR_VARIO))) {
 282         ghstSchedule[index++] = BIT(GHST_FRAME_MAGBARO_INDEX);
 283     }
 284 #endif
 285
 286     ghstScheduleCount = (uint8_t)index;
 287  }
 288
 289 bool checkGhstTelemetryState(void)
 290 {
 291     return ghstTelemetryEnabled;
 292 }
 293
 294 // Called periodically by the scheduler
 295  void handleGhstTelemetry(timeUs_t currentTimeUs)
 296 {
 297     static uint32_t ghstLastCycleTime;
 298
 299     if (!ghstTelemetryEnabled) {
 300         return;
 301     }
 302
 303     // Ready to send telemetry?
 304     if (currentTimeUs >= ghstLastCycleTime + (GHST_CYCLETIME_US / ghstScheduleCount)) {
 305         ghstLastCycleTime = currentTimeUs;
 306         processGhst();
 307     }
 308
 309     // telemetry is sent from the Rx driver, ghstProcessFrame
 310 }
 311
 312 #endif