Add GSM telemetry (#4388)

docs/Cli.md

@@ -479,3 +479,8 @@ A shorter form is also supported to enable and disable functions using `serial <
 | mc_airmode_type | STICK_CENTER | Defines the Airmode state handling type for Multirotors. Default **STICK_CENTER** is the classical approach in which Airmode is always active if enabled, but when the throttle is low and ROLL/PITCH/YAW sticks are centered, Iterms is not allowed to grow (ANTI_WINDUP). **THROTTLE_THRESHOLD** is the Airmode behavior known from Betaflight. In this mode, Airmode is active as soon THROTTLE position is above `mc_airmode_threshold` and stays active until disarm. ANTI_WINDUP is never triggered. For small Multirotors (up to 7-inch propellers) it is suggested to switch to **THROTTLE_THRESHOLD** since it keeps full stabilization no matter what pilot does with the sticks. Fixed Wings always use **STICK_CENTER** mode. |
 | mc_airmode_threshold | 1300 | Defines airmode THROTTLE activation threshold when `mc_airmode_type` **THROTTLE_THRESHOLD** is used |
 | use_dterm_fir_filter | ON | Setting to **OFF** disabled extra filter on Dterm. **OFF** offers faster Dterm and better inflight performance with a cost of being more sensitive to gyro noise. Small and relatively clean multirotors (7 inches and below) are suggested to use **OFF** setting. If motors are getting too hot, switch back to **ON** |
+| sim_ground_station_number | Empty string | Number of phone that is used to communicate with SIM module. Messages / calls from other numbers are ignored. If undefined, can be set by calling or sending a message to the module. |
+| sim_transmission_interval | -60 | Text message transmission interval in seconds for SIM module. Negative value means transmission is off but can be switched on by sending a "T" message. |
+| acc_event_threshold_high | 0 | Acceleration threshold [cm/s/s] for impact detection text messages sent by SIM module. 0 = detection off. |
+| acc_event_threshold_low | 0 | Acceleration threshold [cm/s/s] for low-g / freefall detection text messages sent by SIM module. Valid values: [0-900] |
+| acc_event_threshold_neg_x | 0 | Acceleration threshold [cm/s/s] for backwards acceleration / fixed wing landing detection text messages sent by SIM module. 0 = detection off. |

make/source.mk

@@ -199,6 +199,7 @@ COMMON_SRC = \
             telemetry/mavlink.c \
             telemetry/msp_shared.c \
             telemetry/smartport.c \
+            telemetry/sim.c \
             telemetry/telemetry.c \
             io/vtx.c \
             io/vtx_string.c \

src/main/fc/settings.yaml

@@ -1529,7 +1529,7 @@ groups:
 
   - name: PG_TELEMETRY_CONFIG
     type: telemetryConfig_t
-    headers: ["io/serial.h", "telemetry/telemetry.h", "telemetry/frsky_d.h"]
+    headers: ["io/serial.h", "telemetry/telemetry.h", "telemetry/frsky_d.h", "telemetry/sim.h"]
     condition: USE_TELEMETRY
     members:
       - name: telemetry_switch
@@ -1580,7 +1580,46 @@ groups:
         field: ltmUpdateRate
         condition: USE_TELEMETRY_LTM
         table: ltm_rates
-
+      - name: sim_ground_station_number
+        field: simGroundStationNumber
+        condition: USE_TELEMETRY_SIM
+        type: string
+        max: 16
+      - name: sim_transmit_interval
+        field: simTransmitInterval
+        condition: USE_TELEMETRY_SIM
+        type: uint16_t
+        min: SIM_MIN_TRANSMIT_INTERVAL
+        max: 65535
+      - name: sim_transmit_flags
+        field: simTransmitFlags
+        condition: USE_TELEMETRY_SIM
+        type: string
+        max: SIM_N_TX_FLAGS
+      - name: acc_event_threshold_high
+        field: accEventThresholdHigh
+        condition: USE_TELEMETRY_SIM
+        type: uint16_t
+        min: 0
+        max: 65535
+      - name: acc_event_threshold_low
+        field: accEventThresholdLow
+        condition: USE_TELEMETRY_SIM
+        type: uint16_t
+        min: 0
+        max: 900
+      - name: acc_event_threshold_neg_x
+        field: accEventThresholdNegX
+        condition: USE_TELEMETRY_SIM
+        type: uint16_t
+        min: 0
+        max: 65535
+      - name: sim_low_altitude
+        field: simLowAltitude
+        condition: USE_TELEMETRY_SIM
+        type: int16_t
+        min: INT16_MIN
+        max: INT16_MAX
   - name: PG_ELERES_CONFIG
     type: eleresConfig_t
     headers: ["rx/eleres.h"]

src/main/io/serial.h

@@ -50,6 +50,7 @@ typedef enum {
     FUNCTION_RANGEFINDER         = (1 << 16), // 65536
     FUNCTION_VTX_FFPV            = (1 << 17), // 131072
     FUNCTION_SERIALSHOT          = (1 << 18), // 262144
+    FUNCTION_TELEMETRY_SIM       = (1 << 19)  // 524288
 } serialPortFunction_e;
 
 typedef enum {

src/main/target/common.h

@@ -98,6 +98,8 @@
 
 #define USE_PWM_DRIVER_PCA9685
 
+#define USE_TELEMETRY_SIM
+
 #define NAV_NON_VOLATILE_WAYPOINT_CLI
 
 #define NAV_AUTO_MAG_DECLINATION_PRECISE

src/main/telemetry/sim.c

@@ -0,0 +1,435 @@
+#include "platform.h"
+
+#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_SIM)
+
+#include <string.h>
+
+#include "common/axis.h"
+#include "common/streambuf.h"
+#include "common/utils.h"
+#include "common/printf.h"
+
+#include "drivers/time.h"
+
+#include "fc/config.h"
+#include "fc/fc_core.h"
+#include "fc/rc_controls.h"
+#include "fc/runtime_config.h"
+#include "fc/fc_init.h"
+
+#include "flight/imu.h"
+#include "flight/failsafe.h"
+#include "flight/pid.h"
+
+#include "io/gps.h"
+#include "io/serial.h"
+
+#include "navigation/navigation.h"
+#include "navigation/navigation_private.h"
+
+#include "sensors/acceleration.h"
+#include "sensors/battery.h"
+#include "sensors/boardalignment.h"
+#include "sensors/diagnostics.h"
+#include "sensors/gyro.h"
+#include "sensors/sensors.h"
+
+#include "common/string_light.h"
+#include "common/typeconversion.h"
+#include "build/debug.h"
+
+#include "telemetry/sim.h"
+#include "telemetry/telemetry.h"
+
+static serialPort_t *simPort;
+static serialPortConfig_t *portConfig;
+static bool simEnabled = false;
+
+static uint8_t atCommand[SIM_AT_COMMAND_MAX_SIZE];
+static int simTelemetryState = SIM_STATE_INIT;
+static timeMs_t sim_t_stateChange = 0;
+static uint8_t simResponse[SIM_RESPONSE_BUFFER_SIZE + 1];
+static int atCommandStatus = SIM_AT_OK;
+static bool simWaitAfterResponse = false;
+static uint8_t readState = SIM_READSTATE_RESPONSE;
+static uint8_t transmitFlags = 0;
+static timeMs_t t_lastMessageSent = 0;
+static uint8_t lastMessageTriggeredBy = 0;
+uint8_t simModuleState = SIM_MODULE_NOT_DETECTED;
+
+int simRssi;
+uint8_t accEvent = ACC_EVENT_NONE;
+char* accEventDescriptions[] = { "", "HIT! ", "DROP ", "HIT " };
+char* modeDescriptions[] = { "MAN", "ACR", "ANG", "HOR", "ALH", "POS", "RTH", "WP", "LAU", "FS" };
+const char gpsFixIndicators[] = { '!', '*', ' ' };
+
+
+bool isGroundStationNumberDefined() {
+    return telemetryConfig()->simGroundStationNumber[0] != '\0';
+}
+
+bool checkGroundStationNumber(uint8_t* rv)
+{
+    int i;
+    const uint8_t* gsn = telemetryConfig()->simGroundStationNumber;
+
+    int digitsToCheck = strlen((char*)gsn);
+    if (gsn[0] == '+') {
+        digitsToCheck -= 5;        // ignore country code (max 4 digits)
+    } else if (gsn[0] == '0') { // ignore trunk prefixes: '0', '8', 01', '02', '06'
+        digitsToCheck--;
+        if (gsn[1] == '1' || gsn[1] == '2' || gsn[1] == '6') {
+            digitsToCheck--;
+        }
+    } else if (gsn[0] == '8') {
+        digitsToCheck--;
+    }
+
+    for (i = 0; i < 16 && *gsn != '\0'; i++) gsn++;
+    if (i == 0)
+        return false;
+    for (i = 0; i < 16 && *rv != '\"'; i++) rv++;
+
+    gsn--; rv--;
+    for (i = 0; i < digitsToCheck; i++) {
+        if (*rv != *gsn) return false;
+        gsn--; rv--;
+    }
+    return true;
+}
+
+
+void readOriginatingNumber(uint8_t* rv)
+{
+    int i;
+    uint8_t* gsn = telemetryConfigMutable()->simGroundStationNumber;
+    if (gsn[0] != '\0')
+        return;
+    for (i = 0; i < 15 && rv[i] != '\"'; i++)
+         gsn[i] = rv[i];
+    gsn[i] = '\0';
+}
+
+void readTransmitFlags(const uint8_t* fs)
+{
+    int i;
+
+    transmitFlags = 0;
+    for (i = 0; i < SIM_N_TX_FLAGS && fs[i] != '\0'; i++) {
+        switch (fs[i]) {
+            case 'T': case 't':
+            transmitFlags |= SIM_TX_FLAG;
+            break;
+            case 'F': case 'f':
+            transmitFlags |= SIM_TX_FLAG_FAILSAFE;
+            break;
+            case 'G': case 'g':
+            transmitFlags |= SIM_TX_FLAG_GPS;
+            break;
+            case 'L': case 'l':
+            transmitFlags |= SIM_TX_FLAG_LOW_ALT;
+            break;
+            case 'A': case 'a':
+            transmitFlags |= SIM_TX_FLAG_ACC;
+            break;
+        }
+    }
+}
+
+void requestSendSMS(uint8_t trigger)
+{
+    lastMessageTriggeredBy = trigger;
+    if (simTelemetryState == SIM_STATE_SEND_SMS_ENTER_MESSAGE)
+        return; // sending right now, don't reissue AT command
+    simTelemetryState = SIM_STATE_SEND_SMS;
+    if (atCommandStatus != SIM_AT_WAITING_FOR_RESPONSE)
+        sim_t_stateChange = 0; // send immediately
+}
+
+void readSMS()
+{
+    if (sl_strcasecmp((char*)simResponse, SIM_SMS_COMMAND_RTH) == 0) {
+        if (!posControl.flags.forcedRTHActivated) {
+            activateForcedRTH();
+        } else {
+            abortForcedRTH();
+        }
+    } else {
+        readTransmitFlags(simResponse);
+    }
+    requestSendSMS(SIM_TX_FLAG_RESPONSE);
+}
+
+void readSimResponse()
+{
+    if (readState == SIM_READSTATE_SKIP) {
+        readState = SIM_READSTATE_RESPONSE;
+        return;
+    } else if (readState == SIM_READSTATE_SMS) {
+        readSMS();
+        readState = SIM_READSTATE_RESPONSE;
+        return;
+    }
+
+    uint8_t* resp = simResponse;
+    uint32_t responseCode = 0;
+    if (simResponse[0] == '+') {
+        resp++;
+    }
+    responseCode = *resp++;
+    responseCode <<= 8; responseCode |= *resp++;
+    responseCode <<= 8; responseCode |= *resp++;
+    responseCode <<= 8; responseCode |= *resp++;
+
+    if (responseCode == SIM_RESPONSE_CODE_OK) {
+        // OK
+        atCommandStatus = SIM_AT_OK;
+        if (!simWaitAfterResponse) {
+            sim_t_stateChange = millis() + SIM_AT_COMMAND_DELAY_MIN_MS;
+        }
+        return;
+    } else if (responseCode == SIM_RESPONSE_CODE_ERROR) {
+        // ERROR
+        atCommandStatus = SIM_AT_ERROR;
+        if (!simWaitAfterResponse) {
+            sim_t_stateChange = millis() + SIM_AT_COMMAND_DELAY_MIN_MS;
+        }
+        return;
+    } else if (responseCode == SIM_RESPONSE_CODE_RING) {
+        // RING
+    } else if (responseCode == SIM_RESPONSE_CODE_CSQ) {
+        // +CSQ: 26,0
+        simRssi = fastA2I((char*)&simResponse[6]);
+    } else if (responseCode == SIM_RESPONSE_CODE_CLIP) {
+        // we always get this after a RING when a call is incoming
+        // +CLIP: "+3581234567"
+        readOriginatingNumber(&simResponse[8]);
+        if (checkGroundStationNumber(&simResponse[8])) {
+            requestSendSMS(SIM_TX_FLAG_RESPONSE);
+        }
+    } else if (responseCode == SIM_RESPONSE_CODE_CREG) {
+        // +CREG: 0,1
+        if (simResponse[9] == '1' || simResponse[9] == '5') {
+            simModuleState = SIM_MODULE_REGISTERED;
+        } else {
+            simModuleState = SIM_MODULE_NOT_REGISTERED;
+        }        
+    } else if (responseCode == SIM_RESPONSE_CODE_CMT) {
+        // +CMT: <oa>,[<alpha>],<scts>[,<tooa>,<fo>,<pid>,<dcs>,<sca>,<tosca>,<length>]<CR><LF><data>
+        // +CMT: "+3581234567","","19/02/12,14:57:24+08"
+        readOriginatingNumber(&simResponse[7]);
+        if (checkGroundStationNumber(&simResponse[7])) {
+            readState = SIM_READSTATE_SMS; // next simResponse line will be SMS content
+        } else {
+            readState = SIM_READSTATE_SKIP; // skip SMS content
+        }
+    }
+}
+
+int getAltMeters()
+{
+#if defined(USE_NAV)
+    return getEstimatedActualPosition(Z) / 100;
+#else
+    return sensors(SENSOR_GPS) ? gpsSol.llh.alt / 100 : 0;
+#endif
+}
+
+void transmit()
+{
+    timeMs_t timeSinceMsg = millis() - t_lastMessageSent;
+    uint8_t triggers = SIM_TX_FLAG;
+    uint32_t accSq = sq(imuMeasuredAccelBF.x) + sq(imuMeasuredAccelBF.y) + sq(imuMeasuredAccelBF.z);
+
+    if (telemetryConfig()->accEventThresholdHigh > 0 && accSq > sq(telemetryConfig()->accEventThresholdHigh)) {
+        triggers |= SIM_TX_FLAG_ACC;
+        accEvent = ACC_EVENT_HIGH;
+    } else if (accSq < sq(telemetryConfig()->accEventThresholdLow)) {
+        triggers |= SIM_TX_FLAG_ACC;
+        accEvent = ACC_EVENT_LOW;
+    } else if (telemetryConfig()->accEventThresholdNegX > 0 && imuMeasuredAccelBF.x < -telemetryConfig()->accEventThresholdNegX) {
+        triggers |= SIM_TX_FLAG_ACC;
+        accEvent = ACC_EVENT_NEG_X;
+    }
+
+    if ((lastMessageTriggeredBy & SIM_TX_FLAG_ACC) && timeSinceMsg < 2000)
+        accEvent = ACC_EVENT_NONE;
+
+    if (FLIGHT_MODE(FAILSAFE_MODE))
+        triggers |= SIM_TX_FLAG_FAILSAFE;
+    if (gpsSol.fixType != GPS_NO_FIX && posControl.flags.estPosStatus < EST_USABLE)
+        triggers |= SIM_TX_FLAG_GPS;
+    if (gpsSol.fixType != GPS_NO_FIX && FLIGHT_MODE(SIM_LOW_ALT_WARNING_MODES) && getAltMeters() < telemetryConfig()->simLowAltitude)
+        triggers |= SIM_TX_FLAG_LOW_ALT;
+
+    triggers &= transmitFlags;
+
+    if (!triggers)
+        return;
+    if (!ARMING_FLAG(WAS_EVER_ARMED))
+        return;
+
+    if ((triggers & ~lastMessageTriggeredBy) // if new trigger activated after last msg, don't wait
+        || timeSinceMsg > 1000 * MAX(SIM_MIN_TRANSMIT_INTERVAL, telemetryConfig()->simTransmitInterval)) {
+        requestSendSMS(triggers);
+    }
+}
+
+void sendATCommand(const char* command)
+{
+    atCommandStatus = SIM_AT_WAITING_FOR_RESPONSE;
+    int len = strlen((char*)command);
+    if (len > SIM_AT_COMMAND_MAX_SIZE)
+        len = SIM_AT_COMMAND_MAX_SIZE;
+    serialWriteBuf(simPort, (const uint8_t*) command, len);
+}
+
+void sendSMS(void)
+{
+    int32_t lat = 0, lon = 0;
+    int16_t gs = 0, gc = 0;
+    int vbat = getBatteryVoltage();
+    int16_t amps = isAmperageConfigured() ? getAmperage() / 10 : 0; // 1 = 100 milliamps
+    int avgSpeed = (int)round(10 * calculateAverageSpeed());
+    uint32_t now = millis();
+
+    if (getFlightTime() == 0.0F) {
+        avgSpeed = 0;
+    }
+
+    if (sensors(SENSOR_GPS)) {
+        lat = gpsSol.llh.lat;
+        lon = gpsSol.llh.lon;
+        gs = gpsSol.groundSpeed / 100;
+        gc = gpsSol.groundCourse / 10;
+    }
+
+    int len;
+    int32_t E7 = 10000000;
+    // \x1a sends msg, \x1b cancels
+    len = tfp_sprintf((char*)atCommand, "%s%d.%02dV %d.%dA ALT:%ld SPD:%ld/%d.%d DIS:%d/%d COG:%d SAT:%d%c SIG:%d %s maps.google.com/?q=@%ld.%07ld,%ld.%07ld\x1a",
+        accEventDescriptions[accEvent],
+        vbat / 100, vbat % 100,
+        amps / 10, amps % 10,
+        getAltMeters(),
+        gs, avgSpeed / 10, avgSpeed % 10,
+        GPS_distanceToHome, getTotalTravelDistance() / 100,
+        gc,
+        gpsSol.numSat, gpsFixIndicators[gpsSol.fixType],
+        simRssi,
+        posControl.flags.forcedRTHActivated ? "RTH" : modeDescriptions[getFlightModeForTelemetry()],
+        lat / E7, lat % E7, lon / E7, lon % E7);
+    serialWriteBuf(simPort, atCommand, len);
+    t_lastMessageSent = now;
+    accEvent = ACC_EVENT_NONE;
+    atCommandStatus = SIM_AT_WAITING_FOR_RESPONSE;
+}
+
+void handleSimTelemetry()
+{
+    static uint16_t simResponseIndex = 0;
+    uint32_t now = millis();
+
+    if (!simEnabled)
+        return;
+    if (!simPort)
+        return;
+
+    while (serialRxBytesWaiting(simPort) > 0) {
+        uint8_t c = serialRead(simPort);
+        if (c == '\n' || simResponseIndex == SIM_RESPONSE_BUFFER_SIZE) {
+            simResponse[simResponseIndex] = '\0';
+            if (simResponseIndex > 0) simResponseIndex--;
+            if (simResponse[simResponseIndex] == '\r') simResponse[simResponseIndex] = '\0';
+            simResponseIndex = 0; //data ok
+            readSimResponse();
+            break;
+        } else {
+            simResponse[simResponseIndex] = c;
+            simResponseIndex++;
+        }
+    }
+
+    transmit();
+
+    if (now < sim_t_stateChange)
+        return;
+
+    sim_t_stateChange = now + SIM_AT_COMMAND_DELAY_MS;       // by default, if OK or ERROR not received, wait this long
+    simWaitAfterResponse = false;   // by default, if OK or ERROR received, go to next state immediately.
+    switch (simTelemetryState) {
+        case SIM_STATE_INIT:
+        sendATCommand("AT\r");
+        simTelemetryState = SIM_STATE_INIT2;
+        break;
+        case SIM_STATE_INIT2:
+        sendATCommand("ATE0\r");
+        simTelemetryState = SIM_STATE_INIT_ENTER_PIN;
+        break;
+        case SIM_STATE_INIT_ENTER_PIN:
+        sendATCommand("AT+CPIN=" SIM_PIN "\r");
+        simTelemetryState = SIM_STATE_SET_MODES;
+        break;
+        case SIM_STATE_SET_MODES:
+        sendATCommand("AT+CMGF=1;+CNMI=3,2;+CLIP=1;+CSQ\r");
+        simTelemetryState = SIM_STATE_INIT;
+        sim_t_stateChange = now + SIM_CYCLE_MS;
+        simWaitAfterResponse = true;
+        break;
+        case SIM_STATE_SEND_SMS:
+        sendATCommand("AT+CMGS=\"");
+        sendATCommand((char*)telemetryConfig()->simGroundStationNumber);
+        sendATCommand("\"\r");
+        simTelemetryState = SIM_STATE_SEND_SMS_ENTER_MESSAGE;
+        sim_t_stateChange = now + 100;
+        break;
+        case SIM_STATE_SEND_SMS_ENTER_MESSAGE:
+        sendSMS();
+        simTelemetryState = SIM_STATE_INIT;
+        sim_t_stateChange = now + SIM_CYCLE_MS;
+        simWaitAfterResponse = true;
+        break;
+    }
+}
+
+void freeSimTelemetryPort(void)
+{
+    closeSerialPort(simPort);
+    simPort = NULL;
+    simEnabled = false;
+}
+
+void initSimTelemetry(void)
+{
+    portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_SIM);
+}
+
+void checkSimTelemetryState(void)
+{
+    if (simEnabled) {
+        return;
+    }
+    configureSimTelemetryPort();
+}
+
+void configureSimTelemetryPort(void)
+{
+    if (!portConfig) {
+        return;
+    }
+    baudRate_e baudRateIndex = portConfig->telemetry_baudrateIndex;
+    simPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_SIM, NULL, NULL,
+        baudRates[baudRateIndex], MODE_RXTX, SERIAL_NOT_INVERTED);
+
+    if (!simPort) {
+        return;
+    }
+
+    sim_t_stateChange = millis() + SIM_STARTUP_DELAY_MS;
+    simTelemetryState = SIM_STATE_INIT;
+    readState = SIM_READSTATE_RESPONSE;
+    readTransmitFlags(telemetryConfig()->simTransmitFlags);
+    simEnabled = true;
+}
+
+#endif

src/main/telemetry/sim.h

@@ -0,0 +1,78 @@
+#define SIM_AT_COMMAND_MAX_SIZE 256
+#define SIM_RESPONSE_BUFFER_SIZE 256
+#define SIM_CYCLE_MS 5000 								// wait between sim command cycles
+#define SIM_AT_COMMAND_DELAY_MS 3000
+#define SIM_AT_COMMAND_DELAY_MIN_MS 500
+#define SIM_STARTUP_DELAY_MS 10000
+#define SIM_MIN_TRANSMIT_INTERVAL 10
+#define SIM_DEFAULT_TRANSMIT_INTERVAL 60
+#define SIM_SMS_COMMAND_RTH             "RTH"
+#define SIM_PIN "0000"
+#define SIM_LOW_ALT_WARNING_MODES (NAV_ALTHOLD_MODE || NAV_RTH_MODE || NAV_WP_MODE || FAILSAFE_MODE)
+
+#define SIM_RESPONSE_CODE_OK    ('O' << 24 | 'K' << 16)
+#define SIM_RESPONSE_CODE_ERROR ('E' << 24 | 'R' << 16 | 'R' << 8 | 'O')
+#define SIM_RESPONSE_CODE_RING  ('R' << 24 | 'I' << 16 | 'N' << 8 | 'G')
+#define SIM_RESPONSE_CODE_CLIP  ('C' << 24 | 'L' << 16 | 'I' << 8 | 'P')
+#define SIM_RESPONSE_CODE_CREG  ('C' << 24 | 'R' << 16 | 'E' << 8 | 'G')
+#define SIM_RESPONSE_CODE_CSQ   ('C' << 24 | 'S' << 16 | 'Q' << 8 | ':')
+#define SIM_RESPONSE_CODE_CMT   ('C' << 24 | 'M' << 16 | 'T' << 8 | ':')
+
+typedef enum  {
+    SIM_TX_FLAG                 = (1 << 0),
+    SIM_TX_FLAG_FAILSAFE        = (1 << 1),
+    SIM_TX_FLAG_GPS             = (1 << 2),
+    SIM_TX_FLAG_ACC             = (1 << 3),
+    SIM_TX_FLAG_LOW_ALT         = (1 << 4),
+    SIM_TX_FLAG_RESPONSE        = (1 << 5)
+} simTxFlags_e;
+
+#define SIM_N_TX_FLAGS 5
+#define SIM_DEFAULT_TX_FLAGS "f"
+
+typedef enum  {
+    SIM_MODULE_NOT_DETECTED = 0,
+    SIM_MODULE_NOT_REGISTERED,
+    SIM_MODULE_REGISTERED,
+} simModuleState_e;
+
+typedef enum  {
+    SIM_STATE_INIT = 0,
+    SIM_STATE_INIT2,
+    SIM_STATE_INIT_ENTER_PIN,
+    SIM_STATE_SET_MODES,
+    SIM_STATE_SEND_SMS,
+    SIM_STATE_SEND_SMS_ENTER_MESSAGE
+} simTelemetryState_e;
+
+typedef enum  {
+    SIM_AT_OK = 0,
+    SIM_AT_ERROR,
+    SIM_AT_WAITING_FOR_RESPONSE
+} simATCommandState_e;
+
+typedef enum  {
+    SIM_READSTATE_RESPONSE = 0,
+    SIM_READSTATE_SMS,
+    SIM_READSTATE_SKIP
+} simReadState_e;
+
+typedef enum  {
+    SIM_TX_NO = 0,
+    SIM_TX_FS,
+    SIM_TX
+} simTransmissionState_e;
+
+typedef enum {
+    ACC_EVENT_NONE = 0,
+    ACC_EVENT_HIGH,
+    ACC_EVENT_LOW,
+    ACC_EVENT_NEG_X
+} accEvent_t;
+
+
+void handleSimTelemetry(void);
+void freeSimTelemetryPort(void);
+void initSimTelemetry(void);
+void checkSimTelemetryState(void);
+void configureSimTelemetryPort(void);

src/main/telemetry/telemetry.c

@@ -48,8 +48,10 @@
 #include "telemetry/jetiexbus.h"
 #include "telemetry/ibus.h"
 #include "telemetry/crsf.h"
+#include "telemetry/sim.h"
 
-PG_REGISTER_WITH_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 1);
+
+PG_REGISTER_WITH_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 2);
 
 PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig,
     .gpsNoFixLatitude = 0,
@@ -66,6 +68,12 @@ PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig,
     .smartportFuelUnit = SMARTPORT_FUEL_UNIT_MAH,
     .ibusTelemetryType = 0,
     .ltmUpdateRate = LTM_RATE_NORMAL,
+    .simTransmitInterval = SIM_DEFAULT_TRANSMIT_INTERVAL,
+    .simTransmitFlags = SIM_DEFAULT_TX_FLAGS,
+    .simLowAltitude = INT16_MIN,
+    .accEventThresholdHigh = 0,
+    .accEventThresholdLow = 0,
+    .accEventThresholdNegX = 0
 );
 
 void telemetryInit(void)
@@ -98,6 +106,10 @@ void telemetryInit(void)
     initIbusTelemetry();
 #endif
 
+#if defined(USE_TELEMETRY_SIM)
+    initSimTelemetry();
+#endif
+
 #if defined(USE_SERIALRX_CRSF) && defined(USE_TELEMETRY_CRSF)
     initCrsfTelemetry();
 #endif
@@ -156,6 +168,10 @@ void telemetryCheckState(void)
     checkIbusTelemetryState();
 #endif
 
+#if defined(USE_TELEMETRY_SIM)
+    checkSimTelemetryState();
+#endif
+
 #if defined(USE_SERIALRX_CRSF) && defined(USE_TELEMETRY_CRSF)
     checkCrsfTelemetryState();
 #endif
@@ -193,6 +209,10 @@ void telemetryProcess(timeUs_t currentTimeUs)
     handleIbusTelemetry();
 #endif
 
+#if defined(USE_TELEMETRY_SIM)
+    handleSimTelemetry();
+#endif
+
 #if defined(USE_SERIALRX_CRSF) && defined(USE_TELEMETRY_CRSF)
     handleCrsfTelemetry(currentTimeUs);
 #endif

src/main/telemetry/telemetry.h

@@ -68,6 +68,13 @@ typedef struct telemetryConfig_s {
     smartportFuelUnit_e smartportFuelUnit;
     uint8_t ibusTelemetryType;
     uint8_t ltmUpdateRate;
+    uint16_t simTransmitInterval;
+    uint8_t simTransmitFlags[4];
+    uint16_t accEventThresholdHigh;
+    uint16_t accEventThresholdLow;
+    uint16_t accEventThresholdNegX;
+    int16_t simLowAltitude;
+    uint8_t simGroundStationNumber[16];
 } telemetryConfig_t;
 
 PG_DECLARE(telemetryConfig_t, telemetryConfig);