Paparazzi UAS v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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 "modules/ins/ins.h"
32#include "state.h"
33#if USE_GPS
34// for reset_utm_zone
35#include "modules/gps/gps.h"
36#endif
37
40
42
45
47
52
57{
58 struct EnuCoor_f *pos = stateGetPositionEnu_f();
59 float ph_x = waypoints[WP_HOME].x - pos->x;
60 float ph_y = waypoints[WP_HOME].y - pos->y;
63#ifdef InGeofenceSector
65#endif
66}
67
72{
76 // get distance to home
78 if (dist_to_home > 1.f) {
79 // get windspeed or assume no wind
80 struct FloatVect2 wind = { 0.f, 0.f };
83 }
84 // compute effective windspeed when flying to home point
85 float wind_to_home = (wind.x * vect_to_home.x + wind.y * vect_to_home.y) / dist_to_home;
86 // get airspeed or assume constant nominal airspeed
87 float airspeed = NOMINAL_AIRSPEED;
89 airspeed = stateGetAirspeed_f();
90 }
91 // get estimated ground speed to home
92 float gspeed_to_home = wind_to_home + airspeed;
93 if (gspeed_to_home > 1.) {
94 return dist_to_home / gspeed_to_home; // estimated time to home in seconds
95 }
96 else {
97 return 999999.f; // this might take a long time to go back home
98 }
99 }
100 return 0.f; // too close to home point
101}
102
103
105
108{
109#if USE_GPS
110 struct UtmCoor_f utm0;
112 utm0.north = nav_utm_north0;
113 utm0.east = nav_utm_east0;
114 utm0.alt = ground_alt;
115
116 struct LlaCoor_f lla0;
119 utm0.zone = gps.utm_pos.zone;
120 }
121 else {
122 utm0.zone = 0; // recompute zone from lla
123 }
126
127 /* Set the real UTM ref */
128 nav_utm_zone0 = utm0.zone;
129 nav_utm_east0 = utm0.east;
130 nav_utm_north0 = utm0.north;
131#endif
132}
133
136{
137 /* realign INS */
139
140 /* Set nav UTM ref */
144
145 /* Ground alt */
148}
149
152{
154
155 /* Ground alt */
158}
159
162{
163 uint8_t i;
164 for (i = 0; i < NB_WAYPOINT; i++) {
166 }
167}
168
173
180void nav_move_waypoint(uint8_t wp_id, float ux, float uy, float alt)
181{
182 if (wp_id < nb_waypoint) {
183 float dx, dy;
188 waypoints[wp_id].x = waypoints[WP_HOME].x + dx;
189 waypoints[wp_id].y = waypoints[WP_HOME].y + dy;
190 waypoints[wp_id].a = alt;
191 }
192}
193
200void nav_move_waypoint_enu(uint8_t wp_id, float x, float y, float alt)
201{
202 if (wp_id < nb_waypoint) {
203 float dx, dy;
204 dx = x - waypoints[WP_HOME].x;
205 dy = y - waypoints[WP_HOME].y;
208 waypoints[wp_id].x = waypoints[WP_HOME].x + dx;
209 waypoints[wp_id].y = waypoints[WP_HOME].y + dy;
210 waypoints[wp_id].a = alt;
211 }
212}
213
219{
220 nav_move_waypoint_enu(wp_id, p->x, p->y, p->a);
221}
222
227{
228 if (wp_id < nb_waypoint) {
229 struct UtmCoor_f utm;
231 utm.east = waypoints[wp_id].x + nav_utm_east0;
232 utm.north = waypoints[wp_id].y + nav_utm_north0;
233 utm.alt = waypoints[wp_id].a;
234 DOWNLINK_SEND_WP_MOVED(DefaultChannel, DefaultDevice, &wp_id, &utm.east, &utm.north, &utm.alt, &utm.zone);
235 }
236}
237
uint16_t stage_time
In s.
uint16_t block_time
float get_time_to_home(void)
Compute time to home use wind and airspeed when available.
Definition common_nav.c:71
void common_nav_periodic_task()
Definition common_nav.c:169
void nav_reset_reference(void)
Reset the geographic reference to the current GPS fix.
Definition common_nav.c:135
void nav_send_waypoint(uint8_t wp_id)
Send a waypoint throught default telemetry channel.
Definition common_nav.c:226
float max_dist_from_home
Definition common_nav.c:51
float dist2_to_wp
Definition common_nav.c:39
void nav_reset_alt(void)
Reset the altitude reference to the current GPS alt.
Definition common_nav.c:151
int32_t nav_utm_east0
Definition common_nav.c:48
bool too_far_from_home
Definition common_nav.c:41
void compute_dist2_to_home(void)
Computes squared distance to the HOME waypoint.
Definition common_nav.c:56
void nav_move_waypoint_point(uint8_t wp_id, struct point *p)
Move a waypoint from point structure (local frame).
Definition common_nav.c:218
uint8_t nav_utm_zone0
Definition common_nav.c:50
float ground_alt
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition common_nav.c:46
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:200
void nav_reset_utm_zone(void)
Reset the UTM zone to current GPS fix.
Definition common_nav.c:107
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition common_nav.c:44
float dist2_to_home
Definition common_nav.c:38
const uint8_t nb_waypoint
Definition common_nav.c:43
int32_t nav_utm_north0
Definition common_nav.c:49
static float previous_ground_alt
Definition common_nav.c:104
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:180
void nav_update_waypoints_alt(void)
Shift altitude of the waypoint according to a new ground altitude.
Definition common_nav.c:161
float y
Definition common_nav.h:41
float a
Definition common_nav.h:42
float x
Definition common_nav.h:40
struct GpsState gps
global GPS state
Definition gps.c:74
Device independent GPS code (interface)
struct UtmCoor_i utm_pos
position in UTM (north,east: cm; alt: mm over MSL)
Definition gps.h:93
uint8_t valid_fields
bitfield indicating valid fields (GPS_VALID_x_BIT)
Definition gps.h:88
#define GPS_VALID_POS_UTM_BIT
Definition gps.h:50
static float float_vect2_norm(struct FloatVect2 *v)
uint8_t zone
UTM zone number.
static struct UtmCoor_f * stateGetUtmOrigin_f(void)
Get the coordinate UTM frame origin (int)
Definition state.h:576
static void stateSetLocalUtmOrigin_f(uint16_t id, struct UtmCoor_f *utm_def)
Set the local (flat earth) coordinate frame origin from UTM (float).
Definition state.h:541
float stateGetHmslOrigin_f(void)
Get the HMSL of the frame origin (float)
Definition state.c:204
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition state.h:848
static bool stateIsWindspeedValid(void)
test if wind speed is available.
Definition state.h:1399
static bool stateIsAirspeedValid(void)
test if air speed is available.
Definition state.h:1411
static float stateGetAirspeed_f(void)
Get airspeed (float).
Definition state.h:1590
static struct FloatVect2 * stateGetHorizontalWindspeed_f(void)
Get horizontal windspeed (float).
Definition state.h:1560
Integrated Navigation System interface.
#define INS_RESET_VERTICAL_REF
Definition ins.h:37
#define INS_RESET_REF
flags for INS reset
Definition ins.h:36
static float p[2][2]
uint16_t foo
Definition main_demo5.c:58
#define NAVIGATION_FREQUENCY
Default fixedwing navigation frequency.
Definition nav.h:49
void lla_of_utm_f(struct LlaCoor_f *lla, struct UtmCoor_f *utm)
void utm_of_lla_f(struct UtmCoor_f *utm, struct LlaCoor_f *lla)
Paparazzi floating point math for geodetic calculations.
float alt
in meters (normally above WGS84 reference ellipsoid)
float y
in meters
float x
in meters
uint8_t zone
UTM zone number.
float east
in meters
float north
in meters
vector in East North Up coordinates Units: meters
vector in Latitude, Longitude and Altitude
position in UTM coordinates Units: meters
API to get/set the generic vehicle states.
int int32_t
Typedef defining 32 bit int type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.