Paparazzi UAS  v7.0_unstable
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(nav.heading);
128  float cos_heading = cosf(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 
bool obstacle_detected
uint8_t mode
0 = straight, 1 = right, 2 = left, ...
#define LED_ON(i)
Definition: led_hw.h:51
#define LED_OFF(i)
Definition: led_hw.h:52
arch independent LED (Light Emitting Diodes) API
float waypoint_get_alt(uint8_t wp_id)
Get altitude of waypoint in meters (above reference)
Definition: waypoints.c:113
float waypoint_get_x(uint8_t wp_id)
Get X/East coordinate of waypoint in meters.
Definition: waypoints.c:97
float waypoint_get_y(uint8_t wp_id)
Get Y/North coordinate of waypoint in meters.
Definition: waypoints.c:105
void waypoint_set_enu(uint8_t wp_id, struct EnuCoor_f *enu)
Set local ENU waypoint coordinates.
Definition: waypoints.c:177
float y
in meters
float x
in meters
float z
in meters
vector in East North Up coordinates Units: meters
struct RotorcraftNavigation nav
Definition: navigation.c:51
void nav_set_heading_rad(float rad)
Set nav_heading in radians.
Definition: navigation.c:335
Rotorcraft navigation functions.
#define NavSetWaypointHere
Definition: navigation.h:243
float heading
heading setpoint (in radians)
Definition: navigation.h:133
void stereocam_droplet_init(void)
void stereocam_droplet_periodic(void)
#define StereoGetch()
#define StereoChAvailable()
struct link_device * xdev
static void stereo_parse(uint8_t c)
#define STEREO_PORT
struct AvoidNavigationStruct avoid_navigation_data
global VIDEO state
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
float heading
Definition: wedgebug.c:258