Paparazzi UAS  v4.2.2_stable-4-gcc32f65
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
infrared_i2c.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 
23 #include "sensors/infrared_i2c.h"
24 #include "estimator.h"
25 
26 // IR I2C definitions
27 #define IR_HOR_I2C_ADDR (0x6C << 1)
28 #define IR_VER_I2C_ADDR (0x68 << 1)
29 #define IR_SAMPLE_RATE_SELECT (0 << 2)
30 #define IR_HOR_OC_BIT (0 << 4)
31 #define IR_VER_OC_BIT (1 << 4)
32 #define IR_HOR_I2C_SELECT_IR1 (0 << 5)
33 #define IR_HOR_I2C_SELECT_IR2 (1 << 5)
34 #define IR_START_CONV (1 << 7)
35 
36 
37 #ifndef IR_I2C_IR1_NEUTRAL
38 #define IR_I2C_IR1_NEUTRAL 0
39 #endif
40 
41 #ifndef IR_I2C_IR2_NEUTRAL
42 #define IR_I2C_IR2_NEUTRAL 0
43 #endif
44 
45 #ifndef IR_I2C_TOP_NEUTRAL
46 #define IR_I2C_TOP_NEUTRAL 0
47 #endif
48 
53 
54 // Local variables
55 #define IR_I2C_IDLE 0
56 #define IR_I2C_READ_IR1 1
57 #define IR_I2C_IR2_SELECTED 2
58 #define IR_I2C_READ_IR2 3
59 #define IR_I2C_IR1_SELECTED 4
60 #define IR_I2C_CONFIGURE_HOR 5
61 
63 
64 #define NO_CONF_WORD 0xff
65 #define ValidConfWord(_x) (_x < 0x4)
66 
67 // I2C structure
68 struct i2c_transaction irh_trans, irv_trans;
69 
70 // Standard infrared implementation
71 void infrared_init(void) {
73 }
74 
75 void infrared_update(void) {
77 }
78 
79 void infrared_event(void) {
81 }
82 
85 void infrared_i2c_init( void ) {
89  ir_i2c_conf_word = IR_I2C_DEFAULT_CONF;
92  irh_trans.status = I2CTransDone;
94 
96 }
97 
98 void infrared_i2c_update( void ) {
99 #if ! (defined SITL || defined HITL)
100  // IR horizontal
101  if (irh_trans.status == I2CTransDone && ir_i2c_hor_status == IR_I2C_IDLE) {
103  irh_trans.buf[0] = ir_i2c_conf_word | IR_HOR_OC_BIT | IR_START_CONV ;
104  I2CTransmit(i2c0, irh_trans, IR_HOR_I2C_ADDR, 1);
106  } else {
107  // Read next values
108  I2CReceive(i2c0, irh_trans, IR_HOR_I2C_ADDR, 3);
111  }
112  }
113  // IR vertical
114  if (irv_trans.status == I2CTransDone) {
118  } else {
119  // Read next values
122  }
123  }
124 #endif /* SITL || HITL */
125 }
126 
127 #define FilterIR(_ir_prev, _ir_next) (((1<<ir_i2c_conf_word)*_ir_prev + _ir_next) / ((1<<ir_i2c_conf_word) + 1))
128 
130 #if ! (defined SITL || defined HITL)
131  irh_trans.status = I2CTransDone;
132  switch (ir_i2c_hor_status) {
133  case IR_I2C_IDLE :
134  break;
135  case IR_I2C_READ_IR1 :
136  if (bit_is_set(irh_trans.buf[2],7)) {
137  I2CReceive(i2c0, irh_trans, IR_HOR_I2C_ADDR, 3);
138  break;
139  }
140  // Read IR1 value
141  int16_t ir1 = (irh_trans.buf[0]<<8) | irh_trans.buf[1];
142  ir1 = ir1 - (IR_I2C_IR1_NEUTRAL << ir_i2c_conf_word);
143  ir_i2c.ir1 = FilterIR(ir_i2c.ir1, ir1);
144  // Select IR2 channel
146  I2CTransmit(i2c0, irh_trans, IR_HOR_I2C_ADDR, 1);
148  break;
149  case IR_I2C_IR2_SELECTED :
150  // IR2 selected, asking for IR2 value
151  I2CReceive(i2c0, irh_trans, IR_HOR_I2C_ADDR, 3);
153  break;
154  case IR_I2C_READ_IR2 :
155  // Read IR2 value
156  if (bit_is_set(irh_trans.buf[2],7)) {
157  I2CReceive(i2c0, irh_trans, IR_HOR_I2C_ADDR, 3);
158  break;
159  }
160  int16_t ir2 = (irh_trans.buf[0]<<8) | irh_trans.buf[1];
161  ir2 = ir2 - (IR_I2C_IR2_NEUTRAL << ir_i2c_conf_word);
162  ir_i2c.ir2 = FilterIR(ir_i2c.ir2, ir2);
163  // Update estimator
165 #ifndef IR_I2C_READ_ONLY
170  }
171 #endif
172  // Select IR1 channel
174  I2CTransmit(i2c0, irh_trans, IR_HOR_I2C_ADDR, 1);
176  break;
177  case IR_I2C_IR1_SELECTED :
178  // End reading cycle
180  break;
181  case IR_I2C_CONFIGURE_HOR :
182  // End conf cycle
185  break;
186  }
187 #endif /* !SITL && !HITL */
188 }
189 
191 #if ! (defined SITL || defined HITL)
193  // Read TOP value
194  if (irv_trans.type == I2CTransRx) {
195  int16_t ir3 = (irv_trans.buf[0]<<8) | irv_trans.buf[1];
196  ir3 = ir3 - (IR_I2C_TOP_NEUTRAL << ir_i2c_conf_word);
197  ir_i2c.ir3 = FilterIR(ir_i2c.ir3, ir3);
199 #ifndef IR_I2C_READ_ONLY
204  }
205 #endif
206  }
207  if (irv_trans.type == I2CTransTx) {
209  }
210 #endif /* !SITL && !HITL */
211 }
212 
#define infrared_i2cEvent()
Definition: infrared_i2c.h:47
struct i2c_transaction irh_trans irv_trans
Definition: infrared_i2c.c:68
#define ValidConfWord(_x)
Definition: infrared_i2c.c:65
void infrared_i2c_init(void)
Initialisation.
Definition: infrared_i2c.c:85
#define IR_I2C_IR2_NEUTRAL
Definition: infrared_i2c.c:42
#define I2CReceive(_p, _t, _s_addr, _len)
Definition: i2c.h:140
#define IR_HOR_I2C_SELECT_IR2
Definition: infrared_i2c.c:33
#define IR_VER_OC_BIT
Definition: infrared_i2c.c:31
#define I2CTransmit(_p, _t, _s_addr, _len)
Definition: i2c.h:148
int16_t ir2
Definition: infrared.h:115
static uint8_t ir_i2c_hor_status
Definition: infrared_i2c.c:62
#define IR_I2C_IR1_SELECTED
Definition: infrared_i2c.c:59
void infrared_init(void)
Definition: infrared_i2c.c:71
struct Infrared_raw ir_i2c
Definition: infrared_i2c.c:49
#define IR_HOR_I2C_SELECT_IR1
Definition: infrared_i2c.c:32
#define FALSE
Definition: imu_chimu.h:141
#define IR_I2C_IR1_NEUTRAL
Definition: infrared_i2c.c:38
#define IR_START_CONV
Definition: infrared_i2c.c:34
void infrared_update(void)
Definition: infrared_i2c.c:75
bool_t ir_i2c_conf_hor_done
Definition: infrared_i2c.c:52
bool_t ir_i2c_data_hor_available
Definition: infrared_i2c.c:50
#define FilterIR(_ir_prev, _ir_next)
Definition: infrared_i2c.c:127
uint8_t ir_i2c_conf_word
Definition: infrared_i2c.c:51
void infrared_struct_init(void)
Initialisation of ir structure.
Definition: infrared.c:40
bool_t ir_i2c_data_ver_available
Definition: infrared_i2c.c:50
bool_t ir_i2c_conf_ver_done
Definition: infrared_i2c.c:52
#define IR_I2C_READ_IR1
Definition: infrared_i2c.c:56
#define IR_I2C_IDLE
Definition: infrared_i2c.c:55
signed short int16_t
Definition: types.h:17
#define UpdateIRValue(_v)
Definition: infrared.h:151
enum I2CTransactionStatus status
Definition: i2c.h:47
#define IR_I2C_READ_IR2
Definition: infrared_i2c.c:58
#define TRUE
Definition: imu_chimu.h:144
void infrared_i2c_update(void)
Definition: infrared_i2c.c:98
volatile uint8_t buf[I2C_BUF_LEN]
Definition: i2c.h:46
unsigned char uint8_t
Definition: types.h:14
void infrared_i2c_ver_event(void)
Definition: infrared_i2c.c:190
#define IR_I2C_CONFIGURE_HOR
Definition: infrared_i2c.c:60
int16_t ir1
Definition: infrared.h:114
State estimation, fusioning sensors.
enum I2CTransactionType type
Definition: i2c.h:42
#define IR_HOR_I2C_ADDR
Definition: infrared_i2c.c:27
Definition: i2c.h:10
#define IR_I2C_TOP_NEUTRAL
Definition: infrared_i2c.c:46
#define IR_HOR_OC_BIT
Definition: infrared_i2c.c:30
Definition: i2c.h:9
#define IR_VER_I2C_ADDR
Definition: infrared_i2c.c:28
#define IR_I2C_IR2_SELECTED
Definition: infrared_i2c.c:57
int16_t ir3
Definition: infrared.h:116
void infrared_i2c_hor_event(void)
Definition: infrared_i2c.c:129
void infrared_event(void)
Definition: infrared_i2c.c:79