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