Paparazzi UAS  v4.2.2_stable-4-gcc32f65
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
common_nav.c
Go to the documentation of this file.
1 /*
2  * $Id$
3  *
4  * Copyright (C) 2007-2009 ENAC, Pascal Brisset, Antoine Drouin
5  *
6  * This file is part of paparazzi.
7  *
8  * paparazzi is free software; you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation; either version 2, or (at your option)
11  * any later version.
12  *
13  * paparazzi is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  * GNU General Public License for more details.
17  *
18  * You should have received a copy of the GNU General Public License
19  * along with paparazzi; see the file COPYING. If not, write to
20  * the Free Software Foundation, 59 Temple Place - Suite 330,
21  * Boston, MA 02111-1307, USA.
22  *
23  */
24 
26 #include "estimator.h"
27 #include "generated/flight_plan.h"
28 #include "subsystems/gps.h"
30 
33 
35 
36 const uint8_t nb_waypoint = NB_WAYPOINT;
37 struct point waypoints[NB_WAYPOINT] = WAYPOINTS;
38 
39 float ground_alt;
40 
41 int32_t nav_utm_east0 = NAV_UTM_EAST0;
42 int32_t nav_utm_north0 = NAV_UTM_NORTH0;
43 uint8_t nav_utm_zone0 = NAV_UTM_ZONE0;
44 float max_dist_from_home = MAX_DIST_FROM_HOME;
45 
50  float ph_x = waypoints[WP_HOME].x - estimator_x;
51  float ph_y = waypoints[WP_HOME].y - estimator_y;
52  dist2_to_home = ph_x*ph_x + ph_y *ph_y;
53  too_far_from_home = dist2_to_home > (MAX_DIST_FROM_HOME*MAX_DIST_FROM_HOME);
54 #if defined InAirspace
55  too_far_from_home = too_far_from_home || !(InAirspace(estimator_x, estimator_y));
56 #endif
57 }
58 
59 
60 static float previous_ground_alt;
61 
63 unit_t nav_reset_reference( void ) {
64 #ifdef GPS_USE_LATLONG
65  /* Set the real UTM zone */
66  nav_utm_zone0 = (DegOfRad(gps.lla_pos.lon/1e7)+180) / 6 + 1;
67 
68  /* Recompute UTM coordinates in this zone */
69  struct LlaCoor_f lla;
70  lla.lat = gps.lla_pos.lat/1e7;
71  lla.lon = gps.lla_pos.lon/1e7;
72  struct UtmCoor_f utm;
73  utm.zone = nav_utm_zone0;
74  utm_of_lla_f(&utm, &lla);
75  nav_utm_east0 = utm.east;
76  nav_utm_north0 = utm.north;
77 #else
81 #endif
82 
84  ground_alt = gps.hmsl/1000.;
85  return 0;
86 }
87 
89 unit_t nav_update_waypoints_alt( void ) {
90  uint8_t i;
91  for(i = 0; i < NB_WAYPOINT; i++) {
92  waypoints[i].a += ground_alt - previous_ground_alt;
93  }
94  return 0;
95 }
96 
98  RunOnceEvery(4, { stage_time++; block_time++; });
99 }
100 
101 void nav_move_waypoint(uint8_t wp_id, float ux, float uy, float alt) {
102  if (wp_id < nb_waypoint) {
103  float dx, dy;
104  dx = ux - nav_utm_east0 - waypoints[WP_HOME].x;
105  dy = uy - nav_utm_north0 - waypoints[WP_HOME].y;
106  BoundAbs(dx, max_dist_from_home);
107  BoundAbs(dy, max_dist_from_home);
108  waypoints[wp_id].x = waypoints[WP_HOME].x + dx;
109  waypoints[wp_id].y = waypoints[WP_HOME].y + dy;
110  waypoints[wp_id].a = alt;
111  }
112 }
unit_t nav_update_waypoints_alt(void)
Shift altitude of the waypoint according to a new ground altitude.
Definition: common_nav.c:89
uint16_t block_time
struct LlaCoor_i lla_pos
position in LLA (lat,lon: rad*1e7; alt: mm over ellipsoid)
Definition: gps.h:64
int32_t lat
in radians*1e7
float ground_alt
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:39
uint8_t zone
UTM zone number.
void compute_dist2_to_home(void)
Computes square distance to the HOME waypoint potentially sets too_far_from_home. ...
Definition: common_nav.c:49
float a
Definition: common_nav.h:39
uint8_t nav_utm_zone0
Definition: common_nav.c:43
float estimator_y
north position in meters
Definition: estimator.c:43
int32_t hmsl
height above mean sea level in mm
Definition: gps.h:66
static float previous_ground_alt
Definition: common_nav.c:60
void nav_move_waypoint(uint8_t wp_id, float ux, float uy, float alt)
Definition: common_nav.c:101
float lat
in radians
float dist2_to_home
Definition: common_nav.c:31
vector in Latitude, Longitude and Altitude
Paparazzi floating point math for geodetic calculations.
uint8_t zone
UTM zone number.
void common_nav_periodic_task_4Hz()
Definition: common_nav.c:97
int32_t nav_utm_north0
Definition: common_nav.c:42
float north
in meters
Device independent GPS code (interface)
position in UTM coordinates Units: meters
unit_t nav_reset_reference(void)
Reset the geographic reference to the current GPS fix.
Definition: common_nav.c:63
float estimator_x
east position in meters
Definition: estimator.c:42
int32_t north
in centimeters
float max_dist_from_home
Definition: common_nav.c:44
uint16_t stage_time
In s.
const uint8_t nb_waypoint
Definition: common_nav.c:36
signed long int32_t
Definition: types.h:19
int32_t east
in centimeters
unsigned char uint8_t
Definition: types.h:14
int32_t lon
in radians*1e7
bool_t too_far_from_home
Definition: common_nav.c:34
int32_t nav_utm_east0
Definition: common_nav.c:41
struct point waypoints[NB_WAYPOINT]
Definition: common_nav.c:37
float y
Definition: common_nav.h:38
State estimation, fusioning sensors.
float dist2_to_wp
Definition: common_nav.c:32
struct UtmCoor_i utm_pos
position in UTM (north,east: cm; alt: mm over ellipsoid)
Definition: gps.h:65
float lon
in radians
float east
in meters
float alt
in meters above WGS84 reference ellipsoid
struct GpsState gps
global GPS state
Definition: gps.c:31
float x
Definition: common_nav.h:37
void utm_of_lla_f(struct UtmCoor_f *utm, struct LlaCoor_f *lla)