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
cv_target_localization.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2019 Gautier Hattenberger <gautier.hattenberger@enac.fr>
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  */
20 
30 #include "std.h"
33 #include "state.h"
34 #include "subsystems/abi.h"
37 #include "generated/flight_plan.h"
38 
39 // Default parameters
40 // Camera is looking down and is placed at the center of the frame
41 // With cam X axis pointing to the right, Y down and Z forward of image frame,
42 // the camera is just rotated of pi/2 around body Z axis
43 
44 #ifndef TARGET_LOC_BODY_TO_CAM_PHI
45 #define TARGET_LOC_BODY_TO_CAM_PHI 0.f
46 #endif
47 
48 #ifndef TARGET_LOC_BODY_TO_CAM_THETA
49 #define TARGET_LOC_BODY_TO_CAM_THETA 0.f
50 #endif
51 
52 #ifndef TARGET_LOC_BODY_TO_CAM_PSI
53 #define TARGET_LOC_BODY_TO_CAM_PSI M_PI_2
54 #endif
55 
56 #ifndef TARGET_LOC_CAM_POS_X
57 #define TARGET_LOC_CAM_POS_X 0.f
58 #endif
59 
60 #ifndef TARGET_LOC_CAM_POS_Y
61 #define TARGET_LOC_CAM_POS_Y 0.f
62 #endif
63 
64 #ifndef TARGET_LOC_CAM_POS_Z
65 #define TARGET_LOC_CAM_POS_Z 0.f
66 #endif
67 
68 // Convert pixel unit to m in image plane on x axis
69 // from "mm" to meter by default
70 #ifndef TARGET_LOC_PIXEL_TO_IMAGE_X
71 #define TARGET_LOC_PIXEL_TO_IMAGE_X (1.f / 1000.f)
72 #endif
73 
74 // Convert pixel unit to m in image plane on y axis (same as x by default)
75 #ifndef TARGET_LOC_PIXEL_TO_IMAGE_Y
76 #define TARGET_LOC_PIXEL_TO_IMAGE_Y TARGET_LOC_PIXEL_TO_IMAGE_X
77 #endif
78 
79 // Detection and target
80 struct target_loc_t {
83 
86 
87  struct NedCoor_f target;
88  struct LlaCoor_f pos_lla;
89 
90  bool valid;
92 };
93 
94 static struct target_loc_t target_loc;
95 
96 // external settings
97 
98 // Report current position with a given mark id
100 // Direct waypoint update
102 
103 // association table between marks and waypoints for direct update
104 #ifndef TARGET_LOC_WP_T1_ID
105 #define TARGET_LOC_WP_T1_ID 1
106 #endif
107 #ifndef TARGET_LOC_WP_T2_ID
108 #define TARGET_LOC_WP_T2_ID 2
109 #endif
110 #ifndef TARGET_LOC_WP_T3_ID
111 #define TARGET_LOC_WP_T3_ID 3
112 #endif
114 #ifdef TARGET_LOC_WP_T1
115  { TARGET_LOC_WP_T1_ID, TARGET_LOC_WP_T1 },
116 #endif
117 #ifdef TARGET_LOC_WP_T2
118  { TARGET_LOC_WP_T2_ID, TARGET_LOC_WP_T2 },
119 #endif
120 #ifdef TARGET_LOC_WP_T3
121  { TARGET_LOC_WP_T3_ID, TARGET_LOC_WP_T3 },
122 #endif
123  { 0, 0 } // end of list (0 reserved)
124 };
125 
126 // Abi bindings
127 #ifndef TARGET_LOC_ID
128 #define TARGET_LOC_ID ABI_BROADCAST
129 #endif
130 
132 
133 static void detection_cb(uint8_t sender_id UNUSED,
134  int16_t pixel_x, int16_t pixel_y,
135  int16_t pixel_width UNUSED, int16_t pixel_height UNUSED,
136  int32_t quality UNUSED, int16_t extra)
137 {
138  target_loc.px = pixel_x;
139  target_loc.py = pixel_y;
140 
141  // Prepare rotation matrices
142  struct FloatRMat *ltp_to_body_rmat = stateGetNedToBodyRMat_f();
143  struct FloatRMat ltp_to_cam_rmat;
144  float_rmat_comp(&ltp_to_cam_rmat, ltp_to_body_rmat, &target_loc.body_to_cam);
145  // Prepare cam world position
146  // C_w = P_w + R_w2b * C_b
147  struct FloatVect3 cam_pos_ltp;
148  float_rmat_vmult(&cam_pos_ltp, ltp_to_body_rmat, &target_loc.cam_pos);
149  VECT3_ADD(cam_pos_ltp, *stateGetPositionNed_f());
150 
151  // Compute target position here (pixels in "mm" to meters)
152  struct FloatVect3 target_img = {
155  .z = 1.f
156  };
157  struct FloatVect3 tmp; // before scale factor
158  float_rmat_transp_vmult(&tmp, &ltp_to_cam_rmat, &target_img); // R^-1 * v_img
159 
160  if (fabsf(tmp.z) > 0.1f) {
161  float scale = fabsf(cam_pos_ltp.z / tmp.z); // scale factor
162  VECT3_SUM_SCALED(target_loc.target, cam_pos_ltp, tmp, scale); // T_w = C_w + s*tmp
163  // now, T_w.z should be equal to zero as it is assumed that the target is on a flat ground
164  // compute absolute position
165  struct EcefCoor_f target_ecef;
167  lla_of_ecef_f(&target_loc.pos_lla, &target_ecef);
168 
169  target_loc.type = (uint8_t) extra; // use 'extra' field to encode the type of target
170  target_loc.valid = true;
172  // look for waypoint to update
173  uint8_t i = 0;
174  while (target_loc_wp_tab[i][0] != 0) {
175  if (target_loc_wp_tab[i][0] == target_loc.type) {
176  // update WP (ENU) from target (NED)
178  }
179  i++;
180  }
181  }
182  }
183  else {
184  // if too close from ground, don't do anything
185  target_loc.valid = false;
186  }
187 
188 }
189 
191 {
192  // Init struct
193  target_loc.px = 0;
194  target_loc.py = 0;
195 
196  struct FloatEulers euler = {
200  };
206 
208 
209  target_loc.valid = false;
210 
213 
214  // Bind to ABI message
215  AbiBindMsgVISUAL_DETECTION(TARGET_LOC_ID, &detection_ev, detection_cb);
216 }
217 
219 {
220  // report at fixed frequency if a valid detection occured
221  // this is to prevent telemetry overflow, but might lead to missing
222  // detection if several marks are found between to reports
223  if (target_loc.valid) {
224  float lat_deg = DegOfRad(target_loc.pos_lla.lat);
225  float lon_deg = DegOfRad(target_loc.pos_lla.lon);
226  DOWNLINK_SEND_MARK(DefaultChannel, DefaultDevice, &target_loc.type,
227  &lat_deg, &lon_deg);
228  target_loc.valid = false;
229  }
230 }
231 
233 {
234  // report current position as a given mark
235  // mostly for testing
237  struct LlaCoor_f *pos = stateGetPositionLla_f();
238  float lat_deg = DegOfRad(pos->lat);
239  float lon_deg = DegOfRad(pos->lon);
241  &lat_deg, &lon_deg);
242 }
243 
Event structure to store callbacks in a linked list.
Definition: abi_common.h:65
#define VECT3_ADD(_a, _b)
Definition: pprz_algebra.h:147
uint8_t type
Type of target.
float y
in meters
bool target_localization_update_wp
#define float_rmat_of_eulers
#define TARGET_LOC_WP_T2_ID
vector in EarthCenteredEarthFixed coordinates
struct LtpDef_f ned_origin_f
Definition of the local (flat earth) coordinate system.
Definition: state.h:220
void ecef_of_ned_point_f(struct EcefCoor_f *ecef, struct LtpDef_f *def, struct NedCoor_f *ned)
#define POS_BFP_OF_REAL(_af)
uint8_t last_wp UNUSED
Definition: navigation.c:92
#define VECT3_ASSIGN(_a, _x, _y, _z)
Definition: pprz_algebra.h:125
Main include for ABI (AirBorneInterface).
int16_t px
Target in camera frame.
static struct LlaCoor_f * stateGetPositionLla_f(void)
Get position in LLA coordinates (float).
Definition: state.h:728
#define VECT3_SUM_SCALED(_c, _a, _b, _s)
Definition: pprz_algebra.h:175
struct FloatVect3 cam_pos
Position of camera in body frame.
#define TARGET_LOC_BODY_TO_CAM_PSI
#define TARGET_LOC_BODY_TO_CAM_PHI
#define TARGET_LOC_PIXEL_TO_IMAGE_X
uint8_t target_loc_wp_tab[][2]
euler angles
static struct target_loc_t target_loc
static struct FloatRMat * stateGetNedToBodyRMat_f(void)
Get vehicle body attitude rotation matrix (float).
Definition: state.h:1137
Paparazzi floating point math for geodetic calculations.
bool valid
True if a target have been seen.
vector in Latitude, Longitude and Altitude
struct FloatRMat body_to_cam
Body to camera rotation.
#define FLOAT_VECT3_ZERO(_v)
void float_rmat_vmult(struct FloatVect3 *vb, struct FloatRMat *m_a2b, struct FloatVect3 *va)
rotate 3D vector by rotation matrix.
void float_rmat_comp(struct FloatRMat *m_a2c, struct FloatRMat *m_a2b, struct FloatRMat *m_b2c)
Composition (multiplication) of two rotation matrices.
void float_rmat_transp_vmult(struct FloatVect3 *vb, struct FloatRMat *m_b2a, struct FloatVect3 *va)
rotate 3D vector by transposed rotation matrix.
vector in North East Down coordinates Units: meters
Paparazzi floating point algebra.
void cv_target_localization_report_mark(uint8_t mark)
#define TARGET_LOC_CAM_POS_Z
void target_localization_init(void)
signed short int16_t
Definition: types.h:17
#define TARGET_LOC_CAM_POS_X
#define TARGET_LOC_BODY_TO_CAM_THETA
#define TARGET_LOC_WP_T1_ID
struct LlaCoor_f pos_lla
Target global position in LLA.
#define TARGET_LOC_CAM_POS_Y
static void detection_cb(uint8_t sender_id UNUSED, int16_t pixel_x, int16_t pixel_y, int16_t pixel_width UNUSED, int16_t pixel_height UNUSED, int32_t quality UNUSED, int16_t extra)
signed long int32_t
Definition: types.h:19
static const float scale[]
#define TARGET_LOC_PIXEL_TO_IMAGE_Y
#define TARGET_LOC_WP_T3_ID
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
#define TARGET_LOC_ID
int16_t py
Target in camera frame.
uint8_t target_localization_mark
float lon
in radians
rotation matrix
abi_event detection_ev
void target_localization_report(void)
float lat
in radians
void waypoint_move_xy_i(uint8_t wp_id, int32_t x, int32_t y)
Definition: waypoints.c:150
struct NedCoor_f target
Target position in LTP-NED frame.
static struct NedCoor_f * stateGetPositionNed_f(void)
Get position in local NED coordinates (float).
Definition: state.h:710
void lla_of_ecef_f(struct LlaCoor_f *out, struct EcefCoor_f *in)
struct State state
Definition: state.c:36
float x
in meters