Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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 */
25
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"
37PRINT_CONFIG_VAR(STEREO_UART)
38
39// define coms link for stereocam
40#define STEREO_PORT (&((STEREO_UART).device))
41struct 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
74
76
77
78
79
80
81
82
83static void stereo_parse(uint8_t c);
84static void stereo_parse(uint8_t c)
85{
86 // Protocol is one byte only: store last instance
87 avoid_navigation_data.stereo_bin[0] = c;
88 avoid_navigation_data.timeout = 20;
89}
90
91
93{
94 // Do nothing
95 avoid_navigation_data.mode = 0;
96 avoid_navigation_data.timeout = 0;
97}
99{
100
101 static float heading = 0;
102
103 // Read Serial
104 while (StereoChAvailable()) {
106 }
107
108 if (avoid_navigation_data.timeout <= 0)
109 return;
110
111 avoid_navigation_data.timeout --;
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) ||
120 (avoid_navigation_data.stereo_bin[0] == 100)
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:140
float waypoint_get_x(uint8_t wp_id)
Get X/East coordinate of waypoint in meters.
Definition waypoints.c:124
float waypoint_get_y(uint8_t wp_id)
Get Y/North coordinate of waypoint in meters.
Definition waypoints.c:132
void waypoint_set_enu(uint8_t wp_id, struct EnuCoor_f *enu)
Set local ENU waypoint coordinates.
Definition waypoints.c:204
PRINT_CONFIG_VAR(ONELOOP_ANDI_FILT_CUTOFF)
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:337
Rotorcraft navigation functions.
#define NavSetWaypointHere
Definition navigation.h:244
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.
float heading
Definition wedgebug.c:258