flight/modules/PathFollower/vtolbrakefsm.cpp

   1 /*
   2  ******************************************************************************
   3  *
   4  * @file       vtolbrakefsm.cpp
   5  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
   6  * @brief      Vtol brake finate state machine to regulate behaviour of the
   7  *              brake controller.
   8  * @see        The GNU Public License (GPL) Version 3
   9  *
  10  *****************************************************************************/
  11 /*
  12  * This program is free software; you can redistribute it and/or modify
  13  * it under the terms of the GNU General Public License as published by
  14  * the Free Software Foundation; either version 3 of the License, or
  15  * (at your option) any later version.
  16  *
  17  * This program is distributed in the hope that it will be useful, but
  18  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  19  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  20  * for more details.
  21  *
  22  * You should have received a copy of the GNU General Public License along
  23  * with this program; if not, write to the Free Software Foundation, Inc.,
  24  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  25  */
  26
  27 extern "C" {
  28 #include <openpilot.h>
  29
  30 #include <math.h>
  31 #include <pid.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 <flightstatus.h>
  41 #include <flightmodesettings.h>
  42 #include <pathstatus.h>
  43 #include <positionstate.h>
  44 #include <velocitystate.h>
  45 #include <velocitydesired.h>
  46 #include <stabilizationdesired.h>
  47 #include <attitudestate.h>
  48 #include <manualcontrolcommand.h>
  49 #include <systemsettings.h>
  50 #include <stabilizationbank.h>
  51 #include <stabilizationdesired.h>
  52 #include <vtolselftuningstats.h>
  53 #include <pathsummary.h>
  54 }
  55
  56 // C++ includes
  57 #include <vtolbrakefsm.h>
  58
  59
  60 // Private constants
  61 #define TIMER_COUNT_PER_SECOND                      (1000 / vtolPathFollowerSettings->UpdatePeriod)
  62 #define BRAKE_FRACTIONALPROGRESS_STARTVELOCITYCHECK 0.95f
  63 #define BRAKE_EXIT_VELOCITY_LIMIT                   0.15f
  64
  65 VtolBrakeFSM::PathFollowerFSM_BrakeStateHandler_T VtolBrakeFSM::sBrakeStateTable[BRAKE_STATE_SIZE] = {
  66     [BRAKE_STATE_INACTIVE] = { .setup = 0,                          .run = 0                        },
  67     [BRAKE_STATE_BRAKE]    = { .setup = &VtolBrakeFSM::setup_brake, .run = &VtolBrakeFSM::run_brake },
  68     [BRAKE_STATE_HOLD]     = { .setup = 0,                          .run = 0                        }
  69 };
  70
  71 // pointer to a singleton instance
  72 VtolBrakeFSM *VtolBrakeFSM::p_inst = 0;
  73
  74
  75 VtolBrakeFSM::VtolBrakeFSM()
  76     : mBrakeData(0), vtolPathFollowerSettings(0), pathDesired(0), flightStatus(0)
  77 {}
  78
  79
  80 // Private types
  81
  82 // Private functions
  83 // Public API methods
  84 /**
  85  * Initialise the module, called on startup
  86  * \returns 0 on success or -1 if initialisation failed
  87  */
  88 int32_t VtolBrakeFSM::Initialize(VtolPathFollowerSettingsData *ptr_vtolPathFollowerSettings,
  89                                  PathDesiredData *ptr_pathDesired,
  90                                  FlightStatusData *ptr_flightStatus,
  91                                  PathStatusData *ptr_pathStatus)
  92 {
  93     PIOS_Assert(ptr_vtolPathFollowerSettings);
  94     PIOS_Assert(ptr_pathDesired);
  95     PIOS_Assert(ptr_flightStatus);
  96
  97     // allow for Initialize being called more than once.
  98     if (!mBrakeData) {
  99         mBrakeData = (VtolBrakeFSMData_T *)pios_malloc(sizeof(VtolBrakeFSMData_T));
 100         PIOS_Assert(mBrakeData);
 101     }
 102     memset(mBrakeData, 0, sizeof(VtolBrakeFSMData_T));
 103     vtolPathFollowerSettings = ptr_vtolPathFollowerSettings;
 104     pathDesired  = ptr_pathDesired;
 105     flightStatus = ptr_flightStatus;
 106     pathStatus   = ptr_pathStatus;
 107     initFSM();
 108
 109     return 0;
 110 }
 111
 112 void VtolBrakeFSM::Inactive(void)
 113 {
 114     memset(mBrakeData, 0, sizeof(VtolBrakeFSMData_T));
 115     initFSM();
 116 }
 117
 118 // Initialise the FSM
 119 void VtolBrakeFSM::initFSM(void)
 120 {
 121     mBrakeData->currentState = BRAKE_STATE_INACTIVE;
 122 }
 123
 124 void VtolBrakeFSM::Activate()
 125 {
 126     memset(mBrakeData, 0, sizeof(VtolBrakeFSMData_T));
 127     mBrakeData->currentState = BRAKE_STATE_INACTIVE;
 128     setState(BRAKE_STATE_BRAKE, FSMBRAKESTATUS_STATEEXITREASON_NONE);
 129 }
 130
 131 PathFollowerFSMState_T VtolBrakeFSM::GetCurrentState(void)
 132 {
 133     switch (mBrakeData->currentState) {
 134     case BRAKE_STATE_INACTIVE:
 135         return PFFSM_STATE_INACTIVE;
 136
 137         break;
 138     default:
 139         return PFFSM_STATE_ACTIVE;
 140
 141         break;
 142     }
 143 }
 144
 145 void VtolBrakeFSM::Update()
 146 {
 147     runState();
 148 }
 149
 150 int32_t VtolBrakeFSM::runState(void)
 151 {
 152     uint8_t flTimeout = false;
 153
 154     mBrakeData->stateRunCount++;
 155
 156     if (mBrakeData->stateTimeoutCount > 0 && mBrakeData->stateRunCount > mBrakeData->stateTimeoutCount) {
 157         flTimeout = true;
 158     }
 159
 160     // If the current state has a static function, call it
 161     if (sBrakeStateTable[mBrakeData->currentState].run) {
 162         (this->*sBrakeStateTable[mBrakeData->currentState].run)(flTimeout);
 163     }
 164     return 0;
 165 }
 166
 167 // Set the new state and perform setup for subsequent state run calls
 168 // This is called by state run functions on event detection that drive
 169 // state transitions.
 170 void VtolBrakeFSM::setState(PathFollowerFSM_BrakeState_T newState, __attribute__((unused)) VtolBrakeFSMStatusStateExitReasonOptions reason)
 171 {
 172     // mBrakeData->fsmBrakeStatus.StateExitReason[mBrakeData->currentState] = reason;
 173
 174     if (mBrakeData->currentState == newState) {
 175         return;
 176     }
 177     mBrakeData->currentState      = newState;
 178
 179     // Restart state timer counter
 180     mBrakeData->stateRunCount     = 0;
 181
 182     // Reset state timeout to disabled/zero
 183     mBrakeData->stateTimeoutCount = 0;
 184
 185     if (sBrakeStateTable[mBrakeData->currentState].setup) {
 186         (this->*sBrakeStateTable[mBrakeData->currentState].setup)();
 187     }
 188 }
 189
 190
 191 // Timeout utility function for use by state init implementations
 192 void VtolBrakeFSM::setStateTimeout(int32_t count)
 193 {
 194     mBrakeData->stateTimeoutCount = count;
 195 }
 196
 197 // FSM Setup and Run method implementation
 198
 199 // State: WAITING FOR DESCENT RATE
 200 void VtolBrakeFSM::setup_brake(void)
 201 {
 202     setStateTimeout(TIMER_COUNT_PER_SECOND * pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_TIMEOUT]);
 203     mBrakeData->observationCount  = 0;
 204     mBrakeData->observation2Count = 0;
 205 }
 206
 207
 208 void VtolBrakeFSM::run_brake(uint8_t flTimeout)
 209 {
 210     // Brake mode end condition checks
 211     bool exit_brake = false;
 212     VelocityStateData velocityState;
 213     PathSummaryData pathSummary;
 214
 215     if (flTimeout) {
 216         pathSummary.brake_exit_reason = PATHSUMMARY_BRAKE_EXIT_REASON_TIMEOUT;
 217         exit_brake = true;
 218     } else if (pathStatus->fractional_progress > BRAKE_FRACTIONALPROGRESS_STARTVELOCITYCHECK) {
 219         VelocityStateGet(&velocityState);
 220         if (fabsf(velocityState.East) < BRAKE_EXIT_VELOCITY_LIMIT && fabsf(velocityState.North) < BRAKE_EXIT_VELOCITY_LIMIT) {
 221             pathSummary.brake_exit_reason = PATHSUMMARY_BRAKE_EXIT_REASON_PATHCOMPLETED;
 222             exit_brake = true;
 223         }
 224     }
 225
 226     if (exit_brake) {
 227         // Calculate the distance error between the originally desired
 228         // stopping point and the actual brake-exit point.
 229
 230         PositionStateData p;
 231         PositionStateGet(&p);
 232         float north_offset = pathDesired->End.North - p.North;
 233         float east_offset  = pathDesired->End.East - p.East;
 234         float down_offset  = pathDesired->End.Down - p.Down;
 235         pathSummary.brake_distance_offset = sqrtf(north_offset * north_offset + east_offset * east_offset + down_offset * down_offset);
 236         pathSummary.time_remaining = pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_TIMEOUT] - pathStatus->path_time;
 237         pathSummary.fractional_progress   = pathStatus->fractional_progress;
 238         float cur_velocity = velocityState.North * velocityState.North + velocityState.East * velocityState.East + velocityState.Down * velocityState.Down;
 239         cur_velocity = sqrtf(cur_velocity);
 240         pathSummary.decelrate = (pathDesired->StartingVelocity - cur_velocity) / pathStatus->path_time;
 241         pathSummary.brakeRateActualDesiredRatio = pathSummary.decelrate / vtolPathFollowerSettings->BrakeRate;
 242         pathSummary.velocityIntoHold = cur_velocity;
 243         pathSummary.Mode = PATHSUMMARY_MODE_BRAKE;
 244         pathSummary.UID  = pathStatus->UID;
 245         PathSummarySet(&pathSummary);
 246
 247         setState(BRAKE_STATE_HOLD, FSMBRAKESTATUS_STATEEXITREASON_NONE);
 248     }
 249 }
 250
 251 uint8_t VtolBrakeFSM::PositionHoldState(void)
 252 {
 253     return mBrakeData->currentState == BRAKE_STATE_HOLD;
 254 }