Paparazzi UAS  v5.15_devel-109-gee85905
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
px4flow_i2c.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2013 Gautier Hattenberger
3  * 2016 Michal Podhradsky <michal.podhradsky@aggiemail.usu.edu>
4  *
5  * This file is part of paparazzi.
6  *
7  * paparazzi is free software; you can redistribute it and/or modify
8  * it under the terms of the GNU General Public License as published by
9  * the Free Software Foundation; either version 2, or (at your option)
10  * any later version.
11  *
12  * paparazzi is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15  * GNU General Public License for more details.
16  *
17  * You should have received a copy of the GNU General Public License
18  * along with paparazzi; see the file COPYING. If not, write to
19  * the Free Software Foundation, 59 Temple Place - Suite 330,
20  * Boston, MA 02111-1307, USA.
21  *
22  */
23 
31 #include "subsystems/abi.h"
32 #include "filters/median_filter.h"
33 
34 // State interface for rotation compensation
35 #include "state.h"
36 
37 // Messages
38 #include "pprzlink/messages.h"
40 
41 /*
42  * acceptable quality of optical flow (0-min,1-max)
43  * higher threshold means fewer measurements will be available
44  * but they will be more precise. Generally it is probably a better
45  * idea to have lower threshold, and propagate the noise (1-quality)
46  * to the INS
47  */
48 #ifndef PX4FLOW_QUALITY_THRESHOLD
49 #define PX4FLOW_QUALITY_THRESHOLD 0.1
50 #endif
51 
53 
55 
56 #define PX4FLOW_I2C_FRAME 0x0
57 #define PX4FLOW_I2C_INTEGRAL_FRAME 0x16
58 #define PX4FLOW_I2C_FRAME_LENGTH 22
59 #define PX4FLOW_I2C_INTEGRAL_FRAME_LENGTH 25
60 #define PX4FLOW_I2C_ID 0x4D
61 
65 static inline void px4flow_i2c_frame_cb(void)
66 {
67  static float quality = 0;
68  static float noise = 0;
69  uint32_t now_ts = get_sys_time_usec();
70  quality = ((float)px4flow.i2c_frame.qual) / 255.0;
71  noise = px4flow.stddev + (1 - quality) * px4flow.stddev * 10;
72  noise = noise * noise; // square the noise to get variance of the measurement
73 
74  static float timestamp = 0;
75  static uint32_t time_usec = 0;
76 
77  static float flow_comp_m_x = 0.0;
78  static float flow_comp_m_y = 0.0;
79 
80  if (quality > PX4FLOW_QUALITY_THRESHOLD) {
81  timestamp = get_sys_time_float();
82  time_usec = (uint32_t)(timestamp * 1e6);
83 
84  flow_comp_m_x = ((float)px4flow.i2c_frame.flow_comp_m_x) / 1000.0;
85  flow_comp_m_y = ((float)px4flow.i2c_frame.flow_comp_m_y) / 1000.0;
86 
87  // flip the axis (if the PX4FLOW is mounted as shown in
88  // https://pixhawk.org/modules/px4flow
89  AbiSendMsgVELOCITY_ESTIMATE(VEL_PX4FLOW_ID,
90  time_usec,
91  flow_comp_m_y,
92  flow_comp_m_x,
93  0.0f,
94  noise,
95  noise,
96  -1.f);
97  }
98 
99  // distance is always positive - use median filter to remove outliers
100  static int32_t ground_distance = 0;
101  static float ground_distance_float = 0.0;
102 
103  // update filter
105  ground_distance_float = ((float)ground_distance) / 1000.0;
106 
107  // compensate AGL measurement for body rotation
109  float phi = stateGetNedToBodyEulers_f()->phi;
110  float theta = stateGetNedToBodyEulers_f()->theta;
111  float gain = (float)fabs( (double) (cosf(phi) * cosf(theta)));
112  ground_distance_float = ground_distance_float / gain;
113  }
114 
115  if (px4flow.update_agl) {
116  AbiSendMsgAGL(AGL_SONAR_PX4FLOW_ID, now_ts, ground_distance_float);
117  }
118 }
119 
120 
121 
125 static inline void px4flow_i2c_int_frame_cb(void)
126 {
127 
128 }
129 
130 
135 {
137  px4flow.addr = PX4FLOW_I2C_ADDR;
138 #if REQUEST_INT_FRAME
140 #else
142 #endif
143  px4flow.update_agl = USE_PX4FLOW_AGL;
144  px4flow.compensate_rotation = PX4FLOW_COMPENSATE_ROTATION;
145  px4flow.stddev = PX4FLOW_NOISE_STDDEV;
146 
147  init_median_filter_i(&sonar_filter, PX4FLOW_MEDIAN_LENGTH);
148 }
149 
156 {
157  switch (px4flow.status) {
158  case PX4FLOW_FRAME_REQ:
159  // ask for i2c frame
161  if (i2c_transceive(&PX4FLOW_I2C_DEV, &px4flow.trans, px4flow.addr, 1, PX4FLOW_I2C_FRAME_LENGTH)) {
162  // transaction OK, increment status
164  }
165  break;
166  case PX4FLOW_FRAME_REC:
167  // check if the transaction was successful
169  // retrieve data
170  uint8_t idx = 0;
172  idx += sizeof(uint16_t);
174  idx += sizeof(int16_t);
176  idx += sizeof(int16_t);
178  idx += sizeof(int16_t);
180  idx += sizeof(int16_t);
181  px4flow.i2c_frame.qual = (px4flow.trans.buf[idx + 1] << 8 | px4flow.trans.buf[idx]);
182  idx += sizeof(int16_t);
184  idx += sizeof(int16_t);
186  idx += sizeof(int16_t);
188  idx += sizeof(int16_t);
190  idx += sizeof(uint8_t);
192  idx += sizeof(uint8_t);
194 
195  // propagate measurements
197  }
198  // increment status
199  // ask for regular frame again
201  break;
203  // Send the command to begin a measurement.
205  if (i2c_transmit(&PX4FLOW_I2C_DEV, &px4flow.trans, px4flow.addr, 1)) {
206  // transaction OK, increment status
208  }
209  break;
211  // ask for integral frame
214  // transaction OK, increment status
216  }
217  break;
219  // check if the transaction was successful
221  // retrieve data
222  uint8_t idx = 0;
224  idx += sizeof(uint16_t);
226  idx += sizeof(int16_t);
228  idx += sizeof(uint16_t);
230  idx += sizeof(int16_t);
232  idx += sizeof(int16_t);
234  idx += sizeof(int16_t);
235  px4flow.i2c_int_frame.integration_timespan = (px4flow.trans.buf[idx + 3] << 24 | px4flow.trans.buf[idx + 2] << 16
236  | px4flow.trans.buf[idx + 1] << 8 | px4flow.trans.buf[idx]);
237  idx += sizeof(uint32_t);
238  px4flow.i2c_int_frame.sonar_timestamp = (px4flow.trans.buf[idx + 3] << 24 | px4flow.trans.buf[idx + 2] << 16
239  | px4flow.trans.buf[idx + 1] << 8 | px4flow.trans.buf[idx]);
240  idx += sizeof(uint32_t);
242  idx += sizeof(int16_t);
244  idx += sizeof(int16_t);
246 
247  // propagate measurements
249  }
250  // increment status
252  break;
253  default:
254  break;
255  }
256 }
257 
258 
259 
265 {
266  // Convert i2c_frame into PX4FLOW message
267  uint8_t id = PX4FLOW_I2C_ID;
268 
269  float timestamp = get_sys_time_float();
270 
271 #if REQUEST_INT_FRAME
274 
275  float flow_comp_m_x = 0.0;
276  float flow_comp_m_y = 0.0;
277 
278  uint8_t quality = px4flow.i2c_int_frame.qual;
279  float ground_distance = ((float)px4flow.i2c_int_frame.ground_distance) / 1000.0;
280 #else
283 
284  float flow_comp_m_x = ((float)px4flow.i2c_frame.flow_comp_m_x) / 1000.0;
285  float flow_comp_m_y = ((float)px4flow.i2c_frame.flow_comp_m_y) / 1000.0;
286 
287  uint8_t quality = px4flow.i2c_frame.qual;
288  float ground_distance = ((float)px4flow.i2c_frame.ground_distance) / 1000.0;
289 #endif
290 
291  DOWNLINK_SEND_PX4FLOW(DefaultChannel, DefaultDevice,
292  &timestamp,
293  &id,
294  &flow_x,
295  &flow_y,
296  &flow_comp_m_x,
297  &flow_comp_m_y,
298  &quality,
299  &ground_distance);
300 }
uint8_t addr
Definition: px4flow_i2c.h:80
unsigned short uint16_t
Definition: types.h:16
#define PX4FLOW_I2C_INTEGRAL_FRAME_LENGTH
Definition: px4flow_i2c.c:59
static uint32_t idx
int16_t gyro_x_rate
Definition: px4flow_i2c.h:52
float phi
in radians
int16_t flow_comp_m_y
Definition: px4flow_i2c.h:50
volatile uint8_t buf[I2C_BUF_LEN]
Transaction buffer With I2C_BUF_LEN number of bytes.
Definition: i2c.h:122
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
transaction successfully finished by I2C driver
Definition: i2c.h:57
int16_t pixel_flow_y_sum
Definition: px4flow_i2c.h:48
#define PX4FLOW_I2C_FRAME
Definition: px4flow_i2c.c:56
#define AGL_SONAR_PX4FLOW_ID
uint8_t gyro_range
Definition: px4flow_i2c.h:55
Main include for ABI (AirBorneInterface).
#define PX4FLOW_I2C_INTEGRAL_FRAME
Definition: px4flow_i2c.c:57
uint16_t frame_count_since_last_readout
Definition: px4flow_i2c.h:62
struct i2c_transaction trans
Definition: px4flow_i2c.h:79
static int32_t update_median_filter_i(struct MedianFilterInt *filter, int32_t new_data)
Definition: median_filter.h:68
static void px4flow_i2c_int_frame_cb(void)
Propagate itegral frame.
Definition: px4flow_i2c.c:125
static float get_sys_time_float(void)
Get the time in seconds since startup.
Definition: sys_time.h:129
#define PX4FLOW_I2C_ID
Definition: px4flow_i2c.c:60
int16_t gyro_y_rate
Definition: px4flow_i2c.h:53
void px4flow_i2c_init(void)
Initialization function.
Definition: px4flow_i2c.c:134
#define PX4FLOW_I2C_FRAME_LENGTH
Definition: px4flow_i2c.c:58
float theta
in radians
uint8_t sonar_timestamp
Definition: px4flow_i2c.h:56
float stddev
Definition: px4flow_i2c.h:81
transaction set to done by user level
Definition: i2c.h:59
unsigned long uint32_t
Definition: types.h:18
signed short int16_t
Definition: types.h:17
uint16_t frame_count
Definition: px4flow_i2c.h:46
enum Px4FlowStatus status
Definition: px4flow_i2c.h:82
struct px4flow_data px4flow
Definition: px4flow_i2c.c:52
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:328
enum I2CTransactionStatus status
Transaction status.
Definition: i2c.h:126
bool i2c_transmit(struct i2c_periph *p, struct i2c_transaction *t, uint8_t s_addr, uint8_t len)
Submit a write only transaction.
Definition: i2c.c:308
signed long int32_t
Definition: types.h:19
static void px4flow_i2c_frame_cb(void)
Propagate optical flow information.
Definition: px4flow_i2c.c:65
bool update_agl
Definition: px4flow_i2c.h:83
#define VEL_PX4FLOW_ID
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
#define PX4FLOW_QUALITY_THRESHOLD
Definition: px4flow_i2c.c:49
static void init_median_filter_i(struct MedianFilterInt *filter, uint8_t size)
Definition: median_filter.h:39
int16_t flow_comp_m_x
Definition: px4flow_i2c.h:49
struct px4flow_i2c_frame i2c_frame
Definition: px4flow_i2c.h:77
int16_t ground_distance
Definition: px4flow_i2c.h:57
struct MedianFilterInt sonar_filter
Definition: px4flow_i2c.c:54
void px4flow_i2c_downlink(void)
Downlink message for debug Copy volatile variables from the px4_i2c_frame for safety.
Definition: px4flow_i2c.c:264
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:68
struct px4flow_i2c_integral_frame i2c_int_frame
Definition: px4flow_i2c.h:78
void px4flow_i2c_periodic(void)
Poll px4flow for data 152 i2c frames are created per second, so the PX4FLOW can be polled at up to 15...
Definition: px4flow_i2c.c:155
bool compensate_rotation
Definition: px4flow_i2c.h:84
int16_t gyro_z_rate
Definition: px4flow_i2c.h:54
int16_t pixel_flow_x_sum
Definition: px4flow_i2c.h:47