diff --git a/main/ctrl.cpp b/main/ctrl.cpp index 46938ce..ded8095 100644 --- a/main/ctrl.cpp +++ b/main/ctrl.cpp @@ -175,7 +175,7 @@ static void ProcessCtrlC(void) // print system Format_UnsDec(CONS_UART_Write, GPS_getBaudRate(), 1); Format_String(CONS_UART_Write, "bps"); CONS_UART_Write(','); - Format_UnsDec(CONS_UART_Write, GPS_PosPeriod, 3, 2); + Format_UnsDec(CONS_UART_Write, GPS_PosPeriod, 4, 3); CONS_UART_Write('s'); if(GPS_Status.PPS) Format_String(CONS_UART_Write, ",PPS"); if(GPS_Status.NMEA) Format_String(CONS_UART_Write, ",NMEA"); diff --git a/main/disp_lcd.cpp b/main/disp_lcd.cpp index 5168252..8fc8fe3 100644 --- a/main/disp_lcd.cpp +++ b/main/disp_lcd.cpp @@ -65,7 +65,7 @@ static void LCD_UpdateTime(uint32_t Time, GPS_Position *GPS, bool Redraw=0) if(GPS && GPS->isTimeValid()) { if(GPS->isDateValid()) { Time=GPS->getUnixTime(); Back[Idx]=RGB565_GREEN; } else { Time=GPS->getDayTime(); Back[Idx]=RGB565_GREENYELLOW; } - if(GPS->FracSec>=50) Time++; } + if(GPS->mSec>=500) Time++; } uint8_t Len=Format_HHcMMcSS(Msg[Idx], Time); Msg[Idx][Len]=0; bool Redo = Redraw || Back[Idx]!=Back[Idx^1]; if(Redo) LCD_DrawString(Msg[Idx], LCD_WIDTH-LCD_StringWidth(Msg[Idx])-4, LCD_HEIGHT-LCD_FontHeight(), RGB565_BLACK, Back[Idx]); diff --git a/main/gps.cpp b/main/gps.cpp index 969790e..796ba07 100644 --- a/main/gps.cpp +++ b/main/gps.cpp @@ -41,7 +41,7 @@ static UBX_RxMsg UBX; // UBX messages catcher static MAV_RxMsg MAV; // MAVlink message catcher #endif -uint16_t GPS_PosPeriod = 0; // [0.01s] time between succecive GPS readouts +uint16_t GPS_PosPeriod = 0; // [mss] time between succecive GPS readouts // uint8_t GPS_PowerMode = 2; // 0=shutdown, 1=reduced power, 2=normal @@ -455,9 +455,8 @@ static void GPS_BurstComplete(void) // wh { uint32_t UnixTime=GPS_Pos[GPS_PosIdx].getUnixTime(); GPS_FatTime=GPS_Pos[GPS_PosIdx].getFatTime(); #ifndef WITH_MAVLINK // with MAVlink we sync. with the SYSTEM_TIME message - int32_t msDiff = GPS_Pos[GPS_PosIdx].FracSec; - if(msDiff>=50) { msDiff-=100; UnixTime++; } // [0.01s] - msDiff*=10; // [ms] + int32_t msDiff = GPS_Pos[GPS_PosIdx].mSec; + if(msDiff>=500) { msDiff-=1000; UnixTime++; } // [ms] TimeSync_SoftPPS(Burst_Tick, UnixTime, msDiff+Parameters.PPSdelay); // if(abs(msDiff)<=200) // if (almost) full-second burst // { // TickType_t PPS_Age = Burst_Tick-PPS_Tick; @@ -486,7 +485,7 @@ static void GPS_BurstComplete(void) // wh { uint8_t PrevIdx=(GPS_PosIdx+PosPipeIdxMask)&PosPipeIdxMask; // previous GPS data int16_t TimeDiff = GPS_Pos[GPS_PosIdx].calcTimeDiff(GPS_Pos[PrevIdx]); // difference in time for( ; ; ) // loop - { if(TimeDiff>=95) break; // if at least 0.95sec then enough to calc. the differentials + { if(TimeDiff>=950) break; // if at least 0.950sec then enough to calc. the differentials uint8_t PrevIdx2=(PrevIdx+PosPipeIdxMask)&PosPipeIdxMask; // go back one GPS position if(PrevIdx2==GPS_PosIdx) break; // if we looped all the way back: stop if(!GPS_Pos[PrevIdx2].isValid()) break; // if GPS position not valid: stop @@ -500,7 +499,7 @@ static void GPS_BurstComplete(void) // wh Format_String(CONS_UART_Write, "->"); Format_UnsDec(CONS_UART_Write, (uint16_t)PrevIdx); CONS_UART_Write(' '); - Format_SignDec(CONS_UART_Write, TimeDiff, 3, 2); + Format_SignDec(CONS_UART_Write, TimeDiff, 4, 3); Format_String(CONS_UART_Write, "s\n"); xSemaphoreGive(CONS_Mutex); #endif @@ -522,7 +521,7 @@ static void GPS_BurstComplete(void) // wh } uint8_t NextPosIdx = (GPS_PosIdx+1)&PosPipeIdxMask; // next position to be recorded if( GPS_Pos[GPS_PosIdx].isTimeValid() && GPS_Pos[NextPosIdx].isTimeValid() ) - { int32_t Period = GPS_Pos[GPS_PosIdx].calcTimeDiff(GPS_Pos[NextPosIdx]); // [1/100sec] + { int32_t Period = GPS_Pos[GPS_PosIdx].calcTimeDiff(GPS_Pos[NextPosIdx]); // [msec] if(Period>0) GPS_PosPeriod = (Period+GPS_PosPipeSize/2)/(GPS_PosPipeSize-1); #ifdef DEBUG_PRINT xSemaphoreTake(CONS_Mutex, portMAX_DELAY); @@ -530,19 +529,16 @@ static void GPS_BurstComplete(void) // wh CONS_UART_Write('0'+GPS_PosIdx); CONS_UART_Write(']'); CONS_UART_Write(' '); Format_UnsDec(CONS_UART_Write, (uint16_t)GPS_Pos[GPS_PosIdx].Sec, 2); CONS_UART_Write('.'); - Format_UnsDec(CONS_UART_Write, (uint16_t)GPS_Pos[GPS_PosIdx].FracSec, 2); + Format_UnsDec(CONS_UART_Write, (uint16_t)GPS_Pos[GPS_PosIdx].mSec, 3); Format_String(CONS_UART_Write, "s "); - Format_UnsDec(CONS_UART_Write, GPS_PosPeriod, 3, 2); + Format_SignDec(CONS_UART_Write, Period, 4, 3); Format_String(CONS_UART_Write, "s\n"); xSemaphoreGive(CONS_Mutex); #endif } - GPS_Pos[NextPosIdx].Clear(); // clear the next position - // int8_t Sec = GPS_Pos[GPS_PosIdx].Sec; // - // Sec++; if(Sec>=60) Sec=0; - // GPS_Pos[NextPosIdx].Sec=Sec; // set the correct time for the next position + GPS_Pos[NextPosIdx].Clear(); // clear the next position GPS_Pos[NextPosIdx].copyTime(GPS_Pos[GPS_PosIdx]); // copy time from current position - GPS_Pos[NextPosIdx].incrTimeFrac(GPS_PosPeriod); // increment time by the expected period + GPS_Pos[NextPosIdx].incrTimeFrac(GPS_PosPeriod); // increment time by the expected period Flight.Process(GPS_Pos[GPS_PosIdx]); // GPS_Pos[NextPosIdx].copyDate(GPS_Pos[GPS_PosIdx]); #ifdef DEBUG_PRINT @@ -550,9 +546,9 @@ static void GPS_BurstComplete(void) // wh Format_String(CONS_UART_Write, "GPS -> "); Format_UnsDec(CONS_UART_Write, (uint16_t)GPS_Pos[NextPosIdx].Sec, 2); CONS_UART_Write('.'); - Format_UnsDec(CONS_UART_Write, (uint16_t)GPS_Pos[NextPosIdx].FracSec, 2); + Format_UnsDec(CONS_UART_Write, (uint16_t)GPS_Pos[NextPosIdx].mSec, 3); Format_String(CONS_UART_Write, "s "); - Format_UnsDec(CONS_UART_Write, GPS_PosPeriod, 3, 2); + Format_UnsDec(CONS_UART_Write, GPS_PosPeriod, 4, 3); Format_String(CONS_UART_Write, "s\n"); xSemaphoreGive(CONS_Mutex); #endif @@ -565,15 +561,15 @@ static void GPS_BurstEnd(void) // wh // ---------------------------------------------------------------------------- -GPS_Position *GPS_getPosition(uint8_t &BestIdx, int16_t &BestRes, int8_t Sec, int8_t Frac, bool Ready) // return GPS position closest to the given Sec.Frac -{ int16_t TargetTime = Frac+(int16_t)Sec*100; // target time including the seconds +GPS_Position *GPS_getPosition(uint8_t &BestIdx, int16_t &BestRes, int8_t Sec, int16_t Frac, bool Ready) // return GPS position closest to the given Sec.Frac +{ int32_t TargetTime = Frac+(int32_t)Sec*1000; // target time including the seconds BestIdx=0; BestRes=0x7FFF; for(uint8_t Idx=0; IdxisReady) continue; // if only Ready positions requested: skip those not-ready yet - int16_t Diff = TargetTime - (Pos->FracSec + (int16_t)Pos->Sec*100); // difference from the target time - if(Diff<(-3000)) Diff+=6000; // wrap-around 60 sec - else if(Diff>=3000) Diff-=6000; + int32_t Diff = TargetTime - (Pos->mSec + (int32_t)Pos->Sec*1000); // difference from the target time + if(Diff<(-30000)) Diff+=60000; // wrap-around 60 sec + else if(Diff>=30000) Diff-=60000; if(abs(Diff)=50) { PosSec++; if(PosSec>=60) PosSec-=60; } + { int8_t PosSec = GPS_Pos[Idx].Sec; if(GPS_Pos[Idx].mSec>=500) { PosSec++; if(PosSec>=60) PosSec-=60; } if(Sec==PosSec) return GPS_Pos+Idx; } return 0; } diff --git a/main/gps.h b/main/gps.h index 9e3782c..54a31a5 100644 --- a/main/gps.h +++ b/main/gps.h @@ -25,7 +25,7 @@ extern int16_t GPS_GeoidSepar; // [0.1m] extern uint16_t GPS_LatCosine; // [1.0/(1<<12)] Latitude Cosine for distance calculations extern uint32_t GPS_TimeSinceLock; // [sec] time since GPS has a valid lock extern uint32_t GPS_Random; // random number produced from the GPS data -extern uint16_t GPS_PosPeriod; // [0.01sec] how often (which period) the GPS/MAV is sending the positions +extern uint16_t GPS_PosPeriod; // [msec] how often (which period) the GPS/MAV is sending the positions extern uint16_t GPS_SatSNR; // [0.25dB] average SNR for satellites being used for navigation typedef union @@ -48,7 +48,7 @@ uint32_t GPS_getBaudRate(void); // [bps] GPS_Position *GPS_getPosition(void); GPS_Position *GPS_getPosition(int8_t Sec); // return GPS position for given Sec -GPS_Position *GPS_getPosition(uint8_t &BestIdx, int16_t &BestRes, int8_t Sec, int8_t Frac, bool Ready=1); // return GPS position closest to the given Sec.Frac +GPS_Position *GPS_getPosition(uint8_t &BestIdx, int16_t &BestRes, int8_t Sec, int16_t Frac, bool Ready=1); // return GPS position closest to the given Sec.Frac int16_t GPS_AverageSpeed(void); // [0.1m/s] calc. average speed based on most recent GPS positions diff --git a/main/http.cpp b/main/http.cpp index 651b377..bdc7aa9 100644 --- a/main/http.cpp +++ b/main/http.cpp @@ -280,7 +280,7 @@ static void Table_GPS(httpd_req_t *Req) Len+=Format_UnsDec(Line+Len, GPS->Hour , 2); Line[Len++]=':'; Len+=Format_UnsDec(Line+Len, GPS->Min , 2); Line[Len++]=':'; Len+=Format_UnsDec(Line+Len, GPS->Sec , 2); Line[Len++]='.'; - Len+=Format_UnsDec(Line+Len, GPS->FracSec, 2); + Len+=Format_UnsDec(Line+Len, GPS->mSec, 3); Len+=Format_String(Line+Len, "\n"); httpd_resp_send_chunk(Req, Line, Len); diff --git a/main/ogn.h b/main/ogn.h index bc55eb3..6af6b2f 100644 --- a/main/ogn.h +++ b/main/ogn.h @@ -652,13 +652,14 @@ class GPS_Time { public: int8_t Year, Month, Day; // Date (UTC) from GPS int8_t Hour, Min, Sec; // Time-of-day (UTC) from GPS - int8_t FracSec; // [1/100 sec] some GPS-es give second fraction with the time-of-day - int8_t Spare; + int16_t mSec; // [ms] + // int8_t FracSec; // [1/100 sec] some GPS-es give second fraction with the time-of-day + // int8_t Spare; public: void setDefaultDate() { Year=00; Month=1; Day=1; } // default Date is 01-JAN-2000 - void setDefaultTime() { Hour=0; Min=0; Sec=0; FracSec=0; } // default Time is 00:00:00.00 + void setDefaultTime() { Hour=0; Min=0; Sec=0; mSec=0; } // default Time is 00:00:00.00 bool isTimeValid(void) const // is the GPS time-of-day valid { return (Hour>=0) && (Min>=0) && (Sec>=0); } // all data must have been correctly read: negative means not correctly read) @@ -666,10 +667,11 @@ class GPS_Time bool isDateValid(void) const // is the GPS date valid ? { return (Year>=0) && (Month>=0) && (Day>=0); } - void incrTimeFrac(int8_t Frac) - { if(Frac>=100) { incrTime(); Frac-=100; } - if(Frac>=100-FracSec) { incrTime(); Frac-=100; } - FracSec+=Frac; } + void incrTimeFrac(int16_t msFrac) // [ms] + { mSec+=msFrac; + if(mSec>=1000) { incrTime(); mSec-=1000; } + else if(mSec<0) { decrTime(); mSec+=1000; } + } uint8_t incrTime(void) // increment HH:MM:SS by one second { Sec++; if(Sec<60) return 0; @@ -689,11 +691,11 @@ class GPS_Time Hour=24; return 1; } // return 1 if date needs to be decremented - int32_t calcTimeDiff(GPS_Time &RefTime) const - { int32_t TimeDiff = ((int32_t)Min*6000+(int16_t)Sec*100+FracSec) - ((int32_t)RefTime.Min*6000+(int16_t)RefTime.Sec*100+RefTime.FracSec); - if(TimeDiff<(-180000)) TimeDiff+=360000; // wrap-around 60min - else if(TimeDiff>=180000) TimeDiff-=360000; - return TimeDiff; } // [0.01s] + int32_t calcTimeDiff(const GPS_Time &RefTime) const + { int32_t TimeDiff = msDayTime() - RefTime.msDayTime(); // [ms] + if(TimeDiff<(-(int32_t)mSecsPerDay/2)) TimeDiff+=mSecsPerDay; // wrap-around one day + else if(TimeDiff>= (int32_t)mSecsPerDay/2 ) TimeDiff-=mSecsPerDay; + return TimeDiff; } // [ms] uint8_t MonthDays(void) // number of days per month { const uint16_t Table = 0x0AD5; // 1010 1101 0101 0=30days, 1=31days @@ -717,7 +719,7 @@ class GPS_Time void decrTimeDate(void) { if(decrTime()) decrDate(); } void copyTime(GPS_Time &RefTime) // copy HH:MM:SS.SSS from another record - { FracSec = RefTime.FracSec; + { mSec = RefTime.mSec; Sec = RefTime.Sec; Min = RefTime.Min; Hour = RefTime.Hour; } @@ -728,8 +730,12 @@ class GPS_Time Year = RefTime.Year; } void copyTimeDate(GPS_Time &RefTime) { copyTime(RefTime); copyDate(RefTime); } - uint32_t getDayTime(void) const - { return Times60((uint32_t)(Times60((uint16_t)Hour) + Min)) + Sec; } // this appears to save about 100 bytes of code + + uint32_t msDayTime(void) const // [ms] + { return getDayTime()*1000+mSec; } + + uint32_t getDayTime(void) const // [sec] time within the day + { return Times60((uint32_t)(Times60((uint16_t)Hour) + Min)) + Sec; } // this appears to save about 100 bytes of code on STM32 // return (uint32_t)Hour*SecsPerHour + (uint16_t)Min*SecsPerMin + Sec; } // compared to this line uint32_t getUnixTime(void) const // return the Unix timestamp (tested 2000-2037) @@ -747,30 +753,29 @@ class GPS_Time Hour = DayTime/SecsPerHour; DayTime -= (uint32_t)Hour*SecsPerHour; // Min = DayTime/SecsPerMin; DayTime -= (uint16_t)Min*SecsPerMin; Sec = DayTime; - FracSec=0; + mSec=0; Days -= 10957+1; // [day] since 2000 minus 1 day Year = (Days*4)/((365*4)+1); // [year] since 1970 Days -= 365*Year + (Year/4); Month = Days/31; Day = Days-(uint16_t)Month*31+1; Month++; uint32_t CheckTime = getUnixTime(); - if(CheckTimemin; Sec = TIMEUTC->sec; if(TIMEUTC->nano<0) { decrTimeDate(); TIMEUTC->nano+=1000000000; } - FracSec = (TIMEUTC->nano+5000000)/10000000; // [ms] - if(FracSec>=100) { incrTimeDate(); FracSec-=100; } + mSec = (TIMEUTC->nano+500000)/1000000; // [ms] + if(mSec>=1000) { incrTimeDate(); mSec-=1000; } hasTime = (TIMEUTC->valid&0x02)!=0; return hasTime; } @@ -1035,7 +1040,7 @@ class GPS_Position: public GPS_Time Len+=Format_UnsDec(GGA+Len, (uint16_t)Min, 2); Len+=Format_UnsDec(GGA+Len, (uint16_t)Sec, 2); GGA[Len++]='.'; - Len+=Format_UnsDec(GGA+Len, (uint16_t)FracSec, 2); + Len+=Format_UnsDec(GGA+Len, (uint16_t)mSec, 3); GGA[Len++]=','; Len+=Format_Latitude(GGA+Len, Latitude); GGA[Len]=GGA[Len-1]; GGA[Len-1]=','; Len++; @@ -1137,36 +1142,36 @@ class GPS_Position: public GPS_Time int16_t calcDifferentials(GPS_Position &RefPos) // calculate climb rate and turn rate with an earlier reference position { ClimbRate=0; TurnRate=0; if(RefPos.FixQuality==0) return 0; - int16_t TimeDiff = calcTimeDiff(RefPos); - if(TimeDiff<5) return 0; + int16_t TimeDiff = calcTimeDiff(RefPos); // [ms] + if(TimeDiff<10) return 0; TurnRate = Heading-RefPos.Heading; if(TurnRate>1800) TurnRate-=3600; else if(TurnRate<(-1800)) TurnRate+=3600; ClimbRate = Altitude-RefPos.Altitude; if(hasBaro && RefPos.hasBaro && (abs(Altitude-StdAltitude)<2500) ) { ClimbRate = StdAltitude-RefPos.StdAltitude; } Accel = Speed-RefPos.Speed; - if(TimeDiff==20) + if(TimeDiff==200) { ClimbRate*=5; TurnRate *=5; Accel *=5; } - else if(TimeDiff==50) + else if(TimeDiff==500) { ClimbRate*=2; TurnRate *=2; Accel *=2; } - else if(TimeDiff==100) + else if(TimeDiff==1000) { } - else if(TimeDiff==200) + else if(TimeDiff==2000) { ClimbRate=(ClimbRate+1)>>1; TurnRate=( TurnRate+1)>>1; Accel =( Accel +1)>>1; } else if(TimeDiff!=0) - { ClimbRate = ((int32_t)ClimbRate*100)/TimeDiff; - TurnRate = ((int32_t) TurnRate*100)/TimeDiff; - Accel = ((int32_t) Accel *100)/TimeDiff; } - return TimeDiff; } // [0.01s] + { ClimbRate = ((int32_t)ClimbRate*1000)/TimeDiff; + TurnRate = ((int32_t) TurnRate*1000)/TimeDiff; + Accel = ((int32_t) Accel *1000)/TimeDiff; } + return TimeDiff; } // [ms] void Write(MAV_GPS_RAW_INT *MAV) const - { MAV->time_usec = (int64_t)1000000*getUnixTime()+10000*FracSec; + { MAV->time_usec = (int64_t)1000000*getUnixTime()+1000*mSec; // [usec] MAV->lat = ((int64_t)50*Latitude+1)/3; MAV->lon = ((int64_t)50*Longitude+1)/3; MAV->alt = 100*Altitude; @@ -1253,7 +1258,7 @@ class GPS_Position: public GPS_Time if(PDOP>0) Packet.EncodeDOP(PDOP-10); // encode PDOP from GSA else Packet.EncodeDOP(HDOP-10); // or if no GSA: use HDOP int8_t ShortTime=Sec; // the 6-bit time field in the OGN packet - if(FracSec>=50) { ShortTime+=1; if(ShortTime>=60) ShortTime-=60; } // round to the closest full second + if(mSec>=500) { ShortTime+=1; if(ShortTime>=60) ShortTime-=60; } // round to the closest full second Packet.Position.Time=ShortTime; // Time Packet.EncodeLatitude(Latitude); // Latitude Packet.EncodeLongitude(Longitude); // Longitude @@ -1288,7 +1293,7 @@ class GPS_Position: public GPS_Time void EncodeStatus(OGNx_Packet &Packet) const { Packet.Status.ReportType=0; int ShortTime=Sec; - if(FracSec>=50) { ShortTime+=1; if(ShortTime>=60) ShortTime-=60; } + if(mSec>=500) { ShortTime+=1; if(ShortTime>=60) ShortTime-=60; } Packet.Status.Time=ShortTime; Packet.Status.FixQuality = FixQuality<3 ? FixQuality:3; Packet.Status.Satellites = Satellites<15 ? Satellites:15; @@ -1315,7 +1320,7 @@ class GPS_Position: public GPS_Time FixMode = Packet.Position.FixMode+2; PDOP = 10+Packet.DecodeDOP(); HDOP = PDOP; VDOP = PDOP+PDOP/2; - FracSec=0; Sec=Packet.Position.Time; + mSec=0; Sec=Packet.Position.Time; Speed = Packet.DecodeSpeed(); ClimbRate = Packet.DecodeClimbRate(); TurnRate = Packet.DecodeTurnRate(); @@ -1339,9 +1344,9 @@ class GPS_Position: public GPS_Time int32_t Lat, Lon, Alt; int16_t Head; calcExtrapolation(Lat, Lon, Alt, Head, dTime); int16_t ShortTime=Sec; // the 6-bit time field in the OGN packet - dTime += FracSec; - while(dTime>= 50 ) { dTime-=100; ShortTime++; if(ShortTime>=60) ShortTime-=60; } - while(dTime<(-50)) { dTime+=100; ShortTime--; if(ShortTime< 0) ShortTime+=60; } + dTime += mSec; + while(dTime>= 500 ) { dTime-=1000; ShortTime++; if(ShortTime>=60) ShortTime-=60; } + while(dTime<(-500)) { dTime+=1000; ShortTime--; if(ShortTime< 0) ShortTime+=60; } Packet.Position.Time=ShortTime; // Time Packet.EncodeLatitude(Lat); // Latitude Packet.EncodeLongitude(Lon); // Longitude @@ -1354,55 +1359,55 @@ class GPS_Position: public GPS_Time else Packet.clrBaro(); //or no-baro if pressure sensor data not there } - void Extrapolate(int32_t dTime) // [0.01sec] extrapolate the position by dTime - { int16_t dSpeed = ((int32_t)Accel*dTime)/100; + void Extrapolate(int32_t dTime) // [ms] extrapolate the position by dTime + { int16_t dSpeed = ((int32_t)Accel*dTime)/1000; Speed += dSpeed/2; int16_t HeadAngle = ((int32_t)Heading<<12)/225; // [cordic] heading angle - int16_t TurnAngle = (((dTime*TurnRate)/25)<<9)/225; // [cordic] + int16_t TurnAngle = (((dTime*TurnRate)/250)<<9)/225; // [cordic] HeadAngle += TurnAngle/2; int32_t LatSpeed = ((int32_t)Speed*Icos(HeadAngle)+0x800)>>12; // [0.1m/s] int32_t LonSpeed = ((int32_t)Speed*Isin(HeadAngle)+0x800)>>12; // [0.1m/s] HeadAngle += TurnAngle-TurnAngle/2; Speed += dSpeed-dSpeed/2; if(Speed<0) Speed=0; - Latitude += calcLatitudeExtrapolation (dTime, LatSpeed); - Longitude += calcLongitudeExtrapolation(dTime, LonSpeed); + Latitude += calcLatitudeExtrapolation (dTime, LatSpeed); // + Longitude += calcLongitudeExtrapolation(dTime, LonSpeed); // int32_t dAlt = calcAltitudeExtrapolation(dTime); // [0.1m] Altitude += dAlt; // [0.1m] if(hasBaro) { StdAltitude += dAlt; // [0.1m] Pressure += 4000*dAlt/Atmosphere::PressureLapseRate(Pressure/4, Temperature); } // [0.25Pa] ([Pa], [0.1degC]) - Heading += (dTime*TurnRate)/100; // [0.1deg] + Heading += (dTime*TurnRate)/1000; // [0.1deg] if(Heading<0) Heading+=3600; else if(Heading>=3600) Heading-=3600; // [0.1deg] - int16_t fTime = FracSec+dTime; // [0.01sec] - while(fTime>=100) { incrTimeDate(); fTime-=100; } - while(fTime< 0) { decrTimeDate(); fTime+=100; } - FracSec=fTime; } + int16_t fTime = mSec+dTime; // [msec] + while(fTime>=1000) { incrTimeDate(); fTime-=1000; } + while(fTime< 0) { decrTimeDate(); fTime+=1000; } + mSec=fTime; } // extrapolate GPS position by a fraction of a second - void calcExtrapolation(int32_t &Lat, int32_t &Lon, int32_t &Alt, int16_t &Head, int32_t dTime) const // [0.01sec] + void calcExtrapolation(int32_t &Lat, int32_t &Lon, int32_t &Alt, int16_t &Head, int32_t dTime) const // [msec] { int16_t HeadAngle = ((int32_t)Heading<<12)/225; // [] - int16_t TurnAngle = (((dTime*TurnRate)/25)<<9)/225; // [] + int16_t TurnAngle = (((dTime*TurnRate)/250)<<9)/225; // [] HeadAngle += TurnAngle; int32_t LatSpeed = ((int32_t)Speed*Icos(HeadAngle)+0x800)>>12; // [0.1m/s] int32_t LonSpeed = ((int32_t)Speed*Isin(HeadAngle)+0x800)>>12; // [0.1m/s] Lat = Latitude + calcLatitudeExtrapolation (dTime, LatSpeed); Lon = Longitude + calcLongitudeExtrapolation(dTime, LonSpeed); Alt = Altitude + calcAltitudeExtrapolation(dTime); - Head = Heading + (dTime*TurnRate)/100; + Head = Heading + (dTime*TurnRate)/1000; if(Head<0) Head+=3600; else if(Head>=3600) Head-=3600; } - int32_t calcAltitudeExtrapolation(int32_t Time) const // [0.01s] - { return Time*ClimbRate/100; } // [0.1m] + int32_t calcAltitudeExtrapolation(int32_t Time) const // [ms] + { return Time*ClimbRate/1000; } // [0.1m] - int32_t calcLatitudeExtrapolation(int32_t Time, int32_t LatSpeed) const // [0.01s] [0.1m/s] - { return (Time*LatSpeed*177+0x4000)>>15; } // [0.1m] + int32_t calcLatitudeExtrapolation(int32_t Time, int32_t LatSpeed) const // [ms] [0.1m/s] + { return (Time/10*LatSpeed*177+0x4000)>>15; } // [0.1m] - int32_t calcLongitudeExtrapolation(int32_t Time, int32_t LonSpeed) const // [0.01s] + int32_t calcLongitudeExtrapolation(int32_t Time, int32_t LonSpeed) const // [ms] { int16_t LatCosine = calcLatCosine(calcLatAngle16(Latitude)); return calcLongitudeExtrapolation(Time, LonSpeed, LatCosine); } - int32_t calcLongitudeExtrapolation(int32_t Time, int32_t LonSpeed, int16_t LatCosine) const // [0.01s] - { return ((((int32_t)Time*LonSpeed*177+4)>>3))/LatCosine; } + int32_t calcLongitudeExtrapolation(int32_t Time, int32_t LonSpeed, int16_t LatCosine) const // [ms] + { return (((Time/10*LonSpeed*177+4)>>3))/LatCosine; } // static int32_t calcLatDistance(int32_t Lat1, int32_t Lat2) // [m] distance along latitude // { return ((int64_t)(Lat2-Lat1)*0x2f684bda+0x80000000)>>32; } @@ -1577,7 +1582,7 @@ class GPS_Position: public GPS_Time else if(DOP>255) DOP=255; VDOP=DOP; return 0; } - int8_t ReadTime(const char *Value) // read the Time field: HHMMSS.ss and check if it is a new one or the same one + int8_t ReadTime(const char *Value) // read the Time field: HHMMSS.sss and check if it is a new one or the same one { int8_t Prev; int8_t Same=1; Prev=Hour; Hour=Read_Dec2(Value); if(Hour<0) return -1; // read hour (two digits), return when invalid @@ -1588,10 +1593,13 @@ class GPS_Position: public GPS_Time Prev=Sec; Sec=Read_Dec2(Value+4); if(Sec<0) return -1; // read second (two digits), return when invalid if(Prev!=Sec) Same=0; - Prev=FracSec; + int16_t mPrev = mSec; if(Value[6]=='.') // is there a fraction - { FracSec=Read_Dec2(Value+7); if(FracSec<0) return -1; } // read the fraction, return when invalid - if(Prev!=FracSec) Same=0; // return 0 when time is valid but did not change + { uint16_t Frac=0; int8_t Len=Read_UnsDec(Frac, Value+7); if(Len<1) return -1; // read the fraction, return when invalid + if(Len==1) mSec = Frac*100; + else if(Len==2) mSec = Frac*10; + else if(Len==3) mSec = Frac; } + if(mPrev!=mSec) Same=0; // return 0 when time is valid but did not change return Same; } // return 1 when time did not change (both RMC and GGA were for same time) int8_t ReadDate(const char *Param) // read the field DDMMYY diff --git a/main/proc.cpp b/main/proc.cpp index 399f8ed..9e51ed8 100644 --- a/main/proc.cpp +++ b/main/proc.cpp @@ -513,7 +513,7 @@ void vTaskPROC(void* pvParameters) if(Position) { Format_UnsDec(CONS_UART_Write, (uint16_t)BestIdx); CONS_UART_Write(':'); - Format_SignDec(CONS_UART_Write, BestResid, 3, 2); + Format_SignDec(CONS_UART_Write, BestResid, 4, 3); Format_String(CONS_UART_Write, "s"); } Format_String(CONS_UART_Write, "\n"); xSemaphoreGive(CONS_Mutex); @@ -578,7 +578,7 @@ void vTaskPROC(void* pvParameters) PosPacket.Packet.calcAddrParity(); // parity of (part of) the header if(BestResid==0) Position->Encode(PosPacket.Packet); // encode position/altitude/speed/etc. from GPS position else // extrapolate the position when if not at an exact UTC second - { while(BestResid>=50) BestResid-=100; // remove full seconds + { while(BestResid>=500) BestResid-=1000; // remove full seconds Position->Encode(PosPacket.Packet, BestResid); } PosPacket.Packet.Position.AcftType = Parameters.AcftType; // aircraft-type PosPacket.Packet.Position.Stealth = 0; // Parameters.Stealth; diff --git a/main/sens.cpp b/main/sens.cpp index a81c3f3..8e80712 100644 --- a/main/sens.cpp +++ b/main/sens.cpp @@ -204,8 +204,8 @@ static void ProcBaro(void) // if(Frac==0) { // GPS_Position *PosPtr = GPS_getPosition(Sec/10); // get GPS position record for this second uint8_t BestIdx; int16_t BestRes; - GPS_Position *PosPtr = GPS_getPosition(BestIdx, BestRes, Sec/10, Frac*10, 0); -// #ifdef DEBUG_PRINT + GPS_Position *PosPtr = GPS_getPosition(BestIdx, BestRes, Sec/10, (int16_t)100*Frac, 0); +#ifdef DEBUG_PRINT xSemaphoreTake(CONS_Mutex, portMAX_DELAY); Format_String(CONS_UART_Write, "ProcBaro: "); Format_UnsDec(CONS_UART_Write, (uint16_t)Sec, 3, 1); @@ -213,14 +213,14 @@ static void ProcBaro(void) if(PosPtr) { Format_UnsDec(CONS_UART_Write, (uint16_t)PosPtr->Sec, 2); CONS_UART_Write('.'); - Format_UnsDec(CONS_UART_Write, (uint16_t)PosPtr->FracSec, 2); + Format_UnsDec(CONS_UART_Write, (uint16_t)PosPtr->mSec, 3); CONS_UART_Write('s'); } Format_String(CONS_UART_Write, " => "); - Format_SignDec(CONS_UART_Write, BestRes, 3, 2); - Format_String(CONS_UART_Write, "\n"); + Format_SignDec(CONS_UART_Write, BestRes, 4, 3); + Format_String(CONS_UART_Write, "s\n"); xSemaphoreGive(CONS_Mutex); -// #endif - if(PosPtr && abs(BestRes)<=20) // if found and small time error +#endif + if(PosPtr && abs(BestRes)<=250) // if found and small time error { PosPtr->Pressure = Pressure; // [0.25Pa] PosPtr->StdAltitude = StdAltitude; // store standard pressure altitude PosPtr->ClimbRate = ClimbRate/10; // [0.1m/s] diff --git a/main/ubx.h b/main/ubx.h index 2b89927..0c38912 100644 --- a/main/ubx.h +++ b/main/ubx.h @@ -196,8 +196,8 @@ class UBX_CFG_SBAS // 0x06 0x16 uint8_t maxSBAS; // number of channels: 0..3 uint8_t scanmode2; // #0 = PRN152, ... #6 = PRN158 uint32_t scanmode1; // #0 = PRN120, ... #31 = PRN151 -} ; // when polled from M6: 01 03 03 00 51 62 06 00 - // for auto-select mode: 01 07 03 00 00 00 00 00 +} ; // when polled from M6: 01 03 03 00 51 62 06 00 => 129, 130, 137, 141, 142, 144, 148, 150 + // for auto-select mode: 01 07 03 00 00 00 00 00 => in UE it finds and uses: 123, 125, 136 // for GPS: 0x00, 0x08, 0x10, 0x00, 0x01, 0x00, 0x01, 0x01 (enable) // for SBAS: 0x01, 0x02, 0x03, 0x00, 0x01, 0x00, 0x01, 0x01 (enable)