Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
laser_range_array.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2017 K. N. McGuire
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, see
18  * <http://www.gnu.org/licenses/>.
19  */
20 
21 /*
22  * @file "modules/range_finder/laser_range_array.c"
23  * @author K. N. McGuire
24  * Reads out values through uart of an laser range ring (array), containing multiple ToF IR laser range modules
25  */
26 
28 
29 #include "generated/airframe.h"
30 
31 #include "pprzlink/pprz_transport.h"
32 #include "pprzlink/intermcu_msg.h"
33 #include "mcu_periph/uart.h"
34 
35 #include "modules/core/abi.h"
36 
37 #include "message_pragmas.h"
38 
39 #include "pprzlink/messages.h"
41 
42 /* Main device strcuture */
44  struct link_device *device;
45  struct pprz_transport transport;
47 };
48 
49 static struct laser_range_array_t laser_range_array = {
50  .device = (&((LASER_RANGE_ARRAY_PORT).device)),
51  .msg_available = false
52 };
53 
54 static uint8_t lra_msg_buf[128] __attribute__((aligned));
55 
56 PRINT_CONFIG_VAR(LASER_RANGE_ARRAY_NUM_SENSORS)
57 static float laser_range_array_orientations[] = LASER_RANGE_ARRAY_ORIENTATIONS;
58 static uint8_t agl_id = 255;
59 
60 #define VL53L0_MAX_VAL 8.191f
61 
63 {
64  // Initialize transport protocol
65  pprz_transport_init(&laser_range_array.transport);
66 
67 #if LASER_RANGE_ARRAY_SEND_AGL
68  // find sensor looking down
69  for (int k = 0; k < LASER_RANGE_ARRAY_NUM_SENSORS; k++) {
70  if (fabsf(laser_range_array_orientations[k * 2] + (float)M_PI_2) < RadOfDeg(5)) {
71  agl_id = k;
72  break;
73  }
74  }
75 #endif
76 }
77 
78 /* Parse the InterMCU message */
79 static void laser_range_array_parse_msg(void)
80 {
81  uint8_t msg_id = lra_msg_buf[1];
82 
83  // Get Time of Flight laser range sensor ring messages
84  switch (msg_id) {
85  case DL_IMCU_REMOTE_GROUND: {
86  uint32_t now_ts = get_sys_time_usec();
87  uint8_t id = DL_IMCU_REMOTE_GROUND_id(lra_msg_buf);
88 
89  if (id < LASER_RANGE_ARRAY_NUM_SENSORS) {
90  float range = DL_IMCU_REMOTE_GROUND_range(lra_msg_buf) / 1000.f;
91  // wait till all sensors received before sending update
92  AbiSendMsgOBSTACLE_DETECTION(OBS_DETECTION_RANGE_ARRAY_ID, range, laser_range_array_orientations[id*2],
94 
95  if (id == agl_id && range > 1e-5 && range < VL53L0_MAX_VAL) {
96  AbiSendMsgAGL(AGL_VL53L0_LASER_ARRAY_ID, now_ts, range);
97  }
98  }
99  break;
100  }
101  default:
102  break;
103  }
104 }
105 
107 {
110 
114  }
115 }
Main include for ABI (AirBorneInterface).
#define AGL_VL53L0_LASER_ARRAY_ID
#define OBS_DETECTION_RANGE_ARRAY_ID
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:71
static void laser_range_array_parse_msg(void)
struct link_device * device
The device which is uses for communication.
static uint8_t agl_id
static float laser_range_array_orientations[]
struct pprz_transport transport
The transport layer (PPRZ)
#define VL53L0_MAX_VAL
void laser_range_array_init(void)
void laser_range_array_event(void)
static uint8_t lra_msg_buf[128]
The message buffer.
static struct laser_range_array_t laser_range_array
bool msg_available
If we received a message.
PRINT_CONFIG_VAR(ONELOOP_ANDI_FILT_CUTOFF)
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98