src/main/telemetry/crsf.c

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   4  * Cleanflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Cleanflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <stdbool.h>
  19 #include <stdint.h>
  20 #include <string.h>
  21
  22 #include "platform.h"
  23
  24 #ifdef TELEMETRY
  25
  26 #include "config/feature.h"
  27 #include "build/version.h"
  28
  29 #include "config/parameter_group.h"
  30 #include "config/parameter_group_ids.h"
  31
  32 #include "common/maths.h"
  33 #include "common/streambuf.h"
  34 #include "common/utils.h"
  35
  36 #include "sensors/battery.h"
  37
  38 #include "io/gps.h"
  39 #include "io/serial.h"
  40
  41 #include "fc/rc_controls.h"
  42 #include "fc/runtime_config.h"
  43
  44 #include "io/gps.h"
  45
  46 #include "flight/imu.h"
  47
  48 #include "rx/rx.h"
  49 #include "rx/crsf.h"
  50
  51 #include "telemetry/telemetry.h"
  52 #include "telemetry/crsf.h"
  53
  54 #include "fc/config.h"
  55
  56 #define CRSF_CYCLETIME_US                   100000 // 100ms, 10 Hz
  57
  58 static bool crsfTelemetryEnabled;
  59 static uint8_t crsfFrame[CRSF_FRAME_SIZE_MAX];
  60
  61 static void crsfInitializeFrame(sbuf_t *dst)
  62 {
  63     dst->ptr = crsfFrame;
  64     dst->end = ARRAYEND(crsfFrame);
  65
  66     sbufWriteU8(dst, CRSF_ADDRESS_BROADCAST);
  67 }
  68
  69 static void crsfWriteCrc(sbuf_t *dst, uint8_t *start)
  70 {
  71     uint8_t crc = 0;
  72     uint8_t *end = sbufPtr(dst);
  73     for (uint8_t *ptr = start; ptr < end; ++ptr) {
  74         crc = crc8_dvb_s2(crc, *ptr);
  75     }
  76     sbufWriteU8(dst, crc);
  77 }
  78
  79 static void crsfFinalize(sbuf_t *dst)
  80 {
  81     crsfWriteCrc(dst, &crsfFrame[2]); // start at byte 2, since CRC does not include device address and frame length
  82     sbufSwitchToReader(dst, crsfFrame);
  83     // write the telemetry frame to the receiver.
  84     crsfRxWriteTelemetryData(sbufPtr(dst), sbufBytesRemaining(dst));
  85 }
  86
  87 static int crsfFinalizeBuf(sbuf_t *dst, uint8_t *frame)
  88 {
  89     crsfWriteCrc(dst, &crsfFrame[2]); // start at byte 2, since CRC does not include device address and frame length
  90     sbufSwitchToReader(dst, crsfFrame);
  91     const int frameSize = sbufBytesRemaining(dst);
  92     for (int ii = 0; sbufBytesRemaining(dst); ++ii) {
  93         frame[ii] = sbufReadU8(dst);
  94     }
  95     return frameSize;
  96 }
  97
  98 /*
  99 CRSF frame has the structure:
 100 <Device address> <Frame length> <Type> <Payload> <CRC>
 101 Device address: (uint8_t)
 102 Frame length:   length in  bytes including Type (uint8_t)
 103 Type:           (uint8_t)
 104 CRC:            (uint8_t), crc of <Type> and <Payload>
 105 */
 106
 107 /*
 108 0x02 GPS
 109 Payload:
 110 int32_t     Latitude ( degree / 10`000`000 )
 111 int32_t     Longitude (degree / 10`000`000 )
 112 uint16_t    Groundspeed ( km/h / 10 )
 113 uint16_t    GPS heading ( degree / 100 )
 114 uint16      Altitude ( meter  1000m offset )
 115 uint8_t     Satellites in use ( counter )
 116 */
 117 void crsfFrameGps(sbuf_t *dst)
 118 {
 119     // use sbufWrite since CRC does not include frame length
 120     sbufWriteU8(dst, CRSF_FRAME_GPS_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC);
 121     sbufWriteU8(dst, CRSF_FRAMETYPE_GPS);
 122     sbufWriteU32BigEndian(dst, GPS_coord[LAT]); // CRSF and betaflight use same units for degrees
 123     sbufWriteU32BigEndian(dst, GPS_coord[LON]);
 124     sbufWriteU16BigEndian(dst, (GPS_speed * 36 + 5) / 10); // GPS_speed is in 0.1m/s
 125     sbufWriteU16BigEndian(dst, GPS_ground_course * 10); // GPS_ground_course is degrees * 10
 126     //Send real GPS altitude only if it's reliable (there's a GPS fix)
 127     const uint16_t altitude = (STATE(GPS_FIX) ? GPS_altitude : 0) + 1000;
 128     sbufWriteU16BigEndian(dst, altitude);
 129     sbufWriteU8(dst, GPS_numSat);
 130 }
 131
 132 /*
 133 0x08 Battery sensor
 134 Payload:
 135 uint16_t    Voltage ( mV * 100 )
 136 uint16_t    Current ( mA * 100 )
 137 uint24_t    Capacity ( mAh )
 138 uint8_t     Battery remaining ( percent )
 139 */
 140 void crsfFrameBatterySensor(sbuf_t *dst)
 141 {
 142     // use sbufWrite since CRC does not include frame length
 143     sbufWriteU8(dst, CRSF_FRAME_BATTERY_SENSOR_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC);
 144     sbufWriteU8(dst, CRSF_FRAMETYPE_BATTERY_SENSOR);
 145     sbufWriteU16BigEndian(dst, getBatteryVoltage()); // vbat is in units of 0.1V
 146 #ifdef CLEANFLIGHT
 147     const amperageMeter_t *amperageMeter = getAmperageMeter(batteryConfig()->amperageMeterSource);
 148     const int16_t amperage = constrain(amperageMeter->amperage, -0x8000, 0x7FFF) / 10; // send amperage in 0.01 A steps, range is -320A to 320A
 149     sbufWriteU16BigEndian(dst, amperage); // amperage is in units of 0.1A
 150     const uint32_t batteryCapacity = batteryConfig()->batteryCapacity;
 151     const uint8_t batteryRemainingPercentage = batteryCapacityRemainingPercentage();
 152 #else
 153     sbufWriteU16BigEndian(dst, getAmperage() / 10);
 154     const uint32_t batteryCapacity = batteryConfig()->batteryCapacity;
 155     const uint8_t batteryRemainingPercentage = calculateBatteryPercentageRemaining();
 156 #endif
 157     sbufWriteU8(dst, (batteryCapacity >> 16));
 158     sbufWriteU8(dst, (batteryCapacity >> 8));
 159     sbufWriteU8(dst, (uint8_t)batteryCapacity);
 160
 161     sbufWriteU8(dst, batteryRemainingPercentage);
 162 }
 163
 164 typedef enum {
 165     CRSF_ACTIVE_ANTENNA1 = 0,
 166     CRSF_ACTIVE_ANTENNA2 = 1
 167 } crsfActiveAntenna_e;
 168
 169 typedef enum {
 170     CRSF_RF_MODE_4_HZ = 0,
 171     CRSF_RF_MODE_50_HZ = 1,
 172     CRSF_RF_MODE_150_HZ = 2
 173 } crsrRfMode_e;
 174
 175 typedef enum {
 176     CRSF_RF_POWER_0_mW = 0,
 177     CRSF_RF_POWER_10_mW = 1,
 178     CRSF_RF_POWER_25_mW = 2,
 179     CRSF_RF_POWER_100_mW = 3,
 180     CRSF_RF_POWER_500_mW = 4,
 181     CRSF_RF_POWER_1000_mW = 5,
 182     CRSF_RF_POWER_2000_mW = 6
 183 } crsrRfPower_e;
 184
 185 /*
 186 0x1E Attitude
 187 Payload:
 188 int16_t     Pitch angle ( rad / 10000 )
 189 int16_t     Roll angle ( rad / 10000 )
 190 int16_t     Yaw angle ( rad / 10000 )
 191 */
 192
 193 #define DECIDEGREES_TO_RADIANS10000(angle) ((int16_t)(1000.0f * (angle) * RAD))
 194
 195 void crsfFrameAttitude(sbuf_t *dst)
 196 {
 197      sbufWriteU8(dst, CRSF_FRAME_ATTITUDE_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC);
 198      sbufWriteU8(dst, CRSF_FRAMETYPE_ATTITUDE);
 199      sbufWriteU16BigEndian(dst, DECIDEGREES_TO_RADIANS10000(attitude.values.pitch));
 200      sbufWriteU16BigEndian(dst, DECIDEGREES_TO_RADIANS10000(attitude.values.roll));
 201      sbufWriteU16BigEndian(dst, DECIDEGREES_TO_RADIANS10000(attitude.values.yaw));
 202 }
 203
 204 /*
 205 0x21 Flight mode text based
 206 Payload:
 207 char[]      Flight mode ( Nullterminated string )
 208 */
 209 void crsfFrameFlightMode(sbuf_t *dst)
 210 {
 211     // write zero for frame length, since we don't know it yet
 212     uint8_t *lengthPtr = sbufPtr(dst);
 213     sbufWriteU8(dst, 0);
 214     sbufWriteU8(dst, CRSF_FRAMETYPE_FLIGHT_MODE);
 215
 216     // use same logic as OSD, so telemetry displays same flight text as OSD
 217     const char *flightMode = "ACRO";
 218     if (isAirmodeActive()) {
 219         flightMode = "AIR";
 220     }
 221     if (FLIGHT_MODE(FAILSAFE_MODE)) {
 222         flightMode = "!FS";
 223     } else if (FLIGHT_MODE(ANGLE_MODE)) {
 224         flightMode = "STAB";
 225     } else if (FLIGHT_MODE(HORIZON_MODE)) {
 226         flightMode = "HOR";
 227     }
 228     sbufWriteString(dst, flightMode);
 229     // write in the frame length
 230     *lengthPtr = sbufPtr(dst) - lengthPtr;
 231 }
 232
 233 #define BV(x)  (1 << (x)) // bit value
 234
 235 // schedule array to decide how often each type of frame is sent
 236 #define CRSF_SCHEDULE_COUNT_MAX     5
 237 static uint8_t crsfScheduleCount;
 238 static uint8_t crsfSchedule[CRSF_SCHEDULE_COUNT_MAX];
 239
 240
 241 static void processCrsf(void)
 242 {
 243     static uint8_t crsfScheduleIndex = 0;
 244     const uint8_t currentSchedule = crsfSchedule[crsfScheduleIndex];
 245
 246     sbuf_t crsfPayloadBuf;
 247     sbuf_t *dst = &crsfPayloadBuf;
 248
 249     if (currentSchedule & BV(CRSF_FRAME_ATTITUDE)) {
 250         crsfInitializeFrame(dst);
 251         crsfFrameAttitude(dst);
 252         crsfFinalize(dst);
 253     }
 254     if (currentSchedule & BV(CRSF_FRAME_BATTERY_SENSOR)) {
 255         crsfInitializeFrame(dst);
 256         crsfFrameBatterySensor(dst);
 257         crsfFinalize(dst);
 258     }
 259     if (currentSchedule & BV(CRSF_FRAME_FLIGHT_MODE)) {
 260         crsfInitializeFrame(dst);
 261         crsfFrameFlightMode(dst);
 262         crsfFinalize(dst);
 263     }
 264 #ifdef GPS
 265     if (currentSchedule & BV(CRSF_FRAME_GPS)) {
 266         crsfInitializeFrame(dst);
 267         crsfFrameGps(dst);
 268         crsfFinalize(dst);
 269     }
 270 #endif
 271     crsfScheduleIndex = (crsfScheduleIndex + 1) % crsfScheduleCount;
 272 }
 273
 274 void initCrsfTelemetry(void)
 275 {
 276     // check if there is a serial port open for CRSF telemetry (ie opened by the CRSF RX)
 277     // and feature is enabled, if so, set CRSF telemetry enabled
 278     crsfTelemetryEnabled = crsfRxIsActive();
 279     int index = 0;
 280     crsfSchedule[index++] = BV(CRSF_FRAME_ATTITUDE);
 281     crsfSchedule[index++] = BV(CRSF_FRAME_BATTERY_SENSOR);
 282     crsfSchedule[index++] = BV(CRSF_FRAME_FLIGHT_MODE);
 283     if (feature(FEATURE_GPS)) {
 284         crsfSchedule[index++] = BV(CRSF_FRAME_GPS);
 285     }
 286     crsfScheduleCount = (uint8_t)index;
 287
 288  }
 289
 290 bool checkCrsfTelemetryState(void)
 291 {
 292     return crsfTelemetryEnabled;
 293 }
 294
 295 /*
 296  * Called periodically by the scheduler
 297  */
 298 void handleCrsfTelemetry(timeUs_t currentTimeUs)
 299 {
 300     static uint32_t crsfLastCycleTime;
 301
 302     if (!crsfTelemetryEnabled) {
 303         return;
 304     }
 305     // Give the receiver a chance to send any outstanding telemetry data.
 306     // This needs to be done at high frequency, to enable the RX to send the telemetry frame
 307     // in between the RX frames.
 308     crsfRxSendTelemetryData();
 309
 310     // Actual telemetry data only needs to be sent at a low frequency, ie 10Hz
 311     if (currentTimeUs >= crsfLastCycleTime + CRSF_CYCLETIME_US) {
 312         crsfLastCycleTime = currentTimeUs;
 313         processCrsf();
 314     }
 315 }
 316
 317 int getCrsfFrame(uint8_t *frame, crsfFrameType_e frameType)
 318 {
 319     sbuf_t crsfFrameBuf;
 320     sbuf_t *sbuf = &crsfFrameBuf;
 321
 322     crsfInitializeFrame(sbuf);
 323     switch (frameType) {
 324     default:
 325     case CRSF_FRAME_ATTITUDE:
 326         crsfFrameAttitude(sbuf);
 327         break;
 328     case CRSF_FRAME_BATTERY_SENSOR:
 329         crsfFrameBatterySensor(sbuf);
 330         break;
 331     case CRSF_FRAME_FLIGHT_MODE:
 332         crsfFrameFlightMode(sbuf);
 333         break;
 334 #if defined(GPS)
 335     case CRSF_FRAME_GPS:
 336         crsfFrameGps(sbuf);
 337         break;
 338 #endif
 339     }
 340     const int frameSize = crsfFinalizeBuf(sbuf, frame);
 341     return frameSize;
 342 }
 343 #endif