OP-1350 add a TakeOffLocation Handler to manualcontrol

flight/modules/ManualControl/inc/manualcontrol.h

@@ -75,6 +75,13 @@ void pathFollowerHandler(bool newinit);
  */
 void pathPlannerHandler(bool newinit);
 
+/**
+ * @brief Handler to setup takeofflocation on arming. it is set up during Arming
+ * @input: NONE:
+ * @output: NONE
+ */
+void takeOffLocationHandler(bool newinit);
+
 /*
  * These are assumptions we make in the flight code about the order of settings and their consistency between
  * objects.  Please keep this synchronized to the UAVObjects

flight/modules/ManualControl/manualcontrol.c

@@ -134,6 +134,9 @@ int32_t ManualControlStart()
     // Make sure unarmed on power up
     armHandler(true);
 
+#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
+    takeOffLocationHandler(true);
+#endif
     // Start main task
     PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle);
 
@@ -167,7 +170,9 @@ static void manualControlTask(void)
 {
     // Process Arming
     armHandler(false);
-
+#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
+    takeOffLocationHandler(false);
+#endif
     // Process flight mode
     FlightStatusData flightStatus;
 

flight/modules/ManualControl/takeofflocationhandler.c

@@ -0,0 +1,127 @@
+/**
+ ******************************************************************************
+ *
+ * @file       takeofflocationhandler.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
+ * @brief      handles TakeOffLocation
+ *             --
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+#include "inc/manualcontrol.h"
+#include <stdint.h>
+#include <flightstatus.h>
+#include <takeofflocation.h>
+#include <positionstate.h>
+
+// Private constants
+
+// Private types
+typedef enum {
+    HANDLER_STATUS_UNSET,
+    HANDLER_STATUS_PENDING,
+    HANDLER_STATUS_SET,
+} HandlerStatus_t;
+
+// Private variables
+static HandlerStatus_t handlerStatus;
+static const uint8_t TakeOffStatusInvalid = TAKEOFFLOCATION_STATUS_INVALID;
+static const uint8_t TakeOffStatusValid   = TAKEOFFLOCATION_STATUS_VALID;
+// Private functions
+static void SetTakeOffLocation();
+
+/**
+ * Handles TakeOffPosition location setup
+ * @param newinit
+ */
+void takeOffLocationHandler(bool newinit)
+{
+    if (newinit) {
+        // check whether there is a preset/valid takeoff location
+        uint8_t mode;
+        uint8_t status;
+        TakeOffLocationModeGet(&mode);
+        TakeOffLocationStatusGet(&status);
+        if (mode == TAKEOFFLOCATION_MODE_PRESET && status == TAKEOFFLOCATION_STATUS_VALID) {
+            handlerStatus = HANDLER_STATUS_SET;
+        } else {
+            handlerStatus = HANDLER_STATUS_UNSET;
+        }
+        return;
+    }
+
+    uint8_t armed;
+    FlightStatusArmedGet(&armed);
+
+    // Location already acquired/preset
+    if (armed == FLIGHTSTATUS_ARMED_ARMED && handlerStatus == HANDLER_STATUS_SET) {
+        return;
+    }
+
+    switch (armed) {
+    case FLIGHTSTATUS_ARMED_ARMED:
+        if (handlerStatus == HANDLER_STATUS_UNSET) {
+            // this is just a safety "net", should for any reason the ARMING status is skipped
+            SetTakeOffLocation();
+        } else if (handlerStatus == HANDLER_STATUS_PENDING) {
+            // confirms a "pending" TakeOffPosition
+            uint8_t newStatus = TAKEOFFLOCATION_STATUS_VALID;
+            TakeOffLocationStatusSet(&newStatus);
+            handlerStatus = HANDLER_STATUS_SET;
+        }
+        break;
+
+    case FLIGHTSTATUS_ARMED_DISARMED:
+        // unset if location is to be acquired at each arming
+        if (handlerStatus == HANDLER_STATUS_SET) {
+            uint8_t mode;
+            TakeOffLocationModeGet(&mode);
+            if (mode == TAKEOFFLOCATION_MODE_ARMINGLOCATION) {
+                handlerStatus = HANDLER_STATUS_UNSET;
+                uint8_t newStatus = TAKEOFFLOCATION_STATUS_INVALID;
+                TakeOffLocationStatusSet(&newStatus);
+            }
+            // Clear a previous "pending" flag
+        } else if (handlerStatus == HANDLER_STATUS_PENDING) {
+            handlerStatus = HANDLER_STATUS_UNSET;
+        }
+        break;
+
+    case FLIGHTSTATUS_ARMED_ARMING:
+        if (handlerStatus == HANDLER_STATUS_UNSET) {
+            SetTakeOffLocation();
+        }
+        break;
+    }
+}
+
+/**
+ * Retrieve TakeOffLocation from current PositionStatus
+ */
+void SetTakeOffLocation()
+{
+    TakeOffLocationData takeOffLocation;
+
+    TakeOffLocationGet(&takeOffLocation);
+    PositionStateData positionState;
+    PositionStateGet(&positionState);
+    takeOffLocation.North = positionState.North;
+    takeOffLocation.East  = positionState.East;
+    takeOffLocation.Down  = positionState.Down;
+    handlerStatus = HANDLER_STATUS_PENDING;
+}