Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
lidar_sf11.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2017 Michal Podhradsky <michal.podhradsky@aggiemail.usu.edu>
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 
27 #include "subsystems/abi.h"
28 #include "filters/median_filter.h"
29 
30 // State interface for rotation compensation
31 #include "state.h"
32 
33 // Messages
34 #include "pprzlink/messages.h"
36 
39 
40 
44 void lidar_sf11_init(void)
45 {
47  lidar_sf11.addr = LIDAR_SF11_I2C_ADDR;
49  lidar_sf11.update_agl = USE_LIDAR_SF11_AGL;
50  lidar_sf11.compensate_rotation = LIDAR_SF11_COMPENSATE_ROTATION;
51 
52  lidar_sf11.distance = 0;
54 
56 }
57 
62 void lidar_sf11_event(void)
63 {
64  switch (lidar_sf11.trans.status) {
65  case I2CTransPending:
66  // wait and do nothing
67  break;
68  case I2CTransRunning:
69  // wait and do nothing
70  break;
71  case I2CTransSuccess:
72  // set to done
74  // increment status
76  break;
77  case I2CTransFailed:
78  // set to done
80  break;
81  case I2CTransDone:
82  // do nothing
83  break;
84  default:
85  break;
86  }
87 }
88 
93 {
94  switch (lidar_sf11.status) {
96  // read two bytes
97  lidar_sf11.trans.buf[0] = 0; // set tx to zero
98  i2c_transceive(&LIDAR_SF11_I2C_DEV, &lidar_sf11.trans, lidar_sf11.addr, 1, 2);
99  break;
100  case LIDAR_SF11_READ_OK: {
101  // process results
102  // filter data
103  uint32_t now_ts = get_sys_time_usec();
106  (uint32_t)((lidar_sf11.trans.buf[0] << 8) | lidar_sf11.trans.buf[1]));
107  lidar_sf11.distance = ((float)lidar_sf11.distance_raw)/100.0;
108 
109  // compensate AGL measurement for body rotation
111  float phi = stateGetNedToBodyEulers_f()->phi;
112  float theta = stateGetNedToBodyEulers_f()->theta;
113  float gain = (float)fabs( (double) (cosf(phi) * cosf(theta)));
115  }
116 
117  // send message (if requested)
118  if (lidar_sf11.update_agl) {
119  AbiSendMsgAGL(AGL_LIDAR_SF11_ID, now_ts, lidar_sf11.distance);
120  }
121 
122  // reset status
124  break;
125  }
126  default:
127  break;
128  }
129 }
130 
135 {
136  uint8_t trans = lidar_sf11.trans.status;
138  DOWNLINK_SEND_LIDAR(DefaultChannel, DefaultDevice,
140  &status,
141  &trans);
142 }
LIDAR_SF11_REQ_READ
@ LIDAR_SF11_REQ_READ
Definition: lidar_sf11.h:33
i2c_transaction::buf
volatile uint8_t buf[I2C_BUF_LEN]
Transaction buffer With I2C_BUF_LEN number of bytes.
Definition: i2c.h:122
LIDAR_SF11_READ_OK
@ LIDAR_SF11_READ_OK
Definition: lidar_sf11.h:34
LidarSF11::addr
uint8_t addr
Definition: lidar_sf11.h:40
abi.h
status
uint8_t status
Definition: nps_radio_control_spektrum.c:101
LidarSF11::update_agl
bool update_agl
Definition: lidar_sf11.h:44
lidar_sf11_downlink
void lidar_sf11_downlink(void)
Downlink message for debug.
Definition: lidar_sf11.c:134
lidar_sf11_filter
struct MedianFilterInt lidar_sf11_filter
Definition: lidar_sf11.c:38
lidar_sf11
struct LidarSF11 lidar_sf11
Definition: lidar_sf11.c:37
stateGetNedToBodyEulers_f
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
I2CTransFailed
@ I2CTransFailed
transaction failed
Definition: i2c.h:58
uint32_t
unsigned long uint32_t
Definition: types.h:18
LidarSF11::distance
float distance
Definition: lidar_sf11.h:42
LidarSF11::status
enum LidarSF11Status status
Definition: lidar_sf11.h:43
FloatEulers::theta
float theta
in radians
Definition: pprz_algebra_float.h:86
I2CTransSuccess
@ I2CTransSuccess
transaction successfully finished by I2C driver
Definition: i2c.h:57
get_sys_time_usec
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:68
LidarSF11::compensate_rotation
bool compensate_rotation
Definition: lidar_sf11.h:45
FloatEulers::phi
float phi
in radians
Definition: pprz_algebra_float.h:85
LidarSF11::distance_raw
uint32_t distance_raw
Definition: lidar_sf11.h:41
i2c_transceive
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:344
I2CTransRunning
@ I2CTransRunning
transaction is currently ongoing
Definition: i2c.h:56
lidar_sf11_periodic
void lidar_sf11_periodic(void)
Poll lidar for data.
Definition: lidar_sf11.c:92
uint8_t
unsigned char uint8_t
Definition: types.h:14
i2c_transaction::status
enum I2CTransactionStatus status
Transaction status.
Definition: i2c.h:126
update_median_filter_i
static int32_t update_median_filter_i(struct MedianFilterInt *filter, int32_t new_data)
Definition: median_filter.h:68
lidar_sf11_init
void lidar_sf11_init(void)
Initialization function.
Definition: lidar_sf11.c:44
median_filter.h
lidar_sf11_event
void lidar_sf11_event(void)
Lidar event function Check if the transaction succeded before reading the result.
Definition: lidar_sf11.c:62
MEDIAN_DEFAULT_SIZE
#define MEDIAN_DEFAULT_SIZE
Definition: median_filter.h:28
MedianFilterInt
Definition: median_filter.h:33
I2CTransPending
@ I2CTransPending
transaction is pending in queue
Definition: i2c.h:55
I2CTransDone
@ I2CTransDone
transaction set to done by user level
Definition: i2c.h:59
init_median_filter_i
static void init_median_filter_i(struct MedianFilterInt *filter, uint8_t size)
Definition: median_filter.h:39
lidar_sf11.h
driver for the Parallax SF11-A/B/C Laser Rangefinder connected over i2c bus.
state.h
LidarSF11::trans
struct i2c_transaction trans
Definition: lidar_sf11.h:39
AGL_LIDAR_SF11_ID
#define AGL_LIDAR_SF11_ID
Definition: abi_sender_ids.h:157
LidarSF11
Definition: lidar_sf11.h:37