Paparazzi UAS  v5.10_stable-5-g83a0da5-dirty
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
pressure_board_navarro.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2010 ENAC
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  *
21  */
22 
28 #include "pressure_board_navarro.h"
29 #include "subsystems/abi.h"
30 
31 #ifndef USE_AIRSPEED_PBN
32 #if USE_AIRSPEED
33 #define USE_AIRSPEED_PBN TRUE
34 PRINT_CONFIG_MSG("USE_AIRSPEED_PBN automatically set to TRUE")
35 #endif
36 #endif
37 
38 #if USE_AIRSPEED_PBN
39 #include "state.h"
40 #endif
41 
44 #ifndef PBN_I2C_DEV
45 #define PBN_I2C_DEV i2c0
46 #endif
47 
49 #define PBN_I2C_ADDR 0x28
50 
52 #define OFFSET_NBSAMPLES_AVRG 100
53 
55 #define PBN_START_DELAY 30
56 
58 #define PBN_OFFSET_FILTER 7
59 
61 #ifndef PBN_AIRSPEED_SCALE
62 #define PBN_AIRSPEED_SCALE (1./0.54)
63 #endif
64 
66 #ifndef PBN_ALTITUDE_SCALE
67 #define PBN_ALTITUDE_SCALE 0.32
68 #endif
69 
70 #ifndef PBN_PRESSURE_OFFSET
71 #define PBN_PRESSURE_OFFSET 101325.0
72 #endif
73 
74 
75 // Global variables
76 struct PBNState pbn;
78 
81 
82 void pbn_init(void)
83 {
84  startup_delay = PBN_START_DELAY;
85  pbn.altitude_offset = 0;
86  pbn.airspeed_offset = 0;
87  pbn.airspeed_adc = 0;
88  pbn.altitude_adc = 0;
89  pbn.data_valid = true;
90  offset_cnt = OFFSET_NBSAMPLES_AVRG;
91  pbn.airspeed = 0.;
92  pbn.altitude = 0.;
94 }
95 
96 
97 void pbn_periodic(void)
98 {
99 
100  if (startup_delay > 0) {
101  --startup_delay;
102  return;
103  }
104 
105  // Initiate next read
106  pbn_trans.buf[0] = 0;
108 
109 }
110 
111 void pbn_read_event(void)
112 {
113 
115 
116  // Get raw values from buffer
117  pbn.airspeed_adc = ((uint16_t)(pbn_trans.buf[0]) << 8) | (uint16_t)(pbn_trans.buf[1]);
118  pbn.altitude_adc = ((uint16_t)(pbn_trans.buf[2]) << 8) | (uint16_t)(pbn_trans.buf[3]);
119 
120  // Consider 0 as a wrong value
121  if (pbn.airspeed_adc == 0 || pbn.altitude_adc == 0) {
122  pbn.data_valid = false;
123  } else {
124  pbn.data_valid = true;
125 
126  if (offset_cnt > 0) {
127  // IIR filter to compute an initial offset
128 #ifndef PBN_AIRSPEED_OFFSET
130  (PBN_OFFSET_FILTER + 1);
131 #else
132  pbn.airspeed_offset = PBN_AIRSPEED_OFFSET;
133 #endif
134 #ifndef PBN_ALTITUDE_OFFSET
136  (PBN_OFFSET_FILTER + 1);
137 #else
138  pbn.altitude_offset = PBN_ALTITUDE_OFFSET;
139 #endif
140 
141  // decrease init counter
142  --offset_cnt;
143  } else {
144  // Compute pressure
145  float pressure = PBN_ALTITUDE_SCALE * (float) pbn.altitude_adc + PBN_PRESSURE_OFFSET;
146  AbiSendMsgBARO_ABS(BARO_PBN_SENDER_ID, pressure);
147  // Compute airspeed and altitude
148  //pbn_airspeed = (-4.45 + sqrtf(19.84-0.57*(float)(airspeed_offset-airspeed_adc)))/0.28;
150  float tmp_airspeed = sqrtf((float)diff * PBN_AIRSPEED_SCALE);
151  pbn.airspeed = (pbn.airspeed_filter * pbn.airspeed + tmp_airspeed) /
152  (pbn.airspeed_filter + 1.);
153 #if USE_AIRSPEED_PBN
155 #endif
156 
158  }
159 
160  }
161 }
unsigned short uint16_t
Definition: types.h:16
uint16_t altitude_adc
static uint16_t startup_delay
volatile uint8_t buf[I2C_BUF_LEN]
Transaction buffer With I2C_BUF_LEN number of bytes.
Definition: i2c.h:122
#define PBN_START_DELAY
Number of loops before starting to store data.
#define OFFSET_NBSAMPLES_AVRG
Number of values to compute an offset at startup.
#define PBN_OFFSET_FILTER
Weight for offset IIR filter.
Main include for ABI (AirBorneInterface).
#define PBN_I2C_DEV
Default I2C device on tiny is i2c0.
#define PBN_ALTITUDE_SCALE
Linear scale factor for altitude.
transaction set to done by user level
Definition: i2c.h:59
void pbn_read_event(void)
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
#define BARO_PBN_SENDER_ID
struct PBNState pbn
#define PBN_PRESSURE_OFFSET
uint16_t airspeed_offset
bool i2c_transceive(struct i2c_periph *p, struct i2c_transaction *t, uint8_t s_addr, uint8_t len_w, uint16_t len_r)
Submit a write/read transaction.
Definition: i2c.c:278
#define Max(x, y)
Definition: main_fbw.c:53
I2C transaction structure.
Definition: i2c.h:93
enum I2CTransactionStatus status
Transaction status.
Definition: i2c.h:126
static uint16_t offset_cnt
void pbn_init(void)
API to get/set the generic vehicle states.
uint16_t altitude_offset
uint16_t airspeed_adc
static void stateSetAirspeed_f(float airspeed)
Set airspeed (float).
Definition: state.h:1291
struct i2c_transaction pbn_trans
Pressure Board Navarro (2010)
void pbn_periodic(void)
#define PBN_AIRSPEED_SCALE
Quadratic scale factor for airspeed.
#define PBN_I2C_ADDR
Sensor I2C slave address.