nav_line.c

Go to the documentation of this file.

/*
 * 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.
 */

#include "generated/airframe.h"
#include "modules/nav/nav_line.h"
#include "firmwares/fixedwing/nav.h"

enum line_status { LR12, LQC21, LTC2, LQC22, LR21, LQC12, LTC1, LQC11 };
static enum line_status line_status;

bool nav_line_setup(void)
{
  line_status = LR12;
  return false;
}

bool nav_line_run(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);

  /* Unit vector from l1 to l2 */
  float u_x = l2_l1_x / d;
  float u_y = l2_l1_y / d;

  /* The half circle centers and the other leg */
  struct point l2_c1 = { WaypointX(l1) + radius * u_y,
           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
  };
  struct point l2_c3 = { WaypointX(l1) + radius * -u_y,
           WaypointY(l1) + radius * u_x,
           alt
  };

  struct point l1_c1 = { WaypointX(l2) + radius * -u_y,
           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
  };
  struct point l1_c3 = { WaypointX(l2) + radius * u_y,
           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);
  float qdr_out_2_3 = M_PI - atan2(u_y, u_x);

  /* Vertical target */
  NavVerticalAutoThrottleMode(0); /* No pitch */
  NavVerticalAltitudeMode(WaypointAlt(l1), 0.);

  switch (line_status) {
    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);
      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);
      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);
      if (NavQdrCloseTo(DegOfRad(qdr_out_2_3) - 10)) {
        line_status = LR21;
        nav_init_stage();
      }
      break;
    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);
      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);
      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);
      if (NavQdrCloseTo(DegOfRad(qdr_out_2_3 + M_PI) - 10)) {
        line_status = LR12;
        nav_init_stage();
      }
      break;
    default: /* Should not occur !!! End the pattern */
      return false;
  }
  return true; /* This pattern never ends */
}