[dox][navigation] cleanup indentation, add headers and doxygen file blocks

This commit is contained in:
Felix Ruess
2013-01-18 01:01:29 +01:00
parent fedcde1366
commit 930a3bb655
32 changed files with 734 additions and 428 deletions
@@ -19,6 +19,13 @@
* Boston, MA 02111-1307, USA. * Boston, MA 02111-1307, USA.
*/ */
/**
* @file firmwares/rotorcraft/navigation.c
*
* Rotorcraft navigation functions.
*/
#define NAV_C #define NAV_C
#include "firmwares/rotorcraft/navigation.h" #include "firmwares/rotorcraft/navigation.h"
@@ -19,6 +19,12 @@
* Boston, MA 02111-1307, USA. * Boston, MA 02111-1307, USA.
*/ */
/**
* @file firmwares/rotorcraft/navigation.h
*
* Rotorcraft navigation functions.
*/
#ifndef NAVIGATION_H #ifndef NAVIGATION_H
#define NAVIGATION_H #define NAVIGATION_H
+8 -6
View File
@@ -17,10 +17,11 @@
* along with paparazzi; see the file COPYING. If not, write to * along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330, * the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA. * Boston, MA 02111-1307, USA.
*
*/ */
/** \file subsystems/nav.c
* \brief Regroup functions to compute navigation /**
* @file subsystems/nav.c
* Fixedwing functions to compute navigation.
* *
*/ */
@@ -135,8 +136,9 @@ void nav_circle_XY(float x, float y, float radius) {
float alpha_carrot = nav_circle_trigo_qdr - sign_radius * carrot_angle; float alpha_carrot = nav_circle_trigo_qdr - sign_radius * carrot_angle;
horizontal_mode = HORIZONTAL_MODE_CIRCLE; horizontal_mode = HORIZONTAL_MODE_CIRCLE;
float radius_carrot = abs_radius; float radius_carrot = abs_radius;
if (nav_mode == NAV_MODE_COURSE) if (nav_mode == NAV_MODE_COURSE) {
radius_carrot += (abs_radius / cos(carrot_angle) - abs_radius); radius_carrot += (abs_radius / cos(carrot_angle) - abs_radius);
}
fly_to_xy(x+cos(alpha_carrot)*radius_carrot, fly_to_xy(x+cos(alpha_carrot)*radius_carrot,
y+sin(alpha_carrot)*radius_carrot); y+sin(alpha_carrot)*radius_carrot);
nav_in_circle = TRUE; nav_in_circle = TRUE;
@@ -152,7 +154,7 @@ void nav_circle_XY(float x, float y, float radius) {
float alt = start_alt + nav_leg_progress * diff_alt; \ float alt = start_alt + nav_leg_progress * diff_alt; \
float pre_climb = (*stateGetHorizontalSpeedNorm_f()) * diff_alt / nav_leg_length; \ float pre_climb = (*stateGetHorizontalSpeedNorm_f()) * diff_alt / nav_leg_length; \
NavVerticalAltitudeMode(alt, pre_climb); \ NavVerticalAltitudeMode(alt, pre_climb); \
} }
@@ -184,7 +186,7 @@ void nav_circle_XY(float x, float y, float radius) {
} \ } \
} \ } \
nav_circle_XY(carrot_x, carrot_y, radius); \ nav_circle_XY(carrot_x, carrot_y, radius); \
} }
#define NavFollow(_ac_id, _distance, _height) \ #define NavFollow(_ac_id, _distance, _height) \
+5 -4
View File
@@ -17,14 +17,15 @@
* along with paparazzi; see the file COPYING. If not, write to * along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330, * the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA. * Boston, MA 02111-1307, USA.
*
*/ */
/** \file nav.h /**
* \brief Navigation library * @file subsystems/nav.h
*
* Fixedwing Navigation library.
* *
* This collection of macros and functions is used by the C code generated * This collection of macros and functions is used by the C code generated
* from the XML flight plan * from the XML flight plan.
*/ */
#ifndef NAV_H #ifndef NAV_H
+4
View File
@@ -17,6 +17,10 @@
* along with paparazzi; see the file COPYING. If not, write to * along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330, * the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA. * Boston, MA 02111-1307, USA.
*/
/**
* @file subsystems/navigation/bomb.c
* *
*/ */
+27 -1
View File
@@ -1,3 +1,29 @@
/*
* Copyright (C) 2007-2009 ENAC, Pascal Brisset, Antoine Drouin
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/**
* @file subsystems/navigation/bomb.h
*
*/
#ifndef BOMB_H #ifndef BOMB_H
#define BOMB_H #define BOMB_H
@@ -23,4 +49,4 @@ extern unit_t compute_baseleg( void );
extern float baseleg_alt, downwind_altitude; extern float baseleg_alt, downwind_altitude;
extern const float baseleg_alt_tolerance; extern const float baseleg_alt_tolerance;
#endif #endif // BOMB_H
@@ -1,5 +1,6 @@
/* /*
* Copyright (C) 2012 Tobias Muench * Copyright (C) 2012 Tobias Muench
* modified nav_linie by Anton Kochevar, ENAC
* *
* This file is part of paparazzi. * This file is part of paparazzi.
* *
@@ -17,12 +18,8 @@
* along with paparazzi; see the file COPYING. If not, write to * along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330, * the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA. * Boston, MA 02111-1307, USA.
*
*/ */
//modified nav_linie by Anton Kochevar, ENAC
/** /**
* @file subsystems/navigation/border_line.c * @file subsystems/navigation/border_line.c
* @brief navigate along a border line (line 1-2) with turns in the same direction * @brief navigate along a border line (line 1-2) with turns in the same direction
@@ -30,7 +27,7 @@
* you can use this function to navigate along a border if it is essetial not to cross it * you can use this function to navigate along a border if it is essetial not to cross it
* navigation is along line p1, p2 with turns in the same direction to make sure you dont cross the line * navigation is along line p1, p2 with turns in the same direction to make sure you dont cross the line
* take care youre navigation radius is not to small in strong wind conditions! * take care youre navigation radius is not to small in strong wind conditions!
*/ */
#include "subsystems/navigation/border_line.h" #include "subsystems/navigation/border_line.h"
#include "generated/airframe.h" #include "generated/airframe.h"
@@ -17,7 +17,6 @@
* along with paparazzi; see the file COPYING. If not, write to * along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330, * the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA. * Boston, MA 02111-1307, USA.
*
*/ */
/** /**
@@ -33,6 +32,4 @@
extern bool_t border_line_init( void ); extern bool_t border_line_init( void );
extern bool_t border_line(uint8_t wp1, uint8_t wp2, float radius); extern bool_t border_line(uint8_t wp1, uint8_t wp2, float radius);
#endif #endif /* BORDER_LINE_H */
/* BORDER_LINE_H */
@@ -17,7 +17,11 @@
* along with paparazzi; see the file COPYING. If not, write to * along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330, * the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA. * Boston, MA 02111-1307, USA.
* */
/**
* @file subsystems/navigation/common_flight_plan.c
* Common flight_plan functions shared between fixedwing and rotorcraft.
*/ */
#include "subsystems/navigation/common_flight_plan.h" #include "subsystems/navigation/common_flight_plan.h"
@@ -34,7 +38,6 @@ uint8_t nav_stage, nav_block;
uint8_t last_block, last_stage; uint8_t last_block, last_stage;
void nav_init_block(void) { void nav_init_block(void) {
if (nav_block >= NB_BLOCK) if (nav_block >= NB_BLOCK)
nav_block=NB_BLOCK-1; nav_block=NB_BLOCK-1;
@@ -17,7 +17,11 @@
* along with paparazzi; see the file COPYING. If not, write to * along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330, * the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA. * Boston, MA 02111-1307, USA.
* */
/**
* @file subsystems/navigation/common_flight_plan.c
* Common flight_plan functions shared between fixedwing and rotorcraft.
*/ */
#ifndef COMMON_FLIGHT_PLAN_H #ifndef COMMON_FLIGHT_PLAN_H
@@ -17,6 +17,10 @@
* along with paparazzi; see the file COPYING. If not, write to * along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330, * the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA. * Boston, MA 02111-1307, USA.
*/
/**
* @file subsystems/navigation/common_nav.c
* *
*/ */
@@ -17,6 +17,10 @@
* along with paparazzi; see the file COPYING. If not, write to * along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330, * the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA. * Boston, MA 02111-1307, USA.
*/
/**
* @file subsystems/navigation/common_nav.h
* *
*/ */
@@ -1,3 +1,29 @@
/*
* Copyright (C) 2003-2005 Pascal Brisset, Antoine Drouin
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/**
* @file subsystems/navigation/discsurvey.c
*
*/
#include "subsystems/navigation/discsurvey.h" #include "subsystems/navigation/discsurvey.h"
#include "generated/airframe.h" #include "generated/airframe.h"
+29 -3
View File
@@ -1,9 +1,35 @@
#ifndef SURVEY_H /*
#define SURVEY_H * Copyright (C) 2003-2005 Pascal Brisset, Antoine Drouin
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/**
* @file subsystems/navigation/discsurvey.h
*
*/
#ifndef DISCSURVEY_H
#define DISCSURVEY_H
#include "std.h" #include "std.h"
extern bool_t disc_survey_init( float grid ); extern bool_t disc_survey_init( float grid );
extern bool_t disc_survey(uint8_t c, float radius); extern bool_t disc_survey(uint8_t c, float radius);
#endif /* SURVEY_H */ #endif /* DISCSURVEY_H */
+7 -20
View File
@@ -17,18 +17,12 @@
* along with paparazzi; see the file COPYING. If not, write to * along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330, * the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA. * Boston, MA 02111-1307, USA.
*
*/ */
/** \file flightzone.c /**
* \brief check whether a point is inside the polygon limiting the * @file subsystems/navigation/flightzone.c
* competition area
*
* filename: flightzone.c
* project: MAV 2007
* description: check whether a point is inside the polygon limiting
* the competition area
* *
* Check whether a point is inside the polygon limiting the competition area.
* *
* todo: - support concave/convex polygons * todo: - support concave/convex polygons
* - sort points automatically * - sort points automatically
@@ -48,7 +42,7 @@
typedef struct { COORD_TYPE x; typedef struct { COORD_TYPE x;
COORD_TYPE y; COORD_TYPE y;
} POINT; } POINT;
POINT Corner[] = { POINT Corner[] = {
12, 18, 12, 18,
@@ -83,8 +77,7 @@ void vInitIsInsideBoundaries(void)
#if 0 #if 0
printf("%d: corner (%f, %f), orthogonal (%f, %f)\n", printf("%d: corner (%f, %f), orthogonal (%f, %f)\n",
i, i, Corner[i].x, Corner[i].y,
Corner[i].x, Corner[i].y,
Orthogonal[i].x, Orthogonal[i].y); Orthogonal[i].x, Orthogonal[i].y);
#endif #endif
} }
@@ -107,13 +100,9 @@ int iIsInsideBoundaries(COORD_TYPE x, COORD_TYPE y)
i = 0; i = 0;
while ( (i < bNumberOfCorners) while ((i < bNumberOfCorners) && (r == 1))
&& (r == 1)
)
{ {
if ( ( (x - Corner[i].x) * Orthogonal[i].x if (((x - Corner[i].x) * Orthogonal[i].x + (y - Corner[i].y) * Orthogonal[i].y) < 0.)
+ (y - Corner[i].y) * Orthogonal[i].y
) < 0. )
{ {
r = 0; r = 0;
} }
@@ -123,5 +112,3 @@ int iIsInsideBoundaries(COORD_TYPE x, COORD_TYPE y)
return r; return r;
} }
+8 -10
View File
@@ -17,19 +17,12 @@
* along with paparazzi; see the file COPYING. If not, write to * along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330, * the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA. * Boston, MA 02111-1307, USA.
*
*/ */
/** \file flightzone.h /**
* \brief check whether a point is inside the polygon limiting the * @file subsystems/navigation/flightzone.c
* competition area
*
* filename: flightzone.h
* project: MAV 2007
* description: check whether a point is inside the polygon limiting
* the competition area
*
* *
* Check whether a point is inside the polygon limiting the competition area.
* *
* author: Arnold Schroeter * author: Arnold Schroeter
* history: * history:
@@ -37,7 +30,12 @@
* *
*/ */
#ifndef FLIGHTZONE_H
#define FLIGHTZONE_H
#define COORD_TYPE float #define COORD_TYPE float
void vInitIsInsideBoundaries(void); void vInitIsInsideBoundaries(void);
int iIsInsideBoundaries(COORD_TYPE x, COORD_TYPE y); int iIsInsideBoundaries(COORD_TYPE x, COORD_TYPE y);
#endif /* FLIGHTZONE_H */
+2 -17
View File
@@ -18,7 +18,6 @@
* along with paparazzi; see the file COPYING. If not, write to * along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330, * the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA. * Boston, MA 02111-1307, USA.
*
*/ */
/** /**
@@ -40,7 +39,7 @@
* 1 - only efective with useairspeed flag * 1 - only efective with useairspeed flag
* 2 - defauld is a approach angle of 5 degree which should be fine for most planes * 2 - defauld is a approach angle of 5 degree which should be fine for most planes
* 3 - distance between approach fix and top of decent * 3 - distance between approach fix and top of decent
*/ */
@@ -124,16 +123,13 @@ bool_t gls_init(uint8_t _af, uint8_t _tod, uint8_t _td) {
bool_t gls(uint8_t _af, uint8_t _tod, uint8_t _td) { bool_t gls(uint8_t _af, uint8_t _tod, uint8_t _td) {
if (init){ if (init){
#if USE_AIRSPEED #if USE_AIRSPEED
v_ctl_auto_airspeed_setpoint = target_speed; // set target speed for approach v_ctl_auto_airspeed_setpoint = target_speed; // set target speed for approach
#endif #endif
init = FALSE; init = FALSE;
} }
float final_x = WaypointX(_td) - WaypointX(_tod); float final_x = WaypointX(_td) - WaypointX(_tod);
float final_y = WaypointY(_td) - WaypointY(_tod); float final_y = WaypointY(_td) - WaypointY(_tod);
float final2 = Max(final_x * final_x + final_y * final_y, 1.); float final2 = Max(final_x * final_x + final_y * final_y, 1.);
@@ -151,27 +147,16 @@ bool_t gls(uint8_t _af, uint8_t _tod, uint8_t _td) {
Bound(alt, WaypointAlt(_td), start_alt +(pre_climb/(-v_ctl_altitude_pgain))) // to prevent climbing before intercept Bound(alt, WaypointAlt(_td), start_alt +(pre_climb/(-v_ctl_altitude_pgain))) // to prevent climbing before intercept
if(nav_final_progress < -0.5) { // for smooth intercept if(nav_final_progress < -0.5) { // for smooth intercept
NavVerticalAltitudeMode(WaypointAlt(_tod), 0); // vertical mode (fly straigt and intercept glideslope) NavVerticalAltitudeMode(WaypointAlt(_tod), 0); // vertical mode (fly straigt and intercept glideslope)
NavVerticalAutoThrottleMode(0); // throttle mode NavVerticalAutoThrottleMode(0); // throttle mode
NavSegment(_af, _td); // horizontal mode (stay on localiser) NavSegment(_af, _td); // horizontal mode (stay on localiser)
} }
else { else {
NavVerticalAltitudeMode(alt, pre_climb); // vertical mode (folow glideslope) NavVerticalAltitudeMode(alt, pre_climb); // vertical mode (folow glideslope)
NavVerticalAutoThrottleMode(0); // throttle mode NavVerticalAutoThrottleMode(0); // throttle mode
NavSegment(_af, _td); // horizontal mode (stay on localiser) NavSegment(_af, _td); // horizontal mode (stay on localiser)
} }
return TRUE;
return TRUE;
} // end of gls() } // end of gls()
+1 -5
View File
@@ -1,5 +1,4 @@
/* /*
*
* Copyright (C) 2012, Tobias Muench * Copyright (C) 2012, Tobias Muench
* *
* This file is part of paparazzi. * This file is part of paparazzi.
@@ -18,7 +17,6 @@
* along with paparazzi; see the file COPYING. If not, write to * along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330, * the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA. * Boston, MA 02111-1307, USA.
*
*/ */
/** /**
@@ -32,9 +30,7 @@
#include "std.h" #include "std.h"
#include "paparazzi.h" #include "paparazzi.h"
extern bool_t gls_init(uint8_t _af, uint8_t _tod, uint8_t _td); extern bool_t gls_init(uint8_t _af, uint8_t _tod, uint8_t _td);
extern bool_t gls(uint8_t _af, uint8_t _tod, uint8_t _td); extern bool_t gls(uint8_t _af, uint8_t _tod, uint8_t _td);
#endif #endif // NAV_GLS_H
+4 -3
View File
@@ -17,11 +17,12 @@
* along with paparazzi; see the file COPYING. If not, write to * along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330, * the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA. * Boston, MA 02111-1307, USA.
*
*/ */
/** \file nav_cube.c /**
* \brief Navigation in a cube towards a center * @file subsystems/navigation/nav_cube.c
*
* Fixedwing Navigation in a cube towards a center.
* *
*/ */
+20 -14
View File
@@ -17,6 +17,12 @@
* along with paparazzi; see the file COPYING. If not, write to * along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330, * the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA. * Boston, MA 02111-1307, USA.
*/
/**
* @file subsystems/navigation/nav_cube.h
*
* Fixedwing Navigation in a cube towards a center.
* *
*/ */
@@ -33,22 +39,22 @@ bool_t nav_cube(int8_t j, int8_t i,
uint8_t dest_b, uint8_t dest_e, uint8_t dest_b, uint8_t dest_e,
uint8_t src_b, uint8_t src_e); uint8_t src_b, uint8_t src_e);
extern int32_t cube_alpha; /* angle of flight direction to north, clockwise */ extern int32_t cube_alpha; ///< angle of flight direction to north, clockwise
extern int32_t cube_size_x; /* size of the cube x (perpendicular to flight dir) */ extern int32_t cube_size_x; ///< size of the cube x (perpendicular to flight dir)
extern int32_t cube_size_y; /* size of the cube y (in flight dir) */ extern int32_t cube_size_y; ///< size of the cube y (in flight dir)
extern int32_t cube_size_z; /* height of the cube z */ extern int32_t cube_size_z; ///< height of the cube z
extern int32_t cube_grid_x; /* grid distance x (horizontal) */ extern int32_t cube_grid_x; ///< grid distance x (horizontal)
extern int32_t cube_grid_z; /* grid distance z (vertical) */ extern int32_t cube_grid_z; ///< grid distance z (vertical)
extern int32_t cube_offs_x; /* offset to center x (horizontal) */ extern int32_t cube_offs_x; ///< offset to center x (horizontal)
extern int32_t cube_offs_y; /* offset to center y (in direction) */ extern int32_t cube_offs_y; ///< offset to center y (in direction)
extern int32_t cube_offs_z; /* offset to center z (vertical) */ extern int32_t cube_offs_z; ///< offset to center z (vertical)
extern int32_t cube_sect; /* sector to fly in (1..[nsect_x*nsect_z]) */ extern int32_t cube_sect; ///< sector to fly in (1..[nsect_x*nsect_z])
extern int32_t cube_nsect_x; /* number of sectors horizontal */ extern int32_t cube_nsect_x; ///< number of sectors horizontal
extern int32_t cube_nsect_z; /* number of sectors vertical */ extern int32_t cube_nsect_z; ///< number of sectors vertical
extern int32_t cube_nline_x; /* number of lines x (horizontal) */ extern int32_t cube_nline_x; ///< number of lines x (horizontal)
extern int32_t cube_nline_z; /* number of lines z (vertical) */ extern int32_t cube_nline_z; ///< number of lines z (vertical)
#define nav_cube_SetAlpha(i) { cube_alpha=i; } #define nav_cube_SetAlpha(i) { cube_alpha=i; }
#define nav_cube_SetSect(i) { cube_sect=i; } #define nav_cube_SetSect(i) { cube_sect=i; }
+3 -3
View File
@@ -17,12 +17,12 @@
* along with paparazzi; see the file COPYING. If not, write to * along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330, * the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA. * Boston, MA 02111-1307, USA.
*
*/ */
/** \file nav_line.c /**
* \brief Navigation along a line with nice U-turns * @file subsystems/navigation/nav_line.c
* *
* Fixedwing navigation along a line with nice U-turns.
*/ */
#include "generated/airframe.h" #include "generated/airframe.h"
@@ -17,7 +17,12 @@
* along with paparazzi; see the file COPYING. If not, write to * along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330, * the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA. * Boston, MA 02111-1307, USA.
*/
/**
* @file subsystems/navigation/nav_line.h
* *
* Fixedwing navigation along a line with nice U-turns.
*/ */
#ifndef NAV_LINE_H #ifndef NAV_LINE_H
@@ -1,3 +1,32 @@
/*
* Copyright (C) 2007-2009 ENAC, Pascal Brisset, Antoine Drouin
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/**
* @file subsystems/navigation/nav_survey_rectangle.c
*
* Automatic survey of a rectangle for fixedwings.
*
* Rectangle is defined by two points, sweep can be south-north or west-east.
*/
#include "subsystems/navigation/nav_survey_rectangle.h" #include "subsystems/navigation/nav_survey_rectangle.h"
#include "state.h" #include "state.h"
@@ -1,4 +1,31 @@
/** Automatic survey of a rectangle (defined by two points) (south-north or west-east sweep) */ /*
* Copyright (C) 2007-2009 ENAC, Pascal Brisset, Antoine Drouin
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/**
* @file subsystems/navigation/nav_survey_rectangle.c
*
* Automatic survey of a rectangle for fixedwings.
*
* Rectangle is defined by two points, sweep can be south-north or west-east.
*/
#ifndef NAV_SURVEY_RECTANGLE_H #ifndef NAV_SURVEY_RECTANGLE_H
#define NAV_SURVEY_RECTANGLE_H #define NAV_SURVEY_RECTANGLE_H
@@ -14,4 +41,4 @@ extern void nav_survey_rectangle(uint8_t wp1, uint8_t wp2);
#define NavSurveyRectangle(_wp1, _wp2) nav_survey_rectangle(_wp1, _wp2) #define NavSurveyRectangle(_wp1, _wp2) nav_survey_rectangle(_wp1, _wp2)
#endif #endif // NAV_SURVEY_RECTANGLE_H
@@ -1,3 +1,31 @@
/*
* Copyright (C) 2011 The Paparazzi Team
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/**
* @file subsystems/navigation/poly_survey_adv.c
*
* Advanced polygon survey for fixedwings from Uni Stuttgart.
*
*/
#include "poly_survey_adv.h" #include "poly_survey_adv.h"
#include "subsystems/nav.h" #include "subsystems/nav.h"
@@ -10,9 +38,9 @@
#endif #endif
/** /*
The following variables are set by poly_survey_init and not changed later on The following variables are set by poly_survey_init and not changed later on
**/ */
// precomputed vectors to ease calculations // precomputed vectors to ease calculations
point2d dir_vec; point2d dir_vec;
@@ -33,19 +61,19 @@ float psa_altitude;
int segment_angle; int segment_angle;
int return_angle; int return_angle;
/** /*
The Following variables are dynamic, changed while navigating. The Following variables are dynamic, changed while navigating.
**/ */
/* /*
psa_stage starts at ENTRY and than circles trought the other psa_stage starts at ENTRY and than circles trought the other
states until to polygon is completely covered states until to polygon is completely covered
ENTRY : getting in the right position and height for the first flyover ENTRY : getting in the right position and height for the first flyover
SEG : fly from seg_start to seg_end and take pictures, SEG : fly from seg_start to seg_end and take pictures,
then calculate navigation points of next flyover then calculate navigation points of next flyover
TURN1 : do a 180° turn around seg_center1 TURN1 : do a 180° turn around seg_center1
RET : fly from ret_start to ret_end RET : fly from ret_start to ret_end
TURN2 : do a 180° turn around seg_center2 TURN2 : do a 180° turn around seg_center2
*/ */
survey_stage psa_stage; survey_stage psa_stage;
@@ -76,13 +104,13 @@ static void nav_points(point2d start, point2d end)
} }
/** /**
intercept two lines and give back the point of intersection * intercept two lines and give back the point of intersection
returns : FALSE if no intersection can be found or intersection does not lie between points a and b * @return FALSE if no intersection can be found or intersection does not lie between points a and b
else TRUE * else TRUE
p : returns intersection * @param p returns intersection
x, y : first line is defined by point x and y (goes through this points) * @param x, y first line is defined by point x and y (goes through this points)
a1, a2, b1, b2 : second line by coordinates a1/a2, b1/b2 * @param a1, a2, b1, b2 second line by coordinates a1/a2, b1/b2
**/ */
static bool_t intercept_two_lines(point2d *p, point2d x, point2d y, float a1, float a2, float b1, float b2) static bool_t intercept_two_lines(point2d *p, point2d x, point2d y, float a1, float a2, float b1, float b2)
{ {
float divider, fac; float divider, fac;
@@ -100,11 +128,11 @@ static bool_t intercept_two_lines(point2d *p, point2d x, point2d y, float a1, fl
} }
/** /**
intersects a line with the polygon and gives back the two intersection points * intersects a line with the polygon and gives back the two intersection points
returns : TRUE if two intersection can be found, else FALSE * @return TRUE if two intersection can be found, else FALSE
x, y : intersection points * @param x, y intersection points
a, b : define the line to intersection * @param a, b define the line to intersection
**/ */
static bool_t get_two_intersects(point2d *x, point2d *y, point2d a, point2d b) static bool_t get_two_intersects(point2d *x, point2d *y, point2d a, point2d b)
{ {
int i, count = 0; int i, count = 0;
@@ -151,15 +179,15 @@ static bool_t get_two_intersects(point2d *x, point2d *y, point2d a, point2d b)
} }
/** /**
initializes the variables needed for the survey to start * initializes the variables needed for the survey to start
first_wp : the first Waypoint of the polygon * @param first_wp the first Waypoint of the polygon
size : the number of points that make up the polygon * @param size the number of points that make up the polygon
angle : angle in which to do the flyovers * @param angle angle in which to do the flyovers
sweep_width : distance between the sweeps * @param sweep_width distance between the sweeps
shot_dist : distance between the shots * @param shot_dist distance between the shots
min_rad : minimal radius when navigating * @param min_rad minimal radius when navigating
altitude : the altitude that must be reached before the flyover starts * @param altitude the altitude that must be reached before the flyover starts
**/ **/
bool_t init_poly_survey_adv(uint8_t first_wp, uint8_t size, float angle, float sweep_width, float shot_dist, float min_rad, float altitude) bool_t init_poly_survey_adv(uint8_t first_wp, uint8_t size, float angle, float sweep_width, float shot_dist, float min_rad, float altitude)
{ {
int i; int i;
@@ -265,10 +293,10 @@ bool_t init_poly_survey_adv(uint8_t first_wp, uint8_t size, float angle, float s
} }
/** /**
main navigation routine. This is called periodically evaluates the current * main navigation routine. This is called periodically evaluates the current
Position and stage and navigates accordingly. * Position and stage and navigates accordingly.
Returns True until the survey is finished * Returns True until the survey is finished
**/ */
bool_t poly_survey_adv(void) bool_t poly_survey_adv(void)
{ {
NavVerticalAutoThrottleMode(0.0); NavVerticalAutoThrottleMode(0.0);
@@ -1,3 +1,31 @@
/*
* Copyright (C) 2011 The Paparazzi Team
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/**
* @file subsystems/navigation/poly_survey_adv.h
*
* Advanced polygon survey for fixedwings from Uni Stuttgart.
*
*/
#ifndef POLY_ADV_H #ifndef POLY_ADV_H
#define POLY_ADV_H #define POLY_ADV_H
+27 -2
View File
@@ -1,5 +1,30 @@
/* Smooth navigation to wp_a along an arc (around wp_cd), a /*
segment (from wp_rd to wp_ta) and a second arc (around wp_ca) */ * Copyright (C) 2007-2009 ENAC, Pascal Brisset, Antoine Drouin
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/**
* @file subsystems/navigation/snav.c
*
* Smooth navigation to wp_a along an arc (around wp_cd),
* a segment (from wp_rd to wp_ta) and a second arc (around wp_ca).
*/
#include <math.h> #include <math.h>
#include "generated/airframe.h" #include "generated/airframe.h"
+28
View File
@@ -1,3 +1,31 @@
/*
* Copyright (C) 2007-2009 ENAC, Pascal Brisset, Antoine Drouin
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/**
* @file subsystems/navigation/snav.h
*
* Smooth navigation to wp_a along an arc (around wp_cd),
* a segment (from wp_rd to wp_ta) and a second arc (around wp_ca).
*/
#ifndef SNAV_H #ifndef SNAV_H
#define SNAV_H #define SNAV_H
+30 -6
View File
@@ -1,10 +1,34 @@
/************** Spiral Navigation **********************************************/ /*
* Copyright (C) 2011 The Paparazzi Team
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/** creating a helix: /**
start radius to end radius, increasing after reaching alphamax * @file subsystems/navigation/spiral.c
Alphamax is calculated from given segments *
IMPORTANT: numer of segments has to be larger than 2! * Fixedwing navigation in a spiral/helix from Uni Stuttgart.
*/ *
* creating a helix:
* - start radius to end radius, increasing after reaching alphamax
* - Alphamax is calculated from given segments
* - IMPORTANT: numer of segments has to be larger than 2!
*/
#include "subsystems/navigation/spiral.h" #include "subsystems/navigation/spiral.h"
+29 -1
View File
@@ -1,3 +1,31 @@
/*
* Copyright (C) 2011 The Paparazzi Team
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/**
* @file subsystems/navigation/spiral.h
*
* Fixedwing navigation in a spiral/helix from Uni Stuttgart.
*
*/
#ifndef SPIRAL_H #ifndef SPIRAL_H
#define SPIRAL_H #define SPIRAL_H
@@ -7,5 +35,5 @@ extern bool_t SpiralNav(void);
extern bool_t InitializeSpiral(uint8_t CenterWP, uint8_t EdgeWP, float StartRad, float IncRad, extern bool_t InitializeSpiral(uint8_t CenterWP, uint8_t EdgeWP, float StartRad, float IncRad,
float Segments, float ZKoord ); float Segments, float ZKoord );
#endif #endif // SPIRAL_H
@@ -17,10 +17,12 @@
* along with paparazzi; see the file COPYING. If not, write to * along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330, * the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA. * Boston, MA 02111-1307, USA.
*
*/ */
/** \file traffic_info.c
* \brief Informations relative to the other aircrafts /**
* @file subsystems/naviation/traffic_info.c
*
* Information relative to the other aircrafts.
* *
*/ */
@@ -17,10 +17,12 @@
* along with paparazzi; see the file COPYING. If not, write to * along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330, * the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA. * Boston, MA 02111-1307, USA.
*
*/ */
/** \file traffic_info.h
* \brief Informations relative to the other aircrafts /**
* @file subsystems/navigation/traffic_info.h
*
* Information relative to the other aircrafts.
* *
*/ */
@@ -61,7 +63,7 @@ extern struct ac_info_ the_acs[NB_ACS];
the_acs[the_acs_id[_id]].climb = _climb; \ the_acs[the_acs_id[_id]].climb = _climb; \
the_acs[the_acs_id[_id]].itow = (uint32_t)_itow; \ the_acs[the_acs_id[_id]].itow = (uint32_t)_itow; \
} \ } \
} }
extern void traffic_info_init( void ); extern void traffic_info_init( void );