flight/libraries/plans.c

   1 /**
   2  ******************************************************************************
   3  *
   4  * @file       plan.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 setups RTH/PH and other pathfollower/pathplanner status
   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 <plans.h>
  31 #include <openpilot.h>
  32 #include <attitudesettings.h>
  33 #include <takeofflocation.h>
  34 #include <pathdesired.h>
  35 #include <positionstate.h>
  36 #include <flightmodesettings.h>
  37 #include <flightstatus.h>
  38 #include <velocitystate.h>
  39 #include <manualcontrolcommand.h>
  40 #include <attitudestate.h>
  41 #include <vtolpathfollowersettings.h>
  42 #include <stabilizationbank.h>
  43 #include <stabilizationdesired.h>
  44 #include <sin_lookup.h>
  45 #include <sanitycheck.h>
  46 #include <statusvtolautotakeoff.h>
  47
  48 #define UPDATE_EXPECTED 0.02f
  49 #define UPDATE_MIN      1.0e-6f
  50 #define UPDATE_MAX      1.0f
  51 #define UPDATE_ALPHA    1.0e-2f
  52
  53
  54 static float applyExpo(float value, float expo);
  55
  56
  57 static float applyExpo(float value, float expo)
  58 {
  59     // note: fastPow makes a small error, therefore result needs to be bound
  60     float exp = boundf(fastPow(1.00695f, expo), 0.5f, 2.0f);
  61
  62     // magic number scales expo
  63     // so that
  64     // expo=100 yields value**10
  65     // expo=0 yields value**1
  66     // expo=-100 yields value**(1/10)
  67     // (pow(2.0,1/100)~=1.00695)
  68     if (value > 0.0f) {
  69         return boundf(fastPow(value, exp), 0.0f, 1.0f);
  70     } else if (value < -0.0f) {
  71         return boundf(-fastPow(-value, exp), -1.0f, 0.0f);
  72     } else {
  73         return 0.0f;
  74     }
  75 }
  76
  77
  78 /**
  79  * @brief initialize UAVOs and structs used by this library
  80  */
  81 void plan_initialize()
  82 {
  83     TakeOffLocationInitialize();
  84     PositionStateInitialize();
  85     PathDesiredInitialize();
  86     FlightModeSettingsInitialize();
  87     FlightStatusInitialize();
  88     AttitudeStateInitialize();
  89     ManualControlCommandInitialize();
  90     VelocityStateInitialize();
  91     VtolPathFollowerSettingsInitialize();
  92     StabilizationBankInitialize();
  93     StabilizationDesiredInitialize();
  94 }
  95
  96 /**
  97  * @brief setup pathplanner/pathfollower for positionhold
  98  */
  99 void plan_setup_positionHold()
 100 {
 101     PositionStateData positionState;
 102
 103     PositionStateGet(&positionState);
 104     PathDesiredData pathDesired;
 105     // re-initialise in setup stage
 106     memset(&pathDesired, 0, sizeof(PathDesiredData));
 107
 108     FlightModeSettingsPositionHoldOffsetData offset;
 109     FlightModeSettingsPositionHoldOffsetGet(&offset);
 110
 111     pathDesired.End.North        = positionState.North;
 112     pathDesired.End.East         = positionState.East;
 113     pathDesired.End.Down         = positionState.Down;
 114     pathDesired.Start.North      = positionState.North + offset.Horizontal; // in FlyEndPoint the direction of this vector does not matter
 115     pathDesired.Start.East       = positionState.East;
 116     pathDesired.Start.Down       = positionState.Down;
 117     pathDesired.StartingVelocity = 0.0f;
 118     pathDesired.EndingVelocity   = 0.0f;
 119     pathDesired.Mode = PATHDESIRED_MODE_GOTOENDPOINT;
 120
 121     PathDesiredSet(&pathDesired);
 122 }
 123
 124
 125 /**
 126  * @brief setup pathplanner/pathfollower for return to base
 127  */
 128 void plan_setup_returnToBase()
 129 {
 130     // Simple Return To Base mode - keep altitude the same applying configured delta, fly to takeoff position
 131     float positionStateDown;
 132
 133     PositionStateDownGet(&positionStateDown);
 134
 135     PathDesiredData pathDesired;
 136     // re-initialise in setup stage
 137     memset(&pathDesired, 0, sizeof(PathDesiredData));
 138
 139     TakeOffLocationData takeoffLocation;
 140     TakeOffLocationGet(&takeoffLocation);
 141
 142     // TODO: right now VTOLPF does fly straight to destination altitude.
 143     // For a safer RTB destination altitude will be the higher between takeofflocation and current position (corrected with safety margin)
 144
 145     float destDown;
 146     float destVelocity;
 147     FlightModeSettingsReturnToBaseAltitudeOffsetGet(&destDown);
 148     FlightModeSettingsReturnToBaseVelocityGet(&destVelocity);
 149     destDown = MIN(positionStateDown, takeoffLocation.Down) - destDown;
 150     FlightModeSettingsPositionHoldOffsetData offset;
 151     FlightModeSettingsPositionHoldOffsetGet(&offset);
 152
 153     pathDesired.End.North        = takeoffLocation.North;
 154     pathDesired.End.East         = takeoffLocation.East;
 155     pathDesired.End.Down         = destDown;
 156
 157     pathDesired.Start.North      = takeoffLocation.North + offset.Horizontal; // in FlyEndPoint the direction of this vector does not matter
 158     pathDesired.Start.East       = takeoffLocation.East;
 159     pathDesired.Start.Down       = destDown;
 160
 161     pathDesired.StartingVelocity = destVelocity;
 162     pathDesired.EndingVelocity   = destVelocity;
 163
 164     FlightModeSettingsReturnToBaseNextCommandOptions ReturnToBaseNextCommand;
 165     FlightModeSettingsReturnToBaseNextCommandGet(&ReturnToBaseNextCommand);
 166     pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_NEXTCOMMAND] = (float)ReturnToBaseNextCommand;
 167     pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_UNUSED1]     = 0.0f;
 168     pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_UNUSED2]     = 0.0f;
 169     pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_UNUSED3]     = 0.0f;
 170     pathDesired.Mode = PATHDESIRED_MODE_GOTOENDPOINT;
 171
 172     PathDesiredSet(&pathDesired);
 173 }
 174
 175 void plan_setup_AutoTakeoff()
 176 {
 177     PathDesiredData pathDesired;
 178
 179     memset(&pathDesired, 0, sizeof(PathDesiredData));
 180     PositionStateData positionState;
 181
 182     PositionStateGet(&positionState);
 183     float autotakeoff_height;
 184
 185     FlightModeSettingsAutoTakeOffHeightGet(&autotakeoff_height);
 186     autotakeoff_height      = fabsf(autotakeoff_height);
 187
 188     pathDesired.Start.North = positionState.North;
 189     pathDesired.Start.East  = positionState.East;
 190     pathDesired.Start.Down  = positionState.Down;
 191     pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_NORTH] = 0.0f;
 192     pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_EAST]  = 0.0f;
 193     pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_DOWN]  = 0.0f;
 194     pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_CONTROLSTATE] = 0.0f;
 195
 196     pathDesired.End.North = positionState.North;
 197     pathDesired.End.East  = positionState.East;
 198     pathDesired.End.Down  = positionState.Down - autotakeoff_height;
 199
 200     pathDesired.StartingVelocity = 0.0f;
 201     pathDesired.EndingVelocity   = 0.0f;
 202     pathDesired.Mode = PATHDESIRED_MODE_AUTOTAKEOFF;
 203     PathDesiredSet(&pathDesired);
 204 }
 205
 206 static void plan_setup_land_helper(PathDesiredData *pathDesired)
 207 {
 208     PositionStateData positionState;
 209
 210     PositionStateGet(&positionState);
 211     float velocity_down;
 212
 213     FlightModeSettingsLandingVelocityGet(&velocity_down);
 214
 215     pathDesired->Start.North = positionState.North;
 216     pathDesired->Start.East  = positionState.East;
 217     pathDesired->Start.Down  = positionState.Down;
 218     pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_VELOCITYVECTOR_NORTH] = 0.0f;
 219     pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_VELOCITYVECTOR_EAST]  = 0.0f;
 220     pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_VELOCITYVECTOR_DOWN]  = velocity_down;
 221
 222     pathDesired->End.North = positionState.North;
 223     pathDesired->End.East  = positionState.East;
 224     pathDesired->End.Down  = positionState.Down;
 225
 226     pathDesired->StartingVelocity = 0.0f;
 227     pathDesired->EndingVelocity   = 0.0f;
 228     pathDesired->Mode = PATHDESIRED_MODE_LAND;
 229     pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_OPTIONS] = (float)PATHDESIRED_MODEPARAMETER_LAND_OPTION_HORIZONTAL_PH;
 230 }
 231
 232 void plan_setup_land()
 233 {
 234     PathDesiredData pathDesired;
 235
 236     // re-initialise in setup stage
 237     memset(&pathDesired, 0, sizeof(PathDesiredData));
 238
 239     plan_setup_land_helper(&pathDesired);
 240     PathDesiredSet(&pathDesired);
 241 }
 242
 243 static void plan_setup_land_from_velocityroam()
 244 {
 245     plan_setup_land();
 246     FlightStatusAssistedControlStateOptions assistedControlFlightMode;
 247     assistedControlFlightMode = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_HOLD;
 248     FlightStatusAssistedControlStateSet(&assistedControlFlightMode);
 249 }
 250
 251 /**
 252  * @brief positionvario functionality
 253  */
 254 static bool vario_hold    = true;
 255 static float hold_position[3];
 256 static float vario_control_lowpass[3];
 257 static float vario_course = 0.0f;
 258
 259 static void plan_setup_PositionVario()
 260 {
 261     vario_hold = true;
 262     vario_control_lowpass[0] = 0.0f;
 263     vario_control_lowpass[1] = 0.0f;
 264     vario_control_lowpass[2] = 0.0f;
 265     AttitudeStateYawGet(&vario_course);
 266     plan_setup_positionHold();
 267 }
 268
 269 void plan_setup_CourseLock()
 270 {
 271     plan_setup_PositionVario();
 272 }
 273
 274 void plan_setup_PositionRoam()
 275 {
 276     plan_setup_PositionVario();
 277 }
 278
 279 void plan_setup_VelocityRoam()
 280 {
 281     vario_control_lowpass[0] = 0.0f;
 282     vario_control_lowpass[1] = 0.0f;
 283     vario_control_lowpass[2] = 0.0f;
 284     AttitudeStateYawGet(&vario_course);
 285 }
 286
 287 void plan_setup_HomeLeash()
 288 {
 289     plan_setup_PositionVario();
 290 }
 291
 292 void plan_setup_AbsolutePosition()
 293 {
 294     plan_setup_PositionVario();
 295 }
 296
 297
 298 #define DEADBAND 0.1f
 299 static bool normalizeDeadband(float controlVector[4])
 300 {
 301     bool moving = false;
 302
 303     // roll, pitch, yaw between -1 and +1
 304     // thrust between 0 and 1 mapped to -1 to +1
 305     controlVector[3] = (2.0f * controlVector[3]) - 1.0f;
 306     int t;
 307
 308     for (t = 0; t < 4; t++) {
 309         if (controlVector[t] < -DEADBAND) {
 310             moving = true;
 311             controlVector[t] += DEADBAND;
 312         } else if (controlVector[t] > DEADBAND) {
 313             moving = true;
 314             controlVector[t] -= DEADBAND;
 315         } else {
 316             controlVector[t] = 0.0f;
 317         }
 318         // deadband has been cut out, scale value back to [-1,+1]
 319         controlVector[t] *= (1.0f / (1.0f - DEADBAND));
 320         controlVector[t]  = boundf(controlVector[t], -1.0f, 1.0f);
 321     }
 322
 323     return moving;
 324 }
 325
 326 typedef enum { COURSE, FPV, LOS, NSEW } vario_type;
 327
 328 static void getVector(float controlVector[4], vario_type type)
 329 {
 330     FlightModeSettingsPositionHoldOffsetData offset;
 331
 332     FlightModeSettingsPositionHoldOffsetGet(&offset);
 333
 334     // scale controlVector[3] (thrust) by vertical/horizontal to have vertical plane less sensitive
 335     controlVector[3] *= offset.Vertical / offset.Horizontal;
 336
 337     float length = sqrtf(controlVector[0] * controlVector[0] + controlVector[1] * controlVector[1] + controlVector[3] * controlVector[3]);
 338
 339     if (length <= 1e-9f) {
 340         length = 1.0f; // should never happen as getVector is not called if control within deadband
 341     }
 342     {
 343         float direction[3] = {
 344             controlVector[1] / length, // pitch is north
 345             controlVector[0] / length, // roll is east
 346             controlVector[3] / length // thrust is down
 347         };
 348         controlVector[0] = direction[0];
 349         controlVector[1] = direction[1];
 350         controlVector[2] = direction[2];
 351     }
 352     controlVector[3] = length * offset.Horizontal;
 353
 354     // rotate north and east - rotation angle based on type
 355     float angle;
 356     switch (type) {
 357     case COURSE:
 358         angle = vario_course;
 359         break;
 360     case NSEW:
 361         angle = 0.0f;
 362         // NSEW no rotation takes place
 363         break;
 364     case FPV:
 365         // local rotation, using current yaw
 366         AttitudeStateYawGet(&angle);
 367         break;
 368     case LOS:
 369         // determine location based on vector from takeoff to current location
 370     {
 371         PositionStateData positionState;
 372         PositionStateGet(&positionState);
 373         TakeOffLocationData takeoffLocation;
 374         TakeOffLocationGet(&takeoffLocation);
 375         angle = RAD2DEG(atan2f(positionState.East - takeoffLocation.East, positionState.North - takeoffLocation.North));
 376     }
 377     break;
 378     }
 379     // rotate horizontally by angle
 380     {
 381         float rotated[2] = {
 382             controlVector[0] * cos_lookup_deg(angle) - controlVector[1] * sin_lookup_deg(angle),
 383             controlVector[0] * sin_lookup_deg(angle) + controlVector[1] * cos_lookup_deg(angle)
 384         };
 385         controlVector[0] = rotated[0];
 386         controlVector[1] = rotated[1];
 387     }
 388 }
 389
 390
 391 static void plan_run_PositionVario(vario_type type)
 392 {
 393     float controlVector[4];
 394     float alpha;
 395     PathDesiredData pathDesired;
 396
 397     // Reuse the existing pathdesired object as setup in the setup to avoid
 398     // updating values already set.
 399     PathDesiredGet(&pathDesired);
 400
 401     FlightModeSettingsPositionHoldOffsetData offset;
 402     FlightModeSettingsPositionHoldOffsetGet(&offset);
 403
 404
 405     ManualControlCommandRollGet(&controlVector[0]);
 406     ManualControlCommandPitchGet(&controlVector[1]);
 407     ManualControlCommandYawGet(&controlVector[2]);
 408     ManualControlCommandThrustGet(&controlVector[3]);
 409
 410
 411     FlightModeSettingsVarioControlLowPassAlphaGet(&alpha);
 412     vario_control_lowpass[0] = alpha * vario_control_lowpass[0] + (1.0f - alpha) * controlVector[0];
 413     vario_control_lowpass[1] = alpha * vario_control_lowpass[1] + (1.0f - alpha) * controlVector[1];
 414     vario_control_lowpass[2] = alpha * vario_control_lowpass[2] + (1.0f - alpha) * controlVector[2];
 415     controlVector[0] = vario_control_lowpass[0];
 416     controlVector[1] = vario_control_lowpass[1];
 417     controlVector[2] = vario_control_lowpass[2];
 418
 419     // check if movement is desired
 420     if (normalizeDeadband(controlVector) == false) {
 421         // no movement desired, re-enter positionHold at current start-position
 422         if (!vario_hold) {
 423             vario_hold = true;
 424
 425             // new hold position is the position that was previously the start position
 426             pathDesired.End.North   = hold_position[0];
 427             pathDesired.End.East    = hold_position[1];
 428             pathDesired.End.Down    = hold_position[2];
 429             // while the new start position has the same offset as in position hold
 430             pathDesired.Start.North = pathDesired.End.North + offset.Horizontal; // in FlyEndPoint the direction of this vector does not matter
 431             pathDesired.Start.East  = pathDesired.End.East;
 432             pathDesired.Start.Down  = pathDesired.End.Down;
 433
 434             // set mode explicitly
 435
 436             PathDesiredSet(&pathDesired);
 437         }
 438     } else {
 439         PositionStateData positionState;
 440         PositionStateGet(&positionState);
 441
 442         // flip pitch to have pitch down (away) point north
 443         controlVector[1] = -controlVector[1];
 444         getVector(controlVector, type);
 445
 446         // layout of control Vector : unitVector in movement direction {0,1,2} vector length {3} velocity {4}
 447         if (vario_hold) {
 448             // start position is the position that was previously the hold position
 449             vario_hold = false;
 450             hold_position[0] = pathDesired.End.North;
 451             hold_position[1] = pathDesired.End.East;
 452             hold_position[2] = pathDesired.End.Down;
 453         } else {
 454             // start position is advanced according to movement - in the direction of ControlVector only
 455             // projection using scalar product
 456             float kp = (positionState.North - hold_position[0]) * controlVector[0]
 457                        + (positionState.East - hold_position[1]) * controlVector[1]
 458                        + (positionState.Down - hold_position[2]) * -controlVector[2];
 459             if (kp > 0.0f) {
 460                 hold_position[0] += kp * controlVector[0];
 461                 hold_position[1] += kp * controlVector[1];
 462                 hold_position[2] += kp * -controlVector[2];
 463             }
 464         }
 465         // new destination position is advanced based on controlVector
 466         pathDesired.End.North   = hold_position[0] + controlVector[0] * controlVector[3];
 467         pathDesired.End.East    = hold_position[1] + controlVector[1] * controlVector[3];
 468         pathDesired.End.Down    = hold_position[2] - controlVector[2] * controlVector[3];
 469         // the new start position has the same offset as in position hold
 470         pathDesired.Start.North = pathDesired.End.North + offset.Horizontal; // in FlyEndPoint the direction of this vector does not matter
 471         pathDesired.Start.East  = pathDesired.End.East;
 472         pathDesired.Start.Down  = pathDesired.End.Down;
 473         PathDesiredSet(&pathDesired);
 474     }
 475 }
 476
 477 void plan_run_VelocityRoam()
 478 {
 479     // float alpha;
 480     PathDesiredData pathDesired;
 481
 482     // velocity roam code completely sets pathdesired object. it was not set in setup phase
 483     memset(&pathDesired, 0, sizeof(PathDesiredData));
 484     FlightStatusAssistedControlStateOptions assistedControlFlightMode;
 485     FlightStatusFlightModeOptions flightMode;
 486
 487     FlightModeSettingsPositionHoldOffsetData offset;
 488     FlightModeSettingsPositionHoldOffsetGet(&offset);
 489     FlightStatusAssistedControlStateGet(&assistedControlFlightMode);
 490     FlightStatusFlightModeGet(&flightMode);
 491     StabilizationBankData stabSettings;
 492     StabilizationBankGet(&stabSettings);
 493
 494     ManualControlCommandData cmd;
 495     ManualControlCommandGet(&cmd);
 496
 497     cmd.Roll  = applyExpo(cmd.Roll, stabSettings.StickExpo.Roll);
 498     cmd.Pitch = applyExpo(cmd.Pitch, stabSettings.StickExpo.Pitch);
 499     cmd.Yaw   = applyExpo(cmd.Yaw, stabSettings.StickExpo.Yaw);
 500
 501     bool flagRollPitchHasInput = (fabsf(cmd.Roll) > 0.0f || fabsf(cmd.Pitch) > 0.0f);
 502
 503     if (!flagRollPitchHasInput) {
 504         // no movement desired, re-enter positionHold at current start-position
 505         if (assistedControlFlightMode == FLIGHTSTATUS_ASSISTEDCONTROLSTATE_PRIMARY) {
 506             // initiate braking and change assisted control flight mode to braking
 507             if (flightMode == FLIGHTSTATUS_FLIGHTMODE_LAND) {
 508                 // avoid brake then hold sequence to continue descent.
 509                 plan_setup_land_from_velocityroam();
 510             } else {
 511                 plan_setup_assistedcontrol();
 512             }
 513         }
 514         // otherwise nothing to do in braking/hold modes
 515     } else {
 516         PositionStateData positionState;
 517         PositionStateGet(&positionState);
 518
 519         // Revert assist control state to primary, which in this case implies
 520         // we are in roaming state (a GPS vector assisted velocity roam)
 521         assistedControlFlightMode = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_PRIMARY;
 522
 523         // Calculate desired velocity in each direction
 524         float angle;
 525         AttitudeStateYawGet(&angle);
 526         angle = DEG2RAD(angle);
 527         float cos_angle  = cosf(angle);
 528         float sine_angle = sinf(angle);
 529         float rotated[2] = {
 530             -cmd.Pitch * cos_angle - cmd.Roll * sine_angle,
 531             -cmd.Pitch * sine_angle + cmd.Roll * cos_angle
 532         };
 533         // flip pitch to have pitch down (away) point north
 534         float horizontalVelMax;
 535         float verticalVelMax;
 536         VtolPathFollowerSettingsHorizontalVelMaxGet(&horizontalVelMax);
 537         VtolPathFollowerSettingsVerticalVelMaxGet(&verticalVelMax);
 538         float velocity_north = rotated[0] * horizontalVelMax;
 539         float velocity_east  = rotated[1] * horizontalVelMax;
 540         float velocity_down  = 0.0f;
 541
 542         if (flightMode == FLIGHTSTATUS_FLIGHTMODE_LAND) {
 543             FlightModeSettingsLandingVelocityGet(&velocity_down);
 544         }
 545
 546         float velocity = velocity_north * velocity_north + velocity_east * velocity_east;
 547         velocity = sqrtf(velocity);
 548
 549         // if one stick input (pitch or roll) should we use fly by vector? set arbitrary distance of say 20m after which we
 550         // expect new stick input
 551         // if two stick input pilot is fighting wind manually and we use fly by velocity
 552         // in reality setting velocity desired to zero will fight wind anyway.
 553
 554         pathDesired.Start.North = positionState.North;
 555         pathDesired.Start.East  = positionState.East;
 556         pathDesired.Start.Down  = positionState.Down;
 557         pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_NORTH] = velocity_north;
 558         pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_EAST]  = velocity_east;
 559         pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_DOWN]  = velocity_down;
 560
 561         pathDesired.End.North = positionState.North;
 562         pathDesired.End.East  = positionState.East;
 563         pathDesired.End.Down  = positionState.Down;
 564
 565         pathDesired.StartingVelocity = velocity;
 566         pathDesired.EndingVelocity   = velocity;
 567         pathDesired.Mode = PATHDESIRED_MODE_VELOCITY;
 568         if (flightMode == FLIGHTSTATUS_FLIGHTMODE_LAND) {
 569             pathDesired.Mode = PATHDESIRED_MODE_LAND;
 570             pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_OPTIONS] = (float)PATHDESIRED_MODEPARAMETER_LAND_OPTION_NONE;
 571         } else {
 572             pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_UNUSED] = 0.0f;
 573         }
 574         PathDesiredSet(&pathDesired);
 575         FlightStatusAssistedControlStateSet(&assistedControlFlightMode);
 576     }
 577 }
 578
 579 void plan_run_CourseLock()
 580 {
 581     plan_run_PositionVario(COURSE);
 582 }
 583
 584 void plan_run_PositionRoam()
 585 {
 586     plan_run_PositionVario(FPV);
 587 }
 588
 589 void plan_run_HomeLeash()
 590 {
 591     plan_run_PositionVario(LOS);
 592 }
 593
 594 void plan_run_AbsolutePosition()
 595 {
 596     plan_run_PositionVario(NSEW);
 597 }
 598
 599
 600 /**
 601  * @brief setup pathplanner/pathfollower for AutoCruise
 602  */
 603 static PiOSDeltatimeConfig actimeval;
 604 void plan_setup_AutoCruise()
 605 {
 606     PositionStateData positionState;
 607
 608     PositionStateGet(&positionState);
 609
 610     PathDesiredData pathDesired;
 611     // setup needs to reinitialise the pathdesired object
 612     memset(&pathDesired, 0, sizeof(PathDesiredData));
 613
 614     FlightModeSettingsPositionHoldOffsetData offset;
 615     FlightModeSettingsPositionHoldOffsetGet(&offset);
 616
 617     // initialization is flight in direction of the nose.
 618     // the velocity is not relevant, as it will be reset by the run function even during first call
 619     float angle;
 620     AttitudeStateYawGet(&angle);
 621     float vector[2] = {
 622         cos_lookup_deg(angle),
 623         sin_lookup_deg(angle)
 624     };
 625     hold_position[0]             = positionState.North;
 626     hold_position[1]             = positionState.East;
 627     hold_position[2]             = positionState.Down;
 628     pathDesired.End.North        = hold_position[0] + vector[0];
 629     pathDesired.End.East         = hold_position[1] + vector[1];
 630     pathDesired.End.Down         = hold_position[2];
 631     // start position has the same offset as in position hold
 632     pathDesired.Start.North      = pathDesired.End.North + offset.Horizontal; // in FlyEndPoint the direction of this vector does not matter
 633     pathDesired.Start.East       = pathDesired.End.East;
 634     pathDesired.Start.Down       = pathDesired.End.Down;
 635     pathDesired.StartingVelocity = 0.0f;
 636     pathDesired.EndingVelocity   = 0.0f;
 637     pathDesired.Mode             = PATHDESIRED_MODE_GOTOENDPOINT;
 638
 639     PathDesiredSet(&pathDesired);
 640
 641     // re-iniztializing deltatime is valid and also good practice here since
 642     // getAverageSeconds() has not been called/updated in a long time if we were in a different flightmode.
 643     PIOS_DELTATIME_Init(&actimeval, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA);
 644 }
 645
 646 /**
 647  * @brief execute autocruise
 648  */
 649 void plan_run_AutoCruise()
 650 {
 651     PositionStateData positionState;
 652
 653     PositionStateGet(&positionState);
 654     PathDesiredData pathDesired;
 655     // re-use pathdesired that was setup correctly in setup stage.
 656     PathDesiredGet(&pathDesired);
 657
 658     FlightModeSettingsPositionHoldOffsetData offset;
 659     FlightModeSettingsPositionHoldOffsetGet(&offset);
 660
 661     float controlVector[4];
 662     ManualControlCommandRollGet(&controlVector[0]);
 663     ManualControlCommandPitchGet(&controlVector[1]);
 664     ManualControlCommandYawGet(&controlVector[2]);
 665     controlVector[3] = 0.5f; // dummy, thrust is normalized separately
 666     normalizeDeadband(controlVector); // return value ignored
 667     ManualControlCommandThrustGet(&controlVector[3]); // no deadband as we are using thrust for velocity
 668     controlVector[3] = boundf(controlVector[3], 1e-6f, 1.0f); // bound to above zero, to prevent loss of vector direction
 669
 670     // normalize old desired movement vector
 671     float vector[3] = { pathDesired.End.North - hold_position[0],
 672                         pathDesired.End.East - hold_position[1],
 673                         pathDesired.End.Down - hold_position[2] };
 674     float length    = sqrtf(vector[0] * vector[0] + vector[1] * vector[1] + vector[2] * vector[2]);
 675     if (length < 1e-9f) {
 676         length = 1.0f; // should not happen since initialized properly in setup()
 677     }
 678     vector[0] /= length;
 679     vector[1] /= length;
 680     vector[2] /= length;
 681
 682     // start position is advanced according to actual movement - in the direction of desired vector only
 683     // projection using scalar product
 684     float kp = (positionState.North - hold_position[0]) * vector[0]
 685                + (positionState.East - hold_position[1]) * vector[1]
 686                + (positionState.Down - hold_position[2]) * vector[2];
 687     if (kp > 0.0f) {
 688         hold_position[0] += kp * vector[0];
 689         hold_position[1] += kp * vector[1];
 690         hold_position[2] += kp * vector[2];
 691     }
 692
 693     // new angle is equal to old angle plus offset depending on yaw input and time
 694     // (controlVector is normalized with a deadband, change is zero within deadband)
 695     float angle = RAD2DEG(atan2f(vector[1], vector[0]));
 696     float dT    = PIOS_DELTATIME_GetAverageSeconds(&actimeval);
 697     angle    += 10.0f * controlVector[2] * dT; // TODO magic value could eventually end up in a to be created settings
 698
 699     // resulting movement vector is scaled by velocity demand in controlvector[3] [0.0-1.0]
 700     vector[0] = cosf(DEG2RAD(angle)) * offset.Horizontal * controlVector[3];
 701     vector[1] = sinf(DEG2RAD(angle)) * offset.Horizontal * controlVector[3];
 702     vector[2] = -controlVector[1] * offset.Vertical * controlVector[3];
 703
 704     pathDesired.End.North   = hold_position[0] + vector[0];
 705     pathDesired.End.East    = hold_position[1] + vector[1];
 706     pathDesired.End.Down    = hold_position[2] + vector[2];
 707     // start position has the same offset as in position hold
 708     pathDesired.Start.North = pathDesired.End.North + offset.Horizontal; // in FlyEndPoint the direction of this vector does not matter
 709     pathDesired.Start.East  = pathDesired.End.East;
 710     pathDesired.Start.Down  = pathDesired.End.Down;
 711     PathDesiredSet(&pathDesired);
 712 }
 713
 714 /**
 715  * @brief setup pathplanner/pathfollower for braking in positionroam
 716  *        timeout_occurred = false: Attempt to enter flyvector for braking
 717  *        timeout_occurred = true: Revert to position hold
 718  */
 719 #define ASSISTEDCONTROL_BRAKERATE_MINIMUM  0.2f   // m/s2
 720 #define ASSISTEDCONTROL_TIMETOSTOP_MINIMUM 0.2f // seconds
 721 #define ASSISTEDCONTROL_TIMETOSTOP_MAXIMUM 9.0f // seconds
 722 #define ASSISTEDCONTROL_DELAY_TO_BRAKE     1.0f      // seconds
 723 #define ASSISTEDCONTROL_TIMEOUT_MULTIPLIER 4.0f // actual deceleration rate can be 50% of desired...timeouts need to cater for this
 724 void plan_setup_assistedcontrol()
 725 {
 726     PositionStateData positionState;
 727
 728     PositionStateGet(&positionState);
 729     PathDesiredData pathDesired;
 730     // setup function, avoid carry over from previous mode
 731     memset(&pathDesired, 0, sizeof(PathDesiredData));
 732
 733     FlightStatusAssistedControlStateOptions assistedControlFlightMode;
 734
 735     VelocityStateData velocityState;
 736     VelocityStateGet(&velocityState);
 737     float brakeRate;
 738     VtolPathFollowerSettingsBrakeRateGet(&brakeRate);
 739     if (brakeRate < ASSISTEDCONTROL_BRAKERATE_MINIMUM) {
 740         brakeRate = ASSISTEDCONTROL_BRAKERATE_MINIMUM; // set a minimum to avoid a divide by zero potential below
 741     }
 742     // Calculate the velocity
 743     float velocity = velocityState.North * velocityState.North + velocityState.East * velocityState.East + velocityState.Down * velocityState.Down;
 744     velocity = sqrtf(velocity);
 745
 746     // Calculate the desired time to zero velocity.
 747     float time_to_stopped = ASSISTEDCONTROL_DELAY_TO_BRAKE; // we allow at least 0.5 seconds to rotate to a brake angle.
 748     time_to_stopped += velocity / brakeRate;
 749
 750     // Sanity check the brake rate by ensuring that the time to stop is within a range.
 751     if (time_to_stopped < ASSISTEDCONTROL_TIMETOSTOP_MINIMUM) {
 752         time_to_stopped = ASSISTEDCONTROL_TIMETOSTOP_MINIMUM;
 753     } else if (time_to_stopped > ASSISTEDCONTROL_TIMETOSTOP_MAXIMUM) {
 754         time_to_stopped = ASSISTEDCONTROL_TIMETOSTOP_MAXIMUM;
 755     }
 756
 757     // calculate the distance we will travel
 758     float north_delta = velocityState.North * ASSISTEDCONTROL_DELAY_TO_BRAKE; // we allow at least 0.5s to rotate to brake angle
 759     north_delta += (time_to_stopped - ASSISTEDCONTROL_DELAY_TO_BRAKE) * 0.5f * velocityState.North; // area under the linear deceleration plot
 760     float east_delta  = velocityState.East * ASSISTEDCONTROL_DELAY_TO_BRAKE; // we allow at least 0.5s to rotate to brake angle
 761     east_delta  += (time_to_stopped - ASSISTEDCONTROL_DELAY_TO_BRAKE) * 0.5f * velocityState.East; // area under the linear deceleration plot
 762     float down_delta  = velocityState.Down * ASSISTEDCONTROL_DELAY_TO_BRAKE;
 763     down_delta  += (time_to_stopped - ASSISTEDCONTROL_DELAY_TO_BRAKE) * 0.5f * velocityState.Down; // area under the linear deceleration plot
 764     float net_delta   = east_delta * east_delta + north_delta * north_delta + down_delta * down_delta;
 765     net_delta    = sqrtf(net_delta);
 766
 767     pathDesired.Start.North = positionState.North;
 768     pathDesired.Start.East  = positionState.East;
 769     pathDesired.Start.Down  = positionState.Down;
 770     pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_NORTH] = velocityState.North;
 771     pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_EAST]  = velocityState.East;
 772     pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_DOWN]  = velocityState.Down;
 773
 774     pathDesired.End.North = positionState.North + north_delta;
 775     pathDesired.End.East  = positionState.East + east_delta;
 776     pathDesired.End.Down  = positionState.Down + down_delta;
 777
 778     pathDesired.StartingVelocity = velocity;
 779     pathDesired.EndingVelocity   = 0.0f;
 780     pathDesired.Mode = PATHDESIRED_MODE_BRAKE;
 781     pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_TIMEOUT] = time_to_stopped * ASSISTEDCONTROL_TIMEOUT_MULTIPLIER;
 782     assistedControlFlightMode    = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_BRAKE;
 783     FlightStatusAssistedControlStateSet(&assistedControlFlightMode);
 784     PathDesiredSet(&pathDesired);
 785 }