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

Timestamp:

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

Author:

Sanahuja Guillaume

Message:

add AltitudeSensor class
failsafe altitude sensor in changeable

Location:

trunk/lib/FlairMeta/src

Files:

• MetaUsRangeFinder.cpp (modified) (1 diff)
• MetaUsRangeFinder.h (modified) (3 diffs)
• MetaVrpnObject.cpp (modified) (4 diffs)
• MetaVrpnObject.h (modified) (3 diffs)
• UavStateMachine.cpp (modified) (5 diffs)
• UavStateMachine.h (modified) (5 diffs)

trunk/lib/FlairMeta/src/MetaUsRangeFinder.cpp

@@ -37 +37 @@
 namespace flair {
 namespace meta {
+  
 
 MetaUsRangeFinder::MetaUsRangeFinder(UsRangeFinder *us)
-    : Object(us, us->ObjectName()) {
+    : AltitudeSensor(us, us->ObjectName()) {
   //TODO: mettre une rotation entre la sortie US et le low pass
   //pour eviter les fausses mesures quand le drone est orienté

trunk/lib/FlairMeta/src/MetaUsRangeFinder.h

@@ -14 +14 @@
 #define METAUSRANGEFINDER_H
 
-#include <Object.h>
+#include <AltitudeSensor.h>
 
 namespace flair {
-namespace filter {
-class ButterworthLowPass;
-class EulerDerivative;
-}
-namespace sensor {
-class UsRangeFinder;
-}
-namespace gui {
-class DataPlot1D;
-}
+  namespace filter {
+    class ButterworthLowPass;
+    class EulerDerivative;
+  }
+  namespace sensor {
+    class UsRangeFinder;
+  }
+  namespace gui {
+    class DataPlot1D;
+  }
 }
 
@@ -38 +38 @@
 * Cette classe est adaptée pour un capteur d'altitude.
 */
-class MetaUsRangeFinder : public core::Object {
+class MetaUsRangeFinder : public sensor::AltitudeSensor {
 public:
   MetaUsRangeFinder(sensor::UsRangeFinder *us);
@@ -51 +51 @@
 private:
   sensor::UsRangeFinder *us;
-  filter::ButterworthLowPass *pbas_z, *pbas_vz;
+  filter::ButterworthLowPass *pbas_z,*pbas_vz;
   filter::EulerDerivative *vz_euler;
   gui::DataPlot1D *vz_plot;

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

trunk/lib/FlairMeta/src/MetaVrpnObject.h

@@ -29 +29 @@
                 class LowPassFilter;
         }
+  namespace sensor {
+    class AltitudeSensor;
+  }
 }
 
@@ -51 +54 @@
   void GetSpeed(core::Vector3Df &speed) const;
   filter::EulerDerivative* GetEulerDerivative(void) const;
+  //get the associated AltitudeSensor
+  //MetaVrpnObject do not derivate from AltitudeSensor because frames are differents
+  //and this could be confusing
+  //MetaVrpnObject is in vrpn frame (earth)
+  //AltitudeSensor is in uav frame
+  sensor::AltitudeSensor *GetAltitudeSensor(void) const;
 
 private:
@@ -59 +68 @@
   gui::DataPlot1D *vx_opti_plot, *vy_opti_plot, *vz_opti_plot;
   gui::Tab *plot_tab;
+  sensor::AltitudeSensor *altitudeSensor;
 };
 } // end namespace meta

trunk/lib/FlairMeta/src/UavStateMachine.cpp

@@ -53 +53 @@
 
 UavStateMachine::UavStateMachine(TargetController *controller):
-        Thread(getFrameworkManager(),"UavStateMachine",50),
-        uav(GetUav()),controller(controller),failSafeMode(true),flagConnectionLost(false),flagBatteryLow(false),flagCriticalSensorLost(false),flagZTrajectoryFinished(false),safeToFly(true){
-    altitudeState=AltitudeState_t::Stopped;
-    uav->UseDefaultPlot();
+  Thread(getFrameworkManager(),"UavStateMachine",50),
+  uav(GetUav()),controller(controller),failSafeMode(true),flagConnectionLost(false),flagBatteryLow(false),flagCriticalSensorLost(false),flagZTrajectoryFinished(false),safeToFly(true){
+  altitudeState=AltitudeState_t::Stopped;
+  uav->UseDefaultPlot();
 
   Tab *uavTab = new Tab(getFrameworkManager()->GetTabWidget(), "uav", 0);
@@ -112 +112 @@
   uav->GetMetaUsRangeFinder()->GetVzPlot()->AddCurve(
       altitudeTrajectory->GetMatrix()->Element(1), DataPlot::Green);
+      
+  failSafeAltitudeSensor=uav->GetMetaUsRangeFinder();
 }
 
@@ -125 +127 @@
 
 const TargetController *UavStateMachine::GetTargetController(void) const {
-    return controller;
+  return controller;
 }
 
 MetaDualShock3 *UavStateMachine::GetJoystick(void) const {
-    return joy;
+  return joy;
 }
 
@@ -138 +140 @@
 const Vector3Df &UavStateMachine::GetCurrentAngularSpeed(void) const {
   return currentAngularSpeed;
+}
+
+void UavStateMachine::SetFailSafeAltitudeSensor(AltitudeSensor *altitudeSensor) {
+  Warn("Replacing the FailSafeAltitudeSensor with %s\n",altitudeSensor->ObjectName().c_str());
+  failSafeAltitudeSensor=altitudeSensor;
 }
 
@@ -147 +154 @@
 void UavStateMachine::FailSafeAltitudeValues(float &altitude,
                                              float &verticalSpeed) const {
-  altitude = uav->GetMetaUsRangeFinder()->z();
-  verticalSpeed = uav->GetMetaUsRangeFinder()->Vz();
+  altitude = failSafeAltitudeSensor->z();
+  verticalSpeed = failSafeAltitudeSensor->Vz();
   
+  //a mettre dans le metausrangefinder?
+  //on ne sait pas si failSafeAltitudeSensor est vrpn (dans ce cas pas besoin de rotation)
+  //ou un us range finder
   //+0.04 decalage en z de l'us en simulation
   //faire un reglage

trunk/lib/FlairMeta/src/UavStateMachine.h

@@ -26 +26 @@
 
 namespace flair {
-namespace core {
-class FrameworkManager;
-class AhrsData;
-class io_data;
-}
-namespace gui {
-class PushButton;
-class GridLayout;
-class Tab;
-class DoubleSpinBox;
-}
-namespace filter {
-class ControlLaw;
-class NestedSat;
-class Pid;
-class PidThrust;
-class TrajectoryGenerator1D;
-}
-namespace sensor {
-class TargetController;
-}
-namespace meta {
-class MetaDualShock3;
-class Uav;
-}
+  namespace core {
+    class FrameworkManager;
+    class AhrsData;
+    class io_data;
+  }
+  namespace gui {
+    class PushButton;
+    class GridLayout;
+    class Tab;
+    class DoubleSpinBox;
+  }
+  namespace filter {
+    class ControlLaw;
+    class NestedSat;
+    class Pid;
+    class PidThrust;
+    class TrajectoryGenerator1D;
+  }
+  namespace sensor {
+    class TargetController;
+    class AltitudeSensor;
+  }
+  namespace meta {
+    class MetaDualShock3;
+    class Uav;
+  }
 }
 
@@ -102 +103 @@
   };
 
-        UavStateMachine(sensor::TargetController* controller);
-        ~UavStateMachine();
+  UavStateMachine(sensor::TargetController* controller);
+  ~UavStateMachine();
 
   const core::Quaternion &GetCurrentQuaternion(void) const;
@@ -109 +110 @@
   const core::Vector3Df &GetCurrentAngularSpeed(void) const;
 
-        void Land(void);
-        void EmergencyLand(void);
-        void TakeOff(void);
-        void EmergencyStop(void);
-        //! Used to signal an event
-        /*!
-            \param event the event which occured
-        */
-        virtual void SignalEvent(Event_t event);
+  void Land(void);
+  void EmergencyLand(void);
+  void TakeOff(void);
+  void EmergencyStop(void);
+  //! Used to signal an event
+  /*!
+      \param event the event which occured
+  */
+  virtual void SignalEvent(Event_t event);
 
   virtual const core::AhrsData *GetOrientation(void) const;
@@ -126 +127 @@
   bool ExitFailSafeMode(void);
   void FailSafeAltitudeValues(float &z, float &dz) const; // in uav coordinate!
+  
+  /*!
+  * \brief Set the FailSafe AltitudeSensor
+  *
+  * by default, the failsafe AltitudeSensor is the MetaUsRangeFinder
+  * you can change it (by a vrpn one) using this method in the case us range finder is not working well
+  * use it carefully, trusting only vrpn can be risquee
+  *
+  * \param altitudeSensor altitudeSensor to use when is failsafe
+  */
+  void SetFailSafeAltitudeSensor(sensor::AltitudeSensor *altitudeSensor);
 
   gui::GridLayout *GetButtonsLayout(void) const;
@@ -306 +318 @@
   filter::PidThrust *uZ;
   filter::TrajectoryGenerator1D *altitudeTrajectory;
+  sensor::AltitudeSensor *failSafeAltitudeSensor;
 };
-};
-};
+}; // end namespace meta
+}; // end namespace flair
 #endif // UAVSTATEMACHINE_H