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 #include "build/debug.h"
  33
  34 #include "config/feature.h"
  35 #include "pg/pg.h"
  36 #include "pg/pg_ids.h"
  37
  38 #include "common/crc.h"
  39 #include "common/maths.h"
  40 #include "common/printf.h"
  41 #include "common/streambuf.h"
  42 #include "common/utils.h"
  43
  44 #include "cms/cms.h"
  45
  46 #include "drivers/nvic.h"
  47
  48 #include "config/config.h"
  49 #include "fc/rc_modes.h"
  50 #include "fc/runtime_config.h"
  51
  52 #include "flight/imu.h"
  53 #include "flight/position.h"
  54
  55 #include "io/gps.h"
  56 #include "io/serial.h"
  57
  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 #define GHST_CYCLETIME_US                   100000      // 10x/sec
  70 #define GHST_FRAME_PACK_PAYLOAD_SIZE        10
  71 #define GHST_FRAME_GPS_PAYLOAD_SIZE         10
  72 #define GHST_FRAME_MAGBARO_PAYLOAD_SIZE     10
  73
  74 #define GHST_MSP_BUFFER_SIZE    96
  75 #define GHST_UL_MSP_FRAME_SIZE  10
  76 #define GHST_DL_MSP_FRAME_SIZE   6
  77 #define GHST_MSP_LENGTH_OFFSET   1
  78
  79 static bool ghstTelemetryEnabled;
  80 static uint8_t ghstFrame[GHST_FRAME_SIZE];
  81
  82 static void ghstInitializeFrame(sbuf_t *dst)
  83 {
  84     dst->ptr = ghstFrame;
  85     dst->end = ARRAYEND(ghstFrame);
  86
  87     sbufWriteU8(dst, GHST_ADDR_RX);
  88 }
  89
  90 static void ghstFinalize(sbuf_t *dst)
  91 {
  92     crc8_dvb_s2_sbuf_append(dst, &ghstFrame[2]); // start at byte 2, since CRC does not include device address and frame length
  93     sbufSwitchToReader(dst, ghstFrame);
  94     // write the telemetry frame to the receiver.
  95     ghstRxWriteTelemetryData(sbufPtr(dst), sbufBytesRemaining(dst));
  96 }
  97
  98 // Battery (Pack) status
  99 void ghstFramePackTelemetry(sbuf_t *dst)
 100 {
 101     // use sbufWrite since CRC does not include frame length
 102     sbufWriteU8(dst, GHST_FRAME_PACK_PAYLOAD_SIZE + GHST_FRAME_LENGTH_CRC + GHST_FRAME_LENGTH_TYPE);
 103     sbufWriteU8(dst, GHST_DL_PACK_STAT);
 104
 105     if (telemetryConfig()->report_cell_voltage) {
 106         sbufWriteU16(dst, getBatteryAverageCellVoltage());      // units of 10mV
 107     } else {
 108         sbufWriteU16(dst, getBatteryVoltage());
 109     }
 110     sbufWriteU16(dst, getAmperage());                           // units of 10mA
 111
 112     sbufWriteU16(dst, getMAhDrawn() / 10);                      // units of 10mAh (range of 0-655.36Ah)
 113
 114     sbufWriteU8(dst, 0x00);                     // Rx Voltage, units of 100mV (not passed from BF, added in Ghost Rx)
 115
 116     sbufWriteU8(dst, 0x00);                     // tbd1
 117     sbufWriteU8(dst, 0x00);                     // tbd2
 118     sbufWriteU8(dst, 0x00);                     // tbd3
 119 }
 120
 121 // GPS data, primary, positional data
 122 void ghstFrameGpsPrimaryTelemetry(sbuf_t *dst)
 123 {
 124     // use sbufWrite since CRC does not include frame length
 125     sbufWriteU8(dst, GHST_FRAME_GPS_PAYLOAD_SIZE + GHST_FRAME_LENGTH_CRC + GHST_FRAME_LENGTH_TYPE);
 126     sbufWriteU8(dst, GHST_DL_GPS_PRIMARY);
 127
 128     sbufWriteU32(dst, gpsSol.llh.lat);
 129     sbufWriteU32(dst, gpsSol.llh.lon);
 130
 131     int32_t altitudeCm = gpsSol.llh.altCm;      // gps Altitude (absolute)
 132     if (!STATE(GPS_FIX)) {
 133         altitudeCm = 0;
 134     }
 135
 136     const int16_t altitude = altitudeCm / 100;
 137     sbufWriteU16(dst, altitude);
 138 }
 139
 140 // GPS data, secondary, auxiliary data
 141 void ghstFrameGpsSecondaryTelemetry(sbuf_t *dst)
 142 {
 143     // use sbufWrite since CRC does not include frame length
 144     sbufWriteU8(dst, GHST_FRAME_GPS_PAYLOAD_SIZE + GHST_FRAME_LENGTH_CRC + GHST_FRAME_LENGTH_TYPE);
 145     sbufWriteU8(dst, GHST_DL_GPS_SECONDARY);
 146
 147     sbufWriteU16(dst, gpsSol.groundSpeed);      // speed in 0.1m/s
 148     sbufWriteU16(dst, gpsSol.groundCourse);     // degrees * 10
 149     sbufWriteU8(dst, gpsSol.numSat);
 150
 151     sbufWriteU16(dst, GPS_distanceToHome / 10);    // use units of 10m to increase range of U16 to 655.36km
 152     sbufWriteU16(dst, GPS_directionToHome / 10);
 153
 154     uint8_t gpsFlags = 0;
 155     if (STATE(GPS_FIX)) {
 156         gpsFlags |= GPS_FLAGS_FIX;
 157     }
 158     if (STATE(GPS_FIX_HOME)) {
 159         gpsFlags |= GPS_FLAGS_FIX_HOME;
 160     }
 161     sbufWriteU8(dst, gpsFlags);
 162 }
 163
 164 // Mag, Baro (and Vario) data
 165 void ghstFrameMagBaro(sbuf_t *dst)
 166 {
 167     int16_t vario = 0;
 168     int16_t altitude = 0;
 169     int16_t yaw = 0;
 170     uint8_t flags = 0;
 171
 172 #ifdef USE_VARIO
 173     if (sensors(SENSOR_VARIO) && telemetryIsSensorEnabled(SENSOR_VARIO)) {
 174         vario = getEstimatedVario();       // vario, cm/s
 175         flags |= MISC_FLAGS_VARIO;
 176     }
 177 #endif
 178
 179 #ifdef USE_BARO
 180     if (sensors(SENSOR_BARO) && telemetryIsSensorEnabled(SENSOR_ALTITUDE)) {
 181         flags |= MISC_FLAGS_BAROALT;
 182         altitude = (constrain(getEstimatedAltitudeCm(), -32000 * 100, 32000 * 100) / 100);
 183     }
 184 #endif
 185
 186 #ifdef USE_MAG
 187     if (sensors(SENSOR_MAG) && telemetryIsSensorEnabled(SENSOR_HEADING)) {
 188         flags |= MISC_FLAGS_MAGHEAD;
 189         yaw = attitude.values.yaw;
 190     }
 191 #endif
 192
 193     // use sbufWrite since CRC does not include frame length
 194     sbufWriteU8(dst, GHST_FRAME_MAGBARO_PAYLOAD_SIZE + GHST_FRAME_LENGTH_CRC + GHST_FRAME_LENGTH_TYPE);
 195     sbufWriteU8(dst, GHST_DL_MAGBARO);
 196
 197     sbufWriteU16(dst, yaw);                 // magHeading, deci-degrees
 198     sbufWriteU16(dst, altitude);            // baroAltitude, m
 199     sbufWriteU8(dst, vario);                // cm/s
 200
 201     sbufWriteU16(dst, 0);
 202     sbufWriteU16(dst, 0);
 203
 204     sbufWriteU8(dst, flags);
 205 }
 206
 207 // schedule array to decide how often each type of frame is sent
 208 typedef enum {
 209     GHST_FRAME_START_INDEX = 0,
 210     GHST_FRAME_PACK_INDEX = GHST_FRAME_START_INDEX, // Battery (Pack) data
 211     GHST_FRAME_GPS_PRIMARY_INDEX,                   // GPS, primary values (Lat, Long, Alt)
 212     GHST_FRAME_GPS_SECONDARY_INDEX,                 // GPS, secondary values (Sat Count, HDOP, etc.)
 213     GHST_FRAME_MAGBARO_INDEX,                       // Magnetometer/Baro values
 214     GHST_SCHEDULE_COUNT_MAX
 215 } ghstFrameTypeIndex_e;
 216
 217 static uint8_t ghstScheduleCount;
 218 static uint8_t ghstSchedule[GHST_SCHEDULE_COUNT_MAX];
 219
 220 static bool mspReplyPending;
 221
 222 void ghstScheduleMspResponse()
 223 {
 224     mspReplyPending = true;
 225 }
 226
 227 static void ghstSendMspResponse(uint8_t *payload, const uint8_t payloadSize)
 228 {
 229     sbuf_t ghstPayloadBuf;
 230     sbuf_t *dst = &ghstPayloadBuf;
 231
 232     static uint8_t mspFrameCounter = 0;
 233     DEBUG_SET(DEBUG_GHST_MSP, 1, ++mspFrameCounter);
 234
 235     ghstInitializeFrame(dst);                                                               // addr
 236     sbufWriteU8(dst, GHST_PAYLOAD_SIZE + GHST_FRAME_LENGTH_CRC + GHST_FRAME_LENGTH_TYPE);   // lenght
 237     sbufWriteU8(dst, GHST_DL_MSP_RESP);                                                 // type
 238     sbufWriteData(dst, payload, payloadSize);                                           // payload
 239     for(int i = 0; i < GHST_PAYLOAD_SIZE - payloadSize; ++i) {                          // payload fill zeroes
 240         sbufWriteU8(dst, 0);
 241     }
 242     ghstFinalize(dst);  // crc
 243 }
 244
 245 static void processGhst(void)
 246 {
 247     static uint8_t ghstScheduleIndex = 0;
 248
 249     const uint8_t currentSchedule = ghstSchedule[ghstScheduleIndex];
 250
 251     sbuf_t ghstPayloadBuf;
 252     sbuf_t *dst = &ghstPayloadBuf;
 253
 254     if (currentSchedule & BIT(GHST_FRAME_PACK_INDEX)) {
 255         ghstInitializeFrame(dst);
 256         ghstFramePackTelemetry(dst);
 257         ghstFinalize(dst);
 258     }
 259
 260 #if defined(USE_GPS)
 261     if (currentSchedule & BIT(GHST_FRAME_GPS_PRIMARY_INDEX)) {
 262         ghstInitializeFrame(dst);
 263         ghstFrameGpsPrimaryTelemetry(dst);
 264         ghstFinalize(dst);
 265     }
 266
 267     if (currentSchedule & BIT(GHST_FRAME_GPS_SECONDARY_INDEX)) {
 268         ghstInitializeFrame(dst);
 269         ghstFrameGpsSecondaryTelemetry(dst);
 270         ghstFinalize(dst);
 271     }
 272 #endif
 273
 274     if (currentSchedule & BIT(GHST_FRAME_MAGBARO_INDEX)) {
 275         ghstInitializeFrame(dst);
 276         ghstFrameMagBaro(dst);
 277         ghstFinalize(dst);
 278     }
 279
 280     ghstScheduleIndex = (ghstScheduleIndex + 1) % ghstScheduleCount;
 281 }
 282
 283 void initGhstTelemetry(void)
 284 {
 285     // If the GHST Rx driver is active, since tx and rx share the same pin, assume telemetry
 286     // can be initialized but not enabled yet.
 287     if (!ghstRxIsActive()) {
 288         return;
 289     }
 290
 291     ghstTelemetryEnabled = false;
 292 #if defined(USE_MSP_OVER_TELEMETRY)
 293     mspReplyPending = false;
 294 #endif
 295
 296     int index = 0;
 297     if ((isBatteryVoltageConfigured() && telemetryIsSensorEnabled(SENSOR_VOLTAGE))
 298         || (isAmperageConfigured() && telemetryIsSensorEnabled(SENSOR_CURRENT | SENSOR_FUEL))) {
 299         ghstSchedule[index++] = BIT(GHST_FRAME_PACK_INDEX);
 300     }
 301
 302 #ifdef USE_GPS
 303     if (featureIsEnabled(FEATURE_GPS)
 304        && telemetryIsSensorEnabled(SENSOR_ALTITUDE | SENSOR_LAT_LONG)) {
 305         ghstSchedule[index++] = BIT(GHST_FRAME_GPS_PRIMARY_INDEX);
 306     }
 307
 308     if (featureIsEnabled(FEATURE_GPS)
 309        && telemetryIsSensorEnabled(SENSOR_GROUND_SPEED | SENSOR_HEADING)) {
 310         ghstSchedule[index++] = BIT(GHST_FRAME_GPS_SECONDARY_INDEX);
 311      }
 312 #endif
 313
 314 #if defined(USE_BARO) || defined(USE_MAG) || defined(USE_VARIO)
 315     if ((sensors(SENSOR_BARO) && telemetryIsSensorEnabled(SENSOR_ALTITUDE))
 316         || (sensors(SENSOR_MAG) && telemetryIsSensorEnabled(SENSOR_HEADING))
 317         || (sensors(SENSOR_VARIO) && telemetryIsSensorEnabled(SENSOR_VARIO))) {
 318         ghstSchedule[index++] = BIT(GHST_FRAME_MAGBARO_INDEX);
 319     }
 320 #endif
 321
 322     ghstScheduleCount = index;
 323  }
 324
 325 void setGhstTelemetryState(bool state)
 326 {
 327     ghstTelemetryEnabled = state;
 328 }
 329
 330 bool checkGhstTelemetryState(void)
 331 {
 332     return ghstTelemetryEnabled;
 333 }
 334
 335 // Called periodically by the scheduler
 336 void handleGhstTelemetry(timeUs_t currentTimeUs)
 337 {
 338     static timeUs_t ghstLastCycleTime;
 339
 340     if (!ghstTelemetryEnabled) {
 341         return;
 342     }
 343
 344     // Send ad-hoc response frames as soon as possible
 345 #if defined(USE_MSP_OVER_TELEMETRY)
 346     if (mspReplyPending) {
 347         ghstLastCycleTime = currentTimeUs;
 348         if (ghstRxGetTelemetryBufLen() == 0) {
 349             mspReplyPending = sendMspReply(GHST_DL_MSP_FRAME_SIZE, ghstSendMspResponse);
 350         }
 351         return;
 352     }
 353 #endif
 354
 355     // Ready to send telemetry?
 356     if (currentTimeUs >= ghstLastCycleTime + (GHST_CYCLETIME_US / ghstScheduleCount)) {
 357         ghstLastCycleTime = currentTimeUs;
 358         processGhst();
 359     }
 360
 361     // telemetry is sent from the Rx driver, ghstProcessFrame
 362 }
 363
 364 #endif