flight/modules/PathFollower/fixedwinglandcontroller.cpp

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