From 6a63c7447edc505b53ca18e03d6c19cd63bc3aa1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Ra=C3=BAl=20Ortega?= Date: Wed, 11 Apr 2018 02:17:36 +0200 Subject: [PATCH 1/6] definitions added for cleaning warning messages after compilation --- src/main/rx/rx.h | 2 ++ src/main/tracker/TinyGPS.c | 2 +- src/main/tracker/TinyGPS.h | 2 +- 3 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index b96837f..90b554e 100755 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -156,3 +156,5 @@ void suspendRxSignal(void); void resumeRxSignal(void); void initRxRefreshRate(uint16_t *rxRefreshRatePtr); + +uint8_t calculateRssiPercentage(void); diff --git a/src/main/tracker/TinyGPS.c b/src/main/tracker/TinyGPS.c index 0a56794..b668b90 100755 --- a/src/main/tracker/TinyGPS.c +++ b/src/main/tracker/TinyGPS.c @@ -28,7 +28,7 @@ Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA #define COMBINE(sentence_type, term_number) (((unsigned)(sentence_type) << 5) | term_number) -bool TinyGPS_encode(char c); + int from_hex(char a); unsigned long parse_decimal(); unsigned long parse_degrees(); diff --git a/src/main/tracker/TinyGPS.h b/src/main/tracker/TinyGPS.h index 3894313..d2e2429 100755 --- a/src/main/tracker/TinyGPS.h +++ b/src/main/tracker/TinyGPS.h @@ -125,7 +125,7 @@ Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA enum {_GPS_SENTENCE_GPGGA, _GPS_SENTENCE_GNGGA, _GPS_SENTENCE_GPRMC, _GPS_SENTENCE_OTHER}; - + bool TinyGPS_encode(char c); // internal utilities /*int from_hex(char a); From 534c9d9a2154fdae1e184c6aed50340475c59172 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Ra=C3=BAl=20Ortega?= Date: Wed, 11 Apr 2018 03:01:53 +0200 Subject: [PATCH 2/6] Calculate tracker altitude and target altitude as relative. --- src/main/tracker/defines.h | 1 + src/main/tracker/main.c | 74 ++++++++++++++++++++---------------- src/main/tracker/telemetry.c | 4 +- src/main/tracker/telemetry.h | 2 +- 4 files changed, 46 insertions(+), 35 deletions(-) diff --git a/src/main/tracker/defines.h b/src/main/tracker/defines.h index 3ca51f7..aac81ce 100755 --- a/src/main/tracker/defines.h +++ b/src/main/tracker/defines.h @@ -38,6 +38,7 @@ typedef struct { int32_t lat; int32_t lon; int16_t alt; + int16_t home_alt; uint16_t heading; uint32_t distance; } diff --git a/src/main/tracker/main.c b/src/main/tracker/main.c index fb332fc..03f466c 100755 --- a/src/main/tracker/main.c +++ b/src/main/tracker/main.c @@ -151,7 +151,8 @@ void updateProtocolDetection(void); void protocolInit(void); void trackingInit(void); void telemetryPortInit(void); -void setHomeByLocalGps(positionVector_t *tracker, int32_t lat, int32_t lon, int16_t alt, bool home_updated, bool beep); +void setHomeByLocalGps(positionVector_t *tracker, int32_t lat, int32_t lon, int16_t alt, bool home_updated); +void setHomePossition(positionVector_t *tracker,bool byGPS,int32_t lat,int32_t lon,int16_t alt, bool home_updated); //EASING int16_t _lastTilt; int16_t tilt; @@ -672,37 +673,46 @@ void updateDigitalButtons(void) { } void setHomeByTelemetry(positionVector_t *tracker, positionVector_t *target) { - tracker->lat = target->lat; - tracker->lon = target->lon; - tracker->alt = target->alt; + setHomePossition(&tracker,false, target->lat, target->lon, target->alt,false); + setInitialEPSVector(target->lat,target->lon); + target->home_alt = target->alt; +} - if(feature(FEATURE_DEBUG)){ - tracker->lat = 47403583; tracker->lon = 8535850; tracker->alt = 474; +void setHomeByLocalGps(positionVector_t *tracker, int32_t lat, int32_t lon, int16_t alt, bool home_updated) { + setHomePossition(&tracker,true, lat, lon, alt, home_updated); + if(!home_updated) { + setInitialEPSVector(lat,lon); } +} + +void setHomePossition(positionVector_t *tracker,bool byGPS,int32_t lat,int32_t lon,int16_t alt, bool home_updated){ + + tracker->lat = lat; + tracker->lon = lon; + + if(home_updated) + tracker->alt = alt - tracker->home_alt; + else + { + tracker->alt = 0; + tracker->home_alt = alt; + } + + if(feature(FEATURE_DEBUG)){ + tracker->lat = 47403583; tracker->lon = 8535850; tracker->alt = 474; + } + + homeSet = true; + homeSet = true; + homeSet_BY_GPS = byGPS; + homeReset = false; + home_timer_reset = 0; - homeSet = true; - homeSet_BY_GPS = 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()); } -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; - if(feature(FEATURE_DEBUG)) { - tracker->lat = 47403583; tracker->lon = 8535850; tracker->alt = 474; - } - homeSet = true; - homeSet_BY_GPS = true; - homeReset = false; - home_timer_reset = 0; - if(!home_updated) { - epsVectorLoad(&targetLast,lat,lon,0,0,0); - epsVectorLoad(&targetCurrent,lat,lon,0,0,millis()); - } +void setInitialEPSVector(int32_t lat,int32_t lon){ + epsVectorLoad(&targetLast,lat,lon,0,0,0); + epsVectorLoad(&targetCurrent,lat,lon,0,0,millis()); } void updateBatteryStatus(void){ @@ -801,7 +811,7 @@ void updateTargetPosition(void){ if(!PROTOCOL(TP_SERVOTEST)){ if (gotAlt) { - targetPosition.alt = getTargetAlt(); + targetPosition.alt = getTargetAlt(targetPosition.home_alt); if(PROTOCOL(TP_MFD)){ distance = getDistance(); @@ -983,7 +993,7 @@ void updateSetHomeButton(void){ setHomeByTelemetry(&trackerPosition, &targetPosition); // By local GPS because telemetry hasn't got enought sats and we don't want wait more time. else if(!homeSet && couldLolcalGpsSetHome(true)){ - setHomeByLocalGps(&trackerPosition,GPS_coord[LAT]/10,GPS_coord[LON]/10,GPS_altitude,false,true); + setHomeByLocalGps(&trackerPosition,GPS_coord[LAT]/10,GPS_coord[LON]/10,GPS_altitude,false); } // RESET HOME } else if (homeButtonCurrentState && (millis() - home_timer > 2000) && !PROTOCOL(TP_MFD)) { @@ -1031,14 +1041,14 @@ void updateSetHomeByGPS(void){ homeReset = false; home_timer_reset = 0; //if((!homeSet || (homeSet && homeSet_BY_GPS)) && feature(FEATURE_GPS) && STATE(GPS_FIX) && (GPS_numSat >= masterConfig.home_min_sats)) // || GPS_numSat>3)) - setHomeByLocalGps(&trackerPosition,GPS_coord[LAT]/10,GPS_coord[LON]/10,GPS_altitude,false,true); + setHomeByLocalGps(&trackerPosition,GPS_coord[LAT]/10,GPS_coord[LON]/10,GPS_altitude,false); if(masterConfig.gpsConfig.homeBeeper) beeper(BEEPER_ARMING_GPS_FIX); } else if(!homeSet && couldLolcalGpsSetHome(false)) { homeReset = true; home_timer_reset = 0; } else if(masterConfig.update_home_by_local_gps == 1 && homeSet && couldLolcalGpsSetHome(false)){ - setHomeByLocalGps(&trackerPosition,GPS_coord[LAT]/10,GPS_coord[LON]/10,GPS_altitude,true,false); + setHomeByLocalGps(&trackerPosition,GPS_coord[LAT]/10,GPS_coord[LON]/10,GPS_altitude,true); } } @@ -1058,7 +1068,7 @@ void updateMFD(void){ if (mfdTestMode || (homeSet && gotFix)) { targetPosition.distance = getDistance(); - targetPosition.alt = getTargetAlt(); + targetPosition.alt = getTargetAlt(targetPosition.home_alt); targetPosition.heading = getAzimuth() * 10; gotFix = false; } diff --git a/src/main/tracker/telemetry.c b/src/main/tracker/telemetry.c index 753ab74..e71954a 100755 --- a/src/main/tracker/telemetry.c +++ b/src/main/tracker/telemetry.c @@ -62,8 +62,8 @@ int32_t getTargetLon() { return telemetry_lon; } -int16_t getTargetAlt() { - return telemetry_alt; +int16_t getTargetAlt(int16_t home_alt) { + return telemetry_alt - home_alt; } uint16_t getSats() { diff --git a/src/main/tracker/telemetry.h b/src/main/tracker/telemetry.h index e98373f..1e4853d 100755 --- a/src/main/tracker/telemetry.h +++ b/src/main/tracker/telemetry.h @@ -48,7 +48,7 @@ extern float telemetry_yaw; extern uint8_t telemetry_failed_cs; extern uint8_t telemetry_provider; -int16_t getTargetAlt(); +int16_t getTargetAlt(int16_t home_alt); void encodeTargetData(uint8_t c); int32_t getTargetLat(); int32_t getTargetLon(); From 5aa8f26ec78a4970de7f786482e9b0d707a40410 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Ra=C3=BAl=20Ortega?= Date: Wed, 11 Apr 2018 20:54:40 +0200 Subject: [PATCH 3/6] Revert "Calculate tracker altitude and target altitude as relative." This reverts commit 534c9d9a2154fdae1e184c6aed50340475c59172. --- src/main/tracker/defines.h | 1 - src/main/tracker/main.c | 74 ++++++++++++++++-------------------- src/main/tracker/telemetry.c | 4 +- src/main/tracker/telemetry.h | 2 +- 4 files changed, 35 insertions(+), 46 deletions(-) diff --git a/src/main/tracker/defines.h b/src/main/tracker/defines.h index aac81ce..3ca51f7 100755 --- a/src/main/tracker/defines.h +++ b/src/main/tracker/defines.h @@ -38,7 +38,6 @@ typedef struct { int32_t lat; int32_t lon; int16_t alt; - int16_t home_alt; uint16_t heading; uint32_t distance; } diff --git a/src/main/tracker/main.c b/src/main/tracker/main.c index 03f466c..fb332fc 100755 --- a/src/main/tracker/main.c +++ b/src/main/tracker/main.c @@ -151,8 +151,7 @@ void updateProtocolDetection(void); void protocolInit(void); void trackingInit(void); void telemetryPortInit(void); -void setHomeByLocalGps(positionVector_t *tracker, int32_t lat, int32_t lon, int16_t alt, bool home_updated); -void setHomePossition(positionVector_t *tracker,bool byGPS,int32_t lat,int32_t lon,int16_t alt, bool home_updated); +void setHomeByLocalGps(positionVector_t *tracker, int32_t lat, int32_t lon, int16_t alt, bool home_updated, bool beep); //EASING int16_t _lastTilt; int16_t tilt; @@ -673,46 +672,37 @@ void updateDigitalButtons(void) { } void setHomeByTelemetry(positionVector_t *tracker, positionVector_t *target) { - setHomePossition(&tracker,false, target->lat, target->lon, target->alt,false); - setInitialEPSVector(target->lat,target->lon); - target->home_alt = target->alt; -} + tracker->lat = target->lat; + tracker->lon = target->lon; + tracker->alt = target->alt; -void setHomeByLocalGps(positionVector_t *tracker, int32_t lat, int32_t lon, int16_t alt, bool home_updated) { - setHomePossition(&tracker,true, lat, lon, alt, home_updated); - if(!home_updated) { - setInitialEPSVector(lat,lon); + if(feature(FEATURE_DEBUG)){ + tracker->lat = 47403583; tracker->lon = 8535850; tracker->alt = 474; } -} - -void setHomePossition(positionVector_t *tracker,bool byGPS,int32_t lat,int32_t lon,int16_t alt, bool home_updated){ - - tracker->lat = lat; - tracker->lon = lon; - - if(home_updated) - tracker->alt = alt - tracker->home_alt; - else - { - tracker->alt = 0; - tracker->home_alt = alt; - } - - if(feature(FEATURE_DEBUG)){ - tracker->lat = 47403583; tracker->lon = 8535850; tracker->alt = 474; - } - - homeSet = true; - homeSet = true; - homeSet_BY_GPS = byGPS; - homeReset = false; - home_timer_reset = 0; + homeSet = true; + homeSet_BY_GPS = 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()); } -void setInitialEPSVector(int32_t lat,int32_t lon){ - epsVectorLoad(&targetLast,lat,lon,0,0,0); - epsVectorLoad(&targetCurrent,lat,lon,0,0,millis()); +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; + if(feature(FEATURE_DEBUG)) { + tracker->lat = 47403583; tracker->lon = 8535850; tracker->alt = 474; + } + homeSet = true; + homeSet_BY_GPS = true; + homeReset = false; + home_timer_reset = 0; + if(!home_updated) { + epsVectorLoad(&targetLast,lat,lon,0,0,0); + epsVectorLoad(&targetCurrent,lat,lon,0,0,millis()); + } } void updateBatteryStatus(void){ @@ -811,7 +801,7 @@ void updateTargetPosition(void){ if(!PROTOCOL(TP_SERVOTEST)){ if (gotAlt) { - targetPosition.alt = getTargetAlt(targetPosition.home_alt); + targetPosition.alt = getTargetAlt(); if(PROTOCOL(TP_MFD)){ distance = getDistance(); @@ -993,7 +983,7 @@ void updateSetHomeButton(void){ setHomeByTelemetry(&trackerPosition, &targetPosition); // By local GPS because telemetry hasn't got enought sats and we don't want wait more time. else if(!homeSet && couldLolcalGpsSetHome(true)){ - setHomeByLocalGps(&trackerPosition,GPS_coord[LAT]/10,GPS_coord[LON]/10,GPS_altitude,false); + setHomeByLocalGps(&trackerPosition,GPS_coord[LAT]/10,GPS_coord[LON]/10,GPS_altitude,false,true); } // RESET HOME } else if (homeButtonCurrentState && (millis() - home_timer > 2000) && !PROTOCOL(TP_MFD)) { @@ -1041,14 +1031,14 @@ void updateSetHomeByGPS(void){ homeReset = false; home_timer_reset = 0; //if((!homeSet || (homeSet && homeSet_BY_GPS)) && feature(FEATURE_GPS) && STATE(GPS_FIX) && (GPS_numSat >= masterConfig.home_min_sats)) // || GPS_numSat>3)) - setHomeByLocalGps(&trackerPosition,GPS_coord[LAT]/10,GPS_coord[LON]/10,GPS_altitude,false); + setHomeByLocalGps(&trackerPosition,GPS_coord[LAT]/10,GPS_coord[LON]/10,GPS_altitude,false,true); if(masterConfig.gpsConfig.homeBeeper) beeper(BEEPER_ARMING_GPS_FIX); } else if(!homeSet && couldLolcalGpsSetHome(false)) { homeReset = true; home_timer_reset = 0; } else if(masterConfig.update_home_by_local_gps == 1 && homeSet && couldLolcalGpsSetHome(false)){ - setHomeByLocalGps(&trackerPosition,GPS_coord[LAT]/10,GPS_coord[LON]/10,GPS_altitude,true); + setHomeByLocalGps(&trackerPosition,GPS_coord[LAT]/10,GPS_coord[LON]/10,GPS_altitude,true,false); } } @@ -1068,7 +1058,7 @@ void updateMFD(void){ if (mfdTestMode || (homeSet && gotFix)) { targetPosition.distance = getDistance(); - targetPosition.alt = getTargetAlt(targetPosition.home_alt); + targetPosition.alt = getTargetAlt(); targetPosition.heading = getAzimuth() * 10; gotFix = false; } diff --git a/src/main/tracker/telemetry.c b/src/main/tracker/telemetry.c index e71954a..753ab74 100755 --- a/src/main/tracker/telemetry.c +++ b/src/main/tracker/telemetry.c @@ -62,8 +62,8 @@ int32_t getTargetLon() { return telemetry_lon; } -int16_t getTargetAlt(int16_t home_alt) { - return telemetry_alt - home_alt; +int16_t getTargetAlt() { + return telemetry_alt; } uint16_t getSats() { diff --git a/src/main/tracker/telemetry.h b/src/main/tracker/telemetry.h index 1e4853d..e98373f 100755 --- a/src/main/tracker/telemetry.h +++ b/src/main/tracker/telemetry.h @@ -48,7 +48,7 @@ extern float telemetry_yaw; extern uint8_t telemetry_failed_cs; extern uint8_t telemetry_provider; -int16_t getTargetAlt(int16_t home_alt); +int16_t getTargetAlt(); void encodeTargetData(uint8_t c); int32_t getTargetLat(); int32_t getTargetLon(); From 103ce7d202ae98b84444f7b240d0ca607b60c928 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Ra=C3=BAl=20Ortega?= Date: Wed, 11 Apr 2018 23:40:50 +0200 Subject: [PATCH 4/6] Tracker and target altitudes calculated as relative once the home button is pressed or home is set by gps. --- src/main/tracker/defines.h | 1 + src/main/tracker/main.c | 16 ++++++++++++++-- src/main/tracker/telemetry.c | 4 ++-- src/main/tracker/telemetry.h | 2 +- 4 files changed, 18 insertions(+), 5 deletions(-) diff --git a/src/main/tracker/defines.h b/src/main/tracker/defines.h index 3ca51f7..a23a73e 100755 --- a/src/main/tracker/defines.h +++ b/src/main/tracker/defines.h @@ -40,6 +40,7 @@ typedef struct { int16_t alt; uint16_t heading; uint32_t distance; + int16_t home_alt; } positionVector_t; diff --git a/src/main/tracker/main.c b/src/main/tracker/main.c index fb332fc..bd0df0c 100755 --- a/src/main/tracker/main.c +++ b/src/main/tracker/main.c @@ -680,6 +680,9 @@ void setHomeByTelemetry(positionVector_t *tracker, positionVector_t *target) { tracker->lat = 47403583; tracker->lon = 8535850; tracker->alt = 474; } + tracker->alt = 0; + tracker->home_alt = target->alt; + homeSet = true; homeSet_BY_GPS = false; homeReset = false; @@ -692,9 +695,18 @@ void setHomeByLocalGps(positionVector_t *tracker, int32_t lat, int32_t lon, int1 tracker->lat = lat; tracker->lon = lon; tracker->alt = alt; + if(feature(FEATURE_DEBUG)) { tracker->lat = 47403583; tracker->lon = 8535850; tracker->alt = 474; } + + if(home_updated) { + tracker->alt = alt - tracker->home_alt; + } else { + tracker->alt = 0; + tracker->home_alt = alt; + } + homeSet = true; homeSet_BY_GPS = true; homeReset = false; @@ -801,7 +813,7 @@ void updateTargetPosition(void){ if(!PROTOCOL(TP_SERVOTEST)){ if (gotAlt) { - targetPosition.alt = getTargetAlt(); + targetPosition.alt = getTargetAlt(targetPosition.home_alt); if(PROTOCOL(TP_MFD)){ distance = getDistance(); @@ -1058,7 +1070,7 @@ void updateMFD(void){ if (mfdTestMode || (homeSet && gotFix)) { targetPosition.distance = getDistance(); - targetPosition.alt = getTargetAlt(); + targetPosition.alt = getTargetAlt(targetPosition.home_alt); targetPosition.heading = getAzimuth() * 10; gotFix = false; } diff --git a/src/main/tracker/telemetry.c b/src/main/tracker/telemetry.c index 753ab74..e71954a 100755 --- a/src/main/tracker/telemetry.c +++ b/src/main/tracker/telemetry.c @@ -62,8 +62,8 @@ int32_t getTargetLon() { return telemetry_lon; } -int16_t getTargetAlt() { - return telemetry_alt; +int16_t getTargetAlt(int16_t home_alt) { + return telemetry_alt - home_alt; } uint16_t getSats() { diff --git a/src/main/tracker/telemetry.h b/src/main/tracker/telemetry.h index e98373f..1e4853d 100755 --- a/src/main/tracker/telemetry.h +++ b/src/main/tracker/telemetry.h @@ -48,7 +48,7 @@ extern float telemetry_yaw; extern uint8_t telemetry_failed_cs; extern uint8_t telemetry_provider; -int16_t getTargetAlt(); +int16_t getTargetAlt(int16_t home_alt); void encodeTargetData(uint8_t c); int32_t getTargetLat(); int32_t getTargetLon(); From 500ca55d3d9e8bfd88de6754b391b6567a8f05e0 Mon Sep 17 00:00:00 2001 From: raul-ortega Date: Thu, 12 Apr 2018 02:24:45 +0200 Subject: [PATCH 5/6] target altitude correction --- src/main/tracker/main.c | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/main/tracker/main.c b/src/main/tracker/main.c index bd0df0c..aa1223a 100755 --- a/src/main/tracker/main.c +++ b/src/main/tracker/main.c @@ -380,6 +380,8 @@ void trackingInit(void){ gotNewHeading = false; menuState = 0; + + targetPosition.home_alt = -32768; } void tracker_loop(void) @@ -707,6 +709,9 @@ void setHomeByLocalGps(positionVector_t *tracker, int32_t lat, int32_t lon, int1 tracker->home_alt = alt; } + if(targetPosition.home_alt == -32768) + targetPosition.alt = 0; + homeSet = true; homeSet_BY_GPS = true; homeReset = false; @@ -813,6 +818,9 @@ void updateTargetPosition(void){ if(!PROTOCOL(TP_SERVOTEST)){ if (gotAlt) { + if(telemetry_sats >= masterConfig.telemetry_min_sats && targetPosition.home_alt == -32768) + targetPosition.home_alt = getTargetAlt(0); + targetPosition.alt = getTargetAlt(targetPosition.home_alt); if(PROTOCOL(TP_MFD)){ @@ -1584,4 +1592,3 @@ void protocolInit(void){ break; } } - From e5e63372a60d881872d985329c3d4b87cb28515a Mon Sep 17 00:00:00 2001 From: raul-ortega Date: Fri, 13 Apr 2018 11:30:16 +0200 Subject: [PATCH 6/6] version bump --- src/main/version.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/version.h b/src/main/version.h index e371a1f..dac5574 100755 --- a/src/main/version.h +++ b/src/main/version.h @@ -16,8 +16,8 @@ */ #define FC_VERSION_MAJOR 9 // increment when a major release is made (big new feature, etc) -#define FC_VERSION_MINOR 2 // increment when a minor release is made (small new feature, change etc) -#define FC_VERSION_PATCH_LEVEL 4 // increment when a bug is fixed +#define FC_VERSION_MINOR 3 // 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 STR_HELPER(x) #x #define STR(x) STR_HELPER(x)