Subversion Repositories FlightCtrl

Rev

Go to most recent revision | Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
1993 - 1
/*
2
MS561101BA_altitude.pde - Computes altitude from sea level using pressure readings from the sensor.
3
The algorithm uses the Hypsometric formula as explained in http://keisan.casio.com/has10/SpecExec.cgi?path=06000000.Science%2F02100100.Earth%20science%2F12000300.Altitude%20from%20atmospheric%20pressure%2Fdefault.xml&charset=utf-8
4
TODO: The results are not that good. Suggestions on improvements welcome.
5
 
6
Copyright (C) 2011 Fabio Varesano <fvaresano@yahoo.it>
7
 
8
Development of this code has been supported by the Department of Computer Science,
9
Universita' degli Studi di Torino, Italy within the Piemonte Project
10
http://www.piemonte.di.unito.it/
11
 
12
 
13
This program is free software: you can redistribute it and/or modify
14
it under the terms of the version 3 GNU General Public License as
15
published by the Free Software Foundation.
16
 
17
This program is distributed in the hope that it will be useful,
18
but WITHOUT ANY WARRANTY; without even the implied warranty of
19
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
20
GNU General Public License for more details.
21
 
22
You should have received a copy of the GNU General Public License
23
along with this program.  If not, see <http://www.gnu.org/licenses/>.
24
 
25
*/
26
 
27
 
28
//#define DEBUG_V
29
 
30
#include <Wire.h>
31
//#include <DebugUtils.h>
32
#include <MS561101BA.h>
33
 
34
 
35
#define MOVAVG_SIZE 32
36
 
37
MS561101BA baro = MS561101BA();
38
 
39
float movavg_buff[MOVAVG_SIZE];
40
int movavg_i=0;
41
 
42
const float sea_press = 1013.25;
43
float press, temp;
44
 
45
void setup() {
46
  Wire.begin();
47
  Serial.begin(115200);
48
  delay(1000);
49
 
50
  // Suppose that the CSB pin is connected to GND.
51
  // You'll have to check this on your breakout schematics
52
  baro.init(MS561101BA_ADDR_CSB_LOW);
53
  delay(100);
54
 
55
  // populate movavg_buff before starting loop
56
  for(int i=0; i<MOVAVG_SIZE; i++) {
57
    movavg_buff[i] = baro.getPressure(MS561101BA_OSR_4096);
58
  }
59
}
60
 
61
void loop() {
62
  Serial.print(" temp: ");
63
  temp = baro.getTemperature(MS561101BA_OSR_4096);
64
  Serial.print(temp);
65
  Serial.print(" degC pres: ");
66
  pushAvg(baro.getPressure(MS561101BA_OSR_4096));
67
  press = getAvg(movavg_buff, MOVAVG_SIZE);
68
  Serial.print(press);
69
  Serial.print(" mbar altitude: ");
70
  Serial.print(getAltitude(press, temp));
71
  Serial.println(" m");
72
  delay(100);
73
}
74
 
75
float getAltitude(float press, float temp) {
76
  return ((pow((sea_press / press), 1/5.257) - 1.0) * (temp + 273.15)) / 0.0065;
77
}
78
 
79
void pushAvg(float val) {
80
  movavg_buff[movavg_i] = val;
81
  movavg_i = (movavg_i + 1) % MOVAVG_SIZE;
82
}
83
 
84
float getAvg(float * buff, int size) {
85
  float sum = 0.0;
86
  for(int i=0; i<size; i++) {
87
    sum += buff[i];
88
  }
89
  return sum / size;
90
}