flight/modules/PathFollower/fixedwinglandcontroller.cpp

   1 /*
   2  ******************************************************************************
   3  *
   4  * @file       FixedWingLandController.cpp
   5  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
   6  * @brief      Fixed wing fly controller implementation
   7  * @see        The GNU Public License (GPL) Version 3
   8  *
   9  *****************************************************************************/
  10 /*
  11  * This program is free software; you can redistribute it and/or modify
  12  * it under the terms of the GNU General Public License as published by
  13  * the Free Software Foundation; either version 3 of the License, or
  14  * (at your option) any later version.
  15  *
  16  * This program is distributed in the hope that it will be useful, but
  17  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  18  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  19  * for more details.
  20  *
  21  * You should have received a copy of the GNU General Public License along
  22  * with this program; if not, write to the Free Software Foundation, Inc.,
  23  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  24  */
  25
  26 extern "C" {
  27 #include <openpilot.h>
  28
  29 #include <pathdesired.h>
  30 #include <fixedwingpathfollowersettings.h>
  31 #include <flightstatus.h>
  32 #include <pathstatus.h>
  33 #include <stabilizationdesired.h>
  34 }
  35
  36 // C++ includes
  37 #include "fixedwinglandcontroller.h"
  38
  39 // Private constants
  40
  41 // pointer to a singleton instance
  42 FixedWingLandController *FixedWingLandController::p_inst = 0;
  43
  44 FixedWingLandController::FixedWingLandController()
  45     : fixedWingSettings(NULL), mActive(false), mMode(0)
  46 {}
  47
  48 // Called when mode first engaged
  49 void FixedWingLandController::Activate(void)
  50 {
  51     if (!mActive) {
  52         mActive = true;
  53         SettingsUpdated();
  54         resetGlobals();
  55         mMode   = pathDesired->Mode;
  56     }
  57 }
  58
  59 uint8_t FixedWingLandController::IsActive(void)
  60 {
  61     return mActive;
  62 }
  63
  64 uint8_t FixedWingLandController::Mode(void)
  65 {
  66     return mMode;
  67 }
  68
  69 // Objective updated in pathdesired
  70 void FixedWingLandController::ObjectiveUpdated(void)
  71 {}
  72
  73 void FixedWingLandController::Deactivate(void)
  74 {
  75     if (mActive) {
  76         mActive = false;
  77         resetGlobals();
  78     }
  79 }
  80
  81
  82 void FixedWingLandController::SettingsUpdated(void)
  83 {}
  84
  85 /**
  86  * Initialise the module, called on startup
  87  * \returns 0 on success or -1 if initialisation failed
  88  */
  89 int32_t FixedWingLandController::Initialize(FixedWingPathFollowerSettingsData *ptr_fixedWingSettings)
  90 {
  91     PIOS_Assert(ptr_fixedWingSettings);
  92
  93     fixedWingSettings = ptr_fixedWingSettings;
  94
  95     resetGlobals();
  96
  97     return 0;
  98 }
  99
 100 /**
 101  * reset integrals
 102  */
 103 void FixedWingLandController::resetGlobals()
 104 {
 105     pathStatus->path_time = 0.0f;
 106 }
 107
 108 /**
 109  * fixed wing autopilot
 110  * use fixed attitude heading towards destination waypoint
 111  */
 112 void FixedWingLandController::UpdateAutoPilot()
 113 {
 114     StabilizationDesiredData stabDesired;
 115
 116     stabDesired.Roll   = 0.0f;
 117     stabDesired.Pitch  = fixedWingSettings->LandingPitch;
 118     stabDesired.Thrust = 0.0f;
 119     stabDesired.StabilizationMode.Roll   = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
 120     stabDesired.StabilizationMode.Pitch  = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
 121     stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
 122
 123     // find out vector direction of *runway* (if any)
 124     // and align, otherwise just stay straight ahead
 125     if (fabsf(pathDesired->Start.North - pathDesired->End.North) < 1e-3f &&
 126         fabsf(pathDesired->Start.East - pathDesired->End.East) < 1e-3f) {
 127         stabDesired.Yaw = 0.0f;
 128         stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
 129     } else {
 130         stabDesired.Yaw = RAD2DEG(atan2f(pathDesired->End.East - pathDesired->Start.East, pathDesired->End.North - pathDesired->Start.North));
 131         if (stabDesired.Yaw < -180.0f) {
 132             stabDesired.Yaw += 360.0f;
 133         }
 134         if (stabDesired.Yaw > 180.0f) {
 135             stabDesired.Yaw -= 360.0f;
 136         }
 137         stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
 138     }
 139     StabilizationDesiredSet(&stabDesired);
 140     AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
 141
 142     PathStatusSet(pathStatus);
 143 }