Changeset 432 in flair-src for trunk/lib/FlairMeta/src/MetaVrpnObject.cpp

Timestamp:

Jul 12, 2021, 11:19:21 AM (3 years ago)

Author:

Sanahuja Guillaume

Message:

add AltitudeSensor class
failsafe altitude sensor in changeable

File:

• trunk/lib/FlairMeta/src/MetaVrpnObject.cpp (modified) (4 diffs)

trunk/lib/FlairMeta/src/MetaVrpnObject.cpp

@@ -27 +27 @@
 #include <TabWidget.h>
 #include <Matrix.h>
+#include <AltitudeSensor.h>
 
 using std::string;
@@ -33 +34 @@
 using namespace flair::sensor;
 using namespace flair::filter;
+
+namespace flair {
+namespace sensor {
+class MetaVrpnObjectAltitudeSensor : public AltitudeSensor {
+  public:
+    MetaVrpnObjectAltitudeSensor(meta::MetaVrpnObject* parent,std::string name): AltitudeSensor(parent,name) {
+      this->parent=parent;
+    }
+    ~MetaVrpnObjectAltitudeSensor() {
+    }
+
+    //z and dz must be in uav's frame
+    float z(void) const {
+      Vector3Df uav_pos;
+      parent->GetPosition(uav_pos);
+      return -uav_pos.z;
+    }
+    float Vz(void) const {
+      Vector3Df uav_vel;
+      parent->GetSpeed(uav_vel);
+      return -uav_vel.z;
+    }
+
+  private:
+     meta::MetaVrpnObject* parent;
+      
+};
+}// end namespace sensor
+}// end namespace flair
 
 namespace flair {
@@ -80 +110 @@
   xy_plot = new DataPlot2D(plot_tab->NewRow(), "xy", "y", -5, 5, "x", -5, 5);
   xy_plot->AddCurve(Output()->Element(5, 0), Output()->Element(4, 0));
+  
+  altitudeSensor=new MetaVrpnObjectAltitudeSensor(this,name);
+}
+
+sensor::AltitudeSensor *MetaVrpnObject::GetAltitudeSensor(void) const {
+  return altitudeSensor;
 }
 
@@ -102 +138 @@
 }
 
-} // end namespace sensor
+} // end namespace meta
 } // end namespace flair