From 930a3bb65589ddee8ad4a596fbd45d1039cd388e Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Fri, 18 Jan 2013 01:01:29 +0100 Subject: [PATCH] [dox][navigation] cleanup indentation, add headers and doxygen file blocks --- sw/airborne/firmwares/rotorcraft/navigation.c | 7 + sw/airborne/firmwares/rotorcraft/navigation.h | 6 + sw/airborne/subsystems/nav.c | 100 +++++----- sw/airborne/subsystems/nav.h | 9 +- sw/airborne/subsystems/navigation/bomb.c | 4 + sw/airborne/subsystems/navigation/bomb.h | 28 ++- .../subsystems/navigation/border_line.c | 111 ++++++----- .../subsystems/navigation/border_line.h | 5 +- .../navigation/common_flight_plan.c | 7 +- .../navigation/common_flight_plan.h | 6 +- .../subsystems/navigation/common_nav.c | 4 + .../subsystems/navigation/common_nav.h | 4 + .../subsystems/navigation/discsurvey.c | 40 +++- .../subsystems/navigation/discsurvey.h | 32 +++- .../subsystems/navigation/flightzone.c | 83 ++++----- .../subsystems/navigation/flightzone.h | 18 +- sw/airborne/subsystems/navigation/gls.c | 21 +-- sw/airborne/subsystems/navigation/gls.h | 6 +- sw/airborne/subsystems/navigation/nav_cube.c | 11 +- sw/airborne/subsystems/navigation/nav_cube.h | 34 ++-- sw/airborne/subsystems/navigation/nav_line.c | 30 +-- sw/airborne/subsystems/navigation/nav_line.h | 5 + .../navigation/nav_survey_rectangle.c | 119 +++++++----- .../navigation/nav_survey_rectangle.h | 31 ++- .../subsystems/navigation/poly_survey_adv.c | 106 +++++++---- .../subsystems/navigation/poly_survey_adv.h | 28 +++ sw/airborne/subsystems/navigation/snav.c | 29 ++- sw/airborne/subsystems/navigation/snav.h | 28 +++ sw/airborne/subsystems/navigation/spiral.c | 176 ++++++++++-------- sw/airborne/subsystems/navigation/spiral.h | 30 ++- .../subsystems/navigation/traffic_info.c | 8 +- .../subsystems/navigation/traffic_info.h | 36 ++-- 32 files changed, 734 insertions(+), 428 deletions(-) diff --git a/sw/airborne/firmwares/rotorcraft/navigation.c b/sw/airborne/firmwares/rotorcraft/navigation.c index fed19615e6..5166996b58 100644 --- a/sw/airborne/firmwares/rotorcraft/navigation.c +++ b/sw/airborne/firmwares/rotorcraft/navigation.c @@ -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" diff --git a/sw/airborne/firmwares/rotorcraft/navigation.h b/sw/airborne/firmwares/rotorcraft/navigation.h index 5df3d485f4..4898fdf918 100644 --- a/sw/airborne/firmwares/rotorcraft/navigation.h +++ b/sw/airborne/firmwares/rotorcraft/navigation.h @@ -19,6 +19,12 @@ * Boston, MA 02111-1307, USA. */ +/** + * @file firmwares/rotorcraft/navigation.h + * + * Rotorcraft navigation functions. + */ + #ifndef NAVIGATION_H #define NAVIGATION_H diff --git a/sw/airborne/subsystems/nav.c b/sw/airborne/subsystems/nav.c index 0fa6c1cc0c..1f3beb5e68 100644 --- a/sw/airborne/subsystems/nav.c +++ b/sw/airborne/subsystems/nav.c @@ -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; } } diff --git a/sw/airborne/subsystems/nav.h b/sw/airborne/subsystems/nav.h index 74525e8fce..b810172827 100644 --- a/sw/airborne/subsystems/nav.h +++ b/sw/airborne/subsystems/nav.h @@ -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 diff --git a/sw/airborne/subsystems/navigation/bomb.c b/sw/airborne/subsystems/navigation/bomb.c index cc4b8c2d0e..c8cd2b31ab 100644 --- a/sw/airborne/subsystems/navigation/bomb.c +++ b/sw/airborne/subsystems/navigation/bomb.c @@ -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 * */ diff --git a/sw/airborne/subsystems/navigation/bomb.h b/sw/airborne/subsystems/navigation/bomb.h index a7c23435ed..91d22f8283 100644 --- a/sw/airborne/subsystems/navigation/bomb.h +++ b/sw/airborne/subsystems/navigation/bomb.h @@ -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 diff --git a/sw/airborne/subsystems/navigation/border_line.c b/sw/airborne/subsystems/navigation/border_line.c index 768308de12..898dbde5f8 100644 --- a/sw/airborne/subsystems/navigation/border_line.c +++ b/sw/airborne/subsystems/navigation/border_line.c @@ -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 */ } diff --git a/sw/airborne/subsystems/navigation/border_line.h b/sw/airborne/subsystems/navigation/border_line.h index 73aefb8b6d..7814fc9223 100644 --- a/sw/airborne/subsystems/navigation/border_line.h +++ b/sw/airborne/subsystems/navigation/border_line.h @@ -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 */ diff --git a/sw/airborne/subsystems/navigation/common_flight_plan.c b/sw/airborne/subsystems/navigation/common_flight_plan.c index 9e8a947ddb..b7beb6a922 100644 --- a/sw/airborne/subsystems/navigation/common_flight_plan.c +++ b/sw/airborne/subsystems/navigation/common_flight_plan.c @@ -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; diff --git a/sw/airborne/subsystems/navigation/common_flight_plan.h b/sw/airborne/subsystems/navigation/common_flight_plan.h index b0394c7e45..1eb711c7e3 100644 --- a/sw/airborne/subsystems/navigation/common_flight_plan.h +++ b/sw/airborne/subsystems/navigation/common_flight_plan.h @@ -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 diff --git a/sw/airborne/subsystems/navigation/common_nav.c b/sw/airborne/subsystems/navigation/common_nav.c index e2d92f7fe1..317fa475b3 100644 --- a/sw/airborne/subsystems/navigation/common_nav.c +++ b/sw/airborne/subsystems/navigation/common_nav.c @@ -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 * */ diff --git a/sw/airborne/subsystems/navigation/common_nav.h b/sw/airborne/subsystems/navigation/common_nav.h index 19ede538e2..c7f0343489 100644 --- a/sw/airborne/subsystems/navigation/common_nav.h +++ b/sw/airborne/subsystems/navigation/common_nav.h @@ -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 * */ diff --git a/sw/airborne/subsystems/navigation/discsurvey.c b/sw/airborne/subsystems/navigation/discsurvey.c index 309451fa5c..a2efa53cce 100644 --- a/sw/airborne/subsystems/navigation/discsurvey.c +++ b/sw/airborne/subsystems/navigation/discsurvey.c @@ -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(); } diff --git a/sw/airborne/subsystems/navigation/discsurvey.h b/sw/airborne/subsystems/navigation/discsurvey.h index 83e153cfb1..0bc5cdb32e 100644 --- a/sw/airborne/subsystems/navigation/discsurvey.h +++ b/sw/airborne/subsystems/navigation/discsurvey.h @@ -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 */ diff --git a/sw/airborne/subsystems/navigation/flightzone.c b/sw/airborne/subsystems/navigation/flightzone.c index e16596cfdc..5cb947cf75 100644 --- a/sw/airborne/subsystems/navigation/flightzone.c +++ b/sw/airborne/subsystems/navigation/flightzone.c @@ -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; } - - diff --git a/sw/airborne/subsystems/navigation/flightzone.h b/sw/airborne/subsystems/navigation/flightzone.h index 6055d89dda..be0490d38b 100644 --- a/sw/airborne/subsystems/navigation/flightzone.h +++ b/sw/airborne/subsystems/navigation/flightzone.h @@ -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 */ diff --git a/sw/airborne/subsystems/navigation/gls.c b/sw/airborne/subsystems/navigation/gls.c index 33ea256b38..683754b66a 100644 --- a/sw/airborne/subsystems/navigation/gls.c +++ b/sw/airborne/subsystems/navigation/gls.c @@ -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() diff --git a/sw/airborne/subsystems/navigation/gls.h b/sw/airborne/subsystems/navigation/gls.h index f6f7ea71b8..9e7ee7cde0 100644 --- a/sw/airborne/subsystems/navigation/gls.h +++ b/sw/airborne/subsystems/navigation/gls.h @@ -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 diff --git a/sw/airborne/subsystems/navigation/nav_cube.c b/sw/airborne/subsystems/navigation/nav_cube.c index 7d6d7f976b..574a4348f0 100644 --- a/sw/airborne/subsystems/navigation/nav_cube.c +++ b/sw/airborne/subsystems/navigation/nav_cube.c @@ -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 */ diff --git a/sw/airborne/subsystems/navigation/nav_cube.h b/sw/airborne/subsystems/navigation/nav_cube.h index 207a5bb585..32766e8e8f 100644 --- a/sw/airborne/subsystems/navigation/nav_cube.h +++ b/sw/airborne/subsystems/navigation/nav_cube.h @@ -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; } diff --git a/sw/airborne/subsystems/navigation/nav_line.c b/sw/airborne/subsystems/navigation/nav_line.c index 3ba80f7280..69434451a3 100644 --- a/sw/airborne/subsystems/navigation/nav_line.c +++ b/sw/airborne/subsystems/navigation/nav_line.c @@ -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); diff --git a/sw/airborne/subsystems/navigation/nav_line.h b/sw/airborne/subsystems/navigation/nav_line.h index 7f6e0ec90f..4030b967e0 100644 --- a/sw/airborne/subsystems/navigation/nav_line.h +++ b/sw/airborne/subsystems/navigation/nav_line.h @@ -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 diff --git a/sw/airborne/subsystems/navigation/nav_survey_rectangle.c b/sw/airborne/subsystems/navigation/nav_survey_rectangle.c index 8698f6d937..4a39f00ace 100644 --- a/sw/airborne/subsystems/navigation/nav_survey_rectangle.c +++ b/sw/airborne/subsystems/navigation/nav_survey_rectangle.c @@ -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; diff --git a/sw/airborne/subsystems/navigation/nav_survey_rectangle.h b/sw/airborne/subsystems/navigation/nav_survey_rectangle.h index 21b8f07c3d..443c98ca7a 100644 --- a/sw/airborne/subsystems/navigation/nav_survey_rectangle.h +++ b/sw/airborne/subsystems/navigation/nav_survey_rectangle.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 #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 diff --git a/sw/airborne/subsystems/navigation/poly_survey_adv.c b/sw/airborne/subsystems/navigation/poly_survey_adv.c index c52f329a9f..c3113c4650 100644 --- a/sw/airborne/subsystems/navigation/poly_survey_adv.c +++ b/sw/airborne/subsystems/navigation/poly_survey_adv.c @@ -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); diff --git a/sw/airborne/subsystems/navigation/poly_survey_adv.h b/sw/airborne/subsystems/navigation/poly_survey_adv.h index 90b0164565..c4787263ac 100644 --- a/sw/airborne/subsystems/navigation/poly_survey_adv.h +++ b/sw/airborne/subsystems/navigation/poly_survey_adv.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.h + * + * Advanced polygon survey for fixedwings from Uni Stuttgart. + * + */ + #ifndef POLY_ADV_H #define POLY_ADV_H diff --git a/sw/airborne/subsystems/navigation/snav.c b/sw/airborne/subsystems/navigation/snav.c index 10e4903188..0acf313152 100644 --- a/sw/airborne/subsystems/navigation/snav.c +++ b/sw/airborne/subsystems/navigation/snav.c @@ -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 #include "generated/airframe.h" diff --git a/sw/airborne/subsystems/navigation/snav.h b/sw/airborne/subsystems/navigation/snav.h index 010de70a2b..5bbc2d3957 100644 --- a/sw/airborne/subsystems/navigation/snav.h +++ b/sw/airborne/subsystems/navigation/snav.h @@ -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 diff --git a/sw/airborne/subsystems/navigation/spiral.c b/sw/airborne/subsystems/navigation/spiral.c index d08cb2576d..11e004d3f7 100644 --- a/sw/airborne/subsystems/navigation/spiral.c +++ b/sw/airborne/subsystems/navigation/spiral.c @@ -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; } diff --git a/sw/airborne/subsystems/navigation/spiral.h b/sw/airborne/subsystems/navigation/spiral.h index 7eea57cd54..72356d1ee7 100644 --- a/sw/airborne/subsystems/navigation/spiral.h +++ b/sw/airborne/subsystems/navigation/spiral.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/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 diff --git a/sw/airborne/subsystems/navigation/traffic_info.c b/sw/airborne/subsystems/navigation/traffic_info.c index 5d5646725c..2d8fe65b10 100644 --- a/sw/airborne/subsystems/navigation/traffic_info.c +++ b/sw/airborne/subsystems/navigation/traffic_info.c @@ -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. * */ diff --git a/sw/airborne/subsystems/navigation/traffic_info.h b/sw/airborne/subsystems/navigation/traffic_info.h index 8979aecf40..9d09d6e07a 100644 --- a/sw/airborne/subsystems/navigation/traffic_info.h +++ b/sw/airborne/subsystems/navigation/traffic_info.h @@ -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 );