From 50d60649aa3e584aeceb5f35280572caa7dcf1ba Mon Sep 17 00:00:00 2001
From: abeck70 <abeck70@icloud.com>
Date: Mon, 25 May 2015 22:13:13 +1000
Subject: [PATCH] REVONANO Fix BrakeController to use vtol VerticalVelPID
 instead of LandVerticalVelPID

---
 flight/modules/PathFollower/inc/vtolbrakefsm.h   |  2 --
 .../modules/PathFollower/vtolbrakecontroller.cpp | 16 ++++++++++------
 2 files changed, 10 insertions(+), 8 deletions(-)

diff --git a/flight/modules/PathFollower/inc/vtolbrakefsm.h b/flight/modules/PathFollower/inc/vtolbrakefsm.h
index 6e5151977..ac10faa0d 100644
--- a/flight/modules/PathFollower/inc/vtolbrakefsm.h
+++ b/flight/modules/PathFollower/inc/vtolbrakefsm.h
@@ -66,7 +66,6 @@ public:
     void Activate(void);
     void Update(void);
     PathFollowerFSMState_T GetCurrentState(void);
-    void Abort(void);
     uint8_t PositionHoldState(void);
 
 protected:
@@ -103,7 +102,6 @@ protected:
     // void updateVtolBrakeFSMStatus();
 
     void setStateTimeout(int32_t count);
-    void fallback_to_hold(void);
 
     static PathFollowerFSM_BrakeStateHandler_T sBrakeStateTable[BRAKE_STATE_SIZE];
 };
diff --git a/flight/modules/PathFollower/vtolbrakecontroller.cpp b/flight/modules/PathFollower/vtolbrakecontroller.cpp
index 271ee907c..960e330de 100644
--- a/flight/modules/PathFollower/vtolbrakecontroller.cpp
+++ b/flight/modules/PathFollower/vtolbrakecontroller.cpp
@@ -35,6 +35,7 @@ extern "C" {
 
 #include <math.h>
 #include <pid.h>
+#include <alarms.h>
 #include <CoordinateConversions.h>
 #include <sin_lookup.h>
 #include <pathdesired.h>
@@ -138,10 +139,10 @@ void VtolBrakeController::SettingsUpdated(void)
     controlNE.UpdatePositionalParameters(vtolPathFollowerSettings->HorizontalPosP);
     controlNE.UpdateCommandParameters(-vtolPathFollowerSettings->BrakeMaxPitch, vtolPathFollowerSettings->BrakeMaxPitch, vtolPathFollowerSettings->BrakeVelocityFeedforward);
 
-    controlDown.UpdateParameters(vtolPathFollowerSettings->LandVerticalVelPID.Kp,
-                                 vtolPathFollowerSettings->LandVerticalVelPID.Ki,
-                                 vtolPathFollowerSettings->LandVerticalVelPID.Kd,
-                                 vtolPathFollowerSettings->LandVerticalVelPID.Beta,
+    controlDown.UpdateParameters(vtolPathFollowerSettings->VerticalVelPID.Kp,
+                                 vtolPathFollowerSettings->VerticalVelPID.Ki,
+                                 vtolPathFollowerSettings->VerticalVelPID.Kd,
+                                 vtolPathFollowerSettings->VerticalVelPID.Beta,
                                  dT,
                                  10.0f * vtolPathFollowerSettings->VerticalVelMax); // avoid constraining initial fast entry into brake
     controlDown.UpdatePositionalParameters(vtolPathFollowerSettings->VerticalPosP);
@@ -364,8 +365,11 @@ void VtolBrakeController::UpdateAutoPilot()
 
     int8_t result = UpdateStabilizationDesired();
 
-    if (!result) {
-        fsm->Abort(); // enter PH
+    if (result) {
+        AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
+    } else {
+        pathStatus->Status = PATHSTATUS_STATUS_CRITICAL;
+        AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING);
     }
 
     PathStatusSet(pathStatus);