second option for elektronic compensation using pressures like TEK pr…

…obe does
iltis42 · May 15, 2024 · a74f527 · a74f527
1 parent 4b098fe
commit a74f527
Show file tree

Hide file tree

Showing 7 changed files with 15 additions and 8 deletions.
diff --git a/main/BMPVario.cpp b/main/BMPVario.cpp
@@ -77,7 +77,7 @@ double BMPVario::readTE( float tas, float tep ) {
 
 	bmpTemp = _sensorTE->readTemperature( success );
 	// ESP_LOGI(FNAME,"BMP temp=%0.1f", bmpTemp );
-	if( te_comp_enable.get() ) {
+	if( te_comp_enable.get() == TE_TEK_EPOT ) {
 		_currentAlt = altitude.get(); // already read
 		if( !success )
 			_currentAlt = lastAltitude;  // ignore readout when failed
@@ -87,10 +87,13 @@ double BMPVario::readTE( float tas, float tep ) {
 		_currentAlt += ealt;
 		ESP_LOGD(FNAME,"Energiehöhe @%0.1f km/h: %0.1f cw: %f", tas, ealt, cw );
 	}
+	else if( te_comp_enable.get() == TE_TEK_PRESSURE ){
+		_currentAlt = _sensorTE->calcAltitude(_qnh, baroP-(dynamicP/100.0)*(1+(te_comp_adjust.get()/100.0) ));  // subtract PI pressure like TEK probe does
+	}
 	else{
 		_currentAlt = _sensorTE->calcAltitude(_qnh, tep );
 	}
-	// ESP_LOGI(FNAME,"TE alt: %4.3f m", _currentAlt );
+	ESP_LOGI(FNAME,"TE alt: %4.3f m, ST: %.1f PI: %.1f", _currentAlt, baroP, (dynamicP*100) );
 	averageAlt += (_currentAlt - averageAlt) * 0.1;
 	double adiff = _currentAlt - Altitude;
 	// ESP_LOGI(FNAME,"BMPVario new alt %0.1f err %0.1f", _currentAlt, err);

diff --git a/main/SPL06-007.cpp b/main/SPL06-007.cpp
@@ -210,7 +210,7 @@ double SPL06_007::get_pcomp(bool &ok)
 	double p = double(c00) + praw_sc * (double(c10) + praw_sc * (double(c20) + praw_sc * double(c30))) + traw_sc * double(c01) + traw_sc * praw_sc * ( double(c11) + praw_sc * double(c21));
 	// if( address == 0x76 ) {
 	float t = (double(c0) * 0.5f) + (double(c1) * traw_sc);
-	ESP_LOGI(FNAME,"P:%06x,%d  T:%06x PC:%f T:%f I2C E:%d",_praw, _praw, _traw, p/100, t , errors );
+	// ESP_LOGI(FNAME,"P:%06x,%d  T:%06x PC:%f T:%f I2C E:%d",_praw, _praw, _traw, p/100, t , errors );
 	// }
 	ok = true;
 	last_p = p;

diff --git a/main/SetupMenu.cpp b/main/SetupMenu.cpp
@@ -852,8 +852,9 @@ void SetupMenu::vario_menu_create_s2f( MenuEntry *top ){
 void SetupMenu::vario_menu_create_ec( MenuEntry *top ){
 	SetupMenuSelect * enac = new SetupMenuSelect( "eCompensation", RST_NONE, 0 , false, &te_comp_enable );
 	enac->setHelp( "Enable/Disable electronic TE compensation option; Enable only when TE port is connected to ST (static) pressure");
-	enac->addEntry( "DISABLE");
-	enac->addEntry( "ENABLE");
+	enac->addEntry( "TEK Probe");
+	enac->addEntry( "EPOT");
+	enac->addEntry( "PRESSURE");
 	top->addEntry( enac );
 
 	SetupMenuValFloat * elca = new SetupMenuValFloat( "Adjustment", "%",	-100, 100, 0.1, 0, false, &te_comp_adjust );

diff --git a/main/SetupNG.cpp b/main/SetupNG.cpp
@@ -270,7 +270,7 @@ SetupNG<int>  			nmea_protocol( "NMEA_PROTOCOL", XCVARIO );
 SetupNG<int>		    log_level( "LOG_LEVEL", 3 );
 SetupNG<float>		    audio_factor( "AUDIO_FACTOR", 1 );
 SetupNG<float>		    te_comp_adjust ( "TECOMP_ADJ", 0);
-SetupNG<int>		    te_comp_enable( "TECOMP_ENA", 0 );
+SetupNG<int>		    te_comp_enable( "TECOMP_ENA", TE_TEK_PROBE );
 SetupNG<int>		    rotary_dir( "ROTARY_DIR", 0 );
 SetupNG<int>		    rotary_inc( "ROTARY_INC", 1 );
 SetupNG<int>		    rotary_dir_21( "ROTARY_DIR_21", 1 );

diff --git a/main/SetupNG.h b/main/SetupNG.h
@@ -108,6 +108,7 @@ typedef enum e_hardware_rev { 	HW_UNKNOWN=0,
 typedef enum e_drawing_prio { DP_NEEDLE, DP_BACKGROUND } e_drawing_prio_t;
 typedef enum e_equalizer_type {  AUDIO_EQ_DISABLE, AUDIO_EQ_LS4, AUDIO_EQ_LS8, AUDIO_EQ_LSEXT } e_equalizer_type_t;
 typedef enum e_logging { LOG_DISABLE, LOG_SENSOR_RAW } e_logging_t;
+typedef enum e_tek_compensation { TE_TEK_PROBE, TE_TEK_EPOT, TE_TEK_PRESSURE } e_tek_compensation_t;
 
 
 const int baud[] = { 0, 4800, 9600, 19200, 38400, 57600, 115200 };

diff --git a/main/sensor.cpp b/main/sensor.cpp
@@ -135,7 +135,7 @@ Compass *compass = 0;
 BTSender btsender;
 BLESender blesender;
 
-static float baroP=0; // barometric pressure
+float baroP=0; // barometric pressure
 static float teP=0;   // TE pressure
 static float temperature=15.0;
 static float xcvTemp=15.0;
@@ -144,7 +144,7 @@ long unsigned int _gps_millis = 0;
 
 
 static float battery=0.0;
-static float dynamicP; // Pitot
+float dynamicP; // Pitot
 
 float slipAngle = 0.0;
 

diff --git a/main/sensor.h b/main/sensor.h
@@ -86,6 +86,8 @@ extern S2F Speed2Fly;
 extern float meanClimb;
 extern Protocols OV;
 extern int the_can_mode;
+extern float baroP;    // Static pressure
+extern float dynamicP; // Pitot pressure
 
 extern long unsigned int _gps_millis;