Changeset 214 in flair-src for trunk/lib/FlairMeta/src

Timestamp:

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

Author:

Sanahuja Guillaume

Message:

matrix

Location:

trunk/lib/FlairMeta/src

Files:

• MetaDualShock3.cpp (modified) (1 diff)
• MetaDualShock3_impl.cpp (modified) (2 diffs)
• MetaUsRangeFinder.cpp (modified) (2 diffs)
• MetaVrpnObject.cpp (modified) (4 diffs)
• Uav.cpp (modified) (1 diff)
• UavStateMachine.cpp (modified) (5 diffs)

trunk/lib/FlairMeta/src/MetaDualShock3.cpp

@@ -22 +22 @@
 #include <Tab.h>
 #include <FrameworkManager.h>
-#include <cvmatrix.h>
+#include <Matrix.h>
 #include <Ahrs.h>
 #include <AhrsData.h>

trunk/lib/FlairMeta/src/MetaDualShock3_impl.cpp

@@ -20 +20 @@
 #include <JoyReference.h>
 #include <Tab.h>
-#include <cvmatrix.h>
+#include <Matrix.h>
 #include <FrameworkManager.h>
 
@@ -43 +43 @@
 // receives updates from the controler
 void MetaDualShock3_impl::UpdateFrom(const io_data *data) {
-  cvmatrix *input = (cvmatrix *)data;
+  const Matrix* input = dynamic_cast<const Matrix*>(data);
+  
+  if (!input) {
+      self->Warn("casting %s to Matrix failed\n",data->ObjectName().c_str());
+      return;
+  }
 
   // on prend une fois pour toute le mutex et on fait des accès directs

trunk/lib/FlairMeta/src/MetaUsRangeFinder.cpp

@@ -27 +27 @@
 #include <EulerDerivative.h>
 #include <GroupBox.h>
-#include <cvmatrix.h>
+#include <Matrix.h>
 
 using std::string;
@@ -53 +53 @@
   us->UseDefaultPlot();
 
-  us->GetPlot()->AddCurve(pbas_z->Matrix()->Element(0), DataPlot::Blue);
+  us->GetPlot()->AddCurve(pbas_z->GetMatrix()->Element(0), DataPlot::Blue);
 
   vz_plot = new DataPlot1D(us->GetPlotTab()->LastRowLastCol(), "vz", -2, 2);
-  vz_plot->AddCurve(vz_euler->Matrix()->Element(0));
-  vz_plot->AddCurve(pbas_vz->Matrix()->Element(0), DataPlot::Blue);
+  vz_plot->AddCurve(vz_euler->GetMatrix()->Element(0));
+  vz_plot->AddCurve(pbas_vz->GetMatrix()->Element(0), DataPlot::Blue);
 }
 

trunk/lib/FlairMeta/src/MetaVrpnObject.cpp

@@ -27 +27 @@
 #include <Tab.h>
 #include <TabWidget.h>
-#include <cvmatrix.h>
+#include <Matrix.h>
 
 using std::string;
@@ -54 +54 @@
     desc->SetElementName(i, 0, Output()->Name(i, 0));
   }
-  cvmatrix *prev_value = new cvmatrix(this, desc, floatType, name);
+  Matrix *prev_value = new Matrix(this, desc, floatType, name);
   delete desc;
 
@@ -65 +65 @@
     desc->SetElementName(i, 0, "d" + Output()->Name(i, 0));
   }
-  prev_value = new cvmatrix(this, desc, floatType, name);
+  prev_value = new Matrix(this, desc, floatType, name);
   delete desc;
 
@@ -73 +73 @@
 
   vx_opti_plot = new DataPlot1D(GetPlotTab()->NewRow(), "vx", -3, 3);
-  vx_opti_plot->AddCurve(euler->Matrix()->Element(4));
+  vx_opti_plot->AddCurve(euler->GetMatrix()->Element(4));
   vy_opti_plot = new DataPlot1D(GetPlotTab()->LastRowLastCol(), "vy", -3, 3);
-  vy_opti_plot->AddCurve(euler->Matrix()->Element(5));
+  vy_opti_plot->AddCurve(euler->GetMatrix()->Element(5));
   vz_opti_plot = new DataPlot1D(GetPlotTab()->LastRowLastCol(), "vz", -2, 2);
-  vz_opti_plot->AddCurve(euler->Matrix()->Element(6));
+  vz_opti_plot->AddCurve(euler->GetMatrix()->Element(6));
 
   plot_tab = new Tab(GetVrpnClient()->GetTabWidget(), "Mesures (xy) " + name);

trunk/lib/FlairMeta/src/Uav.cpp

@@ -28 +28 @@
 #include <NmeaGps.h>
 #include <Bldc.h>
-#include <cvmatrix.h>
+#include <Matrix.h>
 #include "MetaUsRangeFinder.h"
 #include "MetaVrpnObject.h"

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");
 }