openXsensor/openXsensor.ino

   1
   2
   3 #include "oXs_config_basic.h"
   4 #include "oXs_config_advanced.h"
   5 #include "oXs_config_macros.h"
   6 #include "oXs_voltage.h"
   7 #include "oXs_ms5611.h"
   8 #include "oXs_bmp180.h"
   9 #include "oXs_bmp280.h"
  10 #include "oXs_4525.h"
  11 #include "oXs_sdp3x.h"
  12 #include "oXs_ads1115.h"
  13 #include "oXs_curr.h"
  14 #include "oXs_out_frsky.h"
  15 #include "oXs_out_multiplex.h"
  16 #include "oXs_out_hott.h"
  17 #include "oXs_out_jeti.h"
  18 #include "oXs_general.h"
  19 #include "oXs_gps.h"
  20 #include "oXs_lora.h"
  21
  22 #ifdef USE_6050
  23   #include "oXs_imu.h"
  24   #include "KalmanFilter.h"
  25   #include "oXs_hmc5883.h"
  26 #endif
  27
  28 #if defined (SAVE_TO_EEPROM ) and ( SAVE_TO_EEPROM == YES )
  29   #include <EEPROM.h>
  30   #include "EEPROMAnything.h"
  31 #endif
  32
  33 #if  ! defined(PROTOCOL)
  34     #error The parameter PROTOCOL in config_basic.h is not defined
  35 #elif ! ( (PROTOCOL == FRSKY_SPORT) or (PROTOCOL == FRSKY_HUB) or (PROTOCOL == FRSKY_SPORT_HUB) or (PROTOCOL == HOTT) or (PROTOCOL == MULTIPLEX)  or (PROTOCOL == JETI))
  36     #error The parameter PROTOCOL in config_basic.h is NOT valid
  37 #endif
  38 #if ( ( (PROTOCOL == HOTT) or (PROTOCOL == MULTIPLEX)  or (PROTOCOL == JETI) ) and ( PIN_SERIALTX == 7 ) )
  39     #error PIN_SERIALTX may be 7 only for Frsky protocols
  40 #endif
  41 #if  defined(VARIO) and (! defined(VSPEED_SOURCE))
  42     #error The parameter VSPEED_SOURCE in config_basic.h is not defined while a type of baro sensor is defined
  43 #elif  defined(VARIO) and  defined(VSPEED_SOURCE) and ( ! ( (VSPEED_SOURCE == FIRST_BARO) or (VSPEED_SOURCE == SECOND_BARO) or (VSPEED_SOURCE == AVERAGE_FIRST_SECOND) \
  44                                                             or (VSPEED_SOURCE == AIRSPEED_COMPENSATED) or (VSPEED_SOURCE == BARO_AND_IMU) or (VSPEED_SOURCE == PPM_SELECTION) ) )
  45     #error The parameter VSPEED_SOURCE in config_basic.h is NOT valid
  46 #endif
  47
  48 #if defined( ARDUINO_MEASURES_A_CURRENT) && (ARDUINO_MEASURES_A_CURRENT == YES) and defined (AN_ADS1115_IS_CONNECTED) and (AN_ADS1115_IS_CONNECTED == YES) and defined(ADS_MEASURE) and defined(ADS_CURRENT_BASED_ON)
  49   #error It is not allowed to ask for current calculation based both on arduino Adc and on ads1115; define only or ARDUINO_MEASURES_A_CURRENT or ADS_CURRENT_BASED_ON
  50 #endif
  51
  52 #if defined (PIN_PPM ) && defined ( USE_6050 ) &&  ( PIN_INT_6050 == PIN_PPM )
  53   #error Error in oXs_config_advanced.h : PIN_PPM may not be equal to PIN_INT_6050
  54 #endif
  55
  56 #if  defined ( PPM_VIA_SPORT ) && ( (PROTOCOL  != FRSKY_SPORT) && (PROTOCOL  != FRSKY_SPORT_HUB) )
  57   #error Error in oXs_config_advanced.h : PPM_VIA_SPORT is allowed only when protocol is FRSKY_SPORT or FRSKY_SPORT_HUB
  58 #endif
  59
  60 #if defined( VFAS_SOURCE ) && ( !  ( ( VFAS_SOURCE == VOLT_1) || ( VFAS_SOURCE == VOLT_2) || ( VFAS_SOURCE == VOLT_3) || ( VFAS_SOURCE == VOLT_4) || ( VFAS_SOURCE == VOLT_5) || ( VFAS_SOURCE == VOLT_6) || ( VFAS_SOURCE == ADS_VOLT_1) || ( VFAS_SOURCE == ADS_VOLT_2) || ( VFAS_SOURCE == ADS_VOLT_3) || ( VFAS_SOURCE == ADS_VOLT_4)) )
  61  #error When defined, VFAS_SOURCE must be one of following values VOLT_1, VOLT_2, VOLT_3, VOLT_4, VOLT_5, VOLT_6, ADS_VOLT_1, ADS_VOLT_2, ADS_VOLT_3, ADS_VOLT_4
  62 #endif
  63 #if defined( FUEL_SOURCE ) && ( !  ( ( FUEL_SOURCE == VOLT_1) || ( FUEL_SOURCE == VOLT_2) || ( FUEL_SOURCE == VOLT_3) || ( FUEL_SOURCE == VOLT_4) || ( FUEL_SOURCE == VOLT_5) || ( FUEL_SOURCE == VOLT_6) || ( VFAS_SOURCE == ADS_VOLT_1) || ( VFAS_SOURCE == ADS_VOLT_2) || ( VFAS_SOURCE == ADS_VOLT_3) || ( VFAS_SOURCE == ADS_VOLT_4)) )
  64  #error When defined, FUEL_SOURCE must be one of following values VOLT_1, VOLT_2, VOLT_3, VOLT_4, VOLT_5, VOLT_6, ADS_VOLT_1, ADS_VOLT_2, ADS_VOLT_3, ADS_VOLT_4
  65 #endif
  66 #if defined( A3_SOURCE ) && ( !  ( ( A3_SOURCE == VOLT_1) || ( A3_SOURCE == VOLT_2) || ( A3_SOURCE == VOLT_3) || ( A3_SOURCE == VOLT_4) || ( A3_SOURCE == VOLT_5) || ( A3_SOURCE == VOLT_6) || ( A3_SOURCE == ADS_VOLT_1) || ( A3_SOURCE == ADS_VOLT_2) || ( A3_SOURCE == ADS_VOLT_3) || ( A3_SOURCE == ADS_VOLT_4) ) )
  67  #error When defined, A3_SOURCE must be one of following values VOLT_1, VOLT_2, VOLT_3, VOLT_4, VOLT_5, VOLT_6, ADS_VOLT_1, ADS_VOLT_2, ADS_VOLT_3, ADS_VOLT_4
  68 #endif
  69 #if defined( A4_SOURCE ) && ( !  ( ( A4_SOURCE == VOLT_1) || ( A4_SOURCE == VOLT_2) || ( A4_SOURCE == VOLT_3) || ( A4_SOURCE == VOLT_4) || ( A4_SOURCE == VOLT_5) || ( A4_SOURCE == VOLT_6) || ( A3_SOURCE == ADS_VOLT_1) || ( A3_SOURCE == ADS_VOLT_2) || ( A3_SOURCE == ADS_VOLT_3) || ( A3_SOURCE == ADS_VOLT_4) ) )
  70  #error When defined, A4_SOURCE must be one of following values VOLT_1, VOLT_2, VOLT_3, VOLT_4, VOLT_5, VOLT_6, ADS_VOLT_1, ADS_VOLT_2, ADS_VOLT_3, ADS_VOLT_4
  71 #endif
  72 #if defined( ACCX_SOURCE ) && ( !  ( ( ACCX_SOURCE == VOLT_1) || ( ACCX_SOURCE == VOLT_2) || ( ACCX_SOURCE == VOLT_3) || ( ACCX_SOURCE == VOLT_4) || ( ACCX_SOURCE == VOLT_5) || ( ACCX_SOURCE == VOLT_6) \
  73     || ( ACCX_SOURCE == ADS_VOLT_1) || ( ACCX_SOURCE == ADS_VOLT_2) || ( ACCX_SOURCE == ADS_VOLT_3) || ( ACCX_SOURCE == ADS_VOLT_4) \
  74     || ( ACCX_SOURCE == TEST_1) || ( ACCX_SOURCE == TEST_2) || ( ACCX_SOURCE == TEST_3) || ( ACCX_SOURCE == GLIDER_RATIO) || ( ACCX_SOURCE == SECONDS_SINCE_T0) || ( ACCX_SOURCE == AVERAGE_VSPEED_SINCE_TO) || ( ACCX_SOURCE == PITCH) || ( ACCX_SOURCE == ROLL) || ( ACCX_SOURCE == YAW) ) )
  75  #error When defined, ACCX_SOURCE must be one of following values TEST_1, TEST_2, TEST_3, GLIDER_RATIO , SECONDS_SINCE_T0 ,AVERAGE_VSPEED_SINCE_TO , VOLT_1, VOLT_2, VOLT_3, VOLT_4, VOLT_5, VOLT_6, ADS_VOLT_1, ADS_VOLT_2, ADS_VOLT_3, ADS_VOLT_4, PITCH, ROLL, YAW
  76 #endif
  77 #if defined( ACCY_SOURCE ) && ( !  ( ( ACCY_SOURCE == VOLT_1) || ( ACCY_SOURCE == VOLT_2) || ( ACCY_SOURCE == VOLT_3) || ( ACCY_SOURCE == VOLT_4) || ( ACCY_SOURCE == VOLT_5) || ( ACCY_SOURCE == VOLT_6) \
  78     || ( ACCY_SOURCE == ADS_VOLT_1) || ( ACCY_SOURCE == ADS_VOLT_2) || ( ACCY_SOURCE == ADS_VOLT_3) || ( ACCY_SOURCE == ADS_VOLT_4) \
  79     || ( ACCY_SOURCE == TEST_1) || ( ACCY_SOURCE == TEST_2) || ( ACCY_SOURCE == TEST_3) || ( ACCY_SOURCE == GLIDER_RATIO) || ( ACCY_SOURCE == SECONDS_SINCE_T0) || ( ACCY_SOURCE == AVERAGE_VSPEED_SINCE_TO) || ( ACCY_SOURCE == PITCH) || ( ACCY_SOURCE == ROLL) || ( ACCY_SOURCE == YAW) ) )
  80  #error When defined, ACCY_SOURCE must be one of following values TEST_1, TEST_2, TEST_3, GLIDER_RATIO , SECONDS_SINCE_T0 ,AVERAGE_VSPEED_SINCE_TO , VOLT_1, VOLT_2, VOLT_3, VOLT_4, VOLT_5, VOLT_6, ADS_VOLT_1, ADS_VOLT_2, ADS_VOLT_3, ADS_VOLT_4, PITCH, ROLL, YAW
  81 #endif
  82 #if defined( ACCZ_SOURCE ) && ( !  ( ( ACCZ_SOURCE == VOLT_1) || ( ACCZ_SOURCE == VOLT_2) || ( ACCZ_SOURCE == VOLT_3) || ( ACCZ_SOURCE == VOLT_4) || ( ACCZ_SOURCE == VOLT_5) || ( ACCZ_SOURCE == VOLT_6) \
  83     || ( ACCZ_SOURCE == ADS_VOLT_1) || ( ACCZ_SOURCE == ADS_VOLT_2) || ( ACCZ_SOURCE == ADS_VOLT_3) || ( ACCZ_SOURCE == ADS_VOLT_4) \
  84     || ( ACCZ_SOURCE == TEST_1) || ( ACCZ_SOURCE == TEST_2) || ( ACCZ_SOURCE == TEST_3) || ( ACCZ_SOURCE == GLIDER_RATIO) || ( ACCZ_SOURCE == SECONDS_SINCE_T0) || ( ACCZ_SOURCE == AVERAGE_VSPEED_SINCE_TO) || ( ACCZ_SOURCE == PITCH) || ( ACCZ_SOURCE == ROLL) || ( ACCZ_SOURCE == YAW) ) )
  85  #error When defined, ACCZ_SOURCE must be one of following values TEST_1, TEST_2, TEST_3, GLIDER_RATIO , SECONDS_SINCE_T0 ,AVERAGE_VSPEED_SINCE_TO , VOLT_1, VOLT_2, VOLT_3, VOLT_4, VOLT_5, VOLT_6, ADS_VOLT_1, ADS_VOLT_2, ADS_VOLT_3, ADS_VOLT_4, PITCH, ROLL, YAW
  86 #endif
  87 #if defined( T1_SOURCE ) && ( !  ( ( T1_SOURCE == PPM) || ( T1_SOURCE == GLIDER_RATIO) || ( T1_SOURCE == SECONDS_SINCE_T0) || ( T1_SOURCE == AVERAGE_VSPEED_SINCE_TO) || ( T1_SOURCE == SENSITIVITY) \
  88     || ( T1_SOURCE == VOLT_1) || ( T1_SOURCE == VOLT_2) || ( T1_SOURCE == VOLT_3) || ( T1_SOURCE == VOLT_4) || ( T1_SOURCE == VOLT_5) || ( T1_SOURCE == VOLT_6) \
  89     || ( T1_SOURCE == ADS_VOLT_1) || ( T1_SOURCE == ADS_VOLT_2) || ( T1_SOURCE == ADS_VOLT_3) || ( T1_SOURCE == ADS_VOLT_4) \
  90     || ( T1_SOURCE == TEST_1) || ( T1_SOURCE == TEST_2) || ( T1_SOURCE == TEST_3) ) )
  91  #error When defined, T1_SOURCE must be one of following values TEST_1, TEST_2, TEST_3, GLIDER_RATIO , SECONDS_SINCE_T0 ,AVERAGE_VSPEED_SINCE_TO , SENSITIVITY , PPM , VOLT_1, VOLT_2, VOLT_3, VOLT_4, VOLT_5, VOLT_6, ADS_VOLT_1, ADS_VOLT_2, ADS_VOLT_3, ADS_VOLT_4,
  92 #endif
  93 #if defined( T2_SOURCE ) && ( !  ( ( T2_SOURCE == PPM) || ( T2_SOURCE == GLIDER_RATIO) || ( T2_SOURCE == SECONDS_SINCE_T0) || ( T2_SOURCE == AVERAGE_VSPEED_SINCE_TO) || ( T2_SOURCE == SENSITIVITY) \
  94     || ( T2_SOURCE == VOLT_1) || ( T2_SOURCE == VOLT_2) || ( T2_SOURCE == VOLT_3) || ( T2_SOURCE == VOLT_4) || ( T2_SOURCE == VOLT_5) || ( T2_SOURCE == VOLT_6)\
  95     || ( T2_SOURCE == ADS_VOLT_1) || ( T2_SOURCE == ADS_VOLT_2) || ( T2_SOURCE == ADS_VOLT_3) || ( T2_SOURCE == ADS_VOLT_4) \
  96     || ( T2_SOURCE == TEST_1) || ( T2_SOURCE == TEST_2) || ( T2_SOURCE == TEST_3) ) )
  97  #error When defined, T2_SOURCE must be one of following values TEST_1, TEST_2, TEST_3, GLIDER_RATIO , SECONDS_SINCE_T0 ,AVERAGE_VSPEED_SINCE_TO , SENSITIVITY , PPM , VOLT_1, VOLT_2, VOLT_3, VOLT_4, VOLT_5, VOLT_6, ADS_VOLT_1, ADS_VOLT_2, ADS_VOLT_3, ADS_VOLT_4,
  98 #endif
  99 #if defined(AN_ADS1115_IS_CONNECTED) && (AN_ADS1115_IS_CONNECTED == YES) && (!defined(ADS_MEASURE))
 100   #error When AN_ADS1115_IS_CONNECTED is set on YES, ADS_MEASURE (in oXs_config_advanced.h) must be uncommented and contains 4 values
 101 #endif
 102 #if defined(ARDUINO_MEASURES_VOLTAGES) && (ARDUINO_MEASURES_VOLTAGES == YES) && (!defined(PIN_VOLTAGE))
 103   #error When ARDUINO_MEASURES_VOLTAGES is set on YES, PIN_VOLTAGE must be uncommented and contains 6 values
 104 #endif
 105 #if defined(ARDUINO_MEASURES_A_CURRENT) && (ARDUINO_MEASURES_A_CURRENT == YES) && (!defined(PIN_CURRENTSENSOR))
 106   #error When ARDUINO_MEASURES_A_CURRENT is set on YES, PIN_CURRENTSENSOR must be uncommented and must specify the analog pin that is connected to the current sensor
 107 #endif
 108
 109 #if defined ( ADS_AIRSPEED_BASED_ON ) && ( ! ( ( ADS_AIRSPEED_BASED_ON == ADS_VOLT_1) || ( ADS_AIRSPEED_BASED_ON == ADS_VOLT_2) || ( ADS_AIRSPEED_BASED_ON == ADS_VOLT_3) || ( ADS_AIRSPEED_BASED_ON == ADS_VOLT_4) ) )
 110  #error When defined, ADS_AIRSPEED_BASED_ON must be one of following values ADS_VOLT_1, ADS_VOLT_2, ADS_VOLT_3, ADS_VOLT_4
 111 #endif
 112 #if defined(GPS_REFRESH_RATE) && ( ! ( (GPS_REFRESH_RATE == 1) || (GPS_REFRESH_RATE == 5) || (GPS_REFRESH_RATE == 10) ))
 113   #error When defined GPS_REFRESH_RATE must be 1, 5 or 10
 114 #endif
 115
 116 #if defined ( MEASURE_RF_LINK_QUALITY ) && ( MEASURE_RF_LINK_QUALITY == YES)
 117   #ifndef PIN_PPM
 118     #error When MEASURE_RF_LINK_QUALITY = YES , PIN_PPM must be defined (in file oXs_config_advanced.h)
 119   #endif
 120   #if defined ( VSPEED_SOURCE ) &&  ( VSPEED_SOURCE == PPM_SELECTION)
 121     #error When MEASURE_RF_LINK_QUALITY = YES , VSPEED_SOURCE may not be set to PPM_SELECTION
 122   #endif
 123   #if defined (AIRSPEED_SENSOR_USE) && (not(AIRSPEED_SENSOR_USE == NO_AIRSPEED))
 124     #error When MEASURE_RF_LINK_QUALITY = YES , SEQUENCE_OUTPUTS may not be defined
 125   #endif
 126   #if defined ( SEQUENCE_OUTPUTS )
 127     #error When MEASURE_RF_LINK_QUALITY = YES , SEQUENCE_OUTPUTS may not be defined
 128   #endif
 129 #endif //defined ( MEASURE_RF_LINK_QUALITY ) && ( MEASURE_RF_LINK_QUALITY == YES)
 130
 131
 132 #ifdef PIN_PPM
 133  #if PIN_PPM == 2
 134         #define PPM_INTERRUPT                   ON // define to use interrupt code in Aserial.cpp
 135         #define PPM_INT_MASK                    0x03
 136         #define PPM_INT_EDGE                    0x01
 137         #define PPM_PIN_HEX                       0x02
 138         #define PPM_INT_BIT                     0x01
 139  #endif
 140
 141  #if PIN_PPM == 3
 142         #define PPM_INTERRUPT                   ON // define to use interrupt code in Aserial.cpp
 143         #define PPM_INT_MASK                    0x0C
 144         #define PPM_INT_EDGE                    0x04
 145         #define PPM_PIN_HEX                     0x04
 146         #define PPM_INT_BIT                     0x02
 147  #endif
 148 #endif
 149
 150
 151 //*************** There is normally no reason changing the 2 next parameters
 152 #define I2C_MS5611_Add           0x77 // 0x77 The I2C Address of the MS5611 breakout board
 153                                // (normally 0x76 or 0x77 configured on the MS5611 module
 154                                // via a solder pin or fixed)
 155
 156 #define I2C_4525_Add        0x28 // 0x28 is the default I2C adress of a 4525DO sensor
 157 #define I2C_SDP3X_Add       0x21 // 0x21 is the default I2C adress of a SDP3X sensor
 158 //#define I2C_SDP3X_Add       0x25 // 0x25 is the I2C adress of a SDP810 sensor
 159
 160
 161 #define I2C_ADS_Add 0x48 // default I2C address of ads1115 when addr pin is connected to ground
 162
 163 #define PIN_LED            13  // The Signal LED (default=13=onboard LED)
 164
 165
 166 extern unsigned long micros( void ) ;
 167 extern unsigned long millis( void ) ;
 168 //static unsigned long extendedMicros ;
 169
 170 #ifdef DEBUG_BLINK  // this does not require that DEBUG is active.; Use only one of the blink
 171    //DEBUG_BLINK_MAINLOOP
 172    //DEBUG_BLINK_CLIMBRATE
 173 #endif
 174
 175 #ifdef DEBUG
 176 //#define DEBUGCOMPENSATEDCLIMBRATE
 177 //#define DEBUGOUTDATATOSERIAL
 178 //#define DEBUGENTERLOOP
 179 //#define DEBUG_ENTER_READSENSORS
 180 //#define DEBUG_CALCULATE_FIELDS
 181 //#define DEBUGSEQUENCE
 182 //#define DEBUG_PPM_AVAILABLE_FROM_INTERRUPT
 183 #define DEBUG_RF_LINK_QUALITY
 184 //#define DEBUGPPMVALUE
 185 //#define DEBUGFORCEPPM
 186 //#define DEBUG_SELECTED_VARIO
 187 //#define DEBUG_VARIO_TIME
 188 //#define DEBUG_VOLTAGE_TIME
 189 //#define DEBUG_READ_SPORT
 190 //#define DEBUG_SIMULATE_FLOW_SENSOR
 191 //#define DEBUG_FLOW_SENSOR
 192 //#define DEBUG_RPM
 193 #endif
 194
 195
 196 // ************ declare some functions being used ***************
 197 int freeRam () ;
 198 void checkButton() ;
 199 void readSensors() ;
 200 void Reset1SecButtonPress() ;
 201 void Reset3SecButtonPress() ;
 202 void Reset10SecButtonPress() ;
 203 void SaveToEEProm() ;
 204 void LoadFromEEProm() ;
 205 void ProcessPPMSignal() ;
 206 //unsigned int ReadPPM() ;
 207 void ReadPPM() ;
 208 bool checkFreeTime() ;
 209 void setNewSequence() ;
 210 void checkSequence() ;
 211 void blinkLed( uint8_t blinkType ) ;
 212 void checkFlowParam() ;
 213
 214 // *********** declare some variables *****************************
 215 #ifdef VARIO
 216 bool newVarioAvailable ;
 217 struct ONE_MEASUREMENT mainVspeed ;
 218 #endif
 219 #ifdef VARIO2
 220 bool newVarioAvailable2 ;
 221 #endif
 222
 223 #if defined (VARIO) && defined ( AIRSPEED_IS_USED)
 224 struct ONE_MEASUREMENT compensatedClimbRate ;
 225 bool switchCompensatedClimbRateAvailable ;
 226 float rawCompensatedClimbRate ;
 227 #endif
 228
 229 #if defined (VARIO) && ( defined (VARIO2) || defined ( AIRSPEED_IS_USED) || defined (USE_6050) )
 230 struct ONE_MEASUREMENT switchVSpeed ;
 231 #endif
 232
 233 #if defined (VARIO) && defined (VARIO2)
 234 struct ONE_MEASUREMENT averageVSpeed ;
 235 float averageVSpeedFloat ;
 236 #endif
 237
 238 #if defined  (VARIO) && defined (GLIDER_RATIO_CALCULATED_AFTER_X_SEC) && GLIDER_RATIO_CALCULATED_AFTER_X_SEC >= 1
 239 struct ONE_MEASUREMENT gliderRatio ;
 240 struct ONE_MEASUREMENT secFromT0 ; // in 1/10 sec
 241 struct ONE_MEASUREMENT averageVspeedSinceT0 ; //in cm/sec
 242 void calculateAverages();
 243 boolean gliderRatioPpmOn = false ;
 244 #endif
 245
 246 #ifdef USE_6050
 247   KalmanFilter kalman ;
 248   float zTrack ;
 249   float vTrack ;
 250   struct ONE_MEASUREMENT vSpeedImu ;
 251   bool vTrackAvailable ;
 252   bool switchVTrackAvailable ;
 253   extern float linear_acceleration_x ;
 254   extern float linear_acceleration_y ;
 255   extern float linear_acceleration_z ;
 256   extern float world_linear_acceleration_z ;
 257   extern bool newImuAvailable;
 258   float altitudeToKalman ;
 259   int countAltitudeToKalman = 100 ;
 260   int32_t altitudeOffsetToKalman ;
 261   extern volatile uint32_t lastImuInterruptMillis ;
 262 #ifdef USE_HMC5883
 263   extern float magHeading ;
 264   extern boolean newMagHeading;
 265 #endif
 266   #ifdef DEBUG_KALMAN_TIME
 267     int delayKalman[5] ;
 268   #endif
 269 #endif // end of USE_6050
 270
 271
 272
 273 #if defined(ADS_AIRSPEED_BASED_ON) and (ADS_AIRSPEED_BASED_ON >= ADS_VOLT1) and (ADS_AIRSPEED_BASED_ON <= ADS_VOLT_4)
 274   extern float ads_sumDifPressureAdc_0 ;
 275   extern uint8_t ads_cntDifPressureAdc_0 ;
 276 #endif
 277
 278 uint16_t ppmus ; // duration of ppm in usec
 279 int prevPpm ; //^previous ppm
 280 struct ONE_MEASUREMENT ppm ; // duration of pulse in range -100 / + 100 ; can exceed those limits
 281 struct ONE_MEASUREMENT newPpm ; // keep the ppm value received via sport before it is processed in other part of the code
 282
 283 #if defined ( A_FLOW_SENSOR_IS_CONNECTED ) && ( A_FLOW_SENSOR_IS_CONNECTED == YES)  // we will use interrupt PCINT0
 284   volatile uint16_t flowMeterCnt ;            // counter of pin change connected to the flow sensor (increased in the interrupt. reset every X sec when flow is processed)
 285   float currentFlow  ;                     // count the consumed ml/min during the last x sec
 286   float consumedML   ;                     // total of consumed ml since the last reset (or restart if reset is not activated); can be saved in EEPROM
 287   struct ONE_MEASUREMENT actualFlow ;             // in ml/min
 288   struct ONE_MEASUREMENT remainingFuelML ;         // in ml
 289   struct ONE_MEASUREMENT fuelPercent ;             // in % of tank capacity
 290   boolean newFlowAvailable = false ;
 291   int16_t flowParam[8] =  { INIT_FLOW_PARAM } ;                      // table that contains the parameters to correct the ml/pulse depending on the flow ; can be loaded by SPORT in EEPROM; can also be defined in a parameter
 292   uint16_t tankCapacity =  TANK_CAPACITY ;     // capacity of fuel tank
 293   uint16_t consumedMLEeprom   ;                 //Last value saved in eeprom
 294   uint16_t tankCapacityEeprom ;                 //Last value saved in eeprom
 295   int16_t flowParamEeprom[8]  ;                 //Last value saved in eeprom
 296 #endif
 297
 298
 299
 300 #ifdef SEQUENCE_OUTPUTS
 301 #ifdef SEQUENCE_m100
 302   uint8_t sequence_m100 [] = { SEQUENCE_m100 } ;
 303 #else
 304   uint8_t sequence_m100 [2] = { 0 , 0 } ;
 305 #endif
 306 #ifdef SEQUENCE_m75
 307   uint8_t sequence_m75 [] = { SEQUENCE_m75 } ;
 308 #else
 309   uint8_t sequence_m75 [2] = { 0 , 0 } ;
 310 #endif
 311 #ifdef SEQUENCE_m50
 312   uint8_t sequence_m50 [] = { SEQUENCE_m50 } ;
 313 #else
 314   uint8_t sequence_m50 [2] = { 0 , 0 } ;
 315 #endif
 316 #ifdef SEQUENCE_m25
 317   uint8_t sequence_m25 [] = { SEQUENCE_m25 } ;
 318 #else
 319   uint8_t sequence_m25 [2] = { 0 , 0 } ;
 320 #endif
 321 #ifdef SEQUENCE_0
 322   uint8_t sequence_0 [] = { SEQUENCE_0 } ;
 323 #else
 324   uint8_t sequence_0 [2] = { 0 , 0 } ;
 325 #endif
 326 #ifdef SEQUENCE_25
 327   uint8_t sequence_25 [] = { SEQUENCE_25 } ;
 328 #else
 329   uint8_t sequence_25 [2] = { 0 , 0 } ;
 330 #endif
 331 #ifdef SEQUENCE_50
 332   uint8_t sequence_50 [] = { SEQUENCE_50 } ;
 333 #else
 334   uint8_t sequence_50 [2] = { 0 , 0 } ;
 335 #endif
 336 #ifdef SEQUENCE_75
 337   uint8_t sequence_75 [] = { SEQUENCE_75 } ;
 338 #else
 339   uint8_t sequence_75 [2] = { 0 , 0 } ;
 340 #endif
 341 #ifdef SEQUENCE_100
 342   uint8_t sequence_100 [] = { SEQUENCE_100 } ;
 343 #else
 344   uint8_t sequence_100 [2] = { 0 , 0 } ;
 345 #endif
 346 #ifdef SEQUENCE_LOW
 347   uint8_t sequence_low [] = { SEQUENCE_LOW } ;
 348 #else
 349   uint8_t sequence_low [2] = { 0 , 0 } ;
 350 #endif
 351
 352
 353     uint16_t sequenceMaxNumber[10] ;  // this array contains the number of events of each sequence
 354     uint8_t * sequencePointer[10] ; // this is an array of pointers; each pointer refers to the first element of a 2 dim array.
 355
 356     uint8_t *seqRef  ;  // seqRef contains a pointer to the first item of the selected sequence array
 357     uint16_t seqMax ; // number of sequences in the selected array
 358     uint8_t seqState ; // say if the sequence is starting (0), running (1) or stopped (2)
 359     uint16_t seqStep ;  // says the current step in the sequence , first step = 0 ;
 360     uint8_t sequenceOutputs = SEQUENCE_OUTPUTS & 0b00111111 ; // Mask to define which pin of portB are outputs
 361 #ifdef SEQUENCE_UNIT
 362     uint16_t sequenceUnit = SEQUENCE_UNIT * 10 ;
 363 #else
 364     uint16_t sequenceUnit = 10 ;
 365 #endif // SEQUENCE_UNIT
 366 #endif // end of SEQUENCE_OUTPUTS
 367
 368 int8_t prevPpmMain = -100 ; // this value is unusual; so it will forced a change at first call
 369 bool lowVoltage = false ;
 370 bool prevLowVoltage = false ;
 371 //uint32_t currentLoopMillis ;
 372
 373 volatile bool RpmSet  ;
 374 volatile uint16_t RpmValue ;
 375 unsigned long lastRpmMillis ;
 376 #if defined (MEASURE_RPM)
 377   struct ONE_MEASUREMENT sport_rpm ;
 378 #endif
 379
 380 int PWRValue; // calculation field for Vertical speed on PWR
 381 unsigned long lastMillisPWR ;
 382
 383 float actualPressure  ; // default pressure in pascal; to actualise if vario exist; is used in airspeed calculation.
 384 int sensitivityPpmMapped ;
 385 int compensationPpmMapped ;
 386 struct ONE_MEASUREMENT test1 ; // used in order to test the transmission of any value
 387 struct ONE_MEASUREMENT test2 ; // used in order to test the transmission of any value
 388 struct ONE_MEASUREMENT test3 ; // used in order to test the transmission of any value
 389 //int32_t test1Value ;// used in order to test the transmission of any value
 390 //bool test1ValueAvailable ;
 391 //int32_t test2Value ;// used in order to test the transmission of any value
 392 //bool test2ValueAvailable ;
 393 //int32_t test3Value ;// used in order to test the transmission of any value
 394 //bool test3ValueAvailable ;
 395 #ifdef VARIO_PRIMARY
 396 uint8_t selectedVario = VARIO_PRIMARY ; // identify the vario to be used when switch vario with PPM is active (1 = first MS5611)
 397 #endif
 398
 399 // to read SPORT (for Frsky protocol
 400 extern uint8_t  volatile TxData[8] ;
 401 extern uint8_t  volatile TxDataIdx ;
 402
 403
 404 //******************* Create instances of the used classes ******************************************
 405 #ifdef VARIO
 406   #ifdef SENSOR_IS_BMP180
 407       #ifdef DEBUG
 408         OXS_BMP180 oXs_MS5611 = OXS_BMP180(Serial);
 409       #else
 410         OXS_BMP180 oXs_MS5611 = OXS_BMP180();
 411       #endif  //DEBUG
 412   #elif defined(SENSOR_IS_BMP280 )
 413       #ifdef DEBUG
 414         OXS_BMP280 oXs_MS5611 = OXS_BMP280(Serial);
 415       #else
 416         OXS_BMP280 oXs_MS5611 = OXS_BMP280();
 417       #endif  //DEBUG
 418   #else // not a BMP180 or BMP280
 419       #ifdef DEBUG
 420         OXS_MS5611 oXs_MS5611(I2C_MS5611_Add,Serial);
 421       #else
 422         OXS_MS5611 oXs_MS5611(I2C_MS5611_Add);
 423       #endif  //DEBUG
 424   #endif // BMP180 or MS5611 sensor
 425 #endif // VARIO
 426
 427
 428 #ifdef VARIO2
 429   #ifdef DEBUG
 430     OXS_MS5611 oXs_MS5611_2(I2C_MS5611_Add - 1,Serial);
 431   #else
 432     OXS_MS5611 oXs_MS5611_2(I2C_MS5611_Add - 1);
 433   #endif  //DEBUG
 434 #endif
 435
 436 #ifdef AIRSPEED_4525 // differential pressure
 437   #ifdef DEBUG
 438     OXS_4525 oXs_4525(I2C_4525_Add ,Serial);
 439   #else
 440     OXS_4525 oXs_4525(I2C_4525_Add);
 441   #endif  //DEBUG
 442 #endif
 443
 444 #ifdef AIRSPEED_SDP3X
 445   #ifdef DEBUG
 446     OXS_SDP3X oXs_sdp3x(I2C_SDP3X_Add ,Serial);
 447   #else
 448     OXS_SDP3X oXs_sdp3x(I2C_SDP3X_Add);
 449   #endif  //DEBUG
 450 #endif
 451
 452 #if defined(ARDUINO_MEASURES_VOLTAGES) && (ARDUINO_MEASURES_VOLTAGES == YES)
 453   #ifdef DEBUG
 454     OXS_VOLTAGE oXs_Voltage(Serial);
 455   #else
 456     OXS_VOLTAGE oXs_Voltage(0);
 457   #endif  //DEBUG
 458 #endif
 459
 460 #if defined(ARDUINO_MEASURES_A_CURRENT) && (ARDUINO_MEASURES_A_CURRENT == YES)
 461   #ifdef DEBUG
 462     OXS_CURRENT oXs_Current(PIN_CURRENTSENSOR,Serial);
 463   #else
 464     OXS_CURRENT oXs_Current(PIN_CURRENTSENSOR);
 465   #endif //DEBUG
 466 #endif
 467
 468 #ifdef GPS_INSTALLED
 469   #ifdef DEBUG
 470     OXS_GPS oXs_Gps(Serial);
 471   #else
 472     OXS_GPS oXs_Gps(0);
 473   #endif //DEBUG
 474 #endif
 475
 476 #if defined(AN_ADS1115_IS_CONNECTED) && (AN_ADS1115_IS_CONNECTED == YES ) && defined(ADS_MEASURE)
 477   #ifdef DEBUG
 478     OXS_ADS1115 oXs_ads1115(  I2C_ADS_Add , Serial);
 479   #else
 480     OXS_ADS1115 oXs_ads1115( I2C_ADS_Add );
 481   #endif //DEBUG
 482 #endif
 483
 484
 485 //Create a class used for telemetry ;content of class depends on the selected protocol (managed via #ifdef in different files)
 486 #ifdef DEBUG
 487   OXS_OUT oXs_Out(PIN_SERIALTX,Serial);
 488 #else
 489   OXS_OUT oXs_Out(PIN_SERIALTX);
 490 #endif
 491 // *********************************** End of create objects *************************************************
 492
 493                             // Mike I do not understand this instruction; could you explain
 494 #define FORCE_INDIRECT(ptr) __asm__ __volatile__ ("" : "=e" (ptr) : "0" (ptr))
 495
 496
 497 //******************************************* Setup () *******************************************************
 498
 499 void setup(){
 500 // set up the UART speed (38400 if GPS installed else 115200)
 501 #ifdef GPS_INSTALLED
 502   Serial.begin(38400); // when GPS is used, baudrate is reduced because main loop must have the time to read the received char.
 503 #endif
 504 #ifdef DEBUG
 505 #ifndef GPS_INSTALLED
 506   Serial.begin(115200L); // when GPS is not used, baudrate can be 115200
 507 #endif
 508   Serial.println(F("openXsensor starting.."));
 509   Serial.print(F(" milli="));
 510   Serial.println(millis());
 511   Serial.print(F("freeRam="));
 512   Serial.println(freeRam());
 513 #endif
 514
 515 // set up pins
 516 #ifdef DEBUG_SETUP_PIN
 517     pinMode(DEBUG_SETUP_PIN, OUTPUT); // Set the start signal to high to say that set up start
 518     digitalWrite(DEBUG_SETUP_PIN, HIGH);
 519 #endif
 520
 521   #ifdef DEBUG_SPORT_PIN
 522     pinMode(DEBUG_SPORT_PIN, OUTPUT); // Set the pulse used during SPORT detection to LOW because detection is not yet started
 523     digitalWrite(DEBUG_SPORT_PIN, LOW);
 524 #endif
 525
 526 #ifdef DEBUG_BLINK
 527   pinMode(PIN_LED, OUTPUT); // The signal LED (used for the function debug loop)
 528 #endif
 529
 530 #ifdef PIN_PUSHBUTTON
 531   pinMode(PIN_PUSHBUTTON, INPUT_PULLUP);
 532 #endif
 533
 534   pinMode(PIN_LED, OUTPUT); // The signal LED (used for the function push button)
 535
 536 //  sensitivityPpmMapped = 0 ;
 537   compensationPpmMapped = 100 ;
 538 //  test1.value = 0 ;
 539 //  test2.value = 0 ;
 540 //  test3.value = 0 ;
 541 //  test1.available = false ;
 542 //  test2.available = false ;
 543 //  test3.available = false ;
 544   actualPressure = 101325 ; // default pressure in pascal; to actualise if vario exist; is used in airspeed calcualtion.
 545
 546
 547   // ******** Invoke all setup methods and set reference
 548 #if defined(ARDUINO_MEASURES_VOLTAGES) && (ARDUINO_MEASURES_VOLTAGES == YES)
 549   oXs_Voltage.setupVoltage();
 550   oXs_Out.voltageData=&oXs_Voltage.voltageData;
 551 #endif
 552
 553 #ifdef VARIO
 554 #ifdef DEBUG
 555   Serial.println(F("vario setting up.."));
 556   delay(1000);
 557 #endif
 558   oXs_MS5611.setup();
 559 #ifdef DEBUG
 560   Serial.println(F("vario is up.."));
 561   delay(1000);
 562 #endif
 563
 564   oXs_Out.varioData=&oXs_MS5611.varioData;
 565   #ifdef PIN_ANALOG_VSPEED
 566     lastMillisPWR = 3500 ; // So we will wait for 3.5 sec before generating a Vertical speed on PWM
 567     analogWrite(PIN_ANALOG_VSPEED,255/5*1.6); // initialize the output pin
 568   #endif
 569 #endif // vario
 570
 571 #ifdef VARIO2
 572   oXs_MS5611_2.setup();
 573   oXs_Out.varioData_2=&oXs_MS5611_2.varioData;
 574 #endif // vario
 575
 576 #ifdef AIRSPEED_4525
 577   oXs_4525.setup();
 578   oXs_Out.airSpeedData=&oXs_4525.airSpeedData;
 579 #endif // end AIRSPEED
 580
 581 #ifdef AIRSPEED_SDP3X
 582   oXs_sdp3x.setup();
 583   oXs_Out.airSpeedData=&oXs_sdp3x.airSpeedData;
 584 #endif // end AIRSPEED SDP3X
 585
 586 #if defined(ARDUINO_MEASURES_A_CURRENT) && (ARDUINO_MEASURES_A_CURRENT == YES)
 587   oXs_Current.setupCurrent( );
 588   oXs_Out.currentData=&oXs_Current.currentData;
 589 #endif
 590
 591 #if defined (VARIO) &&  defined ( AIRSPEED_IS_USED)
 592   compensatedClimbRate.available = false;
 593 //  compensatedClimbRate = 0;
 594 #endif
 595
 596 #ifdef GPS_INSTALLED
 597   oXs_Gps.setupGps();
 598 #endif
 599
 600   oXs_Out.setup();
 601
 602 #ifdef USE_6050
 603     setupImu() ;
 604 #endif
 605
 606 #if defined( USE_HMC5883 ) && defined (USE_6050)
 607   setup_hmc5883() ;  // set up magnetometer
 608 #endif
 609
 610 #if defined(AN_ADS1115_IS_CONNECTED) && (AN_ADS1115_IS_CONNECTED == YES ) && defined(ADS_MEASURE)
 611   oXs_ads1115.setup() ;
 612 #if defined(AN_ADS1115_IS_CONNECTED) && (AN_ADS1115_IS_CONNECTED == YES ) && defined(ADS_MEASURE) && defined(ADS_CURRENT_BASED_ON)
 613   oXs_Out.currentData=&oXs_ads1115.adsCurrentData;
 614 #endif
 615 #if defined(AN_ADS1115_IS_CONNECTED) && (AN_ADS1115_IS_CONNECTED == YES ) && defined(ADS_MEASURE) && defined(ADS_AIRSPEED_BASED_ON)
 616   oXs_Out.airSpeedData=&oXs_ads1115.adsAirSpeedData;
 617 #endif
 618 #endif
 619
 620 #if defined (SAVE_TO_EEPROM ) and ( SAVE_TO_EEPROM == YES )
 621   LoadFromEEProm();
 622 #endif
 623
 624 #ifdef PIN_PPM
 625 //    ppmus = 0 ;
 626 //    prevPpm = 0 ;
 627     ppm.available = false ;
 628 #endif
 629
 630 #ifdef PPM_INTERRUPT      // PPM use INT0 (pin 2) or INT1 (pin 3) that are on port D
 631         PORTD |= PPM_PIN_HEX ;  // Pullup resistor
 632         DDRD &= ~PPM_PIN_HEX ;  // Input
 633         EICRA |= PPM_INT_MASK ;         // Interrupt on rising edge
 634         EIFR = PPM_INT_BIT ;                    // Clear interrupt flag
 635         EIMSK |= PPM_INT_BIT ;          // Enable interrupt
 636 #endif
 637
 638 #if defined ( A_FLOW_SENSOR_IS_CONNECTED ) && ( A_FLOW_SENSOR_IS_CONNECTED == YES)  // we will use interrupt PCINT0
 639   PORTB |= 0X2 ;  // set pullup resistor on pin 9 (= PB1)
 640   DDRB &= ~ 0X2 ; // set pin as input on pin 9 (= PB1)
 641   PCMSK0 = 0X2 ; // prepare to allow pin change interrupt on only pin 9 ( = PB1)
 642   PCICR |= (1<<PCIE0)   ;     // enable pin change interrupt 0 so on pin from PB0 to PB7 = e.g. pins 8 to 13 to collect pin changes for flow sensor
 643 #endif
 644
 645   RpmSet = false ;
 646 //  RpmValue = 0 ;
 647 //  RpmAvailable = false ;
 648
 649 //************************* set up of sequence outputs
 650 #ifdef SEQUENCE_OUTPUTS
 651     sequenceMaxNumber[0] = sizeof(sequence_m100) ; // sizeof(sequence_m100[0]) ;
 652     sequenceMaxNumber[1] = sizeof(sequence_m75) ;// sizeof(sequence_m75[0]) ;
 653     sequenceMaxNumber[2] = sizeof(sequence_m50) ;// sizeof(sequence_m50[0]) ;
 654     sequenceMaxNumber[3] = sizeof(sequence_m25) ;// sizeof(sequence_m25[0]) ;
 655     sequenceMaxNumber[4] = sizeof(sequence_0) ;// sizeof(sequence_0[0]) ;
 656     sequenceMaxNumber[5] = sizeof(sequence_25) ;// sizeof(sequence_25[0]) ;
 657     sequenceMaxNumber[6] = sizeof(sequence_50) ;// sizeof(sequence_50[0]) ;
 658     sequenceMaxNumber[7] = sizeof(sequence_75) ;// sizeof(sequence_75[0]) ;
 659     sequenceMaxNumber[8] = sizeof(sequence_100) ;// sizeof(sequence_100[0]) ;
 660     sequenceMaxNumber[9] = sizeof(sequence_low) ;// sizeof(sequence_100[0]) ;
 661     sequencePointer[0] = &sequence_m100[0] ;
 662     sequencePointer[1] = &sequence_m75[0] ;
 663     sequencePointer[2] = &sequence_m50[0] ;
 664     sequencePointer[3] = &sequence_m25[0] ;
 665     sequencePointer[4] = &sequence_0[0] ;
 666     sequencePointer[5] = &sequence_25[0] ;
 667     sequencePointer[6] = &sequence_50[0] ;
 668     sequencePointer[7] = &sequence_75[0] ;
 669     sequencePointer[8] = &sequence_100[0] ;
 670     sequencePointer[9] = &sequence_low[0] ;
 671 #ifdef DEBUG
 672     seqRef = sequencePointer[0] ;  // seqTab contains pointers to the sequence array
 673     seqMax = sequenceMaxNumber[0] ;
 674     Serial.print(F("SeqRef="));  Serial.println( (uint16_t) seqRef) ;
 675     Serial.print(F("SeqMax="));  Serial.println( (uint16_t) seqMax) ;
 676     Serial.print(F("Sequence_m100="));
 677     for (uint8_t idxDebugSeq = 0 ; idxDebugSeq < (seqMax) ; idxDebugSeq++ ) {
 678       Serial.print( *(seqRef + idxDebugSeq ) );
 679       Serial.print(F(" , "));
 680     }
 681     Serial.println(F(" "));
 682 #endif
 683     seqState = 2 ; // declare that sequence is stopped
 684     PORTB &= ~sequenceOutputs ; // set all output to LOW
 685     DDRB |= sequenceOutputs ; // set pin to output mode
 686
 687 //#ifdef DEBUGSEQUENCE
 688     ppm.value = -100 ; // fix the sequence to be used by default (e.g. when no PPM signal is present);
 689     setNewSequence( ) ;
 690 //#endif
 691 #endif //****************************** end of SEQUENCE_OUTPUTS
 692
 693 #ifdef DEBUG_SPORT_PIN
 694     digitalWrite(DEBUG_SPORT_PIN, LOW);
 695 //    pinMode(DEBUG_SPORT_PIN, INPUT); // Set the pin in input mode because it is not used anymore
 696 #endif
 697 #ifdef DEBUG_SETUP_PIN
 698     digitalWrite(DEBUG_SETUP_PIN, LOW); // Set the setup signal to low to say that set up is done
 699 //    pinMode(DEBUG_SETUP_PIN, OUTPUT);
 700 #endif
 701
 702 #ifdef DEBUG
 703   Serial.println(F("End of general set up"));
 704 #endif
 705
 706 } // ******************** end of Setup *****************************************************
 707
 708
 709 //*******************************************************************************************
 710 //                                Main loop                                               ***
 711 //*******************************************************************************************
 712 void loop(){
 713 /*
 714 uint8_t flagMillis ;
 715 static uint32_t lastLoop20Millis ;
 716 static uint32_t lastLoop50Millis ;
 717 static uint32_t lastLoop200Millis ;
 718 static uint32_t lastLoop500Millis ;
 719
 720 #define BIT20MILLIS 1 ;
 721 #define BIT50MILLIS 2 ;
 722 #define BIT200MILLIS 4 ;
 723 #define BIT500MILLIS 8 ;
 724 */
 725 //currentLoopMillis = millis() ;
 726 /*
 727 if ( currentLoopMillis - lastLoop20Millis > 20 ) {
 728   lastLoop20Millis = currentLoopMillis ;
 729   flagMillis = BIT20MILLIS ;
 730 }
 731 if ( currentLoopMillis - lastLoop50Millis > 50 ) {
 732   lastLoop50Millis = currentLoopMillis ;
 733   flagMillis |= BIT50MILLIS ;
 734 }
 735 if ( currentLoopMillis - lastLoop200Millis > 200 ) {
 736   lastLoop200Millis = currentLoopMillis ;
 737   flagMillis |= BIT200MILLIS ;
 738 }
 739 if ( currentLoopMillis - lastLoop500Millis > 500 ) {
 740   lastLoop500Millis = currentLoopMillis ;
 741   flagMillis |= BIT500MILLIS ;
 742 }
 743 */
 744 #ifdef DEBUGENTERLOOP
 745   Serial.print(F("in loop="));
 746   Serial.println(millis());
 747 #endif
 748 #ifdef DEBUG_BLINK_MAINLOOP
 749     blinkLed(1) ;
 750 #endif
 751   //extern volatile uint8_t state ;
 752   //Serial.println(state) ;
 753     // Check if a button has been pressed
 754 #ifdef PIN_PUSHBUTTON
 755 #if defined (VARIO) || defined (VARIO2)
 756     if (checkFreeTime()) checkButton(); // Do not perform calculation if there is less than 2000 usec before MS5611 ADC is available =  (9000 - 2000)/2
 757 #else
 758     checkButton();
 759 #endif
 760 #endif
 761
 762     // read all sensors
 763     readSensors();
 764 #ifdef DEBUG_CALCULATE_FIELDS
 765   Serial.print("call calculateAllFields at") ; Serial.println(millis()) ;
 766 #endif
 767     // Calculate all fields
 768     calculateAllFields();
 769
 770 #ifdef DEBUG_CALCULATE_FIELDS
 771   Serial.print("end of call calculateAllFields at") ; Serial.println(millis()) ;
 772 #endif
 773
 774     // prepare the telemetry data to be sent (nb: data are prepared but not sent)
 775     if ( millis() > 1500 ) oXs_Out.sendData();
 776
 777 // PPM Processing = Read the ppm Signal from receiver  or use the SPORT ppm value from readSensors and process it
 778 #if defined ( PPM_IS_USED )
 779 #if defined (VARIO) || defined (VARIO2)
 780     if (checkFreeTime()) ProcessPPMSignal();
 781 #else
 782     ProcessPPMSignal();
 783 #endif
 784 #endif //PIN_PPM
 785
 786 // if analog vario, generate the PWR value
 787 #if defined (VARIO) && defined(PIN_ANALOG_VSPEED)
 788     if (millis() > lastMillisPWR + 100) {
 789         if (checkFreeTime()) { // Do not change PWM if there is less than 2000 usec before MS5611 ADC is available =  (9000 - 2000)/2
 790             PWRValue=map( (long) oXs_MS5611.varioData.climbRate.value ,ANALOG_VSPEED_MIN*100,ANALOG_VSPEED_MAX * 100,0,255/5*3.2);
 791             PWRValue=constrain(PWRValue, 0, 255/5*3.2 ) ;
 792             analogWrite(PIN_ANALOG_VSPEED,(int)PWRValue);
 793             lastMillisPWR = millis() ;
 794         }
 795     }
 796 #endif // analog vario
 797
 798 // if a sequence is set up
 799 #ifdef SEQUENCE_OUTPUTS
 800 #if defined( SEQUENCE_MIN_VOLT_6) || defined ( SEQUENCE_MIN_CELL )
 801   if ( (lowVoltage == true) && (prevLowVoltage == false) ) {
 802     prevLowVoltage = true ;
 803     ppm.value = 125 ; // fix the sequence to be used when voltage is low  ; it is sequence +125
 804     prevPpmMain = -10 ; // will force a new sequence because ppmMain will be different from prevPpmMain
 805     setNewSequence() ;
 806     #ifdef DEBUG
 807         Serial.println(F("Start sequence low voltage"));
 808     #endif
 809   }  else if ( (lowVoltage == false) && (prevLowVoltage == true) ) {
 810     prevLowVoltage = false ;
 811     prevPpmMain = -10 ; //will force a new sequence because ppmMain will be different from prevPpmMain
 812 #ifdef PIN_PPM
 813     if ( ppmus == 0) ppm.value = -100 ; // force ppm to -100 (default) when no ppm signal is received ,  else it will use the current ppm value
 814 #else
 815     ppm.value = -100 ; // if ppm is not configure force to go back to sequence -100 (= default);
 816 #endif  // end PIN_PPM def or not
 817     setNewSequence() ;
 818     #ifdef DEBUG
 819         Serial.println(F("End sequence low voltage"));
 820     #endif
 821   }
 822 #endif // end (defined( SEQUENCE_MIN_VOLT_6) || defined ( SEQUENCE_MIN_CELL )
 823   checkSequence() ; // check if outputs have to be modified because time expired
 824 #endif  // SEQUENCE_OUTPUTS
 825
 826 // Save Persistant Data To EEProm every 10 seconds
 827 #if defined (SAVE_TO_EEPROM ) and ( SAVE_TO_EEPROM == YES )
 828   static unsigned long LastEEPromMs=millis();
 829   if (millis() > LastEEPromMs+10000){
 830     LastEEPromMs=millis();
 831     SaveToEEProm();
 832   }
 833 #endif
 834
 835 #if defined( A_LOCATOR_IS_CONNECTED )  and ( A_LOCATOR_IS_CONNECTED == YES)
 836   loraHandle() ;
 837 #endif
 838 }          // ****************   end of main loop *************************************
 839
 840
 841
 842
 843
 844
 845 //**********************************************************************************************************
 846 //***                                            Read all the sensors / Inputs                          ****
 847 //**********************************************************************************************************
 848 extern uint16_t i2cPressureError ;
 849 extern uint16_t i2cTemperatureError ;
 850 extern uint16_t i2cReadCount ;
 851
 852 void readSensors() {
 853
 854 #ifdef AIRSPEED_4525
 855   oXs_4525.readSensor(); // Read a first time the differential pressure on 4525DO, calculate airspeed; note airspeed is read a second time in the loop in order to reduce response time
 856 #endif
 857
 858 #ifdef VARIO
 859 #ifdef DEBUG_ENTER_READSENSORS
 860   Serial.print( "read baro 1 at ") ; Serial.println(millis());
 861 #endif
 862
 863   newVarioAvailable = oXs_MS5611.readSensor(); // Read pressure & temperature on MS5611, calculate Altitude and vertical speed;
 864   if ( oXs_MS5611.varioData.absoluteAlt.available == true and oXs_MS5611.varioData.rawPressure > 100000.0f ) {
 865       actualPressure = oXs_MS5611.varioData.rawPressure / 10000.0 ; // this value can be used when calculating the Airspeed
 866     #ifdef FILL_TEST_1_WITH_VARIO_TEMP
 867       test1.value = oXs_MS5611.varioData.temperature/100.0 ;
 868       test1.available = true ;
 869     #endif
 870   }
 871 #endif
 872
 873 #ifdef VARIO2
 874 #ifdef DEBUG_ENTER_READSENSORS
 875   Serial.print( "read baro 2 at ") ; Serial.println(millis());
 876 #endif
 877
 878   newVarioAvailable2 = oXs_MS5611_2.readSensor(); // Read pressure & temperature on MS5611, calculate Altitude and vertical speed
 879 #endif
 880
 881 #ifdef AIRSPEED_4525
 882   oXs_4525.readSensor(); // Read again the sensor in order to reduce response time/noise
 883 #endif
 884
 885 #ifdef AIRSPEED_SDP3X
 886   oXs_sdp3x.readSensor(); //read the SDP3X sensor
 887 #endif
 888
 889 #ifdef USE_6050
 890     newImuAvailable = read6050() ;
 891 #endif // USE_6050
 892
 893 #if defined( USE_HMC5883 ) && defined (USE_6050)
 894     read_hmc5883() ;
 895 #endif
 896
 897 #if defined(ARDUINO_MEASURES_VOLTAGES) && (ARDUINO_MEASURES_VOLTAGES == YES)
 898     oXs_Voltage.readSensor();    // read voltage only if there enough time to avoid delaying vario reading; It takes about 750 usec to go through the read sensor.
 899 #endif   // end voltage
 900
 901 #if defined(ARDUINO_MEASURES_A_CURRENT) && (ARDUINO_MEASURES_A_CURRENT == YES)
 902     oXs_Current.readSensor() ; // Do not perform calculation if there is less than 2000 usec before MS5611 ADC is available =  (9000 - 2000)/2
 903 #endif             // End defined(ARDUINO_MEASURES_A_CURRENT) && (ARDUINO_MEASURES_A_CURRENT == YES)
 904
 905 #ifdef GPS_INSTALLED
 906 #ifdef DEBUG_ENTER_READSENSORS
 907   Serial.print( "read gps at ") ; Serial.println(millis());
 908 #endif
 909
 910     oXs_Gps.readGps() ; // Do not perform calculation if there is less than 2000 usec before MS5611 ADC is available =  (9000 - 2000)/2
 911 #endif             // End GPS_INSTALLED
 912
 913 #if defined(AN_ADS1115_IS_CONNECTED) && (AN_ADS1115_IS_CONNECTED == YES ) && defined(ADS_MEASURE)
 914     if( oXs_ads1115.readSensor() ) { // return true when a new average is available; it means that the new value has to be stored/processed.
 915     }
 916 #endif             // End ADS_MEASURE
 917
 918
 919 #ifdef MEASURE_RPM
 920   if (millis() > ( lastRpmMillis + 200) ){  // allow transmission of RPM only once every 200 msec
 921         if (RpmSet == true) {               // rpm is set
 922             RpmSet = false ;
 923         } else {
 924             RpmValue = 0 ;
 925         }
 926         sport_rpm.value = RpmValue ;
 927         sport_rpm.available = true ;
 928         lastRpmMillis = millis() ;
 929 #ifdef DEBUG_RPM
 930        Serial.print( "RPM ") ; Serial.println(sport_rpm.value);
 931 #endif
 932   }
 933 #endif
 934
 935 #if defined ( A_FLOW_SENSOR_IS_CONNECTED ) && ( A_FLOW_SENSOR_IS_CONNECTED == YES)
 936     processFlowMeterCnt () ;
 937 #endif
 938
 939
 940 #if ( defined ( PPM_VIA_SPORT ) || ( defined ( A_FLOW_SENSOR_IS_CONNECTED ) && ( A_FLOW_SENSOR_IS_CONNECTED == YES) ) ) && defined (PROTOCOL ) && ( ( PROTOCOL == FRSKY_SPORT ) || ( PROTOCOL == FRSKY_SPORT_HUB ) ) // if it is allowed to read SPORT
 941     uint8_t oReg = SREG ; // save status register
 942     cli() ;
 943     if ( TxDataIdx == 8 ) {
 944       TxDataIdx++ ; // Increase the number of data in order to avoid to read twice
 945       uint16_t txField = ( TxData[2] << 8 ) + TxData[1] ;                                                      // id of the field being received
 946       uint32_t txValue = ( ( ( ( (TxData[6] << 8) + TxData[5] ) << 8 ) + TxData[4] ) << 8 ) + TxData[3] ;      // value of the field being received
 947       SREG = oReg ;  // restore the status register
 948 #ifdef DEBUG_READ_SPORT
 949       Serial.print("At: ") ; Serial.print(millis()) ;
 950       Serial.print(" "); Serial.print(TxData[0], HEX);
 951       Serial.print(" "); Serial.print(TxData[1], HEX);
 952       Serial.print(" "); Serial.print(TxData[2], HEX);
 953       Serial.print(" "); Serial.print(TxData[3], HEX);
 954       Serial.print(" "); Serial.print(TxData[4], HEX);
 955       Serial.print(" "); Serial.print(TxData[5], HEX);
 956       Serial.print(" "); Serial.print(TxData[6], HEX);
 957       Serial.print(" "); Serial.print(TxData[7], HEX);
 958       Serial.println(" ");
 959 #endif
 960 #if  ( defined ( A_FLOW_SENSOR_IS_CONNECTED ) && ( A_FLOW_SENSOR_IS_CONNECTED == YES) )
 961       if (  txField <= TX_FIELD_FLOW_CORR_4)  {  // save values for flow meter parameter in memory
 962         flowParam[txField] = txValue ;
 963       } else if (  txField ==  TX_FIELD_TANK_CAPACITY) {  // save values for capacity in memory
 964         tankCapacity = txValue ;                          // note: values will be saved in eeprom in EEPROM function
 965       } else if (  txField ==  TX_FIELD_RESET_FUEL ) {
 966         consumedML = 0 ;                                  // reset consumption
 967       }
 968 #endif
 969 #if  defined ( PPM_VIA_SPORT ) &&   ( ( PROTOCOL  == FRSKY_SPORT ) || ( PROTOCOL == FRSKY_SPORT_HUB ) )
 970       if ( txField == TX_FIELD_PPM )  {                   // save the value for ppm
 971           newPpm.value =  ((int32_t) txValue) * 100 / 1024 ;
 972           newPpm.available = true ;
 973       }
 974 #endif
 975     } else {
 976       SREG = oReg ;  // restore the status register
 977     }
 978 #endif
 979
 980 }                  // ************** end of readSensors ********************************************
 981
 982
 983 //#define ALTITUDE_AVAILABLE_BIT 1
 984 //#define VSPEED_AVAILABLE_BIT 2
 985 // ********************************* Calculate all fields that could be sent
 986 void calculateAllFields () {
 987
 988 // compensated Vpeed based on MS4525
 989 #if defined(VARIO) &&  defined ( AIRSPEED_IS_USED)
 990     if ( newVarioAvailable ) calculateDte() ;
 991 #endif
 992
 993 // average Vpeed
 994 #if defined (VARIO) &&  defined (VARIO2)
 995   if( (newVarioAvailable ) || ( newVarioAvailable2 ) ) {
 996     averageVSpeedFloat = ( oXs_MS5611.varioData.climbRateFloat + oXs_MS5611_2.varioData.climbRateFloat ) / 2 ;
 997     if ( abs((int32_t)  averageVSpeedFloat - averageVSpeed.value) > VARIOHYSTERESIS ) {
 998           averageVSpeed.value = (int32_t)  averageVSpeedFloat ;    // update only when difference is greater than hysteresis
 999     }
1000     averageVSpeed.available = true ;
1001   }
1002 #endif
1003
1004 // calculate vSpeedImu & vSpeedImuAvailable
1005 #if defined (VARIO) && defined (USE_6050)
1006         if (newImuAvailable) { // newImuAvailable says that a new world_linear_acceleration is available (flag has been returned by read6050()
1007           newImuAvailable = false ;   // reset the flag saying a new world_linear_acceleration is available
1008   #ifdef DEBUG_KALMAN
1009             unsigned long beginKalman = micros();
1010   #endif
1011             if ( countAltitudeToKalman != 0) {                                   // this calculate the initial altitude
1012                 if( oXs_MS5611.varioData.rawAltitude != 0) {
1013                   countAltitudeToKalman-- ;
1014                   altitudeOffsetToKalman = oXs_MS5611.varioData.rawAltitude ;
1015                 }
1016             }
1017             altitudeToKalman = (oXs_MS5611.varioData.rawAltitude - altitudeOffsetToKalman ) / 100 ; // convert from * 100cm to cm
1018             kalman.Update((float) altitudeToKalman  , world_linear_acceleration_z ,  &zTrack, &vTrack);  // Altitude and acceleration are in cm
1019             vSpeedImu.value = vTrack ;
1020             vSpeedImu.available = true ;
1021             switchVTrackAvailable = true ;
1022
1023   //#define FILL_TEST_3_WITH_EXPECTED_ALT    // force a calculation of an expected altitude x seconds in the future
1024   #ifdef FILL_TEST_3_WITH_EXPECTED_ALT
1025             #define EXPECTED_AT_SEC 0.2        // number of sec used for the projection; can have decimals; NB: expected alt = Alt at time 0 + Vspeed at time 0 * seconds  + 0.5 * Accz at time 0 * seconds * seconds
1026             test3.value = ( zTrack - oXs_MS5611.varioData.altOffset ) + ( vTrack *  EXPECTED_AT_SEC ) + ( 0.5 *  world_linear_acceleration_z * EXPECTED_AT_SEC * EXPECTED_AT_SEC ) ;
1027             test3.available = true ;
1028   #endif
1029
1030   //#define FILL_TEST_1_2_3_WITH_LINEAR_ACC
1031   #ifdef FILL_TEST_1_2_3_WITH_LINEAR_ACC
1032             test1.value = linear_acceleration_x * 981 ;
1033             test1.available = true ;
1034             test2.value =  linear_acceleration_y * 981;
1035             test2.available = true ;
1036             test3.value = linear_acceleration_z * 981;
1037             test3.available = true ;
1038   #endif
1039
1040   #ifdef DEBUG_KALMAN_TIME
1041 ///           Serial.print(F("delay Kal ")) ; Serial.print( millis() - beginKalman ) ;Serial.print(F(", "));
1042 //            Serial.print( (int) world_linear_acceleration_z ) ; Serial.print(F(", "));Serial.print( (int) altitudeToKalman) ; Serial.print(F(", ")); Serial.print(oXs_MS5611.varioData.climbRate) ; Serial.print(F(", "));Serial.println(( int )vSpeedImu.value) ;
1043 ///           Serial.print( millis() ) ; Serial.print(F(", ")); Serial.print( (int)  ) ; Serial.print(F(", "));Serial.print( (int) world_linear_acceleration_z ) ; Serial.print(F(", "));Serial.print( (int) altitudeToKalman) ; Serial.print(F(", ")); Serial.print(oXs_MS5611.varioData.climbRate) ; Serial.print(F(", "));Serial.println(( int )vSpeedImu.value) ;
1044               Serial.print("delayK ") ; Serial.print( delayKalman[0] ) ; Serial.print(" ") ; Serial.print( delayKalman[1] ) ;
1045               Serial.print(" ") ; Serial.print( delayKalman[2] ) ; Serial.print(" ") ; Serial.print( delayKalman[3] ) ;Serial.print(" ") ; Serial.println( delayKalman[4] ) ;
1046 #endif
1047         } // end of newimuAvailable
1048 #endif
1049
1050
1051 // calculate selected Vspeed based on ppm
1052 #if defined (VARIO) && ( defined (VARIO2) || defined (AIRSPEED_IS_USED) || defined (USE_6050 ) ) && defined (VARIO_SECONDARY ) && defined( VARIO_PRIMARY )  && defined (PPM_IS_USED)
1053   if (( selectedVario == FIRST_BARO ) && ( newVarioAvailable ) )  {
1054       switchVSpeed.value = oXs_MS5611.varioData.climbRate.value ;
1055       switchVSpeed.available = true ;
1056   }
1057   #if  defined (VARIO2)
1058   else if ( ( selectedVario == SECOND_BARO ) && ( newVarioAvailable2 )) {
1059       switchVSpeed.value = oXs_MS5611_2.varioData.climbRate.value ;
1060       switchVSpeed.available = true ;
1061   }
1062   else if ( ( selectedVario == AVERAGE_FIRST_SECOND ) && ( averageVSpeed.available == true )) {
1063       switchVSpeed.value = averageVSpeed.value ;
1064       switchVSpeed.available = true ;
1065   }
1066   #endif // end of VARIO2
1067   #ifdef AIRSPEED_IS_USED
1068   else if ( ( selectedVario == AIRSPEED_COMPENSATED ) && ( newVarioAvailable )) {
1069     switchVSpeed.value = compensatedClimbRate.value ;
1070     switchVSpeed.available = true ;
1071   #if defined (SWITCH_VARIO_GET_PRIO)
1072     switchCompensatedClimbRateAvailable = true ; // avoid to reset the value on false in order to continue to send the same value as often as possible
1073   #else
1074     switchCompensatedClimbRateAvailable = false ; // this is the normal process in order to avoid sending twice the same data.
1075   #endif  // end  defined (SWITCH_VARIO_GET_PRIO)
1076   }
1077   #endif // end  defined (AIRSPEED) || (defined(AN_ADS1115_IS_CONNECTED) && (AN_ADS1115_IS_CONNECTED == YES ) && defined (ADS_MEASURE) && defined (ADS_AIRSPEED_BASED_ON) )
1078   #if  defined (USE_6050)
1079   else if ( ( selectedVario == BARO_AND_IMU ) && ( switchVTrackAvailable )) {
1080       switchVSpeed.value = vSpeedImu.value ;
1081       switchVSpeed.available = true ;
1082   }
1083   #endif  // end USE_6050
1084 #endif // end #if defined (VARIO) && ( defined (VARIO2) || defined (AIRSPEED_IS_USED) || defined (USE_6050 ) ) && defined (VARIO_SECONDARY ) && defined( VARIO_PRIMARY )  && defined (PPM_IS_USED)
1085
1086 // mainVSpeed (calculated based on the setup in config)
1087 #if defined(VARIO) &&  ( (!defined(VSPEED_SOURCE)) || (defined(VSPEED_SOURCE) && (VSPEED_SOURCE == FIRST_BARO) ) )
1088     mainVspeed.value = oXs_MS5611.varioData.climbRate.value ;
1089     mainVspeed.available = oXs_MS5611.varioData.climbRate.available ;
1090 #elif defined(VARIO) && defined(VARIO2) && (VSPEED_SOURCE == SECOND_BARO)
1091     mainVspeed.value = oXs_MS5611_2.varioData.climbRate.value ;
1092     mainVspeed.available = oXs_MS5611_2.varioData.climbRate.available ;
1093 #elif defined(VARIO) && defined(VARIO2) && (VSPEED_SOURCE == AVERAGE_FIRST_SECOND)
1094     mainVspeed.value = averageVSpeed.value ;
1095         mainVspeed.available = averageVSpeed.available ;
1096 #elif defined(VARIO) && defined(AIRSPEED_IS_USED) && (VSPEED_SOURCE == AIRSPEED_COMPENSATED)
1097     mainVspeed.value = compensatedClimbRate.value ;
1098     mainVspeed.available = compensatedClimbRate.available ;
1099 #elif defined(VARIO) && defined(USE_6050) && (VSPEED_SOURCE == BARO_AND_IMU)
1100     mainVspeed.value = vSpeedImu.value ;
1101     mainVspeed.available = vSpeedImu.available ;
1102 #elif defined(VARIO) && ( defined (VARIO2) || defined (AIRSPEED_IS_USED) || defined (USE_6050) ) && defined (VARIO_SECONDARY ) && defined( VARIO_PRIMARY )  && defined (PPM_IS_USED)  && (VSPEED_SOURCE == PPM_SELECTION)
1103     mainVspeed.value = switchVSpeed.value   ;
1104     mainVspeed.available = switchVSpeed.available   ;
1105 #if defined (DEBUG_SELECTED_VARIO)
1106 static uint32_t next_debug_vspeeds_millis ;
1107 static boolean next_debug_vspeeds_flag = true ;
1108   if (millis() > next_debug_vspeeds_millis) {
1109     if (next_debug_vspeeds_flag) {
1110       Serial.println("vspeeds= first, compensated, switched , main");
1111       next_debug_vspeeds_flag = false ;
1112     } else {
1113       Serial.print("vspeeds= ");Serial.print(oXs_MS5611.varioData.climbRate.value); Serial.print(",");Serial.print(compensatedClimbRate.value);
1114       Serial.print(",");Serial.print(switchVSpeed.value); Serial.print(",");Serial.print(mainVspeed.value); Serial.println(" ");
1115       next_debug_vspeeds_millis = millis() + 500 ;
1116     }
1117   }
1118 #endif //  end DEBUG_SELECTED_VARIO
1119
1120 #endif // end mainVSpeed (calculated based on the setup in config)
1121
1122
1123 // calculate glider ratio and average vertical speed (based on first vario)
1124 #if defined  (VARIO) && defined (GLIDER_RATIO_CALCULATED_AFTER_X_SEC) && GLIDER_RATIO_CALCULATED_AFTER_X_SEC >= 1
1125         calculateAverages();
1126 #endif
1127
1128 // fill test1 and test2 with Vspeed and relative Alt
1129 //#define FILL_TEST_1_2_WITH_VSPEED_AND_ALT_FROM_SECOND_VARIO
1130 #if defined (VARIO) && ( defined (VARIO2) ) && defined(FILL_TEST_1_2_WITH_VSPEED_AND_ALT_FROM_SECOND_VARIO)
1131     test1.value = oXs_MS5611_2.varioData.climbRate.value ;
1132     test1.available = oXs_MS5611_2.varioData.climbRate.available ;
1133     test2.value = oXs_MS5611_2.varioData.relativeAlt.value ;
1134     test2.available = oXs_MS5611_2.varioData.relativeAlt.available ;
1135 #endif
1136
1137 //  fill test1 and test2 with DTE and PPM_COMPENSATION
1138 //#define FILL_TEST_1_WITH_DTE
1139 //#define FILL_TEST_2_WITH_PPM_AIRSPEED_COMPENSATION
1140 #if defined(VARIO) && defined(AIRSPEED_IS_USED)
1141 #ifdef FILL_TEST_1_WITH_DTE
1142   test1.value = compensatedClimbRate.value ;
1143   test1.available = compensatedClimbRate.available ;
1144 #endif
1145 #if  defined(FILL_TEST_2_WITH_PPM_AIRSPEED_COMPENSATION) && defined(PPM_IS_USED)
1146   static uint32_t lastPpmCompensationMillis ;
1147   uint32_t PpmCompensationMillis = millis() ;
1148   if ( ( PpmCompensationMillis - lastPpmCompensationMillis ) > 200 ) {
1149     test2.value =  compensationPpmMapped ;
1150     test2.available = true ;
1151     lastPpmCompensationMillis = PpmCompensationMillis ;
1152   }
1153 #endif
1154 #endif
1155
1156 #if defined (VARIO) &&  defined (USE_6050)
1157 //#define FILL_TEST_1_WITH_YAWRATE
1158 #ifdef FILL_TEST_1_WITH_YAWRATE
1159 static int32_t  previousYaw ;
1160 static uint32_t previousYawRateMillis ;
1161  uint32_t currentMillis ;
1162         currentMillis = millis() ;
1163       if (currentMillis >= previousYawRateMillis + 200 ) {
1164         test1.value = (yaw.value - previousYaw) * 1000 / (int32_t) (currentMillis - previousYawRateMillis )  ; // calculate yaw rate in degree / sec
1165         test1.available = true ;
1166 //#define DEBUGYAWRATE
1167 #ifdef DEBUGYAWRATE
1168         //Serial.print(lastImuInterruptMillis) ;  Serial.print(" ") ; Serial.print( previousYawRateMillis ) ;Serial.print(" ") ; Serial.print( yaw.value ) ; Serial.print(" ") ; Serial.print( test1.value ) ; Serial.print(" ") ; Serial.println(oXs_MS5611.varioData.absoluteAlt.value) ;
1169          Serial.print( currentMillis ) ;Serial.print(" ") ; Serial.print( yaw.value ) ; Serial.print(" ") ; Serial.print( test1.value ) ; Serial.print(" ") ; Serial.println(oXs_MS5611.varioData.absoluteAlt.value) ;
1170 #endif
1171
1172         previousYaw = yaw.value ;
1173         previousYawRateMillis = currentMillis ;
1174
1175       }
1176 //    test1.value = oXs_MS5611.varioData.climbRate.value ;
1177 //    test1.available = oXs_MS5611.varioData.climbRate.available ;
1178 //    test2.value = vSpeedImu.value ;
1179 //    test2.available = vSpeedImu.available ;
1180 #endif
1181 #endif
1182
1183 //#define DEBUG_CHANGE_IN_TEST12
1184 #if defined( DEBUG_CHANGE_IN_TEST12 )
1185       static uint32_t millisLastChangeTest ;
1186       if (millis() > millisLastChangeTest ) {
1187         millisLastChangeTest += 100 ;
1188         test1.value += 1 ;
1189         test1.available = true ;
1190         test2.value += 1 ;
1191         test2.available = true ;
1192       }
1193 #endif
1194
1195 //#define FILL_TEST1_WITH_HEADING_FROM_MAGNETOMETER
1196 #if defined ( USE_HMC5883 )  && defined (USE_6050) && defined (FILL_TEST1_WITH_HEADING_FROM_MAGNETOMETER)
1197       if ( newMagHeading ) {
1198         newMagHeading = false ;
1199         test1.value = magHeading ;
1200         test1.available = true ;
1201       }
1202 #endif
1203
1204
1205
1206 #if defined ( A_FLOW_SENSOR_IS_CONNECTED ) && ( A_FLOW_SENSOR_IS_CONNECTED == YES)&& defined (FILL_TEST_1_2_3_WITH_FLOW_SENSOR_CONSUMPTION)
1207   if( newFlowAvailable ) {
1208     test1.value = actualFlow.value ;
1209     test2.value = remainingFuelML.value ;
1210     test3.value = fuelPercent.value ;
1211     test1.available = test2.available = test3.available = true ;
1212     newFlowAvailable = false ;
1213   }
1214 #endif
1215
1216
1217 #if defined ( A_GPS_IS_CONNECTED ) && ( A_GPS_IS_CONNECTED == YES )
1218   #if defined ( FILL_TEST1_WITH_GPS_NUMBER_OF_SAT )
1219   test1.value = GPS_numSat ;
1220   if ( GPS_fix ) {
1221     test1.value += 100 ;
1222   }
1223   test1.available = true ;
1224   #endif
1225   #if defined ( FILL_TEST2_WITH_GPS_HDOP )
1226   test2.value = GPS_hdop ;
1227   test2.available = true ;
1228   #endif
1229 #endif
1230
1231 //  test1.value = oXs_MS5611.varioData.absoluteAlt.value/10 ;
1232 //  test1.available = true ;
1233
1234 //#if defined(ARDUINO_MEASURES_A_CURRENT) && (ARDUINO_MEASURES_A_CURRENT == YES)
1235 //if ( currentData.consumedMilliAmps.available) {
1236 // test1.value = currentData.consumedMilliAmps.value ;
1237 // test1.available =  true ;
1238 // currentData.consumedMilliAmps.available = false ;
1239 //}
1240 //#endif
1241
1242 //#if defined(AN_ADS1115_IS_CONNECTED) && (AN_ADS1115_IS_CONNECTED == YES ) && defined(ADS_MEASURE) && defined(ADS_CURRENT_BASED_ON)
1243 //if ( oXs_ads1115.adsCurrentData.MilliAmps.available) {
1244 // test2.value = oXs_ads1115.adsCurrentData.MilliAmps.value ;
1245 // test2.available =  true ;
1246 // oXs_ads1115.adsCurrentData.MilliAmps.available = false ;
1247 //}
1248 //if ( oXs_ads1115.adsCurrentData.consumedMilliAmps.available) {
1249 // test3.value = oXs_ads1115.adsCurrentData.consumedMilliAmps.value ;
1250 // test3.available =  true ;
1251 // oXs_ads1115.adsCurrentData.consumedMilliAmps.available = false ;
1252 //}
1253 //#endif
1254
1255 //#if ( defined(AIRSPEED) && (defined(AN_ADS1115_IS_CONNECTED) && (AN_ADS1115_IS_CONNECTED == YES ) && defined (ADS_MEASURE) && defined (ADS_AIRSPEED_BASED_ON) )
1256 //      if ( oXs_ads1115.adsAirSpeedData.airSpeed.available == true ) {
1257 //     test1.value = oXs_ads1115.adsAirSpeedData.airSpeed.value ;
1258 //       test1.available = true ;
1259 //       oXs_ads1115.adsAirSpeedData.airSpeed.available = false ;
1260 //      }
1261 //#endif
1262
1263 //  if ( oXs_MS5611.varioData.climbRate.available ) {
1264 //    test1.value = oXs_MS5611.varioData.temperature ;
1265 //    test1.available = true ;
1266 //  }
1267 } // end of calculate all fields
1268
1269
1270
1271 //***************** checkFreeTime ********* if there is at least 2000 usec before the next MS5611 read (in order to avoid delaying it
1272 bool checkFreeTime() { // return true if there is no vario or if the vario sensor must not be read within a short time.
1273                        // return false if a vario must be read within a short time
1274 #if defined (VARIO) || defined (VARIO2)
1275 //        extendedMicros = micros() ;
1276   #ifdef VARIO
1277         return ( micros() - oXs_MS5611.varioData.lastCommandMicros < 7000 ) ;   // Do not change PWM if there is less than 2000 usec before MS5611 ADC is available =  (9000 - 2000)
1278   #else // only VARIO2
1279         return ( micros() - oXs_MS5611_2.varioData.lastCommandMicros < 7000 ) ;   // Do not change PWM if there is less than 2000 usec before MS5611 ADC is available =  (9000 - 2000)
1280   #endif // enf of at least one vario
1281 #else // No vario at all
1282   return true ;
1283 #endif
1284 }  // ******************************* end of checkFreeTime *****************************
1285
1286 // ********************************** Calculate dTE based on rawAltitude and differential pressure
1287 #if defined (VARIO) && defined ( AIRSPEED_IS_USED)
1288 #define SMOOTHING_DTE_MIN SENSITIVITY_MIN
1289 #define SMOOTHING_DTE_MAX SENSITIVITY_MAX
1290 #define SMOOTHING_DTE_MIN_AT SENSITIVITY_MIN_AT
1291 #define SMOOTHING_DTE_MAX_AT SENSITIVITY_MAX_AT
1292
1293 void calculateDte () {  // is calculated about every 2O ms each time that an altitude is available
1294   static float rawTotalEnergy ;
1295   static float abs_deltaCompensatedClimbRate ;
1296   static float smoothingDteMin =  SMOOTHING_DTE_MIN ;
1297   static float expoSmoothDte_auto ;
1298   static float smoothCompensatedClimbRate ;
1299   static float rawCompensation ;
1300   static float totalEnergyLowPass ;
1301   static float totalEnergyHighPass ;
1302
1303           // for 4525:
1304                //  difPressure (in PSI) = difPressureAdc * 0.0001525972 because 1 PSI = (8192 - 1638) = 6554 steps
1305                //  difPressure (Pa) =  difPressure (in PSI) * 6894.757f  = difPressureAdc * 6894.757 *  0.0001525972 = difPressureAdc * 1.0520
1306                // airspeed = sqr(2 * differential_pressure / air_density) ; air density = pressure  pa / (287.05 * (Temp celcius + 273.15))
1307                // so airspeed m/sec =sqr( 2 * 287.05 * differential_pressure pa * (temperature Celsius + 273.15) / pressure pa )
1308                // total energy = (m * g * altitude) + (m * airspeed * airspeed / 2) => m * 9.81 * (altitude + airspeed * airspeed / 2 /9.81)
1309                // compensation (m/sec) = airspeed * airspeed / 2 / 9.81 =
1310                //                      = 2 * 287.05 * difPressureAdc * 1.0520  * (temperature Celsius + 273.15) / pressure pa /2 /9.81 (m/sec) = 30.78252803 * difPressureAdc * Temp(kelv) / Press (Pa)
1311                // compensation (cm/sec) =  3078.252803 * difPressureAdc * Temp(kelv) / Press (Pa)
1312           // for 7002
1313               // difPressure (Pa) = difPressureAdc * 0.061035156 because 2 kpa = 32768 steps ; so 1 step = 2000 / 32768 = 0.061035156
1314               // compensation (m/sec) = airspeed * airspeed / 2 / 9.81 =
1315               //                      = 2 * 287.05 * difPressureAdc * 0.061035156  * (temperature Celsius + 273.15) / pressure pa /2 /9.81 (m/sec) = 1.785947149 * difPressureAdc * Temp(kelv) / Press (Pa)
1316               // compensation (cm/sec) = 178.5947149 * difPressureAdc * Temp(kelv) / Press (Pa)
1317           // for SDP3X
1318               // difPressure is already in Pa
1319               // airspeed (m/sec) = sqr( 2 * 287.05 * differential_pressure pa * (temperature Celsius + 273.15) / pressure pa )
1320               // compensation (m/sec) = airspeed * airspeed / 2 / 9.81 =
1321               //                      = 2 * 287.05 * difPressureAdc * (temperature Celsius + 273.15) / pressure pa /2 /9.81 (m/sec) =
1322               //                      = 29,26095821* difPressureAdc * Temp(kelv) / Press (Pa)
1323               // compensation (cm/sec) = 2926,09 * difPressureAdc * Temp(kelv) / Press (Pa)
1324
1325 //#define DEBUG_DTE
1326 #if defined( DEBUG_DTE ) && defined(AIRSPEED_7002) // when 7002 is used
1327     static bool firstDteData = true ;
1328     if ( firstDteData ) {
1329             Serial.println(F("at , difPressADC_0 , cnt , rawAlt , rawComp , rawEnerg ")) ;
1330           firstDteData = false ;
1331     }
1332     Serial.print( millis() ); Serial.print(F(" , "));
1333     Serial.print( ads_sumDifPressureAdc_0 ); Serial.print(F(" , "));
1334     Serial.print( ads_cntDifPressureAdc_0); Serial.print(F(" , "));
1335 #endif
1336
1337 #if defined ( AIRSPEED_4525) // when 4525 is used
1338   rawCompensation = 3078.25 * oXs_4525.airSpeedData.difPressureAdc_zero * oXs_4525.airSpeedData.temperature4525  /  actualPressure    ; // 3078.25 = comp = 2 * 287.05 / 2 / 9.81 * 1.0520 * 100 * Temp / Pressure
1339 #elif defined ( AIRSPEED_7002) // when 7002 is used
1340   if ( ads_cntDifPressureAdc_0 > 0 ) {
1341       ads_sumDifPressureAdc_0 = ads_sumDifPressureAdc_0 / ads_cntDifPressureAdc_0 ; // we use the average of airspeed pressure when possible and we keep the average as first value for next loop
1342       ads_cntDifPressureAdc_0 = 1 ;  // so cnt is reset to 1 and not to 0
1343       rawCompensation = 178.5947149 * ads_sumDifPressureAdc_0 * ( 293 )   /  actualPressure    ; //  293 could be replaced by the temperature from mS5611
1344   }
1345 #elif defined( AIRSPEED_SDP3X) // when SDP3X is used
1346   rawCompensation = 2926.09 * oXs_sdp3x.airSpeedData.difPressureAdc_zero * oXs_sdp3x.airSpeedData.temperature4525  /  actualPressure ;
1347 #endif
1348   rawTotalEnergy = (oXs_MS5611.varioData.rawAltitude * 0.01) + rawCompensation * compensationPpmMapped * 0.0115; // 0.01 means 100% compensation but we add 15% because it seems that it is 15% undercompensated.
1349   if (totalEnergyLowPass == 0) {
1350     totalEnergyLowPass = totalEnergyHighPass = rawTotalEnergy ;
1351   }
1352 #if defined  ( DEBUG_DTE ) && defined(AIRSPEED_7002)
1353     Serial.print( oXs_MS5611.varioData.rawAltitude * 0.01 ); Serial.print(F(" , "));
1354     Serial.print( rawCompensation ); Serial.print(F(" , "));
1355     Serial.print( rawTotalEnergy ); Serial.print(F(" , "));
1356     Serial.println(" ") ;
1357 #endif
1358
1359 //  test1.value = rawCompensation;
1360 //  test1.available = true ;
1361
1362   //test2.value = rawTotalEnergy;
1363   //test2.available = true ;
1364
1365   totalEnergyLowPass += 0.085 * ( rawTotalEnergy - totalEnergyLowPass) ;
1366   totalEnergyHighPass += 0.1 * ( rawTotalEnergy - totalEnergyHighPass) ;
1367   rawCompensatedClimbRate = ((totalEnergyHighPass - totalEnergyLowPass )  * 566667.0 ) /     oXs_MS5611.varioData.delaySmooth; // 0.566667 is the parameter to be used for 0.085 and 0.1 filtering if delay is in sec
1368
1369 //  test3.value = rawCompensatedClimbRate ;
1370 //  test3.available = true ;
1371   abs_deltaCompensatedClimbRate =  abs(rawCompensatedClimbRate - smoothCompensatedClimbRate) ;
1372   if ( sensitivityPpmMapped  > 0) smoothingDteMin =   sensitivityPpmMapped  ; // a value of sensitivityPpmMapped = 50 becomes a smoothing factor 0.1
1373     if ( (abs_deltaCompensatedClimbRate <= SMOOTHING_DTE_MIN_AT) || (smoothingDteMin >= SMOOTHING_DTE_MAX ) ){
1374      expoSmoothDte_auto = smoothingDteMin ;
1375   } else if (abs_deltaCompensatedClimbRate >= SMOOTHING_DTE_MAX_AT)  {
1376      expoSmoothDte_auto = SMOOTHING_DTE_MAX ;
1377   } else {
1378      expoSmoothDte_auto = smoothingDteMin + ( SMOOTHING_DTE_MAX - smoothingDteMin ) * (abs_deltaCompensatedClimbRate - SMOOTHING_DTE_MIN_AT) / (SMOOTHING_DTE_MAX_AT - SMOOTHING_DTE_MIN_AT) ;
1379   }
1380   smoothCompensatedClimbRate += expoSmoothDte_auto * (  rawCompensatedClimbRate -  smoothCompensatedClimbRate ) * 0.001;
1381   if ( abs( ((int32_t)  smoothCompensatedClimbRate) - compensatedClimbRate.value) > VARIOHYSTERESIS ) {
1382       compensatedClimbRate.value = smoothCompensatedClimbRate  ;
1383   }
1384   compensatedClimbRate.available = true ; // allows SPORT protocol to transmit the value every 20 ms
1385   switchCompensatedClimbRateAvailable = true ; ; // inform readsensors() that a switchable vspeed is available
1386 //  oXs_MS5611.varioData.altitudeAt20MsecAvailable = false ; // avoid to handel it each time.
1387
1388 #ifdef DEBUGCOMPENSATEDCLIMBRATE
1389   static bool firstCompensatedClimbRate = true ;
1390   if ( firstCompensatedClimbRate ) {
1391         Serial.println(F("at, altitude, difPressureADC_zero, rawCompensation , rawTotalEnergy , delaySmooth , rawCompensatedClimbRate , compensatedClimbRate ")) ;
1392         firstCompensatedClimbRate = false ;
1393   } else {
1394   Serial.print( micros() ) ;Serial.print(F(" , ")) ;
1395   Serial.print( oXs_MS5611.varioData.rawAltitude ) ;Serial.print(F(" , ")) ;
1396   Serial.print( oXs_4525.airSpeedData.difPressureAdc_zero ) ;Serial.print(F(" , ")) ;
1397   Serial.print( rawCompensation ) ;Serial.print(F(" , ")) ;
1398   Serial.print( rawTotalEnergy ) ;Serial.print(F(" , ")) ;
1399   Serial.print( oXs_MS5611.varioData.delaySmooth ) ;Serial.print(F(" , ")) ;
1400   Serial.print( rawCompensatedClimbRate ) ;Serial.print(F(" , ")) ;
1401   Serial.print( compensatedClimbRate.value) ;
1402   Serial.println(F(" ")) ;
1403   }
1404 #endif
1405
1406 } // end calculateDte
1407 #endif //#if defined (VARIO) &&  defined ( AIRSPEED_IS_USED)
1408 // ***************************** end calculate Dte ***********************************************
1409
1410
1411 //****************************** Calculate averages and glider ratio ********************************************
1412 #if defined (VARIO) && defined (GLIDER_RATIO_CALCULATED_AFTER_X_SEC ) && (GLIDER_RATIO_CALCULATED_AFTER_X_SEC >= 1 )  && defined ( PPM_IS_USED ) && defined (GLIDER_RATIO_ON_AT_PPM )
1413 void calculateAverages( ){
1414         static int32_t altitudeAtT0 ; // in cm
1415         static int32_t distanceSinceT0 ; // in cm
1416 //        static int32_t averageVspeedSinceT0 ; //in cm/sec
1417         static int16_t aSpeedAtT0 ;
1418 //        static uint16_t secFromT0 ;  // in 1/10 sec
1419         static uint32_t millisAtT0 ;
1420         static unsigned long prevAverageAltMillis  ; // wait 5 sec before calculating those data ; // save when AverageAltitude has to be calculated
1421         int32_t altitudeDifference ;
1422         unsigned long currentGliderMillis ;
1423         bool aSpeedWithinTolerance = true ;
1424         static boolean previousGliderRatioPpmOn = false ;
1425         currentGliderMillis = millis() ;
1426         if (  prevAverageAltMillis == 0 ) {
1427           prevAverageAltMillis =  currentGliderMillis + 5000  ;
1428           millisAtT0 = currentGliderMillis ;
1429         }
1430         if ( (uint16_t) (currentGliderMillis - prevAverageAltMillis) >   500 ) { // check on tolerance has to be done
1431              if ( ( gliderRatioPpmOn) && (previousGliderRatioPpmOn) ) {
1432               altitudeDifference = oXs_MS5611.varioData.absoluteAlt.value -altitudeAtT0  ; // in cm
1433               secFromT0.value =  ( currentGliderMillis - millisAtT0 ) / 100 ;            // in 1/10 of sec
1434               averageVspeedSinceT0.value = altitudeDifference * 10 / secFromT0.value  ;      // * 10 because secFromT0 is in 1/10 of sec
1435 #ifdef AIRSPEED_4525
1436                 distanceSinceT0 += oXs_4525.airSpeedData.smoothAirSpeed / (1000 /  500) ;  // to adapt if delay is different.
1437 #endif
1438                 if (  secFromT0.value >=  GLIDER_RATIO_CALCULATED_AFTER_X_SEC * 10 ) {         // *10 because secFromT0 is in 1/10 of sec
1439 #ifdef AIRSPEED_4525
1440                     gliderRatio.value = distanceSinceT0  * -10 / altitudeDifference  ;        // when gliderRatio is > (50.0 *10) it it not realistic (*10 is done in order to add a decimal)
1441                     if ( gliderRatio.value > 500) gliderRatio.value = 0 ;                                                   //
1442 #endif
1443                     averageVspeedSinceT0.value = altitudeDifference * 10 / secFromT0.value  ;      // * 10 because secFromT0 is in 1/10 of sec
1444                 }
1445
1446             } else {
1447               altitudeAtT0 = oXs_MS5611.varioData.absoluteAlt.value   ;
1448               millisAtT0 = currentGliderMillis ;
1449               secFromT0.value = 0 ;
1450               averageVspeedSinceT0.value = 0 ;
1451 #ifdef AIRSPEED_4525
1452                 distanceSinceT0 = 0 ;
1453                 gliderRatio.value = 0 ;
1454 #endif
1455             }
1456
1457             prevAverageAltMillis = currentGliderMillis  ;
1458             gliderRatio.available = true;
1459             secFromT0.available = true ;
1460             averageVspeedSinceT0.available = true ;
1461             previousGliderRatioPpmOn = gliderRatioPpmOn ;
1462         }  // end check on 500 ms
1463 }        // end Calculate averages
1464
1465
1466
1467
1468
1469 #else
1470 #if defined  (VARIO) && defined (GLIDER_RATIO_CALCULATED_AFTER_X_SEC) && GLIDER_RATIO_CALCULATED_AFTER_X_SEC >= 1
1471 #if defined (GLIDER_RATIO_CALCULATED_AFTER_X_SEC) && GLIDER_RATIO_CALCULATED_AFTER_X_SEC < 1
1472 #error  when defined, GLIDER_RATIO_CALCULATED_AFTER_X_SEC must be greater or equal to 1 (sec)
1473 #endif
1474 void calculateAverages( ){
1475         static int32_t altitudeAtT0 ; // in cm
1476         static int32_t distanceSinceT0 ; // in cm
1477 //        static int32_t averageVspeedSinceT0 ; //in cm/sec
1478         static int16_t aSpeedAtT0 ;
1479 //        static uint16_t secFromT0 ;  // in 1/10 sec
1480         static uint32_t millisAtT0 ;
1481         static unsigned long prevAverageAltMillis =  millis() + 5000 ; // wait 5 sec before calculating those data ; // save when AverageAltitude has to be calculated
1482         int32_t altitudeDifference ;
1483         unsigned long currentGliderMillis = millis() ;
1484         bool aSpeedWithinTolerance = true ;
1485
1486         if ( (uint16_t) (currentGliderMillis - prevAverageAltMillis) >   500 ) { // check on tolerance has to be done
1487             altitudeDifference = oXs_MS5611.varioData.absoluteAlt.value -altitudeAtT0  ; // in cm
1488             secFromT0.value =  ( currentGliderMillis - millisAtT0 ) / 100 ;            // in 1/10 of sec
1489 #ifdef DEBUG_GLIDER8RATIO
1490             serial.print((F("secFromT0: ")); serial.println( secFromT0.value ) ;
1491 #endif
1492 #ifdef AIRSPEED_4525
1493             if ( (aSpeedAtT0 > 300) && ( oXs_4525.airSpeedData.smoothAirSpeed > 300 ) ) {
1494                 aSpeedWithinTolerance = ( (abs( oXs_4525.airSpeedData.smoothAirSpeed - aSpeedAtT0) * 100L ) / aSpeedAtT0 ) <= SPEED_TOLERANCE ;
1495             } else {
1496                 aSpeedWithinTolerance = false ;
1497             }
1498 #endif
1499             if (  ( oXs_MS5611.varioData.climbRate.value <  VSPEED_MIN_TOLERANCE ) || ( oXs_MS5611.varioData.climbRate.value >  VSPEED_MAX_TOLERANCE ) \
1500                 || ( altitudeDifference > -10 ) || ( aSpeedWithinTolerance == false ) ) {    // reset all when out of tolerance
1501                 altitudeAtT0 = oXs_MS5611.varioData.absoluteAlt.value ;
1502 #ifdef AIRSPEED_4525
1503                 aSpeedAtT0 = oXs_4525.airSpeedData.smoothAirSpeed ;
1504                 distanceSinceT0 = 0 ;
1505 #endif
1506                 secFromT0.value = 0 ;
1507                 millisAtT0 = currentGliderMillis ;
1508                 averageVspeedSinceT0.value = 0 ;
1509                 gliderRatio.value = 0 ;
1510 #ifdef DEBUG_GLIDER8RATIO
1511             serial.println((F("Reset"));
1512 #endif
1513
1514             } else {                                                      // within tolerance, calculate glider ratio and average sinking
1515 #ifdef AIRSPEED_4525
1516                 distanceSinceT0 += oXs_4525.airSpeedData.smoothAirSpeed / (1000 /  500) ;  // to adapt if delay is different.
1517 #endif
1518                 if (  secFromT0.value >=  GLIDER_RATIO_CALCULATED_AFTER_X_SEC * 10 ) {         // *10 because secFromT0 is in 1/10 of sec
1519 #ifdef AIRSPEED_4525
1520                     gliderRatio.value = distanceSinceT0  * -10 / altitudeDifference  ;        // when gliderRatio is > (50.0 *10) it it not realistic (*10 is done in order to add a decimal)
1521                     if ( gliderRatio.value > 500) gliderRatio.value = 0 ;                                                   //
1522 #endif
1523                     averageVspeedSinceT0.value = altitudeDifference * 10 / secFromT0.value  ;      // * 10 because secFromT0 is in 1/10 of sec
1524                 }
1525              }
1526             prevAverageAltMillis += 500  ;
1527             gliderRatio.available = true;
1528             secFromT0.available = true ;
1529             averageVspeedSinceT0.available = true ;
1530         }
1531 }
1532 #endif
1533 #endif // End of #if defined (VARIO) && defined (GLIDER_RATIO_BASED_ON_PPM )
1534 //********end calculate glider ratio************************************************************************************************
1535
1536
1537
1538
1539
1540 #ifdef PIN_PUSHBUTTON   // // ****************** check button
1541 void checkButton() {
1542   static int lastSensorVal=HIGH;
1543   static unsigned int buttonDownMs;
1544   //read the pushbutton value into a variable
1545   int sensorVal = digitalRead(PIN_PUSHBUTTON);
1546   if (sensorVal == HIGH) {
1547     // button not pressed released
1548     digitalWrite(PIN_LED, LOW);
1549   }
1550   else {
1551     //button is currently being pressed down
1552     unsigned int buttonPressDuration=millis()-buttonDownMs;
1553
1554     if( (buttonPressDuration>1000) and (buttonPressDuration<1200) )
1555       digitalWrite(PIN_LED, LOW); // Blink after 1 second
1556
1557     else if( (buttonPressDuration>3000) and (buttonPressDuration<3200) )
1558       digitalWrite(PIN_LED, LOW); // Blink 1 after 3 second
1559     else if( (buttonPressDuration>3300) and (buttonPressDuration<3400) )
1560       digitalWrite(PIN_LED, LOW); // Blink 2 after 3 second
1561
1562     else if( (buttonPressDuration>10000) and (buttonPressDuration<10200) )
1563       digitalWrite(PIN_LED, LOW); // Blink after 10 second
1564     else if( (buttonPressDuration>10300) and (buttonPressDuration<10400) )
1565       digitalWrite(PIN_LED, LOW); // Blink after 10 second
1566     else if( (buttonPressDuration>10500) and (buttonPressDuration<10600) )
1567       digitalWrite(PIN_LED, LOW); // Blink after 10 second
1568     else digitalWrite(PIN_LED, HIGH);
1569   }
1570   if( (lastSensorVal==HIGH) && (sensorVal==LOW))
1571   { // Button has been pressed down
1572     buttonDownMs=millis();
1573   }
1574   if( (lastSensorVal==LOW) && (sensorVal==HIGH))
1575   { // Button has been released
1576     unsigned int buttonPressDuration=millis()-buttonDownMs;
1577     //Serial.print(F("Button Released after ms:"));
1578     //Serial.println(buttonPressDuration);
1579     // Do Something after certain times the button has been pressed for....
1580     if( (buttonPressDuration>=100) and (buttonPressDuration<3000) )
1581       Reset1SecButtonPress();
1582     else if( (buttonPressDuration>=3000) and (buttonPressDuration<8000) )
1583       Reset3SecButtonPress();
1584     //else if( (buttonPressDuration>=5000) and (buttonPressDuration<10000) )
1585     else if( (buttonPressDuration>=10000) and (buttonPressDuration<15000) )
1586       Reset10SecButtonPress();
1587   }
1588
1589   lastSensorVal=sensorVal;
1590 }  // End checkButton
1591
1592 //ToDo: implement different reset actions on button press
1593 void Reset1SecButtonPress()
1594 {
1595 #ifdef DEBUG
1596   Serial.println(F(" Reset 0.1-3 sec"));
1597 #endif
1598
1599 #if defined(ARDUINO_MEASURES_A_CURRENT) && (ARDUINO_MEASURES_A_CURRENT == YES)
1600   struct CURRENTDATA *cd = &oXs_Current.currentData ;
1601 //  FORCE_INDIRECT(cd) ;
1602 //  cd->maxMilliAmps=cd->milliAmps;
1603 //  cd->minMilliAmps=cd->milliAmps;
1604 #endif
1605
1606 //#ifdef VARIO
1607 //  oXs_MS5611.resetValues() ;
1608 //#endif
1609
1610 //#ifdef VARIO2
1611 //  oXs_MS5611_2.resetValues() ;
1612 //#endif
1613
1614 }
1615
1616 void Reset3SecButtonPress()
1617 {
1618 #ifdef DEBUG
1619   Serial.println(F(" Reset 3-5 sec"));
1620 #endif
1621 #if defined(ARDUINO_MEASURES_A_CURRENT) && (ARDUINO_MEASURES_A_CURRENT == YES)
1622   oXs_Current.resetValues();
1623 #elif defined(AN_ADS1115_IS_CONNECTED) && (AN_ADS1115_IS_CONNECTED == YES ) && defined (ADS_MEASURE)  && defined (ADS_CURRENT_BASED_ON)
1624   oXs_ads1115.floatConsumedMilliAmps = 0 ;
1625   oXs_ads1115.adsCurrentData.consumedMilliAmps.value = 0 ;
1626 #endif
1627 #if defined ( A_FLOW_SENSOR_IS_CONNECTED ) && ( A_FLOW_SENSOR_IS_CONNECTED == YES)
1628   consumedML = 0 ;
1629 #endif
1630 }
1631
1632 void Reset10SecButtonPress()
1633 {
1634 #ifdef DEBUG
1635   Serial.println(F("Reset 5-10")) ;
1636 #endif
1637 }
1638
1639 #endif
1640
1641
1642
1643 #if defined (SAVE_TO_EEPROM ) and ( SAVE_TO_EEPROM == YES )
1644 /****************************************/
1645 /* SaveToEEProm => save persistant Data */
1646 /****************************************/
1647 void SaveToEEProm(){
1648   static uint8_t caseWriteEeprom = 0;
1649 #define CASE_MAX_EEPROM 2  // to adapt if thee are more data to save.
1650 #ifdef DEBUG
1651   Serial.print(F("Saving to EEProm:"));
1652   Serial.println(caseWriteEeprom);
1653 #endif
1654   switch (caseWriteEeprom ) {
1655 #if defined(ARDUINO_MEASURES_A_CURRENT) && (ARDUINO_MEASURES_A_CURRENT == YES)
1656   case 0 :
1657     EEPROM_writeAnything( 0 , oXs_Current.currentData.consumedMilliAmps);
1658     break ;
1659 #endif
1660 #if defined ( A_FLOW_SENSOR_IS_CONNECTED ) && ( A_FLOW_SENSOR_IS_CONNECTED == YES)
1661   case ( 1 ) :
1662     if ( ((uint16_t) consumedML) != consumedMLEeprom ) {                            // write in EEPROM only if there is a change
1663       consumedMLEeprom = consumedML ;
1664       EEPROM_writeAnything( 4 , consumedMLEeprom );
1665     }
1666     break;
1667   case ( 2 ) :
1668     if ( tankCapacity != tankCapacityEeprom ) {                            // write in EEPROM only if there is a change
1669       tankCapacityEeprom = tankCapacity ;
1670       EEPROM_writeAnything( 8 , tankCapacityEeprom );
1671     }
1672     for (uint8_t idxWrite = 0 ; idxWrite < 8 ; idxWrite++ ) {
1673       if ( flowParam[idxWrite] != flowParamEeprom[idxWrite] ) {
1674         flowParamEeprom[idxWrite] = flowParam[idxWrite] ;
1675         EEPROM_writeAnything( 12 + (idxWrite * 4 ) , flowParamEeprom[idxWrite] );
1676       }
1677     }
1678     break;
1679 #endif  // end A_FLOW_SENSOR_IS_CONNECTED
1680   }     // end of switch
1681   caseWriteEeprom++;
1682   if(caseWriteEeprom > CASE_MAX_EEPROM) caseWriteEeprom = 0;
1683
1684 // to do : add values from ads1115 about current consumption; to do also in LoadFromEEprom()
1685
1686 } // end SaveToEEPom
1687
1688 /******************************************/
1689 /* LoadFromEEProm => load persistant Data */
1690 /******************************************/
1691 void LoadFromEEProm(){
1692   // Load the last saved value
1693 #if defined(ARDUINO_MEASURES_A_CURRENT) && (ARDUINO_MEASURES_A_CURRENT == YES)
1694   EEPROM_readAnything( 0 , oXs_Current.currentData.consumedMilliAmps);
1695   #ifdef DEBUG
1696     Serial.println(F("Restored consumed mA"));
1697   #endif
1698 #endif
1699 #if defined ( A_FLOW_SENSOR_IS_CONNECTED ) && ( A_FLOW_SENSOR_IS_CONNECTED == YES )
1700     EEPROM_readAnything(4, consumedMLEeprom  ) + 4 ;
1701     consumedML = consumedMLEeprom  ;
1702     EEPROM_readAnything( 8, tankCapacityEeprom  );
1703     tankCapacity = tankCapacityEeprom ;
1704     for (uint8_t idxRead = 0 ; idxRead < 8 ; idxRead++ ) {
1705         EEPROM_readAnything( 12 + ( idxRead * 4) , flowParamEeprom[idxRead] );
1706         flowParam[idxRead] = flowParamEeprom[idxRead] ;
1707     }
1708     checkFlowParam() ;
1709   #ifdef DEBUG
1710     Serial.print(F("Restored consumed= ")); Serial.print(consumedMLEeprom) ; Serial.print(F("  Tank=")) ; Serial.print(tankCapacityEeprom) ; Serial.print(F(" Param=")) ;
1711     for (uint8_t idxRead = 0 ; idxRead < 8 ; idxRead++ ) {
1712       Serial.print(flowParamEeprom[idxRead]) ; Serial.print(" ") ;
1713     }
1714    Serial.println(" ");
1715
1716    Serial.print(F("          consumed= ")); Serial.print(consumedML) ; Serial.print(F("  Tank=")) ; Serial.print(tankCapacity) ; Serial.print(F(" Param=")) ;
1717     for (uint8_t idxRead = 0 ; idxRead < 8 ; idxRead++ ) {
1718       Serial.print(flowParam[idxRead]) ; Serial.print(" ") ;
1719     }
1720    Serial.println(" ");
1721   #endif
1722 #endif
1723 }
1724 #endif //SAVE_TO_EEPROM
1725
1726
1727 /***************************************************************/
1728 /* ProcessPPMSignal => read PPM signal from receiver and       */
1729 /*   use its value to adjust sensitivity and other parameters  */
1730 /***************************************************************/
1731 #if defined ( PPM_IS_USED )
1732
1733 void ProcessPPMSignal(){
1734   boolean getNewPpm = false ; // become true if a new ppm value has been acquired
1735 #if defined( PIN_PPM )  // when ppm is read from a rx channel
1736   ReadPPM(); // set ppmus to 0 if ppm is not available or has not been collected X time, other fill ppmus with the (max) pulse duration in usec
1737 #ifdef DEBUGFORCEPPM
1738 //for debuging ppm without having a connection to ppm; force ppm to a value
1739   ppmus = ( PPM_MIN_100 + PPM_PLUS_100) / 2 ; // force a ppm equal to 0 (neutral)
1740 #endif // end of DEBUGFORCEPPM
1741   if (ppmus>0){  // if a value has been defined for ppm (in microseconds)
1742     ppm.value = map( ppmus , PPM_MIN_100, PPM_PLUS_100, -100 , 100 ) ; // map ppm in order to calibrate between -100 and 100
1743     ppm.available = true ;
1744     getNewPpm = true ;
1745   }
1746 #else   // so when done via SPORT (defined ( PPM_VIA_SPORT ) && ( PROTOCOL  == FRSKY_SPORT ) || (PROTOCOL  == FRSKY_SPORT_HUB) ) )
1747         // newPpm.value and .available are already filled in readSensor() when new values are received
1748       if (newPpm.available ){
1749           ppm.value = newPpm.value ;
1750           ppm.available = true ;
1751           newPpm.available = false ;
1752           getNewPpm = true ;
1753       }
1754 #endif
1755   if  (getNewPpm ) {
1756     getNewPpm = false ; // reset the indicator saying there is a new ppm to handle.
1757 #ifdef DEBUGPPMVALUE
1758        Serial.print(micros()); Serial.print(F(" ppm="));  Serial.println(ppm.value);
1759 #endif
1760 #ifdef SEQUENCE_OUTPUTS
1761     if ( ( ( ppm.value - prevPpm) > 3 ) || (( prevPpm - ppm.value) > 3 )  )  { // handel ppm only if it changes by more than 3
1762         prevPpm = ppm.value ;
1763         setNewSequence( ) ;
1764     }
1765 #else // so if Sequence is not used and so PPM is used for Vario sensitivity , vario compensation , airspeed reset , glider ratio and/or vario source selection
1766     if ( ( ( ppm.value - prevPpm) < 4 ) && ( ( prevPpm - ppm.value) < 4 ) ) {  // test if new value is quite close to previous in order to avoid unstabel handling
1767
1768 #if defined ( VARIO_PRIMARY) && defined ( VARIO_SECONDARY)  && defined (VARIO ) && ( defined (VARIO2) ||  defined ( AIRSPEED_IS_USED)  || defined (USE_6050) )  && defined (PPM_IS_USED)
1769         if ( (ppm.value >= (SWITCH_VARIO_MIN_AT_PPM - 4)) && (ppm.value <= (SWITCH_VARIO_MAX_AT_PPM + 4)) ) {
1770           selectedVario = VARIO_PRIMARY ;
1771         } else if ( ( ppm.value <= (4 - SWITCH_VARIO_MIN_AT_PPM)) && (ppm.value >= (- 4 - SWITCH_VARIO_MAX_AT_PPM)) ) {
1772             selectedVario = VARIO_SECONDARY ;
1773         }
1774 #ifdef DEBUG_SELECTED_VARIO
1775     Serial.print(F("selected vario="));  Serial.println(selectedVario);
1776 #endif
1777 #endif // endif defined ( VARIO_PRIMARY) && defined ( VARIO_SECONDARY)  && ...
1778
1779 #if defined (VARIO) || defined (VARIO2)
1780         if ( (abs(ppm.value) >= (SENSITIVITY_MIN_AT_PPM - 4)) && ( abs(ppm.value) <= (SENSITIVITY_MAX_AT_PPM + 4)) ) {
1781             sensitivityPpmMapped =  map( constrain(abs(ppm.value), SENSITIVITY_MIN_AT_PPM , SENSITIVITY_MAX_AT_PPM ), SENSITIVITY_MIN_AT_PPM , SENSITIVITY_MAX_AT_PPM , SENSITIVITY_PPM_MIN , SENSITIVITY_PPM_MAX); // map value and change stepping to 10
1782 #if defined( VARIO)
1783             oXs_MS5611.varioData.sensitivityPpm = sensitivityPpmMapped ; // adjust sensitivity when abs PPM is within range
1784 #endif
1785 #if defined( VARIO2)
1786             oXs_MS5611_2.varioData.sensitivityPpm = sensitivityPpmMapped ;
1787 #endif
1788         }
1789 #endif  // end else defined (VARIO) || defined (VARIO2)
1790
1791
1792 #if defined ( AIRSPEED_IS_USED)  // adjust compensation
1793         if ( (abs(ppm.value) >= (COMPENSATION_MIN_AT_PPM - 4)) && ( abs(ppm.value) <= (COMPENSATION_MAX_AT_PPM + 4)) ) {
1794             compensationPpmMapped =  map( constrain(abs(ppm.value), COMPENSATION_MIN_AT_PPM , COMPENSATION_MAX_AT_PPM ), COMPENSATION_MIN_AT_PPM , COMPENSATION_MAX_AT_PPM , COMPENSATION_PPM_MIN , COMPENSATION_PPM_MAX); // map value and change stepping to 10
1795             if (compensationPpmMapped == COMPENSATION_PPM_MIN ) compensationPpmMapped = 0 ; // force compensation to 0 when compensation = min
1796         }
1797         if ( ( ppm.value >= (AIRSPEED_RESET_AT_PPM - 4)) && ( ppm.value <= (AIRSPEED_RESET_AT_PPM + 4)) ) {
1798 #if defined (AIRSPEED_4525) // if 4525 is used
1799             oXs_4525.airSpeedData.airspeedReset = true ; // allow a recalculation of offset 4525
1800 #elif defined (AIRSPEED_7002) // when 7002 is used
1801             oXs_ads1115.adsAirSpeedData.airspeedReset = true ; // allow a recalculation of offset mvxp7002 connected to ads1115
1802 #endif   // for SDP3X sensor , there is no need for a reset
1803         }
1804 #endif  // end AIRSPEED_IS_USED
1805
1806 #if defined ( A_FLOW_SENSOR_IS_CONNECTED ) && ( A_FLOW_SENSOR_IS_CONNECTED == YES)
1807         if ( ( ppm.value >= (FLOW_SENSOR_RESET_AT_PPM - 4)) && ( ppm.value <= (FLOW_SENSOR_RESET_AT_PPM + 4)) ) {
1808             consumedML = 0 ;     // reset the flow counter
1809         }
1810 #endif
1811
1812
1813 #if defined (GLIDER_RATIO_CALCULATED_AFTER_X_SEC ) && defined ( GLIDER_RATIO_ON_AT_PPM )
1814          if ( (ppm.value >= (GLIDER_RATIO_ON_AT_PPM - 4)) && (ppm.value <= (GLIDER_RATIO_ON_AT_PPM + 4)) ) {
1815           gliderRatioPpmOn = true ;
1816         } else {
1817             gliderRatioPpmOn = false ;
1818         }
1819 #endif
1820
1821     } // end ppm == prePpm
1822     prevPpm = ppm.value ;
1823 #endif  // end of #ifdef SEQUENCE_OUTPUTS & #else
1824   }  // end if  (ppm.available )
1825
1826 }  // end processPPMSignal
1827
1828
1829 /**********************************************************/
1830 /* ReadPPM => read PPM signal from receiver               */
1831 /*   pre-evaluate its value for validity                  */
1832 /**********************************************************/
1833
1834 #ifdef PPM_INTERRUPT
1835 uint16_t StartTime ;
1836 volatile uint16_t time1 ; // pulsewidth (in millisec)
1837 //volatile uint8_t PulseTime ;          // A byte to avoid
1838 volatile uint32_t ppmPulseMicros ; // timestamp when falling edges occurs in micros sec
1839 volatile boolean  PulseTimeAvailable = false ;
1840
1841 #if defined ( MEASURE_RF_LINK_QUALITY ) && ( MEASURE_RF_LINK_QUALITY == YES)
1842             uint32_t ppmTimestampPrev ; //Timestamp (micros) of the previous PPM signal (used to check that the delay between 2 pulses is about 18ms)
1843             uint16_t widthPrev ;  // width of the previous PWM pulse ( micros)
1844             uint8_t lqError ;     // count the number of errors (new PWM pulses having the same value as the previous one)
1845             uint8_t lqCount ;     // count total number of PWM pulses (to calculate a %); use to reset the counters after XXX (e.g.50) PWM
1846             uint8_t prevFrameInError ; // boolean ; true when the previous PWM was wrong (used to calculate consecutive errors)
1847             uint8_t lqConsecutiveError ;  // count number of consecutive PWM errors
1848             uint8_t lqConsecutiveErrorMax ; // keep the max of consecutive errors within a sequence
1849 #endif
1850 extern uint16_t lastTimerValue ;
1851 extern uint32_t TotalMicros ;
1852
1853
1854 #if PIN_PPM == 2 // When pin 2 is used, arduino handle INT0 (see datasheet)
1855 ISR(INT0_vect, ISR_NOBLOCK)  // NOBLOCK allows other interrupts to be served when this one is activated
1856 #else            //// When pin 3 is used, arduino handle INT1
1857 ISR(INT1_vect, ISR_NOBLOCK)
1858 #endif
1859 {
1860         uint8_t oReg = SREG; // store SREG value
1861         cli() ;
1862         uint16_t time = TCNT1 ; // Read timer 1
1863   uint16_t lastTimerValueTemp  = lastTimerValue ; // save the value to avoid any change in interrupt (not sure it is required
1864   uint32_t TotalMicrosTemp = TotalMicros ; // save the value to avoid any change in interrupt (not sure it is required
1865         SREG = oReg ; // restore SREG value
1866         if ( EICRA & PPM_INT_EDGE ) // a rising edge occurs
1867         {
1868                 StartTime = time ;              // save the value of the timer
1869                 EICRA &= ~PPM_INT_EDGE ;                                // allow a falling edge to generate next interrupt
1870         }
1871         else                       // a falling edge occurs
1872         {
1873     uint16_t elapsed = time - lastTimerValueTemp ;
1874 #if F_CPU == 20000000L   // 20MHz clock
1875    #error Unsupported clock speed
1876 #elif F_CPU == 16000000L  // 16MHz clock
1877       ppmPulseMicros = TotalMicrosTemp + ( elapsed >> 4 ) ;
1878 #elif F_CPU == 8000000L   // 8MHz clock
1879       ppmPulseMicros = TotalMicrosTemp + ( elapsed >> 3 ) ;
1880 #else
1881     #error Unsupported clock speed
1882 #endif
1883
1884                 time -= StartTime ;
1885 #if F_CPU == 20000000L   // 20MHz clock
1886    #error Unsupported clock speed
1887 #elif F_CPU == 16000000L  // 16MHz clock
1888                 time>>= 4 ;             // delay in usec
1889 #elif F_CPU == 8000000L   // 8MHz clock
1890                 time >>= 3 ;            // delay in usec
1891 #else
1892     #error Unsupported clock speed
1893 #endif
1894                 time1 = time ;
1895     PulseTimeAvailable = true ;
1896                 EICRA |= PPM_INT_EDGE ;                         //  allow a Rising edge to generate next interrupt
1897         }
1898 }
1899
1900 void ReadPPM() {    // set ppmus to 0 if ppm is not available or has not been collected X time, other fill ppmus with the (max) pulse duration in usec
1901          static uint8_t ppmIdx ;
1902          static uint16_t ppmTemp ;
1903          static uint16_t ppmMax ; // highest value of ppmTemp received ; Some ppm measurement are wrong (to low) because there are some interrupt some
1904          ppmus = 0 ; // reset the pulse width
1905
1906          if ( PulseTimeAvailable ) { // if new pulse is available
1907 #define PPM_COUNT_MAX 5 // select the max of 5 ppm (so once every 100 msec
1908            uint8_t oReg = SREG ; // save Status register
1909            cli() ;
1910             ppmTemp = time1 ;    // use values from interrupt
1911             uint32_t ppmTimestamp = ppmPulseMicros ;
1912             PulseTimeAvailable = false ;
1913             SREG = oReg ;  // restore Status register
1914 #if defined ( MEASURE_RF_LINK_QUALITY ) && ( MEASURE_RF_LINK_QUALITY == YES)
1915             uint32_t pulseInterval = (ppmTimestamp - ppmTimestampPrev)  ;
1916             if ( (pulseInterval > PULSE_INTERVAL_MIN) && (pulseInterval < PULSE_INTERVAL_MAX ) ) {
1917               if ( abs (( int16_t) (ppmTemp - widthPrev)) <= WIDTH_ERROR_MAX )  {
1918                     lqError++ ;
1919                     if ( prevFrameInError == true) {
1920                       lqConsecutiveError++ ;      // count consecutive errors
1921                       if (lqConsecutiveError > lqConsecutiveErrorMax) {
1922                         lqConsecutiveErrorMax = lqConsecutiveError ;
1923                       }
1924                     }
1925
1926                     prevFrameInError = true ;
1927               } else {
1928                 lqConsecutiveError = 0 ; // reset the counter when a valid PWM is received
1929                 prevFrameInError = false ; // reset the flag when we get a valid PWM signal
1930               }
1931               lqCount++ ;
1932 #if defined ( DEBUG_RF_LINK_QUALITY )
1933               Serial.print("Read ppm at ") ; Serial.print(ppmTimestamp) ;
1934               Serial.print(" Delay= ") ; Serial.print(pulseInterval) ;
1935               Serial.print(" Process at ") ; Serial.print(millis()) ;
1936               Serial.print(" Width= ") ; Serial.print(ppmTemp) ;
1937               Serial.print(" delta=") ; Serial.print( abs (( int16_t) (ppmTemp - widthPrev)) ) ;
1938               Serial.print(" lqC=") ; Serial.print( lqCount ) ;
1939               Serial.print(" lqE=") ; Serial.print( lqError ) ;
1940               Serial.print(" cons=") ; Serial.print( lqConsecutiveError) ;
1941               Serial.print(" Max=") ; Serial.print( lqConsecutiveErrorMax) ;
1942               Serial.print(" %=") ; Serial.println(  (  ( (LQ_COUNT_MAX - (uint16_t) lqError)) *  100 ) / LQ_COUNT_MAX  ) ;
1943 #endif
1944               if ( lqCount >= LQ_COUNT_MAX ) {
1945 #if defined ( FILL_TEST_1_2_WITH_LQ )
1946               test1.value =   (  ( (LQ_COUNT_MAX - (uint16_t) lqError)) *  100 ) / LQ_COUNT_MAX ;
1947               test1.available = true ;
1948               test2.value =  lqConsecutiveErrorMax ;
1949               test2.available = true ;
1950 #endif // FILL_TEST_1_2_WITH_LQ
1951                 lqCount = 0 ;
1952                 lqError = 0 ;
1953                 lqConsecutiveError = 0 ;
1954                 lqConsecutiveErrorMax = 0 ;
1955                 // to do : save the values in data that can be transmitted
1956               }
1957             }
1958             ppmTimestampPrev = ppmTimestamp ;
1959             widthPrev = ppmTemp ;
1960 #endif  // defined ( MEASURE_RF_LINK_QUALITY ) && ( MEASURE_RF_LINK_QUALITY == YES)
1961
1962             if ( ppmIdx >= PPM_COUNT_MAX ) {
1963                 ppmus = ppmMax ; // we keep the highest value
1964                 ppmIdx = 0 ;
1965                 ppmMax = ppmTemp ;
1966             } else {
1967                 ppmIdx++ ;
1968                 if( ppmTemp > ppmMax ) ppmMax = ppmTemp ;  // save the highest value
1969             }  // end test on ppmIdx
1970         } // end test on PulseTimeAvailable
1971 } //end ReadPPM()
1972 #endif // end #ifdef PPM_INTERRUPT
1973
1974 #endif //PIN_PPM
1975
1976
1977 #if defined ( A_FLOW_SENSOR_IS_CONNECTED )
1978   #if ( A_FLOW_SENSOR_IS_CONNECTED == YES)  // flowMeterCnt is updated by interrupt PCINT0
1979
1980
1981
1982 float mapFlowCorr( uint8_t idx) {                             // calculate correction factor depending on current flow
1983   uint8_t flowCorrIdx = idx + TX_FIELD_FLOW_CORR_1 ;
1984   return ( flowParam[flowCorrIdx ] + ( currentFlow - flowParam[idx]) / (flowParam[idx + 1] - flowParam[idx]) * (flowParam[flowCorrIdx + 1] - flowParam[flowCorrIdx ] ) ) ;
1985 }
1986
1987 void checkFlowParam() {
1988   if ( (tankCapacity % 50) != 0) tankCapacity = ( tankCapacity / 50 ) * 50 ; // set tank capacity in steps of 50 ml
1989   tankCapacity = constrain( tankCapacity , 100 , 3000 ) ;
1990   for (uint8_t i = 0; i < 4 ; i++) {
1991       if ( ( flowParam[i] % 5) != 0 ) flowParam[i] = ( flowParam[i] / 5 ) * 5 ;
1992       flowParam[i] = constrain( flowParam[i] , 30 , 800 + (5 * i)) ;
1993       flowParam[ i+ 4] = constrain( flowParam[i+4] , -100 , +100 ) ;
1994   }
1995   for (uint8_t i= 1 ; i < 4 ; i++) {
1996       if ( flowParam[i] <= flowParam[i - 1] )  flowParam[i] = flowParam[i - 1] + 5 ;
1997   }
1998 }
1999
2000 void processFlowMeterCnt () {    // get the flowmeter counter once per X seconds, convert it in mili liter and activate the flag saying a new value is available
2001                                  // save the result in eeprom is done in main loop for all data to be saved in EEPROM
2002 #define FLOW_DELAY 2000                                   // calculates once per 2 sec
2003   float flowCorr ;
2004   uint16_t flowMeterCntCopy ;
2005   static uint32_t prevFlowMillis ;
2006   uint32_t currentFlowMillis = millis() ;
2007   if ( ( currentFlowMillis - prevFlowMillis ) > FLOW_DELAY ) {
2008     prevFlowMillis = currentFlowMillis ;
2009     uint8_t oReg = SREG ; // save status register
2010     cli() ;  // avoid interrupt to ensure that counter is consistent
2011     flowMeterCntCopy = flowMeterCnt ;
2012     flowMeterCnt = 0 ;
2013     SREG = oReg ; // restore status register
2014     sei() ;  // allow interrupt again
2015 #ifdef DEBUG_SIMULATE_FLOW_SENSOR
2016     flowMeterCntCopy = 100 ;        // force a fix value in order to test without a flow sensor
2017 #endif
2018 #define CONVERT_TO_ML_PER_MIN  30.0 / ( PULSES_PER_ML * FLOW_DELAY / 1000.0 ) //  30 = 60 /2 : 60 = 60 sec/min, 1000 = msec instead of sec , 2 because counter is increased by all changes (rise and fall)
2019     currentFlow  =  ((float) flowMeterCntCopy) * CONVERT_TO_ML_PER_MIN ;                                          // expected value should be between 15 and 800 ml/min for the conrad flow meter
2020     if (currentFlow < flowParam[TX_FIELD_FLOW_1] ) {
2021       flowCorr = flowParam[TX_FIELD_FLOW_CORR_1] ;               // flow crrection is supposed to be in %
2022     } else if (currentFlow < flowParam[TX_FIELD_FLOW_2] ) {
2023       flowCorr = mapFlowCorr(  TX_FIELD_FLOW_1 ) ;
2024     } else if (currentFlow < flowParam[TX_FIELD_FLOW_3] ) {
2025       flowCorr = mapFlowCorr(  TX_FIELD_FLOW_2 ) ;
2026     } else if (currentFlow < flowParam[TX_FIELD_FLOW_4] ) {
2027       flowCorr = mapFlowCorr(  TX_FIELD_FLOW_3 ) ;
2028     }  else {
2029       flowCorr = flowParam[TX_FIELD_FLOW_CORR_4] ;
2030     }
2031     currentFlow = ((float) currentFlow ) * ( 100.0 + flowCorr) * 0.01   ; // 0.01 because flowCorr is in %)
2032     actualFlow.value = currentFlow ;
2033     actualFlow.available= true ;
2034     consumedML += currentFlow  * FLOW_DELAY / 60000.0 ;
2035     if ( consumedML > tankCapacity ) consumedML = tankCapacity ;
2036     remainingFuelML.value = tankCapacity - consumedML ;
2037     remainingFuelML.available= true ;
2038     fuelPercent.value =  remainingFuelML.value * 100 / tankCapacity ; // in percent
2039     fuelPercent.available= true ;
2040     newFlowAvailable = true ;   // this is use to fill TEST_1, 2, 3 to avoid conflict with JETI protocol which uses 3 individual flags
2041 #if defined ( DEBUG) && defined (DEBUG_FLOW_SENSOR)
2042     Serial.print(" At " ); Serial.print( prevFlowMillis ) ;
2043     Serial.print(" flcnt " ); Serial.print( flowMeterCntCopy ) ;
2044     Serial.print(" remain " ); Serial.print( remainingFuelML.value ) ;
2045     Serial.print(" ml/min " ); Serial.print( currentFlow ) ;
2046     Serial.print(" cons " ); Serial.println( consumedML ) ;
2047 #endif
2048
2049   }
2050 }      // end processFlowMeterCnt
2051
2052
2053 ISR(PCINT0_vect, )  {       // a pin change interrupt occurs on the pin being connected to the flow sensor; we do not use ISR_NOBLOCK because the interrupt is very short
2054   flowMeterCnt++ ;            // we increase the counter of pin change.
2055 }
2056
2057
2058   #endif
2059 #endif // end A_FLOW_SENSOR_IS_CONNECTED
2060
2061
2062
2063 #ifdef SEQUENCE_OUTPUTS
2064 void setNewSequence() {                   // **********  setNewSequence( ) **************  handle a new ppm value
2065 //    int8_t prevPpmMain = -100 ; // this value is unusual; so it will forced a change at first call
2066     int16_t ppmAbs = ppm.value + 114 ;
2067     int8_t ppmMain = ppmAbs / 25 ;
2068     int8_t ppmRest = ppmAbs % 25 ;
2069     if ( (ppmRest > 4) && (ppmMain >= 0) && (ppmMain <= 9) && (ppmMain != prevPpmMain) ) { // process only if valid and different from previous
2070 #ifdef DEBUGSEQUENCE
2071     Serial.println(F(" "));
2072     Serial.print(F("ppmMain="));  Serial.println(ppmMain );
2073     //Serial.println(F(" "));
2074 #endif
2075        prevPpmMain = ppmMain ;
2076        seqRef = sequencePointer[ppmMain] ;  // seqTab contains pointers to the sequence array
2077        seqMax = sequenceMaxNumber[ppmMain] ;
2078        seqState = 0 ;
2079        seqStep = 0 ;
2080 #ifdef DEBUGSEQUENCE
2081        test1.value = ppmMain;
2082        test1.available = true ;
2083 #endif
2084        checkSequence() ;
2085     }
2086 }
2087
2088 void checkSequence() {
2089   uint8_t portbTemp ;
2090   uint8_t portbTempValue ;
2091   static uint16_t seqDelay ;
2092   static unsigned long  currentMillis ;
2093   static unsigned long  nextSequenceMillis ;
2094 #ifdef DEBUGSEQUENCE
2095 //    Serial.print(F("seqState="));  Serial.println(seqState );
2096 #endif
2097
2098   if ( seqState == 0 ) {
2099     portbTempValue = *(seqRef + 1);
2100     seqDelay = *(seqRef ) * sequenceUnit; //
2101     portbTemp =  (~sequenceOutputs) & PORTB; // reset the bit that are controlled
2102     PORTB = portbTemp | ( portbTempValue & sequenceOutputs ); // set the bit as defined in the sequence if they have to be controlled
2103 #ifdef DEBUGSEQUENCE
2104     Serial.print(F("seqStep="));  Serial.println( seqStep );
2105     Serial.print(F("portbTempValue="));  Serial.println( portbTempValue );
2106     Serial.print(F("seqDelay="));  Serial.println( seqDelay );
2107     Serial.print(F("portbTemp="));  Serial.println( portbTemp );
2108     Serial.print(F("PORTB="));  Serial.println(  portbTempValue & sequenceOutputs );
2109 #endif
2110
2111     if (seqDelay == 0 ) {
2112       seqState = 2 ; // set state to sequence stopped if delay is 0
2113     } else {
2114       seqState = 1 ; //says that sequence is running
2115       nextSequenceMillis = millis() +  seqDelay ;
2116     }
2117   } else if ( seqState == 1 ) { // when sequence is running, check the timestamp to activate next sequence step
2118     currentMillis = millis() ;
2119     if ( currentMillis >  nextSequenceMillis  ) {
2120        seqStep += 2 ; // increase by 2 because there are 2 parameters per step
2121       if ( (seqStep + 1)  >=  seqMax  ) seqStep = 0 ; // return to 0 if end is reached
2122       portbTempValue = *(seqRef + 1 + seqStep );
2123       seqDelay = *(seqRef + seqStep) * sequenceUnit ;
2124       portbTemp =  (~sequenceOutputs) & PORTB; // reset the bit that are controlled
2125       PORTB = portbTemp | ( portbTempValue & sequenceOutputs ); // set the bit as defined in the sequence if they have to be controlled
2126       if (seqDelay == 0 ) {
2127           seqState = 2 ; // set state to sequence stopped if delay is 0
2128       } else {
2129           seqState = 1 ; //says that sequence is running
2130           nextSequenceMillis = currentMillis  +  seqDelay ;
2131       }
2132 #ifdef DEBUGSEQUENCE
2133       Serial.print(F("At"));  Serial.print( currentMillis );
2134       Serial.print(F(" seqStep="));  Serial.println( seqStep );
2135       Serial.print(F("portbTempValue="));  Serial.println( portbTempValue );
2136       Serial.print(F("seqDelay="));  Serial.println( seqDelay );
2137       Serial.print(F("portbTemp="));  Serial.println( portbTemp );
2138       Serial.print(F("PORTB="));  Serial.println( portbTempValue & sequenceOutputs );
2139 #endif
2140
2141     } // end  ( currentMillis >  nextSequenceMillis  )
2142   }   // end  ( seqState == 0 ) or ( seqState == 1 )
2143 }     // end checkSequence()
2144 #endif // end SEQUENCE_OUTPUTS
2145
2146
2147
2148 #ifdef DEBUG
2149 //************************************************************   OutputToSerial
2150 void OutputToSerial(){
2151 //#define DEBUGMINMAX 0
2152 //#define DEBUGVREF 0
2153 //#define DEBUGDIVIDER 0
2154 //#define DEBUGTEMP 0
2155
2156
2157
2158 #ifdef VARIO
2159   Serial.print(F(" Sensitivity PPM="));
2160   Serial.print( oXs_MS5611.varioData.sensitivityPpm);
2161   Serial.print(F(" ;absAlt="));
2162   Serial.print( ( (float)oXs_MS5611.varioData.absoluteAlt.value)/100);
2163   Serial.print(F(" ;Vspd="));
2164   Serial.print( ( (float)oXs_MS5611.varioData.climbRate.value)/100);
2165   Serial.print(F(" ;Temp="));
2166   Serial.print((float)oXs_MS5611.varioData.temperature/10);
2167 #endif // VARIO
2168
2169 #if defined(ARDUINO_MEASURES_A_CURRENT) && (ARDUINO_MEASURES_A_CURRENT == YES)
2170   Serial.print(F(" ;mA="));
2171   Serial.print( ( ((float)(int32_t)(oXs_Current.currentData.milliAmps.value))) );
2172   Serial.print("(");
2173   Serial.print(F(" ;mAh="));
2174   Serial.print( oXs_Current.currentData.consumedMilliAmps.value);
2175 #endif // defined(ARDUINO_MEASURES_A_CURRENT) && (ARDUINO_MEASURES_A_CURRENT == YES)
2176 #ifdef HOTT
2177   Serial.println("H.");
2178 #endif
2179   Serial.println();
2180
2181 }
2182
2183 /***********************************************/
2184 /* freeRam => Cook coffee and vaccuum the flat */
2185 /***********************************************/
2186 int freeRam () {
2187   extern int __heap_start, *__brkval;
2188   int v;
2189   return (int) &v - (__brkval == 0 ? (int) &__heap_start : (int) __brkval);
2190 }
2191 #endif // End DEBUG
2192
2193
2194
2195