From 8bf73761c162fd6d8051d0ac07124d5bec004166 Mon Sep 17 00:00:00 2001 From: Pawel Jalocha Date: Sun, 25 Feb 2018 18:47:03 +0000 Subject: [PATCH] Add pressure sensor support, push I2C to HAL, major files identical to STM32 code for easy passing of new features --- main/atmosphere.cpp | 5 + main/atmosphere.h | 58 +++++++++ main/bmp180.h | 146 ++++++++++++++++++++++ main/bmp280.h | 145 ++++++++++++++++++++++ main/ctrl.cpp | 10 +- main/format.cpp | 35 +++--- main/format.h | 28 ++--- main/hal.cpp | 70 ++++++----- main/hal.h | 28 ++++- main/main.cpp | 5 +- main/ms5607.h | 109 ++++++++++++++++ main/ogn.h | 75 +++++++++--- main/proc.cpp | 1 + main/rf.cpp | 4 - main/rfm.h | 8 +- main/sens.cpp | 293 ++++++++++++++++++++++++++++++++++++++++++++ main/sens.h | 6 + main/slope.h | 44 +++++++ main/timesync.h | 3 +- sdkconfig | 1 + 20 files changed, 973 insertions(+), 101 deletions(-) create mode 100644 main/atmosphere.cpp create mode 100644 main/atmosphere.h create mode 100644 main/bmp180.h create mode 100644 main/bmp280.h create mode 100644 main/ms5607.h create mode 100644 main/sens.cpp create mode 100644 main/sens.h create mode 100644 main/slope.h diff --git a/main/atmosphere.cpp b/main/atmosphere.cpp new file mode 100644 index 0000000..57e55bd --- /dev/null +++ b/main/atmosphere.cpp @@ -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] + diff --git a/main/atmosphere.h b/main/atmosphere.h new file mode 100644 index 0000000..c58cc8c --- /dev/null +++ b/main/atmosphere.h @@ -0,0 +1,58 @@ +#include +#include + +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 + + +} ; diff --git a/main/bmp180.h b/main/bmp180.h new file mode 100644 index 0000000..1ee2506 --- /dev/null +++ b/main/bmp180.h @@ -0,0 +1,146 @@ +#include +#include +#include + +#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 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<>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)<>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; } + +} ; diff --git a/main/bmp280.h b/main/bmp280.h new file mode 100644 index 0000000..51fc925 --- /dev/null +++ b/main/bmp280.h @@ -0,0 +1,145 @@ +#include +#include +#include + +#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); + } + +} ; diff --git a/main/ctrl.cpp b/main/ctrl.cpp index 700fa66..2bd1221 100644 --- a/main/ctrl.cpp +++ b/main/ctrl.cpp @@ -24,13 +24,13 @@ void PrintTasks(void (*CONS_UART_Write)(char)) uxArraySize = uxTaskGetSystemState( pxTaskStatusArray, uxArraySize, NULL ); for(UBaseType_t T=0; TpcTaskName); - 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; } diff --git a/main/format.cpp b/main/format.cpp index 8fe8253..42c6dd1 100644 --- a/main/format.cpp +++ b/main/format.cpp @@ -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>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; } diff --git a/main/format.h b/main/format.h index 461ae65..ec17957 100644 --- a/main/format.h +++ b/main/format.h @@ -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 int8_t Read_Hex(Type &Int, const char *Inp) // convert variable number of digits hexadecimal number into an integer diff --git a/main/hal.cpp b/main/hal.cpp index 13561fb..339bda4 100644 --- a/main/hal.cpp +++ b/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; } + // ====================================================================================================== diff --git a/main/hal.h b/main/hal.h index 6b160ca..0040e3e 100644 --- a/main/hal.h +++ b/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 + 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 + 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__ diff --git a/main/main.cpp b/main/main.cpp index f73737e..fc1dac9 100644 --- a/main/main.cpp +++ b/main/main.cpp @@ -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. diff --git a/main/ms5607.h b/main/ms5607.h new file mode 100644 index 0000000..aedcbe0 --- /dev/null +++ b/main/ms5607.h @@ -0,0 +1,109 @@ +#include +#include +#include + +#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] + +} ; + diff --git a/main/ogn.h b/main/ogn.h index 5c24b46..629497c 100644 --- a/main/ogn.h +++ b/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<>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]< #include "hal.h" + #include "proc.h" #include "ctrl.h" diff --git a/main/rf.cpp b/main/rf.cpp index 7f7eb1f..dd88b88 100644 --- a/main/rf.cpp +++ b/main/rf.cpp @@ -1,7 +1,3 @@ -#include "freertos/FreeRTOS.h" -#include "freertos/task.h" -#include "freertos/semphr.h" - #include "hal.h" #include "rf.h" diff --git a/main/rfm.h b/main/rfm.h index b404af3..a68b79d 100644 --- a/main/rfm.h +++ b/main/rfm.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 diff --git a/main/sens.cpp b/main/sens.cpp new file mode 100644 index 0000000..c4b4006 --- /dev/null +++ b/main/sens.cpp @@ -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 BaroPipe; // 4-point slope-fit pipe for pressure +static LowPass2 BaroNoise; // low pass (average) filter for pressure noise + +static LowPass2 PressAver, // low pass (average) filter for pressure + AltAver; // low pass (average) filter for GPS altitude + +static Delay 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 - + 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 + } +} + diff --git a/main/sens.h b/main/sens.h new file mode 100644 index 0000000..a3b2c52 --- /dev/null +++ b/main/sens.h @@ -0,0 +1,6 @@ +#ifdef __cplusplus + extern "C" +#endif +void vTaskSENS(void* pvParameters); + + diff --git a/main/slope.h b/main/slope.h new file mode 100644 index 0000000..9e66924 --- /dev/null +++ b/main/slope.h @@ -0,0 +1,44 @@ +#include + +template + 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; + } + +} ; + + diff --git a/main/timesync.h b/main/timesync.h index 235c62d..d87c146 100644 --- a/main/timesync.h +++ b/main/timesync.h @@ -3,8 +3,7 @@ #include -#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); diff --git a/sdkconfig b/sdkconfig index 22b32db..862cddd 100644 --- a/sdkconfig +++ b/sdkconfig @@ -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