diff --git a/sw/airborne/nav_line.c b/sw/airborne/nav_line.c index 5885585f8a..49b110edbd 100644 --- a/sw/airborne/nav_line.c +++ b/sw/airborne/nav_line.c @@ -1,19 +1,46 @@ +/* + * $Id$ + * + * Copyright (C) 2007 Anton Kochevar, ENAC + * + * 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 nav_line.c + * \brief Navigation along a line with nice U-turns + * + */ + #include "nav_line.h" #include "nav.h" -/************** Line Navigation **********************************************/ - - - +/** Status along the pattern */ enum line_status { LR12, LQC21, LTC2, LQC22, LR21, LQC12, LTC1, LQC11 }; - static enum line_status line_status; + bool_t nav_line_init( void ) { line_status = LR12; return FALSE; } bool_t nav_line(uint8_t l1, uint8_t l2, float radius) { + radius = fabs(radius); float alt = waypoints[l1].a; waypoints[l2].a = alt; @@ -27,87 +54,85 @@ bool_t nav_line(uint8_t l1, uint8_t l2, float radius) { /* The half circle centers and the other leg */ struct point l2_c1 = { waypoints[l1].x + radius * u_y, - waypoints[l1].y + radius * -u_x, - alt }; + waypoints[l1].y + radius * -u_x, + alt }; struct point l2_c2 = { waypoints[l1].x + 1.732*radius * u_x, - waypoints[l1].y + 1.732*radius * u_y, - alt }; - struct point l2_c3 = { waypoints[l1].x + radius * -u_y, - waypoints[l1].y + radius * u_x, - alt }; + waypoints[l1].y + 1.732*radius * u_y, + alt }; + struct point l2_c3 = { waypoints[l1].x + radius * -u_y, + waypoints[l1].y + radius * u_x, + alt }; struct point l1_c1 = { waypoints[l2].x + radius * -u_y, waypoints[l2].y + radius * u_x, alt }; struct point l1_c2 = { waypoints[l2].x +1.732*radius * -u_x, - waypoints[l2].y + 1.732*radius * -u_y, - alt }; - struct point l1_c3 = { waypoints[l2].x + radius * u_y, - waypoints[l2].y + radius * -u_x, - alt }; + waypoints[l2].y + 1.732*radius * -u_y, + alt }; + struct point l1_c3 = { waypoints[l2].x + radius * u_y, + waypoints[l2].y + 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); - float qdr_out_2_3 = M_PI - atan2(u_y, u_x); + float qdr_out_2_3 = M_PI - atan2(u_y, u_x); switch (line_status) { - - /* LR12, LQC21, LTC2, LQC22, LR21 LQC12, LTC1, LQC11 */ - case LR12: - nav_route_xy(waypoints[l2].x, waypoints[l2].y, waypoints[l1].x, waypoints[l1].y); - if (NavApproaching(l1, CARROT)) { + case LR12: /* From wp l2 to wp l1 */ + NavSegment(l2, l1); + if (NavApproachingFrom(l1, l2, CARROT)) { line_status = LQC21; nav_init_stage(); } break; - case LQC21: - nav_circle_XY(l2_c1.x, l2_c1.y, radius); + case LQC21: + nav_circle_XY(l2_c1.x, l2_c1.y, radius); if (NavQdrCloseTo(DegOfRad(qdr_out_2_1)-10)) { line_status = LTC2; nav_init_stage(); } break; - case LTC2: - nav_circle_XY(l2_c2.x, l2_c2.y, -radius); + case LTC2: + nav_circle_XY(l2_c2.x, l2_c2.y, -radius); if (NavQdrCloseTo(DegOfRad(qdr_out_2_2)+10)) { line_status = LQC22; nav_init_stage(); } break; - case LQC22: - nav_circle_XY(l2_c3.x, l2_c3.y, radius); + case LQC22: + nav_circle_XY(l2_c3.x, l2_c3.y, radius); if (NavQdrCloseTo(DegOfRad(qdr_out_2_3)-10)) { line_status = LR21; nav_init_stage(); } break; - case LR21: - nav_route_xy(waypoints[l1].x, waypoints[l1].y,waypoints[l2].x, waypoints[l2].y); - if (NavApproaching(l2, CARROT)) { + case LR21: /* From wp l1 to wp l2 */ + NavSegment(l1, l2); + if (NavApproachingFrom(l2, l1, CARROT)) { line_status = LQC12; nav_init_stage(); } break; - case LQC12: - nav_circle_XY(l1_c1.x, l1_c1.y, radius); + case LQC12: + nav_circle_XY(l1_c1.x, l1_c1.y, radius); if (NavQdrCloseTo(DegOfRad(qdr_out_2_1 + M_PI)-10)) { line_status = LTC1; nav_init_stage(); } break; - case LTC1: - nav_circle_XY(l1_c2.x, l1_c2.y, -radius); + case LTC1: + nav_circle_XY(l1_c2.x, l1_c2.y, -radius); if (NavQdrCloseTo(DegOfRad(qdr_out_2_2 + M_PI)+10)) { line_status = LQC11; nav_init_stage(); } break; - case LQC11: - nav_circle_XY(l1_c3.x, l1_c3.y, radius); + case LQC11: + nav_circle_XY(l1_c3.x, l1_c3.y, radius); if (NavQdrCloseTo(DegOfRad(qdr_out_2_3 + M_PI)-10)) { line_status = LR12; nav_init_stage(); } } - return TRUE; + return TRUE; /* This pattern never ends */ } diff --git a/sw/airborne/nav_line.h b/sw/airborne/nav_line.h index a337b5c9e0..81d4498be6 100644 --- a/sw/airborne/nav_line.h +++ b/sw/airborne/nav_line.h @@ -1,4 +1,33 @@ +/* + * $Id$ + * + * Copyright (C) 2007 Anton Kochevar, ENAC + * + * 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. + * + */ + +#ifndef NAV_LINE_H +#define NAV_LINE_H + #include "std.h" extern bool_t nav_line_init( void ); -extern bool_t nav_line(uint8_t, uint8_t, float); +extern bool_t nav_line(uint8_t wp1, uint8_t wp2, float radius); + +#endif /* NAV_LINE_H */