flight/libraries/paths.c

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