diff --git a/Makefile b/Makefile index 35a1361..9506324 100755 --- a/Makefile +++ b/Makefile @@ -297,6 +297,7 @@ HIGHEND_SRC = \ telemetry/nmea.c \ telemetry/ltm.c \ telemetry/position_estimation_log.c \ + telemetry/forward.c \ sensors/sonar.c \ sensors/barometer.c diff --git a/src/main/config/config.c b/src/main/config/config.c index a3dbf1b..4b7d79d 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -784,6 +784,12 @@ static void resetConf(void) //ALTITUDE masterConfig.altitude_priority = 0; + // LAST HOME POSITION + masterConfig.restore_last_home = 0; + masterConfig.home_lat = 0; + masterConfig.home_lon = 0; + masterConfig.home_alt = 0; + ///////////////////////////// // copy first profile into remaining profile diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index 9af6852..db475d0 100755 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -151,6 +151,10 @@ typedef struct master_t { uint8_t max_speed_filter; uint8_t altitude_priority; uint8_t pan_inverted; + uint8_t restore_last_home; + int32_t home_lat; + int32_t home_lon; + int16_t home_alt; } master_t; extern master_t masterConfig; diff --git a/src/main/io/display.c b/src/main/io/display.c index 6eeddc0..2ed69cf 100755 --- a/src/main/io/display.c +++ b/src/main/io/display.c @@ -80,6 +80,7 @@ extern bool gotFix; extern bool gotTelemetry; extern bool lostTelemetry; extern bool homeSet_BY_GPS; +extern bool homeSet_BY_LAST; extern bool homeSet; extern bool homeReset; @@ -780,7 +781,9 @@ void showTelemetryPage(void){ if(!PROTOCOL(TP_MFD)) { if(homeSet_BY_GPS && homeSet) tfp_sprintf(lineBuffer, "HOME SET "); - else if(!homeSet_BY_GPS && homeSet) + else if(homeSet_BY_LAST && homeSet) + tfp_sprintf(lineBuffer, "HOME SET "); + else if(homeSet) tfp_sprintf(lineBuffer, "HOME SET "); else if(!homeSet || homeReset) tfp_sprintf(lineBuffer, "HOME NOT SET"); diff --git a/src/main/io/serial.h b/src/main/io/serial.h index 28e546d..a5c50ac 100755 --- a/src/main/io/serial.h +++ b/src/main/io/serial.h @@ -38,6 +38,7 @@ typedef enum { FUNCTION_TELEMETRY_NMEA = (1 << 10), // 1024 FUNCTION_TELEMETRY_LTM = (1 << 11), // 2048 FUNCTION_TELEMETRY_POSEST = (1 << 12), // 4096 + FUNCTION_TELEMETRY_FORWARD = (1 << 13), // 8192 } serialPortFunction_e; typedef enum { diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 1e383f2..11f6f29 100755 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -456,6 +456,7 @@ typedef enum { VAR_INT16 = (3 << VALUE_TYPE_OFFSET), VAR_UINT32 = (4 << VALUE_TYPE_OFFSET), VAR_FLOAT = (5 << VALUE_TYPE_OFFSET), + VAR_INT32 = (6 << VALUE_TYPE_OFFSET), // value section MASTER_VALUE = (0 << VALUE_SECTION_OFFSET), @@ -538,7 +539,11 @@ const clivalue_t valueTable[] = { { "gps_auto_baud", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gpsConfig.autoBaud, .config.lookup = { TABLE_OFF_ON } }, { "gps_min_sats", VAR_UINT8 | MASTER_VALUE, &masterConfig.gps_min_sats, .config.minmax = { 4, 20 } }, { "gps_home_beeper", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gpsConfig.homeBeeper, .config.lookup = { TABLE_OFF_ON } }, - { "update_home_by_local_gps", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.update_home_by_local_gps, .config.lookup = { TABLE_OFF_ON } }, + { "update_home_by_local_gps", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.update_home_by_local_gps, .config.lookup = { TABLE_OFF_ON } }, + { "restore_last_home", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.restore_last_home, .config.lookup = { TABLE_OFF_ON } }, + { "home_lat", VAR_INT32 | MASTER_VALUE, &masterConfig.home_lat, .config.minmax = { -90000000, 90000000 } }, + { "home_lon", VAR_INT32 | MASTER_VALUE, &masterConfig.home_lon, .config.minmax = { -180000000, 180000000 } }, + { "home_alt", VAR_INT16 | MASTER_VALUE, &masterConfig.home_alt, .config.minmax = { 0, 65535 } }, /* { "gps_pos_p", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[PIDPOS], .config.minmax = { 0, 200 } }, { "gps_pos_i", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PIDPOS], .config.minmax = { 0, 200 } }, @@ -733,7 +738,7 @@ const clivalue_t valueTable[] = { { "pan_pin", VAR_UINT8 | MASTER_VALUE, &masterConfig.pan_pin, .config.minmax = { 0, 7 } }, { "pan0", VAR_UINT16 | MASTER_VALUE, &masterConfig.pan0, .config.minmax = { 0, 3000 } }, { "pan0_calibrated", VAR_UINT8 | MASTER_VALUE, &masterConfig.pan0_calibrated, .config.minmax = { 0, 1 } }, - { "pan_calibration_pulse", VAR_UINT16 | MASTER_VALUE, &masterConfig.pan_calibration_pulse, .config.minmax = { 1, 1500 } }, + { "pan_calibration_pulse", VAR_UINT16 | MASTER_VALUE, &masterConfig.pan_calibration_pulse, .config.minmax = { 0, 3000 } }, { "pan_inverted", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.pan_inverted, .config.lookup = { TABLE_OFF_ON } }, { "min_pan_speed", VAR_UINT8 | MASTER_VALUE, &masterConfig.min_pan_speed, .config.minmax = { 0, 100 } }, { "tilt_pin", VAR_UINT8 | MASTER_VALUE, &masterConfig.tilt_pin, .config.minmax = { 0, 7 } }, @@ -2227,6 +2232,10 @@ static void cliPrintVar(const clivalue_t *var, uint32_t full) value = *(uint32_t *)ptr; break; + case VAR_INT32: + value = *(int32_t *)ptr; + break; + case VAR_FLOAT: printf("%s", ftoa(*(float *)ptr, buf)); if (full && (var->type & VALUE_MODE_MASK) == MODE_DIRECT) { @@ -2276,6 +2285,10 @@ static void cliSetVar(const clivalue_t *var, const int_float_value_t value) *(uint32_t *)ptr = value.int_value; break; + case VAR_INT32: + *(int32_t *)ptr = value.int_value; + break; + case VAR_FLOAT: *(float *)ptr = (float)value.float_value; if(ptr==&masterConfig.offset) @@ -2511,7 +2524,7 @@ static void cliMovePanServo(char *cmdline) } else { degrees=atoi(cmdline); if(degrees>=0 && degrees<=360 ) { - printf("Moving pan servo to %u º\r\n", degrees); + printf("Moving pan servo to %u deg.\r\n", degrees); SERVOTEST_HEADING=degrees; ENABLE_SERVO(SERVOPAN_MOVE); } @@ -2530,7 +2543,7 @@ static void cliMoveTiltServo(char *cmdline) } else { degrees=atoi(cmdline); if(degrees >=0 && degrees <= 90 ) { - printf("Moving tilt servo to %u º\r\n", degrees); + printf("Moving tilt servo to %u deg.\r\n", degrees); SERVOTEST_TILT = degrees; ENABLE_SERVO(SERVOTILT_MOVE); } diff --git a/src/main/telemetry/forward.c b/src/main/telemetry/forward.c new file mode 100644 index 0000000..7142c1a --- /dev/null +++ b/src/main/telemetry/forward.c @@ -0,0 +1,124 @@ +#include +#include +#include + +#include "platform.h" + +#ifdef TELEMETRY + +#include "common/maths.h" +#include "common/axis.h" + +#include "drivers/system.h" +#include "drivers/sensor.h" +#include "drivers/accgyro.h" +#include "drivers/gpio.h" +#include "drivers/timer.h" +#include "drivers/serial.h" + + +#include "sensors/sensors.h" +#include "sensors/acceleration.h" +#include "sensors/gyro.h" +#include "sensors/barometer.h" +#include "sensors/battery.h" + +#include "io/serial.h" +#include "io/rc_controls.h" +#include "io/gps.h" + +#include "rx/rx.h" + +#include "flight/mixer.h" +#include "flight/pid.h" +#include "flight/imu.h" +#include "flight/altitudehold.h" + +#include "config/runtime_config.h" +#include "config/config.h" + +#include "telemetry/telemetry.h" +#include "telemetry/forward.h" + +static serialPort_t *forwardPort = NULL; +static serialPortConfig_t *portConfig; + +#define FORWARD_BAUDRATE 57600 +#define FORWARD_INITIAL_PORT_MODE MODE_RXTX + +static telemetryConfig_t *telemetryConfig; +static bool forwardTelemetryEnabled = false; +static portSharing_e forwardPortSharing; + +#define CYCLETIME 125 + +static uint32_t lastCycleTime = 0; +static uint8_t cycleNum = 0; + +void initForwardTelemetry(telemetryConfig_t *initialTelemetryConfig) +{ + telemetryConfig = initialTelemetryConfig; + portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_FORWARD); + forwardPortSharing = determinePortSharing(portConfig, FUNCTION_TELEMETRY_FORWARD); +} + +void freeForwardTelemetryPort(void) +{ + closeSerialPort(forwardPort); + forwardPort = NULL; + forwardTelemetryEnabled = false; +} + +void configureForwardTelemetryPort(void) +{ + if (!portConfig) { + return; + } + + forwardPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_FORWARD, NULL, FORWARD_BAUDRATE, FORWARD_INITIAL_PORT_MODE, telemetryConfig->telemetry_inversion ? SERIAL_INVERTED : SERIAL_NOT_INVERTED); + if (!forwardPort) { + return; + } + + forwardTelemetryEnabled = true; +} + +void checkForwardTelemetryState(void) +{ + bool newTelemetryEnabledValue = telemetryDetermineEnabledState(forwardPortSharing); + + if (newTelemetryEnabledValue == forwardTelemetryEnabled) { + return; + } + + if (newTelemetryEnabledValue) + configureForwardTelemetryPort(); + else + freeForwardTelemetryPort(); +} + +void handleForwardTelemetry(rxConfig_t *rxConfig, uint16_t deadband3d_throttle) +{ + if (!forwardTelemetryEnabled) { + return; + } + + while (serialRxBytesWaiting(forwardPort) > 0) { + uint8_t c = serialRead(forwardPort); + evaluateOtherData(forwardPort,c); + } + + +} + +void forwardTelemetry(uint8_t c){ + serialWrite(forwardPort, c); +} + +bool forwardEnabled(void){ + return(forwardTelemetryEnabled); +} + + +#endif + diff --git a/src/main/telemetry/forward.h b/src/main/telemetry/forward.h new file mode 100644 index 0000000..26e7c5d --- /dev/null +++ b/src/main/telemetry/forward.h @@ -0,0 +1,14 @@ +#include "rx/rx.h" + +#ifndef TELEMETRY_FORWARD_ +#define TELEMETRY_FORWARD_ + +void handleForwardTelemetryvoid(rxConfig_t *rxConfig, uint16_t deadband3d_throttle); +void checkForwardTelemetryState(void); + +void initForwardTelemetry(telemetryConfig_t *telemetryConfig); +void configureForwardTelemetryPort(void); +void freeForwardTelemetryPort(void); +bool forwardEnabled(void); + +#endif /* TELEMETRY_FORWARD_ */ diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index 30d234a..c36a53b 100755 --- a/src/main/telemetry/telemetry.c +++ b/src/main/telemetry/telemetry.c @@ -45,6 +45,7 @@ #include "telemetry/nmea.h" #include "telemetry/ltm.h" #include "telemetry/position_estimation_log.h" +#include "telemetry/forward.h" static telemetryConfig_t *telemetryConfig; @@ -64,7 +65,7 @@ void telemetryInit(void) initNMEATelemetry(telemetryConfig); initLtmTelemetry(telemetryConfig); initPOSESTTelemetry(telemetryConfig); - + initForwardTelemetry(telemetryConfig); telemetryCheckState(); } @@ -93,6 +94,7 @@ void telemetryCheckState(void) checkNMEATelemetryState(); checkLtmTelemetryState(); checkPOSESTTelemetryState(); + checkForwardTelemetryState(); } void telemetryProcess(rxConfig_t *rxConfig, uint16_t deadband3d_throttle) @@ -106,6 +108,7 @@ void telemetryProcess(rxConfig_t *rxConfig, uint16_t deadband3d_throttle) handleNMEATelemetry(); handleLtmTelemetry(); handlePOSESTTelemetry(); + handleForwardTelemetry(); } #endif diff --git a/src/main/tracker/frskyx.c b/src/main/tracker/frskyx.c index 77d4369..d2d9666 100755 --- a/src/main/tracker/frskyx.c +++ b/src/main/tracker/frskyx.c @@ -2,7 +2,7 @@ * This file is part of u360gts, aka amv-open360tracker 32bits: * https://github.com/raul-ortega/amv-open360tracker-32bits * - * The code below is an adaptation by Raúl Ortega of the original code written by Samuel Brucksch: + * The code below is an adaptation by Raúl Ortega of the original code written by Samuel Brucksch: * https://github.com/SamuelBrucksch/open360tracker * * u360gts is free software: you can redistribute it and/or modify @@ -157,7 +157,20 @@ void processHubPacket(uint8_t id, uint16_t value) } } -bool checkSportPacket(uint8_t *packet) +bool checkSportPacket_V1(uint8_t *packet) +{ + short crc = 0; + for (int i = 1; i < FRSKY_RX_PACKET_SIZE; i++) { + crc += packet[i]; //0-1FF + crc += crc >> 8; //0-100 + crc &= 0x00ff; + crc += crc >> 8; //0-0FF + crc &= 0x00ff; + } + return (crc == 0x00ff); +} + +static uint8_t checkSportPacket_V2(uint8_t * packet) { short crc = 0; for (int i = 1; i < FRSKY_RX_PACKET_SIZE; i++) { @@ -180,10 +193,10 @@ void processSportPacket(uint8_t *packet) uint8_t prim = packet[1]; uint16_t appId = *((uint16_t *)(packet + 2)); - if (!checkSportPacket(packet)){ - telemetry_failed_cs++; - return; - } + if (!checkSportPacket_V1(packet) && !checkSportPacket_V2(packet)){ + telemetry_failed_cs++; + return; + } if(appId == GPS_STATUS) telemetry_provider = TELEMETRY_PROVIDER_APM10; diff --git a/src/main/tracker/main.c b/src/main/tracker/main.c index 673bfa5..e6b9b43 100755 --- a/src/main/tracker/main.c +++ b/src/main/tracker/main.c @@ -155,6 +155,8 @@ void setHomeByLocalGps(positionVector_t *tracker, int32_t lat, int32_t lon, int1 void calcTelemetryFrequency(void); uint8_t filterTiltAngle(uint8_t target); void servosInit(void); +void setTelemetryHome(int32_t lat, int32_t lon, int16_t alt, bool save); +void setHomeByLastSaved(positionVector_t *tracker); //EASING int16_t _lastTilt; @@ -227,6 +229,7 @@ bool previousState; bool homeButtonPreviousState; bool gotNewHeading; bool homeSet_BY_GPS = false; +bool homeSet_BY_LAST = false; //TIMERS unsigned long servoPanTimer = 0; @@ -318,7 +321,7 @@ void tracker_setup(void) protocolInit(masterConfig.telemetry_protocol); if(feature(FEATURE_AUTODETECT)) - enableProtocolDetection(); + enableProtocolDetection(masterConfig.telemetry_protocol); trackingInit(); @@ -374,6 +377,7 @@ void trackingInit(void){ gotFix = false; homeSet = false; + homeReset = false; trackingStarted = false; @@ -687,11 +691,11 @@ void updateDigitalButtons(void) { } void setHomeByTelemetry(positionVector_t *tracker, positionVector_t *target) { - tracker->lat = target->lat; - tracker->lon = target->lon; - tracker->alt = target->alt; + tracker->lat = telemetry_lat; + tracker->lon = telemetry_lon; + tracker->alt = telemetry_alt; - setTelemetryHome(target->lat,target->lon,target->alt); + setTelemetryHome(telemetry_lat,telemetry_lon,telemetry_alt, true); if(feature(FEATURE_DEBUG)){ tracker->lat = 47403583; tracker->lon = 8535850; tracker->alt = 474; @@ -702,18 +706,53 @@ void setHomeByTelemetry(positionVector_t *tracker, positionVector_t *target) { homeSet = true; homeSet_BY_GPS = false; + homeSet_BY_LAST = false; homeReset = false; home_timer_reset = 0; + epsVectorLoad(&targetLast,target->lat,target->lon,0,0,0); epsVectorLoad(&targetCurrent,target->lat,target->lon,0,0,millis()); } +bool shouldRestoreHome(void){ + return(!homeSet && !homeSet_BY_LAST && !homeReset && masterConfig.restore_last_home == true && masterConfig.home_lat != 0 && masterConfig.home_lon != 0); +} + +void setTelemetryHome(int32_t lat, int32_t lon, int16_t alt, bool save){ + telemetry_home_lat = lat; + telemetry_home_lon = lon; + telemetry_home_alt = alt; + + if(save == true && lat!=0 && lon != 0){ + masterConfig.home_lat = lat; + masterConfig.home_lon = lon; + masterConfig.home_alt = alt; + writeEEPROM(); + } +} + +void setHomeByLastSaved(positionVector_t *tracker) { + + tracker->lat = masterConfig.home_lat; + tracker->lon = masterConfig.home_lon; + tracker->alt = masterConfig.home_alt; + + homeSet = true; + homeSet_BY_GPS = false; + homeSet_BY_LAST = true; + homeReset = false; + home_timer_reset = 0; + + setTelemetryHome(masterConfig.home_lat, masterConfig.home_lon, masterConfig.home_alt, false); + +} + void setHomeByLocalGps(positionVector_t *tracker, int32_t lat, int32_t lon, int16_t alt, bool home_updated, bool beep) { tracker->lat = lat; tracker->lon = lon; tracker->alt = alt; - setTelemetryHome(lat,lon,alt); + setTelemetryHome(lat,lon,alt, true); if(feature(FEATURE_DEBUG)) { tracker->lat = 47403583; tracker->lon = 8535850; tracker->alt = 474; @@ -731,8 +770,10 @@ void setHomeByLocalGps(positionVector_t *tracker, int32_t lat, int32_t lon, int1 homeSet = true; homeSet_BY_GPS = true; + homeSet_BY_LAST = false; homeReset = false; home_timer_reset = 0; + if(!home_updated) { epsVectorLoad(&targetLast,lat,lon,0,0,0); epsVectorLoad(&targetCurrent,lat,lon,0,0,millis()); @@ -827,9 +868,10 @@ void updateTelemetryLost(void){ if(!gotTelemetry && (millis() - lostTelemetry_timer > 3000)){ lostTelemetry = true; - if(feature(FEATURE_AUTODETECT)){ + if(feature(FEATURE_AUTODETECT) && !isProtocolDetectionEnabled()){ showAutodetectingTitle(0); - enableProtocolDetection(); + enableProtocolDetection(masterConfig.telemetry_protocol); + detection_title_updated = false; } } } @@ -841,7 +883,7 @@ void updateTargetPosition(void){ if(telemetry_sats >= masterConfig.telemetry_min_sats && targetPosition.home_alt == -32768) targetPosition.home_alt = getTargetAlt(0); - targetPosition.alt = getTargetAlt(targetPosition.home_alt); + targetPosition.alt = getTargetAlt(telemetry_home_alt); if(PROTOCOL(TP_MFD)){ distance = getDistance(); @@ -1062,6 +1104,12 @@ void updateSetHomeButton(void){ void updateSetHomeByGPS(void){ if(PROTOCOL(TP_SERVOTEST) || PROTOCOL(TP_MFD) || PROTOCOL(TP_CALIBRATING_MAG) || PROTOCOL(TP_CALIBRATING_PAN0) || PROTOCOL(TP_CALIBRATING_MAXPAN)) return; + + if(shouldRestoreHome()){ + setHomeByLastSaved(&trackerPosition); + return; + } + if(homeReset && lostTelemetry){ telemetry_lat = 0; telemetry_lon = 0; @@ -1098,7 +1146,7 @@ bool couldLolcalGpsSetHome(bool setByUser){ } bool couldTelemetrySetHome(void){ - return (!feature(FEATURE_GPS) && masterConfig.telemetry_home == 1 && telemetry_sats >= masterConfig.telemetry_min_sats); + return (!feature(FEATURE_GPS) && masterConfig.telemetry_home == 1 && telemetry_sats >= masterConfig.telemetry_min_sats && telemetry_lat != 0 && telemetry_lon != 0); } void updateMFD(void){ @@ -1556,28 +1604,27 @@ void updateEPSParams(){ void updateProtocolDetection(void){ uint16_t detected_protocol; - if(!feature(FEATURE_AUTODETECT) || cliMode) + if(!feature(FEATURE_AUTODETECT) || cliMode || !isProtocolDetectionEnabled()) return; detected_protocol = getProtocol(); - if(detected_protocol == 0 && !detection_title_updated ){ - detection_title_updated = true; - updateDisplayProtocolTitle(detected_protocol); - return; - } - - - if(detected_protocol > 0 && isProtocolDetectionEnabled()){ - updateDisplayProtocolTitle(detected_protocol); - detection_title_updated = false; - protocolInit(detected_protocol); - if(!homeSet){ + if(detected_protocol > 0 ) { + protocolInit(detected_protocol); + if(!homeSet){ trackingInit(); if(PROTOCOL(TP_MFD)) settingHome = true; } - } + disableProtocolDetection(); + detection_title_updated = false; + } + + if(detection_title_updated == false){ + updateDisplayProtocolTitle(detected_protocol); + detection_title_updated = true; + } + } void protocolInit(uint16_t protocol){ diff --git a/src/main/tracker/protocol_detection.c b/src/main/tracker/protocol_detection.c index e9689f5..6bbc725 100644 --- a/src/main/tracker/protocol_detection.c +++ b/src/main/tracker/protocol_detection.c @@ -23,38 +23,51 @@ /* */ -#include "config.h" #include #include +#include #include "config/runtime_config.h" - -// machine states -enum protocolDetectionStates { - DETECTION_STATE_IDLE, - DETECTION_STATE_START, - DETECTION_STATE_START_FRXKY, - DETECTION_STATE_START_MAVLINK, - DETECTION_STATE_START_MFD, - DETECTION_STATE_START_PITLAB, - DETECTION_STATE_START_CROSSFIRE0, - DETECTION_STATE_START_CROSSFIRE1, - DETECTION_STATE_DETECTED - }; - -static uint8_t detectionState = DETECTION_STATE_IDLE; -static uint8_t detectionPacketIdex=0; static uint16_t protocolDetected = 0; - +uint8_t current_protocol = 0; +uint32_t protocolDetectionTimer = 0; bool detectionIsEnabled = false; +uint8_t protocol_fixes = 0; + +extern bool gotFix; -void enableProtocolDetection(void){ +static const uint16_t protocols[] = { + TP_MAVLINK, + TP_FRSKY_X, + TP_CROSSFIRE, + TP_MFD, + TP_LTM, + TP_GPS_TELEMETRY, + TP_FRSKY_D, + TP_PITLAB, + TP_RVOSD +}; + +void enableProtocolDetection(uint16_t default_protocol) { protocolDetected = 0; detectionIsEnabled = true; + protocol_fixes = 0; + current_protocol=0; + + //set default protocol + for(uint8_t i=0; i < sizeof(protocols) / sizeof(uint8_t); i++){ + if(protocols[i] == default_protocol){ + current_protocol = i; + break; + } + } + + protocolDetectionTimer = 0; } void disableProtocolDetection(void){ detectionIsEnabled = false; + protocolDetected = 0; } bool isProtocolDetectionEnabled(void){ @@ -65,112 +78,56 @@ uint16_t getProtocol(void){ return protocolDetected; } -void protocolDetectionParser(uint8_t c){ - if(!detectionIsEnabled) - return; - - switch(detectionState){ - case DETECTION_STATE_IDLE: - if (c == 0x08 ) { - detectionState = DETECTION_STATE_START_CROSSFIRE0; - detectionPacketIdex = 0; - break; - } else if (c =='#' || c == 'X') { - detectionState = DETECTION_STATE_START_MFD; - detectionPacketIdex = 0; - } else if (c == 0x7E) - detectionState = DETECTION_STATE_START_FRXKY; - else if (c == 254 && detectionPacketIdex > 10) { - protocolDetected = TP_MAVLINK; - detectionState = DETECTION_STATE_DETECTED; - } else if (c == '$'){ - detectionState = DETECTION_STATE_START_PITLAB; - detectionPacketIdex = 0; - return; - } - detectionPacketIdex ++; - break; - case DETECTION_STATE_START_CROSSFIRE0: - if (c == 0x00){ - detectionState = DETECTION_STATE_START_CROSSFIRE1; - detectionPacketIdex++; - } - break; - case DETECTION_STATE_START_CROSSFIRE1: - if (c == 0xA7 && detectionPacketIdex == 1) { - protocolDetected = TP_CROSSFIRE; - detectionState = DETECTION_STATE_DETECTED; - } +void protocolDetectionParser(uint8_t c) +{ + + if(protocolDetectionTimer == 0) + protocolDetectionTimer = millis(); + + if(millis() - protocolDetectionTimer >= 3000){ + current_protocol ++; + if(current_protocol >= sizeof(protocols) / sizeof(uint16_t)) current_protocol = 0; + protocolDetectionTimer = millis(); + gotFix = false; + } + + switch(protocols[current_protocol]){ + case TP_MFD: + mfd_encodeTargetData(c); break; - case DETECTION_STATE_START_MFD: - if ((c == '#' || c == 'X')) - detectionPacketIdex++; - else if (detectionPacketIdex > 5){ - protocolDetected = TP_MFD; - detectionState = DETECTION_STATE_DETECTED; - } else - detectionState = DETECTION_STATE_IDLE; - break; - case DETECTION_STATE_START_FRXKY: - if (c == 0xFD) { - protocolDetected = TP_FRSKY_D; - detectionState = DETECTION_STATE_DETECTED; - } else if (detectionState == DETECTION_STATE_START_FRXKY && c ==0x10){ - protocolDetected = TP_FRSKY_X; - detectionState = DETECTION_STATE_DETECTED; - detectionPacketIdex = 0; - } else if (detectionPacketIdex > 10) - detectionState = DETECTION_STATE_IDLE; - break; - case DETECTION_STATE_START_MAVLINK: - if (detectionPacketIdex < 5) - detectionPacketIdex++; - else if (c == 24 || c == 33){ - protocolDetected = TP_MAVLINK; - detectionState = DETECTION_STATE_DETECTED; - } else - detectionState = DETECTION_STATE_IDLE; - break; - case DETECTION_STATE_START_PITLAB: - if(c == 'T' && detectionPacketIdex == 0 ){ //This is not PITLAB, it is LTM - protocolDetected = TP_LTM; - detectionState = DETECTION_STATE_DETECTED; - break; - } else if(c == '$' && detectionPacketIdex == 9 ){ - protocolDetected = TP_PITLAB; - detectionState = DETECTION_STATE_DETECTED; - break; - } else if(c == '$' && detectionPacketIdex > 9){ - detectionState = DETECTION_STATE_START; - break; - } - detectionPacketIdex++; - break; - case DETECTION_STATE_START: - switch(c){ - case 'T': - protocolDetected = TP_LTM; - detectionState = DETECTION_STATE_DETECTED; - break; - case 'G': - protocolDetected = TP_GPS_TELEMETRY; - detectionState = DETECTION_STATE_DETECTED; - break; - case '1': - case 'R': - case 'V': - protocolDetected = TP_RVOSD; - detectionState = DETECTION_STATE_DETECTED; - break; - default: - detectionState = DETECTION_STATE_IDLE; - break; - } - break; - case DETECTION_STATE_DETECTED: - detectionState = DETECTION_STATE_IDLE; - detectionPacketIdex = 0; - disableProtocolDetection(); - break; - } + case TP_GPS_TELEMETRY: + gps_encodeTargetData(c); + break; + case TP_MAVLINK: + mavlink_encodeTargetData(c); + break; + case TP_RVOSD: + rvosd_encodeTargetData(c); + break; + case TP_FRSKY_D: + frskyd_encodeTargetData(c); + break; + case TP_FRSKY_X: + frskyx_encodeTargetData(c); + break; + case TP_LTM: + ltm_encodeTargetData(c); + break; + case TP_PITLAB: + pitlab_encodeTargetData(c); + break; + case TP_CROSSFIRE: + crossfire_encodeTargetData(c); + break; + } + + if(gotFix){ + protocol_fixes ++; + if(protocol_fixes > 1) { + protocolDetected = protocols[current_protocol]; + return; + } + gotFix = false; + + } } diff --git a/src/main/tracker/protocol_detection.h b/src/main/tracker/protocol_detection.h index 99d3ce2..22bde45 100644 --- a/src/main/tracker/protocol_detection.h +++ b/src/main/tracker/protocol_detection.h @@ -24,9 +24,15 @@ */ #include +#include + +#ifndef PROTOCOL_DETECTION_H_ +#define PROTOCOL_DETECTION_H_ -void enableProtocolDetection(void); void disableProtocolDetection(void); void protocolDetectionParser(uint8_t c); uint16_t getProtocol(void); bool isProtocolDetectionEnabled(void); +void enableProtocolDetection(uint16_t default_protocol); + +#endif /* PROTOCOL_DETECTION_H_ */ diff --git a/src/main/tracker/telemetry.c b/src/main/tracker/telemetry.c index aefec68..df99b7f 100755 --- a/src/main/tracker/telemetry.c +++ b/src/main/tracker/telemetry.c @@ -86,26 +86,33 @@ void encodeTargetData(uint8_t c) { uint16_t chars = 0; uint8_t sentences = 0; - protocolDetectionParser(c); - - if(PROTOCOL(TP_MFD)) - mfd_encodeTargetData(c); - else if(PROTOCOL(TP_GPS_TELEMETRY)) - gps_encodeTargetData(c); - else if(PROTOCOL(TP_MAVLINK)) - mavlink_encodeTargetData(c); - else if(PROTOCOL(TP_RVOSD)) - rvosd_encodeTargetData(c); - else if(PROTOCOL(TP_FRSKY_D)) - frskyd_encodeTargetData(c); - else if(PROTOCOL(TP_FRSKY_X)) - frskyx_encodeTargetData(c); - else if(PROTOCOL(TP_LTM)) - ltm_encodeTargetData(c); - else if(PROTOCOL(TP_CROSSFIRE)) - crossfire_encodeTargetData(c); - else if(PROTOCOL(TP_PITLAB)) - pitlab_encodeTargetData(c); + if(isProtocolDetectionEnabled()) { + protocolDetectionParser(c); + return; + } + + if(PROTOCOL(TP_MFD)) + mfd_encodeTargetData(c); + else if(PROTOCOL(TP_GPS_TELEMETRY)) + gps_encodeTargetData(c); + else if(PROTOCOL(TP_MAVLINK)) + mavlink_encodeTargetData(c); + else if(PROTOCOL(TP_RVOSD)) + rvosd_encodeTargetData(c); + else if(PROTOCOL(TP_FRSKY_D)) + frskyd_encodeTargetData(c); + else if(PROTOCOL(TP_FRSKY_X)) + frskyx_encodeTargetData(c); + else if(PROTOCOL(TP_LTM)) + ltm_encodeTargetData(c); + else if(PROTOCOL(TP_CROSSFIRE)) + crossfire_encodeTargetData(c); + else if(PROTOCOL(TP_PITLAB)) + pitlab_encodeTargetData(c); + + if(forwardEnabled()){ + forwardTelemetry(c); + } } void gps_encodeTargetData(uint8_t c) { @@ -160,8 +167,4 @@ int32_t gpsToLong(int8_t neg, uint16_t bp, uint16_t ap) { return ((int32_t)(first + second) * (uint32_t)neg); } -void setTelemetryHome(int32_t lat, int32_t lon, int16_t alt){ - telemetry_home_lat = lat; - telemetry_home_lon = lon; - telemetry_home_alt = alt; -} + diff --git a/src/main/tracker/telemetry.h b/src/main/tracker/telemetry.h index cc709a0..06c38e6 100755 --- a/src/main/tracker/telemetry.h +++ b/src/main/tracker/telemetry.h @@ -68,8 +68,5 @@ int32_t getTargetLon(); uint16_t getSats(); uint16_t getDistance(); uint16_t getAzimuth(); -void enableProtocolDetection(void); -void disableProtocolDetection(void); -void setTelemetryHome(int32_t lat, int32_t lon, int16_t alt); #endif diff --git a/src/main/version.h b/src/main/version.h index 71485b7..77b056e 100755 --- a/src/main/version.h +++ b/src/main/version.h @@ -17,7 +17,7 @@ #define FC_VERSION_MAJOR 11 // increment when a major release is made (big new feature, etc) #define FC_VERSION_MINOR 1 // increment when a minor release is made (small new feature, change etc) -#define FC_VERSION_PATCH_LEVEL 0 // increment when a bug is fixed +#define FC_VERSION_PATCH_LEVEL 1 // increment when a bug is fixed #define STR_HELPER(x) #x #define STR(x) STR_HELPER(x)