flight/libraries/plans.c

   1 /**
   2  ******************************************************************************
   3  * @addtogroup OpenPilotLibraries OpenPilot Libraries
   4  * @{
   5  * @addtogroup Navigation
   6  * @brief setups RTH/PH and other pathfollower/pathplanner status
   7  * @{
   8  *
   9  * @file       plan.c
  10  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
  11  *
  12  * @see        The GNU Public License (GPL) Version 3
  13  *
  14  ******************************************************************************/
  15 /*
  16  * This program is free software; you can redistribute it and/or modify
  17  * it under the terms of the GNU General Public License as published by
  18  * the Free Software Foundation; either version 3 of the License, or
  19  * (at your option) any later version.
  20  *
  21  * This program is distributed in the hope that it will be useful, but
  22  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  23  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  24  * for more details.
  25  *
  26  * You should have received a copy of the GNU General Public License along
  27  * with this program; if not, write to the Free Software Foundation, Inc.,
  28  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  29  */
  30
  31 #include <plans.h>
  32 #include <openpilot.h>
  33 #include <attitudesettings.h>
  34 #include <takeofflocation.h>
  35 #include <pathdesired.h>
  36 #include <positionstate.h>
  37 #include <flightmodesettings.h>
  38 #include <manualcontrolcommand.h>
  39 #include <attitudestate.h>
  40 #include <sin_lookup.h>
  41
  42 #define UPDATE_EXPECTED 0.02f
  43 #define UPDATE_MIN      1.0e-6f
  44 #define UPDATE_MAX      1.0f
  45 #define UPDATE_ALPHA    1.0e-2f
  46
  47 /**
  48  * @brief initialize UAVOs and structs used by this library
  49  */
  50 void plan_initialize()
  51 {
  52     TakeOffLocationInitialize();
  53     PositionStateInitialize();
  54     PathDesiredInitialize();
  55     FlightModeSettingsInitialize();
  56     AttitudeStateInitialize();
  57     ManualControlCommandInitialize();
  58 }
  59
  60 /**
  61  * @brief setup pathplanner/pathfollower for positionhold
  62  */
  63 void plan_setup_positionHold()
  64 {
  65     PositionStateData positionState;
  66
  67     PositionStateGet(&positionState);
  68     PathDesiredData pathDesired;
  69     PathDesiredGet(&pathDesired);
  70
  71     FlightModeSettingsPositionHoldOffsetData offset;
  72     FlightModeSettingsPositionHoldOffsetGet(&offset);
  73
  74     pathDesired.End.North        = positionState.North;
  75     pathDesired.End.East         = positionState.East;
  76     pathDesired.End.Down         = positionState.Down;
  77     pathDesired.Start.North      = positionState.North + offset.Horizontal; // in FlyEndPoint the direction of this vector does not matter
  78     pathDesired.Start.East       = positionState.East;
  79     pathDesired.Start.Down       = positionState.Down;
  80     pathDesired.StartingVelocity = 0.0f;
  81     pathDesired.EndingVelocity   = 0.0f;
  82     pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
  83
  84     PathDesiredSet(&pathDesired);
  85 }
  86
  87 /**
  88  * @brief setup pathplanner/pathfollower for return to base
  89  */
  90 void plan_setup_returnToBase()
  91 {
  92     // Simple Return To Base mode - keep altitude the same applying configured delta, fly to takeoff position
  93     float positionStateDown;
  94
  95     PositionStateDownGet(&positionStateDown);
  96
  97     PathDesiredData pathDesired;
  98     PathDesiredGet(&pathDesired);
  99
 100     TakeOffLocationData takeoffLocation;
 101     TakeOffLocationGet(&takeoffLocation);
 102
 103     // TODO: right now VTOLPF does fly straight to destination altitude.
 104     // For a safer RTB destination altitude will be the higher between takeofflocation and current position (corrected with safety margin)
 105
 106     float destDown;
 107     FlightModeSettingsReturnToBaseAltitudeOffsetGet(&destDown);
 108     destDown = MIN(positionStateDown, takeoffLocation.Down) - destDown;
 109     FlightModeSettingsPositionHoldOffsetData offset;
 110     FlightModeSettingsPositionHoldOffsetGet(&offset);
 111
 112     pathDesired.End.North        = takeoffLocation.North;
 113     pathDesired.End.East         = takeoffLocation.East;
 114     pathDesired.End.Down         = destDown;
 115
 116     pathDesired.Start.North      = takeoffLocation.North + offset.Horizontal; // in FlyEndPoint the direction of this vector does not matter
 117     pathDesired.Start.East       = takeoffLocation.East;
 118     pathDesired.Start.Down       = destDown;
 119
 120     pathDesired.StartingVelocity = 0.0f;
 121     pathDesired.EndingVelocity   = 0.0f;
 122     pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
 123
 124     PathDesiredSet(&pathDesired);
 125 }
 126
 127 static PiOSDeltatimeConfig landdT;
 128 void plan_setup_land()
 129 {
 130     float descendspeed;
 131
 132     plan_setup_positionHold();
 133
 134     FlightModeSettingsLandingVelocityGet(&descendspeed);
 135     PathDesiredData pathDesired;
 136     PathDesiredGet(&pathDesired);
 137     pathDesired.StartingVelocity = descendspeed;
 138     pathDesired.EndingVelocity   = descendspeed;
 139     PathDesiredSet(&pathDesired);
 140     PIOS_DELTATIME_Init(&landdT, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA);
 141 }
 142
 143 /**
 144  * @brief execute land
 145  */
 146 void plan_run_land()
 147 {
 148     float downPos, descendspeed;
 149     PathDesiredEndData pathDesiredEnd;
 150
 151     PositionStateDownGet(&downPos); // current down position
 152     PathDesiredEndGet(&pathDesiredEnd); // desired position
 153     PathDesiredEndingVelocityGet(&descendspeed);
 154
 155     // desired position is updated to match the desired descend speed but don't run ahead
 156     // too far if the current position can't keep up. This normaly means we have landed.
 157     if (pathDesiredEnd.Down - downPos < 10) {
 158         pathDesiredEnd.Down += descendspeed * PIOS_DELTATIME_GetAverageSeconds(&landdT);
 159     }
 160
 161     PathDesiredEndSet(&pathDesiredEnd);
 162 }
 163
 164
 165 /**
 166  * @brief positionvario functionality
 167  */
 168 static bool vario_hold    = true;
 169 static float hold_position[3];
 170 static float vario_control_lowpass[4];
 171 static float vario_course = 0.0f;
 172
 173 static void plan_setup_PositionVario()
 174 {
 175     vario_hold = true;
 176     vario_control_lowpass[0] = 0.0f;
 177     vario_control_lowpass[1] = 0.0f;
 178     vario_control_lowpass[2] = 0.0f;
 179     vario_control_lowpass[3] = 0.0f;
 180     AttitudeStateYawGet(&vario_course);
 181     plan_setup_positionHold();
 182 }
 183
 184 void plan_setup_CourseLock()
 185 {
 186     plan_setup_PositionVario();
 187 }
 188
 189 void plan_setup_PositionRoam()
 190 {
 191     plan_setup_PositionVario();
 192 }
 193
 194 void plan_setup_HomeLeash()
 195 {
 196     plan_setup_PositionVario();
 197 }
 198
 199 void plan_setup_AbsolutePosition()
 200 {
 201     plan_setup_PositionVario();
 202 }
 203
 204
 205 #define DEADBAND 0.1f
 206 static bool normalizeDeadband(float controlVector[4])
 207 {
 208     bool moving = false;
 209
 210     // roll, pitch, yaw between -1 and +1
 211     // thrust between 0 and 1 mapped to -1 to +1
 212     controlVector[3] = (2.0f * controlVector[3]) - 1.0f;
 213     int t;
 214
 215     for (t = 0; t < 4; t++) {
 216         if (controlVector[t] < -DEADBAND) {
 217             moving = true;
 218             controlVector[t] += DEADBAND;
 219         } else if (controlVector[t] > DEADBAND) {
 220             moving = true;
 221             controlVector[t] -= DEADBAND;
 222         } else {
 223             controlVector[t] = 0.0f;
 224         }
 225         // deadband has been cut out, scale value back to [-1,+1]
 226         controlVector[t] *= (1.0f / (1.0f - DEADBAND));
 227         controlVector[t]  = boundf(controlVector[t], -1.0f, 1.0f);
 228     }
 229
 230     return moving;
 231 }
 232
 233 typedef enum { COURSE, FPV, LOS, NSEW } vario_type;
 234
 235 static void getVector(float controlVector[4], vario_type type)
 236 {
 237     FlightModeSettingsPositionHoldOffsetData offset;
 238
 239     FlightModeSettingsPositionHoldOffsetGet(&offset);
 240
 241     // scale controlVector[3] (thrust) by vertical/horizontal to have vertical plane less sensitive
 242     controlVector[3] *= offset.Vertical / offset.Horizontal;
 243
 244     float length = sqrtf(controlVector[0] * controlVector[0] + controlVector[1] * controlVector[1] + controlVector[3] * controlVector[3]);
 245
 246     if (length <= 1e-9f) {
 247         length = 1.0f; // should never happen as getVector is not called if control within deadband
 248     }
 249     {
 250         float direction[3] = {
 251             controlVector[1] / length, // pitch is north
 252             controlVector[0] / length, // roll is east
 253             controlVector[3] / length // thrust is down
 254         };
 255         controlVector[0] = direction[0];
 256         controlVector[1] = direction[1];
 257         controlVector[2] = direction[2];
 258     }
 259     controlVector[3] = length * offset.Horizontal;
 260
 261     // rotate north and east - rotation angle based on type
 262     float angle;
 263     switch (type) {
 264     case COURSE:
 265         angle = vario_course;
 266         break;
 267     case NSEW:
 268         angle = 0.0f;
 269         // NSEW no rotation takes place
 270         break;
 271     case FPV:
 272         // local rotation, using current yaw
 273         AttitudeStateYawGet(&angle);
 274         break;
 275     case LOS:
 276         // determine location based on vector from takeoff to current location
 277     {
 278         PositionStateData positionState;
 279         PositionStateGet(&positionState);
 280         TakeOffLocationData takeoffLocation;
 281         TakeOffLocationGet(&takeoffLocation);
 282         angle = RAD2DEG(atan2f(positionState.East - takeoffLocation.East, positionState.North - takeoffLocation.North));
 283     }
 284     break;
 285     }
 286     // rotate horizontally by angle
 287     {
 288         float rotated[2] = {
 289             controlVector[0] * cos_lookup_deg(angle) - controlVector[1] * sin_lookup_deg(angle),
 290             controlVector[0] * sin_lookup_deg(angle) + controlVector[1] * cos_lookup_deg(angle)
 291         };
 292         controlVector[0] = rotated[0];
 293         controlVector[1] = rotated[1];
 294     }
 295 }
 296
 297
 298 static void plan_run_PositionVario(vario_type type)
 299 {
 300     float controlVector[4];
 301     float alpha;
 302     PathDesiredData pathDesired;
 303
 304     PathDesiredGet(&pathDesired);
 305     FlightModeSettingsPositionHoldOffsetData offset;
 306     FlightModeSettingsPositionHoldOffsetGet(&offset);
 307
 308
 309     ManualControlCommandRollGet(&controlVector[0]);
 310     ManualControlCommandPitchGet(&controlVector[1]);
 311     ManualControlCommandYawGet(&controlVector[2]);
 312     ManualControlCommandThrustGet(&controlVector[3]);
 313
 314
 315     FlightModeSettingsVarioControlLowPassAlphaGet(&alpha);
 316     vario_control_lowpass[0] = alpha * vario_control_lowpass[0] + (1.0f - alpha) * controlVector[0];
 317     vario_control_lowpass[1] = alpha * vario_control_lowpass[1] + (1.0f - alpha) * controlVector[1];
 318     vario_control_lowpass[2] = alpha * vario_control_lowpass[2] + (1.0f - alpha) * controlVector[2];
 319     vario_control_lowpass[3] = alpha * vario_control_lowpass[3] + (1.0f - alpha) * controlVector[3];
 320     controlVector[0] = vario_control_lowpass[0];
 321     controlVector[1] = vario_control_lowpass[1];
 322     controlVector[2] = vario_control_lowpass[2];
 323     controlVector[3] = vario_control_lowpass[3];
 324
 325     // check if movement is desired
 326     if (normalizeDeadband(controlVector) == false) {
 327         // no movement desired, re-enter positionHold at current start-position
 328         if (!vario_hold) {
 329             vario_hold = true;
 330
 331             // new hold position is the position that was previously the start position
 332             pathDesired.End.North   = hold_position[0];
 333             pathDesired.End.East    = hold_position[1];
 334             pathDesired.End.Down    = hold_position[2];
 335             // while the new start position has the same offset as in position hold
 336             pathDesired.Start.North = pathDesired.End.North + offset.Horizontal; // in FlyEndPoint the direction of this vector does not matter
 337             pathDesired.Start.East  = pathDesired.End.East;
 338             pathDesired.Start.Down  = pathDesired.End.Down;
 339             PathDesiredSet(&pathDesired);
 340         }
 341     } else {
 342         PositionStateData positionState;
 343         PositionStateGet(&positionState);
 344
 345         // flip pitch to have pitch down (away) point north
 346         controlVector[1] = -controlVector[1];
 347         getVector(controlVector, type);
 348
 349         // layout of control Vector : unitVector in movement direction {0,1,2} vector length {3} velocity {4}
 350         if (vario_hold) {
 351             // start position is the position that was previously the hold position
 352             vario_hold = false;
 353             hold_position[0] = pathDesired.End.North;
 354             hold_position[1] = pathDesired.End.East;
 355             hold_position[2] = pathDesired.End.Down;
 356         } else {
 357             // start position is advanced according to movement - in the direction of ControlVector only
 358             // projection using scalar product
 359             float kp = (positionState.North - hold_position[0]) * controlVector[0]
 360                        + (positionState.East - hold_position[1]) * controlVector[1]
 361                        + (positionState.Down - hold_position[2]) * -controlVector[2];
 362             if (kp > 0.0f) {
 363                 hold_position[0] += kp * controlVector[0];
 364                 hold_position[1] += kp * controlVector[1];
 365                 hold_position[2] += kp * -controlVector[2];
 366             }
 367         }
 368         // new destination position is advanced based on controlVector
 369         pathDesired.End.North   = hold_position[0] + controlVector[0] * controlVector[3];
 370         pathDesired.End.East    = hold_position[1] + controlVector[1] * controlVector[3];
 371         pathDesired.End.Down    = hold_position[2] - controlVector[2] * controlVector[3];
 372         // the new start position has the same offset as in position hold
 373         pathDesired.Start.North = pathDesired.End.North + offset.Horizontal; // in FlyEndPoint the direction of this vector does not matter
 374         pathDesired.Start.East  = pathDesired.End.East;
 375         pathDesired.Start.Down  = pathDesired.End.Down;
 376         PathDesiredSet(&pathDesired);
 377     }
 378 }
 379
 380 void plan_run_CourseLock()
 381 {
 382     plan_run_PositionVario(COURSE);
 383 }
 384
 385 void plan_run_PositionRoam()
 386 {
 387     plan_run_PositionVario(FPV);
 388 }
 389
 390 void plan_run_HomeLeash()
 391 {
 392     plan_run_PositionVario(LOS);
 393 }
 394
 395 void plan_run_AbsolutePosition()
 396 {
 397     plan_run_PositionVario(NSEW);
 398 }
 399
 400
 401 /**
 402  * @brief setup pathplanner/pathfollower for AutoCruise
 403  */
 404 static PiOSDeltatimeConfig actimeval;
 405 void plan_setup_AutoCruise()
 406 {
 407     PositionStateData positionState;
 408
 409     PositionStateGet(&positionState);
 410     PathDesiredData pathDesired;
 411     PathDesiredGet(&pathDesired);
 412
 413     FlightModeSettingsPositionHoldOffsetData offset;
 414     FlightModeSettingsPositionHoldOffsetGet(&offset);
 415
 416     // initialization is flight in direction of the nose.
 417     // the velocity is not relevant, as it will be reset by the run function even during first call
 418     float angle;
 419     AttitudeStateYawGet(&angle);
 420     float vector[2] = {
 421         cos_lookup_deg(angle),
 422         sin_lookup_deg(angle)
 423     };
 424     hold_position[0]             = positionState.North;
 425     hold_position[1]             = positionState.East;
 426     hold_position[2]             = positionState.Down;
 427     pathDesired.End.North        = hold_position[0] + vector[0];
 428     pathDesired.End.East         = hold_position[1] + vector[1];
 429     pathDesired.End.Down         = hold_position[2];
 430     // start position has the same offset as in position hold
 431     pathDesired.Start.North      = pathDesired.End.North + offset.Horizontal; // in FlyEndPoint the direction of this vector does not matter
 432     pathDesired.Start.East       = pathDesired.End.East;
 433     pathDesired.Start.Down       = pathDesired.End.Down;
 434     pathDesired.StartingVelocity = 0.0f;
 435     pathDesired.EndingVelocity   = 0.0f;
 436     pathDesired.Mode             = PATHDESIRED_MODE_FLYENDPOINT;
 437
 438     PathDesiredSet(&pathDesired);
 439
 440     // re-iniztializing deltatime is valid and also good practice here since
 441     // getAverageSeconds() has not been called/updated in a long time if we were in a different flightmode.
 442     PIOS_DELTATIME_Init(&actimeval, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA);
 443 }
 444
 445 /**
 446  * @brief execute autocruise
 447  */
 448 void plan_run_AutoCruise()
 449 {
 450     PositionStateData positionState;
 451
 452     PositionStateGet(&positionState);
 453     PathDesiredData pathDesired;
 454     PathDesiredGet(&pathDesired);
 455     FlightModeSettingsPositionHoldOffsetData offset;
 456     FlightModeSettingsPositionHoldOffsetGet(&offset);
 457
 458     float controlVector[4];
 459     ManualControlCommandRollGet(&controlVector[0]);
 460     ManualControlCommandPitchGet(&controlVector[1]);
 461     ManualControlCommandYawGet(&controlVector[2]);
 462     controlVector[3] = 0.5f; // dummy, thrust is normalized separately
 463     normalizeDeadband(controlVector); // return value ignored
 464     ManualControlCommandThrustGet(&controlVector[3]); // no deadband as we are using thrust for velocity
 465     controlVector[3] = boundf(controlVector[3], 1e-6f, 1.0f); // bound to above zero, to prevent loss of vector direction
 466
 467     // normalize old desired movement vector
 468     float vector[3] = { pathDesired.End.North - hold_position[0],
 469                         pathDesired.End.East - hold_position[1],
 470                         pathDesired.End.Down - hold_position[2] };
 471     float length    = sqrtf(vector[0] * vector[0] + vector[1] * vector[1] + vector[2] * vector[2]);
 472     if (length < 1e-9f) {
 473         length = 1.0f; // should not happen since initialized properly in setup()
 474     }
 475     vector[0] /= length;
 476     vector[1] /= length;
 477     vector[2] /= length;
 478
 479     // start position is advanced according to actual movement - in the direction of desired vector only
 480     // projection using scalar product
 481     float kp = (positionState.North - hold_position[0]) * vector[0]
 482                + (positionState.East - hold_position[1]) * vector[1]
 483                + (positionState.Down - hold_position[2]) * vector[2];
 484     if (kp > 0.0f) {
 485         hold_position[0] += kp * vector[0];
 486         hold_position[1] += kp * vector[1];
 487         hold_position[2] += kp * vector[2];
 488     }
 489
 490     // new angle is equal to old angle plus offset depending on yaw input and time
 491     // (controlVector is normalized with a deadband, change is zero within deadband)
 492     float angle = RAD2DEG(atan2f(vector[1], vector[0]));
 493     float dT    = PIOS_DELTATIME_GetAverageSeconds(&actimeval);
 494     angle    += 10.0f * controlVector[2] * dT; // TODO magic value could eventually end up in a to be created settings
 495
 496     // resulting movement vector is scaled by velocity demand in controlvector[3] [0.0-1.0]
 497     vector[0] = cosf(DEG2RAD(angle)) * offset.Horizontal * controlVector[3];
 498     vector[1] = sinf(DEG2RAD(angle)) * offset.Horizontal * controlVector[3];
 499     vector[2] = -controlVector[1] * offset.Vertical * controlVector[3];
 500
 501     pathDesired.End.North   = hold_position[0] + vector[0];
 502     pathDesired.End.East    = hold_position[1] + vector[1];
 503     pathDesired.End.Down    = hold_position[2] + vector[2];
 504     // start position has the same offset as in position hold
 505     pathDesired.Start.North = pathDesired.End.North + offset.Horizontal; // in FlyEndPoint the direction of this vector does not matter
 506     pathDesired.Start.East  = pathDesired.End.East;
 507     pathDesired.Start.Down  = pathDesired.End.Down;
 508     PathDesiredSet(&pathDesired);
 509 }