flight/modules/PathFollower/vtolvelocitycontroller.cpp

   1 /*
   2  ******************************************************************************
   3  *
   4  * @file       VtolVelocityController.cpp
   5  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
   6  * @brief      Velocity roam controller for vtols
   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 <callbackinfo.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 <vtolpathfollowersettings.h>
  42 #include <flightmodesettings.h>
  43 #include <pathstatus.h>
  44 #include <positionstate.h>
  45 #include <velocitystate.h>
  46 #include <velocitydesired.h>
  47 #include <stabilizationdesired.h>
  48 #include <attitudestate.h>
  49 #include <flightstatus.h>
  50 #include <manualcontrolcommand.h>
  51 #include <systemsettings.h>
  52 #include <stabilizationbank.h>
  53 #include <stabilizationdesired.h>
  54 #include <pathsummary.h>
  55 }
  56
  57 // C++ includes
  58 #include "vtolvelocitycontroller.h"
  59 #include "pidcontrolne.h"
  60
  61 // Private constants
  62
  63 // pointer to a singleton instance
  64 VtolVelocityController *VtolVelocityController::p_inst = 0;
  65
  66 VtolVelocityController::VtolVelocityController()
  67     : vtolPathFollowerSettings(0), mActive(false)
  68 {}
  69
  70 // Called when mode first engaged
  71 void VtolVelocityController::Activate(void)
  72 {
  73     if (!mActive) {
  74         mActive = true;
  75         SettingsUpdated();
  76         controlNE.Activate();
  77     }
  78 }
  79
  80 uint8_t VtolVelocityController::IsActive(void)
  81 {
  82     return mActive;
  83 }
  84
  85 uint8_t VtolVelocityController::Mode(void)
  86 {
  87     return PATHDESIRED_MODE_VELOCITY;
  88 }
  89
  90 void VtolVelocityController::ObjectiveUpdated(void)
  91 {
  92     controlNE.UpdateVelocitySetpoint(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_NORTH],
  93                                      pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_EAST]);
  94 }
  95
  96 void VtolVelocityController::Deactivate(void)
  97 {
  98     if (mActive) {
  99         mActive = false;
 100         controlNE.Deactivate();
 101     }
 102 }
 103
 104 void VtolVelocityController::SettingsUpdated(void)
 105 {
 106     const float dT = vtolPathFollowerSettings->UpdatePeriod / 1000.0f;
 107
 108     controlNE.UpdateParameters(vtolPathFollowerSettings->VelocityRoamHorizontalVelPID.Kp,
 109                                vtolPathFollowerSettings->VelocityRoamHorizontalVelPID.Ki,
 110                                vtolPathFollowerSettings->VelocityRoamHorizontalVelPID.Kd,
 111                                vtolPathFollowerSettings->VelocityRoamHorizontalVelPID.Beta,
 112                                dT,
 113                                vtolPathFollowerSettings->HorizontalVelMax);
 114
 115
 116     controlNE.UpdatePositionalParameters(vtolPathFollowerSettings->HorizontalPosP);
 117     controlNE.UpdateCommandParameters(-vtolPathFollowerSettings->VelocityRoamMaxRollPitch, vtolPathFollowerSettings->VelocityRoamMaxRollPitch, vtolPathFollowerSettings->VelocityFeedforward);
 118 }
 119
 120 int32_t VtolVelocityController::Initialize(VtolPathFollowerSettingsData *ptr_vtolPathFollowerSettings)
 121 {
 122     PIOS_Assert(ptr_vtolPathFollowerSettings);
 123
 124     vtolPathFollowerSettings = ptr_vtolPathFollowerSettings;
 125     return 0;
 126 }
 127
 128
 129 void VtolVelocityController::UpdateVelocityDesired()
 130 {
 131     VelocityStateData velocityState;
 132
 133     VelocityStateGet(&velocityState);
 134     VelocityDesiredData velocityDesired;
 135
 136     controlNE.UpdateVelocityState(velocityState.North, velocityState.East);
 137
 138     velocityDesired.Down  = 0.0f;
 139     float north, east;
 140     controlNE.GetVelocityDesired(&north, &east);
 141     velocityDesired.North = north;
 142     velocityDesired.East  = east;
 143
 144     // update pathstatus
 145     pathStatus->error     = 0.0f;
 146     pathStatus->fractional_progress  = 0.0f;
 147     pathStatus->path_direction_north = velocityDesired.North;
 148     pathStatus->path_direction_east  = velocityDesired.East;
 149     pathStatus->path_direction_down  = velocityDesired.Down;
 150
 151     pathStatus->correction_direction_north = velocityDesired.North - velocityState.North;
 152     pathStatus->correction_direction_east  = velocityDesired.East - velocityState.East;
 153     pathStatus->correction_direction_down  = 0.0f;
 154
 155     VelocityDesiredSet(&velocityDesired);
 156 }
 157
 158 int8_t VtolVelocityController::UpdateStabilizationDesired(__attribute__((unused)) bool yaw_attitude, __attribute__((unused)) float yaw_direction)
 159 {
 160     uint8_t result = 1;
 161     StabilizationDesiredData stabDesired;
 162     AttitudeStateData attitudeState;
 163     StabilizationBankData stabSettings;
 164     float northCommand;
 165     float eastCommand;
 166
 167     StabilizationDesiredGet(&stabDesired);
 168     AttitudeStateGet(&attitudeState);
 169     StabilizationBankGet(&stabSettings);
 170
 171     controlNE.GetNECommand(&northCommand, &eastCommand);
 172
 173     float angle_radians = DEG2RAD(attitudeState.Yaw);
 174     float cos_angle     = cosf(angle_radians);
 175     float sine_angle    = sinf(angle_radians);
 176     float maxPitch = vtolPathFollowerSettings->VelocityRoamMaxRollPitch;
 177     stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
 178     stabDesired.Pitch = boundf(-northCommand * cos_angle - eastCommand * sine_angle, -maxPitch, maxPitch);
 179     stabDesired.StabilizationMode.Roll  = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
 180     stabDesired.Roll = boundf(-northCommand * sine_angle + eastCommand * cos_angle, -maxPitch, maxPitch);
 181
 182     ManualControlCommandData manualControl;
 183     ManualControlCommandGet(&manualControl);
 184
 185     stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
 186     stabDesired.Yaw = stabSettings.MaximumRate.Yaw * manualControl.Yaw;
 187
 188     // default thrust mode to altvario
 189     stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEVARIO;
 190     stabDesired.Thrust = manualControl.Thrust;
 191
 192     StabilizationDesiredSet(&stabDesired);
 193
 194     return result;
 195 }
 196
 197 void VtolVelocityController::UpdateAutoPilot()
 198 {
 199     UpdateVelocityDesired();
 200
 201     bool yaw_attitude = false;
 202     float yaw = 0.0f;
 203
 204     int8_t result     = UpdateStabilizationDesired(yaw_attitude, yaw);
 205     if (result) {
 206         AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
 207     } else {
 208         pathStatus->Status = PATHSTATUS_STATUS_CRITICAL;
 209         AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING);
 210     }
 211
 212     PathStatusSet(pathStatus);
 213 }