Skip to content

Instantly share code, notes, and snippets.

@EDsteve
Created Jun 4, 2017
Embed
What would you like to do?
Vario Paragliding
// Original code by Rolf R Bakke, Oct 2012,
// Modified May 2017 by Thomas Gilson. tbgilson@gmail.com
// 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);
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment