Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
stereocam.c
Go to the documentation of this file.
1/*
2 * Copyright (C) 2015 Kirk Scheper
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, write to
18 * the Free Software Foundation, 59 Temple Place - Suite 330,
19 * Boston, MA 02111-1307, USA.
20 *
21 */
22
28
30
31#include "mcu_periph/uart.h"
33#include "pprzlink/messages.h"
34#include "pprzlink/intermcu_msg.h"
35
36#include "mcu_periph/sys_time.h"
37#include "modules/core/abi.h"
38
40
41
42// forward received image to ground station
43#ifndef FORWARD_IMAGE_DATA
44#define FORWARD_IMAGE_DATA FALSE
45#endif
46
47
48/* This defines the location of the stereocamera with respect to the body fixed coordinates.
49 *
50 * Coordinate system stereocam (image coordinates)
51 * z x
52 * (( * ))----->
53 * | * = arrow pointed into the frame away from you
54 * | y
55 * V
56 *
57 * The conversion order in euler angles is psi (yaw) -> theta (pitch) -> phi (roll)
58 *
59 * Standard rotations: MAV NED body to stereocam in Deg:
60 * - facing forward: 90 -> 0 -> 90
61 * - facing backward: -90 -> 0 -> 90
62 * - facing downward: 90 -> 0 -> 0
63 */
64
65// general stereocam definitions
66#if !defined(STEREO_BODY_TO_STEREO_PHI) || !defined(STEREO_BODY_TO_STEREO_THETA) || !defined(STEREO_BODY_TO_STEREO_PSI)
67#pragma message "STEREO_BODY_TO_STEREO_XXX not defined. Using default Euler rotation angles (0,0,0)"
68#endif
69
70#ifndef STEREO_BODY_TO_STEREO_PHI
71#define STEREO_BODY_TO_STEREO_PHI 0
72#endif
73
74#ifndef STEREO_BODY_TO_STEREO_THETA
75#define STEREO_BODY_TO_STEREO_THETA 0
76#endif
77
78#ifndef STEREO_BODY_TO_STEREO_PSI
79#define STEREO_BODY_TO_STEREO_PSI 0
80#endif
81
83 .device = (&((UART_LINK).device)),
84 .msg_available = false
85};
86static uint8_t stereocam_msg_buf[256] __attribute__((aligned));
87
88#ifndef STEREOCAM_USE_MEDIAN_FILTER
89#define STEREOCAM_USE_MEDIAN_FILTER 0
90#endif
91
94
96{
98 float_rmat_of_eulers(&stereocam.body_to_cam, &euler);
99
100 // Initialize transport protocol
101 pprz_transport_init(&stereocam.transport);
102
104}
105
106/* Parse the InterMCU message */
107static void stereocam_parse_msg(void)
108{
109 uint32_t now_ts = get_sys_time_usec();
110
111 /* Parse the mag-pitot message */
112 uint8_t msg_id = stereocam_msg_buf[1];
113 switch (msg_id) {
114
115 case DL_STEREOCAM_VELOCITY: {
116 static struct FloatVect3 camera_vel;
117
118 float res = (float)DL_STEREOCAM_VELOCITY_resolution(stereocam_msg_buf);
119
120 camera_vel.x = (float)DL_STEREOCAM_VELOCITY_velx(stereocam_msg_buf)/res;
121 camera_vel.y = (float)DL_STEREOCAM_VELOCITY_vely(stereocam_msg_buf)/res;
122 camera_vel.z = (float)DL_STEREOCAM_VELOCITY_velz(stereocam_msg_buf)/res;
123
124 float noise = 1-(float)DL_STEREOCAM_VELOCITY_vRMS(stereocam_msg_buf)/res;
125
126 // Rotate camera frame to body frame
127 struct FloatVect3 body_vel;
128 float_rmat_transp_vmult(&body_vel, &stereocam.body_to_cam, &camera_vel);
129
130 //todo make setting
132 // Use a slight median filter to filter out the large outliers before sending it to state
134 }
135
136 //Send velocities to state
137 AbiSendMsgVELOCITY_ESTIMATE(VEL_STEREOCAM_ID, now_ts,
138 body_vel.x,
139 body_vel.y,
140 body_vel.z,
141 noise,
142 noise,
143 noise
144 );
145
146 // todo activate this after changing optical flow message to be dimentionless instead of in pixels
147 /*
148 static struct FloatVect3 camera_flow;
149
150 float avg_dist = (float)DL_STEREOCAM_VELOCITY_avg_dist(stereocam_msg_buf)/res;
151
152 camera_flow.x = (float)DL_STEREOCAM_VELOCITY_velx(stereocam_msg_buf)/DL_STEREOCAM_VELOCITY_avg_dist(stereocam_msg_buf);
153 camera_flow.y = (float)DL_STEREOCAM_VELOCITY_vely(stereocam_msg_buf)/DL_STEREOCAM_VELOCITY_avg_dist(stereocam_msg_buf);
154 camera_flow.z = (float)DL_STEREOCAM_VELOCITY_velz(stereocam_msg_buf)/DL_STEREOCAM_VELOCITY_avg_dist(stereocam_msg_buf);
155
156 struct FloatVect3 body_flow;
157 float_rmat_transp_vmult(&body_flow, &body_to_stereocam, &camera_flow);
158
159 AbiSendMsgOPTICAL_FLOW(STEREOCAM2STATE_SENDER_ID, now_ts,
160 body_flow.x,
161 body_flow.y,
162 body_flow.z,
163 quality,
164 body_flow.z,
165 avg_dist
166 );
167 */
168 break;
169 }
170
171 case DL_STEREOCAM_ARRAY: {
172#if FORWARD_IMAGE_DATA
173 // forward image to ground station
174 uint8_t type = DL_STEREOCAM_ARRAY_type(stereocam_msg_buf);
175 uint8_t w = DL_STEREOCAM_ARRAY_width(stereocam_msg_buf);
176 uint8_t h = DL_STEREOCAM_ARRAY_height(stereocam_msg_buf);
177 uint8_t nb = DL_STEREOCAM_ARRAY_package_nb(stereocam_msg_buf);
178 uint8_t l = DL_STEREOCAM_ARRAY_image_data_length(stereocam_msg_buf);
179
180 DOWNLINK_SEND_STEREO_IMG(DefaultChannel, DefaultDevice, &type, &w, &h, &nb,
181 l, DL_STEREOCAM_ARRAY_image_data(stereocam_msg_buf));
182#endif
183 break;
184 }
185
186#ifdef STEREOCAM_FOLLOWME
187 // todo is follow me still used?
188 case DL_STEREOCAM_FOLLOW_ME: {
189 follow_me( DL_STEREOCAM_FOLLOW_ME_headingToFollow(stereocam_msg_buf),
190 DL_STEREOCAM_FOLLOW_ME_heightObject(stereocam_msg_buf),
191 DL_STEREOCAM_FOLLOW_ME_distanceToObject(stereocam_msg_buf));
192 break;
193 }
194#endif
195
196 default:
197 break;
198 }
199}
200
201/* We need to wait for incomming messages */
202void stereocam_event(void) {
203 // Check if we got some message from the Magneto or Pitot
204 pprz_check_and_parse(stereocam.device, &stereocam.transport, stereocam_msg_buf, &stereocam.msg_available);
205
206 // If we have a message we should parse it
207 if (stereocam.msg_available) {
209 stereocam.msg_available = false;
210 }
211}
212
213/* Send state to camera to facilitate derotation
214 *
215 */
217{
218 // rotate body angles to camera reference frame
219 static struct FloatEulers cam_angles;
220 float_rmat_mult(&cam_angles, &stereocam.body_to_cam, stateGetNedToBodyEulers_f());
221
222 float agl = 0;//stateGetAgl);
223 pprz_msg_send_STEREOCAM_STATE(&(stereocam.transport.trans_tx), stereocam.device,
224 AC_ID, &(cam_angles.phi), &(cam_angles.theta), &(cam_angles.psi), &agl);
225}
static void h(const real32_T x[7], const real32_T q[4], real32_T y[6])
Main include for ABI (AirBorneInterface).
#define VEL_STEREOCAM_ID
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
float phi
in radians
float theta
in radians
float psi
in radians
#define float_rmat_of_eulers
void float_rmat_mult(struct FloatEulers *rb, const struct FloatRMat *m_a2b, const struct FloatEulers *ra)
rotate angle by rotation matrix.
void float_rmat_transp_vmult(struct FloatVect3 *vb, const struct FloatRMat *m_b2a, const struct FloatVect3 *va)
rotate 3D vector by transposed rotation matrix.
euler angles
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition state.h:1325
#define InitMedianFilterVect3Float(_f, _n)
#define UpdateMedianFilterVect3Float(_f, _v)
#define MEDIAN_DEFAULT_SIZE
void follow_me(uint8_t headingToFollow, uint8_t heightObject, uint8_t distanceToObject)
Definition follow_me.c:62
void stereocam_init(void)
Definition stereocam.c:95
#define STEREO_BODY_TO_STEREO_PHI
Definition stereocam.c:71
struct stereocam_t stereocam
Definition stereocam.c:82
#define STEREO_BODY_TO_STEREO_PSI
Definition stereocam.c:79
#define STEREOCAM_USE_MEDIAN_FILTER
Definition stereocam.c:89
#define STEREO_BODY_TO_STEREO_THETA
Definition stereocam.c:75
struct MedianFilter3Float medianfilter
Definition stereocam.c:93
void state2stereocam(void)
Definition stereocam.c:216
static void stereocam_parse_msg(void)
Definition stereocam.c:107
void stereocam_event(void)
Definition stereocam.c:202
static uint8_t stereocam_msg_buf[256]
The message buffer for the stereocamera.
Definition stereocam.c:86
interface to the TU Delft serial stereocam
bool msg_available
If we received a message.
Definition stereocam.h:38
Architecture independent timing functions.
Periodic telemetry system header (includes downlink utility and generated code).
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.