kopia lustrzana https://github.com/pjalocha/esp32-ogn-tracker
Major update, work with AXP192 power control chip thus the new T-Beam V1.0
Pawel Jalocha
rodzic
60bee324fd
commit
86a24a2913
|
|
@ -0,0 +1,71 @@
|
|||
#define DEFAULT_AcftType 1 // [0..15] default aircraft-type: glider
|
||||
#define DEFAULT_GeoidSepar 40 // [m]
|
||||
#define DEFAULT_CONbaud 115200
|
||||
#define DEFAULT_PPSdelay 100
|
||||
#define DEFAULT_FreqPlan 0
|
||||
|
||||
// #define WITH_HELTEC // HELTEC module: PCB LED on GPI025
|
||||
// #define WITH_TTGO // TTGO module: PCB LED on GPIO2, GPIO25 free to use as DAC2 output
|
||||
// #define WITH_TBEAM // T-Beam module
|
||||
// #define WITH_TBEAM_V10 // T-Beam module
|
||||
// #define WITH_JACEK // JACEK ESP32 OGN-Tracker
|
||||
// #define WITH_M5_JACEK // JACEK M5 ESP32 OGN-Tracker
|
||||
#define WITH_FollowMe // by Avionix
|
||||
|
||||
// #define WITH_ILI9341 // 320x240 M5stack
|
||||
// #define WITH_ST7789 // IPS 240x240 ST7789
|
||||
// #define WITH_TFT_LCD // TFT LCD
|
||||
// #define WITH_OLED // OLED display on the I2C: some TTGO modules are without OLED display
|
||||
// #define WITH_OLED2 // 2nd OLED display, I2C address next higher
|
||||
#define WITH_U8G2_OLED // I2C OLED through the U8g2 library
|
||||
#define WITH_U8G2_SH1106
|
||||
|
||||
#define WITH_RFM95 // RF chip selection: both HELTEC and TTGO use sx1276 which is same as RFM95
|
||||
|
||||
// #define WITH_SLEEP // with software sleep mode controlled by the long-press on the button
|
||||
|
||||
// #define WITH_AXP // with AXP192 power controller (T-BEAM V1.0)
|
||||
#define WITH_BQ // with BQ24295 power controller (FollowMe)
|
||||
|
||||
// #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.
|
||||
#define WITH_GPS_CONFIG // attempt to configure higher GPS baud rate and airborne mode
|
||||
|
||||
// #define WITH_GPS_UBX // GPS understands UBX
|
||||
#define WITH_GPS_MTK // GPS understands MTK
|
||||
// #define WITH_GPS_SRF
|
||||
// #define WITH_MAVLINK
|
||||
|
||||
// #define WITH_GPS_UBX_PASS // to pass directly UBX packets to/from GPS
|
||||
// #define WITH_GPS_NMEA_PASS // to pass directly NMEA to/from GPS
|
||||
|
||||
// #define WITH_BMP180 // BMP180 pressure sensor
|
||||
// #define WITH_BMP280 // BMP280 pressure sensor
|
||||
#define WITH_BME280 // BMP280 with humidity (but still works with BMP280)
|
||||
// #define WITH_MS5607 // MS5607 pressure sensor
|
||||
|
||||
#define WITH_PFLAA // PFLAU and PFLAA for compatibility with XCsoar and LK8000
|
||||
// #define WITH_POGNT
|
||||
#define WITH_LOOKOUT
|
||||
|
||||
#define WITH_CONFIG // interpret the console input: $POGNS to change parameters
|
||||
|
||||
// #define WITH_BEEPER // with digital buzzer
|
||||
// #define WITH_SOUND // with analog sound produced by DAC on pin 25
|
||||
|
||||
// #define WITH_KNOB
|
||||
// #define WITH_VARIO
|
||||
|
||||
#define WITH_SD // use the SD card in SPI mode and FAT file system
|
||||
#define WITH_SPIFFS // use SPIFFS file system in Flash
|
||||
#define WITH_LOG // log own positions and other received to SPIFFS and possibly to uSD
|
||||
|
||||
#define WITH_BT_SPP // Bluetooth serial port for smartphone/tablet link
|
||||
// #define WITH_WIFI // attempt to connect to the wifi router for uploading the log files
|
||||
// #define WITH_SPIFFS_LOG // log transmitted and received packets to SPIFFS
|
||||
|
||||
// #define WITH_ENCRYPT // Encrypt (optionally) the position
|
||||
|
||||
|
|
@ -0,0 +1,71 @@
|
|||
#define DEFAULT_AcftType 1 // [0..15] default aircraft-type: glider
|
||||
#define DEFAULT_GeoidSepar 40 // [m]
|
||||
#define DEFAULT_CONbaud 115200
|
||||
#define DEFAULT_PPSdelay 100
|
||||
#define DEFAULT_FreqPlan 0
|
||||
|
||||
// #define WITH_HELTEC // HELTEC module: PCB LED on GPI025
|
||||
// #define WITH_TTGO // TTGO module: PCB LED on GPIO2, GPIO25 free to use as DAC2 output
|
||||
// #define WITH_TBEAM // T-Beam module
|
||||
#define WITH_TBEAM_V10 // T-Beam module
|
||||
// #define WITH_JACEK // JACEK ESP32 OGN-Tracker
|
||||
// #define WITH_M5_JACEK // JACEK M5 ESP32 OGN-Tracker
|
||||
// #define WITH_FollowMe // by Avionix
|
||||
|
||||
// #define WITH_ILI9341 // 320x240 M5stack
|
||||
// #define WITH_ST7789 // IPS 240x240 ST7789
|
||||
// #define WITH_TFT_LCD // TFT LCD
|
||||
// #define WITH_OLED // OLED display on the I2C: some TTGO modules are without OLED display
|
||||
// #define WITH_OLED2 // 2nd OLED display, I2C address next higher
|
||||
// #define WITH_U8G2_OLED // I2C OLED through the U8g2 library
|
||||
// #define WITH_U8G2_SH1106
|
||||
|
||||
#define WITH_RFM95 // RF chip selection: both HELTEC and TTGO use sx1276 which is same as RFM95
|
||||
|
||||
// #define WITH_SLEEP // with software sleep mode controlled by the long-press on the button
|
||||
|
||||
#define WITH_AXP // with AXP192 power controller (T-BEAM V1.0)
|
||||
// #define WITH_BQ // with BQ24295 power controller (FollowMe)
|
||||
|
||||
// #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.
|
||||
#define WITH_GPS_CONFIG // attempt to configure higher GPS baud rate and airborne mode
|
||||
|
||||
#define WITH_GPS_UBX // GPS understands UBX
|
||||
// #define WITH_GPS_MTK // GPS understands MTK
|
||||
// #define WITH_GPS_SRF
|
||||
// #define WITH_MAVLINK
|
||||
|
||||
// #define WITH_GPS_UBX_PASS // to pass directly UBX packets to/from GPS
|
||||
// #define WITH_GPS_NMEA_PASS // to pass directly NMEA to/from GPS
|
||||
|
||||
// #define WITH_BMP180 // BMP180 pressure sensor
|
||||
// #define WITH_BMP280 // BMP280 pressure sensor
|
||||
#define WITH_BME280 // BMP280 with humidity (but still works with BMP280)
|
||||
// #define WITH_MS5607 // MS5607 pressure sensor
|
||||
|
||||
#define WITH_PFLAA // PFLAU and PFLAA for compatibility with XCsoar and LK8000
|
||||
// #define WITH_POGNT
|
||||
#define WITH_LOOKOUT
|
||||
|
||||
#define WITH_CONFIG // interpret the console input: $POGNS to change parameters
|
||||
|
||||
// #define WITH_BEEPER // with digital buzzer
|
||||
// #define WITH_SOUND // with analog sound produced by DAC on pin 25
|
||||
|
||||
// #define WITH_KNOB
|
||||
// #define WITH_VARIO
|
||||
|
||||
// #define WITH_SD // use the SD card in SPI mode and FAT file system
|
||||
#define WITH_SPIFFS // use SPIFFS file system in Flash
|
||||
#define WITH_LOG // log own positions and other received to SPIFFS and possibly to uSD
|
||||
|
||||
#define WITH_BT_SPP // Bluetooth serial port for smartphone/tablet link
|
||||
// #define WITH_WIFI // attempt to connect to the wifi router for uploading the log files
|
||||
// #define WITH_SPIFFS_LOG // log transmitted and received packets to SPIFFS
|
||||
|
||||
// #define WITH_ENCRYPT // Encrypt (optionally) the position
|
||||
|
||||
|
|
@ -1,3 +1,6 @@
|
|||
#ifndef __ATMOSPHERE_H__
|
||||
#define __ATMOSPHERE_H__
|
||||
|
||||
#include <math.h>
|
||||
#include <stdint.h>
|
||||
|
||||
|
|
@ -56,3 +59,5 @@ class Atmosphere
|
|||
|
||||
|
||||
} ;
|
||||
|
||||
#endif // __ATMOSPHERE_H__
|
||||
|
|
|
|||
|
|
@ -0,0 +1,265 @@
|
|||
#ifndef __AXP192_H__
|
||||
#define __AXP192_H__
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
class AXP192
|
||||
{ private:
|
||||
static const uint8_t AXP192_ADDR = 0x34; // possible I2C addresses
|
||||
|
||||
// registers
|
||||
static const uint8_t REG_STATUS = 0x00; // bit #2 = charge/discharge, bit #0 = power-on triggered by Vbus/ACin
|
||||
static const uint8_t REG_MODE_CHGSTATUS = 0x01; // charge mode and status
|
||||
static const uint8_t REG_ID = 0x03;
|
||||
static const uint8_t REG_LDO234_DC23_CTL = 0x12; // [bit mask] control outputs ON/OFF
|
||||
static const uint8_t REG_DC1_VOLTAGE = 0x26; // [25mV] controls DC1 voltage
|
||||
static const uint8_t REG_OFF_CTL = 0x32; // power-off, battery check, LED control
|
||||
static const uint8_t REG_CHARGE_1 = 0x33; // target voltage, current
|
||||
static const uint8_t REG_CHARGE_2 = 0x34;
|
||||
static const uint8_t REG_BACKUP_CHG = 0x35; // backup battery charge
|
||||
static const uint8_t REG_POK_SET = 0x36; // press-key parameters
|
||||
|
||||
static const uint8_t REG_INTEN1 = 0x40; // IRQ enable
|
||||
static const uint8_t REG_INTEN2 = 0x41;
|
||||
static const uint8_t REG_INTEN3 = 0x42;
|
||||
static const uint8_t REG_INTEN4 = 0x43;
|
||||
static const uint8_t REG_INTEN5 = 0x4A;
|
||||
|
||||
static const uint8_t REG_INTSTS1 = 0x44; // IRQ status
|
||||
static const uint8_t REG_INTSTS2 = 0x45;
|
||||
static const uint8_t REG_INTSTS3 = 0x46;
|
||||
static const uint8_t REG_INTSTS4 = 0x47;
|
||||
static const uint8_t REG_INTSTS5 = 0x4D;
|
||||
|
||||
static const uint8_t REG_VBUS_VOLT_H8 = 0x5A; // [1.7mV]
|
||||
static const uint8_t REG_VBUS_VOLT_L4 = 0x5B;
|
||||
static const uint8_t REG_VBUS_CURR_H8 = 0x5C; // [0.375mA]
|
||||
static const uint8_t REG_VBUS_CURR_L4 = 0x5D;
|
||||
static const uint8_t REG_TEMP_H8 = 0x5E; // [-144.7..264.8/0.1]degC
|
||||
static const uint8_t REG_TEMP_L4 = 0x5F;
|
||||
|
||||
static const uint8_t REG_BAT_VOLT_H8 = 0x78; // [0.0..4504.5/1.1]mV
|
||||
static const uint8_t REG_BAT_VOLT_L4 = 0x79;
|
||||
static const uint8_t REG_BAT_INP_CURR_H8 = 0x7A; // [0.5mA]
|
||||
static const uint8_t REG_BAT_INP_CURR_L5 = 0x7B;
|
||||
static const uint8_t REG_BAT_OUT_CURR_H8 = 0x7C; // [0.5mA]
|
||||
static const uint8_t REG_BAT_OUT_CURR_L5 = 0x7D;
|
||||
|
||||
static const uint8_t REG_ADC_EN1 = 0x82; // battery voltage enabled by default
|
||||
static const uint8_t REG_ADC_EN2 = 0x83; // internal temperature enabled by default
|
||||
static const uint8_t REG_ADC_SPEED = 0x84; // 25Hz by default
|
||||
static const uint8_t REG_ADC_RANGE = 0x85;
|
||||
|
||||
static const uint8_t REG_BAT_INP_CHG = 0xB0;
|
||||
static const uint8_t REG_BAT_OUT_CHG = 0xB4;
|
||||
static const uint8_t REG_BAT_MON = 0xB8;
|
||||
|
||||
static const uint8_t AXP192_ID = 0x03; // ID for the AXP192
|
||||
|
||||
public:
|
||||
static const uint8_t OUT_DCDC1 = 0; // supply outputs
|
||||
static const uint8_t OUT_DCDC3 = 1;
|
||||
static const uint8_t OUT_LDO2 = 2;
|
||||
static const uint8_t OUT_LDO3 = 3;
|
||||
static const uint8_t OUT_DCDC2 = 4;
|
||||
static const uint8_t OUT_EXTEN = 6;
|
||||
|
||||
static const uint16_t ADC_TEMP = 0x8000; // ADC sampling inputs
|
||||
static const uint16_t ADC_BAT_VOLT = 0x0080;
|
||||
static const uint16_t ADC_BAT_CURR = 0x0040;
|
||||
static const uint16_t ADC_VBUS_VOLT = 0x0008;
|
||||
static const uint16_t ADC_VBUS_CURR = 0x0004;
|
||||
|
||||
static const uint32_t AXP202_PEK_LONGPRESS_IRQ = 0x00010000;
|
||||
static const uint32_t AXP202_PEK_SHORTPRESS_IRQ = 0x00020000;
|
||||
|
||||
public:
|
||||
uint8_t Bus; // which I2C bus
|
||||
uint8_t ADDR; // detected I2C address
|
||||
uint8_t ID;
|
||||
|
||||
public:
|
||||
uint8_t Error; // error on the I2C bus (0=no error)
|
||||
|
||||
uint8_t checkID(void) // check ID, to make sure the AXP192 is reachable
|
||||
{ ADDR=0;
|
||||
Error=I2C_Read(Bus, AXP192_ADDR, REG_ID, ID);
|
||||
if( (!Error) && (ID==AXP192_ID) ) { ADDR=AXP192_ADDR; return 0; }
|
||||
return 1; } // 0 => no error and correct ID
|
||||
|
||||
uint8_t readStatus(void)
|
||||
{ uint8_t Byte=0; Error=I2C_Read(Bus, ADDR, REG_STATUS, Byte); return Byte; }
|
||||
|
||||
uint8_t setPOK(uint8_t Byte=0xDC)
|
||||
{ Error=I2C_Write(Bus, ADDR, REG_POK_SET, Byte);
|
||||
return Error; }
|
||||
|
||||
uint8_t setPowerOutput(uint8_t Channel, bool ON=1)
|
||||
{ uint8_t Byte;
|
||||
Error=I2C_Read(Bus, ADDR, REG_LDO234_DC23_CTL, Byte); if(Error) return Error;
|
||||
uint8_t Mask=1; Mask<<=Channel;
|
||||
if(ON) Byte |= Mask;
|
||||
else Byte &= ~Mask;
|
||||
Error=I2C_Write(Bus, ADDR, REG_LDO234_DC23_CTL, Byte);
|
||||
return Error; }
|
||||
|
||||
uint8_t setDCDC1(uint16_t mV) // [mV] set voltage on DCDC1 output
|
||||
{ if(mV>3500) mV=3500;
|
||||
else if(mV<700) mV=700;
|
||||
uint8_t Byte = (mV-700+12)/25;
|
||||
Error=I2C_Write(Bus, ADDR, REG_DC1_VOLTAGE, Byte);
|
||||
return Error; }
|
||||
|
||||
uint8_t ShutDown(void)
|
||||
{ uint8_t Byte;
|
||||
Error=I2C_Read(Bus, ADDR, REG_OFF_CTL, Byte); if(Error) return Error;
|
||||
Byte |= 0x80;
|
||||
Error=I2C_Write(Bus, ADDR, REG_OFF_CTL, Byte);
|
||||
return Error; }
|
||||
|
||||
uint8_t setLED(uint8_t Mode) // 0=OFF, 1=1Hz, 2=4Hz, 3=ON, 4=reflect charging status
|
||||
{ uint8_t Byte;
|
||||
Error=I2C_Read(Bus, ADDR, REG_OFF_CTL, Byte); if(Error) return Error;
|
||||
if(Mode<4)
|
||||
{ Byte |= 0x08; // control LED by software
|
||||
Byte &= 0xCF;
|
||||
Byte |= Mode<<4; } // set LED state: 0, 1, 2, 3
|
||||
else Byte &= 0xF7; // control LED by hardware
|
||||
Error=I2C_Write(Bus, ADDR, REG_OFF_CTL, Byte);
|
||||
return Error; }
|
||||
|
||||
uint8_t enableADC(uint16_t Mask)
|
||||
{ Error=I2C_Write(Bus, ADDR, REG_ADC_EN1, Mask ); if(Error) return Error;
|
||||
Error=I2C_Write(Bus, ADDR, REG_ADC_EN2, Mask>>8); return Error; }
|
||||
|
||||
uint16_t readH8L4(uint8_t Reg) // read two-byte H8:L4 value
|
||||
{ uint8_t Byte[2]; Error=I2C_Read(Bus, ADDR, Reg, Byte, 2); if(Error) return 0;
|
||||
uint16_t Word = Byte[0]; Word<<=4; Word|=Byte[1]&0x0F; return Word; }
|
||||
|
||||
uint16_t readVbusVoltage(void)
|
||||
{ uint16_t Volt=readH8L4(REG_VBUS_VOLT_H8); return (Volt*17+5)/10; } // [1mV]
|
||||
|
||||
uint16_t readVbusCurrent(void)
|
||||
{ uint16_t Curr=readH8L4(REG_VBUS_CURR_H8); return (Curr*15+20)/40; } // [mA]
|
||||
|
||||
uint16_t readBatteryVoltage(void)
|
||||
{ uint16_t Volt=readH8L4(REG_BAT_VOLT_H8); return (Volt*11+5)/10; } // [mV]
|
||||
|
||||
// uint16_t readVbusVoltage(void)
|
||||
// { uint8_t H8; Error=I2C_Read(Bus, ADDR, REG_VBUS_VOLT_H8, H8); if(Error) return 0;
|
||||
// uint8_t L4; Error=I2C_Read(Bus, ADDR, REG_VBUS_VOLT_L4, L4); if(Error) return 0;
|
||||
// uint16_t Volt=H8; Volt<<=4; Volt|=L4&0x0F; return (Volt*17+5)/10; } // [mV]
|
||||
|
||||
// uint16_t readVbusCurrent(void)
|
||||
// { uint8_t H8; Error=I2C_Read(Bus, ADDR, REG_VBUS_CURR_H8, H8); if(Error) return 0;
|
||||
// uint8_t L4; Error=I2C_Read(Bus, ADDR, REG_VBUS_CURR_L4, L4); if(Error) return 0;
|
||||
// uint16_t Curr=H8; Curr<<=4; Curr|=L4&0x0F; return (Curr*15+20)/40; } // [1mA]
|
||||
|
||||
// uint16_t readBatteryVoltage(void)
|
||||
// { uint8_t H8; Error=I2C_Read(Bus, ADDR, REG_BAT_VOLT_H8, H8); if(Error) return 0;
|
||||
// uint8_t L4; Error=I2C_Read(Bus, ADDR, REG_BAT_VOLT_L4, L4); if(Error) return 0;
|
||||
// uint16_t Volt=H8; Volt<<=4; Volt|=L4&0x0F; return (Volt*11+5)/10; } // [1mV]
|
||||
|
||||
int16_t readTemperature(void)
|
||||
{ int16_t Temp=readH8L4(REG_TEMP_H8); return Temp-1447; } // [0.1degC]
|
||||
|
||||
// int16_t readTemperature(void)
|
||||
// { uint8_t H8; Error=I2C_Read(Bus, ADDR, REG_TEMP_H8, H8); if(Error) return 0;
|
||||
// uint8_t L4; Error=I2C_Read(Bus, ADDR, REG_TEMP_L4, L4); if(Error) return 0;
|
||||
// int16_t Temp=H8; Temp<<=4; Temp|=L4&0x0F; return Temp-1447; } // [0.1degC]
|
||||
|
||||
uint16_t readH8L5(uint8_t Reg)
|
||||
{ uint8_t Byte[2]; Error=I2C_Read(Bus, ADDR, Reg, Byte, 2); if(Error) return 0;
|
||||
uint16_t Word = Byte[0]; Word<<=5; Word|=Byte[1]&0x01F; return Word; }
|
||||
|
||||
uint16_t readBatteryInpCurrent(void)
|
||||
{ uint16_t Curr=readH8L5(REG_BAT_INP_CURR_H8); return Curr/2; } // [mA]
|
||||
|
||||
uint16_t readBatteryOutCurrent(void)
|
||||
{ uint16_t Curr=readH8L5(REG_BAT_OUT_CURR_H8); return Curr/2; } // [mA]
|
||||
|
||||
// uint16_t readBatteryInpCurrent(void)
|
||||
// { uint8_t H8; Error=I2C_Read(Bus, ADDR, REG_BAT_INP_CURR_H8, H8); if(Error) return 0;
|
||||
// uint8_t L5; Error=I2C_Read(Bus, ADDR, REG_BAT_INP_CURR_L5, L5); if(Error) return 0;
|
||||
// uint16_t Curr=H8; Curr<<=5; Curr|=L5&0x1F; return Curr/2; } // [1mA]
|
||||
|
||||
// uint16_t readBatteryOutCurrent(void)
|
||||
// { uint8_t H8; Error=I2C_Read(Bus, ADDR, REG_BAT_OUT_CURR_H8, H8); if(Error) return 0;
|
||||
// uint8_t L5; Error=I2C_Read(Bus, ADDR, REG_BAT_OUT_CURR_L5, L5); if(Error) return 0;
|
||||
// uint16_t Curr=H8; Curr<<=5; Curr|=L5&0x1F; return Curr/2; } // [1mA]
|
||||
|
||||
uint16_t read32bit(uint8_t Reg)
|
||||
{ uint8_t Byte[4]; Error=I2C_Read(Bus, ADDR, Reg, Byte, 4); if(Error) return 0;
|
||||
uint32_t Word=Byte[0];
|
||||
for(uint8_t Idx=1; Idx<4; Idx++)
|
||||
{ Word<<=8; Word|=Byte[Idx]; }
|
||||
return Word; }
|
||||
|
||||
uint32_t readBatteryInpCharge(void) // [0x8000/25 mAs]
|
||||
{ uint32_t Charge = read32bit(REG_BAT_INP_CHG); return Charge; }
|
||||
|
||||
uint32_t readBatteryOutCharge(void) // [0x8000/25 mAs]
|
||||
{ uint32_t Charge = read32bit(REG_BAT_OUT_CHG); return Charge; }
|
||||
|
||||
uint8_t setBatMon(uint8_t Byte=0x80)
|
||||
{ Error=I2C_Write(Bus, ADDR, REG_BAT_MON, Byte); return Error; }
|
||||
|
||||
uint8_t enableBatMon(void) { return setBatMon(0x80); }
|
||||
uint8_t disableBatMon(void) { return setBatMon(0x00); }
|
||||
uint8_t stopBatMon(void) { return setBatMon(0xC0); }
|
||||
uint8_t clearBatMon(void) { return setBatMon(0xA0); }
|
||||
|
||||
bool isBatteryConnected(void)
|
||||
{ uint8_t Byte;
|
||||
Error=I2C_Read(Bus, ADDR, REG_MODE_CHGSTATUS, Byte); if(Error) return 0;
|
||||
return Byte&0x20; } // 1=battery connected, 0=no battery connected
|
||||
|
||||
bool isBatteryCharging(void)
|
||||
{ uint8_t Byte;
|
||||
Error=I2C_Read(Bus, ADDR, REG_MODE_CHGSTATUS, Byte); if(Error) return 0;
|
||||
return Byte&0x40; } // 1=charging, 0=charge finished or not charging
|
||||
|
||||
bool isBatteryActive(void)
|
||||
{ uint8_t Byte;
|
||||
Error=I2C_Read(Bus, ADDR, REG_MODE_CHGSTATUS, Byte); if(Error) return 0;
|
||||
return Byte&0x08; } // 1=active
|
||||
|
||||
bool isChargeNotEnough(void)
|
||||
{ uint8_t Byte;
|
||||
Error=I2C_Read(Bus, ADDR, REG_MODE_CHGSTATUS, Byte); if(Error) return 0;
|
||||
return Byte&0x04; } // 1=not enough charging current
|
||||
|
||||
uint8_t enableIRQ(uint32_t IrqMask, bool Enable=1)
|
||||
{ for(uint8_t Idx=0; Idx<4; Idx++)
|
||||
{ uint8_t Mask = IrqMask;
|
||||
if(Mask)
|
||||
{ uint8_t Byte;
|
||||
Error=I2C_Read(Bus, ADDR, REG_INTEN1+Idx, Byte); // if(Error) continue;
|
||||
if(Enable) Byte |= Mask;
|
||||
else Byte &= ~Mask;
|
||||
Error=I2C_Write(Bus, ADDR, REG_INTEN1+Idx, Byte); // if(Error) continue;
|
||||
}
|
||||
IrqMask>>=8;
|
||||
}
|
||||
return Error; }
|
||||
|
||||
bool readLongPressIRQ(void)
|
||||
{ uint8_t Byte=0;
|
||||
Error=I2C_Read(Bus, ADDR, REG_INTSTS1+2, Byte); if(Error) return 0;
|
||||
return Byte&0x01; }
|
||||
|
||||
bool readShortPressIRQ(void)
|
||||
{ uint8_t Byte=0;
|
||||
Error=I2C_Read(Bus, ADDR, REG_INTSTS1+2, Byte); if(Error) return 0;
|
||||
return Byte&0x02; }
|
||||
|
||||
uint8_t clearIRQ(void)
|
||||
{ uint8_t Byte=0xFF;
|
||||
for(uint8_t Idx=0; Idx<4; Idx++)
|
||||
{ Error=I2C_Write(Bus, ADDR, REG_INTSTS1+Idx, Byte); }
|
||||
Error=I2C_Write(Bus, ADDR, REG_INTSTS5, Byte);
|
||||
return Error; }
|
||||
|
||||
} ;
|
||||
|
||||
#endif // __AXP192_H__
|
||||
|
|
@ -0,0 +1,72 @@
|
|||
#ifndef __BQ24295_H__
|
||||
#define __BQ24295_H__
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
class BQ24295
|
||||
{ private:
|
||||
static const uint8_t BQ24295_ADDR = 0x6B; // possible I2C addresses
|
||||
|
||||
static const uint8_t REG_SOURCE = 0x00;
|
||||
static const uint8_t REG_POWERON = 0x01;
|
||||
static const uint8_t REG_CHG_CURR = 0x02; // charging current
|
||||
static const uint8_t REG_PRE_CHG = 0x03; // pre-/post- charge current
|
||||
static const uint8_t REG_CHG_VOLT = 0x04;
|
||||
static const uint8_t REG_TIMER_CTRL = 0x05;
|
||||
static const uint8_t REG_BOOST = 0x06;
|
||||
static const uint8_t REG_MISC = 0x07;
|
||||
static const uint8_t REG_STATUS = 0x08; //
|
||||
static const uint8_t REG_FAULT = 0x09;
|
||||
static const uint8_t REG_ID = 0x0A; // reads 0x23 but should read 0xC0
|
||||
|
||||
static const uint8_t BQ24295_ID = 0xC0; // ID for the BQ24295, but the chip returns 0x23 ?
|
||||
|
||||
public:
|
||||
uint8_t Bus; // which I2C bus
|
||||
uint8_t ADDR; // detected I2C address
|
||||
uint8_t ID;
|
||||
|
||||
public:
|
||||
uint8_t Error; // error on the I2C bus (0=no error)
|
||||
|
||||
uint8_t readReg(uint8_t Reg)
|
||||
{ uint8_t Byte=0; Error=I2C_Read(Bus, ADDR, Reg, Byte); return Byte; }
|
||||
|
||||
uint8_t readStatus(void) { return readReg(REG_STATUS); } // VVCCDPTS VV=Vbus status, CC=Charge status, D=DPM, P=Power-Good, T=Thermal reg., S=Vsys-min reg.
|
||||
uint8_t readFault (void) { return readReg(REG_FAULT); } // WOCCBrNN W=Watchdog, O=OTG, CC=charge fault, NN=NTC(temperature)
|
||||
uint8_t readSource(void) { return readReg(REG_SOURCE); } //
|
||||
uint8_t readID (void) { return readReg(REG_ID); }
|
||||
|
||||
uint8_t checkID(void) // check ID, to make sure the BQ24295 is reachable
|
||||
{ ADDR=0;
|
||||
Error=I2C_Read(Bus, BQ24295_ADDR, REG_ID, ID);
|
||||
if( (!Error) /* && (ID==BQ24295_ID) */ ) { ADDR=BQ24295_ADDR; return 0; }
|
||||
return 1; } // 0 => no error and correct ID
|
||||
|
||||
uint8_t writeSource(uint8_t Byte=0x05) // disable, 3.88V, 1.5A
|
||||
{ Error=I2C_Write(Bus, ADDR, REG_SOURCE, Byte);
|
||||
return Error; }
|
||||
|
||||
uint8_t writePowerON(uint8_t Byte=0x3F) // OTG enable, charge enable, 3.7V
|
||||
{ Error=I2C_Write(Bus, ADDR, REG_POWERON, Byte);
|
||||
return Error; }
|
||||
|
||||
uint8_t writeChargeCurr(uint8_t Byte=0x00) // 512mA
|
||||
{ Error=I2C_Write(Bus, ADDR, REG_CHG_CURR, Byte);
|
||||
return Error; }
|
||||
|
||||
uint8_t writePreCharge(uint8_t Byte=0x00) // 128mA, 128mA
|
||||
{ Error=I2C_Write(Bus, ADDR, REG_PRE_CHG, Byte);
|
||||
return Error; }
|
||||
|
||||
uint8_t writeChargeVolt(uint8_t Byte=0x9A) // 4.112V, 3.0V, 100mV
|
||||
{ Error=I2C_Write(Bus, ADDR, REG_CHG_VOLT, Byte);
|
||||
return Error; }
|
||||
|
||||
uint8_t writeTimerCtrl(uint8_t Byte=0x8C) // disable watchdog
|
||||
{ Error=I2C_Write(Bus, ADDR, REG_TIMER_CTRL, Byte);
|
||||
return Error; }
|
||||
|
||||
} ;
|
||||
|
||||
#endif // __BQ24295_H__
|
||||
|
|
@ -1,19 +1,31 @@
|
|||
#define DEFAULT_AcftType 1 // [0..15] default aircraft-type: glider
|
||||
#define DEFAULT_GeoidSepar 40 // [m]
|
||||
#define DEFAULT_CONbaud 115200
|
||||
#define DEFAULT_PPSdelay 80
|
||||
#define DEFAULT_PPSdelay 100
|
||||
#define DEFAULT_FreqPlan 0
|
||||
|
||||
// #define WITH_HELTEC // HELTEC module: PCB LED on GPI025
|
||||
#define WITH_TTGO // TTGO module: PCB LED on GPIO2, GPIO25 free to use as DAC2 output
|
||||
// #define WITH_TTGO // TTGO module: PCB LED on GPIO2, GPIO25 free to use as DAC2 output
|
||||
// #define WITH_TBEAM // T-Beam module
|
||||
#define WITH_TBEAM_V10 // T-Beam module
|
||||
// #define WITH_JACEK // JACEK ESP32 OGN-Tracker
|
||||
// #define WITH_M5_JACEK // JACEK M5 ESP32 OGN-Tracker
|
||||
// #define WITH_FollowMe // by Avionix
|
||||
|
||||
#define WITH_RFM95
|
||||
// #define WITH_RFM69
|
||||
|
||||
#define WITH_OLED // OLED display on the I2C: some TTGO modules are without OLED display
|
||||
// #define WITH_ILI9341 // 320x240 M5stack
|
||||
// #define WITH_ST7789 // IPS 240x240 ST7789
|
||||
// #define WITH_TFT_LCD // TFT LCD
|
||||
// #define WITH_OLED // OLED display on the I2C: some TTGO modules are without OLED display
|
||||
// #define WITH_OLED2 // 2nd OLED display, I2C address next higher
|
||||
// #define WITH_U8G2_OLED // I2C OLED through the U8g2 library
|
||||
// #define WITH_U8G2_SH1106
|
||||
|
||||
#define WITH_RFM95 // RF chip selection: both HELTEC and TTGO use sx1276 which is same as RFM95
|
||||
|
||||
// #define WITH_SLEEP // with software sleep mode controlled by the long-press on the button
|
||||
|
||||
#define WITH_AXP // with AXP192 power controller (T-BEAM V1.0)
|
||||
// #define WITH_BQ // with BQ24295 power controller (FollowMe)
|
||||
|
||||
// #define WITH_LED_RX
|
||||
// #define WITH_LED_TX
|
||||
|
|
@ -21,6 +33,7 @@
|
|||
// #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.
|
||||
#define WITH_GPS_CONFIG // attempt to configure higher GPS baud rate and airborne mode
|
||||
|
||||
#define WITH_GPS_UBX // GPS understands UBX
|
||||
// #define WITH_GPS_MTK // GPS understands MTK
|
||||
// #define WITH_GPS_SRF
|
||||
|
|
@ -31,20 +44,28 @@
|
|||
|
||||
// #define WITH_BMP180 // BMP180 pressure sensor
|
||||
// #define WITH_BMP280 // BMP280 pressure sensor
|
||||
// #define WITH_BME280 // BMP280 with humidity (but still works with BMP280)
|
||||
#define WITH_BME280 // BMP280 with humidity (but still works with BMP280)
|
||||
// #define WITH_MS5607 // MS5607 pressure sensor
|
||||
|
||||
#define WITH_PFLAA // PFLAU and PFLAA for compatibility with XCsoar and LK8000
|
||||
// #define WITH_POGNT
|
||||
#define WITH_LOOKOUT
|
||||
|
||||
#define WITH_CONFIG // interpret the console input: $POGNS to change parameters
|
||||
|
||||
// #define WITH_BEEPER
|
||||
#define WITH_BEEPER // with digital buzzer
|
||||
// #define WITH_SOUND // with analog sound produced by DAC on pin 25
|
||||
|
||||
// #define WITH_KNOB
|
||||
// #define WITH_VARIO
|
||||
|
||||
// #define WITH_SD // use the SD card in SPI mode and FAT file system
|
||||
// #define WITH_SPIFFS // use SPIFFS file system in Flash
|
||||
// #define WITH_LOG // log own positions and other received to SPIFFS and possibly to uSD
|
||||
#define WITH_SPIFFS // use SPIFFS file system in Flash
|
||||
#define WITH_LOG // log own positions and other received to SPIFFS and possibly to uSD
|
||||
|
||||
#define WITH_BT_SPP // Bluetooth serial port for smartphone/tablet link
|
||||
// #define WITH_WIFI // attempt to connect to the wifi router for uploading the log files
|
||||
// #define WITH_SPIFFS_LOG // log transmitted and received packets to SPIFFS
|
||||
|
||||
// #define WITH_ENCRYPT // Encrypt (optionally) the position
|
||||
|
||||
|
|
|
|||
634
main/ctrl.cpp
634
main/ctrl.cpp
|
|
@ -6,12 +6,14 @@
|
|||
#include <unistd.h>
|
||||
|
||||
#include "esp_system.h"
|
||||
// #include "esp_sleep.h"
|
||||
|
||||
#include "hal.h"
|
||||
|
||||
#include "rf.h"
|
||||
#include "sens.h"
|
||||
#include "rf.h"
|
||||
#include "ctrl.h"
|
||||
#include "proc.h"
|
||||
#include "log.h"
|
||||
|
||||
#include "gps.h"
|
||||
|
|
@ -19,10 +21,17 @@
|
|||
#include "timesync.h"
|
||||
#include "format.h"
|
||||
|
||||
#include "disp_oled.h"
|
||||
#include "disp_lcd.h"
|
||||
|
||||
// #include "ymodem.h"
|
||||
|
||||
// #define DEBUG_PRINT
|
||||
|
||||
static char Line[128];
|
||||
|
||||
FIFO<uint8_t, 8> KeyBuffer;
|
||||
|
||||
// ========================================================================================================================
|
||||
|
||||
void PrintTasks(void (*CONS_UART_Write)(char))
|
||||
|
|
@ -53,338 +62,6 @@ void PrintTasks(void (*CONS_UART_Write)(char))
|
|||
|
||||
// ========================================================================================================================
|
||||
|
||||
#ifdef WITH_OLED
|
||||
int OLED_DisplayStatus(uint32_t Time, uint8_t LineIdx=0)
|
||||
{ Format_String(Line , "OGN Tx/Rx ");
|
||||
Format_HHMMSS(Line+10, Time);
|
||||
OLED_PutLine(LineIdx++, Line);
|
||||
Parameters.Print(Line);
|
||||
OLED_PutLine(LineIdx++, Line);
|
||||
return 0; }
|
||||
|
||||
int OLED_DisplayPosition(GPS_Position *GPS=0, uint8_t LineIdx=2)
|
||||
{ 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(LineIdx , Line);
|
||||
Format_SignDec(Line, GPS->Longitude/60, 7, 4);
|
||||
Format_SignDec(Line+10, GPS->ClimbRate, 4, 1);
|
||||
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(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(LineIdx+3, Line);
|
||||
}
|
||||
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]='.';
|
||||
Format_UnsDec (Line+ 6, (uint16_t)GPS->Year , 2, 0); Line[8]=' '; Line[9]=' '; }
|
||||
else Format_String(Line, " ");
|
||||
if(GPS && GPS->isTimeValid())
|
||||
{ Format_UnsDec (Line+10, (uint16_t)GPS->Hour, 2, 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(LineIdx+4, Line);
|
||||
Line[0]=0;
|
||||
if(GPS && GPS->hasBaro)
|
||||
{ Format_String(Line , "0000.0hPa 00000m");
|
||||
Format_UnsDec(Line , GPS->Pressure/40, 5, 1);
|
||||
Format_UnsDec(Line+10, GPS->StdAltitude/10, 5, 0); }
|
||||
OLED_PutLine(LineIdx+5, Line);
|
||||
return 0; }
|
||||
#endif
|
||||
|
||||
#ifdef WITH_U8G2
|
||||
|
||||
void OLED_PutLine(u8g2_t *OLED, uint8_t LineIdx, const char *Line)
|
||||
{ if(Line==0) return;
|
||||
#ifdef DEBUG_PRINT
|
||||
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
Format_String(CONS_UART_Write, "OLED_PutLine( ,");
|
||||
Format_UnsDec(CONS_UART_Write, (uint16_t)LineIdx);
|
||||
CONS_UART_Write(',');
|
||||
Format_String(CONS_UART_Write, Line);
|
||||
Format_String(CONS_UART_Write, ")\n");
|
||||
xSemaphoreGive(CONS_Mutex);
|
||||
#endif
|
||||
// u8g2_SetFont(OLED, u8g2_font_5x8_tr);
|
||||
u8g2_SetFont(OLED, u8g2_font_amstrad_cpc_extended_8r);
|
||||
u8g2_DrawStr(OLED, 0, (LineIdx+1)*8, Line);
|
||||
}
|
||||
|
||||
void OLED_DrawStatus(u8g2_t *OLED, uint32_t Time, uint8_t LineIdx=0)
|
||||
{ Format_String(Line , "OGN Tx/Rx ");
|
||||
Format_HHMMSS(Line+10, Time); Line[16]=0;
|
||||
OLED_PutLine(OLED, LineIdx++, Line);
|
||||
Parameters.Print(Line); Line[16]=0;
|
||||
OLED_PutLine(OLED, LineIdx++, Line); }
|
||||
|
||||
void OLED_DrawPosition(u8g2_t *OLED, GPS_Position *GPS=0, uint8_t LineIdx=2)
|
||||
{ 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(OLED, LineIdx , Line);
|
||||
Format_SignDec(Line, GPS->Longitude/60, 7, 4);
|
||||
Format_SignDec(Line+10, GPS->ClimbRate, 4, 1);
|
||||
OLED_PutLine(OLED, 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(OLED, 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(OLED, LineIdx+3, Line);
|
||||
}
|
||||
// else { OLED_PutLine(OLED, LineIdx, 0); OLED_PutLine(OLED, 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]='.';
|
||||
Format_UnsDec (Line+ 6, (uint16_t)GPS->Year , 2, 0); Line[8]=' '; Line[9]=' '; }
|
||||
else Format_String(Line, " ");
|
||||
if(GPS && GPS->isTimeValid())
|
||||
{ Format_UnsDec (Line+10, (uint16_t)GPS->Hour, 2, 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(OLED, LineIdx+4, Line);
|
||||
Line[0]=0;
|
||||
if(GPS && GPS->hasBaro)
|
||||
{ Format_String(Line , "0000.0hPa 00000m");
|
||||
Format_UnsDec(Line , GPS->Pressure/40, 5, 1);
|
||||
Format_UnsDec(Line+10, GPS->StdAltitude/10, 5, 0); }
|
||||
OLED_PutLine(OLED, LineIdx+5, Line);
|
||||
}
|
||||
|
||||
void OLED_DrawGPS(u8g2_t *OLED, GPS_Position *GPS=0) // GPS time, position, altitude
|
||||
{ // u8g2_SetFont(OLED, u8g2_font_ncenB14_tr);
|
||||
u8g2_SetFont(OLED, u8g2_font_7x13_tf); // 5 lines, 12 pixels/line
|
||||
uint8_t Len=0;
|
||||
Len+=Format_String(Line+Len, "GPS ");
|
||||
if(GPS && GPS->isValid())
|
||||
{ Line[Len++]='0'+GPS->FixMode; Line[Len++]='D'; Line[Len++]='/';
|
||||
Len+=Format_UnsDec(Line+Len, GPS->Satellites, 1);
|
||||
Len+=Format_String(Line+Len, "sat DOP");
|
||||
Len+=Format_UnsDec(Line+Len, (uint16_t)GPS->HDOP, 2, 1); }
|
||||
else
|
||||
{ Len+=Format_String(Line+Len, "(no lock)"); }
|
||||
Line[Len]=0;
|
||||
u8g2_DrawStr(OLED, 0, 12, Line);
|
||||
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]='.';
|
||||
Format_UnsDec (Line+ 6, (uint16_t)GPS->Year , 2, 0); Line[8]=' ';
|
||||
} else Format_String(Line, " . . ");
|
||||
if(GPS && GPS->isTimeValid())
|
||||
{ Format_UnsDec (Line+ 9, (uint16_t)GPS->Hour, 2, 0); Line[11]=':';
|
||||
Format_UnsDec (Line+12, (uint16_t)GPS->Min, 2, 0); Line[14]=':';
|
||||
Format_UnsDec (Line+15, (uint16_t)GPS->Sec, 2, 0);
|
||||
} else Format_String(Line+9, " : : ");
|
||||
Line[17]=0;
|
||||
u8g2_DrawStr(OLED, 0, 24, Line);
|
||||
Len=0;
|
||||
Len+=Format_String(Line+Len, "Lat: ");
|
||||
if(GPS && GPS->isValid())
|
||||
{ Len+=Format_SignDec(Line+Len, GPS->Latitude /6, 7, 5);
|
||||
Line[Len++]=0xB0; }
|
||||
Line[Len]=0;
|
||||
u8g2_DrawStr(OLED, 0, 36, Line);
|
||||
Len=0;
|
||||
Len+=Format_String(Line+Len, "Lon: ");
|
||||
if(GPS && GPS->isValid())
|
||||
{ Len+=Format_SignDec(Line+Len, GPS->Longitude /6, 8, 5);
|
||||
Line[Len++]=0xB0; }
|
||||
Line[Len]=0;
|
||||
u8g2_DrawStr(OLED, 0, 48, Line);
|
||||
Len=0;
|
||||
Len+=Format_String(Line+Len, "Alt: ");
|
||||
if(GPS && GPS->isValid())
|
||||
{ Len+=Format_SignDec(Line+Len, GPS->Altitude, 4, 1);
|
||||
Line[Len++]='m'; }
|
||||
Line[Len]=0;
|
||||
u8g2_DrawStr(OLED, 0, 60, Line);
|
||||
}
|
||||
|
||||
void OLED_DrawRF(u8g2_t *OLED) // RF
|
||||
{ u8g2_SetFont(OLED, u8g2_font_7x13_tf); // 5 lines. 12 pixels/line
|
||||
uint8_t Len=0;
|
||||
#ifdef WITH_RFM69
|
||||
Len+=Format_String(Line+Len, "RFM69"); // Type of RF chip used
|
||||
if(isTxTypeHW()) Line[Len++]='H';
|
||||
Line[Len++]='W';
|
||||
#endif
|
||||
#ifdef WITH_RFM95
|
||||
Len+=Format_String(Line+Len, "RFM95");
|
||||
#endif
|
||||
#ifdef WITH_SX1272
|
||||
Len+=Format_String(Line+Len, "SX1272");
|
||||
#endif
|
||||
Line[Len++]=':';
|
||||
Len+=Format_SignDec(Line+Len, (int16_t)Parameters.getTxPower()); // Tx power
|
||||
Len+=Format_String(Line+Len, "dBm");
|
||||
Line[Len++]=' ';
|
||||
Len+=Format_SignDec(Line+Len, (int32_t)Parameters.RFchipFreqCorr, 2, 1); // frequency correction
|
||||
Len+=Format_String(Line+Len, "ppm");
|
||||
Line[Len]=0;
|
||||
u8g2_DrawStr(OLED, 0, 12, Line);
|
||||
Len=0;
|
||||
Len+=Format_String(Line+Len, "Rx:"); //
|
||||
Len+=Format_SignDec(Line+Len, -5*TRX.averRSSI, 2, 1); // noise level seen by the receiver
|
||||
Len+=Format_String(Line+Len, "dBm ");
|
||||
Len+=Format_UnsDec(Line+Len, RX_OGN_Count64); // received packet/min
|
||||
Len+=Format_String(Line+Len, "/min");
|
||||
Line[Len]=0;
|
||||
u8g2_DrawStr(OLED, 0, 24, Line);
|
||||
Len=0;
|
||||
Len+=Format_SignDec(Line+Len, (int16_t)TRX.chipTemp); // RF chip internal temperature (not calibrated)
|
||||
// Line[Len++]=0xB0;
|
||||
// Line[Len++]='C';
|
||||
Len+=Format_String(Line+Len, "\260C RxFIFO:");
|
||||
Len+=Format_UnsDec(Line+Len, RF_RxFIFO.Full()); // how many packets wait in the RX queue
|
||||
Line[Len]=0;
|
||||
u8g2_DrawStr(OLED, 0, 36, Line);
|
||||
// u8g2_DrawStr(OLED, 0, 48, RF_FreqPlan.getPlanName());
|
||||
Len=0;
|
||||
Len+=Format_String(Line+Len, RF_FreqPlan.getPlanName()); // name of the frequency plan
|
||||
Line[Len++]=' ';
|
||||
Len+=Format_UnsDec(Line+Len, (uint16_t)(RF_FreqPlan.getCenterFreq()/100000), 3, 1); // center frequency
|
||||
Len+=Format_String(Line+Len, "MHz");
|
||||
Line[Len]=0;
|
||||
u8g2_DrawStr(OLED, 0, 48, Line);
|
||||
|
||||
}
|
||||
|
||||
void OLED_DrawBaro(u8g2_t *OLED, GPS_Position *GPS=0)
|
||||
{ u8g2_SetFont(OLED, u8g2_font_7x13_tf); // 5 lines, 12 pixels/line
|
||||
uint8_t Len=0;
|
||||
#ifdef WITH_BMP180
|
||||
Len+=Format_String(Line+Len, "BMP180 ");
|
||||
#endif
|
||||
#ifdef WITH_BMP280
|
||||
Len+=Format_String(Line+Len, "BMP280 ");
|
||||
#endif
|
||||
#ifdef WITH_BME280
|
||||
Len+=Format_String(Line+Len, "BME280 ");
|
||||
#endif
|
||||
#ifdef WITH_MS5607
|
||||
Len+=Format_String(Line+Len, "MS5607 ");
|
||||
#endif
|
||||
Len+=Format_UnsDec(Line+Len, GPS->Pressure/4, 5, 2);
|
||||
Len+=Format_String(Line+Len, "Pa");
|
||||
Line[Len]=0;
|
||||
u8g2_DrawStr(OLED, 0, 12, Line);
|
||||
Len=0;
|
||||
Len+=Format_UnsDec(Line+Len, GPS->StdAltitude, 5, 1);
|
||||
Len+=Format_String(Line+Len, "m ");
|
||||
Len+=Format_SignDec(Line+Len, GPS->ClimbRate, 2, 1);
|
||||
Len+=Format_String(Line+Len, "m/s");
|
||||
Line[Len]=0;
|
||||
u8g2_DrawStr(OLED, 0, 24, Line);
|
||||
Len=0;
|
||||
Len+=Format_SignDec(Line+Len, GPS->Temperature, 2, 1);
|
||||
Line[Len++]=0xB0;
|
||||
Line[Len++]='C';
|
||||
Line[Len++]=' ';
|
||||
Len+=Format_SignDec(Line+Len, GPS->Humidity, 2, 1);
|
||||
Line[Len++]='%';
|
||||
Line[Len]=0;
|
||||
u8g2_DrawStr(OLED, 0, 36, Line);
|
||||
}
|
||||
|
||||
void OLED_DrawSystem(u8g2_t *OLED)
|
||||
{
|
||||
u8g2_SetFont(OLED, u8g2_font_7x13_tf); // 5 lines, 12 pixels/line
|
||||
uint8_t Len=0;
|
||||
Len+=Format_String(Line+Len, "GPS ");
|
||||
#ifdef WITH_GPS_UBX
|
||||
Len+=Format_String(Line+Len, "UBX ");
|
||||
#endif
|
||||
#ifdef WITH_GPS_MTK
|
||||
Len+=Format_String(Line+Len, "MTK ");
|
||||
#endif
|
||||
#ifdef WITH_GPS_SRF
|
||||
Len+=Format_String(Line+Len, "SRF ");
|
||||
#endif
|
||||
Len+=Format_UnsDec(Line+Len, GPS_getBaudRate(), 1);
|
||||
Len+=Format_String(Line+Len, "bps");
|
||||
Line[Len]=0;
|
||||
u8g2_DrawStr(OLED, 0, 12, Line);
|
||||
|
||||
Len=0;
|
||||
#ifdef WITH_RFM69
|
||||
Len+=Format_String(Line+Len, "RFM69 v"); // Type of RF chip used
|
||||
if(isTxTypeHW()) Line[Len++]='H';
|
||||
Line[Len++]='W';
|
||||
#endif
|
||||
#ifdef WITH_RFM95
|
||||
Len+=Format_String(Line+Len, "RFM95 v");
|
||||
#endif
|
||||
#ifdef WITH_SX1272
|
||||
Len+=Format_String(Line+Len, "SX1272 v");
|
||||
#endif
|
||||
Len+=Format_Hex(Line+Len, TRX.chipVer);
|
||||
Line[Len++]=' ';
|
||||
Len+=Format_SignDec(Line+Len, (int16_t)TRX.chipTemp);
|
||||
Line[Len++]=0xB0;
|
||||
Line[Len++]='C';
|
||||
Line[Len]=0;
|
||||
u8g2_DrawStr(OLED, 0, 24, Line);
|
||||
|
||||
Len=0;
|
||||
#ifdef WITH_BMP180
|
||||
Len+=Format_String(Line+Len, "BMP180 0x");
|
||||
Len+=Format_Hex(Line+Len, Baro.ADDR);
|
||||
#endif
|
||||
#ifdef WITH_BMP280
|
||||
Len+=Format_String(Line+Len, "BMP280 0x");
|
||||
Len+=Format_Hex(Line+Len, Baro.ADDR);
|
||||
#endif
|
||||
#ifdef WITH_BME280
|
||||
Len+=Format_String(Line+Len, "BME280 0x");
|
||||
Len+=Format_Hex(Line+Len, Baro.ADDR);
|
||||
#endif
|
||||
#ifdef WITH_MS5607
|
||||
Len+=Format_String(Line+Len, "MS5607 0x");
|
||||
Len+=Format_Hex(Line+Len, Baro.ADDR);
|
||||
#endif
|
||||
Line[Len]=0;
|
||||
u8g2_DrawStr(OLED, 0, 36, Line);
|
||||
|
||||
#ifdef WITH_SD
|
||||
Len=0;
|
||||
Len += Format_String(Line+Len, "SD ");
|
||||
if(SD_isMounted())
|
||||
{ Len+=Format_UnsDec(Line+Len, (uint32_t)SD_getSectors());
|
||||
Line[Len++]='x';
|
||||
Len+=Format_UnsDec(Line+Len, (uint32_t)SD_getSectorSize()*5/512, 2, 1);
|
||||
Len+=Format_String(Line+Len, "KB"); }
|
||||
else
|
||||
{ Len+=Format_String(Line+Len, "none"); }
|
||||
Line[Len]=0;
|
||||
u8g2_DrawStr(OLED, 0, 60, Line);
|
||||
|
||||
#endif
|
||||
|
||||
}
|
||||
|
||||
void OLED_DrawID(u8g2_t *OLED)
|
||||
{ u8g2_SetFont(OLED, u8g2_font_9x15_tr);
|
||||
strcpy(Line, "ID: "); Parameters.Print(Line+4); Line[14]=0;
|
||||
u8g2_DrawStr(OLED, 0, 20, Line);
|
||||
u8g2_SetFont(OLED, u8g2_font_10x20_tr);
|
||||
#ifdef WITH_FollowMe
|
||||
u8g2_DrawStr(OLED, 8, 40, "FollowMe868");
|
||||
u8g2_DrawStr(OLED, 16, 60, "by AVIONIX");
|
||||
#endif
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
// ========================================================================================================================
|
||||
|
||||
|
|
@ -457,6 +134,21 @@ static void ProcessNMEA(void) // process a valid NMEA that got to the consol
|
|||
#endif
|
||||
}
|
||||
|
||||
static uint8_t Verbose=0;
|
||||
static uint32_t VerboseSuspendTime=0;
|
||||
const uint32_t VerboseSuspendTimeout=60;
|
||||
|
||||
static void CheckCtrlV(void)
|
||||
{ if(VerboseSuspendTime==0) return;
|
||||
uint32_t Time = TimeSync_Time()-VerboseSuspendTime;
|
||||
if(Time>VerboseSuspendTimeout) { Parameters.Verbose=Verbose; VerboseSuspendTime=0; }
|
||||
}
|
||||
|
||||
static void ProcessCtrlV(void)
|
||||
{ if(VerboseSuspendTime) { Parameters.Verbose=Verbose; VerboseSuspendTime=0; return; }
|
||||
Verbose = Parameters.Verbose; VerboseSuspendTime=TimeSync_Time(); Parameters.Verbose=0;
|
||||
}
|
||||
|
||||
static void ProcessCtrlC(void) // print system state to the console
|
||||
{ xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
Parameters.Print(Line);
|
||||
|
|
@ -532,13 +224,107 @@ static void ProcessCtrlL(void) // print syste
|
|||
#endif
|
||||
}
|
||||
|
||||
void SleepIn(void)
|
||||
{ xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
Format_String(CONS_UART_Write, "Sleep-in\n");
|
||||
xSemaphoreGive(CONS_Mutex);
|
||||
|
||||
#ifdef WITH_GPS_UBX
|
||||
#ifdef WITH_GPA_ENA
|
||||
GPS_DISABLE();
|
||||
#endif
|
||||
UBX_RXM_PMREQ PMREQ;
|
||||
PMREQ.duration = 0;
|
||||
PMREQ.flags = 0x01;
|
||||
UBX_RxMsg::Send(0x02, 0x41, GPS_UART_Write, (uint8_t *)(&PMREQ), sizeof(PMREQ));
|
||||
#endif
|
||||
|
||||
#ifdef WITH_GPS_MTK
|
||||
#ifdef WITH_GPA_ENA
|
||||
GPS_DISABLE();
|
||||
Format_String(GPS_UART_Write, "$PMTK225,4*2F\r\n"); // backup mode: 7uA
|
||||
#else
|
||||
Format_String(GPS_UART_Write, "$PMTK161,0*28\r\n"); // standby mode: 1mA
|
||||
#endif
|
||||
#endif
|
||||
|
||||
#ifdef WITH_OLED
|
||||
OLED_DisplayON(0);
|
||||
#endif
|
||||
|
||||
#ifdef WITH_U8G2_OLED
|
||||
u8g2_SetPowerSave(&U8G2_OLED, 1);
|
||||
#endif
|
||||
|
||||
#if defined(WITH_ST7789) || defined(WITH_ILI9341)
|
||||
LCD_SetBacklightLevel(0);
|
||||
#endif
|
||||
|
||||
PowerMode=0;
|
||||
vTaskDelay(1000); }
|
||||
|
||||
void SleepOut(void)
|
||||
{
|
||||
#ifdef WITH_GPS_ENABLE
|
||||
GPS_DISABLE();
|
||||
#endif
|
||||
PowerMode=2;
|
||||
#if defined(WITH_ST7789) || defined(WITH_ILI9341)
|
||||
LCD_SetBacklightLevel(6);
|
||||
#endif
|
||||
#ifdef WITH_U8G2_OLED
|
||||
u8g2_SetPowerSave(&U8G2_OLED, 0);
|
||||
#endif
|
||||
#ifdef WITH_OLED
|
||||
OLED_DisplayON(1);
|
||||
#endif
|
||||
|
||||
#ifdef WITH_GPS_UBX
|
||||
#ifdef WITH_GPS_ENABLE
|
||||
GPS_ENABLE();
|
||||
#endif
|
||||
Format_String(GPS_UART_Write, "\n");
|
||||
#endif
|
||||
|
||||
#ifdef WITH_GPS_MTK
|
||||
#ifdef WITH_GPS_ENABLE
|
||||
GPS_ENABLE();
|
||||
#else
|
||||
Format_String(GPS_UART_Write, "$PMTK161,1*29\r\n");
|
||||
#endif
|
||||
#endif
|
||||
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
Format_String(CONS_UART_Write, "Sleep-out\n");
|
||||
xSemaphoreGive(CONS_Mutex); }
|
||||
|
||||
#ifdef WITH_SLEEP
|
||||
static TickType_t LowBatt_Time=0;
|
||||
|
||||
static void LowBatt_Watch(void) // check battery voltage
|
||||
{ uint16_t BattVolt = BatteryVoltage>>8; // [mV]
|
||||
if(BattVolt>=3250 || BattVolt<=700 ) { LowBatt_Time=0; return; }
|
||||
int16_t BattRate = (BatteryVoltageRate+128)>>8; // [mv/sec]
|
||||
if(BattRate>0) { LowBatt_Time=0; return; }
|
||||
TickType_t Now=xTaskGetTickCount();
|
||||
if(LowBatt_Time==0) { LowBatt_Time=Now; return; }
|
||||
Now-=LowBatt_Time;
|
||||
if(Now>=30000) // if low battery and voltage dropping persists for 30sec
|
||||
{ SleepIn(); //
|
||||
Sleep(); // enter sleep
|
||||
SleepOut(); // wake up
|
||||
LowBatt_Time=0; } // reset time counter
|
||||
}
|
||||
#endif
|
||||
|
||||
static void ProcessInput(void)
|
||||
{ for( ; ; )
|
||||
{ uint8_t Byte; int Err=CONS_UART_Read(Byte); if(Err<=0) break; // get byte from console, if none: exit the loop
|
||||
#ifndef WITH_GPS_UBX_PASS
|
||||
if(Byte==0x03) ProcessCtrlC(); // if Ctrl-C received
|
||||
if(Byte==0x0C) ProcessCtrlL(); // if Ctrl-C received
|
||||
if(Byte==0x03) ProcessCtrlC(); // if Ctrl-C received: print parameters
|
||||
if(Byte==0x0C) ProcessCtrlL(); // if Ctrl-L received: list log files
|
||||
if(Byte==0x16) ProcessCtrlV(); // if Ctrl-L received: suspend (verbose) printout
|
||||
if(Byte==0x18) esp_restart() ; // if Ctrl-X received then restart
|
||||
// if(Byte==0x19) Shutdown(); // if Ctrl-Y receiver: shutdown
|
||||
#endif
|
||||
NMEA.ProcessByte(Byte); // pass the byte through the NMEA processor
|
||||
if(NMEA.isComplete()) // if complete NMEA:
|
||||
|
|
@ -560,56 +346,164 @@ static void ProcessInput(void)
|
|||
|
||||
// ========================================================================================================================
|
||||
|
||||
const uint8_t OLED_Pages = 6;
|
||||
static uint8_t OLED_Page = 1;
|
||||
|
||||
extern "C"
|
||||
void vTaskCTRL(void* pvParameters)
|
||||
{ uint32_t PrevTime=0;
|
||||
{
|
||||
uint32_t PrevTime=0;
|
||||
GPS_Position *PrevGPS=0;
|
||||
for( ; ; )
|
||||
for( ; ; ) //
|
||||
{ ProcessInput(); // process console input
|
||||
|
||||
#ifdef WITH_SLEEP
|
||||
#if defined(WITH_FollowMe) || defined(WITH_TBEAM)
|
||||
LowBatt_Watch();
|
||||
#endif
|
||||
#endif
|
||||
vTaskDelay(1); //
|
||||
|
||||
#ifdef WITH_AXP
|
||||
bool PowerOffRequest = AXP.readLongPressIRQ() /* || AXP.readShortPressIRQ() */ ;
|
||||
if(PowerOffRequest)
|
||||
{ PowerMode=0;
|
||||
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
Format_String(CONS_UART_Write, "Power-Off Request\n");
|
||||
xSemaphoreGive(CONS_Mutex);
|
||||
// vTaskDelay(1000);
|
||||
AXP.setLED(4);
|
||||
#ifdef WITH_OLED
|
||||
OLED_DisplayON(0);
|
||||
#endif
|
||||
#ifdef WITH_U8G2_OLED
|
||||
u8g2_SetPowerSave(&U8G2_OLED, 1);
|
||||
#endif
|
||||
#if defined(WITH_ST7789) || defined(WITH_ILI9341)
|
||||
LCD_SetBacklightLevel(0);
|
||||
#endif
|
||||
AXP.setPowerOutput(AXP.OUT_LDO2, 0); // turn off RFM power
|
||||
AXP.setPowerOutput(AXP.OUT_LDO3, 0); // turn off GPS power
|
||||
AXP.setPowerOutput(AXP.OUT_DCDC1, 0);
|
||||
// AXP.setPowerOutput(AXP.OUT_DCDC2, 0);
|
||||
// AXP.setPowerOutput(AXP.OUT_DCDC3, 0);
|
||||
// AXP.setPowerOutput(AXP.OUT_EXTEN, 0);
|
||||
AXP.ShutDown();
|
||||
vTaskDelay(1000);
|
||||
// #define PIN_AXP_IRQ GPIO_NUM_35
|
||||
// esp_sleep_enable_ext0_wakeup(PIN_AXP_IRQ, 0); // 1 = High, 0 = Low
|
||||
// esp_deep_sleep_start();
|
||||
}
|
||||
bool ShortPress = AXP.readShortPressIRQ();
|
||||
if(ShortPress)
|
||||
{ KeyBuffer.Write(0x04);
|
||||
#ifdef DEBUG_PRINT
|
||||
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
Format_String(CONS_UART_Write, "AXP short-press\n");
|
||||
xSemaphoreGive(CONS_Mutex);
|
||||
#endif
|
||||
AXP.clearIRQ(); }
|
||||
#endif
|
||||
|
||||
LED_TimerCheck(1); // update the LED flashes
|
||||
#ifdef WITH_BEEPER
|
||||
Play_TimerCheck(); // update the LED flashes
|
||||
#endif
|
||||
bool PageChange=0;
|
||||
int32_t PressRelease=Button_TimerCheck();
|
||||
|
||||
int32_t PressRelease=Button_TimerCheck(); // 0 = no change
|
||||
// #ifdef DEBUG_PRINT
|
||||
if(PressRelease!=0)
|
||||
{ xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
Format_String(CONS_UART_Write, "PressRelease = ");
|
||||
Format_String(CONS_UART_Write, "Button: ");
|
||||
Format_SignDec(CONS_UART_Write, PressRelease);
|
||||
Format_String(CONS_UART_Write, "ms\n");
|
||||
xSemaphoreGive(CONS_Mutex); }
|
||||
// #endif
|
||||
if(PressRelease>0)
|
||||
{ if(PressRelease<=300) // short button push: switch pages
|
||||
{ OLED_Page++; if(OLED_Page>=OLED_Pages) OLED_Page=0;
|
||||
PageChange=1; }
|
||||
else if(PressRelease<=2000) // long button push: some page action
|
||||
{ if(PressRelease<=700) // short button push: switch pages
|
||||
{ KeyBuffer.Write(0x01); }
|
||||
else if(PressRelease<=3000) // longer button push: some page action
|
||||
{ KeyBuffer.Write(0x41); }
|
||||
else // very long push
|
||||
{ }
|
||||
}
|
||||
|
||||
uint32_t Time=TimeSync_Time();
|
||||
GPS_Position *GPS = GPS_getPosition();
|
||||
bool TimeChange = Time!=PrevTime;
|
||||
uint32_t Sec = (Time-1)%60;
|
||||
GPS_Position *GPS = GPS_getPosition(Sec);
|
||||
bool GPSchange = GPS!=PrevGPS;
|
||||
if( (!TimeChange) && (!GPSchange) ) continue;
|
||||
PrevTime=Time; PrevGPS=GPS;
|
||||
|
||||
#ifdef WITH_OLED
|
||||
if(Button_SleepRequest)
|
||||
{ OLED_DisplayON(0); }
|
||||
else
|
||||
{ esp_err_t StatErr=ESP_OK;
|
||||
esp_err_t PosErr=ESP_OK;
|
||||
if(TimeChange)
|
||||
{ StatErr = OLED_DisplayStatus(Time, 0); }
|
||||
if(GPSchange)
|
||||
{ PosErr = OLED_DisplayPosition(GPS, 2); }
|
||||
}
|
||||
if(TimeChange) CheckCtrlV();
|
||||
|
||||
#ifdef DEBUG_PRINT
|
||||
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
if(TimeChange)
|
||||
{ Format_String(CONS_UART_Write, "TimeChange: ");
|
||||
// Format_HHMMSS(CONS_UART_Write, Sec);
|
||||
Format_HHMMSS(CONS_UART_Write, Time);
|
||||
Format_String(CONS_UART_Write, "\n"); }
|
||||
if(GPSchange && GPS)
|
||||
{ Format_String(CONS_UART_Write, "GPSchange: ");
|
||||
GPS->PrintLine(Line);
|
||||
Format_String(CONS_UART_Write, Line); }
|
||||
xSemaphoreGive(CONS_Mutex);
|
||||
#endif
|
||||
|
||||
#ifdef WITH_BQ
|
||||
#ifdef DEBUG_PRINT
|
||||
if(TimeChange)
|
||||
{ uint16_t Batt = BatterySense(2);
|
||||
// uint8_t ID = BQ.readID();
|
||||
// uint8_t Status = BQ.readStatus();
|
||||
// uint8_t Fault = BQ.readFault();
|
||||
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
Format_String(CONS_UART_Write, "BQ24295:");
|
||||
for(uint8_t Reg=0; Reg<=10; Reg++)
|
||||
{ uint8_t Val=BQ.readReg(Reg); CONS_UART_Write(' '); Format_Hex(CONS_UART_Write, Val); }
|
||||
// Format_Hex(CONS_UART_Write, ID);
|
||||
// CONS_UART_Write('/');
|
||||
// Format_Hex(CONS_UART_Write, Status);
|
||||
// CONS_UART_Write('/');
|
||||
// Format_Hex(CONS_UART_Write, Fault);
|
||||
Format_String(CONS_UART_Write, " Batt=");
|
||||
Format_UnsDec(CONS_UART_Write, Batt, 4, 3);
|
||||
Format_String(CONS_UART_Write, "V\n");
|
||||
xSemaphoreGive(CONS_Mutex); }
|
||||
#endif
|
||||
#endif
|
||||
|
||||
#ifdef WITH_AXP
|
||||
// #ifdef DEBUG_PRINT
|
||||
if(TimeChange)
|
||||
{ uint16_t Batt=AXP.readBatteryVoltage(); // [mV]
|
||||
uint16_t InpCurr=AXP.readBatteryInpCurrent(); // [mA]
|
||||
uint16_t OutCurr=AXP.readBatteryOutCurrent(); // [mA]
|
||||
uint16_t Vbus=AXP.readVbusVoltage(); // [mV]
|
||||
uint16_t VbusCurr=AXP.readVbusCurrent(); // [mA]
|
||||
int16_t Temp=AXP.readTemperature(); // [0.1degC]
|
||||
uint32_t InpCharge=AXP.readBatteryInpCharge();
|
||||
uint32_t OutCharge=AXP.readBatteryOutCharge();
|
||||
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
Format_String(CONS_UART_Write, "AXP192: Batt=");
|
||||
Format_UnsDec(CONS_UART_Write, Batt, 4, 3);
|
||||
Format_String(CONS_UART_Write, "V, ");
|
||||
Format_UnsDec(CONS_UART_Write, InpCurr, 4, 3);
|
||||
Format_String(CONS_UART_Write, "/");
|
||||
Format_UnsDec(CONS_UART_Write, OutCurr, 4, 3);
|
||||
Format_String(CONS_UART_Write, "A, USB: ");
|
||||
Format_UnsDec(CONS_UART_Write, Vbus, 4, 3);
|
||||
Format_String(CONS_UART_Write, "V, ");
|
||||
Format_UnsDec(CONS_UART_Write, VbusCurr, 4, 3);
|
||||
Format_String(CONS_UART_Write, "A, Charge=");
|
||||
Format_UnsDec(CONS_UART_Write, ((InpCharge<<12)+562)/1125, 2, 1);
|
||||
Format_String(CONS_UART_Write, "-");
|
||||
Format_UnsDec(CONS_UART_Write, ((OutCharge<<12)+562)/1125, 2, 1);
|
||||
Format_String(CONS_UART_Write, "mAh, Temp=");
|
||||
Format_SignDec(CONS_UART_Write, Temp, 2, 1);
|
||||
Format_String(CONS_UART_Write, "degC\n");
|
||||
xSemaphoreGive(CONS_Mutex); }
|
||||
// #endif
|
||||
#endif
|
||||
|
||||
#ifdef DEBUG_PRINT
|
||||
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
if(TimeChange)
|
||||
|
|
@ -622,26 +516,6 @@ void vTaskCTRL(void* pvParameters)
|
|||
Format_String(CONS_UART_Write, "\n"); }
|
||||
xSemaphoreGive(CONS_Mutex);
|
||||
#endif
|
||||
#endif // WITH_OLED
|
||||
|
||||
#ifdef WITH_U8G2
|
||||
if(Button_SleepRequest)
|
||||
{ u8g2_SetPowerSave(&U8G2_OLED, 0); }
|
||||
else if(TimeChange || PageChange)
|
||||
{ u8g2_ClearBuffer(&U8G2_OLED);
|
||||
switch(OLED_Page)
|
||||
{ case 2: OLED_DrawGPS (&U8G2_OLED, GPS); break;
|
||||
case 3: OLED_DrawRF (&U8G2_OLED); break;
|
||||
case 4: OLED_DrawBaro (&U8G2_OLED, GPS); break;
|
||||
case 1: OLED_DrawID (&U8G2_OLED); break;
|
||||
case 5: OLED_DrawSystem(&U8G2_OLED); break;
|
||||
default:
|
||||
{ OLED_DrawStatus(&U8G2_OLED, Time, 0);
|
||||
OLED_DrawPosition(&U8G2_OLED, GPS, 2); }
|
||||
}
|
||||
u8g2_SendBuffer(&U8G2_OLED);
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef DEBUG_PRINT // in debug mode print the parameters and state every 60sec
|
||||
if((Time%60)!=0) continue;
|
||||
|
|
|
|||
|
|
@ -1,5 +1,8 @@
|
|||
#include "fifo.h"
|
||||
#include "hal.h"
|
||||
|
||||
extern FIFO<uint8_t, 8> KeyBuffer;
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C"
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -0,0 +1,182 @@
|
|||
#include <stdio.h>
|
||||
|
||||
#include <sys/types.h>
|
||||
#include <dirent.h>
|
||||
#include <sys/stat.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#include "esp_system.h"
|
||||
// #include "esp_sleep.h"
|
||||
|
||||
#include "hal.h"
|
||||
|
||||
#include "sens.h"
|
||||
#include "rf.h"
|
||||
#include "ctrl.h"
|
||||
#include "proc.h"
|
||||
#include "log.h"
|
||||
|
||||
#include "gps.h"
|
||||
#include "ubx.h"
|
||||
#include "timesync.h"
|
||||
#include "format.h"
|
||||
|
||||
#include "disp_oled.h"
|
||||
#include "disp_lcd.h"
|
||||
|
||||
#ifdef WITH_U8G2_OLED
|
||||
const uint8_t DISP_Pages = 6;
|
||||
static uint8_t DISP_Page = 1;
|
||||
#endif
|
||||
#if defined(WITH_ST7789) || defined(WITH_ILI9341)
|
||||
const uint8_t DISP_Pages = 9;
|
||||
static uint8_t DISP_Page = 0;
|
||||
#endif
|
||||
|
||||
|
||||
extern "C"
|
||||
void vTaskDISP(void* pvParameters)
|
||||
{
|
||||
#ifdef WITH_U8G2_OLED
|
||||
u8g2_ClearBuffer(&U8G2_OLED);
|
||||
OLED_DrawLogo(&U8G2_OLED);
|
||||
u8g2_SendBuffer(&U8G2_OLED);
|
||||
#endif
|
||||
#if defined(WITH_ST7789) || defined(WITH_ILI9341)
|
||||
// LCD_Start();
|
||||
LCD_LogoPage_Draw();
|
||||
LCD_SetBacklightLevel(6); // backlight level
|
||||
#endif
|
||||
|
||||
uint32_t PrevTime=0;
|
||||
GPS_Position *PrevGPS=0;
|
||||
for( ; ; ) //
|
||||
{
|
||||
#if defined(WITH_ST7789) || defined(WITH_ILI9341)
|
||||
if(PowerMode==0)
|
||||
{ vTaskDelay(200); LCD_SetBacklightLevel(0); continue; }
|
||||
#endif
|
||||
#ifdef WITH_U8G2_OLED
|
||||
if(PowerMode==0)
|
||||
{ vTaskDelay(200); /* u8g2_SetPowerSave(&U8G2_OLED, 1); */ continue; }
|
||||
#endif
|
||||
vTaskDelay(1); //
|
||||
|
||||
bool PageChange = 0;
|
||||
uint8_t Key = 0;
|
||||
KeyBuffer.Read(Key); // read key pressed from the buffer
|
||||
if(Key) PageChange=1; // page-change on any key
|
||||
|
||||
uint32_t Time=TimeSync_Time();
|
||||
bool TimeChange = Time!=PrevTime; // did time change = a new second ?
|
||||
uint32_t Sec = (Time-1)%60;
|
||||
GPS_Position *GPS = GPS_getPosition(Sec);
|
||||
bool GPSchange = GPS!=PrevGPS; // did GPS data change = new position ?
|
||||
if(!PageChange) // if no page change was requested
|
||||
{ if( (!TimeChange) && (!GPSchange) ) continue;
|
||||
PrevTime=Time; PrevGPS=GPS; }
|
||||
|
||||
#if defined(WITH_U8G2_OLED) || defined(WITH_ST7789) || defined(WITH_ILI9341)
|
||||
if(PageChange) DISP_Page++; if(DISP_Page>=DISP_Pages) DISP_Page=0;
|
||||
#endif
|
||||
|
||||
#if defined(WITH_ST7789) || defined(WITH_ILI9341)
|
||||
static uint8_t LCD_Backlight = 8*16+8;
|
||||
const uint8_t LCD_BacklightLimit=4*16+8; // lower limit for the backlight
|
||||
#ifdef WITH_AXP
|
||||
uint16_t Vbus=AXP.readVbusVoltage(); // external supply (USB) voltage
|
||||
if(PageChange || Vbus>=4000) LCD_Backlight=8*16+8; // high backlight on page-change or when charging
|
||||
#else
|
||||
if(PageChange) LCD_Backlight=8*16+8; // high backlight on page change
|
||||
#endif
|
||||
switch(DISP_Page)
|
||||
{ case 0: if(PageChange) LCD_LogoPage_Draw(Time, GPS); // logo with basic information
|
||||
LCD_LogoPage_Update(Time, GPS, TimeChange, GPSchange);
|
||||
break;
|
||||
case 1: if(PageChange) LCD_GPSpage_Draw(Time, GPS); // GPS data
|
||||
LCD_GPSpage_Update(Time, GPS, TimeChange, GPSchange);
|
||||
break;
|
||||
case 2: if(PageChange) LCD_RFpage_Draw(Time, GPS); // RF data
|
||||
LCD_RFpage_Update(Time, GPS, TimeChange, GPSchange);
|
||||
break;
|
||||
case 3: if(PageChange) LCD_BaroPage_Draw(Time, GPS); // Baro data
|
||||
LCD_BaroPage_Update(Time, GPS, TimeChange, GPSchange);
|
||||
break;
|
||||
case 4: if(PageChange) LCD_BattPage_Draw(Time, GPS); // Battery
|
||||
LCD_BattPage_Update(Time, GPS, TimeChange, GPSchange);
|
||||
break;
|
||||
case 5: if(PageChange) LCD_ParmPage_Draw(Time, GPS); // ID and parameters
|
||||
LCD_ParmPage_Update(Time, GPS, TimeChange, GPSchange);
|
||||
break;
|
||||
case 6: if(PageChange) LCD_RelayPage_Draw(Time, GPS); // RELAY list
|
||||
LCD_RelayPage_Update(Time, GPS, TimeChange, GPSchange);
|
||||
break;
|
||||
case 7: if(PageChange) LCD_LookPage_Draw(Time, GPS); // Look targets
|
||||
LCD_LookPage_Update(Time, GPS, TimeChange, GPSchange);
|
||||
break;
|
||||
case 8: if(PageChange) LCD_SysPage_Draw(Time, GPS); // System overview
|
||||
LCD_SysPage_Update(Time, GPS, TimeChange, GPSchange);
|
||||
break;
|
||||
}
|
||||
if(TimeChange) // on each new second
|
||||
{ LCD_SetBacklightLevel(LCD_Backlight/16); //
|
||||
if(LCD_Backlight>LCD_BacklightLimit) LCD_Backlight--; } // reduce backlight a lttle if above minimum
|
||||
#endif // if WITH_ST7789 or WITH_ILI9341
|
||||
|
||||
#ifdef WITH_OLED
|
||||
// if(Button_SleepRequest)
|
||||
// { OLED_DisplayON(0); }
|
||||
// else
|
||||
{ esp_err_t StatErr=ESP_OK;
|
||||
esp_err_t PosErr=ESP_OK;
|
||||
if(TimeChange)
|
||||
{ StatErr = OLED_DisplayStatus(Time, 0); }
|
||||
if(GPSchange)
|
||||
{ PosErr = OLED_DisplayPosition(GPS, 2); }
|
||||
}
|
||||
#endif // WITH_OLED
|
||||
|
||||
#ifdef DEBUG_PRINT
|
||||
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
if(TimeChange)
|
||||
{ Format_String(CONS_UART_Write, "TimeChange: ");
|
||||
// Format_SignDec(CONS_UART_Write, StatErr);
|
||||
Format_String(CONS_UART_Write, "\n"); }
|
||||
if(GPSchange)
|
||||
{ Format_String(CONS_UART_Write, "GPSchange: ");
|
||||
// Format_SignDec(CONS_UART_Write, PosErr);
|
||||
Format_String(CONS_UART_Write, "\n"); }
|
||||
xSemaphoreGive(CONS_Mutex);
|
||||
#endif
|
||||
|
||||
#ifdef WITH_U8G2_OLED
|
||||
// if(Button_SleepRequest)
|
||||
// { u8g2_SetPowerSave(&U8G2_OLED, 0); }
|
||||
// else
|
||||
if(PageChange || ( GPS?GPSchange:TimeChange) )
|
||||
{ u8g2_ClearBuffer(&U8G2_OLED);
|
||||
// if(Look.WarnLevel)
|
||||
// { OLED_DrawTrafWarn(&U8G2_OLED, GPS); }
|
||||
// else
|
||||
{ switch(DISP_Page)
|
||||
{ case 2: OLED_DrawGPS (&U8G2_OLED, GPS); break;
|
||||
case 3: OLED_DrawRF (&U8G2_OLED); break;
|
||||
case 4: OLED_DrawBaro (&U8G2_OLED, GPS); break;
|
||||
case 1: OLED_DrawID (&U8G2_OLED); break;
|
||||
case 5: OLED_DrawSystem(&U8G2_OLED); break;
|
||||
// case 6: OLED_DrawRelay (&U8G2_OLED, GPS); break;
|
||||
// case 7: OLED_DrawLookout(&U8G2_OLED, GPS); break;
|
||||
case 0: OLED_DrawBattery(&U8G2_OLED); break;
|
||||
// case 9: OLED_DrawTrafWarn(&U8G2_OLED, GPS); break;
|
||||
// default:
|
||||
// { OLED_DrawStatus(&U8G2_OLED, Time, 0);
|
||||
// OLED_DrawPosition(&U8G2_OLED, GPS, 2); }
|
||||
}
|
||||
}
|
||||
OLED_DrawStatusBar(&U8G2_OLED, GPS);
|
||||
u8g2_SendBuffer(&U8G2_OLED);
|
||||
}
|
||||
#endif
|
||||
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,7 @@
|
|||
#include "hal.h"
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C"
|
||||
#endif
|
||||
void vTaskDISP(void* pvParameters);
|
||||
|
||||
|
|
@ -0,0 +1,713 @@
|
|||
#include <stdio.h>
|
||||
|
||||
#include <sys/types.h>
|
||||
#include <dirent.h>
|
||||
#include <sys/stat.h>
|
||||
#include <unistd.h>
|
||||
|
||||
// #include "esp_system.h"
|
||||
// #include "esp_sleep.h"
|
||||
|
||||
#include "hal.h"
|
||||
|
||||
#include "sens.h"
|
||||
#include "rf.h"
|
||||
#include "ctrl.h"
|
||||
#include "proc.h"
|
||||
// #include "log.h"
|
||||
|
||||
#include "gps.h"
|
||||
// #include "ubx.h"
|
||||
// #include "timesync.h"
|
||||
#include "format.h"
|
||||
|
||||
// #include "ymodem.h"
|
||||
|
||||
// #define DEBUG_PRINT
|
||||
|
||||
// static char Line[128];
|
||||
|
||||
// ========================================================================================================================
|
||||
|
||||
#if defined(WITH_ST7789) || defined(WITH_ILI9341)
|
||||
|
||||
#include "st7789.h"
|
||||
#include "lcd_battery.h"
|
||||
|
||||
// Embedded images
|
||||
// #ifdef WITH_M5_JACEK
|
||||
// extern const uint8_t OGN_logo_jpg[] asm("_binary_OGN_logo_320x240_jpg_start");
|
||||
// extern const uint8_t OGN_logo_end[] asm("_binary_OGN_logo_320x240_jpg_end");
|
||||
// const int OGN_logo_size = OGN_logo_end-OGN_logo_jpg;
|
||||
// #else
|
||||
extern const uint8_t OGN_logo_jpg[] asm("_binary_OGN_logo_240x240_jpg_start");
|
||||
extern const uint8_t OGN_logo_end[] asm("_binary_OGN_logo_240x240_jpg_end");
|
||||
const int OGN_logo_size = OGN_logo_end-OGN_logo_jpg;
|
||||
// #endif
|
||||
|
||||
// extern const uint8_t Club_logo_jpg[] asm("_binary_AP_logo_240x240_jpg_start");
|
||||
// extern const uint8_t Club_logo_end[] asm("_binary_AP_logo_240x240_jpg_end");
|
||||
// const int Club_logo_size = Club_logo_end-Club_logo_jpg;
|
||||
|
||||
#ifdef FOR_CKL
|
||||
extern const uint8_t Club_logo_jpg[] asm("_binary_CKL_logo_240x240_jpg_start");
|
||||
extern const uint8_t Club_logo_end[] asm("_binary_CKL_logo_240x240_jpg_end");
|
||||
const int Club_logo_size = Club_logo_end-Club_logo_jpg;
|
||||
#endif
|
||||
|
||||
// uint8_t LCD_Backlight = 8*16;
|
||||
|
||||
static void LCD_UpdateTime(uint32_t Time, GPS_Position *GPS, bool Redraw=0)
|
||||
{ static char Msg[2][10] = { { 0 }, { 0 } };
|
||||
static uint16_t Back[2] = { 0, 0 };
|
||||
static bool Idx = 0;
|
||||
Back[Idx] = RGB565_LIGHTRED;
|
||||
if(GPS && GPS->isTimeValid())
|
||||
{ if(GPS->isDateValid()) { Time=GPS->getUnixTime(); Back[Idx]=RGB565_GREEN; }
|
||||
else { Time=GPS->getDayTime(); Back[Idx]=RGB565_GREENYELLOW; }
|
||||
if(GPS->FracSec>=50) Time++; }
|
||||
uint8_t Len=Format_HHcMMcSS(Msg[Idx], Time); Msg[Idx][Len]=0;
|
||||
bool Redo = Redraw || Back[Idx]!=Back[Idx^1];
|
||||
if(Redo) LCD_DrawString(Msg[Idx], LCD_WIDTH-LCD_StringWidth(Msg[Idx])-4, LCD_HEIGHT-LCD_FontHeight(), RGB565_BLACK, Back[Idx]);
|
||||
else LCD_UpdateString(Msg[Idx], Msg[Idx^1], LCD_WIDTH-LCD_StringWidth(Msg[Idx])-4, LCD_HEIGHT-LCD_FontHeight(), RGB565_BLACK, Back[Idx]);
|
||||
Idx^=1; }
|
||||
|
||||
static LCD_BattSymb BattSymb;
|
||||
|
||||
static uint8_t BattCapacity(uint16_t mVolt)
|
||||
{ if(mVolt>=4100) return 100; // 4.1V or above => full capacity
|
||||
if(mVolt<=3600) return 0; // 3.6V or below => zero capacity
|
||||
return (mVolt-3600+2)/5; } // linear dependence (simplified)
|
||||
|
||||
static void LCD_UpdateBattery(bool Redraw=0)
|
||||
{ // static char Volt[2][8] = { { 0 }, { 0 } };
|
||||
// static uint16_t Back[2] = { 0, 0 };
|
||||
static uint16_t BattCol[2] = { 0, 0 };
|
||||
static bool Idx = 0;
|
||||
uint16_t mVolt = (BatteryVoltage+128)>>8;
|
||||
// uint16_t Voltage = (mVolt+5)/10; // [0.01V]
|
||||
uint8_t Capacity = BattCapacity(mVolt); // [mv] => [%]
|
||||
#ifdef WITH_AXP
|
||||
static char Curr[2][8] = { { 0 }, { 0 } };
|
||||
uint16_t InpCurr=AXP.readBatteryInpCurrent(); // [mA]
|
||||
uint16_t OutCurr=AXP.readBatteryOutCurrent(); // [mA]
|
||||
int16_t Current = InpCurr-OutCurr;
|
||||
uint8_t Charging = Current>0;
|
||||
#endif
|
||||
/*
|
||||
Back[Idx] = RGB565_LIGHTRED;
|
||||
if(Voltage>=350) Back[Idx] = RGB565_YELLOW;
|
||||
if(Voltage>=365) Back[Idx] = RGB565_GREENYELLOW;
|
||||
if(Voltage>=375) Back[Idx] = RGB565_GREEN;
|
||||
if(Voltage>=410) Back[Idx] = RGB565_CYAN;
|
||||
if(Voltage>=420) Back[Idx] = RGB565_LIGHTBLUE;
|
||||
if(Voltage>=425) Back[Idx] = RGB565_MAGENTA;
|
||||
Format_UnsDec(Volt[Idx], Voltage, 3, 2); Volt[Idx][4]='V'; Volt[Idx][5]=0;
|
||||
bool Redo = Redraw || Back[Idx]!=Back[Idx^1];
|
||||
if(Redo) LCD_DrawString(Volt[Idx], 4, LCD_HEIGHT-LCD_FontHeight(), RGB565_BLACK, Back[Idx]);
|
||||
else LCD_UpdateString(Volt[Idx], Volt[Idx^1], 4, LCD_HEIGHT-LCD_FontHeight(), RGB565_BLACK, Back[Idx]);
|
||||
*/
|
||||
uint8_t BattLev=(Capacity+10)/20;
|
||||
#ifdef WITH_AXP
|
||||
static uint8_t DispLev = 0;
|
||||
if(Charging) { DispLev++; if(DispLev>5) DispLev = BattLev?BattLev-1:0; }
|
||||
else { DispLev = BattLev; }
|
||||
#else
|
||||
uint8_t &DispLev = BattLev;
|
||||
#endif
|
||||
|
||||
// static uint8_t Level = 0;
|
||||
const uint16_t LevelCol[6] = { RGB565_RED, RGB565_RED, RGB565_ORANGE, RGB565_YELLOW, RGB565_GREENYELLOW, RGB565_GREEN };
|
||||
// const uint16_t LevelCol[6] = { RGB565_RED, RGB565_RED, RGB565_DARKORANGE, RGB565_DARKYELLOW, RGB565_DARKGREENYELLOW, RGB565_DARKGREEN };
|
||||
BattSymb.setLevel(DispLev);
|
||||
#ifdef PRINT_DEBUG
|
||||
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
Format_String(CONS_UART_Write, "LCD_UpdateBattery() ");
|
||||
Format_Hex(CONS_UART_Write, BattSymb.CellMap);
|
||||
CONS_UART_Write('/');
|
||||
Format_Hex(CONS_UART_Write, BattSymb.Flags);
|
||||
Format_String(CONS_UART_Write, "\n");
|
||||
xSemaphoreGive(CONS_Mutex);
|
||||
#endif
|
||||
BattCol[Idx]=LevelCol[BattLev];
|
||||
if(Redraw || BattCol[Idx]!=BattCol[Idx^1])
|
||||
{ BattSymb.Xpos=0; BattSymb.Ypos=LCD_HEIGHT-BattSymb.Width;
|
||||
BattSymb.FrameCol=RGB565_DARKGREY; BattSymb.FillCol=RGB565_LIGHTGREY; BattSymb.CellCol=BattCol[Idx];
|
||||
if(BattSymb.CellCol==RGB565_RED) BattSymb.FrameCol=RGB565_RED;
|
||||
BattSymb.Draw(); }
|
||||
else
|
||||
BattSymb.Update();
|
||||
// Level+=1; if(Level>5) Level=0;
|
||||
|
||||
#ifndef FOR_CKL
|
||||
#ifdef WITH_AXP
|
||||
strcpy(Curr[Idx], " mA ");
|
||||
Format_SignDec(Curr[Idx], Current, 3);
|
||||
bool Redo = Redraw || Curr[Idx][0]!=Curr[Idx^1][0];
|
||||
if(Redo)
|
||||
LCD_DrawString(Curr[Idx], 0, LCD_HEIGHT-LCD_FontHeight()-LCD_FontHeight(tft_Dejavu18), RGB565_BLACK, RGB565_WHITE, tft_Dejavu18);
|
||||
else
|
||||
LCD_UpdateString(Curr[Idx], Curr[Idx^1], 0, LCD_HEIGHT-LCD_FontHeight()-LCD_FontHeight(tft_Dejavu18), RGB565_BLACK, RGB565_WHITE, tft_Dejavu18);
|
||||
#endif
|
||||
#endif
|
||||
Idx^=1; }
|
||||
|
||||
static void LCD_UpdateRX(bool Redraw=0)
|
||||
{ static char Count[2][12] = { { 0 }, { 0 } };
|
||||
static char Noise[2][12] = { { 0 }, { 0 } };
|
||||
static bool Idx = 0;
|
||||
uint8_t Len=0;
|
||||
Len+=Format_String(Count[Idx]+Len, " Rx:"); //
|
||||
Len+=Format_UnsDec(Count[Idx]+Len, RX_OGN_Count64); // received packet/min
|
||||
Len+=Format_String(Count[Idx]+Len, "/min");
|
||||
Count[Idx][Len]=0;
|
||||
if(Redraw) LCD_DrawString(Count[Idx], LCD_WIDTH-LCD_StringWidth(Count[Idx], tft_Dejavu18), 4, RGB565_BLACK, RGB565_WHITE, tft_Dejavu18);
|
||||
else LCD_UpdateString(Count[Idx], Noise[Idx^1], LCD_WIDTH-LCD_StringWidth(Count[Idx], tft_Dejavu18), 4, RGB565_BLACK, RGB565_WHITE, tft_Dejavu18);
|
||||
Len=0;
|
||||
Len+=Format_String(Noise[Idx]+Len, " ");
|
||||
Len+=Format_SignDec(Noise[Idx]+Len, -5*TRX.averRSSI, 4, 1); // noise level seen by the receiver
|
||||
Len+=Format_String(Noise[Idx]+Len, "dBm");
|
||||
Noise[Idx][Len]=0;
|
||||
if(Redraw) LCD_DrawString(Noise[Idx], LCD_WIDTH-LCD_StringWidth(Noise[Idx], tft_Dejavu18), 4+LCD_FontHeight(tft_Dejavu18), RGB565_BLACK, RGB565_WHITE, tft_Dejavu18);
|
||||
else LCD_UpdateString(Noise[Idx], Noise[Idx^1], LCD_WIDTH-LCD_StringWidth(Noise[Idx], tft_Dejavu18), 4+LCD_FontHeight(tft_Dejavu18), RGB565_BLACK, RGB565_WHITE, tft_Dejavu18);
|
||||
Idx^=1; }
|
||||
|
||||
static void LCD_UpdateGPS(GPS_Position *GPS, bool Redraw=0)
|
||||
{ static char Sat[2][8] = { { 0 }, { 0 } };
|
||||
static uint16_t Back[2] = { 0, 0 };
|
||||
// static char SNR[2][8] = { { 0 }, { 0 } };
|
||||
static char Alt[2][8] = { { 0 }, { 0 } };
|
||||
static bool Idx = 0;
|
||||
Back[Idx] = RGB565_LIGHTRED;
|
||||
if(GPS)
|
||||
{ uint16_t Sats = 0;
|
||||
if(GPS->isValid()) Sats = GPS->Satellites;
|
||||
if(Sats>=5) Back[Idx] = RGB565_GREEN;
|
||||
else if(Sats>=4) Back[Idx] = RGB565_GREENYELLOW;
|
||||
else if(Sats>=3) Back[Idx] = RGB565_YELLOW;
|
||||
if(GPS->Sec&3) { Format_UnsDec(Sat[Idx], Sats, 2); Format_String(Sat[Idx]+2, "sat"); }
|
||||
else { Format_UnsDec(Sat[Idx], (GPS_SatSNR+2)/4, 2); Format_String(Sat[Idx]+2, "dB "); }
|
||||
Sat[Idx][5]=0; }
|
||||
else // no data from GPS ?
|
||||
{ Format_String(Sat[Idx], "GPS ?"); Sat[Idx][5]=0; }
|
||||
bool Redo = Redraw || Back[Idx]!=Back[Idx^1];
|
||||
if(Redo) LCD_DrawString(Sat[Idx], 0, 0, RGB565_BLACK, Back[Idx]);
|
||||
else LCD_UpdateString(Sat[Idx], Sat[Idx^1], 0, 0, RGB565_BLACK, Back[Idx]);
|
||||
if(GPS && GPS->isValid())
|
||||
{ int32_t Altitude=GPS->Altitude; if(Altitude<0) Altitude=0; Altitude=(Altitude+5)/10;
|
||||
uint8_t Len=Format_UnsDec(Alt[Idx], (uint32_t)Altitude);
|
||||
Len+=Format_String(Alt[Idx]+Len, "m ");
|
||||
Alt[Idx][Len]=0;
|
||||
#ifdef FOR_CKL
|
||||
if(Redraw) LCD_DrawString(Alt[Idx], LCD_WIDTH-6*16, 0, RGB565_BLACK, RGB565_WHITE);
|
||||
else LCD_UpdateString(Alt[Idx], Alt[Idx^1], LCD_WIDTH-6*16, 0, RGB565_BLACK, RGB565_WHITE);
|
||||
#else
|
||||
if(Redraw) LCD_DrawString(Alt[Idx], 0, LCD_FontHeight(), RGB565_BLACK, RGB565_WHITE);
|
||||
else LCD_UpdateString(Alt[Idx], Alt[Idx^1], 0, LCD_FontHeight(), RGB565_BLACK, RGB565_WHITE);
|
||||
#endif
|
||||
}
|
||||
else
|
||||
{ }
|
||||
Idx^=1; }
|
||||
|
||||
static void LCD_UpdatePosition(GPS_Position *GPS, bool Redraw=0)
|
||||
{ static bool Idx = 0;
|
||||
int PosY = 0;
|
||||
|
||||
static char Lock[2][20] = { { 0 }, { 0 } };
|
||||
if(GPS)
|
||||
{ strcpy(Lock[Idx], "0/00sat/00dB/00.0 ");
|
||||
Lock[Idx][0] = '0'+GPS->FixQuality;
|
||||
Format_UnsDec(Lock[Idx]+2, GPS->Satellites, 2);
|
||||
Format_UnsDec(Lock[Idx]+8, (GPS_SatSNR+2)/4, 2);
|
||||
Format_UnsDec(Lock[Idx]+13, GPS->HDOP, 3, 1); }
|
||||
else
|
||||
{ strcpy(Lock[Idx], "No data from GPS "); }
|
||||
if(Redraw) LCD_DrawString(Lock[Idx], 4, PosY, RGB565_BLACK, RGB565_WHITE);
|
||||
else LCD_UpdateString(Lock[Idx], Lock[Idx^1], 4, PosY, RGB565_BLACK, RGB565_WHITE);
|
||||
PosY+=LCD_FontHeight();
|
||||
|
||||
static char Time[2][18] = { { 0 }, { 0 } };
|
||||
static char Lat [2][18] = { { 0 }, { 0 } };
|
||||
static char Lon [2][18] = { { 0 }, { 0 } };
|
||||
static char Alt [2][18] = { { 0 }, { 0 } };
|
||||
static char Vrt [2][18] = { { 0 }, { 0 } };
|
||||
static char Spd [2][18] = { { 0 }, { 0 } };
|
||||
static char Trk [2][18] = { { 0 }, { 0 } };
|
||||
static char Trn [2][18] = { { 0 }, { 0 } };
|
||||
strcpy(Time[Idx], "00.00.0000 000000");
|
||||
strcpy(Lat[Idx], "Lat: __._____ ");
|
||||
strcpy(Lon[Idx], "Lon: ___._____ ");
|
||||
strcpy(Alt[Idx], "Alt: ____._m ");
|
||||
strcpy(Vrt[Idx], "Vrt: __._ m/s ");
|
||||
strcpy(Spd[Idx], "Spd: __._ m/s ");
|
||||
strcpy(Trk[Idx], "Trk: ___._ deg ");
|
||||
strcpy(Trn[Idx], "Trn: __._ deg/s ");
|
||||
if(GPS)
|
||||
{ if(GPS->isTimeValid())
|
||||
{ Format_UnsDec (Time[Idx]+11, (uint16_t)GPS->Hour, 2, 0);
|
||||
Format_UnsDec (Time[Idx]+13, (uint16_t)GPS->Min, 2, 0);
|
||||
Format_UnsDec (Time[Idx]+15, (uint16_t)GPS->Sec, 2, 0); }
|
||||
if(GPS->isDateValid())
|
||||
{ Format_UnsDec (Time[Idx] , (uint16_t)GPS->Day, 2, 0);
|
||||
Format_UnsDec (Time[Idx]+ 3, (uint16_t)GPS->Month, 2, 0);
|
||||
Format_UnsDec (Time[Idx]+ 6, (uint16_t)GPS->Year , 4, 0); }
|
||||
Format_SignDec(Lat[Idx]+6, GPS->Latitude /6, 7, 5);
|
||||
Format_SignDec(Lon[Idx]+5, GPS->Longitude/6, 8, 5);
|
||||
Format_SignDec(Alt[Idx]+6, GPS->Altitude, 5, 1);
|
||||
Format_UnsDec (Spd[Idx]+5, GPS->Speed, 3, 1);
|
||||
Format_SignDec(Vrt[Idx]+5, GPS->ClimbRate, 3, 1);
|
||||
Format_UnsDec (Trk[Idx]+5, GPS->Heading, 4, 1);
|
||||
Format_SignDec(Trn[Idx]+5, GPS->TurnRate, 3, 1); }
|
||||
if(Redraw)
|
||||
LCD_DrawString(Time[Idx], 4, PosY, RGB565_BLACK, RGB565_WHITE);
|
||||
else
|
||||
LCD_UpdateString(Time[Idx], Time[Idx^1], 4, PosY, RGB565_BLACK, RGB565_WHITE);
|
||||
PosY+=LCD_FontHeight();
|
||||
if(Redraw || Lat[Idx][6]!=Lat[Idx^1][6])
|
||||
LCD_DrawString(Lat[Idx], 4, PosY, RGB565_BLACK, RGB565_WHITE);
|
||||
else
|
||||
LCD_UpdateString(Lat[Idx], Lat[Idx^1], 4, PosY, RGB565_BLACK, RGB565_WHITE);
|
||||
PosY+=LCD_FontHeight();
|
||||
if(Redraw || Lon[Idx][5]!=Lon[Idx^1][5])
|
||||
LCD_DrawString(Lon[Idx], 4, PosY, RGB565_BLACK, RGB565_WHITE);
|
||||
else
|
||||
LCD_UpdateString(Lon[Idx], Lon[Idx^1], 4, PosY, RGB565_BLACK, RGB565_WHITE);
|
||||
PosY+=LCD_FontHeight();
|
||||
if(Redraw || Alt[Idx][6]!=Alt[Idx^1][6])
|
||||
LCD_DrawString(Alt[Idx], 4, PosY, RGB565_BLACK, RGB565_WHITE);
|
||||
else
|
||||
LCD_UpdateString(Alt[Idx], Alt[Idx^1], 4, PosY, RGB565_BLACK, RGB565_WHITE);
|
||||
PosY+=LCD_FontHeight();
|
||||
if(Redraw || Vrt[Idx][5]!=Vrt[Idx^1][5])
|
||||
LCD_DrawString(Vrt[Idx], 4, PosY, RGB565_BLACK, RGB565_WHITE);
|
||||
else
|
||||
LCD_UpdateString(Vrt[Idx], Vrt[Idx^1], 4, PosY, RGB565_BLACK, RGB565_WHITE);
|
||||
PosY+=LCD_FontHeight();
|
||||
if(Redraw)
|
||||
LCD_DrawString(Spd[Idx], 4, PosY, RGB565_BLACK, RGB565_WHITE);
|
||||
else
|
||||
LCD_UpdateString(Spd[Idx], Spd[Idx^1], 4, PosY, RGB565_BLACK, RGB565_WHITE);
|
||||
PosY+=LCD_FontHeight();
|
||||
if(Redraw)
|
||||
LCD_DrawString(Trk[Idx], 4, PosY, RGB565_BLACK, RGB565_WHITE);
|
||||
else
|
||||
LCD_UpdateString(Trk[Idx], Trk[Idx^1], 4, PosY, RGB565_BLACK, RGB565_WHITE);
|
||||
PosY+=LCD_FontHeight();
|
||||
if(Redraw || Trn[Idx][5]!=Trn[Idx^1][5])
|
||||
LCD_DrawString(Trn[Idx], 4, PosY, RGB565_BLACK, RGB565_WHITE);
|
||||
else
|
||||
LCD_UpdateString(Trn[Idx], Trn[Idx^1], 4, PosY, RGB565_BLACK, RGB565_WHITE);
|
||||
PosY+=LCD_FontHeight();
|
||||
|
||||
Idx^=1; }
|
||||
|
||||
static void LCD_UpdateSys(bool Redraw=0)
|
||||
{ if(!Redraw) return;
|
||||
int PosY = 2;
|
||||
char Line[24];
|
||||
uint8_t Len=0;
|
||||
Len+=Format_String(Line+Len, "GPS ");
|
||||
#ifdef WITH_GPS_UBX
|
||||
Len+=Format_String(Line+Len, "UBX ");
|
||||
#endif
|
||||
#ifdef WITH_GPS_MTK
|
||||
Len+=Format_String(Line+Len, "MTK ");
|
||||
#endif
|
||||
#ifdef WITH_GPS_SRF
|
||||
Len+=Format_String(Line+Len, "SRF ");
|
||||
#endif
|
||||
Len+=Format_UnsDec(Line+Len, GPS_getBaudRate(), 1);
|
||||
Len+=Format_String(Line+Len, "bps");
|
||||
Line[Len]=0;
|
||||
LCD_DrawString(Line, 4, PosY, RGB565_BLACK, RGB565_WHITE);
|
||||
PosY+=LCD_FontHeight();
|
||||
|
||||
Len=0;
|
||||
#ifdef WITH_RFM69
|
||||
Len+=Format_String(Line+Len, "RFM69 v"); // Type of RF chip used
|
||||
if(Parameters.isTxTypeHW()) Line[Len++]='H';
|
||||
Line[Len++]='W';
|
||||
#endif
|
||||
#ifdef WITH_RFM95
|
||||
Len+=Format_String(Line+Len, "RFM95 v");
|
||||
#endif
|
||||
#ifdef WITH_SX1272
|
||||
Len+=Format_String(Line+Len, "SX1272 v");
|
||||
#endif
|
||||
Len+=Format_Hex(Line+Len, TRX.chipVer);
|
||||
// Line[Len++]=' ';
|
||||
// Len+=Format_SignDec(Line+Len, (int16_t)TRX.chipTemp);
|
||||
// Line[Len++]=0xB0;
|
||||
// Line[Len++]='C';
|
||||
Line[Len]=0;
|
||||
LCD_DrawString(Line, 4, PosY, RGB565_BLACK, RGB565_WHITE);
|
||||
PosY+=LCD_FontHeight();
|
||||
|
||||
Len=0;
|
||||
#ifdef WITH_BMP180
|
||||
Len+=Format_String(Line+Len, "BMP180 0x");
|
||||
Len+=Format_Hex(Line+Len, Baro.ADDR);
|
||||
#endif
|
||||
#ifdef WITH_BMP280
|
||||
Len+=Format_String(Line+Len, "BMP280 0x");
|
||||
Len+=Format_Hex(Line+Len, Baro.ADDR);
|
||||
#endif
|
||||
#ifdef WITH_BME280
|
||||
Len+=Format_String(Line+Len, "BME280 0x");
|
||||
Len+=Format_Hex(Line+Len, Baro.ADDR);
|
||||
#endif
|
||||
#ifdef WITH_MS5607
|
||||
Len+=Format_String(Line+Len, "MS5607 0x");
|
||||
Len+=Format_Hex(Line+Len, Baro.ADDR);
|
||||
#endif
|
||||
Line[Len]=0;
|
||||
LCD_DrawString(Line, 4, PosY, RGB565_BLACK, RGB565_WHITE);
|
||||
PosY+=LCD_FontHeight();
|
||||
|
||||
Len=0;
|
||||
#ifdef WITH_ST7789
|
||||
Len+=Format_String(Line+Len, "ST7789 240x240");
|
||||
#endif
|
||||
#ifdef WITH_ILI9341
|
||||
Len+=Format_String(Line+Len, "ILI9341 320x240");
|
||||
#endif
|
||||
Line[Len]=0;
|
||||
LCD_DrawString(Line, 4, PosY, RGB565_BLACK, RGB565_WHITE);
|
||||
PosY+=LCD_FontHeight();
|
||||
|
||||
#ifdef WITH_SPIFFS
|
||||
Len=0;
|
||||
Len+=Format_String(Line+Len, "SPIFFS ");
|
||||
size_t Total, Used;
|
||||
if(SPIFFS_Info(Total, Used)==0) // get the SPIFFS usage summary
|
||||
{ Len+=Format_UnsDec(Line+Len, (Total-Used)/1024);
|
||||
Len+=Format_String(Line+Len, "/");
|
||||
Len+=Format_UnsDec(Line+Len, Total/1024);
|
||||
Len+=Format_String(Line+Len, "kB"); }
|
||||
Line[Len]=0;
|
||||
LCD_DrawString(Line, 4, PosY, RGB565_BLACK, RGB565_WHITE);
|
||||
PosY+=LCD_FontHeight();
|
||||
#endif
|
||||
|
||||
#ifdef WITH_SD
|
||||
Len=0;
|
||||
Len+=Format_String(Line+Len, "SD ");
|
||||
if(SD_isMounted())
|
||||
{ Len+=Format_UnsDec(Line+Len, (uint32_t)SD_getSectors());
|
||||
Line[Len++]='x';
|
||||
Len+=Format_UnsDec(Line+Len, (uint32_t)SD_getSectorSize()*5/512, 2, 1);
|
||||
Len+=Format_String(Line+Len, "KB"); }
|
||||
else
|
||||
{ Len+=Format_String(Line+Len, "none"); }
|
||||
Line[Len]=0;
|
||||
LCD_DrawString(Line, 4, PosY, RGB565_BLACK, RGB565_WHITE);
|
||||
PosY+=LCD_FontHeight();
|
||||
#endif
|
||||
|
||||
|
||||
}
|
||||
|
||||
static void LCD_UpdateRF(bool Redraw=0)
|
||||
{ static bool Idx = 0;
|
||||
if(!Redraw) return;
|
||||
|
||||
int PosY = 2;
|
||||
char Line[24];
|
||||
uint8_t Len=0;
|
||||
#ifdef WITH_RFM69
|
||||
Len+=Format_String(Line+Len, "RFM69"); // Type of RF chip used
|
||||
if(Parameters.isTxTypeHW()) Line[Len++]='H';
|
||||
Line[Len++]='W';
|
||||
#endif
|
||||
#ifdef WITH_RFM95
|
||||
Len+=Format_String(Line+Len, "RFM95");
|
||||
#endif
|
||||
#ifdef WITH_SX1272
|
||||
Len+=Format_String(Line+Len, "SX1272");
|
||||
#endif
|
||||
Line[Len++]=':'; Line[Len++]=' ';
|
||||
Len+=Format_SignDec(Line+Len, (int16_t)Parameters.getTxPower()); // Tx power
|
||||
Len+=Format_String(Line+Len, "dBm");
|
||||
Line[Len]=0;
|
||||
LCD_DrawString(Line, 4, PosY, RGB565_BLACK, RGB565_WHITE);
|
||||
PosY+=LCD_FontHeight();
|
||||
|
||||
Len=0;
|
||||
Len+=Format_UnsDec(Line+Len, (uint16_t)(RF_FreqPlan.getCenterFreq()/100000), 3, 1); // center frequency
|
||||
Len+=Format_String(Line+Len, "MHz");
|
||||
Line[Len++]=' ';
|
||||
Len+=Format_String(Line+Len, RF_FreqPlan.getPlanName()); // name of the frequency plan
|
||||
Line[Len]=0;
|
||||
LCD_DrawString(Line, 4, PosY, RGB565_BLACK, RGB565_WHITE);
|
||||
PosY+=LCD_FontHeight();
|
||||
|
||||
Len=0;
|
||||
Len+=Format_SignDec(Line+Len, (int32_t)Parameters.RFchipFreqCorr, 2, 1); // frequency correction
|
||||
Len+=Format_String(Line+Len, "ppm");
|
||||
Line[Len]=0;
|
||||
LCD_DrawString(Line, 4, PosY, RGB565_BLACK, RGB565_WHITE);
|
||||
PosY+=LCD_FontHeight();
|
||||
|
||||
|
||||
Idx^=1; }
|
||||
|
||||
static void LCD_UpdatePower(bool Redraw=0)
|
||||
{ static bool Idx = 0;
|
||||
|
||||
static char USB[2][20] = { { 0 }, { 0 } };
|
||||
|
||||
Idx^=1; }
|
||||
|
||||
static void LCD_UpdateParameters(bool Redraw=0)
|
||||
{ if(!Redraw) return;
|
||||
char Line[32];
|
||||
int PosY = 2;
|
||||
uint8_t Len=Format_String(Line, "ID=");
|
||||
Line[Len++]=HexDigit(Parameters.AcftType); Line[Len++]=':';
|
||||
Line[Len++]=HexDigit(Parameters.AddrType); Line[Len++]=':';
|
||||
Len+=Format_Hex(Line+Len, Parameters.Address, 6);
|
||||
Line[Len]=0;
|
||||
LCD_DrawString(Line, 4, PosY, RGB565_BLACK, RGB565_WHITE);
|
||||
PosY+=LCD_FontHeight();
|
||||
|
||||
for(uint8_t Idx=0; Idx<Parameters.InfoParmNum; Idx++)
|
||||
{ if(PosY>(LCD_HEIGHT-2*LCD_FontHeight())) break;
|
||||
const char *Value = Parameters.InfoParmValue(Idx); if(Value[0]==0) continue;
|
||||
uint8_t Len=Format_String(Line, OGN_Packet::InfoParmName(Idx));
|
||||
Line[Len++]='=';
|
||||
Len+=Format_String(Line+Len, Value);
|
||||
Line[Len]=0;
|
||||
LCD_DrawString(Line, 4, PosY, RGB565_BLACK, RGB565_WHITE);
|
||||
PosY+=LCD_FontHeight(); }
|
||||
|
||||
}
|
||||
|
||||
static void LCD_UpdateRelayList(bool Redraw=0)
|
||||
{ static uint8_t PrevLines=0;
|
||||
if(Redraw) PrevLines=0;
|
||||
char Line[24];
|
||||
int PosY = 2;
|
||||
uint8_t Lines=0;
|
||||
for( uint8_t Idx=0; Idx<RelayQueueSize; Idx++)
|
||||
{ if(PosY>(LCD_HEIGHT-2*LCD_FontHeight())) break;
|
||||
OGN_RxPacket<OGN_Packet> *Packet = RelayQueue.Packet+Idx; if(Packet->Rank==0) continue;
|
||||
uint8_t Len=0;
|
||||
Line[Len++]='0'+Packet->Packet.Header.AddrType;
|
||||
Line[Len++]=':';
|
||||
Len+=Format_Hex(Line+Len, Packet->Packet.Header.Address, 6);
|
||||
Line[Len++]=' ';
|
||||
Len+=Format_Hex(Line+Len, Packet->Rank);
|
||||
Line[Len++]=' ';
|
||||
Line[Len++]=':';
|
||||
Len+=Format_UnsDec(Line+Len, Packet->Packet.Position.Time, 2);
|
||||
Line[Len]=0;
|
||||
LCD_DrawString(Line, 4, PosY, RGB565_BLACK, RGB565_WHITE);
|
||||
PosY+=LCD_FontHeight(); Lines++; }
|
||||
for(uint8_t Line=Lines; Line<PrevLines; Line++)
|
||||
{ if(PosY>(LCD_HEIGHT-2*LCD_FontHeight())) break;
|
||||
LCD_DrawBox(0, PosY, LCD_WIDTH, LCD_FontHeight(), RGB565_WHITE);
|
||||
PosY+=LCD_FontHeight(); }
|
||||
PrevLines=Lines; }
|
||||
|
||||
static void LCD_UpdateLookList(bool Redraw=0)
|
||||
{ static uint8_t PrevLines=0;
|
||||
if(Redraw) PrevLines=0;
|
||||
char Line[24];
|
||||
int PosY = 2;
|
||||
uint8_t Lines=0;
|
||||
for( uint8_t Idx=0; Idx<Look.MaxTargets; Idx++)
|
||||
{ if(PosY>(LCD_HEIGHT-2*LCD_FontHeight())) break;
|
||||
const LookOut_Target *Tgt = Look.Target+Idx; if(!Tgt->Alloc) continue;
|
||||
uint16_t Bkg = RGB565_GREEN;
|
||||
uint8_t Len=0;
|
||||
Len+=Format_Hex(Line+Len, Tgt->ID, 7);
|
||||
Line[Len++]=' ';
|
||||
if(Tgt->DistMargin) continue;
|
||||
uint8_t Warn = Tgt->WarnLevel;
|
||||
if(Warn>=3) Bkg = RGB565_LIGHTRED;
|
||||
else if(Warn==2) Bkg = RGB565_ORANGE;
|
||||
else if(Warn==1) Bkg = RGB565_YELLOW;
|
||||
Len+=Format_UnsDec(Line+Len, Tgt->TimeMargin>>1, 2);
|
||||
Line[Len++]='s'; Line[Len++]=' ';
|
||||
// Len+=Format_UnsDec(Line+Len, ((Tgt->MissDist>>1)+50)/100, 2, 1);
|
||||
Len+=Format_UnsDec(Line+Len, ((Tgt->HorDist>>1)+50)/100, 2, 1);
|
||||
Line[Len++]='k'; Line[Len++]='m'; Line[Len++]=' '; Line[Len]=0;
|
||||
LCD_DrawString(Line, 4, PosY, RGB565_BLACK, Bkg);
|
||||
PosY+=LCD_FontHeight(); Lines++; }
|
||||
for( uint8_t Idx=0; Idx<Look.MaxTargets; Idx++)
|
||||
{ if(PosY>(LCD_HEIGHT-2*LCD_FontHeight())) break;
|
||||
const LookOut_Target *Tgt = Look.Target+Idx; if(!Tgt->Alloc) continue;
|
||||
uint16_t Bkg = RGB565_GREEN;
|
||||
uint8_t Len=0;
|
||||
Len+=Format_Hex(Line+Len, Tgt->ID, 7);
|
||||
Line[Len++]=' ';
|
||||
if(Tgt->DistMargin==0) continue;
|
||||
Line[Len++]=' '; Line[Len++]=' '; Line[Len++]=' '; Line[Len++]=' '; Line[Len++]=' ';
|
||||
Len+=Format_UnsDec(Line+Len, ((Tgt->HorDist>>1)+50)/100, 2, 1);
|
||||
Line[Len++]='k'; Line[Len++]='m'; Line[Len++]=' '; Line[Len]=0;
|
||||
LCD_DrawString(Line, 4, PosY, RGB565_BLACK, Bkg);
|
||||
PosY+=LCD_FontHeight(); Lines++; }
|
||||
for(uint8_t Line=Lines; Line<PrevLines; Line++)
|
||||
{ if(PosY>(LCD_HEIGHT-2*LCD_FontHeight())) break;
|
||||
LCD_DrawBox(0, PosY, LCD_WIDTH, LCD_FontHeight(), RGB565_WHITE);
|
||||
PosY+=LCD_FontHeight(); }
|
||||
PrevLines=Lines; }
|
||||
|
||||
static void LCD_DrawID(void)
|
||||
{ const uint8_t *Font = tft_Dejavu18;
|
||||
char ID[16];
|
||||
uint8_t Len=0;
|
||||
ID[Len++]=HexDigit(Parameters.AcftType); ID[Len++]=':';
|
||||
ID[Len++]=HexDigit(Parameters.AddrType); ID[Len++]=':';
|
||||
Len+=Format_Hex(ID+Len, Parameters.Address, 6);
|
||||
ID[Len]=0;
|
||||
int PosX = LCD_WIDTH -LCD_StringWidth(ID, Font) -4;
|
||||
int PosY = LCD_HEIGHT -LCD_FontHeight(Font) -LCD_FontHeight();
|
||||
LCD_DrawTranspString(ID, PosX, PosY, RGB565_BLUE, Font);
|
||||
|
||||
}
|
||||
|
||||
// ------------------------------------------------------------------------------------------------------------------------
|
||||
|
||||
void LCD_LogoPage_Draw(void) // Logo page: initial background draw
|
||||
{ if(LCD_WIDTH!=240) LCD_ClearDisplay(RGB565_WHITE);
|
||||
#ifdef FOR_CKL
|
||||
LCD_DrawJPEG(Club_logo_jpg, Club_logo_size, (LCD_WIDTH-240)/2, 0);
|
||||
#else
|
||||
// #ifdef WITH_M5_JACEK
|
||||
// LCD_DrawJPEG( OGN_logo_jpg, OGN_logo_size, (LCD_WIDTH-320)/2, 0);
|
||||
// #else
|
||||
LCD_DrawJPEG( OGN_logo_jpg, OGN_logo_size, (LCD_WIDTH-240)/2, 0);
|
||||
// #endif
|
||||
LCD_DrawID();
|
||||
#endif
|
||||
}
|
||||
|
||||
void LCD_LogoPage_Draw(uint32_t Time, GPS_Position *GPS) // Logo page: initial draw with Time/GPS data
|
||||
{ LCD_LogoPage_Draw();
|
||||
LCD_UpdateTime(Time, GPS, 1); LCD_UpdateBattery(1);
|
||||
#ifndef FOR_CKL
|
||||
LCD_UpdateRX(1);
|
||||
#endif
|
||||
LCD_UpdateGPS(GPS, 1); }
|
||||
|
||||
void LCD_LogoPage_Update(uint32_t Time, GPS_Position *GPS, bool TimeChange, bool GPSchange) // Logo page: update the data in the corners
|
||||
{ if(TimeChange)
|
||||
{ LCD_UpdateTime(Time, GPS);
|
||||
LCD_UpdateBattery();
|
||||
#ifndef FOR_CKL
|
||||
LCD_UpdateRX();
|
||||
#endif
|
||||
}
|
||||
if(GPSchange) { LCD_UpdateGPS(GPS); }
|
||||
}
|
||||
|
||||
void LCD_GPSpage_Draw(void)
|
||||
{ LCD_ClearDisplay(RGB565_WHITE); }
|
||||
|
||||
void LCD_GPSpage_Draw(uint32_t Time, GPS_Position *GPS)
|
||||
{ LCD_GPSpage_Draw();
|
||||
LCD_UpdateTime(Time, GPS, 1); LCD_UpdateBattery(1);
|
||||
LCD_UpdatePosition(GPS, 1); }
|
||||
|
||||
void LCD_GPSpage_Update(uint32_t Time, GPS_Position *GPS, bool TimeChange, bool GPSchange)
|
||||
{ if(TimeChange) { LCD_UpdateTime(Time, GPS); LCD_UpdateBattery(); }
|
||||
if(GPSchange) LCD_UpdatePosition(GPS); }
|
||||
|
||||
|
||||
void LCD_RFpage_Draw(void)
|
||||
{ LCD_ClearDisplay(RGB565_WHITE); }
|
||||
|
||||
void LCD_RFpage_Draw(uint32_t Time, GPS_Position *GPS)
|
||||
{ LCD_RFpage_Draw();
|
||||
LCD_UpdateTime(Time, GPS, 1); LCD_UpdateBattery(1);
|
||||
LCD_UpdateRF(1); }
|
||||
|
||||
void LCD_RFpage_Update(uint32_t Time, GPS_Position *GPS, bool TimeChange, bool GPSchange)
|
||||
{ if(TimeChange) { LCD_UpdateTime(Time, GPS); LCD_UpdateBattery(); }
|
||||
if(TimeChange) LCD_UpdateRF(); }
|
||||
|
||||
|
||||
void LCD_BattPage_Draw(void)
|
||||
{ LCD_ClearDisplay(RGB565_WHITE); }
|
||||
|
||||
void LCD_BattPage_Draw(uint32_t Time, GPS_Position *GPS)
|
||||
{ LCD_BattPage_Draw();
|
||||
LCD_UpdateTime(Time, GPS, 1); LCD_UpdateBattery(1);
|
||||
}
|
||||
|
||||
void LCD_BattPage_Update(uint32_t Time, GPS_Position *GPS, bool TimeChange, bool GPSchange)
|
||||
{ if(TimeChange) { LCD_UpdateTime(Time, GPS); LCD_UpdateBattery(); }
|
||||
}
|
||||
|
||||
|
||||
void LCD_ParmPage_Draw(void)
|
||||
{ LCD_ClearDisplay(RGB565_WHITE); }
|
||||
|
||||
void LCD_ParmPage_Draw(uint32_t Time, GPS_Position *GPS)
|
||||
{ LCD_ParmPage_Draw();
|
||||
LCD_UpdateTime(Time, GPS, 1); LCD_UpdateBattery(1);
|
||||
LCD_UpdateParameters(1); }
|
||||
|
||||
void LCD_ParmPage_Update(uint32_t Time, GPS_Position *GPS, bool TimeChange, bool GPSchange)
|
||||
{ if(TimeChange) { LCD_UpdateTime(Time, GPS); LCD_UpdateBattery(); }
|
||||
LCD_UpdateParameters(); }
|
||||
|
||||
|
||||
void LCD_BaroPage_Draw(void)
|
||||
{ LCD_ClearDisplay(RGB565_WHITE); }
|
||||
|
||||
void LCD_BaroPage_Draw(uint32_t Time, GPS_Position *GPS)
|
||||
{ LCD_BaroPage_Draw();
|
||||
LCD_UpdateTime(Time, GPS, 1); LCD_UpdateBattery(1);
|
||||
}
|
||||
|
||||
void LCD_BaroPage_Update(uint32_t Time, GPS_Position *GPS, bool TimeChange, bool GPSchange)
|
||||
{ if(TimeChange) { LCD_UpdateTime(Time, GPS); LCD_UpdateBattery(); }
|
||||
}
|
||||
|
||||
|
||||
void LCD_SysPage_Draw(void)
|
||||
{ LCD_ClearDisplay(RGB565_WHITE); }
|
||||
|
||||
void LCD_SysPage_Draw(uint32_t Time, GPS_Position *GPS)
|
||||
{ LCD_SysPage_Draw();
|
||||
LCD_UpdateTime(Time, GPS, 1); LCD_UpdateBattery(1);
|
||||
LCD_UpdateSys(1); }
|
||||
|
||||
void LCD_SysPage_Update(uint32_t Time, GPS_Position *GPS, bool TimeChange, bool GPSchange)
|
||||
{ if(TimeChange) { LCD_UpdateTime(Time, GPS); LCD_UpdateBattery(); }
|
||||
if(TimeChange) LCD_UpdateSys(); }
|
||||
|
||||
|
||||
void LCD_LookPage_Draw(void)
|
||||
{ LCD_ClearDisplay(RGB565_WHITE); }
|
||||
|
||||
void LCD_LookPage_Draw(uint32_t Time, GPS_Position *GPS)
|
||||
{ LCD_LookPage_Draw();
|
||||
LCD_UpdateTime(Time, GPS, 1); LCD_UpdateBattery(1);
|
||||
LCD_UpdateLookList(1); }
|
||||
|
||||
void LCD_LookPage_Update(uint32_t Time, GPS_Position *GPS, bool TimeChange, bool GPSchange)
|
||||
{ if(TimeChange) { LCD_UpdateTime(Time, GPS); LCD_UpdateBattery(); }
|
||||
if(GPSchange) LCD_UpdateLookList(); }
|
||||
|
||||
|
||||
void LCD_RelayPage_Draw(void)
|
||||
{ LCD_ClearDisplay(RGB565_WHITE); }
|
||||
|
||||
void LCD_RelayPage_Draw(uint32_t Time, GPS_Position *GPS)
|
||||
{ LCD_RelayPage_Draw();
|
||||
LCD_UpdateTime(Time, GPS, 1); LCD_UpdateBattery(1);
|
||||
LCD_UpdateRelayList(1); }
|
||||
|
||||
void LCD_RelayPage_Update(uint32_t Time, GPS_Position *GPS, bool TimeChange, bool GPSchange)
|
||||
{ if(TimeChange) { LCD_UpdateTime(Time, GPS); LCD_UpdateBattery(); }
|
||||
if(GPSchange) LCD_UpdateRelayList(); }
|
||||
|
||||
// ------------------------------------------------------------------------------------------------------------------------
|
||||
|
||||
#endif
|
||||
|
||||
// ========================================================================================================================
|
||||
|
|
@ -0,0 +1,41 @@
|
|||
#if defined(WITH_ST7789) || defined(WITH_ILI9341)
|
||||
|
||||
#include "st7789.h"
|
||||
|
||||
void LCD_LogoPage_Draw(void);
|
||||
void LCD_LogoPage_Draw(uint32_t Time, GPS_Position *GPS);
|
||||
void LCD_LogoPage_Update(uint32_t Time, GPS_Position *GPS, bool TimeChange, bool GPSchange);
|
||||
|
||||
void LCD_GPSpage_Draw(void);
|
||||
void LCD_GPSpage_Draw(uint32_t Time, GPS_Position *GPS);
|
||||
void LCD_GPSpage_Update(uint32_t Time, GPS_Position *GPS, bool TimeChange, bool GPSchange);
|
||||
|
||||
void LCD_RFpage_Draw(void);
|
||||
void LCD_RFpage_Draw(uint32_t Time, GPS_Position *GPS);
|
||||
void LCD_RFpage_Update(uint32_t Time, GPS_Position *GPS, bool TimeChange, bool GPSchange);
|
||||
|
||||
void LCD_BattPage_Draw(void);
|
||||
void LCD_BattPage_Draw(uint32_t Time, GPS_Position *GPS);
|
||||
void LCD_BattPage_Update(uint32_t Time, GPS_Position *GPS, bool TimeChange, bool GPSchange);
|
||||
|
||||
void LCD_ParmPage_Draw(void);
|
||||
void LCD_ParmPage_Draw(uint32_t Time, GPS_Position *GPS);
|
||||
void LCD_ParmPage_Update(uint32_t Time, GPS_Position *GPS, bool TimeChange, bool GPSchange);
|
||||
|
||||
void LCD_BaroPage_Draw(void);
|
||||
void LCD_BaroPage_Draw(uint32_t Time, GPS_Position *GPS);
|
||||
void LCD_BaroPage_Update(uint32_t Time, GPS_Position *GPS, bool TimeChange, bool GPSchange);
|
||||
|
||||
void LCD_SysPage_Draw(void);
|
||||
void LCD_SysPage_Draw(uint32_t Time, GPS_Position *GPS);
|
||||
void LCD_SysPage_Update(uint32_t Time, GPS_Position *GPS, bool TimeChange, bool GPSchange);
|
||||
|
||||
void LCD_LookPage_Draw(void);
|
||||
void LCD_LookPage_Draw(uint32_t Time, GPS_Position *GPS);
|
||||
void LCD_LookPage_Update(uint32_t Time, GPS_Position *GPS, bool TimeChange, bool GPSchange);
|
||||
|
||||
void LCD_RelayPage_Draw(void);
|
||||
void LCD_RelayPage_Draw(uint32_t Time, GPS_Position *GPS);
|
||||
void LCD_RelayPage_Update(uint32_t Time, GPS_Position *GPS, bool TimeChange, bool GPSchange);
|
||||
|
||||
#endif
|
||||
|
|
@ -0,0 +1,635 @@
|
|||
#include <stdio.h>
|
||||
|
||||
#include <sys/types.h>
|
||||
#include <dirent.h>
|
||||
#include <sys/stat.h>
|
||||
#include <unistd.h>
|
||||
|
||||
// #include "esp_system.h"
|
||||
// #include "esp_sleep.h"
|
||||
|
||||
#include "hal.h"
|
||||
|
||||
#include "sens.h"
|
||||
#include "rf.h"
|
||||
#include "ctrl.h"
|
||||
#include "proc.h"
|
||||
// #include "log.h"
|
||||
|
||||
#include "gps.h"
|
||||
// #include "ubx.h"
|
||||
// #include "timesync.h"
|
||||
#include "format.h"
|
||||
|
||||
static char Line[128];
|
||||
|
||||
// ========================================================================================================================
|
||||
|
||||
#ifdef WITH_OLED
|
||||
|
||||
#include "disp_oled.h"
|
||||
|
||||
int OLED_DisplayStatus(uint32_t Time, uint8_t LineIdx)
|
||||
{ Format_String(Line , "OGN Tx/Rx ");
|
||||
Format_HHMMSS(Line+10, Time);
|
||||
OLED_PutLine(LineIdx++, Line);
|
||||
Parameters.Print(Line);
|
||||
OLED_PutLine(LineIdx++, Line);
|
||||
return 0; }
|
||||
|
||||
int OLED_DisplayPosition(GPS_Position *GPS=0, uint8_t LineIdx=2)
|
||||
{ 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(LineIdx , Line);
|
||||
Format_SignDec(Line, GPS->Longitude/60, 7, 4);
|
||||
Format_SignDec(Line+10, GPS->ClimbRate, 4, 1);
|
||||
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(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(LineIdx+3, Line);
|
||||
}
|
||||
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]='.';
|
||||
Format_UnsDec (Line+ 6, (uint16_t)GPS->Year , 2, 0); Line[8]=' '; Line[9]=' '; }
|
||||
else Format_String(Line, " ");
|
||||
if(GPS && GPS->isTimeValid())
|
||||
{ Format_UnsDec (Line+10, (uint16_t)GPS->Hour, 2, 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(LineIdx+4, Line);
|
||||
Line[0]=0;
|
||||
if(GPS && GPS->hasBaro)
|
||||
{ Format_String(Line , "0000.0hPa 00000m");
|
||||
Format_UnsDec(Line , GPS->Pressure/40, 5, 1);
|
||||
Format_UnsDec(Line+10, GPS->StdAltitude/10, 5, 0); }
|
||||
OLED_PutLine(LineIdx+5, Line);
|
||||
return 0; }
|
||||
#endif
|
||||
|
||||
// ========================================================================================================================
|
||||
|
||||
#ifdef WITH_U8G2_OLED
|
||||
|
||||
void OLED_DrawLogo(u8g2_t *OLED) // draw logo and hardware options in software
|
||||
{
|
||||
u8g2_DrawCircle(OLED, 96, 32, 30, U8G2_DRAW_ALL);
|
||||
u8g2_DrawCircle(OLED, 96, 32, 34, U8G2_DRAW_UPPER_RIGHT);
|
||||
u8g2_DrawCircle(OLED, 96, 32, 38, U8G2_DRAW_UPPER_RIGHT);
|
||||
// u8g2_SetFont(OLED, u8g2_font_open_iconic_all_4x_t);
|
||||
// u8g2_DrawGlyph(OLED, 64, 32, 0xF0);
|
||||
u8g2_SetFont(OLED, u8g2_font_ncenB14_tr);
|
||||
u8g2_DrawStr(OLED, 74, 31, "OGN");
|
||||
u8g2_SetFont(OLED, u8g2_font_8x13_tr);
|
||||
u8g2_DrawStr(OLED, 69, 43, "Tracker");
|
||||
|
||||
#ifdef WITH_FollowMe
|
||||
u8g2_DrawStr(OLED, 0, 16 ,"FollowMe");
|
||||
#endif
|
||||
#ifdef WITH_TTGO
|
||||
u8g2_DrawStr(OLED, 0, 16 ,"TTGO");
|
||||
#endif
|
||||
#ifdef WITH_HELTEC
|
||||
u8g2_DrawStr(OLED, 0, 16 ,"HELTEC");
|
||||
#endif
|
||||
#if defined(WITH_TBEAM) || defined(WITH_TBEAM_V10)
|
||||
u8g2_DrawStr(OLED, 0, 16 ,"T-BEAM");
|
||||
#endif
|
||||
|
||||
#ifdef WITH_GPS_MTK
|
||||
u8g2_DrawStr(OLED, 0, 28 ,"MTK GPS");
|
||||
#endif
|
||||
#ifdef WITH_GPS_UBX
|
||||
u8g2_DrawStr(OLED, 0, 28 ,"UBX GPS");
|
||||
#endif
|
||||
#ifdef WITH_GPS_SRF
|
||||
u8g2_DrawStr(OLED, 0, 28 ,"SRF GPS");
|
||||
#endif
|
||||
|
||||
#ifdef WITH_RFM95
|
||||
u8g2_DrawStr(OLED, 0, 40 ,"RFM95");
|
||||
#endif
|
||||
#ifdef WITH_RFM69
|
||||
u8g2_DrawStr(OLED, 0, 40 ,"RFM69");
|
||||
#endif
|
||||
|
||||
#ifdef WITH_BMP180
|
||||
u8g2_DrawStr(OLED, 0, 52 ,"BMP180");
|
||||
#endif
|
||||
#ifdef WITH_BMP280
|
||||
u8g2_DrawStr(OLED, 0, 52 ,"BMP280");
|
||||
#endif
|
||||
#ifdef WITH_BME280
|
||||
u8g2_DrawStr(OLED, 0, 52 ,"BME280");
|
||||
#endif
|
||||
|
||||
#ifdef WITH_BT_SPP
|
||||
u8g2_DrawStr(OLED, 0, 64 ,"BT SPP");
|
||||
#endif
|
||||
}
|
||||
|
||||
void OLED_PutLine(u8g2_t *OLED, uint8_t LineIdx, const char *Line)
|
||||
{ if(Line==0) return;
|
||||
#ifdef DEBUG_PRINT
|
||||
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
Format_String(CONS_UART_Write, "OLED_PutLine( ,");
|
||||
Format_UnsDec(CONS_UART_Write, (uint16_t)LineIdx);
|
||||
CONS_UART_Write(',');
|
||||
Format_String(CONS_UART_Write, Line);
|
||||
Format_String(CONS_UART_Write, ")\n");
|
||||
xSemaphoreGive(CONS_Mutex);
|
||||
#endif
|
||||
// u8g2_SetFont(OLED, u8g2_font_5x8_tr);
|
||||
u8g2_SetFont(OLED, u8g2_font_amstrad_cpc_extended_8r);
|
||||
u8g2_DrawStr(OLED, 0, (LineIdx+1)*8, Line);
|
||||
}
|
||||
|
||||
void OLED_DrawStatus(u8g2_t *OLED, uint32_t Time, uint8_t LineIdx=0)
|
||||
{ Format_String(Line , "OGN Tx/Rx ");
|
||||
Format_HHMMSS(Line+10, Time); Line[16]=0;
|
||||
OLED_PutLine(OLED, LineIdx++, Line);
|
||||
Parameters.Print(Line); Line[16]=0;
|
||||
OLED_PutLine(OLED, LineIdx++, Line); }
|
||||
|
||||
void OLED_DrawPosition(u8g2_t *OLED, GPS_Position *GPS=0, uint8_t LineIdx=2)
|
||||
{ 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(OLED, LineIdx , Line);
|
||||
Format_SignDec(Line, GPS->Longitude/60, 7, 4);
|
||||
Format_SignDec(Line+10, GPS->ClimbRate, 4, 1);
|
||||
OLED_PutLine(OLED, 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(OLED, 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(OLED, LineIdx+3, Line);
|
||||
}
|
||||
// else { OLED_PutLine(OLED, LineIdx, 0); OLED_PutLine(OLED, 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]='.';
|
||||
Format_UnsDec (Line+ 6, (uint16_t)GPS->Year , 2, 0); Line[8]=' '; Line[9]=' '; }
|
||||
else Format_String(Line, " ");
|
||||
if(GPS && GPS->isTimeValid())
|
||||
{ Format_UnsDec (Line+10, (uint16_t)GPS->Hour, 2, 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(OLED, LineIdx+4, Line);
|
||||
Line[0]=0;
|
||||
if(GPS && GPS->hasBaro)
|
||||
{ Format_String(Line , "0000.0hPa 00000m");
|
||||
Format_UnsDec(Line , GPS->Pressure/40, 5, 1);
|
||||
Format_UnsDec(Line+10, GPS->StdAltitude/10, 5, 0); }
|
||||
OLED_PutLine(OLED, LineIdx+5, Line);
|
||||
}
|
||||
|
||||
void OLED_DrawGPS(u8g2_t *OLED, GPS_Position *GPS) // GPS time, position, altitude
|
||||
{ // u8g2_SetFont(OLED, u8g2_font_ncenB14_tr);
|
||||
u8g2_SetFont(OLED, u8g2_font_7x13_tf); // 5 lines, 12 pixels/line
|
||||
uint8_t Len=0;
|
||||
/*
|
||||
Len+=Format_String(Line+Len, "GPS ");
|
||||
if(GPS && GPS->isValid())
|
||||
{ Line[Len++]='0'+GPS->FixMode; Line[Len++]='D'; Line[Len++]='/';
|
||||
Len+=Format_UnsDec(Line+Len, GPS->Satellites, 1);
|
||||
Len+=Format_String(Line+Len, "sat DOP");
|
||||
Len+=Format_UnsDec(Line+Len, (uint16_t)GPS->HDOP, 2, 1); }
|
||||
else
|
||||
{ Len+=Format_String(Line+Len, "(no lock)"); }
|
||||
Line[Len]=0;
|
||||
u8g2_DrawStr(OLED, 0, 12, Line);
|
||||
*/
|
||||
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]='.';
|
||||
Format_UnsDec (Line+ 6, (uint16_t)GPS->Year , 2, 0); Line[8]=' ';
|
||||
} else Format_String(Line, " . . ");
|
||||
if(GPS && GPS->isTimeValid())
|
||||
{ Format_UnsDec (Line+ 9, (uint16_t)GPS->Hour, 2, 0); Line[11]=':';
|
||||
Format_UnsDec (Line+12, (uint16_t)GPS->Min, 2, 0); Line[14]=':';
|
||||
Format_UnsDec (Line+15, (uint16_t)GPS->Sec, 2, 0);
|
||||
} else Format_String(Line+9, " : : ");
|
||||
Line[17]=0;
|
||||
u8g2_DrawStr(OLED, 0, 24, Line);
|
||||
|
||||
Len=0;
|
||||
Len+=Format_String(Line+Len, "Lat: ");
|
||||
if(GPS && GPS->isValid())
|
||||
{ Len+=Format_SignDec(Line+Len, GPS->Latitude /6, 7, 5);
|
||||
Line[Len++]=0xB0; }
|
||||
else Len+=Format_String(Line+Len, "---.-----");
|
||||
Line[Len]=0;
|
||||
u8g2_DrawStr(OLED, 0, 36, Line);
|
||||
Len=0;
|
||||
Len+=Format_String(Line+Len, "Lon: ");
|
||||
if(GPS && GPS->isValid())
|
||||
{ Len+=Format_SignDec(Line+Len, GPS->Longitude /6, 8, 5);
|
||||
Line[Len++]=0xB0; }
|
||||
else Len+=Format_String(Line+Len, "----.-----");
|
||||
Line[Len]=0;
|
||||
u8g2_DrawStr(OLED, 0, 48, Line);
|
||||
Len=0;
|
||||
Len+=Format_String(Line+Len, "Alt: ");
|
||||
if(GPS && GPS->isValid())
|
||||
{ Len+=Format_SignDec(Line+Len, GPS->Altitude, 4, 1);
|
||||
Line[Len++]='m'; }
|
||||
else Len+=Format_String(Line+Len, "-----.-");
|
||||
Line[Len]=0;
|
||||
u8g2_DrawStr(OLED, 0, 60, Line);
|
||||
}
|
||||
|
||||
void OLED_DrawRF(u8g2_t *OLED) // RF
|
||||
{ u8g2_SetFont(OLED, u8g2_font_7x13_tf); // 5 lines. 12 pixels/line
|
||||
uint8_t Len=0;
|
||||
#ifdef WITH_RFM69
|
||||
Len+=Format_String(Line+Len, "RFM69"); // Type of RF chip used
|
||||
if(Parameters.isTxTypeHW()) Line[Len++]='H';
|
||||
Line[Len++]='W';
|
||||
#endif
|
||||
#ifdef WITH_RFM95
|
||||
Len+=Format_String(Line+Len, "RFM95");
|
||||
#endif
|
||||
#ifdef WITH_SX1272
|
||||
Len+=Format_String(Line+Len, "SX1272");
|
||||
#endif
|
||||
Line[Len++]=':';
|
||||
Len+=Format_SignDec(Line+Len, (int16_t)Parameters.getTxPower()); // Tx power
|
||||
Len+=Format_String(Line+Len, "dBm");
|
||||
Line[Len++]=' ';
|
||||
Len+=Format_SignDec(Line+Len, (int32_t)Parameters.RFchipFreqCorr, 2, 1); // frequency correction
|
||||
Len+=Format_String(Line+Len, "ppm");
|
||||
Line[Len]=0;
|
||||
u8g2_DrawStr(OLED, 0, 24, Line);
|
||||
Len=0;
|
||||
Len+=Format_String(Line+Len, "Rx:"); //
|
||||
Len+=Format_SignDec(Line+Len, -5*TRX.averRSSI, 2, 1); // noise level seen by the receiver
|
||||
Len+=Format_String(Line+Len, "dBm ");
|
||||
Len+=Format_UnsDec(Line+Len, RX_OGN_Count64); // received packet/min
|
||||
Len+=Format_String(Line+Len, "/min");
|
||||
Line[Len]=0;
|
||||
u8g2_DrawStr(OLED, 0, 36, Line);
|
||||
Len=0;
|
||||
Len+=Format_SignDec(Line+Len, (int16_t)TRX.chipTemp); // RF chip internal temperature (not calibrated)
|
||||
// Line[Len++]=0xB0;
|
||||
// Line[Len++]='C';
|
||||
Len+=Format_String(Line+Len, "\260C RxFIFO:");
|
||||
Len+=Format_UnsDec(Line+Len, RF_RxFIFO.Full()); // how many packets wait in the RX queue
|
||||
Line[Len]=0;
|
||||
u8g2_DrawStr(OLED, 0, 48, Line);
|
||||
// u8g2_DrawStr(OLED, 0, 48, RF_FreqPlan.getPlanName());
|
||||
Len=0;
|
||||
Len+=Format_String(Line+Len, RF_FreqPlan.getPlanName()); // name of the frequency plan
|
||||
Line[Len++]=' ';
|
||||
Len+=Format_UnsDec(Line+Len, (uint16_t)(RF_FreqPlan.getCenterFreq()/100000), 3, 1); // center frequency
|
||||
Len+=Format_String(Line+Len, "MHz");
|
||||
Line[Len]=0;
|
||||
u8g2_DrawStr(OLED, 0, 60, Line);
|
||||
}
|
||||
|
||||
void OLED_DrawRelay(u8g2_t *OLED, GPS_Position *GPS)
|
||||
{ u8g2_SetFont(OLED, u8g2_font_amstrad_cpc_extended_8r);
|
||||
uint8_t Len=Format_String(Line, "Relay queue :");
|
||||
if(GPS && GPS->Sec>=0) Len+=Format_UnsDec(Line+Len, (uint16_t)(GPS->Sec), 2);
|
||||
Line[Len]=0;
|
||||
u8g2_DrawStr(OLED, 0, 24, Line);
|
||||
uint8_t LineIdx=1;
|
||||
for( uint8_t Idx=0; Idx<RelayQueueSize; Idx++)
|
||||
{ OGN_RxPacket<OGN_Packet> *Packet = RelayQueue.Packet+Idx; if(Packet->Rank==0) continue;
|
||||
uint8_t Len=0;
|
||||
Line[Len++]='0'+Packet->Packet.Header.AddrType;
|
||||
Line[Len++]=':';
|
||||
Len+=Format_Hex(Line+Len, Packet->Packet.Header.Address, 6);
|
||||
Line[Len++]=' ';
|
||||
Len+=Format_Hex(Line+Len, Packet->Rank);
|
||||
Line[Len++]=' ';
|
||||
Line[Len++]=':';
|
||||
Len+=Format_UnsDec(Line+Len, Packet->Packet.Position.Time, 2);
|
||||
Line[Len]=0;
|
||||
u8g2_DrawStr(OLED, 0, (LineIdx+3)*8, Line);
|
||||
LineIdx++; if(LineIdx>=8) break;
|
||||
}
|
||||
}
|
||||
|
||||
void OLED_DrawLookout(u8g2_t *OLED, GPS_Position *GPS)
|
||||
{ u8g2_SetFont(OLED, u8g2_font_amstrad_cpc_extended_8r);
|
||||
uint8_t Len=Format_String(Line, "Look ");
|
||||
if(Look.WarnLevel)
|
||||
{ const LookOut_Target *Tgt = Look.Target+Look.WorstTgtIdx;
|
||||
Len+=Format_Hex(Line+Len, Tgt->ID, 7);
|
||||
Line[Len++]='/';
|
||||
Line[Len++]='0'+Tgt->WarnLevel;
|
||||
Line[Len++]=' ';
|
||||
Len+=Format_UnsDec(Line+Len, Tgt->TimeMargin>>1);
|
||||
Line[Len++]='s';
|
||||
}
|
||||
Line[Len]=0;
|
||||
u8g2_DrawStr(OLED, 0, 24, Line);
|
||||
uint8_t LineIdx=1;
|
||||
for( uint8_t Idx=0; Idx<Look.MaxTargets; Idx++)
|
||||
{ const LookOut_Target *Tgt = Look.Target+Idx; if(!Tgt->Alloc) continue;
|
||||
uint8_t Len=0;
|
||||
Len+=Format_Hex(Line+Len, Tgt->ID, 7);
|
||||
Line[Len++]=' ';
|
||||
if(Tgt->DistMargin)
|
||||
{ Len+=Format_UnsDec(Line+Len, Tgt->HorDist>>1);
|
||||
Line[Len++]='m'; }
|
||||
else
|
||||
{ Len+=Format_UnsDec(Line+Len, Tgt->TimeMargin>>1);
|
||||
Line[Len++]='s';
|
||||
Line[Len++]=' ';
|
||||
Len+=Format_UnsDec(Line+Len, Tgt->MissDist>>1);
|
||||
Line[Len++]='m'; }
|
||||
Line[Len]=0;
|
||||
u8g2_DrawStr(OLED, 0, (LineIdx+3)*8, Line);
|
||||
LineIdx++; if(LineIdx>=8) break;
|
||||
}
|
||||
}
|
||||
|
||||
void OLED_DrawTrafWarn(u8g2_t *OLED, GPS_Position *GPS)
|
||||
{ u8g2_SetFont(OLED, u8g2_font_9x15_tr);
|
||||
if(Look.WarnLevel==0) return;
|
||||
const LookOut_Target *Tgt = Look.Target+Look.WorstTgtIdx;
|
||||
uint8_t Len=0;
|
||||
Len+=Format_Hex(Line+Len, Tgt->ID, 7);
|
||||
Line[Len++]='/';
|
||||
Line[Len++]='0'+Tgt->WarnLevel;
|
||||
Line[Len]=0;
|
||||
u8g2_DrawStr(OLED, 0, 30, Line);
|
||||
Len=0;
|
||||
Len+=Format_UnsDec(Line+Len, Tgt->TimeMargin*5, 2, 1);
|
||||
Line[Len++]='s';
|
||||
Line[Len++]=' ';
|
||||
Len+=Format_UnsDec(Line+Len, Tgt->MissDist*5, 2, 1);
|
||||
Line[Len++]='m';
|
||||
Line[Len]=0;
|
||||
u8g2_DrawStr(OLED, 0, 45, Line);
|
||||
Len=0;
|
||||
Len+=Format_UnsDec(Line+Len, Tgt->getRelHorSpeed()*5, 2, 1);
|
||||
Line[Len++]='m';
|
||||
Line[Len++]='/';
|
||||
Line[Len++]='s';
|
||||
Line[Len++]=' ';
|
||||
Len+=Format_UnsDec(Line+Len, Tgt->HorDist*5, 2, 1);
|
||||
Line[Len++]='m';
|
||||
Line[Len]=0;
|
||||
u8g2_DrawStr(OLED, 0, 60, Line);
|
||||
}
|
||||
|
||||
void OLED_DrawBaro(u8g2_t *OLED, GPS_Position *GPS)
|
||||
{ u8g2_SetFont(OLED, u8g2_font_7x13_tf); // 5 lines, 12 pixels/line
|
||||
uint8_t Len=0;
|
||||
#ifdef WITH_BMP180
|
||||
Len+=Format_String(Line+Len, "BMP180 ");
|
||||
#endif
|
||||
#ifdef WITH_BMP280
|
||||
Len+=Format_String(Line+Len, "BMP280 ");
|
||||
#endif
|
||||
#ifdef WITH_BME280
|
||||
Len+=Format_String(Line+Len, "BME280 ");
|
||||
#endif
|
||||
#ifdef WITH_MS5607
|
||||
Len+=Format_String(Line+Len, "MS5607 ");
|
||||
#endif
|
||||
if(GPS && GPS->hasBaro)
|
||||
{ Len+=Format_UnsDec(Line+Len, GPS->Pressure/4, 5, 2);
|
||||
Len+=Format_String(Line+Len, "Pa "); }
|
||||
else Len+=Format_String(Line+Len, "----.--Pa ");
|
||||
Line[Len]=0;
|
||||
u8g2_DrawStr(OLED, 0, 24, Line);
|
||||
Len=0;
|
||||
if(GPS && GPS->hasBaro)
|
||||
{ Len+=Format_SignDec(Line+Len, GPS->StdAltitude, 5, 1);
|
||||
Len+=Format_String(Line+Len, "m ");
|
||||
Len+=Format_SignDec(Line+Len, GPS->ClimbRate, 2, 1);
|
||||
Len+=Format_String(Line+Len, "m/s "); }
|
||||
else Len+=Format_String(Line+Len, "-----.-m --.-m/s ");
|
||||
Line[Len]=0;
|
||||
u8g2_DrawStr(OLED, 0, 36, Line);
|
||||
Len=0;
|
||||
if(GPS && GPS->hasBaro)
|
||||
{ Len+=Format_SignDec(Line+Len, GPS->Temperature, 2, 1);
|
||||
Line[Len++]=0xB0;
|
||||
Line[Len++]='C';
|
||||
Line[Len++]=' ';
|
||||
Len+=Format_SignDec(Line+Len, GPS->Humidity, 2, 1);
|
||||
Line[Len++]='%'; }
|
||||
else Len+=Format_String(Line+Len, "---.- C --.-% ");
|
||||
Line[Len]=0;
|
||||
u8g2_DrawStr(OLED, 0, 48, Line);
|
||||
}
|
||||
|
||||
static uint8_t BattCapacity(uint16_t mVolt)
|
||||
{ if(mVolt>=4100) return 100;
|
||||
if(mVolt<=3600) return 0;
|
||||
return (mVolt-3600+2)/5; }
|
||||
|
||||
void OLED_DrawBattery(u8g2_t *OLED)
|
||||
{ uint8_t Cap=BattCapacity(BatteryVoltage>>8);
|
||||
// u8g2_SetFont(OLED, u8g2_font_battery19_tn);
|
||||
// u8g2_DrawGlyph(OLED, 120, 60, '0'+(Cap+10)/20);
|
||||
|
||||
// u8g2_SetFont(OLED, u8g2_font_6x10_tr);
|
||||
// u8g2_DrawStr(OLED, 0, 24, Line);
|
||||
|
||||
// u8g2_DrawStr(OLED, 0, 24, "Battery");
|
||||
|
||||
u8g2_SetFont(OLED, u8g2_font_9x15_tr);
|
||||
|
||||
strcpy(Line, " %");
|
||||
if(Cap>=100) Format_UnsDec(Line, Cap, 3);
|
||||
else if(Cap>=10) Format_UnsDec(Line+1, Cap, 2);
|
||||
else Line[2]='0'+Cap;
|
||||
u8g2_DrawStr (OLED, 16, 32, Line);
|
||||
u8g2_DrawFrame(OLED, 12, 20, 42, 14);
|
||||
u8g2_DrawBox (OLED, 8, 23, 4, 8);
|
||||
|
||||
strcpy(Line, " . V");
|
||||
Format_UnsDec(Line, BatteryVoltage>>8, 4, 3);
|
||||
u8g2_DrawStr(OLED, 0, 48, Line);
|
||||
|
||||
strcpy(Line, " . mV/min ");
|
||||
Format_SignDec(Line, (600*BatteryVoltageRate+128)>>8, 3, 1);
|
||||
u8g2_DrawStr(OLED, 0, 60, Line);
|
||||
|
||||
#ifdef WITH_BQ
|
||||
// uint8_t Status = BQ.readStatus();
|
||||
// uint8_t State = (Status>>4)&0x03;
|
||||
// const char *StateName[4] = { "Not charging", "Pre-charge", "Fast charge", "Full" } ;
|
||||
// u8g2_SetFont(OLED, u8g2_font_amstrad_cpc_extended_8r);
|
||||
// u8g2_SetFont(OLED, u8g2_font_6x10_tr);
|
||||
// u8g2_DrawStr(OLED, 0, 50, StateName[State]);
|
||||
// u8g2_DrawStr(OLED, 0, 60, Status&0x04?"Power-is-Good":"Power-is-not-Good");
|
||||
|
||||
/* print BQ registers for debug
|
||||
u8g2_SetFont(OLED, u8g2_font_6x10_tr);
|
||||
for(uint8_t Reg=0; Reg<=10; Reg++)
|
||||
{ uint8_t Val = BQ.readReg(Reg);
|
||||
Format_Hex(Line+3*Reg, Val); Line[3*Reg+2]=' '; }
|
||||
Line[33]=0;
|
||||
u8g2_DrawStr(OLED, 0, 60, Line+15);
|
||||
Line[15]=0; u8g2_DrawStr(OLED, 0, 50, Line);
|
||||
*/
|
||||
#endif
|
||||
}
|
||||
|
||||
void OLED_DrawStatusBar(u8g2_t *OLED, GPS_Position *GPS)
|
||||
{ static bool Odd=0;
|
||||
uint8_t Cap=BattCapacity(BatteryVoltage>>8);
|
||||
uint8_t BattLev = (Cap+10)/20;
|
||||
uint8_t Charging = 0;
|
||||
#ifdef WITH_BQ
|
||||
uint8_t Status = BQ.readStatus();
|
||||
Charging = (Status>>4)&0x03;
|
||||
static uint8_t DispLev = 0;
|
||||
if(Charging==1 || Charging==2) { DispLev++; if(DispLev>5) DispLev = BattLev?BattLev-1:0; }
|
||||
else { DispLev = BattLev; }
|
||||
#else
|
||||
uint8_t &DispLev = BattLev;
|
||||
#endif
|
||||
if(BattLev==0 && !Charging && Odd) // when battery is empty, then flash it at 0.5Hz
|
||||
{ }
|
||||
else
|
||||
{ u8g2_SetFont(OLED, u8g2_font_battery19_tn);
|
||||
u8g2_SetFontDirection(OLED, 3);
|
||||
u8g2_DrawGlyph(OLED, 24, 10, '0'+DispLev);
|
||||
u8g2_SetFontDirection(OLED, 0); }
|
||||
Odd=!Odd;
|
||||
|
||||
// u8g2_SetFont(OLED, u8g2_font_5x7_tr);
|
||||
// u8g2_SetFont(OLED, u8g2_font_5x8_tr);
|
||||
static uint8_t Sec=0;
|
||||
u8g2_SetFont(OLED, u8g2_font_6x12_tr);
|
||||
// strcpy(Line, "[ %] --sat --:--");
|
||||
strcpy(Line, "--sat --:--Z");
|
||||
if(GPS && GPS->isTimeValid())
|
||||
{ Format_UnsDec (Line+6, (uint16_t)GPS->Hour, 2, 0); Line[8]=':';
|
||||
Format_UnsDec (Line+9, (uint16_t)GPS->Min, 2, 0);
|
||||
} else Format_String(Line+6, "--:--");
|
||||
if(GPS)
|
||||
{ if(Sec)
|
||||
{ Format_UnsDec(Line, (uint16_t)GPS->Satellites, 2); memcpy(Line+2, "sat", 3); }
|
||||
else
|
||||
{ Format_UnsDec(Line, (uint16_t)(GPS_SatSNR+2)/4, 2); memcpy(Line+2, "dB ", 3);}
|
||||
}
|
||||
else Format_String(Line, "--sat");
|
||||
u8g2_DrawStr(OLED, 40, 10, Line);
|
||||
Sec++; if(Sec>=3) Sec=0; }
|
||||
|
||||
void OLED_DrawSystem(u8g2_t *OLED)
|
||||
{
|
||||
u8g2_SetFont(OLED, u8g2_font_7x13_tf); // 5 lines, 12 pixels/line
|
||||
uint8_t Len=0;
|
||||
Len+=Format_String(Line+Len, "GPS ");
|
||||
#ifdef WITH_GPS_UBX
|
||||
Len+=Format_String(Line+Len, "UBX ");
|
||||
#endif
|
||||
#ifdef WITH_GPS_MTK
|
||||
Len+=Format_String(Line+Len, "MTK ");
|
||||
#endif
|
||||
#ifdef WITH_GPS_SRF
|
||||
Len+=Format_String(Line+Len, "SRF ");
|
||||
#endif
|
||||
Len+=Format_UnsDec(Line+Len, GPS_getBaudRate(), 1);
|
||||
Len+=Format_String(Line+Len, "bps");
|
||||
Line[Len]=0;
|
||||
u8g2_DrawStr(OLED, 0, 24, Line);
|
||||
|
||||
Len=0;
|
||||
#ifdef WITH_RFM69
|
||||
Len+=Format_String(Line+Len, "RFM69 v"); // Type of RF chip used
|
||||
if(Parameters.isTxTypeHW()) Line[Len++]='H';
|
||||
Line[Len++]='W';
|
||||
#endif
|
||||
#ifdef WITH_RFM95
|
||||
Len+=Format_String(Line+Len, "RFM95 v");
|
||||
#endif
|
||||
#ifdef WITH_SX1272
|
||||
Len+=Format_String(Line+Len, "SX1272 v");
|
||||
#endif
|
||||
Len+=Format_Hex(Line+Len, TRX.chipVer);
|
||||
Line[Len++]=' ';
|
||||
Len+=Format_SignDec(Line+Len, (int16_t)TRX.chipTemp);
|
||||
Line[Len++]=0xB0;
|
||||
Line[Len++]='C';
|
||||
Line[Len]=0;
|
||||
u8g2_DrawStr(OLED, 0, 36, Line);
|
||||
|
||||
Len=0;
|
||||
#ifdef WITH_BMP180
|
||||
Len+=Format_String(Line+Len, "BMP180 0x");
|
||||
Len+=Format_Hex(Line+Len, Baro.ADDR);
|
||||
#endif
|
||||
#ifdef WITH_BMP280
|
||||
Len+=Format_String(Line+Len, "BMP280 0x");
|
||||
Len+=Format_Hex(Line+Len, Baro.ADDR);
|
||||
#endif
|
||||
#ifdef WITH_BME280
|
||||
Len+=Format_String(Line+Len, "BME280 0x");
|
||||
Len+=Format_Hex(Line+Len, Baro.ADDR);
|
||||
#endif
|
||||
#ifdef WITH_MS5607
|
||||
Len+=Format_String(Line+Len, "MS5607 0x");
|
||||
Len+=Format_Hex(Line+Len, Baro.ADDR);
|
||||
#endif
|
||||
Line[Len]=0;
|
||||
u8g2_DrawStr(OLED, 0, 48, Line);
|
||||
|
||||
#ifdef WITH_SPIFFS
|
||||
Len=0;
|
||||
Len+=Format_String(Line+Len, "SPIFFS ");
|
||||
size_t Total, Used;
|
||||
if(SPIFFS_Info(Total, Used)==0) // get the SPIFFS usage summary
|
||||
{ Len+=Format_UnsDec(Line+Len, (Total-Used)/1024);
|
||||
Len+=Format_String(Line+Len, "/");
|
||||
Len+=Format_UnsDec(Line+Len, Total/1024);
|
||||
Len+=Format_String(Line+Len, "kB"); }
|
||||
Line[Len]=0;
|
||||
u8g2_DrawStr(OLED, 0, 60, Line);
|
||||
#endif
|
||||
/*
|
||||
#ifdef WITH_SD
|
||||
Len=0;
|
||||
Len+=Format_String(Line+Len, "SD ");
|
||||
if(SD_isMounted())
|
||||
{ Len+=Format_UnsDec(Line+Len, (uint32_t)SD_getSectors());
|
||||
Line[Len++]='x';
|
||||
Len+=Format_UnsDec(Line+Len, (uint32_t)SD_getSectorSize()*5/512, 2, 1);
|
||||
Len+=Format_String(Line+Len, "KB"); }
|
||||
else
|
||||
{ Len+=Format_String(Line+Len, "none"); }
|
||||
Line[Len]=0;
|
||||
u8g2_DrawStr(OLED, 0, 60, Line);
|
||||
#endif
|
||||
*/
|
||||
}
|
||||
|
||||
void OLED_DrawID(u8g2_t *OLED)
|
||||
{ u8g2_SetFont(OLED, u8g2_font_9x15_tr);
|
||||
strcpy(Line, "ID "); Parameters.Print(Line+3); Line[13]=0;
|
||||
u8g2_DrawStr(OLED, 0, 28, Line);
|
||||
// u8g2_SetFont(OLED, u8g2_font_10x20_tr);
|
||||
#ifdef WITH_FollowMe
|
||||
u8g2_SetFont(OLED, u8g2_font_7x13_tf);
|
||||
u8g2_DrawStr(OLED, 15, 44, "FollowMe868");
|
||||
u8g2_DrawStr(OLED, 20, 56, "by AVIONIX");
|
||||
#endif
|
||||
u8g2_SetFont(OLED, u8g2_font_5x8_tr);
|
||||
u8g2_DrawStr(OLED, 96, 62, "v0.0.0");
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
// ========================================================================================================================
|
||||
|
|
@ -0,0 +1,25 @@
|
|||
|
||||
#ifdef WITH_OLED
|
||||
|
||||
int OLED_DisplayStatus(uint32_t Time, uint8_t LineIdx=0);
|
||||
int OLED_DisplayPosition(GPS_Position *GPS=0, uint8_t LineIdx=2);
|
||||
|
||||
#endif
|
||||
|
||||
#ifdef WITH_U8G2_OLED
|
||||
|
||||
void OLED_DrawLogo(u8g2_t *OLED);
|
||||
void OLED_DrawStatus(u8g2_t *OLED, uint32_t Time, uint8_t LineIdx=0);
|
||||
void OLED_DrawPosition(u8g2_t *OLED, GPS_Position *GPS=0, uint8_t LineIdx=2);
|
||||
void OLED_DrawGPS(u8g2_t *OLED, GPS_Position *GPS=0);
|
||||
void OLED_DrawRF(u8g2_t *OLED);
|
||||
void OLED_DrawRelay(u8g2_t *OLED, GPS_Position *GPS=0);
|
||||
void OLED_DrawLookout(u8g2_t *OLED, GPS_Position *GPS=0);
|
||||
void OLED_DrawTrafWarn(u8g2_t *OLED, GPS_Position *GPS=0);
|
||||
void OLED_DrawBaro(u8g2_t *OLED, GPS_Position *GPS=0);
|
||||
void OLED_DrawBattery(u8g2_t *OLED);
|
||||
void OLED_DrawStatusBar(u8g2_t *OLED, GPS_Position *GPS);
|
||||
void OLED_DrawSystem(u8g2_t *OLED);
|
||||
void OLED_DrawID(u8g2_t *OLED);
|
||||
|
||||
#endif
|
||||
|
|
@ -69,6 +69,22 @@ void Format_Hex( void (*Output)(char), uint32_t Word )
|
|||
{ Format_Hex(Output, (uint8_t)(Word>>24)); Format_Hex(Output, (uint8_t)(Word>>16));
|
||||
Format_Hex(Output, (uint8_t)(Word>>8)); Format_Hex(Output, (uint8_t)Word); }
|
||||
|
||||
void Format_MAC( void (*Output)(char), uint8_t *MAC, uint8_t Len)
|
||||
{ for(uint8_t Idx=0; Idx<Len; Idx++)
|
||||
{ if(Idx) (*Output)(':');
|
||||
Format_Hex(Output, MAC[Idx]); }
|
||||
}
|
||||
|
||||
uint8_t Format_HHcMMcSS(char *Out, uint32_t Time)
|
||||
{ uint32_t DayTime=Time%86400;
|
||||
uint32_t Hour=DayTime/3600; DayTime-=Hour*3600;
|
||||
uint32_t Min=DayTime/60; DayTime-=Min*60;
|
||||
uint32_t Sec=DayTime;
|
||||
uint32_t HHMMSS = 1000000*Hour + 1000*Min + Sec;
|
||||
uint8_t Len=Format_UnsDec(Out, HHMMSS, 8);
|
||||
Out[2]=':'; Out[5]=':';
|
||||
return Len; }
|
||||
|
||||
uint8_t Format_HHMMSS(char *Out, uint32_t Time)
|
||||
{ uint32_t DayTime=Time%86400;
|
||||
uint32_t Hour=DayTime/3600; DayTime-=Hour*3600;
|
||||
|
|
@ -77,6 +93,14 @@ uint8_t Format_HHMMSS(char *Out, uint32_t Time)
|
|||
uint32_t HHMMSS = 10000*Hour + 100*Min + Sec;
|
||||
return Format_UnsDec(Out, HHMMSS, 6); }
|
||||
|
||||
void Format_HHMMSS(void (*Output)(char), uint32_t Time)
|
||||
{ uint32_t DayTime=Time%86400;
|
||||
uint32_t Hour=DayTime/3600; DayTime-=Hour*3600;
|
||||
uint32_t Min=DayTime/60; DayTime-=Min*60;
|
||||
uint32_t Sec=DayTime;
|
||||
uint32_t HHMMSS = 10000*Hour + 100*Min + Sec;
|
||||
Format_UnsDec(Output, HHMMSS, 6); }
|
||||
|
||||
void Format_UnsDec( void (*Output)(char), uint16_t Value, uint8_t MinDigits, uint8_t DecPoint)
|
||||
{ uint16_t Base; uint8_t Pos;
|
||||
for( Pos=5, Base=10000; Base; Base/=10, Pos--)
|
||||
|
|
|
|||
108
main/format.h
108
main/format.h
|
|
@ -3,6 +3,8 @@
|
|||
|
||||
#include <stdint.h>
|
||||
|
||||
#define WITH_AUTOCR
|
||||
|
||||
char HexDigit(uint8_t Val);
|
||||
|
||||
void Format_Bytes ( void (*Output)(char), const uint8_t *Bytes, uint8_t Len);
|
||||
|
|
@ -14,6 +16,7 @@ void Format_String( void (*Output)(char), const char *String, uint8_t MinLen,
|
|||
void Format_Hex( void (*Output)(char), uint8_t Byte );
|
||||
void Format_Hex( void (*Output)(char), uint16_t Word );
|
||||
void Format_Hex( void (*Output)(char), uint32_t Word );
|
||||
void Format_MAC( void (*Output)(char), uint8_t *MAC, uint8_t Len=6);
|
||||
|
||||
void Format_UnsDec ( void (*Output)(char), uint16_t Value, uint8_t MinDigits=1, uint8_t DecPoint=0);
|
||||
void Format_SignDec( void (*Output)(char), int16_t Value, uint8_t MinDigits=1, uint8_t DecPoint=0);
|
||||
|
|
@ -34,8 +37,19 @@ uint8_t Format_Hex( char *Output, uint8_t Byte );
|
|||
uint8_t Format_Hex( char *Output, uint16_t Word );
|
||||
uint8_t Format_Hex( char *Output, uint32_t Word );
|
||||
uint8_t Format_Hex( char *Output, uint32_t Word, uint8_t Digits);
|
||||
uint8_t Format_Hex( char *Output, uint64_t Word );
|
||||
// uint8_t Format_Hex( char *Output, uint64_t Word, uint8_t Digits);
|
||||
|
||||
template <class Type>
|
||||
uint8_t Format_Hex( char *Output, Type Word, uint8_t Digits)
|
||||
{ for(uint8_t Idx=Digits; Idx>0; )
|
||||
{ Output[--Idx]=HexDigit(Word&0x0F);
|
||||
Word>>=4; }
|
||||
return Digits; }
|
||||
|
||||
uint8_t Format_HHcMMcSS(char *Out, uint32_t Time);
|
||||
uint8_t Format_HHMMSS(char *Out, uint32_t Time);
|
||||
void Format_HHMMSS(void (*Output)(char), uint32_t Time);
|
||||
|
||||
uint8_t Format_Latitude (char *Out, int32_t Lat); // [1/600000deg] => DDMM.MMMMs
|
||||
uint8_t Format_Longitude(char *Out, int32_t Lon); // [1/600000deg] => DDDMM.MMMMs
|
||||
|
|
@ -49,60 +63,60 @@ int16_t Read_Dec3(const char *Inp); // convert three digit decimal n
|
|||
int16_t Read_Dec4(const char *Inp); // convert three digit decimal number into an integer
|
||||
|
||||
template <class Type>
|
||||
int8_t Read_Hex(Type &Int, const char *Inp) // convert variable number of digits hexadecimal number into an integer
|
||||
{ Int=0; int8_t Len=0;
|
||||
if(Inp==0) return 0;
|
||||
for( ; ; )
|
||||
int8_t Read_Hex(Type &Int, const char *Inp, uint8_t MaxDig=0) // convert variable number of digits hexadecimal number into an integer
|
||||
{ if(Inp==0) return 0;
|
||||
if(MaxDig==0) MaxDig=2*sizeof(Type);
|
||||
Int=0; int8_t Len=0;
|
||||
for( ; MaxDig; MaxDig--)
|
||||
{ int8_t Dig=Read_Hex1(Inp[Len]); if(Dig<0) break;
|
||||
Int = (Int<<4) + Dig; Len++; }
|
||||
return Len; } // return number of characters read
|
||||
|
||||
template <class Type>
|
||||
int8_t Read_UnsDec(Type &Int, const char *Inp) // convert variable number of digits unsigned decimal number into an integer
|
||||
{ Int=0; int8_t Len=0;
|
||||
if(Inp==0) return 0;
|
||||
for( ; ; )
|
||||
{ int8_t Dig=Read_Dec1(Inp[Len]); if(Dig<0) break;
|
||||
Int = 10*Int + Dig; Len++; }
|
||||
return Len; } // return number of characters read
|
||||
template <class Type>
|
||||
int8_t Read_UnsDec(Type &Int, const char *Inp) // convert variable number of digits unsigned decimal number into an integer
|
||||
{ Int=0; int8_t Len=0;
|
||||
if(Inp==0) return 0;
|
||||
for( ; ; )
|
||||
{ int8_t Dig=Read_Dec1(Inp[Len]); if(Dig<0) break;
|
||||
Int = 10*Int + Dig; Len++; }
|
||||
return Len; } // return number of characters read
|
||||
|
||||
template <class Type>
|
||||
int8_t Read_SignDec(Type &Int, const char *Inp) // convert signed decimal number into in16_t or int32_t
|
||||
{ Int=0; int8_t Len=0;
|
||||
if(Inp==0) return 0;
|
||||
char Sign=Inp[0];
|
||||
if((Sign=='+')||(Sign=='-')) Len++;
|
||||
Len+=Read_UnsDec(Int, Inp+Len); if(Sign=='-') Int=(-Int);
|
||||
return Len; } // return number of characters read
|
||||
template <class Type>
|
||||
int8_t Read_SignDec(Type &Int, const char *Inp) // convert signed decimal number into in16_t or int32_t
|
||||
{ Int=0; int8_t Len=0;
|
||||
if(Inp==0) return 0;
|
||||
char Sign=Inp[0];
|
||||
if((Sign=='+')||(Sign=='-')) Len++;
|
||||
Len+=Read_UnsDec(Int, Inp+Len); if(Sign=='-') Int=(-Int);
|
||||
return Len; } // return number of characters read
|
||||
|
||||
template <class Type>
|
||||
int8_t Read_Int(Type &Value, const char *Inp)
|
||||
{ Value=0; int8_t Len=0;
|
||||
if(Inp==0) return 0;
|
||||
char Sign=Inp[0]; int8_t Dig;
|
||||
if((Sign=='+')||(Sign=='-')) Len++;
|
||||
if((Inp[Len]=='0')&&(Inp[Len+1]=='x'))
|
||||
{ Len+=2; Dig=Read_Hex(Value, Inp+Len); }
|
||||
else
|
||||
{ Dig=Read_UnsDec(Value, Inp+Len); }
|
||||
if(Dig<=0) return Dig;
|
||||
Len+=Dig;
|
||||
if(Sign=='-') Value=(-Value); return Len; }
|
||||
|
||||
template <class Type>
|
||||
int8_t Read_Float1(Type &Value, const char *Inp) // read floating point, take just one digit after decimal point
|
||||
{ Value=0; int8_t Len=0;
|
||||
if(Inp==0) return 0;
|
||||
char Sign=Inp[0]; int8_t Dig;
|
||||
if((Sign=='+')||(Sign=='-')) Len++;
|
||||
Len+=Read_UnsDec(Value, Inp+Len); Value*=10;
|
||||
if(Inp[Len]!='.') goto Ret;
|
||||
Len++;
|
||||
Dig=Read_Dec1(Inp[Len]); if(Dig<0) goto Ret;
|
||||
Value+=Dig; Len++;
|
||||
Dig=Read_Dec1(Inp[Len]); if(Dig>=5) Value++;
|
||||
Ret: if(Sign=='-') Value=(-Value); return Len; }
|
||||
template <class Type>
|
||||
int8_t Read_Int(Type &Value, const char *Inp)
|
||||
{ Value=0; int8_t Len=0;
|
||||
if(Inp==0) return 0;
|
||||
char Sign=Inp[0]; int8_t Dig;
|
||||
if((Sign=='+')||(Sign=='-')) Len++;
|
||||
if((Inp[Len]=='0')&&(Inp[Len+1]=='x'))
|
||||
{ Len+=2; Dig=Read_Hex(Value, Inp+Len); }
|
||||
else
|
||||
{ Dig=Read_UnsDec(Value, Inp+Len); }
|
||||
if(Dig<=0) return Dig;
|
||||
Len+=Dig;
|
||||
if(Sign=='-') Value=(-Value); return Len; }
|
||||
|
||||
template <class Type>
|
||||
int8_t Read_Float1(Type &Value, const char *Inp) // read floating point, take just one digit after decimal point
|
||||
{ Value=0; int8_t Len=0;
|
||||
if(Inp==0) return 0;
|
||||
char Sign=Inp[0]; int8_t Dig;
|
||||
if((Sign=='+')||(Sign=='-')) Len++;
|
||||
Len+=Read_UnsDec(Value, Inp+Len); Value*=10;
|
||||
if(Inp[Len]!='.') goto Ret;
|
||||
Len++;
|
||||
Dig=Read_Dec1(Inp[Len]); if(Dig<0) goto Ret;
|
||||
Value+=Dig; Len++;
|
||||
Dig=Read_Dec1(Inp[Len]); if(Dig>=5) Value++;
|
||||
Ret: if(Sign=='-') Value=(-Value); return Len; }
|
||||
|
||||
int8_t Read_LatDDMMSS(int32_t &Lat, const char *Inp);
|
||||
int8_t Read_LonDDMMSS(int32_t &Lon, const char *Inp);
|
||||
|
|
|
|||
298
main/gps.cpp
298
main/gps.cpp
|
|
@ -1,9 +1,10 @@
|
|||
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
#include "hal.h"
|
||||
#include "gps.h"
|
||||
// #include "ctrl.h"
|
||||
#include "ctrl.h"
|
||||
|
||||
#include "nmea.h"
|
||||
#include "ubx.h"
|
||||
|
|
@ -23,6 +24,7 @@
|
|||
|
||||
#ifdef DEBUG_PRINT
|
||||
static char Line[128];
|
||||
static void CONS_HexDump(char Byte) { Format_Hex(CONS_UART_Write, (uint8_t)Byte); }
|
||||
#endif
|
||||
|
||||
// ----------------------------------------------------------------------------
|
||||
|
|
@ -37,13 +39,16 @@ static UBX_RxMsg UBX; // UBX messages catcher
|
|||
static MAV_RxMsg MAV; // MAVlink message catcher
|
||||
#endif
|
||||
|
||||
uint16_t GPS_PosPeriod = 0;
|
||||
uint16_t GPS_PosPeriod = 0; // [0.01s] time between succecive GPS readouts
|
||||
|
||||
// uint8_t GPS_PowerMode = 2; // 0=shutdown, 1=reduced power, 2=normal
|
||||
|
||||
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
|
||||
static GPS_Position Position[GPS_PosPipeSize]; // GPS position pipe
|
||||
|
||||
static TickType_t Burst_TickCount; // [msec] TickCount when the data burst from GPS started
|
||||
static TickType_t PPS_Tick; // [msec] System Tick when the PPS arrived
|
||||
static TickType_t Burst_Tick; // [msec] System Tick 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
|
||||
|
|
@ -52,18 +57,20 @@ static TickType_t Burst_TickCount; // [msec] TickCount when the data bur
|
|||
int32_t GPS_Longitude = 0; //
|
||||
int16_t GPS_GeoidSepar= 0; // [0.1m]
|
||||
uint16_t GPS_LatCosine = 3000; //
|
||||
uint32_t GPS_Random = 0x12345678; // random number from the LSB of the GPS data
|
||||
uint16_t GPS_SatSNR = 0; // [0.25dB]
|
||||
|
||||
Status GPS_Status;
|
||||
|
||||
static union
|
||||
{ uint8_t Flags;
|
||||
struct
|
||||
{ bool Spare:1; //
|
||||
bool Active:1; // has started
|
||||
bool GxRMC:1; // GPRMC or GNRMC registered
|
||||
{ bool GxRMC:1; // GPRMC or GNRMC registered
|
||||
bool GxGGA:1; // GPGGA or GNGGA registered
|
||||
bool GxGSA:1; // GPGSA or GNGSA registered
|
||||
bool Complete:1; // all GPS data is supplied and thus ready for processing
|
||||
bool Spare:1;
|
||||
bool Active:1; // has started and data from the GPS is flowing
|
||||
bool Complete:1; // all GPS data we need is supplied and thus ready for processing
|
||||
} ;
|
||||
} GPS_Burst;
|
||||
// for the autobaud on the GPS port
|
||||
|
|
@ -77,7 +84,65 @@ uint32_t GPS_getBaudRate (void) { return BaudRate[BaudRateIdx]; }
|
|||
uint32_t GPS_nextBaudRate(void) { BaudRateIdx++; if(BaudRateIdx>=BaudRates) BaudRateIdx=0; return GPS_getBaudRate(); }
|
||||
|
||||
const uint32_t GPS_TargetBaudRate = 57600; // BaudRate[4]; // [bps] must be one of the baud rates known by the autbaud
|
||||
const uint8_t GPS_TargetDynModel = 7; // for UBX GPS's: 6 = airborne with >1g, 7 = with >2g
|
||||
// const uint8_t GPS_TargetDynModel = 7; // for UBX GPS's: 6 = airborne with >1g, 7 = with >2g
|
||||
|
||||
static char GPS_Cmd[64];
|
||||
|
||||
// ----------------------------------------------------------------------------
|
||||
|
||||
static uint16_t SatSNRsum = 0;
|
||||
static uint8_t SatSNRcount = 0;
|
||||
|
||||
struct GPS_Sat // store GPS satellite data in single 32-bit word
|
||||
{ union
|
||||
{ uint32_t Word;
|
||||
struct
|
||||
{ uint16_t Azim: 9; // [deg]
|
||||
uint8_t Elev: 7; // [deg]
|
||||
uint8_t SNR: 7; // [dB/Hz]
|
||||
uint16_t PRN: 9; // [1..96] GPS:1..32, SBAS:33..64, GNSS:65..96
|
||||
} ;
|
||||
} ;
|
||||
} ;
|
||||
|
||||
static void ProcessGSV(NMEA_RxMsg &GSV) // process GxGSV to extract satellite data
|
||||
{
|
||||
#ifdef DEBUG_PRINT
|
||||
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
Format_String(CONS_UART_Write, (const char *)GSV.Data, 0, GSV.Len);
|
||||
Format_String(CONS_UART_Write, " (");
|
||||
Format_UnsDec(CONS_UART_Write, (uint16_t)GSV.Parms);
|
||||
Format_String(CONS_UART_Write, ")\n");
|
||||
xSemaphoreGive(CONS_Mutex);
|
||||
#endif
|
||||
if(!GSV.isGPGSV()) return; // for now, only the GPS satellites, before we learn to mix the others in
|
||||
if(GSV.Parms<3) return;
|
||||
int8_t Pkts=Read_Dec1((const char *)GSV.ParmPtr(0)); if(Pkts<0) return;
|
||||
int8_t Pkt =Read_Dec1((const char *)GSV.ParmPtr(1)); if(Pkt <0) return;
|
||||
int8_t Sats=Read_Dec2((const char *)GSV.ParmPtr(2));
|
||||
if(Sats<0) Sats=Read_Dec1((const char *)GSV.ParmPtr(2));
|
||||
if(Sats<0) return;
|
||||
for( int Parm=3; Parm<GSV.Parms; )
|
||||
{ int8_t PRN =Read_Dec2((const char *)GSV.ParmPtr(Parm++)); if(PRN <0) break;
|
||||
int8_t Elev=Read_Dec2((const char *)GSV.ParmPtr(Parm++)); if(Elev<0) break;
|
||||
int16_t Azim=Read_Dec3((const char *)GSV.ParmPtr(Parm++)); if(Azim<0) break;
|
||||
int8_t SNR =Read_Dec2((const char *)GSV.ParmPtr(Parm++)); if(SNR<=0) continue;
|
||||
SatSNRsum+=SNR; SatSNRcount++; }
|
||||
if(Pkt==Pkts)
|
||||
{
|
||||
#ifdef DEBUG_PRINT
|
||||
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
Format_String(CONS_UART_Write, "SatSNR: ");
|
||||
Format_UnsDec(CONS_UART_Write, SatSNRsum);
|
||||
CONS_UART_Write('/');
|
||||
Format_UnsDec(CONS_UART_Write, (uint16_t)SatSNRcount);
|
||||
Format_String(CONS_UART_Write, "\n");
|
||||
xSemaphoreGive(CONS_Mutex);
|
||||
#endif
|
||||
if(SatSNRcount) GPS_SatSNR = (4*SatSNRsum+SatSNRcount/2)/SatSNRcount;
|
||||
else GPS_SatSNR = 0;
|
||||
SatSNRsum=0; SatSNRcount=0; }
|
||||
}
|
||||
|
||||
// ----------------------------------------------------------------------------
|
||||
|
||||
|
|
@ -97,21 +162,21 @@ int16_t GPS_AverageSpeed(void) // get average speed based
|
|||
|
||||
static void GPS_PPS_On(void) // called on rising edge of PPS
|
||||
{ static TickType_t PrevTickCount=0;
|
||||
TickType_t TickCount = xTaskGetTickCount(); // [ms] TickCount now
|
||||
TickType_t Delta = TickCount-PrevTickCount; // [ms] time difference to the previous PPS
|
||||
PrevTickCount = TickCount; // [ms]
|
||||
if(abs((int)Delta-1000)>10) return; // [ms] filter out difference away from 1.00sec
|
||||
TimeSync_HardPPS(TickCount);
|
||||
PPS_Tick = xTaskGetTickCount(); // [ms] TickCount now
|
||||
TickType_t Delta = PPS_Tick-PrevTickCount; // [ms] time difference to the previous PPS
|
||||
PrevTickCount = PPS_Tick; // [ms]
|
||||
if(abs((int)Delta-1000)>=20) return; // [ms] filter out difference away from 1.00sec
|
||||
TimeSync_HardPPS(PPS_Tick); // [ms] synchronize the UTC time to the PPS at given Tick
|
||||
#ifdef DEBUG_PRINT
|
||||
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, " -> PPS\n");
|
||||
xSemaphoreGive(CONS_Mutex);
|
||||
#endif
|
||||
GPS_Status.PPS=1;
|
||||
LED_PCB_Flash(50);
|
||||
LED_PCB_Flash(100);
|
||||
// uint8_t Sec=GPS_Sec; Sec++; if(Sec>=60) Sec=0; GPS_Sec=Sec;
|
||||
// GPS_UnixTime++;
|
||||
// #ifdef WITH_MAVLINK
|
||||
|
|
@ -158,12 +223,12 @@ static void GPS_LockEnd(void) // called when GPS looses a
|
|||
// ----------------------------------------------------------------------------
|
||||
|
||||
static void GPS_BurstStart(void) // when GPS starts sending the data on the serial port
|
||||
{ Burst_TickCount=xTaskGetTickCount();
|
||||
{ Burst_Tick=xTaskGetTickCount();
|
||||
#ifdef DEBUG_PRINT
|
||||
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
Format_UnsDec(CONS_UART_Write, TimeSync_Time()%60);
|
||||
Format_UnsDec(CONS_UART_Write, TimeSync_Time(Burst_Tick)%60, 2);
|
||||
CONS_UART_Write('.');
|
||||
Format_UnsDec(CONS_UART_Write, TimeSync_msTime(),3);
|
||||
Format_UnsDec(CONS_UART_Write, TimeSync_msTime(Burst_Tick), 3);
|
||||
Format_String(CONS_UART_Write, " -> GPS_BurstStart() GPS:");
|
||||
Format_Hex(CONS_UART_Write, GPS_Status.Flags);
|
||||
Format_String(CONS_UART_Write, "\n");
|
||||
|
|
@ -196,34 +261,71 @@ static void GPS_BurstStart(void) // wh
|
|||
CFG_MSG.msgID = 0x04; // ID for GSA
|
||||
UBX_RxMsg::Send(0x06, 0x01, GPS_UART_Write, (uint8_t *)(&CFG_MSG), sizeof(CFG_MSG));
|
||||
}
|
||||
#endif
|
||||
#ifdef WITH_GPS_MTK
|
||||
if(Parameters.NavRate)
|
||||
{ strcpy(GPS_Cmd, "$PMTK220,"); // MTK command to change the navigation rate
|
||||
uint8_t Len = strlen(GPS_Cmd);
|
||||
uint16_t OneSec = 1000;
|
||||
Len += Format_UnsDec(GPS_Cmd+Len, OneSec/Parameters.NavRate);
|
||||
Len += NMEA_AppendCheck(GPS_Cmd, Len);
|
||||
GPS_Cmd[Len++]='\r';
|
||||
GPS_Cmd[Len++]='\n';
|
||||
GPS_Cmd[Len]=0;
|
||||
Format_String(GPS_UART_Write, GPS_Cmd, Len, 0);
|
||||
GPS_Status.ModeConfig=1;
|
||||
}
|
||||
if(Parameters.NavMode)
|
||||
{ strcpy(GPS_Cmd, "$PMTK886,"); // MTK command to change the navigation mode
|
||||
uint8_t Len = strlen(GPS_Cmd);
|
||||
GPS_Cmd[Len++]='0'+Parameters.NavMode;
|
||||
Len += NMEA_AppendCheck(GPS_Cmd, Len);
|
||||
GPS_Cmd[Len++]='\r';
|
||||
GPS_Cmd[Len++]='\n';
|
||||
GPS_Cmd[Len]=0;
|
||||
Format_String(GPS_UART_Write, GPS_Cmd, Len, 0);
|
||||
GPS_Status.ModeConfig=1;
|
||||
}
|
||||
#endif
|
||||
}
|
||||
if(!GPS_Status.BaudConfig) // if GPS baud config is not done yet
|
||||
{ // Format_String(CONS_UART_Write, "CFG_PRT query...\n");
|
||||
#ifdef WITH_GPS_UBX
|
||||
// uint8_t UART1_Port=1;
|
||||
// UBX_RxMsg::Send(0x06, 0x00, GPS_UART_Write, &UART1_Port, 1); // send the query for the port config to have a template configuration packet
|
||||
UBX_CFG_PRT CFG_PRT; // send in blind the config message for the UART
|
||||
CFG_PRT.portID=1;
|
||||
CFG_PRT.reserved1=0x00;
|
||||
CFG_PRT.txReady=0x0000;
|
||||
CFG_PRT.mode=0x08D0;
|
||||
CFG_PRT.baudRate=GPS_TargetBaudRate;
|
||||
CFG_PRT.inProtoMask=3;
|
||||
CFG_PRT.outProtoMask=3;
|
||||
CFG_PRT.flags=0x0000;
|
||||
CFG_PRT.reserved2=0x0000;
|
||||
UBX_RxMsg::Send(0x06, 0x00, GPS_UART_Write, (uint8_t*)(&CFG_PRT), sizeof(CFG_PRT));
|
||||
#ifdef DEBUG_PRINT
|
||||
Format_String(CONS_UART_Write, "GPS <- CFG_PRT: ");
|
||||
UBX_RxMsg::Send(0x06, 0x00, CONS_HexDump, (uint8_t*)(&CFG_PRT), sizeof(CFG_PRT));
|
||||
Format_String(CONS_UART_Write, "\n");
|
||||
#endif
|
||||
UBX_RxMsg::Send(0x06, 0x00, GPS_UART_Write); // send the query for the port config to have a template configuration packet
|
||||
#endif
|
||||
#ifdef WITH_GPS_MTK
|
||||
char GPS_Cmd[36];
|
||||
strcpy(GPS_Cmd, "$PMTK251,"); // MTK command to change the baud rate
|
||||
uint8_t Len = strlen(GPS_Cmd);
|
||||
Len += Format_UnsDec(GPS_Cmd+Len, GPS_TargetBaudRate);
|
||||
Len += NMEA_AppendCheck(GPS_Cmd, Len);
|
||||
GPS_Cmd[Len++]='\r'; // this is apparently needed but it should not, as ESP32 does auto-CR ??
|
||||
GPS_Cmd[Len++]='\n';
|
||||
GPS_Cmd[Len]=0;
|
||||
Format_String(GPS_UART_Write, GPS_Cmd, Len, 0);
|
||||
{ strcpy(GPS_Cmd, "$PMTK251,"); // MTK command to change the baud rate
|
||||
uint8_t Len = strlen(GPS_Cmd);
|
||||
Len += Format_UnsDec(GPS_Cmd+Len, GPS_TargetBaudRate);
|
||||
Len += NMEA_AppendCheck(GPS_Cmd, Len);
|
||||
GPS_Cmd[Len++]='\r'; // this is apparently needed but it should not, as ESP32 does auto-CR ??
|
||||
GPS_Cmd[Len++]='\n';
|
||||
GPS_Cmd[Len]=0;
|
||||
Format_String(GPS_UART_Write, GPS_Cmd, Len, 0); }
|
||||
#ifdef DEBUG_PRINT
|
||||
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
Format_String(CONS_UART_Write, "GPS <- ");
|
||||
Format_String(CONS_UART_Write, GPS_Cmd, Len, 0);
|
||||
xSemaphoreGive(CONS_Mutex);
|
||||
#endif
|
||||
#endif
|
||||
#endif // WITH_GPS_MTK
|
||||
#ifdef WITH_GPS_SRF
|
||||
char GPS_Cmd[36];
|
||||
strcpy(GPS_Cmd, "$PSRF100,1,"); // SiRF command to change the baud rate
|
||||
Len = strlen(GPS_Cmd);
|
||||
Len += Format_UnsDec(GPS_Cmd+Len, GPS_TargetBaudRate);
|
||||
|
|
@ -234,7 +336,7 @@ static void GPS_BurstStart(void) // wh
|
|||
GPS_Cmd[Len++]='\n';
|
||||
GPS_Cmd[Len]=0;
|
||||
Format_String(GPS_UART_Write, GPS_Cmd, Len, 0);
|
||||
#endif
|
||||
#endif // WITH_GPS_SRF
|
||||
}
|
||||
QueryWait=60;
|
||||
}
|
||||
|
|
@ -243,6 +345,18 @@ static void GPS_BurstStart(void) // wh
|
|||
#endif // WITH_GPS_CONFIG
|
||||
}
|
||||
|
||||
static void GPS_Random_Update(uint8_t Bit)
|
||||
{ GPS_Random = (GPS_Random<<1) | (Bit&1); }
|
||||
|
||||
static void GPS_Random_Update(GPS_Position *Pos)
|
||||
{ if(Position==0) return;
|
||||
GPS_Random_Update(Pos->Altitude);
|
||||
GPS_Random_Update(Pos->Speed);
|
||||
GPS_Random_Update(Pos->Latitude);
|
||||
GPS_Random_Update(Pos->Longitude);
|
||||
if(Pos->hasBaro) GPS_Random_Update(Pos->Pressure);
|
||||
XorShift32(GPS_Random); }
|
||||
|
||||
static void GPS_BurstComplete(void) // when GPS has sent the essential data for position fix
|
||||
{
|
||||
#ifdef WITH_MAVLINK
|
||||
|
|
@ -262,14 +376,15 @@ static void GPS_BurstComplete(void) // wh
|
|||
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
Format_UnsDec(CONS_UART_Write, TimeSync_Time()%60, 2);
|
||||
CONS_UART_Write('.');
|
||||
Format_UnsDec(CONS_UART_Write, TimeSync_msTime(),3);
|
||||
Format_UnsDec(CONS_UART_Write, TimeSync_msTime(), 3);
|
||||
Format_String(CONS_UART_Write, " -> GPS_BurstComplete() GPS:");
|
||||
Format_Hex(CONS_UART_Write, GPS_Status.Flags);
|
||||
Format_String(CONS_UART_Write, "\nGPS");
|
||||
CONS_UART_Write('0'+PosIdx); CONS_UART_Write(':'); CONS_UART_Write(' ');
|
||||
Format_String(CONS_UART_Write, "\nGPS[");
|
||||
CONS_UART_Write('0'+PosIdx); CONS_UART_Write(']'); CONS_UART_Write(' ');
|
||||
Format_String(CONS_UART_Write, Line);
|
||||
xSemaphoreGive(CONS_Mutex);
|
||||
#endif
|
||||
GPS_Random_Update(Position+PosIdx);
|
||||
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
|
||||
|
|
@ -277,7 +392,15 @@ static void GPS_BurstComplete(void) // wh
|
|||
{ uint32_t UnixTime=Position[PosIdx].getUnixTime();
|
||||
GPS_FatTime=Position[PosIdx].getFatTime();
|
||||
#ifndef WITH_MAVLINK // with MAVlink we sync. with the SYSTEM_TIME message
|
||||
TimeSync_SoftPPS(Burst_TickCount, UnixTime, Parameters.PPSdelay);
|
||||
int32_t msDiff = Position[PosIdx].FracSec;
|
||||
if(msDiff>=50) { msDiff-=100; UnixTime++; } // [0.01s]
|
||||
msDiff*=10; // [ms]
|
||||
// if(abs(msDiff)<=200) // if (almost) full-second burst
|
||||
{ // TickType_t PPS_Age = Burst_Tick-PPS_Tick;
|
||||
// if(PPS_Age>10000) TimeSync_SoftPPS(Burst_Tick, UnixTime, Parameters.PPSdelay);
|
||||
// else TimeSync_SetTime(Burst_Tick-Parameters.PPSdelay, UnixTime);
|
||||
TimeSync_SoftPPS(Burst_Tick, UnixTime, msDiff+Parameters.PPSdelay);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
|
@ -305,7 +428,7 @@ static void GPS_BurstComplete(void) // wh
|
|||
if(!Position[PrevIdx2].isValid()) break;
|
||||
TimeDiff = Position[PosIdx].calcTimeDiff(Position[PrevIdx2]);
|
||||
PrevIdx=PrevIdx2; }
|
||||
TimeDiff=Position[PosIdx].calcDifferences(Position[PrevIdx]);
|
||||
TimeDiff=Position[PosIdx].calcDifferentials(Position[PrevIdx]);
|
||||
#ifdef DEBUG_PRINT
|
||||
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
Format_String(CONS_UART_Write, "calcDiff() => ");
|
||||
|
|
@ -317,7 +440,7 @@ static void GPS_BurstComplete(void) // wh
|
|||
Format_String(CONS_UART_Write, "s\n");
|
||||
xSemaphoreGive(CONS_Mutex);
|
||||
#endif
|
||||
LED_PCB_Flash(100); }
|
||||
LED_PCB_Flash(200); }
|
||||
}
|
||||
else // complete but no valid lock
|
||||
{ if(GPS_TimeSinceLock) { GPS_LockEnd(); GPS_TimeSinceLock=0; }
|
||||
|
|
@ -339,13 +462,13 @@ static void GPS_BurstComplete(void) // wh
|
|||
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");
|
||||
CONS_UART_Write('0'+PosIdx); CONS_UART_Write(':'); CONS_UART_Write(' ');
|
||||
Format_String(CONS_UART_Write,"GPS[");
|
||||
CONS_UART_Write('0'+PosIdx); CONS_UART_Write(']'); 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_UnsDec(CONS_UART_Write, GPS_PosPeriod, 3, 2);
|
||||
Format_String(CONS_UART_Write, "s\n");
|
||||
xSemaphoreGive(CONS_Mutex);
|
||||
#endif
|
||||
|
|
@ -367,15 +490,15 @@ static void GPS_BurstEnd(void) // wh
|
|||
// ----------------------------------------------------------------------------
|
||||
|
||||
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;
|
||||
{ int16_t TargetTime = Frac+(int16_t)Sec*100; // target time including the seconds
|
||||
BestIdx=0; BestRes=0x7FFF;
|
||||
for(uint8_t Idx=0; Idx<GPS_PosPipeSize; Idx++)
|
||||
for(uint8_t Idx=0; Idx<GPS_PosPipeSize; Idx++) // run through the GPS positions stored in the pipe
|
||||
{ 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; }
|
||||
if(!Pos->isReady) continue; // skip those not-ready yet
|
||||
int16_t Diff = TargetTime - (Pos->FracSec + (int16_t)Pos->Sec*100); // difference from the target time
|
||||
if(Diff<(-3000)) Diff+=6000; // wrap-around 60 sec
|
||||
else if(Diff>=3000) Diff-=6000;
|
||||
if(fabs(Diff)<fabs(BestRes)) { BestRes=Diff; BestIdx=Idx; } // store the smallest difference from target
|
||||
}
|
||||
return BestRes==0x7FFF ? 0:Position+BestIdx; }
|
||||
|
||||
|
|
@ -399,25 +522,17 @@ GPS_Position *GPS_getPosition(int8_t Sec) // retu
|
|||
static void GPS_NMEA(void) // when GPS gets a correct NMEA sentence
|
||||
{ GPS_Status.NMEA=1;
|
||||
GPS_Status.BaudConfig = (GPS_getBaudRate() == GPS_TargetBaudRate);
|
||||
LED_PCB_Flash(2); // Flash the LED for 2 ms
|
||||
Position[PosIdx].ReadNMEA(NMEA); // read position elements from NMEA
|
||||
LED_PCB_Flash(10); // Flash the LED for 2 ms
|
||||
if(NMEA.isGxGSV()) ProcessGSV(NMEA); // process satellite data
|
||||
Position[PosIdx].ReadNMEA(NMEA); // read position elements from NMEA
|
||||
if(NMEA.isGxRMC()) GPS_Burst.GxRMC=1;
|
||||
if(NMEA.isGxGGA()) GPS_Burst.GxGGA=1;
|
||||
if(NMEA.isGxGSA()) GPS_Burst.GxGSA=1;
|
||||
if(Button_SleepRequest)
|
||||
{
|
||||
#ifdef WITH_GPS_MTK
|
||||
#ifdef WITH_GPS_ENABLE
|
||||
GPS_DISABLE();
|
||||
#endif
|
||||
Format_String(GPS_UART_Write, /* "$PMTK161,0*28\r\n", */ "$PMTK225,4*2F\r\n", 15, 0); // 225 or "$PMTK161,0*28\r\n" request to the GPS to enter sleep
|
||||
#endif
|
||||
}
|
||||
#ifdef DEBUG_PRINT
|
||||
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_UnsDec(CONS_UART_Write, TimeSync_msTime(), 3);
|
||||
Format_String(CONS_UART_Write, " -> ");
|
||||
Format_Bytes(CONS_UART_Write, NMEA.Data, 6);
|
||||
CONS_UART_Write(' '); Format_Hex(CONS_UART_Write, GPS_Burst.Flags);
|
||||
|
|
@ -429,7 +544,7 @@ static void GPS_NMEA(void) // wh
|
|||
if( NMEA.isP() || NMEA.isGxRMC() || NMEA.isGxGGA() || NMEA.isGxGSA() || NMEA.isGPTXT() )
|
||||
// we would need to patch the GGA here for the GPS which does not calc. nor correct for GeoidSepar
|
||||
#endif
|
||||
{ // if(CONS_UART_Free()>=128)
|
||||
{ if(Parameters.Verbose)
|
||||
{ xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
Format_String(CONS_UART_Write, (const char *)NMEA.Data, 0, NMEA.Len);
|
||||
Format_String(CONS_UART_Write, "\n");
|
||||
|
|
@ -460,7 +575,7 @@ static void DumpUBX(void)
|
|||
static void GPS_UBX(void) // when GPS gets an UBX packet
|
||||
{ GPS_Status.UBX=1;
|
||||
GPS_Status.BaudConfig = (GPS_getBaudRate() == GPS_TargetBaudRate);
|
||||
LED_PCB_Flash(2);
|
||||
LED_PCB_Flash(10);
|
||||
// DumpUBX();
|
||||
// Position[PosIdx].ReadUBX(UBX);
|
||||
#ifdef WITH_GPS_UBX_PASS
|
||||
|
|
@ -477,7 +592,7 @@ static void GPS_UBX(void)
|
|||
{ class UBX_CFG_PRT *CFG = (class UBX_CFG_PRT *)UBX.Word; // create pointer to the packet content
|
||||
#ifdef DEBUG_PRINT
|
||||
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
Format_String(CONS_UART_Write, "TaskGPS: CFG_PRT\n");
|
||||
Format_String(CONS_UART_Write, "CFG_PRT: ");
|
||||
DumpUBX();
|
||||
Format_Hex(CONS_UART_Write, CFG->portID);
|
||||
CONS_UART_Write(':');
|
||||
|
|
@ -504,14 +619,15 @@ static void GPS_UBX(void)
|
|||
{ class UBX_CFG_NAV5 *CFG = (class UBX_CFG_NAV5 *)UBX.Word;
|
||||
#ifdef DEBUG_PRINT
|
||||
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
Format_String(CONS_UART_Write, "TaskGPS: CFG_NAV5 ");
|
||||
Format_String(CONS_UART_Write, "CFG_NAV5: ");
|
||||
Format_Hex(CONS_UART_Write, CFG->dynModel);
|
||||
Format_String(CONS_UART_Write, "\n");
|
||||
xSemaphoreGive(CONS_Mutex);
|
||||
#endif
|
||||
if(CFG->dynModel==GPS_TargetDynModel) GPS_Status.ModeConfig=1; // dynamic model = 6 => Airborne with >1g acceleration
|
||||
// if(CFG->dynModel==GPS_TargetDynModel) GPS_Status.ModeConfig=1; // dynamic model = 6 => Airborne with >1g acceleration
|
||||
if(CFG->dynModel==Parameters.NavMode) GPS_Status.ModeConfig=1; // dynamic model = 6 => Airborne with >1g acceleration
|
||||
else
|
||||
{ CFG->dynModel=GPS_TargetDynModel; CFG->mask = 0x01; //
|
||||
{ CFG->dynModel=Parameters.NavMode; CFG->mask = 0x01; //
|
||||
UBX.RecalcCheck(); // reclaculate the check sum
|
||||
UBX.Send(GPS_UART_Write); // send this UBX packet
|
||||
}
|
||||
|
|
@ -529,7 +645,7 @@ static void GPS_UBX(void)
|
|||
xSemaphoreGive(CONS_Mutex);
|
||||
/*
|
||||
if(UBX.Byte[0]==0x06 && UBX.Byte[1]==0x00 && UBX.ID==0) // negative ACK to CFG-PRT
|
||||
{ static char GPS_Cmd[36];
|
||||
{
|
||||
strcpy(GPS_Cmd, "$PUBX,41,1,0007,0003,"); // $PUBX command to change the baud rate
|
||||
uint8_t Len = strlen(GPS_Cmd);
|
||||
Len += Format_UnsDec(GPS_Cmd+Len, GPS_TargetBaudRate);
|
||||
|
|
@ -582,7 +698,7 @@ static uint64_t MAV_getUnixTime(void) // [m
|
|||
static void GPS_MAV(void) // when GPS gets an MAV packet
|
||||
{ TickType_t TickCount=xTaskGetTickCount();
|
||||
GPS_Status.MAV=1;
|
||||
LED_PCB_Flash(2);
|
||||
LED_PCB_Flash(10);
|
||||
GPS_Status.BaudConfig = (GPS_getBaudRate() == GPS_TargetBaudRate);
|
||||
uint8_t MsgID = MAV.getMsgID();
|
||||
uint64_t UnixTime_ms = MAV_getUnixTime(); // get the time from the MAVlink message
|
||||
|
|
@ -615,7 +731,7 @@ static void GPS_MAV(void) // wh
|
|||
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);
|
||||
TimeSync_SoftPPS(TickCount-UnixFrac, UnixTime, Parameters.PPSdelay);
|
||||
#ifdef DEBUG_PRINT
|
||||
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
Format_String(CONS_UART_Write, "MAV_SYSTEM_TIME: ");
|
||||
|
|
@ -736,9 +852,9 @@ void vTaskGPS(void* pvParameters)
|
|||
GPS_Status.Flags = 0;
|
||||
|
||||
// PPS_TickCount=0;
|
||||
Burst_TickCount=0;
|
||||
Burst_Tick=0;
|
||||
|
||||
vTaskDelay(5);
|
||||
vTaskDelay(5); // put some initial delay for lighter startup load
|
||||
|
||||
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
Format_String(CONS_UART_Write, "TaskGPS:");
|
||||
|
|
@ -789,6 +905,7 @@ void vTaskGPS(void* pvParameters)
|
|||
uint16_t MaxBytesPerTick = 1+(GPS_getBaudRate()+2500)/5000;
|
||||
for( ; ; ) // loop over bytes in the GPS UART buffer
|
||||
{ uint8_t Byte; int Err=GPS_UART_Read(Byte); if(Err<=0) break; // get Byte from serial port, if no bytes then break this loop
|
||||
// CONS_UART_Write(Byte); // copy the GPS output to console (for debug only)
|
||||
Bytes++;
|
||||
LineIdle=0; // if there was a byte: restart idle counting
|
||||
NMEA.ProcessByte(Byte); // process through the NMEA interpreter
|
||||
|
|
@ -827,23 +944,40 @@ void vTaskGPS(void* pvParameters)
|
|||
#endif
|
||||
*/
|
||||
if(LineIdle==0) // if any bytes were received ?
|
||||
{ if(!GPS_Burst.Active) GPS_BurstStart(); // burst started
|
||||
{ if(!GPS_Burst.Active) GPS_BurstStart(); // if not already started then declare burst started
|
||||
GPS_Burst.Active=1;
|
||||
if( (!GPS_Burst.Complete) && (GPS_Burst.GxGGA && GPS_Burst.GxRMC && GPS_Burst.GxGSA) )
|
||||
{ GPS_Burst.Complete=1; GPS_BurstComplete(); }
|
||||
if( (!GPS_Burst.Complete) && (GPS_Burst.GxGGA && GPS_Burst.GxRMC && GPS_Burst.GxGSA) ) // if GGA+RMC+GSA received
|
||||
{ GPS_Burst.Complete=1; GPS_BurstComplete(); } // declare burst complete
|
||||
}
|
||||
else if(LineIdle>=GPS_BurstTimeout) // if GPS sends no more data for 10 time ticks
|
||||
{ if(GPS_Burst.Active) // if still in burst
|
||||
{ if(!GPS_Burst.Complete) GPS_BurstComplete();
|
||||
GPS_BurstEnd(); } // burst just ended
|
||||
else if(LineIdle>=GPS_BurstTimeout) // if GPS sends no more data for GPS_BurstTimeout ticks
|
||||
{ if(GPS_Burst.Active) // if burst was active
|
||||
{ if(!GPS_Burst.Complete && GPS_Burst.GxGGA && GPS_Burst.GxRMC) GPS_BurstComplete(); // if not complete yet, then declare burst complete
|
||||
GPS_BurstEnd(); } // declare burst ended
|
||||
else if(LineIdle>=1500) // if idle for more than 1.5 sec
|
||||
{ GPS_Status.Flags=0; }
|
||||
GPS_Burst.Flags=0;
|
||||
GPS_Burst.Flags=0; // clear all flags: active and complete
|
||||
}
|
||||
|
||||
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
|
||||
if(PowerMode>0)
|
||||
{
|
||||
#ifdef WITH_GPS_UBS
|
||||
#ifdef WITH_GPS_ENABLE
|
||||
GPS_ENABLE();
|
||||
#endif
|
||||
GPS_UART_Write('\n');
|
||||
#endif
|
||||
#ifdef WITH_GPS_MTK
|
||||
#ifdef WITH_GPS_ENABLE
|
||||
GPS_DISABLE();
|
||||
vTaskDelay(1);
|
||||
GPS_ENABLE();
|
||||
#endif
|
||||
GPS_UART_Write('\n');
|
||||
#endif
|
||||
}
|
||||
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
Format_String(CONS_UART_Write, "TaskGPS: ");
|
||||
Format_UnsDec(CONS_UART_Write, NewBaudRate);
|
||||
|
|
@ -854,3 +988,5 @@ void vTaskGPS(void* pvParameters)
|
|||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -9,6 +9,8 @@
|
|||
|
||||
const uint8_t GPS_PosPipeSize = 4; // number of GPS positions held in a pipe
|
||||
|
||||
// extern uint8_t GPS_PowerMode; // 0=shutdown, 1=reduced, 2=normal
|
||||
|
||||
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,7 +18,9 @@ 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 uint32_t GPS_Random; // random number produced from the GPS data
|
||||
extern uint16_t GPS_PosPeriod; // [0.01sec] how often (which period) the GPS/MAV is sending the positions
|
||||
extern uint16_t GPS_SatSNR; // [0.25dB] average SNR for satellites being used for navigation
|
||||
|
||||
typedef union
|
||||
{ uint8_t Flags;
|
||||
|
|
@ -26,8 +30,8 @@ typedef union
|
|||
bool MAV:1; // got at least one valid MAV message
|
||||
bool PPS:1; // got at least one PPS signal
|
||||
bool BaudConfig:1; // baudrate is configured
|
||||
bool ModeConfig:1; // mode is configured
|
||||
bool :1; //
|
||||
bool ModeConfig:1; // navigation mode is configured
|
||||
bool RateConfig:1; // navigation rate is configured
|
||||
bool :1; //
|
||||
} ;
|
||||
} Status;
|
||||
|
|
|
|||
898
main/hal.cpp
898
main/hal.cpp
Plik diff jest za duży
Load Diff
52
main/hal.h
52
main/hal.h
|
|
@ -26,10 +26,18 @@
|
|||
|
||||
// ============================================================================================================
|
||||
|
||||
#ifdef WITH_U8G2
|
||||
extern uint8_t PowerMode; // 0=sleep/minimal power, 1=comprimize, 2=full power
|
||||
|
||||
// ============================================================================================================
|
||||
|
||||
#ifdef WITH_U8G2_OLED
|
||||
#include "u8g2.h"
|
||||
#endif
|
||||
|
||||
#ifdef WITH_ST7789
|
||||
#include "st7789.h"
|
||||
#endif
|
||||
|
||||
extern uint8_t BARO_I2C;
|
||||
|
||||
#ifdef WITH_MAVLINK
|
||||
|
|
@ -81,7 +89,7 @@ int OLED_SetContrast(uint8_t Contrast, uint8_t DispIdx=0);
|
|||
int OLED_PutLine(uint8_t Line, const char *Text, uint8_t DispIdx=0);
|
||||
#endif
|
||||
|
||||
#ifdef WITH_U8G2
|
||||
#ifdef WITH_U8G2_OLED
|
||||
extern u8g2_t U8G2_OLED;
|
||||
#endif
|
||||
|
||||
|
|
@ -94,7 +102,11 @@ int SD_getSectorSize(void);
|
|||
#endif
|
||||
|
||||
#ifdef WITH_BEEPER
|
||||
#ifdef WITH_KNOB
|
||||
extern volatile uint8_t KNOB_Tick;
|
||||
#else
|
||||
const uint8_t KNOB_Tick=15; // for now, when there is no knob
|
||||
#endif
|
||||
|
||||
const uint8_t Play_Vol_0 = 0x00;
|
||||
const uint8_t Play_Vol_1 = 0x40;
|
||||
|
|
@ -115,6 +127,20 @@ void Beep(uint16_t Freq, uint8_t Duty, uint8_t DoubleAmpl);
|
|||
void Beep_Note(uint8_t Note);
|
||||
#endif // WITH_BEEPER
|
||||
|
||||
#ifdef WITH_SOUND
|
||||
const uint32_t Sound_SampleRate = 16000;
|
||||
void Sound_PlaySilence(uint16_t Len);
|
||||
void Sound_PlayU8(const uint8_t *Data, uint16_t Len);
|
||||
void Sound_PlayS8(const int8_t *Data, uint16_t Len, uint8_t Vol=8);
|
||||
void Sound_Beep(int16_t Freq, uint16_t Len, int16_t Ampl);
|
||||
#endif
|
||||
|
||||
#ifdef WITH_VARIO
|
||||
extern uint8_t Vario_Note;
|
||||
extern uint16_t Vario_Period;
|
||||
extern uint16_t Vario_Fill;
|
||||
#endif
|
||||
|
||||
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]
|
||||
|
|
@ -133,7 +159,7 @@ void LED_RX_Flash(uint8_t Time=100);
|
|||
|
||||
void LED_TimerCheck(uint8_t Ticks=1);
|
||||
|
||||
extern bool Button_SleepRequest;
|
||||
// extern bool Button_SleepRequest;
|
||||
int32_t Button_TimerCheck(uint8_t Ticks=1);
|
||||
|
||||
void IO_Configuration(void); // Configure I/O
|
||||
|
|
@ -152,6 +178,9 @@ int SPIFFS_Info(size_t &Total, size_t &Used, const char *Label=0);
|
|||
uint8_t I2C_Read (uint8_t Bus, uint8_t Addr, uint8_t Reg, uint8_t *Data, uint8_t Len, uint8_t Wait=10);
|
||||
uint8_t I2C_Write(uint8_t Bus, uint8_t Addr, uint8_t Reg, uint8_t *Data, uint8_t Len, uint8_t Wait=10);
|
||||
|
||||
inline uint8_t I2C_Write(uint8_t Bus, uint8_t Addr, uint8_t Reg, uint8_t Byte, uint8_t Wait=10)
|
||||
{ return I2C_Write(Bus, Addr, Reg, &Byte, 1, Wait); }
|
||||
|
||||
template <class Type>
|
||||
inline uint8_t I2C_Write(uint8_t Bus, uint8_t Addr, uint8_t Reg, Type &Object, uint8_t Wait=10)
|
||||
{ return I2C_Write(Bus, Addr, Reg, (uint8_t *)&Object, sizeof(Type), Wait); }
|
||||
|
|
@ -163,5 +192,22 @@ template <class Type>
|
|||
uint8_t I2C_Restart(uint8_t Bus);
|
||||
|
||||
uint16_t BatterySense(int Samples=4); // [mV]
|
||||
#ifdef WITH_TBEAM
|
||||
uint16_t KnobSense (int Samples=4); // [ADC]
|
||||
#endif
|
||||
|
||||
#ifdef WITH_BQ
|
||||
#include "bq24295.h"
|
||||
extern BQ24295 BQ;
|
||||
#endif
|
||||
|
||||
#ifdef WITH_AXP
|
||||
#include "axp192.h"
|
||||
extern AXP192 AXP;
|
||||
#endif
|
||||
|
||||
#ifdef WITH_SLEEP
|
||||
void Sleep(void);
|
||||
#endif
|
||||
|
||||
#endif // __HAL_H__
|
||||
|
|
|
|||
|
|
@ -0,0 +1,11 @@
|
|||
#ifdef WITH_KNOB
|
||||
|
||||
extern volatile uint8_t KNOB_Tick;
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C"
|
||||
#endif
|
||||
void vTaskKNOB(void* pvParameters);
|
||||
|
||||
#endif
|
||||
|
||||
|
|
@ -0,0 +1,50 @@
|
|||
#include <stdint.h>
|
||||
#include "st7789.h"
|
||||
|
||||
class LCD_BattSymb
|
||||
{ public:
|
||||
static const uint8_t Width = 24;
|
||||
static const uint8_t Border = 2;
|
||||
static const uint8_t Cells = 5;
|
||||
static const uint8_t CellWidth = Width-Border*4;
|
||||
static const uint8_t CellLength = CellWidth*3/4;
|
||||
static const uint8_t Length = (CellLength+Border)*Cells+Border*3;
|
||||
static const uint8_t TipLen = Border*2;
|
||||
int16_t Xpos, Ypos;
|
||||
int16_t FrameCol, CellCol, FillCol;
|
||||
uint8_t CellMap; // which cells are to be displayed
|
||||
uint8_t Flags; // which cells and other elements are displayed
|
||||
|
||||
public:
|
||||
LCD_BattSymb() { CellMap=0; Xpos=0; Ypos=0; FillCol=RGB565_LIGHTGREY; CellCol=RGB565_DARKGREEN; FrameCol=RGB565_BLACK; Flags=0; }
|
||||
|
||||
void setLevel(uint8_t Level)
|
||||
{ CellMap=0; uint8_t Mask=1;
|
||||
if(Level>Cells) Level=Cells;
|
||||
for(uint8_t Cell=0; Cell<Level; Cell++)
|
||||
{ CellMap|=Mask; Mask<<=1; } // set cells to be displayed after the battery level
|
||||
}
|
||||
|
||||
void Draw(void)
|
||||
{ LCD_DrawBox(Xpos, Ypos, Length, Width, FrameCol); // frame box
|
||||
LCD_DrawBox(Xpos+Length, Ypos+Width/4, TipLen, Width/2, FrameCol); // tip
|
||||
LCD_DrawBox(Xpos+Border, Ypos+Border, Length-Border*2, Width-Border*2, FillCol); // inner space
|
||||
uint8_t Mask=1;
|
||||
for(uint8_t Cell=0; Cell<Cells; Cell++)
|
||||
{ if(CellMap&Mask)
|
||||
{ uint16_t Xofs=Border*2+(CellLength+Border)*Cell;
|
||||
LCD_DrawBox(Xpos+Xofs, Ypos+Border*2, CellLength, CellWidth, CellCol); }
|
||||
Mask<<=1; }
|
||||
Flags = 0x80 | CellMap; }
|
||||
|
||||
void Update(void)
|
||||
{ uint8_t Mask=1;
|
||||
for(uint8_t Cell=0; Cell<Cells; Cell++)
|
||||
{ if((CellMap^Flags)&Mask)
|
||||
{ uint16_t Xofs=Border*2+(CellLength+Border)*Cell;
|
||||
LCD_DrawBox(Xpos+Xofs, Ypos+Border*2, CellLength, CellWidth, CellMap&Mask ? CellCol:FillCol); } // display or erase a cell
|
||||
Mask<<=1; }
|
||||
Flags = 0x80 | CellMap; }
|
||||
|
||||
} ;
|
||||
|
||||
|
|
@ -0,0 +1,495 @@
|
|||
#ifndef __LOKOUT_H__
|
||||
#define __LOKOUT_H__
|
||||
|
||||
#include <stdio.h>
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "intmath.h"
|
||||
|
||||
// #define DEBUG_PRINT
|
||||
|
||||
#include "relpos.h"
|
||||
|
||||
// =======================================================================================================
|
||||
|
||||
class LookOut_Target
|
||||
{ public:
|
||||
uint32_t ID; // ID of the target = aircraft ID
|
||||
Acft_RelPos Pos; // Position relative to the reference Lat/Lon/Alt
|
||||
int8_t Pred; // [0.5sec] amount of time by which this position has been predicted/extrapolated
|
||||
uint8_t GpsPrec; // GPS position error including prediction
|
||||
|
||||
union
|
||||
{ uint8_t Flags; // flags
|
||||
struct
|
||||
{ bool isMoving :1; // is a moving target
|
||||
bool Alloc :1; // is allocated or not (a free slot, where a new target can go into)
|
||||
// bool Reported :1; // this target has already been reported with $PFLAA
|
||||
} ;
|
||||
} ;
|
||||
|
||||
union
|
||||
{ uint32_t Rank; // rank: lowest means shorter time margin, shorter distance margin thus bigger thread
|
||||
struct
|
||||
{ uint16_t DistMargin; // [0.5m] remaining safety margin: if positive, then considered no thread at all
|
||||
uint8_t TimeMargin; // [0.5s] time to target (if no distance margin left)
|
||||
uint8_t WarnLevel; // assigned warning level: 0, 1, 2 or 3
|
||||
} ;
|
||||
} ;
|
||||
|
||||
int16_t dX; // [0.5m] relative position of target
|
||||
int16_t dY; // [0.5m]
|
||||
int16_t dZ; // [0.5m]
|
||||
|
||||
int16_t Vx; // [0.5m/s] relative speed of target
|
||||
int16_t Vy; // [0.5m/s]
|
||||
int16_t Vz; // [0.5m/s]
|
||||
|
||||
// int16_t Ax; // [1/16m/s^2] relative acceleration of target
|
||||
// int16_t Ay; // [1/16m/s^2]
|
||||
|
||||
uint16_t HorDist; // [0.5m] relltive hor. distance to target
|
||||
int16_t MissTime; // [0.5s] estimated closest approach time
|
||||
uint16_t MissDist; // [0.5m] estimated closest approach distance
|
||||
|
||||
public:
|
||||
void Clear(void) { Pred=0; Flags=0; HorDist=0; MissDist=0; Rank=0xFFFF; }
|
||||
|
||||
// uint16_t HorRelSpeed(void) const { }
|
||||
|
||||
uint32_t DistSqr(void) const { return (int32_t)dX*dX + (int32_t)dY*dY + (int32_t)dZ*dZ; } // [0.25m^2] Distance-square to the Target
|
||||
uint32_t VelSqr (void) const { return (int32_t)Vx*Vx + (int32_t)Vy*Vy + (int32_t)Vz*Vz; } // [0.5m/s^2] Relative velocity square of the Target
|
||||
|
||||
void calcVel(Acft_RelPos &RefPos) // calc. relative target velocity against a reference position
|
||||
{ Pos.getSpeedVector(Vx, Vy); // get horizontal speed vector from Speed and Heading
|
||||
int16_t rVx, rVy; RefPos.getSpeedVector(rVx, rVy); // same for the reference position
|
||||
Vx -= rVx; Vy -=rVy; // difference
|
||||
Vz = Pos.Climb-RefPos.Climb; } // vertical different
|
||||
|
||||
int16_t getBearing (void) const { return IntAtan2(dY, dX); } // bearing to the target
|
||||
uint16_t getHorDist (void) const { return Acft_RelPos::FastDistance(dX, dY); } // relative horizontal distance to the target
|
||||
uint16_t getRelHorSpeed(void) const { return Acft_RelPos::FastDistance(Vx, Vy); } // relative horiz. speed of the target
|
||||
|
||||
void Print(void) const
|
||||
{ printf("%08lX/%+5.1fs/%7.1fm/%7.1fm/%5.1fs/%5.1fm/%+5.1fs/w%d", (long int)ID,
|
||||
0.5*Pred, 0.5*DistMargin, 0.5*HorDist, 0.5*TimeMargin, 0.5*MissDist, 0.5*MissTime, WarnLevel);
|
||||
// printf(" [%+7.1f,%+7.1f,%+7.1f]m [%+5.1f,%+5.1f,%+5.1f]m/s", 0.5*dX, 0.5*dY, 0.5*dZ, 0.5*Vx, 0.5*Vy, 0.5*Vz);
|
||||
// printf(" [%+5.2f,%+5.2f]m/s^-2", 0.0625*Ax, 0.0625*Ay);
|
||||
Pos.Print(); }
|
||||
|
||||
uint8_t Print(char *output)
|
||||
{ uint8_t Len=0;
|
||||
|
||||
return Len; }
|
||||
|
||||
uint8_t WritePFLAA(char *NMEA)
|
||||
{ uint8_t Len=0;
|
||||
Len+=Format_String(NMEA+Len, "$PFLAA,"); // sentence name and alarm-level (but no alarms for trackers)
|
||||
NMEA[Len++]='0'+WarnLevel;
|
||||
NMEA[Len++]=',';
|
||||
Len+=Format_SignDec(NMEA+Len, dX/2);
|
||||
NMEA[Len++]=',';
|
||||
Len+=Format_SignDec(NMEA+Len, dY/2);
|
||||
NMEA[Len++]=',';
|
||||
Len+=Format_SignDec(NMEA+Len, dZ/2); // [m] relative altitude
|
||||
NMEA[Len++]=',';
|
||||
NMEA[Len++]='0'+((ID>>24)&0x03); // address-type (3=OGN)
|
||||
NMEA[Len++]=',';
|
||||
uint32_t Addr = ID&0xFFFFFF; // [24-bit] address
|
||||
Len+=Format_Hex(NMEA+Len, (uint8_t)(Addr>>16)); // 24-bit address: RND, ICAO, FLARM, OGN
|
||||
Len+=Format_Hex(NMEA+Len, (uint16_t)Addr);
|
||||
NMEA[Len++]=',';
|
||||
Len+=Format_UnsDec(NMEA+Len, (225*Pos.Heading+0x800)>>12, 4, 1); // [deg] heading (by GPS)
|
||||
NMEA[Len++]=',';
|
||||
Len+=Format_SignDec(NMEA+Len, (225*Pos.Turn+0x800)>>12, 2, 1); // [deg/sec] turn rate
|
||||
NMEA[Len++]=',';
|
||||
Len+=Format_UnsDec(NMEA+Len, 5*Pos.Speed, 2, 1); // [approx. m/s] ground speed
|
||||
NMEA[Len++]=',';
|
||||
Len+=Format_SignDec(NMEA+Len, 5*Pos.Climb, 2, 1); // [m/s] climb/sink rate
|
||||
NMEA[Len++]=',';
|
||||
NMEA[Len++]=HexDigit(ID>>26); // [0..F] aircraft-type: 1=glider, 2=tow plane, etc.
|
||||
Len+=NMEA_AppendCheckCRNL(NMEA, Len);
|
||||
NMEA[Len]=0;
|
||||
return Len; } // return number of formatted characters
|
||||
|
||||
} ;
|
||||
|
||||
// =======================================================================================================
|
||||
|
||||
class LookOut
|
||||
{ public:
|
||||
uint32_t ID; // ID of me (own aircraft)
|
||||
Acft_RelPos Pos; // Position relative to the reference Lat/Lon/Alt
|
||||
int8_t Pred; // [0.5sec] amount of time by which position has been predicted/extrapolated
|
||||
|
||||
union
|
||||
{ uint8_t Flags;
|
||||
struct
|
||||
{ uint8_t GpsPrec :6; // GPS position precision
|
||||
bool isMoving :1; // own position moving
|
||||
bool hasPosition :1; // own position is valid
|
||||
} ;
|
||||
} ;
|
||||
|
||||
uint8_t WarnLevel; // highest warning level of all the targets
|
||||
uint8_t WeakestIdx; // index for the weakest target (or a not allocated target)
|
||||
uint32_t WeakestRank; // rank of the weakest target
|
||||
|
||||
uint8_t Targets; // [aircrafts] actual number of targets monitored
|
||||
uint8_t WorstTgtIdx; // [] most dangereous target
|
||||
uint8_t WorstTgtTime; // [0.5s] time to closest approach
|
||||
|
||||
uint8_t RefTime; // [sec] ref. T for the local T,X,Y,Z coord. system
|
||||
int16_t LatCos; // [2^-12]
|
||||
// ref. X,Y,Z for the local T,X,Y,Z coord. system
|
||||
int32_t RefLat; // [1/60000deg]
|
||||
int32_t RefLon; // [1/60000deg]
|
||||
int32_t RefAlt; // [m]
|
||||
|
||||
const static uint8_t MaxTargets = 32; // maximum number of targets
|
||||
LookOut_Target Target[MaxTargets]; // array of Targets
|
||||
|
||||
const static int32_t DistRange = 7000; // [m] drop immediately anything beyond this distance
|
||||
const static int16_t MinHorizSepar = 30; // [m] minimum horizontal separation
|
||||
const static int16_t MinVertSepar = 20; // [m] minimum vertical separation
|
||||
const static int16_t WarnTime = 20; // [sec] target warning prior to impact
|
||||
|
||||
Acft_RelPos PredMe, PredTgt; // for temporary storage for predictions.
|
||||
|
||||
char Line[80]; // for printing
|
||||
|
||||
public:
|
||||
|
||||
void Clear(void)
|
||||
{ Flags=0; ID=0; Pos.Clear(); Pred=0;
|
||||
Targets=0; WeakestIdx=0; WeakestRank=0xFFFFFFFF;
|
||||
WorstTgtIdx=0; WorstTgtTime=0xFF;
|
||||
for(uint8_t Idx=0; Idx<MaxTargets; Idx++)
|
||||
{ Target[Idx].Clear(); }
|
||||
}
|
||||
|
||||
int16_t getRelBearing(const LookOut_Target *Tgt) const // [360/0x10000 deg] relative bearing to the target
|
||||
{ return Tgt->getBearing()-Pos.Heading; }
|
||||
|
||||
uint8_t WritePOGNA(char *NMEA, const LookOut_Target *Tgt) // Alert NMEA centence
|
||||
{ uint8_t Len=0;
|
||||
Len+=Format_String(NMEA+Len, "$POGNA,");
|
||||
|
||||
Len+=NMEA_AppendCheckCRNL(NMEA, Len);
|
||||
NMEA[Len]=0;
|
||||
return Len; }
|
||||
|
||||
void PrintPFLA(void) // print (for debug) $PFLAU and PFLAA
|
||||
{ WritePFLAU(Line); printf("%s", Line);
|
||||
for(uint8_t Idx=0; Idx<MaxTargets; Idx++)
|
||||
{ if(!Target[Idx].Alloc) continue;
|
||||
if( Target[Idx].DistMargin) continue;
|
||||
Target[Idx].WritePFLAA(Line);
|
||||
printf("%s", Line);
|
||||
}
|
||||
}
|
||||
|
||||
void WritePFLA(void (*Output)(char)) // produce $PFLAU and PFLAA on the console output
|
||||
{ WritePFLAU(Line); Format_String(Output, Line);
|
||||
for(uint8_t Idx=0; Idx<MaxTargets; Idx++)
|
||||
{ if(!Target[Idx].Alloc) continue;
|
||||
if( Target[Idx].DistMargin) continue;
|
||||
Target[Idx].WritePFLAA(Line);
|
||||
Format_String(Output, Line);
|
||||
}
|
||||
}
|
||||
|
||||
uint8_t WritePFLAU(char *NMEA) // produce the FLAM anti-collision status
|
||||
{ const LookOut_Target *Tgt = 0;
|
||||
if(WarnLevel>0) Tgt = Target + WorstTgtIdx;
|
||||
uint8_t Len=0;
|
||||
Len+=Format_String(NMEA+Len, "$PFLAU,");
|
||||
Len+=Format_UnsDec(NMEA+Len, Targets, 1); // number of targets received
|
||||
NMEA[Len++]=',';
|
||||
NMEA[Len++]='0'+hasPosition; // TX status
|
||||
NMEA[Len++]=',';
|
||||
NMEA[Len++]='0'+hasPosition; // GPS status: 0=no fix, 1=on the ground, 2=airborne
|
||||
NMEA[Len++]=',';
|
||||
NMEA[Len++]='1'; // power status: one could monitor the supply
|
||||
NMEA[Len++]=',';
|
||||
NMEA[Len++]='0'+WarnLevel; // Warning level: 0..3
|
||||
NMEA[Len++]=',';
|
||||
if(Tgt) // [deg] relative bearing: -180..+180
|
||||
{ Len+=Format_SignDec(NMEA+Len, ((int32_t)getRelBearing(Tgt)*45+0x1000)>>13, 1); }
|
||||
NMEA[Len++]=',';
|
||||
NMEA[Len++]='0'+((WarnLevel>0)<<1); // alarm-type: 0=none, 2=aircraft, 3=obstacle/zone/terrain
|
||||
NMEA[Len++]=',';
|
||||
if(Tgt) // [m] relative vertical distance
|
||||
{ Len+=Format_SignDec(NMEA+Len, (Tgt->dZ)>>1, 1); }
|
||||
NMEA[Len++]=',';
|
||||
if(Tgt) // [m] relative horizontal distance
|
||||
{ Len+=Format_UnsDec(NMEA+Len, (Tgt->HorDist)>>1, 1); }
|
||||
if(Tgt) // ID
|
||||
{ NMEA[Len++]=',';
|
||||
Len+=Format_Hex(NMEA+Len, Tgt->ID); }
|
||||
Len+=NMEA_AppendCheckCRNL(NMEA, Len);
|
||||
NMEA[Len]=0;
|
||||
return Len; }
|
||||
|
||||
void Print(void) const
|
||||
{ if(!hasPosition) return;
|
||||
printf("Ref: %02d: [%+10.6f, %+11.6f]deg %ldm\n", RefTime, 0.0001/60*RefLat, 0.0001/60*RefLon, (long int)RefAlt);
|
||||
printf("%08lX/%+5.1fs/ Margin/ HorDist/Margin/ Miss/ Miss/w%d", (long int)ID, 0.5*Pred, WarnLevel); Pos.Print();
|
||||
for(uint8_t Idx=0; Idx<MaxTargets; Idx++)
|
||||
{ const LookOut_Target *Tgt = Target+Idx;
|
||||
if(Tgt->Alloc) Tgt->Print();
|
||||
}
|
||||
}
|
||||
|
||||
template <class OGNx_Packet>
|
||||
int32_t Start(OGNx_Packet &Me)
|
||||
{ Clear();
|
||||
ID = Me.getAddressAndType() | ((uint32_t)Me.Position.AcftType<<26) ;
|
||||
RefTime = Me.Position.Time;
|
||||
RefLat = Me.DecodeLatitude();
|
||||
RefLon = Me.DecodeLongitude();
|
||||
RefAlt = Me.DecodeAltitude();
|
||||
LatCos = Icos(GPS_Position::calcLatAngle16(RefLat));
|
||||
Pred=0;
|
||||
return Pos.Read(Me, RefTime, RefLat, RefLon, RefAlt, LatCos, DistRange); }
|
||||
|
||||
template <class OGNx_Packet>
|
||||
const LookOut_Target *ProcessOwn(OGNx_Packet &Me) // process my own position
|
||||
{ // printf("ProcessOwn() ... entry\n");
|
||||
if(hasPosition) // in my position is valid
|
||||
{ Pred=0;
|
||||
if(Pos.Read(Me, RefTime, RefLat, RefLon, RefAlt, LatCos, DistRange)<0) // read the new position
|
||||
{ hasPosition = Start(Me)>=0; } // if this fails, attempt to start from the new position
|
||||
}
|
||||
else
|
||||
{ hasPosition = Start(Me)>=0;
|
||||
}
|
||||
|
||||
if(hasPosition) // if already started
|
||||
{ AdjustRefTime(Pos.T); // adjust time ref. point if needed
|
||||
AdjustRefAlt(); // adjust vertical ref. altitude if needed
|
||||
AdjustRefLatLon(Me); } // adjuest horizontal Lat/Lon position if needed.
|
||||
|
||||
WarnLevel=0;
|
||||
Targets=0;
|
||||
WorstTgtIdx=0;
|
||||
WorstTgtTime=0xFF;
|
||||
for(uint8_t Idx=0; Idx<MaxTargets; Idx++) // go over targets
|
||||
{ LookOut_Target *Tgt = Target+Idx;
|
||||
if(!Tgt->Alloc) continue; // skip empty slots
|
||||
if(Tgt->DistMargin==0) // those with no safety margin
|
||||
{ while(Tgt->Pos.T<=(Pos.T-4)) // bring closer in time to my (new) position
|
||||
{ Tgt->Pos.StepFwd2secs(); Tgt->Pred+=4; }
|
||||
}
|
||||
uint8_t Warn=calcTarget(Tgt); // (re)calculate the target
|
||||
if(Warn)
|
||||
{ if(Warn>WarnLevel) WarnLevel=Warn;
|
||||
if(Tgt->TimeMargin<WorstTgtTime) { WorstTgtTime=Tgt->TimeMargin; WorstTgtIdx=Idx; }
|
||||
}
|
||||
Targets++; }
|
||||
// printf("ProcessOwn() ... exit\n");
|
||||
// if(Targets==0) return 0; // return NULL if no targets are tracked
|
||||
LookOut_Target *Tgt = Target+WorstTgtIdx;
|
||||
if( (!Tgt->Alloc) || (Tgt->DistMargin>0) ) return 0; // return NULL if target is not a thread
|
||||
return Tgt; } // return the pointer to the most dangerous target
|
||||
|
||||
template <class OGNx_Packet>
|
||||
const LookOut_Target *ProcessTarget(OGNx_Packet &Packet) // process positions of other aircrafts
|
||||
{ // printf("ProcessTarget(%d) ... entry\n", WeakestIdx);
|
||||
LookOut_Target *New = Target+WeakestIdx; // get a free or lowest rank slot
|
||||
New->Clear();
|
||||
if(New->Pos.Read(Packet, RefTime, RefLat, RefLon, RefAlt, LatCos, DistRange)<0) return 0; // calculate the position against the reference position
|
||||
uint32_t ID = Packet.getAddressAndType() | ((uint32_t)Packet.Position.AcftType<<26) ; // get ID
|
||||
New->ID = ID; // set ID of this position
|
||||
// printf("ProcessTarget() ... %08X\n", ID);
|
||||
uint8_t OldIdx;
|
||||
for(OldIdx=0; OldIdx<MaxTargets; OldIdx++) // scan targets already on the list
|
||||
{ if(Target[OldIdx].Alloc==0) continue;
|
||||
if(OldIdx==WeakestIdx) continue;
|
||||
if(Target[OldIdx].ID==ID) break; } // to find previous position for the target
|
||||
if(OldIdx<MaxTargets) // if found
|
||||
{ if((Target[OldIdx].Pos.T-Target[OldIdx].Pred)>Target[WeakestIdx].Pos.T) return Target+OldIdx; // if position is not really older than stop processing this (not new) position
|
||||
Target[OldIdx].Alloc=0; } // mark old position as "not allocated"
|
||||
|
||||
New->Alloc=1; // mark this position as allocated
|
||||
|
||||
AdjustRefTime(New->Pos.T); // possibly adjust the time reference after this new position time
|
||||
// printf("ProcessTarget() ... AdjustRefTime()\n");
|
||||
|
||||
if(Pos.T<=(New->Pos.T-4)) // bring my position closer in time
|
||||
{ Pos.StepFwd2secs(); Pred+=4; }
|
||||
|
||||
uint8_t Warn=calcTarget(New); // calculate the safety margin for the target
|
||||
if(Warn>WarnLevel) WarnLevel=Warn;
|
||||
// printf("ProcessTarget() ... calc()\n");
|
||||
|
||||
uint8_t MaxIdx=WeakestIdx; uint16_t Max=New->Rank; // look for the lowest rank position on the list
|
||||
for( uint8_t Idx=MaxIdx; ; ) // go over targets
|
||||
{ Idx++; if(Idx>=MaxTargets) Idx=0;
|
||||
if(Idx==WeakestIdx) break; // end the loop when back at New
|
||||
LookOut_Target &Tgt = Target[Idx];
|
||||
if(!Tgt.Alloc) { MaxIdx=Idx; break; } // if unallocated target found: stop the search
|
||||
if(Tgt.Rank==0xFFFF) { MaxIdx=Idx; break; } // if abs. weakest target found: stop the search
|
||||
if(Tgt.Rank>=Max) { Max=Tgt.Rank; MaxIdx=Idx; } // if weaker rank found: note it
|
||||
}
|
||||
WeakestIdx=MaxIdx; // tqke the weakest slot for the nest time
|
||||
|
||||
return New; }
|
||||
|
||||
uint8_t calcTarget(LookOut_Target *Tgt) // calculate the savety margin for the (new) target
|
||||
{
|
||||
Tgt->TimeMargin=0xFF; // initially set inf. time margin
|
||||
Tgt->WarnLevel=0; // warning level=0
|
||||
Tgt->MissTime=0;
|
||||
Tgt->MissDist=0;
|
||||
uint16_t Margin = calcVertMargin(Tgt); // [0.5m] calc. vertical margin
|
||||
if(Margin==0) Margin = calcHorizMargin(Tgt); // [0.5m] if vertical margin iz zero then get horizontal margin
|
||||
Tgt->DistMargin = Margin; // [0.5m]
|
||||
if(Margin>0) // if there is still safety margin, no more calc. (dealloc. ?)
|
||||
{ // Tgt->TimeMargin = Margin/;
|
||||
return 0; } // return warning level = 0
|
||||
// at this point the distance is possibly small enough so we may hit the target
|
||||
#ifdef DEBUG_PRINT
|
||||
printf("calcTarget(0x%08X) ... no abs. safety margin\n", Tgt->ID);
|
||||
#endif
|
||||
Tgt->calcVel(Pos); // calculate relative velocity
|
||||
// uint32_t RelVelSqr = Tgt->VelSqr(); // [0.25(m/s)^2] velocity square
|
||||
int16_t dT = Pos.T - Tgt->Pos.T; // [0.5s] we need to recalc. the distance if the target time is not same as mine
|
||||
if(dT) // [0.5s] if time difference is non-zero
|
||||
{ int16_t Vx,Vy,Vz; // [0.5m/s] Target speed vector
|
||||
Tgt->Pos.getSpeedVector(Vx, Vy); Vz=Tgt->Pos.Climb; // [0.5m/s]
|
||||
Tgt->dX += (dT*Vx)>>1; // update Target relative distance
|
||||
Tgt->dY += (dT*Vy)>>1;
|
||||
Tgt->dZ += (dT*Vz)>>1;
|
||||
Tgt->HorDist = Acft_RelPos::FastDistance(Tgt->dX, Tgt->dY); } // update Target horizontal distance
|
||||
// uint32_t RelDistSqr = Tgt->DistSqr(); // [0.25m^2] distance square
|
||||
// uint32_t WarnTimeSqr = (uint32_t)WarnTime*WarnTime; // [s] warning time square
|
||||
// printf("calcTarget(0x%08X) ...\n", Tgt->ID);
|
||||
// printf("RelDistSqr = %3.1fm^ RelVelSqr=%3.1f(m/s)^2 Time=%ds\n", 0.25*RelDistSqr, 0.25*RelVelSqr, RelVelSqr ? IntSqrt(RelDistSqr/RelVelSqr):0xFFFF);
|
||||
// if((RelVelSqr*WarnTimeSqr)<=RelDistSqr) return 0;
|
||||
uint16_t RelVel = Acft_RelPos::FastDistance(Tgt->Vx, Tgt->Vy, Tgt->Vz); // [0.5m/s]
|
||||
uint16_t MinMissDist = 4*RelVel+Pos.Error + Tgt->Pos.Error + 2*MinHorizSepar; // [0.5m]
|
||||
#ifdef DEBUG_PRINT
|
||||
printf("Target: [%+4.1f, %+4.1f, %+4.1f] = %3.1fm/s MinMissDist=%3.1fm\n",
|
||||
0.5*Tgt->Vx, 0.5*Tgt->Vy, 0.5*Tgt->Vz, 0.5*RelVel, 0.5*MinMissDist);
|
||||
#endif
|
||||
|
||||
PredMe=Pos; PredTgt=Tgt->Pos; // copy my position and the target to temporary variables
|
||||
int16_t TimeMargin = PredMe.StepTillMinSepar(PredTgt, MinMissDist, 2*(WarnTime+2)); // predict when minimum separation is reached
|
||||
#ifdef DEBUG_PRINT
|
||||
printf("StepTillMinSepar(0x%08X, %3.1fm, %1ds) => %+4.1fs\n", Tgt->ID, 0.5*MinMissDist, WarnTime+2, 0.5*TimeMargin);
|
||||
#endif
|
||||
Tgt->TimeMargin=TimeMargin; // store the time margin till minimum separation
|
||||
Tgt->MissTime=TimeMargin;
|
||||
if(TimeMargin>(2*WarnTime)) return 0; // if time-to-margin longer than warning time then return no warning
|
||||
Tgt->WarnLevel=1; // otherwise set already the first warning level
|
||||
if(TimeMargin>WarnTime) return Tgt->WarnLevel; // is time-to-margin longer than half the warning time, then stop calculations here, return 1st warning level
|
||||
|
||||
#ifdef DEBUG_PRINT
|
||||
printf("Me :"); PredMe.Print();
|
||||
printf("Tgt:"); PredTgt.Print();
|
||||
#endif
|
||||
for(uint8_t Count=3; Count; Count--)
|
||||
{ int16_t MissTime = PredMe.MissTime(PredTgt, WarnTime);
|
||||
#ifdef DEBUG_PRINT
|
||||
printf("%d: MissTime = %+4.1fs\n", Count, 0.5*MissTime);
|
||||
#endif
|
||||
if(abs(MissTime)<=1) break;
|
||||
PredMe.StepFwd(MissTime);
|
||||
PredTgt.StepFwd(PredMe.T-PredTgt.T); }
|
||||
#ifdef DEBUG_PRINT
|
||||
printf("Me :"); PredMe.Print();
|
||||
printf("Tgt:"); PredTgt.Print();
|
||||
#endif
|
||||
Tgt->MissDist = PredMe.FastDistance(PredTgt);
|
||||
Tgt->MissTime = PredMe.T-Pos.T;
|
||||
#ifdef DEBUG_PRINT
|
||||
printf("MissTime = %+4.1f, MissDist = %4.1f\n", 0.5*Tgt->MissTime, 0.5*Tgt->MissDist);
|
||||
#endif
|
||||
if( (Tgt->MissTime<0) || (Tgt->MissTime>(2*WarnTime)) || (Tgt->MissDist>MinMissDist) ) Tgt->WarnLevel=0;
|
||||
else if(Tgt->MissDist<(2*MinHorizSepar)) { Tgt->WarnLevel=2; if(Tgt->MissTime<(2*WarnTime/3)) Tgt->WarnLevel=3; }
|
||||
#ifdef DEBUG_PRINT
|
||||
printf("calcTarget(%08X) V=[%+5.1f, %+5.1f, %+5.1f]m/s D=[%+7.1f, %+7.1f, %+7.1f]m MissTime=%5.1fsec MissDist=%6.1fm\n",
|
||||
Tgt->ID, 0.5*Tgt->Vx, 0.5*Tgt->Vy, 0.5*Tgt->Vz, 0.5*Tgt->dX, 0.5*Tgt->dY, 0.5*Tgt->dZ, 0.5*Tgt->MissTime, 0.5*Tgt->MissDist);
|
||||
#endif
|
||||
return Tgt->WarnLevel; }
|
||||
|
||||
uint16_t calcVertMargin(LookOut_Target *Tgt) // calculate vertical savety margin
|
||||
{ Tgt->dZ = Tgt->Pos.Z - Pos.Z; // [0.5m] relative vertical distance
|
||||
Tgt->Vz = Tgt->Pos.Climb - Pos.Climb; // [0.5ms/s] relative vertical speed
|
||||
int16_t VertError = Pos.Error+Tgt->Pos.Error; VertError+=VertError/2; // [0.5m] est. total vertical error
|
||||
VertError += 2*MinVertSepar; // [0.5m]
|
||||
if(abs(Tgt->dZ)<=VertError) return 0; // if vertical distance less than margin required: return zero margin
|
||||
if(Tgt->dZ>0) { if(Tgt->Vz>=0) return Tgt->dZ-VertError; } // if target is higher and is climbing return relative vertical distance
|
||||
else { if(Tgt->Vz<=0) return -Tgt->dZ-VertError; } // if target is lower and is falling, return like above
|
||||
int16_t dT = Tgt->Pos.T - Pos.T; // [0.5sec] time diff. in measured positions
|
||||
int32_t MaxAlt = ( (int32_t)Tgt->Vz * (2*(WarnTime+4)+abs(dT)) )>>1; // [0.5m] max. altitude change within the warning time
|
||||
MaxAlt = abs(MaxAlt) + VertError; // [0.5m]
|
||||
// printf("calcVertMargin() dAlt=%+4.1f dT=%+4.1f Climb=%+4.1f MaxAlt=%+4.1f\n", 0.5*Tgt->dZ, 0.5*dT, 0.5*Tgt->Vz, 0.5*MaxAlt);
|
||||
if(Tgt->dZ>0) // if target is above
|
||||
{ if(Tgt->dZ<MaxAlt) return 0;
|
||||
else return ( 2*Tgt->dZ-MaxAlt); } // [0.5m]
|
||||
else // if target is below
|
||||
{ if((-Tgt->dZ)<MaxAlt) return 0;
|
||||
else return (-2*Tgt->dZ-MaxAlt); } // [0.5m]
|
||||
} // return the vertical margin: if positive: we are safe, if zero: we are too close
|
||||
|
||||
uint16_t calcHorizMargin(LookOut_Target *Tgt)
|
||||
{ Tgt->dX = Tgt->Pos.X - Pos.X; // [0.5m] relative distance
|
||||
Tgt->dY = Tgt->Pos.Y - Pos.Y; // [0.5m]
|
||||
Tgt->HorDist = Acft_RelPos::FastDistance(Tgt->dX, Tgt->dY); // [0.5m] estimate horizontal distance
|
||||
int16_t HorError = Pos.Error+Tgt->Pos.Error; // [0.5m] sum GPS error from me and the target
|
||||
HorError += 2*MinHorizSepar; // [0.5m] add the min. separation required
|
||||
int16_t dT = abs(Tgt->Pos.T - Pos.T); // [0.5m] time difference between my data and target data
|
||||
int32_t MaxDistT = ((int32_t)Tgt->Pos.Speed * (2*(WarnTime+4)+dT))>>1; // [0.5m] possible distance covered by the target
|
||||
int32_t MaxDistM = ((int32_t) Pos.Speed * (2*(WarnTime+4)+dT))>>1; // [0.5m] possible distance covered by me
|
||||
int32_t MaxDist = MaxDistT + MaxDistM + HorError; // [0.5m] add horizontal separation
|
||||
if(MaxDist<Tgt->HorDist) return Tgt->HorDist-MaxDist; // [0.5m] return the (positive) difference: we are safe
|
||||
return 0; } // zero-margin => bad !
|
||||
|
||||
void AdjustRefTime(int16_t TimeDelta) // adjust the time reference point
|
||||
{ if(TimeDelta<(2*12)) return; // in more than 10sec into the future from the current reference
|
||||
TimeDelta/=2;
|
||||
RefTime+=TimeDelta; if(RefTime>=60) RefTime-=60; // shift time reference
|
||||
Pos.T-=2*TimeDelta; // shift the relative time on my own position
|
||||
if(Pos.T<(-2*60)) hasPosition=0; // if older than 60sec declare "no position"
|
||||
for(uint8_t Idx=0; Idx<MaxTargets; Idx++) // go over the targets
|
||||
{ LookOut_Target &Tgt = Target[Idx]; if(!Tgt.Alloc) continue; // skip unallocated
|
||||
Tgt.Pos.T-=2*TimeDelta; // shift the relative time
|
||||
if((Tgt.Pos.T-Tgt.Pred)<(-2*30)) Tgt.Alloc=0; // if older than 30sec then drop the target
|
||||
}
|
||||
}
|
||||
|
||||
void AdjustRefAlt(void) // shift the vertical reference point when we get too far off
|
||||
{ if(fabs(Pos.Z)<(2*200)) return; // don't shift if less than 200m from the reference point
|
||||
int16_t AltDelta=Pos.Z/2;
|
||||
RefAlt+=AltDelta;
|
||||
Pos.Z-=2*AltDelta;
|
||||
for(uint8_t Idx=0; Idx<MaxTargets; Idx++)
|
||||
{ if(Target[Idx].Alloc) Target[Idx].Pos.Z-=2*AltDelta; }
|
||||
}
|
||||
|
||||
template <class OGNx_Packet>
|
||||
void AdjustRefLatLon(OGNx_Packet &Me) // shift the horizontal reference point when we get too far off
|
||||
{ if( (fabs(Pos.X)<(2*1000)) && (fabs(Pos.Y)<(2*500)) ) return;
|
||||
int32_t LatDist, LonDist;
|
||||
if(Me.calcDistanceVector(LatDist, LonDist, RefLat, RefLon, LatCos, DistRange)<0) { return; }
|
||||
RefLat = Me.DecodeLatitude();
|
||||
RefLon = Me.DecodeLongitude();
|
||||
LatDist*=2;
|
||||
LonDist*=2;
|
||||
Pos.X -= LatDist;
|
||||
Pos.Y -= LonDist;
|
||||
for(uint8_t Idx=0; Idx<MaxTargets; Idx++)
|
||||
{ if(Target[Idx].Alloc) { Target[Idx].Pos.X-=LatDist; Target[Idx].Pos.Y-=LonDist; } }
|
||||
LatCos = Icos(GPS_Position::calcLatAngle16(RefLat));
|
||||
}
|
||||
|
||||
} ;
|
||||
|
||||
// =======================================================================================================
|
||||
|
||||
#endif // __LOKOUT_H__
|
||||
|
|
@ -11,6 +11,9 @@
|
|||
#include "sens.h"
|
||||
#include "ctrl.h" // Control task
|
||||
#include "log.h" // Data logging task
|
||||
#include "knob.h" // potentiometer as rotary encoder
|
||||
#include "sound.h" // sounds, warnings, alarms
|
||||
#include "disp.h"
|
||||
|
||||
#ifdef WITH_AERO
|
||||
#include "aero.h"
|
||||
|
|
@ -38,6 +41,14 @@ void app_main(void)
|
|||
#endif
|
||||
IO_Configuration(); // initialize the GPIO/UART/I2C/SPI for Radio, GPS, OLED, Baro
|
||||
|
||||
#ifdef WITH_SD
|
||||
if(SD_isMounted())
|
||||
{ Parameters.SaveToFlash=0;
|
||||
if(Parameters.ReadFromFile("/sdcard/TRACKER.CFG")>0)
|
||||
{ if(Parameters.SaveToFlash) Parameters.WriteToNVS(); }
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef WITH_BT_SPP
|
||||
{ int32_t Err=BT_SPP_Init(); // start BT SPP
|
||||
// #ifdef DEBUG_PRINT
|
||||
|
|
@ -54,16 +65,25 @@ void app_main(void)
|
|||
#ifdef WITH_LOG
|
||||
xTaskCreate(vTaskLOG , "LOG", 2560, 0, tskIDLE_PRIORITY+1, 0);
|
||||
#endif
|
||||
xTaskCreate(vTaskPROC, "PROC", 4096, 0, tskIDLE_PRIORITY+3, 0);
|
||||
xTaskCreate(vTaskPROC, "PROC", 2048, 0, tskIDLE_PRIORITY+3, 0);
|
||||
xTaskCreate(vTaskGPS, "GPS", 2048, 0, tskIDLE_PRIORITY+4, 0);
|
||||
#if defined(WITH_BMP180) || defined(WITH_BMP280) || defined(WITH_BME280) || defined(WITH_MS5607)
|
||||
xTaskCreate(vTaskSENS, "SENS", 2048, 0, tskIDLE_PRIORITY+4, 0);
|
||||
#endif
|
||||
#ifdef WITH_KNOB
|
||||
xTaskCreate(vTaskKNOB, "KNOB", 2048, 0, tskIDLE_PRIORITY+3, 0);
|
||||
#endif
|
||||
#ifdef WITH_AERO
|
||||
xTaskCreate(vTaskAERO, "AERO", 2048, 0, tskIDLE_PRIORITY+4, 0);
|
||||
#endif
|
||||
#ifdef WITH_WIFI
|
||||
xTaskCreate(vTaskWIFI, "WIFI", 4096, 0, tskIDLE_PRIORITY+2, 0);
|
||||
#endif
|
||||
#if defined(WITH_OLED) || defined(WITH_U8G2_OLED) || defined(WITH_ST7789) || defined(WITH_ILI9341)
|
||||
xTaskCreate(vTaskDISP, "DISP", 2048, 0, tskIDLE_PRIORITY+2, 0);
|
||||
#endif
|
||||
#ifdef WITH_SOUND
|
||||
xTaskCreate(vTaskSOUND, "SOUND", 2048, 0, tskIDLE_PRIORITY+3, 0);
|
||||
#endif
|
||||
// xTaskCreate(vTaskCTRL, "CTRL", 1536, 0, tskIDLE_PRIORITY+2, 0);
|
||||
vTaskCTRL(0); // run directly the CTRL task, instead of creating a separate one.
|
||||
|
|
|
|||
22
main/nmea.h
22
main/nmea.h
|
|
@ -11,7 +11,7 @@ inline uint8_t NMEA_AppendCheckCRNL(char *NMEA, uint8_t Len) { return NMEA_Appen
|
|||
|
||||
class NMEA_RxMsg // receiver for the NMEA sentences
|
||||
{ public:
|
||||
static const uint8_t MaxLen=96; // maximum length
|
||||
static const uint8_t MaxLen=104; // maximum length
|
||||
static const uint8_t MaxParms=24; // maximum number of parameters (commas)
|
||||
uint8_t Data[MaxLen]; // the message itself
|
||||
uint8_t Len; // number of bytes
|
||||
|
|
@ -141,7 +141,7 @@ inline uint8_t NMEA_AppendCheckCRNL(char *NMEA, uint8_t Len) { return NMEA_Appen
|
|||
if(Data[4]!='G') return 0;
|
||||
return Data[5]=='A'; }
|
||||
|
||||
uint8_t isGPGSA(void) const // GPS satellite data
|
||||
uint8_t isGPGSA(void) const //
|
||||
{ if(!isGP()) return 0;
|
||||
if(Data[3]!='G') return 0;
|
||||
if(Data[4]!='S') return 0;
|
||||
|
|
@ -159,6 +159,24 @@ inline uint8_t NMEA_AppendCheckCRNL(char *NMEA, uint8_t Len) { return NMEA_Appen
|
|||
if(Data[4]!='S') return 0;
|
||||
return Data[5]=='A'; }
|
||||
|
||||
uint8_t isGxGSV(void) const // GPS satellite data
|
||||
{ if(!isGx()) return 0;
|
||||
if(Data[3]!='G') return 0;
|
||||
if(Data[4]!='S') return 0;
|
||||
return Data[5]=='V'; }
|
||||
|
||||
uint8_t isGPGSV(void) const // GPS satellite data
|
||||
{ if(!isGP()) return 0;
|
||||
if(Data[3]!='G') return 0;
|
||||
if(Data[4]!='S') return 0;
|
||||
return Data[5]=='V'; }
|
||||
|
||||
uint8_t isGNGSV(void) const // GPS satellite data
|
||||
{ if(!isGN()) return 0;
|
||||
if(Data[3]!='G') return 0;
|
||||
if(Data[4]!='S') return 0;
|
||||
return Data[5]=='V'; }
|
||||
|
||||
uint8_t isGPTXT(void) const // GPS satellite data
|
||||
{ if(!isGP()) return 0;
|
||||
if(Data[3]!='T') return 0;
|
||||
|
|
|
|||
174
main/ogn.h
174
main/ogn.h
|
|
@ -22,9 +22,13 @@
|
|||
|
||||
#include "format.h"
|
||||
|
||||
#include "ognconv.h"
|
||||
|
||||
#include "ogn1.h" // OGN v1
|
||||
#include "ogn2.h" // OGN v2
|
||||
|
||||
#include "atmosphere.h"
|
||||
|
||||
// ---------------------------------------------------------------------------------------------------------------------
|
||||
|
||||
template <class OGNx_Packet, class OGNy_Packet>
|
||||
|
|
@ -607,8 +611,9 @@ template<class OGNx_Packet, uint8_t Size=8>
|
|||
|
||||
void cleanTime(uint8_t Time) // clean up slots of given Time
|
||||
{ for(int Idx=0; Idx<Size; Idx++)
|
||||
{ if( (Packet[Idx].Rank) && (Packet[Idx].Packet.Position.Time==Time) )
|
||||
{ clean(Idx); }
|
||||
{ if(Packet[Idx].Rank==0) continue;
|
||||
uint8_t PktTime=Packet[Idx].Packet.Position.Time;
|
||||
if( PktTime==Time || PktTime>=60) clean(Idx);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -624,15 +629,15 @@ template<class OGNx_Packet, uint8_t Size=8>
|
|||
|
||||
uint8_t Print(char *Out)
|
||||
{ uint8_t Len=0;
|
||||
for(uint8_t Idx=0; Idx<Size; Idx++)
|
||||
for(uint8_t Idx=0; Idx<Size; Idx++) // loop through the slots
|
||||
{ uint8_t Rank=Packet[Idx].Rank;
|
||||
Out[Len++]=' '; Len+=Format_Hex(Out+Len, Rank);
|
||||
if(Rank)
|
||||
{ Out[Len++]='/'; Len+=Format_Hex(Out+Len, Packet[Idx].Packet.getAddressAndType() );
|
||||
Out[Len++]=':'; Len+=Format_UnsDec(Out+Len, Packet[Idx].Packet.Position.Time, 2 ); }
|
||||
Out[Len++]=' '; Len+=Format_Hex(Out+Len, Rank); // print the slot Rank
|
||||
if(Rank) // if Rank is none-zero
|
||||
{ Out[Len++]='/'; Len+=Format_Hex(Out+Len, Packet[Idx].Packet.getAddressAndType() ); // print address-type and address
|
||||
Out[Len++]=':'; Len+=Format_UnsDec(Out+Len, Packet[Idx].Packet.Position.Time, 2 ); } // [sec] print time
|
||||
}
|
||||
Out[Len++]=' '; Len+=Format_Hex(Out+Len, Sum);
|
||||
Out[Len++]='/'; Len+=Format_Hex(Out+Len, LowIdx);
|
||||
Out[Len++]=' '; Len+=Format_Hex(Out+Len, Sum); // sum of all Ranks
|
||||
Out[Len++]='/'; Len+=Format_Hex(Out+Len, LowIdx); // index of the lowest Rank or a free slot
|
||||
Out[Len++]='\n'; Out[Len]=0; return Len; }
|
||||
|
||||
} ;
|
||||
|
|
@ -641,20 +646,24 @@ class GPS_Position
|
|||
{ public:
|
||||
|
||||
union
|
||||
{ uint8_t Flags; // bit #0 = GGA and RMC had same Time
|
||||
{ uint8_t Flags; // bit #0 = GGA and RMC had same Time
|
||||
struct
|
||||
{ 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 hasHum :1; //
|
||||
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
|
||||
// bool hasHum :1; //
|
||||
// bool hasGSV :1;
|
||||
} ;
|
||||
} ;
|
||||
|
||||
// uint16_t SatSNRsum; // sum of cSNR from GPGSV
|
||||
// uint8_t SatSNRcount; // count of satellites from GPGSV
|
||||
|
||||
int8_t FixQuality; // 0 = none, 1 = GPS, 2 = Differential GPS (can be WAAS)
|
||||
int8_t FixMode; // 0 = not set (from GSA) 1 = none, 2 = 2-D, 3 = 3-D
|
||||
int8_t Satellites; // number of active satellites
|
||||
|
|
@ -676,7 +685,7 @@ class GPS_Position
|
|||
int16_t GeoidSeparation; // [0.1 meter] difference between Geoid and Ellipsoid
|
||||
int32_t Altitude; // [0.1 meter] height above Geoid (sea level)
|
||||
|
||||
int32_t Latitude; // [0.0001/60 deg] about 0.018m accuracy (to convert to u-Blox GPS 1e-7deg units mult by 50/3)
|
||||
int32_t Latitude; // [0.0001/60 deg] about 0.18m accuracy (to convert to u-Blox GPS 1e-7deg units mult by 50/3)
|
||||
int32_t Longitude; // [0.0001/60 deg]
|
||||
uint16_t LatitudeCosine; // [2^-12] Latitude cosine for distance calculation
|
||||
|
||||
|
|
@ -684,6 +693,7 @@ class GPS_Position
|
|||
uint32_t Pressure; // [0.25 Pa] from pressure sensor
|
||||
int32_t StdAltitude; // [0.1 meter] standard pressure altitude (from the pressure sensor and atmosphere calculator)
|
||||
int16_t Humidity; // [0.1%] relative humidity
|
||||
int16_t Accel; // [0.1m/s^2] acceleration along the track
|
||||
|
||||
public:
|
||||
|
||||
|
|
@ -692,11 +702,12 @@ class GPS_Position
|
|||
void Clear(void)
|
||||
{ Flags=0; FixQuality=0; FixMode=0;
|
||||
PDOP=0; HDOP=0; VDOP=0;
|
||||
// SatSNRsum=0; SatSNRcount=0;
|
||||
setDefaultDate(); setDefaultTime();
|
||||
Latitude=0; Longitude=0; LatitudeCosine=3000;
|
||||
Altitude=0; GeoidSeparation=0;
|
||||
Speed=0; Heading=0; ClimbRate=0; TurnRate=0;
|
||||
Temperature=0; Pressure=0; StdAltitude=0; }
|
||||
Temperature=0; Pressure=0; StdAltitude=0; Humidity=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
|
||||
|
|
@ -779,7 +790,8 @@ class GPS_Position
|
|||
printf("FixQuality/Mode=%d/%d: %d satellites DOP/H/V=%3.1f/%3.1f/%3.1f ", FixQuality, FixMode, Satellites, 0.1*PDOP, 0.1*HDOP, 0.1*VDOP);
|
||||
printf("FixQuality=%d: %d satellites HDOP=%3.1f ", FixQuality, Satellites, 0.1*HDOP);
|
||||
printf("Lat/Lon/Alt = [%+10.6f,%+10.6f]deg %+3.1f(%+3.1f)m LatCosine=%+6.3f ", 0.0001/60*Latitude, 0.0001/60*Longitude, 0.1*Altitude, 0.1*GeoidSeparation, 1.0/(1<<12)*LatitudeCosine);
|
||||
printf("Speed/Heading = %3.1fm/s %05.1fdeg\n", 0.1*Speed, 0.1*Heading);
|
||||
printf("Speed/Heading = %3.1fm/s %05.1fdeg ", 0.1*Speed, 0.1*Heading);
|
||||
printf("Climb = %+5.1fm/s Turn = %+5.1fdeg/sec\n", 0.1*ClimbRate, 0.1*TurnRate);
|
||||
}
|
||||
|
||||
int Print(char *Out) const
|
||||
|
|
@ -820,8 +832,8 @@ class GPS_Position
|
|||
Out[Len++]='/'; Len+=Format_UnsDec(Out+Len, HDOP, 2, 1);
|
||||
Out[Len++]='/'; Len+=Format_UnsDec(Out+Len, VDOP, 2, 1);
|
||||
Out[Len++]=' ';
|
||||
Out[Len++]='['; Len+=Format_SignDec(Out+Len, Latitude/60, 6, 4);
|
||||
Out[Len++]=','; Len+=Format_SignDec(Out+Len, Longitude/60, 7, 4);
|
||||
Out[Len++]='['; Len+=Format_SignDec(Out+Len, Latitude/6, 7, 5);
|
||||
Out[Len++]=','; Len+=Format_SignDec(Out+Len, Longitude/6, 8, 5);
|
||||
Out[Len++]=']'; Out[Len++]='d'; Out[Len++]='e'; Out[Len++]='g';
|
||||
Out[Len++]=' '; Len+=Format_SignDec(Out+Len, Altitude, 4, 1); Out[Len++]='m';
|
||||
Out[Len++]='/'; Len+=Format_SignDec(Out+Len, GeoidSeparation, 4, 1); Out[Len++]='m';
|
||||
|
|
@ -878,6 +890,7 @@ class GPS_Position
|
|||
if(RxMsg.isGNRMC()) return ReadRMC(RxMsg);
|
||||
if(RxMsg.isGPGSA()) return ReadGSA(RxMsg);
|
||||
if(RxMsg.isGNGSA()) return ReadGSA(RxMsg);
|
||||
// if(RxMsg.isGxGSV()) return ReadGSV(RxMsg);
|
||||
return 0; }
|
||||
|
||||
int8_t ReadNMEA(const char *NMEA)
|
||||
|
|
@ -885,6 +898,7 @@ class GPS_Position
|
|||
Err=ReadGGA(NMEA); if(Err!=(-1)) return Err;
|
||||
Err=ReadGSA(NMEA); if(Err!=(-1)) return Err;
|
||||
Err=ReadRMC(NMEA); if(Err!=(-1)) return Err;
|
||||
// Err=ReadGSV(NMEA); if(Err!=(-1)) return Err;
|
||||
return 0; }
|
||||
|
||||
int8_t ReadGGA(NMEA_RxMsg &RxMsg)
|
||||
|
|
@ -899,9 +913,43 @@ class GPS_Position
|
|||
ReadLongitude(*RxMsg.ParmPtr(4), (const char *)RxMsg.ParmPtr(3)); // Longitude
|
||||
ReadAltitude(*RxMsg.ParmPtr(9), (const char *)RxMsg.ParmPtr(8)); // Altitude
|
||||
ReadGeoidSepar(*RxMsg.ParmPtr(11), (const char *)RxMsg.ParmPtr(10)); // Geoid separation
|
||||
// calcLatitudeCosine();
|
||||
calcLatitudeCosine();
|
||||
return 1; }
|
||||
|
||||
uint8_t WriteGGA(char *GGA)
|
||||
{ uint8_t Len=0;
|
||||
Len+=Format_String(GGA+Len, "$GPGGA,");
|
||||
Len+=Format_UnsDec(GGA+Len, (uint16_t)Hour, 2);
|
||||
Len+=Format_UnsDec(GGA+Len, (uint16_t)Min, 2);
|
||||
Len+=Format_UnsDec(GGA+Len, (uint16_t)Sec, 2);
|
||||
GGA[Len++]='.';
|
||||
Len+=Format_UnsDec(GGA+Len, (uint16_t)FracSec, 2);
|
||||
GGA[Len++]=',';
|
||||
Len+=Format_Latitude(GGA+Len, Latitude);
|
||||
GGA[Len]=GGA[Len-1]; GGA[Len-1]=','; Len++;
|
||||
GGA[Len++]=',';
|
||||
Len+=Format_Longitude(GGA+Len, Longitude);
|
||||
GGA[Len]=GGA[Len-1]; GGA[Len-1]=','; Len++;
|
||||
GGA[Len++]=',';
|
||||
GGA[Len++]='0'+FixQuality;
|
||||
GGA[Len++]=',';
|
||||
Len+=Format_UnsDec(GGA+Len, (uint16_t)Satellites);
|
||||
GGA[Len++]=',';
|
||||
Len+=Format_UnsDec(GGA+Len, (uint16_t)HDOP, 2, 1);
|
||||
GGA[Len++]=',';
|
||||
Len+=Format_SignDec(GGA+Len, Altitude, 3, 1);
|
||||
GGA[Len++]=',';
|
||||
GGA[Len++]='M';
|
||||
GGA[Len++]=',';
|
||||
Len+=Format_SignDec(GGA+Len, GeoidSeparation, 3, 1);
|
||||
GGA[Len++]=',';
|
||||
GGA[Len++]='M';
|
||||
GGA[Len++]=',';
|
||||
GGA[Len++]=',';
|
||||
Len += NMEA_AppendCheckCRNL(GGA, Len);
|
||||
GGA[Len]=0;
|
||||
return Len; }
|
||||
|
||||
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
|
||||
|
|
@ -915,7 +963,7 @@ class GPS_Position
|
|||
ReadLongitude(GGA[Index[4]], GGA+Index[3]); // Longitude
|
||||
ReadAltitude(GGA[Index[9]], GGA+Index[8]); // Altitude
|
||||
ReadGeoidSepar(GGA[Index[11]], GGA+Index[10]); // Geoid separation
|
||||
// calcLatitudeCosine();
|
||||
calcLatitudeCosine();
|
||||
return 1; }
|
||||
|
||||
int8_t ReadGSA(NMEA_RxMsg &RxMsg)
|
||||
|
|
@ -934,7 +982,17 @@ class GPS_Position
|
|||
ReadHDOP(GSA+Index[15]);
|
||||
ReadVDOP(GSA+Index[16]);
|
||||
return 1; }
|
||||
/*
|
||||
int8_t ReadGSV(NMEA_RxMsg &RxMsg)
|
||||
{ //
|
||||
return 1; }
|
||||
|
||||
int8_t ReadGSV(const char *GSV)
|
||||
{ if( (memcmp(GSV, "$GPGSV", 6)!=0) && (memcmp(GSV, "$GNGSV", 6)!=0) ) return -1; // check if the right sequence
|
||||
uint8_t Index[24]; if(IndexNMEA(Index, GSV)<20) return -2; // index parameters and check the sum
|
||||
//
|
||||
return 1; }
|
||||
*/
|
||||
int ReadRMC(NMEA_RxMsg &RxMsg)
|
||||
{ if(RxMsg.Parms<12) return -1; // no less than 12 parameters
|
||||
hasGPS = ReadTime((const char *)RxMsg.ParmPtr(0))>0; // read time and check if same as the GGA says
|
||||
|
|
@ -964,7 +1022,7 @@ class GPS_Position
|
|||
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
|
||||
int16_t calcDifferentials(GPS_Position &RefPos) // calculate climb rate and turn rate with an earlier reference position
|
||||
{ ClimbRate=0; TurnRate=0;
|
||||
if(RefPos.FixQuality==0) return 0;
|
||||
int16_t TimeDiff = calcTimeDiff(RefPos);
|
||||
|
|
@ -974,19 +1032,29 @@ class GPS_Position
|
|||
ClimbRate = Altitude-RefPos.Altitude;
|
||||
if(hasBaro && RefPos.hasBaro && (abs(Altitude-StdAltitude)<2500) )
|
||||
{ ClimbRate = StdAltitude-RefPos.StdAltitude; }
|
||||
if(TimeDiff==100)
|
||||
Accel = Speed-RefPos.Speed;
|
||||
if(TimeDiff==20)
|
||||
{ ClimbRate*=5;
|
||||
TurnRate *=5;
|
||||
Accel *=5; }
|
||||
else if(TimeDiff==50)
|
||||
{ ClimbRate*=2;
|
||||
TurnRate *=2;
|
||||
Accel *=2; }
|
||||
else if(TimeDiff==100)
|
||||
{ }
|
||||
else if(TimeDiff==200)
|
||||
{ ClimbRate=(ClimbRate+1)>>1;
|
||||
TurnRate=(TurnRate+1)>>1; }
|
||||
else
|
||||
TurnRate=( TurnRate+1)>>1;
|
||||
Accel =( Accel +1)>>1; }
|
||||
else if(TimeDiff!=0)
|
||||
{ ClimbRate = ((int32_t)ClimbRate*100)/TimeDiff;
|
||||
TurnRate = ((int32_t)TurnRate *100)/TimeDiff; }
|
||||
TurnRate = ((int32_t) TurnRate*100)/TimeDiff;
|
||||
Accel = ((int32_t) Accel *100)/TimeDiff; }
|
||||
return TimeDiff; } // [0.01s]
|
||||
|
||||
void Write(MAV_GPS_RAW_INT *MAV) const
|
||||
// { MAV->time_usec = (int64_t)1000000*getUnixTime()+10000*FracSec;
|
||||
{ MAV->time_usec = getUnixTime_ms()*1000; // (int64_t)1000000*getUnixTime()+10000*FracSec;
|
||||
{ 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;
|
||||
|
|
@ -1080,12 +1148,10 @@ class GPS_Position
|
|||
if(hasBaro)
|
||||
{ Packet.EncodeTemperature(Temperature);
|
||||
Packet.Status.Pressure = (Pressure+16)>>5;
|
||||
Packet.EncodeHumidity(Humidity);
|
||||
}
|
||||
Packet.EncodeHumidity(Humidity); }
|
||||
else
|
||||
{ Packet.Status.Pressure = 0;
|
||||
Packet.clrHumidity();
|
||||
}
|
||||
Packet.clrHumidity(); }
|
||||
}
|
||||
|
||||
// uint8_t getFreqPlan(void) const // get the frequency plan from Lat/Lon: 1 = Europe + Africa, 2 = USA/CAnada, 3 = Australia + South America, 4 = New Zeeland
|
||||
|
|
@ -1120,12 +1186,37 @@ class GPS_Position
|
|||
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
|
||||
void Extrapolate(int32_t dTime) // [0.01sec] extrapolate the position by dTime
|
||||
{ int16_t dSpeed = ((int32_t)Accel*dTime)/100;
|
||||
Speed += dSpeed/2;
|
||||
int16_t HeadAngle = ((int32_t)Heading<<12)/225; // [cordic] heading angle
|
||||
int16_t TurnAngle = (((dTime*TurnRate)/25)<<9)/225; // [cordic]
|
||||
HeadAngle += TurnAngle/2;
|
||||
int32_t LatSpeed = ((int32_t)Speed*Icos(HeadAngle)+0x800)>>12; // [0.1m/s]
|
||||
int32_t LonSpeed = ((int32_t)Speed*Isin(HeadAngle)+0x800)>>12; // [0.1m/s]
|
||||
HeadAngle += TurnAngle-TurnAngle/2;
|
||||
Speed += dSpeed-dSpeed/2; if(Speed<0) Speed=0;
|
||||
Latitude += calcLatitudeExtrapolation (dTime, LatSpeed);
|
||||
Longitude += calcLongitudeExtrapolation(dTime, LonSpeed);
|
||||
int32_t dAlt = calcAltitudeExtrapolation(dTime); // [0.1m]
|
||||
Altitude += dAlt; // [0.1m]
|
||||
if(hasBaro)
|
||||
{ StdAltitude += dAlt; // [0.1m]
|
||||
Pressure += 4000*dAlt/Atmosphere::PressureLapseRate(Pressure/4, Temperature); } // [0.25Pa] ([Pa], [0.1degC])
|
||||
Heading += (dTime*TurnRate)/100; // [0.1deg]
|
||||
if(Heading<0) Heading+=3600; else if(Heading>=3600) Heading-=3600; // [0.1deg]
|
||||
int16_t fTime = FracSec+dTime; // [0.01sec]
|
||||
while(fTime>=100) { incrTimeDate(); fTime-=100; }
|
||||
while(fTime< 0) { decrTimeDate(); fTime+=100; }
|
||||
FracSec=fTime; }
|
||||
|
||||
// extrapolate GPS position by a fraction of a second
|
||||
void calcExtrapolation(int32_t &Lat, int32_t &Lon, int32_t &Alt, int16_t &Head, int32_t dTime) const // [0.01sec]
|
||||
{ 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]
|
||||
int32_t LatSpeed = ((int32_t)Speed*Icos(HeadAngle)+0x800)>>12; // [0.1m/s]
|
||||
int32_t LonSpeed = ((int32_t)Speed*Isin(HeadAngle)+0x800)>>12; // [0.1m/s]
|
||||
Lat = Latitude + calcLatitudeExtrapolation (dTime, LatSpeed);
|
||||
Lon = Longitude + calcLongitudeExtrapolation(dTime, LonSpeed);
|
||||
Alt = Altitude + calcAltitudeExtrapolation(dTime);
|
||||
|
|
@ -1135,15 +1226,15 @@ class GPS_Position
|
|||
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 calcLatitudeExtrapolation(int32_t Time, int32_t LatSpeed) const // [0.01s] [0.1m/s]
|
||||
{ return (Time*LatSpeed*177+0x4000)>>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; }
|
||||
{ return ((((int32_t)Time*LonSpeed*177+4)>>3))/LatCosine; }
|
||||
|
||||
// static int32_t calcLatDistance(int32_t Lat1, int32_t Lat2) // [m] distance along latitude
|
||||
// { return ((int64_t)(Lat2-Lat1)*0x2f684bda+0x80000000)>>32; }
|
||||
|
|
@ -1173,7 +1264,7 @@ class GPS_Position
|
|||
// // printf("Latitude=%+d, LatAngle=%04X LatCos=%08X\n", Latitude, (uint16_t)LatAngle, LatCos);
|
||||
// return ((int64_t)Dist*LatCos+0x40000000)>>31; } // distance corrected by the latitude cosine
|
||||
|
||||
void calcLatitudeCosine(void)
|
||||
void calcLatitudeCosine(void)
|
||||
{ int16_t LatAngle = calcLatAngle16(Latitude);
|
||||
LatitudeCosine = calcLatCosine(LatAngle); }
|
||||
|
||||
|
|
@ -1288,10 +1379,13 @@ class GPS_Position
|
|||
// printf("%s => [%d]\n", Seq, Params);
|
||||
return Params; }
|
||||
|
||||
uint32_t getDayTime(void) const
|
||||
{ return Times60((uint32_t)(Times60((uint16_t)Hour) + Min)) + Sec; } // this appears to save about 100 bytes of code
|
||||
// return (uint32_t)Hour*SecsPerHour + (uint16_t)Min*SecsPerMin + Sec; } // compared to this line
|
||||
|
||||
uint32_t getUnixTime(void) const // return the Unix timestamp (tested 2000-2037)
|
||||
{ uint16_t Days = DaysSinceYear2000() + DaysSimce1jan();
|
||||
return Times60(Times60(Times24((uint32_t)(Days+10957)))) + Times60((uint32_t)(Times60((uint16_t)Hour) + Min)) + Sec; } // this appears to save about 100 bytes of code
|
||||
// return (uint32_t)(Days+10957)*SecsPerDay + (uint32_t)Hour*SecsPerHour + (uint16_t)Min*SecsPerMin + Sec; } // compared to this line
|
||||
return Times60(Times60(Times24((uint32_t)(Days+10957)))) + getDayTime(); }
|
||||
|
||||
uint32_t getFatTime(void) const // return timestamp in FAT format
|
||||
{ uint16_t Date = ((uint16_t)(Year+20)<<9) | ((uint16_t)Month<<5) | Day;
|
||||
|
|
@ -1319,10 +1413,6 @@ class GPS_Position
|
|||
setUnixTime(Time);
|
||||
FracSec = (Time_ms-(uint64_t)Time*1000)/10; }
|
||||
|
||||
uint64_t getUnixTime_ms(void) const
|
||||
{ return (uint64_t)getUnixTime()*1000 + (uint32_t)FracSec*10; }
|
||||
|
||||
|
||||
private:
|
||||
|
||||
static const uint32_t SecsPerMin = 60;
|
||||
|
|
|
|||
21
main/ogn1.h
21
main/ogn1.h
|
|
@ -85,7 +85,8 @@ class OGN1_Packet // Packet structure for the OGN tracker
|
|||
struct
|
||||
{ unsigned int Pulse : 8; // [bpm] // pilot: heart pulse rate
|
||||
unsigned int Oxygen : 7; // [%] // pilot: oxygen level in the blood
|
||||
unsigned int FEScurr : 5; // [A] // FES current
|
||||
unsigned int SatSNR : 5; // [dB] // average SNR of GPS signals
|
||||
// unsigned int FEScurr : 5; // [A] // FES current
|
||||
unsigned int RxRate : 4; // [/min] // log2 of received packet rate
|
||||
unsigned int Time : 6; // [sec] // same as in the position packet
|
||||
unsigned int FixQuality: 2;
|
||||
|
|
@ -98,7 +99,7 @@ class OGN1_Packet // Packet structure for the OGN tracker
|
|||
unsigned int Satellites: 4; // [ ]
|
||||
unsigned int Firmware : 8; // [ ] // firmware version
|
||||
unsigned int Hardware : 8; // [ ] // hardware version
|
||||
unsigned int TxPower : 4; // [dBm] // RF trancmitter power
|
||||
unsigned int TxPower : 4; // [dBm] // RF trancmitter power (offset = 4)
|
||||
unsigned int ReportType: 4; // [0] // 0 for the status report
|
||||
unsigned int Voltage : 8; // [1/64V] VR // supply/battery voltage
|
||||
} Status;
|
||||
|
|
@ -138,10 +139,10 @@ class OGN1_Packet // Packet structure for the OGN tracker
|
|||
|
||||
// void recvBytes(const uint8_t *SrcPacket) { memcpy(Byte(), SrcPacket, Bytes); } // load data bytes e.g. from a demodulator
|
||||
|
||||
static const uint8_t InfoParmNum = 12; // [int] number of info-parameters and their names
|
||||
static const uint8_t InfoParmNum = 14; // [int] number of info-parameters and their names
|
||||
static const char *InfoParmName(uint8_t Idx) { static const char *Name[InfoParmNum] =
|
||||
{ "Pilot", "Manuf", "Model", "Type", "SN", "Reg", "ID", "Class",
|
||||
"Task" , "Base" , "ICE" , "PilotID" } ;
|
||||
"Task" , "Base" , "ICE" , "PilotID", "Hard", "Soft" } ;
|
||||
return Idx<InfoParmNum ? Name[Idx]:0; }
|
||||
|
||||
#ifndef __AVR__
|
||||
|
|
@ -226,8 +227,8 @@ class OGN1_Packet // Packet structure for the OGN tracker
|
|||
0.1*DecodeSpeed(), 0.1*DecodeHeading(), 0.1*DecodeClimbRate(), 0.1*DecodeTurnRate() );
|
||||
printf("\n");
|
||||
}
|
||||
|
||||
/* void Encode(MAV_ADSB_VEHICLE *MAV)
|
||||
/*
|
||||
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;
|
||||
|
|
@ -241,7 +242,6 @@ class OGN1_Packet // Packet structure for the OGN tracker
|
|||
MAV->tslc = 0;
|
||||
MAV->emiter_type = 0; }
|
||||
*/
|
||||
|
||||
void Encode(MAV_ADSB_VEHICLE *MAV)
|
||||
{ MAV->ICAO_address = Header.Address;
|
||||
MAV->lat = ((int64_t)50*DecodeLatitude()+1)/3; // convert coordinates to [1e-7deg]
|
||||
|
|
@ -465,8 +465,8 @@ class OGN1_Packet // Packet structure for the OGN tracker
|
|||
Msg[Len++] = ' ';
|
||||
Msg[Len++] = '!';
|
||||
Msg[Len++] = 'W';
|
||||
Msg[Len++] = '0'+(Lat+5)/10;
|
||||
Msg[Len++] = '0'+(Lon+5)/10;
|
||||
Msg[Len++] = '0'+Lat/10;
|
||||
Msg[Len++] = '0'+Lon/10;
|
||||
Msg[Len++] = '!';
|
||||
|
||||
Msg[Len++] = ' '; Msg[Len++] = 'i'; Msg[Len++] = 'd'; Len+=Format_Hex(Msg+Len, ((uint32_t)Position.AcftType<<26) | ((uint32_t)Header.AddrType<<24) | Header.Address);
|
||||
|
|
@ -727,6 +727,9 @@ class OGN1_Packet // Packet structure for the OGN tracker
|
|||
|
||||
// --------------------------------------------------------------------------------------------------------------
|
||||
|
||||
void Encrypt (const uint32_t Key[4]) { XXTEA_Encrypt(Data, 4, Key, 8); } // encrypt with given Key
|
||||
void Decrypt (const uint32_t Key[4]) { XXTEA_Decrypt(Data, 4, Key, 8); } // decrypt with given Key
|
||||
|
||||
void Whiten (void) { TEA_Encrypt_Key0(Data, 8); TEA_Encrypt_Key0(Data+2, 8); } // whiten the position
|
||||
void Dewhiten(void) { TEA_Decrypt_Key0(Data, 8); TEA_Decrypt_Key0(Data+2, 8); } // de-whiten the position
|
||||
|
||||
|
|
|
|||
13
main/ogn2.h
13
main/ogn2.h
|
|
@ -40,13 +40,13 @@ class OGN2_Packet // Packet structure for the OGN tracker
|
|||
unsigned int Relay : 1; // 0 = direct packet, 1 = relayed packet
|
||||
unsigned int Parity : 1; // parity takes into account bits 0..27 thus only the 28 lowest bits
|
||||
unsigned int NonPos : 1; // 0 = position packet, 1 = other information like status
|
||||
unsigned int Auth : 2; // Authentication: 00 = no auth. 01 = auth. this packet, 10/11 = auth response
|
||||
// unsigned int Auth : 2; // Authentication: 00 = no auth. 01 = auth. this packet, 10/11 = auth response
|
||||
// Auth:NonPos: 000 = position, 010 = position, to be followed by crypto-response,
|
||||
// 101 = response #0, 111 = response #1
|
||||
// 001 = non-position: status, info, etc.
|
||||
// 100 = ???, 110 = ???, 011 = ???
|
||||
// unsigned int NonOGN : 1; // 0 = OGN packet, 1 = other systems, like MAVlink
|
||||
// unsigned int Encrypted : 1; // packet is encrypted
|
||||
unsigned int NonOGN : 1; // 0 = OGN packet, 1 = other systems, like MAVlink
|
||||
unsigned int Encrypted : 1; // packet is encrypted
|
||||
unsigned int Emergency : 1; // aircraft in emergency (not used for now)
|
||||
} Header ;
|
||||
|
||||
|
|
@ -89,7 +89,8 @@ class OGN2_Packet // Packet structure for the OGN tracker
|
|||
|
||||
unsigned int Pulse : 8; // [bpm] // pilot: heart pulse rate
|
||||
unsigned int Oxygen : 7; // [%] // pilot: oxygen level in the blood
|
||||
unsigned int FEScurr : 5; // [A] //
|
||||
// unsigned int FEScurr : 5; // [A] //
|
||||
unsigned int SatSNR : 5; // [dB] // average SNR of GPS signals
|
||||
unsigned int RxRate : 4; // [/min] // log2 of received packet rate
|
||||
unsigned int Time : 6; // [sec] // same as in the position packet
|
||||
unsigned int FixQuality: 2;
|
||||
|
|
@ -115,10 +116,10 @@ class OGN2_Packet // Packet structure for the OGN tracker
|
|||
uint8_t *Byte(void) const { return (uint8_t *)&HeaderWord; } // packet as bytes
|
||||
uint32_t *Word(void) const { return (uint32_t *)&HeaderWord; } // packet as words
|
||||
|
||||
static const uint8_t InfoParmNum = 12; // [int] number of info-parameters and their names
|
||||
static const uint8_t InfoParmNum = 14; // [int] number of info-parameters and their names
|
||||
static const char *InfoParmName(uint8_t Idx) { static const char *Name[InfoParmNum] =
|
||||
{ "Pilot", "Manuf", "Model", "Type", "SN", "Reg", "ID", "Class",
|
||||
"Task" , "Base" , "ICE" , "PilotID" } ;
|
||||
"Task" , "Base" , "ICE" , "PilotID", "Hard", "Soft" } ;
|
||||
return Idx<InfoParmNum ? Name[Idx]:0; }
|
||||
|
||||
void Dump(void) const
|
||||
|
|
|
|||
|
|
@ -145,6 +145,9 @@ uint32_t DecodeGray(uint32_t Gray)
|
|||
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
|
||||
|
|
@ -188,6 +191,42 @@ void TEA_Decrypt_Key0 (uint32_t* Data, int Loops)
|
|||
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
|
||||
|
|
@ -202,5 +241,31 @@ void xorshift64(uint64_t &Seed)
|
|||
|
||||
// ==============================================================================================
|
||||
|
||||
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; }
|
||||
|
||||
// ==============================================================================================
|
||||
|
||||
|
|
|
|||
|
|
@ -73,7 +73,13 @@ void TEA_Decrypt (uint32_t* Data, const uint32_t *Key, int Loops);
|
|||
void TEA_Encrypt_Key0 (uint32_t* Data, int Loops);
|
||||
void TEA_Decrypt_Key0 (uint32_t* Data, int Loops);
|
||||
|
||||
void XXTEA_Encrypt(uint32_t *Data, uint8_t Words, const uint32_t Key[4], uint8_t Loops);
|
||||
void XXTEA_Decrypt(uint32_t *Data, uint8_t Words, const uint32_t Key[4], uint8_t Loops);
|
||||
|
||||
void XorShift32(uint32_t &Seed); // simple random number generator
|
||||
void xorshift64(uint64_t &Seed);
|
||||
|
||||
uint8_t EncodeAscii85( char *Ascii, uint32_t Word ); // Encode 32-bit Word into 5-char Ascii-85 string
|
||||
uint8_t DecodeAscii85(uint32_t &Word, const char *Ascii); // Decode 5-char Ascii-85 to 32-bit Word
|
||||
|
||||
#endif // __OGNCONV_H__
|
||||
|
|
|
|||
|
|
@ -14,6 +14,10 @@
|
|||
#include "nvs.h"
|
||||
#endif
|
||||
|
||||
#ifdef WITH_SAMD21
|
||||
#include "flashsize.h"
|
||||
#endif
|
||||
|
||||
#ifdef WITH_STM32
|
||||
#include "stm32f10x_flash.h"
|
||||
#include "flashsize.h"
|
||||
|
|
@ -29,8 +33,8 @@ class FlashParameters
|
|||
{ uint32_t AcftID; // identification: Private:AcftType:AddrType:Address - must be different for every tracker
|
||||
struct
|
||||
{ uint32_t Address:24; // address (ID)
|
||||
uint8_t AddrType:2;
|
||||
uint8_t AcftType:4;
|
||||
uint8_t AddrType:2; // 0=RND, 1=ICAO, 2=FLR, 3=OGN
|
||||
uint8_t AcftType:4; // 1=glider, 2=towplane, 3=helicopter, etc.
|
||||
bool NoTrack:1; // unused
|
||||
bool Stealth:1; // unused
|
||||
} ;
|
||||
|
|
@ -44,15 +48,19 @@ class FlashParameters
|
|||
|
||||
int16_t PressCorr; // [0.25Pa] pressure correction for the baro
|
||||
union
|
||||
{ uint8_t Flags;
|
||||
{ uint16_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
|
||||
bool manGeoidSepar:1; // GeoidSepar is manually configured as the GPS or MAVlink are not able to deliver it
|
||||
bool Encrypt:1; // encrypt the position
|
||||
uint8_t NavMode:3; // GPS navigation mode/model
|
||||
uint8_t NavRate:2; // [Hz]
|
||||
uint8_t Verbose:2; //
|
||||
int8_t TimeCorr:4; // [sec] it appears for ArduPilot you need to correct time by 3 seconds
|
||||
} ;
|
||||
} ; //
|
||||
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)
|
||||
// or it could be a problem of some GPSes
|
||||
|
|
@ -60,7 +68,7 @@ class FlashParameters
|
|||
uint8_t FreqPlan; // force given frequency hopping plan
|
||||
|
||||
static const uint8_t InfoParmLen = 16; // [char] max. size of an infp-parameter
|
||||
static const uint8_t InfoParmNum = 12; // [int] number of info-parameters
|
||||
static const uint8_t InfoParmNum = 14; // [int] number of info-parameters
|
||||
char *InfoParmValue(uint8_t Idx) { return Idx<InfoParmNum ? Pilot + Idx*InfoParmLen:0; }
|
||||
uint8_t InfoParmValueLen(uint8_t Idx) { return strlen(InfoParmValue(Idx)); }
|
||||
// const char *InfoParmName(uint8_t Idx) const { static const char *Name[InfoParmNum] =
|
||||
|
|
@ -79,6 +87,8 @@ class FlashParameters
|
|||
char Base[InfoParmLen]; // Base airfield
|
||||
char ICE[InfoParmLen]; // In Case of Emergency
|
||||
char PilotID[InfoParmLen]; // Pilot ID based on his BT or WiFi MAC
|
||||
char Hard[InfoParmLen]; // Hardware
|
||||
char Soft[InfoParmLen]; // Software
|
||||
|
||||
// char Copilot[16]
|
||||
// char Category[16]
|
||||
|
|
@ -97,7 +107,15 @@ class FlashParameters
|
|||
|
||||
char WIFIname[WIFIsets][32];
|
||||
char WIFIpass[WIFIsets][64];
|
||||
#endif
|
||||
|
||||
#ifdef WITH_ENCRYPT
|
||||
uint32_t EncryptKey[4]; // encryption key
|
||||
#endif
|
||||
|
||||
uint32_t CheckSum;
|
||||
|
||||
#ifdef WITH_WIFI
|
||||
const char *getWIFIpass(const char *NetName) const
|
||||
{ for(uint8_t Idx=0; Idx<WIFIsets; Idx++)
|
||||
{ if(strcmp(NetName, WIFIname[Idx])==0) return WIFIpass[Idx]; }
|
||||
|
|
@ -116,6 +134,18 @@ class FlashParameters
|
|||
|
||||
static const uint32_t CheckInit = 0x89ABCDEF;
|
||||
|
||||
uint32_t static calcCheckSum(volatile 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 calcCheckSum(void) const // calc. check-sum of this class data
|
||||
{ return calcCheckSum((volatile uint32_t *)this, sizeof(FlashParameters)/sizeof(uint32_t) ); }
|
||||
|
||||
void setCheckSum(void) { CheckSum -= calcCheckSum(); }
|
||||
bool goodCheckSum(void) const { return calcCheckSum()==0; }
|
||||
|
||||
uint8_t getAprsCall(char *Call)
|
||||
{ const char *AddrTypeName[4] = { "RND", "ICA", "FLR", "OGN" };
|
||||
memcpy(Call, AddrTypeName[AddrType], 3);
|
||||
|
|
@ -136,6 +166,17 @@ class FlashParameters
|
|||
#else
|
||||
RFchipTxPower = 0x80 | 14; // [dBm] for RFM69HW
|
||||
#endif
|
||||
|
||||
Flags = 0;
|
||||
#ifdef WITH_GPS_UBX
|
||||
NavMode = 6; // Avionic mode 1g for UBX
|
||||
#endif
|
||||
#ifdef WITH_GPS_MTK
|
||||
NavMode = 2; // Avionic mode for MTK
|
||||
#endif
|
||||
NavRate = 1; // [Hz]
|
||||
Verbose = 1;
|
||||
|
||||
RFchipTempCorr = 0; // [degC]
|
||||
CONbaud = DEFAULT_CONbaud; // [bps]
|
||||
PressCorr = 0; // [0.25Pa]
|
||||
|
|
@ -144,7 +185,9 @@ class FlashParameters
|
|||
|
||||
FreqPlan = DEFAULT_FreqPlan; // [0..5]
|
||||
PPSdelay = DEFAULT_PPSdelay; // [ms]
|
||||
|
||||
#ifdef WITH_ENCRYPT
|
||||
for(uint8_t Idx=0; Idx<4; Idx++) EncryptKey[Idx]=0;
|
||||
#endif
|
||||
for(uint8_t Idx=0; Idx<InfoParmNum; Idx++)
|
||||
InfoParmValue(Idx)[0] = 0;
|
||||
#ifdef WITH_BT_SPP
|
||||
|
|
@ -192,7 +235,71 @@ class FlashParameters
|
|||
return Err; }
|
||||
#endif // WITH_ESP32
|
||||
|
||||
#ifdef WITH_SAMD21
|
||||
static uint32_t *DefaultFlashAddr(void) { return FlashStart+((uint32_t)(getFlashSizeKB()-1)<<8); } // the last KB
|
||||
|
||||
int8_t ReadFromFlash(volatile uint32_t *Addr=0) // read parameters from Flash
|
||||
{ if(Addr==0) Addr = DefaultFlashAddr(); // default address: the last KB
|
||||
const uint32_t Words=sizeof(FlashParameters)/sizeof(uint32_t);
|
||||
if(calcCheckSum(Addr, Words)!=0) return -1; // agree with the check-sum in Flash ?
|
||||
uint32_t *Dst = (uint32_t *)this;
|
||||
for(uint32_t Idx=0; Idx<Words; Idx++) // read data from Flash
|
||||
{ Dst[Idx] = Addr[Idx]; }
|
||||
return 1; } // return: correct
|
||||
|
||||
bool CompareToFlash(volatile uint32_t *Addr=0) // are the parameters identical to those in the flash ?
|
||||
{ if(Addr==0) Addr = DefaultFlashAddr(); // address in the Flash
|
||||
const uint32_t Words=sizeof(FlashParameters)/sizeof(uint32_t);
|
||||
if(calcCheckSum(Addr, Words)!=0) return 0; // agree with the check-sum in Flash ?
|
||||
uint32_t *Dst = (uint32_t *)this;
|
||||
for(uint32_t Idx=0; Idx<Words; Idx++) // read data from Flash
|
||||
{ if(Dst[Idx] != Addr[Idx]) return 0; }
|
||||
return 1; } // return: correct
|
||||
|
||||
void ErasePage4x64(volatile uint32_t *Addr) const // erase a 4x64 = 256-byte page
|
||||
{ NVMCTRL->ADDR.reg = ((uint32_t)Addr)>>1;
|
||||
NVMCTRL->CTRLA.reg = NVMCTRL_CTRLA_CMDEX_KEY | NVMCTRL_CTRLA_CMD_ER;
|
||||
while (!NVMCTRL->INTFLAG.bit.READY) { }
|
||||
}
|
||||
|
||||
void EraseSize(volatile uint32_t *Addr, uint32_t Size=1024) const // erase multiple pages for given size
|
||||
{ for( ; ; )
|
||||
{ if(Size==0) break;
|
||||
ErasePage4x64(Addr); //
|
||||
if(Size<256) break;
|
||||
Addr+=64; Size-=256; }
|
||||
}
|
||||
|
||||
int WritePage64(volatile uint32_t *Addr, const uint32_t *Data, uint32_t Words) const
|
||||
{ // NVMCTRL->ADDR.reg = ((uint32_t)Addr)>>1;
|
||||
NVMCTRL->CTRLB.bit.MANW = 1; // disable Automatic Page Write
|
||||
NVMCTRL->CTRLA.reg = NVMCTRL_CTRLA_CMDEX_KEY | NVMCTRL_CTRLA_CMD_PBC; // execute Page Buffer Clear
|
||||
while (NVMCTRL->INTFLAG.bit.READY == 0) { }
|
||||
uint32_t Idx=0;
|
||||
for(Idx=0; (Idx<16) && (Idx<Words); Idx++) // copy Data
|
||||
{ Addr[Idx] = Data[Idx]; }
|
||||
NVMCTRL->CTRLA.reg = NVMCTRL_CTRLA_CMDEX_KEY | NVMCTRL_CTRLA_CMD_WP; // execute Write Page
|
||||
while (NVMCTRL->INTFLAG.bit.READY == 0) { }
|
||||
return Idx; }
|
||||
|
||||
int WriteSize(volatile uint32_t *Addr, const uint32_t *Data, uint32_t Words)
|
||||
{ for( ; ; )
|
||||
{ int Len = WritePage64(Addr, Data, Words);
|
||||
Addr+=Len; Data+=Len; Words-=Len; if(Words==0) break; }
|
||||
return 0; }
|
||||
|
||||
int8_t WriteToFlash(volatile uint32_t *Addr=0) // write parameters to Flash
|
||||
{ if(Addr==0) Addr = DefaultFlashAddr();
|
||||
setCheckSum();
|
||||
const uint32_t Words=sizeof(FlashParameters)/sizeof(uint32_t);
|
||||
EraseSize(Addr, Words);
|
||||
WriteSize(Addr, (const uint32_t *)this, Words);
|
||||
if(calcCheckSum(Addr, Words)!=0) return -1; // verify check-sum in Flash
|
||||
return 0; }
|
||||
#endif // WITH_SAMD21
|
||||
|
||||
#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++)
|
||||
|
|
@ -201,9 +308,27 @@ class FlashParameters
|
|||
|
||||
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(volatile uint32_t *Addr=0) // read parameters from Flash
|
||||
{ if(Addr==0) Addr = DefaultFlashAddr(); // default address: the last KB
|
||||
const uint32_t Words=sizeof(FlashParameters)/sizeof(uint32_t);
|
||||
if(calcCheckSum(Addr, Words)!=0) return -1; // agree with the check-sum in Flash ?
|
||||
uint32_t *Dst = (uint32_t *)this;
|
||||
for(uint32_t Idx=0; Idx<Words; Idx++) // read data from Flash
|
||||
{ Dst[Idx] = Addr[Idx]; }
|
||||
return 1; } // return: correct
|
||||
|
||||
bool CompareToFlash(volatile uint32_t *Addr=0) // are the parameters identical to those in the flash ?
|
||||
{ if(Addr==0) Addr = DefaultFlashAddr(); // address in the Flash
|
||||
const uint32_t Words=sizeof(FlashParameters)/sizeof(uint32_t);
|
||||
if(calcCheckSum(Addr, Words)!=0) return 0; // agree with the check-sum in Flash ?
|
||||
uint32_t *Dst = (uint32_t *)this;
|
||||
for(uint32_t Idx=0; Idx<Words; Idx++) // read data from Flash
|
||||
{ if(Dst[Idx] != Addr[Idx]) return 0; }
|
||||
return 1; } // return: correct
|
||||
/*
|
||||
int8_t ReadFromFlash(uint32_t *Addr=0) // read parameters from Flash
|
||||
{ if(Addr==0) Addr = DefaultFlashAddr();
|
||||
const uint32_t Words=sizeof(FlashParameters)/sizeof(uint32_t);
|
||||
|
|
@ -214,7 +339,7 @@ class FlashParameters
|
|||
{ Dst[Idx] = Addr[Idx]; }
|
||||
return 1; } // return: correct
|
||||
|
||||
int8_t CompareToFlash(uint32_t *Addr=0)
|
||||
bool CompareToFlash(uint32_t *Addr=0)
|
||||
{ if(Addr==0) Addr = DefaultFlashAddr(); // address in the Flash
|
||||
const uint32_t Words=sizeof(FlashParameters)/sizeof(uint32_t);
|
||||
uint32_t Check=CheckSum(Addr, Words); // check-sum of Flash data
|
||||
|
|
@ -223,9 +348,11 @@ class FlashParameters
|
|||
for(uint32_t Idx=0; Idx<Words; Idx++) // read data from Flash
|
||||
{ if(Dst[Idx]!=Addr[Idx]) return 0; }
|
||||
return 1; } // return: correct
|
||||
*/
|
||||
|
||||
int8_t WriteToFlash(uint32_t *Addr=0) const // write parameters to Flash
|
||||
int8_t WriteToFlash(volatile uint32_t *Addr=0) // write parameters to Flash
|
||||
{ if(Addr==0) Addr = DefaultFlashAddr();
|
||||
setCheckSum();
|
||||
const uint32_t Words=sizeof(FlashParameters)/sizeof(uint32_t);
|
||||
FLASH_Unlock(); // unlock Flash
|
||||
FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_PGERR | FLASH_FLAG_WRPRTERR);
|
||||
|
|
@ -233,9 +360,9 @@ class FlashParameters
|
|||
uint32_t *Data=(uint32_t *)this; // take data of this object
|
||||
for(uint32_t Idx=0; Idx<Words; Idx++) // word by word
|
||||
{ FLASH_ProgramWord((uint32_t)Addr, Data[Idx]); Addr++; } // !=FLASH_COMPLETE ? // write to Flash
|
||||
FLASH_ProgramWord((uint32_t)Addr, CheckSum(Data, Words) ); // write the check-sum
|
||||
// FLASH_ProgramWord((uint32_t)Addr, CheckSum(Data, Words) ); // write the check-sum
|
||||
FLASH_Lock(); // re-lock Flash
|
||||
if(CheckSum(Addr, Words)!=Addr[Words]) return -1; // verify check-sum in Flash
|
||||
if(calcCheckSum(Addr, Words)!=0) return -1; // verify check-sum in Flash
|
||||
return 0; }
|
||||
#endif // WITH_STM32
|
||||
|
||||
|
|
@ -261,8 +388,7 @@ class FlashParameters
|
|||
Line[Len++]='/';
|
||||
Len+=Format_SignDec(Line+Len, (int16_t)getTxPower());
|
||||
Len+=Format_String(Line+Len, "dBm");
|
||||
Line[Len++]=' '; Len+=Format_SignDec(Line+Len, (int32_t)RFchipFreqCorr, 2, 1);
|
||||
Len+=Format_String(Line+Len, "ppm");
|
||||
Line[Len++]=' '; Len+=Format_SignDec(Line+Len, (int32_t)RFchipFreqCorr, 2, 1); Len+=Format_String(Line+Len, "ppm");
|
||||
Len+=Format_String(Line+Len, " CON:");
|
||||
Len+=Format_UnsDec(Line+Len, CONbaud);
|
||||
Len+=Format_String(Line+Len, "bps\n");
|
||||
|
|
@ -339,8 +465,32 @@ class FlashParameters
|
|||
if(strcmp(Name, "GeoidSepar")==0)
|
||||
{ return Read_Float1(GeoidSepar, Value)<=0; }
|
||||
if(strcmp(Name, "manGeoidSepar")==0)
|
||||
{ int32_t Man=0; if(Read_Int(Man, Value)<=0) return 0;
|
||||
manGeoidSepar=Man; }
|
||||
{ int32_t Man=0; if(Read_Int(Man, Value)<=0) return 0;
|
||||
manGeoidSepar=Man; return 1; }
|
||||
if(strcmp(Name, "NavMode")==0)
|
||||
{ int32_t Mode=0; if(Read_Int(Mode, Value)<=0) return 0;
|
||||
NavMode=Mode; return 1; }
|
||||
if(strcmp(Name, "NavRate")==0)
|
||||
{ int32_t Mode=0; if(Read_Int(Mode, Value)<=0) return 0;
|
||||
if(Mode<1) Mode=1; NavRate=Mode; return 1; }
|
||||
if(strcmp(Name, "Verbose")==0)
|
||||
{ int32_t Mode=0; if(Read_Int(Mode, Value)<=0) return 0;
|
||||
Verbose=Mode; return 1; }
|
||||
#ifdef WITH_ENCRYPT
|
||||
if(strcmp(Name, "Encrypt")==0)
|
||||
{ int32_t Encr=0; if(Read_Int(Encr, Value)<=0) return 0;
|
||||
Encrypt=Encr; return 1; }
|
||||
if(strcmp(Name, "EncryptKey")==0)
|
||||
{ for( uint8_t Idx=0; Idx<4; Idx++)
|
||||
{ uint32_t Key;
|
||||
uint8_t Len=Read_Hex(Key, Value);
|
||||
if(Len!=8) break;
|
||||
EncryptKey[Idx]=Key;
|
||||
Value+=Len;
|
||||
if((*Value)!=':') break;
|
||||
Value++; }
|
||||
return 1; }
|
||||
#endif
|
||||
#ifdef WITH_BT_PWR
|
||||
if(strcmp(Name, "Bluetooth")==0)
|
||||
{ int32_t bton=0; if(Read_Int(bton, Value)<=0) return 0;
|
||||
|
|
@ -363,6 +513,9 @@ class FlashParameters
|
|||
if( (memcmp(Name, "WIFIpass", 8)==0) && (strlen(Name)==9) )
|
||||
{ int Idx=Name[8]-'0'; if( (Idx>=0) && (Idx<WIFIsets) ) return Read_String(WIFIpass[Idx], Value, WIFIpassLen)<=0; }
|
||||
#endif
|
||||
if(strcmp(Name, "SaveToFlash")==0)
|
||||
{ int32_t Save=0; if(Read_Int(Save, Value)<=0) return 0;
|
||||
SaveToFlash=Save; return 1; }
|
||||
return 0; }
|
||||
|
||||
bool ReadLine(char *Line) // read a parameter line
|
||||
|
|
@ -442,20 +595,30 @@ class FlashParameters
|
|||
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_UnsDec (Line, "manGeoidSepar" , manGeoidSepar ); strcat(Line, " # [ 1|0]\n"); if(fputs(Line, File)==EOF) return EOF;
|
||||
Write_UnsDec (Line, "NavMode" , (uint32_t)NavMode ); strcat(Line, " # [ 0..7]\n"); if(fputs(Line, File)==EOF) return EOF;
|
||||
Write_UnsDec (Line, "NavRate" , (uint32_t)NavRate ); strcat(Line, " # [ 1,2]\n"); if(fputs(Line, File)==EOF) return EOF;
|
||||
#ifdef WITH_ENCRYPT
|
||||
Write_UnsDec (Line, "Encrypt" , Encrypt ); strcat(Line, " # [ 1|0]\n"); if(fputs(Line, File)==EOF) return EOF;
|
||||
// Write_Hex (Line, "EncryptKey[0]", EncryptKey[0] , 8); strcat(Line, " # [32-bit]\n"); if(fputs(Line, File)==EOF) return EOF;
|
||||
// Write_Hex (Line, "EncryptKey[1]", EncryptKey[1] , 8); strcat(Line, " # [32-bit]\n"); if(fputs(Line, File)==EOF) return EOF;
|
||||
// Write_Hex (Line, "EncryptKey[2]", EncryptKey[2] , 8); strcat(Line, " # [32-bit]\n"); if(fputs(Line, File)==EOF) return EOF;
|
||||
// Write_Hex (Line, "EncryptKey[3]", EncryptKey[3] , 8); strcat(Line, " # [32-bit]\n"); if(fputs(Line, File)==EOF) return EOF;
|
||||
#endif
|
||||
Write_UnsDec (Line, "Verbose" , (uint32_t)Verbose ); strcat(Line, " # [ 0..3]\n"); if(fputs(Line, File)==EOF) return EOF;
|
||||
Write_UnsDec (Line, "PPSdelay" ,(uint32_t)PPSdelay ); strcat(Line, " # [ ms]\n"); if(fputs(Line, File)==EOF) return EOF;
|
||||
#ifdef WITH_BT_PWR
|
||||
Write_UnsDec (Line, "Bluetooth" , BT_ON ); strcat(Line, " # [ 1|0]\n"); if(fputs(Line, File)==EOF) return EOF;
|
||||
#endif
|
||||
for(uint8_t Idx=0; Idx<InfoParmNum; Idx++)
|
||||
{ Write_String (Line, OGN_Packet::InfoParmName(Idx), InfoParmValue(Idx)); strcat(Line, " # [char]\n"); if(fputs(Line, File)==EOF) return EOF; }
|
||||
{ Write_String (Line, OGN_Packet::InfoParmName(Idx), InfoParmValue(Idx)); strcat(Line, "; # [char]\n"); if(fputs(Line, File)==EOF) return EOF; }
|
||||
#ifdef WITH_BT_SPP
|
||||
strcpy(Line, "BTname = "); strcat(Line, BTname); strcat(Line, " # [char]\n"); if(fputs(Line, File)==EOF) return EOF;
|
||||
strcpy(Line, "BTname = "); strcat(Line, BTname); strcat(Line, "; # [char]\n"); if(fputs(Line, File)==EOF) return EOF;
|
||||
#endif
|
||||
#ifdef WITH_WIFI
|
||||
for(uint8_t Idx=0; Idx<WIFIsets; Idx++)
|
||||
{ if(WIFIname[Idx][0]==0) continue;
|
||||
strcpy(Line, "WIFIname"); Line[8]='0'+Idx; Line[9]='='; strcpy(Line+10, WIFIname[Idx]); strcat(Line, " # [char]\n"); if(fputs(Line, File)==EOF) return EOF;
|
||||
strcpy(Line, "WIFIpass"); Line[8]='0'+Idx; Line[9]='='; strcpy(Line+10, WIFIpass[Idx]); strcat(Line, " # [char]\n"); if(fputs(Line, File)==EOF) return EOF; }
|
||||
strcpy(Line, "WIFIname"); Line[8]='0'+Idx; Line[9]='='; strcpy(Line+10, WIFIname[Idx]); strcat(Line, "; # [char]\n"); if(fputs(Line, File)==EOF) return EOF;
|
||||
strcpy(Line, "WIFIpass"); Line[8]='0'+Idx; Line[9]='='; strcpy(Line+10, WIFIpass[Idx]); strcat(Line, "; # [char]\n"); if(fputs(Line, File)==EOF) return EOF; }
|
||||
// Write_String (Line, "WIFIname", WIFIname[0]); strcat(Line, " # [char]\n"); if(fputs(Line, File)==EOF) return EOF;
|
||||
// Write_String (Line, "WIFIpass", WIFIpass[0]); strcat(Line, " # [char]\n"); if(fputs(Line, File)==EOF) return EOF;
|
||||
#endif
|
||||
|
|
@ -481,20 +644,30 @@ class FlashParameters
|
|||
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_UnsDec (Line, "manGeoidSepar" , manGeoidSepar ); strcat(Line, " # [ 1|0]\n"); Format_String(Output, Line);
|
||||
Write_UnsDec (Line, "NavMode" , (uint32_t)NavMode ); strcat(Line, " # [ 0..7]\n"); Format_String(Output, Line);
|
||||
Write_UnsDec (Line, "NavRate" , (uint32_t)NavRate ); strcat(Line, " # [ 1,2]\n"); Format_String(Output, Line);
|
||||
#ifdef WITH_ENCRYPT
|
||||
Write_UnsDec (Line, "Encrypt" , Encrypt ); strcat(Line, " # [ 1|0]\n"); Format_String(Output, Line);
|
||||
Write_Hex (Line, "EncryptKey[0]", EncryptKey[0] , 8); strcat(Line, " # [32-bit]\n"); Format_String(Output, Line);
|
||||
Write_Hex (Line, "EncryptKey[1]", EncryptKey[1] , 8); strcat(Line, " # [32-bit]\n"); Format_String(Output, Line);
|
||||
Write_Hex (Line, "EncryptKey[2]", EncryptKey[2] , 8); strcat(Line, " # [32-bit]\n"); Format_String(Output, Line);
|
||||
Write_Hex (Line, "EncryptKey[3]", EncryptKey[3] , 8); strcat(Line, " # [32-bit]\n"); Format_String(Output, Line);
|
||||
#endif
|
||||
Write_UnsDec (Line, "Verbose" , (uint32_t)Verbose ); strcat(Line, " # [ 0..3]\n"); Format_String(Output, Line);
|
||||
Write_UnsDec (Line, "PPSdelay" ,(uint32_t)PPSdelay ); strcat(Line, " # [ ms]\n"); Format_String(Output, Line);
|
||||
#ifdef WITH_BT_PWR
|
||||
Write_UnsDec (Line, "Bluetooth" , BT_ON ); strcat(Line, " # [ 1|0]\n"); Format_String(Output, Line);
|
||||
#endif
|
||||
#ifdef WITH_BT_SPP
|
||||
strcpy(Line, "BTname = "); strcat(Line, BTname); strcat(Line, " # [char]\n"); Format_String(Output, Line);
|
||||
strcpy(Line, "BTname = "); strcat(Line, BTname); strcat(Line, "; # [char]\n"); Format_String(Output, Line);
|
||||
#endif
|
||||
for(uint8_t Idx=0; Idx<InfoParmNum; Idx++)
|
||||
{ Write_String (Line, OGN_Packet::InfoParmName(Idx), InfoParmValue(Idx)); strcat(Line, " # [char]\n"); Format_String(Output, Line); }
|
||||
{ Write_String (Line, OGN_Packet::InfoParmName(Idx), InfoParmValue(Idx)); strcat(Line, "; # [char]\n"); Format_String(Output, Line); }
|
||||
#ifdef WITH_WIFI
|
||||
for(uint8_t Idx=0; Idx<WIFIsets; Idx++)
|
||||
{ if(WIFIname[Idx][0]==0) continue;
|
||||
strcpy(Line, "WIFIname"); Line[8]='0'+Idx; Line[9]='='; strcpy(Line+10, WIFIname[Idx]); strcat(Line, " # [char]\n"); Format_String(Output, Line);
|
||||
strcpy(Line, "WIFIpass"); Line[8]='0'+Idx; Line[9]='='; strcpy(Line+10, WIFIpass[Idx]); strcat(Line, " # [char]\n"); Format_String(Output, Line);; }
|
||||
strcpy(Line, "WIFIname"); Line[8]='0'+Idx; Line[9]='='; strcpy(Line+10, WIFIname[Idx]); strcat(Line, "; # [char]\n"); Format_String(Output, Line);
|
||||
strcpy(Line, "WIFIpass"); Line[8]='0'+Idx; Line[9]='='; strcpy(Line+10, WIFIpass[Idx]); strcat(Line, "; # [char]\n"); Format_String(Output, Line);; }
|
||||
// Write_String (Line, "WIFIname", WIFIname[0]); strcat(Line, " # [char]\n"); Format_String(Output, Line);
|
||||
// Write_String (Line, "WIFIpass", WIFIpass[0]); strcat(Line, " # [char]\n"); Format_String(Output, Line);
|
||||
#endif
|
||||
|
|
|
|||
242
main/proc.cpp
242
main/proc.cpp
|
|
@ -11,14 +11,58 @@
|
|||
#include "rf.h" // RF task: transmission and reception of radio packets
|
||||
#include "gps.h" // GPS task: get own time and position, set the GPS baudrate and navigation mode
|
||||
|
||||
#include "fifo.h"
|
||||
|
||||
#ifdef WITH_FLASHLOG // log own track to unused Flash pages (STM32 only)
|
||||
#include "flashlog.h"
|
||||
#endif
|
||||
|
||||
#ifdef WITH_SOUND
|
||||
#include "sound.h"
|
||||
#endif
|
||||
|
||||
#ifdef WITH_LOOKOUT // traffic awareness and warnings
|
||||
#include "lookout.h"
|
||||
LookOut Look;
|
||||
#ifdef WITH_SOUND
|
||||
const char *Dir[16] = { "N", "NNE", "NE", "NEE", "E", "SEE", "SE", "SSE", "S", "SSW", "SW", "SWW", "W", "NWW", "NW", "NNW" };
|
||||
const char *RelDir[8] = { "A", "AR", "R", "BR", "B", "BL", "L", "AL" };
|
||||
|
||||
void Sound_TrafficWarn(const LookOut_Target *Tgt)
|
||||
{ if(!Tgt) return;
|
||||
uint8_t WarnLevel = Tgt->WarnLevel;
|
||||
// uint16_t DistMargin = Tgt->DistMargin; // [0.5m]
|
||||
uint16_t TimeMargin = Tgt->TimeMargin; // [0.5s]
|
||||
uint16_t HorDist = Tgt->HorDist; // [0.5]
|
||||
uint16_t Bearing = Tgt->getBearing(); //
|
||||
int16_t RelBearing = Look.getRelBearing(Tgt);
|
||||
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
Format_String(CONS_UART_Write, "Traffic: ");
|
||||
CONS_UART_Write('#');
|
||||
CONS_UART_Write('0'+WarnLevel);
|
||||
CONS_UART_Write(' ');
|
||||
// Format_Hex(CONS_UART_Write, Bearing);
|
||||
// CONS_UART_Write(' ');
|
||||
uint16_t DirIdx = (Bearing+0x800)>>12; DirIdx&=0x0F;
|
||||
Format_String(CONS_UART_Write, Dir[DirIdx]);
|
||||
CONS_UART_Write(' ');
|
||||
uint16_t RelDirIdx = (RelBearing+0x1000)>>13; RelDirIdx&=0x07;
|
||||
Format_String(CONS_UART_Write, RelDir[RelDirIdx]);
|
||||
CONS_UART_Write(' ');
|
||||
Format_UnsDec(CONS_UART_Write, (uint16_t)(HorDist/2));
|
||||
Format_String(CONS_UART_Write, "m ");
|
||||
Format_UnsDec(CONS_UART_Write, (uint16_t)(TimeMargin/2));
|
||||
Format_String(CONS_UART_Write, "s\n");
|
||||
xSemaphoreGive(CONS_Mutex);
|
||||
// SoundMsg("Traffic");
|
||||
}
|
||||
#endif
|
||||
#endif
|
||||
|
||||
// static uint16_t PrevBattVolt = 0; // [mV]
|
||||
static Delay<uint16_t, 32> BatteryVoltagePipe;
|
||||
uint32_t BatteryVoltage = 0; // [1/256 mV] low-pass filtered battery voltage
|
||||
int32_t BatteryVoltageRate = 0; // [1/256 mV/sec] low-pass filtered battery voltage rise/drop rate
|
||||
|
||||
static char Line[128]; // for printing out to the console, etc.
|
||||
|
||||
|
|
@ -31,12 +75,12 @@ static LDPC_Decoder Decoder; // decoder and error corrector for the OGN
|
|||
#ifdef WITH_LOG
|
||||
|
||||
static int SPIFFSlog(OGN_RxPacket<OGN_Packet> *Packet, uint32_t Time)
|
||||
{ OGN_LogPacket<OGN_Packet> *LogPacket = LOG_FIFO.getWrite(); if(LogPacket==0) return -1;
|
||||
LogPacket->Packet = Packet->Packet;
|
||||
{ OGN_LogPacket<OGN_Packet> *LogPacket = LOG_FIFO.getWrite(); if(LogPacket==0) return -1; // allocate new packet in the LOG_FIFO
|
||||
LogPacket->Packet = Packet->Packet; // copy the packet
|
||||
LogPacket->Flags=0x80;
|
||||
LogPacket->setTime(Time);
|
||||
LogPacket->setCheck();
|
||||
LOG_FIFO.Write();
|
||||
LOG_FIFO.Write(); // finalize the write
|
||||
return 1; }
|
||||
|
||||
static int SPIFFSlog(OGN_TxPacket<OGN_Packet> *Packet, uint32_t Time)
|
||||
|
|
@ -52,13 +96,13 @@ static int SPIFFSlog(OGN_TxPacket<OGN_Packet> *Packet, uint32_t Time)
|
|||
|
||||
// ---------------------------------------------------------------------------------------------------------------------------------------
|
||||
|
||||
#ifdef WITH_ESP32
|
||||
const uint8_t RelayQueueSize = 32;
|
||||
#else
|
||||
const uint8_t RelayQueueSize = 16;
|
||||
#endif
|
||||
// #ifdef WITH_ESP32
|
||||
// const uint8_t RelayQueueSize = 32;
|
||||
// #else
|
||||
// const uint8_t RelayQueueSize = 16;
|
||||
// #endif
|
||||
|
||||
static OGN_PrioQueue<OGN_Packet, RelayQueueSize> RelayQueue; // received packets and candidates to be relayed
|
||||
OGN_PrioQueue<OGN_Packet, RelayQueueSize> RelayQueue; // received packets and candidates to be relayed
|
||||
|
||||
#ifdef DEBUG_PRINT
|
||||
static void PrintRelayQueue(uint8_t Idx) // for debug
|
||||
|
|
@ -78,7 +122,8 @@ static bool GetRelayPacket(OGN_TxPacket<OGN_Packet> *Packet) // prepare a p
|
|||
uint8_t Idx=RelayQueue.getRand(RX_Random); // get weight-random packet from the relay queue
|
||||
if(RelayQueue.Packet[Idx].Rank==0) return 0; // should not happen ...
|
||||
memcpy(Packet->Packet.Byte(), RelayQueue[Idx]->Byte(), OGN_Packet::Bytes); // copy the packet
|
||||
Packet->Packet.Header.Relay=1; // increment the relay count (in fact we only do single relay)
|
||||
Packet->Packet.Header.Relay=1; // increment the relay count (in fact we only do single relay)
|
||||
// Packet->Packet.calcAddrParity();
|
||||
Packet->Packet.Whiten(); Packet->calcFEC(); // whiten and calc. the FEC code => packet ready for transmission
|
||||
// PrintRelayQueue(Idx); // for debug
|
||||
RelayQueue.decrRank(Idx); // reduce the rank of the packet selected for relay
|
||||
|
|
@ -153,20 +198,60 @@ static void ReadStatus(OGN_Packet &Packet)
|
|||
|
||||
#ifdef WITH_ESP32
|
||||
// Packet.clrTemperature();
|
||||
uint32_t Battery = BatterySense(); // [mV]
|
||||
Packet.EncodeVoltage(((Battery*64)+500)/1000); // [1/64V]
|
||||
|
||||
uint16_t BattVolt = BatterySense(); // [mV] measure battery voltage
|
||||
if(BatteryVoltage>0)
|
||||
{ // int32_t PrevVolt = BatteryVoltage;
|
||||
int32_t Rate = ((uint32_t)BattVolt<<8) - BatteryVoltage; // [1/256 mV]
|
||||
BatteryVoltage += (Rate+32)>>6; // [1/256 mV] low-pass battery voltage measurement
|
||||
uint16_t Volt = (BatteryVoltage+16)>>5;
|
||||
int16_t Diff = Volt-BatteryVoltagePipe.Input(Volt);
|
||||
BatteryVoltageRate = Diff; }
|
||||
// BatteryVoltageRate = BatteryVoltage - PrevVolt; }
|
||||
// int16_t BattVoltDiff = BattVolt - PrevBattVolt;
|
||||
// int32_t Diff = ((int32_t)BattVoltDiff<<8) - BatteryVoltageRate;
|
||||
// BatteryVoltageRate += Diff/256; }
|
||||
// BatteryVoltageRate = (((int32_t)BattVoltDiff<<8) + BatteryVoltageRate*255 + 128)>>8; }
|
||||
else
|
||||
{ BatteryVoltage = BattVolt<<8;
|
||||
// PrevBattVolt = BattVolt;
|
||||
BatteryVoltagePipe.Clear(BattVolt<<3);
|
||||
BatteryVoltageRate = 0; }
|
||||
// PrevBattVolt = BattVolt;
|
||||
Packet.EncodeVoltage(((BatteryVoltage>>2)+500)/1000); // [1/64V] encode into the status packet
|
||||
|
||||
#ifdef DEBUG_PRINT
|
||||
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
Format_String(CONS_UART_Write, "Battery: ");
|
||||
// Format_UnsDec(CONS_UART_Write, BattVolt);
|
||||
// CONS_UART_Write(' ');
|
||||
// Format_UnsDec(CONS_UART_Write, PrevBattVolt);
|
||||
// CONS_UART_Write(' ');
|
||||
// Format_UnsDec(CONS_UART_Write, BatteryVoltage, 2);
|
||||
// CONS_UART_Write(' ');
|
||||
// Format_SignDec(CONS_UART_Write, BatteryVoltageRate, 2);
|
||||
// CONS_UART_Write(' ');
|
||||
Format_UnsDec(CONS_UART_Write, (10*BatteryVoltage+128)>>8, 5, 4);
|
||||
Format_String(CONS_UART_Write, "V ");
|
||||
Format_SignDec(CONS_UART_Write, (600*BatteryVoltageRate+128)>>8, 3, 1);
|
||||
Format_String(CONS_UART_Write, "mV/min\n");
|
||||
xSemaphoreGive(CONS_Mutex);
|
||||
#endif
|
||||
#endif
|
||||
|
||||
if(Packet.Status.Pressure==0) Packet.EncodeTemperature(TRX.chipTemp*10); // [0.1degC]
|
||||
Packet.Status.RadioNoise = TRX.averRSSI; // [-0.5dBm] write radio noise to the status packet
|
||||
|
||||
Packet.Status.TxPower = Parameters.getTxPower()-4;
|
||||
uint8_t TxPower = Parameters.getTxPower()-4;
|
||||
if(TxPower>15) TxPower=15;
|
||||
Packet.Status.TxPower = TxPower;
|
||||
|
||||
uint16_t RxRate = RX_OGN_Count64+1;
|
||||
uint8_t RxRateLog2=0; RxRate>>=1; while(RxRate) { RxRate>>=1; RxRateLog2++; }
|
||||
Packet.Status.RxRate = RxRateLog2;
|
||||
|
||||
{ uint8_t Len=0;
|
||||
if(Parameters.Verbose)
|
||||
{ uint8_t Len=0;
|
||||
Len+=Format_String(Line+Len, "$POGNR,"); // NMEA report: radio status
|
||||
Len+=Format_UnsDec(Line+Len, RF_FreqPlan.Plan); // which frequency plan
|
||||
Line[Len++]=',';
|
||||
|
|
@ -201,10 +286,6 @@ static void ReadStatus(OGN_Packet &Packet)
|
|||
Format_String(Log_Write, Line, Len); // send the NMEA out to the log file
|
||||
xSemaphoreGive(Log_Mutex); }
|
||||
#endif
|
||||
|
||||
// #ifdef WITH_FollowMe
|
||||
// Format_String(ADSB_UART_Write, "ADS-B UART test\n");
|
||||
// #endif
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -244,10 +325,13 @@ static void ProcessRxPacket(OGN_RxPacket<OGN_Packet> *RxPacket, uint8_t RxPacket
|
|||
if(DistOK)
|
||||
{ RxPacket->calcRelayRank(GPS_Altitude/10); // calculate the relay-rank (priority for relay)
|
||||
OGN_RxPacket<OGN_Packet> *PrevRxPacket = 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_POGNT
|
||||
if(Parameters.Verbose)
|
||||
{ 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); }
|
||||
#endif
|
||||
// Len=RxPacket->Packet.WriteAPRS(Line, RxTime); // print on the console as APRS message
|
||||
// xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
// Format_String(CONS_UART_Write, Line, 0, Len);
|
||||
|
|
@ -256,9 +340,9 @@ static void ProcessRxPacket(OGN_RxPacket<OGN_Packet> *RxPacket, uint8_t RxPacket
|
|||
const LookOut_Target *Tgt=Look.ProcessTarget(RxPacket->Packet); // process the received target postion
|
||||
if(Tgt) Warn=Tgt->WarnLevel; // remember warning level of this target
|
||||
#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+2*Warn), 3+16*Warn);
|
||||
#endif
|
||||
#else // WITH_LOOKOUT
|
||||
#else // if not 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
|
||||
|
|
@ -266,7 +350,7 @@ static void ProcessRxPacket(OGN_RxPacket<OGN_Packet> *RxPacket, uint8_t RxPacket
|
|||
#ifdef WITH_LOG
|
||||
bool Signif = PrevRxPacket!=0;
|
||||
if(!Signif) Signif=OGN_isSignif(&(RxPacket->Packet), &(PrevRxPacket->Packet));
|
||||
if(Signif) SPIFFSlog(RxPacket, RxTime);
|
||||
if(Signif) SPIFFSlog(RxPacket, RxTime); // log only significant packets
|
||||
#endif
|
||||
#ifdef WITH_SDLOG
|
||||
if(Log_Free()>=128)
|
||||
|
|
@ -275,22 +359,23 @@ static void ProcessRxPacket(OGN_RxPacket<OGN_Packet> *RxPacket, uint8_t RxPacket
|
|||
xSemaphoreGive(Log_Mutex); }
|
||||
#endif
|
||||
#ifdef WITH_PFLAA
|
||||
Len=RxPacket->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);
|
||||
if( Parameters.Verbose // print PFLAA on the console for received packets
|
||||
#ifdef WITH_LOOKOUT
|
||||
&& (!Tgt)
|
||||
#endif
|
||||
)
|
||||
{ uint8_t Len=RxPacket->WritePFLAA(Line, Warn, LatDist, LonDist, RxPacket->Packet.DecodeAltitude()-GPS_Altitude/10);
|
||||
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);
|
||||
MAV_RxMsg::Send(sizeof(MAV_RxReport), MAV_Seq++, MAV_SysID, MAV_COMP_ID_ADSB, MAV_ID_ADSB_VEHICLE, (const uint8_t *)&MAV_RxReport, GPS_UART_Write);
|
||||
#ifdef DEBUG_PRINT
|
||||
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
Format_String(CONS_UART_Write, "MAV_ADSB_VEHICLE sent. ID=");
|
||||
Format_Hex(CONS_UART_Write, MAV_RxReport.ICAO_address);
|
||||
CONS_UART_Write('\r'); CONS_UART_Write('\n');
|
||||
xSemaphoreGive(CONS_Mutex);
|
||||
#endif
|
||||
// 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
|
||||
}
|
||||
}
|
||||
|
|
@ -354,7 +439,7 @@ void vTaskPROC(void* pvParameters)
|
|||
{ vTaskDelay(1);
|
||||
|
||||
RFM_RxPktData *RxPkt = RF_RxFIFO.getRead(); // check for new received packets
|
||||
if(RxPkt)
|
||||
if(RxPkt) // if there is a new received packet
|
||||
{
|
||||
#ifdef DEBUG_PRINT
|
||||
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
|
|
@ -367,11 +452,11 @@ void vTaskPROC(void* pvParameters)
|
|||
xSemaphoreGive(CONS_Mutex);
|
||||
#endif
|
||||
DecodeRxPacket(RxPkt); // decode and process the received packet
|
||||
RF_RxFIFO.Read(); }
|
||||
RF_RxFIFO.Read(); } // remove this packet from the queue
|
||||
|
||||
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(TimeSync_msTime()<340) 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
|
||||
|
|
@ -380,13 +465,13 @@ void vTaskPROC(void* pvParameters)
|
|||
#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);
|
||||
GPS_Position *Position = GPS_getPosition(BestIdx, BestResid, SlotTime%60, 0); // get GPS position which isReady
|
||||
#endif
|
||||
#ifdef DEBUG_PRINT
|
||||
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
Format_String(CONS_UART_Write, "getPos() => ");
|
||||
Format_String(CONS_UART_Write, "getPos(");
|
||||
Format_UnsDec(CONS_UART_Write, SlotTime%60, 2);
|
||||
CONS_UART_Write(' ');
|
||||
Format_String(CONS_UART_Write, ") => ");
|
||||
Format_UnsDec(CONS_UART_Write, (uint16_t)BestIdx);
|
||||
CONS_UART_Write(':');
|
||||
Format_SignDec(CONS_UART_Write, BestResid, 3, 2);
|
||||
|
|
@ -395,12 +480,16 @@ void vTaskPROC(void* pvParameters)
|
|||
#endif
|
||||
// GPS_Position *Position = GPS_getPosition();
|
||||
if(Position) Position->EncodeStatus(StatPacket.Packet); // encode GPS altitude and pressure/temperature/humidity
|
||||
if( Position && Position->isReady && (!Position->Sent) && Position->isReady && Position->isValid() )
|
||||
else { StatPacket.Packet.Status.FixQuality=0; StatPacket.Packet.Status.Satellites=0; } // or lack of the GPS lock
|
||||
{ uint8_t SatSNR = (GPS_SatSNR+2)/4;
|
||||
if(SatSNR>8) { SatSNR-=8; if(SatSNR>31) SatSNR=31; }
|
||||
else { SatSNR=0; }
|
||||
StatPacket.Packet.Status.SatSNR = SatSNR; }
|
||||
if( Position && Position->isReady && (!Position->Sent) && Position->isValid() )
|
||||
{ int16_t AverSpeed=GPS_AverageSpeed(); // [0.1m/s] average speed, including the vertical speed
|
||||
if(Parameters.FreqPlan==0)
|
||||
RF_FreqPlan.setPlan(Position->Latitude, Position->Longitude); // set the frequency plan according to the GPS position
|
||||
else RF_FreqPlan.setPlan(Parameters.FreqPlan);
|
||||
/*
|
||||
#ifdef DEBUG_PRINT
|
||||
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
Format_UnsDec(CONS_UART_Write, TimeSync_Time()%60);
|
||||
|
|
@ -409,7 +498,6 @@ void vTaskPROC(void* pvParameters)
|
|||
Format_String(CONS_UART_Write, " -> Sent\n");
|
||||
xSemaphoreGive(CONS_Mutex);
|
||||
#endif
|
||||
*/
|
||||
PosTime=Position->getUnixTime();
|
||||
PosPacket.Packet.HeaderWord=0;
|
||||
PosPacket.Packet.Header.Address = Parameters.Address; // set address
|
||||
|
|
@ -420,12 +508,17 @@ void vTaskPROC(void* pvParameters)
|
|||
#endif
|
||||
PosPacket.Packet.calcAddrParity(); // parity of (part of) the header
|
||||
if(BestResid==0) Position->Encode(PosPacket.Packet); // encode position/altitude/speed/etc. from GPS position
|
||||
else Position->Encode(PosPacket.Packet, BestResid);
|
||||
else // extrapolate the position when if not at an exact UTC second
|
||||
{ while(BestResid>=50) BestResid-=100; // remove full seconds
|
||||
Position->Encode(PosPacket.Packet, BestResid); }
|
||||
PosPacket.Packet.Position.AcftType = Parameters.AcftType; // aircraft-type
|
||||
// { uint8_t Len=PosPacket.Packet.WriteAPRS(Line, PosTime); // print on the console as APRS message
|
||||
// xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
// Format_String(CONS_UART_Write, Line, 0, Len);
|
||||
// xSemaphoreGive(CONS_Mutex); }
|
||||
PosPacket.Packet.Position.Stealth = 0; // Parameters.Stealth;
|
||||
#ifdef DEBUG_PRINT
|
||||
{ uint8_t Len=PosPacket.Packet.WriteAPRS(Line, PosTime); // print on the console as APRS message
|
||||
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
Format_String(CONS_UART_Write, Line, 0, Len);
|
||||
xSemaphoreGive(CONS_Mutex); }
|
||||
#endif
|
||||
OGN_TxPacket<OGN_Packet> *TxPacket = RF_TxFIFO.getWrite();
|
||||
TxPacket->Packet = PosPacket.Packet; // copy the position packet to the TxFIFO
|
||||
#ifdef WITH_ENCRYPT
|
||||
|
|
@ -437,7 +530,7 @@ void vTaskPROC(void* pvParameters)
|
|||
TxPacket->calcFEC(); // whiten and calculate FEC code
|
||||
#ifdef DEBUG_PRINT
|
||||
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, " TxFIFO <- ");
|
||||
|
|
@ -446,40 +539,28 @@ void vTaskPROC(void* pvParameters)
|
|||
xSemaphoreGive(CONS_Mutex);
|
||||
#endif
|
||||
XorShift32(RX_Random);
|
||||
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
|
||||
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_MAVLINK
|
||||
// { MAV_HEARTBEAT MAV_HeartBeat;
|
||||
// // = { custom_mode:0,
|
||||
// // type:0,
|
||||
// // autopilot:0,
|
||||
// // base_mode:0,
|
||||
// // system_status:4,
|
||||
// // mavlink_version:1
|
||||
// // };
|
||||
// MAV_HeartBeat.custom_mode=0;
|
||||
// MAV_HeartBeat.type=0;
|
||||
// MAV_HeartBeat.autopilot=0;
|
||||
// MAV_HeartBeat.base_mode=0;
|
||||
// MAV_HeartBeat.system_status=4;
|
||||
// MAV_HeartBeat.mavlink_version=1;
|
||||
// MAV_RxMsg::Send(sizeof(MAV_HeartBeat), MAV_Seq++, MAV_SysID, MAV_COMP_ID_ADSB, MAV_ID_HEARTBEAT, (const uint8_t *)&MAV_HeartBeat, GPS_UART_Write);
|
||||
// }
|
||||
// #endif
|
||||
#ifdef WITH_LOOKOUT
|
||||
const LookOut_Target *Tgt=Look.ProcessOwn(PosPacket.Packet); // process own position, get the most dangerous target
|
||||
const LookOut_Target *Tgt=Look.ProcessOwn(PosPacket.Packet); // process own position, get the most dangerous target
|
||||
#ifdef WITH_PFLAA
|
||||
uint8_t Len=Look.WritePFLAU(Line);
|
||||
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
Format_String(CONS_UART_Write, Line, 0, Len);
|
||||
xSemaphoreGive(CONS_Mutex);
|
||||
if(Parameters.Verbose)
|
||||
{ xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
Look.WritePFLA(CONS_UART_Write); // produce PFLAU and PFLAA for all tracked targets
|
||||
xSemaphoreGive(CONS_Mutex); }
|
||||
#else
|
||||
if(Parameters.Verbose)
|
||||
{ uint8_t Len=Look.WritePFLAU(Line); // $PFLAU, overall status
|
||||
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
Format_String(CONS_UART_Write, Line, 0, Len);
|
||||
xSemaphoreGive(CONS_Mutex); }
|
||||
#endif // WITH_PFLAA
|
||||
uint8_t Warn = 0;
|
||||
if(Tgt) Warn = Tgt->WarnLevel; // what is the warning level ?
|
||||
if( (Warn>0) && (AverSpeed>=10) ) // if non-zero warning level and we seem to be moving
|
||||
{ int16_t RelBearing = Look.getRelBearing(Tgt); // relative bearing to the Target
|
||||
int8_t Bearing = (12*(int32_t)RelBearing+0x8000)>>16; // [-12..+12]
|
||||
if( (Warn>0) /* && (AverSpeed>=10) */ ) // if non-zero warning level and we seem to be moving
|
||||
{ // int16_t RelBearing = Look.getRelBearing(Tgt); // relative bearing to the Target
|
||||
// int8_t Bearing = (12*(int32_t)RelBearing+0x8000)>>16; // [-12..+12]
|
||||
#ifdef WITH_BEEPER // make the sound according to the level
|
||||
if(Warn<=1)
|
||||
{ if(KNOB_Tick>8)
|
||||
|
|
@ -498,6 +579,9 @@ void vTaskPROC(void* pvParameters)
|
|||
}
|
||||
|
||||
#endif // WITH_BEEPER
|
||||
#ifdef WITH_SOUND
|
||||
Sound_TrafficWarn(Tgt);
|
||||
#endif
|
||||
}
|
||||
#endif // WITH_LOOKOUT
|
||||
#ifdef WITH_FLASHLOG
|
||||
|
|
|
|||
17
main/proc.h
17
main/proc.h
|
|
@ -1,3 +1,20 @@
|
|||
extern uint32_t BatteryVoltage; // [1/256 mV] averaged
|
||||
extern int32_t BatteryVoltageRate; // [1/256 mV] averaged
|
||||
|
||||
#ifdef WITH_LOOKOUT // traffic awareness and warnings
|
||||
#include "lookout.h"
|
||||
extern LookOut Look;
|
||||
#endif
|
||||
|
||||
|
||||
#ifdef WITH_ESP32
|
||||
const uint8_t RelayQueueSize = 32;
|
||||
#else
|
||||
const uint8_t RelayQueueSize = 16;
|
||||
#endif
|
||||
|
||||
extern OGN_PrioQueue<OGN_Packet, RelayQueueSize> RelayQueue; // received packets and candidates to be relayed
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C"
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -0,0 +1,279 @@
|
|||
#include <stdio.h>
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "intmath.h"
|
||||
|
||||
#include "ogn.h"
|
||||
|
||||
// =======================================================================================================
|
||||
|
||||
class Acft_RelPos // 3-D relative position with speed and turn rate
|
||||
{ public:
|
||||
int16_t T; // [0.5sec]
|
||||
int16_t X,Y,Z; // [0.5m]
|
||||
uint16_t Speed; // [0.5m/s]
|
||||
uint16_t Heading; // [360/0x10000 deg]
|
||||
int16_t Climb; // [0.5m/s]
|
||||
int16_t Turn; // [360/0x10000 deg/s] [2*PI/0x10000 rad/sec]
|
||||
int16_t Dx,Dy; // [2^-12] directon vector - calc. on Heading
|
||||
uint8_t Error; // [0.5m]
|
||||
uint8_t Spare;
|
||||
// int16_t Ax,Ay; // [1/16m/s^2] acceleration vactor
|
||||
// int16_t R; // [0.5m] (signed) turning radius - calc. from Turn and Speed
|
||||
// int16_t Ox,Oy; // [0.5m] turning circle center - only valid when R!=0
|
||||
|
||||
public:
|
||||
void Print(void) const
|
||||
{ printf("%+7.1f: [%+7.1f,%+7.1f,%+7.1f]m %5.1fm/s %05.1fdeg %+5.1fm/s %+5.1fdeg/sec [%3.1fm]",
|
||||
0.5*T, 0.5*X, 0.5*Y, 0.5*Z, 0.5*Speed, (360.0/0x10000)*Heading, 0.5*Climb, (360.0/0x10000)*Turn, 0.5*Error);
|
||||
// printf(" [%+6.2f,%+6.2f]m/s^2", 0.0625*Ax, 0.0625*Ay);
|
||||
// if(R) printf(" R:%+8.1fm [%+7.1f, %+7.1f]", 0.5*R, 0.5*Ox, 0.5*Oy);
|
||||
printf("\n"); }
|
||||
|
||||
void Clear(void)
|
||||
{ T =0;
|
||||
X =0; Y =0; Z =0;
|
||||
Speed=0; Heading=0; Climb=0; Turn=0;
|
||||
Error=4; }
|
||||
|
||||
uint32_t SqrDistance(Acft_RelPos &Target)
|
||||
{ int32_t dX = Target.X-X;
|
||||
int32_t dY = Target.Y-Y;
|
||||
int32_t dZ = Target.Z-Z;
|
||||
return dX*dX+dY*dY+dZ*dZ; } // [0.25m^2]
|
||||
|
||||
static uint32_t SqrDistance(int16_t dX, int16_t dY, int16_t dZ)
|
||||
{ return (int32_t)dX*dX + (int32_t)dY*dY + (int32_t)dZ*dZ; }
|
||||
|
||||
static uint32_t SqrDistance(int16_t dX, int16_t dY)
|
||||
{ return (int32_t)dX*dX + (int32_t)dY*dY; }
|
||||
|
||||
uint32_t FastDistance(Acft_RelPos &Target)
|
||||
{ int16_t dX = Target.X-X;
|
||||
int16_t dY = Target.Y-Y;
|
||||
int16_t dZ = Target.Z-Z;
|
||||
return FastDistance(dX, dY, dZ); } // [0.5m]
|
||||
|
||||
static uint16_t FastDistance(int16_t dX, int16_t dY)
|
||||
{ dX = abs(dX); dY = abs(dY);
|
||||
if(dX>dY) return dX+dY/2;
|
||||
else return dY+dX/2; }
|
||||
|
||||
static uint16_t FastDistance(int16_t dX, int16_t dY, int16_t dZ)
|
||||
{ return FastDistance((int16_t)FastDistance(dX, dY), dZ); }
|
||||
|
||||
// predict self and target until MinSepar is reached but no longer than MaxTime
|
||||
int16_t StepTillMinSepar(Acft_RelPos &Target, uint16_t MinSepar, int16_t MaxTime=40) // [0.5m] [0.5s]
|
||||
{ int16_t PredTime=0; // count time by which we predict
|
||||
uint16_t MaxTurn=abs(Turn); // the max. turn rate
|
||||
uint16_t Turn2=abs(Target.Turn); if(Turn2>MaxTurn) MaxTurn=Turn2;
|
||||
int16_t MaxStepTime = 32; // [0.5s] max. allowed stpping time period
|
||||
if(MaxTurn>=0x100) MaxStepTime=0x2000/MaxTurn; // [0.5s] maximup step time (for sharp turns)
|
||||
if(MaxStepTime<2) MaxStepTime=2; // [0.5s] but don't do smaller steps than 1sec
|
||||
uint16_t TotSpeed = FastDistance(Speed, Climb) + FastDistance(Target.Speed, Target.Climb); // [0.5m/s] "total" speed, thus the sum of the two speeds magnitudes
|
||||
#ifdef DEBUG_PRINT
|
||||
printf("StepTillMinSepar( , MinSepar=%3.1fm, MaxTime=%3.1fs)\n", 0.5*MinSepar, 0.5*MaxTime);
|
||||
Print(); Target.Print();
|
||||
#endif
|
||||
for( ; ; )
|
||||
{ if(MaxTime==0) return PredTime; // if max. prediction time reached: stop
|
||||
int32_t DistMargin = FastDistance(Target); // [0.5m] Distance margin to the target
|
||||
if(DistMargin<=MinSepar) return PredTime; // If distance margin already below minimum
|
||||
int16_t dT = Target.T-T; // [0.5sec] Target may not be exact same time
|
||||
int32_t dS = (dT*TotSpeed)>>1; // [0.5m] thus some extra distance margin
|
||||
DistMargin -= abs(dS); // [0.5m] subtract margin due to time difference
|
||||
if(DistMargin<=MinSepar) return PredTime; // [0.5m] if distance margin below minimum
|
||||
DistMargin -= MinSepar; // [0.5m] subtract minimum separation from the distance margin
|
||||
if((TotSpeed*MaxTime) < (2*DistMargin) ) // If plenty enough margin given the speed
|
||||
{ uint16_t TimeMargin=MaxTime+2;
|
||||
if((2*DistMargin)<(TotSpeed*TimeMargin)) TimeMargin=2*DistMargin/TotSpeed+1;
|
||||
TimeMargin+=PredTime; // if(TimeMargin>240) TimeMargin=240;
|
||||
return TimeMargin; }
|
||||
int16_t StepTime = (2*DistMargin)/TotSpeed+1; // [0.5s] Time margin given distance margin and speed
|
||||
#ifdef DEBUG_PRINT
|
||||
printf("DistMargin=%3.1fm => StepTime=%+4.1fs\n", 0.5*DistMargin, 0.5*StepTime);
|
||||
#endif
|
||||
// if(StepTime==0) return PredTime;
|
||||
// if(StepTime<1) StepTime=1; // minimum step time for prediction
|
||||
if(StepTime>MaxStepTime) StepTime=MaxStepTime; // [0.5s] maximum step time for prediction
|
||||
if(StepTime>MaxTime) StepTime=MaxTime;
|
||||
int16_t NextTime = T+StepTime; // [0.5s] time
|
||||
StepFwd(StepTime);
|
||||
int16_t TgtStepTime = NextTime-Target.T;
|
||||
if(TgtStepTime>0) Target.StepFwd(TgtStepTime);
|
||||
#ifdef DEBUG_PRINT
|
||||
Print(); Target.Print();
|
||||
#endif
|
||||
PredTime+=StepTime; // [0.5s] count time by which we predict
|
||||
MaxTime-=StepTime; } // [0.5s] decrement the max. time we should predict
|
||||
return MaxTime+1; } // return estimated time margin before the separation could fall below minimum
|
||||
|
||||
// predict the minimum distance: does not take turn into account thus must not be used on significant distances
|
||||
int16_t MissTime(Acft_RelPos &Target, int16_t MaxTime=40) // [0.5s]
|
||||
{ int32_t dX = Target.X; // [0.5m] Target distance vector
|
||||
int32_t dY = Target.Y;
|
||||
int32_t dZ = Target.Z;
|
||||
int32_t tVx, tVy; Target.getSpeedVector(tVx, tVy); // [0.5m/s] Target speed vector
|
||||
int32_t tVz = Target.Climb;
|
||||
int16_t dT = Target.T - T; // [0.5sec] time difference betwen target and me
|
||||
if(dT)
|
||||
{ dX -= (dT*tVx)>>1; // adjust target position for the time diff.
|
||||
dY -= (dT*tVy)>>1;
|
||||
dZ -= (dT*tVz)>>1; }
|
||||
dX -= X; // [0.5m] Target relative position
|
||||
dY -= Y;
|
||||
dZ -= Z;
|
||||
int32_t dVx, dVy; getSpeedVector(dVx, dVy); // [0.5m/s] Target relative speed
|
||||
dVx = tVx-dVx;
|
||||
dVy = tVy-dVy;
|
||||
int32_t dVz = tVz - Climb;
|
||||
|
||||
int32_t DistSqrRate = dVx*dX + dVy*dY + dVz*dZ; // [0.25m2/s] 3-D scalar product: rel. distance x rel. speed
|
||||
// if(DistSqrRate==0) return MaxTime; // if target neither moving away nor closing
|
||||
// if(DistSqrRate>0) return MaxTime; // if target moving away from me
|
||||
|
||||
int32_t RelVelSqr = dVx*dVx + dVy*dVy + dVz*dVz; // [0.25m2/s2] 3-D relative velocity square
|
||||
// printf("MissTime() DistSqrRate = %+4.1fm^2/s RelVelSqr = %3.1f(m/s)^2\n", 0.25*DistSqrRate, 0.5*RelVelSqr);
|
||||
if( ( RelVelSqr*MaxTime) <= (-2*DistSqrRate) ) return MaxTime; // if min. approach time is longer than maximum
|
||||
if( (-RelVelSqr*MaxTime) >= (-2*DistSqrRate) ) return -MaxTime; // if min. approach time is longer than maximum
|
||||
return (-2*DistSqrRate+(RelVelSqr/2))/RelVelSqr; } // [0.5sec] time of the closest approach
|
||||
|
||||
void calcDir(void) // calculate the direction unity vector
|
||||
{ Dx = Icos(Heading); // DirX = cos(Heading)
|
||||
Dy = Isin(Heading); } // DirY = sin(Heading)
|
||||
|
||||
// void calcAccel(void)
|
||||
// { int32_t A = ((int32_t)Turn*Speed*201+0x8000)>>16; // [1/64m/s^2]
|
||||
// Ax = (-A*Dy+0x2000)>>14; // [1/16m/s^2]
|
||||
// Ay = (+A*Dx+0x2000)>>14; }
|
||||
|
||||
// void calcTurn(int16_t MaxRadius=10000) // calculate the turning radius and turning circle center
|
||||
// { R=getTurnRadius(MaxRadius); if(R==0) return;
|
||||
// Ox = X - ((Dy*R)>>12);
|
||||
// Oy = Y + ((Dx*R)>>12); }
|
||||
|
||||
int16_t getTurnDev(int16_t dT) // approx. horizontal deviation from straight line due to the turn
|
||||
{ int32_t A = ((int32_t)Turn*Speed*201+0x8000)>>16; // [1/64m/s^2] acceleration
|
||||
// printf("getTurnDev() A=%6.3fm/s^2\n", A/64.0);
|
||||
return (A*dT*dT+0x80)>>8; } // [0.5m]
|
||||
|
||||
int16_t getTurnRadius(int32_t MaxRadius=0x7FFF) const
|
||||
{ int32_t Radius = (int32_t)Speed*10436;
|
||||
if(Turn==0) return 0; // return zero for radius larger than maximum defined
|
||||
Radius/=Turn;
|
||||
if(abs(Radius)>MaxRadius) return 0;
|
||||
return Radius; } // return turning radius [0.5m] = turning circle diameter [m]
|
||||
|
||||
void getSpeedVector(int32_t &Vx, int32_t &Vy) // [0.5m/s] [0.5m/s]
|
||||
{ Vx = ((int32_t)Dx*Speed+0x800)>>12; // Vx = DirX * Speed
|
||||
Vy = ((int32_t)Dy*Speed+0x800)>>12; } // Vy = DirY * Speed
|
||||
|
||||
void getSpeedVector(int16_t &Vx, int16_t &Vy) // [0.5m/s] [0.5m/s]
|
||||
{ Vx = ((int32_t)Dx*Speed+0x800)>>12; // Vx = DirX * Speed
|
||||
Vy = ((int32_t)Dy*Speed+0x800)>>12; } // Vy = DirY * Speed
|
||||
|
||||
void StepFwd(int16_t Time) // [0.5s] predict the position into the future
|
||||
{ int16_t Vx, Vy;
|
||||
int16_t Time1 = Time>>1;
|
||||
int16_t Time2 = Time-Time1;
|
||||
getSpeedVector(Vx, Vy);
|
||||
int16_t dX = Time1*Vx; int16_t dY = Time1*Vy;
|
||||
Heading += (Time*Turn)>>1;
|
||||
calcDir(); // calcAccel();
|
||||
getSpeedVector(Vx, Vy);
|
||||
dX += Time2*Vx; dY += Time2*Vy;
|
||||
X += dX/2;
|
||||
Y += dY/2;
|
||||
Z += (Time*Climb)>>1;
|
||||
T += Time; }
|
||||
|
||||
void StepFwdSecs(int16_t Secs) // [sec] predict the position Secs into the future
|
||||
{ int16_t Vx, Vy;
|
||||
int16_t Secs1=Secs>>1;
|
||||
int16_t Secs2=Secs-Secs1;
|
||||
getSpeedVector(Vx, Vy);
|
||||
X += Secs1*Vx; Y += Secs1*Vy;
|
||||
Heading += Secs*Turn;
|
||||
calcDir(); // calcAccel();
|
||||
getSpeedVector(Vx, Vy);
|
||||
X += Secs2*Vx; Y += Secs2*Vy;
|
||||
Z += Secs*Climb;
|
||||
T += 2*Secs; }
|
||||
|
||||
void StepFwd4secs(void) // predict the position four second into the future
|
||||
{ int16_t Vx, Vy;
|
||||
getSpeedVector(Vx, Vy);
|
||||
X += 2*Vx; Y += 2*Vy;
|
||||
Heading += 4*Turn;
|
||||
calcDir(); // calcAccel();
|
||||
getSpeedVector(Vx, Vy);
|
||||
X += 2*Vx; Y += 2*Vy;
|
||||
Z += 4*Climb;
|
||||
T += 8; }
|
||||
|
||||
void StepFwd2secs(void) // predict the position two seconds into the future
|
||||
{ int16_t Vx, Vy;
|
||||
getSpeedVector(Vx, Vy); // get hor. speed vector from speed and dir. vector
|
||||
X += Vx; Y += Vy; // incr. the hor. coordinates by half the speed
|
||||
Heading += 2*Turn; // increment the heading by the turning rate
|
||||
calcDir(); // calcAccel(); // recalc. direction and acceleration
|
||||
getSpeedVector(Vx, Vy); // recalc. the speed vector
|
||||
X += Vx; Y += Vy; // incr. horizotal coord. by half the speed vector
|
||||
Z += 2*Climb; // increment the rel. altitude by the climb rate
|
||||
T += 4; } // increment time by 2sec
|
||||
|
||||
void StepFwd1sec(void) // predict the position one second into the future
|
||||
{ int16_t Vx, Vy;
|
||||
getSpeedVector(Vx, Vy);
|
||||
X += Vx/2; Y += Vy/2;
|
||||
Heading += Turn;
|
||||
calcDir(); // calcAccel();
|
||||
getSpeedVector(Vx, Vy);
|
||||
X += Vx/2; Y += Vy/2;
|
||||
Z += Climb;
|
||||
T += 2; }
|
||||
|
||||
template <class OGNx_Packet> // read position from an OGN packet, use provided reference
|
||||
int32_t Read(OGNx_Packet &Packet, uint8_t RefTime, int32_t RefLat, int32_t RefLon, int32_t RefAlt, uint16_t LatCos=3000, int32_t MaxDist=10000)
|
||||
{ T = (int16_t)Packet.Position.Time-(int16_t)RefTime;
|
||||
if(T<=(-30)) T+=60; else if(T>30) T-=60;
|
||||
T<<=1;
|
||||
int32_t LatDist, LonDist;
|
||||
if(Packet.calcDistanceVector(LatDist, LonDist, RefLat, RefLon, LatCos, MaxDist)<0) return -1;
|
||||
X = LatDist<<1; // [m] => [0.5m]
|
||||
Y = LonDist<<1; // [m] => [0.5m]
|
||||
Z = (Packet.DecodeAltitude()-RefAlt)<<1; // [m] => [0.5m]
|
||||
Speed = (Packet.DecodeSpeed()+2)/5; // [0.1m/s] => [0.5m/s]
|
||||
Heading = Packet.getHeadingAngle(); // [360/0x10000deg]
|
||||
Climb = Packet.DecodeClimbRate()/5; // [0.1m/s] => [0.5m/s]
|
||||
Turn = ((int32_t)Packet.DecodeTurnRate()*1165+32)>>6; // [0.1deg/s] => [360/0x10000deg/s]
|
||||
calcDir();
|
||||
Error = (2*Packet.DecodeDOP()+22)/5;
|
||||
// calcAccel();
|
||||
// calcTurn();
|
||||
// printf("Read: "); Packet.Print();
|
||||
// Print();
|
||||
return 1; }
|
||||
|
||||
template <class OGNx_Packet> // write position into the OGN packet, using given reference
|
||||
void Write(OGNx_Packet &Packet, uint8_t RefTime, int32_t RefLat, int32_t RefLon, int32_t RefAlt, uint16_t LatCos=3000)
|
||||
{ int16_t Time=RefTime+(T>>1);
|
||||
// if(Time<0) Time+=60; else if(Time>=60) Time-=60;
|
||||
Packet.Position.Time = Time%60;
|
||||
Packet.setDistanceVector(X>>1, Y>>1, RefLat, RefLon, LatCos);
|
||||
Packet.EncodeAltitude(RefAlt+(Z>>1)); //
|
||||
Packet.clrBaro(); // don't know the standard pressure altitude
|
||||
Packet.EncodeSpeed(Speed*5); // [0.5m/s] => [0.1m/s]
|
||||
Packet.setHeadingAngle(Heading); //
|
||||
Packet.EncodeClimbRate(Climb*5); // [0.5m/s] => [0.1m/s]
|
||||
Packet.EncodeTurnRate((Turn*7+64)>>7); // [360/0x10000deg/s] => [0.1deg/s]
|
||||
Packet.EncodeDOP((5*Error)/2-10);
|
||||
}
|
||||
|
||||
} ;
|
||||
|
||||
// =======================================================================================================
|
||||
|
||||
38
main/rf.cpp
38
main/rf.cpp
|
|
@ -157,16 +157,27 @@ static void SetFreqPlan(void) // set the RF TRX a
|
|||
}
|
||||
|
||||
static uint8_t StartRFchip(void)
|
||||
{ TRX.RESET(1); // RESET active
|
||||
vTaskDelay(10); // wait 10ms
|
||||
{ TRX.WriteMode(RF_OPMODE_STANDBY);
|
||||
vTaskDelay(1);
|
||||
TRX.RESET(1); // RESET active
|
||||
vTaskDelay(1); // wait 10ms
|
||||
TRX.RESET(0); // RESET released
|
||||
vTaskDelay(10); // wait 10ms
|
||||
vTaskDelay(5); // wait 10ms
|
||||
SetFreqPlan(); // set TRX base frequency and channel separation after the frequency hopping plan
|
||||
#ifdef DEBUG_PRINT
|
||||
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
TRX.PrintReg(CONS_UART_Write);
|
||||
xSemaphoreGive(CONS_Mutex);
|
||||
#endif
|
||||
#ifdef WITH_RFM95
|
||||
TRX.WriteDefaultReg();
|
||||
#endif
|
||||
TRX.Configure(0, OGN_SYNC); // setup RF chip parameters and set to channel #0
|
||||
TRX.WriteMode(RF_OPMODE_STANDBY); // set RF chip mode to STANDBY
|
||||
uint8_t Version = TRX.ReadVersion();
|
||||
#ifdef DEBUG_PRINT
|
||||
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
TRX.PrintReg(CONS_UART_Write);
|
||||
Format_String(CONS_UART_Write, "StartRFchip() v");
|
||||
Format_Hex(CONS_UART_Write, Version);
|
||||
CONS_UART_Write(' ');
|
||||
|
|
@ -196,7 +207,7 @@ extern "C"
|
|||
TRX.DIO0_isOn = RFM_IRQ_isOn;
|
||||
TRX.RESET = RFM_RESET;
|
||||
|
||||
RF_FreqPlan.setPlan(Parameters.FreqPlan); // 1 = Europe/Africa, 2 = USA/CA, 3 = Australia and South America
|
||||
RF_FreqPlan.setPlan(Parameters.FreqPlan); // 1 = Europe/Africa, 2 = USA/CA, 3 = Australia and South America
|
||||
|
||||
vTaskDelay(5);
|
||||
|
||||
|
|
@ -204,8 +215,8 @@ extern "C"
|
|||
{ uint8_t ChipVersion = StartRFchip();
|
||||
|
||||
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
Format_String(CONS_UART_Write, "TaskRF: ");
|
||||
CONS_UART_Write('v'); Format_Hex(CONS_UART_Write, ChipVersion);
|
||||
Format_String(CONS_UART_Write, "TaskRF: v");
|
||||
Format_Hex(CONS_UART_Write, ChipVersion);
|
||||
CONS_UART_Write('\r'); CONS_UART_Write('\n');
|
||||
xSemaphoreGive(CONS_Mutex);
|
||||
|
||||
|
|
@ -227,9 +238,10 @@ extern "C"
|
|||
|
||||
for( ; ; )
|
||||
{
|
||||
while(Button_SleepRequest)
|
||||
{ TRX.WriteMode(RF_OPMODE_SLEEP);
|
||||
vTaskDelay(100); }
|
||||
|
||||
// #ifdef DEBUG_PRINT
|
||||
// RF_Print();
|
||||
// #endif
|
||||
|
||||
uint32_t RxRssiSum=0; uint16_t RxRssiCount=0; // measure the average RSSI for lower frequency
|
||||
do
|
||||
|
|
@ -246,6 +258,14 @@ extern "C"
|
|||
|
||||
TRX.WriteMode(RF_OPMODE_STANDBY); // switch to standy
|
||||
vTaskDelay(1);
|
||||
|
||||
if(PowerMode==0)
|
||||
{ TRX.WriteMode(RF_OPMODE_SLEEP);
|
||||
while(PowerMode==0)
|
||||
vTaskDelay(1);
|
||||
TRX.WriteMode(RF_OPMODE_STANDBY);
|
||||
vTaskDelay(1); }
|
||||
|
||||
SetFreqPlan();
|
||||
|
||||
TRX.averRSSI=RX_RSSI.getOutput();
|
||||
|
|
|
|||
135
main/rfm.h
135
main/rfm.h
|
|
@ -23,12 +23,12 @@ class RFM_RxPktData // packet received by the RF chip
|
|||
void Print(void (*CONS_UART_Write)(char), uint8_t WithData=0) const
|
||||
{ // uint8_t ManchErr = Count1s(RxPktErr, 26);
|
||||
Format_String(CONS_UART_Write, "RxPktData: ");
|
||||
Format_UnsDec(CONS_UART_Write, Time);
|
||||
Format_HHMMSS(CONS_UART_Write, Time);
|
||||
CONS_UART_Write('+');
|
||||
Format_UnsDec(CONS_UART_Write, msTime, 4, 3);
|
||||
CONS_UART_Write(' '); Format_Hex(CONS_UART_Write, Channel);
|
||||
CONS_UART_Write('/');
|
||||
Format_SignDec(CONS_UART_Write, (int16_t)(-5*(int16_t)RSSI), 4, 1);
|
||||
Format_SignDec(CONS_UART_Write, (int16_t)(-5*(int16_t)RSSI), 3, 1);
|
||||
Format_String(CONS_UART_Write, "dBm\n");
|
||||
if(WithData==0) return;
|
||||
for(uint8_t Idx=0; Idx<Bytes; Idx++)
|
||||
|
|
@ -128,17 +128,17 @@ class RFM_RxPktData // packet received by the RF chip
|
|||
#define RF_IRQ_PllLock 0x1000 //
|
||||
#define RF_IRQ_Rssi 0x0800
|
||||
#define RF_IRQ_Timeout 0x0400
|
||||
//
|
||||
#define RF_IRQ_PreambleDetect 0x0200
|
||||
#define RF_IRQ_SyncAddrMatch 0x0100
|
||||
|
||||
#define RF_IRQ_FifoFull 0x0080 //
|
||||
#define RF_IRQ_FifoNotEmpty 0x0040 // at least one byte in the FIFO
|
||||
#define RF_IRQ_FifoLevel 0x0020 // more bytes than FifoThreshold
|
||||
#define RF_IRQ_FifoOverrun 0x0010 // write this bit to clear the FIFO
|
||||
#define RF_IRQ_PacketSent 0x0008 // packet transmission was completed
|
||||
#define RF_IRQ_PayloadReady 0x0004
|
||||
#define RF_IRQ_CrcOk 0x0002
|
||||
#define RF_IRQ_LowBat 0x0001
|
||||
#define RF_IRQ_FifoFull 0x0080 //
|
||||
#define RF_IRQ_FifoNotEmpty 0x0040 // at least one byte in the FIFO
|
||||
#define RF_IRQ_FifoLevel 0x0020 // more bytes than FifoThreshold
|
||||
#define RF_IRQ_FifoOverrun 0x0010 // write this bit to clear the FIFO
|
||||
#define RF_IRQ_PacketSent 0x0008 // packet transmission was completed
|
||||
#define RF_IRQ_PayloadReady 0x0004
|
||||
#define RF_IRQ_CrcOk 0x0002
|
||||
#define RF_IRQ_LowBat 0x0001
|
||||
|
||||
#include "manchester.h"
|
||||
|
||||
|
|
@ -180,8 +180,30 @@ class RFM_TRX
|
|||
uint8_t chipVer; // [] version ID read from the RF chip
|
||||
int8_t chipTemp; // [degC] temperature read from the RF chip
|
||||
uint8_t averRSSI; // [-0.5dB]
|
||||
uint8_t dummy;
|
||||
|
||||
#ifdef WITH_RFM95
|
||||
void WriteDefaultReg(void)
|
||||
{ const uint8_t Default[64] = { 0x00, 0x01, 0x1A, 0x0B, 0x00, 0x52, 0xE4, 0xC0, 0x00, 0x0F, 0x19, 0x2B, 0x20, 0x08, 0x02, 0x0A,
|
||||
0xFF, 0x00, 0x15, 0x0B, 0x28, 0x0C, 0x12, 0x47, 0x32, 0x3E, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40,
|
||||
0x00, 0x00, 0x00, 0x00, 0x05, 0x00, 0x03, 0x93, 0x55, 0x55, 0x55, 0x55, 0x55, 0x55, 0x55, 0x55,
|
||||
0x90, 0x40, 0x40, 0x00, 0x00, 0x0F, 0x00, 0x00, 0x00, 0xF5, 0x20, 0x82, 0x00, 0x02, 0x80, 0x40 };
|
||||
WriteBytes(Default+0x01, 0x0F, 0x01);
|
||||
WriteBytes(Default+0x10, 0x10, 0x10);
|
||||
WriteBytes(Default+0x20, 0x10, 0x20);
|
||||
WriteBytes(Default+0x30, 0x10, 0x30);
|
||||
WriteWord(0x0000, 0x40);
|
||||
WriteByte(0x2D, 0x44);
|
||||
WriteByte(0x09, 0x4B);
|
||||
WriteByte(0x84, 0x4D);
|
||||
WriteByte(0x00, 0x5D);
|
||||
WriteByte(0x13, 0x61);
|
||||
WriteByte(0x0E, 0x62);
|
||||
WriteByte(0x5B, 0x63);
|
||||
WriteByte(0xDB, 0x64);
|
||||
}
|
||||
#endif
|
||||
|
||||
// private:
|
||||
static uint32_t calcSynthFrequency(uint32_t Frequency) { return (((uint64_t)Frequency<<16)+7812)/15625; }
|
||||
|
||||
public:
|
||||
|
|
@ -355,7 +377,7 @@ class RFM_TRX
|
|||
{ if(SyncTol>7) SyncTol=7;
|
||||
if(WriteSize>8) WriteSize=8;
|
||||
WriteBytes(SyncData+(8-WriteSize), WriteSize, REG_SYNCVALUE1); // write the SYNC, skip some initial bytes
|
||||
WriteByte( 0x90 | (WriteSize-1), REG_SYNCCONFIG); // write SYNC length [bytes]
|
||||
WriteByte( 0x90 | (WriteSize-1), REG_SYNCCONFIG); // write SYNC length [bytes] (or 0xB0 for reversed preamble) => p.92
|
||||
WriteWord( 9-WriteSize, REG_PREAMBLEMSB); } // write preamble length [bytes] (page 71)
|
||||
// ^ 8 or 9 ?
|
||||
#endif
|
||||
|
|
@ -366,7 +388,7 @@ class RFM_TRX
|
|||
|
||||
uint16_t ReadIrqFlags(void) { return ReadWord(REG_IRQFLAGS1); }
|
||||
|
||||
void ClearIrqFlags(void) { WriteWord(RF_IRQ_FifoOverrun | RF_IRQ_Rssi, REG_IRQFLAGS1); }
|
||||
void ClearIrqFlags(void) { WriteWord(RF_IRQ_FifoOverrun | RF_IRQ_Rssi | RF_IRQ_PreambleDetect | RF_IRQ_SyncAddrMatch, REG_IRQFLAGS1); }
|
||||
|
||||
#ifdef WITH_RFM69
|
||||
void WriteTxPower_W(int8_t TxPower=10) // [dBm] for RFM69W: -18..+13dBm
|
||||
|
|
@ -401,12 +423,12 @@ class RFM_TRX
|
|||
|
||||
void WriteTxPowerMin(void) { WriteTxPower_W(-18); } // set minimal Tx power and setup for reception
|
||||
|
||||
int Configure(int16_t Channel, const uint8_t *Sync)
|
||||
int Configure(int16_t Channel, const uint8_t *Sync, bool PW=0)
|
||||
{ WriteMode(RF_OPMODE_STANDBY); // mode = STDBY
|
||||
ClearIrqFlags();
|
||||
WriteByte( 0x02, REG_DATAMODUL); // [0x00] Packet mode, FSK, 0x02: BT=0.5, 0x01: BT=1.0, 0x03: BT=0.3
|
||||
WriteWord(0x0140, REG_BITRATEMSB); // bit rate = 100kbps
|
||||
WriteWord(0x0333, REG_FDEVMSB); // FSK deviation = +/-50kHz
|
||||
WriteWord(PW?0x0341:0x0140, REG_BITRATEMSB); // bit rate = 100kbps
|
||||
WriteWord(PW?0x013B:0x0333, REG_FDEVMSB); // FSK deviation = +/-50kHz
|
||||
setChannel(Channel); // operating channel
|
||||
WriteSYNC(8, 7, Sync); // SYNC pattern (setup for reception)
|
||||
WriteByte( 0x00, REG_PACKETCONFIG1); // [0x10] Fixed size packet, no DC-free encoding, no CRC, no address filtering
|
||||
|
|
@ -434,42 +456,95 @@ class RFM_TRX
|
|||
#if defined(WITH_RFM95) || defined(WITH_SX1272)
|
||||
|
||||
void WriteTxPower(int8_t TxPower=0)
|
||||
{ if(TxPower<2) TxPower=2;
|
||||
else if(TxPower>17) TxPower=17;
|
||||
{ if(TxPower>17)
|
||||
{ if(TxPower>20) TxPower=20;
|
||||
WriteByte(0x87, REG_PADAC);
|
||||
WriteByte(0xF0 | (TxPower-5), REG_PACONFIG); }
|
||||
else // if(TxPower>14)
|
||||
{ if(TxPower<2) TxPower=2;
|
||||
WriteByte(0x84, REG_PADAC);
|
||||
WriteByte(0xF0 | (TxPower-2), REG_PACONFIG); }
|
||||
// else
|
||||
// { if(TxPower<0) TxPower=0;
|
||||
// WriteByte(0x84, REG_PADAC);
|
||||
// WriteByte(0x70 | (TxPower+1), REG_PACONFIG); }
|
||||
|
||||
// if(TxPower<2) TxPower=2;
|
||||
// else if(TxPower>17) TxPower=17;
|
||||
// if(TxPower<=14)
|
||||
// { WriteByte(0x70 | TxPower , REG_PACONFIG);
|
||||
// }
|
||||
// else
|
||||
{ WriteByte(0xF0 | (TxPower-2), REG_PACONFIG);
|
||||
}
|
||||
// { WriteByte(0xF0 | (TxPower-2), REG_PACONFIG); }
|
||||
}
|
||||
|
||||
void WriteTxPowerMin(void) { WriteTxPower(0); }
|
||||
|
||||
int Configure(int16_t Channel, const uint8_t *Sync)
|
||||
int Configure(int16_t Channel, const uint8_t *Sync, bool PW=0)
|
||||
{ WriteMode(RF_OPMODE_STANDBY); // mode: STDBY, modulation: FSK, no LoRa
|
||||
// usleep(1000);
|
||||
WriteTxPower(0);
|
||||
// ClearIrqFlags();
|
||||
WriteWord(0x0140, REG_BITRATEMSB); // bit rate = 100kbps (32MHz/100000)
|
||||
ClearIrqFlags();
|
||||
WriteWord(PW?0x0341:0x0140, REG_BITRATEMSB); // bit rate = 100kbps (32MHz/100000) (0x0341 = 38.415kbps)
|
||||
WriteByte(0x00, REG_BITRATEFRAC); //
|
||||
// ReadWord(REG_BITRATEMSB);
|
||||
WriteWord(0x0333, REG_FDEVMSB); // FSK deviation = +/-50kHz [32MHz/(1<<19)]
|
||||
WriteWord(PW?0x013B:0x0333, REG_FDEVMSB); // FSK deviation = +/-50kHz [32MHz/(1<<19)] (0x013B = 19.226kHz)
|
||||
// ReadWord(REG_FDEVMSB);
|
||||
setChannel(Channel); // operating channel
|
||||
WriteSYNC(8, 7, Sync); // SYNC pattern (setup for reception)
|
||||
WriteByte( 0x8A, REG_PREAMBLEDETECT); // preamble detect: 1 byte, page 92
|
||||
WriteByte( 0x85, REG_PREAMBLEDETECT); // preamble detect: 1 byte, page 92 (or 0x85 ?)
|
||||
WriteByte( 0x00, REG_PACKETCONFIG1); // Fixed size packet, no DC-free encoding, no CRC, no address filtering
|
||||
WriteByte( 0x40, REG_PACKETCONFIG2); // Packet mode
|
||||
WriteByte( 51, REG_FIFOTHRESH); // TxStartCondition=FifoNotEmpty, FIFO threshold = 51 bytes
|
||||
WriteByte( 2*26, REG_PAYLOADLENGTH); // Packet size = 26 bytes Manchester encoded into 52 bytes
|
||||
WriteByte( 51, REG_FIFOTHRESH); // TxStartCondition=FifoNotEmpty, FIFO threshold = 51 bytes
|
||||
WriteWord(0x3030, REG_DIOMAPPING1); // DIO signals: DIO0=00, DIO1=11, DIO2=00, DIO3=00, DIO4=00, DIO5=11, => p.64, 99
|
||||
WriteByte( 0x4A, REG_RXBW); // +/-100kHz Rx bandwidth => p.27+67
|
||||
WriteByte( 0x02, REG_RXBW); // +/-125kHz Rx (single-side) bandwidth => p.27,67,83,90
|
||||
WriteByte( 0x02, REG_AFCBW); // +/-125kHz AFC bandwidth
|
||||
WriteByte( 0x49, REG_PARAMP); // BT=0.5 shaping, 40us ramp up/down
|
||||
WriteByte( 0x07, REG_RSSICONFIG); // 256 samples for RSSI, p.90
|
||||
WriteByte( 0x0E, REG_RXCONFIG); // => p.90 (or 0x8E ?)
|
||||
WriteByte( 0x07, REG_RSSICONFIG); // 256 samples for RSSI, no offset, => p.90,82
|
||||
WriteByte( 0x20, REG_LNA); // max. LNA gain, => p.89
|
||||
|
||||
return 0; }
|
||||
|
||||
uint8_t ReadLowBat(void) { return ReadByte(REG_LOWBAT ); }
|
||||
uint8_t ReadLowBat(void) { return ReadByte(REG_LOWBAT ); }
|
||||
|
||||
void PrintReg(void (*CONS_UART_Write)(char))
|
||||
{ Format_String(CONS_UART_Write, "RFM95 Mode:");
|
||||
uint8_t RxMode=ReadMode();
|
||||
Format_Hex(CONS_UART_Write, RxMode);
|
||||
CONS_UART_Write(' '); CONS_UART_Write('0'+DIO0_isOn());
|
||||
Format_String(CONS_UART_Write, " IRQ:");
|
||||
Format_Hex(CONS_UART_Write, ReadWord(REG_IRQFLAGS1));
|
||||
Format_String(CONS_UART_Write, " Pre:");
|
||||
Format_Hex(CONS_UART_Write, ReadWord(REG_PREAMBLEMSB));
|
||||
Format_String(CONS_UART_Write, " SYNC:");
|
||||
Format_Hex(CONS_UART_Write, ReadByte(REG_SYNCCONFIG));
|
||||
CONS_UART_Write('/');
|
||||
for(uint8_t Idx=0; Idx<8; Idx++)
|
||||
Format_Hex(CONS_UART_Write, ReadByte(REG_SYNCVALUE1+Idx));
|
||||
Format_String(CONS_UART_Write, " FREQ:");
|
||||
Format_Hex(CONS_UART_Write, ReadByte(REG_FRFMSB));
|
||||
Format_Hex(CONS_UART_Write, ReadByte(REG_FRFMID));
|
||||
Format_Hex(CONS_UART_Write, ReadByte(REG_FRFLSB));
|
||||
Format_String(CONS_UART_Write, " RATE:");
|
||||
Format_Hex(CONS_UART_Write, ReadWord(REG_BITRATEMSB));
|
||||
Format_String(CONS_UART_Write, " FDEV:");
|
||||
Format_Hex(CONS_UART_Write, ReadWord(REG_FDEVMSB));
|
||||
Format_String(CONS_UART_Write, " DIO:");
|
||||
Format_Hex(CONS_UART_Write, ReadWord(REG_DIOMAPPING1));
|
||||
Format_String(CONS_UART_Write, " CFG:");
|
||||
Format_Hex(CONS_UART_Write, ReadByte(REG_PREAMBLEDETECT));
|
||||
Format_Hex(CONS_UART_Write, ReadByte(REG_PACKETCONFIG1));
|
||||
Format_Hex(CONS_UART_Write, ReadByte(REG_PACKETCONFIG2));
|
||||
Format_Hex(CONS_UART_Write, ReadByte(REG_FIFOTHRESH));
|
||||
Format_Hex(CONS_UART_Write, ReadByte(REG_PAYLOADLENGTH));
|
||||
Format_Hex(CONS_UART_Write, ReadByte(REG_RXBW));
|
||||
Format_Hex(CONS_UART_Write, ReadByte(REG_RSSICONFIG));
|
||||
Format_String(CONS_UART_Write, " PA:");
|
||||
Format_Hex(CONS_UART_Write, ReadByte(REG_PARAMP));
|
||||
Format_Hex(CONS_UART_Write, ReadByte(REG_PACONFIG));
|
||||
Format_String(CONS_UART_Write, "\n"); }
|
||||
|
||||
#endif
|
||||
|
||||
|
|
|
|||
|
|
@ -6,6 +6,7 @@
|
|||
|
||||
#include "parameters.h"
|
||||
|
||||
#include "proc.h"
|
||||
#include "ctrl.h"
|
||||
#include "gps.h"
|
||||
|
||||
|
|
@ -13,6 +14,22 @@
|
|||
|
||||
#if defined(WITH_BMP180) || defined(WITH_BMP280) || defined(WITH_MS5607) || defined(WITH_BME280)
|
||||
|
||||
#ifdef WITH_BMP180
|
||||
#include "bmp180.h"
|
||||
#endif
|
||||
|
||||
#ifdef WITH_BMP280
|
||||
#include "bmp280.h"
|
||||
#endif
|
||||
|
||||
#ifdef WITH_BME280
|
||||
#include "bme280.h"
|
||||
#endif
|
||||
|
||||
#ifdef WITH_MS5607
|
||||
#include "ms5607.h"
|
||||
#endif
|
||||
|
||||
#include "atmosphere.h"
|
||||
#include "slope.h"
|
||||
#include "lowpass2.h"
|
||||
|
|
@ -191,7 +208,9 @@ static void ProcBaro(void)
|
|||
PosPtr->Temperature = Baro.Temperature; // and temperature in the GPS record
|
||||
#ifdef WITH_BME280
|
||||
if(Baro.hasHumidity())
|
||||
PosPtr->Humidity = Baro.Humidity;
|
||||
{ PosPtr->Humidity = Baro.Humidity;
|
||||
// PosPtr->hasHum=1;
|
||||
}
|
||||
#endif
|
||||
PosPtr->hasBaro=1; } // tick "hasBaro" flag
|
||||
}
|
||||
|
|
@ -218,7 +237,7 @@ static void ProcBaro(void)
|
|||
Line[Len++]=','; }
|
||||
#endif
|
||||
Len+=NMEA_AppendCheckCRNL(Line, Len);
|
||||
// if(CONS_UART_Free()>=128)
|
||||
if(Parameters.Verbose)
|
||||
{ xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
Format_String(CONS_UART_Write, Line, 0, Len); // send NMEA sentence to the console (UART1)
|
||||
xSemaphoreGive(CONS_Mutex); }
|
||||
|
|
@ -236,7 +255,25 @@ static void ProcBaro(void)
|
|||
Len+=Format_String(Line+Len, "m,"); // normally f for feet, but metres and m works with XcSoar
|
||||
Len+=Format_String(Line+Len, "3"); // 1 no fix, 2 - 2D, 3 - 3D; assume 3D for now
|
||||
Len+=NMEA_AppendCheckCRNL(Line, Len);
|
||||
// if(CONS_UART_Free()>=128)
|
||||
if(Parameters.Verbose)
|
||||
{ xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
Format_String(CONS_UART_Write, Line, 0, Len); // send NMEA sentence to the console (UART1)
|
||||
xSemaphoreGive(CONS_Mutex); }
|
||||
|
||||
Len=0;
|
||||
Len+=Format_String(Line+Len, "$LK8EX1,");
|
||||
Len+=Format_UnsDec(Line+Len, (uint32_t)(Pressure+2)>>2); // [Pa] pressure
|
||||
Line[Len++]=',';
|
||||
Len+=Format_SignDec(Line+Len, (StdAltitude+5)/10); // [m] standard altitude (calc. from pressure)
|
||||
Line[Len++]=',';
|
||||
Len+=Format_SignDec(Line+Len, ClimbRate); // [cm/s] climb rate
|
||||
Line[Len++]=',';
|
||||
Len+=Format_SignDec(Line+Len, (Baro.Temperature+5)/10); // [degC] temperature
|
||||
Line[Len++]=',';
|
||||
Len+=Format_UnsDec(Line+Len, (BatteryVoltage+128)>>8, 4, 3); // [mV] Battery voltage
|
||||
// Len+=Format_String(Line+Len, "999"); // [%] battery level
|
||||
Len+=NMEA_AppendCheckCRNL(Line, Len);
|
||||
if(Parameters.Verbose)
|
||||
{ xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
Format_String(CONS_UART_Write, Line, 0, Len); // send NMEA sentence to the console (UART1)
|
||||
xSemaphoreGive(CONS_Mutex); }
|
||||
|
|
@ -299,10 +336,9 @@ void vTaskSENS(void* pvParameters)
|
|||
|
||||
while(1)
|
||||
{
|
||||
if(Button_SleepRequest)
|
||||
{ vTaskDelay(1000); }
|
||||
#if defined(WITH_BMP180) || defined(WITH_BMP280) || defined(WITH_MS5607) || defined(WITH_BME280)
|
||||
ProcBaro();
|
||||
if(PowerMode) ProcBaro();
|
||||
else vTaskDelay(100);
|
||||
#else
|
||||
vTaskDelay(1000);
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -0,0 +1,11 @@
|
|||
#include "hal.h"
|
||||
|
||||
void SoundMsg(char ch);
|
||||
int SoundMsg(const char *Msg);
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C"
|
||||
#endif
|
||||
void vTaskSOUND(void* pvParameters);
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,731 @@
|
|||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include "freertos/FreeRTOS.h"
|
||||
#include "freertos/task.h"
|
||||
#include "esp_system.h"
|
||||
#include "driver/gpio.h"
|
||||
#include "driver/spi_master.h" // for SPI
|
||||
#include "esp32/rom/tjpgd.h" // for JPEG
|
||||
#include "driver/ledc.h" // for PWM backlight control
|
||||
|
||||
#include "st7789.h"
|
||||
|
||||
#include "hal.h"
|
||||
#include "format.h"
|
||||
|
||||
// #define LCD_FLIP // flip the LCD: rotate by 180deg, works only for 240x240 displays
|
||||
|
||||
// =============================================================================
|
||||
|
||||
// pins specific for this LCD
|
||||
gpio_num_t LCD_PIN_CS = GPIO_NUM_NC; // connected to ground: constantly active
|
||||
gpio_num_t LCD_PIN_DC = GPIO_NUM_0; // D/C: Command = 0, Data = 1
|
||||
gpio_num_t LCD_PIN_RST = GPIO_NUM_NC; // actually connected to system RESET
|
||||
gpio_num_t LCD_PIN_BCKL = GPIO_NUM_4; // back-light: HIGH active
|
||||
|
||||
#ifdef LCD_FLIP
|
||||
const int LCD_XOFS = 80; // [pixels]
|
||||
#endif
|
||||
|
||||
int LCD_TYPE = 0; // 0 = ST7789, 1 = ILI9341
|
||||
int LCD_WIDTH = 240; // [pixels]
|
||||
int LCD_HEIGHT = 240; // [pixels]
|
||||
|
||||
static spi_device_handle_t LCD_SPI;
|
||||
|
||||
// =============================================================================
|
||||
|
||||
/*
|
||||
typedef struct
|
||||
{ uint8_t cmd; // command
|
||||
uint8_t databytes; // Number of parameter bytes; bit 7 = delay after set; 0xFF = end of cmds.
|
||||
uint8_t data[14]; // up to 16 parameter bytes per command
|
||||
} lcd_init_cmd_t;
|
||||
|
||||
static const lcd_init_cmd_t ST7789_init_cmds[] = {
|
||||
{ 0x01, 0xC0, {0} }, // LCD software reset, wait 120ms
|
||||
{ 0x36, 1, {0b01100000} }, // Memory Data Access Control, MX=MV=1, MY=ML=MH=0, RGB=0
|
||||
{ 0x3A, 1, {0x55} }, // Interface Pixel Format, 16bits/pixel for RGB/MCU interface
|
||||
{ 0xB2, 5, {0x0c, 0x0c, 0x00, 0x33, 0x33} }, // Porch Setting
|
||||
{ 0xB7, 1, {0x45} }, // Gate Control, Vgh=13.65V, Vgl=-10.43V
|
||||
{ 0xBB, 1, {0x2B} }, // VCOM Setting, VCOM=1.175V
|
||||
{ 0xC0, 1, {0x2C} }, // LCM Control, XOR: BGR, MX, MH
|
||||
{ 0xC2, 2, {0x01, 0xff} }, // VDV and VRH Command Enable, enable=1
|
||||
{ 0xC3, 1, {0x11} }, // VRH Set, Vap=4.4+...
|
||||
{ 0xC4, 1, {0x20} }, // VDV Set, VDV=0
|
||||
{ 0xC6, 1, {0x0f} }, // Frame Rate Control, 60Hz, inversion=0
|
||||
{ 0xD0, 2, {0xA4, 0xA1} }, // Power Control 1, AVDD=6.8V, AVCL=-4.8V, VDDS=2.3V
|
||||
// Positive Voltage Gamma Control
|
||||
{ 0xE0, 14, {0xD0, 0x00, 0x05, 0x0E, 0x15, 0x0D, 0x37, 0x43, 0x47, 0x09, 0x15, 0x12, 0x16, 0x19} },
|
||||
// Negative Voltage Gamma Control
|
||||
{ 0xE1, 14, {0xD0, 0x00, 0x05, 0x0D, 0x0C, 0x06, 0x2D, 0x44, 0x40, 0x0E, 0x1C, 0x18, 0x16, 0x19} },
|
||||
{ 0x11, 0xC0, {0} }, // Sleep Out, wait 120ms
|
||||
{ 0x21, 0, {0} }, // Invert ON
|
||||
{ 0x29, 0x10, {0} }, // Display ON, wait 10ms ?
|
||||
{ 0x00, 0xFF, {0} }
|
||||
};
|
||||
*/
|
||||
|
||||
static const uint8_t ST7789_init[] = {
|
||||
// 0x00, 0x10,
|
||||
0x01, 0xE0, // LCD software reset, wait 140ms
|
||||
0x01, 0xE0, // 2nd LCD software reset, wait 140ms
|
||||
#ifdef LCD_FLIP
|
||||
0x36, 0x01, 0b10100000, // flipped by 180deg but needs horizontal offset
|
||||
#else
|
||||
0x36, 0x01, 0b01100000, // Memory Data Access Control, MX MY MV ML RGB MH 1 0
|
||||
#endif
|
||||
0x3A, 0x01, 0x55, // Interface Pixel Format, 16bits/pixel for RGB/MCU interface
|
||||
0xB2, 0x05, 0x0c, 0x0c, 0x00, 0x33, 0x33, // Porch Setting
|
||||
0xB7, 0x01, 0x45, // Gate Control, Vgh=13.65V, Vgl=-10.43V
|
||||
0xBB, 0x01, 0x2B, // VCOM Setting, VCOM=1.175V
|
||||
0xC0, 0x01, 0x2C, // LCM Control, XOR: BGR, MX, MH
|
||||
0xC2, 0x02, 0x01,0xff, // VDV and VRH Command Enable, enable=1
|
||||
0xC3, 0x01, 0x11, // VRH Set, Vap=4.4+...
|
||||
0xC4, 0x01, 0x20, // VDV Set, VDV=0
|
||||
0xC6, 0x01, 0x0f, // Frame Rate Control, 60Hz, inversion=0
|
||||
0xD0, 0x02, 0xA4, 0xA1, // Power Control 1, AVDD=6.8V, AVCL=-4.8V, VDDS=2.3V
|
||||
0xE0, 0x0E, 0xD0, 0x00, 0x05, 0x0E, 0x15, 0x0D, 0x37, 0x43, 0x47, 0x09, 0x15, 0x12, 0x16, 0x19, // Positive Voltage Gamma Control
|
||||
0xE1, 0x0E, 0xD0, 0x00, 0x05, 0x0D, 0x0C, 0x06, 0x2D, 0x44, 0x40, 0x0E, 0x1C, 0x18, 0x16, 0x19, // Negative Voltage Gamma Control
|
||||
0x11, 0xE0, // Sleep Out, wait 140ms
|
||||
0x21, 0x00, // Invert ON
|
||||
0x29, 0x20, // Display ON, wait 20ms ?
|
||||
0x00, 0xFF // terminator
|
||||
};
|
||||
|
||||
static const uint8_t ILI9341_init[] = {
|
||||
0x01, 0xD0, // Software RESET
|
||||
0xCF, 3, 0x00, 0x83, 0x30 , // Power contorl B, power control = 0, DC_ENA = 1
|
||||
0xED, 4, 0x64, 0x03, 0x12, 0x81 , // Power on sequence control, cp1 keeps 1 frame, 1st frame enable, vcl = 0, ddvdh=3, vgh=1, vgl=2, DDVDH_ENH=1
|
||||
0xE8, 3, 0x85, 0x01, 0x79 , // Driver timing control A, non-overlap=default +1, EQ=default - 1, CR=default, pre-charge=default - 1
|
||||
0xCB, 5, 0x39, 0x2C, 0x00, 0x34, 0x02 , // Power control A, Vcore=1.6V, DDVDH=5.6V
|
||||
0xF7, 1, 0x20 , // Pump ratio control, DDVDH=2xVCl
|
||||
0xEA, 2, 0x00, 0x00 , // Driver timing control, all=0 unit
|
||||
// 0xC0, 1, 0x23,
|
||||
// 0xC1, 1, 0x10,
|
||||
// 0xC5, 2, 0x3E, 0x28,
|
||||
// 0xC7, 1, 0x86,
|
||||
0xC0, 1, 0x26, // Power control 1, GVDD=4.75V
|
||||
0xC1, 1, 0x11, // Power control 2, DDVDH=VCl*2, VGH=VCl*7, VGL=-VCl*3
|
||||
0xC5, 2, 0x35, 0x3E , // VCOM control 1, VCOMH=4.025V, VCOML=-0.950V
|
||||
0xC7, 1, 0xBE, // VCOM control 2, VCOMH=VMH-2, VCOML=VML-2
|
||||
0x36, 1, 0b00001000 , // Memory Data Access Control, MX=MV=0, MY=ML=MH=0, RGB=1
|
||||
0x3A, 1, 0x55 , // Pixel format, 16bits/pixel for RGB/MCU interface
|
||||
0xB1, 2, 0x00, 0x1B , // Frame rate control, f=fosc, 70Hz fps
|
||||
// 0xF2, 1, 0x08 , // Enable 3G, disabled
|
||||
0xF2, 1, 0x00 , // 3Gamma function disable
|
||||
0x26, 1, 0x01 , // Gamma set, curve 1
|
||||
// 0x26, 1, 0x02 , // Gamma set, curve 1
|
||||
// 0xE0, 15, 0x1F, 0x1A, 0x18, 0x0A, 0x0F, 0x06, 0x45, 0X87, 0x32, 0x0A, 0x07, 0x02, 0x07, 0x05, 0x00 , // Positive gamma correction
|
||||
// 0xE1, 15, 0x00, 0x25, 0x27, 0x05, 0x10, 0x09, 0x3A, 0x78, 0x4D, 0x05, 0x18, 0x0D, 0x38, 0x3A, 0x1F , // Negative gamma correction
|
||||
0xE0, 15, 0x0f, 0x31, 0x2b, 0x0c, 0x0e, 0x08, 0x4e, 0xf1, 0x37, 0x07, 0x10, 0x03, 0x0e, 0x09, 0x00,
|
||||
0xE1, 15, 0x00, 0x0e, 0x14, 0x03, 0x11, 0x07, 0x31, 0xc1, 0x48, 0x08, 0x0f, 0x0c, 0x31, 0x36, 0x0f,
|
||||
// 0xE0, 15, 0x0F, 0x31, 0x2B, 0x0C, 0x0E, 0x08, 0x4E, 0xF1, 0x37, 0x07, 0x10, 0x03, 0x0E, 0x09, 0x00,
|
||||
// 0xE1, 15, 0x00, 0x0E, 0x14, 0x03, 0x11, 0x07, 0x31, 0xC1, 0x48, 0x08, 0x0F, 0x0C, 0x31, 0x36, 0x0F,
|
||||
0x2A, 4, 0x00, 0x00, 0x00, 0xEF , // Column address set, SC=0, EC=0xEF
|
||||
0x2B, 4, 0x00, 0x00, 0x01, 0x3f , // Page address set, SP=0, EP=0x013F
|
||||
0x2C, 0, // Memory write
|
||||
0xB7, 1, 0x07 , // Entry mode set, Low vol detect disabled, normal display
|
||||
0xB6, 4, 0x0A, 0x82, 0x27, 0x00 , // Display function control
|
||||
0x11, 0xC0, // Sleep out
|
||||
0x29, 0x20, // Display on
|
||||
0x00, 0xFF
|
||||
};
|
||||
|
||||
/* Send a command to the LCD. Uses spi_device_polling_transmit, which waits
|
||||
* until the transfer is complete.
|
||||
*
|
||||
* Since command transactions are usually small, they are handled in polling
|
||||
* mode for higher speed. The overhead of interrupt transactions is more than
|
||||
* just waiting for the transaction to complete.
|
||||
*/
|
||||
static esp_err_t lcd_cmd(const uint8_t cmd)
|
||||
{ spi_transaction_t t;
|
||||
memset(&t, 0, sizeof(t)); // Zero out the transaction
|
||||
t.length=8; // Command is 8 bits
|
||||
t.tx_buffer=&cmd; // The data is the cmd itself
|
||||
t.user=(void*)0; // D/C needs to be set to 0
|
||||
return spi_device_polling_transmit(LCD_SPI, &t); } // Transmit!
|
||||
|
||||
/* Send data to the LCD. Uses spi_device_polling_transmit, which waits until the
|
||||
* transfer is complete.
|
||||
*
|
||||
* Since data transactions are usually small, they are handled in polling
|
||||
* mode for higher speed. The overhead of interrupt transactions is more than
|
||||
* just waiting for the transaction to complete.
|
||||
*/
|
||||
|
||||
static DRAM_ATTR uint8_t ram_buff[16];
|
||||
|
||||
static esp_err_t lcd_data(const uint8_t *data, int len)
|
||||
{ // uint8_t ram_buff[len];
|
||||
memcpy(ram_buff, data, len); // copy to RAM buffer, otherwise DMA would not work
|
||||
spi_transaction_t t;
|
||||
if (len==0) return ESP_OK; // no need to send anything
|
||||
memset(&t, 0, sizeof(t)); // Zero out the transaction
|
||||
t.length=len*8; // Len is in bytes, transaction length is in bits.
|
||||
t.tx_buffer=ram_buff; // Data
|
||||
t.user=(void*)1; // D/C needs to be set to 1
|
||||
return spi_device_polling_transmit(LCD_SPI, &t); } // Transmit!
|
||||
|
||||
// This function is called (in irq context!) just before a transmission starts. It will
|
||||
// set the D/C line to the value indicated in the user field.
|
||||
static void lcd_spi_pre_transfer_callback(spi_transaction_t *t)
|
||||
{ int DC=(int)t->user;
|
||||
gpio_set_level(LCD_PIN_DC, DC);
|
||||
// if(LCD_PIN_CS>GPIO_NUM_NC) gpio_set_level(LCD_PIN_CS, 0);
|
||||
}
|
||||
|
||||
// static void lcd_spi_post_transfer_callback(spi_transaction_t *t)
|
||||
// { if(LCD_PIN_CS>GPIO_NUM_NC) gpio_set_level(LCD_PIN_CS, 1); }
|
||||
|
||||
static ledc_timer_config_t LEDC_Timer =
|
||||
{ speed_mode : LEDC_HIGH_SPEED_MODE, // timer mode
|
||||
duty_resolution : LEDC_TIMER_8_BIT, // resolution of PWM duty: 0..255
|
||||
timer_num : LEDC_TIMER_0, // timer index
|
||||
freq_hz : 1000 // frequency of PWM signal
|
||||
} ;
|
||||
|
||||
static ledc_channel_config_t LEDC_Channel =
|
||||
{ gpio_num : LCD_PIN_BCKL,
|
||||
speed_mode : LEDC_HIGH_SPEED_MODE,
|
||||
channel : LEDC_CHANNEL_1,
|
||||
intr_type : LEDC_INTR_DISABLE,
|
||||
timer_sel : LEDC_TIMER_0,
|
||||
duty : 0,
|
||||
hpoint : 0
|
||||
} ;
|
||||
|
||||
void LCD_SetBacklightLevel(uint8_t Level)
|
||||
{ if(Level>8) Level=0;
|
||||
uint8_t Duty=1; Duty<<=Level; Duty--;
|
||||
if(LCD_PIN_BCKL>GPIO_NUM_NC)
|
||||
{ ledc_set_duty(LEDC_Channel.speed_mode, LEDC_Channel.channel, Duty);
|
||||
ledc_update_duty(LEDC_Channel.speed_mode, LEDC_Channel.channel); }
|
||||
}
|
||||
|
||||
// Initialize the display
|
||||
static void lcd_gpio_init(void)
|
||||
{
|
||||
if(LCD_PIN_BCKL>GPIO_NUM_NC)
|
||||
{ LEDC_Channel.gpio_num = LCD_PIN_BCKL;
|
||||
ledc_timer_config(&LEDC_Timer); // Set configuration of timer for high speed channels
|
||||
ledc_channel_config(&LEDC_Channel);
|
||||
LCD_SetBacklightOFF(); }
|
||||
|
||||
gpio_set_direction(LCD_PIN_DC, GPIO_MODE_OUTPUT);
|
||||
|
||||
if(LCD_PIN_RST>GPIO_NUM_NC)
|
||||
{ gpio_set_direction(LCD_PIN_RST, GPIO_MODE_OUTPUT); }
|
||||
|
||||
// if(LCD_PIN_CS>GPIO_NUM_NC)
|
||||
// { gpio_set_direction(LCD_PIN_CS, GPIO_MODE_OUTPUT);
|
||||
// gpio_set_level(LCD_PIN_CS, 1); }
|
||||
}
|
||||
|
||||
static void lcd_start(const uint8_t *cmd) // reset controller and send initial config commands
|
||||
{
|
||||
if(LCD_PIN_RST>GPIO_NUM_NC)
|
||||
{ gpio_set_level(LCD_PIN_RST, 0);
|
||||
vTaskDelay(100);
|
||||
gpio_set_level(LCD_PIN_RST, 1);
|
||||
vTaskDelay(140);
|
||||
cmd+=2; } // skip Software Reset
|
||||
else
|
||||
{ } // include Software Reset
|
||||
|
||||
for( ; ; )
|
||||
{ uint8_t Cmd = *cmd++;
|
||||
uint8_t Len = *cmd++; if(Len==0xFF) break;
|
||||
uint8_t Wait = Len>>4; Len&=0x0F;
|
||||
lcd_cmd(Cmd);
|
||||
lcd_data(cmd, Len);
|
||||
if(Wait) vTaskDelay(10*Wait);
|
||||
// else vTaskDelay(1);
|
||||
cmd+=Len; }
|
||||
|
||||
}
|
||||
|
||||
// =============================================================================
|
||||
|
||||
// const int LCD_BUFF_SIZE = 12*320;
|
||||
DRAM_ATTR static uint16_t lcd_buffer[LCD_BUFF_SIZE];
|
||||
static int lcd_buffer_filled = 0; // buffer is prefilled up to this size with a fixed RGB565
|
||||
|
||||
static void lcd_buffer_fill(int size, uint16_t RGB565) // fill the buffer with given RGB565 up to the desired size
|
||||
{ if(lcd_buffer[0]!=RGB565) lcd_buffer_filled=0; // if filled with a different RGB565 then assume not filled
|
||||
if(lcd_buffer_filled>=size) return; // if filled up to the desired size then we are done
|
||||
for(int x=lcd_buffer_filled; x<size; x++) // fill up to the desired size
|
||||
{ lcd_buffer[x]=RGB565; }
|
||||
lcd_buffer_filled=size; } // mark as filled till the requested size
|
||||
|
||||
static spi_transaction_t lcd_trans[6]; // six SPI transactions to transfer pixel data
|
||||
static bool lcd_transaction_active = 0; // initially not active
|
||||
|
||||
static void lcd_trans_init(void) // initial setup of the transactions
|
||||
{ for (int x=0; x<6; x++)
|
||||
{ memset(&lcd_trans[x], 0, sizeof(spi_transaction_t));
|
||||
if ((x&1)==0) // Even transfers are commands
|
||||
{ lcd_trans[x].length = 8; // 8-bit = single byte command
|
||||
lcd_trans[x].user=(void*)0; } // D/C = LOW for command
|
||||
else // Odd transfers are data
|
||||
{ lcd_trans[x].length = 8*4; // 4 byte = arguments (except for the last transaction)
|
||||
lcd_trans[x].user=(void*)1; } // D/C = HIGH for data
|
||||
lcd_trans[x].flags=SPI_TRANS_USE_TXDATA; }
|
||||
lcd_transaction_active=0; }
|
||||
|
||||
static void lcd_trans_wait(void) // wait for the six SPI transaction to complete
|
||||
{ if(!lcd_transaction_active) return;
|
||||
spi_transaction_t *rtrans;
|
||||
for (int x=0; x<6; x++)
|
||||
{ esp_err_t ret=spi_device_get_trans_result(LCD_SPI, &rtrans, portMAX_DELAY); }
|
||||
lcd_transaction_active=0; }
|
||||
|
||||
static void lcd_trans_start(void) // start the six SPI transactions to write data to given X/Y
|
||||
{ if(lcd_transaction_active) lcd_trans_wait(); // if previous transaction set not finished, then wait for them
|
||||
for (int x=0; x<6; x++) // start the six SPI transactions
|
||||
{ esp_err_t ret=spi_device_queue_trans(LCD_SPI, &lcd_trans[x], portMAX_DELAY); }
|
||||
lcd_transaction_active=1; } // mark the transactions as beig active
|
||||
|
||||
// positions here must fit into the screen, there is no check and no correction if they don't fit
|
||||
static void lcd_trans_setup(int xpos, int ypos, int xsize, int ysize, uint16_t *data)
|
||||
{
|
||||
#ifdef LCD_FLIP
|
||||
xpos+=LCD_XOFS;
|
||||
#endif
|
||||
if(lcd_transaction_active) lcd_trans_wait(); // if previous transaction set not finished, then wait for them
|
||||
lcd_trans[0].tx_data[0] = 0x2A; // Column Address Set
|
||||
lcd_trans[1].tx_data[0] = xpos>>8; // Start Col MSB
|
||||
lcd_trans[1].tx_data[1] = xpos&0xFF; // Start Col LSB
|
||||
xpos += xsize-1;
|
||||
lcd_trans[1].tx_data[2] = xpos>>8; // End Col MSB
|
||||
lcd_trans[1].tx_data[3] = xpos&0xFF; // End Col LSB
|
||||
lcd_trans[2].tx_data[0] = 0x2B; // Page address set
|
||||
lcd_trans[3].tx_data[0] = ypos>>8; // Start page MSB
|
||||
lcd_trans[3].tx_data[1] = ypos&0xFF; // start page LSB
|
||||
ypos += ysize-1;
|
||||
lcd_trans[3].tx_data[2] = ypos>>8; // end page MSB
|
||||
lcd_trans[3].tx_data[3] = ypos&0xFF; // end page LSB
|
||||
lcd_trans[4].tx_data[0] = 0x2C; // memory write
|
||||
lcd_trans[5].tx_buffer = data; // finally send the line data
|
||||
lcd_trans[5].length = xsize*ysize*2*8; // Data length, in bits
|
||||
lcd_trans[5].rxlength = 0; // need to set this, otherwise a previous value is taken and an error occurs
|
||||
lcd_trans[5].flags=0; } // undo SPI_TRANS_USE_TXDATA flag
|
||||
|
||||
// ================================================================================
|
||||
|
||||
void LCD_DrawBox(int xpos, int ypos, int xsize, int ysize, uint16_t RGB565)
|
||||
{ if(xsize==0) return;
|
||||
if(ysize==0) return;
|
||||
if(xpos>=LCD_WIDTH) return;
|
||||
if(ypos>=LCD_HEIGHT) return;
|
||||
if((xpos+xsize)<=0) return;
|
||||
if((ypos+ysize)<=0) return;
|
||||
if(xpos<0) { xsize+=xpos; xpos=0; }
|
||||
if(ypos<0) { ysize+=ypos; ypos=0; }
|
||||
if((xpos+xsize)>LCD_WIDTH) { xsize=LCD_WIDTH-xpos; }
|
||||
if((ypos+ysize)>LCD_HEIGHT) { ysize=LCD_HEIGHT-ypos; }
|
||||
|
||||
if(lcd_transaction_active) lcd_trans_wait();
|
||||
|
||||
int lines_per_batch = LCD_BUFF_SIZE/xsize; // number of lines we can do per batch given the buffer size
|
||||
if(lines_per_batch>ysize) lines_per_batch=ysize; // if bigger than we need to do then cut it down
|
||||
int pixels_per_batch = lines_per_batch*xsize; // number of pixels per batch
|
||||
lcd_buffer_fill(pixels_per_batch, RGB565); // fill the buffer with uniform color, if not filled already
|
||||
for( ; ysize; ) // send given number of lines: do it in batches for speed
|
||||
{ if(lines_per_batch>ysize) lines_per_batch=ysize;
|
||||
lcd_trans_setup(xpos, ypos, xsize, lines_per_batch, lcd_buffer);
|
||||
lcd_trans_start();
|
||||
ypos+=lines_per_batch; ysize-=lines_per_batch; }
|
||||
}
|
||||
|
||||
// void LCD_DrawHorLine(int xpos, int ypos, int xsize, uint16_t RGB565)
|
||||
// { LCD_DrawBox(xpos, ypos, xsize, 1, RGB565); }
|
||||
|
||||
// void LCD_DrawVerLine(int xpos, int ypos, int ysize, uint16_t RGB565)
|
||||
// { LCD_DrawBox(xpos, ypos, 1, ysize, RGB565); }
|
||||
|
||||
// void LCD_DrawPixel(int xpos, int ypos, uint16_t RGB565)
|
||||
// { LCD_DrawBox(xpos, ypos, 1, 1, RGB565); }
|
||||
|
||||
// ================================================================================
|
||||
|
||||
static void swap(int &a, int &b) { int t = a; a = b; b = t; }
|
||||
|
||||
void LCD_DrawLine(int x0, int y0, int x1, int y1, uint16_t RGB565)
|
||||
{
|
||||
if (x0 == x1)
|
||||
{ if (y0 <= y1) LCD_DrawVerLine(x0, y0, y1-y0+1, RGB565);
|
||||
else LCD_DrawVerLine(x0, y1, y0-y1+1, RGB565);
|
||||
return; }
|
||||
if (y0 == y1)
|
||||
{ if (x0 <= x1) LCD_DrawHorLine(x0, y0, x1-x0+1, RGB565);
|
||||
else LCD_DrawHorLine(x1, y0, x0-x1+1, RGB565);
|
||||
return; }
|
||||
|
||||
int steep = 0;
|
||||
if (abs(y1-y0) > abs(x1-x0)) steep = 1;
|
||||
if (steep) { swap(x0, y0); swap(x1, y1); }
|
||||
if (x0>x1) { swap(x0, x1); swap(y0, y1); }
|
||||
|
||||
int dx = x1 - x0;
|
||||
int dy = abs(y1 - y0);
|
||||
int err = dx >> 1;
|
||||
int ystep = -1;
|
||||
int xs = x0;
|
||||
int dlen = 0;
|
||||
|
||||
if (y0<y1) ystep = 1;
|
||||
|
||||
if (steep)
|
||||
{ for ( ; x0<=x1; x0++)
|
||||
{ dlen++;
|
||||
err-=dy;
|
||||
if(err<0)
|
||||
{ err+=dx;
|
||||
if (dlen==1) LCD_DrawPixel(y0, xs, RGB565);
|
||||
else LCD_DrawVerLine(y0, xs, dlen, RGB565);
|
||||
dlen=0; y0+=ystep; xs=x0+1;
|
||||
}
|
||||
}
|
||||
if (dlen) LCD_DrawVerLine(y0, xs, dlen, RGB565);
|
||||
}
|
||||
else
|
||||
{ for ( ; x0<=x1; x0++)
|
||||
{ dlen++;
|
||||
err-=dy;
|
||||
if(err<0)
|
||||
{ err+=dx;
|
||||
if (dlen==1) LCD_DrawPixel(xs, y0, RGB565);
|
||||
else LCD_DrawHorLine(xs, y0, dlen, RGB565);
|
||||
dlen=0; y0+=ystep; xs=x0+1;
|
||||
}
|
||||
}
|
||||
if (dlen) LCD_DrawHorLine(xs, y0, dlen, RGB565);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void LCD_DrawCircle(int x, int y, int radius, uint16_t RGB565)
|
||||
{ int f = 1 - radius;
|
||||
int ddF_x = 1;
|
||||
int ddF_y = -2 * radius;
|
||||
int x1 = 0;
|
||||
int y1 = radius;
|
||||
|
||||
LCD_DrawPixel(x, y + radius, RGB565);
|
||||
LCD_DrawPixel(x, y - radius, RGB565);
|
||||
LCD_DrawPixel(x + radius, y, RGB565);
|
||||
LCD_DrawPixel(x - radius, y, RGB565);
|
||||
|
||||
while(x1<y1)
|
||||
{ if (f >= 0)
|
||||
{ y1--;
|
||||
ddF_y += 2;
|
||||
f += ddF_y; }
|
||||
x1++;
|
||||
ddF_x += 2;
|
||||
f += ddF_x;
|
||||
LCD_DrawPixel(x + x1, y + y1, RGB565);
|
||||
LCD_DrawPixel(x - x1, y + y1, RGB565);
|
||||
LCD_DrawPixel(x + x1, y - y1, RGB565);
|
||||
LCD_DrawPixel(x - x1, y - y1, RGB565);
|
||||
LCD_DrawPixel(x + y1, y + x1, RGB565);
|
||||
LCD_DrawPixel(x - y1, y + x1, RGB565);
|
||||
LCD_DrawPixel(x + y1, y - x1, RGB565);
|
||||
LCD_DrawPixel(x - y1, y - x1, RGB565);
|
||||
}
|
||||
}
|
||||
|
||||
// ================================================================================
|
||||
// Proportional fonts for Arduino
|
||||
|
||||
class propChar
|
||||
{ public:
|
||||
uint8_t charCode; // for example 32 for ' '
|
||||
uint8_t yOffset; // empty horizontal space on top
|
||||
uint8_t width; // core width
|
||||
uint8_t height; // core height
|
||||
uint8_t xOffset; // empty vertical space on the left
|
||||
uint8_t xDelta; // shift X-pos for the next character
|
||||
uint8_t Data[]; //
|
||||
public:
|
||||
uint16_t CoreBits(void) const { return (uint16_t)width*height; }
|
||||
uint8_t DataBytes(void) const { return (CoreBits()+7)/8; }
|
||||
} ;
|
||||
|
||||
int LCD_FontHeight(const uint8_t *propFont) { return propFont[1]; } // height of the given font is in the 2nd byte of the font header
|
||||
|
||||
static const propChar *FindChar(char Char, const uint8_t *propFont) // find given character
|
||||
{ propFont+=4; // skip the font header: 4 bytes
|
||||
for( ; ; ) // loop over the characters
|
||||
{ const propChar *Geom = (const propChar *)propFont; // pointer to the next character geometry
|
||||
if(Geom->charCode==0xFF) break; // 0xFF is terminator: no more characters
|
||||
if(Geom->charCode==(uint8_t)Char) return Geom; // if code matches: return the pointer
|
||||
propFont += 6 + Geom->DataBytes(); } // if not: skip this character and go to the next
|
||||
return 0; } // if went through all and not found: return NULL
|
||||
|
||||
// slow, because goes pixel-by-pixel, but simple to code, no big deal with out-of-screen positions
|
||||
int LCD_DrawTranspChar(char Char, int xpos, int ypos, uint16_t RGB565, const uint8_t *propFont)
|
||||
{ const propChar *Geom = FindChar(Char, propFont); if(Geom==0) return 0;
|
||||
xpos += Geom->xOffset;
|
||||
ypos += Geom->yOffset;
|
||||
const uint8_t *Data = Geom->Data;
|
||||
uint8_t Byte = 0x00;
|
||||
uint8_t Mask = 0x00;
|
||||
for(int dy=0; dy < Geom->height; dy++)
|
||||
{ for(int dx=0; dx < Geom->width; dx++)
|
||||
{ if(Mask==0) { Byte = *Data++; Mask=0x80; }
|
||||
if(Byte&Mask) LCD_DrawPixel(xpos+dx, ypos+dy, RGB565);
|
||||
Mask>>=1; }
|
||||
}
|
||||
return Geom->xDelta; } // return by how much move the cursor to draw the next character
|
||||
|
||||
int LCD_DrawTranspString(const char *String, int xpos, int ypos, uint16_t RGB565, const uint8_t *propFont)
|
||||
{ int Len = 0;
|
||||
for( ; ; )
|
||||
{ char Char = *String++; if(Char==0) break;
|
||||
Len += LCD_DrawTranspChar(Char, xpos+Len, ypos, RGB565, propFont); }
|
||||
return Len; }
|
||||
|
||||
int LCD_DrawChar(char Char, int xpos, int ypos, uint16_t Fore, uint16_t Back, const uint8_t *propFont)
|
||||
{ const propChar *Geom = FindChar(Char, propFont); if(Geom==0) return 0;
|
||||
int Width = Geom->xDelta; // the box for the character to be printed
|
||||
int Height = LCD_FontHeight(propFont);
|
||||
int CropLeft = 0; if(xpos<0) { CropLeft=(-xpos); xpos=0; Width -=CropLeft; }
|
||||
int CropTop = 0; if(ypos<0) { CropTop =(-ypos); ypos=0; Height-=CropTop ; }
|
||||
int CropRight = 0; if((xpos+Width)>LCD_WIDTH) { CropRight=xpos-LCD_WIDTH; Width-=CropRight; }
|
||||
int CropBottom = 0; if((ypos+Height)>LCD_HEIGHT) { CropBottom=ypos-LCD_HEIGHT; Height-=CropBottom; }
|
||||
if(Width<=0 || Height<=0) return Geom->xDelta;
|
||||
int Pixels = Width*Height;
|
||||
lcd_buffer_filled=0;
|
||||
for(int Pix=0; Pix<Pixels; Pix++) lcd_buffer[Pix]=Back;
|
||||
const uint8_t *Data = Geom->Data;
|
||||
uint8_t Byte = 0x00;
|
||||
uint8_t Mask = 0x00;
|
||||
for(int dy=0; dy < Geom->height; dy++)
|
||||
{ for(int dx=0; dx < Geom->width; dx++)
|
||||
{ if(Mask==0) { Byte = *Data++; Mask=0x80; }
|
||||
if(Byte&Mask)
|
||||
{ int X = Geom->xOffset+dx-CropLeft;
|
||||
int Y = Geom->yOffset+dy-CropTop;
|
||||
if( X>=0 && X<Width && Y>=0 && Y<Height)
|
||||
{ lcd_buffer[Y*Width+X] = Fore; }
|
||||
}
|
||||
Mask>>=1; }
|
||||
}
|
||||
lcd_trans_setup(xpos, ypos, Width, Height, lcd_buffer);
|
||||
lcd_trans_start();
|
||||
lcd_trans_wait();
|
||||
return Width; }
|
||||
|
||||
int LCD_CharWidth(char Char, const uint8_t *propFont)
|
||||
{ const propChar *Geom = FindChar(Char, propFont); if(Geom==0) return 0;
|
||||
return Geom->xDelta; }
|
||||
|
||||
int LCD_StringWidth(const char *String, const uint8_t *propFont)
|
||||
{ int Len = 0;
|
||||
for( ; ; )
|
||||
{ char Char = *String++; if(Char==0) break;
|
||||
Len += LCD_CharWidth(Char, propFont); }
|
||||
return Len; }
|
||||
|
||||
// draw opaque characters of a string
|
||||
int LCD_DrawString(const char *String, int xpos, int ypos, uint16_t Fore, uint16_t Back, const uint8_t *propFont)
|
||||
{ int Len = 0;
|
||||
for( ; ; )
|
||||
{ char Char = *String++; if(Char==0) break;
|
||||
Len += LCD_DrawChar(Char, xpos+Len, ypos, Fore, Back, propFont); }
|
||||
return Len; }
|
||||
|
||||
// draw only those characters of a string which have changed to minimize the LCD transfer thus maximize the speed
|
||||
int LCD_UpdateString(const char *String, const char *RefString, int xpos, int ypos, uint16_t Fore, uint16_t Back, const uint8_t *propFont)
|
||||
{ int Len = 0;
|
||||
for( ; ; )
|
||||
{ char Char = *String; if(Char) String++;
|
||||
char RefChar = *RefString; if(RefChar) RefString++;
|
||||
if(Char==0 && RefChar==0) break;
|
||||
if(Char==RefChar)
|
||||
{ Len += LCD_CharWidth(Char, propFont); }
|
||||
else
|
||||
{ if(Char) { Len += LCD_DrawChar(Char, xpos+Len, ypos, Fore, Back, propFont); }
|
||||
else { int Width = LCD_CharWidth(RefChar, propFont);
|
||||
LCD_DrawBox(xpos+Len, ypos, Width, LCD_FontHeight(propFont), Back);
|
||||
Len+=LCD_CharWidth(RefChar, propFont); }
|
||||
}
|
||||
}
|
||||
return Len; }
|
||||
|
||||
// =============================================================================
|
||||
|
||||
typedef struct
|
||||
{ int16_t xpos; // top left point X position on the LCD
|
||||
int16_t ypos; // top left point Y position on the LCD
|
||||
const uint8_t * Input; // memory buffer containing the image
|
||||
uint32_t InpSize; // size of the input image
|
||||
uint32_t InpPtr; // input image current position
|
||||
} JPGIODEV;
|
||||
|
||||
static UINT tjd_buf_input (
|
||||
JDEC* jd, // Decompression object
|
||||
BYTE* buff, // Pointer to the read buffer (NULL:skip)
|
||||
UINT nd ) // Number of bytes to read/skip from input stream
|
||||
{ JPGIODEV *dev = (JPGIODEV*)jd->device;
|
||||
// CONS_UART_Write('i');
|
||||
// Format_Hex(CONS_UART_Write, (uint32_t)(dev->Input));
|
||||
// CONS_UART_Write(':');
|
||||
// Format_Hex(CONS_UART_Write, (uint32_t)(buff));
|
||||
// CONS_UART_Write(' ');
|
||||
// return 0;
|
||||
if(!dev->Input) return 0; // if Input pointer is NULL (can it be ?)
|
||||
if(dev->InpPtr >= dev->InpSize) return 0; // if past the InpSize: end of stream
|
||||
if( (dev->InpPtr+nd) > dev->InpSize) nd = dev->InpSize-dev->InpPtr;
|
||||
if(buff) // if not NULL pointer then copy, otherwise just skip
|
||||
{ memcpy(buff, dev->Input + dev->InpPtr, nd); } // copy the nd bytes of the input stream
|
||||
dev->InpPtr += nd; // incremeant the input pointer
|
||||
return nd; } // return the number of bytes copied or skipped
|
||||
|
||||
static UINT tjd_output (
|
||||
JDEC* jd, // Decompression object of current session
|
||||
void* bitmap, // Bitmap data to be output
|
||||
JRECT* rect // Rectangular region to output
|
||||
)
|
||||
{ // Device identifier for the session (5th argument of jd_prepare function)
|
||||
JPGIODEV *dev = (JPGIODEV*)jd->device;
|
||||
// CONS_UART_Write('o');
|
||||
|
||||
int Lx0 = rect->left + dev->xpos; // coordinates on the screen
|
||||
int Ly0 = rect->top + dev->ypos;
|
||||
int Lx1 = rect->right+1 + dev->xpos; //
|
||||
int Ly1 = rect->bottom+1 + dev->ypos;
|
||||
int Jxs = Lx1-Lx0; // X-size of the JPEG rectagle
|
||||
int Jys = Ly1-Ly0; // Y-size of the JPEG rectagle
|
||||
|
||||
int CropLeft = 0; if(Lx0<0) { CropLeft = (-Lx0); Lx0=0; } // how much crop the JPEG rectangle on the left
|
||||
int CropTop = 0; if(Ly0<0) { CropTop = (-Ly0); Ly0=0; } // how much crop the JPEG rectangle on the right
|
||||
int CropRight = 0; if(Lx1>LCD_WIDTH) { CropRight = LCD_WIDTH-Lx1; Lx1=LCD_WIDTH; } //
|
||||
int CropBottom = 0; if(Ly1>LCD_HEIGHT) { CropBottom = LCD_HEIGHT-Ly1; Ly1=LCD_HEIGHT; } //
|
||||
int Llines = Ly1-Ly0; // number of lines going to LCD
|
||||
int Lrows = Lx1-Lx0; // number of pixels per line going to LCD
|
||||
if(Llines<=0) return 1;
|
||||
if(Lrows<=0) return 1;
|
||||
|
||||
uint16_t *Dst = lcd_buffer; // buffer to form RGB565 for transfer
|
||||
uint8_t *Src = (uint8_t *)bitmap; // RGB from JPEG decoder
|
||||
if(CropTop) Src += CropTop*3*Jxs; // Advance by the nuber of lines to skip
|
||||
for(int Line=0; Line<Llines; Line++) // Loop over line to display
|
||||
{ uint8_t *Ptr = Src + 3*CropLeft; // advance by the pixels to skip on the left
|
||||
for(int Row=0; Row<Lrows; Row++) // loop over pixel in this line
|
||||
{ Dst[Row] = RGB565(Ptr); Ptr+=3; } // convert to RGB565 and store in the buffer
|
||||
Src+=3*Jxs; Dst+=Lrows; } // advance by the number of pixels
|
||||
lcd_buffer_filled=0;
|
||||
|
||||
lcd_trans_setup(Lx0, Ly0, Llines, Lrows, lcd_buffer);
|
||||
lcd_trans_start();
|
||||
|
||||
return 1; } // continue with decompression
|
||||
|
||||
void LCD_DrawJPEG(const uint8_t *JPEG, int JPEGsize, int xpos, int ypos, int Scale)
|
||||
{ if(Scale<0) Scale=0;
|
||||
else if(Scale>3) Scale=3;
|
||||
JPGIODEV dev; // input/output device
|
||||
JDEC decoder; // Decompression object (70 bytes)
|
||||
// CONS_UART_Write('J');
|
||||
const UINT WorkSize = 3800;
|
||||
char *Work = (char *)malloc(WorkSize); // JPEG decode work space
|
||||
if(Work==0) return;
|
||||
// CONS_UART_Write('P');
|
||||
|
||||
dev.Input = JPEG;
|
||||
dev.InpSize = JPEGsize;
|
||||
dev.InpPtr = 0;
|
||||
dev.xpos = xpos;
|
||||
dev.ypos = ypos;
|
||||
|
||||
if(jd_prepare(&decoder, tjd_buf_input, (void *)Work, WorkSize, &dev)!=JDR_OK) { free(Work); return ; }
|
||||
// CONS_UART_Write('E');
|
||||
JRESULT rc = jd_decomp(&decoder, tjd_output, Scale);
|
||||
// CONS_UART_Write('G');
|
||||
free(Work); }
|
||||
|
||||
// ================================================================================
|
||||
|
||||
void LCD_Start(void)
|
||||
{ // if(LCD_TYPE==1) lcd_start(ILI9341_init); // reset, send initial commands
|
||||
// else lcd_start(ST7789_init); // reset, send initial commands
|
||||
if(LCD_TYPE==1) lcd_start(ILI9341_init); // reset, send initial commands
|
||||
else lcd_start(ST7789_init); // reset, send initial commands
|
||||
lcd_trans_init(); // initialize SPI transactions
|
||||
LCD_DrawBox(0, 0, LCD_WIDTH, LCD_HEIGHT, RGB565_WHITE); // screen all-white
|
||||
}
|
||||
|
||||
void LCD_Init(spi_host_device_t LCD_SPI_HOST, uint8_t LCD_SPI_MODE, int LCD_SPI_SPEED) // Initialize SPI and LCD
|
||||
{
|
||||
spi_device_interface_config_t DevConfig = // specific device on the SPI bus
|
||||
{
|
||||
.command_bits = 0,
|
||||
.address_bits = 0,
|
||||
.dummy_bits = 0,
|
||||
.mode = LCD_SPI_MODE, // SPI mode 3, but cna be 0 for other displays
|
||||
.duty_cycle_pos = 0,
|
||||
.cs_ena_pretrans = 0,
|
||||
.cs_ena_posttrans = 0,
|
||||
.clock_speed_hz = LCD_SPI_SPEED,
|
||||
.input_delay_ns = 0, // seems to help with the reliability
|
||||
.spics_io_num = LCD_PIN_CS, // CS pin
|
||||
.flags = 0,
|
||||
.queue_size = 8, // We want to be able to queue 7 transactions at a time
|
||||
.pre_cb = lcd_spi_pre_transfer_callback, // Specify pre-transfer callback to handle D/C line
|
||||
.post_cb = 0 // lcd_spi_post_transfer_callback
|
||||
};
|
||||
|
||||
esp_err_t ret = spi_bus_add_device(LCD_SPI_HOST, &DevConfig, &LCD_SPI); // Attach the LCD to the SPI bus
|
||||
|
||||
lcd_gpio_init(); // setup GPIO
|
||||
|
||||
// LCD_Start();
|
||||
|
||||
LCD_SetBacklightLevel(8); // max. backlight to signal power-on
|
||||
|
||||
// LCD_DrawJPEG(OGN_logo_jpg, OGN_logo_size, 0, 0); // draw logo
|
||||
// // LCD_DrawJPEG(Club_logo_jpg, Club_logo_size, 0, 0);
|
||||
/*
|
||||
LCD_DrawBox(LCD_SPI, 0, 0, LCD_WIDTH, LCD_HEIGHT, RGB565_RED);
|
||||
LCD_DrawBox(LCD_SPI, 32, 32, LCD_WIDTH-64, LCD_HEIGHT-64, RGB565_BLUE);
|
||||
LCD_DrawBox(LCD_SPI, 64, 64, LCD_WIDTH-128, LCD_HEIGHT-128, RGB565_GREEN);
|
||||
*/
|
||||
/*
|
||||
LCD_DrawLine( 0, 240, 240, 0, RGB565_MAGENTA);
|
||||
LCD_DrawLine( 0, 180, 240, 60, RGB565_MAGENTA);
|
||||
LCD_DrawLine( 0, 120, 240, 120, RGB565_MAGENTA);
|
||||
LCD_DrawLine( 0, 60, 240, 180, RGB565_MAGENTA);
|
||||
LCD_DrawLine( 0, 0, 240, 240, RGB565_MAGENTA);
|
||||
LCD_DrawLine( 180, 0, 60, 240, RGB565_MAGENTA);
|
||||
LCD_DrawLine( 120, 0, 120, 240, RGB565_MAGENTA);
|
||||
LCD_DrawLine( 60, 0, 180, 240, RGB565_MAGENTA);
|
||||
|
||||
LCD_DrawCircle(120, 120, 25, RGB565_MAGENTA);
|
||||
LCD_DrawCircle(120, 120, 50, RGB565_MAGENTA);
|
||||
LCD_DrawCircle(120, 120, 75, RGB565_MAGENTA);
|
||||
LCD_DrawCircle(120, 120, 100, RGB565_MAGENTA);
|
||||
*/
|
||||
// LCD_DrawString("OGN-Tracker", 40, 210, RGB565_BLUE, RGB565_PINK, tft_Dejavu24);
|
||||
// LCD_DrawTranspString("OGN-Tracker", 40, 210, RGB565_BLUE, tft_Dejavu24);
|
||||
// LCD_DrawTranspString("with T-Beam", 4, 20, RGB565_BLACK, tft_minya24);
|
||||
// LCD_DrawString("Test Test Test Test Test Test Test Test Test", -10, -5, RGB565_BLUE, RGB565_PINK, tft_Dejavu24);
|
||||
|
||||
// LCD_clearDisplay(RGB565_GREEN);
|
||||
}
|
||||
|
||||
void LCD_ClearDisplay(uint16_t RGB565)
|
||||
{ LCD_DrawBox(0, 0, LCD_WIDTH, LCD_HEIGHT, RGB565); }
|
||||
|
||||
uint16_t RGB565(uint8_t Red, uint8_t Green, uint8_t Blue) // convert 8/8/8-bit RGB to 5/6/5-bit RGB
|
||||
{ uint16_t RGB = (Red>>3);
|
||||
RGB = (RGB<<5) | (Green>>2);
|
||||
RGB = (RGB<<6) | (Blue>>3);
|
||||
return (RGB>>8) | (RGB<<8); }
|
||||
|
||||
// uint16_t RGB565(uint8_t *RGB)
|
||||
// { return RGB565(RGB[0], RGB[1], RGB[2]); }
|
||||
|
|
@ -0,0 +1,102 @@
|
|||
#ifndef __ST7789_H__
|
||||
#define __ST7789_H__
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#include "driver/gpio.h"
|
||||
#include "driver/spi_master.h"
|
||||
|
||||
// some predefine colors Red Green Blue
|
||||
const uint16_t RGB565_BLACK = 0x0000; // 0, 0, 0
|
||||
const uint16_t RGB565_LIGHTGREY = 0x18C6; // 192, 192, 192
|
||||
const uint16_t RGB565_DARKGREY = 0xEF7B; // 128, 128, 128
|
||||
const uint16_t RGB565_WHITE = 0xFFFF; // 255, 255, 255
|
||||
|
||||
const uint16_t RGB565_NAVY = 0x0F00; // 0, 0, 128
|
||||
const uint16_t RGB565_BLUE = 0x1F00; // 0, 0, 255
|
||||
const uint16_t RGB565_LIGHTBLUE = 0xFF7B; // 127, 127, 255
|
||||
|
||||
// const uint16_t RGB565_DARKGREEN = 0x0004; // 0, 128, 0
|
||||
const uint16_t RGB565_DARKGREEN = 0x0006; // 0, 192, 0
|
||||
const uint16_t RGB565_GREEN = 0xE007; // 0, 255, 0
|
||||
|
||||
const uint16_t RGB565_DARKCYAN = 0xEF03; // 0, 128, 128
|
||||
const uint16_t RGB565_CYAN = 0xFF07; // 0, 255, 255
|
||||
|
||||
const uint16_t RGB565_MAROON = 0x0078; // 128, 0, 0
|
||||
const uint16_t RGB565_RED = 0x00F8; // 255, 0, 0
|
||||
const uint16_t RGB565_LIGHTRED = 0xEFFB; // 255, 127, 127
|
||||
|
||||
const uint16_t RGB565_PURPLE = 0x0F78; // 128, 0, 128
|
||||
const uint16_t RGB565_MAGENTA = 0x1FF8; // 255, 0, 255
|
||||
|
||||
const uint16_t RGB565_OLIVE = 0xE07B; // 128, 128, 0 ?
|
||||
|
||||
// const uint16_t RGB565_DARKYELLOW = 0x0084; // 128, 128, 0
|
||||
const uint16_t RGB565_DARKYELLOW = 0x00C6; // 192, 192, 0
|
||||
const uint16_t RGB565_YELLOW = 0xE0FF; // 255, 255, 0
|
||||
const uint16_t RGB565_ORANGE = 0xA0FD; // 255, 180, 0
|
||||
const uint16_t RGB565_DARKORANGE = 0xC082; // 128, 90, 0
|
||||
const uint16_t RGB565_GREENYELLOW = 0xE0B7; // 180, 255, 0
|
||||
const uint16_t RGB565_DARKGREENYELLOW = 0x005C; // 90, 128, 0
|
||||
|
||||
const uint16_t RGB565_PINK = 0x9FFC;
|
||||
|
||||
// python formula: "%04X" % ( ((Red>>3)<<11) | ((Green>>2)<<5) | (Blue>>3) )
|
||||
// and then swap the MSB and LSB bytes
|
||||
|
||||
uint16_t RGB565(uint8_t Red, uint8_t Green, uint8_t Blue); // create RGB565 from RGB888
|
||||
inline uint16_t RGB565(const uint8_t *RGB) { return RGB565(RGB[0], RGB[1], RGB[2]); }
|
||||
|
||||
// Embedded fonts
|
||||
extern uint8_t tft_SmallFont[];
|
||||
extern uint8_t tft_DefaultFont[];
|
||||
extern uint8_t tft_Dejavu18[];
|
||||
extern uint8_t tft_Dejavu24[];
|
||||
extern uint8_t tft_Ubuntu16[];
|
||||
extern uint8_t tft_Comic24[];
|
||||
extern uint8_t tft_minya24[];
|
||||
extern uint8_t tft_tooney32[];
|
||||
extern uint8_t tft_def_small[];
|
||||
|
||||
// pins specific for this LCD
|
||||
extern gpio_num_t LCD_PIN_CS; // connected to ground: constantly active
|
||||
extern gpio_num_t LCD_PIN_DC; // D/C: Command = 0, Data = 1
|
||||
extern gpio_num_t LCD_PIN_RST; // actually connected to system RESET
|
||||
extern gpio_num_t LCD_PIN_BCKL; // back-light: HIGH active
|
||||
|
||||
extern int LCD_TYPE; // 0 = ST7789
|
||||
extern int LCD_WIDTH; // [pixels]
|
||||
extern int LCD_HEIGHT; // [pixels]
|
||||
|
||||
const int LCD_BUFF_SIZE = 12*320;
|
||||
|
||||
void LCD_Init(spi_host_device_t LCD_SPI_HOST, uint8_t LCD_SPI_MODE, int LCD_SPI_SPEED=10000000 /*, int LCD_TYPE=0 */ );
|
||||
void LCD_Start(void);
|
||||
void LCD_ClearDisplay(uint16_t RGB565);
|
||||
|
||||
void LCD_SetBacklightLevel(uint8_t Level);
|
||||
inline void LCD_SetBacklightON(void) { LCD_SetBacklightLevel(8); }
|
||||
inline void LCD_SetBacklightOFF(void) { LCD_SetBacklightLevel(0); }
|
||||
|
||||
void LCD_DrawBox(int xpos, int ypos, int xsize, int ysize, uint16_t RGB565);
|
||||
|
||||
void LCD_DrawLine(int x0, int y0, int x1, int y1, uint16_t RGB565);
|
||||
inline void LCD_DrawPixel(int xpos, int ypos, uint16_t RGB565) { LCD_DrawBox(xpos, ypos, 1, 1, RGB565); }
|
||||
inline void LCD_DrawHorLine(int xpos, int ypos, int xsize, uint16_t RGB565) { LCD_DrawBox(xpos, ypos, xsize, 1, RGB565); }
|
||||
inline void LCD_DrawVerLine(int xpos, int ypos, int ysize, uint16_t RGB565) { LCD_DrawBox(xpos, ypos, 1, ysize, RGB565); }
|
||||
|
||||
void LCD_DrawCircle(int x, int y, int radius, uint16_t RGB565);
|
||||
|
||||
void LCD_DrawJPEG(const uint8_t *JPEG, int JPEGsize, int xpos=0, int ypos=0, int Scale=0);
|
||||
|
||||
int LCD_DrawTranspChar(char Char, int xpos, int ypos, uint16_t RGB565, const uint8_t *propFont = tft_Dejavu24);
|
||||
int LCD_DrawTranspString(const char *String, int xpos, int ypos, uint16_t RGB565, const uint8_t *propFont = tft_Dejavu24);
|
||||
int LCD_DrawChar(char Char, int xpos, int ypos, uint16_t Fore, uint16_t Back, const uint8_t *propFont = tft_Dejavu24);
|
||||
int LCD_DrawString(const char *String, int xpos, int ypos, uint16_t Fore, uint16_t Back, const uint8_t *propFont = tft_Dejavu24);
|
||||
int LCD_UpdateString(const char *String, const char *RefString, int xpos, int ypos, uint16_t Fore, uint16_t Back, const uint8_t *propFont = tft_Dejavu24);
|
||||
int LCD_CharWidth(char Char, const uint8_t *propFont = tft_Dejavu24);
|
||||
int LCD_StringWidth(const char *String, const uint8_t *propFont = tft_Dejavu24);
|
||||
int LCD_FontHeight(const uint8_t *propFont = tft_Dejavu24);
|
||||
|
||||
#endif // __ST7789_H__
|
||||
|
|
@ -101,6 +101,12 @@ class UBX_NAV_TIMEUTC // 0x01 0x21
|
|||
uint8_t valid; // bits: 0:ToW, 1:WN, 2:UTC
|
||||
} ;
|
||||
|
||||
class UBX_RXM_PMREQ // 0x02 0x41
|
||||
{ public:
|
||||
uint32_t duration; // [ms]
|
||||
uint32_t flags; // bit #1 = enter backup mode
|
||||
} ;
|
||||
|
||||
class UBX_CFG_CFG
|
||||
{ public:
|
||||
uint32_t clearMask;
|
||||
|
|
|
|||
Ładowanie…
Reference in New Issue