[paparazzi-commits] [5981] pressure board (total and differential pressure) on I2C

[Gautier Hattenberger]

Revision: 5981
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=5981
Author:   gautier
Date:     2010-09-28 08:33:15 +0000 (Tue, 28 Sep 2010)
Log Message:
-----------
pressure board (total and differential pressure) on I2C

Added Paths:
-----------
    paparazzi3/trunk/conf/modules/pbn.xml
    paparazzi3/trunk/sw/airborne/modules/sensors/pressure_board_navarro.c
    paparazzi3/trunk/sw/airborne/modules/sensors/pressure_board_navarro.h

Added: paparazzi3/trunk/conf/modules/pbn.xml
===================================================================
--- paparazzi3/trunk/conf/modules/pbn.xml                               (rev 0)
+++ paparazzi3/trunk/conf/modules/pbn.xml       2010-09-28 08:33:15 UTC (rev 
5981)
@@ -0,0 +1,29 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+
+<!--
+  
+  Pressure Board Navarro (2010)
+
+  by Matthieu Navarro
+
+  combine differential and absolute pressure sensor (ETS raw sensors)
+  controlled by a dspic
+  return scaled values over I2C
+
+-->
+
+<module name="pbn" dir="sensors">
+  
+  <header>
+    <file name="pressure_board_navarro.h"/>
+  </header>
+  <init fun="pbn_init()"/>
+  <periodic fun="pbn_periodic()" freq="10"/>
+  <event fun="PbnEvent()"/>
+
+  <makefile target="ap"> 
+    <file name="pressure_board_navarro.c"/> 
+  </makefile>
+
+</module>
+

Added: paparazzi3/trunk/sw/airborne/modules/sensors/pressure_board_navarro.c
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/sensors/pressure_board_navarro.c       
                        (rev 0)
+++ paparazzi3/trunk/sw/airborne/modules/sensors/pressure_board_navarro.c       
2010-09-28 08:33:15 UTC (rev 5981)
@@ -0,0 +1,129 @@
+/*
+ * Copyright (C) 2010 ENAC
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+/*
+ * Pressure Board Navarro
+ */
+
+
+#include "pressure_board_navarro.h"
+#include "estimator.h"
+
+/* Default I2C device on tiny is i2c0
+ */
+#ifndef PBN_I2C_DEVICE
+#define PBN_I2C_DEVICE i2c0
+#endif
+
+/* Sensor I2C slave address */
+#define PBN_I2C_ADDR 0x28
+
+/* Number of values to compute an offset at startup */
+#define OFFSET_NBSAMPLES_AVRG 100
+
+/* Number of loops before starting to store data */
+#define PBN_START_DELAY 30
+
+/* Weight for offset IIR filter */
+#define PBN_OFFSET_FILTER 7
+
+
+// Global variables
+uint16_t altitude_adc;
+uint16_t airspeed_adc;
+bool_t data_valid;
+struct i2c_transaction pbn_trans;
+
+
+uint32_t airspeed_offset_tmp;
+uint32_t altitude_offset_tmp;
+uint16_t offset_cnt;
+uint16_t altitude_offset;
+uint16_t airspeed_offset;
+float pbn_altitude;
+float pbn_airspeed;
+uint16_t startup_delay;
+
+void pbn_init( void ) {
+  startup_delay = PBN_START_DELAY;
+  altitude_offset = 0;
+  airspeed_offset = 0;
+  airspeed_adc = 0;
+  altitude_adc = 0;
+  data_valid = TRUE;
+  offset_cnt = OFFSET_NBSAMPLES_AVRG;
+  pbn_airspeed = 0.;
+  pbn_altitude = 0.;
+}
+
+
+void pbn_periodic( void ) {
+
+  if ( startup_delay > 0 ) {
+    --startup_delay;
+    return;
+  }
+
+  // Initiate next read  
+  pbn_trans.buf[0] = 0;
+  I2CTransceive(PBN_I2C_DEVICE, pbn_trans, PBN_I2C_ADDR, 1, 4);
+
+}
+
+void pbn_read_event( void ) {
+
+  pbn_trans.status = I2CTransDone;
+
+  // Get raw values from buffer
+  airspeed_adc = ((uint16_t)(pbn_trans.buf[0]) << 8) | 
(uint16_t)(pbn_trans.buf[1]);
+  altitude_adc = ((uint16_t)(pbn_trans.buf[2]) << 8) | 
(uint16_t)(pbn_trans.buf[3]);
+
+  // Consider 0 as a wrong value
+  if (airspeed_adc == 0 || altitude_adc == 0) {
+    data_valid = FALSE;
+  }
+  else {
+    data_valid = TRUE;   
+
+    if (offset_cnt > 0) {
+      // IIR filter to compute an initial offset
+      airspeed_offset = (PBN_OFFSET_FILTER * airspeed_offset + airspeed_adc) / 
(PBN_OFFSET_FILTER + 1);
+      altitude_offset = (PBN_OFFSET_FILTER * altitude_offset + altitude_adc) / 
(PBN_OFFSET_FILTER + 1);
+
+      // decrease init counter
+      --offset_cnt;
+    }
+    else {
+      // Compute airspeed and altitude
+      pbn_airspeed = (-4.45 + 
sqrtf(19.84-0.57*(float)(airspeed_offset-airspeed_adc)))/0.28;
+      pbn_altitude = 0.32*(float)(altitude_adc-altitude_offset);
+
+      //estimator_airspeed = (7*estimator_airspeed + pbn_airspeed ) / 8;
+      //estimator_airspeed = Max(estimator_airspeed, 0.);
+      //EstimatorSetAirspeed(pbn_airspeed);
+      //alt_kalman(pbn_altitude);
+
+    }
+
+  }
+}
+

Added: paparazzi3/trunk/sw/airborne/modules/sensors/pressure_board_navarro.h
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/sensors/pressure_board_navarro.h       
                        (rev 0)
+++ paparazzi3/trunk/sw/airborne/modules/sensors/pressure_board_navarro.h       
2010-09-28 08:33:15 UTC (rev 5981)
@@ -0,0 +1,58 @@
+/*
+ * Copyright (C) 2010 ENAC
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+
+/*
+ * Pressure Board Navarro (2010)
+ *
+ * by Matthieu Navarro
+ *
+ * combine differential and absolute pressure sensor (ETS raw sensors)
+ * controlled by a dspic
+ * return scaled values over I2C
+ *
+ */
+
+
+#ifndef PRESSURE_BOARD_NAVARRO_H
+#define PRESSURE_BOARD_NAVARRO_H
+
+#include "std.h"
+#include "i2c.h"
+
+extern uint16_t altitude_adc;
+extern uint16_t airspeed_adc;
+extern uint16_t altitude_offset;
+extern uint16_t airspeed_offset;
+extern float pbn_altitude, pbn_airspeed;
+
+extern bool_t data_valid;
+extern struct i2c_transaction pbn_trans;
+
+extern void pbn_init( void );
+extern void pbn_periodic( void );
+extern void pbn_read_event( void );
+
+#define PbnEvent() { if (pbn_trans.status == I2CTransSuccess) 
pbn_read_event(); }
+
+#define PERIODIC_SEND_PBN(_chan) 
DOWNLINK_SEND_PBN(DefaultChannel,&airspeed_adc,&altitude_adc,&pbn_airspeed,&pbn_altitude,&airspeed_offset,&altitude_offset);
+
+#endif // PRESSURE_BOARD_NAVARRO_H