Merge branch 'next' into laurent/OP-1337_French_translations_updates

flight/libraries/inc/CoordinateConversions.h

@@ -75,5 +75,27 @@ void quat_inverse(float q[4]);
 void quat_copy(const float q[4], float qnew[4]);
 void quat_mult(const float q1[4], const float q2[4], float qout[4]);
 void rot_mult(float R[3][3], const float vec[3], float vec_out[3]);
+/**
+ * matrix_mult_3x3f - perform a multiplication between two 3x3 float matrices
+ * result = a*b
+ * @param a
+ * @param b
+ * @param result
+ */
+inline void matrix_mult_3x3f(float a[3][3], float b[3][3], float result[3][3])
+{
+    result[0][0] = a[0][0] * b[0][0] + a[1][0] * b[0][1] + a[2][0] * b[0][2];
+    result[0][1] = a[0][1] * b[0][0] + a[1][1] * b[0][1] + a[2][1] * b[0][2];
+    result[0][2] = a[0][2] * b[0][0] + a[1][2] * b[0][1] + a[2][2] * b[0][2];
+
+    result[1][0] = a[0][0] * b[1][0] + a[1][0] * b[1][1] + a[2][0] * b[1][2];
+    result[1][1] = a[0][1] * b[1][0] + a[1][1] * b[1][1] + a[2][1] * b[1][2];
+    result[1][2] = a[0][2] * b[1][0] + a[1][2] * b[1][1] + a[2][2] * b[1][2];
+
+    result[2][0] = a[0][0] * b[2][0] + a[1][0] * b[2][1] + a[2][0] * b[2][2];
+    result[2][1] = a[0][1] * b[2][0] + a[1][1] * b[2][1] + a[2][1] * b[2][2];
+    result[2][2] = a[0][2] * b[2][0] + a[1][2] * b[2][1] + a[2][2] * b[2][2];
+}
+
 
 #endif // COORDINATECONVERSIONS_H_

flight/libraries/inc/plans.h

@@ -0,0 +1,69 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotLibraries OpenPilot Libraries
+ * @{
+ * @addtogroup Navigation
+ * @brief setups RTH/PH and other pathfollower/pathplanner status
+ * @{
+ *
+ * @file       plan.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
+ *
+ * @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
+ */
+
+#ifndef PLANS_H_
+#define PLANS_H_
+#include <pios_math.h>
+
+/** \page standard Plans
+   How to use this library
+   \li Call plan_initialize() to init all needed struct and uavos at startup.<br>
+   It may be safely called more than once.<br>
+
+   \li Functions called plan_setup_* needs to be called once, every time the requested function is engaged.<br>
+
+   \li Functions called plan_run_* are to be periodically called while the requested mode is engaged.<br>
+ */
+
+/**
+ * @brief initialize UAVOs and structs used by this library
+ */
+void plan_initialize();
+
+/**
+ * @brief setup pathplanner/pathfollower for positionhold
+ */
+void plan_setup_positionHold();
+
+/**
+ * @brief setup pathplanner/pathfollower for return to base
+ */
+void plan_setup_returnToBase();
+
+/**
+ * @brief setup pathplanner/pathfollower for land
+ */
+void plan_setup_land();
+
+/**
+ * @brief execute land
+ */
+void plan_run_land();
+#endif /* PLANS_H_ */

flight/libraries/plans.c

@@ -0,0 +1,130 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotLibraries OpenPilot Libraries
+ * @{
+ * @addtogroup Navigation
+ * @brief setups RTH/PH and other pathfollower/pathplanner status
+ * @{
+ *
+ * @file       plan.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
+ *
+ * @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 <plans.h>
+#include <openpilot.h>
+#include <attitudesettings.h>
+#include <takeofflocation.h>
+#include <pathdesired.h>
+#include <positionstate.h>
+#include <flightmodesettings.h>
+
+/**
+ * @brief initialize UAVOs and structs used by this library
+ */
+void plan_initialize()
+{
+    TakeOffLocationInitialize();
+    PositionStateInitialize();
+    PathDesiredInitialize();
+    FlightModeSettingsInitialize();
+}
+
+/**
+ * @brief setup pathplanner/pathfollower for positionhold
+ */
+void plan_setup_positionHold()
+{
+    PositionStateData positionState;
+
+    PositionStateGet(&positionState);
+    PathDesiredData pathDesired;
+    PathDesiredGet(&pathDesired);
+
+    pathDesired.Start.North      = positionState.North;
+    pathDesired.Start.East       = positionState.East;
+    pathDesired.Start.Down       = positionState.Down;
+    pathDesired.End.North        = positionState.North;
+    pathDesired.End.East         = positionState.East;
+    pathDesired.End.Down         = positionState.Down;
+    pathDesired.StartingVelocity = 1;
+    pathDesired.EndingVelocity   = 0;
+    pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
+
+    PathDesiredSet(&pathDesired);
+}
+
+/**
+ * @brief setup pathplanner/pathfollower for return to base
+ */
+void plan_setup_returnToBase()
+{
+    // Simple Return To Base mode - keep altitude the same applying configured delta, fly to takeoff position
+    float positionStateDown;
+
+    PositionStateDownGet(&positionStateDown);
+
+    PathDesiredData pathDesired;
+    PathDesiredGet(&pathDesired);
+
+    TakeOffLocationData takeoffLocation;
+    TakeOffLocationGet(&takeoffLocation);
+
+    // TODO: right now VTOLPF does fly straight to destination altitude.
+    // For a safer RTB destination altitude will be the higher between takeofflocation and current position (corrected with safety margin)
+
+    float destDown;
+    FlightModeSettingsReturnToBaseAltitudeOffsetGet(&destDown);
+    destDown = MIN(positionStateDown, takeoffLocation.Down) - destDown;
+
+    pathDesired.Start.North      = takeoffLocation.North;
+    pathDesired.Start.East       = takeoffLocation.East;
+    pathDesired.Start.Down       = destDown;
+
+    pathDesired.End.North        = takeoffLocation.North;
+    pathDesired.End.East         = takeoffLocation.East;
+    pathDesired.End.Down         = destDown;
+
+    pathDesired.StartingVelocity = 1;
+    pathDesired.EndingVelocity   = 0;
+    pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
+
+    PathDesiredSet(&pathDesired);
+}
+
+void plan_setup_land()
+{
+    plan_setup_positionHold();
+}
+
+/**
+ * @brief execute land
+ */
+void plan_run_land()
+{
+    PathDesiredEndData pathDesiredEnd;
+
+    PathDesiredEndGet(&pathDesiredEnd);
+
+    PositionStateDownGet(&pathDesiredEnd.Down);
+    pathDesiredEnd.Down += 5;
+
+    PathDesiredEndSet(&pathDesiredEnd);
+}

flight/modules/FixedWingPathFollower/fixedwingpathfollower.c

@@ -63,6 +63,7 @@
 #include "taskinfo.h"
 #include <pios_struct_helper.h>
 
+#include "sin_lookup.h"
 #include "paths.h"
 #include "CoordinateConversions.h"
 
@@ -85,6 +86,7 @@ static void updatePathVelocity();
 static uint8_t updateFixedDesiredAttitude();
 static void updateFixedAttitude();
 static void airspeedStateUpdatedCb(UAVObjEvent *ev);
+static bool correctCourse(float *C, float *V, float *F, float s);
 
 /**
  * Initialise the module, called on startup
@@ -125,17 +127,17 @@ int32_t FixedWingPathFollowerInitialize()
 }
 MODULE_INITCALL(FixedWingPathFollowerInitialize, FixedWingPathFollowerStart);
 
-static float northVelIntegral = 0;
+static float northVelIntegral = 0.0f;
-static float eastVelIntegral  = 0;
+static float eastVelIntegral  = 0.0f;
-static float downVelIntegral  = 0;
+static float downVelIntegral  = 0.0f;
 
-static float courseIntegral   = 0;
+static float courseIntegral   = 0.0f;
-static float speedIntegral    = 0;
+static float speedIntegral    = 0.0f;
-static float powerIntegral    = 0;
+static float powerIntegral    = 0.0f;
-static float airspeedErrorInt = 0;
+static float airspeedErrorInt = 0.0f;
 
 // correct speed by measured airspeed
-static float indicatedAirspeedStateBias = 0;
+static float indicatedAirspeedStateBias = 0.0f;
 
 /**
  * Module thread, should not return.
@@ -225,12 +227,12 @@ static void pathfollowerTask(__attribute__((unused)) void *parameters)
             }
         } else {
             // Be cleaner and get rid of global variables
-            northVelIntegral = 0;
+            northVelIntegral = 0.0f;
-            eastVelIntegral  = 0;
+            eastVelIntegral  = 0.0f;
-            downVelIntegral  = 0;
+            downVelIntegral  = 0.0f;
-            courseIntegral   = 0;
+            courseIntegral   = 0.0f;
-            speedIntegral    = 0;
+            speedIntegral    = 0.0f;
-            powerIntegral    = 0;
+            powerIntegral    = 0.0f;
         }
         PathStatusSet(&pathStatus);
     }
@@ -276,14 +278,14 @@ static void updatePathVelocity()
     case PATHDESIRED_MODE_DRIVEVECTOR:
     default:
         groundspeed = pathDesired.StartingVelocity + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) *
-                      boundf(progress.fractional_progress, 0, 1);
+                      boundf(progress.fractional_progress, 0.0f, 1.0f);
         altitudeSetpoint = pathDesired.Start.Down + (pathDesired.End.Down - pathDesired.Start.Down) *
-                           boundf(progress.fractional_progress, 0, 1);
+                           boundf(progress.fractional_progress, 0.0f, 1.0f);
         break;
     }
     // make sure groundspeed is not zero
-    if (groundspeed < 1e-2f) {
+    if (groundspeed < 1e-6f) {
-        groundspeed = 1e-2f;
+        groundspeed = 1e-6f;
     }
 
     // calculate velocity - can be zero if waypoints are too close
@@ -302,24 +304,12 @@ static void updatePathVelocity()
     // in this case the plane effectively needs to be turned around
     // indicators:
     // difference between correction_direction and velocitystate >90 degrees and
-    // difference between path_direction and velocitystate >90 degrees  ( 4th sector, facing away from eerything )
+    // difference between path_direction and velocitystate >90 degrees  ( 4th sector, facing away from everything )
     // fix: ignore correction, steer in path direction until the situation has become better (condition doesn't apply anymore)
-    float angle1 = RAD2DEG(atan2f(progress.path_direction[1], progress.path_direction[0]) - atan2f(velocityState.East, velocityState.North));
+    if ( // calculating angles < 90 degrees through dot products
-    float angle2 = RAD2DEG(atan2f(progress.correction_direction[1], progress.correction_direction[0]) - atan2f(velocityState.East, velocityState.North));
+        ((progress.path_direction[0] * velocityState.North + progress.path_direction[1] * velocityState.East) < 0.0f) &&
-    if (angle1 < -180.0f) {
+        ((progress.correction_direction[0] * velocityState.North + progress.correction_direction[1] * velocityState.East) < 0.0f)) {
-        angle1 += 360.0f;
+        error_speed = 0.0f;
-    }
-    if (angle1 > 180.0f) {
-        angle1 -= 360.0f;
-    }
-    if (angle2 < -180.0f) {
-        angle2 += 360.0f;
-    }
-    if (angle2 > 180.0f) {
-        angle2 -= 360.0f;
-    }
-    if (fabsf(angle1) >= 90.0f && fabsf(angle2) >= 90.0f) {
-        error_speed = 0;
     }
 
     // calculate correction - can also be zero if correction vector is 0 or no error present
@@ -328,14 +318,16 @@ static void updatePathVelocity()
 
     // scale to correct length
     float l = sqrtf(velocityDesired.North * velocityDesired.North + velocityDesired.East * velocityDesired.East);
-    velocityDesired.North *= groundspeed / l;
+    if (l > 1e-9f) {
-    velocityDesired.East  *= groundspeed / l;
+        velocityDesired.North *= groundspeed / l;
+        velocityDesired.East  *= groundspeed / l;
+    }
 
     float downError = altitudeSetpoint - positionState.Down;
-    velocityDesired.Down   = downError * fixedwingpathfollowerSettings.VerticalPosP;
+    velocityDesired.Down = downError * fixedwingpathfollowerSettings.VerticalPosP;
 
     // update pathstatus
-    pathStatus.error = progress.error;
+    pathStatus.error     = progress.error;
     pathStatus.fractional_progress = progress.fractional_progress;
 
     VelocityDesiredSet(&velocityDesired);
@@ -384,8 +376,7 @@ static uint8_t updateFixedDesiredAttitude()
     AirspeedStateData airspeedState;
     SystemSettingsData systemSettings;
 
-    float groundspeedState;
+    float groundspeedProjection;
-    float groundspeedDesired;
     float indicatedAirspeedState;
     float indicatedAirspeedDesired;
     float airspeedError;
@@ -396,10 +387,9 @@ static uint8_t updateFixedDesiredAttitude()
     float descentspeedError;
     float powerCommand;
 
-    float bearing;
+    float airspeedVector[2];
-    float heading;
+    float fluidMovement[2];
-    float headingError;
+    float courseComponent[2];
-    float course;
     float courseError;
     float courseCommand;
 
@@ -413,24 +403,54 @@ static uint8_t updateFixedDesiredAttitude()
     AirspeedStateGet(&airspeedState);
     SystemSettingsGet(&systemSettings);
 
-
     /**
-     * Compute speed error (required for thrust and pitch)
+     * Compute speed error and course
      */
+    // missing sensors for airspeed-direction we have to assume within
+    // reasonable error that measured airspeed is actually the airspeed
+    // component in forward pointing direction
+    // airspeedVector is normalized
+    airspeedVector[0]     = cos_lookup_deg(attitudeState.Yaw);
+    airspeedVector[1]     = sin_lookup_deg(attitudeState.Yaw);
 
-    // Current ground speed
+    // current ground speed projected in forward direction
-    groundspeedState = sqrtf(velocityState.East * velocityState.East + velocityState.North * velocityState.North);
+    groundspeedProjection = velocityState.North * airspeedVector[0] + velocityState.East * airspeedVector[1];
-    // note that airspeedStateBias is ( calibratedAirspeed - groundSpeed ) at the time of measurement,
+
-    // but thanks to accelerometers,  groundspeed reacts faster to changes in direction
+    // note that airspeedStateBias is ( calibratedAirspeed - groundspeedProjection ) at the time of measurement,
+    // but thanks to accelerometers,  groundspeedProjection reacts faster to changes in direction
     // than airspeed and gps sensors alone
-    indicatedAirspeedState   = groundspeedState + indicatedAirspeedStateBias;
+    indicatedAirspeedState = groundspeedProjection + indicatedAirspeedStateBias;
 
-    // Desired ground speed
+    // fluidMovement is a vector describing the aproximate movement vector of
-    groundspeedDesired       = sqrtf(velocityDesired.North * velocityDesired.North + velocityDesired.East * velocityDesired.East);
+    // the surrounding fluid in 2d space (aka wind vector)
-    indicatedAirspeedDesired = boundf(groundspeedDesired + indicatedAirspeedStateBias,
+    fluidMovement[0] = velocityState.North - (indicatedAirspeedState * airspeedVector[0]);
+    fluidMovement[1] = velocityState.East - (indicatedAirspeedState * airspeedVector[1]);
+
+    // calculate the movement vector we need to fly to reach velocityDesired -
+    // taking fluidMovement into account
+    courseComponent[0] = velocityDesired.North - fluidMovement[0];
+    courseComponent[1] = velocityDesired.East - fluidMovement[1];
+
+    indicatedAirspeedDesired = boundf(sqrtf(courseComponent[0] * courseComponent[0] + courseComponent[1] * courseComponent[1]),
                                       fixedwingpathfollowerSettings.HorizontalVelMin,
                                       fixedwingpathfollowerSettings.HorizontalVelMax);
 
+    // if we could fly at arbitrary speeds, we'd just have to move towards the
+    // courseComponent vector as previously calculated and we'd be fine
+    // unfortunately however we are bound by min and max air speed limits, so
+    // we need to recalculate the correct course to meet at least the
+    // velocityDesired vector direction at our current speed
+    // this overwrites courseComponent
+    bool valid = correctCourse(courseComponent, (float *)&velocityDesired.North, fluidMovement, indicatedAirspeedDesired);
+
+    // Error condition: wind speed too high, we can't go where we want anymore
+    fixedwingpathfollowerStatus.Errors.Wind = 0;
+    if ((!valid) &&
+        fixedwingpathfollowerSettings.Safetymargins.Wind > 0.5f) { // alarm switched on
+        fixedwingpathfollowerStatus.Errors.Wind = 1;
+        result = 0;
+    }
+
     // Airspeed error
     airspeedError = indicatedAirspeedDesired - indicatedAirspeedState;
 
@@ -461,23 +481,18 @@ static uint8_t updateFixedDesiredAttitude()
         result = 0;
     }
 
-    if (indicatedAirspeedState < 1e-6f) {
-        // prevent division by zero, abort without controlling anything. This guidance mode is not suited for takeoff or touchdown, or handling stationary planes
-        // also we cannot handle planes flying backwards, lets just wait until the nose drops
-        fixedwingpathfollowerStatus.Errors.Lowspeed = 1;
-        return 0;
-    }
-
     /**
      * Compute desired thrust command
      */
     // compute saturated integral error thrust response. Make integral leaky for better performance. Approximately 30s time constant.
-    if (fixedwingpathfollowerSettings.PowerPI.Ki > 0) {
+    if (fixedwingpathfollowerSettings.PowerPI.Ki > 0.0f) {
         powerIntegral = boundf(powerIntegral + -descentspeedError * dT,
                                -fixedwingpathfollowerSettings.PowerPI.ILimit / fixedwingpathfollowerSettings.PowerPI.Ki,
                                fixedwingpathfollowerSettings.PowerPI.ILimit / fixedwingpathfollowerSettings.PowerPI.Ki
                                ) * (1.0f - 1.0f / (1.0f + 30.0f / dT));
-    } else { powerIntegral = 0; }
+    } else {
+        powerIntegral = 0.0f;
+    }
 
     // Compute the cross feed from vertical speed to pitch, with saturation
     float speedErrorToPowerCommandComponent = boundf(
@@ -504,9 +519,9 @@ static uint8_t updateFixedDesiredAttitude()
     // Error condition: plane cannot hold altitude at current speed.
     fixedwingpathfollowerStatus.Errors.Lowpower = 0;
     if (fixedwingpathfollowerSettings.ThrustLimit.Neutral + powerCommand >= fixedwingpathfollowerSettings.ThrustLimit.Max && // thrust at maximum
-        velocityState.Down > 0 && // we ARE going down
+        velocityState.Down > 0.0f && // we ARE going down
-        descentspeedDesired < 0 && // we WANT to go up
+        descentspeedDesired < 0.0f && // we WANT to go up
-        airspeedError > 0 && // we are too slow already
+        airspeedError > 0.0f && // we are too slow already
         fixedwingpathfollowerSettings.Safetymargins.Lowpower > 0.5f) { // alarm switched on
         fixedwingpathfollowerStatus.Errors.Lowpower = 1;
         result = 0;
@@ -514,9 +529,9 @@ static uint8_t updateFixedDesiredAttitude()
     // Error condition: plane keeps climbing despite minimum thrust (opposite of above)
     fixedwingpathfollowerStatus.Errors.Highpower = 0;
     if (fixedwingpathfollowerSettings.ThrustLimit.Neutral + powerCommand <= fixedwingpathfollowerSettings.ThrustLimit.Min && // thrust at minimum
-        velocityState.Down < 0 && // we ARE going up
+        velocityState.Down < 0.0f && // we ARE going up
-        descentspeedDesired > 0 && // we WANT to go down
+        descentspeedDesired > 0.0f && // we WANT to go down
-        airspeedError < 0 && // we are too fast already
+        airspeedError < 0.0f && // we are too fast already
         fixedwingpathfollowerSettings.Safetymargins.Highpower > 0.5f) { // alarm switched on
         fixedwingpathfollowerStatus.Errors.Highpower = 1;
         result = 0;
@@ -526,7 +541,6 @@ static uint8_t updateFixedDesiredAttitude()
     /**
      * Compute desired pitch command
      */
-
     if (fixedwingpathfollowerSettings.SpeedPI.Ki > 0) {
         // Integrate with saturation
         airspeedErrorInt = boundf(airspeedErrorInt + airspeedError * dT,
@@ -556,48 +570,18 @@ static uint8_t updateFixedDesiredAttitude()
     // Error condition: high speed dive
     fixedwingpathfollowerStatus.Errors.Pitchcontrol = 0;
     if (fixedwingpathfollowerSettings.PitchLimit.Neutral + pitchCommand >= fixedwingpathfollowerSettings.PitchLimit.Max && // pitch demand is full up
-        velocityState.Down > 0 && // we ARE going down
+        velocityState.Down > 0.0f && // we ARE going down
-        descentspeedDesired < 0 && // we WANT to go up
+        descentspeedDesired < 0.0f && // we WANT to go up
-        airspeedError < 0 && // we are too fast already
+        airspeedError < 0.0f && // we are too fast already
         fixedwingpathfollowerSettings.Safetymargins.Pitchcontrol > 0.5f) { // alarm switched on
         fixedwingpathfollowerStatus.Errors.Pitchcontrol = 1;
         result = 0;
     }
 
-    /**
-     * Calculate where we are heading and why (wind issues)
-     */
-    bearing = attitudeState.Yaw;
-    heading = RAD2DEG(atan2f(velocityState.East, velocityState.North));
-    headingError = heading - bearing;
-    if (headingError < -180.0f) {
-        headingError += 360.0f;
-    }
-    if (headingError > 180.0f) {
-        headingError -= 360.0f;
-    }
-    // Error condition: wind speed is higher than airspeed. We are forced backwards!
-    fixedwingpathfollowerStatus.Errors.Wind = 0;
-    if ((headingError > fixedwingpathfollowerSettings.Safetymargins.Wind ||
-         headingError < -fixedwingpathfollowerSettings.Safetymargins.Wind) &&
-        fixedwingpathfollowerSettings.Safetymargins.Highpower > 0.5f) { // alarm switched on
-        // we are flying backwards
-        fixedwingpathfollowerStatus.Errors.Wind = 1;
-        result = 0;
-    }
-
     /**
      * Compute desired roll command
      */
-    if (groundspeedDesired > 1e-6f) {
+    courseError = RAD2DEG(atan2f(courseComponent[1], courseComponent[0])) - attitudeState.Yaw;
-        course = RAD2DEG(atan2f(velocityDesired.East, velocityDesired.North));
-
-        courseError = course - heading;
-    } else {
-        // if we are not supposed to move, run in a circle
-        courseError = -90.0f;
-        result = 0;
-    }
 
     if (courseError < -180.0f) {
         courseError += 360.0f;
@@ -606,6 +590,19 @@ static uint8_t updateFixedDesiredAttitude()
         courseError -= 360.0f;
     }
 
+    // overlap calculation. Theres a dead zone behind the craft where the
+    // counter-yawing of some craft while rolling could render a desired right
+    // turn into a desired left turn. Making the turn direction based on
+    // current roll angle keeps the plane committed to a direction once chosen
+    if (courseError < -180.0f + (fixedwingpathfollowerSettings.ReverseCourseOverlap * 0.5f)
+        && attitudeState.Roll > 0.0f) {
+        courseError += 360.0f;
+    }
+    if (courseError > 180.0f - (fixedwingpathfollowerSettings.ReverseCourseOverlap * 0.5f)
+        && attitudeState.Roll < 0.0f) {
+        courseError -= 360.0f;
+    }
+
     courseIntegral = boundf(courseIntegral + courseError * dT * fixedwingpathfollowerSettings.CoursePI.Ki,
                             -fixedwingpathfollowerSettings.CoursePI.ILimit,
                             fixedwingpathfollowerSettings.CoursePI.ILimit);
@@ -628,7 +625,7 @@ static uint8_t updateFixedDesiredAttitude()
      * Compute desired yaw command
      */
     // TODO implement raw control mode for yaw and base on Accels.Y
-    stabDesired.Yaw = 0;
+    stabDesired.Yaw = 0.0f;
 
 
     stabDesired.StabilizationMode.Roll   = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
@@ -656,10 +653,116 @@ static void airspeedStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 
     AirspeedStateGet(&airspeedState);
     VelocityStateGet(&velocityState);
-    float groundspeed = sqrtf(velocityState.East * velocityState.East + velocityState.North * velocityState.North);
+    float airspeedVector[2];
+    float yaw;
+    AttitudeStateYawGet(&yaw);
+    airspeedVector[0] = cos_lookup_deg(yaw);
+    airspeedVector[1] = sin_lookup_deg(yaw);
+    // vector projection of groundspeed on airspeed vector to handle both forward and backwards movement
+    float groundspeedProjection = velocityState.North * airspeedVector[0] + velocityState.East * airspeedVector[1];
 
-
+    indicatedAirspeedStateBias = airspeedState.CalibratedAirspeed - groundspeedProjection;
-    indicatedAirspeedStateBias = airspeedState.CalibratedAirspeed - groundspeed;
+    // note - we do fly by Indicated Airspeed (== calibrated airspeed) however
-    // note - we do fly by Indicated Airspeed (== calibrated airspeed)
+    // since airspeed is updated less often than groundspeed, we use sudden
-    // however since airspeed is updated less often than groundspeed, we use sudden changes to groundspeed to offset the airspeed by the same measurement.
+    // changes to groundspeed to offset the airspeed by the same measurement.
+    // This has a side effect that in the absence of any airspeed updates, the
+    // pathfollower will fly using groundspeed.
+}
+
+
+/**
+ * Function to calculate course vector C based on airspeed s, fluid movement F
+ * and desired movement vector V
+ * parameters in: V,F,s
+ * parameters out: C
+ * returns true if a valid solution could be found for V,F,s, false if not
+ * C will be set to a best effort attempt either way
+ */
+static bool correctCourse(float *C, float *V, float *F, float s)
+{
+    // Approach:
+    // Let Sc be a circle around origin marking possible movement vectors
+    // of the craft with airspeed s (all possible options for C)
+    // Let Vl be a line through the origin along movement vector V where fr any
+    // point v on line Vl v = k * (V / |V|) = k' * V
+    // Let Wl be a line parallel to Vl where for any point v on line Vl exists
+    // a point w on WL with w = v - F
+    // Then any intersection between circle Sc and line Wl represents course
+    // vector which would result in a movement vector
+    // V' = k * ( V / |V|) = k' * V
+    // If there is no intersection point, S is insufficient to compensate
+    // for F and we can only try to fly in direction of V (thus having wind drift
+    // but at least making progress orthogonal to wind)
+
+    s = fabsf(s);
+    float f = sqrtf(F[0] * F[0] + F[1] * F[1]);
+
+    // normalize Cn=V/|V|, |V| must be >0
+    float v = sqrtf(V[0] * V[0] + V[1] * V[1]);
+    if (v < 1e-6f) {
+        // if |V|=0, we aren't supposed to move, turn into the wind
+        // (this allows hovering)
+        C[0] = -F[0];
+        C[1] = -F[1];
+        // if desired airspeed matches fluidmovement a hover is actually
+        // intended so return true
+        return fabsf(f - s) < 1e-3f;
+    }
+    float Vn[2] = { V[0] / v, V[1] / v };
+
+    // project F on V
+    float fp    = F[0] * Vn[0] + F[1] * Vn[1];
+
+    // find component Fo of F that is orthogonal to V
+    // (which is exactly the distance between Vl and Wl)
+    float Fo[2] = { F[0] - (fp * Vn[0]), F[1] - (fp * Vn[1]) };
+    float fo2   = Fo[0] * Fo[0] + Fo[1] * Fo[1];
+
+    // find k where k * Vn = C - Fo
+    // |C|=s is the hypothenuse in any rectangular triangle formed by k * Vn and Fo
+    // so k^2 + fo^2 = s^2 (since |Vn|=1)
+    float k2 = s * s - fo2;
+    if (k2 <= -1e-3f) {
+        // there is no solution, we will be drifted off either way
+        // fallback: fly stupidly in direction of V and hope for the best
+        C[0] = V[0];
+        C[1] = V[1];
+        return false;
+    } else if (k2 <= 1e-3f) {
+        // there is exactly one solution: -Fo
+        C[0] = -Fo[0];
+        C[1] = -Fo[1];
+        return true;
+    }
+    // we have two possible solutions k positive and k negative as there are
+    // two intersection points between Wl and Sc
+    // which one is better? two criteria:
+    // 1. we MUST move in the right direction, if any k leads to -v its invalid
+    // 2. we should minimize the speed error
+    float k     = sqrt(k2);
+    float C1[2] = { -k * Vn[0] - Fo[0], -k * Vn[1] - Fo[1] };
+    float C2[2] = { k *Vn[0] - Fo[0], k * Vn[1] - Fo[1] };
+    // project C+F on Vn to find signed resulting movement vector length
+    float vp1   = (C1[0] + F[0]) * Vn[0] + (C1[1] + F[1]) * Vn[1];
+    float vp2   = (C2[0] + F[0]) * Vn[0] + (C2[1] + F[1]) * Vn[1];
+    if (vp1 >= 0.0f && fabsf(v - vp1) < fabsf(v - vp2)) {
+        // in this case the angle between course and resulting movement vector
+        // is greater than 90 degrees - so we actually fly backwards
+        C[0] = C1[0];
+        C[1] = C1[1];
+        return true;
+    }
+    C[0] = C2[0];
+    C[1] = C2[1];
+    if (vp2 >= 0.0f) {
+        // in this case the angle between course and movement vector is less than
+        // 90 degrees, but we do move in the right direction
+        return true;
+    } else {
+        // in this case we actually get driven in the opposite direction of V
+        // with both solutions for C
+        // this might be reached in headwind stronger than maximum allowed
+        // airspeed.
+        return false;
+    }
 }

flight/modules/ManualControl/inc/manualcontrol.h

@@ -75,6 +75,18 @@ 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();
+
+/**
+ * @brief Initialize TakeoffLocationHanlder
+ */
+void takeOffLocationHandlerInit();
+
 /*
  * 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
+    takeOffLocationHandlerInit();
+#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();
+#endif
     // Process flight mode
     FlightStatusData flightStatus;
 

flight/modules/ManualControl/pathfollowerhandler.c

@@ -35,8 +35,9 @@
 #include <positionstate.h>
 #include <flightmodesettings.h>
 
-
 #if defined(REVOLUTION)
+#include <plans.h>
+
 // Private constants
 
 // Private types
@@ -51,47 +52,30 @@
 void pathFollowerHandler(bool newinit)
 {
     if (newinit) {
-        PathDesiredInitialize();
+        plan_initialize();
-        PositionStateInitialize();
     }
 
-    FlightStatusData flightStatus;
+    uint8_t flightMode;
-    FlightStatusGet(&flightStatus);
+    FlightStatusFlightModeGet(&flightMode);
 
     if (newinit) {
         // After not being in this mode for a while init at current height
-        PositionStateData positionState;
+        switch (flightMode) {
-        PositionStateGet(&positionState);
-        FlightModeSettingsData settings;
-        FlightModeSettingsGet(&settings);
-        PathDesiredData pathDesired;
-        PathDesiredGet(&pathDesired);
-        switch (flightStatus.FlightMode) {
         case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE:
-            // Simple Return To Base mode - keep altitude the same, fly to home position
+            plan_setup_returnToBase();
-
-
-            pathDesired.Start.North      = 0;
-            pathDesired.Start.East       = 0;
-            pathDesired.Start.Down       = positionState.Down - settings.ReturnToHomeAltitudeOffset;
-            pathDesired.End.North        = 0;
-            pathDesired.End.East         = 0;
-            pathDesired.End.Down         = positionState.Down - settings.ReturnToHomeAltitudeOffset;
-            pathDesired.StartingVelocity = 1;
-            pathDesired.EndingVelocity   = 0;
-            pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
             break;
-        default:
 
-            pathDesired.Start.North      = positionState.North;
+        case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
-            pathDesired.Start.East       = positionState.East;
+            plan_setup_positionHold();
-            pathDesired.Start.Down       = positionState.Down;
+            break;
-            pathDesired.End.North        = positionState.North;
+
-            pathDesired.End.East         = positionState.East;
+        case FLIGHTSTATUS_FLIGHTMODE_LAND:
-            pathDesired.End.Down         = positionState.Down;
+            plan_setup_land();
-            pathDesired.StartingVelocity = 1;
+            break;
-            pathDesired.EndingVelocity   = 0;
+
-            pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
+        default:
+            plan_setup_positionHold();
+
             /* Disable this section, until such time as proper discussion can be had about how to implement it for all types of crafts.
                } else {
                PathDesiredData pathDesired;
@@ -103,17 +87,15 @@ void pathFollowerHandler(bool newinit)
              */
             break;
         }
-        PathDesiredSet(&pathDesired);
     }
 
+    switch (flightMode) {
     // special handling of autoland - behaves like positon hold but with slow altitude decrease
-    if (flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_LAND) {
+    case FLIGHTSTATUS_FLIGHTMODE_LAND:
-        PositionStateData positionState;
+        plan_run_land();
-        PositionStateGet(&positionState);
+        break;
-        PathDesiredData pathDesired;
+    default:
-        PathDesiredGet(&pathDesired);
+        break;
-        pathDesired.End.Down = positionState.Down + 5;
-        PathDesiredSet(&pathDesired);
     }
 }
 

flight/modules/ManualControl/takeofflocationhandler.c

@@ -0,0 +1,125 @@
+/**
+ ******************************************************************************
+ *
+ * @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
+
+// Private variables
+static bool locationSet;
+// Private functions
+static void SetTakeOffLocation();
+
+void takeOffLocationHandlerInit()
+{
+    TakeOffLocationInitialize();
+    // check whether there is a preset/valid takeoff location
+    uint8_t mode;
+    uint8_t status;
+    TakeOffLocationModeGet(&mode);
+    TakeOffLocationStatusGet(&status);
+    // preset with invalid location will actually behave like FirstTakeoff
+    if (mode == TAKEOFFLOCATION_MODE_PRESET && status == TAKEOFFLOCATION_STATUS_VALID) {
+        locationSet = true;
+    } else {
+        locationSet = false;
+        status = TAKEOFFLOCATION_STATUS_INVALID;
+        TakeOffLocationStatusSet(&status);
+    }
+}
+/**
+ * Handles TakeOffPosition location setup
+ * @param newinit
+ */
+void takeOffLocationHandler()
+{
+    uint8_t armed;
+    uint8_t status;
+
+    FlightStatusArmedGet(&armed);
+
+    // Location already acquired/preset
+    if (armed == FLIGHTSTATUS_ARMED_ARMED && locationSet) {
+        return;
+    }
+
+    TakeOffLocationStatusGet(&status);
+
+    switch (armed) {
+    case FLIGHTSTATUS_ARMED_ARMING:
+    case FLIGHTSTATUS_ARMED_ARMED:
+        if (!locationSet || status != TAKEOFFLOCATION_STATUS_VALID) {
+            uint8_t mode;
+            TakeOffLocationModeGet(&mode);
+
+            if ((mode != TAKEOFFLOCATION_MODE_PRESET) || (status == TAKEOFFLOCATION_STATUS_INVALID)) {
+                SetTakeOffLocation();
+            } else {
+                locationSet = true;
+            }
+        }
+        break;
+
+    case FLIGHTSTATUS_ARMED_DISARMED:
+        // unset if location is to be acquired at each arming
+        if (locationSet) {
+            uint8_t mode;
+            TakeOffLocationModeGet(&mode);
+            if (mode == TAKEOFFLOCATION_MODE_ARMINGLOCATION) {
+                locationSet = false;
+                status = TAKEOFFLOCATION_STATUS_INVALID;
+                TakeOffLocationStatusSet(&status);
+            }
+        }
+        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;
+    takeOffLocation.Status = TAKEOFFLOCATION_STATUS_VALID;
+
+    TakeOffLocationSet(&takeOffLocation);
+    locationSet = true;
+}

flight/modules/PathPlanner/pathplanner.c

@@ -45,6 +45,7 @@
 #include "flightmodesettings.h"
 #include <pios_struct_helper.h>
 #include "paths.h"
+#include "plans.h"
 
 // Private constants
 #define STACK_SIZE_BYTES            1024
@@ -89,6 +90,7 @@ static bool pathplanner_active = false;
  */
 int32_t PathPlannerStart()
 {
+    plan_initialize();
     // when the active waypoint changes, update pathDesired
     WaypointConnectCallback(commandUpdated);
     WaypointActiveConnectCallback(commandUpdated);
@@ -169,21 +171,7 @@ static void pathPlannerTask()
         if (!failsafeRTHset) {
             failsafeRTHset = 1;
             // copy pasta: same calculation as in manualcontrol, set return to home coordinates
-            PositionStateData positionState;
+            plan_setup_positionHold();
-            PositionStateGet(&positionState);
-            FlightModeSettingsData settings;
-            FlightModeSettingsGet(&settings);
-
-            pathDesired.Start.North      = 0;
-            pathDesired.Start.East       = 0;
-            pathDesired.Start.Down       = positionState.Down - settings.ReturnToHomeAltitudeOffset;
-            pathDesired.End.North        = 0;
-            pathDesired.End.East         = 0;
-            pathDesired.End.Down         = positionState.Down - settings.ReturnToHomeAltitudeOffset;
-            pathDesired.StartingVelocity = 1;
-            pathDesired.EndingVelocity   = 0;
-            pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
-            PathDesiredSet(&pathDesired);
         }
         AlarmsSet(SYSTEMALARMS_ALARM_PATHPLAN, SYSTEMALARMS_ALARM_ERROR);
 

flight/modules/Sensors/sensors.c

@@ -62,6 +62,7 @@
 #include <CoordinateConversions.h>
 
 #include <pios_board_info.h>
+#include <pios_struct_helper.h>
 
 // Private constants
 #define STACK_SIZE_BYTES 1000
@@ -474,35 +475,7 @@ static void settingsUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv)
     RPY2Quaternion(rpy, rotationQuat);
     Quaternion2R(rotationQuat, R);
 
-    mag_transform[0][0] = R[0][0] * cal.mag_transform.r0c0 +
+    matrix_mult_3x3f((float(*)[3])cast_struct_to_array(cal.mag_transform, cal.mag_transform.r0c0), R, mag_transform);
-                          R[1][0] * cal.mag_transform.r0c1 +
-                          R[2][0] * cal.mag_transform.r0c2;
-    mag_transform[0][1] = R[0][1] * cal.mag_transform.r0c0 +
-                          R[1][1] * cal.mag_transform.r0c1 +
-                          R[2][1] * cal.mag_transform.r0c2;
-    mag_transform[0][2] = R[0][2] * cal.mag_transform.r0c0 +
-                          R[1][2] * cal.mag_transform.r0c1 +
-                          R[2][2] * cal.mag_transform.r0c2;
-
-    mag_transform[1][0] = R[0][0] * cal.mag_transform.r1c0 +
-                          R[1][0] * cal.mag_transform.r1c1 +
-                          R[2][0] * cal.mag_transform.r1c2;
-    mag_transform[1][1] = R[0][1] * cal.mag_transform.r1c0 +
-                          R[1][1] * cal.mag_transform.r1c1 +
-                          R[2][1] * cal.mag_transform.r1c2;
-    mag_transform[1][2] = R[0][2] * cal.mag_transform.r1c0 +
-                          R[1][2] * cal.mag_transform.r1c1 +
-                          R[2][2] * cal.mag_transform.r1c2;
-
-    mag_transform[1][0] = R[0][0] * cal.mag_transform.r2c0 +
-                          R[1][0] * cal.mag_transform.r2c1 +
-                          R[2][0] * cal.mag_transform.r2c2;
-    mag_transform[2][1] = R[0][1] * cal.mag_transform.r2c0 +
-                          R[1][1] * cal.mag_transform.r2c1 +
-                          R[2][1] * cal.mag_transform.r2c2;
-    mag_transform[2][2] = R[0][2] * cal.mag_transform.r2c0 +
-                          R[1][2] * cal.mag_transform.r2c1 +
-                          R[2][2] * cal.mag_transform.r2c2;
 }
 /**
  * @}

flight/targets/boards/discoveryf4bare/firmware/Makefile

@@ -82,6 +82,7 @@ ifndef TESTAPP
 
     ## Misc library functions
     SRC += $(FLIGHTLIB)/paths.c
+	SRC += $(FLIGHTLIB)/plans.c
     SRC += $(FLIGHTLIB)/WorldMagModel.c
     SRC += $(FLIGHTLIB)/insgps13state.c
 

flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc

@@ -113,6 +113,7 @@ UAVOBJSRCFILENAMES += poilocation
 UAVOBJSRCFILENAMES += poilearnsettings
 UAVOBJSRCFILENAMES += mpu6000settings
 UAVOBJSRCFILENAMES += txpidsettings
+UAVOBJSRCFILENAMES += takeofflocation
 
 UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(OPUAVSYNTHDIR)/$(UAVOBJSRCFILE).c )
 UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) )

flight/targets/boards/revolution/firmware/Makefile

@@ -84,6 +84,7 @@ ifndef TESTAPP
 
     ## Misc library functions
     SRC += $(FLIGHTLIB)/paths.c
+    SRC += $(FLIGHTLIB)/plans.c
     SRC += $(FLIGHTLIB)/WorldMagModel.c
     SRC += $(FLIGHTLIB)/insgps13state.c
 

flight/targets/boards/revolution/firmware/UAVObjects.inc

@@ -113,6 +113,7 @@ UAVOBJSRCFILENAMES += poilocation
 UAVOBJSRCFILENAMES += poilearnsettings
 UAVOBJSRCFILENAMES += mpu6000settings
 UAVOBJSRCFILENAMES += txpidsettings
+UAVOBJSRCFILENAMES += takeofflocation
 
 UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(OPUAVSYNTHDIR)/$(UAVOBJSRCFILE).c )
 UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) )

flight/targets/boards/revoproto/firmware/Makefile

@@ -83,6 +83,7 @@ ifndef TESTAPP
 
     ## Misc library functions
     SRC += $(FLIGHTLIB)/paths.c
+    SRC += $(FLIGHTLIB)/plans.c
     SRC += $(FLIGHTLIB)/WorldMagModel.c
     SRC += $(FLIGHTLIB)/insgps13state.c
 

flight/targets/boards/revoproto/firmware/UAVObjects.inc

@@ -113,6 +113,7 @@ UAVOBJSRCFILENAMES += poilocation
 UAVOBJSRCFILENAMES += poilearnsettings
 UAVOBJSRCFILENAMES += mpu6000settings
 UAVOBJSRCFILENAMES += txpidsettings
+UAVOBJSRCFILENAMES += takeofflocation
 
 UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(OPUAVSYNTHDIR)/$(UAVOBJSRCFILE).c )
 UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) )

flight/targets/boards/simposix/firmware/Makefile

@@ -94,6 +94,7 @@ SRC += $(FLIGHTLIB)/fifo_buffer.c
 SRC += $(FLIGHTLIB)/WorldMagModel.c
 SRC += $(FLIGHTLIB)/insgps13state.c
 SRC += $(FLIGHTLIB)/paths.c
+SRC += $(FLIGHTLIB)/plans.c
 SRC += $(FLIGHTLIB)/sanitycheck.c
 
 SRC += $(MATHLIB)/sin_lookup.c

flight/targets/boards/simposix/firmware/UAVObjects.inc

@@ -111,6 +111,7 @@ UAVOBJSRCFILENAMES += revosettings
 UAVOBJSRCFILENAMES += altitudeholdstatus
 UAVOBJSRCFILENAMES += ekfconfiguration
 UAVOBJSRCFILENAMES += ekfstatevariance
+UAVOBJSRCFILENAMES += takeofflocation
 
 UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(UAVOBJSYNTHDIR)/$(UAVOBJSRCFILE).c )
 UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) )

ground/openpilotgcs/src/libs/opmapcontrol/src/core/urlfactory.cpp

@@ -197,7 +197,6 @@ QString UrlFactory::MakeImageUrl(const MapType::Types &type, const Point &pos, c
         QString sec2    = ""; // after &zoom=...
         GetSecGoogleWords(pos, sec1, sec2);
         TryCorrectGoogleVersions();
-        QString VersionGoogleSatellite = "132";
         return QString("https://%1%2.google.com/%3/v=%4&hl=%5&x=%6%7&y=%8&z=%9&s=%10").arg(server).arg(GetServerNum(pos, 4)).arg(request).arg(VersionGoogleSatellite).arg(language).arg(pos.X()).arg(sec1).arg(pos.Y()).arg(zoom).arg(sec2);
     }
     break;

ground/openpilotgcs/src/libs/utils/coordinateconversions.cpp

@@ -44,7 +44,7 @@ CoordinateConversions::CoordinateConversions()
  * @param[in] LLA Longitude latitude altitude for this location
  * @param[out] Rne[3][3] Rotation matrix
  */
-void CoordinateConversions::RneFromLLA(double LLA[3], double Rne[3][3])
+void CoordinateConversions::RneFromLLA(double LLA[3], float Rne[3][3])
 {
     float sinLat, sinLon, cosLat, cosLon;
 
@@ -134,7 +134,7 @@ int CoordinateConversions::NED2LLA_HomeECEF(double BaseECEFm[3], double NED[3],
     // stored value is in cm, convert to m
     double BaseLLA[3];
     double ECEF[3];
-    double Rne[3][3];
+    float Rne[3][3];
 
     // Get LLA address to compute conversion matrix
     ECEF2LLA(BaseECEFm, BaseLLA);

ground/openpilotgcs/src/libs/utils/coordinateconversions.h

@@ -41,7 +41,7 @@ public:
     CoordinateConversions();
     int NED2LLA_HomeECEF(double BaseECEFcm[3], double NED[3], double position[3]);
     int NED2LLA_HomeLLA(double LLA[3], double NED[3], double position[3]);
-    void RneFromLLA(double LLA[3], double Rne[3][3]);
+    void RneFromLLA(double LLA[3], float Rne[3][3]);
     void LLA2ECEF(double LLA[3], double ECEF[3]);
     int ECEF2LLA(double ECEF[3], double LLA[3]);
     void LLA2Base(double LLA[3], double BaseECEF[3], float Rne[3][3], float NED[3]);

ground/openpilotgcs/src/plugins/hitl/fgsimulator.cpp

@@ -268,12 +268,12 @@ void FGSimulator::processUpdate(const QByteArray & inp)
     float temperature  = fields[19].toFloat();
     // Get pressure (kpa)
     float pressure     = fields[20].toFloat() * INHG2KPA;
-    // Get VelocityState Down (cm/s)
+    // Get VelocityState Down (m/s)
-    float velocityStateDown  = -fields[21].toFloat() * FPS2CMPS;
+    float velocityStateDown  = -fields[21].toFloat() * FPS2CMPS * 1e-2f;
-    // Get VelocityState East (cm/s)
+    // Get VelocityState East (m/s)
-    float velocityStateEast  = fields[22].toFloat() * FPS2CMPS;
+    float velocityStateEast  = fields[22].toFloat() * FPS2CMPS * 1e-2f;
-    // Get VelocityState Down (cm/s)
+    // Get VelocityState Down (m/s)
-    float velocityStateNorth = fields[23].toFloat() * FPS2CMPS;
+    float velocityStateNorth = fields[23].toFloat() * FPS2CMPS * 1e-2f;
 
     // Get UDP packets received by FG
     int n = fields[24].toInt();
@@ -286,16 +286,15 @@ void FGSimulator::processUpdate(const QByteArray & inp)
     Output2Hardware out;
     memset(&out, 0, sizeof(Output2Hardware));
 
+    HomeLocation::DataFields homeData = posHome->getData();
+    double HomeLLA[3] = { (double)homeData.Latitude * 1e-7, (double)homeData.Longitude * 1e-7, homeData.Altitude };
+    double HomeECEF[3];
+    float HomeRNE[3][3];
+    double LLA[3]     = { latitude, longitude, altitude_msl };
     float NED[3];
-    // convert from cm back to meters
+    Utils::CoordinateConversions().RneFromLLA(HomeLLA, HomeRNE);
-
+    Utils::CoordinateConversions().LLA2ECEF(HomeLLA, HomeECEF);
-    double LLA[3] = { latitude, longitude, altitude_msl };
+    Utils::CoordinateConversions().LLA2Base(LLA, HomeECEF, HomeRNE, NED);
-    double ECEF[3];
-    double RNE[9];
-    Utils::CoordinateConversions().RneFromLLA(LLA, (double(*)[3])RNE);
-    Utils::CoordinateConversions().LLA2ECEF(LLA, ECEF);
-    Utils::CoordinateConversions().LLA2Base(LLA, ECEF, (float(*)[3])RNE, NED);
-
 
     // Update GPS Position objects
     out.latitude    = latitude * 1e7;

ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro

@@ -125,7 +125,8 @@ HEADERS += \
     $$UAVOBJECT_SYNTHETICS/osdsettings.h \
     $$UAVOBJECT_SYNTHETICS/waypoint.h \
     $$UAVOBJECT_SYNTHETICS/waypointactive.h \
-    $$UAVOBJECT_SYNTHETICS/mpu6000settings.h
+    $$UAVOBJECT_SYNTHETICS/mpu6000settings.h \
+    $$UAVOBJECT_SYNTHETICS/takeofflocation.h
 
 SOURCES += \
     $$UAVOBJECT_SYNTHETICS/accelgyrosettings.cpp \
@@ -227,4 +228,5 @@ SOURCES += \
     $$UAVOBJECT_SYNTHETICS/osdsettings.cpp \
     $$UAVOBJECT_SYNTHETICS/waypoint.cpp \
     $$UAVOBJECT_SYNTHETICS/waypointactive.cpp \
-    $$UAVOBJECT_SYNTHETICS/mpu6000settings.cpp
+    $$UAVOBJECT_SYNTHETICS/mpu6000settings.cpp \
+    $$UAVOBJECT_SYNTHETICS/takeofflocation.cpp

shared/uavobjectdefinition/fixedwingpathfollowersettings.xml

@@ -12,6 +12,8 @@
 
         <field name="CourseFeedForward" units="s" type="float" elements="1" defaultvalue="3.0"/>
                 <!-- how many seconds to plan the flight vector ahead when initiating necessary heading changes - increase for planes with sluggish response -->
+        <field name="ReverseCourseOverlap" units="deg" type="float" elements="1" defaultvalue="20.0"/>
+		<!-- how big the overlapping area behind the plane is, where, if the desired course lies behind, the current bank angle will determine if the plane goes left or right -->
 
         <field name="HorizontalPosP" units="(m/s)/m" type="float" elements="1" defaultvalue="0.05"/>
                 <!-- proportional coefficient for correction vector in path vector field to get back on course - reduce for fast planes to prevent course oscillations -->

shared/uavobjectdefinition/flightmodesettings.xml

@@ -108,7 +108,7 @@
         <field name="ArmedTimeout" units="ms" type="uint16" elements="1" defaultvalue="30000"/>
         <field name="ArmingSequenceTime" units="ms" type="uint16" elements="1" defaultvalue="1000"/>
         <field name="DisarmingSequenceTime" units="ms" type="uint16" elements="1" defaultvalue="1000"/>
-        <field name="ReturnToHomeAltitudeOffset" units="m" type="float" elements="1" defaultvalue="10"/>
+        <field name="ReturnToBaseAltitudeOffset" units="m" type="float" elements="1" defaultvalue="10"/>
         <access gcs="readwrite" flight="readwrite"/>
         <telemetrygcs acked="true" updatemode="onchange" period="0"/>
         <telemetryflight acked="true" updatemode="onchange" period="0"/>

shared/uavobjectdefinition/systemalarms.xml

@@ -45,8 +45,8 @@
 		</elementnames>
 	</field>
         <access gcs="readwrite" flight="readwrite"/>
-        <telemetrygcs acked="true" updatemode="manual" period="0"/>
+        <telemetrygcs acked="false" updatemode="manual" period="0"/>
-        <telemetryflight acked="true" updatemode="onchange" period="0"/>
+        <telemetryflight acked="false" updatemode="onchange" period="0"/>
 	<logging updatemode="manual" period="0"/>
     </object>
 </xml>

shared/uavobjectdefinition/takeofflocation.xml

@@ -0,0 +1,14 @@
+<xml>
+    <object name="TakeOffLocation" singleinstance="true" settings="true" category="Navigation">
+        <description>TakeOffLocation setting which contains the destination of a ReturnToBase operation</description>
+        <field name="North" units="m" type="float" elements="1" defaultvalue="0" />
+        <field name="East" units="m" type="float" elements="1" defaultvalue="0" />
+        <field name="Down" units="m" type="float" elements="1" defaultvalue="0" />
+        <field name="Mode" units="" type="enum" elements="1" options="ArmingLocation,FirstArmingLocation,Preset" defaultvalue="ArmingLocation"/>
+        <field name="Status" units="" type="enum" elements="1" options="Valid,Invalid" defaultvalue="Invalid"/>
+        <access gcs="readwrite" flight="readwrite"/>
+        <telemetrygcs acked="true" updatemode="onchange" period="0"/>
+        <telemetryflight acked="true" updatemode="onchange" period="0"/>
+        <logging updatemode="manual" period="0"/>
+    </object>
+</xml>