Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
stereocam_droplet.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) C. DW
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  */
27 
28 // Know waypoint numbers and blocks
29 #include "generated/flight_plan.h"
30 
32 
33 
34 
35 // Serial Port
36 #include "mcu_periph/uart.h"
37 PRINT_CONFIG_VAR(STEREO_UART)
38 
39 // define coms link for stereocam
40 #define STEREO_PORT (&((STEREO_UART).device))
41 struct link_device *xdev = STEREO_PORT;
42 
43 #define StereoGetch() STEREO_PORT ->get_byte(STEREO_PORT->periph)
44 #define StereoSend1(c) STEREO_PORT->put_byte(STEREO_PORT->periph, 0, c)
45 #define StereoUartSend1(c) StereoSend1(c)
46 #define StereoSend(_dat,_len) { for (uint8_t i = 0; i< (_len); i++) StereoSend1(_dat[i]); };
47 #define StereoUartSetBaudrate(_b) uart_periph_set_baudrate(STEREO_PORT, _b);
48 #define StereoChAvailable()(xdev->char_available(xdev->periph))
49 
50 
51 
52 
53 
54 
55 // Downlink
56 #ifndef DOWNLINK_DEVICE
57 #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
58 #endif
59 #include "pprzlink/messages.h"
61 
62 #include "led.h"
63 
64 
65 
66 
67 
68 // Module data
73 };
74 
76 
77 
78 
79 
80 
81 
82 
83 static void stereo_parse(uint8_t c);
84 static void stereo_parse(uint8_t c)
85 {
86  // Protocol is one byte only: store last instance
89 }
90 
91 
93 {
94  // Do nothing
97 }
99 {
100 
101  static float heading = 0;
102 
103  // Read Serial
104  while (StereoChAvailable()) {
106  }
107 
109  return;
110 
112 
113  // Results
114  DOWNLINK_SEND_PAYLOAD(DefaultChannel, DefaultDevice, 1, avoid_navigation_data.stereo_bin);
115 
116  volatile bool once = true;
117  // Move waypoint with constant speed in current direction
118  if (
119  (avoid_navigation_data.stereo_bin[0] == 97) ||
121  ) {
122  once = true;
123  struct EnuCoor_f enu;
124  enu.x = waypoint_get_x(WP_GOAL);
125  enu.y = waypoint_get_y(WP_GOAL);
126  enu.z = waypoint_get_alt(WP_GOAL);
127  float sin_heading = sinf(ANGLE_FLOAT_OF_BFP(nav_heading));
128  float cos_heading = cosf(ANGLE_FLOAT_OF_BFP(nav_heading));
129  enu.x += (sin_heading * 1.3 / 20);
130  enu.y += (cos_heading * 1.3 / 20);
131  waypoint_set_enu(WP_GOAL, &enu);
132  } else if (avoid_navigation_data.stereo_bin[0] == 98) {
133  // STOP!!!
134  if (once) {
135  NavSetWaypointHere(WP_GOAL);
136  once = false;
137  }
138  } else {
139  once = true;
140  }
141 
142 
143  switch (avoid_navigation_data.stereo_bin[0]) {
144  case 99: // Turn
145  heading += 4;
146  if (heading > 360) { heading = 0; }
147  nav_set_heading_rad(RadOfDeg(heading));
148  break;
149  default: // do nothing
150  break;
151  }
152 
153 #ifdef STEREO_LED
154  if (obstacle_detected) {
155  LED_ON(STEREO_LED);
156  } else {
157  LED_OFF(STEREO_LED);
158  }
159 #endif
160 
161 }
162 
163 
164 
obstacle_detected
bool obstacle_detected
Definition: avoid_navigation.c:54
c
VIC slots used for the LPC2148 define name e g gps UART1_VIC_SLOT e g modem SPI1_VIC_SLOT SPI1 in mcu_periph spi_arch c or spi_slave_hs_arch c(and some others not using the SPI peripheral yet..) I2C0_VIC_SLOT 8 mcu_periph/i2c_arch.c I2C1_VIC_SLOT 9 mcu_periph/i2c_arch.c USB_VIC_SLOT 10 usb
StereoGetch
#define StereoGetch()
Definition: stereocam_droplet.c:43
waypoint_set_enu
void waypoint_set_enu(uint8_t wp_id, struct EnuCoor_f *enu)
Set local ENU waypoint coordinates.
Definition: waypoints.c:113
AvoidNavigationStruct::timeout
uint8_t timeout
Definition: stereocam_droplet.c:72
ANGLE_FLOAT_OF_BFP
#define ANGLE_FLOAT_OF_BFP(_ai)
Definition: pprz_algebra_int.h:211
LED_OFF
#define LED_OFF(i)
Definition: led_hw.h:52
STEREO_PORT
#define STEREO_PORT
Definition: stereocam_droplet.c:40
StereoChAvailable
#define StereoChAvailable()
Definition: stereocam_droplet.c:48
stereocam_droplet.h
stereo_parse
static void stereo_parse(uint8_t c)
Definition: stereocam_droplet.c:84
waypoint_get_alt
float waypoint_get_alt(uint8_t wp_id)
Get altitude of waypoint in meters (above reference)
Definition: waypoints.c:93
LED_ON
#define LED_ON(i)
Definition: led_hw.h:51
waypoint_get_y
float waypoint_get_y(uint8_t wp_id)
Get Y/North coordinate of waypoint in meters.
Definition: waypoints.c:85
nav_heading
int32_t nav_heading
with INT32_ANGLE_FRAC
Definition: navigation.c:108
EnuCoor_f::y
float y
in meters
Definition: pprz_geodetic_float.h:74
EnuCoor_f::z
float z
in meters
Definition: pprz_geodetic_float.h:75
avoid_navigation_data
struct AvoidNavigationStruct avoid_navigation_data
global VIDEO state
Definition: stereocam_droplet.c:75
AvoidNavigationStruct
Definition: stereocam_droplet.c:69
uart.h
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
AvoidNavigationStruct::stereo_bin
uint8_t stereo_bin[8]
Definition: stereocam_droplet.c:71
waypoint_get_x
float waypoint_get_x(uint8_t wp_id)
Get X/East coordinate of waypoint in meters.
Definition: waypoints.c:77
uint8_t
unsigned char uint8_t
Definition: types.h:14
stereocam_droplet_periodic
void stereocam_droplet_periodic(void)
Definition: stereocam_droplet.c:98
EnuCoor_f
vector in East North Up coordinates Units: meters
Definition: pprz_geodetic_float.h:72
led.h
arch independent LED (Light Emitting Diodes) API
AvoidNavigationStruct::mode
uint8_t mode
0 = straight, 1 = right, 2 = left, ...
Definition: stereocam_droplet.c:70
xdev
struct link_device * xdev
Definition: stereocam_droplet.c:41
navigation.h
EnuCoor_f::x
float x
in meters
Definition: pprz_geodetic_float.h:73
stereocam_droplet_init
void stereocam_droplet_init(void)
Definition: stereocam_droplet.c:92
nav_set_heading_rad
void nav_set_heading_rad(float rad)
Set nav_heading in radians.
Definition: navigation.c:489
NavSetWaypointHere
#define NavSetWaypointHere
Definition: navigation.h:165
heading
float heading
Definition: wedgebug.c:258