Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-05 06:36:41 +08:00
Code Issues Actions 25 Packages Projects Releases Wiki Activity

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

Browse Source
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
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/firmwares/rotorcraft/navigation.c
+7
View File
@@ -19,6 +19,13 @@
* Boston, MA 02111-1307, USA.
*/
/**
* @file firmwares/rotorcraft/navigation.c
*
* Rotorcraft navigation functions.
*/
#define NAV_C
#include "firmwares/rotorcraft/navigation.h"
sw/airborne/firmwares/rotorcraft/navigation.h
+6
View File
@@ -19,6 +19,12 @@
* Boston, MA 02111-1307, USA.
*/
/**
* @file firmwares/rotorcraft/navigation.h
*
* Rotorcraft navigation functions.
*/
#ifndef NAVIGATION_H
#define NAVIGATION_H
sw/airborne/subsystems/nav.c
+51 -49
View File
@@ -17,10 +17,11 @@
* 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/nav.c
* \brief Regroup functions to compute navigation
/**
* @file subsystems/nav.c
* Fixedwing functions to compute navigation.
*
*/
@@ -125,7 +126,7 @@ void nav_circle_XY(float x, float y, float radius) {
/** Computes a prebank. Go straight if inside or outside the circle */
circle_bank =
(dist2_center > Square(abs_radius + dist_carrot)
|| dist2_center < Square(abs_radius - dist_carrot)) ?
|| dist2_center < Square(abs_radius - dist_carrot)) ?
0 :
atan((*stateGetHorizontalSpeedNorm_f())*(*stateGetHorizontalSpeedNorm_f()) / (G*radius));
@@ -135,10 +136,11 @@ void nav_circle_XY(float x, float y, float radius) {
float alpha_carrot = nav_circle_trigo_qdr - sign_radius * carrot_angle;
horizontal_mode = HORIZONTAL_MODE_CIRCLE;
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);
}
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_circle_x = x;
nav_circle_y = y;
@@ -146,13 +148,13 @@ void nav_circle_XY(float x, float y, float radius) {
}
#define NavGlide(_last_wp, _wp) { \
float start_alt = waypoints[_last_wp].a; \
float diff_alt = waypoints[_wp].a - start_alt; \
float alt = start_alt + nav_leg_progress * diff_alt; \
float pre_climb = (*stateGetHorizontalSpeedNorm_f()) * diff_alt / nav_leg_length; \
NavVerticalAltitudeMode(alt, pre_climb); \
}
#define NavGlide(_last_wp, _wp) { \
float start_alt = waypoints[_last_wp].a; \
float diff_alt = waypoints[_wp].a - start_alt; \
float alt = start_alt + nav_leg_progress * diff_alt; \
float pre_climb = (*stateGetHorizontalSpeedNorm_f()) * diff_alt / nav_leg_length; \
NavVerticalAltitudeMode(alt, pre_climb); \
}
@@ -161,33 +163,33 @@ void nav_circle_XY(float x, float y, float radius) {
#define MIN_HEIGHT_CARROT 50.
#define MAX_HEIGHT_CARROT 150.
#define Goto3D(radius) { \
if (pprz_mode == PPRZ_MODE_AUTO2) { \
int16_t yaw = fbw_state->channels[RADIO_YAW]; \
if (yaw > MIN_DX || yaw < -MIN_DX) { \
carrot_x += FLOAT_OF_PPRZ(yaw, 0, -20.); \
carrot_x = Min(carrot_x, MAX_DIST_CARROT); \
carrot_x = Max(carrot_x, -MAX_DIST_CARROT); \
} \
int16_t pitch = fbw_state->channels[RADIO_PITCH]; \
if (pitch > MIN_DX || pitch < -MIN_DX) { \
carrot_y += FLOAT_OF_PPRZ(pitch, 0, -20.); \
carrot_y = Min(carrot_y, MAX_DIST_CARROT); \
carrot_y = Max(carrot_y, -MAX_DIST_CARROT); \
} \
v_ctl_mode = V_CTL_MODE_AUTO_ALT; \
int16_t roll = fbw_state->channels[RADIO_ROLL]; \
if (roll > MIN_DX || roll < -MIN_DX) { \
nav_altitude += FLOAT_OF_PPRZ(roll, 0, -1.0); \
nav_altitude = Max(nav_altitude, MIN_HEIGHT_CARROT+ground_alt); \
nav_altitude = Min(nav_altitude, MAX_HEIGHT_CARROT+ground_alt); \
} \
} \
nav_circle_XY(carrot_x, carrot_y, radius); \
}
#define Goto3D(radius) { \
if (pprz_mode == PPRZ_MODE_AUTO2) { \
int16_t yaw = fbw_state->channels[RADIO_YAW]; \
if (yaw > MIN_DX || yaw < -MIN_DX) { \
carrot_x += FLOAT_OF_PPRZ(yaw, 0, -20.); \
carrot_x = Min(carrot_x, MAX_DIST_CARROT); \
carrot_x = Max(carrot_x, -MAX_DIST_CARROT); \
} \
int16_t pitch = fbw_state->channels[RADIO_PITCH]; \
if (pitch > MIN_DX || pitch < -MIN_DX) { \
carrot_y += FLOAT_OF_PPRZ(pitch, 0, -20.); \
carrot_y = Min(carrot_y, MAX_DIST_CARROT); \
carrot_y = Max(carrot_y, -MAX_DIST_CARROT); \
} \
v_ctl_mode = V_CTL_MODE_AUTO_ALT; \
int16_t roll = fbw_state->channels[RADIO_ROLL]; \
if (roll > MIN_DX || roll < -MIN_DX) { \
nav_altitude += FLOAT_OF_PPRZ(roll, 0, -1.0); \
nav_altitude = Max(nav_altitude, MIN_HEIGHT_CARROT+ground_alt); \
nav_altitude = Min(nav_altitude, MAX_HEIGHT_CARROT+ground_alt); \
} \
} \
nav_circle_XY(carrot_x, carrot_y, radius); \
}
#define NavFollow(_ac_id, _distance, _height) \
#define NavFollow(_ac_id, _distance, _height) \
nav_follow(_ac_id, _distance, _height);
@@ -620,18 +622,18 @@ void nav_oval(uint8_t p1, uint8_t p2, float radius) {
/* The half circle centers and the other leg */
struct point p1_center = { waypoints[p1].x + radius * -u_y,
waypoints[p1].y + radius * u_x,
alt };
waypoints[p1].y + radius * u_x,
alt };
struct point p1_out = { waypoints[p1].x + 2*radius * -u_y,
waypoints[p1].y + 2*radius * u_x,
alt };
waypoints[p1].y + 2*radius * u_x,
alt };
struct point p2_in = { waypoints[p2].x + 2*radius * -u_y,
waypoints[p2].y + 2*radius * u_x,
alt };
waypoints[p2].y + 2*radius * u_x,
alt };
struct point p2_center = { waypoints[p2].x + radius * -u_y,
waypoints[p2].y + radius * u_x,
alt };
waypoints[p2].y + radius * u_x,
alt };
float qdr_out_2 = M_PI - atan2(u_y, u_x);
float qdr_out_1 = qdr_out_2 + M_PI;
@@ -668,7 +670,7 @@ void nav_oval(uint8_t p1, uint8_t p2, float radius) {
InitStage();
LINE_START_FUNCTION;
}
return;
return;
case OR21:
nav_route_xy(waypoints[p2].x, waypoints[p2].y, waypoints[p1].x, waypoints[p1].y);
@@ -679,7 +681,7 @@ void nav_oval(uint8_t p1, uint8_t p2, float radius) {
}
return;
default: /* Should not occur !!! Doing nothing */
return;
default: /* Should not occur !!! Doing nothing */
return;
}
}
sw/airborne/subsystems/nav.h
+5 -4
View File
@@ -17,14 +17,15 @@
* 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 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
* from the XML flight plan
* from the XML flight plan.
*/
#ifndef NAV_H
sw/airborne/subsystems/navigation/bomb.c
+4
View File
@@ -17,6 +17,10 @@
* 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.c
*
*/
sw/airborne/subsystems/navigation/bomb.h
+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
#define BOMB_H
@@ -23,4 +49,4 @@ extern unit_t compute_baseleg( void );
extern float baseleg_alt, downwind_altitude;
extern const float baseleg_alt_tolerance;
#endif
#endif // BOMB_H
sw/airborne/subsystems/navigation/border_line.c
+54 -57
View File
@@ -1,5 +1,6 @@
/*
* Copyright (C) 2012 Tobias Muench
* modified nav_linie by Anton Kochevar, ENAC
*
* This file is part of paparazzi.
*
@@ -17,12 +18,8 @@
* 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.
*
*/
//modified nav_linie by Anton Kochevar, ENAC
/**
* @file subsystems/navigation/border_line.c
* @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
* 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!
*/
*/
#include "subsystems/navigation/border_line.h"
#include "generated/airframe.h"
@@ -60,20 +57,20 @@ bool_t border_line(uint8_t l1, uint8_t l2, float radius) {
float angle = atan2((WaypointY(l1)-WaypointY(l2)),(WaypointX(l2)-WaypointX(l1)));
struct point l2_c1 = { WaypointX(l1) - sin(angle)*radius,
WaypointY(l1) - cos(angle)*radius,
alt };
WaypointY(l1) - cos(angle)*radius,
alt };
struct point l2_c2 = { l2_c1.x + 2*radius*cos(angle) ,
l2_c1.y - 2*radius*sin(angle),
alt };
l2_c1.y - 2*radius*sin(angle),
alt };
struct point l1_c2 = { WaypointX(l2) - sin(angle)*radius,
WaypointY(l2) - cos(angle)*radius,
alt };
WaypointY(l2) - cos(angle)*radius,
alt };
struct point l1_c3 = { l1_c2.x - 2*radius*cos(angle) ,
l1_c2.y + 2*radius*sin(angle),
alt };
l1_c2.y + 2*radius*sin(angle),
alt };
float qdr_out_2_1 = M_PI/3. - atan2(u_y, u_x);
float qdr_out_2_2 = -M_PI/3. - atan2(u_y, u_x);
@@ -84,53 +81,53 @@ bool_t border_line(uint8_t l1, uint8_t l2, float radius) {
NavVerticalAltitudeMode(WaypointAlt(l1), 0.);
switch (border_line_status) {
case LR12:
NavSegment(l2, l1);
if (NavApproachingFrom(l1, l2, CARROT)) {
border_line_status = LQC21;
nav_init_stage();
}
break;
case LQC21:
nav_circle_XY(l2_c1.x, l2_c1.y, -radius);
if (NavQdrCloseTo(DegOfRad(qdr_out_2_2)-10)) {
border_line_status = LTC2;
nav_init_stage();
}
break;
case LTC2:
nav_circle_XY(l2_c2.x, l2_c2.y, radius);
if (NavQdrCloseTo(DegOfRad(qdr_out_2_3)-10)) {
border_line_status = LR21;
nav_init_stage();
}
break;
case LR12:
NavSegment(l2, l1);
if (NavApproachingFrom(l1, l2, CARROT)) {
border_line_status = LQC21;
nav_init_stage();
}
break;
case LQC21:
nav_circle_XY(l2_c1.x, l2_c1.y, -radius);
if (NavQdrCloseTo(DegOfRad(qdr_out_2_2)-10)) {
border_line_status = LTC2;
nav_init_stage();
}
break;
case LTC2:
nav_circle_XY(l2_c2.x, l2_c2.y, radius);
if (NavQdrCloseTo(DegOfRad(qdr_out_2_3)-10)) {
border_line_status = LR21;
nav_init_stage();
}
break;
case LR21:
NavSegment(l1, l2);
if (NavApproachingFrom(l2, l1, CARROT)) {
border_line_status = LTC1;
nav_init_stage();
}
break;
case LR21:
NavSegment(l1, l2);
if (NavApproachingFrom(l2, l1, CARROT)) {
border_line_status = LTC1;
nav_init_stage();
}
break;
case LTC1:
nav_circle_XY(l1_c2.x, l1_c2.y, radius);
if (NavQdrCloseTo(DegOfRad(qdr_out_2_1) + 10)) {
border_line_status = LQC11;
nav_init_stage();
}
break;
case LQC11:
nav_circle_XY(l1_c3.x, l1_c3.y, -radius);
if (NavQdrCloseTo(DegOfRad(qdr_out_2_3 + M_PI + 10))) {
border_line_status = LR12;
nav_init_stage();
}
break;
case LTC1:
nav_circle_XY(l1_c2.x, l1_c2.y, radius);
if (NavQdrCloseTo(DegOfRad(qdr_out_2_1) + 10)) {
border_line_status = LQC11;
nav_init_stage();
}
break;
case LQC11:
nav_circle_XY(l1_c3.x, l1_c3.y, -radius);
if (NavQdrCloseTo(DegOfRad(qdr_out_2_3 + M_PI + 10))) {
border_line_status = LR12;
nav_init_stage();
}
break;
default: /* Should not occur !!! End the pattern */
return FALSE;
default: /* Should not occur !!! End the pattern */
return FALSE;
}
return TRUE; /* This pattern never ends */
}
sw/airborne/subsystems/navigation/border_line.h
+1 -4
View File
@@ -17,7 +17,6 @@
* 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.
*
*/
/**
@@ -33,6 +32,4 @@
extern bool_t border_line_init( void );
extern bool_t border_line(uint8_t wp1, uint8_t wp2, float radius);
#endif
/* BORDER_LINE_H */
#endif /* BORDER_LINE_H */
sw/airborne/subsystems/navigation/common_flight_plan.c
+5 -2
View File
@@ -17,7 +17,11 @@
* 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/common_flight_plan.c
* Common flight_plan functions shared between fixedwing and rotorcraft.
*/
#include "subsystems/navigation/common_flight_plan.h"
@@ -34,7 +38,6 @@ uint8_t nav_stage, nav_block;
uint8_t last_block, last_stage;
void nav_init_block(void) {
if (nav_block >= NB_BLOCK)
nav_block=NB_BLOCK-1;
sw/airborne/subsystems/navigation/common_flight_plan.h
+5 -1
View File
@@ -17,7 +17,11 @@
* 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/common_flight_plan.c
* Common flight_plan functions shared between fixedwing and rotorcraft.
*/
#ifndef COMMON_FLIGHT_PLAN_H
sw/airborne/subsystems/navigation/common_nav.c
+4
View File
@@ -17,6 +17,10 @@
* 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/common_nav.c
*
*/
sw/airborne/subsystems/navigation/common_nav.h
+4
View File
@@ -17,6 +17,10 @@
* 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/common_nav.h
*
*/
sw/airborne/subsystems/navigation/discsurvey.c
+33 -7
View File
@@ -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 "generated/airframe.h"
@@ -42,17 +68,17 @@ bool_t disc_survey( uint8_t center, float radius) {
float d = ScalarProduct(upwind_x, upwind_y, stateGetPositionEnu_f()->x-WaypointX(center), stateGetPositionEnu_f()->y-WaypointY(center));
if (d > radius) {
status = DOWNWIND;
status = DOWNWIND;
} else {
float w = sqrt(radius*radius - d*d) - 1.5*grid;
float w = sqrt(radius*radius - d*d) - 1.5*grid;
float crosswind_x = - upwind_y;
float crosswind_y = upwind_x;
float crosswind_x = - upwind_y;
float crosswind_y = upwind_x;
c2.x = WaypointX(center)+d*upwind_x-w*sign*crosswind_x;
c2.y = WaypointY(center)+d*upwind_y-w*sign*crosswind_y;
c2.x = WaypointX(center)+d*upwind_x-w*sign*crosswind_x;
c2.y = WaypointY(center)+d*upwind_y-w*sign*crosswind_y;
status = SEGMENT;
status = SEGMENT;
}
nav_init_stage();
}
sw/airborne/subsystems/navigation/discsurvey.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"
extern bool_t disc_survey_init( float grid );
extern bool_t disc_survey(uint8_t c, float radius);
#endif /* SURVEY_H */
#endif /* DISCSURVEY_H */
sw/airborne/subsystems/navigation/flightzone.c
+35 -48
View File
@@ -17,18 +17,12 @@
* 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 flightzone.c
* \brief check whether a point is inside the polygon limiting the
* competition area
*
* filename: flightzone.c
* project: MAV 2007
* description: check whether a point is inside the polygon limiting
* the competition area
/**
* @file subsystems/navigation/flightzone.c
*
* Check whether a point is inside the polygon limiting the competition area.
*
* todo: - support concave/convex polygons
* - sort points automatically
@@ -47,17 +41,17 @@
#include "flightzone.h"
typedef struct { COORD_TYPE x;
COORD_TYPE y;
} POINT;
COORD_TYPE y;
} POINT;
POINT Corner[] = {
12, 18,
12, 25,
15, 29,
18, 25,
18, 18,
13.5, 16,
0 , 0}; // last corner is a dummy, which must not be deleted!!!
12, 18,
12, 25,
15, 29,
18, 25,
18, 18,
13.5, 16,
0 , 0}; // last corner is a dummy, which must not be deleted!!!
POINT Orthogonal[20]; // Attention!!! array must be at least as long as Corner[]
@@ -69,25 +63,24 @@ unsigned char bNumberOfCorners = 0;
;*******************************************************************/
void vInitIsInsideBoundaries(void)
{
unsigned char i;
unsigned char i;
bNumberOfCorners = sizeof(Corner)/sizeof(POINT) - 1; // last corner is always a dummy
bNumberOfCorners = sizeof(Corner)/sizeof(POINT) - 1; // last corner is always a dummy
Corner[bNumberOfCorners].x = Corner[0].x;
Corner[bNumberOfCorners].y = Corner[0].y;
Corner[bNumberOfCorners].x = Corner[0].x;
Corner[bNumberOfCorners].y = Corner[0].y;
for (i = 0; i < bNumberOfCorners; i++)
{
Orthogonal[i].x = Corner[i+1].y - Corner[i].y;
Orthogonal[i].y = - (Corner[i+1].x - Corner[i].x);
for (i = 0; i < bNumberOfCorners; i++)
{
Orthogonal[i].x = Corner[i+1].y - Corner[i].y;
Orthogonal[i].y = - (Corner[i+1].x - Corner[i].x);
#if 0
printf("%d: corner (%f, %f), orthogonal (%f, %f)\n",
i,
Corner[i].x, Corner[i].y,
Orthogonal[i].x, Orthogonal[i].y);
printf("%d: corner (%f, %f), orthogonal (%f, %f)\n",
i, Corner[i].x, Corner[i].y,
Orthogonal[i].x, Orthogonal[i].y);
#endif
}
}
}
@@ -102,26 +95,20 @@ void vInitIsInsideBoundaries(void)
int iIsInsideBoundaries(COORD_TYPE x, COORD_TYPE y)
{
int r = 1;
static unsigned char i;
int r = 1;
static unsigned char i;
i = 0;
i = 0;
while ( (i < bNumberOfCorners)
&& (r == 1)
)
while ((i < bNumberOfCorners) && (r == 1))
{
if (((x - Corner[i].x) * Orthogonal[i].x + (y - Corner[i].y) * Orthogonal[i].y) < 0.)
{
if ( ( (x - Corner[i].x) * Orthogonal[i].x
+ (y - Corner[i].y) * Orthogonal[i].y
) < 0. )
{
r = 0;
}
++i;
r = 0;
}
return r;
++i;
}
return r;
}
sw/airborne/subsystems/navigation/flightzone.h
+8 -10
View File
@@ -17,19 +17,12 @@
* 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 flightzone.h
* \brief check whether a point is inside the polygon limiting the
* competition area
*
* filename: flightzone.h
* project: MAV 2007
* description: check whether a point is inside the polygon limiting
* the competition area
*
/**
* @file subsystems/navigation/flightzone.c
*
* Check whether a point is inside the polygon limiting the competition area.
*
* author: Arnold Schroeter
* history:
@@ -37,7 +30,12 @@
*
*/
#ifndef FLIGHTZONE_H
#define FLIGHTZONE_H
#define COORD_TYPE float
void vInitIsInsideBoundaries(void);
int iIsInsideBoundaries(COORD_TYPE x, COORD_TYPE y);
#endif /* FLIGHTZONE_H */
sw/airborne/subsystems/navigation/gls.c
+3 -18
View File
@@ -18,7 +18,6 @@
* 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.
*
*/
/**
@@ -40,7 +39,7 @@
* 1 - only efective with useairspeed flag
* 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
*/
*/
@@ -74,7 +73,7 @@ bool_t init = TRUE;
static inline bool_t gls_compute_TOD(uint8_t _af, uint8_t _tod, uint8_t _td) {
if ((WaypointX(_af)==WaypointX(_td))&&(WaypointY(_af)==WaypointY(_td))){
WaypointX(_af)=WaypointX(_td)-1;
WaypointX(_af)=WaypointX(_td)-1;
}
float td_af_x = WaypointX(_af) - WaypointX(_td);
@@ -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) {
if (init){
#if USE_AIRSPEED
v_ctl_auto_airspeed_setpoint = target_speed; // set target speed for approach
#endif
init = FALSE;
}
float final_x = WaypointX(_td) - WaypointX(_tod);
float final_y = WaypointY(_td) - WaypointY(_tod);
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
if(nav_final_progress < -0.5) { // for smooth intercept
NavVerticalAltitudeMode(WaypointAlt(_tod), 0); // vertical mode (fly straigt and intercept glideslope)
NavVerticalAutoThrottleMode(0); // throttle mode
NavSegment(_af, _td); // horizontal mode (stay on localiser)
}
else {
NavVerticalAltitudeMode(alt, pre_climb); // vertical mode (folow glideslope)
NavVerticalAutoThrottleMode(0); // throttle mode
NavSegment(_af, _td); // horizontal mode (stay on localiser)
}
return TRUE;
return TRUE;
} // end of gls()
sw/airborne/subsystems/navigation/gls.h
+1 -5
View File
@@ -1,5 +1,4 @@
/*
*
* Copyright (C) 2012, Tobias Muench
*
* This file is part of paparazzi.
@@ -18,7 +17,6 @@
* 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.
*
*/
/**
@@ -32,9 +30,7 @@
#include "std.h"
#include "paparazzi.h"
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);
#endif
#endif // NAV_GLS_H
sw/airborne/subsystems/navigation/nav_cube.c
+6 -5
View File
@@ -17,11 +17,12 @@
* 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 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.
*
*/
@@ -83,7 +84,7 @@ bool_t nav_cube_init(uint8_t center, uint8_t tb, uint8_t te) {
/* calculate lower left start begin/end x coord */
start_bx = WaypointX(center) - (((cube_nline_x_t-1) * cube_grid_x)/2)
+ cube_offs_x;
+ cube_offs_x;
start_ex = start_bx;
/* calculate lower left start end point y coord */
@@ -94,7 +95,7 @@ bool_t nav_cube_init(uint8_t center, uint8_t tb, uint8_t te) {
/* calculate lower left start begin/end z coord */
start_bz = waypoints[center].a - (((cube_nline_z_t-1) * cube_grid_z)/2)
+ (cube_line_z_start*cube_grid_z) + cube_offs_z;
+ (cube_line_z_start*cube_grid_z) + cube_offs_z;
start_ez = start_bz;
/* reset all waypoints to the standby position */
sw/airborne/subsystems/navigation/nav_cube.h
+20 -14
View File
@@ -17,6 +17,12 @@
* 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_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 src_b, uint8_t src_e);
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_y; /* size of the cube y (in flight dir) */
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_z; /* grid distance z (vertical) */
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_z; /* offset to center z (vertical) */
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_y; ///< size of the cube y (in flight dir)
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_z; ///< grid distance z (vertical)
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_z; ///< offset to center z (vertical)
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_z; /* number of sectors vertical */
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_z; ///< number of sectors vertical
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_x; ///< number of lines x (horizontal)
extern int32_t cube_nline_z; ///< number of lines z (vertical)
#define nav_cube_SetAlpha(i) { cube_alpha=i; }
#define nav_cube_SetSect(i) { cube_sect=i; }
sw/airborne/subsystems/navigation/nav_line.c
+15 -15
View File
@@ -17,12 +17,12 @@
* 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 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"
@@ -53,24 +53,24 @@ bool_t nav_line(uint8_t l1, uint8_t l2, float radius) {
/* The half circle centers and the other leg */
struct point l2_c1 = { WaypointX(l1) + radius * u_y,
WaypointY(l1) + radius * -u_x,
alt };
WaypointY(l1) + radius * -u_x,
alt };
struct point l2_c2 = { WaypointX(l1) + 1.732*radius * u_x,
WaypointY(l1) + 1.732*radius * u_y,
alt };
WaypointY(l1) + 1.732*radius * u_y,
alt };
struct point l2_c3 = { WaypointX(l1) + radius * -u_y,
WaypointY(l1) + radius * u_x,
alt };
WaypointY(l1) + radius * u_x,
alt };
struct point l1_c1 = { WaypointX(l2) + radius * -u_y,
WaypointY(l2) + radius * u_x,
alt };
WaypointY(l2) + radius * u_x,
alt };
struct point l1_c2 = { WaypointX(l2) +1.732*radius * -u_x,
WaypointY(l2) + 1.732*radius * -u_y,
alt };
WaypointY(l2) + 1.732*radius * -u_y,
alt };
struct point l1_c3 = { WaypointX(l2) + radius * u_y,
WaypointY(l2) + radius * -u_x,
alt };
WaypointY(l2) + radius * -u_x,
alt };
float qdr_out_2_1 = M_PI/3. - atan2(u_y, u_x);
float qdr_out_2_2 = -M_PI/3. - atan2(u_y, u_x);
sw/airborne/subsystems/navigation/nav_line.h
+5
View File
@@ -17,7 +17,12 @@
* 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_line.h
*
* Fixedwing navigation along a line with nice U-turns.
*/
#ifndef NAV_LINE_H
sw/airborne/subsystems/navigation/nav_survey_rectangle.c
+74 -45
View File
@@ -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 "state.h"
@@ -81,62 +110,62 @@ void nav_survey_rectangle(uint8_t wp1, uint8_t wp2) {
if (! survey_uturn) { /* S-N, N-S, W-E or E-W straight route */
if ((stateGetPositionEnu_f()->y < nav_survey_north && SurveyGoingNorth()) ||
(stateGetPositionEnu_f()->y > nav_survey_south && SurveyGoingSouth()) ||
(stateGetPositionEnu_f()->x < nav_survey_east && SurveyGoingEast()) ||
(stateGetPositionEnu_f()->x < nav_survey_east && SurveyGoingEast()) ||
(stateGetPositionEnu_f()->x > nav_survey_west && SurveyGoingWest())) {
/* Continue ... */
nav_route_xy(survey_from.x, survey_from.y, survey_to.x, survey_to.y);
} else {
if (survey_orientation == NS) {
/* North or South limit reached, prepare U-turn and next leg */
float x0 = survey_from.x; /* Current longitude */
if (x0+nav_survey_shift < nav_survey_west || x0+nav_survey_shift > nav_survey_east) {
x0 += nav_survey_shift / 2;
nav_survey_shift = -nav_survey_shift;
}
/* North or South limit reached, prepare U-turn and next leg */
float x0 = survey_from.x; /* Current longitude */
if (x0+nav_survey_shift < nav_survey_west || x0+nav_survey_shift > nav_survey_east) {
x0 += nav_survey_shift / 2;
nav_survey_shift = -nav_survey_shift;
}
x0 = x0 + nav_survey_shift; /* Longitude of next leg */
survey_from.x = survey_to.x = x0;
x0 = x0 + nav_survey_shift; /* Longitude of next leg */
survey_from.x = survey_to.x = x0;
/* Swap South and North extremities */
float tmp = survey_from.y;
survey_from.y = survey_to.y;
survey_to.y = tmp;
/* Swap South and North extremities */
float tmp = survey_from.y;
survey_from.y = survey_to.y;
survey_to.y = tmp;
/** Do half a circle around WP 0 */
waypoints[0].x = x0 - nav_survey_shift/2.;
waypoints[0].y = survey_from.y;
/** Do half a circle around WP 0 */
waypoints[0].x = x0 - nav_survey_shift/2.;
waypoints[0].y = survey_from.y;
/* Computes the right direction for the circle */
survey_radius = nav_survey_shift / 2.;
if (SurveyGoingNorth()) {
survey_radius = -survey_radius;
}
/* Computes the right direction for the circle */
survey_radius = nav_survey_shift / 2.;
if (SurveyGoingNorth()) {
survey_radius = -survey_radius;
}
} else { /* (survey_orientation == WE) */
/* East or West limit reached, prepare U-turn and next leg */
/* There is a y0 declared in math.h (for ARM) !!! */
float my_y0 = survey_from.y; /* Current latitude */
if (my_y0+nav_survey_shift < nav_survey_south || my_y0+nav_survey_shift > nav_survey_north) {
my_y0 += nav_survey_shift / 2;
nav_survey_shift = -nav_survey_shift;
}
/* East or West limit reached, prepare U-turn and next leg */
/* There is a y0 declared in math.h (for ARM) !!! */
float my_y0 = survey_from.y; /* Current latitude */
if (my_y0+nav_survey_shift < nav_survey_south || my_y0+nav_survey_shift > nav_survey_north) {
my_y0 += nav_survey_shift / 2;
nav_survey_shift = -nav_survey_shift;
}
my_y0 = my_y0 + nav_survey_shift; /* Longitude of next leg */
survey_from.y = survey_to.y = my_y0;
my_y0 = my_y0 + nav_survey_shift; /* Longitude of next leg */
survey_from.y = survey_to.y = my_y0;
/* Swap West and East extremities */
float tmp = survey_from.x;
survey_from.x = survey_to.x;
survey_to.x = tmp;
/* Swap West and East extremities */
float tmp = survey_from.x;
survey_from.x = survey_to.x;
survey_to.x = tmp;
/** Do half a circle around WP 0 */
waypoints[0].x = survey_from.x;
waypoints[0].y = my_y0 - nav_survey_shift/2.;
/** Do half a circle around WP 0 */
waypoints[0].x = survey_from.x;
waypoints[0].y = my_y0 - nav_survey_shift/2.;
/* Computes the right direction for the circle */
survey_radius = nav_survey_shift / 2.;
if (SurveyGoingWest()) {
survey_radius = -survey_radius;
}
/* Computes the right direction for the circle */
survey_radius = nav_survey_shift / 2.;
if (SurveyGoingWest()) {
survey_radius = -survey_radius;
}
}
nav_in_segment = FALSE;
@@ -145,9 +174,9 @@ void nav_survey_rectangle(uint8_t wp1, uint8_t wp2) {
}
} else { /* U-turn */
if ((SurveyGoingNorth() && NavCourseCloseTo(0)) ||
(SurveyGoingSouth() && NavCourseCloseTo(180)) ||
(SurveyGoingEast() && NavCourseCloseTo(90)) ||
(SurveyGoingWest() && NavCourseCloseTo(270))) {
(SurveyGoingSouth() && NavCourseCloseTo(180)) ||
(SurveyGoingEast() && NavCourseCloseTo(90)) ||
(SurveyGoingWest() && NavCourseCloseTo(270))) {
/* U-turn finished, back on a segment */
survey_uturn = FALSE;
nav_in_circle = FALSE;
sw/airborne/subsystems/navigation/nav_survey_rectangle.h
+29 -2
View File
@@ -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
#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)
#endif
#endif // NAV_SURVEY_RECTANGLE_H
sw/airborne/subsystems/navigation/poly_survey_adv.c
+67 -39
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/poly_survey_adv.c
*
* Advanced polygon survey for fixedwings from Uni Stuttgart.
*
*/
#include "poly_survey_adv.h"
#include "subsystems/nav.h"
@@ -10,9 +38,9 @@
#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
point2d dir_vec;
@@ -33,19 +61,19 @@ float psa_altitude;
int segment_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
states until to polygon is completely covered
ENTRY : getting in the right position and height for the first flyover
SEG : fly from seg_start to seg_end and take pictures,
then calculate navigation points of next flyover
TURN1 : do a 180° turn around seg_center1
RET : fly from ret_start to ret_end
TURN2 : do a 180° turn around seg_center2
psa_stage starts at ENTRY and than circles trought the other
states until to polygon is completely covered
ENTRY : getting in the right position and height for the first flyover
SEG : fly from seg_start to seg_end and take pictures,
then calculate navigation points of next flyover
TURN1 : do a 180° turn around seg_center1
RET : fly from ret_start to ret_end
TURN2 : do a 180° turn around seg_center2
*/
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
returns : FALSE if no intersection can be found or intersection does not lie between points a and b
else TRUE
p : returns intersection
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
**/
* intercept two lines and give back the point of intersection
* @return FALSE if no intersection can be found or intersection does not lie between points a and b
* else TRUE
* @param p returns intersection
* @param x, y first line is defined by point x and y (goes through this points)
* @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)
{
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
returns : TRUE if two intersection can be found, else FALSE
x, y : intersection points
a, b : define the line to intersection
**/
* intersects a line with the polygon and gives back the two intersection points
* @return TRUE if two intersection can be found, else FALSE
* @param x, y intersection points
* @param a, b define the line to intersection
*/
static bool_t get_two_intersects(point2d *x, point2d *y, point2d a, point2d b)
{
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
first_wp : the first Waypoint of the polygon
size : the number of points that make up the polygon
angle : angle in which to do the flyovers
sweep_width : distance between the sweeps
shot_dist : distance between the shots
min_rad : minimal radius when navigating
altitude : the altitude that must be reached before the flyover starts
**/
* initializes the variables needed for the survey to start
* @param first_wp the first Waypoint of the polygon
* @param size the number of points that make up the polygon
* @param angle angle in which to do the flyovers
* @param sweep_width distance between the sweeps
* @param shot_dist distance between the shots
* @param min_rad minimal radius when navigating
* @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)
{
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
Position and stage and navigates accordingly.
Returns True until the survey is finished
**/
* main navigation routine. This is called periodically evaluates the current
* Position and stage and navigates accordingly.
* Returns True until the survey is finished
*/
bool_t poly_survey_adv(void)
{
NavVerticalAutoThrottleMode(0.0);
sw/airborne/subsystems/navigation/poly_survey_adv.h
+28
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/poly_survey_adv.h
*
* Advanced polygon survey for fixedwings from Uni Stuttgart.
*
*/
#ifndef POLY_ADV_H
#define POLY_ADV_H
sw/airborne/subsystems/navigation/snav.c
+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 "generated/airframe.h"
sw/airborne/subsystems/navigation/snav.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
#define SNAV_H
sw/airborne/subsystems/navigation/spiral.c
+100 -76
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
Alphamax is calculated from given segments
IMPORTANT: numer of segments has to be larger than 2!
*/
/**
* @file subsystems/navigation/spiral.c
*
* 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"
@@ -50,8 +74,8 @@ static float nav_radius_min;
bool_t InitializeSpiral(uint8_t CenterWP, uint8_t EdgeWP, float StartRad, float IncRad, float Segments, float ZKoord)
{
Center = CenterWP; // center of the helix
Edge = EdgeWP; // edge point on the maximaum radius
Center = CenterWP; // center of the helix
Edge = EdgeWP; // edge point on the maximaum radius
SRad = StartRad; // start radius of the helix
Segmente = Segments;
ZPoint = ZKoord;
@@ -69,9 +93,9 @@ bool_t InitializeSpiral(uint8_t CenterWP, uint8_t EdgeWP, float StartRad, float
TransCurrentZ = stateGetPositionEnu_f()->z - ZPoint;
DistanceFromCenter = sqrt(TransCurrentX*TransCurrentX+TransCurrentY*TransCurrentY);
// SpiralTheta = atan2(TransCurrentY,TransCurrentX);
// Fly2X = Spiralradius*cos(SpiralTheta+M_PI)+WaypointX(Center);
// Fly2Y = Spiralradius*sin(SpiralTheta+M_PI)+WaypointY(Center);
// SpiralTheta = atan2(TransCurrentY,TransCurrentX);
// Fly2X = Spiralradius*cos(SpiralTheta+M_PI)+WaypointX(Center);
// Fly2Y = Spiralradius*sin(SpiralTheta+M_PI)+WaypointY(Center);
// Alphalimit denotes angle, where the radius will be increased
Alphalimit = 2*M_PI / Segments;
@@ -80,7 +104,7 @@ bool_t InitializeSpiral(uint8_t CenterWP, uint8_t EdgeWP, float StartRad, float
FlyFromY = stateGetPositionEnu_f()->y;
if(DistanceFromCenter > Spiralradius)
CSpiralStatus = Outside;
CSpiralStatus = Outside;
return FALSE;
}
@@ -94,76 +118,76 @@ bool_t SpiralNav(void)
float CircleAlpha;
switch(CSpiralStatus)
{
case Outside:
//flys until center of the helix is reached an start helix
nav_route_xy(FlyFromX,FlyFromY,WaypointX(Center), WaypointY(Center));
// center reached?
if (nav_approaching_xy(WaypointX(Center), WaypointY(Center), FlyFromX, FlyFromY, 0)) {
// nadir image
{
case Outside:
//flys until center of the helix is reached an start helix
nav_route_xy(FlyFromX,FlyFromY,WaypointX(Center), WaypointY(Center));
// center reached?
if (nav_approaching_xy(WaypointX(Center), WaypointY(Center), FlyFromX, FlyFromY, 0)) {
// nadir image
#ifdef DIGITAL_CAM
dc_send_command(DC_SHOOT);
dc_send_command(DC_SHOOT);
#endif
CSpiralStatus = StartCircle;
}
break;
case StartCircle:
// Starts helix
// storage of current coordinates
// calculation needed, State switch to Circle
nav_circle_XY(WaypointX(Center), WaypointY(Center), SRad);
if(DistanceFromCenter >= SRad){
LastCircleX = stateGetPositionEnu_f()->x;
LastCircleY = stateGetPositionEnu_f()->y;
CSpiralStatus = Circle;
// Start helix
#ifdef DIGITAL_CAM
dc_Circle(360/Segmente);
#endif
}
break;
case Circle: {
nav_circle_XY(WaypointX(Center), WaypointY(Center), SRad);
// Trigonometrische Berechnung des bereits geflogenen Winkels alpha
// equation:
// alpha = 2 * asin ( |Starting position angular - current positon| / (2* SRad)
// if alphamax already reached, increase radius.
//DistanceStartEstim = |Starting position angular - current positon|
DistanceStartEstim = sqrt (((LastCircleX-stateGetPositionEnu_f()->x)*(LastCircleX-stateGetPositionEnu_f()->x))
+ ((LastCircleY-stateGetPositionEnu_f()->y)*(LastCircleY-stateGetPositionEnu_f()->y)));
CircleAlpha = (2.0 * asin (DistanceStartEstim / (2 * SRad)));
if (CircleAlpha >= Alphalimit) {
LastCircleX = stateGetPositionEnu_f()->x;
LastCircleY = stateGetPositionEnu_f()->y;
CSpiralStatus = IncSpiral;
}
break;
CSpiralStatus = StartCircle;
}
case IncSpiral:
// increasing circle radius as long as it is smaller than max helix radius
if(SRad + IRad < Spiralradius)
{
SRad = SRad + IRad;
break;
case StartCircle:
// Starts helix
// storage of current coordinates
// calculation needed, State switch to Circle
nav_circle_XY(WaypointX(Center), WaypointY(Center), SRad);
if(DistanceFromCenter >= SRad){
LastCircleX = stateGetPositionEnu_f()->x;
LastCircleY = stateGetPositionEnu_f()->y;
CSpiralStatus = Circle;
// Start helix
#ifdef DIGITAL_CAM
if (dc_cam_tracing) {
// calculating Cam angle for camera alignment
TransCurrentZ = stateGetPositionEnu_f()->z - ZPoint;
dc_cam_angle = atan(SRad/TransCurrentZ) * 180 / M_PI;
}
dc_Circle(360/Segmente);
#endif
}
else {
SRad = Spiralradius;
}
break;
case Circle: {
nav_circle_XY(WaypointX(Center), WaypointY(Center), SRad);
// Trigonometrische Berechnung des bereits geflogenen Winkels alpha
// equation:
// alpha = 2 * asin ( |Starting position angular - current positon| / (2* SRad)
// if alphamax already reached, increase radius.
//DistanceStartEstim = |Starting position angular - current positon|
DistanceStartEstim = sqrt (((LastCircleX-stateGetPositionEnu_f()->x)*(LastCircleX-stateGetPositionEnu_f()->x))
+ ((LastCircleY-stateGetPositionEnu_f()->y)*(LastCircleY-stateGetPositionEnu_f()->y)));
CircleAlpha = (2.0 * asin (DistanceStartEstim / (2 * SRad)));
if (CircleAlpha >= Alphalimit) {
LastCircleX = stateGetPositionEnu_f()->x;
LastCircleY = stateGetPositionEnu_f()->y;
CSpiralStatus = IncSpiral;
}
break;
}
case IncSpiral:
// increasing circle radius as long as it is smaller than max helix radius
if(SRad + IRad < Spiralradius)
{
SRad = SRad + IRad;
#ifdef DIGITAL_CAM
// Stopps DC
dc_stop();
if (dc_cam_tracing) {
// calculating Cam angle for camera alignment
TransCurrentZ = stateGetPositionEnu_f()->z - ZPoint;
dc_cam_angle = atan(SRad/TransCurrentZ) * 180 / M_PI;
}
#endif
}
CSpiralStatus = Circle;
break;
default:
break;
}
}
else {
SRad = Spiralradius;
#ifdef DIGITAL_CAM
// Stopps DC
dc_stop();
#endif
}
CSpiralStatus = Circle;
break;
default:
break;
}
return TRUE;
}
sw/airborne/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
#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,
float Segments, float ZKoord );
#endif
#endif // SPIRAL_H
sw/airborne/subsystems/navigation/traffic_info.c
+5 -3
View File
@@ -17,10 +17,12 @@
* 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 traffic_info.c
* \brief Informations relative to the other aircrafts
/**
* @file subsystems/naviation/traffic_info.c
*
* Information relative to the other aircrafts.
*
*/
sw/airborne/subsystems/navigation/traffic_info.h
+19 -17
View File
@@ -17,10 +17,12 @@
* 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 traffic_info.h
* \brief Informations relative to the other aircrafts
/**
* @file subsystems/navigation/traffic_info.h
*
* Information relative to the other aircrafts.
*
*/
@@ -48,20 +50,20 @@ extern struct ac_info_ the_acs[NB_ACS];
// 0 is reserved for ground station (id=0)
// 1 is reserved for this AC (id=AC_ID)
#define SetAcInfo(_id, _utm_x /*m*/, _utm_y /*m*/, _course/*rad(CW)*/, _alt/*m*/,_gspeed/*m/s*/,_climb, _itow) { \
if (acs_idx < NB_ACS) { \
if (_id > 0 && the_acs_id[_id] == 0) { \
the_acs_id[_id] = acs_idx++; \
the_acs[the_acs_id[_id]].ac_id = (uint8_t)_id; \
} \
the_acs[the_acs_id[_id]].east = _utm_x - nav_utm_east0; \
the_acs[the_acs_id[_id]].north = _utm_y - nav_utm_north0; \
the_acs[the_acs_id[_id]].course = _course; \
the_acs[the_acs_id[_id]].alt = _alt; \
the_acs[the_acs_id[_id]].gspeed = _gspeed; \
the_acs[the_acs_id[_id]].climb = _climb; \
the_acs[the_acs_id[_id]].itow = (uint32_t)_itow; \
} \
}
if (acs_idx < NB_ACS) { \
if (_id > 0 && the_acs_id[_id] == 0) { \
the_acs_id[_id] = acs_idx++; \
the_acs[the_acs_id[_id]].ac_id = (uint8_t)_id; \
} \
the_acs[the_acs_id[_id]].east = _utm_x - nav_utm_east0; \
the_acs[the_acs_id[_id]].north = _utm_y - nav_utm_north0; \
the_acs[the_acs_id[_id]].course = _course; \
the_acs[the_acs_id[_id]].alt = _alt; \
the_acs[the_acs_id[_id]].gspeed = _gspeed; \
the_acs[the_acs_id[_id]].climb = _climb; \
the_acs[the_acs_id[_id]].itow = (uint32_t)_itow; \
} \
}
extern void traffic_info_init( void );