Merge pull request #116 from TobiasMue/devTob_gls

gps landing routines and new border_line nav routine

conf/autopilot/subsystems/fixedwing/navigation_extra.makefile

@@ -17,4 +17,6 @@ $(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/OSAMNav.c
 $(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/snav.c
 $(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/spiral.c
 $(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/poly_survey_adv.c
+$(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/gls.c
+$(TARGET).srcs += $(SRC_SUBSYSTEMS)/navigation/border_line.c
 

sw/airborne/subsystems/navigation/border_line.c

@@ -0,0 +1,137 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2012 Tobias Muench
+ *
+ * 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.
+ *
+ */
+
+
+//modified nav_linie by Anton Kochevar, ENAC
+
+/**
+ * \brief navigate along a border line (line 1-2) with turns in the same direction
+ * 
+ * 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 smal in strong wind conditions! 
+*/
+
+#include "subsystems/navigation/border_line.h"
+#include "generated/airframe.h"
+#include "subsystems/nav.h"
+
+
+enum border_line_status { LR12, LQC21, LTC2, LQC22, LR21, LQC12, LTC1, LQC11 };
+static enum border_line_status border_line_status;
+
+bool_t border_line_init( void ) {
+  border_line_status = LR12;
+  return FALSE;
+}
+
+bool_t border_line(uint8_t l1, uint8_t l2, float radius) {
+  radius = fabs(radius);
+  float alt = waypoints[l1].a;
+  waypoints[l2].a = alt;
+
+  float l2_l1_x = WaypointX(l1) - WaypointX(l2);
+  float l2_l1_y = WaypointY(l1) - WaypointY(l2);
+  float d = sqrt(l2_l1_x*l2_l1_x+l2_l1_y*l2_l1_y);
+
+  float u_x = l2_l1_x / d;
+  float u_y = l2_l1_y / d;
+
+  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  };
+  struct point l2_c2 = { l2_c1.x + 2*radius*cos(angle) ,
+       l2_c1.y - 2*radius*sin(angle),
+       alt  };
+
+
+
+  struct point l1_c2 = { WaypointX(l2) - sin(angle)*radius,
+       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  };
+
+  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);
+
+
+  NavVerticalAutoThrottleMode(0);
+  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 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;
+
+  default: /* Should not occur !!! End the pattern */
+    return FALSE;
+  }
+  return TRUE; /* This pattern never ends */
+}

sw/airborne/subsystems/navigation/border_line.h

@@ -0,0 +1,35 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2012, Tobias Muench
+ *
+ * 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 BORDER_LINE_H
+#define BORDER_LINE_H
+
+#include "std.h"
+
+extern bool_t border_line_init( void );
+extern bool_t border_line(uint8_t wp1, uint8_t wp2, float radius);
+
+#endif
+
+/* BORDER_LINE_H */

sw/airborne/subsystems/navigation/gls.c

@@ -0,0 +1,175 @@
+/*
+ *
+ * Copyright (C) 2012, Tobias Münch
+ *
+ * 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.
+ *
+ */
+
+/**
+ * \brief gps landing system
+ * gps landing system
+ * -automatic calculation of top of decent for const app angle
+ * -smooth intercept posible
+ * -landing direction is set by app fix / also possible in flight!!!
+ *
+ * in airframe.xml
+ * it is possible to define
+ *
+ * 1. target_speed
+ * 2. app_angle
+ * 3. app_intercept_af_tod
+ *
+ * 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
+*/
+
+
+
+#include "generated/airframe.h"
+#include "estimator.h"
+#include "subsystems/navigation/gls.h"
+#include "subsystems/nav.h"
+#include "generated/flight_plan.h"
+
+
+
+float target_speed;
+float app_angle;
+float app_intercept_af_tod;
+
+bool_t init = TRUE;
+
+#ifndef APP_TARGET_SPEED
+#define APP_TARGET_SPEED V_CTL_AUTO_AIRSPEED_SETPOINT;
+#endif
+
+#ifndef APP_ANGLE
+#define APP_ANGLE 5;
+#endif
+
+#ifndef APP_INTERCEPT_AF_TOD
+#define APP_INTERCEPT_AF_TOD 100
+#endif
+
+
+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;
+  }
+
+  float td_af_x = WaypointX(_af) - WaypointX(_td);
+  float td_af_y = WaypointY(_af) - WaypointY(_td);
+  float td_af = sqrt( td_af_x*td_af_x + td_af_y*td_af_y);
+  float td_tod = (WaypointAlt(_af) - WaypointAlt(_td)) / (tan(RadOfDeg(app_angle)));
+
+  WaypointX(_tod) = WaypointX(_td) + td_af_x / td_af * td_tod;
+  WaypointY(_tod) = WaypointY(_td) + td_af_y / td_af * td_tod;
+  WaypointAlt(_tod) = WaypointAlt(_af);
+
+  if (td_tod > td_af) {
+  WaypointX(_af) = WaypointX(_tod) + td_af_x / td_af * app_intercept_af_tod;
+  WaypointY(_af) = WaypointY(_tod) + td_af_y / td_af * app_intercept_af_tod;
+  }
+  return FALSE;
+}	// end of gls_copute_TOD
+
+
+//###############################################################################################
+
+bool_t gls_init(uint8_t _af, uint8_t _tod, uint8_t _td) {
+
+  init = TRUE;
+
+  #ifdef USE_AIRSPEED
+//  float wind_additional = sqrt(wind_east*wind_east + wind_north*wind_north); // should be gusts only!
+//  Bound(wind_additional, 0, 0.5);
+//  target_speed = FL_ENVE_V_S * 1.3 + wind_additional; FIXME
+  target_speed =  APP_TARGET_SPEED; //  ok for now!
+  #endif
+
+  app_angle = APP_ANGLE;
+  app_intercept_af_tod = APP_INTERCEPT_AF_TOD;
+  Bound(app_intercept_af_tod,0,200);
+
+
+  gls_compute_TOD(_af, _tod, _td);	// calculate Top Of Decent
+
+  return FALSE;
+}  // end of gls_init()
+
+
+//###############################################################################################
+
+
+bool_t gls(uint8_t _af, uint8_t _tod, uint8_t _td) {
+
+
+  if (init){
+
+  #ifdef 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.);
+
+  float nav_final_progress = ((estimator_x - WaypointX(_tod)) * final_x + (estimator_y - WaypointY(_tod)) * final_y) / final2;
+  Bound(nav_final_progress,-1,1);
+  float nav_final_length = sqrt(final2);
+
+  float pre_climb = -(WaypointAlt(_tod) - WaypointAlt(_td)) / (nav_final_length / estimator_hspeed_mod);
+  Bound(pre_climb, -5, 0.);
+
+  float start_alt = WaypointAlt(_tod);
+  float diff_alt = WaypointAlt(_td) - start_alt;
+  float alt = start_alt + nav_final_progress * diff_alt;
+  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;
+
+}	// end of gls()

sw/airborne/subsystems/navigation/gls.h

@@ -0,0 +1,35 @@
+/*
+ *  
+ * Copyright (C) 2012, Tobias Muench
+ *
+ * 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_GLS_H
+#define NAV_GLS_H
+
+#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