src/main/telemetry/sim.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 "platform.h"
  19
  20 #if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_SIM)
  21
  22 #include <string.h>
  23
  24 #include "common/printf.h"
  25 #include "common/olc.h"
  26
  27 #include "drivers/time.h"
  28
  29 #include "fc/fc_core.h"
  30 #include "fc/runtime_config.h"
  31
  32 #include "flight/imu.h"
  33
  34 #include "io/gps.h"
  35 #include "io/serial.h"
  36
  37 #include "navigation/navigation.h"
  38
  39 #include "sensors/battery.h"
  40 #include "sensors/sensors.h"
  41
  42 #include "common/string_light.h"
  43 #include "common/typeconversion.h"
  44
  45 #include "telemetry/sim.h"
  46 #include "telemetry/telemetry.h"
  47
  48 #define SIM_AT_COMMAND_MAX_SIZE 255
  49 #define SIM_RESPONSE_BUFFER_SIZE 255
  50 #define SIM_CYCLE_MS 5000                       // wait between sim command cycles
  51 #define SIM_AT_COMMAND_DELAY_MS 3000
  52 #define SIM_AT_COMMAND_DELAY_MIN_MS 500
  53 #define SIM_STARTUP_DELAY_MS 10000
  54 #define SIM_SMS_COMMAND_RTH "RTH"
  55 #define SIM_LOW_ALT_WARNING_MODES (NAV_ALTHOLD_MODE | NAV_RTH_MODE | NAV_WP_MODE | FAILSAFE_MODE)
  56
  57 #define SIM_RESPONSE_CODE_OK    ('O' << 24 | 'K' << 16)
  58 #define SIM_RESPONSE_CODE_ERROR ('E' << 24 | 'R' << 16 | 'R' << 8 | 'O')
  59 #define SIM_RESPONSE_CODE_RING  ('R' << 24 | 'I' << 16 | 'N' << 8 | 'G')
  60 #define SIM_RESPONSE_CODE_CLIP  ('C' << 24 | 'L' << 16 | 'I' << 8 | 'P')
  61 #define SIM_RESPONSE_CODE_CREG  ('C' << 24 | 'R' << 16 | 'E' << 8 | 'G')
  62 #define SIM_RESPONSE_CODE_CSQ   ('C' << 24 | 'S' << 16 | 'Q' << 8 | ':')
  63 #define SIM_RESPONSE_CODE_CMT   ('C' << 24 | 'M' << 16 | 'T' << 8 | ':')
  64
  65
  66 typedef enum  {
  67     SIM_MODULE_NOT_DETECTED = 0,
  68     SIM_MODULE_NOT_REGISTERED,
  69     SIM_MODULE_REGISTERED,
  70 } simModuleState_e;
  71
  72 typedef enum  {
  73     SIM_STATE_INIT = 0,
  74     SIM_STATE_INIT2,
  75     SIM_STATE_INIT_ENTER_PIN,
  76     SIM_STATE_SET_MODES,
  77     SIM_STATE_SEND_SMS,
  78     SIM_STATE_SEND_SMS_ENTER_MESSAGE
  79 } simTelemetryState_e;
  80
  81 typedef enum  {
  82     SIM_AT_OK = 0,
  83     SIM_AT_ERROR,
  84     SIM_AT_WAITING_FOR_RESPONSE
  85 } simATCommandState_e;
  86
  87 typedef enum  {
  88     SIM_READSTATE_RESPONSE = 0,
  89     SIM_READSTATE_SMS,
  90     SIM_READSTATE_SKIP
  91 } simReadState_e;
  92
  93 typedef enum  {
  94     SIM_TX_NO = 0,
  95     SIM_TX_FS,
  96     SIM_TX
  97 } simTransmissionState_e;
  98
  99 typedef enum {
 100     ACC_EVENT_NONE = 0,
 101     ACC_EVENT_HIGH,
 102     ACC_EVENT_LOW,
 103     ACC_EVENT_NEG_X
 104 } accEvent_t;
 105
 106
 107 static serialPort_t *simPort;
 108 static serialPortConfig_t *portConfig;
 109 static bool simEnabled = false;
 110
 111 static uint8_t atCommand[SIM_AT_COMMAND_MAX_SIZE];
 112 static int simTelemetryState = SIM_STATE_INIT;
 113 static timeMs_t sim_t_stateChange = 0;
 114 static uint8_t simResponse[SIM_RESPONSE_BUFFER_SIZE + 1];
 115 static int atCommandStatus = SIM_AT_OK;
 116 static bool simWaitAfterResponse = false;
 117 static uint8_t readState = SIM_READSTATE_RESPONSE;
 118 static timeMs_t t_lastMessageSent = 0;
 119 static uint8_t lastMessageTriggeredBy = 0;
 120 static uint8_t simModuleState = SIM_MODULE_NOT_DETECTED;
 121
 122 static int simRssi;
 123 static uint8_t accEvent = ACC_EVENT_NONE;
 124 static char* accEventDescriptions[] = { "", "HIT! ", "DROP ", "HIT " };
 125 static char* modeDescriptions[] = { "MAN", "ACR", "AIR", "ANG", "HOR", "ALH", "POS", "RTH", "WP", "CRS", "LAU", "FS", "ANH" };
 126 static const char gpsFixIndicators[] = { '!', '*', ' ' };
 127
 128 static bool checkGroundStationNumber(uint8_t* rv)
 129 {
 130     int i;
 131     const char* gsn = telemetryConfig()->simGroundStationNumber;
 132
 133     int digitsToCheck = strlen((char*)gsn);
 134     if (gsn[0] == '+') {
 135         digitsToCheck -= 5;        // ignore country code (max 4 digits)
 136     } else if (gsn[0] == '0') { // ignore trunk prefixes: '0', '8', 01', '02', '06'
 137         digitsToCheck--;
 138         if (gsn[1] == '1' || gsn[1] == '2' || gsn[1] == '6') {
 139             digitsToCheck--;
 140         }
 141     } else if (gsn[0] == '8') {
 142         digitsToCheck--;
 143     }
 144
 145     for (i = 0; i < 16 && *gsn != '\0'; i++) gsn++;
 146     if (i == 0)
 147         return false;
 148     for (i = 0; i < 16 && *rv != '\"'; i++) rv++;
 149
 150     gsn--; rv--;
 151     for (i = 0; i < digitsToCheck; i++) {
 152         if (*rv != *gsn) return false;
 153         gsn--; rv--;
 154     }
 155     return true;
 156 }
 157
 158
 159 static void readOriginatingNumber(uint8_t* rv)
 160 {
 161     int i;
 162     char* gsn = telemetryConfigMutable()->simGroundStationNumber;
 163     if (gsn[0] != '\0')
 164         return;
 165     for (i = 0; i < 15 && rv[i] != '\"'; i++)
 166          gsn[i] = rv[i];
 167     gsn[i] = '\0';
 168 }
 169
 170 static void readTransmitFlags(const uint8_t* fs)
 171 {
 172     int i;
 173
 174     uint8_t transmitFlags = 0;
 175     for (i = 0; i < SIM_N_TX_FLAGS && fs[i] != '\0'; i++) {
 176         switch (fs[i]) {
 177             case 'T': case 't':
 178             transmitFlags |= SIM_TX_FLAG;
 179             break;
 180             case 'F': case 'f':
 181             transmitFlags |= SIM_TX_FLAG_FAILSAFE;
 182             break;
 183             case 'G': case 'g':
 184             transmitFlags |= SIM_TX_FLAG_GPS;
 185             break;
 186             case 'L': case 'l':
 187             transmitFlags |= SIM_TX_FLAG_LOW_ALT;
 188             break;
 189             case 'A': case 'a':
 190             transmitFlags |= SIM_TX_FLAG_ACC;
 191             break;
 192         }
 193     }
 194
 195     telemetryConfigMutable()->simTransmitFlags = transmitFlags;
 196 }
 197
 198 static void requestSendSMS(uint8_t trigger)
 199 {
 200     lastMessageTriggeredBy = trigger;
 201     if (simTelemetryState == SIM_STATE_SEND_SMS_ENTER_MESSAGE)
 202         return; // sending right now, don't reissue AT command
 203     simTelemetryState = SIM_STATE_SEND_SMS;
 204     if (atCommandStatus != SIM_AT_WAITING_FOR_RESPONSE)
 205         sim_t_stateChange = 0; // send immediately
 206 }
 207
 208 static void readSMS(void)
 209 {
 210     if (sl_strcasecmp((char*)simResponse, SIM_SMS_COMMAND_RTH) == 0) {
 211         if (getStateOfForcedRTH() == RTH_IDLE) {
 212             activateForcedRTH();
 213         } else {
 214             abortForcedRTH();
 215         }
 216     } else {
 217         readTransmitFlags(simResponse);
 218     }
 219     requestSendSMS(SIM_TX_FLAG_RESPONSE);
 220 }
 221
 222 static void readSimResponse(void)
 223 {
 224     if (readState == SIM_READSTATE_SKIP) {
 225         readState = SIM_READSTATE_RESPONSE;
 226         return;
 227     } else if (readState == SIM_READSTATE_SMS) {
 228         readSMS();
 229         readState = SIM_READSTATE_RESPONSE;
 230         return;
 231     }
 232
 233     uint8_t* resp = simResponse;
 234     uint32_t responseCode = 0;
 235     if (simResponse[0] == '+') {
 236         resp++;
 237     }
 238     responseCode = *resp++;
 239     responseCode <<= 8; responseCode |= *resp++;
 240     responseCode <<= 8; responseCode |= *resp++;
 241     responseCode <<= 8; responseCode |= *resp++;
 242
 243     if (responseCode == SIM_RESPONSE_CODE_OK) {
 244         // OK
 245         atCommandStatus = SIM_AT_OK;
 246         if (!simWaitAfterResponse) {
 247             sim_t_stateChange = millis() + SIM_AT_COMMAND_DELAY_MIN_MS;
 248         }
 249         return;
 250     } else if (responseCode == SIM_RESPONSE_CODE_ERROR) {
 251         // ERROR
 252         atCommandStatus = SIM_AT_ERROR;
 253         if (!simWaitAfterResponse) {
 254             sim_t_stateChange = millis() + SIM_AT_COMMAND_DELAY_MIN_MS;
 255         }
 256         return;
 257     } else if (responseCode == SIM_RESPONSE_CODE_RING) {
 258         // RING
 259     } else if (responseCode == SIM_RESPONSE_CODE_CSQ) {
 260         // +CSQ: 26,0
 261         simRssi = fastA2I((char*)&simResponse[6]);
 262     } else if (responseCode == SIM_RESPONSE_CODE_CLIP) {
 263         // we always get this after a RING when a call is incoming
 264         // +CLIP: "+3581234567"
 265         readOriginatingNumber(&simResponse[8]);
 266         if (checkGroundStationNumber(&simResponse[8])) {
 267             requestSendSMS(SIM_TX_FLAG_RESPONSE);
 268         }
 269     } else if (responseCode == SIM_RESPONSE_CODE_CREG) {
 270         // +CREG: 0,1
 271         if (simResponse[9] == '1' || simResponse[9] == '5') {
 272             simModuleState = SIM_MODULE_REGISTERED;
 273         } else {
 274             simModuleState = SIM_MODULE_NOT_REGISTERED;
 275         }
 276     } else if (responseCode == SIM_RESPONSE_CODE_CMT) {
 277         // +CMT: <oa>,[<alpha>],<scts>[,<tooa>,<fo>,<pid>,<dcs>,<sca>,<tosca>,<length>]<CR><LF><data>
 278         // +CMT: "+3581234567","","19/02/12,14:57:24+08"
 279         readOriginatingNumber(&simResponse[7]);
 280         if (checkGroundStationNumber(&simResponse[7])) {
 281             readState = SIM_READSTATE_SMS; // next simResponse line will be SMS content
 282         } else {
 283             readState = SIM_READSTATE_SKIP; // skip SMS content
 284         }
 285     }
 286 }
 287
 288 static int16_t getAltitudeMeters(void)
 289 {
 290     return getEstimatedActualPosition(Z) / 100;
 291 }
 292
 293 static void transmit(void)
 294 {
 295     timeMs_t timeSinceMsg = millis() - t_lastMessageSent;
 296     uint8_t triggers = SIM_TX_FLAG;
 297     uint32_t accSq = sq(imuMeasuredAccelBF.x) + sq(imuMeasuredAccelBF.y) + sq(imuMeasuredAccelBF.z);
 298
 299     if (telemetryConfig()->accEventThresholdHigh > 0 && accSq > sq(telemetryConfig()->accEventThresholdHigh)) {
 300         triggers |= SIM_TX_FLAG_ACC;
 301         accEvent = ACC_EVENT_HIGH;
 302     } else if (accSq < sq(telemetryConfig()->accEventThresholdLow)) {
 303         triggers |= SIM_TX_FLAG_ACC;
 304         accEvent = ACC_EVENT_LOW;
 305     } else if (telemetryConfig()->accEventThresholdNegX > 0 && imuMeasuredAccelBF.x < -telemetryConfig()->accEventThresholdNegX) {
 306         triggers |= SIM_TX_FLAG_ACC;
 307         accEvent = ACC_EVENT_NEG_X;
 308     }
 309
 310     if ((lastMessageTriggeredBy & SIM_TX_FLAG_ACC) && timeSinceMsg < 2000)
 311         accEvent = ACC_EVENT_NONE;
 312
 313     if (FLIGHT_MODE(FAILSAFE_MODE))
 314         triggers |= SIM_TX_FLAG_FAILSAFE;
 315     if (!navigationPositionEstimateIsHealthy())
 316         triggers |= SIM_TX_FLAG_GPS;
 317     if (gpsSol.fixType != GPS_NO_FIX && FLIGHT_MODE(SIM_LOW_ALT_WARNING_MODES) && getAltitudeMeters() < telemetryConfig()->simLowAltitude)
 318         triggers |= SIM_TX_FLAG_LOW_ALT;
 319
 320     triggers &= telemetryConfig()->simTransmitFlags;
 321
 322     if (!triggers)
 323         return;
 324     if (!ARMING_FLAG(WAS_EVER_ARMED))
 325         return;
 326
 327     if ((triggers & ~lastMessageTriggeredBy) // if new trigger activated after last msg, don't wait
 328         || timeSinceMsg > 1000 * MAX(SIM_MIN_TRANSMIT_INTERVAL, telemetryConfig()->simTransmitInterval)) {
 329         requestSendSMS(triggers);
 330     }
 331 }
 332
 333 static void sendATCommand(const char* command)
 334 {
 335     atCommandStatus = SIM_AT_WAITING_FOR_RESPONSE;
 336     uint8_t len = MIN((uint8_t)strlen(command), SIM_AT_COMMAND_MAX_SIZE);
 337     serialWriteBuf(simPort, (const uint8_t*) command, len);
 338 }
 339
 340 static void sendSMS(void)
 341 {
 342     char pluscode_url[20];
 343     int16_t groundSpeed = 0;
 344     uint16_t vbat = getBatteryVoltage();
 345     int16_t amps = isAmperageConfigured() ? getAmperage() / 10 : 0; // 1 = 100 milliamps
 346     uint16_t avgSpeed = lrintf(10 * calculateAverageSpeed());
 347     uint32_t now = millis();
 348
 349     ZERO_FARRAY(pluscode_url);
 350
 351     if ((sensors(SENSOR_GPS) && STATE(GPS_FIX))
 352 #ifdef USE_GPS_FIX_ESTIMATION
 353             || STATE(GPS_ESTIMATED_FIX)
 354 #endif
 355         ) {
 356         groundSpeed = gpsSol.groundSpeed / 100;
 357
 358         char buf[20];
 359         olc_encode(gpsSol.llh.lat, gpsSol.llh.lon, 11, buf, sizeof(buf));
 360
 361         // URLencode plus code (replace plus sign with %2B)
 362         for (char *in = buf, *out = pluscode_url; *in; ) {
 363             if (*in == '+') {
 364                 in++;
 365                 *out++ = '%';
 366                 *out++ = '2';
 367                 *out++ = 'B';
 368             }
 369             else {
 370                 *out++ = *in++;
 371             }
 372         }
 373     }
 374
 375     // \x1a sends msg, \x1b cancels
 376     uint8_t len = tfp_sprintf((char*)atCommand, "%s%d.%02dV %d.%dA ALT:%d SPD:%d/%d.%d DIS:%lu/%lu HDG:%d SAT:%d%c SIG:%d %s https://maps.google.com/?q=%s\x1a",
 377         accEventDescriptions[accEvent],
 378         vbat / 100, vbat % 100,
 379         amps / 10, amps % 10,
 380         getAltitudeMeters(),
 381         groundSpeed, avgSpeed / 10, avgSpeed % 10,
 382         (unsigned long)GPS_distanceToHome, getTotalTravelDistance() / 100ul,
 383         (int)DECIDEGREES_TO_DEGREES(attitude.values.yaw),
 384         gpsSol.numSat, gpsFixIndicators[gpsSol.fixType],
 385         simRssi,
 386         getStateOfForcedRTH() == RTH_IDLE ? modeDescriptions[getFlightModeForTelemetry()] : "RTH",
 387         pluscode_url);
 388
 389     serialWriteBuf(simPort, atCommand, len);
 390     t_lastMessageSent = now;
 391     accEvent = ACC_EVENT_NONE;
 392     atCommandStatus = SIM_AT_WAITING_FOR_RESPONSE;
 393 }
 394
 395 void handleSimTelemetry(void)
 396 {
 397     static uint16_t simResponseIndex = 0;
 398     uint32_t now = millis();
 399
 400     if (!simEnabled)
 401         return;
 402     if (!simPort)
 403         return;
 404
 405     while (serialRxBytesWaiting(simPort) > 0) {
 406         uint8_t c = serialRead(simPort);
 407         if (c == '\n' || simResponseIndex == SIM_RESPONSE_BUFFER_SIZE) {
 408             simResponse[simResponseIndex] = '\0';
 409             if (simResponseIndex > 0) simResponseIndex--;
 410             if (simResponse[simResponseIndex] == '\r') simResponse[simResponseIndex] = '\0';
 411             simResponseIndex = 0; //data ok
 412             readSimResponse();
 413             break;
 414         } else {
 415             simResponse[simResponseIndex] = c;
 416             simResponseIndex++;
 417         }
 418     }
 419
 420     transmit();
 421
 422     if (now < sim_t_stateChange)
 423         return;
 424
 425     sim_t_stateChange = now + SIM_AT_COMMAND_DELAY_MS;  // by default, if OK or ERROR not received, wait this long
 426     simWaitAfterResponse = false;                       // by default, if OK or ERROR received, go to next state immediately.
 427     switch (simTelemetryState) {
 428         case SIM_STATE_INIT:
 429         sendATCommand("AT\r");
 430         simTelemetryState = SIM_STATE_INIT2;
 431         break;
 432         case SIM_STATE_INIT2:
 433         sendATCommand("ATE0\r");
 434         simTelemetryState = SIM_STATE_INIT_ENTER_PIN;
 435         break;
 436         case SIM_STATE_INIT_ENTER_PIN:
 437         sendATCommand("AT+CPIN=");
 438         sendATCommand((char*)telemetryConfig()->simPin);
 439         sendATCommand("\r");
 440         simTelemetryState = SIM_STATE_SET_MODES;
 441         break;
 442         case SIM_STATE_SET_MODES:
 443         sendATCommand("AT+CMGF=1;+CNMI=3,2;+CLIP=1;+CSQ\r");
 444         simTelemetryState = SIM_STATE_INIT;
 445         sim_t_stateChange = now + SIM_CYCLE_MS;
 446         simWaitAfterResponse = true;
 447         break;
 448         case SIM_STATE_SEND_SMS:
 449         sendATCommand("AT+CMGS=\"");
 450         sendATCommand((char*)telemetryConfig()->simGroundStationNumber);
 451         sendATCommand("\"\r");
 452         simTelemetryState = SIM_STATE_SEND_SMS_ENTER_MESSAGE;
 453         sim_t_stateChange = now + 100;
 454         break;
 455         case SIM_STATE_SEND_SMS_ENTER_MESSAGE:
 456         sendSMS();
 457         simTelemetryState = SIM_STATE_INIT;
 458         sim_t_stateChange = now + SIM_CYCLE_MS;
 459         simWaitAfterResponse = true;
 460         break;
 461     }
 462 }
 463
 464 void initSimTelemetry(void)
 465 {
 466     portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_SIM);
 467 }
 468
 469 static void configureSimTelemetryPort(void)
 470 {
 471     if (!portConfig) {
 472         return;
 473     }
 474     baudRate_e baudRateIndex = portConfig->telemetry_baudrateIndex;
 475     simPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_SIM, NULL, NULL,
 476         baudRates[baudRateIndex], MODE_RXTX, SERIAL_NOT_INVERTED);
 477
 478     if (!simPort) {
 479         return;
 480     }
 481
 482     sim_t_stateChange = millis() + SIM_STARTUP_DELAY_MS;
 483     simTelemetryState = SIM_STATE_INIT;
 484     readState = SIM_READSTATE_RESPONSE;
 485     simEnabled = true;
 486 }
 487
 488 void checkSimTelemetryState(void)
 489 {
 490     if (simEnabled) {
 491         return;
 492     }
 493     configureSimTelemetryPort();
 494 }
 495
 496 #endif