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