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