flight/libraries/paths.c

   1 /**
   2  ******************************************************************************
   3  *
   4  * @file       paths.c
   5  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
   6  * @brief      Library path manipulation
   7  *
   8  * @see        The GNU Public License (GPL) Version 3
   9  *
  10  *****************************************************************************/
  11 /*
  12  * This program is free software; you can redistribute it and/or modify
  13  * it under the terms of the GNU General Public License as published by
  14  * the Free Software Foundation; either version 3 of the License, or
  15  * (at your option) any later version.
  16  *
  17  * This program is distributed in the hope that it will be useful, but
  18  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  19  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  20  * for more details.
  21  *
  22  * You should have received a copy of the GNU General Public License along
  23  * with this program; if not, write to the Free Software Foundation, Inc.,
  24  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  25  */
  26
  27 #include <pios.h>
  28 #include <pios_math.h>
  29 #include <mathmisc.h>
  30
  31 #include "uavobjectmanager.h" // <--.
  32 #include "pathdesired.h" // <-- needed only for correct ENUM macro usage with path modes (PATHDESIRED_MODE_xxx,
  33 #include "paths.h"
  34 // no direct UAVObject usage allowed in this file
  35
  36 // private functions
  37 static void path_endpoint(PathDesiredData *path, float *cur_point, struct path_status *status, bool mode);
  38 static void path_vector(PathDesiredData *path, float *cur_point, struct path_status *status, bool mode);
  39 static void path_circle(PathDesiredData *path, float *cur_point, struct path_status *status, bool clockwise);
  40
  41 /**
  42  * @brief Compute progress along path and deviation from it
  43  * @param[in] path  PathDesired structure
  44  * @param[in] cur_point Current location
  45  * @param[out] status Structure containing progress along path and deviation
  46  */
  47 void path_progress(PathDesiredData *path, float *cur_point, struct path_status *status, bool mode3D)
  48 {
  49     switch (path->Mode) {
  50     case PATHDESIRED_MODE_BRAKE:
  51     case PATHDESIRED_MODE_FOLLOWVECTOR:
  52         return path_vector(path, cur_point, status, mode3D);
  53
  54         break;
  55     case PATHDESIRED_MODE_CIRCLERIGHT:
  56         return path_circle(path, cur_point, status, true);
  57
  58         break;
  59     case PATHDESIRED_MODE_CIRCLELEFT:
  60         return path_circle(path, cur_point, status, false);
  61
  62         break;
  63     case PATHDESIRED_MODE_GOTOENDPOINT:
  64         return path_endpoint(path, cur_point, status, mode3D);
  65
  66         break;
  67     case PATHDESIRED_MODE_LAND:
  68     default:
  69         // use the endpoint as default failsafe if called in unknown modes
  70         return path_endpoint(path, cur_point, status, false);
  71
  72         break;
  73     }
  74 }
  75
  76 /**
  77  * @brief Compute progress towards endpoint. Deviation equals distance
  78  * @param[in] path PathDesired
  79  * @param[in] cur_point Current location
  80  * @param[out] status Structure containing progress along path and deviation
  81  * @param[in] mode3D set true to include altitude in distance and progress calculation
  82  */
  83 static void path_endpoint(PathDesiredData *path, float *cur_point, struct path_status *status, bool mode3D)
  84 {
  85     float diff[3];
  86     float dist_path, dist_diff;
  87
  88     // Distance to go
  89     status->path_vector[0] = path->End.North - path->Start.North;
  90     status->path_vector[1] = path->End.East - path->Start.East;
  91     status->path_vector[2] = mode3D ? path->End.Down - path->Start.Down : 0.0f;
  92
  93     // Current progress location relative to end
  94     diff[0]   = path->End.North - cur_point[0];
  95     diff[1]   = path->End.East - cur_point[1];
  96     diff[2]   = mode3D ? path->End.Down - cur_point[2] : 0.0f;
  97
  98     dist_diff = vector_lengthf(diff, 3);
  99     dist_path = vector_lengthf(status->path_vector, 3);
 100
 101     if (dist_diff < 1e-6f) {
 102         status->fractional_progress  = 1;
 103         status->error = 0.0f;
 104         status->correction_vector[0] = status->correction_vector[1] = status->correction_vector[2] = 0.0f;
 105         // we have no base movement direction in this mode
 106         status->path_vector[0] = status->path_vector[1] = status->path_vector[2] = 0.0f;
 107
 108         return;
 109     }
 110
 111     if (fmaxf(dist_path, 1.0f) > dist_diff) {
 112         status->fractional_progress = 1 - dist_diff / fmaxf(dist_path, 1.0f);
 113     } else {
 114         status->fractional_progress = 0; // we don't want fractional_progress to become negative
 115     }
 116     status->error = dist_diff;
 117
 118     // Compute correction vector
 119     status->correction_vector[0] = diff[0];
 120     status->correction_vector[1] = diff[1];
 121     status->correction_vector[2] = diff[2];
 122
 123     // base movement direction in this mode is a constant velocity offset on top of correction in the same direction
 124     status->path_vector[0] = path->EndingVelocity * status->correction_vector[0] / dist_diff;
 125     status->path_vector[1] = path->EndingVelocity * status->correction_vector[1] / dist_diff;
 126     status->path_vector[2] = path->EndingVelocity * status->correction_vector[2] / dist_diff;
 127 }
 128
 129 /**
 130  * @brief Compute progress along path and deviation from it
 131  * @param[in] path PathDesired
 132  * @param[in] cur_point Current location
 133  * @param[out] status Structure containing progress along path and deviation
 134  * @param[in] mode3D set true to include altitude in distance and progress calculation
 135  */
 136 static void path_vector(PathDesiredData *path, float *cur_point, struct path_status *status, bool mode3D)
 137 {
 138     float diff[3];
 139     float dist_path;
 140     float dot;
 141     float velocity;
 142     float track_point[3];
 143
 144     // Distance to go
 145     status->path_vector[0] = path->End.North - path->Start.North;
 146     status->path_vector[1] = path->End.East - path->Start.East;
 147     status->path_vector[2] = mode3D ? path->End.Down - path->Start.Down : 0.0f;
 148
 149     // Current progress location relative to start
 150     diff[0]   = cur_point[0] - path->Start.North;
 151     diff[1]   = cur_point[1] - path->Start.East;
 152     diff[2]   = mode3D ? cur_point[2] - path->Start.Down : 0.0f;
 153
 154     dot       = status->path_vector[0] * diff[0] + status->path_vector[1] * diff[1] + status->path_vector[2] * diff[2];
 155     dist_path = vector_lengthf(status->path_vector, 3);
 156
 157     if (dist_path > 1e-6f) {
 158         // Compute direction to travel & progress
 159         status->fractional_progress = dot / (dist_path * dist_path);
 160     } else {
 161         // Fly towards the endpoint to prevent flying away,
 162         // but assume progress=1 either way.
 163         path_endpoint(path, cur_point, status, mode3D);
 164         status->fractional_progress = 1;
 165         return;
 166     }
 167     // Compute point on track that is closest to our current position.
 168     track_point[0] = status->fractional_progress * status->path_vector[0] + path->Start.North;
 169     track_point[1] = status->fractional_progress * status->path_vector[1] + path->Start.East;
 170     track_point[2] = status->fractional_progress * status->path_vector[2] + path->Start.Down;
 171
 172     status->correction_vector[0] = track_point[0] - cur_point[0];
 173     status->correction_vector[1] = track_point[1] - cur_point[1];
 174     status->correction_vector[2] = track_point[2] - cur_point[2];
 175
 176     status->error = vector_lengthf(status->correction_vector, 3);
 177
 178     // correct movement vector to current velocity
 179     velocity = path->StartingVelocity + boundf(status->fractional_progress, 0.0f, 1.0f) * (path->EndingVelocity - path->StartingVelocity);
 180     status->path_vector[0] = velocity * status->path_vector[0] / dist_path;
 181     status->path_vector[1] = velocity * status->path_vector[1] / dist_path;
 182     status->path_vector[2] = velocity * status->path_vector[2] / dist_path;
 183 }
 184
 185 /**
 186  * @brief Compute progress along circular path and deviation from it
 187  * @param[in] path PathDesired
 188  * @param[in] cur_point Current location
 189  * @param[out] status Structure containing progress along path and deviation
 190  */
 191 static void path_circle(PathDesiredData *path, float *cur_point, struct path_status *status, bool clockwise)
 192 {
 193     float radius_north, radius_east, diff_north, diff_east, diff_down;
 194     float radius, cradius;
 195     float normal[2];
 196     float progress;
 197     float a_diff, a_radius;
 198
 199     // Radius
 200     radius_north = path->End.North - path->Start.North;
 201     radius_east  = path->End.East - path->Start.East;
 202
 203     // Current location relative to center
 204     diff_north   = cur_point[0] - path->End.North;
 205     diff_east    = cur_point[1] - path->End.East;
 206     diff_down    = cur_point[2] - path->End.Down;
 207
 208     radius  = sqrtf(squaref(radius_north) + squaref(radius_east));
 209     cradius = sqrtf(squaref(diff_north) + squaref(diff_east));
 210
 211     // circles are always horizontal (for now - TODO: allow 3d circles - problem: clockwise/counterclockwise does no longer apply)
 212     status->path_vector[2] = 0.0f;
 213
 214     // error is current radius minus wanted radius - positive if too close
 215     status->error = radius - cradius;
 216
 217     if (cradius < 1e-6f) {
 218         // cradius is zero, just fly somewhere
 219         status->fractional_progress  = 1;
 220         status->correction_vector[0] = 0;
 221         status->correction_vector[1] = 0;
 222         status->path_vector[0] = path->EndingVelocity;
 223         status->path_vector[1] = 0;
 224     } else {
 225         if (clockwise) {
 226             // Compute the normal to the radius clockwise
 227             normal[0] = -diff_east / cradius;
 228             normal[1] = diff_north / cradius;
 229         } else {
 230             // Compute the normal to the radius counter clockwise
 231             normal[0] = diff_east / cradius;
 232             normal[1] = -diff_north / cradius;
 233         }
 234
 235         // normalize progress to 0..1
 236         a_diff   = atan2f(diff_north, diff_east);
 237         a_radius = atan2f(radius_north, radius_east);
 238
 239         if (a_diff < 0) {
 240             a_diff += 2.0f * M_PI_F;
 241         }
 242         if (a_radius < 0) {
 243             a_radius += 2.0f * M_PI_F;
 244         }
 245
 246         progress = (a_diff - a_radius + M_PI_F) / (2.0f * M_PI_F);
 247
 248         if (progress < 0.0f) {
 249             progress += 1.0f;
 250         } else if (progress >= 1.0f) {
 251             progress -= 1.0f;
 252         }
 253
 254         if (clockwise) {
 255             progress = 1.0f - progress;
 256         }
 257
 258         status->fractional_progress = progress;
 259
 260         // Compute direction to travel
 261         status->path_vector[0] = normal[0] * path->EndingVelocity;
 262         status->path_vector[1] = normal[1] * path->EndingVelocity;
 263
 264         // Compute direction to correct error
 265         status->correction_vector[0] = status->error * diff_north / cradius;
 266         status->correction_vector[1] = status->error * diff_east / cradius;
 267     }
 268
 269     status->correction_vector[2] = -diff_down;
 270
 271     status->error = fabs(status->error);
 272 }