Changeset 432 in flair-src for trunk/lib


Ignore:
Timestamp:
Jul 12, 2021, 11:19:21 AM (5 months ago)
Author:
Sanahuja Guillaume
Message:

add AltitudeSensor? class
failsafe altitude sensor in changeable

Location:
trunk/lib
Files:
1 added
6 edited

Legend:

Unmodified
Added
Removed
  • trunk/lib/FlairMeta/src/MetaUsRangeFinder.cpp

    r429 r432  
    3737namespace flair {
    3838namespace meta {
     39 
    3940
    4041MetaUsRangeFinder::MetaUsRangeFinder(UsRangeFinder *us)
    41     : Object(us, us->ObjectName()) {
     42    : AltitudeSensor(us, us->ObjectName()) {
    4243  //TODO: mettre une rotation entre la sortie US et le low pass
    4344  //pour eviter les fausses mesures quand le drone est orienté
  • trunk/lib/FlairMeta/src/MetaUsRangeFinder.h

    r429 r432  
    1414#define METAUSRANGEFINDER_H
    1515
    16 #include <Object.h>
     16#include <AltitudeSensor.h>
    1717
    1818namespace flair {
    19 namespace filter {
    20 class ButterworthLowPass;
    21 class EulerDerivative;
    22 }
    23 namespace sensor {
    24 class UsRangeFinder;
    25 }
    26 namespace gui {
    27 class DataPlot1D;
    28 }
     19  namespace filter {
     20    class ButterworthLowPass;
     21    class EulerDerivative;
     22  }
     23  namespace sensor {
     24    class UsRangeFinder;
     25  }
     26  namespace gui {
     27    class DataPlot1D;
     28  }
    2929}
    3030
     
    3838* Cette classe est adaptée pour un capteur d'altitude.
    3939*/
    40 class MetaUsRangeFinder : public core::Object {
     40class MetaUsRangeFinder : public sensor::AltitudeSensor {
    4141public:
    4242  MetaUsRangeFinder(sensor::UsRangeFinder *us);
     
    5151private:
    5252  sensor::UsRangeFinder *us;
    53   filter::ButterworthLowPass *pbas_z, *pbas_vz;
     53  filter::ButterworthLowPass *pbas_z,*pbas_vz;
    5454  filter::EulerDerivative *vz_euler;
    5555  gui::DataPlot1D *vz_plot;
  • trunk/lib/FlairMeta/src/MetaVrpnObject.cpp

    r420 r432  
    2727#include <TabWidget.h>
    2828#include <Matrix.h>
     29#include <AltitudeSensor.h>
    2930
    3031using std::string;
     
    3334using namespace flair::sensor;
    3435using namespace flair::filter;
     36
     37namespace flair {
     38namespace sensor {
     39class MetaVrpnObjectAltitudeSensor : public AltitudeSensor {
     40  public:
     41    MetaVrpnObjectAltitudeSensor(meta::MetaVrpnObject* parent,std::string name): AltitudeSensor(parent,name) {
     42      this->parent=parent;
     43    }
     44    ~MetaVrpnObjectAltitudeSensor() {
     45    }
     46
     47    //z and dz must be in uav's frame
     48    float z(void) const {
     49      Vector3Df uav_pos;
     50      parent->GetPosition(uav_pos);
     51      return -uav_pos.z;
     52    }
     53    float Vz(void) const {
     54      Vector3Df uav_vel;
     55      parent->GetSpeed(uav_vel);
     56      return -uav_vel.z;
     57    }
     58
     59  private:
     60     meta::MetaVrpnObject* parent;
     61     
     62};
     63}// end namespace sensor
     64}// end namespace flair
    3565
    3666namespace flair {
     
    80110  xy_plot = new DataPlot2D(plot_tab->NewRow(), "xy", "y", -5, 5, "x", -5, 5);
    81111  xy_plot->AddCurve(Output()->Element(5, 0), Output()->Element(4, 0));
     112 
     113  altitudeSensor=new MetaVrpnObjectAltitudeSensor(this,name);
     114}
     115
     116sensor::AltitudeSensor *MetaVrpnObject::GetAltitudeSensor(void) const {
     117  return altitudeSensor;
    82118}
    83119
     
    102138}
    103139
    104 } // end namespace sensor
     140} // end namespace meta
    105141} // end namespace flair
  • trunk/lib/FlairMeta/src/MetaVrpnObject.h

    r420 r432  
    2929                class LowPassFilter;
    3030        }
     31  namespace sensor {
     32    class AltitudeSensor;
     33  }
    3134}
    3235
     
    5154  void GetSpeed(core::Vector3Df &speed) const;
    5255  filter::EulerDerivative* GetEulerDerivative(void) const;
     56  //get the associated AltitudeSensor
     57  //MetaVrpnObject do not derivate from AltitudeSensor because frames are differents
     58  //and this could be confusing
     59  //MetaVrpnObject is in vrpn frame (earth)
     60  //AltitudeSensor is in uav frame
     61  sensor::AltitudeSensor *GetAltitudeSensor(void) const;
    5362
    5463private:
     
    5968  gui::DataPlot1D *vx_opti_plot, *vy_opti_plot, *vz_opti_plot;
    6069  gui::Tab *plot_tab;
     70  sensor::AltitudeSensor *altitudeSensor;
    6171};
    6272} // end namespace meta
  • trunk/lib/FlairMeta/src/UavStateMachine.cpp

    r422 r432  
    5353
    5454UavStateMachine::UavStateMachine(TargetController *controller):
    55         Thread(getFrameworkManager(),"UavStateMachine",50),
    56         uav(GetUav()),controller(controller),failSafeMode(true),flagConnectionLost(false),flagBatteryLow(false),flagCriticalSensorLost(false),flagZTrajectoryFinished(false),safeToFly(true){
    57     altitudeState=AltitudeState_t::Stopped;
    58     uav->UseDefaultPlot();
     55  Thread(getFrameworkManager(),"UavStateMachine",50),
     56  uav(GetUav()),controller(controller),failSafeMode(true),flagConnectionLost(false),flagBatteryLow(false),flagCriticalSensorLost(false),flagZTrajectoryFinished(false),safeToFly(true){
     57  altitudeState=AltitudeState_t::Stopped;
     58  uav->UseDefaultPlot();
    5959
    6060  Tab *uavTab = new Tab(getFrameworkManager()->GetTabWidget(), "uav", 0);
     
    112112  uav->GetMetaUsRangeFinder()->GetVzPlot()->AddCurve(
    113113      altitudeTrajectory->GetMatrix()->Element(1), DataPlot::Green);
     114     
     115  failSafeAltitudeSensor=uav->GetMetaUsRangeFinder();
    114116}
    115117
     
    125127
    126128const TargetController *UavStateMachine::GetTargetController(void) const {
    127     return controller;
     129  return controller;
    128130}
    129131
    130132MetaDualShock3 *UavStateMachine::GetJoystick(void) const {
    131     return joy;
     133  return joy;
    132134}
    133135
     
    138140const Vector3Df &UavStateMachine::GetCurrentAngularSpeed(void) const {
    139141  return currentAngularSpeed;
     142}
     143
     144void UavStateMachine::SetFailSafeAltitudeSensor(AltitudeSensor *altitudeSensor) {
     145  Warn("Replacing the FailSafeAltitudeSensor with %s\n",altitudeSensor->ObjectName().c_str());
     146  failSafeAltitudeSensor=altitudeSensor;
    140147}
    141148
     
    147154void UavStateMachine::FailSafeAltitudeValues(float &altitude,
    148155                                             float &verticalSpeed) const {
    149   altitude = uav->GetMetaUsRangeFinder()->z();
    150   verticalSpeed = uav->GetMetaUsRangeFinder()->Vz();
     156  altitude = failSafeAltitudeSensor->z();
     157  verticalSpeed = failSafeAltitudeSensor->Vz();
    151158 
     159  //a mettre dans le metausrangefinder?
     160  //on ne sait pas si failSafeAltitudeSensor est vrpn (dans ce cas pas besoin de rotation)
     161  //ou un us range finder
    152162  //+0.04 decalage en z de l'us en simulation
    153163  //faire un reglage
  • trunk/lib/FlairMeta/src/UavStateMachine.h

    r313 r432  
    2626
    2727namespace flair {
    28 namespace core {
    29 class FrameworkManager;
    30 class AhrsData;
    31 class io_data;
    32 }
    33 namespace gui {
    34 class PushButton;
    35 class GridLayout;
    36 class Tab;
    37 class DoubleSpinBox;
    38 }
    39 namespace filter {
    40 class ControlLaw;
    41 class NestedSat;
    42 class Pid;
    43 class PidThrust;
    44 class TrajectoryGenerator1D;
    45 }
    46 namespace sensor {
    47 class TargetController;
    48 }
    49 namespace meta {
    50 class MetaDualShock3;
    51 class Uav;
    52 }
     28  namespace core {
     29    class FrameworkManager;
     30    class AhrsData;
     31    class io_data;
     32  }
     33  namespace gui {
     34    class PushButton;
     35    class GridLayout;
     36    class Tab;
     37    class DoubleSpinBox;
     38  }
     39  namespace filter {
     40    class ControlLaw;
     41    class NestedSat;
     42    class Pid;
     43    class PidThrust;
     44    class TrajectoryGenerator1D;
     45  }
     46  namespace sensor {
     47    class TargetController;
     48    class AltitudeSensor;
     49  }
     50  namespace meta {
     51    class MetaDualShock3;
     52    class Uav;
     53  }
    5354}
    5455
     
    102103  };
    103104
    104         UavStateMachine(sensor::TargetController* controller);
    105         ~UavStateMachine();
     105  UavStateMachine(sensor::TargetController* controller);
     106  ~UavStateMachine();
    106107
    107108  const core::Quaternion &GetCurrentQuaternion(void) const;
     
    109110  const core::Vector3Df &GetCurrentAngularSpeed(void) const;
    110111
    111         void Land(void);
    112         void EmergencyLand(void);
    113         void TakeOff(void);
    114         void EmergencyStop(void);
    115         //! Used to signal an event
    116         /*!
    117             \param event the event which occured
    118         */
    119         virtual void SignalEvent(Event_t event);
     112  void Land(void);
     113  void EmergencyLand(void);
     114  void TakeOff(void);
     115  void EmergencyStop(void);
     116  //! Used to signal an event
     117  /*!
     118      \param event the event which occured
     119  */
     120  virtual void SignalEvent(Event_t event);
    120121
    121122  virtual const core::AhrsData *GetOrientation(void) const;
     
    126127  bool ExitFailSafeMode(void);
    127128  void FailSafeAltitudeValues(float &z, float &dz) const; // in uav coordinate!
     129 
     130  /*!
     131  * \brief Set the FailSafe AltitudeSensor
     132  *
     133  * by default, the failsafe AltitudeSensor is the MetaUsRangeFinder
     134  * you can change it (by a vrpn one) using this method in the case us range finder is not working well
     135  * use it carefully, trusting only vrpn can be risquee
     136  *
     137  * \param altitudeSensor altitudeSensor to use when is failsafe
     138  */
     139  void SetFailSafeAltitudeSensor(sensor::AltitudeSensor *altitudeSensor);
    128140
    129141  gui::GridLayout *GetButtonsLayout(void) const;
     
    306318  filter::PidThrust *uZ;
    307319  filter::TrajectoryGenerator1D *altitudeTrajectory;
     320  sensor::AltitudeSensor *failSafeAltitudeSensor;
    308321};
    309 };
    310 };
     322}; // end namespace meta
     323}; // end namespace flair
    311324#endif // UAVSTATEMACHINE_H
Note: See TracChangeset for help on using the changeset viewer.