Paparazzi UAS  v5.15_devel-230-gc96ce27
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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 
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 "subsystems/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 #warning "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 };
86 static 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 
92 #include "filters/median_filter.h"
94 
95 void stereocam_init(void)
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 */
107 static 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 */
202 void 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  */
216 void state2stereocam(void)
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 }
bool msg_available
If we received a message.
Definition: stereocam.h:38
#define VEL_STEREOCAM_ID
#define InitMedianFilterVect3Float(_f, _n)
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
float phi
in radians
#define float_rmat_of_eulers
void float_rmat_mult(struct FloatEulers *rb, struct FloatRMat *m_a2b, struct FloatEulers *ra)
rotate angle by rotation matrix.
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
Periodic telemetry system header (includes downlink utility and generated code).
#define UpdateMedianFilterVect3Float(_f, _v)
Main include for ABI (AirBorneInterface).
float psi
in radians
struct FloatRMat body_to_cam
IMU to stereocam rotation.
Definition: stereocam.h:37
static void stereocam_parse_msg(void)
Definition: stereocam.c:107
#define STEREO_BODY_TO_STEREO_PSI
Definition: stereocam.c:79
euler angles
float theta
in radians
static uint8_t stereocam_msg_buf[256]
The message buffer for the stereocamera.
Definition: stereocam.c:86
struct pprz_transport transport
The transport layer (PPRZ)
Definition: stereocam.h:36
void float_rmat_transp_vmult(struct FloatVect3 *vb, struct FloatRMat *m_b2a, struct FloatVect3 *va)
rotate 3D vector by transposed rotation matrix.
Architecture independent timing functions.
void state2stereocam(void)
Definition: stereocam.c:216
interface to the TU Delft serial stereocam
unsigned long uint32_t
Definition: types.h:18
struct MedianFilter3Float medianfilter
Definition: stereocam.c:93
struct stereocam_t stereocam
Definition: stereocam.c:82
#define STEREOCAM_USE_MEDIAN_FILTER
Definition: stereocam.c:89
struct link_device * device
The device which is uses for communication.
Definition: stereocam.h:35
static void h(const real32_T x[7], const real32_T q[4], real32_T y[6])
unsigned char uint8_t
Definition: types.h:14
void follow_me(uint8_t headingToFollow, uint8_t heightObject, uint8_t distanceToObject)
Definition: follow_me.c:62
#define MEDIAN_DEFAULT_SIZE
Definition: median_filter.h:28
#define STEREO_BODY_TO_STEREO_PHI
Definition: stereocam.c:71
void stereocam_event(void)
Definition: stereocam.c:202
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:68
#define STEREO_BODY_TO_STEREO_THETA
Definition: stereocam.c:75
void stereocam_init(void)
Definition: stereocam.c:95