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     GroundPathFollowerSettingsInitialize();
 158     FixedWingPathFollowerSettingsInitialize();
 159     FixedWingPathFollowerStatusInitialize();
 160     VtolPathFollowerSettingsInitialize();
 161     FlightStatusInitialize();
 162     FlightModeSettingsInitialize();
 163     PathStatusInitialize();
 164     PathSummaryInitialize();
 165     PathDesiredInitialize();
 166     PositionStateInitialize();
 167     VelocityStateInitialize();
 168     VelocityDesiredInitialize();
 169     StabilizationDesiredInitialize();
 170     AirspeedStateInitialize();
 171     AttitudeStateInitialize();
 172     TakeOffLocationInitialize();
 173     PoiLocationInitialize();
 174     ManualControlCommandInitialize();
 175     SystemSettingsInitialize();
 176     StabilizationBankInitialize();
 177     VtolSelfTuningStatsInitialize();
 178     PIDStatusInitialize();
 179     StatusVtolLandInitialize();
 180     StatusGroundDriveInitialize();
 181     StatusVtolAutoTakeoffInitialize();
 182
 183     // VtolLandFSM additional objects
 184     HomeLocationInitialize();
 185     AccelStateInitialize();
 186
 187     // Init references to controllers
 188     PathFollowerControl::Initialize(&pathDesired, &flightStatus, &pathStatus);
 189
 190     // Create object queue
 191     pathFollowerCBInfo = PIOS_CALLBACKSCHEDULER_Create(&pathFollowerTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_PATHFOLLOWER, STACK_SIZE_BYTES);
 192     FixedWingPathFollowerSettingsConnectCallback(&SettingsUpdatedCb);
 193     GroundPathFollowerSettingsConnectCallback(&SettingsUpdatedCb);
 194     VtolPathFollowerSettingsConnectCallback(&SettingsUpdatedCb);
 195     PathDesiredConnectCallback(&pathFollowerObjectiveUpdatedCb);
 196     FlightStatusConnectCallback(&flightStatusUpdatedCb);
 197     SystemSettingsConnectCallback(&SettingsUpdatedCb);
 198     AirspeedStateConnectCallback(&airspeedStateUpdatedCb);
 199     VtolSelfTuningStatsConnectCallback(&SettingsUpdatedCb);
 200
 201     return 0;
 202 }
 203 MODULE_INITCALL(PathFollowerInitialize, PathFollowerStart);
 204
 205 void pathFollowerInitializeControllersForFrameType()
 206 {
 207     static uint8_t multirotor_initialised = 0;
 208     static uint8_t fixedwing_initialised  = 0;
 209     static uint8_t ground_initialised     = 0;
 210
 211     switch (frameType) {
 212     case FRAME_TYPE_MULTIROTOR:
 213     case FRAME_TYPE_HELI:
 214         if (!multirotor_initialised) {
 215             VtolLandController::instance()->Initialize(&vtolPathFollowerSettings);
 216             VtolAutoTakeoffController::instance()->Initialize(&vtolPathFollowerSettings);
 217             VtolVelocityController::instance()->Initialize(&vtolPathFollowerSettings);
 218             VtolFlyController::instance()->Initialize(&vtolPathFollowerSettings);
 219             VtolBrakeController::instance()->Initialize(&vtolPathFollowerSettings);
 220             multirotor_initialised = 1;
 221         }
 222         break;
 223
 224     case FRAME_TYPE_FIXED_WING:
 225         if (!fixedwing_initialised) {
 226             FixedWingFlyController::instance()->Initialize(&fixedWingPathFollowerSettings);
 227             FixedWingAutoTakeoffController::instance()->Initialize(&fixedWingPathFollowerSettings);
 228             FixedWingLandController::instance()->Initialize(&fixedWingPathFollowerSettings);
 229             fixedwing_initialised = 1;
 230         }
 231         break;
 232
 233     case FRAME_TYPE_GROUND:
 234         if (!ground_initialised) {
 235             GroundDriveController::instance()->Initialize(&groundPathFollowerSettings);
 236             ground_initialised = 1;
 237         }
 238         break;
 239
 240     default:
 241         break;
 242     }
 243 }
 244
 245 static void pathFollowerSetActiveController(void)
 246 {
 247     // Init controllers for the frame type
 248     if (activeController == 0) {
 249         // Initialise
 250         pathFollowerInitializeControllersForFrameType();
 251         AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
 252
 253         switch (frameType) {
 254         case FRAME_TYPE_MULTIROTOR:
 255         case FRAME_TYPE_HELI:
 256
 257             switch (pathDesired.Mode) {
 258             case PATHDESIRED_MODE_BRAKE: // brake then hold sequence controller
 259                 activeController = VtolBrakeController::instance();
 260                 activeController->Activate();
 261                 break;
 262             case PATHDESIRED_MODE_VELOCITY: // velocity roam controller
 263                 activeController = VtolVelocityController::instance();
 264                 activeController->Activate();
 265                 break;
 266             case PATHDESIRED_MODE_GOTOENDPOINT:
 267             case PATHDESIRED_MODE_FOLLOWVECTOR:
 268             case PATHDESIRED_MODE_CIRCLERIGHT:
 269             case PATHDESIRED_MODE_CIRCLELEFT:
 270                 activeController = VtolFlyController::instance();
 271                 activeController->Activate();
 272                 break;
 273             case PATHDESIRED_MODE_LAND: // land with optional velocity roam option
 274                 activeController = VtolLandController::instance();
 275                 activeController->Activate();
 276                 break;
 277             case PATHDESIRED_MODE_AUTOTAKEOFF:
 278                 activeController = VtolAutoTakeoffController::instance();
 279                 activeController->Activate();
 280                 break;
 281             default:
 282                 activeController = 0;
 283                 AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_UNINITIALISED);
 284                 break;
 285             }
 286             break;
 287
 288         case FRAME_TYPE_FIXED_WING:
 289
 290             switch (pathDesired.Mode) {
 291             case PATHDESIRED_MODE_GOTOENDPOINT:
 292             case PATHDESIRED_MODE_FOLLOWVECTOR:
 293             case PATHDESIRED_MODE_CIRCLERIGHT:
 294             case PATHDESIRED_MODE_CIRCLELEFT:
 295                 activeController = FixedWingFlyController::instance();
 296                 activeController->Activate();
 297                 break;
 298             case PATHDESIRED_MODE_LAND: // land with optional velocity roam option
 299                 activeController = FixedWingLandController::instance();
 300                 activeController->Activate();
 301                 break;
 302             case PATHDESIRED_MODE_AUTOTAKEOFF:
 303                 activeController = FixedWingAutoTakeoffController::instance();
 304                 activeController->Activate();
 305                 break;
 306             default:
 307                 activeController = 0;
 308                 AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_UNINITIALISED);
 309                 break;
 310             }
 311             break;
 312
 313         case FRAME_TYPE_GROUND:
 314
 315             switch (pathDesired.Mode) {
 316             case PATHDESIRED_MODE_GOTOENDPOINT:
 317             case PATHDESIRED_MODE_FOLLOWVECTOR:
 318             case PATHDESIRED_MODE_CIRCLERIGHT:
 319             case PATHDESIRED_MODE_CIRCLELEFT:
 320                 activeController = GroundDriveController::instance();
 321                 activeController->Activate();
 322                 break;
 323             default:
 324                 activeController = 0;
 325                 AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_UNINITIALISED);
 326                 break;
 327             }
 328             break;
 329
 330         default:
 331             activeController = 0;
 332             AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_UNINITIALISED);
 333             break;
 334         }
 335     }
 336 }
 337
 338 /**
 339  * Module thread, should not return.
 340  */
 341 static void pathFollowerTask(void)
 342 {
 343     FlightStatusGet(&flightStatus);
 344
 345     if (flightStatus.ControlChain.PathFollower != FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
 346         AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_UNINITIALISED);
 347         PIOS_CALLBACKSCHEDULER_Schedule(pathFollowerCBInfo, PF_IDLE_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER);
 348         return;
 349     }
 350
 351     pathStatus.UID    = pathDesired.UID;
 352     pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS;
 353
 354     pathFollowerSetActiveController();
 355
 356     if (activeController) {
 357         activeController->UpdateAutoPilot();
 358         PIOS_CALLBACKSCHEDULER_Schedule(pathFollowerCBInfo, updatePeriod, CALLBACK_UPDATEMODE_SOONER);
 359         return;
 360     }
 361
 362     switch (pathDesired.Mode) {
 363     case PATHDESIRED_MODE_FIXEDATTITUDE:
 364         updateFixedAttitude(pathDesired.ModeParameters);
 365         AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
 366         break;
 367     case PATHDESIRED_MODE_DISARMALARM:
 368         AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL);
 369         break;
 370     default:
 371         pathStatus.Status = PATHSTATUS_STATUS_CRITICAL;
 372         AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
 373         break;
 374     }
 375     PathStatusSet(&pathStatus);
 376
 377     PIOS_CALLBACKSCHEDULER_Schedule(pathFollowerCBInfo, updatePeriod, CALLBACK_UPDATEMODE_SOONER);
 378 }
 379
 380
 381 static void pathFollowerObjectiveUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 382 {
 383     PathDesiredGet(&pathDesired);
 384
 385     if (activeController && pathDesired.Mode != activeController->Mode()) {
 386         activeController->Deactivate();
 387         activeController = 0;
 388     }
 389
 390     pathFollowerSetActiveController();
 391
 392     if (activeController) {
 393         activeController->ObjectiveUpdated();
 394     }
 395 }
 396
 397 // we use this to deactivate active controllers as the pathdesired
 398 // objective never gets updated with switching to a non-pathfollower flight mode
 399 static void flightStatusUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 400 {
 401     if (!activeController) {
 402         return;
 403     }
 404
 405     FlightStatusControlChainData controlChain;
 406     FlightStatusControlChainGet(&controlChain);
 407
 408     if (controlChain.PathFollower != FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
 409         activeController->Deactivate();
 410         activeController = 0;
 411     }
 412 }
 413
 414
 415 static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 416 {
 417     VtolPathFollowerSettingsGet(&vtolPathFollowerSettings);
 418
 419     FrameType_t previous_frameType = frameType;
 420     frameType = GetCurrentFrameType();
 421
 422     if (frameType == FRAME_TYPE_CUSTOM) {
 423         switch (vtolPathFollowerSettings.TreatCustomCraftAs) {
 424         case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_FIXEDWING:
 425             frameType = FRAME_TYPE_FIXED_WING;
 426             break;
 427         case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_VTOL:
 428             frameType = FRAME_TYPE_MULTIROTOR;
 429             break;
 430         case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_GROUND:
 431             frameType = FRAME_TYPE_GROUND;
 432             break;
 433         }
 434     }
 435
 436     // If frame type changes, initialise the frame type controllers
 437     if (frameType != previous_frameType) {
 438         pathFollowerInitializeControllersForFrameType();
 439     }
 440
 441     switch (frameType) {
 442     case FRAME_TYPE_MULTIROTOR:
 443     case FRAME_TYPE_HELI:
 444         updatePeriod = vtolPathFollowerSettings.UpdatePeriod;
 445         break;
 446     case FRAME_TYPE_FIXED_WING:
 447         FixedWingPathFollowerSettingsGet(&fixedWingPathFollowerSettings);
 448         updatePeriod = fixedWingPathFollowerSettings.UpdatePeriod;
 449         break;
 450     case FRAME_TYPE_GROUND:
 451         GroundPathFollowerSettingsGet(&groundPathFollowerSettings);
 452         updatePeriod = groundPathFollowerSettings.UpdatePeriod;
 453         break;
 454     default:
 455         updatePeriod = vtolPathFollowerSettings.UpdatePeriod;
 456         break;
 457     }
 458
 459     if (activeController) {
 460         activeController->SettingsUpdated();
 461     }
 462 }
 463
 464
 465 static void airspeedStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 466 {
 467     FixedWingFlyController::instance()->AirspeedStateUpdatedCb(ev);
 468     FixedWingAutoTakeoffController::instance()->AirspeedStateUpdatedCb(ev);
 469 }
 470
 471
 472 /**
 473  * Compute desired attitude from a fixed preset
 474  *
 475  */
 476 static void updateFixedAttitude(float *attitude)
 477 {
 478     StabilizationDesiredData stabDesired;
 479
 480     StabilizationDesiredGet(&stabDesired);
 481     stabDesired.Roll   = attitude[0];
 482     stabDesired.Pitch  = attitude[1];
 483     stabDesired.Yaw    = attitude[2];
 484     stabDesired.Thrust = attitude[3];
 485     stabDesired.StabilizationMode.Roll   = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
 486     stabDesired.StabilizationMode.Pitch  = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
 487     stabDesired.StabilizationMode.Yaw    = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
 488     stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
 489     StabilizationDesiredSet(&stabDesired);
 490 }