Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-25 17:25:18 +03:00
Code Issues Wiki Activity

Add GSM telemetry (#4388)

Browse source
This commit is contained in:
potater1 2019-05-25 18:21:56 +03:00 • committed by Michel Pastor
parent 3485795dea
commit b207ba55de
9 changed files with 591 additions and 3 deletions
Download patch file Download diff file Expand all files Collapse all files

5
docs/Cli.md
View file

@ -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. |

1
make/source.mk
View file

@ -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 \

43
src/main/fc/settings.yaml
View file

@ -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"]

1
src/main/io/serial.h
View file

@ -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 {

2
src/main/target/common.h
View file

@ -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

435
src/main/telemetry/sim.c Normal file
View file

@ -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

78
src/main/telemetry/sim.h Normal file
View file

@ -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);

22
src/main/telemetry/telemetry.c
View file

@ -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

7
src/main/telemetry/telemetry.h
View file

@ -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);