kopia lustrzana https://github.com/pjalocha/esp32-ogn-tracker
Correct the unique ID, add some string-valued parameters
Pawel Jalocha
rodzic
6853ba7207
commit
367ba6d4ce
|
|
@ -39,30 +39,34 @@ void PrintTasks(void (*CONS_UART_Write)(char))
|
|||
// ========================================================================================================================
|
||||
|
||||
#ifdef WITH_OLED
|
||||
int OLED_DisplayStatus(uint32_t Time, GPS_Position *GPS=0)
|
||||
int OLED_DisplayStatus(uint32_t Time, uint8_t LineIdx=0)
|
||||
{ char Line[20];
|
||||
Format_String(Line , "OGN Tx/Rx ");
|
||||
Format_HHMMSS(Line+10, Time);
|
||||
OLED_PutLine(0, Line);
|
||||
OLED_PutLine(LineIdx++, Line);
|
||||
Parameters.Print(Line);
|
||||
OLED_PutLine(1, Line);
|
||||
OLED_PutLine(LineIdx++, Line);
|
||||
return 0; }
|
||||
|
||||
int OLED_DisplayPosition(GPS_Position *GPS=0, uint8_t LineIdx=2)
|
||||
{ char Line[20];
|
||||
if(GPS && GPS->isValid())
|
||||
{ Line[0]=' ';
|
||||
Format_SignDec(Line+1, GPS->Latitude /60, 6, 4); Line[9]=' ';
|
||||
Format_UnsDec (Line+10, GPS->Altitude /10, 5, 0); Line[15]='m';
|
||||
OLED_PutLine(2, Line);
|
||||
OLED_PutLine(LineIdx , Line);
|
||||
Format_SignDec(Line, GPS->Longitude/60, 7, 4);
|
||||
Format_SignDec(Line+10, GPS->ClimbRate, 4, 1);
|
||||
OLED_PutLine(3, Line);
|
||||
OLED_PutLine(LineIdx+1, Line);
|
||||
Format_UnsDec (Line , GPS->Speed, 4, 1); Format_String(Line+5, "m/s ");
|
||||
Format_UnsDec (Line+10, GPS->Heading, 4, 1); Line[15]='^';
|
||||
OLED_PutLine(4, Line);
|
||||
OLED_PutLine(LineIdx+2, Line);
|
||||
Format_String(Line, "0D/00sat DOP00.0");
|
||||
Line[0]+=GPS->FixMode; Format_UnsDec(Line+3, GPS->Satellites, 2);
|
||||
Format_UnsDec(Line+12, (uint16_t)GPS->HDOP, 3, 1);
|
||||
OLED_PutLine(5, Line);
|
||||
OLED_PutLine(LineIdx+3, Line);
|
||||
}
|
||||
else { OLED_PutLine(2, 0); OLED_PutLine(3, 0); OLED_PutLine(4, 0); }
|
||||
else { OLED_PutLine(LineIdx, 0); OLED_PutLine(LineIdx+1, 0); OLED_PutLine(LineIdx+2, 0); OLED_PutLine(LineIdx+3, 0); }
|
||||
if(GPS && GPS->isDateValid())
|
||||
{ Format_UnsDec (Line , (uint16_t)GPS->Day, 2, 0); Line[2]='.';
|
||||
Format_UnsDec (Line+ 3, (uint16_t)GPS->Month, 2, 0); Line[5]='.';
|
||||
|
|
@ -73,13 +77,13 @@ int OLED_DisplayStatus(uint32_t Time, GPS_Position *GPS=0)
|
|||
Format_UnsDec (Line+12, (uint16_t)GPS->Min, 2, 0);
|
||||
Format_UnsDec (Line+14, (uint16_t)GPS->Sec, 2, 0);
|
||||
} else Line[10]=0;
|
||||
OLED_PutLine(6, Line);
|
||||
OLED_PutLine(LineIdx+4, Line);
|
||||
Line[0]=0;
|
||||
if(GPS && GPS->Baro)
|
||||
if(GPS && GPS->hasBaro)
|
||||
{ Format_String(Line , "0000.0hPa 00000m");
|
||||
Format_UnsDec(Line , GPS->Pressure/4, 5, 1);
|
||||
Format_UnsDec(Line+10, GPS->StdAltitude/10, 5, 0); }
|
||||
OLED_PutLine(7, Line);
|
||||
OLED_PutLine(LineIdx+5, Line);
|
||||
return 0; }
|
||||
#endif
|
||||
|
||||
|
|
@ -102,7 +106,8 @@ static void ReadParameters(void) // read parameters requested by the user in th
|
|||
{ PrintParameters();
|
||||
if(NMEA.Parms==0) // if no parameter given
|
||||
{ xSemaphoreTake(CONS_Mutex, portMAX_DELAY); // print a help message
|
||||
Format_String(CONS_UART_Write, "$POGNS,<aircraft-type>,<addr-type>,<address>,<RFM69(H)W>,<Tx-power[dBm]>,<freq.corr.[kHz]>,<console baudrate[bps]>,<RF temp. corr.[degC]>,<pressure corr.[Pa]>\n");
|
||||
// Format_String(CONS_UART_Write, "$POGNS,<aircraft-type>,<addr-type>,<address>,<RFM69(H)W>,<Tx-power[dBm]>,<freq.corr.[kHz]>,<console baudrate[bps]>,<RF temp. corr.[degC]>,<pressure corr.[Pa]>\n");
|
||||
Format_String(CONS_UART_Write, "$POGNS[,<Name>=<Value>]\n");
|
||||
xSemaphoreGive(CONS_Mutex); //
|
||||
return; }
|
||||
Parameters.ReadPOGNS(NMEA);
|
||||
|
|
@ -131,6 +136,9 @@ static void ProcessCtrlC(void) // print system
|
|||
Format_String(CONS_UART_Write, "GPS: ");
|
||||
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);
|
||||
CONS_UART_Write('s');
|
||||
if(GPS_Status.PPS) Format_String(CONS_UART_Write, ",PPS");
|
||||
if(GPS_Status.NMEA) Format_String(CONS_UART_Write, ",NMEA");
|
||||
if(GPS_Status.UBX) Format_String(CONS_UART_Write, ",UBX");
|
||||
|
|
@ -174,10 +182,11 @@ void vTaskCTRL(void* pvParameters)
|
|||
GPS_Position *GPS = GPS_getPosition();
|
||||
bool TimeChange = Time!=PrevTime;
|
||||
bool GPSchange = GPS!=PrevGPS;
|
||||
if( (Time==PrevTime) && (GPS==PrevGPS) ) continue;
|
||||
if( (!TimeChange) && (!GPSchange) ) continue;
|
||||
PrevTime=Time; PrevGPS=GPS;
|
||||
#ifdef WITH_OLED
|
||||
OLED_DisplayStatus(Time, GPS);
|
||||
if(TimeChange) OLED_DisplayStatus(Time, 0);
|
||||
if(GPSchange) OLED_DisplayPosition(GPS, 2);
|
||||
#endif
|
||||
#ifdef DEBUG_PRINT
|
||||
if(!TimeChange || (Time%60)!=0) continue;
|
||||
|
|
|
|||
|
|
@ -26,12 +26,27 @@ uint8_t Format_String(char *Str, const char *String)
|
|||
Str[OutLen++]=ch; }
|
||||
return OutLen; }
|
||||
|
||||
uint8_t Format_String(char *Str, const char *String, uint8_t Len)
|
||||
{ uint8_t OutLen=0;
|
||||
for(uint8_t Idx=0; Idx<Len; Idx++)
|
||||
{ char ch = (*String++);
|
||||
void Format_String( void (*Output)(char), const char *String, uint8_t MinLen, uint8_t MaxLen)
|
||||
{ if(MaxLen<MinLen) MaxLen=MinLen;
|
||||
uint8_t Idx;
|
||||
for(Idx=0; Idx<MaxLen; Idx++)
|
||||
{ char ch = String[Idx]; if(ch==0) break;
|
||||
if(ch=='\n') (*Output)('\r');
|
||||
(*Output)(ch); }
|
||||
for( ; Idx<MinLen; Idx++)
|
||||
(*Output)(' ');
|
||||
}
|
||||
|
||||
uint8_t Format_String(char *Str, const char *String, uint8_t MinLen, uint8_t MaxLen)
|
||||
{ if(MaxLen<MinLen) MaxLen=MinLen;
|
||||
uint8_t OutLen=0;
|
||||
uint8_t Idx;
|
||||
for(Idx=0; Idx<MaxLen; Idx++)
|
||||
{ char ch = String[Idx]; if(ch==0) break;
|
||||
if(ch=='\n') Str[OutLen++]='\r';
|
||||
Str[OutLen++]=ch; }
|
||||
for( ; Idx<MinLen; Idx++)
|
||||
Str[OutLen++]=' ';
|
||||
return OutLen; }
|
||||
|
||||
void Format_Hex( void (*Output)(char), uint8_t Byte )
|
||||
|
|
@ -90,6 +105,25 @@ void Format_SignDec( void (*Output)(char), int32_t Value, uint8_t MinDigits, uin
|
|||
else { (*Output)('+'); }
|
||||
Format_UnsDec(Output, (uint32_t)Value, MinDigits, DecPoint); }
|
||||
|
||||
void Format_UnsDec( void (*Output)(char), uint64_t Value, uint8_t MinDigits, uint8_t DecPoint)
|
||||
{ uint64_t Base; uint8_t Pos;
|
||||
for( Pos=20, Base=10000000000000000000; Base; Base/=10, Pos--)
|
||||
{ uint8_t Dig;
|
||||
if(Value>=Base)
|
||||
{ Dig=Value/Base; Value-=Dig*Base; }
|
||||
else
|
||||
{ Dig=0; }
|
||||
if(Pos==DecPoint) (*Output)('.');
|
||||
if( (Pos<=MinDigits) || (Dig>0) || (Pos<=DecPoint) )
|
||||
{ (*Output)('0'+Dig); MinDigits=Pos; }
|
||||
}
|
||||
}
|
||||
|
||||
void Format_SignDec( void (*Output)(char), int64_t Value, uint8_t MinDigits, uint8_t DecPoint)
|
||||
{ if(Value<0) { (*Output)('-'); Value=(-Value); }
|
||||
else { (*Output)('+'); }
|
||||
Format_UnsDec(Output, (uint32_t)Value, MinDigits, DecPoint); }
|
||||
|
||||
// ------------------------------------------------------------------------------------------
|
||||
|
||||
uint8_t Format_UnsDec(char *Str, uint32_t Value, uint8_t MinDigits, uint8_t DecPoint)
|
||||
|
|
|
|||
|
|
@ -9,7 +9,7 @@ char HexDigit(uint8_t Val);
|
|||
inline void Format_Bytes ( void (*Output)(char), const char *Bytes, uint8_t Len) { Format_Bytes(Output, (const uint8_t *)Bytes, Len); }
|
||||
|
||||
void Format_String( void (*Output)(char), const char *String);
|
||||
inline void Format_String( void (*Output)(char), const char *String, uint8_t Len) { Format_Bytes(Output, (const uint8_t *)String, Len); }
|
||||
void Format_String( void (*Output)(char), const char *String, uint8_t MinLen, uint8_t MaxLen);
|
||||
|
||||
void Format_Hex( void (*Output)(char), uint8_t Byte );
|
||||
void Format_Hex( void (*Output)(char), uint16_t Word );
|
||||
|
|
@ -21,8 +21,11 @@ void Format_SignDec( void (*Output)(char), int16_t Value, uint8_t MinDigits=1,
|
|||
void Format_UnsDec ( void (*Output)(char), uint32_t Value, uint8_t MinDigits=1, uint8_t DecPoint=0);
|
||||
void Format_SignDec( void (*Output)(char), int32_t Value, uint8_t MinDigits=1, uint8_t DecPoint=0);
|
||||
|
||||
void Format_UnsDec ( void (*Output)(char), uint64_t Value, uint8_t MinDigits=1, uint8_t DecPoint=0);
|
||||
void Format_SignDec( void (*Output)(char), int64_t Value, uint8_t MinDigits=1, uint8_t DecPoint=0);
|
||||
|
||||
uint8_t Format_String(char *Str, const char *String);
|
||||
uint8_t Format_String(char *Str, const char *String, uint8_t Len);
|
||||
uint8_t Format_String(char *Str, const char *String, uint8_t MinLen, uint8_t MaxLen);
|
||||
|
||||
uint8_t Format_UnsDec (char *Str, uint32_t Value, uint8_t MinDigits=1, uint8_t DecPoint=0);
|
||||
uint8_t Format_SignDec(char *Str, int32_t Value, uint8_t MinDigits=1, uint8_t DecPoint=0);
|
||||
|
|
|
|||
264
main/gps.cpp
264
main/gps.cpp
|
|
@ -1,12 +1,15 @@
|
|||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
#include "hal.h"
|
||||
#include "gps.h"
|
||||
#include "ctrl.h"
|
||||
|
||||
#include "nmea.h"
|
||||
#include "ubx.h"
|
||||
#ifdef WITH_MAVLINK
|
||||
#include "mavlink.h"
|
||||
#include "atmosphere.h"
|
||||
#endif
|
||||
|
||||
#include "ogn.h"
|
||||
|
|
@ -18,6 +21,10 @@
|
|||
|
||||
// #define DEBUG_PRINT
|
||||
|
||||
#ifdef DEBUG_PRINT
|
||||
static char Line[128];
|
||||
#endif
|
||||
|
||||
// ----------------------------------------------------------------------------
|
||||
|
||||
// void Debug_Print(uint8_t Byte) { while(!UART1_TxEmpty()) taskYIELD(); UART1_TxChar(Byte); }
|
||||
|
|
@ -30,18 +37,21 @@ static UBX_RxMsg UBX; // UBX messages catcher
|
|||
static MAV_RxMsg MAV; // MAVlink message catcher
|
||||
#endif
|
||||
|
||||
static GPS_Position Position[4]; // GPS position pipe
|
||||
static uint8_t PosIdx; // Pipe index, increments with every GPS position received
|
||||
uint16_t GPS_PosPeriod = 0;
|
||||
|
||||
static TickType_t Burst_TickCount; // [msec] TickCount when the data burst from GPS started
|
||||
const uint8_t PosPipeIdxMask = GPS_PosPipeSize-1;
|
||||
static GPS_Position Position[GPS_PosPipeSize]; // GPS position pipe
|
||||
static uint8_t PosIdx; // Pipe index, increments with every GPS position received
|
||||
|
||||
uint32_t GPS_TimeSinceLock; // [sec] time since the GPS has a lock
|
||||
uint32_t GPS_FatTime = 0; // [sec] UTC date/time in FAT format
|
||||
int32_t GPS_Altitude = 0; // [0.1m] last valid altitude
|
||||
int32_t GPS_Latitude = 0; //
|
||||
int32_t GPS_Longitude = 0; //
|
||||
int16_t GPS_GeoidSepar= 0; // [0.1m]
|
||||
uint16_t GPS_LatCosine = 3000; //
|
||||
static TickType_t Burst_TickCount; // [msec] TickCount when the data burst from GPS started
|
||||
|
||||
uint32_t GPS_TimeSinceLock; // [sec] time since the GPS has a lock
|
||||
uint32_t GPS_FatTime = 0; // [sec] UTC date/time in FAT format
|
||||
int32_t GPS_Altitude = 0; // [0.1m] last valid altitude
|
||||
int32_t GPS_Latitude = 0; //
|
||||
int32_t GPS_Longitude = 0; //
|
||||
int16_t GPS_GeoidSepar= 0; // [0.1m]
|
||||
uint16_t GPS_LatCosine = 3000; //
|
||||
|
||||
Status GPS_Status;
|
||||
|
||||
|
|
@ -76,7 +86,7 @@ int16_t GPS_AverageSpeed(void) // get average speed based
|
|||
int16_t Speed=0;
|
||||
for(uint8_t Idx=0; Idx<4; Idx++) // loop over GPS positions
|
||||
{ GPS_Position *Pos = Position+Idx;
|
||||
if( !Pos->GPS || !Pos->isValid() ) continue; // skip invalid positions
|
||||
if( !Pos->hasGPS || !Pos->isValid() ) continue; // skip invalid positions
|
||||
Speed += Pos->Speed +abs(Pos->ClimbRate); Count++;
|
||||
}
|
||||
if(Count==0) return -1;
|
||||
|
|
@ -104,14 +114,14 @@ static void GPS_PPS_On(void) // called on rising edge o
|
|||
LED_PCB_Flash(50);
|
||||
// uint8_t Sec=GPS_Sec; Sec++; if(Sec>=60) Sec=0; GPS_Sec=Sec;
|
||||
// GPS_UnixTime++;
|
||||
#ifdef WITH_MAVLINK
|
||||
static MAV_SYSTEM_TIME MAV_Time;
|
||||
MAV_Time.time_unix_usec = (uint64_t)1000000*TimeSync_Time();
|
||||
MAV_Time.time_boot_ms = TickCount;
|
||||
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
MAV_RxMsg::Send(sizeof(MAV_Time), MAV_Seq++, MAV_SysID, MAV_COMP_ID_GPS, MAV_ID_SYSTEM_TIME, (const uint8_t *)&MAV_Time, CONS_UART_Write);
|
||||
xSemaphoreGive(CONS_Mutex);
|
||||
#endif
|
||||
// #ifdef WITH_MAVLINK
|
||||
// static MAV_SYSTEM_TIME MAV_Time;
|
||||
// MAV_Time.time_unix_usec = (uint64_t)1000000*TimeSync_Time();
|
||||
// MAV_Time.time_boot_ms = TickCount;
|
||||
// xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
// MAV_RxMsg::Send(sizeof(MAV_Time), MAV_Seq++, MAV_SysID, MAV_COMP_ID_GPS, MAV_ID_SYSTEM_TIME, (const uint8_t *)&MAV_Time, CONS_UART_Write);
|
||||
// xSemaphoreGive(CONS_Mutex);
|
||||
// #endif
|
||||
}
|
||||
|
||||
static void GPS_PPS_Off(void) // called on falling edge of PPS
|
||||
|
|
@ -209,21 +219,37 @@ static void GPS_BurstStart(void) // wh
|
|||
|
||||
static void GPS_BurstComplete(void) // when GPS has sent the essential data for position fix
|
||||
{
|
||||
#ifdef WITH_MAVLINK
|
||||
GPS_Position *GPS = Position+PosIdx;
|
||||
if(GPS->hasTime && GPS->hasGPS && GPS->hasBaro)
|
||||
{ int32_t StdAlt1 = Atmosphere::StdAltitude((GPS->Pressure+2)/4); // [0.1m] we try to fix the cheap chinese ArduPilot with baro chip cs5607 instead of cs5611
|
||||
int32_t StdAlt2 = Atmosphere::StdAltitude((GPS->Pressure+1)/2); // [0.1m] the cx5607 is very close but gives pressure is twice as larger units
|
||||
int32_t Alt = GPS->Altitude; // [0.1m] thus it appears to give pressure readout lower by a factor of two.
|
||||
int32_t Delta1 = StdAlt1-Alt; // [0.1m] Here we check which pressure fits better the GPS altitude
|
||||
int32_t Delta2 = StdAlt2-Alt; // [0.1m]
|
||||
if(fabs(Delta1)<fabs(Delta2)) { GPS->StdAltitude=StdAlt1; } //
|
||||
else { GPS->Pressure*=2; GPS->StdAltitude=StdAlt2; }
|
||||
}
|
||||
#endif
|
||||
#ifdef DEBUG_PRINT
|
||||
Position[PosIdx].PrintLine(Line);
|
||||
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
Format_UnsDec(CONS_UART_Write, TimeSync_Time()%60);
|
||||
Format_UnsDec(CONS_UART_Write, TimeSync_Time()%60, 2);
|
||||
CONS_UART_Write('.');
|
||||
Format_UnsDec(CONS_UART_Write, TimeSync_msTime(),3);
|
||||
Format_String(CONS_UART_Write, " -> ...complete\n");
|
||||
Format_String(CONS_UART_Write, Line);
|
||||
xSemaphoreGive(CONS_Mutex);
|
||||
#endif
|
||||
if(Position[PosIdx].GPS) // GPS position data complete
|
||||
{ Position[PosIdx].Ready=1;
|
||||
if(Position[PosIdx].hasGPS) // GPS position data complete
|
||||
{ Position[PosIdx].isReady=1; // mark this record as ready for processing => producing packets for transmission
|
||||
if(Position[PosIdx].isTimeValid()) // if time is valid already
|
||||
{ if(Position[PosIdx].isDateValid()) // if date is valid as well
|
||||
{ uint32_t UnixTime=Position[PosIdx].getUnixTime();
|
||||
GPS_FatTime=Position[PosIdx].getFatTime();
|
||||
TimeSync_SoftPPS(Burst_TickCount, UnixTime, GPS_BurstDelay); //
|
||||
#ifndef WITH_MAVLINK // with MAVlink we sync. with the SYSTEM_TIME message
|
||||
TimeSync_SoftPPS(Burst_TickCount, UnixTime, GPS_BurstDelay);
|
||||
#endif
|
||||
}
|
||||
}
|
||||
if(Position[PosIdx].isValid()) // position is complete and locked
|
||||
|
|
@ -235,28 +261,65 @@ static void GPS_BurstComplete(void) // wh
|
|||
GPS_GeoidSepar=Position[PosIdx].GeoidSeparation;
|
||||
GPS_LatCosine=Position[PosIdx].LatitudeCosine;
|
||||
// GPS_FreqPlan=Position[PosIdx].getFreqPlan();
|
||||
if(GPS_TimeSinceLock==1)
|
||||
if(GPS_TimeSinceLock==1) // if we just acquired the lock a moment ago
|
||||
{ GPS_LockStart(); }
|
||||
if(GPS_TimeSinceLock>1)
|
||||
{ uint8_t PrevIdx=(PosIdx+3)&3;
|
||||
Position[PosIdx].calcDifferences(Position[PrevIdx]);
|
||||
if(GPS_TimeSinceLock>1) // if the lock is more persistant
|
||||
{ uint8_t PrevIdx=(PosIdx+PosPipeIdxMask)&PosPipeIdxMask;
|
||||
int16_t TimeDiff = Position[PosIdx].calcTimeDiff(Position[PrevIdx]);
|
||||
for( ; ; )
|
||||
{ if(TimeDiff>=95) break;
|
||||
uint8_t PrevIdx2=(PrevIdx+PosPipeIdxMask)&PosPipeIdxMask;
|
||||
if(PrevIdx2==PosIdx) break;
|
||||
if(!Position[PrevIdx2].isValid()) break;
|
||||
TimeDiff = Position[PosIdx].calcTimeDiff(Position[PrevIdx2]);
|
||||
PrevIdx=PrevIdx2; }
|
||||
TimeDiff=Position[PosIdx].calcDifferences(Position[PrevIdx]);
|
||||
#ifdef DEBUG_PRINT
|
||||
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
Format_String(CONS_UART_Write, "calcDiff() => ");
|
||||
Format_UnsDec(CONS_UART_Write, (uint16_t)PosIdx);
|
||||
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_String(CONS_UART_Write, "s\n");
|
||||
xSemaphoreGive(CONS_Mutex);
|
||||
#endif
|
||||
LED_PCB_Flash(100); }
|
||||
}
|
||||
else // complete but no valid lock
|
||||
{ if(GPS_TimeSinceLock) { GPS_LockEnd(); GPS_TimeSinceLock=0; }
|
||||
}
|
||||
#ifdef WITH_MAVLINK
|
||||
static MAV_GPS_RAW_INT MAV_Position;
|
||||
Position[PosIdx].Encode(MAV_Position);
|
||||
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
MAV_RxMsg::Send(sizeof(MAV_Position), MAV_Seq++, MAV_SysID, MAV_COMP_ID_GPS, MAV_ID_GPS_RAW_INT, (const uint8_t *)&MAV_Position, CONS_UART_Write);
|
||||
xSemaphoreGive(CONS_Mutex);
|
||||
#endif
|
||||
// #ifdef WITH_MAVLINK
|
||||
// static MAV_GPS_RAW_INT MAV_Position;
|
||||
// Position[PosIdx].Encode(MAV_Position);
|
||||
// xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
// MAV_RxMsg::Send(sizeof(MAV_Position), MAV_Seq++, MAV_SysID, MAV_COMP_ID_GPS, MAV_ID_GPS_RAW_INT, (const uint8_t *)&MAV_Position, CONS_UART_Write);
|
||||
// xSemaphoreGive(CONS_Mutex);
|
||||
// #endif
|
||||
}
|
||||
else // posiiton not complete, no GPS lock
|
||||
{ if(GPS_TimeSinceLock) { GPS_LockEnd(); GPS_TimeSinceLock=0; }
|
||||
}
|
||||
uint8_t NextPosIdx = (PosIdx+1)&3; // next position to be recorded
|
||||
uint8_t NextPosIdx = (PosIdx+1)&PosPipeIdxMask; // next position to be recorded
|
||||
if( Position[PosIdx].isTimeValid() && Position[NextPosIdx].isTimeValid() )
|
||||
{ int16_t Period = Position[PosIdx].calcTimeDiff(Position[NextPosIdx]);
|
||||
if(Period>0) GPS_PosPeriod = (Period+GPS_PosPipeSize/2)/(GPS_PosPipeSize-1);
|
||||
#ifdef DEBUG_PRINT
|
||||
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
Format_String(CONS_UART_Write,"GPS: ");
|
||||
Format_UnsDec(CONS_UART_Write, (uint16_t)PosIdx);
|
||||
CONS_UART_Write(' ');
|
||||
Format_UnsDec(CONS_UART_Write, (uint16_t)Position[PosIdx].Sec, 2);
|
||||
CONS_UART_Write('.');
|
||||
Format_UnsDec(CONS_UART_Write, (uint16_t)Position[PosIdx].FracSec, 2);
|
||||
Format_String(CONS_UART_Write, "s ");
|
||||
Format_SignDec(CONS_UART_Write, Period, 3, 2);
|
||||
Format_String(CONS_UART_Write, "s\n");
|
||||
xSemaphoreGive(CONS_Mutex);
|
||||
#endif
|
||||
|
||||
}
|
||||
Position[NextPosIdx].Clear(); // clear the next position
|
||||
int8_t Sec = Position[PosIdx].Sec; //
|
||||
Sec++; if(Sec>=60) Sec=0;
|
||||
|
|
@ -269,17 +332,32 @@ static void GPS_BurstComplete(void) // wh
|
|||
static void GPS_BurstEnd(void) // when GPS stops sending the data on the serial port
|
||||
{ }
|
||||
|
||||
GPS_Position *GPS_getPosition(void) // return most recent GPS_Position which is complete
|
||||
// ----------------------------------------------------------------------------
|
||||
|
||||
GPS_Position *GPS_getPosition(uint8_t &BestIdx, int16_t &BestRes, int8_t Sec, int8_t Frac) // return GPS position closest to the given Sec.Frac
|
||||
{ int16_t TargetTime = Frac+(int16_t)Sec*100;
|
||||
BestIdx=0; BestRes=0x7FFF;
|
||||
for(uint8_t Idx=0; Idx<GPS_PosPipeSize; Idx++)
|
||||
{ GPS_Position *Pos=Position+Idx;
|
||||
if(!Pos->isReady) continue;
|
||||
int16_t Diff = TargetTime - (Pos->FracSec + (int16_t)Pos->Sec*100);
|
||||
if(Diff<(-3000)) Diff+=6000;
|
||||
else if(Diff>3000) Diff-=6000;
|
||||
if(fabs(Diff)<fabs(BestRes)) { BestRes=Diff; BestIdx=Idx; }
|
||||
}
|
||||
return BestRes==0x7FFF ? 0:Position+BestIdx; }
|
||||
|
||||
GPS_Position *GPS_getPosition(void) // return most recent GPS_Position which has time/position data
|
||||
{ uint8_t PrevIdx=PosIdx;
|
||||
GPS_Position *PrevPos = Position+PrevIdx;
|
||||
if(PrevPos->GPS) return PrevPos;
|
||||
PrevIdx=(PrevIdx+3)&3;
|
||||
if(PrevPos->isReady) return PrevPos;
|
||||
PrevIdx=(PrevIdx+PosPipeIdxMask)&PosPipeIdxMask;
|
||||
PrevPos = Position+PrevIdx;
|
||||
if(PrevPos->GPS) return PrevPos;
|
||||
if(PrevPos->isReady) return PrevPos;
|
||||
return 0; }
|
||||
|
||||
GPS_Position *GPS_getPosition(int8_t Sec) // return the GPS_Position which corresponds to given Sec (may be incomplete and not valid)
|
||||
{ for(uint8_t Idx=0; Idx<4; Idx++)
|
||||
{ for(uint8_t Idx=0; Idx<GPS_PosPipeSize; Idx++)
|
||||
{ if(Sec==Position[Idx].Sec) return Position+Idx; }
|
||||
return 0; }
|
||||
|
||||
|
|
@ -321,13 +399,13 @@ static void GPS_NMEA(void) // wh
|
|||
}
|
||||
}
|
||||
|
||||
#ifdef WITH_GPS_UBX
|
||||
static void DumpUBX(void)
|
||||
{ Format_String(CONS_UART_Write, "UBX:");
|
||||
for(uint8_t Idx=0; Idx<20; Idx++)
|
||||
{ CONS_UART_Write(' '); Format_Hex(CONS_UART_Write, UBX.Byte[Idx]); }
|
||||
Format_String(CONS_UART_Write, "\n"); }
|
||||
|
||||
#ifdef WITH_GPS_UBX
|
||||
static void GPS_UBX(void) // when GPS gets an UBX packet
|
||||
{ GPS_Status.UBX=1;
|
||||
GPS_Status.BaudConfig = (GPS_getBaudRate() == GPS_TargetBaudRate);
|
||||
|
|
@ -402,8 +480,108 @@ static void GPS_UBX(void)
|
|||
|
||||
#ifdef WITH_MAVLINK
|
||||
static void GPS_MAV(void) // when GPS gets an MAV packet
|
||||
{ GPS_Status.MAV=1;
|
||||
{ static int64_t MAV_TimeOfs_ms=0;
|
||||
TickType_t TickCount=xTaskGetTickCount();
|
||||
GPS_Status.MAV=1;
|
||||
LED_PCB_Flash(2);
|
||||
GPS_Status.BaudConfig = (GPS_getBaudRate() == GPS_TargetBaudRate);
|
||||
int32_t TimeCorr_ms = (int32_t)Parameters.TimeCorr*1000; // apparently ArduPilot needs some time correction, as it "manually" converts from GPS to UTC time
|
||||
uint8_t MsgID = MAV.getMsgID();
|
||||
if(MsgID==MAV_ID_HEARTBEAT)
|
||||
{ const MAV_HEARTBEAT *Heartbeat = (const MAV_HEARTBEAT *)MAV.getPayload();
|
||||
#ifdef DEBUG_PRINT
|
||||
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
Format_String(CONS_UART_Write, "MAV_HEARBEAT: ");
|
||||
Format_Hex(CONS_UART_Write, Heartbeat->system_status);
|
||||
Format_String(CONS_UART_Write, "\n");
|
||||
xSemaphoreGive(CONS_Mutex);
|
||||
#endif
|
||||
} else if(MsgID==MAV_ID_SYSTEM_TIME)
|
||||
{ const MAV_SYSTEM_TIME *SysTime = (const MAV_SYSTEM_TIME *)MAV.getPayload();
|
||||
uint64_t UnixTime_ms = SysTime->time_unix_usec/1000 + TimeCorr_ms; // [ms] Unix Time
|
||||
uint32_t UnixTime = UnixTime_ms/1000; // [ s] Unix Time
|
||||
uint32_t UnixFrac = UnixTime_ms-(uint64_t)UnixTime*1000; // [ms] Second fraction of the Unix time
|
||||
MAV_TimeOfs_ms=UnixTime_ms-SysTime->time_boot_ms; // [ms] difference between the Unix Time and the Ardupilot time-since-boot
|
||||
TimeSync_SoftPPS(TickCount-UnixFrac, UnixTime, 70);
|
||||
#ifdef DEBUG_PRINT
|
||||
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
Format_String(CONS_UART_Write, "MAV_SYSTEM_TIME: ");
|
||||
Format_UnsDec(CONS_UART_Write, UnixTime, 10);
|
||||
CONS_UART_Write('.');
|
||||
Format_UnsDec(CONS_UART_Write, UnixFrac, 3);
|
||||
CONS_UART_Write(' ');
|
||||
Format_SignDec(CONS_UART_Write, MAV_TimeOfs_ms, 13, 3);
|
||||
Format_String(CONS_UART_Write, "\n");
|
||||
xSemaphoreGive(CONS_Mutex);
|
||||
#endif
|
||||
} else if(MsgID==MAV_ID_GLOBAL_POSITION_INT)
|
||||
{ const MAV_GLOBAL_POSITION_INT *Pos = (const MAV_GLOBAL_POSITION_INT *)MAV.getPayload();
|
||||
uint64_t UnixTime_ms = Pos->time_boot_ms + MAV_TimeOfs_ms;
|
||||
Position[PosIdx].Read(Pos, UnixTime_ms);
|
||||
#ifdef DEBUG_PRINT
|
||||
Position[PosIdx].PrintLine(Line);
|
||||
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
Format_String(CONS_UART_Write, "MAV_GLOBAL_POSITION_INT: ");
|
||||
Format_UnsDec(CONS_UART_Write, UnixTime_ms, 13, 3);
|
||||
Format_String(CONS_UART_Write, "\n");
|
||||
Format_String(CONS_UART_Write, Line);
|
||||
xSemaphoreGive(CONS_Mutex);
|
||||
#endif
|
||||
} else if(MsgID==MAV_ID_GPS_RAW_INT)
|
||||
{ MAV_GPS_RAW_INT *RawGPS = (MAV_GPS_RAW_INT *)MAV.getPayload();
|
||||
uint64_t UnixTime_ms = RawGPS->time_usec/1000 + MAV_TimeOfs_ms;
|
||||
// RawGPS->time_usec += (int64_t)MAV_TimeOfs_ms*1000;
|
||||
Position[PosIdx].Read(RawGPS, UnixTime_ms);
|
||||
#ifdef DEBUG_PRINT
|
||||
Position[PosIdx].PrintLine(Line);
|
||||
uint32_t UnixTime = (UnixTime_ms+500)/1000;
|
||||
int32_t TimeDiff = (int64_t)UnixTime_ms-(int64_t)UnixTime*1000;
|
||||
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
Format_String(CONS_UART_Write, "MAV_GPS_RAW_INT: ");
|
||||
Format_UnsDec(CONS_UART_Write, UnixTime_ms, 13, 3);
|
||||
CONS_UART_Write(' ');
|
||||
Format_SignDec(CONS_UART_Write, TimeDiff, 4, 3);
|
||||
CONS_UART_Write(abs(TimeDiff)<250 ? '*':' ');
|
||||
Format_String(CONS_UART_Write, "\n");
|
||||
Format_String(CONS_UART_Write, Line);
|
||||
xSemaphoreGive(CONS_Mutex);
|
||||
#endif
|
||||
} else if(MsgID==MAV_ID_SCALED_PRESSURE)
|
||||
{ const MAV_SCALED_PRESSURE *Press = (const MAV_SCALED_PRESSURE *)MAV.getPayload();
|
||||
uint64_t UnixTime_ms = Press->time_boot_ms + MAV_TimeOfs_ms;
|
||||
Position[PosIdx].Read(Press, UnixTime_ms);
|
||||
#ifdef DEBUG_PRINT
|
||||
Position[PosIdx].PrintLine(Line);
|
||||
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
Format_String(CONS_UART_Write, "MAV_SCALED_PRESSURE: ");
|
||||
Format_UnsDec(CONS_UART_Write, UnixTime_ms, 13, 3);
|
||||
Format_String(CONS_UART_Write, "\n");
|
||||
Format_String(CONS_UART_Write, Line);
|
||||
xSemaphoreGive(CONS_Mutex);
|
||||
#endif
|
||||
} else if(MsgID==MAV_ID_SYS_STATUS)
|
||||
{ const MAV_SYS_STATUS *Status = (const MAV_SYS_STATUS *)MAV.getPayload();
|
||||
#ifdef DEBUG_PRINT
|
||||
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
Format_String(CONS_UART_Write, "MAV_SYS_STATUS: ");
|
||||
Format_UnsDec(CONS_UART_Write, Status->battery_voltage, 4, 3);
|
||||
Format_String(CONS_UART_Write, "V ");
|
||||
Format_SignDec(CONS_UART_Write, Status->battery_current, 3, 2);
|
||||
Format_String(CONS_UART_Write, "A\n");
|
||||
xSemaphoreGive(CONS_Mutex);
|
||||
#endif
|
||||
// } else if(MsgID==MAV_ID_STATUSTEXT)
|
||||
// {
|
||||
}
|
||||
#ifdef DEBUG_PRINT
|
||||
else
|
||||
{ xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
Format_String(CONS_UART_Write, "MAV: MsgID=");
|
||||
Format_UnsDec(CONS_UART_Write, (uint16_t)MAV.getMsgID(), 3);
|
||||
Format_String(CONS_UART_Write, "\n");
|
||||
xSemaphoreGive(CONS_Mutex);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
#endif
|
||||
|
||||
|
|
@ -552,7 +730,7 @@ void vTaskGPS(void* pvParameters)
|
|||
GPS_Burst.Flags=0;
|
||||
}
|
||||
|
||||
if(NoValidData>=1200) // if no valid data from GPS for 1sec
|
||||
if(NoValidData>=2000) // if no valid data from GPS for 1sec
|
||||
{ GPS_Status.Flags=0; GPS_Burst.Flags=0; // assume GPS state is unknown
|
||||
uint32_t NewBaudRate = GPS_nextBaudRate(); // switch to the next baud rate
|
||||
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
|
|
|
|||
11
main/gps.h
11
main/gps.h
|
|
@ -1,6 +1,4 @@
|
|||
|
||||
#ifdef __cplusplus
|
||||
|
||||
#include "hal.h"
|
||||
|
||||
#include "timesync.h"
|
||||
|
|
@ -9,6 +7,8 @@
|
|||
|
||||
#include "lowpass2.h"
|
||||
|
||||
const uint8_t GPS_PosPipeSize = 4; // number of GPS positions held in a pipe
|
||||
|
||||
extern uint32_t GPS_FatTime; // [2 sec] UTC time in FAT format (for FatFS)
|
||||
extern int32_t GPS_Altitude; // [0.1m] altitude (height above Geoid)
|
||||
extern int32_t GPS_Latitude; // [0.0001/60 deg]
|
||||
|
|
@ -16,6 +16,7 @@ extern int32_t GPS_Longitude; // [0.0001/60 deg]
|
|||
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 uint16_t GPS_PosPeriod; // [0.01sec] how often (which period) the GPS/MAV is sending the positions
|
||||
|
||||
typedef union
|
||||
{ uint8_t Flags;
|
||||
|
|
@ -36,13 +37,11 @@ extern Status GPS_Status;
|
|||
uint32_t GPS_getBaudRate(void); // [bps]
|
||||
|
||||
GPS_Position *GPS_getPosition(void);
|
||||
GPS_Position *GPS_getPosition(int8_t Sec);
|
||||
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); // 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
|
||||
|
||||
#endif // __cplusplus
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C"
|
||||
#endif
|
||||
|
|
|
|||
155
main/hal.cpp
155
main/hal.cpp
|
|
@ -1,4 +1,6 @@
|
|||
#include <stdint.h>
|
||||
#include <string.h>
|
||||
#include <stdbool.h>
|
||||
// #include <sys/select.h>
|
||||
|
||||
#include "freertos/FreeRTOS.h"
|
||||
|
|
@ -34,7 +36,7 @@
|
|||
|
||||
// ======================================================================================================
|
||||
/*
|
||||
The HELTEC Automation board WiFi LoRa 32 with sx1276 (RFM95) for 868/915MHz
|
||||
The HELTEC AUtomation board WiFi LoRa 32 with sx1278 (RFM95)
|
||||
|
||||
Referenced: http://esp32.net/
|
||||
Pinout: http://esp32.net/images/Heltec/WIFI-LoRa-32/Heltec_WIFI-LoRa-32_DiagramPinoutFromTop.jpg
|
||||
|
|
@ -49,7 +51,7 @@ OLED datasheet: https://cdn-shop.adafruit.com/datasheets/SSD1306.pdf
|
|||
OLED example: https://github.com/yanbe/ssd1306-esp-idf-i2c
|
||||
OLED article: http://robotcantalk.blogspot.co.uk/2015/03/interfacing-arduino-with-ssd1306-driven.html
|
||||
|
||||
SX1276 pins:
|
||||
SX1278 pins:
|
||||
14 = GPIO14 = RST
|
||||
5 = GPIO5 = SCK
|
||||
18 = GPIO18 = CS = SS
|
||||
|
|
@ -85,6 +87,8 @@ UART2 pins:
|
|||
*/
|
||||
|
||||
#define PIN_LED_PCB GPIO_NUM_25 // status LED on the PCB
|
||||
// #define PIN_LED_TX GPIO_NUM_??
|
||||
// #define PIN_LED_RX GPIO_NUM_??
|
||||
|
||||
#define PIN_RFM_RST GPIO_NUM_14 // Reset
|
||||
#define PIN_RFM_IRQ GPIO_NUM_26 // packet done on receive or transmit
|
||||
|
|
@ -93,11 +97,13 @@ UART2 pins:
|
|||
#define PIN_RFM_MISO GPIO_NUM_19 // SPI MISO
|
||||
#define PIN_RFM_MOSI GPIO_NUM_27 // SPI MOSI
|
||||
|
||||
// wire colours for VK2828U GPS
|
||||
#define PIN_GPS_TXD GPIO_NUM_12 // green
|
||||
#define PIN_GPS_RXD GPIO_NUM_35 // blue
|
||||
// VK2828U GPS MAVlink port
|
||||
#define PIN_GPS_TXD GPIO_NUM_12 // green green
|
||||
#define PIN_GPS_RXD GPIO_NUM_35 // blue yellow
|
||||
#define PIN_GPS_PPS GPIO_NUM_34 // white
|
||||
#define PIN_GPS_ENA GPIO_NUM_13 // yellow -> well, I had a problem here, thus I tied the enable wire to 3.3V for the time being.
|
||||
#define PIN_GPS_ENA GPIO_NUM_13 // yellow
|
||||
|
||||
// Note: I had a problem GPS ENABLE on GPIO13, thus I tied the enable wire to 3.3V for the time being.
|
||||
|
||||
#define CONS_UART UART_NUM_0 // UART0 for the console (the system does this for us)
|
||||
#define GPS_UART UART_NUM_1 // UART1 for GPS data read and dialog
|
||||
|
|
@ -116,6 +122,25 @@ UART2 pins:
|
|||
uint64_t getUniqueID(void)
|
||||
{ uint64_t ID=0; esp_err_t ret=esp_efuse_mac_get_default((uint8_t *)&ID); return ID; }
|
||||
|
||||
uint32_t getUniqueAddress(void)
|
||||
{ uint32_t ID = getUniqueID()>>24;
|
||||
ID &= 0x00FFFFFF;
|
||||
ID = (ID>>16) | (ID&0x00FF00) | (ID<<16);
|
||||
ID &= 0x00FFFFFF;
|
||||
return ID; }
|
||||
|
||||
// ======================================================================================================
|
||||
|
||||
#ifdef WITH_MAVLINK
|
||||
uint8_t MAV_Seq=0; // sequence number for MAVlink message sent out
|
||||
#endif
|
||||
|
||||
// ======================================================================================================
|
||||
|
||||
// system_get_time() - return s 32-bit time in microseconds since the system start
|
||||
// gettimeofday()
|
||||
// xthal_get_ccount() - gets Xtal or master clock counts ?
|
||||
|
||||
// ======================================================================================================
|
||||
|
||||
FlashParameters Parameters;
|
||||
|
|
@ -123,6 +148,7 @@ FlashParameters Parameters;
|
|||
//--------------------------------------------------------------------------------------------------------
|
||||
// STatus LED
|
||||
|
||||
void LED_PCB_Dir (void) { gpio_set_direction(PIN_LED_PCB, GPIO_MODE_OUTPUT); }
|
||||
void LED_PCB_On (void) { gpio_set_level(PIN_LED_PCB, 1); } // LED is on GPIO25
|
||||
void LED_PCB_Off (void) { gpio_set_level(PIN_LED_PCB, 0); }
|
||||
|
||||
|
|
@ -131,6 +157,15 @@ void LED_PCB_Off (void) { gpio_set_level(PIN_LED_PCB, 0); }
|
|||
|
||||
SemaphoreHandle_t CONS_Mutex;
|
||||
|
||||
/*
|
||||
bool CONS_InpReady(void)
|
||||
{ struct timeval tv = { tv_sec:0, tv_usec:0} ;
|
||||
fd_set fds;
|
||||
FD_ZERO(&fds);
|
||||
FD_SET(STDIN_FILENO, &fds);
|
||||
select(STDIN_FILENO+1, &fds, NULL, NULL, &tv);
|
||||
return (FD_ISSET(0, &fds)); }
|
||||
*/
|
||||
// int CONS_UART_Read (uint8_t &Byte) { return uart_read_bytes (CONS_UART, &Byte, 1, 0); } // non-blocking
|
||||
// void CONS_UART_Write (char Byte) { uart_write_bytes (CONS_UART, &Byte, 1); } // blocking ?
|
||||
void CONS_UART_Write (char Byte) { putchar(Byte); }
|
||||
|
|
@ -155,14 +190,24 @@ bool GPS_PPS_isOn(void) { return gpio_get_level(PIN_GPS_PPS); }
|
|||
//--------------------------------------------------------------------------------------------------------
|
||||
// RF chip
|
||||
|
||||
inline void RFM_RESET_Dir (void) { gpio_set_direction(PIN_RFM_RST, GPIO_MODE_OUTPUT); }
|
||||
inline void RFM_RESET_High(void) { gpio_set_level(PIN_RFM_RST, 1); }
|
||||
inline void RFM_RESET_Low (void) { gpio_set_level(PIN_RFM_RST, 0); }
|
||||
|
||||
#ifdef WITH_RFM95
|
||||
void RFM_RESET(uint8_t On)
|
||||
{ if(On) RFM_RESET_Low();
|
||||
else RFM_RESET_High(); }
|
||||
#endif
|
||||
|
||||
bool RFM_DIO0_isOn(void) { return gpio_get_level(PIN_RFM_IRQ); }
|
||||
#ifdef WITH_RFM69
|
||||
void RFM_RESET(uint8_t On)
|
||||
{ if(On) RFM_RESET_High();
|
||||
else RFM_RESET_Low(); }
|
||||
#endif
|
||||
|
||||
inline void RFM_IRQ_Dir (void) { gpio_set_direction(PIN_RFM_IRQ, GPIO_MODE_INPUT); }
|
||||
bool RFM_IRQ_isOn(void) { return gpio_get_level(PIN_RFM_IRQ); }
|
||||
|
||||
static spi_device_handle_t RFM_SPI;
|
||||
|
||||
|
|
@ -248,9 +293,18 @@ esp_err_t OLED_Clear(void)
|
|||
//--------------------------------------------------------------------------------------------------------
|
||||
|
||||
volatile uint8_t LED_PCB_Counter = 0;
|
||||
|
||||
void LED_PCB_Flash(uint8_t Time) { if(Time>LED_PCB_Counter) LED_PCB_Counter=Time; } // [ms]
|
||||
|
||||
#ifdef WITH_LED_TX
|
||||
volatile uint8_t LED_TX_Counter = 0;
|
||||
void LED_TX_Flash(uint8_t Time) { if(Time>LED_TX_Counter) LED_TX_Counter=Time; } // [ms]
|
||||
#endif
|
||||
|
||||
#ifdef WITH_LED_RX
|
||||
volatile uint8_t LED_RX_Counter = 0;
|
||||
void LED_RX_Flash(uint8_t Time) { if(Time>LED_RX_Counter) LED_RX_Counter=Time; } // [ms]
|
||||
#endif
|
||||
|
||||
void LED_TimerCheck(uint8_t Ticks)
|
||||
{ uint8_t Counter=LED_PCB_Counter;
|
||||
if(Counter)
|
||||
|
|
@ -259,6 +313,24 @@ void LED_TimerCheck(uint8_t Ticks)
|
|||
if(Counter) LED_PCB_On();
|
||||
else LED_PCB_Off();
|
||||
LED_PCB_Counter=Counter; }
|
||||
#ifdef WITH_LED_TX
|
||||
Counter=LED_TX_Counter;
|
||||
if(Counter)
|
||||
{ if(Ticks<Counter) Counter-=Ticks;
|
||||
else Counter =0;
|
||||
if(Counter) LED_TX_On();
|
||||
else LED_TX_Off();
|
||||
LED_TX_Counter=Counter; }
|
||||
#endif
|
||||
#ifdef WITH_LED_TX
|
||||
Counter=LED_RX_Counter;
|
||||
if(Counter)
|
||||
{ if(Ticks<Counter) Counter-=Ticks;
|
||||
else Counter =0;
|
||||
if(Counter) LED_RX_On();
|
||||
else LED_RX_Off();
|
||||
LED_RX_Counter=Counter; }
|
||||
#endif
|
||||
}
|
||||
|
||||
/*
|
||||
|
|
@ -275,15 +347,18 @@ void vApplicationTickHook(void) // RTOS timer tick hook
|
|||
|
||||
//--------------------------------------------------------------------------------------------------------
|
||||
|
||||
// void CONS_Configuration(void)
|
||||
// {
|
||||
// CONS_Mutex = xSemaphoreCreateMutex();
|
||||
// }
|
||||
|
||||
void IO_Configuration(void)
|
||||
{
|
||||
CONS_Mutex = xSemaphoreCreateMutex();
|
||||
|
||||
gpio_set_direction(PIN_LED_PCB, GPIO_MODE_OUTPUT); // LED
|
||||
LED_PCB_Dir();
|
||||
LED_PCB_Off();
|
||||
|
||||
gpio_set_direction(PIN_RFM_RST, GPIO_MODE_OUTPUT); // RF chip GPIO pins
|
||||
gpio_set_direction(PIN_RFM_IRQ, GPIO_MODE_INPUT);
|
||||
RFM_RESET_Dir();
|
||||
RFM_IRQ_Dir();
|
||||
RFM_RESET(0);
|
||||
|
||||
spi_bus_config_t BusCfg = // RF chip SPI
|
||||
|
|
@ -360,18 +435,52 @@ void IO_Configuration(void)
|
|||
|
||||
// ======================================================================================================
|
||||
|
||||
static const esp_spp_mode_t esp_spp_mode = ESP_SPP_MODE_CB;
|
||||
// ~/esp-idf/components/bt/bluedroid/api/include/esp_spp_api.h
|
||||
// esp_err_t esp_spp_write(uint32_t handle, int len, uint8_t *p_data);
|
||||
|
||||
static void esp_spp_cb(esp_spp_cb_event_t event, esp_spp_cb_param_t *param)
|
||||
{ xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
#ifdef WITH_BT_SPP
|
||||
|
||||
static const esp_spp_mode_t esp_spp_mode = ESP_SPP_MODE_CB;
|
||||
static const esp_spp_sec_t sec_mask = ESP_SPP_SEC_NONE;
|
||||
static const esp_spp_role_t role_slave = ESP_SPP_ROLE_SLAVE;
|
||||
|
||||
// static uint32_t ConnHandle=0;
|
||||
|
||||
extern "C"
|
||||
void esp_spp_cb(esp_spp_cb_event_t Event, esp_spp_cb_param_t *Param)
|
||||
{ switch (Event)
|
||||
{ case ESP_SPP_INIT_EVT:
|
||||
esp_bt_dev_set_device_name("TRACKER");
|
||||
esp_bt_gap_set_scan_mode(ESP_BT_SCAN_MODE_CONNECTABLE_DISCOVERABLE);
|
||||
esp_spp_start_srv(sec_mask, role_slave, 0, "OGN");
|
||||
break;
|
||||
/*
|
||||
case ESP_SPP_SRV_OPEN_EVT: // open connection: new handle comes
|
||||
// Param->open.handle, Param->open.rem_bda
|
||||
case ESP_SPP_SRV_CLOSE_EVT: // connection closes for given handle
|
||||
// Param->close.handle, Param->close.rem_bda
|
||||
break;
|
||||
case ESP_SPP_DATA_IND_EVT: // data is sent by the client
|
||||
// Param->data_ind.handle, Param->data_ind.data, Param->data_ind.len
|
||||
break;
|
||||
case ESP_SPP_WRITE_EVT: // data has been sent to the cielnt
|
||||
break;
|
||||
*/
|
||||
default:
|
||||
break;
|
||||
}
|
||||
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
Format_String(CONS_UART_Write, "BT_SPP: ");
|
||||
Format_Hex(CONS_UART_Write, (uint32_t)event);
|
||||
Format_Hex(CONS_UART_Write, (uint32_t)Event);
|
||||
CONS_UART_Write(' ');
|
||||
Format_String(CONS_UART_Write, "\n");
|
||||
xSemaphoreGive(CONS_Mutex); }
|
||||
xSemaphoreGive(CONS_Mutex);
|
||||
}
|
||||
|
||||
int BT_SPP_Init(void)
|
||||
{ esp_bt_controller_config_t BTconf = BT_CONTROLLER_INIT_CONFIG_DEFAULT();
|
||||
esp_err_t Err = esp_bt_controller_init(&BTconf); if(Err!=ESP_OK) return Err;
|
||||
esp_err_t Err;
|
||||
Err = esp_bt_controller_init(&BTconf); if(Err!=ESP_OK) return Err;
|
||||
Err = esp_bt_controller_enable(ESP_BT_MODE_CLASSIC_BT); if(Err!=ESP_OK) return Err;
|
||||
Err = esp_bluedroid_init(); if(Err!=ESP_OK) return Err;
|
||||
Err = esp_bluedroid_enable(); if(Err!=ESP_OK) return Err;
|
||||
|
|
@ -379,6 +488,8 @@ int BT_SPP_Init(void)
|
|||
Err = esp_spp_init(esp_spp_mode); if(Err!=ESP_OK) return Err;
|
||||
return Err; }
|
||||
|
||||
#endif // WITH_BT_SPP
|
||||
|
||||
// ======================================================================================================
|
||||
|
||||
int NVS_Init(void)
|
||||
|
|
@ -387,9 +498,9 @@ int NVS_Init(void)
|
|||
{ nvs_flash_erase();
|
||||
Err = nvs_flash_init(); }
|
||||
|
||||
if(Parameters.ReadFromNVS()!=ESP_OK)
|
||||
{ Parameters.setDefault(getUniqueID());
|
||||
Parameters.WriteToNVS(); }
|
||||
// if(Parameters.ReadFromNVS()!=ESP_OK)
|
||||
// { Parameters.setDefault(getUniqueID());
|
||||
// Parameters.WriteToNVS(); }
|
||||
|
||||
return Err; }
|
||||
|
||||
|
|
|
|||
44
main/hal.h
44
main/hal.h
|
|
@ -8,6 +8,10 @@
|
|||
#include "freertos/task.h"
|
||||
#include "freertos/queue.h"
|
||||
|
||||
// ============================================================================================================
|
||||
|
||||
#define WITH_ESP32
|
||||
|
||||
#define HARDWARE_ID 0x02
|
||||
#define SOFTWARE_ID 0x01
|
||||
|
||||
|
|
@ -15,6 +19,8 @@
|
|||
|
||||
#define WITH_RFM95 // RF chip selection
|
||||
// #define WITH_RFM69
|
||||
// #define WITH_LED_RX
|
||||
// #define WITH_LED_TX
|
||||
|
||||
// #define WITH_GPS_ENABLE // use GPS_ENABLE control line to turn the GPS ON/OFF
|
||||
#define WITH_GPS_PPS // use the PPS signal from GPS for precise time-sync.
|
||||
|
|
@ -22,18 +28,31 @@
|
|||
#define WITH_GPS_UBX // GPS understands UBX
|
||||
// #define WITH_GPS_MTK // GPS understands MTK
|
||||
// #define WITH_GPS_SRF
|
||||
// #define WITH_MAVLINK
|
||||
|
||||
#define WITH_PFLAA
|
||||
#define WITH_PFLAA // PFLAU and PFLAA for compatibility with XCsoar and LK8000
|
||||
#define WITH_CONFIG // interpret the console input: $POGNS to change parameters
|
||||
#define WITH_OLED // OLED display on the I2C
|
||||
|
||||
// #define WITH_BT_SPP // Bluetooth serial port fo smartphone/tablet link
|
||||
|
||||
// ============================================================================================================
|
||||
|
||||
#ifdef WITH_MAVLINK
|
||||
const uint8_t MAV_SysID = 1; // System-ID for MAVlink messages we send out
|
||||
extern uint8_t MAV_Seq; // sequence number for MAVlink message sent out
|
||||
#endif
|
||||
|
||||
// ============================================================================================================
|
||||
|
||||
extern SemaphoreHandle_t CONS_Mutex; // console port Mutex
|
||||
|
||||
uint64_t getUniqueID(void); // get some unique ID of the CPU/chip
|
||||
uint32_t getUniqueAddress(void); // get unique 24-bit address for the transmitted IF
|
||||
|
||||
#include "parameters.h"
|
||||
extern FlashParameters Parameters;
|
||||
|
||||
uint64_t getUniqueID(void); // get some unique ID of the CPU/chip
|
||||
|
||||
int CONS_UART_Read (uint8_t &Byte); // non-blocking
|
||||
void CONS_UART_Write (char Byte); // blocking
|
||||
int CONS_UART_Free (void); // how many bytes can be written to the transmit buffer
|
||||
|
|
@ -48,7 +67,7 @@ bool GPS_PPS_isOn(void);
|
|||
|
||||
void RFM_TransferBlock(uint8_t *Data, uint8_t Len);
|
||||
void RFM_RESET(uint8_t On); // RF module reset
|
||||
bool RFM_DIO0_isOn(void); // query the IRQ state
|
||||
bool RFM_IRQ_isOn(void); // query the IRQ state
|
||||
|
||||
#ifdef WITH_OLED
|
||||
int OLED_SetContrast(uint8_t Contrast);
|
||||
|
|
@ -57,15 +76,30 @@ int OLED_PutLine(uint8_t Line, const char *Text);
|
|||
|
||||
void LED_PCB_On (void); // LED on the PCB for vizual indications
|
||||
void LED_PCB_Off (void);
|
||||
|
||||
void LED_PCB_Flash(uint8_t Time=100); // Flash the PCB LED for a period of [ms]
|
||||
|
||||
#ifdef WITH_LED_TX
|
||||
void LED_TX_On (void);
|
||||
void LED_TX_Off (void);
|
||||
void LED_TX_Flash (uint8_t Time=100);
|
||||
#endif
|
||||
|
||||
#ifdef WITH_LED_RX
|
||||
void LED_TX_On (void);
|
||||
void LED_TX_Off (void);
|
||||
void LED_RX_Flash(uint8_t Time=100);
|
||||
#endif
|
||||
|
||||
void LED_TimerCheck(uint8_t Ticks=1);
|
||||
|
||||
void IO_Configuration(void); // Configure I/O
|
||||
|
||||
int NVS_Init(void); // initialize non-volatile-storage in the Flash
|
||||
|
||||
#ifdef WITH_BT_SPP
|
||||
int BT_SPP_Init(void);
|
||||
#endif
|
||||
|
||||
int SPIFFS_Register(const char *Path="/spiffs", const char *Label=0, size_t MaxOpenFiles=5);
|
||||
int SPIFFS_Info(size_t &Total, size_t &Used, const char *Label=0);
|
||||
|
||||
|
|
|
|||
|
|
@ -1,28 +1,35 @@
|
|||
// ======================================================================================================
|
||||
|
||||
#include "freertos/FreeRTOS.h"
|
||||
#include "freertos/FreeRTOS.h" // FreeeRTOS
|
||||
#include "freertos/task.h"
|
||||
// #include "esp_system.h"
|
||||
// #include "nvs_flash.h"
|
||||
|
||||
#include "hal.h"
|
||||
#include "hal.h" // Hardware Abstraction Layer
|
||||
|
||||
#include "rf.h"
|
||||
#include "proc.h"
|
||||
#include "gps.h"
|
||||
#include "ctrl.h"
|
||||
#include "rf.h" // RF control/transmission/reception task
|
||||
#include "proc.h" // GPS/RF process taskk
|
||||
#include "gps.h" // GPS control data acquisiton
|
||||
#include "ctrl.h" // Control task
|
||||
|
||||
extern "C"
|
||||
void app_main(void)
|
||||
{
|
||||
// printf("OGN Tracker on ESP32\n");
|
||||
|
||||
NVS_Init(); // initialize Non-Volatile-Storage in Flash and read the tracker parameters
|
||||
SPIFFS_Register(); // initialize the file system in the Flash
|
||||
CONS_Mutex = xSemaphoreCreateMutex(); // semaphore used for writing to the console
|
||||
|
||||
// BT_SPP_Init();
|
||||
NVS_Init(); // initialize Non-Volatile-Storage in Flash and read the tracker parameters
|
||||
|
||||
IO_Configuration(); // initialize the GPIO/UART/I2C/SPI for Radio, GPS, OLED, Baro
|
||||
Parameters.setDefault(getUniqueAddress()); // set default parameter values
|
||||
if(Parameters.ReadFromNVS()!=ESP_OK) // try to get parameters from NVS
|
||||
{ Parameters.WriteToNVS(); } // if did not work: try to save (default) parameters to NVS
|
||||
|
||||
SPIFFS_Register(); // initialize the file system in the Flash
|
||||
|
||||
#ifdef WITH_BT_SPP
|
||||
BT_SPP_Init(); // start BT SPP
|
||||
#endif
|
||||
|
||||
IO_Configuration(); // initialize the GPIO/UART/I2C/SPI for Radio, GPS, OLED, Baro
|
||||
|
||||
xTaskCreate(vTaskRF, "RF", 2048, 0, tskIDLE_PRIORITY+4, 0);
|
||||
xTaskCreate(vTaskPROC, "PROC", 2048, 0, tskIDLE_PRIORITY+3, 0);
|
||||
|
|
|
|||
277
main/ogn.h
277
main/ogn.h
|
|
@ -234,20 +234,19 @@ class OGN_Packet // Packet structure for the OGN tracker
|
|||
printf("\n");
|
||||
}
|
||||
|
||||
void Encode(MAV_ADSB_VEHICLE &MAV)
|
||||
{ MAV.ICAO_address = HeaderWord&0x03FFFFFF;
|
||||
MAV.lat = ((int64_t)50*DecodeLatitude()+1)/3;
|
||||
MAV.lon = ((int64_t)50*DecodeLongitude()+1)/3;
|
||||
MAV.altitude = 1000*DecodeAltitude();
|
||||
MAV.heading = 10*DecodeHeading();
|
||||
MAV.hor_velocity = 10*DecodeSpeed();
|
||||
MAV.ver_velocity = 10*DecodeClimbRate();
|
||||
MAV.flags = 0x17;
|
||||
MAV.altitude_type = 1;
|
||||
MAV.callsign[0] = 0;
|
||||
MAV.tslc = 0;
|
||||
MAV.emiter_type = 0;
|
||||
}
|
||||
void Encode(MAV_ADSB_VEHICLE *MAV)
|
||||
{ MAV->ICAO_address = HeaderWord&0x03FFFFFF;
|
||||
MAV->lat = ((int64_t)50*DecodeLatitude()+1)/3;
|
||||
MAV->lon = ((int64_t)50*DecodeLongitude()+1)/3;
|
||||
MAV->altitude = 1000*DecodeAltitude();
|
||||
MAV->heading = 10*DecodeHeading();
|
||||
MAV->hor_velocity = 10*DecodeSpeed();
|
||||
MAV->ver_velocity = 10*DecodeClimbRate();
|
||||
MAV->flags = 0x17;
|
||||
MAV->altitude_type = 1;
|
||||
MAV->callsign[0] = 0;
|
||||
MAV->tslc = 0;
|
||||
MAV->emiter_type = 0; }
|
||||
|
||||
int8_t ReadAPRS(const char *Msg) // read an APRS position message
|
||||
{ Clear();
|
||||
|
|
@ -1259,10 +1258,14 @@ class GPS_Position
|
|||
union
|
||||
{ uint8_t Flags; // bit #0 = GGA and RMC had same Time
|
||||
struct
|
||||
{ bool GPS :1; // all required GPS information has been supplied (but this is not the GPS lock status)
|
||||
bool Baro :1; // barometric information has beed supplied
|
||||
bool Ready :1; // is ready for the following treaement
|
||||
{ bool hasGPS :1; // all required GPS information has been supplied (but this is not the GPS lock status)
|
||||
bool hasBaro :1; // barometric information has beed supplied
|
||||
bool isReady :1; // is ready for the following treaement
|
||||
bool Sent :1; // has been transmitted
|
||||
bool hasTime :1; // Time has been supplied
|
||||
bool hasRMC :1; // GxRMC has been supplied
|
||||
bool hasGGA :1; // GxGGA has been supplied
|
||||
bool hasGSA :1; // GxGSA has been supplied
|
||||
} ;
|
||||
} ;
|
||||
|
||||
|
|
@ -1306,18 +1309,18 @@ class GPS_Position
|
|||
Latitude=0; Longitude=0; LatitudeCosine=3000;
|
||||
Altitude=0; GeoidSeparation=0;
|
||||
Speed=0; Heading=0; ClimbRate=0; TurnRate=0;
|
||||
StdAltitude=0; Temperature=0; }
|
||||
Temperature=0; Pressure=0; StdAltitude=0; }
|
||||
|
||||
void setDefaultDate() { Year=00; Month=1; Day=1; }
|
||||
void setDefaultTime() { Hour=0; Min=0; Sec=0; FracSec=0; }
|
||||
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
|
||||
|
||||
bool isTimeValid(void) const // is the GPS time-of-day valid ?
|
||||
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)
|
||||
|
||||
bool isDateValid(void) const // is the GPS date valid ?
|
||||
{ return (Year>=0) && (Month>=0) && (Day>=0); }
|
||||
|
||||
bool isValid(void) const // is GPS lock there ?
|
||||
bool isValid(void) const // is GPS data is valid = GPS lock
|
||||
{ if(!isTimeValid()) return 0; // is GPS time valid/present ?
|
||||
if(!isDateValid()) return 0; // is GPS date valid/present ?
|
||||
if(FixQuality==0) return 0; // Fix quality must be 1=GPS or 2=DGPS
|
||||
|
|
@ -1393,8 +1396,22 @@ class GPS_Position
|
|||
printf("\n"); }
|
||||
|
||||
int PrintLine(char *Out) const
|
||||
{ int Len=PrintDateTime(Out);
|
||||
Len+=sprintf(Out+Len, " %d/%d/%02d", FixQuality, FixMode, Satellites);
|
||||
{ int Len=0; // PrintDateTime(Out);
|
||||
Out[Len++]=hasTime?'T':'_';
|
||||
Out[Len++]=hasGPS ?'G':'_';
|
||||
Out[Len++]=hasBaro?'B':'_';
|
||||
Out[Len++]=hasRMC ?'R':'_';
|
||||
Out[Len++]=hasGGA ?'G':'_';
|
||||
Out[Len++]=hasGSA ?'G':'_';
|
||||
Out[Len++]=isValid() ?'V':'_';
|
||||
Out[Len++]=isTimeValid() ?'T':'_';
|
||||
Out[Len++]=isDateValid() ?'D':'_';
|
||||
|
||||
Out[Len++]=' '; Len+=Format_UnsDec(Out+Len, (uint16_t)Hour, 2);
|
||||
Out[Len++]=':'; Len+=Format_UnsDec(Out+Len, (uint16_t)Min, 2);
|
||||
Out[Len++]=':'; Len+=Format_UnsDec(Out+Len, (uint16_t)Sec, 2);
|
||||
Out[Len++]='.'; Len+=Format_UnsDec(Out+Len, (uint16_t)FracSec, 2);
|
||||
Len+=sprintf(Out+Len, " %d/%d/%02d", FixQuality, FixMode, Satellites);
|
||||
Out[Len++]='/'; Len+=Format_UnsDec(Out+Len, PDOP, 2, 1);
|
||||
Out[Len++]='/'; Len+=Format_UnsDec(Out+Len, HDOP, 2, 1);
|
||||
Out[Len++]='/'; Len+=Format_UnsDec(Out+Len, VDOP, 2, 1);
|
||||
|
|
@ -1406,6 +1423,9 @@ class GPS_Position
|
|||
Out[Len++]='/'; Len+=Format_SignDec(Out+Len, GeoidSeparation, 4, 1); Out[Len++]='m';
|
||||
Out[Len++]=' '; Len+=Format_UnsDec(Out+Len, Speed, 2, 1); Out[Len++]='m'; Out[Len++]='/'; Out[Len++]='s';
|
||||
Out[Len++]=' '; Len+=Format_UnsDec(Out+Len, Heading, 4, 1); Out[Len++]='d'; Out[Len++]='e'; Out[Len++]='g';
|
||||
Out[Len++]=' '; Len+=Format_SignDec(Out+Len, Temperature, 2, 1); Out[Len++]='C';
|
||||
Out[Len++]=' '; Len+=Format_UnsDec(Out+Len, Pressure/4 ); Out[Len++]='P'; Out[Len++]='a';
|
||||
Out[Len++]=' '; Len+=Format_SignDec(Out+Len, StdAltitude, 2, 1); Out[Len++]='m';
|
||||
Out[Len++]='\n'; Out[Len++]=0; return Len; }
|
||||
#endif // __AVR__
|
||||
|
||||
|
|
@ -1427,7 +1447,7 @@ class GPS_Position
|
|||
|
||||
int8_t ReadGGA(NMEA_RxMsg &RxMsg)
|
||||
{ if(RxMsg.Parms<14) return -1; // no less than 14 paramaters
|
||||
GPS = ReadTime((const char *)RxMsg.ParmPtr(0))>0; // read time and check if same as the RMC says
|
||||
hasGPS = ReadTime((const char *)RxMsg.ParmPtr(0))>0; // read time and check if same as the RMC says
|
||||
FixQuality =Read_Dec1(*RxMsg.ParmPtr(5)); if(FixQuality<0) FixQuality=0; // fix quality: 0=invalid, 1=GPS, 2=DGPS
|
||||
Satellites=Read_Dec2((const char *)RxMsg.ParmPtr(6)); // number of satellites
|
||||
if(Satellites<0) Satellites=Read_Dec1(RxMsg.ParmPtr(6)[0]);
|
||||
|
|
@ -1443,7 +1463,7 @@ class GPS_Position
|
|||
int8_t ReadGGA(const char *GGA)
|
||||
{ if( (memcmp(GGA, "$GPGGA", 6)!=0) && (memcmp(GGA, "$GNGGA", 6)!=0) ) return -1; // check if the right sequence
|
||||
uint8_t Index[20]; if(IndexNMEA(Index, GGA)<14) return -2; // index parameters and check the sum
|
||||
GPS = ReadTime(GGA+Index[0])>0;
|
||||
hasGPS = ReadTime(GGA+Index[0])>0;
|
||||
FixQuality =Read_Dec1(GGA[Index[5]]); if(FixQuality<0) FixQuality=0; // fix quality
|
||||
Satellites=Read_Dec2(GGA+Index[6]); // number of satellites
|
||||
if(Satellites<0) Satellites=Read_Dec1(GGA[Index[6]]);
|
||||
|
|
@ -1475,7 +1495,7 @@ class GPS_Position
|
|||
|
||||
int ReadRMC(NMEA_RxMsg &RxMsg)
|
||||
{ if(RxMsg.Parms<12) return -1; // no less than 12 parameters
|
||||
GPS = ReadTime((const char *)RxMsg.ParmPtr(0))>0; // read time and check if same as the GGA says
|
||||
hasGPS = ReadTime((const char *)RxMsg.ParmPtr(0))>0; // read time and check if same as the GGA says
|
||||
if(ReadDate((const char *)RxMsg.ParmPtr(8))<0) setDefaultDate(); // date
|
||||
ReadLatitude(*RxMsg.ParmPtr(3), (const char *)RxMsg.ParmPtr(2)); // Latitude
|
||||
ReadLongitude(*RxMsg.ParmPtr(5), (const char *)RxMsg.ParmPtr(4)); // Longitude
|
||||
|
|
@ -1487,7 +1507,7 @@ class GPS_Position
|
|||
int8_t ReadRMC(const char *RMC)
|
||||
{ if( (memcmp(RMC, "$GPRMC", 6)!=0) && (memcmp(RMC, "$GNRMC", 6)!=0) ) return -1; // check if the right sequence
|
||||
uint8_t Index[20]; if(IndexNMEA(Index, RMC)<12) return -2; // index parameters and check the sum
|
||||
GPS = ReadTime(RMC+Index[0])>0;
|
||||
hasGPS = ReadTime(RMC+Index[0])>0;
|
||||
if(ReadDate(RMC+Index[8])<0) setDefaultDate();
|
||||
ReadLatitude( RMC[Index[3]], RMC+Index[2]);
|
||||
ReadLongitude(RMC[Index[5]], RMC+Index[4]);
|
||||
|
|
@ -1496,56 +1516,96 @@ class GPS_Position
|
|||
calcLatitudeCosine();
|
||||
return 1; }
|
||||
|
||||
int8_t calcDifferences(GPS_Position &RefPos) // calculate climb rate and turn rate with an earlier reference position
|
||||
int16_t calcTimeDiff(GPS_Position &RefPos) const
|
||||
{ int16_t TimeDiff = (FracSec+(int16_t)Sec*100) - (RefPos.FracSec+(int16_t)RefPos.Sec*100);
|
||||
if(TimeDiff<(-3000)) TimeDiff+=6000;
|
||||
else if(TimeDiff>=3000) TimeDiff-=6000;
|
||||
return TimeDiff; } // [0.01s]
|
||||
|
||||
int16_t calcDifferences(GPS_Position &RefPos) // calculate climb rate and turn rate with an earlier reference position
|
||||
{ ClimbRate=0; TurnRate=0;
|
||||
if(RefPos.FixQuality==0) return 0;
|
||||
int TimeDiff=Sec-RefPos.Sec; if(TimeDiff<(-30)) TimeDiff+=60;
|
||||
if(TimeDiff==0) return 0;
|
||||
ClimbRate = Altitude-RefPos.Altitude;
|
||||
int16_t TimeDiff = calcTimeDiff(RefPos);
|
||||
if(TimeDiff<5) return 0;
|
||||
TurnRate = Heading-RefPos.Heading;
|
||||
if(TurnRate>1800) TurnRate-=3600; else if(TurnRate<(-1800)) TurnRate+=3600;
|
||||
if(Baro && RefPos.Baro && (abs(Altitude-StdAltitude)<2500) )
|
||||
ClimbRate = Altitude-RefPos.Altitude;
|
||||
if(hasBaro && RefPos.hasBaro && (abs(Altitude-StdAltitude)<2500) )
|
||||
{ ClimbRate = StdAltitude-RefPos.StdAltitude; }
|
||||
if(TimeDiff==1)
|
||||
if(TimeDiff==100)
|
||||
{ }
|
||||
else if(TimeDiff==2)
|
||||
else if(TimeDiff==200)
|
||||
{ ClimbRate=(ClimbRate+1)>>1;
|
||||
TurnRate=(TurnRate+1)>>1; }
|
||||
else
|
||||
{ ClimbRate/=TimeDiff;
|
||||
TurnRate/=TimeDiff; }
|
||||
return TimeDiff; }
|
||||
{ ClimbRate = ((int32_t)ClimbRate*100)/TimeDiff;
|
||||
TurnRate = ((int32_t)TurnRate *100)/TimeDiff; }
|
||||
return TimeDiff; } // [0.01s]
|
||||
|
||||
void Encode(MAV_GPS_RAW_INT &MAV) const
|
||||
{ MAV.time_usec = (int64_t)1000000*getUnixTime();
|
||||
MAV.lat = ((int64_t)50*Latitude+1)/3;
|
||||
MAV.lon = ((int64_t)50*Longitude+1)/3;
|
||||
MAV.alt = 100*Altitude;
|
||||
MAV.vel = 10*Speed;
|
||||
MAV.cog = 10*Heading;;
|
||||
MAV.fix_type = 1+FixQuality;
|
||||
MAV.eph = 10*HDOP;
|
||||
MAV.epv = 10*VDOP;
|
||||
MAV.satellites_visible = Satellites; }
|
||||
void Write(MAV_GPS_RAW_INT *MAV) const
|
||||
{ MAV->time_usec = (int64_t)1000000*getUnixTime()+10000*FracSec;
|
||||
MAV->lat = ((int64_t)50*Latitude+1)/3;
|
||||
MAV->lon = ((int64_t)50*Longitude+1)/3;
|
||||
MAV->alt = 100*Altitude;
|
||||
MAV->vel = 10*Speed;
|
||||
MAV->cog = 10*Heading;;
|
||||
MAV->fix_type = 1+FixQuality;
|
||||
MAV->eph = 10*HDOP;
|
||||
MAV->epv = 10*VDOP;
|
||||
MAV->satellites_visible = Satellites; }
|
||||
|
||||
void Read(const MAV_GPS_RAW_INT *MAV, uint64_t UnixTime_ms=0)
|
||||
{ if(UnixTime_ms) setUnixTime_ms(UnixTime_ms);
|
||||
Latitude = ((int64_t)MAV->lat*3+25)/50;
|
||||
Longitude = ((int64_t)MAV->lon*3+25)/50;
|
||||
Altitude = (MAV->alt+50)/100; // [0.1m]
|
||||
Heading = (MAV->cog+5)/10; // [0.1deg]
|
||||
Speed = (MAV->vel+5)/10; // [0.1m/s]
|
||||
HDOP = (MAV->eph+5)/10;
|
||||
VDOP = (MAV->epv+5)/10;
|
||||
// GeoidSeparation = Parameters.GeoidSepar;
|
||||
Satellites = MAV->satellites_visible;
|
||||
FixMode = MAV->fix_type-1;
|
||||
FixQuality = 1;
|
||||
hasGPS = 1; }
|
||||
|
||||
void Read(const MAV_GLOBAL_POSITION_INT *MAV, uint64_t UnixTime_ms=0)
|
||||
{ if(UnixTime_ms) setUnixTime_ms(UnixTime_ms);
|
||||
Latitude = ((int64_t)MAV->lat*3+25)/50;
|
||||
Longitude = ((int64_t)MAV->lon*3+25)/50;
|
||||
Altitude = (MAV->alt+50)/100; // [0.1m]
|
||||
ClimbRate = -MAV->vz/10; // [0.1m/s]
|
||||
Heading = (uint32_t)((uint16_t)IntAtan2(MAV->vy, MAV->vx)*(uint32_t)450+0x1000)>>13; // [0.1degC]
|
||||
Speed = IntSqrt((int32_t)MAV->vx*MAV->vx+(int32_t)MAV->vy*MAV->vy)/10; // [0.1m/s]
|
||||
// GeoidSeparation = Parameters.GeoidSepar;
|
||||
FixMode = 3;
|
||||
FixQuality = 1;
|
||||
hasGPS = 1; }
|
||||
|
||||
void Read(const MAV_SCALED_PRESSURE *MAV, uint64_t UnixTime_ms=0)
|
||||
{ if(UnixTime_ms) setUnixTime_ms(UnixTime_ms);
|
||||
Pressure = 100*4*MAV->press_abs;
|
||||
Temperature = MAV->temperature/10;
|
||||
hasBaro=1; }
|
||||
|
||||
void Encode(OGN_Packet &Packet) const
|
||||
{ Packet.Position.FixQuality = FixQuality<3 ? FixQuality:3;
|
||||
if((FixQuality>0)&&(FixMode>=2)) Packet.Position.FixMode = FixMode-2;
|
||||
{ Packet.Position.FixQuality = FixQuality<3 ? FixQuality:3; //
|
||||
if((FixQuality>0)&&(FixMode>=2)) Packet.Position.FixMode = FixMode-2; //
|
||||
else Packet.Position.FixMode = 0;
|
||||
if(PDOP>0) Packet.EncodeDOP(PDOP-10); // encode PDOP from GSA
|
||||
else Packet.EncodeDOP(HDOP-10); // or if no GSA: use HDOP
|
||||
int ShortTime=Sec;
|
||||
if(FracSec>=50) { ShortTime+=1; if(ShortTime>=60) ShortTime-=60; }
|
||||
Packet.Position.Time=ShortTime;
|
||||
Packet.EncodeLatitude(Latitude);
|
||||
Packet.EncodeLongitude(Longitude);
|
||||
Packet.EncodeSpeed(Speed);
|
||||
Packet.EncodeHeading(Heading);
|
||||
Packet.EncodeClimbRate(ClimbRate);
|
||||
Packet.EncodeTurnRate(TurnRate);
|
||||
Packet.EncodeAltitude((Altitude+5)/10);
|
||||
if(Baro) Packet.EncodeStdAltitude((StdAltitude+5)/10);
|
||||
else Packet.clrBaro();
|
||||
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
|
||||
Packet.Position.Time=ShortTime; // Time
|
||||
Packet.EncodeLatitude(Latitude); // Latitude
|
||||
Packet.EncodeLongitude(Longitude); // Longitude
|
||||
Packet.EncodeSpeed(Speed); // Speed
|
||||
Packet.EncodeHeading(Heading); // Heading = track-over-ground
|
||||
Packet.EncodeClimbRate(ClimbRate); // Climb rate
|
||||
Packet.EncodeTurnRate(TurnRate); // Turn rate
|
||||
Packet.EncodeAltitude((Altitude+5)/10); // Altitude
|
||||
if(hasBaro) Packet.EncodeStdAltitude((StdAltitude+5)/10); // Pressure altitude
|
||||
else Packet.clrBaro(); //or no-baro if pressure sensor data not there
|
||||
}
|
||||
|
||||
void EncodeStatus(OGN_Packet &Packet) const
|
||||
|
|
@ -1556,7 +1616,7 @@ class GPS_Position
|
|||
Packet.Status.FixQuality = FixQuality<3 ? FixQuality:3;
|
||||
Packet.Status.Satellites = Satellites<15 ? Satellites:15;
|
||||
Packet.EncodeAltitude((Altitude+5)/10);
|
||||
if(Baro)
|
||||
if(hasBaro)
|
||||
{ Packet.EncodeTemperature(Temperature);
|
||||
Packet.Status.Pressure = (Pressure+16)>>5; }
|
||||
else
|
||||
|
|
@ -1571,13 +1631,62 @@ class GPS_Position
|
|||
// return 3; } // => Australia + South America: upper half of 915MHz band
|
||||
// return 2; } // => USA/Canada: full 915MHz band
|
||||
|
||||
void Encode(OGN_Packet &Packet, int16_t dTime) const // Encode position which is extrapolated by the given fraction of a second
|
||||
{ Packet.Position.FixQuality = FixQuality<3 ? FixQuality:3; //
|
||||
if((FixQuality>0)&&(FixMode>=2)) Packet.Position.FixMode = FixMode-2; //
|
||||
else Packet.Position.FixMode = 0;
|
||||
if(PDOP>0) Packet.EncodeDOP(PDOP-10); // encode PDOP from GSA
|
||||
else Packet.EncodeDOP(HDOP-10); // or if no GSA: use HDOP
|
||||
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; }
|
||||
Packet.Position.Time=ShortTime; // Time
|
||||
Packet.EncodeLatitude(Lat); // Latitude
|
||||
Packet.EncodeLongitude(Lon); // Longitude
|
||||
Packet.EncodeSpeed(Speed); // Speed
|
||||
Packet.EncodeHeading(Head); // Heading = track-over-ground
|
||||
Packet.EncodeClimbRate(ClimbRate); // Climb rate
|
||||
Packet.EncodeTurnRate(TurnRate); // Turn rate
|
||||
Packet.EncodeAltitude((Alt+5)/10); // Altitude
|
||||
if(hasBaro) Packet.EncodeStdAltitude((StdAltitude+(Alt-Altitude)+5)/10); // Pressure altitude
|
||||
else Packet.clrBaro(); //or no-baro if pressure sensor data not there
|
||||
}
|
||||
|
||||
void calcExtrapolation(int32_t &Lat, int32_t &Lon, int32_t &Alt, int16_t &Head, int32_t dTime) const // extrapolate GPS position by a fraction of a second
|
||||
{ int16_t HeadAngle = ((int32_t)Heading<<12)/225; // []
|
||||
int16_t TurnAngle = (((dTime*TurnRate)/25)<<9)/225; // []
|
||||
HeadAngle += TurnAngle;
|
||||
int32_t LatSpeed = ((int32_t)Speed*Icos(HeadAngle))>>12; // [0.1m/s]
|
||||
int32_t LonSpeed = ((int32_t)Speed*Isin(HeadAngle))>>12; // [0.1m/s]
|
||||
Lat = Latitude + calcLatitudeExtrapolation (dTime, LatSpeed);
|
||||
Lon = Longitude + calcLongitudeExtrapolation(dTime, LonSpeed);
|
||||
Alt = Altitude + calcAltitudeExtrapolation(dTime);
|
||||
Head = Heading + (dTime*TurnRate)/100;
|
||||
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 calcLatitudeExtrapolation(int32_t Time, int32_t LatSpeed) const // [0.01s]
|
||||
{ return (Time*LatSpeed*177)>>15; } // [0.1m]
|
||||
|
||||
int32_t calcLongitudeExtrapolation(int32_t Time, int32_t LonSpeed) const // [0.01s]
|
||||
{ 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)>>3)/LatCosine; }
|
||||
|
||||
// static int32_t calcLatDistance(int32_t Lat1, int32_t Lat2) // [m] distance along latitude
|
||||
// { return ((int64_t)(Lat2-Lat1)*0x2f684bda+0x80000000)>>32; }
|
||||
|
||||
// static int32_t calcLatAngle32(int32_t Lat) // convert latitude to 32-bit integer angle
|
||||
// { return ((int64_t)Lat*2668799779u+0x4000000)>>27; }
|
||||
|
||||
static int16_t calcLatAngle16(int32_t Lat) // convert latitude to 16-bit integer angle
|
||||
static int16_t calcLatAngle16(int32_t Lat) // convert latitude to 16-bit integer angle
|
||||
{ return ((int64_t)Lat*1303125+0x80000000)>>32; }
|
||||
|
||||
// static int32_t calcLatCosine(int32_t LatAngle) // calculate the cosine of the latitude 32-bit integer angle
|
||||
|
|
@ -1667,28 +1776,28 @@ class GPS_Position
|
|||
else if(DOP>255) DOP=255;
|
||||
VDOP=DOP; return 0; }
|
||||
|
||||
int8_t ReadTime(const char *Value)
|
||||
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 Prev; int8_t Same=1;
|
||||
Prev=Hour;
|
||||
Hour=Read_Dec2(Value); if(Hour<0) return -1; // read hour (two digits)
|
||||
Hour=Read_Dec2(Value); if(Hour<0) return -1; // read hour (two digits), return when invalid
|
||||
if(Prev!=Hour) Same=0;
|
||||
Prev=Min;
|
||||
Min=Read_Dec2(Value+2); if(Min<0) return -1; // read minute (two digits)
|
||||
Min=Read_Dec2(Value+2); if(Min<0) return -1; // read minute (two digits), return when invalid
|
||||
if(Prev!=Min) Same=0;
|
||||
Prev=Sec;
|
||||
Sec=Read_Dec2(Value+4); if(Sec<0) return -1; // read second (two digits)
|
||||
Sec=Read_Dec2(Value+4); if(Sec<0) return -1; // read second (two digits), return when invalid
|
||||
if(Prev!=Sec) Same=0;
|
||||
Prev=FracSec;
|
||||
if(Value[6]=='.') // is there a second fraction ?
|
||||
{ FracSec=Read_Dec2(Value+7); if(FracSec<0) return -1; }
|
||||
if(Prev!=FracSec) Same=0;
|
||||
return Same; } // return 1 when time did not change (both RMC and GGA were for same time)
|
||||
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
|
||||
return Same; } // return 1 when time did not change (both RMC and GGA were for same time)
|
||||
|
||||
int8_t ReadDate(const char *Param)
|
||||
{ Day=Read_Dec2(Param); if(Day<0) return -1; // read calendar year (two digits - thus need to be extended to four)
|
||||
Month=Read_Dec2(Param+2); if(Month<0) return -1; // read calendar month
|
||||
Year=Read_Dec2(Param+4); if(Year<0) return -1; // read calendar day
|
||||
return 0; }
|
||||
int8_t ReadDate(const char *Param) // read the field DDMMYY
|
||||
{ Day=Read_Dec2(Param); if(Day<0) return -1; // read calendar year (two digits - thus need to be extended to four)
|
||||
Month=Read_Dec2(Param+2); if(Month<0) return -1; // read calendar month
|
||||
Year=Read_Dec2(Param+4); if(Year<0) return -1; // read calendar day
|
||||
return 0; } // return 0 when field valid and was read correctly
|
||||
|
||||
int8_t static IndexNMEA(uint8_t Index[20], const char *Seq) // index parameters and verify the NMEA checksum
|
||||
{ if(Seq[0]!='$') return -1;
|
||||
|
|
@ -1724,6 +1833,7 @@ class GPS_Position
|
|||
Hour = DayTime/SecsPerHour; DayTime -= (uint32_t)Hour*SecsPerHour; //
|
||||
Min = DayTime/SecsPerMin; DayTime -= (uint16_t)Min*SecsPerMin;
|
||||
Sec = DayTime;
|
||||
FracSec=0;
|
||||
Days -= 10957+1; // [day] since 2000 minus 1 day
|
||||
Year = (Days*4)/((365*4)+1); // [year] since 1970
|
||||
Days -= 365*Year + (Year/4);
|
||||
|
|
@ -1731,7 +1841,12 @@ class GPS_Position
|
|||
Day = Days-(uint16_t)Month*31+1; Month++;
|
||||
uint32_t CheckTime = getUnixTime();
|
||||
if(CheckTime<Time) incrDate((Time-CheckTime)/SecsPerDay);
|
||||
}
|
||||
hasTime=1; }
|
||||
|
||||
void setUnixTime_ms(uint64_t Time_ms)
|
||||
{ uint32_t Time=Time_ms/1000;
|
||||
setUnixTime(Time);
|
||||
FracSec = (Time_ms-(uint64_t)Time*1000)/10; }
|
||||
|
||||
private:
|
||||
|
||||
|
|
|
|||
|
|
@ -4,11 +4,16 @@
|
|||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "nvs.h"
|
||||
// #include "flashsize.h"
|
||||
// #include "uniqueid.h"
|
||||
#include "hal.h"
|
||||
|
||||
// #include "stm32f10x_flash.h"
|
||||
#ifdef WITH_ESP32
|
||||
#include "nvs.h"
|
||||
#endif
|
||||
|
||||
#ifdef WITH_STM32
|
||||
#include "stm32f10x_flash.h"
|
||||
#include "flashsize.h"
|
||||
#endif
|
||||
|
||||
#include "nmea.h"
|
||||
#include "format.h"
|
||||
|
|
@ -26,19 +31,32 @@ class FlashParameters
|
|||
bool Stealth:1;
|
||||
} ;
|
||||
} ;
|
||||
|
||||
int16_t RFchipFreqCorr; // [10Hz] frequency correction for crystal frequency offset
|
||||
int8_t RFchipTxPower; // [dBm] highest bit set => HW module (up to +20dBm Tx power)
|
||||
int8_t RFchipTempCorr; // [degC] correction to the temperature measured in the RF chip
|
||||
|
||||
uint32_t CONbaud; // [bps] Console baud rate
|
||||
uint16_t PressCorr; // [0.25Pa] pressure correction for the baro
|
||||
|
||||
int16_t PressCorr; // [0.25Pa] pressure correction for the baro
|
||||
union
|
||||
{ uint16_t Flags;
|
||||
{ uint8_t Flags;
|
||||
struct
|
||||
{ bool SaveToFlash:1; // Save parameters from the config file to Flash
|
||||
bool hasBT:1; // has BT interface on the console
|
||||
bool BT_ON:1; // BT on after power up
|
||||
} ;
|
||||
} ; //
|
||||
int8_t TimeCorr; // [sec] it appears for ArduPilot you need to correct time by 3 seconds
|
||||
|
||||
int16_t GeoidSepar; // [0.1m] Geoid-Separation, apparently ArduPilot MAVlink does not give this value (although present in the format)
|
||||
uint16_t SoftPPSdelay; // [ms]
|
||||
char Pilot[16];
|
||||
char Manuf[16];
|
||||
char Type[16];
|
||||
char Reg[16];
|
||||
char Base[16];
|
||||
char ICE[16];
|
||||
// char BTname[8];
|
||||
// char BTpin[4];
|
||||
// char Pilot[16];
|
||||
|
|
@ -59,8 +77,11 @@ class FlashParameters
|
|||
public:
|
||||
// void setDefault(void) { setDefaults(UniqueID[0] ^ UniqueID[1] ^ UniqueID[2]); }
|
||||
|
||||
void setDefault(uint32_t UniqueID)
|
||||
{ AcftID = 0x07000000 | (UniqueID&0x00FFFFFF);
|
||||
void setDefault(void)
|
||||
{ setDefault(getUniqueAddress()); }
|
||||
|
||||
void setDefault(uint32_t UniqueAddr)
|
||||
{ AcftID = 0x07000000 | (UniqueAddr&0x00FFFFFF);
|
||||
RFchipFreqCorr = 0; // [10Hz]
|
||||
#ifdef WITH_RFM69W
|
||||
RFchipTxPower = 13; // [dBm] for RFM69W
|
||||
|
|
@ -68,19 +89,20 @@ class FlashParameters
|
|||
RFchipTxPower = 0x80 | 14; // [dBm] for RFM69HW
|
||||
#endif
|
||||
RFchipTempCorr = 0; // [degC]
|
||||
PressCorr = 0; // [0.25Pa]
|
||||
CONbaud = 115200; // [bps]
|
||||
PressCorr = 0; // [0.25Pa]
|
||||
TimeCorr = 0; // [sec]
|
||||
GeoidSepar = 470; // [0.1m]
|
||||
|
||||
Pilot[0] = 0;
|
||||
Manuf[0] = 0;
|
||||
Type[0] = 0;
|
||||
Reg[0] = 0;
|
||||
Base[0] = 0;
|
||||
ICE[0] = 0;
|
||||
}
|
||||
|
||||
uint32_t static CheckSum(const uint32_t *Word, uint32_t Words) // calculate check-sum of pointed data
|
||||
{ uint32_t Check=CheckInit;
|
||||
for(uint32_t Idx=0; Idx<Words; Words++)
|
||||
{ Check+=Word[Idx]; }
|
||||
return Check; }
|
||||
|
||||
uint32_t CheckSum(void) const // calc. check-sum of this class data
|
||||
{ return CheckSum((uint32_t *)this, sizeof(FlashParameters)/sizeof(uint32_t) ); }
|
||||
|
||||
#ifdef WITH_ESP32
|
||||
esp_err_t WriteToNVS(const char *Name="Parameters", const char *NameSpace="TRACKER")
|
||||
{ nvs_handle Handle;
|
||||
esp_err_t Err = nvs_open(NameSpace, NVS_READWRITE, &Handle);
|
||||
|
|
@ -95,13 +117,23 @@ class FlashParameters
|
|||
esp_err_t Err = nvs_open(NameSpace, NVS_READWRITE, &Handle);
|
||||
if(Err!=ESP_OK) return Err;
|
||||
size_t Size=0;
|
||||
Err = nvs_get_blob(Handle, Name, 0, &Size);
|
||||
if( (Err==ESP_OK) && (Size==sizeof(FlashParameters)) )
|
||||
Err = nvs_get_blob(Handle, Name, this, &Size);
|
||||
Err = nvs_get_blob(Handle, Name, 0, &Size); // get the Size of the blob in the Flash
|
||||
if( (Err==ESP_OK) && (Size<=sizeof(FlashParameters)) )
|
||||
Err = nvs_get_blob(Handle, Name, this, &Size); // read the Blob from the Flash
|
||||
nvs_close(Handle);
|
||||
return Err; }
|
||||
#endif // WITH_ESP32
|
||||
|
||||
#ifdef WITH_STM32
|
||||
uint32_t static CheckSum(const uint32_t *Word, uint32_t Words) // calculate check-sum of pointed data
|
||||
{ uint32_t Check=CheckInit;
|
||||
for(uint32_t Idx=0; Idx<Words; Idx++)
|
||||
{ Check+=Word[Idx]; }
|
||||
return Check; }
|
||||
|
||||
uint32_t CheckSum(void) const // calc. check-sum of this class data
|
||||
{ return CheckSum((uint32_t *)this, sizeof(FlashParameters)/sizeof(uint32_t) ); }
|
||||
|
||||
/*
|
||||
static uint32_t *DefaultFlashAddr(void) { return FlashStart+((uint32_t)(getFlashSizeKB()-1)<<8); }
|
||||
|
||||
int8_t ReadFromFlash(uint32_t *Addr=0) // read parameters from Flash
|
||||
|
|
@ -137,12 +169,16 @@ class FlashParameters
|
|||
FLASH_Lock(); // re-lock Flash
|
||||
if(CheckSum(Addr, Words)!=Addr[Words]) return -1; // verify check-sum in Flash
|
||||
return 0; }
|
||||
*/
|
||||
#endif // WITH_STM32
|
||||
|
||||
uint8_t Print(char *Line) // print parameters on a single line, suitable for console output
|
||||
{ uint8_t Len=0;
|
||||
Line[Len++]=HexDigit(AcftType); Line[Len++]=':';
|
||||
Line[Len++]=HexDigit(AddrType); Line[Len++]=':';
|
||||
Len+=Format_Hex(Line+Len, Address, 6);
|
||||
uint32_t DefaultAddr=getUniqueAddress();
|
||||
if(Address!=DefaultAddr)
|
||||
{ Line[Len++]='/'; Len+=Format_Hex(Line+Len, DefaultAddr, 6); }
|
||||
#ifdef WITH_RFM69
|
||||
Len+=Format_String(Line+Len, " RFM69");
|
||||
if(isTxTypeHW()) Line[Len++]='H';
|
||||
|
|
@ -161,6 +197,13 @@ class FlashParameters
|
|||
Line[Len]=0;
|
||||
return Len; }
|
||||
|
||||
int ReadPOGNS(NMEA_RxMsg &NMEA)
|
||||
{ int Count=0;
|
||||
for(uint8_t Idx=0; ; Idx++)
|
||||
{ char *Parm = (char *)NMEA.ParmPtr(Idx); if(Parm==0) break;
|
||||
if(ReadLine(Parm)) Count++; }
|
||||
return Count; }
|
||||
/*
|
||||
int ReadPOGNS(NMEA_RxMsg &NMEA)
|
||||
{ const char *Parm; int8_t Val;
|
||||
Parm = (const char *)NMEA.ParmPtr(0); // [0..15] aircraft-type: 1=glider, 2=tow plane, 3=helicopter, ...
|
||||
|
|
@ -193,29 +236,37 @@ class FlashParameters
|
|||
Len=Read_UnsDec(BaudRate, Parm);
|
||||
if( (Len>0) && (BaudRate<=230400) ) CONbaud = BaudRate;
|
||||
return 0; }
|
||||
*/
|
||||
static bool isStringChar (char ch) // characters accepted as part of the string values
|
||||
{ if( (ch>='0') && (ch<='9') ) return 1; // numbers
|
||||
if( (ch>='A') && (ch<='Z') ) return 1; // uppercase letters
|
||||
if( (ch>='a') && (ch<='z') ) return 1; // lowercase letters
|
||||
if(ch=='.') return 1;
|
||||
if(ch=='-') return 1;
|
||||
if(ch=='+') return 1;
|
||||
if(ch=='_') return 1;
|
||||
return 0; }
|
||||
|
||||
static char *SkipBlanks(char *Inp)
|
||||
static int8_t Read_String(char *Value, const char *Inp, uint8_t MaxLen)
|
||||
{ const char *Val = SkipBlanks(Inp);
|
||||
uint8_t Idx;
|
||||
for(Idx=0; Idx<MaxLen; Idx++)
|
||||
{ char ch=(*Val); if(ch==0) break;
|
||||
if(!isStringChar(ch)) break;
|
||||
Value[Idx] = ch;
|
||||
Val++; }
|
||||
for( ; Idx<MaxLen; Idx++)
|
||||
Value[Idx]=0;
|
||||
return Val-Inp; }
|
||||
|
||||
static const char *SkipBlanks(const char *Inp)
|
||||
{ for( ; ; )
|
||||
{ char ch=(*Inp); if(ch==0) break;
|
||||
if(ch>' ') break;
|
||||
Inp++; }
|
||||
return Inp; }
|
||||
|
||||
bool ReadLine(char *Line)
|
||||
{ char *Name = SkipBlanks(Line); if((*Name)==0) return 0;
|
||||
Line = Name;
|
||||
for( ; ; )
|
||||
{ char ch=(*Line);
|
||||
if(ch<=' ') break;
|
||||
if(ch=='=') break;
|
||||
Line++; }
|
||||
char *NameEnd=Line;
|
||||
Line=SkipBlanks(Line); if((*Line)!='=') return 0;
|
||||
char *Value = SkipBlanks(Line+1); if((*Value)<=' ') return 0;
|
||||
*NameEnd=0;
|
||||
return ReadParam(Name, Value); }
|
||||
|
||||
bool ReadParam(const char *Name, const char *Value)
|
||||
bool ReadParam(const char *Name, const char *Value) // interprete "Name = Value" line
|
||||
{ if(strcmp(Name, "Address")==0)
|
||||
{ uint32_t Addr=0; if(Read_Int(Addr, Value)<=0) return 0;
|
||||
Address=Addr; return 1; }
|
||||
|
|
@ -237,16 +288,47 @@ class FlashParameters
|
|||
if(strcmp(Name, "PressCorr")==0)
|
||||
{ int32_t Corr=0; if(Read_Float1(Corr, Value)<=0) return 0;
|
||||
PressCorr=4*Corr/10; return 1; }
|
||||
if(strcmp(Name, "TimeCorr")==0)
|
||||
{ int32_t Corr=0; if(Read_Int(Corr, Value)<=0) return 0;
|
||||
TimeCorr=Corr; return 1; }
|
||||
if(strcmp(Name, "GeoidSepar")==0)
|
||||
{ return Read_Float1(GeoidSepar, Value)<=0; }
|
||||
if(strcmp(Name, "Pilot")==0)
|
||||
{ return Read_String(Pilot, Value, 16)<=0; }
|
||||
if(strcmp(Name, "Manuf")==0)
|
||||
{ return Read_String(Manuf, Value, 16 )<=0; }
|
||||
if(strcmp(Name, "Type")==0)
|
||||
{ return Read_String(Type , Value, 16)<=0; }
|
||||
if(strcmp(Name, "Reg")==0)
|
||||
{ return Read_String(Reg , Value, 16)<=0; }
|
||||
if(strcmp(Name, "Base")==0)
|
||||
{ return Read_String(Base , Value, 16)<=0; }
|
||||
if(strcmp(Name, "ICE")==0)
|
||||
{ return Read_String(ICE , Value, 16)<=0; }
|
||||
return 0; }
|
||||
|
||||
bool ReadLine(char *Line) // read a parameter line
|
||||
{ char *Name = (char *)SkipBlanks(Line); if((*Name)==0) return 0; // skip blanks and get to parameter name
|
||||
Line = Name;
|
||||
for( ; ; ) // step through the characters
|
||||
{ char ch=(*Line); // next character
|
||||
if(ch<=' ') break; // break at blank
|
||||
if(ch=='=') break; // or equal sign
|
||||
Line++; }
|
||||
char *NameEnd=Line; // remember where the parameter name ends
|
||||
Line = (char *)SkipBlanks(Line); if((*Line)!='=') return 0; // next should be the equal sign
|
||||
char *Value = (char *)SkipBlanks(Line+1); if((*Value)<=' ') return 0;
|
||||
*NameEnd=0; // put NULL character just after the parameter name
|
||||
return ReadParam(Name, Value); }
|
||||
|
||||
int ReadFile(FILE *File)
|
||||
{ char Line[80];
|
||||
size_t Lines=0;
|
||||
for( ; ; )
|
||||
{ if(fgets(Line, 80, File)==0) break;
|
||||
if(strchr(Line, '\n')==0) break;
|
||||
if(ReadLine(Line)) Lines++; }
|
||||
return Lines; }
|
||||
{ char Line[80]; // line buffer
|
||||
size_t Lines=0; // count interpreted lines
|
||||
for( ; ; ) // loop over lines
|
||||
{ if(fgets(Line, 80, File)==0) break; // break on EOF or other trouble reading the file
|
||||
if(strchr(Line, '\n')==0) break; // if no NL then break, line was too long
|
||||
if(ReadLine(Line)) Lines++; } // interprete the line, count if positive
|
||||
return Lines; } // return number of interpreted lines
|
||||
|
||||
int ReadFile(const char *Name = "/spiffs/TRACKER.CFG")
|
||||
{ FILE *File=fopen(Name, "rt"); if(File==0) return 0;
|
||||
|
|
@ -254,50 +336,84 @@ class FlashParameters
|
|||
fclose(File); return Lines; }
|
||||
|
||||
int Write_Hex(char *Line, const char *Name, uint32_t Value, uint8_t Digits)
|
||||
{ uint8_t Len=Format_String(Line, Name);
|
||||
Len+=Format_String(Line+Len, "=0x");
|
||||
{ uint8_t Len=Format_String(Line, Name, 12, 0);
|
||||
Len+=Format_String(Line+Len, " = 0x");
|
||||
Len+=Format_Hex(Line+Len, Value, Digits);
|
||||
Len+=Format_String(Line+Len, ";");
|
||||
Line[Len]=0; return Len; }
|
||||
|
||||
int Write_UnsDec(char *Line, const char *Name, uint32_t Value)
|
||||
{ uint8_t Len=Format_String(Line, Name);
|
||||
Len+=Format_String(Line+Len, "=");
|
||||
{ uint8_t Len=Format_String(Line, Name, 12, 0);
|
||||
Len+=Format_String(Line+Len, " = ");
|
||||
Len+=Format_UnsDec(Line+Len, Value);
|
||||
Len+=Format_String(Line+Len, ";");
|
||||
Line[Len]=0; return Len; }
|
||||
|
||||
int Write_SignDec(char *Line, const char *Name, int32_t Value)
|
||||
{ uint8_t Len=Format_String(Line, Name);
|
||||
Len+=Format_String(Line+Len, "=");
|
||||
{ uint8_t Len=Format_String(Line, Name, 12, 0);
|
||||
Len+=Format_String(Line+Len, " = ");
|
||||
Len+=Format_SignDec(Line+Len, Value);
|
||||
Len+=Format_String(Line+Len, ";");
|
||||
Line[Len]=0; return Len; }
|
||||
|
||||
int Write_Float1(char *Line, const char *Name, int32_t Value)
|
||||
{ uint8_t Len=Format_String(Line, Name);
|
||||
Len+=Format_String(Line+Len, "=");
|
||||
{ uint8_t Len=Format_String(Line, Name, 12, 0);
|
||||
Len+=Format_String(Line+Len, " = ");
|
||||
Len+=Format_SignDec(Line+Len, Value, 2, 1);
|
||||
Len+=Format_String(Line+Len, ";");
|
||||
Line[Len]=0; return Len; }
|
||||
|
||||
int Write_String(char *Line, const char *Name, char *Value, uint8_t MaxLen=16)
|
||||
{ uint8_t Len=Format_String(Line, Name, 12, 0);
|
||||
Len+=Format_String(Line+Len, " = ");
|
||||
Len+=Format_String(Line+Len, Value, 0, MaxLen);
|
||||
Line[Len]=0; return Len; }
|
||||
|
||||
int WriteFile(FILE *File)
|
||||
{ char Line[80];
|
||||
Write_Hex (Line, "Address" , Address , 6); strcat(Line, " # [24-bit]\n"); if(fputs(Line, File)==EOF) return EOF;
|
||||
Write_Hex (Line, "AddrType", AddrType, 1); strcat(Line, " # [2-bit]\n"); if(fputs(Line, File)==EOF) return EOF;
|
||||
Write_Hex (Line, "AcftType", AcftType, 1); strcat(Line, " # [4-bit]\n"); if(fputs(Line, File)==EOF) return EOF;
|
||||
Write_UnsDec (Line, "Console", CONbaud); strcat(Line, " # [bps]\n"); if(fputs(Line, File)==EOF) return EOF;
|
||||
Write_SignDec(Line, "TxPower", getTxPower()); strcat(Line, " # [ dBm]\n"); if(fputs(Line, File)==EOF) return EOF;
|
||||
Write_Float1 (Line, "FreqCorr", (int32_t)RFchipFreqCorr/10); strcat(Line, " # [kHz]\n"); if(fputs(Line, File)==EOF) return EOF;
|
||||
Write_SignDec(Line, "TempCorr", (int32_t)RFchipTempCorr ); strcat(Line, " # [degC]\n"); if(fputs(Line, File)==EOF) return EOF;
|
||||
Write_Float1 (Line, "PressCorr",(int32_t)PressCorr*10/4 ); strcat(Line, " # [Pa]\n"); if(fputs(Line, File)==EOF) return EOF;
|
||||
return 8; }
|
||||
Write_Hex (Line, "Address" , Address , 6); strcat(Line, " # [24-bit]\n"); if(fputs(Line, File)==EOF) return EOF;
|
||||
Write_Hex (Line, "AddrType" , AddrType, 1); strcat(Line, " # [2-bit]\n"); if(fputs(Line, File)==EOF) return EOF;
|
||||
Write_Hex (Line, "AcftType" , AcftType, 1); strcat(Line, " # [4-bit]\n"); if(fputs(Line, File)==EOF) return EOF;
|
||||
Write_UnsDec (Line, "Console" , CONbaud ); strcat(Line, " # [ bps]\n"); if(fputs(Line, File)==EOF) return EOF;
|
||||
Write_SignDec(Line, "TxPower" , getTxPower() ); strcat(Line, " # [ dBm]\n"); if(fputs(Line, File)==EOF) return EOF;
|
||||
Write_Float1 (Line, "FreqCorr" , (int32_t)RFchipFreqCorr/10); strcat(Line, " # [ kHz]\n"); if(fputs(Line, File)==EOF) return EOF;
|
||||
Write_SignDec(Line, "TempCorr" , (int32_t)RFchipTempCorr ); strcat(Line, " # [ degC]\n"); if(fputs(Line, File)==EOF) return EOF;
|
||||
Write_Float1 (Line, "PressCorr" , (int32_t)PressCorr*10/4 ); strcat(Line, " # [ Pa]\n"); if(fputs(Line, File)==EOF) return EOF;
|
||||
Write_SignDec(Line, "TimeCorr" , (int32_t)TimeCorr ); strcat(Line, " # [ s]\n"); if(fputs(Line, File)==EOF) return EOF;
|
||||
Write_Float1 (Line, "GeoidSepar", GeoidSepar ); strcat(Line, " # [ m]\n"); if(fputs(Line, File)==EOF) return EOF;
|
||||
Write_String (Line, "Pilot" , Pilot ); strcat(Line, " # [16char]\n"); if(fputs(Line, File)==EOF) return EOF;
|
||||
Write_String (Line, "Manuf" , Manuf ); strcat(Line, " # [16char]\n"); if(fputs(Line, File)==EOF) return EOF;
|
||||
Write_String (Line, "Type" , Type ); strcat(Line, " # [16char]\n"); if(fputs(Line, File)==EOF) return EOF;
|
||||
Write_String (Line, "Reg" , Reg ); strcat(Line, " # [16char]\n"); if(fputs(Line, File)==EOF) return EOF;
|
||||
Write_String (Line, "Base" , Base ); strcat(Line, " # [16char]\n"); if(fputs(Line, File)==EOF) return EOF;
|
||||
Write_String (Line, "ICE" , ICE ); strcat(Line, " # [16char]\n"); if(fputs(Line, File)==EOF) return EOF;
|
||||
return 16; }
|
||||
|
||||
int WriteFile(const char *Name = "/spiffs/TRACKER.CFG")
|
||||
{ FILE *File=fopen(Name, "wt"); if(File==0) return 0;
|
||||
int Lines=WriteFile(File);
|
||||
fclose(File); return Lines; }
|
||||
|
||||
void Write(void (*Output)(char))
|
||||
{ char Line[80];
|
||||
Write_Hex (Line, "Address" , Address , 6); strcat(Line, " # [24-bit]\n"); Format_String(Output, Line);
|
||||
Write_Hex (Line, "AddrType" , AddrType, 1); strcat(Line, " # [2-bit]\n"); Format_String(Output, Line);
|
||||
Write_Hex (Line, "AcftType" , AcftType, 1); strcat(Line, " # [4-bit]\n"); Format_String(Output, Line);
|
||||
Write_UnsDec (Line, "Console" , CONbaud ); strcat(Line, " # [ bps]\n"); Format_String(Output, Line);
|
||||
Write_SignDec(Line, "TxPower" , getTxPower() ); strcat(Line, " # [ dBm]\n"); Format_String(Output, Line);
|
||||
Write_Float1 (Line, "FreqCorr" , (int32_t)RFchipFreqCorr/10); strcat(Line, " # [ kHz]\n"); Format_String(Output, Line);
|
||||
Write_SignDec(Line, "TempCorr" , (int32_t)RFchipTempCorr ); strcat(Line, " # [ degC]\n"); Format_String(Output, Line);
|
||||
Write_Float1 (Line, "PressCorr" , (int32_t)PressCorr*10/4 ); strcat(Line, " # [ Pa]\n"); Format_String(Output, Line);
|
||||
Write_SignDec(Line, "TimeCorr" , (int32_t)TimeCorr ); strcat(Line, " # [ s]\n"); Format_String(Output, Line);
|
||||
Write_Float1 (Line, "GeoidSepar", GeoidSepar ); strcat(Line, " # [ m]\n"); Format_String(Output, Line);
|
||||
Write_String (Line, "Pilot" , Pilot ); strcat(Line, " # [16char]\n"); Format_String(Output, Line);
|
||||
Write_String (Line, "Manuf" , Manuf ); strcat(Line, " # [16char]\n"); Format_String(Output, Line);
|
||||
Write_String (Line, "Type" , Type ); strcat(Line, " # [16char]\n"); Format_String(Output, Line);
|
||||
Write_String (Line, "Reg" , Reg ); strcat(Line, " # [16char]\n"); Format_String(Output, Line);
|
||||
Write_String (Line, "Base" , Base ); strcat(Line, " # [16char]\n"); Format_String(Output, Line);
|
||||
Write_String (Line, "ICE" , ICE ); strcat(Line, " # [16char]\n"); Format_String(Output, Line);
|
||||
}
|
||||
|
||||
} ;
|
||||
|
||||
#endif // __PARAMETERS_H__
|
||||
|
|
|
|||
166
main/proc.cpp
166
main/proc.cpp
|
|
@ -2,12 +2,17 @@
|
|||
|
||||
#include "hal.h"
|
||||
#include "proc.h"
|
||||
#include "ctrl.h"
|
||||
|
||||
#include "ogn.h"
|
||||
|
||||
#include "rf.h"
|
||||
#include "gps.h"
|
||||
|
||||
#ifdef WITH_FLASHLOG
|
||||
#include "flashlog.h"
|
||||
#endif
|
||||
|
||||
static char Line[128]; // for printing out to serial port, etc.
|
||||
|
||||
static LDPC_Decoder Decoder; // error corrector for the OGN Gallager code
|
||||
|
|
@ -51,15 +56,18 @@ static void CleanRelayQueue(uint32_t Time, uint32_t Delay=20)
|
|||
static void ReadStatus(OGN_TxPacket &StatPacket)
|
||||
{
|
||||
|
||||
// uint16_t MCU_VCC = Measure_MCU_VCC(); // [0.001V]
|
||||
// StatPacket.Packet.EncodeVoltage(((MCU_VCC<<3)+62)/125); // [1/64V]
|
||||
// int16_t MCU_Temp = Measure_MCU_Temp(); // [0.1degC]
|
||||
// if(StatPacket.Packet.Status.Pressure==0) StatPacket.Packet.EncodeTemperature(MCU_Temp); // [0.1degC]
|
||||
#ifdef WITH_STM32
|
||||
uint16_t MCU_VCC = Measure_MCU_VCC(); // [0.001V]
|
||||
StatPacket.Packet.EncodeVoltage(((MCU_VCC<<3)+62)/125); // [1/64V]
|
||||
int16_t MCU_Temp = Measure_MCU_Temp(); // [0.1degC]
|
||||
#endif
|
||||
|
||||
if(StatPacket.Packet.Status.Pressure==0) StatPacket.Packet.EncodeTemperature(RF_Temp*10); // [0.1degC]
|
||||
StatPacket.Packet.Status.RadioNoise = RX_AverRSSI; // [-0.5dBm] write radio noise to the status packet
|
||||
|
||||
StatPacket.Packet.Status.TxPower = Parameters.getTxPower()-4;
|
||||
|
||||
uint16_t RxRate = RX_OGN_Count64;
|
||||
uint16_t RxRate = RX_OGN_Count64+1;
|
||||
uint8_t RxRateLog2=0; RxRate>>=1; while(RxRate) { RxRate>>=1; RxRateLog2++; }
|
||||
StatPacket.Packet.Status.RxRate = RxRateLog2;
|
||||
|
||||
|
|
@ -84,7 +92,7 @@ static void ReadStatus(OGN_TxPacket &StatPacket)
|
|||
// LogLine(Line);
|
||||
// if(CONS_UART_Free()>=128)
|
||||
{ xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
Format_String(CONS_UART_Write, Line, Len); // send the NMEA out to the console
|
||||
Format_String(CONS_UART_Write, Line, 0, Len); // send the NMEA out to the console
|
||||
xSemaphoreGive(CONS_Mutex); }
|
||||
#ifdef WITH_SDLOG
|
||||
if(Log_Free()>=128)
|
||||
|
|
@ -99,30 +107,21 @@ static void ReadStatus(OGN_TxPacket &StatPacket)
|
|||
|
||||
static void ProcessRxPacket(OGN_RxPacket *RxPacket, uint8_t RxPacketIdx) // process every (correctly) received packet
|
||||
{ int32_t LatDist=0, LonDist=0; uint8_t Warn=0;
|
||||
if( RxPacket->Packet.Header.Other || RxPacket->Packet.Header.Encrypted ) return ; // status packet or encrypted: ignore
|
||||
if( RxPacket->Packet.Header.Other || RxPacket->Packet.Header.Encrypted ) return ; // status packet or encrypted: ignore
|
||||
uint8_t MyOwnPacket = ( RxPacket->Packet.Header.Address == Parameters.Address )
|
||||
&& ( RxPacket->Packet.Header.AddrType == Parameters.AddrType );
|
||||
if(MyOwnPacket) return; // don't process my own (relayed) packets
|
||||
bool DistOK = RxPacket->Packet.calcDistanceVector(LatDist, LonDist, GPS_Latitude, GPS_Longitude, GPS_LatCosine)>=0;
|
||||
if(DistOK)
|
||||
{ if(!MyOwnPacket)
|
||||
{ RxPacket->calcRelayRank(GPS_Altitude/10); // calculate the relay-rank (priority for relay)
|
||||
RelayQueue.addNew(RxPacketIdx); }
|
||||
uint8_t Len=RxPacket->WritePOGNT(Line); // print on the console as $POGNT
|
||||
if(!MyOwnPacket)
|
||||
{ xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
Format_String(CONS_UART_Write, Line, Len);
|
||||
xSemaphoreGive(CONS_Mutex); }
|
||||
#ifdef WITH_LOOKOUT
|
||||
const LookOut_Target *Tgt=Look.ProcessTarget(RxPacket->Packet); // process the received target postion
|
||||
if(Tgt) Warn=Tgt->WarnLevel; // remember warning level of this target
|
||||
{ RxPacket->calcRelayRank(GPS_Altitude/10); // calculate the relay-rank (priority for relay)
|
||||
RelayQueue.addNew(RxPacketIdx);
|
||||
uint8_t Len=RxPacket->WritePOGNT(Line); // print on the console as $POGNT
|
||||
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
Format_String(CONS_UART_Write, Line, 0, Len);
|
||||
xSemaphoreGive(CONS_Mutex);
|
||||
#ifdef WITH_BEEPER
|
||||
if(KNOB_Tick>12) Play(Play_Vol_1 | Play_Oct_2 | (7+2*Warn), 3+16*Warn); // if Knob>12 => make a beep for every received packet
|
||||
if(KNOB_Tick>12) Play(Play_Vol_1 | Play_Oct_2 | 7, 3); // if Knob>12 => make a beep for every received packet
|
||||
#endif
|
||||
#else // WITH_LOOKOUT
|
||||
#ifdef WITH_BEEPER
|
||||
if(KNOB_Tick>12) Play(Play_Vol_1 | Play_Oct_2 | 7, 3); // if Knob>12 => make a beep for every received packet
|
||||
#endif
|
||||
#endif // WITH_LOOKOUT
|
||||
#ifdef WITH_SDLOG
|
||||
if(Log_Free()>=128)
|
||||
{ xSemaphoreTake(Log_Mutex, portMAX_DELAY);
|
||||
|
|
@ -130,20 +129,18 @@ static void ProcessRxPacket(OGN_RxPacket *RxPacket, uint8_t RxPacketIdx)
|
|||
xSemaphoreGive(Log_Mutex); }
|
||||
#endif
|
||||
#ifdef WITH_PFLAA
|
||||
if(!MyOwnPacket)
|
||||
{ Len=RxPacket->Packet.WritePFLAA(Line, Warn, LatDist, LonDist, RxPacket->Packet.DecodeAltitude()-GPS_Altitude/10); // print on the console $
|
||||
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
Format_String(CONS_UART_Write, Line, Len);
|
||||
xSemaphoreGive(CONS_Mutex); }
|
||||
#endif
|
||||
#ifdef WITH_MAVLINK
|
||||
if(!MyOwnPacket)
|
||||
{ MAV_ADSB_VEHICLE MAV_RxReport;
|
||||
RxPacket->Packet.Encode(MAV_RxReport);
|
||||
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
MAV_RxMsg::Send(sizeof(MAV_RxReport), MAV_Seq++, MAV_SysID, MAV_COMP_ID_ADSB, MAV_ID_ADSB_VEHICLE, (const uint8_t *)&MAV_RxReport, CONS_UART_Write);
|
||||
xSemaphoreGive(CONS_Mutex); }
|
||||
Len=RxPacket->Packet.WritePFLAA(Line, Warn, LatDist, LonDist, RxPacket->Packet.DecodeAltitude()-GPS_Altitude/10); // print on the console
|
||||
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
Format_String(CONS_UART_Write, Line, 0, Len);
|
||||
xSemaphoreGive(CONS_Mutex);
|
||||
#endif
|
||||
// #ifdef WITH_MAVLINK
|
||||
// MAV_ADSB_VEHICLE MAV_RxReport;
|
||||
// RxPacket->Packet.Encode(&MAV_RxReport);
|
||||
// xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
// MAV_RxMsg::Send(sizeof(MAV_RxReport), MAV_Seq++, MAV_SysID, MAV_COMP_ID_ADSB, MAV_ID_ADSB_VEHICLE, (const uint8_t *)&MAV_RxReport, CONS_UART_Write);
|
||||
// xSemaphoreGive(CONS_Mutex);
|
||||
// #endif
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -155,22 +152,23 @@ static void DecodeRxPacket(RFM_RxPktData *RxPkt)
|
|||
// RxPacket->RxRSSI=RxPkt.RSSI;
|
||||
// TickType_t ExecTime=xTaskGetTickCount();
|
||||
|
||||
RX_OGN_Packets++;
|
||||
uint8_t Check = RxPkt->Decode(*RxPacket, Decoder);
|
||||
{ RX_OGN_Packets++;
|
||||
uint8_t Check = RxPkt->Decode(*RxPacket, Decoder);
|
||||
#ifdef DEBUG_PRINT
|
||||
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
Format_String(CONS_UART_Write, "RxPacket: ");
|
||||
Format_Hex(CONS_UART_Write, RxPacket->Packet.HeaderWord);
|
||||
CONS_UART_Write(' ');
|
||||
Format_UnsDec(CONS_UART_Write, (uint16_t)Check);
|
||||
CONS_UART_Write('/');
|
||||
Format_UnsDec(CONS_UART_Write, (uint16_t)RxPacket->RxErr);
|
||||
Format_String(CONS_UART_Write, "\n");
|
||||
xSemaphoreGive(CONS_Mutex);
|
||||
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
Format_String(CONS_UART_Write, "RxPacket: ");
|
||||
Format_Hex(CONS_UART_Write, RxPacket->Packet.HeaderWord);
|
||||
CONS_UART_Write(' ');
|
||||
Format_UnsDec(CONS_UART_Write, (uint16_t)Check);
|
||||
CONS_UART_Write('/');
|
||||
Format_UnsDec(CONS_UART_Write, (uint16_t)RxPacket->RxErr);
|
||||
Format_String(CONS_UART_Write, "\n");
|
||||
xSemaphoreGive(CONS_Mutex);
|
||||
#endif
|
||||
if( (Check==0) && (RxPacket->RxErr<15) ) // what limit on number of detected bit errors ?
|
||||
{ RxPacket->Packet.Dewhiten();
|
||||
ProcessRxPacket(RxPacket, RxPacketIdx); }
|
||||
if( (Check==0) && (RxPacket->RxErr<15) ) // what limit on number of detected bit errors ?
|
||||
{ RxPacket->Packet.Dewhiten();
|
||||
ProcessRxPacket(RxPacket, RxPacketIdx); }
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -181,19 +179,24 @@ static void DecodeRxPacket(RFM_RxPktData *RxPkt)
|
|||
#endif
|
||||
void vTaskPROC(void* pvParameters)
|
||||
{
|
||||
#ifdef WITH_FLASHLOG
|
||||
uint16_t kB = FlashLog_OpenForWrite();
|
||||
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
Format_String(CONS_UART_Write, "TaskPROC: ");
|
||||
Format_UnsDec(CONS_UART_Write, kB);
|
||||
Format_String(CONS_UART_Write, "KB FlashLog\n");
|
||||
xSemaphoreGive(CONS_Mutex);
|
||||
#endif
|
||||
RelayQueue.Clear();
|
||||
|
||||
OGN_TxPacket PosPacket; // position packet
|
||||
uint32_t PosTime=0; // [sec] when the position was recorded
|
||||
OGN_TxPacket StatPacket; // status report packet
|
||||
OGN_TxPacket PosPacket; // position packet
|
||||
uint32_t PosTime=0; // [sec] when the position was recorded
|
||||
OGN_TxPacket StatPacket; // status report packet
|
||||
|
||||
for( ; ; )
|
||||
{ vTaskDelay(1);
|
||||
static uint32_t PrevSlotTime=0;
|
||||
uint32_t SlotTime = TimeSync_Time();
|
||||
if(TimeSync_msTime()<300) SlotTime--;
|
||||
|
||||
RFM_RxPktData *RxPkt = RF_RxFIFO.getRead(); // check for new received packets
|
||||
RFM_RxPktData *RxPkt = RF_RxFIFO.getRead(); // check for new received packets
|
||||
if(RxPkt)
|
||||
{
|
||||
#ifdef DEBUG_PRINT
|
||||
|
|
@ -206,15 +209,36 @@ void vTaskPROC(void* pvParameters)
|
|||
// CONS_UART_Write('\r'); CONS_UART_Write('\n');
|
||||
xSemaphoreGive(CONS_Mutex);
|
||||
#endif
|
||||
DecodeRxPacket(RxPkt); // decode and process the received packet
|
||||
DecodeRxPacket(RxPkt); // decode and process the received packet
|
||||
RF_RxFIFO.Read(); }
|
||||
|
||||
if(SlotTime==PrevSlotTime) continue;
|
||||
PrevSlotTime=SlotTime;
|
||||
static uint32_t PrevSlotTime=0; // remember previous time slot to detect a change
|
||||
uint32_t SlotTime = TimeSync_Time(); // time slot
|
||||
if(TimeSync_msTime()<300) SlotTime--; // lasts up to 0.300sec after the PPS
|
||||
|
||||
if(SlotTime==PrevSlotTime) continue; // stil same time slot, go back to RX processing
|
||||
PrevSlotTime=SlotTime; // new slot started
|
||||
// this part of the loop is executed only once per slot-time
|
||||
GPS_Position *Position = GPS_getPosition();
|
||||
uint8_t BestIdx; int16_t BestResid;
|
||||
#ifdef WITH_MAVLINK
|
||||
GPS_Position *Position = GPS_getPosition(BestIdx, BestResid, (SlotTime-1)%60, 0);
|
||||
#else
|
||||
GPS_Position *Position = GPS_getPosition(BestIdx, BestResid, SlotTime%60, 0);
|
||||
#endif
|
||||
#ifdef DEBUG_PRINT
|
||||
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
Format_String(CONS_UART_Write, "getPos() => ");
|
||||
Format_UnsDec(CONS_UART_Write, SlotTime%60, 2);
|
||||
CONS_UART_Write(' ');
|
||||
Format_UnsDec(CONS_UART_Write, (uint16_t)BestIdx);
|
||||
CONS_UART_Write(':');
|
||||
Format_SignDec(CONS_UART_Write, BestResid, 3, 2);
|
||||
Format_String(CONS_UART_Write, "s\n");
|
||||
xSemaphoreGive(CONS_Mutex);
|
||||
#endif
|
||||
// GPS_Position *Position = GPS_getPosition();
|
||||
if(Position) Position->EncodeStatus(StatPacket.Packet); // encode GPS altitude and pressure
|
||||
if( Position && Position->Ready && (!Position->Sent) && Position->GPS && Position->isValid() )
|
||||
if( Position && Position->isReady && (!Position->Sent) && Position->isReady && Position->isValid() )
|
||||
{ int16_t AverSpeed=GPS_AverageSpeed(); // [0.1m/s] average speed, including the vertical speed
|
||||
RF_FreqPlan.setPlan(Position->Latitude, Position->Longitude); // set the frequency plan according to the GPS position
|
||||
/*
|
||||
|
|
@ -232,7 +256,8 @@ void vTaskPROC(void* pvParameters)
|
|||
PosPacket.Packet.Header.Address = Parameters.Address; // set address
|
||||
PosPacket.Packet.Header.AddrType = Parameters.AddrType; // address-type
|
||||
PosPacket.Packet.calcAddrParity(); // parity of (part of) the header
|
||||
Position->Encode(PosPacket.Packet); // encode position/altitude/speed/etc. from GPS position
|
||||
if(BestResid==0) Position->Encode(PosPacket.Packet); // encode position/altitude/speed/etc. from GPS position
|
||||
else Position->Encode(PosPacket.Packet, BestResid);
|
||||
PosPacket.Packet.Position.Stealth = Parameters.Stealth;
|
||||
PosPacket.Packet.Position.AcftType = Parameters.AcftType; // aircraft-type
|
||||
OGN_TxPacket *TxPacket = RF_TxFIFO.getWrite();
|
||||
|
|
@ -252,7 +277,15 @@ void vTaskPROC(void* pvParameters)
|
|||
if( (AverSpeed>10) || ((RX_Random&0x3)==0) ) // send only some positions if the speed is less than 1m/s
|
||||
RF_TxFIFO.Write(); // complete the write into the TxFIFO
|
||||
Position->Sent=1;
|
||||
|
||||
#ifdef WITH_FLASHLOG
|
||||
bool Written=FlashLog_Process(PosPacket.Packet, PosTime);
|
||||
// if(Written)
|
||||
// { uint8_t Len=FlashLog_Print(Line);
|
||||
// xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
// Format_String(CONS_UART_Write, Line);
|
||||
// xSemaphoreGive(CONS_Mutex);
|
||||
// }
|
||||
#endif // WITH_FLASHLOG
|
||||
} else // if GPS position is not complete, contains no valid position, etc.
|
||||
{ if((SlotTime-PosTime)>=30) { PosPacket.Packet.Position.Time=0x3F; } // if no valid position for more than 30 seconds then set the time as unknown for the transmitted packet
|
||||
OGN_TxPacket *TxPacket = RF_TxFIFO.getWrite();
|
||||
|
|
@ -293,8 +326,7 @@ void vTaskPROC(void* pvParameters)
|
|||
*StatusPacket = StatPacket;
|
||||
StatusPacket->Packet.Whiten();
|
||||
StatusPacket->calcFEC();
|
||||
RF_TxFIFO.Write();
|
||||
}
|
||||
RF_TxFIFO.Write(); }
|
||||
|
||||
while(RF_TxFIFO.Full()<2)
|
||||
{ OGN_TxPacket *RelayPacket = RF_TxFIFO.getWrite();
|
||||
|
|
|
|||
|
|
@ -167,7 +167,7 @@ extern "C"
|
|||
TRX.Deselect = RFM_Deselect;
|
||||
TRX.TransferByte = RFM_TransferByte;
|
||||
#endif
|
||||
TRX.DIO0_isOn = RFM_DIO0_isOn;
|
||||
TRX.DIO0_isOn = RFM_IRQ_isOn;
|
||||
TRX.RESET = RFM_RESET;
|
||||
|
||||
RF_FreqPlan.setPlan(0); // 1 = Europe/Africa, 2 = USA/CA, 3 = Australia and South America
|
||||
|
|
|
|||
|
|
@ -135,6 +135,8 @@ CONFIG_GATTC_ENABLE=y
|
|||
CONFIG_BLE_SMP_ENABLE=y
|
||||
CONFIG_BT_STACK_NO_LOG=
|
||||
CONFIG_BT_ACL_CONNECTIONS=4
|
||||
CONFIG_BT_ALLOCATION_FROM_SPIRAM_FIRST=
|
||||
CONFIG_BT_BLE_DYNAMIC_ENV_MEMORY=
|
||||
CONFIG_SMP_ENABLE=y
|
||||
CONFIG_BT_RESERVE_DRAM=0x10000
|
||||
|
||||
|
|
@ -344,6 +346,7 @@ CONFIG_LOG_COLORS=y
|
|||
# LWIP
|
||||
#
|
||||
CONFIG_L2_TO_L3_COPY=
|
||||
CONFIG_LWIP_IRAM_OPTIMIZATION=
|
||||
CONFIG_LWIP_MAX_SOCKETS=4
|
||||
CONFIG_LWIP_SO_REUSE=
|
||||
CONFIG_LWIP_SO_RCVBUF=
|
||||
|
|
@ -515,6 +518,7 @@ CONFIG_SPIFFS_CACHE_STATS=
|
|||
CONFIG_SPIFFS_PAGE_CHECK=y
|
||||
CONFIG_SPIFFS_GC_MAX_RUNS=10
|
||||
CONFIG_SPIFFS_GC_STATS=
|
||||
CONFIG_SPIFFS_PAGE_SIZE=256
|
||||
CONFIG_SPIFFS_OBJ_NAME_LEN=32
|
||||
CONFIG_SPIFFS_USE_MAGIC=y
|
||||
CONFIG_SPIFFS_USE_MAGIC_LENGTH=y
|
||||
|
|
|
|||
Ładowanie…
Reference in New Issue