kopia lustrzana https://github.com/pjalocha/esp32-ogn-tracker
Add pressure sensor support, push I2C to HAL, major files identical to STM32 code for easy passing of new features
Pawel Jalocha
rodzic
367ba6d4ce
commit
8bf73761c1
|
|
@ -0,0 +1,5 @@
|
|||
#include "atmosphere.h"
|
||||
|
||||
// 200, 300, 400, 500, 600, 700, 800, 900, 1000, 1100 // [ hPa]
|
||||
const int32_t Atmosphere::StdAltTable[10] = { 117764, 91652, 71864, 55752, 42070, 30126, 19493, 9886, 1109, -6984 } ; // [0.1 m]
|
||||
|
||||
|
|
@ -0,0 +1,58 @@
|
|||
#include <math.h>
|
||||
#include <stdint.h>
|
||||
|
||||
class Atmosphere
|
||||
{ public:
|
||||
// int32_t Pressure; // [ Pa ]
|
||||
// int32_t Altitude; // [0.1 m ]
|
||||
// int32_t Temperature; // [0.1 degC]
|
||||
|
||||
// altitude vs. pressure // 20000, 30000, 40000, 50000, 60000, 70000, 80000, 90000, 100000, 110000 // [ Pa]
|
||||
static const int32_t StdAltTable[10]; // = { 117764, 91652, 71864, 55752, 42070, 30126, 19493, 9886, 1109, -6984 } ; // [0.1 m]
|
||||
|
||||
public:
|
||||
// dH/dP = -R/g*T => R = 287.04m^2/K/sec^2, g = 9.80655m/s^2
|
||||
|
||||
static int32_t PressureLapseRate(int32_t Pressure, int32_t Temperature=150) // [Pa], [0.1 degC] => [0.0001 m/Pa]
|
||||
{ return -((int32_t)29270*(Temperature+2732)+Pressure/2)/Pressure; }
|
||||
// int32_t PressureLapseRate(void) { return PressureLapseRate(Pressure, Temperature); }
|
||||
|
||||
static const int32_t StdPressureAtSeaLevel = 101325; // [Pa]
|
||||
static const int32_t StdPressureAt11km = 22632; // [Pa]
|
||||
static const int32_t StdPressureAt20km = 5475; // [Pa]
|
||||
static const int32_t StdTemperatureLapseRate = -65; // [0.1 degC/1000m] valid till 11km
|
||||
static const int32_t StdTemperatureAtSeaLevel = 150; // [0.1 degC ]
|
||||
static const int32_t StdTemperatureAt11km = -565; // [0.1 degC ]
|
||||
|
||||
static int32_t StdTemperature(int32_t Altitude) // [0.1 m ] valid till 20km
|
||||
{ if(Altitude>110000) return StdTemperatureAt11km;
|
||||
return StdTemperatureAtSeaLevel+(StdTemperatureLapseRate*Altitude-5000)/10000; }
|
||||
|
||||
static int32_t AltitudeDelta(int32_t PressureDelta, int32_t PressureLapseRate) // [Pa], [0.0001 m/Pa]
|
||||
{ return (PressureDelta*PressureLapseRate)/100; } // [0.01m]
|
||||
|
||||
static int32_t AltitudeDelta(int32_t PressureDelta, int32_t Pressure, int32_t Temperature) // [Pa], [Pa], [0.1degC]
|
||||
{ int32_t PLR=PressureLapseRate(Pressure, Temperature); return AltitudeDelta(PressureDelta, PLR); } // [0.01m]
|
||||
|
||||
static int32_t StdAltitude(int32_t Pressure, int32_t PressStep=100) // [Pa]
|
||||
{ int32_t Idx=(Pressure+5000)/10000; Idx-=2;
|
||||
if(Idx<0) Idx=0; else if(Idx>9) Idx=9;
|
||||
int32_t Press = 10000*(Idx+2);
|
||||
int32_t Altitude = 10*StdAltTable[Idx];
|
||||
for( ; ; )
|
||||
{ int32_t Temp=StdTemperature(Altitude/10);
|
||||
int32_t Delta=Pressure-Press; if(Delta==0) break;
|
||||
if(Delta>PressStep) Delta=PressStep;
|
||||
else if(Delta<(-PressStep)) Delta=(-PressStep);
|
||||
Altitude+=AltitudeDelta(Delta, Press, Temp);
|
||||
Press+=Delta;
|
||||
}
|
||||
return Altitude/10; } // [0.1m]
|
||||
|
||||
#ifdef NO_RTOS
|
||||
static int32_t StdAltitude_float(int32_t Pressure)
|
||||
{ return floor(443300*(1-powf((float)Pressure/(float)101325.0, (float)0.190295))+0.5); }
|
||||
#endif
|
||||
|
||||
|
||||
} ;
|
||||
|
|
@ -0,0 +1,146 @@
|
|||
#include <stdint.h>
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "hal.h"
|
||||
|
||||
class BMP180
|
||||
{ private:
|
||||
public:
|
||||
static const uint8_t ADDR = 0x77; // BMP180 I2C address
|
||||
private:
|
||||
static const uint8_t REG_CALIB = 0xAA; // calibration register: 11x16bit (MSB first)
|
||||
static const uint8_t REG_ID = 0xD0; // ID register: always reads 0x55
|
||||
static const uint8_t REG_RESET = 0xE0; // write 0xB6 to perform soft-reset
|
||||
static const uint8_t REG_MEAS = 0xF4; // measurement control: SSCMMMMM SS=oversampling, C=conversion, MMMMM=mode
|
||||
static const uint8_t MEAS_TEMP = 0x2E; // measure temperature
|
||||
static const uint8_t MEAS_PRESS = 0x34; // measure pressure
|
||||
static const uint8_t MEAS_BUSY = 0x20; // measurement-busy flag
|
||||
static const uint8_t REG_ADC = 0xF6; // ADC result: 2 or 3 bytes
|
||||
static const uint8_t REG_ADC_MSB = 0xF6; // ADC result: MSB
|
||||
static const uint8_t REG_ADC_LSB = 0xF7; // ADC result: LSB
|
||||
static const uint8_t REG_ADC_XLSB = 0xF8; // ADC result: more LSB
|
||||
static const uint8_t MEAS_OSS = 0x03; // oversampling factor: 0=single, 1=two, 2=four, 3=eight samples
|
||||
public:
|
||||
uint8_t Bus; // which I2C bus
|
||||
private:
|
||||
union
|
||||
{ int16_t Calib[11];
|
||||
struct
|
||||
{ int16_t AC1, AC2, AC3; // 11 calibration values from EEPROM
|
||||
uint16_t AC4, AC5, AC6;
|
||||
int16_t B1, B2;
|
||||
int16_t MB, MC, MD;
|
||||
} ;
|
||||
} ;
|
||||
int32_t B5; // temperature compensation for pressure ?
|
||||
public:
|
||||
uint8_t Error; // error on the I2C bus (0=no error)
|
||||
uint16_t RawTemp; // raw temperature - to be processed
|
||||
int16_t Temperature; // [0.1 degC] after processing
|
||||
uint32_t RawPress; // raw pressure - to be processed
|
||||
uint32_t Pressure; // [ Pa ] after processing
|
||||
|
||||
private:
|
||||
static uint16_t SwapBytes(uint16_t Word) { return (Word>>8) | (Word<<8); }
|
||||
static uint16_t SwapBytes( int16_t Word) { return SwapBytes((uint16_t)Word); }
|
||||
static void SwapBytes(uint16_t *Word, uint8_t Bytes)
|
||||
{ uint8_t Words=Bytes>>1;
|
||||
for(uint8_t Idx=0; Idx<Words; Idx++)
|
||||
Word[Idx]=SwapBytes(Word[Idx]);
|
||||
}
|
||||
|
||||
public:
|
||||
|
||||
void DefaultCalib(void) // set default calibration constants
|
||||
{ AC1 = 408;
|
||||
AC2 = -72;
|
||||
AC3 = -14383;
|
||||
AC4 = 32741;
|
||||
AC5 = 32757;
|
||||
AC6 = 23153;
|
||||
B1 = 6190;
|
||||
B2 = 4;
|
||||
MB = -32768;
|
||||
MC = -8711;
|
||||
MD = 2868; }
|
||||
|
||||
uint8_t CheckID(void) // check ID, to make sure the BMP180 is connected and works correctly
|
||||
{ uint8_t ID;
|
||||
Error=I2C_Read(Bus, ADDR, REG_ID, ID); if(Error) return Error;
|
||||
return ID!=0x55; } // 0 => no error and correct ID
|
||||
|
||||
uint8_t ReadCalib(void) // read the calibration constants from the EEPROM
|
||||
{ Error=I2C_Read(Bus, ADDR, REG_CALIB, (uint8_t *)Calib, 2*11); if(Error) return Error;
|
||||
SwapBytes((uint16_t *)Calib, 2*11 ); return Error; }
|
||||
|
||||
uint8_t ReadReady(void) // check if temperature or pressure conversion is done
|
||||
{ uint8_t MeasCtrl;
|
||||
Error=I2C_Read(Bus, ADDR, REG_MEAS, MeasCtrl); if(Error) return Error;
|
||||
return (MeasCtrl&MEAS_BUSY)==0; } // 0 = busy, 1 => ready
|
||||
|
||||
uint8_t WaitReady(uint8_t Timeout=4, uint8_t Wait=4) // wait for the conversion to be ready
|
||||
{ vTaskDelay(Wait);
|
||||
for(; Timeout; Timeout--)
|
||||
{ uint8_t Err=ReadReady(); if(Err>1) return Err;
|
||||
if(Err==1) return 0;
|
||||
vTaskDelay(1); }
|
||||
return 0xFF; }
|
||||
|
||||
uint8_t TriggRawTemperature(void) // start a temperature measurement
|
||||
{ uint8_t Ctrl=MEAS_TEMP;
|
||||
Error=I2C_Write(Bus, ADDR, REG_MEAS, Ctrl); return Error; }
|
||||
|
||||
uint8_t ReadRawTemperature(void)
|
||||
{ Error=I2C_Read(Bus, ADDR, REG_ADC, RawTemp); if(Error) return Error;
|
||||
RawTemp=SwapBytes(RawTemp); return 0; }
|
||||
|
||||
uint8_t AcquireRawTemperature(void) //
|
||||
{ uint8_t Err=TriggRawTemperature(); if(Err) return Err;
|
||||
Err=WaitReady(4, 4); if(Err) return Err;
|
||||
return ReadRawTemperature(); }
|
||||
|
||||
uint8_t TrigRawPressure(void) // start a pressure measurement
|
||||
{ uint8_t Ctrl=(MEAS_OSS<<6) | MEAS_PRESS;
|
||||
Error=I2C_Write(Bus, ADDR, REG_MEAS, Ctrl); return Error; }
|
||||
|
||||
uint8_t ReadRawPressure(void)
|
||||
{ RawPress=0;
|
||||
Error=I2C_Read(Bus, ADDR, REG_ADC, (uint8_t *)(&RawPress), 3); if(Error) return Error;
|
||||
RawPress = ((RawPress<<16)&0xFF0000) | (RawPress&0x00FF00) | ((RawPress>>16)&0x0000FF);
|
||||
RawPress>>=(8-MEAS_OSS);
|
||||
return 0; }
|
||||
|
||||
uint8_t AcquireRawPressure(void)
|
||||
{ uint8_t Err=TrigRawPressure(); if(Err) return Err;
|
||||
uint8_t Timeout = (((uint8_t)1<<MEAS_OSS)+2)<<1;
|
||||
Err=WaitReady(Timeout, Timeout); if(Err) return Err;
|
||||
return ReadRawPressure(); }
|
||||
|
||||
// temperature and pressure calculation routines takes from the datasheet
|
||||
|
||||
void CalcTemperature(void) // calculate the temperature from raw readout and calibration values
|
||||
{ int32_t X1 = ( (( (int32_t)RawTemp - (int32_t)AC6 )*(int32_t)AC5) )>>15;
|
||||
int32_t X2 = ((int32_t)MC<<11)/(X1+(int32_t)MD);
|
||||
B5 = X1+X2;
|
||||
Temperature = (B5+8)>>4; }
|
||||
|
||||
void CalcPressure(void) // calculate the pressure - must calculate the temperature first !
|
||||
{ int32_t B6 = B5-4000;
|
||||
int32_t X1 = ((int32_t)B2*((B6*B6)>>12))>>11;
|
||||
int32_t X2 = ((int32_t)AC2*B6)>>11;
|
||||
int32_t X3 = X1+X2;
|
||||
int32_t B3 = (((((int32_t)AC1<<2)+X3)<<MEAS_OSS)+2)>>2;
|
||||
X1 = ((int32_t)AC3*B6)>>13;
|
||||
X2 = ((int32_t)B1*((B6*B6)>>12))>>16;
|
||||
X3 = ((X1+X2)+2)>>2;
|
||||
uint32_t B4 = ((uint32_t)AC4*(uint32_t)(X3+32768))>>15;
|
||||
uint32_t B7 = (RawPress-B3)*((uint32_t)50000>>MEAS_OSS);
|
||||
if(B7&0x8000000) { Pressure = (B7/B4)<<1; }
|
||||
else { Pressure = (B7<<1)/B4; }
|
||||
X1 = (Pressure>>8)*(Pressure>>8);
|
||||
X1 = (X1*3038)>>16;
|
||||
X2 = (-7357*(int32_t)Pressure)>>16;
|
||||
Pressure += (X1+X2+3791)>>4; }
|
||||
|
||||
} ;
|
||||
|
|
@ -0,0 +1,145 @@
|
|||
#include <stdint.h>
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "hal.h"
|
||||
|
||||
class BMP280
|
||||
{ private:
|
||||
static const uint8_t ADDR0 = 0x76; // possible I2C addresses
|
||||
static const uint8_t ADDR1 = 0x77;
|
||||
|
||||
static const uint8_t REG_CALIB = 0x88; // calibration register:
|
||||
static const uint8_t REG_ID = 0xD0; // ID register: always reads 0x58
|
||||
static const uint8_t REG_RESET = 0xE0; // write 0xB6 to perform soft-reset
|
||||
static const uint8_t REG_STATUS = 0xF3; // status: ____C__I C=conversion in progress
|
||||
static const uint8_t REG_CTRL = 0xF4; // control: TTTPPPMM TTT=temp. oversampling, PPP=pressure oversampling, MM=power mode
|
||||
static const uint8_t REG_CONFIG = 0xF5; // config: TTTFFF_S TTT=? FFF=Filter S=3-wire SPI
|
||||
|
||||
static const uint8_t REG_PRESS = 0xF7; // Pressure result:
|
||||
static const uint8_t REG_PRESS_MSB = 0xF7; // Pressure result: MSB
|
||||
static const uint8_t REG_PRESS_LSB = 0xF8; // Pressure result: LSB
|
||||
static const uint8_t REG_PRESS_XLSB = 0xF9; // Pressure result: more LSB
|
||||
|
||||
static const uint8_t REG_TEMP = 0xFA; // Temperature result:
|
||||
static const uint8_t REG_TEMP_MSB = 0xFA; // Temperature result: MSB
|
||||
static const uint8_t REG_TEMP_LSB = 0xFB; // Temperature result: LSB
|
||||
static const uint8_t REG_TEMP_XLSB = 0xFC; // Temperature result: more LSB
|
||||
public:
|
||||
uint8_t Bus; // which I2C bus
|
||||
uint8_t ADDR; // detected I2C address
|
||||
private:
|
||||
union
|
||||
{ int16_t Calib[13];
|
||||
struct
|
||||
{ uint16_t T1; // 13 calibration values from EEPROM
|
||||
int16_t T2, T3;
|
||||
uint16_t P1;
|
||||
int16_t P2, P3, P4, P5, P6, P7, P8, P9;
|
||||
int16_t D;
|
||||
} ;
|
||||
} ;
|
||||
public:
|
||||
uint8_t Error; // error on the I2C bus (0=no error)
|
||||
int32_t RawTemp; // raw temperature - to be processed
|
||||
int32_t FineTemp; // for pressure calc.
|
||||
int16_t Temperature; // [0.1 degC] after processing
|
||||
int32_t RawPress; // raw pressure - to be processed
|
||||
uint32_t Pressure; // [0.25Pa ] after processing
|
||||
|
||||
public:
|
||||
|
||||
void DefaultCalib(void) // set default calibration constants
|
||||
{ T1 = 27504;
|
||||
T2 = 26435;
|
||||
T3 = -1000;
|
||||
P1 = 36477;
|
||||
P2 = -10685;
|
||||
P3 = 3024;
|
||||
P4 = 2855;
|
||||
P5 = 140;
|
||||
P6 = -7;
|
||||
P7 = 15500;
|
||||
P8 = -14600;
|
||||
P9 = 6000;
|
||||
D = 0; }
|
||||
|
||||
uint8_t CheckID(void) // check ID, to make sure the BMP280 is connected and works correctly
|
||||
{ uint8_t ID;
|
||||
ADDR=0;
|
||||
Error=I2C_Read(Bus, ADDR0, REG_ID, ID);
|
||||
if( (!Error) && ( (ID==0x58) || (ID==0x60) ) ) { ADDR=ADDR0; return 0; }
|
||||
Error=I2C_Read(Bus, ADDR1, REG_ID, ID);
|
||||
if( (!Error) && ( (ID==0x58) || (ID==0x60) ) ) { ADDR=ADDR1; return 0; }
|
||||
return 1; } // 0 => no error and correct ID
|
||||
|
||||
uint8_t ReadCalib(void) // read the calibration constants from the EEPROM
|
||||
{ Error=I2C_Read(Bus, ADDR, REG_CALIB, (uint8_t *)Calib, 2*13);
|
||||
return Error; }
|
||||
|
||||
uint8_t ReadReady(void) // check if temperature and pressure conversion is done
|
||||
{ uint8_t Status;
|
||||
Error=I2C_Read(Bus, ADDR, REG_STATUS, Status); if(Error) return Error;
|
||||
return (Status&0x09)==0; } // 1 = ready, 0 => busy
|
||||
|
||||
uint8_t WaitReady(uint8_t Timeout=50, uint8_t Wait=30) // wait for the conversion to be ready
|
||||
{ vTaskDelay(Wait);
|
||||
for(; Timeout; Timeout--)
|
||||
{ uint8_t Err=ReadReady(); if(Err) return Err; // 1 is ready, >1 is an I2C bus error
|
||||
vTaskDelay(1); }
|
||||
return 0xFF; } // return "timeout" error
|
||||
|
||||
uint8_t Trigger(void) // start a temperature+pressure measurement
|
||||
{ uint8_t Data=0x00; Error=I2C_Write(Bus, ADDR, REG_CONFIG, Data); if(Error) return Error;
|
||||
Data=0x55; Error=I2C_Write(Bus, ADDR, REG_CTRL, Data); return Error; } // P.osp=16x, T.osp=2x
|
||||
|
||||
uint8_t ReadRawPress(void)
|
||||
{ RawPress=0;
|
||||
Error=I2C_Read(Bus, ADDR, REG_PRESS, (uint8_t *)(&RawPress), 3); if(Error) return Error;
|
||||
RawPress = ((RawPress<<16)&0xFF0000) | (RawPress&0x00FF00) | ((RawPress>>16)&0x0000FF);
|
||||
RawPress>>=4;
|
||||
return 0; }
|
||||
|
||||
uint8_t ReadRawTemp(void)
|
||||
{ RawTemp=0;
|
||||
Error=I2C_Read(Bus, ADDR, REG_TEMP, (uint8_t *)(&RawTemp), 3); if(Error) return Error;
|
||||
RawTemp = ((RawTemp<<16)&0xFF0000) | (RawTemp&0x00FF00) | ((RawTemp>>16)&0x0000FF);
|
||||
RawTemp>>=4;
|
||||
return 0; }
|
||||
|
||||
uint8_t Acquire(void)
|
||||
{ if(Trigger()) return Error;
|
||||
if(WaitReady()!=1) return Error;
|
||||
if(ReadRawTemp()) return Error;
|
||||
return ReadRawPress(); }
|
||||
|
||||
void Calculate(void)
|
||||
{ CalcTemperature();
|
||||
CalcPressure(); }
|
||||
|
||||
void CalcTemperature(void) // calculate the temperature from raw readout and calibration values
|
||||
{ int32_t var1 = ((((RawTemp>>3) - ((int32_t)T1<<1))) * ((int32_t)T2)) >> 11;
|
||||
int32_t var2 = ( ( (((RawTemp>>4) - (int32_t)T1) * ((RawTemp>>4) - (int32_t)T1) ) >> 12) * ((int32_t)T3) ) >> 14;
|
||||
FineTemp = var1 + var2;
|
||||
Temperature = ( (var1+var2) + 256) >> 9; // [0.1degC]
|
||||
// Temperature = ( (var1+var2) * 5 + 128) >> 8; // [0.01degC]
|
||||
}
|
||||
|
||||
void CalcPressure(void) // calculate the pressure - must calculate the temperature first !
|
||||
{
|
||||
int64_t var1 = ((int64_t)FineTemp) - 128000;
|
||||
int64_t var2 = var1 * var1 * (int64_t)P6;
|
||||
var2 = var2 + ((var1*(int64_t)P5)<<17);
|
||||
var2 = var2 + (((int64_t)P4)<<35);
|
||||
var1 = ((var1 * var1 * (int64_t)P3)>>8) + ((var1 * (int64_t)P2)<<12);
|
||||
var1 = (((((int64_t)1)<<47)+var1))*((int64_t)P1)>>33;
|
||||
if (var1 == 0) { Pressure=0; return; }
|
||||
int64_t p = 1048576-RawPress;
|
||||
p = (((p<<31)-var2)*3125)/var1;
|
||||
var1 = (((int64_t)P9) * (p>>13) * (p>>13)) >> 25;
|
||||
var2 = (((int64_t)P8) * p) >> 19;
|
||||
p = ((p + var1 + var2) >> 8) + (((int64_t)P7)<<4);
|
||||
Pressure = ((p+32)>>6);
|
||||
}
|
||||
|
||||
} ;
|
||||
|
|
@ -24,13 +24,13 @@ void PrintTasks(void (*CONS_UART_Write)(char))
|
|||
uxArraySize = uxTaskGetSystemState( pxTaskStatusArray, uxArraySize, NULL );
|
||||
for(UBaseType_t T=0; T<uxArraySize; T++)
|
||||
{ TaskStatus_t *Task = pxTaskStatusArray+T;
|
||||
uint8_t Len=Format_String(Line, Task->pcTaskName);
|
||||
for( ; Len<=configMAX_TASK_NAME_LEN; )
|
||||
Line[Len++]=' ';
|
||||
uint8_t Len=Format_String(Line, Task->pcTaskName, configMAX_TASK_NAME_LEN, 0);
|
||||
// for( ; Len<=configMAX_TASK_NAME_LEN; )
|
||||
// Line[Len++]=' ';
|
||||
Len+=Format_UnsDec(Line+Len, Task->uxCurrentPriority, 2); Line[Len++]=' ';
|
||||
// Line[Len++]='0'+Task->uxCurrentPriority; Line[Len++]=' ';
|
||||
Len+=Format_UnsDec(Line+Len, Task->usStackHighWaterMark, 3);
|
||||
Line[Len++]='\n'; Line[Len++]=0;
|
||||
Line[Len++]='\n'; Line[Len]=0;
|
||||
Format_String(CONS_UART_Write, Line);
|
||||
}
|
||||
vPortFree( pxTaskStatusArray );
|
||||
|
|
@ -81,7 +81,7 @@ int OLED_DisplayPosition(GPS_Position *GPS=0, uint8_t LineIdx=2)
|
|||
Line[0]=0;
|
||||
if(GPS && GPS->hasBaro)
|
||||
{ Format_String(Line , "0000.0hPa 00000m");
|
||||
Format_UnsDec(Line , GPS->Pressure/4, 5, 1);
|
||||
Format_UnsDec(Line , GPS->Pressure/40, 5, 1);
|
||||
Format_UnsDec(Line+10, GPS->StdAltitude/10, 5, 0); }
|
||||
OLED_PutLine(LineIdx+5, Line);
|
||||
return 0; }
|
||||
|
|
|
|||
|
|
@ -18,12 +18,13 @@ void Format_String( void (*Output)(char), const char *String)
|
|||
(*Output)(ch); }
|
||||
}
|
||||
|
||||
uint8_t Format_String(char *Str, const char *String)
|
||||
uint8_t Format_String(char *Out, const char *String)
|
||||
{ uint8_t OutLen=0;
|
||||
for( ; ; )
|
||||
{ char ch = (*String++); if(ch==0) break;
|
||||
if(ch=='\n') Str[OutLen++]='\r';
|
||||
Str[OutLen++]=ch; }
|
||||
if(ch=='\n') Out[OutLen++]='\r';
|
||||
Out[OutLen++]=ch; }
|
||||
// Out[OutLen]=0;
|
||||
return OutLen; }
|
||||
|
||||
void Format_String( void (*Output)(char), const char *String, uint8_t MinLen, uint8_t MaxLen)
|
||||
|
|
@ -37,16 +38,17 @@ void Format_String( void (*Output)(char), const char *String, uint8_t MinLen, ui
|
|||
(*Output)(' ');
|
||||
}
|
||||
|
||||
uint8_t Format_String(char *Str, const char *String, uint8_t MinLen, uint8_t MaxLen)
|
||||
uint8_t Format_String(char *Out, const char *String, uint8_t MinLen, uint8_t MaxLen)
|
||||
{ if(MaxLen<MinLen) MaxLen=MinLen;
|
||||
uint8_t OutLen=0;
|
||||
uint8_t Idx;
|
||||
for(Idx=0; Idx<MaxLen; Idx++)
|
||||
{ char ch = String[Idx]; if(ch==0) break;
|
||||
if(ch=='\n') Str[OutLen++]='\r';
|
||||
Str[OutLen++]=ch; }
|
||||
if(ch=='\n') Out[OutLen++]='\r';
|
||||
Out[OutLen++]=ch; }
|
||||
for( ; Idx<MinLen; Idx++)
|
||||
Str[OutLen++]=' ';
|
||||
Out[OutLen++]=' ';
|
||||
// Out[OutLen++]=0;
|
||||
return OutLen; }
|
||||
|
||||
void Format_Hex( void (*Output)(char), uint8_t Byte )
|
||||
|
|
@ -59,13 +61,13 @@ 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); }
|
||||
|
||||
uint8_t Format_HHMMSS(char *Str, uint32_t Time)
|
||||
uint8_t Format_HHMMSS(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 = 10000*Hour + 100*Min + Sec;
|
||||
return Format_UnsDec(Str, HHMMSS, 6); }
|
||||
return Format_UnsDec(Out, HHMMSS, 6); }
|
||||
|
||||
void Format_UnsDec( void (*Output)(char), uint16_t Value, uint8_t MinDigits, uint8_t DecPoint)
|
||||
{ uint16_t Base; uint8_t Pos;
|
||||
|
|
@ -126,7 +128,7 @@ void Format_SignDec( void (*Output)(char), int64_t Value, uint8_t MinDigits, uin
|
|||
|
||||
// ------------------------------------------------------------------------------------------
|
||||
|
||||
uint8_t Format_UnsDec(char *Str, uint32_t Value, uint8_t MinDigits, uint8_t DecPoint)
|
||||
uint8_t Format_UnsDec(char *Out, uint32_t Value, uint8_t MinDigits, uint8_t DecPoint)
|
||||
{ uint32_t Base; uint8_t Pos, Len=0;
|
||||
for( Pos=10, Base=1000000000; Base; Base/=10, Pos--)
|
||||
{ uint8_t Dig;
|
||||
|
|
@ -134,16 +136,17 @@ uint8_t Format_UnsDec(char *Str, uint32_t Value, uint8_t MinDigits, uint8_t DecP
|
|||
{ Dig=Value/Base; Value-=Dig*Base; }
|
||||
else
|
||||
{ Dig=0; }
|
||||
if(Pos==DecPoint) { (*Str++)='.'; Len++; }
|
||||
if(Pos==DecPoint) { (*Out++)='.'; Len++; }
|
||||
if( (Pos<=MinDigits) || (Dig>0) || (Pos<=DecPoint) )
|
||||
{ (*Str++)='0'+Dig; Len++; MinDigits=Pos; }
|
||||
{ (*Out++)='0'+Dig; Len++; MinDigits=Pos; }
|
||||
// (*Out)=0;
|
||||
}
|
||||
return Len; }
|
||||
|
||||
uint8_t Format_SignDec(char *Str, int32_t Value, uint8_t MinDigits, uint8_t DecPoint)
|
||||
{ if(Value<0) { (*Str++)='-'; Value=(-Value); }
|
||||
else { (*Str++)='+'; }
|
||||
return 1+Format_UnsDec(Str, Value, MinDigits, DecPoint); }
|
||||
uint8_t Format_SignDec(char *Out, int32_t Value, uint8_t MinDigits, uint8_t DecPoint)
|
||||
{ if(Value<0) { (*Out++)='-'; Value=(-Value); }
|
||||
else { (*Out++)='+'; }
|
||||
return 1+Format_UnsDec(Out, Value, MinDigits, DecPoint); }
|
||||
|
||||
uint8_t Format_Hex( char *Output, uint8_t Byte )
|
||||
{ (*Output++) = HexDigit(Byte>>4); (*Output++)=HexDigit(Byte&0x0F); return 2; }
|
||||
|
|
|
|||
|
|
@ -8,8 +8,8 @@ char HexDigit(uint8_t Val);
|
|||
void Format_Bytes ( void (*Output)(char), const uint8_t *Bytes, uint8_t Len);
|
||||
inline void Format_Bytes ( void (*Output)(char), const char *Bytes, uint8_t Len) { Format_Bytes(Output, (const uint8_t *)Bytes, Len); }
|
||||
|
||||
void Format_String( void (*Output)(char), const char *String);
|
||||
void Format_String( void (*Output)(char), const char *String, uint8_t MinLen, uint8_t MaxLen);
|
||||
void Format_String( void (*Output)(char), const char *String);
|
||||
void Format_String( void (*Output)(char), const char *String, uint8_t MinLen, uint8_t MaxLen);
|
||||
|
||||
void Format_Hex( void (*Output)(char), uint8_t Byte );
|
||||
void Format_Hex( void (*Output)(char), uint16_t Word );
|
||||
|
|
@ -24,27 +24,27 @@ void Format_SignDec( void (*Output)(char), int32_t Value, uint8_t MinDigits=1,
|
|||
void Format_UnsDec ( void (*Output)(char), uint64_t Value, uint8_t MinDigits=1, uint8_t DecPoint=0);
|
||||
void Format_SignDec( void (*Output)(char), int64_t Value, uint8_t MinDigits=1, uint8_t DecPoint=0);
|
||||
|
||||
uint8_t Format_String(char *Str, const char *String);
|
||||
uint8_t Format_String(char *Str, const char *String, uint8_t MinLen, uint8_t MaxLen);
|
||||
uint8_t Format_String(char *Out, const char *String);
|
||||
uint8_t Format_String(char *Out, const char *String, uint8_t MinLen, uint8_t MaxLen);
|
||||
|
||||
uint8_t Format_UnsDec (char *Str, uint32_t Value, uint8_t MinDigits=1, uint8_t DecPoint=0);
|
||||
uint8_t Format_SignDec(char *Str, int32_t Value, uint8_t MinDigits=1, uint8_t DecPoint=0);
|
||||
uint8_t Format_UnsDec (char *Out, uint32_t Value, uint8_t MinDigits=1, uint8_t DecPoint=0);
|
||||
uint8_t Format_SignDec(char *Out, int32_t Value, uint8_t MinDigits=1, uint8_t DecPoint=0);
|
||||
|
||||
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_HHMMSS(char *Str, uint32_t Time);
|
||||
uint8_t Format_HHMMSS(char *Out, uint32_t Time);
|
||||
|
||||
int8_t Read_Hex1(char Digit);
|
||||
int16_t Read_Hex2(const char *Inp);
|
||||
int8_t Read_Hex1(char Digit);
|
||||
int16_t Read_Hex2(const char *Inp);
|
||||
|
||||
int8_t Read_Dec1(char Digit); // convert single digit into an integer
|
||||
inline int8_t Read_Dec1(const char *Inp) { return Read_Dec1(Inp[0]); }
|
||||
int8_t Read_Dec2(const char *Inp); // convert two digit decimal number into an integer
|
||||
int16_t Read_Dec3(const char *Inp); // convert three digit decimal number into an integer
|
||||
int16_t Read_Dec4(const char *Inp); // convert three digit decimal number into an integer
|
||||
int8_t Read_Dec1(char Digit); // convert single digit into an integer
|
||||
inline int8_t Read_Dec1(const char *Inp) { return Read_Dec1(Inp[0]); }
|
||||
int8_t Read_Dec2(const char *Inp); // convert two digit decimal number into an integer
|
||||
int16_t Read_Dec3(const char *Inp); // convert three digit decimal number into an integer
|
||||
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
|
||||
|
|
|
|||
70
main/hal.cpp
70
main/hal.cpp
|
|
@ -51,7 +51,7 @@ OLED datasheet: https://cdn-shop.adafruit.com/datasheets/SSD1306.pdf
|
|||
OLED example: https://github.com/yanbe/ssd1306-esp-idf-i2c
|
||||
OLED article: http://robotcantalk.blogspot.co.uk/2015/03/interfacing-arduino-with-ssd1306-driven.html
|
||||
|
||||
SX1278 pins:
|
||||
SX1276 pins:
|
||||
14 = GPIO14 = RST
|
||||
5 = GPIO5 = SCK
|
||||
18 = GPIO18 = CS = SS
|
||||
|
|
@ -86,7 +86,7 @@ UART2 pins:
|
|||
|
||||
*/
|
||||
|
||||
#define PIN_LED_PCB GPIO_NUM_25 // status LED on the PCB
|
||||
#define PIN_LED_PCB GPIO_NUM_2 // status LED on the PCB: 25 or 2. GPIO25 id DAC2
|
||||
// #define PIN_LED_TX GPIO_NUM_??
|
||||
// #define PIN_LED_RX GPIO_NUM_??
|
||||
|
||||
|
|
@ -96,12 +96,13 @@ UART2 pins:
|
|||
#define PIN_RFM_SCK GPIO_NUM_5 // SPI clock
|
||||
#define PIN_RFM_MISO GPIO_NUM_19 // SPI MISO
|
||||
#define PIN_RFM_MOSI GPIO_NUM_27 // SPI MOSI
|
||||
#define RFM_SPI_SPEED 4000000 // [Hz] 4MHz SPI clock rate for RF chip
|
||||
|
||||
// VK2828U GPS MAVlink port
|
||||
#define PIN_GPS_TXD GPIO_NUM_12 // green green
|
||||
#define PIN_GPS_RXD GPIO_NUM_35 // blue yellow
|
||||
#define PIN_GPS_PPS GPIO_NUM_34 // white
|
||||
#define PIN_GPS_ENA GPIO_NUM_13 // yellow
|
||||
// VK2828U GN-801 MAVlink
|
||||
#define PIN_GPS_TXD GPIO_NUM_12 // green green green
|
||||
#define PIN_GPS_RXD GPIO_NUM_35 // blue yellow yellow
|
||||
#define PIN_GPS_PPS GPIO_NUM_34 // white blue
|
||||
#define PIN_GPS_ENA GPIO_NUM_13 // yellow white
|
||||
|
||||
// Note: I had a problem GPS ENABLE on GPIO13, thus I tied the enable wire to 3.3V for the time being.
|
||||
|
||||
|
|
@ -109,12 +110,14 @@ UART2 pins:
|
|||
#define GPS_UART UART_NUM_1 // UART1 for GPS data read and dialog
|
||||
|
||||
#define I2C_BUS I2C_NUM_1 // use bus #1 to talk to OLED and Baro sensor
|
||||
#define I2C_SPEED 1000000 // 1MHz clock on I2C
|
||||
// #define I2C_SPEED 1000000 // [Hz] 1MHz clock on I2C - defined inb hal.h
|
||||
#define PIN_I2C_SCL GPIO_NUM_15 // SCL pin
|
||||
#define PIN_I2C_SDA GPIO_NUM_4 // SDA pin
|
||||
|
||||
uint8_t BARO_I2C = (uint8_t)I2C_BUS;
|
||||
|
||||
#define OLED_I2C_ADDR 0x3C // I2C address of the OLED display
|
||||
#define PIN_OLED_RST GPIO_NUM_16 // OLED RESET pin
|
||||
#define PIN_OLED_RST GPIO_NUM_16 // OLED RESET: low-active
|
||||
|
||||
// ======================================================================================================
|
||||
// 48-bit unique ID of the chip
|
||||
|
|
@ -190,20 +193,21 @@ bool GPS_PPS_isOn(void) { return gpio_get_level(PIN_GPS_PPS); }
|
|||
//--------------------------------------------------------------------------------------------------------
|
||||
// RF chip
|
||||
|
||||
inline void RFM_RESET_Dir (void) { gpio_set_direction(PIN_RFM_RST, GPIO_MODE_OUTPUT); }
|
||||
inline void RFM_RESET_High(void) { gpio_set_level(PIN_RFM_RST, 1); }
|
||||
inline void RFM_RESET_Low (void) { gpio_set_level(PIN_RFM_RST, 0); }
|
||||
inline void RFM_RESET_Dir (void) { gpio_set_direction(PIN_RFM_RST, GPIO_MODE_OUTPUT); }
|
||||
inline void RFM_RESET_Set (bool High) { gpio_set_level(PIN_RFM_RST, High); }
|
||||
// inline void RFM_RESET_High(void) { gpio_set_level(PIN_RFM_RST, 1); }
|
||||
// inline void RFM_RESET_Low (void) { gpio_set_level(PIN_RFM_RST, 0); }
|
||||
|
||||
#ifdef WITH_RFM95
|
||||
void RFM_RESET(uint8_t On)
|
||||
{ if(On) RFM_RESET_Low();
|
||||
else RFM_RESET_High(); }
|
||||
void RFM_RESET(uint8_t On) { RFM_RESET_Set(~On); }
|
||||
// { if(On) RFM_RESET_Low();
|
||||
// else RFM_RESET_High(); }
|
||||
#endif
|
||||
|
||||
#ifdef WITH_RFM69
|
||||
void RFM_RESET(uint8_t On)
|
||||
{ if(On) RFM_RESET_High();
|
||||
else RFM_RESET_Low(); }
|
||||
void RFM_RESET(uint8_t On) { RFM_RESET_Set(On); }
|
||||
// { if(On) RFM_RESET_High();
|
||||
// else RFM_RESET_Low(); }
|
||||
#endif
|
||||
|
||||
inline void RFM_IRQ_Dir (void) { gpio_set_direction(PIN_RFM_IRQ, GPIO_MODE_INPUT); }
|
||||
|
|
@ -347,11 +351,6 @@ void vApplicationTickHook(void) // RTOS timer tick hook
|
|||
|
||||
//--------------------------------------------------------------------------------------------------------
|
||||
|
||||
// void CONS_Configuration(void)
|
||||
// {
|
||||
// CONS_Mutex = xSemaphoreCreateMutex();
|
||||
// }
|
||||
|
||||
void IO_Configuration(void)
|
||||
{
|
||||
LED_PCB_Dir();
|
||||
|
|
@ -377,7 +376,7 @@ void IO_Configuration(void)
|
|||
duty_cycle_pos: 0,
|
||||
cs_ena_pretrans: 0,
|
||||
cs_ena_posttrans: 0,
|
||||
clock_speed_hz: 4000000,
|
||||
clock_speed_hz: RFM_SPI_SPEED,
|
||||
spics_io_num: PIN_RFM_SS,
|
||||
flags: 0,
|
||||
queue_size: 3,
|
||||
|
|
@ -518,30 +517,35 @@ int SPIFFS_Info(size_t &Total, size_t &Used, const char *Label)
|
|||
{ return esp_spiffs_info(Label, &Total, &Used); }
|
||||
|
||||
// ======================================================================================================
|
||||
/*
|
||||
uint8_t I2C_Read(i2c_port_t Bus, uint8_t Addr, uint8_t Reg, uint8_t *Data, uint8_t Len, uint8_t Wait=10)
|
||||
|
||||
SemaphoreHandle_t I2C_Mutex;
|
||||
|
||||
uint8_t I2C_Read(uint8_t Bus, uint8_t Addr, uint8_t Reg, uint8_t *Data, uint8_t Len, uint8_t Wait)
|
||||
{ i2c_cmd_handle_t Cmd = i2c_cmd_link_create();
|
||||
i2c_master_start(Cmd);
|
||||
i2c_master_write_byte(Cmd, (Addr<<1) | I2C_MASTER_READ , I2C_MASTER_ACK);
|
||||
i2c_master_write_byte(Cmd, Reg , I2C_MASTER_ACK);
|
||||
i2c_master_write_byte(Cmd, (Addr<<1) | I2C_MASTER_WRITE, I2C_MASTER_ACK);
|
||||
i2c_master_write_byte(Cmd, Reg, I2C_MASTER_ACK);
|
||||
i2c_master_start(Cmd);
|
||||
i2c_master_write_byte(Cmd, (Addr<<1) | I2C_MASTER_READ, I2C_MASTER_ACK);
|
||||
i2c_master_read(Cmd, Data, Len, I2C_MASTER_LAST_NACK);
|
||||
i2c_master_stop(Cmd);
|
||||
esp_err_t Ret = i2c_master_cmd_begin(Bus, Cmd, Wait);
|
||||
esp_err_t Ret = i2c_master_cmd_begin((i2c_port_t)Bus, Cmd, Wait);
|
||||
i2c_cmd_link_delete(Cmd);
|
||||
return Ret; }
|
||||
|
||||
|
||||
uint8_t I2C_Write(i2c_port_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)
|
||||
{ i2c_cmd_handle_t Cmd = i2c_cmd_link_create();
|
||||
i2c_master_start(Cmd);
|
||||
i2c_master_write_byte(Cmd, (Addr<<1) | I2C_MASTER_WRITE , I2C_MASTER_ACK);
|
||||
i2c_master_write_byte(Cmd, Reg , I2C_MASTER_ACK);
|
||||
i2c_master_write(Cmd, Data, Len, I2C_MASTER_NACK);
|
||||
i2c_master_stop(Cmd);
|
||||
esp_err_t Ret = i2c_master_cmd_begin(Bus, Cmd, Wait);
|
||||
esp_err_t Ret = i2c_master_cmd_begin((i2c_port_t)Bus, Cmd, Wait);
|
||||
i2c_cmd_link_delete(Cmd);
|
||||
return Ret; }
|
||||
*/
|
||||
|
||||
uint8_t I2C_Restart(uint8_t Bus)
|
||||
{ return 0; }
|
||||
|
||||
// ======================================================================================================
|
||||
|
||||
|
|
|
|||
28
main/hal.h
28
main/hal.h
|
|
@ -22,7 +22,7 @@
|
|||
// #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_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
|
||||
|
|
@ -30,14 +30,24 @@
|
|||
// #define WITH_GPS_SRF
|
||||
// #define WITH_MAVLINK
|
||||
|
||||
// #define WITH_BMP180 // BMP180 pressure sensor
|
||||
#define WITH_BMP280 // BMP280 pressure sensor
|
||||
// #define WITH_MS5607 // MS5607 pressure sensor
|
||||
|
||||
#define I2C_SPEED 1000000 // [Hz]
|
||||
|
||||
#define WITH_PFLAA // PFLAU and PFLAA for compatibility with XCsoar and LK8000
|
||||
|
||||
#define WITH_CONFIG // interpret the console input: $POGNS to change parameters
|
||||
#define WITH_OLED // OLED display on the I2C
|
||||
|
||||
// #define WITH_OLED // OLED display on the I2C
|
||||
|
||||
// #define WITH_BT_SPP // Bluetooth serial port fo smartphone/tablet link
|
||||
|
||||
// ============================================================================================================
|
||||
|
||||
extern uint8_t BARO_I2C;
|
||||
|
||||
#ifdef WITH_MAVLINK
|
||||
const uint8_t MAV_SysID = 1; // System-ID for MAVlink messages we send out
|
||||
extern uint8_t MAV_Seq; // sequence number for MAVlink message sent out
|
||||
|
|
@ -46,6 +56,7 @@ extern uint8_t MAV_Seq; // sequence number for MAVlink messag
|
|||
// ============================================================================================================
|
||||
|
||||
extern SemaphoreHandle_t CONS_Mutex; // console port Mutex
|
||||
extern SemaphoreHandle_t I2C_Mutex; // I2C port Mutex (OLED and Baro)
|
||||
|
||||
uint64_t getUniqueID(void); // get some unique ID of the CPU/chip
|
||||
uint32_t getUniqueAddress(void); // get unique 24-bit address for the transmitted IF
|
||||
|
|
@ -103,4 +114,17 @@ int BT_SPP_Init(void);
|
|||
int SPIFFS_Register(const char *Path="/spiffs", const char *Label=0, size_t MaxOpenFiles=5);
|
||||
int SPIFFS_Info(size_t &Total, size_t &Used, const char *Label=0);
|
||||
|
||||
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);
|
||||
|
||||
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); }
|
||||
|
||||
template <class Type>
|
||||
inline uint8_t I2C_Read (uint8_t Bus, uint8_t Addr, uint8_t Reg, Type &Object, uint8_t Wait=10)
|
||||
{ return I2C_Read (Bus, Addr, Reg, (uint8_t *)&Object, sizeof(Type), Wait); }
|
||||
|
||||
uint8_t I2C_Restart(uint8_t Bus);
|
||||
|
||||
#endif // __HAL_H__
|
||||
|
|
|
|||
|
|
@ -8,6 +8,7 @@
|
|||
#include "rf.h" // RF control/transmission/reception task
|
||||
#include "proc.h" // GPS/RF process taskk
|
||||
#include "gps.h" // GPS control data acquisiton
|
||||
#include "sens.h"
|
||||
#include "ctrl.h" // Control task
|
||||
|
||||
extern "C"
|
||||
|
|
@ -15,7 +16,8 @@ void app_main(void)
|
|||
{
|
||||
// printf("OGN Tracker on ESP32\n");
|
||||
|
||||
CONS_Mutex = xSemaphoreCreateMutex(); // semaphore used for writing to the console
|
||||
CONS_Mutex = xSemaphoreCreateMutex(); // semaphore for sharing the writing to the console
|
||||
I2C_Mutex = xSemaphoreCreateMutex(); // semaphore for sharing the I2C bus
|
||||
|
||||
NVS_Init(); // initialize Non-Volatile-Storage in Flash and read the tracker parameters
|
||||
|
||||
|
|
@ -34,6 +36,7 @@ void app_main(void)
|
|||
xTaskCreate(vTaskRF, "RF", 2048, 0, tskIDLE_PRIORITY+4, 0);
|
||||
xTaskCreate(vTaskPROC, "PROC", 2048, 0, tskIDLE_PRIORITY+3, 0);
|
||||
xTaskCreate(vTaskGPS, "GPS", 2048, 0, tskIDLE_PRIORITY+4, 0);
|
||||
xTaskCreate(vTaskSENS, "SENS", 2048, 0, tskIDLE_PRIORITY+4, 0);
|
||||
// xTaskCreate(vTaskCTRL, "CTRL", 1536, 0, tskIDLE_PRIORITY+2, 0);
|
||||
vTaskCTRL(0); // run directly the CTRL task, instead of creating a separate one.
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,109 @@
|
|||
#include <stdint.h>
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "hal.h"
|
||||
|
||||
class MS5607
|
||||
{ private:
|
||||
static const uint8_t ADDR0 = 0x76; // possible I2C addresses
|
||||
static const uint8_t ADDR1 = 0x77;
|
||||
|
||||
static const uint8_t CMD_RESET = 0x1E; // ADC reset command
|
||||
static const uint8_t CMD_ADC_READ = 0x00; // ADC read command
|
||||
static const uint8_t CMD_ADC_CONV = 0x40; // ADC conversion command
|
||||
static const uint8_t CMD_ADC_D1 = 0x00; // ADC D1 conversion
|
||||
static const uint8_t CMD_ADC_D2 = 0x10; // ADC D2 conversion
|
||||
static const uint8_t CMD_ADC_256 = 0x00; // ADC OSR=256
|
||||
static const uint8_t CMD_ADC_512 = 0x02; // ADC OSR=512
|
||||
static const uint8_t CMD_ADC_1024 = 0x04; // ADC OSR=1024
|
||||
static const uint8_t CMD_ADC_2048 = 0x06; // ADC OSR=2048
|
||||
static const uint8_t CMD_ADC_4096 = 0x08; // ADC OSR=4096
|
||||
static const uint8_t CMD_PROM_READ = 0xA0; // Prom read command
|
||||
public:
|
||||
uint8_t Bus; // which I2C bus
|
||||
uint8_t ADDR; // detected I2C address
|
||||
private:
|
||||
union
|
||||
{ int16_t Calib[8];
|
||||
struct
|
||||
{ uint16_t C0, C1, C2, C3, C4, C5, C6, C7; }; // 6 calibration values from EEPROM
|
||||
} ;
|
||||
public:
|
||||
uint8_t Error; // error on the I2C bus (0=no error)
|
||||
int32_t RawTemp; // raw temperature - to be processed
|
||||
int32_t RawPress; // raw pressure - to be processed
|
||||
int32_t Temperature; // [0.1 degC] temperature after correction
|
||||
uint32_t Pressure; // [0.25Pa ] pressure after correction
|
||||
|
||||
public:
|
||||
|
||||
void DefaultCalib(void) // set default calibration constants
|
||||
{ C0 = 0;
|
||||
C1 = 46372;
|
||||
C2 = 43981;
|
||||
C3 = 29059;
|
||||
C4 = 27842;
|
||||
C5 = 31553;
|
||||
C6 = 28165;
|
||||
C7 = 0; }
|
||||
|
||||
uint8_t Reset(uint8_t Addr) // RESET: takes 2.8ms
|
||||
{ Error=I2C_Write(Bus, Addr, CMD_RESET, 0, 0);
|
||||
return Error; }
|
||||
|
||||
uint8_t CheckID(void)
|
||||
{ ADDR=0;
|
||||
Error=Reset(ADDR0); if(!Error) { vTaskDelay(3); ADDR=ADDR0; return 0; }
|
||||
Error=Reset(ADDR1); if(!Error) { vTaskDelay(3); ADDR=ADDR1; return 0; }
|
||||
return 1; }
|
||||
|
||||
uint16_t SwapBytes(uint16_t Word)
|
||||
{ return (Word<<8) | (Word>>8); }
|
||||
|
||||
uint8_t ReadCalib(void) // read the calibration constants from the PROM
|
||||
{ for(uint8_t Idx=0; Idx<8; Idx++)
|
||||
{ Error=I2C_Read(Bus, ADDR, CMD_PROM_READ + 2*Idx, (uint8_t *)(Calib+Idx), 2); if(Error) break;
|
||||
Calib[Idx]=SwapBytes(Calib[Idx]); }
|
||||
return 0; }
|
||||
|
||||
uint8_t ReadRawPress(void) // convert and read raw pressure
|
||||
{ RawPress=0;
|
||||
Error=I2C_Write(Bus, ADDR, CMD_ADC_CONV+CMD_ADC_D1+CMD_ADC_4096, 0, 0); if(Error) return Error;
|
||||
vTaskDelay(12);
|
||||
Error=I2C_Read(Bus, ADDR, CMD_ADC_READ, (uint8_t *)(&RawPress), 3); if(Error) return Error;
|
||||
RawPress = ((RawPress<<16)&0xFF0000) | (RawPress&0x00FF00) | ((RawPress>>16)&0x0000FF); // swap bytes
|
||||
return 0; }
|
||||
|
||||
uint8_t ReadRawTemp(void) // convert and read raw temperature
|
||||
{ RawTemp=0;
|
||||
Error=I2C_Write(Bus, ADDR, CMD_ADC_CONV+CMD_ADC_D2+CMD_ADC_4096, 0, 0); if(Error) return Error;
|
||||
vTaskDelay(12);
|
||||
Error=I2C_Read(Bus, ADDR, CMD_ADC_READ, (uint8_t *)(&RawTemp), 3); if(Error) return Error;
|
||||
RawTemp = ((RawTemp<<16)&0xFF0000) | (RawTemp&0x00FF00) | ((RawTemp>>16)&0x0000FF); // swap bytes
|
||||
return 0; }
|
||||
|
||||
uint8_t Acquire(void) // convert and read raw pressure then temperature
|
||||
{ if(ReadRawPress()) return Error;
|
||||
return ReadRawTemp(); }
|
||||
|
||||
void Calculate(void) // process temperature and pressure with the calibration constants
|
||||
{ int32_t dT = RawTemp - ((int32_t)C5<<8);
|
||||
int32_t TEMP = 2000 + (((int64_t)dT*C6)>>23); // [0.01degC]
|
||||
int64_t OFF = ((uint64_t)C2<<17) + (((int64_t)dT*C4)>>6);
|
||||
int64_t SENS = ((uint32_t)C1<<16) + (((int64_t)dT*C3)>>7);
|
||||
if(TEMP<2000)
|
||||
{ int32_t dT2 = ((int64_t)dT*dT)>>31;
|
||||
int32_t OFF2 = (61*(TEMP-2000)*(TEMP-2000))>>4;
|
||||
int32_t SENS2 = 2*(TEMP-2000)*(TEMP-2000);
|
||||
if(TEMP<(-1500))
|
||||
{ OFF2 += 15*(TEMP+1500)*(TEMP+1500);
|
||||
SENS2 += 8*(TEMP+1500)*(TEMP+1500); }
|
||||
TEMP -= dT2;
|
||||
OFF -= OFF2;
|
||||
SENS -= SENS2; }
|
||||
Temperature = (TEMP+5)/10; // [0.1degC]
|
||||
Pressure = (((SENS*RawPress)>>21) - OFF)>>13; } // [0.25Pa]
|
||||
|
||||
} ;
|
||||
|
||||
75
main/ogn.h
75
main/ogn.h
|
|
@ -136,24 +136,6 @@ class OGN_Packet // Packet structure for the OGN tracker
|
|||
unsigned int BaroAltDiff: 8; // [m] // lower 8 bits of the altitude difference between baro and GPS
|
||||
} Position;
|
||||
|
||||
struct
|
||||
{ signed int Latitude:24; // // Latitude
|
||||
unsigned int Time: 6; // [sec] // time, just second thus ambiguity every every minute
|
||||
unsigned int : 2; //
|
||||
signed int Longitude:24; // // Longitude
|
||||
unsigned int : 6; // //
|
||||
unsigned int BaroMSB: 1; // // negated bit #8 of the altitude difference between baro and GPS
|
||||
unsigned int : 1; //
|
||||
unsigned int Altitude:14; // [m] VR // RRRR RRRR SSSS SSSS SSAA AAAA AAAA AAAA RR..=turn Rate:8, SS..=Speed:10, AA..=Alt:14
|
||||
unsigned int Speed:10; // [0.1m/s] VR
|
||||
unsigned int : 8; //
|
||||
unsigned int Heading:10; // // BBBB BBBB YYYY PCCC CCCC CCDD DDDD DDDD BB..=Baro altitude:8, YYYY=AcftType:4, P=Stealth:1, CC..=Climb:9, DD..=Heading:10
|
||||
unsigned int ClimbRate: 9; // [0.1m/s] VR
|
||||
unsigned int : 1;
|
||||
unsigned int ReportType: 4; // // 1 for wind/thermal report
|
||||
unsigned int BaroAltDiff: 8; // [m] // lower 8 bits of the altitude difference between baro and GPS
|
||||
} Wind;
|
||||
|
||||
struct
|
||||
{ unsigned int Pulse : 8; // [bpm] // pilot: heart pulse rate
|
||||
unsigned int Oxygen : 7; // [%] // pilot: oxygen level in the blood
|
||||
|
|
@ -171,10 +153,35 @@ class OGN_Packet // Packet structure for the OGN tracker
|
|||
unsigned int Firmware : 8; // [ ] // firmware version
|
||||
unsigned int Hardware : 8; // [ ] // hardware version
|
||||
unsigned int TxPower : 4; // [dBm] // RF trancmitter power
|
||||
unsigned int ReportType: 4; // [ ] // 0 for the status report
|
||||
unsigned int ReportType: 4; // [0] // 0 for the status report
|
||||
unsigned int Voltage : 8; // [1/64V] VR // supply/battery voltage
|
||||
} Status;
|
||||
|
||||
struct
|
||||
{ uint8_t Data[14]; // [16x7bit]packed string of 16-char: 7bit/char
|
||||
unsigned int DataChars: 4; // [int] number of characters in the packed string
|
||||
unsigned int ReportType: 4; // [1] // 1 for the Info packets
|
||||
uint8_t Check; // CRC check
|
||||
} Info;
|
||||
|
||||
struct
|
||||
{ signed int Latitude:24; // // Latitude of the measurement
|
||||
unsigned int Time: 6; // [sec] // time, just second thus ambiguity every every minute
|
||||
unsigned int : 2; // // spare
|
||||
signed int Longitude:24; // // Longitude of the measurement
|
||||
unsigned int : 6; // // spare
|
||||
unsigned int BaroMSB: 1; // // negated bit #8 of the altitude difference between baro and GPS
|
||||
unsigned int : 1; // // spare
|
||||
unsigned int Altitude:14; // [m] VR // Altitude of the measurement
|
||||
unsigned int Speed:10; // [0.1m/s] VR // Horizontal wind speed
|
||||
unsigned int : 8; // // spare
|
||||
unsigned int Heading:10; // // Wind direction
|
||||
unsigned int ClimbRate: 9; // [0.1m/s] VR // Vertical wind speed
|
||||
unsigned int : 1; // // spare
|
||||
unsigned int ReportType: 4; // // 2 for wind/thermal report
|
||||
unsigned int BaroAltDiff: 8; // [m] // lower 8 bits of the altitude difference between baro and GPS
|
||||
} Wind;
|
||||
|
||||
} ;
|
||||
|
||||
uint8_t *Byte(void) const { return (uint8_t *)&HeaderWord; } // packet as bytes
|
||||
|
|
@ -502,7 +509,7 @@ class OGN_Packet // Packet structure for the OGN tracker
|
|||
bool hasBaro(void) const { return Position.BaroMSB || Position.BaroAltDiff; }
|
||||
void clrBaro(void) { Position.BaroMSB=0; Position.BaroAltDiff=0; }
|
||||
int16_t getBaroAltDiff(void) const { int16_t AltDiff=Position.BaroAltDiff; if(Position.BaroMSB==0) AltDiff|=0xFF00; return AltDiff; }
|
||||
void setBaroAltDiff(int16_t AltDiff)
|
||||
void setBaroAltDiff(int32_t AltDiff)
|
||||
{ if(AltDiff<(-255)) AltDiff=(-255); else if(AltDiff>255) AltDiff=255;
|
||||
Position.BaroMSB = (AltDiff&0xFF00)==0; Position.BaroAltDiff=AltDiff&0xFF; }
|
||||
void EncodeStdAltitude(int32_t StdAlt) { setBaroAltDiff((StdAlt-DecodeAltitude())); }
|
||||
|
|
@ -677,6 +684,34 @@ class OGN_Packet // Packet structure for the OGN tracker
|
|||
void EncodeVoltage(uint16_t Voltage) { Status.Voltage=EncodeUR2V6(Voltage); } // [1/64V]
|
||||
uint16_t DecodeVoltage(void) const { return DecodeUR2V6(Status.Voltage); }
|
||||
|
||||
// --------------------------------------------------------------------------------------------------------------
|
||||
// Info fields: pack and unpack 7-bit char into the Info packets
|
||||
|
||||
void setInfoChar(uint8_t Char, uint8_t Idx) // put 7-bit Char onto give position
|
||||
{ if(Idx>=16) return;
|
||||
Char&=0x7F;
|
||||
uint8_t BitIdx = Idx*7; // [bits] bit index to the target field
|
||||
Idx = BitIdx>>3; // [bytes] index of the first byte to change
|
||||
uint8_t Ofs = BitIdx&0x07;
|
||||
if(Ofs==0) { Info.Data[Idx] = (Info.Data[Idx]&0x80) | Char ; return; }
|
||||
if(Ofs==1) { Info.Data[Idx] = (Info.Data[Idx]&0x01) | (Char<<1) ; return; }
|
||||
uint8_t Len1 = 8-Ofs;
|
||||
uint8_t Len2 = Ofs-1;
|
||||
uint8_t Msk1 = 0xFF; Msk1<<=Ofs;
|
||||
uint8_t Msk2 = 0x01; Msk2 = (Msk2<<Len2)-1;
|
||||
Info.Data[Idx ] = (Info.Data[Idx ]&(~Msk1)) | (Char<<Ofs);
|
||||
Info.Data[Idx+1] = (Info.Data[Idx+1]&(~Msk2)) | (Char>>Len1); }
|
||||
|
||||
uint8_t getInfoChar(uint8_t Idx) // get 7-bit Char from given position
|
||||
{ if(Idx>=16) return 0;
|
||||
uint8_t BitIdx = Idx*7; // [bits] bit index to the target field
|
||||
Idx = BitIdx>>3; // [bytes] index of the first byte to change
|
||||
uint8_t Ofs = BitIdx&0x07;
|
||||
if(Ofs==0) return Info.Data[Idx]&0x7F;
|
||||
if(Ofs==1) return Info.Data[Idx]>>1;
|
||||
uint8_t Len = 8-Ofs;
|
||||
return (Info.Data[Idx]>>Ofs) | ((Info.Data[Idx+1]<<Len)&0x7F); }
|
||||
|
||||
// --------------------------------------------------------------------------------------------------------------
|
||||
|
||||
// void Whiten (void) { TEA_Encrypt(Position, OGN_WhitenKey, 4); TEA_Encrypt(Position+2, OGN_WhitenKey, 4); } // whiten the position
|
||||
|
|
|
|||
|
|
@ -1,6 +1,7 @@
|
|||
#include <stdint.h>
|
||||
|
||||
#include "hal.h"
|
||||
|
||||
#include "proc.h"
|
||||
#include "ctrl.h"
|
||||
|
||||
|
|
|
|||
|
|
@ -1,7 +1,3 @@
|
|||
#include "freertos/FreeRTOS.h"
|
||||
#include "freertos/task.h"
|
||||
#include "freertos/semphr.h"
|
||||
|
||||
#include "hal.h"
|
||||
#include "rf.h"
|
||||
|
||||
|
|
|
|||
|
|
@ -351,7 +351,7 @@ class RFM_TRX
|
|||
void ClearIrqFlags(void) { WriteWord(RF_IRQ_FifoOverrun | RF_IRQ_Rssi, REG_IRQFLAGS1); }
|
||||
|
||||
#ifdef WITH_RFM69
|
||||
void WriteTxPower_W(int8_t TxPower=10) const // [dBm] for RFM69W: -18..+13dBm
|
||||
void WriteTxPower_W(int8_t TxPower=10) // [dBm] for RFM69W: -18..+13dBm
|
||||
{ if(TxPower<(-18)) TxPower=(-18); // check limits
|
||||
if(TxPower> 13 ) TxPower= 13 ;
|
||||
WriteByte( 0x80+(18+TxPower), REG_PALEVEL);
|
||||
|
|
@ -360,7 +360,7 @@ class RFM_TRX
|
|||
WriteByte( 0x70 , REG_TESTPA2);
|
||||
}
|
||||
|
||||
void WriteTxPower_HW(int8_t TxPower=10) const // [dBm] // for RFM69HW: -14..+20dBm
|
||||
void WriteTxPower_HW(int8_t TxPower=10) // [dBm] // for RFM69HW: -14..+20dBm
|
||||
{ if(TxPower<(-14)) TxPower=(-14); // check limits
|
||||
if(TxPower> 20 ) TxPower= 20 ;
|
||||
if(TxPower<=17)
|
||||
|
|
@ -376,12 +376,12 @@ class RFM_TRX
|
|||
}
|
||||
}
|
||||
|
||||
void WriteTxPower(int8_t TxPower, uint8_t isHW) const
|
||||
void WriteTxPower(int8_t TxPower, uint8_t isHW)
|
||||
{ WriteByte( 0x09, REG_PARAMP); // Tx ramp up/down time 0x06=100us, 0x09=40us, 0x0C=20us, 0x0F=10us (page 66)
|
||||
if(isHW) WriteTxPower_HW(TxPower);
|
||||
else WriteTxPower_W (TxPower); }
|
||||
|
||||
void WriteTxPowerMin(void) const { WriteTxPower_W(-18); } // set minimal Tx power and setup for reception
|
||||
void WriteTxPowerMin(void) { WriteTxPower_W(-18); } // set minimal Tx power and setup for reception
|
||||
|
||||
int Configure(int16_t Channel, const uint8_t *Sync)
|
||||
{ WriteMode(RF_OPMODE_STANDBY); // mode = STDBY
|
||||
|
|
|
|||
|
|
@ -0,0 +1,293 @@
|
|||
|
||||
#include "hal.h"
|
||||
#include "sens.h"
|
||||
|
||||
#include "timesync.h"
|
||||
|
||||
#include "parameters.h"
|
||||
|
||||
#include "ctrl.h"
|
||||
#include "gps.h"
|
||||
|
||||
// #define DEBUG_PRINT
|
||||
|
||||
#if defined(WITH_BMP180) || defined(WITH_BMP280) || defined(WITH_MS5607)
|
||||
|
||||
#ifdef WITH_BMP180
|
||||
#include "bmp180.h"
|
||||
#endif
|
||||
|
||||
#ifdef WITH_BMP280
|
||||
#include "bmp280.h"
|
||||
#endif
|
||||
|
||||
#ifdef WITH_MS5607
|
||||
#include "ms5607.h"
|
||||
#endif
|
||||
|
||||
#include "atmosphere.h"
|
||||
#include "slope.h"
|
||||
#include "lowpass2.h"
|
||||
#include "intmath.h"
|
||||
|
||||
#include "fifo.h"
|
||||
|
||||
// static const uint8_t VarioVolume = 2; // [0..3]
|
||||
static const uint16_t VarioBasePeriod = 800; // [ms]
|
||||
|
||||
#ifdef WITH_BEEPER
|
||||
void VarioSound(int32_t ClimbRate)
|
||||
{
|
||||
uint8_t VarioVolume = KNOB_Tick>>1; if(VarioVolume>3) VarioVolume=3; // take vario volume from the user knob
|
||||
if(ClimbRate>=50) // if climb > 1/2 m/s
|
||||
{ uint8_t Note=(ClimbRate-50)/50; // one semitone per 1/2 m/s
|
||||
if(Note>=0x0F) Note=0x0F; // stop at 15 thus 8 m/s
|
||||
uint16_t Period=(VarioBasePeriod+(Note>>1))/(1+Note); // beeping period
|
||||
Vario_Period=Period; Vario_Fill=Period>>1; // period shorter (faster beeping) with stronger climb
|
||||
Vario_Note = (VarioVolume<<6) | (0x10+Note); } // note to play: higher for stronger climb
|
||||
else if(ClimbRate<=(-100)) // if sink > 1 m/s
|
||||
{ uint8_t Note=(-ClimbRate-100)/100; // one semitone per 1 m/s
|
||||
if(Note>=0x0B) Note=0x0B; //
|
||||
Vario_Period=VarioBasePeriod; Vario_Fill=VarioBasePeriod; // continues tone
|
||||
Vario_Note = (VarioVolume<<6) | (0x0B-Note); } // note to play: lower for stronger sink
|
||||
else // if climb less than 1/2 m/s or sink less than 1 m/s
|
||||
{ Vario_Note=0x00; } // no sound
|
||||
}
|
||||
#endif // WITH_BEEPER
|
||||
|
||||
#ifdef WITH_BMP180
|
||||
static BMP180 Baro; // BMP180 barometer sensor
|
||||
#endif
|
||||
|
||||
#ifdef WITH_BMP280
|
||||
static BMP280 Baro; // BMP280 barometer sensor
|
||||
#endif
|
||||
|
||||
#ifdef WITH_MS5607
|
||||
static MS5607 Baro; // BMP280 barometer sensor
|
||||
#endif
|
||||
|
||||
static uint32_t AverPress; // [ Pa] summed Pressure over several readouts
|
||||
static uint8_t AverCount; // [int] number of summed Pressure readouts
|
||||
|
||||
static SlopePipe<int32_t> BaroPipe; // 4-point slope-fit pipe for pressure
|
||||
static LowPass2<int32_t,6,4,8> BaroNoise; // low pass (average) filter for pressure noise
|
||||
|
||||
static LowPass2<int64_t,10,9,12> PressAver, // low pass (average) filter for pressure
|
||||
AltAver; // low pass (average) filter for GPS altitude
|
||||
|
||||
static Delay<int32_t, 8> PressDelay; // 4-second delay for long-term climb rate
|
||||
|
||||
static char Line[64]; // line to prepare the barometer NMEA sentence
|
||||
|
||||
static uint8_t InitBaro()
|
||||
{ xSemaphoreTake(I2C_Mutex, portMAX_DELAY);
|
||||
Baro.Bus=BARO_I2C;
|
||||
uint8_t Err=Baro.CheckID();
|
||||
if(Err==0) Err=Baro.ReadCalib();
|
||||
#ifdef WITH_BMP180
|
||||
if(Err==0) Err=Baro.AcquireRawTemperature();
|
||||
if(Err==0) { Baro.CalcTemperature(); AverPress=0; AverCount=0; }
|
||||
#endif
|
||||
#if defined(WITH_BMP280) || defined(WITH_MS5607)
|
||||
if(Err==0) Err=Baro.Acquire();
|
||||
if(Err==0) { Baro.Calculate(); }
|
||||
#endif
|
||||
xSemaphoreGive(I2C_Mutex);
|
||||
// if(Err) LED_BAT_On();
|
||||
return Err==0 ? Baro.ADDR:0; }
|
||||
|
||||
static void ProcBaro(void)
|
||||
{
|
||||
static uint8_t PipeCount=0;
|
||||
|
||||
int16_t Sec = 10*(TimeSync_Time()%60); // [0.1sec]
|
||||
uint16_t Phase = TimeSync_msTime(); // sync to the GPS PPS
|
||||
if(Phase>=500) { Sec+=10; vTaskDelay(1000-Phase); } // wait till start of the measuring period
|
||||
else { Sec+= 5; vTaskDelay( 500-Phase); }
|
||||
if(Sec>=600) Sec-=600; // [0.1sec] pressure measurement time
|
||||
|
||||
#ifdef WITH_BMP180
|
||||
TickType_t Start=xTaskGetTickCount();
|
||||
xSemaphoreTake(I2C_Mutex, portMAX_DELAY);
|
||||
uint8_t Err=Baro.AcquireRawTemperature(); // measure temperature
|
||||
xSemaphoreGive(I2C_Mutex);
|
||||
if(Err==0) { Baro.CalcTemperature(); AverPress=0; AverCount=0; } // clear the average
|
||||
else { PipeCount=0;
|
||||
xSemaphoreTake(I2C_Mutex, portMAX_DELAY);
|
||||
I2C_Restart(Baro.Bus);
|
||||
xSemaphoreGive(I2C_Mutex);
|
||||
vTaskDelay(20);
|
||||
InitBaro(); // try to recover I2C bus and baro
|
||||
return; }
|
||||
|
||||
for(uint8_t Idx=0; Idx<24; Idx++)
|
||||
{ xSemaphoreTake(I2C_Mutex, portMAX_DELAY);
|
||||
uint8_t Err=Baro.AcquireRawPressure(); // take pressure measurement
|
||||
xSemaphoreGive(I2C_Mutex);
|
||||
if(Err==0) { Baro.CalcPressure(); AverPress+=Baro.Pressure; AverCount++; } // sum-up average pressure
|
||||
TickType_t Time=xTaskGetTickCount()-Start; if(Time>=200) break; } // but no longer than 250ms to fit into 0.5 second slot
|
||||
|
||||
if(AverCount==0) { PipeCount=0; return ; } // and we summed-up some measurements
|
||||
AverPress = ( (AverPress<<2) + (AverCount>>1) )/AverCount; // [0.25Pa]] make the average
|
||||
#endif
|
||||
#if defined(WITH_BMP280) || defined(WITH_MS5607)
|
||||
xSemaphoreTake(I2C_Mutex, portMAX_DELAY);
|
||||
uint8_t Err=Baro.Acquire();
|
||||
xSemaphoreGive(I2C_Mutex);
|
||||
if(Err==0) { Baro.Calculate(); }
|
||||
else { PipeCount=0; return; }
|
||||
AverPress = Baro.Pressure; // [0.25Pa]
|
||||
#endif
|
||||
BaroPipe.Input(AverPress); // [0.25Pa]
|
||||
if(PipeCount<255) PipeCount++; // count data going to the slope fitting pipe
|
||||
if(PipeCount<4) return;
|
||||
|
||||
BaroPipe.FitSlope(); // fit the average and slope from the four most recent pressure points
|
||||
int32_t PLR = Atmosphere::PressureLapseRate(((AverPress+2)>>2), Baro.Temperature); // [0.0001m/Pa]
|
||||
int32_t ClimbRate = (BaroPipe.Slope*PLR)/800; // [1/16Pa/0.5sec] * [0.0001m/Pa] x 800 => [0.01m/sec]
|
||||
|
||||
BaroPipe.CalcNoise(); // calculate the noise (average square residue)
|
||||
uint32_t Noise=BaroNoise.Process(BaroPipe.Noise); // pass the noise through the low pass filter
|
||||
Noise=(IntSqrt(25*Noise)+64)>>7; // [0.1 Pa] noise (RMS) measured on the pressure
|
||||
|
||||
int32_t Pressure=BaroPipe.Aver; // [1/16Pa]
|
||||
int32_t StdAltitude = Atmosphere::StdAltitude((Pressure+8)>>4); // [0.1 m]
|
||||
int32_t ClimbRate4sec = ((Pressure-PressDelay.Input(Pressure))*PLR)/3200; // [0.01m/sec] climb rate over 4 sec.
|
||||
#ifdef WITH_VARIO
|
||||
VarioSound(ClimbRate);
|
||||
// if(abs(ClimbRate4sec)>50) VarioSound(ClimbRate);
|
||||
// else VarioSound(2*ClimbRate4sec);
|
||||
#endif
|
||||
Pressure = (Pressure+2)>>2; // [0.25 Pa]
|
||||
if( (Phase>=500) && GPS_TimeSinceLock )
|
||||
{ PressAver.Process(Pressure); // [0.25 Pa] pass pressure through low pass filter
|
||||
AltAver.Process(GPS_Altitude); // [0.1 m] pass GPS altitude through same low pass filter
|
||||
}
|
||||
int32_t PressDiff=Pressure-((PressAver.Out+2048)>>12); // [0.25 Pa] pressure - <pressure>
|
||||
int32_t AltDiff = (PressDiff*(PLR>>4))/250; // [0.1 m]
|
||||
int32_t Altitude=((AltAver.Out+2048)>>12)+AltDiff; // [0.1 m]
|
||||
|
||||
uint8_t Frac = Sec%10; // [0.1s]
|
||||
if(Frac==0)
|
||||
{ GPS_Position *PosPtr = GPS_getPosition(Sec/10); // get GPS position record for this second
|
||||
#ifdef DEBUG_PRINT
|
||||
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
Format_String(CONS_UART_Write, "ProcBaro: ");
|
||||
Format_UnsDec(CONS_UART_Write, (uint16_t)Sec, 3, 1);
|
||||
Format_String(CONS_UART_Write, "s -> GPS: ");
|
||||
if(PosPtr)
|
||||
{ Format_UnsDec(CONS_UART_Write, (uint16_t)PosPtr->Sec, 2);
|
||||
CONS_UART_Write('.');
|
||||
Format_UnsDec(CONS_UART_Write, (uint16_t)PosPtr->FracSec, 2);
|
||||
CONS_UART_Write('s'); }
|
||||
Format_String(CONS_UART_Write, "\n");
|
||||
xSemaphoreGive(CONS_Mutex);
|
||||
#endif
|
||||
if(PosPtr) // if found:
|
||||
{ PosPtr->Pressure = Pressure; // [0.25Pa]
|
||||
PosPtr->StdAltitude = StdAltitude; // store standard pressure altitude
|
||||
PosPtr->Temperature = Baro.Temperature; // and temperature in the GPS record
|
||||
PosPtr->hasBaro=1; } // tick "hasBaro" flag
|
||||
}
|
||||
|
||||
uint8_t Len=0; // start preparing the barometer NMEA sentence
|
||||
Len+=Format_String(Line+Len, "$POGNB,");
|
||||
Len+=Format_UnsDec(Line+Len, Sec, 3, 1); // [sec] measurement time
|
||||
Line[Len++]=',';
|
||||
Len+=Format_SignDec(Line+Len, Baro.Temperature, 2, 1); // [degC] temperature
|
||||
Line[Len++]=',';
|
||||
Len+=Format_UnsDec(Line+Len, (uint32_t)(10*Pressure+2)>>2, 2, 1); // [Pa] pressure
|
||||
Line[Len++]=',';
|
||||
Len+=Format_UnsDec(Line+Len, Noise, 2, 1); // [Pa] pressure noise
|
||||
Line[Len++]=',';
|
||||
Len+=Format_SignDec(Line+Len, StdAltitude, 2, 1); // [m] standard altitude (calc. from pressure)
|
||||
Line[Len++]=',';
|
||||
Len+=Format_SignDec(Line+Len, Altitude, 2, 1); // [m] altitude (from cross-calc. with the GPS)
|
||||
Line[Len++]=',';
|
||||
Len+=Format_SignDec(Line+Len, ClimbRate, 3, 2); // [m/s] climb rate
|
||||
Line[Len++]=',';
|
||||
Len+=NMEA_AppendCheckCRNL(Line, Len);
|
||||
// if(CONS_UART_Free()>=128)
|
||||
{ xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
Format_String(CONS_UART_Write, Line, 0, Len); // send NMEA sentence to the console (UART1)
|
||||
xSemaphoreGive(CONS_Mutex); }
|
||||
#ifdef WITH_SDLOG
|
||||
if(Log_Free()>=128)
|
||||
{ xSemaphoreTake(Log_Mutex, portMAX_DELAY);
|
||||
Format_String(Log_Write, Line, Len); // send NMEA sentence to the log file
|
||||
xSemaphoreGive(Log_Mutex); }
|
||||
#endif
|
||||
|
||||
Len=0; // start preparing the PGRMZ NMEA sentence
|
||||
Len+=Format_String(Line+Len, "$PGRMZ,");
|
||||
Len+=Format_SignDec(Line+Len, StdAltitude, 2, 1); // [m] standard altitude (calc. from pressure)
|
||||
Line[Len++]=',';
|
||||
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)
|
||||
{ xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
Format_String(CONS_UART_Write, Line, 0, Len); // send NMEA sentence to the console (UART1)
|
||||
xSemaphoreGive(CONS_Mutex); }
|
||||
|
||||
}
|
||||
|
||||
#endif // WITH_BMP180/BMP280
|
||||
|
||||
|
||||
|
||||
extern "C"
|
||||
void vTaskSENS(void* pvParameters)
|
||||
{ vTaskDelay(20); // this delay seems to be essential - if you don't wait long enough, the BMP180 won't respond properly.
|
||||
|
||||
// #ifdef WITH_BEEPER
|
||||
// VarioSound(0);
|
||||
// #endif
|
||||
|
||||
#if defined(WITH_BMP180) || defined(WITH_BMP280) || defined(WITH_MS5607)
|
||||
BaroPipe.Clear (4*90000);
|
||||
BaroNoise.Set(12*16); // guess the pressure noise level
|
||||
|
||||
// initialize the GPS<->Baro correlator
|
||||
AltAver.Set(0); // [m] Altitude at sea level
|
||||
PressAver.Set(4*101300); // [Pa] Pressure at sea level
|
||||
PressDelay.Clear(4*101300);
|
||||
|
||||
uint8_t Detected = InitBaro();
|
||||
#endif
|
||||
|
||||
xSemaphoreTake(CONS_Mutex, portMAX_DELAY);
|
||||
Format_String(CONS_UART_Write, "TaskSENS:");
|
||||
|
||||
#ifdef WITH_BMP180
|
||||
Format_String(CONS_UART_Write, " BMP180: ");
|
||||
if(Detected) { Format_String(CONS_UART_Write, " @"); Format_Hex(CONS_UART_Write, Detected); }
|
||||
else Format_String(CONS_UART_Write, " ?!");
|
||||
#endif
|
||||
|
||||
#ifdef WITH_BMP280
|
||||
Format_String(CONS_UART_Write, " BMP280: ");
|
||||
if(Detected) { Format_String(CONS_UART_Write, " @"); Format_Hex(CONS_UART_Write, Detected); }
|
||||
else Format_String(CONS_UART_Write, " ?!");
|
||||
#endif
|
||||
|
||||
#ifdef WITH_MS5607
|
||||
Format_String(CONS_UART_Write, " MS5607: ");
|
||||
if(Detected) { Format_String(CONS_UART_Write, " @"); Format_Hex(CONS_UART_Write, Detected); }
|
||||
else Format_String(CONS_UART_Write, " ?!");
|
||||
#endif
|
||||
|
||||
Format_String(CONS_UART_Write, "\n");
|
||||
xSemaphoreGive(CONS_Mutex);
|
||||
|
||||
while(1)
|
||||
{
|
||||
#if defined(WITH_BMP180) || defined(WITH_BMP280) || defined(WITH_MS5607)
|
||||
ProcBaro();
|
||||
#else
|
||||
vTaskDelay(1000);
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -0,0 +1,6 @@
|
|||
#ifdef __cplusplus
|
||||
extern "C"
|
||||
#endif
|
||||
void vTaskSENS(void* pvParameters);
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,44 @@
|
|||
#include <stdint.h>
|
||||
|
||||
template <class Int>
|
||||
class SlopePipe
|
||||
{ public:
|
||||
Int Data[4];
|
||||
uint8_t Ptr;
|
||||
|
||||
Int Aver, Slope; // [1/ 4 Inp ] Average and Slope estimate
|
||||
Int Noise; // [1/16 Inp^2] Average squared residue
|
||||
|
||||
public:
|
||||
void Clear(Int Inp=0)
|
||||
{ for(Ptr=0; Ptr<4; Ptr++)
|
||||
Data[Ptr]=Inp;
|
||||
Ptr=0; }
|
||||
|
||||
void Input(Int Inp)
|
||||
{ Data[Ptr++]=Inp; Ptr&=3; }
|
||||
|
||||
void FitSlope(void)
|
||||
{ Int A =Data[Ptr++]; Ptr&=3;
|
||||
Int B =Data[Ptr++]; Ptr&=3;
|
||||
Int C =Data[Ptr++]; Ptr&=3;
|
||||
Int D =Data[Ptr++]; Ptr&=3;
|
||||
Aver = A+B+C+D;
|
||||
Slope = (C+D)-(A+B)+((D-A)<<1);
|
||||
Slope = ((Slope<<1))/5;
|
||||
}
|
||||
|
||||
void CalcNoise(void)
|
||||
{ Int Extr, Diff;
|
||||
Extr = Aver-Slope-(Slope>>1);
|
||||
Noise=0;
|
||||
for(uint8_t Idx=0; Idx<4; Idx++)
|
||||
{ Diff = (Data[Ptr++]<<2)-Extr; Ptr&=3;
|
||||
Noise+=Diff*Diff;
|
||||
Extr+= Slope; }
|
||||
Noise=(Noise+2)>>2;
|
||||
}
|
||||
|
||||
} ;
|
||||
|
||||
|
||||
|
|
@ -3,8 +3,7 @@
|
|||
|
||||
#include <stdint.h>
|
||||
|
||||
#include "freertos/FreeRTOS.h"
|
||||
#include "freertos/task.h"
|
||||
#include "hal.h"
|
||||
|
||||
void TimeSync_HardPPS(TickType_t Tick); // hardware PPS at the give system tick
|
||||
void TimeSync_HardPPS(void);
|
||||
|
|
|
|||
|
|
@ -83,6 +83,7 @@ CONFIG_PARTITION_TABLE_CUSTOM_FILENAME="partitions.csv"
|
|||
CONFIG_PARTITION_TABLE_CUSTOM_APP_BIN_OFFSET=0x10000
|
||||
CONFIG_PARTITION_TABLE_FILENAME="partitions.csv"
|
||||
CONFIG_APP_OFFSET=0x10000
|
||||
CONFIG_PARTITION_TABLE_MD5=y
|
||||
|
||||
#
|
||||
# Compiler options
|
||||
|
|
|
|||
Ładowanie…
Reference in New Issue