flight/modules/ManualControl/takeofflocationhandler.c

   1 /**
   2  ******************************************************************************
   3  *
   4  * @file       takeofflocationhandler.c
   5  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
   6  * @brief      handles TakeOffLocation
   7  *             --
   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 #include "inc/manualcontrol.h"
  27 #include <stdint.h>
  28 #include <flightstatus.h>
  29 #include <takeofflocation.h>
  30 #include <positionstate.h>
  31
  32 // Private constants
  33
  34 // Private types
  35
  36 // Private variables
  37 static bool locationSet;
  38 // Private functions
  39 static void SetTakeOffLocation();
  40
  41 void takeOffLocationHandlerInit()
  42 {
  43     TakeOffLocationInitialize();
  44     // check whether there is a preset/valid takeoff location
  45     TakeOffLocationModeOptions mode;
  46     TakeOffLocationStatusOptions status;
  47     TakeOffLocationModeGet(&mode);
  48     TakeOffLocationStatusGet(&status);
  49     // preset with invalid location will actually behave like FirstTakeoff
  50     if (mode == TAKEOFFLOCATION_MODE_PRESET && status == TAKEOFFLOCATION_STATUS_VALID) {
  51         locationSet = true;
  52     } else {
  53         locationSet = false;
  54         status = TAKEOFFLOCATION_STATUS_INVALID;
  55         TakeOffLocationStatusSet(&status);
  56     }
  57 }
  58 /**
  59  * Handles TakeOffPosition location setup
  60  * @param newinit
  61  */
  62 void takeOffLocationHandler()
  63 {
  64     FlightStatusArmedOptions armed;
  65     TakeOffLocationStatusOptions status;
  66
  67     FlightStatusArmedGet(&armed);
  68
  69     // Location already acquired/preset
  70     if (armed == FLIGHTSTATUS_ARMED_ARMED && locationSet) {
  71         return;
  72     }
  73
  74     TakeOffLocationStatusGet(&status);
  75
  76     switch (armed) {
  77     case FLIGHTSTATUS_ARMED_ARMING:
  78     case FLIGHTSTATUS_ARMED_ARMED:
  79         if (!locationSet || status != TAKEOFFLOCATION_STATUS_VALID) {
  80             TakeOffLocationModeOptions mode;
  81             TakeOffLocationModeGet(&mode);
  82
  83             if ((mode != TAKEOFFLOCATION_MODE_PRESET) || (status == TAKEOFFLOCATION_STATUS_INVALID)) {
  84                 SetTakeOffLocation();
  85             } else {
  86                 locationSet = true;
  87             }
  88         }
  89         break;
  90
  91     case FLIGHTSTATUS_ARMED_DISARMED:
  92         // unset if location is to be acquired at each arming
  93         if (locationSet) {
  94             TakeOffLocationModeOptions mode;
  95             TakeOffLocationModeGet(&mode);
  96             if (mode == TAKEOFFLOCATION_MODE_ARMINGLOCATION) {
  97                 locationSet = false;
  98                 status = TAKEOFFLOCATION_STATUS_INVALID;
  99                 TakeOffLocationStatusSet(&status);
 100             }
 101         }
 102         break;
 103     }
 104 }
 105
 106 /**
 107  * Retrieve TakeOffLocation from current PositionStatus
 108  */
 109 void SetTakeOffLocation()
 110 {
 111     TakeOffLocationData takeOffLocation;
 112
 113     TakeOffLocationGet(&takeOffLocation);
 114
 115     PositionStateData positionState;
 116     PositionStateGet(&positionState);
 117
 118     takeOffLocation.North  = positionState.North;
 119     takeOffLocation.East   = positionState.East;
 120     takeOffLocation.Down   = positionState.Down;
 121     takeOffLocation.Status = TAKEOFFLOCATION_STATUS_VALID;
 122
 123     TakeOffLocationSet(&takeOffLocation);
 124     locationSet = true;
 125 }