Vario Paragliding

MS5611.ino

// Original code by Rolf R Bakke,  Oct 2012, 
// Modified May 2017 by Thomas Gilson. [email protected]
// code is for the arduino pro/pro mini, using MS5611 barometer and speaker on pin 2.
// can be hooked up to a button for different mode changes on the "fly" (he he).
// made especially for paragliders. 
// before using this, you should know roughly what your min sink rate will be.
// this code is designed to sound no tone when descending normally at speeds near your min sink rate.
// it sounds (relatively) continuous low frequency tones when descending faster than user-settable sink threshold.
// it sounds high-frequency beeps when gaining altitude faster than user-settable ascend threshold.
// it may detect when you are getting thermal uplift but still descending. This can be disabled if preferred.
// to test your new tone settings, go to the toneGenerate function and change the variable dr to a constant. 
// please see the buttonPressed() function section for detailed comments and mode changes.
 
#include <Wire.h>

// global variables will be visible in all functions.
const byte led = 13;
unsigned int calibrationData[7];
long pressure;

int samplesPerSecond;
int samplesToAverage;
bool firstLoop = true;
int msResolution;

int  ascendFreqRange;
int   ascendBeepRange;
int   ascendDelayRange;
float ascendThreshold;
float ascendMaxMetersPerSecond;
int ascendStartFreq;
int ascendStartBeepLength;
int ascendStartBeepDelay;

int ascendFinalFreq;
int ascendFinalBeepLength;
int ascendFinalBeepDelay;


//thermals
float thermalThreshold;
int thermalFreq;
int thermalBeepLength;
int thermalBeepDelay;
int thermaldetectWait;
bool detectThermals;


//sink
float sinkThreshold;
float sinkMaxMetersPerSecond;
int sinkStartFreq;
int sinkFinalFreq;
int sinkFreqRange;









unsigned long thermalInTime;
bool inThermalCount;

unsigned long lastCalcMillis = 0;
unsigned long currentCalcMillis = 0;
unsigned long lastBeep;
unsigned long lastModeChangeTime;
float beepTime;
double currentDescentRate = 0.0;

double lastAvgPressure = 0.0;
double rollingPressureAvg;
double rollingDescentRate = 0;
double dr;
unsigned long startTime;


/*

   1 meter-of-air-15°C = 0.12015397 millibars

   MS5611-01BA03 sensor error band with autozero at one pressure point, 25 degree C, 1100-700 mbar (roughly seal level to 3000 meters)  is +-0.5 mbar


   1 meter of air = .120 mbar @15 degree C
   2 meter of air = .240 mbar @ 15 degree C
   10 meter of air = 1.20 mbar @ 15 degree C
   So mbar and meters are linear relation



*/


void setup()
{
  
  Wire.begin();
  Serial.begin(115200);
  
  setupSensor();
  buttonPressed(); //initialize the mode variables. start at mode 1.
  rollingPressureAvg = (double)getPressure();
  lastCalcMillis = millis();
  lastBeep = millis();
  //beepTime = 1000;
  inThermalCount=false;
  startTime=millis();
}


void loop()
{

  static long loopCounter=0;
  loopCounter++;
  
  pressure = getPressure();
  rollingPressureAvg -= rollingPressureAvg / (double)samplesToAverage ;
  rollingPressureAvg += (double)pressure / (double)samplesToAverage ;

  unsigned long now = millis();
  if ((now - lastCalcMillis) > 250)
  {
    getDescentRate();
    if (detectThermals && (rollingDescentRate < thermalThreshold) && (rollingDescentRate > ascendThreshold)) {

      if (inThermalCount == false) {

        thermalInTime = now;
        inThermalCount = true;
      }

    } else {
      inThermalCount = false;
    }

  }


 // check to see if buttonPressed is working 
  // if (loopCounter==160) buttonPressed();
  // if (loopCounter==360) buttonPressed();

  toneGenerate();


  delay(msResolution);


}





float getDescentRate() { // returns the current descent rate (if positive) or ascent rate (if negative) in meters per second


  float pressureDifference;
  float secondsDifference;
  float metersDifference;

  pressureDifference = rollingPressureAvg - lastAvgPressure;
  lastAvgPressure = rollingPressureAvg;
  metersDifference = pressureDifference / 10.0;
  currentCalcMillis = millis();
  secondsDifference = (float)(currentCalcMillis - lastCalcMillis) / 1000.0;
  lastCalcMillis = currentCalcMillis;

  currentDescentRate = metersDifference / secondsDifference;


  if (firstLoop == false) {

    rollingDescentRate -= (rollingDescentRate / 4.0) ;
    rollingDescentRate += (currentDescentRate / 4.0) ;

    Serial.print(currentDescentRate);
    Serial.print(" ");
    Serial.println(rollingDescentRate);
    //toneGenerate();
  }
  firstLoop = false;
}


long getPressure()
{
  //why is P a long (int) here and a float in the loop?
  long D1, D2, dT, P;
  float TEMP;
  int64_t OFF, SENS;
  D1 = getData(0x48, 10);
  D2 = getData(0x50, 1);
  dT = D2 - ((long)calibrationData[5] << 8);
  TEMP = (2000 + (((int64_t)dT * (int64_t)calibrationData[6]) >> 23)) / (float)100;
  OFF = ((unsigned long)calibrationData[2] << 16) + (((int64_t)calibrationData[4] * dT) >> 7);
  SENS = ((unsigned long)calibrationData[1] << 15) + (((int64_t)calibrationData[3] * dT) >> 8);
  P = (((D1 * SENS) >> 21) - OFF) >> 15;
  //Serial.println(TEMP);
  //Serial.println(P);
  return P;
}
long getData(byte command, byte del)
{
  long result = 0;
  twiSendCommand(0x77, command);
  delay(del);
  twiSendCommand(0x77, 0x00);
  Wire.requestFrom(0x77, 3);
  if (Wire.available() != 3) Serial.println("Error: raw data not available");
  for (int i = 0; i <= 2; i++)
  {
    result = (result << 8) | Wire.read();
  }
  return result;
}




void toneGenerate() {

  //rollingDescentRate will contain the current averaged descent rate;   negative means going up

  // I am considering 9 m/s to give us the maximum climb rate and 7 m/s max descent rate (full stall).
  // I had the most amazing experience surfing a vortex ring in a very strong thermal once. I was going up at 9+ m/s
  // In a full stall you get something in the neighborhood of 7 m/s - not too much.
  // Even a very weak thermal has 0.5 m/s. You average thermal has 2-3 m/s.




  dr = rollingDescentRate;
  // to check your  tones, just uncomment this and set to a constant value;
 // dr = -3.5;

  if ((millis() - lastBeep  ) < beepTime) return;
 
  float diffPercent;
  // gaining altitude faster than ascendThreshold
  if (dr <= ascendThreshold) { 

    if (dr <  ascendMaxMetersPerSecond) dr = ascendMaxMetersPerSecond;
    diffPercent = ((ascendThreshold - dr) /   abs((ascendMaxMetersPerSecond - ascendThreshold))); ///gonna be positive or zero here
    beepTime = (ascendStartBeepLength - (ascendBeepRange * diffPercent)) + (ascendStartBeepDelay - (ascendDelayRange * diffPercent));
    tone(2, ascendStartFreq + (ascendFreqRange * diffPercent),  ascendStartBeepLength - (ascendBeepRange * diffPercent) );
    lastBeep = millis();
    return;
  }




  // sinking faster than sinkThreshold
  if (dr >= sinkThreshold) {

    if (dr >  sinkMaxMetersPerSecond) dr = sinkMaxMetersPerSecond;
    diffPercent = ((dr - sinkThreshold) /   abs((sinkMaxMetersPerSecond - sinkThreshold)));
    beepTime = 500;
    tone(2, sinkStartFreq - (sinkFreqRange * diffPercent));
    lastBeep = millis();

    return;
  }

  // descent rate is less than min sink rate of glider. set by thermalThreshold
  if (dr <=  thermalThreshold) {

    if (detectThermals && inThermalCount && ((millis()-thermalInTime) >= thermaldetectWait)) {
      
      beepTime = thermalBeepLength + thermalBeepDelay;
      tone(2, thermalFreq, thermalBeepLength);
      lastBeep = millis();
      return;

    }

  }




  //not in one of the above 3 scenarios? turn off tones.
  noTone(2);


}


bool buttonPressed()
{

  static int mode=0;              // don't touch this
  static int numberOfModes=3;     // if you add or subtract a mode, add or subtract this.
 
   mode++;
   
   if (mode==numberOfModes+1) mode=1; //loop around to first mode when last mode is reached 



  // these are the modes. you can copy and paste the variables in mode 1 to your other modes. But if you just want to change certain variables, then just copy the
  // variables you want to change. The values of the previous mode will still be in effect.
  
  if (mode == 1) {
    samplesPerSecond = 40;            // this can range from  30 to 50. don't touch this for now
    samplesToAverage = 10;            // 10 to 20       don't touch this for now.  


    //gaining altitude                // all of these tones and beeps are extrapolated linearly for now. Probably better to do a log funtion to get more tone variation when in average thermals.
    ascendThreshold = -0.2;            // when gaining altitude at this many M/S, start sounding the ascend tones. must be 0 (maintaining altitude) or negative (going up).
    ascendMaxMetersPerSecond = -5.0;   // any ascent rate higher than this and we get the ascendFinalFreq. must be negative and greater than ascendThreshold  setting this lower negative value means more tone variation at slower ascent rates
    ascendStartFreq = 500;             // freq in HZ at  ascendThreshold
    ascendStartBeepLength = 700;       // beep length in milliseconds at  ascendThreshold
    ascendStartBeepDelay = 300;       // delay in milliseconds between beeps at  ascendThreshold
    ascendFinalFreq = 1300;           // freq in hz when ascendMaxMetersPerSecond is reached
    ascendFinalBeepLength = 90;      // beep length in milliseconds  when ascendMaxMetersPerSecond is reached
    ascendFinalBeepDelay = 40;        // delay in milliseconds between beeps  when ascendMaxMetersPerSecond is reached


    // when getting uplift but still losing altitude
    detectThermals = true;           // you may want to know when there is an updraft, even though you are losing altitude. for example, if your minimum sink rate is 1.2 m/sec and you 
                                     // are descending at 0.2 M/S  then you may be in a thermal uplift of 1 M/S. To detect this and sound tones, set this to true;
                                     // to turn it off entirely, set it false.
    thermaldetectWait = 2000;       // milliseconds to wait before sounding tones while you are in the thermal zone between thermalThreshold and ascendThreshold;
                                    // set this to 2000 milliseconds or above to reduce extraneous beeping. 
    thermalThreshold = 0.8;         // M/S descent rate at which to start considering you are experiencing a thermal uplift. try 0.4 M/S less than min sink rate.  
    thermalFreq = 320;              // freq in HZ for the thermal tone
    thermalBeepLength = 40;         // beep length in milliseconds of thermal tone
    thermalBeepDelay = 800;         // delay in milliseconds between thermal beeps
    

    //sink
    sinkThreshold = 1.8;              // M/S sink rate at which to start sounding the descend tones
    sinkMaxMetersPerSecond = 5;      // if you are sinking  faster than this (in M/S) you get the  sinkFinalFreq tone. setting this lower means more tone variation at slower descent rates
    sinkStartFreq = 220;            // freq in HZ tone to sound when sinkThreshold is reached
    sinkFinalFreq = 70;             // freq in HZ tone to sound when sinkMaxMetersPerSecond is reached



  }


  if (mode == 2) { // on a mode change, the new mode takes all the values of previous mode except the ones you change.
     detectThermals=false;
     sinkThreshold=2.2;
  }

  if (mode == 3) {     
     detectThermals=true;
     thermalFreq=290;
     thermalBeepLength=70;
     
  }




  lastModeChangeTime = millis();

  //don't touch below here. it is all calculated
  msResolution = (1 / (float)samplesPerSecond) * 1000;
  ascendFreqRange = (ascendFinalFreq - ascendStartFreq);
  ascendBeepRange =  (ascendStartBeepLength - ascendFinalBeepLength);
  ascendDelayRange = (ascendStartBeepDelay - ascendFinalBeepDelay);

  sinkFreqRange = (sinkStartFreq - sinkFinalFreq);


 


  //sound the mode change
  noTone(2);
  for (int i = 1; i <= mode; i++)
  {
    tone(2, 500, 100);
    delay(150);
  }

   beepTime = 1000;
   return true;
}








void setupSensor()
{
  /*
    #define MS5611_CMD_ADC_READ           (0x00)
    #define MS5611_CMD_RESET              (0x1E)
    #define MS5611_CMD_CONV_D1            (0x40)
    #define MS5611_CMD_CONV_D2            (0x50)
    #define MS5611_CMD_READ_PROM          (0xA2)

    typedef enum
    {
        MS5611_ULTRA_HIGH_RES   = 0x08,
        MS5611_HIGH_RES         = 0x06,
        MS5611_STANDARD         = 0x04,
        MS5611_LOW_POWER        = 0x02,
        MS5611_ULTRA_LOW_POWER  = 0x00
    } ms5611_osr_t;

  */


  twiSendCommand(0x77, 0x1e); //reset
  delay(100);
  for (byte i = 1; i <= 6; i++)
  {
    unsigned int low, high;
    twiSendCommand(0x77, 0xa0 + i * 2);
    Wire.requestFrom(0x77, 2);
    if (Wire.available() != 2) Serial.println("Error: calibration data not available");
    high = Wire.read();
    low = Wire.read();
    calibrationData[i] = high << 8 | low;
    //Serial.print("calibration data #");
    //Serial.print(i);
    //Serial.print(" = ");
    //Serial.println( calibrationData[i] );
  }
}
void twiSendCommand(byte address, byte command)
{
  Wire.beginTransmission(address);
  if (!Wire.write(command)) Serial.println("Error: write()");
  if (Wire.endTransmission())
  {
    Serial.print("Error when sending command: ");
    Serial.println(command, HEX);
  }
}

void ledOn()
{
  digitalWrite(led, 1);
}

void ledOff()
{
  digitalWrite(led, 0);
}