flight/modules/PathFollower/vtollandcontroller.cpp

   1 /*
   2  ******************************************************************************
   3  *
   4  * @file       vtollandcontroller.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      Vtol landing controller loop
   8  * @see        The GNU Public License (GPL) Version 3
   9  * @addtogroup LibrePilot LibrePilotModules Modules PathFollower Navigation
  10  *
  11  *****************************************************************************/
  12 /*
  13  * This program is free software; you can redistribute it and/or modify
  14  * it under the terms of the GNU General Public License as published by
  15  * the Free Software Foundation; either version 3 of the License, or
  16  * (at your option) any later version.
  17  *
  18  * This program is distributed in the hope that it will be useful, but
  19  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  20  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  21  * for more details.
  22  *
  23  * You should have received a copy of the GNU General Public License along
  24  * with this program; if not, write to the Free Software Foundation, Inc.,
  25  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  26  */
  27
  28 extern "C" {
  29 #include <openpilot.h>
  30
  31 #include <math.h>
  32 #include <pid.h>
  33 #include <alarms.h>
  34 #include <CoordinateConversions.h>
  35 #include <sin_lookup.h>
  36 #include <pathdesired.h>
  37 #include <paths.h>
  38 #include "plans.h"
  39 #include <sanitycheck.h>
  40
  41 #include <accelstate.h>
  42 #include <vtolpathfollowersettings.h>
  43 #include <flightstatus.h>
  44 #include <flightmodesettings.h>
  45 #include <pathstatus.h>
  46 #include <positionstate.h>
  47 #include <velocitystate.h>
  48 #include <velocitydesired.h>
  49 #include <stabilizationdesired.h>
  50 #include <attitudestate.h>
  51 #include <takeofflocation.h>
  52 #include <manualcontrolcommand.h>
  53 #include <systemsettings.h>
  54 #include <stabilizationbank.h>
  55 #include <stabilizationdesired.h>
  56 #include <vtolselftuningstats.h>
  57 #include <pathsummary.h>
  58 }
  59
  60 // C++ includes
  61 #include "vtollandcontroller.h"
  62 #include "pathfollowerfsm.h"
  63 #include "vtollandfsm.h"
  64 #include "pidcontroldown.h"
  65
  66 // Private constants
  67
  68 // pointer to a singleton instance
  69 VtolLandController *VtolLandController::p_inst = 0;
  70
  71 VtolLandController::VtolLandController()
  72     : fsm(NULL), vtolPathFollowerSettings(NULL), mActive(false)
  73 {}
  74
  75 // Called when mode first engaged
  76 void VtolLandController::Activate(void)
  77 {
  78     if (!mActive) {
  79         mActive   = true;
  80         mOverride = true;
  81         SettingsUpdated();
  82         fsm->Activate();
  83         controlDown.Activate();
  84         controlNE.Activate();
  85     }
  86 }
  87
  88 uint8_t VtolLandController::IsActive(void)
  89 {
  90     return mActive;
  91 }
  92
  93 uint8_t VtolLandController::Mode(void)
  94 {
  95     return PATHDESIRED_MODE_LAND;
  96 }
  97
  98 // Objective updated in pathdesired, e.g. same flight mode but new target velocity
  99 void VtolLandController::ObjectiveUpdated(void)
 100 {
 101     if (mOverride) {
 102         // override pathDesired from PathPLanner with current position,
 103         // as we deliberately don' not care about the location of the waypoints on the map
 104         float velocity_down;
 105         PositionStateData positionState;
 106         PositionStateGet(&positionState);
 107         FlightModeSettingsLandingVelocityGet(&velocity_down);
 108         controlDown.UpdateVelocitySetpoint(velocity_down);
 109         controlNE.UpdateVelocitySetpoint(0.0f, 0.0f);
 110         controlNE.UpdatePositionSetpoint(positionState.North, positionState.East);
 111         mOverride = false; // further updates always come from ManualControl and will control horizontal position
 112     } else {
 113         // Set the objective's target velocity
 114         controlDown.UpdateVelocitySetpoint(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_DOWN]);
 115         controlNE.UpdateVelocitySetpoint(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_NORTH],
 116                                          pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_EAST]);
 117         controlNE.UpdatePositionSetpoint(pathDesired->End.North, pathDesired->End.East);
 118     }
 119 }
 120 void VtolLandController::Deactivate(void)
 121 {
 122     if (mActive) {
 123         mActive = false;
 124         fsm->Inactive();
 125         controlDown.Deactivate();
 126         controlNE.Deactivate();
 127     }
 128 }
 129
 130
 131 void VtolLandController::SettingsUpdated(void)
 132 {
 133     const float dT = vtolPathFollowerSettings->UpdatePeriod / 1000.0f;
 134
 135     controlNE.UpdateParameters(vtolPathFollowerSettings->HorizontalVelPID.Kp,
 136                                vtolPathFollowerSettings->HorizontalVelPID.Ki,
 137                                vtolPathFollowerSettings->HorizontalVelPID.Kd,
 138                                vtolPathFollowerSettings->HorizontalVelPID.Beta,
 139                                dT,
 140                                vtolPathFollowerSettings->HorizontalVelMax);
 141
 142
 143     controlNE.UpdatePositionalParameters(vtolPathFollowerSettings->HorizontalPosP);
 144     controlNE.UpdateCommandParameters(-vtolPathFollowerSettings->MaxRollPitch, vtolPathFollowerSettings->MaxRollPitch, vtolPathFollowerSettings->VelocityFeedforward);
 145
 146     controlDown.UpdateParameters(vtolPathFollowerSettings->LandVerticalVelPID.Kp,
 147                                  vtolPathFollowerSettings->LandVerticalVelPID.Ki,
 148                                  vtolPathFollowerSettings->LandVerticalVelPID.Kd,
 149                                  vtolPathFollowerSettings->LandVerticalVelPID.Beta,
 150                                  dT,
 151                                  vtolPathFollowerSettings->VerticalVelMax);
 152
 153     // The following is not currently used in the landing control.
 154     controlDown.UpdatePositionalParameters(vtolPathFollowerSettings->VerticalPosP);
 155
 156     VtolSelfTuningStatsData vtolSelfTuningStats;
 157     VtolSelfTuningStatsGet(&vtolSelfTuningStats);
 158     controlDown.UpdateNeutralThrust(vtolSelfTuningStats.NeutralThrustOffset + vtolPathFollowerSettings->ThrustLimits.Neutral);
 159     // initialise limits on thrust but note the FSM can override.
 160     controlDown.SetThrustLimits(vtolPathFollowerSettings->ThrustLimits.Min, vtolPathFollowerSettings->ThrustLimits.Max);
 161     fsm->SettingsUpdated();
 162 }
 163
 164 /**
 165  * Initialise the module, called on startup
 166  * \returns 0 on success or -1 if initialisation failed
 167  */
 168 int32_t VtolLandController::Initialize(VtolPathFollowerSettingsData *ptr_vtolPathFollowerSettings)
 169 {
 170     PIOS_Assert(ptr_vtolPathFollowerSettings);
 171     if (fsm == 0) {
 172         fsm = (PathFollowerFSM *)VtolLandFSM::instance();
 173         VtolLandFSM::instance()->Initialize(ptr_vtolPathFollowerSettings, pathDesired, flightStatus);
 174         vtolPathFollowerSettings = ptr_vtolPathFollowerSettings;
 175         controlDown.Initialize(fsm);
 176     }
 177
 178     return 0;
 179 }
 180
 181
 182 void VtolLandController::UpdateVelocityDesired()
 183 {
 184     VelocityStateData velocityState;
 185
 186     VelocityStateGet(&velocityState);
 187     VelocityDesiredData velocityDesired;
 188
 189     controlDown.UpdateVelocityState(velocityState.Down);
 190     controlNE.UpdateVelocityState(velocityState.North, velocityState.East);
 191
 192     // Implement optional horizontal position hold.
 193     if ((((uint8_t)pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_OPTIONS]) == PATHDESIRED_MODEPARAMETER_LAND_OPTION_HORIZONTAL_PH) ||
 194         (flightStatus->ControlChain.PathPlanner == FLIGHTSTATUS_CONTROLCHAIN_TRUE)) {
 195         // landing flight mode has stored original horizontal position in pathdesired
 196         PositionStateData positionState;
 197         PositionStateGet(&positionState);
 198         controlNE.UpdatePositionState(positionState.North, positionState.East);
 199         controlNE.ControlPosition();
 200     }
 201
 202     velocityDesired.Down  = controlDown.GetVelocityDesired();
 203     float north, east;
 204     controlNE.GetVelocityDesired(&north, &east);
 205     velocityDesired.North = north;
 206     velocityDesired.East  = east;
 207
 208     // update pathstatus
 209     pathStatus->error     = 0.0f;
 210     pathStatus->fractional_progress = 0.0f;
 211     if (fsm->GetCurrentState() == PFFSM_STATE_DISARMED) {
 212         pathStatus->fractional_progress = 1.0f;
 213     }
 214     pathStatus->path_direction_north = velocityDesired.North;
 215     pathStatus->path_direction_east  = velocityDesired.East;
 216     pathStatus->path_direction_down  = velocityDesired.Down;
 217
 218     pathStatus->correction_direction_north = velocityDesired.North - velocityState.North;
 219     pathStatus->correction_direction_east  = velocityDesired.East - velocityState.East;
 220     pathStatus->correction_direction_down  = velocityDesired.Down - velocityState.Down;
 221
 222
 223     VelocityDesiredSet(&velocityDesired);
 224 }
 225
 226 int8_t VtolLandController::UpdateStabilizationDesired(bool yaw_attitude, float yaw_direction)
 227 {
 228     uint8_t result = 1;
 229     StabilizationDesiredData stabDesired;
 230     AttitudeStateData attitudeState;
 231     StabilizationBankData stabSettings;
 232     float northCommand;
 233     float eastCommand;
 234
 235     StabilizationDesiredGet(&stabDesired);
 236     AttitudeStateGet(&attitudeState);
 237     StabilizationBankGet(&stabSettings);
 238
 239     controlNE.GetNECommand(&northCommand, &eastCommand);
 240     stabDesired.Thrust = controlDown.GetDownCommand();
 241
 242     float angle_radians = DEG2RAD(attitudeState.Yaw);
 243     float cos_angle     = cosf(angle_radians);
 244     float sine_angle    = sinf(angle_radians);
 245     float maxPitch = vtolPathFollowerSettings->MaxRollPitch;
 246     stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
 247     stabDesired.Pitch = boundf(-northCommand * cos_angle - eastCommand * sine_angle, -maxPitch, maxPitch);
 248     stabDesired.StabilizationMode.Roll  = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
 249     stabDesired.Roll = boundf(-northCommand * sine_angle + eastCommand * cos_angle, -maxPitch, maxPitch);
 250
 251     ManualControlCommandData manualControl;
 252     ManualControlCommandGet(&manualControl);
 253
 254     if (yaw_attitude) {
 255         stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
 256         stabDesired.Yaw = yaw_direction;
 257     } else {
 258         stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
 259         stabDesired.Yaw = stabSettings.MaximumRate.Yaw * manualControl.Yaw;
 260     }
 261
 262     // default thrust mode to cruise control
 263     stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL;
 264
 265     fsm->ConstrainStabiDesired(&stabDesired); // excludes thrust
 266     StabilizationDesiredSet(&stabDesired);
 267
 268     return result;
 269 }
 270
 271 void VtolLandController::UpdateAutoPilot()
 272 {
 273     fsm->Update();
 274
 275     UpdateVelocityDesired();
 276
 277     // yaw behaviour is configurable in vtolpathfollower, select yaw control algorithm
 278     bool yaw_attitude = false;
 279     float yaw = 0.0f;
 280
 281     fsm->GetYaw(yaw_attitude, yaw);
 282
 283     int8_t result = UpdateStabilizationDesired(yaw_attitude, yaw);
 284     if (result) {
 285         AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
 286     } else {
 287         pathStatus->Status = PATHSTATUS_STATUS_CRITICAL;
 288         AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING);
 289     }
 290
 291     PathStatusSet(pathStatus);
 292 }