LP-89 - Port OP_15.05.01 fixes. Release notes:

--- RELEASE-15.05.01 HOTFIX --- Banana Split ---
This release fixes an important bug.  All Revolution hardware running 15.05 should upgrade to 15.05.01. Note that this is a hotfix; it can
simply be flashed without an erase settings. Furthermore, please review your vtolpathfollowersettings:HorizontalVelMax; a value of around 4m/s would be more appropriate for preliminary trialing of a new release and will be changed in future.

Release Notes - OP Next Generation - Version OP15.05.01

** Bug
    * [NG-55] - 15.05 PositionHold exhibits fly-away behaviour at the vtolpathfollowersettings maxRollPitch and HorizontalVelMax values.
---
 WHATSNEW.txt                                      | 13 ++++++++++-
 flight/libraries/pid/pidcontroldown.cpp           | 26 ++++++++++-----------
 flight/libraries/plans.c                          | 28 +++++++++++++++++++----
 flight/modules/PathFollower/inc/pidcontrolne.h    |  1 -
 flight/modules/PathFollower/pidcontrolne.cpp      | 23 +++++++++++++++----
 flight/modules/PathFollower/vtolflycontroller.cpp | 16 ++++++++-----
 6 files changed, 77 insertions(+), 30 deletions(-)

WHATSNEW.txt

@@ -1,4 +1,15 @@
---- RELEASE-15.05 --- Revolution Nano ---
+--- RELEASE-15.05.01 HOTFIX --- Banana Split ---
+This release fixes an important bug.  All Revolution hardware running 15.05 should upgrade to 15.05.01. Note that this is a hotfix; it can
+simply be flashed without an erase settings. Furthermore, please review your vtolpathfollowersettings:HorizontalVelMax; a value of around 4m/s would be more appropriate for preliminary trialing of a new release and will be changed in future.
+
+Release Notes - OP Next Generation - Version OP15.05.01
+
+** Bug
+    * [NG-55] - 15.05 PositionHold exhibits fly-away behaviour at the vtolpathfollowersettings maxRollPitch and HorizontalVelMax values.
+
+
+
+--- RELEASE-15.05 --- Banana Split ---
 
 This release introduces new features and new hardware support for the Revolution Nano.  
 Note that the CC3D is not supported by this feature release.  

flight/libraries/pid/pidcontroldown.cpp

@@ -41,7 +41,7 @@ extern "C" {
 #include <paths.h>
 #include "plans.h"
 #include <stabilizationdesired.h>
-#include <pidstatus.h>
+// #include <pidstatus.h>
 #include <vtolselftuningstats.h>
 }
 
@@ -348,7 +348,7 @@ float PIDControlDown::GetVelocityDesired(void)
 
 float PIDControlDown::GetDownCommand(void)
 {
-    PIDStatusData pidStatus;
+    // PIDStatusData pidStatus;
     float ulow  = mMinThrust;
     float uhigh = mMaxThrust;
 
@@ -356,17 +356,17 @@ float PIDControlDown::GetDownCommand(void)
         mCallback->BoundThrust(ulow, uhigh);
     }
     float downCommand = pid2_apply(&PID, mVelocitySetpointCurrent, mVelocityState, ulow, uhigh);
-    pidStatus.setpoint = mVelocitySetpointCurrent;
+    // pidStatus.setpoint = mVelocitySetpointCurrent;
-    pidStatus.actual   = mVelocityState;
+    // pidStatus.actual   = mVelocityState;
-    pidStatus.error    = mVelocitySetpointCurrent - mVelocityState;
+    // pidStatus.error    = mVelocitySetpointCurrent - mVelocityState;
-    pidStatus.setpoint = mVelocitySetpointCurrent;
+    // pidStatus.setpoint = mVelocitySetpointCurrent;
-    pidStatus.ulow     = ulow;
+    // pidStatus.ulow     = ulow;
-    pidStatus.uhigh    = uhigh;
+    // pidStatus.uhigh    = uhigh;
-    pidStatus.command  = downCommand;
+    // pidStatus.command  = downCommand;
-    pidStatus.P  = PID.P;
+    // pidStatus.P  = PID.P;
-    pidStatus.I  = PID.I;
+    // pidStatus.I  = PID.I;
-    pidStatus.D  = PID.D;
+    // pidStatus.D  = PID.D;
-    PIDStatusSet(&pidStatus);
+    // PIDStatusSet(&pidStatus);
     mDownCommand = downCommand;
     return mDownCommand;
 }

flight/libraries/plans.c

@@ -102,7 +102,8 @@ void plan_setup_positionHold()
 
     PositionStateGet(&positionState);
     PathDesiredData pathDesired;
-    PathDesiredGet(&pathDesired);
+    // re-initialise in setup stage
+    memset(&pathDesired, 0, sizeof(PathDesiredData));
 
     FlightModeSettingsPositionHoldOffsetData offset;
     FlightModeSettingsPositionHoldOffsetGet(&offset);
@@ -132,7 +133,8 @@ void plan_setup_returnToBase()
     PositionStateDownGet(&positionStateDown);
 
     PathDesiredData pathDesired;
-    PathDesiredGet(&pathDesired);
+    // re-initialise in setup stage
+    memset(&pathDesired, 0, sizeof(PathDesiredData));
 
     TakeOffLocationData takeoffLocation;
     TakeOffLocationGet(&takeoffLocation);
@@ -241,6 +243,8 @@ void plan_setup_AutoTakeoff()
             // stabi command.
         }
         PathDesiredData pathDesired;
+        // re-initialise in setup stage
+        memset(&pathDesired, 0, sizeof(PathDesiredData));
         plan_setup_AutoTakeoff_helper(&pathDesired);
         PathDesiredSet(&pathDesired);
     }
@@ -312,6 +316,8 @@ void plan_run_AutoTakeoff()
         autotakeoffState != STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_POSITIONHOLD) {
         if (priorState != autotakeoffState) {
             PathDesiredData pathDesired;
+            // re-initialise in setup stage
+            memset(&pathDesired, 0, sizeof(PathDesiredData));
             plan_setup_AutoTakeoff_helper(&pathDesired);
             PathDesiredSet(&pathDesired);
         }
@@ -348,6 +354,9 @@ void plan_setup_land()
 {
     PathDesiredData pathDesired;
 
+    // re-initialise in setup stage
+    memset(&pathDesired, 0, sizeof(PathDesiredData));
+
     plan_setup_land_helper(&pathDesired);
     PathDesiredSet(&pathDesired);
 }
@@ -506,7 +515,10 @@ static void plan_run_PositionVario(vario_type type)
     float alpha;
     PathDesiredData pathDesired;
 
+    // Reuse the existing pathdesired object as setup in the setup to avoid
+    // updating values already set.
     PathDesiredGet(&pathDesired);
+
     FlightModeSettingsPositionHoldOffsetData offset;
     FlightModeSettingsPositionHoldOffsetGet(&offset);
 
@@ -587,10 +599,11 @@ void plan_run_VelocityRoam()
 {
     // float alpha;
     PathDesiredData pathDesired;
+    // velocity roam code completely sets pathdesired object. it was not set in setup phase
+    memset(&pathDesired, 0, sizeof(PathDesiredData));
     FlightStatusAssistedControlStateOptions assistedControlFlightMode;
     FlightStatusFlightModeOptions flightMode;
 
-    PathDesiredGet(&pathDesired);
     FlightModeSettingsPositionHoldOffsetData offset;
     FlightModeSettingsPositionHoldOffsetGet(&offset);
     FlightStatusAssistedControlStateGet(&assistedControlFlightMode);
@@ -711,10 +724,11 @@ static PiOSDeltatimeConfig actimeval;
 void plan_setup_AutoCruise()
 {
     PositionStateData positionState;
-
     PositionStateGet(&positionState);
+
     PathDesiredData pathDesired;
-    PathDesiredGet(&pathDesired);
+    // setup needs to reinitialise the pathdesired object
+    memset(&pathDesired, 0, sizeof(PathDesiredData));
 
     FlightModeSettingsPositionHoldOffsetData offset;
     FlightModeSettingsPositionHoldOffsetGet(&offset);
@@ -757,7 +771,9 @@ void plan_run_AutoCruise()
 
     PositionStateGet(&positionState);
     PathDesiredData pathDesired;
+    // re-use pathdesired that was setup correctly in setup stage.
     PathDesiredGet(&pathDesired);
+
     FlightModeSettingsPositionHoldOffsetData offset;
     FlightModeSettingsPositionHoldOffsetGet(&offset);
 
@@ -830,6 +846,8 @@ void plan_setup_assistedcontrol()
 
     PositionStateGet(&positionState);
     PathDesiredData pathDesired;
+    // setup function, avoid carry over from previous mode
+    memset(&pathDesired, 0, sizeof(PathDesiredData));
 
     FlightStatusAssistedControlStateOptions assistedControlFlightMode;
 

flight/modules/PathFollower/inc/pidcontrolne.h

@@ -39,7 +39,6 @@ class PIDControlNE {
 public:
     PIDControlNE();
     ~PIDControlNE();
-    void Initialize();
     void Deactivate();
     void Activate();
     void UpdateParameters(float kp, float ki, float kd, __attribute__((unused)) float ilimit, float dT, float velocityMax);

flight/modules/PathFollower/pidcontrolne.cpp

@@ -47,13 +47,24 @@ extern "C" {
 
 PIDControlNE::PIDControlNE()
     : deltaTime(0), mNECommand(0), mNeutral(0), mVelocityMax(0), mMinCommand(0), mMaxCommand(0), mVelocityFeedforward(0), mActive(false)
-{}
+{
+    memset((void *)PIDvel, 0, 2 * sizeof(pid2));
+    memset((void *)PIDposH, 0, 2 * sizeof(pid));
+
+    mVelocitySetpointTarget[0]  = 0.0f;
+    mVelocitySetpointTarget[1]  = 0.0f;
+    mVelocityState[0] = 0.0f;
+    mVelocityState[1] = 0.0f;
+    mPositionSetpointTarget[0]  = 0.0f;
+    mPositionSetpointTarget[1]  = 0.0f;
+    mPositionState[0] = 0.0f;
+    mPositionState[1] = 0.0f;
+    mVelocitySetpointCurrent[0] = 0.0f;
+    mVelocitySetpointCurrent[1] = 0.0f;
+}
 
 PIDControlNE::~PIDControlNE() {}
 
-void PIDControlNE::Initialize()
-{}
-
 void PIDControlNE::Deactivate()
 {
     mActive = false;
@@ -62,6 +73,10 @@ void PIDControlNE::Deactivate()
 void PIDControlNE::Activate()
 {
     mActive = true;
+    pid2_transfer(&PIDvel[0], 0.0f);
+    pid2_transfer(&PIDvel[1], 0.0f);
+    pid_zero(&PIDposH[0]);
+    pid_zero(&PIDposH[1]);
 }
 
 void PIDControlNE::UpdateParameters(float kp, float ki, float kd, float beta, float dT, float velocityMax)

flight/modules/PathFollower/vtolflycontroller.cpp

@@ -84,6 +84,7 @@ void VtolFlyController::Activate(void)
         mManualThrust = false;
         SettingsUpdated();
         controlDown.Activate();
+        controlNE.UpdateVelocitySetpoint(0.0f, 0.0f);
         controlNE.Activate();
         mMode = pathDesired->Mode;
 
@@ -175,8 +176,6 @@ void VtolFlyController::UpdateVelocityDesired()
 
     VelocityStateData velocityState;
     VelocityStateGet(&velocityState);
-    controlNE.UpdateVelocityState(velocityState.North, velocityState.East);
-    controlDown.UpdateVelocityState(velocityState.Down);
 
     VelocityDesiredData velocityDesired;
 
@@ -192,6 +191,9 @@ void VtolFlyController::UpdateVelocityDesired()
         controlDown.ControlPositionWithPath(&progress);
     }
 
+    controlNE.UpdateVelocityState(velocityState.North, velocityState.East);
+    controlDown.UpdateVelocityState(velocityState.Down);
+
     float north, east;
     controlNE.GetVelocityDesired(&north, &east);
     velocityDesired.North = north;
@@ -368,10 +370,12 @@ void VtolFlyController::UpdateAutoPilot()
     // to the pathDesired to initiate a Landing sequence. This is the simpliest approach. plans.c
     // can't manage this.  And pathplanner whilst similar does not manage this as it is not a
     // waypoint traversal and is not aware of flight modes other than path plan.
-    if ((uint8_t)pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_NEXTCOMMAND] == FLIGHTMODESETTINGS_RETURNTOBASENEXTCOMMAND_LAND) {
+    if (flightStatus->FlightMode == FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE) {
-        if (pathStatus->fractional_progress > RTB_LAND_FRACTIONAL_PROGRESS_START_CHECKS) {
+        if ((uint8_t)pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_NEXTCOMMAND] == FLIGHTMODESETTINGS_RETURNTOBASENEXTCOMMAND_LAND) {
-            if (fabsf(pathStatus->correction_direction_north) < RTB_LAND_NE_DISTANCE_REQUIRED_TO_START_LAND_SEQUENCE && fabsf(pathStatus->correction_direction_east) < RTB_LAND_NE_DISTANCE_REQUIRED_TO_START_LAND_SEQUENCE) {
+            if (pathStatus->fractional_progress > RTB_LAND_FRACTIONAL_PROGRESS_START_CHECKS) {
-                plan_setup_land();
+                if (fabsf(pathStatus->correction_direction_north) < RTB_LAND_NE_DISTANCE_REQUIRED_TO_START_LAND_SEQUENCE && fabsf(pathStatus->correction_direction_east) < RTB_LAND_NE_DISTANCE_REQUIRED_TO_START_LAND_SEQUENCE) {
+                    plan_setup_land();
+                }
             }
         }
     }