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 "config/parameter_group_ids.h"
  35
  36 #include "common/crc.h"
  37 #include "common/maths.h"
  38 #include "common/printf.h"
  39 #include "common/streambuf.h"
  40 #include "common/utils.h"
  41
  42 #include "cms/cms.h"
  43
  44 #include "drivers/nvic.h"
  45
  46 #include "fc/config.h"
  47 #include "fc/rc_modes.h"
  48 #include "fc/runtime_config.h"
  49
  50 #include "flight/imu.h"
  51
  52 #include "io/gps.h"
  53 #include "io/serial.h"
  54
  55 #include "navigation/navigation.h"
  56
  57 #include "rx/rx.h"
  58 #include "rx/ghst.h"
  59 #include "rx/ghst_protocol.h"
  60
  61 #include "sensors/battery.h"
  62 #include "sensors/sensors.h"
  63
  64 #include "telemetry/telemetry.h"
  65 #include "telemetry/msp_shared.h"
  66
  67 #include "telemetry/ghst.h"
  68
  69 #include "build/debug.h"
  70
  71 #define GHST_CYCLETIME_US                   100000      // 10x/sec
  72 #define GHST_FRAME_PACK_PAYLOAD_SIZE        10
  73 #define GHST_FRAME_GPS_PAYLOAD_SIZE         10
  74 #define GHST_FRAME_LENGTH_CRC               1
  75 #define GHST_FRAME_LENGTH_TYPE              1
  76
  77 static bool ghstTelemetryEnabled;
  78 static uint8_t ghstFrame[GHST_FRAME_SIZE_MAX];
  79
  80 static void ghstInitializeFrame(sbuf_t *dst)
  81 {
  82     dst->ptr = ghstFrame;
  83     dst->end = ARRAYEND(ghstFrame);
  84
  85     sbufWriteU8(dst, GHST_ADDR_RX);
  86 }
  87
  88 static void ghstFinalize(sbuf_t *dst)
  89 {
  90     crc8_dvb_s2_sbuf_append(dst, &ghstFrame[2]); // start at byte 2, since CRC does not include device address and frame length
  91     sbufSwitchToReader(dst, ghstFrame);
  92     // write the telemetry frame to the receiver.
  93     ghstRxWriteTelemetryData(sbufPtr(dst), sbufBytesRemaining(dst));
  94 }
  95
  96 // Battery (Pack) status
  97 void ghstFramePackTelemetry(sbuf_t *dst)
  98 {
  99     // use sbufWrite since CRC does not include frame length
 100     sbufWriteU8(dst, GHST_FRAME_PACK_PAYLOAD_SIZE + GHST_FRAME_LENGTH_CRC + GHST_FRAME_LENGTH_TYPE);
 101     sbufWriteU8(dst, 0x23);                     // GHST_DL_PACK_STAT
 102
 103     if (telemetryConfig()->report_cell_voltage) {
 104         sbufWriteU16(dst, getBatteryAverageCellVoltage());      // units of 10mV
 105     } else {
 106         sbufWriteU16(dst, getBatteryVoltage());
 107     }
 108     sbufWriteU16(dst, getAmperage());                           // units of 10mA
 109
 110     sbufWriteU16(dst, getMAhDrawn() / 10);                      // units of 10mAh (range of 0-655.36Ah)
 111
 112     sbufWriteU8(dst, 0x00);                     // Rx Voltage, units of 100mV (not passed from BF, added in Ghost Rx)
 113
 114     sbufWriteU8(dst, 0x00);                     // tbd1
 115     sbufWriteU8(dst, 0x00);                     // tbd2
 116     sbufWriteU8(dst, 0x00);                     // tbd3
 117 }
 118
 119
 120 // GPS data, primary, positional data
 121 void ghstFrameGpsPrimaryTelemetry(sbuf_t *dst)
 122 {
 123     // use sbufWrite since CRC does not include frame length
 124     sbufWriteU8(dst, GHST_FRAME_GPS_PAYLOAD_SIZE + GHST_FRAME_LENGTH_CRC + GHST_FRAME_LENGTH_TYPE);
 125     sbufWriteU8(dst, GHST_DL_GPS_PRIMARY);
 126
 127     sbufWriteU32(dst, gpsSol.llh.lat);
 128     sbufWriteU32(dst, gpsSol.llh.lon);
 129
 130     // constrain alt. from -32,000m to +32,000m, units of meters
 131     const int16_t altitude = (constrain(getEstimatedActualPosition(Z), -32000 * 100, 32000 * 100) / 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 // schedule array to decide how often each type of frame is sent
 160 typedef enum {
 161     GHST_FRAME_START_INDEX = 0,
 162     GHST_FRAME_PACK_INDEX = GHST_FRAME_START_INDEX, // Battery (Pack) data
 163     GHST_FRAME_GPS_PRIMARY_INDEX,                   // GPS, primary values (Lat, Long, Alt)
 164     GHST_FRAME_GPS_SECONDARY_INDEX,                 // GPS, secondary values (Sat Count, HDOP, etc.)
 165    GHST_SCHEDULE_COUNT_MAX
 166 } ghstFrameTypeIndex_e;
 167
 168 static uint8_t ghstScheduleCount;
 169 static uint8_t ghstSchedule[GHST_SCHEDULE_COUNT_MAX];
 170
 171 static void processGhst(void)
 172 {
 173     static uint8_t ghstScheduleIndex = 0;
 174
 175     const uint8_t currentSchedule = ghstSchedule[ghstScheduleIndex];
 176
 177     sbuf_t ghstPayloadBuf;
 178     sbuf_t *dst = &ghstPayloadBuf;
 179
 180     if (currentSchedule & BIT(GHST_FRAME_PACK_INDEX)) {
 181         ghstInitializeFrame(dst);
 182         ghstFramePackTelemetry(dst);
 183         ghstFinalize(dst);
 184     }
 185
 186     if (currentSchedule & BIT(GHST_FRAME_GPS_PRIMARY_INDEX)) {
 187         ghstInitializeFrame(dst);
 188         ghstFrameGpsPrimaryTelemetry(dst);
 189         ghstFinalize(dst);
 190     }
 191
 192    if (currentSchedule & BIT(GHST_FRAME_GPS_SECONDARY_INDEX)) {
 193         ghstInitializeFrame(dst);
 194         ghstFrameGpsSecondaryTelemetry(dst);
 195         ghstFinalize(dst);
 196     }
 197
 198     ghstScheduleIndex = (ghstScheduleIndex + 1) % ghstScheduleCount;
 199 }
 200
 201 void initGhstTelemetry(void)
 202 {
 203     // If the GHST Rx driver is active, since tx and rx share the same pin, assume telemetry is enabled.
 204     ghstTelemetryEnabled = ghstRxIsActive();
 205
 206     if (!ghstTelemetryEnabled) {
 207         return;
 208     }
 209
 210     int index = 0;
 211     if (isBatteryVoltageConfigured() || isAmperageConfigured()) {
 212         ghstSchedule[index++] = BIT(GHST_FRAME_PACK_INDEX);
 213     }
 214
 215 #ifdef USE_GPS
 216     if (feature(FEATURE_GPS)) {
 217         ghstSchedule[index++] = BIT(GHST_FRAME_GPS_PRIMARY_INDEX);
 218         ghstSchedule[index++] = BIT(GHST_FRAME_GPS_SECONDARY_INDEX);
 219     }
 220 #endif
 221
 222     ghstScheduleCount = (uint8_t)index;
 223  }
 224
 225 bool checkGhstTelemetryState(void)
 226 {
 227     return ghstTelemetryEnabled;
 228 }
 229
 230 // Called periodically by the scheduler
 231  void handleGhstTelemetry(timeUs_t currentTimeUs)
 232 {
 233     static timeUs_t ghstLastCycleTime;
 234
 235     if (!ghstTelemetryEnabled) {
 236         return;
 237     }
 238
 239     // Ready to send telemetry?
 240     if (currentTimeUs >= ghstLastCycleTime + (GHST_CYCLETIME_US / ghstScheduleCount)) {
 241         ghstLastCycleTime = currentTimeUs;
 242         processGhst();
 243     }
 244
 245     // telemetry is sent from the Rx driver, ghstProcessFrame
 246 }
 247
 248 #endif