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