flight/modules/PathFollower/vtolbrakecontroller.cpp

   1 /*
   2  ******************************************************************************
   3  *
   4  * @file       vtolbrakecontroller.cpp
   5  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
   6  * @brief      This landing state machine is a helper state machine to the
   7  *              pathfollower task/thread to implement detailed landing controls.
   8  *              This is to be called only from the pathfollower task.
   9  *              Note that initiation of the land occurs in the manual control
  10  *              command thread calling plans.c plan_setup_land which writes
  11  *              the required PathDesired LAND mode.
  12  * @see        The GNU Public License (GPL) Version 3
  13  *
  14  *****************************************************************************/
  15 /*
  16  * This program is free software; you can redistribute it and/or modify
  17  * it under the terms of the GNU General Public License as published by
  18  * the Free Software Foundation; either version 3 of the License, or
  19  * (at your option) any later version.
  20  *
  21  * This program is distributed in the hope that it will be useful, but
  22  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  23  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  24  * for more details.
  25  *
  26  * You should have received a copy of the GNU General Public License along
  27  * with this program; if not, write to the Free Software Foundation, Inc.,
  28  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  29  */
  30
  31 extern "C" {
  32 #include <openpilot.h>
  33
  34 #include <callbackinfo.h>
  35
  36 #include <math.h>
  37 #include <pid.h>
  38 #include <alarms.h>
  39 #include <CoordinateConversions.h>
  40 #include <sin_lookup.h>
  41 #include <pathdesired.h>
  42 #include <paths.h>
  43 #include "plans.h"
  44 #include <sanitycheck.h>
  45
  46 #include <accelstate.h>
  47 #include <vtolpathfollowersettings.h>
  48 #include <flightstatus.h>
  49 #include <flightmodesettings.h>
  50 #include <pathstatus.h>
  51 #include <positionstate.h>
  52 #include <velocitystate.h>
  53 #include <velocitydesired.h>
  54 #include <stabilizationdesired.h>
  55 #include <attitudestate.h>
  56 #include <takeofflocation.h>
  57 #include <manualcontrolcommand.h>
  58 #include <systemsettings.h>
  59 #include <stabilizationbank.h>
  60 #include <stabilizationdesired.h>
  61 #include <vtolselftuningstats.h>
  62 #include <pathsummary.h>
  63 }
  64
  65 // C++ includes
  66 #include "vtolbrakecontroller.h"
  67 #include "pathfollowerfsm.h"
  68 #include "vtolbrakefsm.h"
  69 #include "pidcontroldown.h"
  70
  71 // Private constants
  72 #define BRAKE_RATE_MINIMUM 0.2f
  73
  74 // pointer to a singleton instance
  75 VtolBrakeController *VtolBrakeController::p_inst = 0;
  76
  77 VtolBrakeController::VtolBrakeController()
  78     : fsm(0), vtolPathFollowerSettings(0), mActive(false), mHoldActive(false), mManualThrust(false)
  79 {}
  80
  81 // Called when mode first engaged
  82 void VtolBrakeController::Activate(void)
  83 {
  84     if (!mActive) {
  85         mActive       = true;
  86         mHoldActive   = false;
  87         mManualThrust = false;
  88         SettingsUpdated();
  89         fsm->Activate();
  90         controlDown.Activate();
  91         controlNE.Activate();
  92     }
  93 }
  94
  95 uint8_t VtolBrakeController::IsActive(void)
  96 {
  97     return mActive;
  98 }
  99
 100 uint8_t VtolBrakeController::Mode(void)
 101 {
 102     return PATHDESIRED_MODE_BRAKE;
 103 }
 104
 105 // Objective updated in pathdesired
 106 void VtolBrakeController::ObjectiveUpdated(void)
 107 {
 108     // Set the objective's target velocity
 109     controlDown.UpdateVelocitySetpoint(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_DOWN]);
 110     controlNE.UpdateVelocitySetpoint(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_NORTH],
 111                                      pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_EAST]);
 112     if (pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_TIMEOUT] > 0.0f) {
 113         pathStatus->path_time = 0.0f;
 114     }
 115 }
 116 void VtolBrakeController::Deactivate(void)
 117 {
 118     if (mActive) {
 119         mActive       = false;
 120         mHoldActive   = false;
 121         mManualThrust = false;
 122         fsm->Inactive();
 123         controlDown.Deactivate();
 124         controlNE.Deactivate();
 125     }
 126 }
 127
 128
 129 void VtolBrakeController::SettingsUpdated(void)
 130 {
 131     const float dT = vtolPathFollowerSettings->UpdatePeriod / 1000.0f;
 132
 133     controlNE.UpdateParameters(vtolPathFollowerSettings->BrakeHorizontalVelPID.Kp,
 134                                vtolPathFollowerSettings->BrakeHorizontalVelPID.Ki,
 135                                vtolPathFollowerSettings->BrakeHorizontalVelPID.Kd,
 136                                vtolPathFollowerSettings->BrakeHorizontalVelPID.Beta,
 137                                dT,
 138                                10.0f * vtolPathFollowerSettings->HorizontalVelMax); // avoid constraining initial fast entry into brake
 139     controlNE.UpdatePositionalParameters(vtolPathFollowerSettings->HorizontalPosP);
 140     controlNE.UpdateCommandParameters(-vtolPathFollowerSettings->BrakeMaxPitch, vtolPathFollowerSettings->BrakeMaxPitch, vtolPathFollowerSettings->BrakeVelocityFeedforward);
 141
 142     controlDown.UpdateParameters(vtolPathFollowerSettings->VerticalVelPID.Kp,
 143                                  vtolPathFollowerSettings->VerticalVelPID.Ki,
 144                                  vtolPathFollowerSettings->VerticalVelPID.Kd,
 145                                  vtolPathFollowerSettings->VerticalVelPID.Beta,
 146                                  dT,
 147                                  10.0f * vtolPathFollowerSettings->VerticalVelMax); // avoid constraining initial fast entry into brake
 148     controlDown.UpdatePositionalParameters(vtolPathFollowerSettings->VerticalPosP);
 149     controlDown.SetThrustLimits(vtolPathFollowerSettings->ThrustLimits.Min, vtolPathFollowerSettings->ThrustLimits.Max);
 150
 151     VtolSelfTuningStatsData vtolSelfTuningStats;
 152     VtolSelfTuningStatsGet(&vtolSelfTuningStats);
 153     controlDown.UpdateNeutralThrust(vtolSelfTuningStats.NeutralThrustOffset + vtolPathFollowerSettings->ThrustLimits.Neutral);
 154     fsm->SettingsUpdated();
 155 }
 156
 157 /**
 158  * Initialise the module, called on startup
 159  * \returns 0 on success or -1 if initialisation failed
 160  */
 161 int32_t VtolBrakeController::Initialize(VtolPathFollowerSettingsData *ptr_vtolPathFollowerSettings)
 162 {
 163     PIOS_Assert(ptr_vtolPathFollowerSettings);
 164     vtolPathFollowerSettings = ptr_vtolPathFollowerSettings;
 165     if (fsm == 0) {
 166         VtolBrakeFSM::instance()->Initialize(vtolPathFollowerSettings, pathDesired, flightStatus, pathStatus);
 167         fsm = (PathFollowerFSM *)VtolBrakeFSM::instance();
 168         controlDown.Initialize(fsm);
 169     }
 170     return 0;
 171 }
 172
 173
 174 void VtolBrakeController::UpdateVelocityDesired()
 175 {
 176     VelocityStateData velocityState;
 177
 178     VelocityStateGet(&velocityState);
 179     VelocityDesiredData velocityDesired;
 180
 181     float brakeRate = vtolPathFollowerSettings->BrakeRate;
 182     if (brakeRate < BRAKE_RATE_MINIMUM) {
 183         brakeRate = BRAKE_RATE_MINIMUM; // set a minimum to avoid a divide by zero potential below
 184     }
 185
 186     if (fsm->PositionHoldState()) {
 187         PositionStateData positionState;
 188         PositionStateGet(&positionState);
 189
 190         // On first engagement set the position setpoint
 191         if (!mHoldActive) {
 192             mHoldActive = true;
 193             controlNE.UpdatePositionSetpoint(positionState.North, positionState.East);
 194             if (!mManualThrust) {
 195                 controlDown.UpdatePositionSetpoint(positionState.Down);
 196             }
 197
 198
 199             FlightStatusFlightModeAssistOptions flightModeAssist;
 200             FlightStatusFlightModeAssistGet(&flightModeAssist);
 201             if (flightModeAssist != FLIGHTSTATUS_FLIGHTMODEASSIST_NONE) {
 202                 // Notify manualcommand via setting hold state in flightstatus assistedcontrolstate
 203                 FlightStatusAssistedControlStateOptions assistedControlFlightMode;
 204                 FlightStatusAssistedControlStateGet(&assistedControlFlightMode);
 205                 // sanity check that we are in brake state according to flight status, which means
 206                 // we are being used for gpsassist
 207                 if (assistedControlFlightMode == FLIGHTSTATUS_ASSISTEDCONTROLSTATE_BRAKE) {
 208                     assistedControlFlightMode = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_HOLD;
 209                     FlightStatusAssistedControlStateSet(&assistedControlFlightMode);
 210                 }
 211             }
 212         }
 213
 214         // Update position state and control position to create inputs to velocity control
 215         controlNE.UpdatePositionState(positionState.North, positionState.East);
 216         controlNE.ControlPosition();
 217         if (!mManualThrust) {
 218             controlDown.UpdatePositionState(positionState.Down);
 219             controlDown.ControlPosition();
 220         }
 221
 222         controlNE.UpdateVelocityState(velocityState.North, velocityState.East);
 223         if (!mManualThrust) {
 224             controlDown.UpdateVelocityState(velocityState.Down);
 225         }
 226     } else {
 227         controlNE.UpdateVelocityStateWithBrake(velocityState.North, velocityState.East, pathStatus->path_time, brakeRate);
 228         if (!mManualThrust) {
 229             controlDown.UpdateVelocityStateWithBrake(velocityState.Down, pathStatus->path_time, brakeRate);
 230         }
 231     }
 232
 233     if (!mManualThrust) {
 234         velocityDesired.Down = controlDown.GetVelocityDesired();
 235     } else { velocityDesired.Down = 0.0f; }
 236
 237     float north, east;
 238     controlNE.GetVelocityDesired(&north, &east);
 239     velocityDesired.North = north;
 240     velocityDesired.East  = east;
 241
 242     VelocityDesiredSet(&velocityDesired);
 243
 244     // update pathstatus
 245     float cur_velocity     = velocityState.North * velocityState.North + velocityState.East * velocityState.East + velocityState.Down * velocityState.Down;
 246     cur_velocity      = sqrtf(cur_velocity);
 247     float desired_velocity = velocityDesired.North * velocityDesired.North + velocityDesired.East * velocityDesired.East + velocityDesired.Down * velocityDesired.Down;
 248     desired_velocity  = sqrtf(desired_velocity);
 249     pathStatus->error = cur_velocity - desired_velocity;
 250     pathStatus->fractional_progress = 1.0f;
 251     if (pathDesired->StartingVelocity > 0.0f) {
 252         pathStatus->fractional_progress = (pathDesired->StartingVelocity - cur_velocity) / pathDesired->StartingVelocity;
 253
 254         // sometimes current velocity can exceed starting velocity due to initial acceleration
 255         if (pathStatus->fractional_progress < 0.0f) {
 256             pathStatus->fractional_progress = 0.0f;
 257         }
 258     }
 259     pathStatus->path_direction_north = velocityDesired.North;
 260     pathStatus->path_direction_east  = velocityDesired.East;
 261     pathStatus->path_direction_down  = velocityDesired.Down;
 262
 263     pathStatus->correction_direction_north = velocityDesired.North - velocityState.North;
 264     pathStatus->correction_direction_east  = velocityDesired.East - velocityState.East;
 265 }
 266
 267 int8_t VtolBrakeController::UpdateStabilizationDesired(void)
 268 {
 269     uint8_t result = 1;
 270     StabilizationDesiredData stabDesired;
 271     AttitudeStateData attitudeState;
 272     StabilizationBankData stabSettings;
 273     float northCommand;
 274     float eastCommand;
 275
 276     StabilizationDesiredGet(&stabDesired);
 277     AttitudeStateGet(&attitudeState);
 278     StabilizationBankGet(&stabSettings);
 279
 280     controlNE.GetNECommand(&northCommand, &eastCommand);
 281
 282     float angle_radians = DEG2RAD(attitudeState.Yaw);
 283     float cos_angle     = cosf(angle_radians);
 284     float sine_angle    = sinf(angle_radians);
 285     float maxPitch = vtolPathFollowerSettings->BrakeMaxPitch;
 286     stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
 287     stabDesired.Pitch = boundf(-northCommand * cos_angle - eastCommand * sine_angle, -maxPitch, maxPitch); // this should be in the controller
 288     stabDesired.StabilizationMode.Roll  = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
 289     stabDesired.Roll = boundf(-northCommand * sine_angle + eastCommand * cos_angle, -maxPitch, maxPitch);
 290
 291     ManualControlCommandData manualControl;
 292     ManualControlCommandGet(&manualControl);
 293
 294     stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
 295     stabDesired.Yaw = stabSettings.MaximumRate.Yaw * manualControl.Yaw;
 296
 297     // default thrust mode to cruise control
 298     stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL;
 299
 300     // when flight mode assist is active but in primary-thrust mode, the thrust mode must be set to the same as per the primary mode.
 301     if (flightStatus->FlightModeAssist == FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST_PRIMARYTHRUST) {
 302         FlightModeSettingsData settings;
 303         FlightModeSettingsGet(&settings);
 304         uint8_t thrustMode = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_CRUISECONTROL;
 305
 306         switch (flightStatus->FlightMode) {
 307         case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
 308             thrustMode = settings.Stabilization1Settings.Thrust;
 309             break;
 310         case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
 311             thrustMode = settings.Stabilization2Settings.Thrust;
 312             break;
 313         case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
 314             thrustMode = settings.Stabilization3Settings.Thrust;
 315             break;
 316         case FLIGHTSTATUS_FLIGHTMODE_STABILIZED4:
 317             thrustMode = settings.Stabilization4Settings.Thrust;
 318             break;
 319         case FLIGHTSTATUS_FLIGHTMODE_STABILIZED5:
 320             thrustMode = settings.Stabilization5Settings.Thrust;
 321             break;
 322         case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6:
 323             thrustMode = settings.Stabilization6Settings.Thrust;
 324             break;
 325         case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
 326             thrustMode = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEVARIO;
 327             break;
 328         case FLIGHTSTATUS_FLIGHTMODE_VELOCITYROAM:
 329             thrustMode = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEVARIO;
 330             break;
 331         default:
 332             break;
 333         }
 334         stabDesired.StabilizationMode.Thrust = (StabilizationDesiredStabilizationModeOptions)thrustMode;
 335     }
 336
 337     // set the thrust value
 338     if (mManualThrust) {
 339         stabDesired.Thrust = manualControl.Thrust;
 340     } else {
 341         stabDesired.Thrust = controlDown.GetDownCommand();
 342     }
 343
 344     StabilizationDesiredSet(&stabDesired);
 345
 346     return result;
 347 }
 348
 349
 350 void VtolBrakeController::UpdateAutoPilot()
 351 {
 352     if (pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_TIMEOUT] > 0.0f) {
 353         pathStatus->path_time += vtolPathFollowerSettings->UpdatePeriod / 1000.0f;
 354     }
 355
 356     if (flightStatus->FlightModeAssist && flightStatus->AssistedThrottleState == FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL) {
 357         mManualThrust = true;
 358     } else {
 359         mManualThrust = false;
 360     }
 361
 362     fsm->Update(); // This will run above end condition checks
 363
 364     UpdateVelocityDesired();
 365
 366     int8_t result = UpdateStabilizationDesired();
 367
 368     if (result) {
 369         AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
 370     } else {
 371         pathStatus->Status = PATHSTATUS_STATUS_CRITICAL;
 372         AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING);
 373     }
 374
 375     PathStatusSet(pathStatus);
 376 }