Paparazzi UAS  v5.14.0_stable-0-g3f680d1
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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 "subsystems/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 
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] + 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  uint8_t id = DL_IMCU_REMOTE_GROUND_id(lra_msg_buf);
87 
88  if (id < LASER_RANGE_ARRAY_NUM_SENSORS) {
89  float range = DL_IMCU_REMOTE_GROUND_range(lra_msg_buf) / 1000.f;
90  // wait till all sensors received before sending update
91  AbiSendMsgOBSTACLE_DETECTION(OBS_DETECTION_RANGE_ARRAY_ID, range, laser_range_array_orientations[id*2],
93 
94  if (id == agl_id && range > 1e-5 && range < VL53L0_MAX_VAL) {
95  AbiSendMsgAGL(AGL_VL53L0_LASER_ARRAY_ID, range);
96  }
97  }
98  break;
99  }
100  default:
101  break;
102  }
103 }
104 
106 {
107  pprz_check_and_parse(laser_range_array.device, &laser_range_array.transport, lra_msg_buf,
108  &laser_range_array.msg_available);
109 
110  if (laser_range_array.msg_available) {
112  laser_range_array.msg_available = false;
113  }
114 }
void laser_range_array_init(void)
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
bool msg_available
If we received a message.
void laser_range_array_event(void)
Main include for ABI (AirBorneInterface).
struct pprz_transport transport
The transport layer (PPRZ)
#define VL53L0_MAX_VAL
#define AGL_VL53L0_LASER_ARRAY_ID
static uint8_t lra_msg_buf[128]
The message buffer.
static uint8_t agl_id
static struct laser_range_array_t laser_range_array
#define OBS_DETECTION_RANGE_ARRAY_ID
unsigned char uint8_t
Definition: types.h:14
static float laser_range_array_orientations[]
struct link_device * device
The device which is uses for communication.
static void laser_range_array_parse_msg(void)