Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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
28
30#include "std.h"
33#include "state.h"
34#include "modules/core/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
93
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
133static 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;
166 ecef_of_ned_point_f(&target_ecef, stateGetNedOrigin_f(), &target_loc.target);
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 };
201 float_rmat_of_eulers(&target_loc.body_to_cam, &euler);
202 VECT3_ASSIGN(target_loc.cam_pos,
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
Main include for ABI (AirBorneInterface).
struct abi_struct abi_event
Definition abi_common.h:101
#define UNUSED(x)
int16_t px
Target in camera frame.
#define TARGET_LOC_CAM_POS_X
#define TARGET_LOC_ID
abi_event detection_ev
#define TARGET_LOC_PIXEL_TO_IMAGE_Y
#define TARGET_LOC_CAM_POS_Z
#define TARGET_LOC_BODY_TO_CAM_PSI
#define TARGET_LOC_WP_T3_ID
void cv_target_localization_report_mark(uint8_t mark)
struct FloatVect3 cam_pos
Position of camera in body frame.
uint8_t target_localization_mark
#define TARGET_LOC_BODY_TO_CAM_PHI
static struct target_loc_t target_loc
#define TARGET_LOC_BODY_TO_CAM_THETA
void target_localization_init(void)
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)
struct FloatRMat body_to_cam
Body to camera rotation.
uint8_t type
Type of target.
int16_t py
Target in camera frame.
struct LlaCoor_f pos_lla
Target global position in LLA.
bool target_localization_update_wp
struct NedCoor_f target
Target position in LTP-NED frame.
bool valid
True if a target have been seen.
#define TARGET_LOC_WP_T1_ID
uint8_t target_loc_wp_tab[][2]
#define TARGET_LOC_PIXEL_TO_IMAGE_X
void target_localization_report(void)
#define TARGET_LOC_CAM_POS_Y
#define TARGET_LOC_WP_T2_ID
#define FLOAT_VECT3_ZERO(_v)
#define float_rmat_of_eulers
void float_rmat_vmult(struct FloatVect3 *vb, const struct FloatRMat *m_a2b, const struct FloatVect3 *va)
rotate 3D vector 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.
void float_rmat_comp(struct FloatRMat *m_a2c, const struct FloatRMat *m_a2b, const struct FloatRMat *m_b2c)
Composition (multiplication) of two rotation matrices.
euler angles
rotation matrix
#define VECT3_ASSIGN(_a, _x, _y, _z)
#define VECT3_ADD(_a, _b)
#define VECT3_SUM_SCALED(_c, _a, _b, _s)
#define POS_BFP_OF_REAL(_af)
static struct FloatRMat * stateGetNedToBodyRMat_f(void)
Get vehicle body attitude rotation matrix (float).
Definition state.h:1319
static struct LlaCoor_f * stateGetPositionLla_f(void)
Get position in LLA coordinates (float).
Definition state.h:868
static struct LtpDef_f * stateGetNedOrigin_f(void)
Get the coordinate NED frame origin (float).
Definition state.h:577
static struct NedCoor_f * stateGetPositionNed_f(void)
Get position in local NED coordinates (float).
Definition state.h:850
void waypoint_move_xy_i(uint8_t wp_id, int32_t x, int32_t y)
Definition waypoints.c:240
Paparazzi floating point algebra.
void ecef_of_ned_point_f(struct EcefCoor_f *ecef, struct LtpDef_f *def, struct NedCoor_f *ned)
void lla_of_ecef_f(struct LlaCoor_f *out, struct EcefCoor_f *in)
Paparazzi floating point math for geodetic calculations.
float lon
in radians
float lat
in radians
vector in EarthCenteredEarthFixed coordinates
vector in Latitude, Longitude and Altitude
vector in North East Down coordinates Units: meters
API to get/set the generic vehicle states.
static const float scale[]
int int32_t
Typedef defining 32 bit int type.
short int16_t
Typedef defining 16 bit short type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.