Paparazzi UAS  v6.2_unstable
Paparazzi is a free software Unmanned Aircraft System.
common_nav.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2007-2009 ENAC, Pascal Brisset, Antoine Drouin
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 
27 #include "modules/nav/common_nav.h"
28 #include "generated/flight_plan.h"
29 #include "modules/ins/ins.h"
32 
35 
37 
38 const uint8_t nb_waypoint = NB_WAYPOINT;
39 struct point waypoints[NB_WAYPOINT] = WAYPOINTS_UTM;
40 
41 float ground_alt;
42 
43 int32_t nav_utm_east0 = NAV_UTM_EAST0;
44 int32_t nav_utm_north0 = NAV_UTM_NORTH0;
45 uint8_t nav_utm_zone0 = NAV_UTM_ZONE0;
46 float max_dist_from_home = MAX_DIST_FROM_HOME;
47 
52 {
53  struct EnuCoor_f *pos = stateGetPositionEnu_f();
54  float ph_x = waypoints[WP_HOME].x - pos->x;
55  float ph_y = waypoints[WP_HOME].y - pos->y;
56  dist2_to_home = ph_x * ph_x + ph_y * ph_y;
57  too_far_from_home = dist2_to_home > (MAX_DIST_FROM_HOME * MAX_DIST_FROM_HOME);
58 #ifdef InGeofenceSector
59  too_far_from_home = too_far_from_home || !(InGeofenceSector(pos->x, pos->y));
60 #endif
61 }
62 
66 float get_time_to_home(void)
67 {
68  struct FloatVect2 vect_to_home;
69  vect_to_home.x = waypoints[WP_HOME].x - stateGetPositionEnu_f()->x;
70  vect_to_home.y = waypoints[WP_HOME].y - stateGetPositionEnu_f()->y;
71  // get distance to home
72  float dist_to_home = float_vect2_norm(&vect_to_home);
73  if (dist_to_home > 1.f) {
74  // get windspeed or assume no wind
75  struct FloatVect2 wind = { 0.f, 0.f };
76  if (stateIsWindspeedValid()) {
78  }
79  // compute effective windspeed when flying to home point
80  float wind_to_home = (wind.x * vect_to_home.x + wind.y * vect_to_home.y) / dist_to_home;
81  // get airspeed or assume constant nominal airspeed
82  float airspeed = NOMINAL_AIRSPEED;
83  if (stateIsAirspeedValid()) {
84  airspeed = stateGetAirspeed_f();
85  }
86  // get estimated ground speed to home
87  float gspeed_to_home = wind_to_home + airspeed;
88  if (gspeed_to_home > 1.) {
89  return dist_to_home / gspeed_to_home; // estimated time to home in seconds
90  }
91  else {
92  return 999999.f; // this might take a long time to go back home
93  }
94  }
95  return 0.f; // too close to home point
96 }
97 
98 
99 static float previous_ground_alt;
100 
103 {
104 
105  struct UtmCoor_f utm0;
106  utm0.zone = nav_utm_zone0;
107  utm0.north = nav_utm_north0;
108  utm0.east = nav_utm_east0;
109  utm0.alt = ground_alt;
110  ins_reset_utm_zone(&utm0);
111 
112  /* Set the real UTM ref */
113  nav_utm_zone0 = utm0.zone;
114  nav_utm_east0 = utm0.east;
115  nav_utm_north0 = utm0.north;
116 }
117 
120 {
121  /* realign INS */
123 
124  /* Set nav UTM ref */
128 
129  /* Ground alt */
132 }
133 
135 void nav_reset_alt(void)
136 {
138 
139  /* Ground alt */
142 }
143 
146 {
147  uint8_t i;
148  for (i = 0; i < NB_WAYPOINT; i++) {
150  }
151 }
152 
154 {
155  RunOnceEvery(NAVIGATION_FREQUENCY, { stage_time++; block_time++; });
156 }
157 
164 void nav_move_waypoint(uint8_t wp_id, float ux, float uy, float alt)
165 {
166  if (wp_id < nb_waypoint) {
167  float dx, dy;
168  dx = ux - nav_utm_east0 - waypoints[WP_HOME].x;
169  dy = uy - nav_utm_north0 - waypoints[WP_HOME].y;
170  BoundAbs(dx, max_dist_from_home);
171  BoundAbs(dy, max_dist_from_home);
172  waypoints[wp_id].x = waypoints[WP_HOME].x + dx;
173  waypoints[wp_id].y = waypoints[WP_HOME].y + dy;
174  waypoints[wp_id].a = alt;
175  }
176 }
177 
184 void nav_move_waypoint_enu(uint8_t wp_id, float x, float y, float alt)
185 {
186  if (wp_id < nb_waypoint) {
187  float dx, dy;
188  dx = x - waypoints[WP_HOME].x;
189  dy = y - waypoints[WP_HOME].y;
190  BoundAbs(dx, max_dist_from_home);
191  BoundAbs(dy, max_dist_from_home);
192  waypoints[wp_id].x = waypoints[WP_HOME].x + dx;
193  waypoints[wp_id].y = waypoints[WP_HOME].y + dy;
194  waypoints[wp_id].a = alt;
195  }
196 }
197 
203 {
204  nav_move_waypoint_enu(wp_id, p->x, p->y, p->a);
205 }
206 
211 {
212  if (wp_id < nb_waypoint) {
213  struct UtmCoor_f utm;
214  utm.zone = nav_utm_zone0;
215  utm.east = waypoints[wp_id].x + nav_utm_east0;
216  utm.north = waypoints[wp_id].y + nav_utm_north0;
217  utm.alt = waypoints[wp_id].a;
218  DOWNLINK_SEND_WP_MOVED(DefaultChannel, DefaultDevice, &wp_id, &utm.east, &utm.north, &utm.alt, &utm.zone);
219  }
220 }
221 
point::a
float a
Definition: common_nav.h:42
ins.h
common_nav_periodic_task
void common_nav_periodic_task()
Definition: common_nav.c:153
uint8_t
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
block_time
uint16_t block_time
Definition: common_flight_plan.c:33
nav_send_waypoint
void nav_send_waypoint(uint8_t wp_id)
Send a waypoint throught default telemetry channel.
Definition: common_nav.c:210
UtmCoor_f::north
float north
in meters
Definition: pprz_geodetic_float.h:82
previous_ground_alt
static float previous_ground_alt
Definition: common_nav.c:99
nav_utm_east0
int32_t nav_utm_east0
Definition: common_nav.c:43
ins_reset_utm_zone
void WEAK ins_reset_utm_zone(struct UtmCoor_f *utm)
INS utm zone reset.
Definition: ins.c:68
stateGetPositionEnu_f
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
UtmCoor_f::east
float east
in meters
Definition: pprz_geodetic_float.h:83
nav_reset_alt
void nav_reset_alt(void)
Reset the altitude reference to the current GPS alt.
Definition: common_nav.c:135
common_nav.h
point
Definition: common_nav.h:39
waypoints
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:39
nav_utm_north0
int32_t nav_utm_north0
Definition: common_nav.c:44
EnuCoor_f::y
float y
in meters
Definition: pprz_geodetic_float.h:74
FloatVect2
Definition: pprz_algebra_float.h:49
pprz_geodetic_float.h
Paparazzi floating point math for geodetic calculations.
float_vect2_norm
static float float_vect2_norm(struct FloatVect2 *v)
Definition: pprz_algebra_float.h:139
nav_reset_reference
void nav_reset_reference(void)
Reset the geographic reference to the current GPS fix.
Definition: common_nav.c:119
nav_reset_utm_zone
void nav_reset_utm_zone(void)
Reset the UTM zone to current GPS fix.
Definition: common_nav.c:102
UtmCoor_f::alt
float alt
in meters (above WGS84 reference ellipsoid or above MSL)
Definition: pprz_geodetic_float.h:84
nb_waypoint
const uint8_t nb_waypoint
Definition: common_nav.c:38
UtmCoor_f::zone
uint8_t zone
UTM zone number.
Definition: pprz_geodetic_float.h:85
stateGetAirspeed_f
static float stateGetAirspeed_f(void)
Get airspeed (float).
Definition: state.h:1407
stateIsAirspeedValid
static bool stateIsAirspeedValid(void)
test if air speed is available.
Definition: state.h:1244
NAVIGATION_FREQUENCY
#define NAVIGATION_FREQUENCY
Default fixedwing navigation frequency.
Definition: nav.h:49
point::x
float x
Definition: common_nav.h:40
EnuCoor_f
vector in East North Up coordinates Units: meters
Definition: pprz_geodetic_float.h:72
dist2_to_wp
float dist2_to_wp
squared distance to next waypoint
Definition: common_nav.c:34
nav_move_waypoint
void nav_move_waypoint(uint8_t wp_id, float ux, float uy, float alt)
Move a waypoint to given UTM coordinates.
Definition: common_nav.c:164
stateIsWindspeedValid
static bool stateIsWindspeedValid(void)
test if wind speed is available.
Definition: state.h:1232
nav_update_waypoints_alt
void nav_update_waypoints_alt(void)
Shift altitude of the waypoint according to a new ground altitude.
Definition: common_nav.c:145
stage_time
uint16_t stage_time
In s.
Definition: common_flight_plan.c:33
nav_move_waypoint_enu
void nav_move_waypoint_enu(uint8_t wp_id, float x, float y, float alt)
Move a waypoint in local frame.
Definition: common_nav.c:184
f
uint16_t f
Camera baseline, in meters (i.e. horizontal distance between the two cameras of the stereo setup)
Definition: wedgebug.c:204
FloatVect2::y
float y
Definition: pprz_algebra_float.h:51
stateGetHorizontalWindspeed_f
static struct FloatVect2 * stateGetHorizontalWindspeed_f(void)
Get horizontal windspeed (float).
Definition: state.h:1377
dist2_to_home
float dist2_to_home
squared distance to home waypoint
Definition: common_nav.c:33
int32_t
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
ins_reset_altitude_ref
void WEAK ins_reset_altitude_ref(void)
INS altitude reference reset.
Definition: ins.c:65
ground_alt
float ground_alt
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:41
UtmCoor_f
position in UTM coordinates Units: meters
Definition: pprz_geodetic_float.h:81
nav_utm_zone0
uint8_t nav_utm_zone0
Definition: common_nav.c:45
point::y
float y
Definition: common_nav.h:41
State::utm_origin_f
struct UtmCoor_f utm_origin_f
Definition of the origin of Utm coordinate system.
Definition: state.h:233
compute_dist2_to_home
void compute_dist2_to_home(void)
Computes squared distance to the HOME waypoint.
Definition: common_nav.c:51
FloatVect2::x
float x
Definition: pprz_algebra_float.h:50
EnuCoor_f::x
float x
in meters
Definition: pprz_geodetic_float.h:73
ins_reset_local_origin
void WEAK ins_reset_local_origin(void)
INS local origin reset.
Definition: ins.c:55
too_far_from_home
bool too_far_from_home
Definition: common_nav.c:36
max_dist_from_home
float max_dist_from_home
Definition: common_nav.c:46
state
struct State state
Definition: state.c:36
p
static float p[2][2]
Definition: ins_alt_float.c:268
nav_move_waypoint_point
void nav_move_waypoint_point(uint8_t wp_id, struct point *p)
Move a waypoint from point structure (local frame).
Definition: common_nav.c:202
get_time_to_home
float get_time_to_home(void)
Compute time to home use wind and airspeed when available.
Definition: common_nav.c:66