mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-07 00:53:41 +08:00
Bug fixes:
- now works with negative radius - detection of end of line was wrong
This commit is contained in:
Pascal Brisset
+64
-39
@@ -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 */
|
||||
}
|
||||
|
||||
+30
-1
@@ -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 */
|
||||
|
||||
Reference in New Issue
Block a user