Paparazzi UAS  v6.3_unstable
Paparazzi is a free software Unmanned Aircraft System.
target_pos.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2021 Freek van Tienen <freek.v.tienen@gmail.com>
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 
27 #include "target_pos.h"
28 #include <math.h>
29 
31 #include "modules/core/abi.h"
32 
33 // The timeout when receiving GPS messages from the ground in ms
34 #ifndef TARGET_POS_TIMEOUT
35 #define TARGET_POS_TIMEOUT 5000
36 #endif
37 
38 // The timeout when recceiving an RTK gps message from the GPS
39 #ifndef TARGET_RTK_TIMEOUT
40 #define TARGET_RTK_TIMEOUT 1000
41 #endif
42 
43 #ifndef TARGET_OFFSET_HEADING
44 #define TARGET_OFFSET_HEADING 180.0
45 #endif
46 
47 #ifndef TARGET_OFFSET_DISTANCE
48 #define TARGET_OFFSET_DISTANCE 12.0
49 #endif
50 
51 #ifndef TARGET_OFFSET_HEIGHT
52 #define TARGET_OFFSET_HEIGHT -1.2
53 #endif
54 
55 #ifndef TARGET_INTEGRATE_XY
56 #define TARGET_INTEGRATE_XY true
57 #endif
58 
59 #ifndef TARGET_INTEGRATE_Z
60 #define TARGET_INTEGRATE_Z false
61 #endif
62 
63 /* Initialize the main structure */
64 struct target_t target = {
65  .pos = {0},
66  .offset = {
68  .distance = TARGET_OFFSET_DISTANCE,
69  .height = TARGET_OFFSET_HEIGHT,
70  },
71  .target_pos_timeout = TARGET_POS_TIMEOUT,
72  .rtk_timeout = TARGET_RTK_TIMEOUT,
73  .integrate_xy = TARGET_INTEGRATE_XY,
74  .integrate_z = TARGET_INTEGRATE_Z
75 };
76 
77 /* GPS abi callback */
79 static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s);
80 
81 #if PERIODIC_TELEMETRY
83 static void send_target_pos_info(struct transport_tx *trans, struct link_device *dev)
84 {
85  pprz_msg_send_TARGET_POS_INFO(trans, dev, AC_ID,
86  &target.pos.lla.lat,
87  &target.pos.lla.lon,
88  &target.pos.lla.alt,
90  &target.pos.climb,
91  &target.pos.course,
96 }
97 #endif
98 
99 void target_pos_init(void)
100 {
101 #if PERIODIC_TELEMETRY
103 #endif
104 
105  AbiBindMsgGPS(ABI_BROADCAST, &gps_ev, gps_cb);
106 }
107 
108 /* Get the GPS lla position */
109 static void gps_cb(uint8_t sender_id __attribute__((unused)),
110  uint32_t stamp __attribute__((unused)),
111  struct GpsState *gps_s)
112 {
113  target.gps_lla.lat = gps_s->lla_pos.lat;
114  target.gps_lla.lon = gps_s->lla_pos.lon;
115  target.gps_lla.alt = gps_s->lla_pos.alt;
116 }
117 
122 {
123  if(DL_TARGET_POS_ac_id(buf) != AC_ID)
124  return;
125 
126  // Save the received values
128  target.pos.tow = gps_tow_from_sys_ticks(sys_time.nb_tick); // FIXME: need to get from the real GPS
129  target.pos.lla.lat = DL_TARGET_POS_lat(buf);
130  target.pos.lla.lon = DL_TARGET_POS_lon(buf);
131  target.pos.lla.alt = DL_TARGET_POS_alt(buf);
132  target.pos.ground_speed = DL_TARGET_POS_speed(buf);
133  target.pos.climb = DL_TARGET_POS_climb(buf);
134  target.pos.course = DL_TARGET_POS_course(buf);
135  target.pos.heading = DL_TARGET_POS_heading(buf);
136  target.pos.valid = true;
137 }
138 
142 bool target_get_pos(struct NedCoor_f *pos, float *heading) {
143  float time_diff = 0;
144 
145  /* When we have a valid target_pos message, state ned is initialized and no timeout */
147  struct NedCoor_i target_pos_cm, drone_pos_cm;
148 
149  // Convert from LLA to NED using origin from the UAV
150  ned_of_lla_point_i(&target_pos_cm, &state.ned_origin_i, &target.pos.lla);
151  // Convert from LLA to NED using origin from the UAV
153 
154  // Convert to floating point (cm to meters)
155  pos->x = (target_pos_cm.x - drone_pos_cm.x) * 0.01;
156  pos->y = (target_pos_cm.y - drone_pos_cm.y) * 0.01;
157  pos->z = (target_pos_cm.z - drone_pos_cm.z) * 0.01;
158 
159  // In seconds, overflow uint32_t in 49,7 days
160  time_diff = (get_sys_time_msec() - target.pos.recv_time) * 0.001; // FIXME: should be based on TOW of ground gps
161 
162  // Return the heading
164 
165  // If we have a velocity measurement try to integrate the x-y position when enabled
166  struct NedCoor_f vel = {0};
167  bool got_vel = target_get_vel(&vel);
168  if(target.integrate_xy && got_vel) {
169  pos->x = pos->x + vel.x * time_diff;
170  pos->y = pos->y + vel.y * time_diff;
171  }
172 
173  if(target.integrate_z && got_vel) {
174  pos->z = pos->z + vel.z * time_diff;
175  }
176 
177  // Offset the target
178  pos->x += target.offset.distance * cosf((*heading + target.offset.heading)/180.*M_PI);
179  pos->y += target.offset.distance * sinf((*heading + target.offset.heading)/180.*M_PI);
180  pos->z -= target.offset.height;
181 
182  return true;
183  }
184 
185  return false;
186 }
187 
191 bool target_get_vel(struct NedCoor_f *vel) {
192 
193  /* When we have a valid target_pos message, state ned is initialized and no timeout */
195  // Calculate baed on ground speed and course
196  vel->x = target.pos.ground_speed * cosf(target.pos.course/180.*M_PI);
197  vel->y = target.pos.ground_speed * sinf(target.pos.course/180.*M_PI);
198  vel->z = -target.pos.climb;
199 
200  return true;
201  }
202 
203  return false;
204 }
205 
209 bool target_pos_set_current_offset(float unk __attribute__((unused))) {
211  struct NedCoor_i target_pos_cm;
212  struct NedCoor_f uav_pos = *stateGetPositionNed_f();
213 
214  // Convert from LLA to NED using origin from the UAV
215  ned_of_lla_point_i(&target_pos_cm, &state.ned_origin_i, &target.pos.lla);
216 
217  // Convert to floating point (cm to meters)
218  struct NedCoor_f pos;
219  pos.x = target_pos_cm.x * 0.01;
220  pos.y = target_pos_cm.y * 0.01;
221  pos.z = target_pos_cm.z * 0.01;
222 
223  target.offset.distance = sqrtf(powf(uav_pos.x - pos.x, 2) + powf(uav_pos.y - pos.y, 2));
224  target.offset.height = -(uav_pos.z - pos.z);
225  target.offset.heading = atan2f((uav_pos.y - pos.y), (uav_pos.x - pos.x))*180.0/M_PI - target.pos.heading;
226  }
227 
228  return false;
229 }
Main include for ABI (AirBorneInterface).
#define ABI_BROADCAST
Broadcast address.
Definition: abi_common.h:58
Event structure to store callbacks in a linked list.
Definition: abi_common.h:67
uint32_t get_sys_time_msec(void)
Get the time in milliseconds since startup.
Definition: sys_time_arch.c:98
uint32_t gps_tow_from_sys_ticks(uint32_t sys_ticks)
Convert time in sys_time ticks to GPS time of week.
Definition: gps.c:355
struct LlaCoor_i lla_pos
position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid)
Definition: gps.h:95
data structure for GPS information
Definition: gps.h:90
int32_t lat
in degrees*1e7
int32_t alt
in millimeters above WGS84 reference ellipsoid
int32_t z
Down.
int32_t y
East.
int32_t lon
in degrees*1e7
int32_t x
North.
void ned_of_lla_point_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct LlaCoor_i *lla)
Convert a point from LLA to local NED.
vector in North East Down coordinates
struct State state
Definition: state.c:36
struct LtpDef_i ned_origin_i
Definition of the local (flat earth) coordinate system.
Definition: state.h:166
bool ned_initialized_i
true if local int coordinate frame is initialsed
Definition: state.h:171
static struct NedCoor_f * stateGetPositionNed_f(void)
Get position in local NED coordinates (float).
Definition: state.h:710
float z
in meters
float x
in meters
float y
in meters
vector in North East Down coordinates Units: meters
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
volatile uint32_t nb_tick
SYS_TIME_TICKS since startup.
Definition: sys_time.h:74
bool target_get_pos(struct NedCoor_f *pos, float *heading)
Get the current target position (NED) and heading.
Definition: target_pos.c:142
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
Definition: target_pos.c:109
struct target_t target
Definition: target_pos.c:64
#define TARGET_OFFSET_HEIGHT
Definition: target_pos.c:52
void target_parse_target_pos(uint8_t *buf)
Receive a TARGET_POS message from the ground.
Definition: target_pos.c:121
static void send_target_pos_info(struct transport_tx *trans, struct link_device *dev)
Definition: target_pos.c:83
#define TARGET_OFFSET_HEADING
Definition: target_pos.c:44
#define TARGET_POS_TIMEOUT
Definition: target_pos.c:35
#define TARGET_INTEGRATE_Z
Definition: target_pos.c:60
#define TARGET_OFFSET_DISTANCE
Definition: target_pos.c:48
#define TARGET_INTEGRATE_XY
Definition: target_pos.c:56
void target_pos_init(void)
Definition: target_pos.c:99
#define TARGET_RTK_TIMEOUT
Definition: target_pos.c:40
bool target_get_vel(struct NedCoor_f *vel)
Get the current target velocity (NED)
Definition: target_pos.c:191
static abi_event gps_ev
Definition: target_pos.c:78
bool target_pos_set_current_offset(float unk)
Set the current measured distance and heading as offset.
Definition: target_pos.c:209
float height
Target offset height.
Definition: target_pos.h:47
float course
Ground course of the target [deg].
Definition: target_pos.h:39
uint32_t recv_time
Time of when the target position message was received [msec].
Definition: target_pos.h:35
float ground_speed
Ground speed of the target [m/s].
Definition: target_pos.h:38
float heading
Target offset heading.
Definition: target_pos.h:45
struct LlaCoor_i lla
Lat, lon and altitude position of the target.
Definition: target_pos.h:37
struct target_pos_t pos
The target position message.
Definition: target_pos.h:51
float distance
Target offset distance.
Definition: target_pos.h:46
uint32_t tow
Time of week of the target position measurement.
Definition: target_pos.h:36
bool integrate_z
Enable integration of the position in Z (Up) frame.
Definition: target_pos.h:56
float climb
Climb speed, z-up [m/s].
Definition: target_pos.h:41
struct LlaCoor_i gps_lla
GPS LLA position.
Definition: target_pos.h:57
struct target_offset_t offset
The target offset relative to ground heading.
Definition: target_pos.h:52
bool valid
If the data of the target position is valid.
Definition: target_pos.h:34
uint32_t target_pos_timeout
Ground target position message timeout [msec].
Definition: target_pos.h:53
bool integrate_xy
Enable integration of the position in X-Y (North/East) frame.
Definition: target_pos.h:55
float heading
Heading of the target [deg].
Definition: target_pos.h:40
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:51
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
float heading
Definition: wedgebug.c:258