Paparazzi UAS  v6.2.0_stable
Paparazzi is a free software Unmanned Aircraft System.
All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
approach_moving_target.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2021 Ewoud Smeur <e.j.j.smeur@tudelft.nl>
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  */
26 #include "approach_moving_target.h"
27 
28 #include "generated/modules.h"
29 #include "modules/core/abi.h"
30 
31 float amt_err_slowdown_gain = AMT_ERR_SLOWDOWN_GAIN;
32 
34 
35 #define DEBUG_AMT TRUE
36 #include <stdio.h>
37 
38 struct Amt amt = {
39  .distance = 60,
40  .speed = -1.0,
41  .pos_gain = 0.3,
42  .psi_ref = 180.0,
43  .slope_ref = 35.0,
44  .speed_gain = 1.0,
45  .relvel_gain = 1.0,
46  .enabled_time = 0,
47  .wp_id = 0,
48 };
49 
50 struct AmtTelem {
51  struct FloatVect3 des_pos;
52  struct FloatVect3 des_vel;
53 };
54 
55 struct AmtTelem amt_telem;
57 
58 struct FloatVect3 nav_get_speed_sp_from_diagonal(struct EnuCoor_i target, float pos_gain, float rope_heading);
59 void update_waypoint(uint8_t wp_id, struct FloatVect3 * target_ned);
60 
61 #if PERIODIC_TELEMETRY
63 static void send_approach_moving_target(struct transport_tx *trans, struct link_device *dev)
64 {
65  pprz_msg_send_APPROACH_MOVING_TARGET(trans, dev, AC_ID,
72  &amt.distance);
73 }
74 #endif
75 
77 {
78 #if PERIODIC_TELEMETRY
79  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_APPROACH_MOVING_TARGET, send_approach_moving_target);
80 #endif
81 }
82 
83 // Update a waypoint such that you can see on the GCS where the drone wants to go
84 void update_waypoint(uint8_t wp_id, struct FloatVect3 * target_ned) {
85 
86  // Update the waypoint
87  struct EnuCoor_f target_enu;
88  ENU_OF_TO_NED(target_enu, *target_ned);
89  waypoint_set_enu(wp_id, &target_enu);
90 
91  // Send waypoint update every half second
92  RunOnceEvery(100/2, {
93  // Send to the GCS that the waypoint has been moved
94  DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id,
95  &waypoints[wp_id].enu_i.x,
96  &waypoints[wp_id].enu_i.y,
97  &waypoints[wp_id].enu_i.z);
98  } );
99 }
100 
101 // Function to enable from flight plan (call repeatedly!)
104  amt.wp_id = wp_id;
105 }
106 
112 
113  // Check if the flight plan recently called the enable function
114  if ( (get_sys_time_msec() - amt.enabled_time) > (2000 / NAVIGATION_FREQUENCY)) {
115  return;
116  }
117 
118  // Get the target position, velocity and heading
119  struct NedCoor_f target_pos, target_vel = {0};
120  float target_heading;
121  if(!target_get_pos(&target_pos, &target_heading)) {
122  // TODO: What to do? Same can be checked for the velocity
123  return;
124  }
125  target_get_vel(&target_vel);
126  VECT3_SMUL(target_vel, target_vel, amt.speed_gain);
127 
128  // Reference model
129  float gamma_ref = RadOfDeg(amt.slope_ref);
130  float psi_ref = RadOfDeg(target_heading + amt.psi_ref);
131 
132  amt.rel_unit_vec.x = cosf(gamma_ref) * cosf(psi_ref);
133  amt.rel_unit_vec.y = cosf(gamma_ref) * sinf(psi_ref);
134  amt.rel_unit_vec.z = -sinf(gamma_ref);
135 
136  // desired position = rel_pos + target_pos
137  struct FloatVect3 ref_relpos;
138  VECT3_SMUL(ref_relpos, amt.rel_unit_vec, amt.distance);
139 
140  // ATTENTION, target_pos is already relative now!
141  struct FloatVect3 des_pos;
142  VECT3_SUM(des_pos, ref_relpos, target_pos);
143 
144  struct FloatVect3 ref_relvel;
146 
147  // error controller
148  struct FloatVect3 pos_err;
149  struct FloatVect3 ec_vel;
150  struct NedCoor_f *drone_pos = stateGetPositionNed_f();
151  // ATTENTION, target_pos is already relative now!
152  // VECT3_DIFF(pos_err, des_pos, *drone_pos);
153  VECT3_COPY(pos_err, des_pos);
154  VECT3_SMUL(ec_vel, pos_err, amt.pos_gain);
155 
156  // desired velocity = rel_vel + target_vel + error_controller(using NED position)
157  struct FloatVect3 des_vel = {
158  ref_relvel.x + target_vel.x + ec_vel.x,
159  ref_relvel.y + target_vel.y + ec_vel.y,
160  ref_relvel.z + target_vel.z + ec_vel.z,
161  };
162 
163  vect_bound_in_3d(&des_vel, 10.0);
164 
165  // Bound vertical speed setpoint
166  if(stateGetAirspeed_f() > 13.0) {
167  Bound(des_vel.z, -4.0, 5.0);
168  } else {
169  Bound(des_vel.z, -nav_climb_vspeed, -nav_descend_vspeed);
170  }
171 
172  AbiSendMsgVEL_SP(VEL_SP_FCR_ID, &des_vel);
173 
174  /* limit the speed such that the vertical component is small enough
175  * and doesn't outrun the vehicle
176  */
177  float min_speed;
178  float sin_gamma_ref = sinf(gamma_ref);
179  if (sin_gamma_ref > 0.05) {
180  min_speed = (nav_descend_vspeed+0.1) / sin_gamma_ref;
181  } else {
182  min_speed = -5.0; // prevent dividing by zero
183  }
184 
185  // The upper bound is not very important
186  Bound(amt.speed, min_speed, 4.0);
187 
188  // Reduce approach speed if the error is large
189  float norm_pos_err_sq = VECT3_NORM2(pos_err);
190  float int_speed = amt.speed / (norm_pos_err_sq * amt_err_slowdown_gain + 1.0);
191 
192  // integrate speed to get the distance
193  float dt = FOLLOW_DIAGONAL_APPROACH_PERIOD;
194  amt.distance += int_speed*dt;
195 
196  // For display purposes
197  struct FloatVect3 disp_pos_target;
198  VECT3_SUM(disp_pos_target, des_pos, *drone_pos);
199  update_waypoint(amt.wp_id, &disp_pos_target);
200 
201  // Debug target pos:
202  VECT3_DIFF(amt_telem.des_pos, *drone_pos, target_pos);
203 
204  // Update values for telemetry
205  VECT3_COPY(amt_telem.des_pos, des_pos);
206  VECT3_COPY(amt_telem.des_vel, des_vel);
207 }
Main include for ABI (AirBorneInterface).
#define VEL_SP_FCR_ID
float approach_moving_target_angle_deg
void follow_diagonal_approach(void)
Generates a velocity reference from a diagonal approach path.
struct FloatVect3 nav_get_speed_sp_from_diagonal(struct EnuCoor_i target, float pos_gain, float rope_heading)
struct AmtTelem amt_telem
struct Amt amt
float amt_err_slowdown_gain
struct FloatVect3 des_vel
bool approach_moving_target_enabled
struct FloatVect3 des_pos
static void send_approach_moving_target(struct transport_tx *trans, struct link_device *dev)
void approach_moving_target_init(void)
void update_waypoint(uint8_t wp_id, struct FloatVect3 *target_ned)
void approach_moving_target_enable(uint8_t wp_id)
float speed_gain
int32_t enabled_time
uint8_t wp_id
float relvel_gain
struct FloatVect3 rel_unit_vec
uint32_t get_sys_time_msec(void)
Get the time in milliseconds since startup.
Definition: sys_time_arch.c:86
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:39
float y
Definition: common_nav.h:41
float x
Definition: common_nav.h:40
void vect_bound_in_3d(struct FloatVect3 *vect3, float bound)
#define VECT3_SUM(_c, _a, _b)
Definition: pprz_algebra.h:161
#define VECT3_NORM2(_v)
Definition: pprz_algebra.h:252
#define VECT3_SMUL(_vo, _vi, _s)
Definition: pprz_algebra.h:189
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:140
#define VECT3_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:182
#define ENU_OF_TO_NED(_po, _pi)
Definition: pprz_geodetic.h:41
vector in East North Up coordinates
static struct NedCoor_f * stateGetPositionNed_f(void)
Get position in local NED coordinates (float).
Definition: state.h:710
static float stateGetAirspeed_f(void)
Get airspeed (float).
Definition: state.h:1407
void waypoint_set_enu(uint8_t wp_id, struct EnuCoor_f *enu)
Set local ENU waypoint coordinates.
Definition: waypoints.c:113
#define NAVIGATION_FREQUENCY
Default fixedwing navigation frequency.
Definition: nav.h:49
float z
in meters
float x
in meters
float y
in meters
vector in East North Up coordinates Units: meters
vector in North East Down coordinates Units: meters
float nav_descend_vspeed
Definition: navigation.c:116
float nav_climb_vspeed
Definition: navigation.c:116
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
bool target_get_pos(struct NedCoor_f *pos, float *heading)
Get the current target position (NED) and heading.
Definition: target_pos.c:142
struct target_t target
Definition: target_pos.c:64
bool target_get_vel(struct NedCoor_f *vel)
Get the current target velocity (NED)
Definition: target_pos.c:191
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 char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98