flight/modules/PathFollower/grounddrivecontroller.cpp

   1 /*
   2  ******************************************************************************
   3  *
   4  * @file       grounddrivecontroller.cpp
   5  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
   6  * @brief      Ground drive controller
   7  *              the required PathDesired LAND mode.
   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 <homelocation.h>
  40 #include <accelstate.h>
  41 #include <groundpathfollowersettings.h>
  42 #include <flightstatus.h>
  43 #include <flightmodesettings.h>
  44 #include <pathstatus.h>
  45 #include <positionstate.h>
  46 #include <velocitystate.h>
  47 #include <velocitydesired.h>
  48 #include <stabilizationdesired.h>
  49 #include <airspeedstate.h>
  50 #include <attitudestate.h>
  51 #include <takeofflocation.h>
  52 #include <poilocation.h>
  53 #include <manualcontrolcommand.h>
  54 #include <systemsettings.h>
  55 #include <stabilizationbank.h>
  56 #include <stabilizationdesired.h>
  57 #include <pathsummary.h>
  58 #include <statusgrounddrive.h>
  59 }
  60
  61 // C++ includes
  62 #include "grounddrivecontroller.h"
  63
  64 // Private constants
  65
  66 // pointer to a singleton instance
  67 GroundDriveController *GroundDriveController::p_inst = 0;
  68
  69 GroundDriveController::GroundDriveController()
  70     : groundSettings(0), mActive(false), mMode(0)
  71 {}
  72
  73 // Called when mode first engaged
  74 void GroundDriveController::Activate(void)
  75 {
  76     if (!mActive) {
  77         mActive = true;
  78         SettingsUpdated();
  79         controlNE.Activate();
  80         mMode   = pathDesired->Mode;
  81     }
  82 }
  83
  84 uint8_t GroundDriveController::IsActive(void)
  85 {
  86     return mActive;
  87 }
  88
  89 uint8_t GroundDriveController::Mode(void)
  90 {
  91     return mMode;
  92 }
  93
  94 // Objective updated in pathdesired
  95 void GroundDriveController::ObjectiveUpdated(void)
  96 {}
  97
  98 void GroundDriveController::Deactivate(void)
  99 {
 100     if (mActive) {
 101         mActive = false;
 102         controlNE.Deactivate();
 103     }
 104 }
 105
 106
 107 void GroundDriveController::SettingsUpdated(void)
 108 {
 109     const float dT = groundSettings->UpdatePeriod / 1000.0f;
 110
 111     controlNE.UpdatePositionalParameters(groundSettings->HorizontalPosP);
 112     controlNE.UpdateParameters(groundSettings->SpeedPI.Kp,
 113                                groundSettings->SpeedPI.Ki,
 114                                groundSettings->SpeedPI.Kd,
 115                                groundSettings->SpeedPI.Beta,
 116                                dT,
 117                                groundSettings->HorizontalVelMax);
 118
 119
 120     // max/min are NE command values equivalent to thrust but must be symmetrical as this is NE not forward/reverse.
 121     controlNE.UpdateCommandParameters(-groundSettings->ThrustLimit.Max, groundSettings->ThrustLimit.Max, groundSettings->VelocityFeedForward);
 122 }
 123
 124 /**
 125  * Initialise the module, called on startup
 126  * \returns 0 on success or -1 if initialisation failed
 127  */
 128 int32_t GroundDriveController::Initialize(GroundPathFollowerSettingsData *ptr_groundSettings)
 129 {
 130     PIOS_Assert(ptr_groundSettings);
 131
 132     groundSettings = ptr_groundSettings;
 133
 134     return 0;
 135 }
 136
 137 void GroundDriveController::UpdateAutoPilot()
 138 {
 139     uint8_t result = updateAutoPilotGround();
 140
 141     if (result) {
 142         AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
 143     } else {
 144         pathStatus->Status = PATHSTATUS_STATUS_CRITICAL;
 145         AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING);
 146     }
 147
 148     PathStatusSet(pathStatus);
 149 }
 150
 151 /**
 152  * fixed wing autopilot:
 153  * straight forward:
 154  * 1. update path velocity for limited motion crafts
 155  * 2. update attitude according to default fixed wing pathfollower algorithm
 156  */
 157 uint8_t GroundDriveController::updateAutoPilotGround()
 158 {
 159     updatePathVelocity(groundSettings->CourseFeedForward);
 160     return updateGroundDesiredAttitude();
 161 }
 162
 163 /**
 164  * Compute desired velocity from the current position and path
 165  */
 166 void GroundDriveController::updatePathVelocity(float kFF)
 167 {
 168     PositionStateData positionState;
 169
 170     PositionStateGet(&positionState);
 171     VelocityStateData velocityState;
 172     VelocityStateGet(&velocityState);
 173     VelocityDesiredData velocityDesired;
 174     controlNE.UpdateVelocityState(velocityState.North, velocityState.East);
 175
 176     // look ahead kFF seconds
 177     float cur[3] = { positionState.North + (velocityState.North * kFF),
 178                      positionState.East + (velocityState.East * kFF),
 179                      positionState.Down + (velocityState.Down * kFF) };
 180     struct path_status progress;
 181     path_progress(pathDesired, cur, &progress, false);
 182
 183     // GOTOENDPOINT: correction_vector is distance array to endpoint, path_vector is velocity vector
 184     // FOLLOWVECTOR:  correct_vector is distance to vector path, path_vector is the desired velocity vector
 185
 186     // Calculate the desired velocity from the lateral vector path errors (correct_vector) and the desired velocity vector (path_vector)
 187     controlNE.ControlPositionWithPath(&progress);
 188     float north, east;
 189     controlNE.GetVelocityDesired(&north, &east);
 190     velocityDesired.North = north;
 191     velocityDesired.East  = east;
 192     velocityDesired.Down  = 0.0f;
 193
 194     // update pathstatus
 195     pathStatus->error     = progress.error;
 196     pathStatus->fractional_progress  = progress.fractional_progress;
 197     // FOLLOWVECTOR: desired velocity vector
 198     pathStatus->path_direction_north = progress.path_vector[0];
 199     pathStatus->path_direction_east  = progress.path_vector[1];
 200     pathStatus->path_direction_down  = progress.path_vector[2];
 201
 202     // FOLLOWVECTOR: correction distance to vector path
 203     pathStatus->correction_direction_north = progress.correction_vector[0];
 204     pathStatus->correction_direction_east  = progress.correction_vector[1];
 205     pathStatus->correction_direction_down  = progress.correction_vector[2];
 206
 207     VelocityDesiredSet(&velocityDesired);
 208 }
 209
 210 /**
 211  * Compute desired attitude for ground vehicles
 212  */
 213 uint8_t GroundDriveController::updateGroundDesiredAttitude()
 214 {
 215     StatusGroundDriveData statusGround;
 216     VelocityStateData velocityState;
 217
 218     VelocityStateGet(&velocityState);
 219     AttitudeStateData attitudeState;
 220     AttitudeStateGet(&attitudeState);
 221     statusGround.State.Yaw = attitudeState.Yaw;
 222     statusGround.State.Velocity = sqrtf(velocityState.North * velocityState.North + velocityState.East * velocityState.East);
 223
 224     StabilizationDesiredData stabDesired;
 225     StabilizationDesiredGet(&stabDesired);
 226
 227     // estimate a north/east command value to control the velocity error.
 228     // ThrustLimits.Max(+-) limits the range.  Think of this as a command unit vector
 229     // of the ultimate direction to reduce lateral error and achieve the target direction (desired angle).
 230     float northCommand, eastCommand;
 231     controlNE.GetNECommand(&northCommand, &eastCommand);
 232
 233     // Get current vehicle orientation
 234     float angle_radians  = DEG2RAD(attitudeState.Yaw); // (+-pi)
 235     float cos_angle      = cosf(angle_radians);
 236     float sine_angle     = sinf(angle_radians);
 237
 238     float courseCommand  = 0.0f;
 239     float speedCommand   = 0.0f;
 240     float lateralCommand = boundf(-northCommand * sine_angle + eastCommand * cos_angle, -groundSettings->ThrustLimit.Max, groundSettings->ThrustLimit.Max);
 241     float forwardCommand = boundf(northCommand * cos_angle + eastCommand * sine_angle, -groundSettings->ThrustLimit.Max, groundSettings->ThrustLimit.Max);
 242     // +ve facing correct direction, lateral command should just correct angle,
 243     if (forwardCommand > 0.0f) {
 244         // if +ve forward command, -+ lateralCommand drives steering to manage lateral error and angular error
 245
 246         courseCommand = boundf(lateralCommand, -1.0f, 1.0f);
 247         speedCommand  = boundf(forwardCommand, groundSettings->ThrustLimit.SlowForward, groundSettings->ThrustLimit.Max);
 248
 249         // reinstate max thrust
 250         controlNE.UpdateCommandParameters(-groundSettings->ThrustLimit.Max, groundSettings->ThrustLimit.Max, groundSettings->VelocityFeedForward);
 251
 252         statusGround.ControlState = STATUSGROUNDDRIVE_CONTROLSTATE_ONTRACK;
 253     } else {
 254         // -ve facing opposite direction, lateral command irrelevant, need to turn to change direction and do so slowly.
 255
 256         // Reduce steering angle based on current velocity
 257         float steeringReductionFactor = 1.0f;
 258         if (stabDesired.Thrust > 0.3f) {
 259             steeringReductionFactor = (groundSettings->HorizontalVelMax - statusGround.State.Velocity) / groundSettings->HorizontalVelMax;
 260             steeringReductionFactor = boundf(steeringReductionFactor, 0.05f, 1.0f);
 261         }
 262
 263         // should we turn left or right?
 264         if (lateralCommand >= 0.1f) {
 265             courseCommand = 1.0f * steeringReductionFactor;
 266             statusGround.ControlState = STATUSGROUNDDRIVE_CONTROLSTATE_TURNAROUNDRIGHT;
 267         } else {
 268             courseCommand = -1.0f * steeringReductionFactor;
 269             statusGround.ControlState = STATUSGROUNDDRIVE_CONTROLSTATE_TURNAROUNDLEFT;
 270         }
 271
 272         // Impose limits to slow down.
 273         controlNE.UpdateCommandParameters(-groundSettings->ThrustLimit.SlowForward, groundSettings->ThrustLimit.SlowForward, 0.0f);
 274
 275         speedCommand = groundSettings->ThrustLimit.SlowForward;
 276     }
 277
 278     stabDesired.Roll   = 0.0f;
 279     stabDesired.Pitch  = 0.0f;
 280     stabDesired.Yaw    = courseCommand;
 281
 282     // Mix yaw into thrust limit TODO
 283     stabDesired.Thrust = boundf(speedCommand, groundSettings->ThrustLimit.Min, groundSettings->ThrustLimit.Max);
 284
 285     stabDesired.StabilizationMode.Roll   = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
 286     stabDesired.StabilizationMode.Pitch  = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
 287     stabDesired.StabilizationMode.Yaw    = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
 288     stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
 289     StabilizationDesiredSet(&stabDesired);
 290
 291     statusGround.NECommand.North       = northCommand;
 292     statusGround.NECommand.East        = eastCommand;
 293     statusGround.State.Thrust          = stabDesired.Thrust;
 294     statusGround.BodyCommand.Forward   = forwardCommand;
 295     statusGround.BodyCommand.Right     = lateralCommand;
 296     statusGround.ControlCommand.Course = courseCommand;
 297     statusGround.ControlCommand.Speed  = speedCommand;
 298     StatusGroundDriveSet(&statusGround);
 299
 300     return 1;
 301 }