diff --git a/docs/Cli.md b/docs/Cli.md index 6899aad2e7e..271133a3c6b 100644 --- a/docs/Cli.md +++ b/docs/Cli.md @@ -478,3 +478,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. | \ No newline at end of file diff --git a/make/source.mk b/make/source.mk index e38323f3555..7720fc8c85b 100644 --- a/make/source.mk +++ b/make/source.mk @@ -198,6 +198,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 \ diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 4510fe02a81..10a58628d05 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1527,7 +1527,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 @@ -1578,7 +1578,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"] diff --git a/src/main/io/serial.h b/src/main/io/serial.h index 22ec7cb7bd3..617fb19b6d2 100644 --- a/src/main/io/serial.h +++ b/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 { diff --git a/src/main/target/common.h b/src/main/target/common.h index 325c00bed42..6b1a29ec005 100755 --- a/src/main/target/common.h +++ b/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 diff --git a/src/main/telemetry/sim.c b/src/main/telemetry/sim.c new file mode 100644 index 00000000000..c4f9550d57d --- /dev/null +++ b/src/main/telemetry/sim.c @@ -0,0 +1,435 @@ +#include "platform.h" + +#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_SIM) + +#include + +#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: ,[],[,,,,,,,] + // +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 diff --git a/src/main/telemetry/sim.h b/src/main/telemetry/sim.h new file mode 100644 index 00000000000..182f4962160 --- /dev/null +++ b/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); diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index cc5b20b8575..7ac55c5ee16 100644 --- a/src/main/telemetry/telemetry.c +++ b/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 diff --git a/src/main/telemetry/telemetry.h b/src/main/telemetry/telemetry.h index 9c03c8711d2..4a867ffb96f 100644 --- a/src/main/telemetry/telemetry.h +++ b/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);