Changeset 214 in flair-src for trunk/lib/FlairMeta/src/UavStateMachine.cpp

Timestamp:

Feb 7, 2018, 5:49:27 PM (7 years ago)

Author:

Sanahuja Guillaume

Message:

matrix

File:

• trunk/lib/FlairMeta/src/UavStateMachine.cpp (modified) (5 diffs)

trunk/lib/FlairMeta/src/UavStateMachine.cpp

@@ -39 +39 @@
 #include <Vector3D.h>
 #include <Vector2D.h>
-#include <cvmatrix.h>
+#include <Matrix.h>
 #include <stdio.h>
 #include <TrajectoryGenerator1D.h>
@@ -109 +109 @@
       new TrajectoryGenerator1D(uavTab->NewRow(), "alt cons", "m");
   uav->GetMetaUsRangeFinder()->GetZPlot()->AddCurve(
-      altitudeTrajectory->Matrix()->Element(0), DataPlot::Green);
+      altitudeTrajectory->GetMatrix()->Element(0), DataPlot::Green);
   uav->GetMetaUsRangeFinder()->GetVzPlot()->AddCurve(
-      altitudeTrajectory->Matrix()->Element(1), DataPlot::Green);
+      altitudeTrajectory->GetMatrix()->Element(1), DataPlot::Green);
 }
 
@@ -459 +459 @@
 void UavStateMachine::TakeOff(void) {
   flagZTrajectoryFinished = false;
-
-  if((altitudeState==AltitudeState_t::Stopped) && safeToFly && uav->isReadyToFly()  && uav->GetBatteryMonitor()->IsBatteryLow()==false && !flagConnectionLost) {
+  
+  if(altitudeState!=AltitudeState_t::Stopped) {
+    Warn("cannot takeoff, altitudeState!=AltitudeState_t::Stopped\n");
+    joy->ErrorNotify();
+  } else if(!safeToFly) {
+    Warn("cannot takeoff, uav is not safe to fly\n");
+    joy->ErrorNotify();
+  } else if(!uav->isReadyToFly()) {
+    Warn("cannot takeoff, uav is not ready\n");
+    joy->ErrorNotify();
+  } else if(uav->GetBatteryMonitor()->IsBatteryLow()) {
+    Warn("cannot takeoff, battery is low\n");
+    joy->ErrorNotify();
+  } else if(flagConnectionLost) {
+    Warn("cannot takeoff, connection with flairgcs lost\n");
+    joy->ErrorNotify();
+  } else {
     //The uav always takes off in fail safe mode
     flagBatteryLow=false;
@@ -480 +495 @@
     altitudeState = AltitudeState_t::TakingOff;
     SignalEvent(Event_t::TakingOff);
-  } else {
-    Warn("cannot takeoff\n");
-    joy->ErrorNotify();
   }
 }
@@ -511 +523 @@
     altitudeState=AltitudeState_t::FinishLanding;
     safeToFly=false;
-Printf("Emergency landing!\n");
+    Warn("Emergency landing!\n");
+    Warn("You will not be able to take off again\n");
 }