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 #ifdef USE_GPS_FIX_ESTIMATION
 152                 || STATE(GPS_ESTIMATED_FIX)
 153 #endif
 154             ) {
 155         gpsFlags |= GPS_FLAGS_FIX;
 156     }
 157     if (STATE(GPS_FIX_HOME)) {
 158         gpsFlags |= GPS_FLAGS_FIX_HOME;
 159     }
 160     sbufWriteU8(dst, gpsFlags);
 161 }
 162
 163 // schedule array to decide how often each type of frame is sent
 164 typedef enum {
 165     GHST_FRAME_START_INDEX = 0,
 166     GHST_FRAME_PACK_INDEX = GHST_FRAME_START_INDEX, // Battery (Pack) data
 167     GHST_FRAME_GPS_PRIMARY_INDEX,                   // GPS, primary values (Lat, Long, Alt)
 168     GHST_FRAME_GPS_SECONDARY_INDEX,                 // GPS, secondary values (Sat Count, HDOP, etc.)
 169    GHST_SCHEDULE_COUNT_MAX
 170 } ghstFrameTypeIndex_e;
 171
 172 static uint8_t ghstScheduleCount;
 173 static uint8_t ghstSchedule[GHST_SCHEDULE_COUNT_MAX];
 174
 175 static void processGhst(void)
 176 {
 177     static uint8_t ghstScheduleIndex = 0;
 178
 179     const uint8_t currentSchedule = ghstSchedule[ghstScheduleIndex];
 180
 181     sbuf_t ghstPayloadBuf;
 182     sbuf_t *dst = &ghstPayloadBuf;
 183
 184     if (currentSchedule & BIT(GHST_FRAME_PACK_INDEX)) {
 185         ghstInitializeFrame(dst);
 186         ghstFramePackTelemetry(dst);
 187         ghstFinalize(dst);
 188     }
 189
 190     if (currentSchedule & BIT(GHST_FRAME_GPS_PRIMARY_INDEX)) {
 191         ghstInitializeFrame(dst);
 192         ghstFrameGpsPrimaryTelemetry(dst);
 193         ghstFinalize(dst);
 194     }
 195
 196    if (currentSchedule & BIT(GHST_FRAME_GPS_SECONDARY_INDEX)) {
 197         ghstInitializeFrame(dst);
 198         ghstFrameGpsSecondaryTelemetry(dst);
 199         ghstFinalize(dst);
 200     }
 201
 202     ghstScheduleIndex = (ghstScheduleIndex + 1) % ghstScheduleCount;
 203 }
 204
 205 void initGhstTelemetry(void)
 206 {
 207     // If the GHST Rx driver is active, since tx and rx share the same pin, assume telemetry is enabled.
 208     ghstTelemetryEnabled = ghstRxIsActive();
 209
 210     if (!ghstTelemetryEnabled) {
 211         return;
 212     }
 213
 214     int index = 0;
 215     if (isBatteryVoltageConfigured() || isAmperageConfigured()) {
 216         ghstSchedule[index++] = BIT(GHST_FRAME_PACK_INDEX);
 217     }
 218
 219 #ifdef USE_GPS
 220     if (feature(FEATURE_GPS)) {
 221         ghstSchedule[index++] = BIT(GHST_FRAME_GPS_PRIMARY_INDEX);
 222         ghstSchedule[index++] = BIT(GHST_FRAME_GPS_SECONDARY_INDEX);
 223     }
 224 #endif
 225
 226     ghstScheduleCount = (uint8_t)index;
 227  }
 228
 229 bool checkGhstTelemetryState(void)
 230 {
 231     return ghstTelemetryEnabled;
 232 }
 233
 234 // Called periodically by the scheduler
 235  void handleGhstTelemetry(timeUs_t currentTimeUs)
 236 {
 237     static timeUs_t ghstLastCycleTime;
 238
 239     if (!ghstTelemetryEnabled) {
 240         return;
 241     }
 242
 243     // Ready to send telemetry?
 244     if (currentTimeUs >= ghstLastCycleTime + (GHST_CYCLETIME_US / ghstScheduleCount)) {
 245         ghstLastCycleTime = currentTimeUs;
 246         processGhst();
 247     }
 248
 249     // telemetry is sent from the Rx driver, ghstProcessFrame
 250 }
 251
 252 #endif