Paparazzi UAS  v5.15_devel-187-g731cc33
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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 
28 #include "generated/flight_plan.h"
29 #include "subsystems/ins.h"
31 
34 
36 
37 const uint8_t nb_waypoint = NB_WAYPOINT;
38 struct point waypoints[NB_WAYPOINT] = WAYPOINTS_UTM;
39 
40 float ground_alt;
41 
42 int32_t nav_utm_east0 = NAV_UTM_EAST0;
43 int32_t nav_utm_north0 = NAV_UTM_NORTH0;
44 uint8_t nav_utm_zone0 = NAV_UTM_ZONE0;
45 float max_dist_from_home = MAX_DIST_FROM_HOME;
46 
51 {
52  struct EnuCoor_f *pos = stateGetPositionEnu_f();
53  float ph_x = waypoints[WP_HOME].x - pos->x;
54  float ph_y = waypoints[WP_HOME].y - pos->y;
55  dist2_to_home = ph_x * ph_x + ph_y * ph_y;
56  too_far_from_home = dist2_to_home > (MAX_DIST_FROM_HOME * MAX_DIST_FROM_HOME);
57 #ifdef InGeofenceSector
58  too_far_from_home = too_far_from_home || !(InGeofenceSector(pos->x, pos->y));
59 #endif
60 }
61 
65 float get_time_to_home(void)
66 {
67  struct FloatVect2 vect_to_home;
68  vect_to_home.x = waypoints[WP_HOME].x - stateGetPositionEnu_f()->x;
69  vect_to_home.y = waypoints[WP_HOME].y - stateGetPositionEnu_f()->y;
70  // get distance to home
71  float dist_to_home = float_vect2_norm(&vect_to_home);
72  if (dist_to_home > 1.f) {
73  // get windspeed or assume no wind
74  struct FloatVect2 wind = { 0.f, 0.f };
75  if (stateIsWindspeedValid()) {
77  }
78  // compute effective windspeed when flying to home point
79  float wind_to_home = (wind.x * vect_to_home.x + wind.y * vect_to_home.y) / dist_to_home;
80  // get airspeed or assume constant nominal airspeed
81  float airspeed = NOMINAL_AIRSPEED;
82  if (stateIsAirspeedValid()) {
83  airspeed = stateGetAirspeed_f();
84  }
85  // get estimated ground speed to home
86  float gspeed_to_home = wind_to_home + airspeed;
87  if (gspeed_to_home > 1.) {
88  return dist_to_home / gspeed_to_home; // estimated time to home in seconds
89  }
90  else {
91  return 999999.f; // this might take a long time to go back home
92  }
93  }
94  return 0.f; // too close to home point
95 }
96 
97 
98 static float previous_ground_alt;
99 
102 {
103 
104  struct UtmCoor_f utm0;
105  utm0.zone = nav_utm_zone0;
106  utm0.north = nav_utm_north0;
107  utm0.east = nav_utm_east0;
108  utm0.alt = ground_alt;
109  ins_reset_utm_zone(&utm0);
110 
111  /* Set the real UTM ref */
112  nav_utm_zone0 = utm0.zone;
113  nav_utm_east0 = utm0.east;
114  nav_utm_north0 = utm0.north;
115 }
116 
119 {
120  /* realign INS */
122 
123  /* Set nav UTM ref */
127 
128  /* Ground alt */
131 }
132 
134 void nav_reset_alt(void)
135 {
137 
138  /* Ground alt */
141 }
142 
145 {
146  uint8_t i;
147  for (i = 0; i < NB_WAYPOINT; i++) {
148  waypoints[i].a += ground_alt - previous_ground_alt;
149  }
150 }
151 
153 {
154  RunOnceEvery(4, { stage_time++; block_time++; });
155 }
156 
163 void nav_move_waypoint(uint8_t wp_id, float ux, float uy, float alt)
164 {
165  if (wp_id < nb_waypoint) {
166  float dx, dy;
167  dx = ux - nav_utm_east0 - waypoints[WP_HOME].x;
168  dy = uy - nav_utm_north0 - waypoints[WP_HOME].y;
169  BoundAbs(dx, max_dist_from_home);
170  BoundAbs(dy, max_dist_from_home);
171  waypoints[wp_id].x = waypoints[WP_HOME].x + dx;
172  waypoints[wp_id].y = waypoints[WP_HOME].y + dy;
173  waypoints[wp_id].a = alt;
174  }
175 }
uint16_t block_time
float x
Definition: common_nav.h:40
float east
in meters
void WEAK ins_reset_utm_zone(struct UtmCoor_f *utm)
INS utm zone reset.
Definition: ins.c:68
float north
in meters
float alt
in meters (above WGS84 reference ellipsoid or above MSL)
float ground_alt
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:40
void ins_reset_local_origin(void)
local implemetation of the ins_reset functions
void compute_dist2_to_home(void)
Computes squared distance to the HOME waypoint.
Definition: common_nav.c:50
vector in East North Up coordinates Units: meters
static float stateGetAirspeed_f(void)
Get airspeed (float).
Definition: state.h:1407
void nav_update_waypoints_alt(void)
Shift altitude of the waypoint according to a new ground altitude.
Definition: common_nav.c:144
uint8_t nav_utm_zone0
Definition: common_nav.c:44
Integrated Navigation System interface.
void nav_reset_reference(void)
Reset the geographic reference to the current GPS fix.
Definition: common_nav.c:118
static bool stateIsAirspeedValid(void)
test if air speed is available.
Definition: state.h:1244
static float previous_ground_alt
Definition: common_nav.c:98
void nav_reset_alt(void)
Reset the altitude reference to the current GPS alt.
Definition: common_nav.c:134
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:163
position in UTM coordinates Units: meters
float dist2_to_home
squared distance to home waypoint
Definition: common_nav.c:32
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
static bool stateIsWindspeedValid(void)
test if wind speed is available.
Definition: state.h:1232
Paparazzi floating point math for geodetic calculations.
float y
Definition: common_nav.h:41
void common_nav_periodic_task_4Hz()
Definition: common_nav.c:152
int32_t nav_utm_north0
Definition: common_nav.c:43
static float float_vect2_norm(struct FloatVect2 *v)
float x
in meters
void nav_reset_utm_zone(void)
Reset the UTM zone to current GPS fix.
Definition: common_nav.c:101
float max_dist_from_home
Definition: common_nav.c:45
uint8_t zone
UTM zone number.
struct UtmCoor_f utm_origin_f
Definition of the origin of Utm coordinate system.
Definition: state.h:233
uint16_t stage_time
In s.
const uint8_t nb_waypoint
Definition: common_nav.c:37
signed long int32_t
Definition: types.h:19
float get_time_to_home(void)
Compute time to home use wind and airspeed when available.
Definition: common_nav.c:65
unsigned char uint8_t
Definition: types.h:14
int32_t nav_utm_east0
Definition: common_nav.c:42
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:38
static struct FloatVect2 * stateGetHorizontalWindspeed_f(void)
Get horizontal windspeed (float).
Definition: state.h:1377
float dist2_to_wp
squared distance to next waypoint
Definition: common_nav.c:33
float a
Definition: common_nav.h:42
void ins_reset_altitude_ref(void)
INS altitude reference reset.
float y
in meters
struct State state
Definition: state.c:36
bool too_far_from_home
Definition: common_nav.c:35