En esta entrada vamos a ver cómo utilizar el sensor barométrico MS5611 para estimar la altitud respecto a un punto con un error inferior a 10cm. El sensor que utilizo se muestra a continuación:

Comprar MS5611 en Amazon

Sensor MS5611, presión atmosférica de alta precisión

El sensor MS5611 es un sensor barométrico de alta precisión. Proporciona lecturas de la presión atmosférica con una resolución máxima de 0.012mbar (a OSR 4096). Partiendo de este valor, podremos calcular la altitud de forma precisa, ya sea respecto al nivel del mar o respecto del suelo del lugar donde nos encontremos (orientado a control de altitud para drones). La altitud respecto al nivel del mar (o respecto al punto que nosotros queramos) es proporcional a la diferencia de presión atmosférica de los dos puntos, por lo que conociendo el valor de esta podremos estimar la altitud de nuestro drone de forma precisa. A mayor altitud, menor presión atmosférica:

La precisión de este método es mayor de que pueda parecer en un principio, pudiendo llegar a precisiones inferiores a 10cm con el sensor a máxima resolución. La ecuación que utilizaremos para calcular la altitud a partir de las lecturas de presión atmosférica es la siguiente:

Donde:

• presionΟ: presión en el punto de origen (en Pascales). Punto de presión respecto al cual se calculará la altitud. 
• presionActual: presión instantánea (en Pascales)
• Para pasar de mBar a Pascales bastará con dividir el valor en mBar / 1000. 

Como vemos en la siguiente ecuación, con una resolución de 0.012mBar de podemos conseguir una precisión teórica de hasta 8.8cm (aunque luego en la práctica haya más factores que influyen sobre el sensor y hagan que este número sea algo mayor):

Después de esta breve introducción, pasamos directamente a la programación del sensor MS5611 en Arduino.

Os dejo también esta entrada con todo los necesario para estimar la altitud utilizando el sensor HC-SR04 😉

MS5611, programación en Arduino

El conexionado necesario para conectar el censor MS5611 con la placa Arduino Nano se muestra a continuación:

Como hemos hecho con los posts anteriores, voy a explicar paso por paso las diferentes partes del código que utilizo para la estimación de la altitud de vuelo partiendo de los datos del datasheet del sensor MS5611. La lectura de datos del sensor se basa exclusivamente en las ecuaciones proporcionadas por el datasheet. Es necesario seguir una serie de pautas para llegar a obtener medidas de presión atmosférica del sensor. Estas pautas se muestran a continuación (están sacadas del datasheet):

Veamos como traducir estas pautas a código que pueda ser entendido por la placa Arduino.

MS5611 – void setup()

El primer paso es conocer la dirección I2C del sensor, para lo que utilizaremos el siguiente sketch. En mi caso la dirección es 0x77.

Hacemos un reset del sensor enviando la trama 0x1E como indica en datasheet. ‘The Reset sequence shall be sent once after power-on to make sure that the calibration PROM gets loaded into the internal register’:

Wire.begin(); 
Wire.beginTransmission(MS5611); 
Wire.write(0x1E); // Send reset command 
Wire.endTransmission(); 
delay(2);

Leemos de la memoria EPROOM del sensor los valores de calibración que vienen de fábrica. Estas constantes C1, C2… C6 serán utilizados después para calcular la temperatura y la presión reales según las ecuaciones proporcionados por el datasheet. Al ser constantes y no variar con el tiempo, basta con leerlas una vez antes de entrar al loop principal.

for (int i = 0; i < 6; i++) {
  Wire.beginTransmission(MS5611);
  Wire.write(0xA2 + (2 * i));  // Select data register
  Wire.endTransmission();

  Wire.requestFrom(MS5611, 2);
  if (Wire.available() == 2) { // Read 2 bytes of data: Coff msb, Coff lsb
    data[0] = Wire.read();
    data[1] = Wire.read();
  }
  Coff[i] = ((data[0] * 256) + data[1]);
}

El objetivo de utilizar este sensor es poder medir la altitud de vuelo del drone respecto al punto de despegue, es decir, respecto al suelo del lugar del que nos encontremos. Para ello tenemos que medir la presión atmosférica en ese punto. Ese valor de presión será nuestra referencia para calcular la altitud relativa. Para ello, simplemente tomamos 270 muestras de la presión actual y calculamos la media de todas ellas. El valor resultante será nuestro valor de presionΟ:

for (int cal_int = 0; cal_int < 270 ; cal_int ++) {
  delay(10);
  CalcPresion();
  presion0 += presionActual;
}
presion0 = presion0 / 270;

MS5611 – void loop()

El primer paso según las pautas mostradas en el datasheet es leer las variables D1 y D2, que correspondes con la presión atmosférica y la temperatura respectivamente. Estos valores de presión y temperatura son simplemente lecturas sin procesar (y sin compensar) hechas por el sensor. Llamaremos lecturas raw a estas lecturas sin procesar. Más adelante estas lecturas serán compensadas para obtener lecturas en mBar y ºC.

La resolución de las muestras D1 y D2 tomadas por el sensor MS5611 es configurable. Si queremos conseguir una resolución de altitud menos a 10cm, es imprescindible pedir lecturas al sensor a máxima resolución, es decir, a OSR 4096. Para ello, habrá que hacer la petición de datos (o actualización de datos) al sensor según se indica en el datasheet, ‘0x48’ para la variable D1 (presión atmosférica) y ‘0x58’ para la variable D2 (temperatura ambiente):

Si traducimos esto a código Arduino, obtenemos algo como esto:

Wire.beginTransmission(MS5611);
Wire.write(0x48);  // Pedimos al sensor la presion a OSR = 4096
Wire.endTransmission();
delay(10);        // Esperamos 10ms

Wire.beginTransmission(MS5611);
Wire.write(0x58);  // Pedimos al sensor la temperatura a OSR = 4096
Wire.endTransmission();
delay(10);         // Esperamos 10ms

Después de cada petición de datos es necesario esperar un tiempo mínimo (Conversion time de los ADC) hasta que las lecturas estén disponibles para ser leídas. Este tiempo varía en función del OSR a la que hayamos hecho la petición. En nuestro caso este tiempo puede ser de hasta 9.04ms, por lo que operaremos 10ms después de cada petición:

Una vez terminado el tiempo de conversión es hora de leer la presión y temperatura del sensor. Para ello utilizaremos el comando 0x00 como muestra el datasheet:

Wire.beginTransmission(MS5611);
Wire.write(0x58);  // Pedimos al sensor la temperatura a OSR = 4096
Wire.endTransmission();
delay(10);         // Esperamos 10ms
Wire.beginTransmission(MS5611);
Wire.write(0x00);  // Leer ADC
Wire.endTransmission();

Cada lectura de presión (D1) y temperatura (D2) que entrega el sensor está compuesta de 24bit. Si acudimos al datasheet y analizamos la forma en la que es sensor barométrico envía los datos, vemos como la información se envía en 3 ‘paquetes’ de 8bit cada uno (Data 23-16, Data 8-15 y Data7-0, en ese orden). Concatenando adecuadamente estos tres ‘paquetes’ conseguiremos las lecturas raw de presión y temperatura.

Para concatenar los tres ‘paquetes’ de forma correcta será necesario desplazarlos (shift) hacia la izquierda hasta que cada uno de ellos ocupe su lugar en la variable de 24bit que vamos a crear:

11011001 00000000 00000000 ⇒ Desplazamos ‘Data 23-16’, 16 posiciones hacia la izquierda

                      10111101 00000000 ⇒ Desplazamos ‘Data 15-8’, 8 posiciones hacia la izquierda

                                            11101111 ⇒ No desplazamos Data 7-0

Finalmente, concatenando estos tres datos y obtendremos la lectura final de 24bit:

11011001 10111101 11101111

Esta entrada de Arduino reference expone varios conceptos interesantes sobre BitShift que recomiendo que reviséis. En concreto, nos interesa la parte donde dice que desplazar bits hacia la izquierda equivale a elevar el mismo número de desplazamientos a la potencia de dos. Así, desplazar ‘Data 23-16’ 16 posiciones a la izquierda equivale a multiplicar ‘Data 23-16’*2^16, o lo que es lo mismo, ‘Data 23-16’*65.536. Seguimos el mismo procedimiento para desplazar ‘Data 15-8’, 8 posiciones hacia la izquierda,  ‘Data 23-16’*2^8, o ‘Data 23-16’*256. Podéis utilizar la calculadora de Windows en binario y comprobar que esto es realmente así.

D1 = ‘Data 23-16’*65.536 + ‘Data 23-16’*256 + ‘Data 7-0’

D2 = ‘Data 23-16’*65.536 + ‘Data 23-16’*256 + ‘Data 7-0’

Wire.beginTransmission(MS5611);
Wire.write(0x48);  // Pedimos al sensor la presion a OSR = 4096
Wire.endTransmission();
delay(10);  // Esperamos 10ms
Wire.beginTransmission(MS5611);
Wire.write(0x00);   // Leer ADC
Wire.endTransmission();
Wire.requestFrom(MS5611, 3);
if (Wire.available() == 3) {
  dataP[0] = Wire.read();
  dataP[1] = Wire.read();
  dataP[2] = Wire.read();
}

ptemp = ((dataP[0] * 65536.0) + (dataP[1] * 256.0) + dataP[2]); // D1 (presion)

Wire.beginTransmission(MS5611);
Wire.write(0x58);  // Pedimos al sensor la temperatura a OSR = 4096
Wire.endTransmission();
delay(10);         // Esperamos 10ms
Wire.beginTransmission(MS5611);
Wire.write(0x00);  // Leer ADC
Wire.endTransmission();
Wire.requestFrom(MS5611, 3);
if (Wire.available() == 3) {
  data[0] = Wire.read();
  data[1] = Wire.read();
  data[2] = Wire.read();
}

// Calcular la presion segun las ecuaciones del datasheet
temp = ((data[0] * 65536.0) + (data[1] * 256.0) + data[2]);  // D2 (temp)

Existen diferentes formas de hacer BitShift. A continuación, muestro otra de ellas. Utilizando el comando ‘<<’ podemos indicar cuantas posiciones que queremos desplazar los datos hacia la izquierda. Para poder hacerlo de esta forma será necesario declarar las variables D1 y D2 como uint_32 (no existe uint_24) ya que declarándola simplemente como int, Arduino tomará esta variable como una variable de 16bit y no podríamos concatenar los tres ‘paquetes’ de forma correcta. Me he decantado por la primera solución simplemente porque ya tenía declaradas las variables como int. A efectos son equivalentes:

D2  = Wire.read() << 16 | Wire.read() << 8 | Wire.read();

Una vez obtenidas las variables D1 y D2 es necesario procesar adecuadamente estos datos para obtener valores de presión y temperatura compensados. Si os fijáis en el código que he utilizado para ello, lo único que se hace es copiar las ecuaciones de la imagen inferior e ir siguiendo las pautas que se indican. Las constantes C1, C2… C6 se utilizan en este punto para calcular los diferentes parámetros:

dT = temp - ((Coff[4] * 256));
temp = 2000 + (dT * (Coff[5] / pow(2, 23)));
off = Coff[1] * 65536 + (Coff[3] * dT) / 128;
sens = Coff[0] * 32768 + (Coff[2] * dT) / 256;

ptemp = (ptemp * sens / 2097152.00 - off) / 32768.00;
presionActual = ptemp / 100.00;

Por otro lado, para minimizar el error al mínimo posible, se hace una compensación de la lectura de la presión atmosférica en función de la temperatura ambiente (también como se indica en el datasheet):

if (temp >= 2000) {
  Ti = 0;
  offi = 0;
  sensi = 0;
}
else if (temp < 2000) {
  Ti = (dT * dT) / (pow(2, 31));
  offi = 5 * ((pow((temp - 2000), 2))) / 2;
  sensi =  5 * ((pow((temp - 2000), 2))) / 4;
  if (temp < -1500) {
    offi = offi + 7 * ((pow((temp + 1500), 2)));
    sensi = sensi + 11 * ((pow((temp + 1500), 2))) / 2;
  }
}

Finalmente, se calcular la altitud de vuelo con la diferencia de presión atmosférica del drone respecto al punto de presionΟ. La ecuación es la misma a la presentada en la introducción:

Alt_cm = log((presion0 / 100) / (presionActual / 100)) * 723800.3;

Aquí os dejo una imagen de la estimación de la altitud. Simplemente he cogido el sensor y lo he elevado y bajado dos veces a una altura aproximada de medio metro, los resultados como veis son bastante precisos. 

Código completo

Os dejo el código completo para que podáis ejecutarlo vosotros mismos:

#include <Wire.h>
#define MS5611 0x77

unsigned long Coff[6], Ti = 0, offi = 0, sensi = 0;
unsigned int data[3], dataP[3];
float PresionSum, ctemp, presionActual, presion0, Alt_cm, AltFilt, ptempFilt, pressureF;
long ptemp, temp, dT, loop_timer;
long long off, sens;
const int Rd = 25;
float PresionVector[Rd];
int iPres;

void setup() {
  Wire.begin();
  Wire.beginTransmission(MS5611);
  Wire.write(0x1E); // Send reset command
  Wire.endTransmission();
  delay(2);

  pinMode(13, OUTPUT);
  digitalWrite(13, LOW);
  Serial.begin(115200);

  // Leemos los valores de configuracion de la EPROM segun explica el datasheet
  for (int i = 0; i < 6; i++) {
    Wire.beginTransmission(MS5611);
    Wire.write(0xA2 + (2 * i));  // Select data register
    Wire.endTransmission();

    Wire.requestFrom(MS5611, 2);
    if (Wire.available() == 2) { // Read 2 bytes of data: Coff msb, Coff lsb
      data[0] = Wire.read();
      data[1] = Wire.read();
    }
    Coff[i] = ((data[0] * 256) + data[1]);
  }

  // Calibracion de sensor para conocer la presion a nivel del suelo.
  // La altitud será calculada tomando este valor como referencia
  for (int cal_int = 0; cal_int < 270 ; cal_int ++) {
    delay(10);
    CalcPresion();
    presion0 += presionActual;
  }
  presion0 = presion0 / 270;
}

void loop() {
  // Lectura de presion atmosferica del sensor MS5611
  CalcPresion();
  // Con la lectura de presión y el valor de calibracion, calculamos la altitud respecto al suelo.
  Alt_cm = log((presion0 / 100) / (pressureF / 100)) * 723800.3;
  // Filtramos la señal
  AltFilt = AltFilt * 0.92 + Alt_cm * 0.08;

  Serial.println(AltFilt);
}

// RUTINA DE LECTURA DEL SENSOR MS5611
void CalcPresion() {
  Wire.beginTransmission(MS5611);
  Wire.write(0x48);  // Pedimos al sensor la presion a OSR = 4096
  Wire.endTransmission();
  delay(10);  // Esperamos 10ms
  Wire.beginTransmission(MS5611);
  Wire.write(0x00);   // Leer ADC
  Wire.endTransmission();
  Wire.requestFrom(MS5611, 3);
  if (Wire.available() == 3) {
    dataP[0] = Wire.read();
    dataP[1] = Wire.read();
    dataP[2] = Wire.read();
  }

  ptemp = ((dataP[0] * 65536.0) + (dataP[1] * 256.0) + dataP[2]); // D1 (presion)

  Wire.beginTransmission(MS5611);
  Wire.write(0x58);  // Pedimos al sensor la temperatura a OSR = 4096
  Wire.endTransmission();
  delay(10);         // Esperamos 10ms
  Wire.beginTransmission(MS5611);
  Wire.write(0x00);  // Leer ADC
  Wire.endTransmission();
  Wire.requestFrom(MS5611, 3);
  if (Wire.available() == 3) {
    data[0] = Wire.read();
    data[1] = Wire.read();
    data[2] = Wire.read();
  }

  // Calcular la presion segun las ecuaciones del datasheet
  temp = ((data[0] * 65536.0) + (data[1] * 256.0) + data[2]);  // D2 (temp)
  dT = temp - ((Coff[4] * 256));
  temp = 2000 + (dT * (Coff[5] / pow(2, 23)));
  off = Coff[1] * 65536 + (Coff[3] * dT) / 128;
  sens = Coff[0] * 32768 + (Coff[2] * dT) / 256;

  // Compensacion de temperatura
  if (temp >= 2000) {
    Ti = 0;
    offi = 0;
    sensi = 0;
  }
  else if (temp < 2000) {
    Ti = (dT * dT) / (pow(2, 31));
    offi = 5 * ((pow((temp - 2000), 2))) / 2;
    sensi =  5 * ((pow((temp - 2000), 2))) / 4;
    if (temp < -1500) {
      offi = offi + 7 * ((pow((temp + 1500), 2)));
      sensi = sensi + 11 * ((pow((temp + 1500), 2))) / 2;
    }
  }

  temp -= Ti;
  off -= offi;
  sens -= sensi;

  ptemp = (ptemp * sens / 2097152.00 - off) / 32768.00;
  ptempFilt = ptempFilt * 0.8 + ptemp * 0.2;
  presionActual = ptemp / 100.00;
  ctemp = temp / 100.00;

  //  Calculamos la media de los ultimos Rd valores para filtrar la señal
  PresionVector[iPres] = presionActual;
  PresionSum = 0;
  for (int sumVect = 0; sumVect < Rd ; sumVect++) {
    PresionSum += PresionVector[sumVect];
  }
  pressureF = PresionSum / Rd;
  iPres++;
  if (iPres == Rd)iPres = 0;
}

Sin quieres saber como hacer un drone con Arduino paso a paso y desde cero, aquí os dejo todo lo necesario 🙂

Indice ‘Drone con Arduino desde cero’:

1. Conceptos generales sobre drones
2. Material necesario y montaje de los componentes hardware
3. Mando RC y receptor. Programación en Arduino
4. MPU6050 y su programación en Arduino
5. Batería LiPo
6. Control de estabilidad y PID
7. Motores, ESC y su programación en Arduino
8. Calibración de hélices y motores
9. Software completo y esquema detallado
10. Probando el software completo antes de volar
11. Como leer variables de Arduino en Matlab