flight/modules/PathFollower/pathfollower.cpp

   1 /**
   2  ******************************************************************************
   3  *
   4  * @file       pathfollower.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  * @brief      This module compared @ref PositionActuatl to @ref ActiveWaypoint
   8  * and sets @ref AttitudeDesired.  It only does this when the FlightMode field
   9  * of @ref ManualControlCommand is Auto.
  10  *
  11  * @see        The GNU Public License (GPL) Version 3
  12  * @addtogroup LibrePilot LibrePilotModules Modules PathFollower Navigation
  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 /**
  32  * Input object: PathDesired
  33  * Input object: PositionState
  34  * Input object: ManualControlCommand
  35  * Output object: StabilizationDesired
  36  *
  37  * This module acts as "autopilot" - it controls the setpoints of stabilization
  38  * based on current flight situation and desired flight path (PathDesired) as
  39  * directed by flightmode selection or pathplanner
  40  * This is a periodic delayed callback module
  41  *
  42  * Modules have no API, all communication to other modules is done through UAVObjects.
  43  * However modules may use the API exposed by shared libraries.
  44  * See the OpenPilot wiki for more details.
  45  * http://www.openpilot.org/OpenPilot_Application_Architecture
  46  *
  47  */
  48
  49 extern "C" {
  50 #include <openpilot.h>
  51
  52 #include <callbackinfo.h>
  53
  54 #include <math.h>
  55 #include <pid.h>
  56 #include <CoordinateConversions.h>
  57 #include <sin_lookup.h>
  58 #include <pathdesired.h>
  59 #include <paths.h>
  60 #include "plans.h"
  61 #include <sanitycheck.h>
  62
  63 #include <groundpathfollowersettings.h>
  64 #include <fixedwingpathfollowersettings.h>
  65 #include <fixedwingpathfollowerstatus.h>
  66 #include <vtolpathfollowersettings.h>
  67 #include <flightstatus.h>
  68 #include <flightmodesettings.h>
  69 #include <pathstatus.h>
  70 #include <positionstate.h>
  71 #include <velocitystate.h>
  72 #include <velocitydesired.h>
  73 #include <stabilizationdesired.h>
  74 #include <airspeedstate.h>
  75 #include <attitudestate.h>
  76 #include <takeofflocation.h>
  77 #include <poilocation.h>
  78 #include <manualcontrolcommand.h>
  79 #include <systemsettings.h>
  80 #include <stabilizationbank.h>
  81 #include <vtolselftuningstats.h>
  82 #include <pathsummary.h>
  83 #include <pidstatus.h>
  84 #include <homelocation.h>
  85 #include <accelstate.h>
  86 #include <statusvtolautotakeoff.h>
  87 #include <statusvtolland.h>
  88 #include <statusgrounddrive.h>
  89 }
  90
  91 #include "pathfollowercontrol.h"
  92 #include "vtollandcontroller.h"
  93 #include "vtolautotakeoffcontroller.h"
  94 #include "vtolvelocitycontroller.h"
  95 #include "vtolbrakecontroller.h"
  96 #include "vtolflycontroller.h"
  97 #include "fixedwingflycontroller.h"
  98 #include "fixedwingautotakeoffcontroller.h"
  99 #include "fixedwinglandcontroller.h"
 100 #include "grounddrivecontroller.h"
 101
 102 // Private constants
 103
 104 #define CALLBACK_PRIORITY      CALLBACK_PRIORITY_LOW
 105 #define CBTASK_PRIORITY        CALLBACK_TASK_FLIGHTCONTROL
 106
 107 #define PF_IDLE_UPDATE_RATE_MS 100
 108
 109 #define STACK_SIZE_BYTES       2048
 110
 111
 112 // Private types
 113
 114 // Private variables
 115 static DelayedCallbackInfo *pathFollowerCBInfo;
 116 static uint32_t updatePeriod = PF_IDLE_UPDATE_RATE_MS;
 117 static FrameType_t frameType = FRAME_TYPE_MULTIROTOR;
 118 static PathStatusData pathStatus;
 119 static PathDesiredData pathDesired;
 120 static FixedWingPathFollowerSettingsData fixedWingPathFollowerSettings;
 121 static GroundPathFollowerSettingsData groundPathFollowerSettings;
 122 static VtolPathFollowerSettingsData vtolPathFollowerSettings;
 123 static FlightStatusData flightStatus;
 124 static PathFollowerControl *activeController = 0;
 125
 126 // Private functions
 127 static void pathFollowerTask(void);
 128 static void SettingsUpdatedCb(UAVObjEvent *ev);
 129 static void airspeedStateUpdatedCb(UAVObjEvent *ev);
 130 static void pathFollowerObjectiveUpdatedCb(UAVObjEvent *ev);
 131 static void flightStatusUpdatedCb(UAVObjEvent *ev);
 132 static void updateFixedAttitude(float *attitude);
 133 static void pathFollowerInitializeControllersForFrameType();
 134
 135 /**
 136  * Initialise the module, called on startup
 137  * \returns 0 on success or -1 if initialisation failed
 138  */
 139 extern "C" int32_t PathFollowerStart()
 140 {
 141     // Start main task
 142     PathStatusGet(&pathStatus);
 143     SettingsUpdatedCb(NULL);
 144     PIOS_CALLBACKSCHEDULER_Dispatch(pathFollowerCBInfo);
 145
 146     return 0;
 147 }
 148
 149
 150 /**
 151  * Initialise the module, called on startup
 152  * \returns 0 on success or -1 if initialisation failed
 153  */
 154 extern "C" int32_t PathFollowerInitialize()
 155 {
 156     // initialize objects
 157     FixedWingPathFollowerStatusInitialize();
 158     FlightStatusInitialize();
 159     PathStatusInitialize();
 160     PathSummaryInitialize();
 161     PathDesiredInitialize();
 162     PositionStateInitialize();
 163     VelocityStateInitialize();
 164     VelocityDesiredInitialize();
 165     StabilizationDesiredInitialize();
 166     AirspeedStateInitialize();
 167     AttitudeStateInitialize();
 168     PoiLocationInitialize();
 169     ManualControlCommandInitialize();
 170     StabilizationBankInitialize();
 171     VtolSelfTuningStatsInitialize();
 172     PIDStatusInitialize();
 173     StatusVtolLandInitialize();
 174     StatusGroundDriveInitialize();
 175     StatusVtolAutoTakeoffInitialize();
 176
 177     // VtolLandFSM additional objects
 178     AccelStateInitialize();
 179
 180     // Init references to controllers
 181     PathFollowerControl::Initialize(&pathDesired, &flightStatus, &pathStatus);
 182
 183     // Create object queue
 184     pathFollowerCBInfo = PIOS_CALLBACKSCHEDULER_Create(&pathFollowerTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_PATHFOLLOWER, STACK_SIZE_BYTES);
 185     FixedWingPathFollowerSettingsConnectCallback(&SettingsUpdatedCb);
 186     GroundPathFollowerSettingsConnectCallback(&SettingsUpdatedCb);
 187     VtolPathFollowerSettingsConnectCallback(&SettingsUpdatedCb);
 188     PathDesiredConnectCallback(&pathFollowerObjectiveUpdatedCb);
 189     FlightStatusConnectCallback(&flightStatusUpdatedCb);
 190     SystemSettingsConnectCallback(&SettingsUpdatedCb);
 191     AirspeedStateConnectCallback(&airspeedStateUpdatedCb);
 192     VtolSelfTuningStatsConnectCallback(&SettingsUpdatedCb);
 193
 194     return 0;
 195 }
 196 MODULE_INITCALL(PathFollowerInitialize, PathFollowerStart);
 197
 198 void pathFollowerInitializeControllersForFrameType()
 199 {
 200     static uint8_t multirotor_initialised = 0;
 201     static uint8_t fixedwing_initialised  = 0;
 202     static uint8_t ground_initialised     = 0;
 203
 204     switch (frameType) {
 205     case FRAME_TYPE_MULTIROTOR:
 206     case FRAME_TYPE_HELI:
 207         if (!multirotor_initialised) {
 208             VtolLandController::instance()->Initialize(&vtolPathFollowerSettings);
 209             VtolAutoTakeoffController::instance()->Initialize(&vtolPathFollowerSettings);
 210             VtolVelocityController::instance()->Initialize(&vtolPathFollowerSettings);
 211             VtolFlyController::instance()->Initialize(&vtolPathFollowerSettings);
 212             VtolBrakeController::instance()->Initialize(&vtolPathFollowerSettings);
 213             multirotor_initialised = 1;
 214         }
 215         break;
 216
 217     case FRAME_TYPE_FIXED_WING:
 218         if (!fixedwing_initialised) {
 219             FixedWingFlyController::instance()->Initialize(&fixedWingPathFollowerSettings);
 220             FixedWingAutoTakeoffController::instance()->Initialize(&fixedWingPathFollowerSettings);
 221             FixedWingLandController::instance()->Initialize(&fixedWingPathFollowerSettings);
 222             fixedwing_initialised = 1;
 223         }
 224         break;
 225
 226     case FRAME_TYPE_GROUND:
 227         if (!ground_initialised) {
 228             GroundDriveController::instance()->Initialize(&groundPathFollowerSettings);
 229             ground_initialised = 1;
 230         }
 231         break;
 232
 233     default:
 234         break;
 235     }
 236 }
 237
 238 static void pathFollowerSetActiveController(void)
 239 {
 240     // Init controllers for the frame type
 241     if (activeController == 0) {
 242         // Initialise
 243         pathFollowerInitializeControllersForFrameType();
 244         AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
 245
 246         switch (frameType) {
 247         case FRAME_TYPE_MULTIROTOR:
 248         case FRAME_TYPE_HELI:
 249
 250             switch (pathDesired.Mode) {
 251             case PATHDESIRED_MODE_BRAKE: // brake then hold sequence controller
 252                 activeController = VtolBrakeController::instance();
 253                 activeController->Activate();
 254                 break;
 255             case PATHDESIRED_MODE_VELOCITY: // velocity roam controller
 256                 activeController = VtolVelocityController::instance();
 257                 activeController->Activate();
 258                 break;
 259             case PATHDESIRED_MODE_GOTOENDPOINT:
 260             case PATHDESIRED_MODE_FOLLOWVECTOR:
 261             case PATHDESIRED_MODE_CIRCLERIGHT:
 262             case PATHDESIRED_MODE_CIRCLELEFT:
 263                 activeController = VtolFlyController::instance();
 264                 activeController->Activate();
 265                 break;
 266             case PATHDESIRED_MODE_LAND: // land with optional velocity roam option
 267                 activeController = VtolLandController::instance();
 268                 activeController->Activate();
 269                 break;
 270             case PATHDESIRED_MODE_AUTOTAKEOFF:
 271                 activeController = VtolAutoTakeoffController::instance();
 272                 activeController->Activate();
 273                 break;
 274             default:
 275                 activeController = 0;
 276                 AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_UNINITIALISED);
 277                 break;
 278             }
 279             break;
 280
 281         case FRAME_TYPE_FIXED_WING:
 282
 283             switch (pathDesired.Mode) {
 284             case PATHDESIRED_MODE_GOTOENDPOINT:
 285             case PATHDESIRED_MODE_FOLLOWVECTOR:
 286             case PATHDESIRED_MODE_CIRCLERIGHT:
 287             case PATHDESIRED_MODE_CIRCLELEFT:
 288                 activeController = FixedWingFlyController::instance();
 289                 activeController->Activate();
 290                 break;
 291             case PATHDESIRED_MODE_LAND: // land with optional velocity roam option
 292                 activeController = FixedWingLandController::instance();
 293                 activeController->Activate();
 294                 break;
 295             case PATHDESIRED_MODE_AUTOTAKEOFF:
 296                 activeController = FixedWingAutoTakeoffController::instance();
 297                 activeController->Activate();
 298                 break;
 299             default:
 300                 activeController = 0;
 301                 AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_UNINITIALISED);
 302                 break;
 303             }
 304             break;
 305
 306         case FRAME_TYPE_GROUND:
 307
 308             switch (pathDesired.Mode) {
 309             case PATHDESIRED_MODE_GOTOENDPOINT:
 310             case PATHDESIRED_MODE_FOLLOWVECTOR:
 311             case PATHDESIRED_MODE_CIRCLERIGHT:
 312             case PATHDESIRED_MODE_CIRCLELEFT:
 313                 activeController = GroundDriveController::instance();
 314                 activeController->Activate();
 315                 break;
 316             default:
 317                 activeController = 0;
 318                 AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_UNINITIALISED);
 319                 break;
 320             }
 321             break;
 322
 323         default:
 324             activeController = 0;
 325             AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_UNINITIALISED);
 326             break;
 327         }
 328     }
 329 }
 330
 331 /**
 332  * Module thread, should not return.
 333  */
 334 static void pathFollowerTask(void)
 335 {
 336     FlightStatusGet(&flightStatus);
 337
 338     if (flightStatus.ControlChain.PathFollower != FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
 339         AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_UNINITIALISED);
 340         PIOS_CALLBACKSCHEDULER_Schedule(pathFollowerCBInfo, PF_IDLE_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER);
 341         return;
 342     }
 343
 344     pathStatus.UID    = pathDesired.UID;
 345     pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS;
 346
 347     pathFollowerSetActiveController();
 348
 349     if (activeController) {
 350         activeController->UpdateAutoPilot();
 351         PIOS_CALLBACKSCHEDULER_Schedule(pathFollowerCBInfo, updatePeriod, CALLBACK_UPDATEMODE_SOONER);
 352         return;
 353     }
 354
 355     switch (pathDesired.Mode) {
 356     case PATHDESIRED_MODE_FIXEDATTITUDE:
 357         updateFixedAttitude(pathDesired.ModeParameters);
 358         AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
 359         break;
 360     case PATHDESIRED_MODE_DISARMALARM:
 361         AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL);
 362         break;
 363     default:
 364         pathStatus.Status = PATHSTATUS_STATUS_CRITICAL;
 365         AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
 366         break;
 367     }
 368     PathStatusSet(&pathStatus);
 369
 370     PIOS_CALLBACKSCHEDULER_Schedule(pathFollowerCBInfo, updatePeriod, CALLBACK_UPDATEMODE_SOONER);
 371 }
 372
 373
 374 static void pathFollowerObjectiveUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 375 {
 376     PathDesiredGet(&pathDesired);
 377
 378     if (activeController && pathDesired.Mode != activeController->Mode()) {
 379         activeController->Deactivate();
 380         activeController = 0;
 381     }
 382
 383     pathFollowerSetActiveController();
 384
 385     if (activeController) {
 386         activeController->ObjectiveUpdated();
 387     }
 388 }
 389
 390 // we use this to deactivate active controllers as the pathdesired
 391 // objective never gets updated with switching to a non-pathfollower flight mode
 392 static void flightStatusUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 393 {
 394     if (!activeController) {
 395         return;
 396     }
 397
 398     FlightStatusControlChainData controlChain;
 399     FlightStatusControlChainGet(&controlChain);
 400
 401     if (controlChain.PathFollower != FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
 402         activeController->Deactivate();
 403         activeController = 0;
 404     }
 405 }
 406
 407
 408 static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 409 {
 410     VtolPathFollowerSettingsGet(&vtolPathFollowerSettings);
 411
 412     FrameType_t previous_frameType = frameType;
 413     frameType = GetCurrentFrameType();
 414
 415     if (frameType == FRAME_TYPE_CUSTOM) {
 416         switch (vtolPathFollowerSettings.TreatCustomCraftAs) {
 417         case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_FIXEDWING:
 418             frameType = FRAME_TYPE_FIXED_WING;
 419             break;
 420         case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_VTOL:
 421             frameType = FRAME_TYPE_MULTIROTOR;
 422             break;
 423         case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_GROUND:
 424             frameType = FRAME_TYPE_GROUND;
 425             break;
 426         }
 427     }
 428
 429     // If frame type changes, initialise the frame type controllers
 430     if (frameType != previous_frameType) {
 431         pathFollowerInitializeControllersForFrameType();
 432     }
 433
 434     switch (frameType) {
 435     case FRAME_TYPE_MULTIROTOR:
 436     case FRAME_TYPE_HELI:
 437         updatePeriod = vtolPathFollowerSettings.UpdatePeriod;
 438         break;
 439     case FRAME_TYPE_FIXED_WING:
 440         FixedWingPathFollowerSettingsGet(&fixedWingPathFollowerSettings);
 441         updatePeriod = fixedWingPathFollowerSettings.UpdatePeriod;
 442         break;
 443     case FRAME_TYPE_GROUND:
 444         GroundPathFollowerSettingsGet(&groundPathFollowerSettings);
 445         updatePeriod = groundPathFollowerSettings.UpdatePeriod;
 446         break;
 447     default:
 448         updatePeriod = vtolPathFollowerSettings.UpdatePeriod;
 449         break;
 450     }
 451
 452     if (activeController) {
 453         activeController->SettingsUpdated();
 454     }
 455 }
 456
 457
 458 static void airspeedStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 459 {
 460     FixedWingFlyController::instance()->AirspeedStateUpdatedCb(ev);
 461     FixedWingAutoTakeoffController::instance()->AirspeedStateUpdatedCb(ev);
 462 }
 463
 464
 465 /**
 466  * Compute desired attitude from a fixed preset
 467  *
 468  */
 469 static void updateFixedAttitude(float *attitude)
 470 {
 471     StabilizationDesiredData stabDesired;
 472
 473     StabilizationDesiredGet(&stabDesired);
 474     stabDesired.Roll   = attitude[0];
 475     stabDesired.Pitch  = attitude[1];
 476     stabDesired.Yaw    = attitude[2];
 477     stabDesired.Thrust = attitude[3];
 478     stabDesired.StabilizationMode.Roll   = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
 479     stabDesired.StabilizationMode.Pitch  = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
 480     stabDesired.StabilizationMode.Yaw    = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
 481     stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
 482     StabilizationDesiredSet(&stabDesired);
 483 }