mirror
/
esp32-ogn-tracker
kopia lustrzana https://github.com/pjalocha/esp32-ogn-tracker
1
0
Forkuj 0
Kod Zgłoszenia Projekty Wydania Wiki Aktywność
esp32-ogn-tracker/main/ognconv.cpp

272 wiersze
11 KiB
C++
Czysty Wina Historia

#include <stdint.h>
#include "ognconv.h"
// ==============================================================================================
uint32_t FeetToMeters(uint32_t Altitude) { return (Altitude*312+512)>>10; } // [feet] => [m]
uint32_t MetersToFeet(uint32_t Altitude) { return (Altitude*3360+512)>>10; } // [m] => [feet]
// ==============================================================================================
uint16_t EncodeUR2V8(uint16_t Value) // Encode unsigned 12bit (0..3832) as 10bit
{ if(Value<0x100) { }
else if(Value<0x300) Value = 0x100 | ((Value-0x100)>>1);
else if(Value<0x700) Value = 0x200 | ((Value-0x300)>>2);
else if(Value<0xF00) Value = 0x300 | ((Value-0x700)>>3);
else Value = 0x3FF;
return Value; }
uint16_t DecodeUR2V8(uint16_t Value) // Decode 10bit 0..0x3FF
{ uint16_t Range = Value>>8;
Value &= 0x0FF;
if(Range==0) return Value; // 000..0FF
if(Range==1) return 0x101+(Value<<1); // 100..2FE
if(Range==2) return 0x302+(Value<<2); // 300..6FC
return 0x704+(Value<<3); } // 700..EF8 // in 12bit (0..3832)
uint8_t EncodeUR2V5(uint16_t Value) // Encode unsigned 9bit (0..472) as 7bit
{ if(Value<0x020) { }
else if(Value<0x060) Value = 0x020 | ((Value-0x020)>>1);
else if(Value<0x0E0) Value = 0x040 | ((Value-0x060)>>2);
else if(Value<0x1E0) Value = 0x060 | ((Value-0x0E0)>>3);
else Value = 0x07F;
return Value; }
uint16_t DecodeUR2V5(uint16_t Value) // Decode 7bit as unsigned 9bit (0..472)
{ uint8_t Range = (Value>>5)&0x03;
Value &= 0x1F;
if(Range==0) { } // 000..01F
else if(Range==1) { Value = 0x021+(Value<<1); } // 020..05E
else if(Range==2) { Value = 0x062+(Value<<2); } // 060..0DC
else { Value = 0x0E4+(Value<<3); } // 0E0..1D8 => max. Value = 472
return Value; }
uint8_t EncodeSR2V5(int16_t Value) // Encode signed 10bit (-472..+472) as 8bit
{ uint8_t Sign=0; if(Value<0) { Value=(-Value); Sign=0x80; }
Value = EncodeUR2V5(Value);
return Value | Sign; }
int16_t DecodeSR2V5( int16_t Value) // Decode
{ int16_t Sign = Value&0x80;
Value = DecodeUR2V5(Value&0x7F);
return Sign ? -Value: Value; }
uint16_t EncodeUR2V6(uint16_t Value) // Encode unsigned 10bit (0..952) as 8 bit
{ if(Value<0x040) { }
else if(Value<0x0C0) Value = 0x040 | ((Value-0x040)>>1);
else if(Value<0x1C0) Value = 0x080 | ((Value-0x0C0)>>2);
else if(Value<0x3C0) Value = 0x0C0 | ((Value-0x1C0)>>3);
else Value = 0x0FF;
return Value; }
uint16_t DecodeUR2V6(uint16_t Value) // Decode 8bit as unsigned 10bit (0..952)
{ uint16_t Range = (Value>>6)&0x03;
Value &= 0x3F;
if(Range==0) { } // 000..03F
else if(Range==1) { Value = 0x041+(Value<<1); } // 040..0BE
else if(Range==2) { Value = 0x0C2+(Value<<2); } // 0C0..1BC
else { Value = 0x1C4+(Value<<3); } // 1C0..3B8 => max. Value = 952
return Value; }
uint16_t EncodeSR2V6(int16_t Value) // Encode signed 11bit (-952..+952) as 9bit
{ uint16_t Sign=0; if(Value<0) { Value=(-Value); Sign=0x100; }
Value = EncodeUR2V6(Value);
return Value | Sign; }
int16_t DecodeSR2V6( int16_t Value) // Decode 9bit as signed 11bit (-952..+952)
{ int16_t Sign = Value&0x100;
Value = DecodeUR2V6(Value&0x00FF);
return Sign ? -Value: Value; }
uint8_t EncodeUR2V4(uint8_t DOP)
{ if(DOP<0x10) { }
else if(DOP<0x30) DOP = 0x10 | ((DOP-0x10)>>1);
else if(DOP<0x70) DOP = 0x20 | ((DOP-0x30)>>2);
else if(DOP<0xF0) DOP = 0x30 | ((DOP-0x70)>>3);
else DOP = 0x3F;
return DOP; }
uint8_t DecodeUR2V4(uint8_t DOP)
{ uint8_t Range = DOP>>4;
DOP &= 0x0F;
if(Range==0) return DOP; // 00..0F
if(Range==1) return 0x11+(DOP<<1); // 10..2E
if(Range==2) return 0x32+(DOP<<2); // 30..6C
return 0x74+(DOP<<3); } // 70..E8 => max. DOP = 232*0.1=23.2
uint16_t EncodeUR2V12(uint16_t Value) // encode unsigned 16-bit (0..61432) as 14-bit
{ if(Value<0x1000) { }
else if(Value<0x3000) Value = 0x1000 | ((Value-0x1000)>>1);
else if(Value<0x7000) Value = 0x2000 | ((Value-0x3000)>>2);
else if(Value<0xF000) Value = 0x3000 | ((Value-0x7000)>>3);
else Value = 0x3FFF;
return Value; }
uint16_t DecodeUR2V12(uint16_t Value)
{ uint16_t Range = Value>>12;
Value &=0x0FFF;
if(Range==0) return Value; // 0000..0FFF
if(Range==1) return 0x1001+(Value<<1); // 1000..2FFE
if(Range==2) return 0x3002+(Value<<2); // 3000..6FFC
return 0x7004+(Value<<3); } // 7000..EFF8 => max: 61432
// ==============================================================================================
uint8_t EncodeGray(uint8_t Binary)
{ return Binary ^ (Binary>>1); }
uint8_t DecodeGray(uint8_t Gray)
{ Gray ^= (Gray >> 4);
Gray ^= (Gray >> 2);
Gray ^= (Gray >> 1);
return Gray; }
uint16_t EncodeGray(uint16_t Binary)
{ return Binary ^ (Binary>>1); }
uint16_t DecodeGray(uint16_t Gray)
{ Gray ^= (Gray >> 8);
Gray ^= (Gray >> 4);
Gray ^= (Gray >> 2);
Gray ^= (Gray >> 1);
return Gray; }
uint32_t EncodeGray(uint32_t Binary)
{ return Binary ^ (Binary>>1); }
uint32_t DecodeGray(uint32_t Gray)
{ Gray ^= (Gray >>16);
Gray ^= (Gray >> 8);
Gray ^= (Gray >> 4);
Gray ^= (Gray >> 2);
Gray ^= (Gray >> 1);
return Gray; }
// ==============================================================================================
// TEA encryption/decryption
// Data is 2 x 32-bit word
// Key is 4 x 32-bit word
void TEA_Encrypt (uint32_t* Data, const uint32_t *Key, int Loops)
{ uint32_t v0=Data[0], v1=Data[1]; // set up
const uint32_t delta=0x9e3779b9; uint32_t sum=0; // a key schedule constant
uint32_t k0=Key[0], k1=Key[1], k2=Key[2], k3=Key[3]; // cache key
for (int i=0; i < Loops; i++) // basic cycle start
{ sum += delta;
v0 += ((v1<<4) + k0) ^ (v1 + sum) ^ ((v1>>5) + k1);
v1 += ((v0<<4) + k2) ^ (v0 + sum) ^ ((v0>>5) + k3); } // end cycle
Data[0]=v0; Data[1]=v1;
}
void TEA_Decrypt (uint32_t* Data, const uint32_t *Key, int Loops)
{ uint32_t v0=Data[0], v1=Data[1]; // set up
const uint32_t delta=0x9e3779b9; uint32_t sum=delta*Loops; // a key schedule constant
uint32_t k0=Key[0], k1=Key[1], k2=Key[2], k3=Key[3]; // cache key
for (int i=0; i < Loops; i++) // basic cycle start */
{ v1 -= ((v0<<4) + k2) ^ (v0 + sum) ^ ((v0>>5) + k3);
v0 -= ((v1<<4) + k0) ^ (v1 + sum) ^ ((v1>>5) + k1);
sum -= delta; } // end cycle
Data[0]=v0; Data[1]=v1;
}
void TEA_Encrypt_Key0 (uint32_t* Data, int Loops)
{ uint32_t v0=Data[0], v1=Data[1]; // set up
const uint32_t delta=0x9e3779b9; uint32_t sum=0; // a key schedule constant
for (int i=0; i < Loops; i++) // basic cycle start
{ sum += delta;
v0 += (v1<<4) ^ (v1 + sum) ^ (v1>>5);
v1 += (v0<<4) ^ (v0 + sum) ^ (v0>>5); } // end cycle
Data[0]=v0; Data[1]=v1;
}
void TEA_Decrypt_Key0 (uint32_t* Data, int Loops)
{ uint32_t v0=Data[0], v1=Data[1]; // set up
const uint32_t delta=0x9e3779b9; uint32_t sum=delta*Loops; // a key schedule constant
for (int i=0; i < Loops; i++) // basic cycle start
{ v1 -= (v0<<4) ^ (v0 + sum) ^ (v0>>5);
v0 -= (v1<<4) ^ (v1 + sum) ^ (v1>>5);
sum -= delta; } // end cycle
Data[0]=v0; Data[1]=v1;
}
// ==============================================================================================
// XXTEA encryption/decryption
static uint32_t XXTEA_MX(uint8_t E, uint32_t Y, uint32_t Z, uint8_t P, uint32_t Sum, const uint32_t Key[4])
{ return ((((Z>>5) ^ (Y<<2)) + ((Y>>3) ^ (Z<<4))) ^ ((Sum^Y) + (Key[(P&3)^E] ^ Z))); }
void XXTEA_Encrypt(uint32_t *Data, uint8_t Words, const uint32_t Key[4], uint8_t Loops)
{ const uint32_t Delta = 0x9e3779b9;
uint32_t Sum = 0;
uint32_t Z = Data[Words-1]; uint32_t Y;
for( ; Loops; Loops--)
{ Sum += Delta;
uint8_t E = (Sum>>2)&3;
for (uint8_t P=0; P<(Words-1); P++)
{ Y = Data[P+1];
Z = Data[P] += XXTEA_MX(E, Y, Z, P, Sum, Key); }
Y = Data[0];
Z = Data[Words-1] += XXTEA_MX(E, Y, Z, Words-1, Sum, Key);
}
}
void XXTEA_Decrypt(uint32_t *Data, uint8_t Words, const uint32_t Key[4], uint8_t Loops)
{ const uint32_t Delta = 0x9e3779b9;
uint32_t Sum = Loops*Delta;
uint32_t Y = Data[0]; uint32_t Z;
for( ; Loops; Loops--)
{ uint8_t E = (Sum>>2)&3;
for (uint8_t P=Words-1; P; P--)
{ Z = Data[P-1];
Y = Data[P] -= XXTEA_MX(E, Y, Z, P, Sum, Key); }
Z = Data[Words-1];
Y = Data[0] -= XXTEA_MX(E, Y, Z, 0, Sum, Key);
Sum -= Delta;
}
}
// ==============================================================================================
void XorShift32(uint32_t &Seed) // simple random number generator
{ Seed ^= Seed << 13;
Seed ^= Seed >> 17;
Seed ^= Seed << 5; }
void xorshift64(uint64_t &Seed)
{ Seed ^= Seed >> 12;
Seed ^= Seed << 25;
Seed ^= Seed >> 27; }
// ==============================================================================================
const static unsigned char MapAscii85[86] = "0123456789ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz!#$%&()*+-;<=>?@^_`{|}~";
const static uint8_t UnmapAscii85[128] =
{ 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85, 85,
85, 62, 85, 63, 64, 65, 66, 85, 67, 68, 69, 70, 85, 71, 85, 85, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 85, 72, 73, 74, 75, 76,
77, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 85, 85, 85, 78, 79,
80, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59, 60, 61, 81, 82, 83, 84, 85 };
uint8_t EncodeAscii85(char *Ascii, uint32_t Word)
{ for( uint8_t Idx=5; Idx; )
{ uint32_t Div = Word/85;
Idx--;
Ascii[Idx]=MapAscii85[Word-Div*85];
Word=Div; }
Ascii[5]=0;
return 5; }
uint8_t DecodeAscii85(uint32_t &Word, const char *Ascii)
{ Word=0;
for( uint8_t Idx=0; Idx<5; Idx++)
{ char Char = Ascii[Idx]; if(Char<=0) return 0;
uint8_t Dig = UnmapAscii85[(uint8_t)Char];
if(Dig>=85) return 0;
Word = Word*85+Dig; }
return 5; }
// ==============================================================================================
Reference in New Issue View Git Blame Kopiuj bezpośredni odnośnik