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