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


Ignore:
Timestamp:
Apr 8, 2016, 3:40:57 PM (9 years ago)
Author:
Bayard Gildas
Message:

sources reformatted with flair-format-dir script

Location:
trunk/lib/FlairMeta/src
Files:
22 edited

Legend:

Unmodified
Added
Removed

  • trunk/lib/FlairMeta/src/HdsX8.cpp

    r10 r15  
    3232using namespace flair::actuator;
    3333
    34 namespace flair { namespace meta {
     34namespace flair {
     35namespace meta {
    3536
    36 HdsX8::HdsX8(FrameworkManager* parent,string uav_name,filter::UavMultiplex *multiplex) : Uav(parent,uav_name,multiplex) {
    37     RTDM_I2cPort* i2cport=new RTDM_I2cPort(parent,"rtdm_i2c","rti2c3");
    38     RTDM_SerialPort* imu_port=new RTDM_SerialPort(parent,"imu_port","rtser1");
     37HdsX8::HdsX8(FrameworkManager *parent, string uav_name,
     38             filter::UavMultiplex *multiplex)
     39    : Uav(parent, uav_name, multiplex) {
     40  RTDM_I2cPort *i2cport = new RTDM_I2cPort(parent, "rtdm_i2c", "rti2c3");
     41  RTDM_SerialPort *imu_port = new RTDM_SerialPort(parent, "imu_port", "rtser1");
    3942
    40     if(multiplex==NULL) SetMultiplex(new X4X8Multiplex(parent,"motors",X4X8Multiplex::X8));
     43  if (multiplex == NULL)
     44    SetMultiplex(new X4X8Multiplex(parent, "motors", X4X8Multiplex::X8));
    4145
    42     SetBldc(new BlCtrlV2(GetUavMultiplex(),GetUavMultiplex()->GetLayout(),"motors",GetUavMultiplex()->MotorsCount(),i2cport));
    43     SetUsRangeFinder(new Srf08(parent,"SRF08",i2cport,0x70,60));
    44     SetAhrs(new Gx3_25_ahrs(parent,"imu",imu_port,Gx3_25_imu::EulerAnglesAndAngularRates,70));
    45     SetBatteryMonitor(((BlCtrlV2*)GetBldc())->GetBatteryMonitor());
     46  SetBldc(new BlCtrlV2(GetUavMultiplex(), GetUavMultiplex()->GetLayout(),
     47                       "motors", GetUavMultiplex()->MotorsCount(), i2cport));
     48  SetUsRangeFinder(new Srf08(parent, "SRF08", i2cport, 0x70, 60));
     49  SetAhrs(new Gx3_25_ahrs(parent, "imu", imu_port,
     50                          Gx3_25_imu::EulerAnglesAndAngularRates, 70));
     51  SetBatteryMonitor(((BlCtrlV2 *)GetBldc())->GetBatteryMonitor());
    4652
    47 /*
    48         if(VRPNType==Auto || VRPNType==AutoSerialPort)
    49         {
    50             RTDM_SerialPort* vrpn_port=new RTDM_SerialPort(parent,"vrpn_port","rtser3");
     53  /*
     54          if(VRPNType==Auto || VRPNType==AutoSerialPort)
     55          {
     56              RTDM_SerialPort* vrpn_port=new
     57     RTDM_SerialPort(parent,"vrpn_port","rtser3");
    5158
    52             vrpnclient=new VrpnClient(parent,"vrpn",vrpn_port,10000,80);
    53             uav_vrpn=new MetaVrpnObject(vrpnclient,uav_name,VRPNSerialObjectId);
    54         }
    55 */
    56     SetVerticalCamera(new Ps3Eye(parent,"camv",0,50));
     59              vrpnclient=new VrpnClient(parent,"vrpn",vrpn_port,10000,80);
     60              uav_vrpn=new
     61     MetaVrpnObject(vrpnclient,uav_name,VRPNSerialObjectId);
     62          }
     63  */
     64  SetVerticalCamera(new Ps3Eye(parent, "camv", 0, 50));
    5765}
    5866
    59 HdsX8::~HdsX8() {
    60 
    61 }
     67HdsX8::~HdsX8() {}
    6268
    6369void HdsX8::StartSensors(void) {
    64     ((Gx3_25_ahrs*)GetAhrs())->Start();
    65     ((Srf08*)GetUsRangeFinder())->Start();
    66     ((Ps3Eye *)GetVerticalCamera())->Start();
    67     Uav::StartSensors();
     70  ((Gx3_25_ahrs *)GetAhrs())->Start();
     71  ((Srf08 *)GetUsRangeFinder())->Start();
     72  ((Ps3Eye *)GetVerticalCamera())->Start();
     73  Uav::StartSensors();
    6874}
    6975

  • trunk/lib/FlairMeta/src/HdsX8.h

    r10 r15  
    1616#include "Uav.h"
    1717
    18 namespace flair
    19 {
    20 namespace meta
    21 {
    22     /*! \class HdsX8
    23     *
    24     * \brief Class defining an ardrone2 uav
    25     */
    26     class HdsX8 : public Uav {
    27         public:
    28             HdsX8(core::FrameworkManager* parent,std::string uav_name,filter::UavMultiplex *multiplex=NULL);
    29             ~HdsX8();
    30             void StartSensors(void);
     18namespace flair {
     19namespace meta {
     20/*! \class HdsX8
     21*
     22* \brief Class defining an ardrone2 uav
     23*/
     24class HdsX8 : public Uav {
     25public:
     26  HdsX8(core::FrameworkManager *parent, std::string uav_name,
     27        filter::UavMultiplex *multiplex = NULL);
     28  ~HdsX8();
     29  void StartSensors(void);
    3130
    32         private:
    33 
    34     };
     31private:
     32};
    3533} // end namespace meta
    3634} // end namespace flair

  • trunk/lib/FlairMeta/src/MetaDualShock3.cpp

    r10 r15  
    3131using namespace flair::gui;
    3232
    33 namespace flair { namespace meta {
     33namespace flair {
     34namespace meta {
    3435
    35 MetaDualShock3::MetaDualShock3(FrameworkManager* parent,string name,uint16_t port,uint8_t priority) : TargetEthController(parent,name,port,priority) {
    36     pimpl_=new MetaDualShock3_impl(this,name);
    37     parent->AddDeviceToLog(pimpl_->joy_ref);
    38     Start();
     36MetaDualShock3::MetaDualShock3(FrameworkManager *parent, string name,
     37                               uint16_t port, uint8_t priority)
     38    : TargetEthController(parent, name, port, priority) {
     39  pimpl_ = new MetaDualShock3_impl(this, name);
     40  parent->AddDeviceToLog(pimpl_->joy_ref);
     41  Start();
    3942}
    4043
    41 MetaDualShock3::~MetaDualShock3() {
    42     delete pimpl_;
    43 }
     44MetaDualShock3::~MetaDualShock3() { delete pimpl_; }
    4445
    45 AhrsData* MetaDualShock3::GetReferenceOrientation(void) const {
    46     return pimpl_->joy_ref->GetReferenceOrientation();
     46AhrsData *MetaDualShock3::GetReferenceOrientation(void) const {
     47  return pimpl_->joy_ref->GetReferenceOrientation();
    4748}
    4849
    4950void MetaDualShock3::ErrorNotify(void) {
    50     TargetEthController::FlashLed(4,10,0);
    51     TargetEthController::Rumble(0xff,20,0,0);
     51  TargetEthController::FlashLed(4, 10, 0);
     52  TargetEthController::Rumble(0xff, 20, 0, 0);
    5253}
    5354
    54 void MetaDualShock3::Rumble(uint8_t left_force,uint8_t left_timeout,uint8_t right_force,uint8_t right_timeout) {
    55     TargetEthController::Rumble(left_force,left_timeout,right_force,right_timeout);
     55void MetaDualShock3::Rumble(uint8_t left_force, uint8_t left_timeout,
     56                            uint8_t right_force, uint8_t right_timeout) {
     57  TargetEthController::Rumble(left_force, left_timeout, right_force,
     58                              right_timeout);
    5659}
    5760
    5861void MetaDualShock3::SetLedON(unsigned int ledId) {
    59     TargetEthController::SetLedOn(ledId);
     62  TargetEthController::SetLedOn(ledId);
    6063}
    6164
    6265void MetaDualShock3::SetLedOFF(unsigned int ledId) {
    63     TargetEthController::SetLedOff(ledId);
     66  TargetEthController::SetLedOff(ledId);
    6467}
    6568
    66 void MetaDualShock3::FlashLed(unsigned int ledId,uint8_t on_timeout,uint8_t off_timeout) {
    67     TargetEthController::FlashLed(ledId,on_timeout,off_timeout);
     69void MetaDualShock3::FlashLed(unsigned int ledId, uint8_t on_timeout,
     70                              uint8_t off_timeout) {
     71  TargetEthController::FlashLed(ledId, on_timeout, off_timeout);
    6872}
    6973
    70 float MetaDualShock3::ZRef(void) const {
    71     return pimpl_->joy_ref->ZRef();
    72 }
     74float MetaDualShock3::ZRef(void) const { return pimpl_->joy_ref->ZRef(); }
    7375
    74 float MetaDualShock3::DzRef(void) const {
    75     return pimpl_->joy_ref->DzRef();
    76 }
     76float MetaDualShock3::DzRef(void) const { return pimpl_->joy_ref->DzRef(); }
    7777
    7878void MetaDualShock3::SetYawRef(float value) {
    79     pimpl_->joy_ref->SetYawRef(value);
     79  pimpl_->joy_ref->SetYawRef(value);
    8080}
    8181
    8282void MetaDualShock3::SetYawRef(Quaternion const &value) {
    83     pimpl_->joy_ref->SetYawRef(value);
     83  pimpl_->joy_ref->SetYawRef(value);
    8484}
    8585
    86 void MetaDualShock3::SetZRef(float value) {
    87     pimpl_->joy_ref->SetZRef(value);
    88 }
     86void MetaDualShock3::SetZRef(float value) { pimpl_->joy_ref->SetZRef(value); }
    8987
    9088float MetaDualShock3::RollTrim(void) const {
    91     return pimpl_->joy_ref->RollTrim();
     89  return pimpl_->joy_ref->RollTrim();
    9290}
    9391
    9492float MetaDualShock3::PitchTrim(void) const {
    95     return pimpl_->joy_ref->PitchTrim();
     93  return pimpl_->joy_ref->PitchTrim();
    9694}
    9795

  • trunk/lib/FlairMeta/src/MetaDualShock3.h

    r10 r15  
    1717
    1818namespace flair {
    19     namespace core {
    20         class AhrsData;
    21         class Quaternion;
    22     }
    23     namespace filter {
    24         class Ahrs;
    25     }
     19namespace core {
     20class AhrsData;
     21class Quaternion;
     22}
     23namespace filter {
     24class Ahrs;
     25}
    2626}
    2727
    … …  
    3131namespace meta {
    3232
    33     /*! \class MetaDualShock3
    34     *
    35     * \brief Classe intégrant la manette MetaDualShock3
    36     */
    37     class MetaDualShock3 : public sensor::TargetEthController {
    38         friend class ::MetaDualShock3_impl;
     33/*! \class MetaDualShock3
     34*
     35* \brief Classe intégrant la manette MetaDualShock3
     36*/
     37class MetaDualShock3 : public sensor::TargetEthController {
     38  friend class ::MetaDualShock3_impl;
    3939
    40         public:
    41             MetaDualShock3(core::FrameworkManager* parent,std::string name,uint16_t port,uint8_t priority);
    42             ~MetaDualShock3();
    43             core::AhrsData* GetReferenceOrientation(void) const;
    44             float ZRef(void) const;
    45             float DzRef(void) const;
    46             void SetYawRef(float value);
    47             /*!
    48             * \brief Set yaw reference
    49             *
    50             * Yaw part of the output quaternion is obtained by integrating the wz desired angular speed.\n
    51             * This method reset the yaw.
    52             *
    53             * \param value value, only the yaw part of the quaternion is used
    54             */
    55             void SetYawRef(core::Quaternion const &value);
    56             void SetZRef(float value);
    57             float RollTrim(void) const;
    58             float PitchTrim(void) const;
    59             void ErrorNotify(void);
    60             void Rumble(uint8_t left_force,uint8_t left_timeout=20,uint8_t right_force=0,uint8_t right_timeout=0);
    61             void SetLedON(unsigned int ledId);
    62             void SetLedOFF(unsigned int ledId);
    63             void FlashLed(unsigned int ledId,uint8_t on_timeout,uint8_t off_timeout);
     40public:
     41  MetaDualShock3(core::FrameworkManager *parent, std::string name,
     42                 uint16_t port, uint8_t priority);
     43  ~MetaDualShock3();
     44  core::AhrsData *GetReferenceOrientation(void) const;
     45  float ZRef(void) const;
     46  float DzRef(void) const;
     47  void SetYawRef(float value);
     48  /*!
     49  * \brief Set yaw reference
     50  *
     51  * Yaw part of the output quaternion is obtained by integrating the wz desired
     52  *angular speed.\n
     53  * This method reset the yaw.
     54  *
     55  * \param value value, only the yaw part of the quaternion is used
     56  */
     57  void SetYawRef(core::Quaternion const &value);
     58  void SetZRef(float value);
     59  float RollTrim(void) const;
     60  float PitchTrim(void) const;
     61  void ErrorNotify(void);
     62  void Rumble(uint8_t left_force, uint8_t left_timeout = 20,
     63              uint8_t right_force = 0, uint8_t right_timeout = 0);
     64  void SetLedON(unsigned int ledId);
     65  void SetLedOFF(unsigned int ledId);
     66  void FlashLed(unsigned int ledId, uint8_t on_timeout, uint8_t off_timeout);
    6467
    65         private:
    66             class MetaDualShock3_impl* pimpl_;
    67 
    68     };
     68private:
     69  class MetaDualShock3_impl *pimpl_;
     70};
    6971} // end namespace meta
    7072} // end namespace flair

  • trunk/lib/FlairMeta/src/MetaDualShock3_impl.cpp

    r10 r15  
    2929using namespace flair::meta;
    3030
    31 MetaDualShock3_impl::MetaDualShock3_impl(MetaDualShock3* self,string name): IODevice((IODevice*)self,name) {
    32     joy_ref=new JoyReference(self->GetTab()->NewRow(),"consignes joy");
    33     this->self=self;
    34     joy_init=false;
    35     wasRollTrimUpButtonPressed=false;
    36     wasRollTrimDownButtonPressed=false;
    37     wasPitchTrimUpButtonPressed=false;
    38     wasPitchTrimDownButtonPressed=false;
     31MetaDualShock3_impl::MetaDualShock3_impl(MetaDualShock3 *self, string name)
     32    : IODevice((IODevice *)self, name) {
     33  joy_ref = new JoyReference(self->GetTab()->NewRow(), "consignes joy");
     34  this->self = self;
     35  joy_init = false;
     36  wasRollTrimUpButtonPressed = false;
     37  wasRollTrimDownButtonPressed = false;
     38  wasPitchTrimUpButtonPressed = false;
     39  wasPitchTrimDownButtonPressed = false;
    3940}
    4041
    41 MetaDualShock3_impl::~MetaDualShock3_impl() {
     42MetaDualShock3_impl::~MetaDualShock3_impl() {}
    4243
     44// receives updates from the controler
     45void MetaDualShock3_impl::UpdateFrom(const io_data *data) {
     46  cvmatrix *input = (cvmatrix *)data;
     47
     48  // on prend une fois pour toute le mutex et on fait des accès directs
     49  input->GetMutex();
     50
     51  // up
     52  if (self->IsButtonPressed(12)) {
     53    if (!wasPitchTrimDownButtonPressed) {
     54      joy_ref->PitchTrimDown();
     55      wasPitchTrimDownButtonPressed = true;
     56    }
     57  } else {
     58    wasPitchTrimDownButtonPressed = false;
     59  }
     60
     61  // down
     62  if (self->IsButtonPressed(13)) {
     63    if (!wasPitchTrimUpButtonPressed) {
     64      joy_ref->PitchTrimUp();
     65      wasPitchTrimUpButtonPressed = true;
     66    }
     67  } else {
     68    wasPitchTrimUpButtonPressed = false;
     69  }
     70
     71  // right
     72  if (self->IsButtonPressed(15)) {
     73    if (!wasRollTrimUpButtonPressed) {
     74      joy_ref->RollTrimUp();
     75      wasRollTrimUpButtonPressed = true;
     76    }
     77  } else {
     78    wasRollTrimUpButtonPressed = false;
     79  }
     80
     81  // left
     82  if (self->IsButtonPressed(14)) {
     83    if (!wasRollTrimDownButtonPressed) {
     84      joy_ref->RollTrimDown();
     85      wasRollTrimDownButtonPressed = true;
     86    }
     87  } else {
     88    wasRollTrimDownButtonPressed = false;
     89  }
     90
     91  if (!getFrameworkManager()->ConnectionLost()) {
     92    input->GetMutex();
     93    joy_ref->SetRollAxis(input->ValueNoMutex(0, 0));
     94    joy_ref->SetPitchAxis(input->ValueNoMutex(1, 0));
     95    joy_ref->SetYawAxis(input->ValueNoMutex(2, 0));
     96    joy_ref->SetAltitudeAxis(input->ValueNoMutex(3, 0));
     97    input->ReleaseMutex();
     98  } else {
     99    joy_ref->SetRollAxis(0);
     100    joy_ref->SetPitchAxis(0);
     101    joy_ref->SetYawAxis(0);
     102    joy_ref->SetAltitudeAxis(0);
     103  }
     104  input->ReleaseMutex();
     105
     106  joy_ref->Update(data->DataTime());
     107
     108  if (!joy_init) {
     109    self->TargetEthController::FlashLed(1, 10, 10);
     110    joy_init = true;
     111  }
    43112}
    44 
    45 //receives updates from the controler
    46 void MetaDualShock3_impl::UpdateFrom(const io_data *data) {
    47     cvmatrix *input=(cvmatrix*)data;
    48 
    49     //on prend une fois pour toute le mutex et on fait des accès directs
    50     input->GetMutex();
    51 
    52     //up
    53     if(self->IsButtonPressed(12)) {
    54         if(!wasPitchTrimDownButtonPressed) {
    55             joy_ref->PitchTrimDown();
    56             wasPitchTrimDownButtonPressed=true;
    57         }
    58     } else {
    59         wasPitchTrimDownButtonPressed=false;
    60     }
    61 
    62     //down
    63     if(self->IsButtonPressed(13)) {
    64         if(!wasPitchTrimUpButtonPressed) {
    65             joy_ref->PitchTrimUp();
    66             wasPitchTrimUpButtonPressed=true;
    67         }
    68     } else {
    69         wasPitchTrimUpButtonPressed=false;
    70     }
    71 
    72     //right
    73     if(self->IsButtonPressed(15)) {
    74         if(!wasRollTrimUpButtonPressed) {
    75             joy_ref->RollTrimUp();
    76             wasRollTrimUpButtonPressed=true;
    77         }
    78     } else {
    79         wasRollTrimUpButtonPressed=false;
    80     }
    81 
    82     //left
    83     if(self->IsButtonPressed(14)) {
    84         if(!wasRollTrimDownButtonPressed) {
    85             joy_ref->RollTrimDown();
    86             wasRollTrimDownButtonPressed=true;
    87         }
    88     } else {
    89         wasRollTrimDownButtonPressed=false;
    90     }
    91 
    92     if(!getFrameworkManager()->ConnectionLost()) {
    93         input->GetMutex();
    94         joy_ref->SetRollAxis(input->ValueNoMutex(0,0));
    95         joy_ref->SetPitchAxis(input->ValueNoMutex(1,0));
    96         joy_ref->SetYawAxis(input->ValueNoMutex(2,0));
    97         joy_ref->SetAltitudeAxis(input->ValueNoMutex(3,0));
    98         input->ReleaseMutex();
    99     } else {
    100         joy_ref->SetRollAxis(0);
    101         joy_ref->SetPitchAxis(0);
    102         joy_ref->SetYawAxis(0);
    103         joy_ref->SetAltitudeAxis(0);
    104     }
    105     input->ReleaseMutex();
    106 
    107     joy_ref->Update(data->DataTime());
    108 
    109     if(!joy_init) {
    110         self->TargetEthController::FlashLed(1,10,10);
    111         joy_init=true;
    112     }
    113 }

  • trunk/lib/FlairMeta/src/MetaUsRangeFinder.cpp

    r10 r15  
    3535using namespace flair::sensor;
    3636
    37 namespace flair { namespace meta
    38 {
     37namespace flair {
     38namespace meta {
    3939
    40 MetaUsRangeFinder::MetaUsRangeFinder(UsRangeFinder* us): Object(us,us->ObjectName()) {
    41     this->us=us;
    42     pbas_z=new ButterworthLowPass(us,us->GetLayout()->NewRow(),"Passe bas",3);
    43     vz_euler=new EulerDerivative(pbas_z,us->GetLayout()->NewRow(),"Vz");
    44     pbas_vz=new ButterworthLowPass(vz_euler,us->GetLayout()->NewRow(),"Passe bas v",3);
    45 
     40MetaUsRangeFinder::MetaUsRangeFinder(UsRangeFinder *us)
     41    : Object(us, us->ObjectName()) {
     42  this->us = us;
     43  pbas_z =
     44      new ButterworthLowPass(us, us->GetLayout()->NewRow(), "Passe bas", 3);
     45  vz_euler = new EulerDerivative(pbas_z, us->GetLayout()->NewRow(), "Vz");
     46  pbas_vz = new ButterworthLowPass(vz_euler, us->GetLayout()->NewRow(),
     47                                   "Passe bas v", 3);
    4648}
    4749
    48 MetaUsRangeFinder::~MetaUsRangeFinder() {
     50MetaUsRangeFinder::~MetaUsRangeFinder() {}
    4951
     52void MetaUsRangeFinder::UseDefaultPlot(void) {
     53  us->UseDefaultPlot();
     54
     55  us->GetPlot()->AddCurve(pbas_z->Matrix()->Element(0), DataPlot::Blue);
     56
     57  vz_plot = new DataPlot1D(us->GetPlotTab()->LastRowLastCol(), "vz", -2, 2);
     58  vz_plot->AddCurve(vz_euler->Matrix()->Element(0));
     59  vz_plot->AddCurve(pbas_vz->Matrix()->Element(0), DataPlot::Blue);
    5060}
    5161
    52 void MetaUsRangeFinder::UseDefaultPlot(void) {
    53     us->UseDefaultPlot();
     62gui::DataPlot1D *MetaUsRangeFinder::GetZPlot() { return us->GetPlot(); }
    5463
    55     us->GetPlot()->AddCurve(pbas_z->Matrix()->Element(0),DataPlot::Blue);
     64gui::DataPlot1D *MetaUsRangeFinder::GetVzPlot() { return vz_plot; }
    5665
    57     vz_plot=new DataPlot1D(us->GetPlotTab()->LastRowLastCol(),"vz",-2,2);
    58     vz_plot->AddCurve(vz_euler->Matrix()->Element(0));
    59     vz_plot->AddCurve(pbas_vz->Matrix()->Element(0),DataPlot::Blue);
    60 }
     66float MetaUsRangeFinder::z(void) const { return pbas_z->Output(); }
    6167
    62 gui::DataPlot1D *MetaUsRangeFinder::GetZPlot() {
    63     return us->GetPlot();
    64 }
    65 
    66 gui::DataPlot1D *MetaUsRangeFinder::GetVzPlot() {
    67     return vz_plot;
    68 }
    69 
    70 float MetaUsRangeFinder::z(void) const {
    71     return pbas_z->Output();
    72 }
    73 
    74 float MetaUsRangeFinder::Vz(void) const {
    75     return pbas_vz->Output();
    76 }
     68float MetaUsRangeFinder::Vz(void) const { return pbas_vz->Output(); }
    7769
    7870} // end namespace sensor

  • trunk/lib/FlairMeta/src/MetaUsRangeFinder.h

    r10 r15  
    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     }
     19namespace filter {
     20class ButterworthLowPass;
     21class EulerDerivative;
     22}
     23namespace sensor {
     24class UsRangeFinder;
     25}
     26namespace gui {
     27class DataPlot1D;
     28}
    2929}
    3030
    31 namespace flair
    32 {
    33 namespace meta
    34 {
    35     /*! \class MetaUsRangeFinder
    36     *
    37     * \brief Classe haut niveau pour capteur à ultra son
    38     *
    39     * Contient une dérivée d'euler et un passe bas.
    40     * Cette classe est adaptée pour un capteur d'altitude.
    41     */
    42     class MetaUsRangeFinder: public core::Object {
    43         public:
    44             MetaUsRangeFinder(sensor::UsRangeFinder* us);
    45             ~MetaUsRangeFinder();
    46             void UseDefaultPlot(void);
    47             float z(void) const;
    48             float Vz(void) const;
    49             gui::DataPlot1D *GetZPlot();
    50             gui::DataPlot1D *GetVzPlot();
     31namespace flair {
     32namespace meta {
     33/*! \class MetaUsRangeFinder
     34*
     35* \brief Classe haut niveau pour capteur à ultra son
     36*
     37* Contient une dérivée d'euler et un passe bas.
     38* Cette classe est adaptée pour un capteur d'altitude.
     39*/
     40class MetaUsRangeFinder : public core::Object {
     41public:
     42  MetaUsRangeFinder(sensor::UsRangeFinder *us);
     43  ~MetaUsRangeFinder();
     44  void UseDefaultPlot(void);
     45  float z(void) const;
     46  float Vz(void) const;
     47  gui::DataPlot1D *GetZPlot();
     48  gui::DataPlot1D *GetVzPlot();
    5149
    52         private:
    53             sensor::UsRangeFinder* us;
    54             filter::ButterworthLowPass *pbas_z,*pbas_vz;
    55             filter::EulerDerivative *vz_euler;
    56             gui::DataPlot1D* vz_plot;
    57     };
     50private:
     51  sensor::UsRangeFinder *us;
     52  filter::ButterworthLowPass *pbas_z, *pbas_vz;
     53  filter::EulerDerivative *vz_euler;
     54  gui::DataPlot1D *vz_plot;
     55};
    5856} // end namespace meta
    5957} // end namespace flair

  • trunk/lib/FlairMeta/src/MetaVrpnObject.cpp

    r10 r15  
    3636using namespace flair::filter;
    3737
    38 namespace flair { namespace meta {
     38namespace flair {
     39namespace meta {
    3940
    40 MetaVrpnObject::MetaVrpnObject(const VrpnClient *parent,string name): VrpnObject(parent,name,parent->GetTabWidget())
    41 {
    42     ConstructorCommon(parent,name);
     41MetaVrpnObject::MetaVrpnObject(const VrpnClient *parent, string name)
     42    : VrpnObject(parent, name, parent->GetTabWidget()) {
     43  ConstructorCommon(parent, name);
    4344}
    4445
    45 MetaVrpnObject::MetaVrpnObject(const VrpnClient *parent,std::string name,uint8_t id): VrpnObject(parent,name,id,parent->GetTabWidget())
    46 {
    47     ConstructorCommon(parent,name);
     46MetaVrpnObject::MetaVrpnObject(const VrpnClient *parent, std::string name,
     47                               uint8_t id)
     48    : VrpnObject(parent, name, id, parent->GetTabWidget()) {
     49  ConstructorCommon(parent, name);
    4850}
    4951
    50 void MetaVrpnObject::ConstructorCommon(const VrpnClient *parent,string name) {
    51     cvmatrix_descriptor* desc=new cvmatrix_descriptor(6,1);
    52     for(int i=0;i<6;i++) {
    53         desc->SetElementName(i,0,Output()->Name(i,0));
    54     }
    55     cvmatrix* prev_value=new cvmatrix(this,desc,elementDataType,name);
    56     for(int i=0;i<6;i++) {
    57         prev_value->SetValue(i,0,0);
    58     }
     52void MetaVrpnObject::ConstructorCommon(const VrpnClient *parent, string name) {
     53  cvmatrix_descriptor *desc = new cvmatrix_descriptor(6, 1);
     54  for (int i = 0; i < 6; i++) {
     55    desc->SetElementName(i, 0, Output()->Name(i, 0));
     56  }
     57  cvmatrix *prev_value = new cvmatrix(this, desc, elementDataType, name);
     58  for (int i = 0; i < 6; i++) {
     59    prev_value->SetValue(i, 0, 0);
     60  }
    5961
    60     pbas=new LowPassFilter(this,parent->GetLayout()->NewRow(),name + " Passe bas",prev_value);
     62  pbas = new LowPassFilter(this, parent->GetLayout()->NewRow(),
     63                           name + " Passe bas", prev_value);
    6164
    62     desc=new cvmatrix_descriptor(6,1);
    63     for(int i=0;i<6;i++) {
    64         desc->SetElementName(i,0,"d" + Output()->Name(i,0));
    65     }
    66     prev_value=new cvmatrix(this,desc,elementDataType,name);
    67     for(int i=0;i<6;i++) {
    68         prev_value->SetValue(i,0,0);
    69     }
     65  desc = new cvmatrix_descriptor(6, 1);
     66  for (int i = 0; i < 6; i++) {
     67    desc->SetElementName(i, 0, "d" + Output()->Name(i, 0));
     68  }
     69  prev_value = new cvmatrix(this, desc, elementDataType, name);
     70  for (int i = 0; i < 6; i++) {
     71    prev_value->SetValue(i, 0, 0);
     72  }
    7073
    71     euler=new EulerDerivative(pbas,parent->GetLayout()->NewRow(),name + "_euler",prev_value);
     74  euler = new EulerDerivative(pbas, parent->GetLayout()->NewRow(),
     75                              name + "_euler", prev_value);
    7276
    73     vx_opti_plot=new DataPlot1D(GetPlotTab()->NewRow(),"vx",-3,3);
    74     vx_opti_plot->AddCurve(euler->Matrix()->Element(3));
    75     vy_opti_plot=new DataPlot1D(GetPlotTab()->LastRowLastCol(),"vy",-3,3);
    76     vy_opti_plot->AddCurve(euler->Matrix()->Element(4));
    77     vz_opti_plot=new DataPlot1D(GetPlotTab()->LastRowLastCol(),"vz",-2,2);
    78     vz_opti_plot->AddCurve(euler->Matrix()->Element(5));
     77  vx_opti_plot = new DataPlot1D(GetPlotTab()->NewRow(), "vx", -3, 3);
     78  vx_opti_plot->AddCurve(euler->Matrix()->Element(3));
     79  vy_opti_plot = new DataPlot1D(GetPlotTab()->LastRowLastCol(), "vy", -3, 3);
     80  vy_opti_plot->AddCurve(euler->Matrix()->Element(4));
     81  vz_opti_plot = new DataPlot1D(GetPlotTab()->LastRowLastCol(), "vz", -2, 2);
     82  vz_opti_plot->AddCurve(euler->Matrix()->Element(5));
    7983
    80     plot_tab=new Tab(parent->GetTabWidget(),"Mesures (xy) "+ name);
    81     xy_plot=new DataPlot2D(plot_tab->NewRow(),"xy","y",-5,5,"x",-5,5);
    82         xy_plot->AddCurve(Output()->Element(4,0),Output()->Element(3,0));
     84  plot_tab = new Tab(parent->GetTabWidget(), "Mesures (xy) " + name);
     85  xy_plot = new DataPlot2D(plot_tab->NewRow(), "xy", "y", -5, 5, "x", -5, 5);
     86  xy_plot->AddCurve(Output()->Element(4, 0), Output()->Element(3, 0));
    8387}
    8488
    85 MetaVrpnObject::~MetaVrpnObject() {
    86     delete plot_tab;
    87 }
     89MetaVrpnObject::~MetaVrpnObject() { delete plot_tab; }
    8890
    89 DataPlot1D* MetaVrpnObject::VxPlot(void) const
    90 {
    91     return vx_opti_plot;
    92 }
     91DataPlot1D *MetaVrpnObject::VxPlot(void) const { return vx_opti_plot; }
    9392
    94 DataPlot1D* MetaVrpnObject::VyPlot(void) const
    95 {
    96     return vy_opti_plot;
    97 }
     93DataPlot1D *MetaVrpnObject::VyPlot(void) const { return vy_opti_plot; }
    9894
    99 DataPlot1D* MetaVrpnObject::VzPlot(void) const
    100 {
    101     return vz_opti_plot;
    102 }
     95DataPlot1D *MetaVrpnObject::VzPlot(void) const { return vz_opti_plot; }
    10396
    104 DataPlot2D* MetaVrpnObject::XyPlot(void) const {
    105     return xy_plot;
    106 }
     97DataPlot2D *MetaVrpnObject::XyPlot(void) const { return xy_plot; }
    10798
    108 void MetaVrpnObject::GetSpeed(Vector3D &speed) const
    109 {
    110     speed.x=euler->Output(3,0);
    111     speed.y=euler->Output(4,0);
    112     speed.z=euler->Output(5,0);
     99void MetaVrpnObject::GetSpeed(Vector3D &speed) const {
     100  speed.x = euler->Output(3, 0);
     101  speed.y = euler->Output(4, 0);
     102  speed.z = euler->Output(5, 0);
    113103}
    114104

  • trunk/lib/FlairMeta/src/MetaVrpnObject.h

    r10 r15  
    1717#include "io_data.h"
    1818
    19 namespace flair
    20 {
    21     namespace core
    22     {
    23         class Vector3D;
    24         class FloatType;
    25     }
    26     namespace gui
    27     {
    28         class DataPlot1D;
    29         class DataPlot2D;
    30         class Tab;
    31     }
    32     namespace filter
    33     {
    34         class EulerDerivative;
    35         class LowPassFilter;
    36     }
    37     namespace sensor
    38     {
    39         class VrpnClient;
    40     }
     19namespace flair {
     20namespace core {
     21class Vector3D;
     22class FloatType;
     23}
     24namespace gui {
     25class DataPlot1D;
     26class DataPlot2D;
     27class Tab;
     28}
     29namespace filter {
     30class EulerDerivative;
     31class LowPassFilter;
     32}
     33namespace sensor {
     34class VrpnClient;
     35}
    4136}
    4237
    43 namespace flair
    44 {
    45 namespace meta
    46 {
     38namespace flair {
     39namespace meta {
    4740
    48     /*! \class MetaVrpnObject
    49     *
    50     * \brief Classe haut niveau intégrant un objet VRPN
    51     *
    52     * Contient un objet VRPN et une dérivée, d'euler.
    53     */
    54     class MetaVrpnObject: public sensor::VrpnObject {
    55         public:
    56             MetaVrpnObject(const sensor::VrpnClient *parent,std::string name);
    57             MetaVrpnObject(const sensor::VrpnClient *parent,std::string name,uint8_t id);
    58             ~MetaVrpnObject();
    59             gui::DataPlot1D* VxPlot(void) const;//1,0
    60             gui::DataPlot1D* VyPlot(void) const;//1,1
    61             gui::DataPlot1D* VzPlot(void) const;//1,2
    62             gui::DataPlot2D* XyPlot(void) const;
    63             void GetSpeed(core::Vector3D &speed) const;
     41/*! \class MetaVrpnObject
     42*
     43* \brief Classe haut niveau intégrant un objet VRPN
     44*
     45* Contient un objet VRPN et une dérivée, d'euler.
     46*/
     47class MetaVrpnObject : public sensor::VrpnObject {
     48public:
     49  MetaVrpnObject(const sensor::VrpnClient *parent, std::string name);
     50  MetaVrpnObject(const sensor::VrpnClient *parent, std::string name,
     51                 uint8_t id);
     52  ~MetaVrpnObject();
     53  gui::DataPlot1D *VxPlot(void) const; // 1,0
     54  gui::DataPlot1D *VyPlot(void) const; // 1,1
     55  gui::DataPlot1D *VzPlot(void) const; // 1,2
     56  gui::DataPlot2D *XyPlot(void) const;
     57  void GetSpeed(core::Vector3D &speed) const;
    6458
    65         private:
    66             void ConstructorCommon(const sensor::VrpnClient *parent,std::string name);
    67             filter::LowPassFilter *pbas;
    68             filter::EulerDerivative *euler;
    69             gui::DataPlot2D *xy_plot;
    70             gui::DataPlot1D *vx_opti_plot,*vy_opti_plot,*vz_opti_plot;
    71             gui::Tab* plot_tab;
    72             core::FloatType elementDataType;
    73 
    74     };
     59private:
     60  void ConstructorCommon(const sensor::VrpnClient *parent, std::string name);
     61  filter::LowPassFilter *pbas;
     62  filter::EulerDerivative *euler;
     63  gui::DataPlot2D *xy_plot;
     64  gui::DataPlot1D *vx_opti_plot, *vy_opti_plot, *vz_opti_plot;
     65  gui::Tab *plot_tab;
     66  core::FloatType elementDataType;
     67};
    7568} // end namespace meta
    7669} // end namespace flair

  • trunk/lib/FlairMeta/src/SimuX4.cpp

    r10 r15  
    3434using namespace flair::actuator;
    3535
    36 namespace flair { namespace meta {
     36namespace flair {
     37namespace meta {
    3738
    38 SimuX4::SimuX4(FrameworkManager* parent,string uav_name,int simu_id,filter::UavMultiplex *multiplex) : Uav(parent,uav_name,multiplex) {
     39SimuX4::SimuX4(FrameworkManager *parent, string uav_name, int simu_id,
     40               filter::UavMultiplex *multiplex)
     41    : Uav(parent, uav_name, multiplex) {
    3942
    40     if(multiplex==NULL) SetMultiplex(new X4X8Multiplex(parent,"motors",X4X8Multiplex::X4));
     43  if (multiplex == NULL)
     44    SetMultiplex(new X4X8Multiplex(parent, "motors", X4X8Multiplex::X4));
    4145
    42     SetBldc(new SimuBldc(GetUavMultiplex(),GetUavMultiplex()->GetLayout(),"motors",GetUavMultiplex()->MotorsCount(),simu_id));
    43     SetUsRangeFinder(new SimuUs(parent,"us",simu_id,60));
    44     SetAhrs(new SimuAhrs(parent,"imu",simu_id,70));
    45     Tab* bat_tab=new Tab(parent->GetTabWidget(),"battery");
    46     SetBatteryMonitor(new BatteryMonitor(bat_tab->NewRow(),"battery"));
    47     GetBatteryMonitor()->SetBatteryValue(12);
    48     SetVerticalCamera(new SimuCamera(parent, "simu_cam_v", 320, 240, 3, simu_id, 10));
     46  SetBldc(new SimuBldc(GetUavMultiplex(), GetUavMultiplex()->GetLayout(),
     47                       "motors", GetUavMultiplex()->MotorsCount(), simu_id));
     48  SetUsRangeFinder(new SimuUs(parent, "us", simu_id, 60));
     49  SetAhrs(new SimuAhrs(parent, "imu", simu_id, 70));
     50  Tab *bat_tab = new Tab(parent->GetTabWidget(), "battery");
     51  SetBatteryMonitor(new BatteryMonitor(bat_tab->NewRow(), "battery"));
     52  GetBatteryMonitor()->SetBatteryValue(12);
     53  SetVerticalCamera(
     54      new SimuCamera(parent, "simu_cam_v", 320, 240, 3, simu_id, 10));
    4955}
    5056
    51 SimuX4::~SimuX4() {
     57SimuX4::~SimuX4() {}
    5258
     59void SimuX4::StartSensors(void) {
     60  ((SimuAhrs *)GetAhrs())->Start();
     61  ((SimuUs *)GetUsRangeFinder())->Start();
     62  ((SimuCamera *)GetVerticalCamera())->Start();
     63  Uav::StartSensors();
    5364}
    5465
    55 void SimuX4::StartSensors(void) {
    56     ((SimuAhrs*)GetAhrs())->Start();
    57     ((SimuUs*)GetUsRangeFinder())->Start();
    58     ((SimuCamera *)GetVerticalCamera())->Start();
    59     Uav::StartSensors();
    60 }
    61 
    62 void SimuX4::SetupVRPNAutoIP(string name) {
    63     SetupVRPN("127.0.0.1:3883",name);
    64 }
     66void SimuX4::SetupVRPNAutoIP(string name) { SetupVRPN("127.0.0.1:3883", name); }
    6567
    6668} // end namespace meta

  • trunk/lib/FlairMeta/src/SimuX4.h

    r10 r15  
    1616#include "Uav.h"
    1717
    18 namespace flair
    19 {
    20 namespace meta
    21 {
    22     /*! \class SimuX4
    23     *
    24     * \brief Class defining a simultation x4 uav
    25     */
    26     class SimuX4 : public Uav {
    27         public:
    28             //simu_id: 0 if simulating only one UAV
    29             //>0 otherwise
    30             SimuX4(core::FrameworkManager* parent,std::string uav_name,int simu_id=0,filter::UavMultiplex *multiplex=NULL);
    31             ~SimuX4();
    32             void StartSensors(void);
    33             void SetupVRPNAutoIP(std::string name);
    34     };
     18namespace flair {
     19namespace meta {
     20/*! \class SimuX4
     21*
     22* \brief Class defining a simultation x4 uav
     23*/
     24class SimuX4 : public Uav {
     25public:
     26  // simu_id: 0 if simulating only one UAV
     27  //>0 otherwise
     28  SimuX4(core::FrameworkManager *parent, std::string uav_name, int simu_id = 0,
     29         filter::UavMultiplex *multiplex = NULL);
     30  ~SimuX4();
     31  void StartSensors(void);
     32  void SetupVRPNAutoIP(std::string name);
     33};
    3534} // end namespace meta
    3635} // end namespace flair

  • trunk/lib/FlairMeta/src/SimuX8.cpp

    r10 r15  
    3434using namespace flair::actuator;
    3535
    36 namespace flair { namespace meta {
     36namespace flair {
     37namespace meta {
    3738
    38 SimuX8::SimuX8(FrameworkManager* parent,string uav_name,int simu_id,filter::UavMultiplex *multiplex) : Uav(parent,uav_name,multiplex) {
     39SimuX8::SimuX8(FrameworkManager *parent, string uav_name, int simu_id,
     40               filter::UavMultiplex *multiplex)
     41    : Uav(parent, uav_name, multiplex) {
    3942
    40     if(multiplex==NULL) SetMultiplex(new X4X8Multiplex(parent,"motors",X4X8Multiplex::X8));
     43  if (multiplex == NULL)
     44    SetMultiplex(new X4X8Multiplex(parent, "motors", X4X8Multiplex::X8));
    4145
    42     SetBldc(new SimuBldc(GetUavMultiplex(),GetUavMultiplex()->GetLayout(),"motors",GetUavMultiplex()->MotorsCount(),simu_id));
    43     SetUsRangeFinder(new SimuUs(parent,"us",simu_id,60));
    44     SetAhrs(new SimuAhrs(parent,"imu",simu_id,70));
    45     Tab* bat_tab=new Tab(parent->GetTabWidget(),"battery");
    46     SetBatteryMonitor(new BatteryMonitor(bat_tab->NewRow(),"battery"));
    47     GetBatteryMonitor()->SetBatteryValue(12);
    48     SetVerticalCamera(new SimuCamera(parent, "simu_cam_v", 320, 240, 3, simu_id, 10));
     46  SetBldc(new SimuBldc(GetUavMultiplex(), GetUavMultiplex()->GetLayout(),
     47                       "motors", GetUavMultiplex()->MotorsCount(), simu_id));
     48  SetUsRangeFinder(new SimuUs(parent, "us", simu_id, 60));
     49  SetAhrs(new SimuAhrs(parent, "imu", simu_id, 70));
     50  Tab *bat_tab = new Tab(parent->GetTabWidget(), "battery");
     51  SetBatteryMonitor(new BatteryMonitor(bat_tab->NewRow(), "battery"));
     52  GetBatteryMonitor()->SetBatteryValue(12);
     53  SetVerticalCamera(
     54      new SimuCamera(parent, "simu_cam_v", 320, 240, 3, simu_id, 10));
    4955}
    5056
    51 SimuX8::~SimuX8() {
     57SimuX8::~SimuX8() {}
    5258
     59void SimuX8::StartSensors(void) {
     60  ((SimuAhrs *)GetAhrs())->Start();
     61  ((SimuUs *)GetUsRangeFinder())->Start();
     62  ((SimuCamera *)GetVerticalCamera())->Start();
     63  Uav::StartSensors();
    5364}
    5465
    55 void SimuX8::StartSensors(void) {
    56     ((SimuAhrs*)GetAhrs())->Start();
    57     ((SimuUs*)GetUsRangeFinder())->Start();
    58     ((SimuCamera *)GetVerticalCamera())->Start();
    59     Uav::StartSensors();
    60 }
    61 
    62 void SimuX8::SetupVRPNAutoIP(string name) {
    63     SetupVRPN("127.0.0.1:3883",name);
    64 }
     66void SimuX8::SetupVRPNAutoIP(string name) { SetupVRPN("127.0.0.1:3883", name); }
    6567
    6668} // end namespace meta

  • trunk/lib/FlairMeta/src/SimuX8.h

    r10 r15  
    1616#include "Uav.h"
    1717
    18 namespace flair
    19 {
    20 namespace meta
    21 {
    22     /*! \class SimuX8
    23     *
    24     * \brief Class defining a simultation x8 uav
    25     */
    26     class SimuX8 : public Uav {
    27         public:
    28             //simu_id: 0 if simulating only one UAV
    29             //>0 otherwise
    30             SimuX8(core::FrameworkManager* parent,std::string uav_name,int simu_id=0,filter::UavMultiplex *multiplex=NULL);
    31             ~SimuX8();
    32             void StartSensors(void);
    33             void SetupVRPNAutoIP(std::string name);
    34     };
     18namespace flair {
     19namespace meta {
     20/*! \class SimuX8
     21*
     22* \brief Class defining a simultation x8 uav
     23*/
     24class SimuX8 : public Uav {
     25public:
     26  // simu_id: 0 if simulating only one UAV
     27  //>0 otherwise
     28  SimuX8(core::FrameworkManager *parent, std::string uav_name, int simu_id = 0,
     29         filter::UavMultiplex *multiplex = NULL);
     30  ~SimuX8();
     31  void StartSensors(void);
     32  void SetupVRPNAutoIP(std::string name);
     33};
    3534} // end namespace meta
    3635} // end namespace flair

  • trunk/lib/FlairMeta/src/Uav.cpp

    r10 r15  
    3838using namespace flair::actuator;
    3939
    40 namespace flair { namespace meta {
     40namespace flair {
     41namespace meta {
    4142
    42 Uav::Uav(FrameworkManager* parent,string name,UavMultiplex *multiplex): Object(parent,name) {
    43     vrpnclient=NULL;
    44     uav_vrpn=NULL;
    45     verticalCamera=NULL;
    46     this->multiplex=multiplex;
     43Uav::Uav(FrameworkManager *parent, string name, UavMultiplex *multiplex)
     44    : Object(parent, name) {
     45  vrpnclient = NULL;
     46  uav_vrpn = NULL;
     47  verticalCamera = NULL;
     48  this->multiplex = multiplex;
    4749}
    4850
    49 Uav::~Uav() {
    50 }
     51Uav::~Uav() {}
    5152
    5253void Uav::SetUsRangeFinder(const UsRangeFinder *inUs) {
    53     us=(UsRangeFinder*)inUs;
    54     meta_us=new MetaUsRangeFinder(us);
    55     getFrameworkManager()->AddDeviceToLog(us);
     54  us = (UsRangeFinder *)inUs;
     55  meta_us = new MetaUsRangeFinder(us);
     56  getFrameworkManager()->AddDeviceToLog(us);
    5657}
    5758
    5859void Uav::SetAhrs(const Ahrs *inAhrs) {
    59     ahrs=(Ahrs*)inAhrs;
    60     imu=(Imu*)ahrs->GetImu();
    61     getFrameworkManager()->AddDeviceToLog(imu);
     60  ahrs = (Ahrs *)inAhrs;
     61  imu = (Imu *)ahrs->GetImu();
     62  getFrameworkManager()->AddDeviceToLog(imu);
    6263}
    6364
    64 void Uav::SetBldc(const Bldc* inBldc) {
    65     bldc=(Bldc*)inBldc;
    66 }
     65void Uav::SetBldc(const Bldc *inBldc) { bldc = (Bldc *)inBldc; }
    6766
    68 void Uav::SetBatteryMonitor(const BatteryMonitor* inBattery) {
    69     battery=(BatteryMonitor*)inBattery;
     67void Uav::SetBatteryMonitor(const BatteryMonitor *inBattery) {
     68  battery = (BatteryMonitor *)inBattery;
    7069}
    7170
    7271void Uav::SetMultiplex(const UavMultiplex *inMultiplex) {
    73     multiplex=(UavMultiplex*)inMultiplex;
    74     getFrameworkManager()->AddDeviceToLog(multiplex);
     72  multiplex = (UavMultiplex *)inMultiplex;
     73  getFrameworkManager()->AddDeviceToLog(multiplex);
    7574}
    7675
    77 void Uav::SetVerticalCamera(const Camera* inVerticalCamera) {
    78     verticalCamera=(Camera*)inVerticalCamera;
     76void Uav::SetVerticalCamera(const Camera *inVerticalCamera) {
     77  verticalCamera = (Camera *)inVerticalCamera;
    7978}
    8079/*
    81 void Uav::SetupVRPNSerial(SerialPort *vrpn_port,string name,int VRPNSerialObjectId) {
     80void Uav::SetupVRPNSerial(SerialPort *vrpn_port,string name,int
     81VRPNSerialObjectId) {
    8282    vrpnclient=new VrpnClient(getFrameworkManager(),"vrpn",vrpn_port,10000,80);
    8383    uav_vrpn=new MetaVrpnObject(vrpnclient,name,VRPNSerialObjectId);
    … …  
    8787*/
    8888void Uav::SetupVRPNAutoIP(string name) {
    89     SetupVRPN("192.168.147.197:3883",name);
     89  SetupVRPN("192.168.147.197:3883", name);
    9090}
    9191
    92 void Uav::SetupVRPN(string optitrack_address,string name) {
    93     vrpnclient=new VrpnClient(getFrameworkManager(),"vrpn",optitrack_address,10000,80);
    94     uav_vrpn=new MetaVrpnObject(vrpnclient,name);
     92void Uav::SetupVRPN(string optitrack_address, string name) {
     93  vrpnclient = new VrpnClient(getFrameworkManager(), "vrpn", optitrack_address,
     94                              10000, 80);
     95  uav_vrpn = new MetaVrpnObject(vrpnclient, name);
    9596
    96     getFrameworkManager()->AddDeviceToLog(uav_vrpn);
     97  getFrameworkManager()->AddDeviceToLog(uav_vrpn);
    9798
    98     GetAhrs()->YawPlot()->AddCurve(uav_vrpn->State()->Element(2),DataPlot::Green);
    99     //GetAhrs()->RollPlot()->AddCurve(uav_vrpn->State()->Element(0),DataPlot::Green);
    100     //GetAhrs()->PitchPlot()->AddCurve(uav_vrpn->State()->Element(1),DataPlot::Green);
     99  GetAhrs()->YawPlot()->AddCurve(uav_vrpn->State()->Element(2),
     100                                 DataPlot::Green);
     101  // GetAhrs()->RollPlot()->AddCurve(uav_vrpn->State()->Element(0),DataPlot::Green);
     102  // GetAhrs()->PitchPlot()->AddCurve(uav_vrpn->State()->Element(1),DataPlot::Green);
    101103}
    102104
    103 void Uav::StartSensors(void)  {
    104     if(vrpnclient!=NULL) {
    105         vrpnclient->Start();
    106     }
     105void Uav::StartSensors(void) {
     106  if (vrpnclient != NULL) {
     107    vrpnclient->Start();
     108  }
    107109}
    108110void Uav::UseDefaultPlot(void) {
    109     multiplex->UseDefaultPlot();
     111  multiplex->UseDefaultPlot();
    110112
    111     if(bldc->HasSpeedMeasurement()) {
    112         Tab* plot_tab=new Tab(multiplex->GetTabWidget(),"Speeds");
    113         DataPlot1D* plots[4];
    114         plots[0]=new DataPlot1D(plot_tab->NewRow(),"front left",0,7000);
    115         plots[1]=new DataPlot1D(plot_tab->LastRowLastCol(),"front right",0,7000);
    116         plots[2]=new DataPlot1D(plot_tab->NewRow(),"rear left",0,7000);
    117         plots[3]=new DataPlot1D(plot_tab->LastRowLastCol(),"rear right",0,7000);
     113  if (bldc->HasSpeedMeasurement()) {
     114    Tab *plot_tab = new Tab(multiplex->GetTabWidget(), "Speeds");
     115    DataPlot1D *plots[4];
     116    plots[0] = new DataPlot1D(plot_tab->NewRow(), "front left", 0, 7000);
     117    plots[1] =
     118        new DataPlot1D(plot_tab->LastRowLastCol(), "front right", 0, 7000);
     119    plots[2] = new DataPlot1D(plot_tab->NewRow(), "rear left", 0, 7000);
     120    plots[3] =
     121        new DataPlot1D(plot_tab->LastRowLastCol(), "rear right", 0, 7000);
    118122
    119         if(bldc->MotorsCount()==8) {
    120             for(int i=0;i<4;i++) plots[i]->AddCurve(bldc->Output()->Element(multiplex->MultiplexValue(i),0),DataPlot::Red,"top");
    121             for(int i=0;i<4;i++) plots[i]->AddCurve(bldc->Output()->Element(multiplex->MultiplexValue(i+4),0),DataPlot::Blue,"bottom");
    122         } else {
    123             for(int i=0;i<4;i++) plots[i]->AddCurve(bldc->Output()->Element(multiplex->MultiplexValue(i),0));
    124         }
     123    if (bldc->MotorsCount() == 8) {
     124      for (int i = 0; i < 4; i++)
     125        plots[i]->AddCurve(
     126            bldc->Output()->Element(multiplex->MultiplexValue(i), 0),
     127            DataPlot::Red, "top");
     128      for (int i = 0; i < 4; i++)
     129        plots[i]->AddCurve(
     130            bldc->Output()->Element(multiplex->MultiplexValue(i + 4), 0),
     131            DataPlot::Blue, "bottom");
     132    } else {
     133      for (int i = 0; i < 4; i++)
     134        plots[i]->AddCurve(
     135            bldc->Output()->Element(multiplex->MultiplexValue(i), 0));
    125136    }
     137  }
    126138
    127     if(bldc->HasCurrentMeasurement()) {
    128         Tab* plot_tab=new Tab(multiplex->GetTabWidget(),"Currents");
    129         DataPlot1D* plots[4];
    130         plots[0]=new DataPlot1D(plot_tab->NewRow(),"front left",0,10);
    131         plots[1]=new DataPlot1D(plot_tab->LastRowLastCol(),"front right",0,10);
    132         plots[2]=new DataPlot1D(plot_tab->NewRow(),"rear left",0,10);
    133         plots[3]=new DataPlot1D(plot_tab->LastRowLastCol(),"rear right",0,10);
     139  if (bldc->HasCurrentMeasurement()) {
     140    Tab *plot_tab = new Tab(multiplex->GetTabWidget(), "Currents");
     141    DataPlot1D *plots[4];
     142    plots[0] = new DataPlot1D(plot_tab->NewRow(), "front left", 0, 10);
     143    plots[1] = new DataPlot1D(plot_tab->LastRowLastCol(), "front right", 0, 10);
     144    plots[2] = new DataPlot1D(plot_tab->NewRow(), "rear left", 0, 10);
     145    plots[3] = new DataPlot1D(plot_tab->LastRowLastCol(), "rear right", 0, 10);
    134146
    135         if(bldc->MotorsCount()==8) {
    136             for(int i=0;i<4;i++) plots[i]->AddCurve(bldc->Output()->Element(multiplex->MultiplexValue(i),1),DataPlot::Red,"top");
    137             for(int i=0;i<4;i++) plots[i]->AddCurve(bldc->Output()->Element(multiplex->MultiplexValue(i+4),1),DataPlot::Blue,"bottom");
    138         } else {
    139             for(int i=0;i<4;i++) plots[i]->AddCurve(bldc->Output()->Element(multiplex->MultiplexValue(i),1));
    140         }
     147    if (bldc->MotorsCount() == 8) {
     148      for (int i = 0; i < 4; i++)
     149        plots[i]->AddCurve(
     150            bldc->Output()->Element(multiplex->MultiplexValue(i), 1),
     151            DataPlot::Red, "top");
     152      for (int i = 0; i < 4; i++)
     153        plots[i]->AddCurve(
     154            bldc->Output()->Element(multiplex->MultiplexValue(i + 4), 1),
     155            DataPlot::Blue, "bottom");
     156    } else {
     157      for (int i = 0; i < 4; i++)
     158        plots[i]->AddCurve(
     159            bldc->Output()->Element(multiplex->MultiplexValue(i), 1));
    141160    }
     161  }
    142162
    143     meta_us->UseDefaultPlot();
    144     ahrs->UseDefaultPlot();
     163  meta_us->UseDefaultPlot();
     164  ahrs->UseDefaultPlot();
    145165}
    146166
    147 UavMultiplex* Uav::GetUavMultiplex(void) const {
    148     return multiplex;
     167UavMultiplex *Uav::GetUavMultiplex(void) const { return multiplex; }
     168
     169Bldc *Uav::GetBldc(void) const { return bldc; }
     170
     171Ahrs *Uav::GetAhrs(void) const { return ahrs; }
     172
     173Imu *Uav::GetImu(void) const { return imu; }
     174
     175MetaUsRangeFinder *Uav::GetMetaUsRangeFinder(void) const { return meta_us; }
     176
     177UsRangeFinder *Uav::GetUsRangeFinder(void) const { return us; }
     178
     179BatteryMonitor *Uav::GetBatteryMonitor(void) const { return battery; }
     180
     181VrpnClient *Uav::GetVrpnClient(void) const {
     182  if (vrpnclient == NULL)
     183    Err("vrpn is not setup, call SetupVRPN before\n");
     184  return vrpnclient;
    149185}
    150186
    151 Bldc* Uav::GetBldc(void) const {
    152     return bldc;
    153 }
     187MetaVrpnObject *Uav::GetVrpnObject(void) const { return uav_vrpn; }
    154188
    155 Ahrs* Uav::GetAhrs(void) const {
    156     return ahrs;
    157 }
    158 
    159 Imu* Uav::GetImu(void) const {
    160     return imu;
    161 }
    162 
    163 MetaUsRangeFinder* Uav::GetMetaUsRangeFinder(void) const {
    164     return meta_us;
    165 }
    166 
    167 UsRangeFinder* Uav::GetUsRangeFinder(void) const {
    168     return us;
    169 }
    170 
    171 BatteryMonitor* Uav::GetBatteryMonitor(void) const {
    172     return battery;
    173 }
    174 
    175 VrpnClient* Uav::GetVrpnClient(void) const {
    176     if(vrpnclient==NULL) Err("vrpn is not setup, call SetupVRPN before\n");
    177     return vrpnclient;
    178 }
    179 
    180 MetaVrpnObject* Uav::GetVrpnObject(void) const {
    181     return uav_vrpn;
    182 }
    183 
    184 Camera* Uav::GetVerticalCamera(void) const {
    185     return verticalCamera;
    186 }
     189Camera *Uav::GetVerticalCamera(void) const { return verticalCamera; }
    187190
    188191} // end namespace meta

  • trunk/lib/FlairMeta/src/Uav.h

    r10 r15  
    1717
    1818namespace flair {
    19     namespace core {
    20         class FrameworkManager;
    21     }
    22     namespace filter {
    23         class Ahrs;
    24         class UavMultiplex;
    25     }
    26     namespace actuator {
    27         class Bldc;
    28     }
    29     namespace sensor {
    30         class UsRangeFinder;
    31         class BatteryMonitor;
    32         class VrpnClient;
    33         class Imu;
    34         class Camera;
    35     }
     19namespace core {
     20class FrameworkManager;
     21}
     22namespace filter {
     23class Ahrs;
     24class UavMultiplex;
     25}
     26namespace actuator {
     27class Bldc;
     28}
     29namespace sensor {
     30class UsRangeFinder;
     31class BatteryMonitor;
     32class VrpnClient;
     33class Imu;
     34class Camera;
     35}
    3636}
    3737
    3838namespace flair {
    3939namespace meta {
    40     class MetaUsRangeFinder;
    41     class MetaVrpnObject;
     40class MetaUsRangeFinder;
     41class MetaVrpnObject;
    4242
    43     /*! \class Uav
    44     *
    45     * \brief Base class to construct sensors/actuators depending on uav type
    46     */
    47     class Uav: public core::Object {
    48         public:
    49         /*
    50             typedef enum {
    51                 None,
    52                 Auto,//rt serial if hds x4 ou x8, auto ip sinon
    53                 AutoIP,
    54                 UserDefinedIP,
    55                 AutoSerialPort,
    56                 } VRPNType_t;
     43/*! \class Uav
     44*
     45* \brief Base class to construct sensors/actuators depending on uav type
    5746*/
    58             //uav_name: for optitrack
    59             Uav(core::FrameworkManager* parent,std::string name,filter::UavMultiplex *multiplex=NULL);
    60             ~Uav();
    61             void SetupVRPN(std::string optitrack_address,std::string name);
    62             //vrpn serial: broken, need to add serial port in uav specific code
    63             //void SetupVRPNSerial(core::SerialPort *vrpn_port,std::string name,int VRPNSerialObjectId);
    64             virtual void SetupVRPNAutoIP(std::string name);
     47class Uav : public core::Object {
     48public:
     49  /*
     50      typedef enum {
     51          None,
     52          Auto,//rt serial if hds x4 ou x8, auto ip sinon
     53          AutoIP,
     54          UserDefinedIP,
     55          AutoSerialPort,
     56          } VRPNType_t;
     57*/
     58  // uav_name: for optitrack
     59  Uav(core::FrameworkManager *parent, std::string name,
     60      filter::UavMultiplex *multiplex = NULL);
     61  ~Uav();
     62  void SetupVRPN(std::string optitrack_address, std::string name);
     63  // vrpn serial: broken, need to add serial port in uav specific code
     64  // void SetupVRPNSerial(core::SerialPort *vrpn_port,std::string name,int
     65  // VRPNSerialObjectId);
     66  virtual void SetupVRPNAutoIP(std::string name);
    6567
    66             virtual void StartSensors(void);
    67             void UseDefaultPlot(void);
    68             actuator::Bldc* GetBldc(void) const;
    69             filter::UavMultiplex* GetUavMultiplex(void) const;
    70             sensor::Imu* GetImu(void) const;
    71             filter::Ahrs* GetAhrs(void) const;
    72             MetaUsRangeFinder* GetMetaUsRangeFinder(void) const;
    73             sensor::UsRangeFinder* GetUsRangeFinder(void) const;
    74             sensor::BatteryMonitor* GetBatteryMonitor(void) const;
    75             sensor::VrpnClient* GetVrpnClient(void) const;
    76             meta::MetaVrpnObject* GetVrpnObject(void) const;
    77             sensor::Camera* GetVerticalCamera(void) const;
     68  virtual void StartSensors(void);
     69  void UseDefaultPlot(void);
     70  actuator::Bldc *GetBldc(void) const;
     71  filter::UavMultiplex *GetUavMultiplex(void) const;
     72  sensor::Imu *GetImu(void) const;
     73  filter::Ahrs *GetAhrs(void) const;
     74  MetaUsRangeFinder *GetMetaUsRangeFinder(void) const;
     75  sensor::UsRangeFinder *GetUsRangeFinder(void) const;
     76  sensor::BatteryMonitor *GetBatteryMonitor(void) const;
     77  sensor::VrpnClient *GetVrpnClient(void) const;
     78  meta::MetaVrpnObject *GetVrpnObject(void) const;
     79  sensor::Camera *GetVerticalCamera(void) const;
    7880
    79         protected:
    80             void SetBldc(const actuator::Bldc *bldc);
    81             void SetMultiplex(const filter::UavMultiplex *multiplex);
    82             void SetAhrs(const filter::Ahrs *ahrs);
    83             void SetUsRangeFinder(const sensor::UsRangeFinder *us);
    84             void SetBatteryMonitor(const sensor::BatteryMonitor *battery);
    85             void SetVerticalCamera(const sensor::Camera *verticalCamera);
     81protected:
     82  void SetBldc(const actuator::Bldc *bldc);
     83  void SetMultiplex(const filter::UavMultiplex *multiplex);
     84  void SetAhrs(const filter::Ahrs *ahrs);
     85  void SetUsRangeFinder(const sensor::UsRangeFinder *us);
     86  void SetBatteryMonitor(const sensor::BatteryMonitor *battery);
     87  void SetVerticalCamera(const sensor::Camera *verticalCamera);
    8688
    87         private:
    88             sensor::Imu* imu;
    89             filter::Ahrs* ahrs;
    90             actuator::Bldc* bldc;
    91             filter::UavMultiplex *multiplex;
    92             sensor::UsRangeFinder* us;
    93             MetaUsRangeFinder* meta_us;
    94             sensor::VrpnClient *vrpnclient;
    95             MetaVrpnObject* uav_vrpn;
    96             sensor::BatteryMonitor* battery;
    97             sensor::Camera* verticalCamera;
    98     };
     89private:
     90  sensor::Imu *imu;
     91  filter::Ahrs *ahrs;
     92  actuator::Bldc *bldc;
     93  filter::UavMultiplex *multiplex;
     94  sensor::UsRangeFinder *us;
     95  MetaUsRangeFinder *meta_us;
     96  sensor::VrpnClient *vrpnclient;
     97  MetaVrpnObject *uav_vrpn;
     98  sensor::BatteryMonitor *battery;
     99  sensor::Camera *verticalCamera;
     100};
    99101} // end namespace meta
    100102} // end namespace flair

  • trunk/lib/FlairMeta/src/UavFactory.cpp

    r10 r15  
    2929using namespace flair::meta;
    3030
    31 Uav* CreateUav(FrameworkManager* parent,string uav_name,string uav_type,UavMultiplex *multiplex) {
    32     /*if(uav_type=="ardrone2") {
     31Uav *CreateUav(FrameworkManager *parent, string uav_name, string uav_type,
     32               UavMultiplex *multiplex) {
     33  /*if(uav_type=="ardrone2") {
    3334        return new ArDrone2(parent,uav_name,multiplex);
    34     } else */if(uav_type=="hds_x4") {
    35         parent->Err("UAV type %s not yet implemented\n",uav_type.c_str());
    36         return NULL;
    37     } else if(uav_type=="hds_x8") {
    38         return new HdsX8(parent,uav_name,multiplex);
    39     } else if(uav_type=="xair") {
    40         return new XAir(parent,uav_name,multiplex);
    41     } else if(uav_type=="hds_xufo") {
    42         parent->Err("UAV type %s not yet implemented\n",uav_type.c_str());
    43         return NULL;
    44     } else if(uav_type.compare(0,7,"x4_simu")==0) {
    45         int simu_id=0;
    46         if(uav_type.size()>7) {
    47             simu_id=atoi(uav_type.substr (7,uav_type.size()-7).c_str());
    48         }
    49         return new SimuX4(parent,uav_name,simu_id,multiplex);
    50     } else if(uav_type.compare(0,7,"x8_simu") == 0) {
    51         int simu_id=0;
    52         if(uav_type.size()>7) {
    53             simu_id=atoi(uav_type.substr (7,uav_type.size()-7).c_str());
    54         }
    55         return new SimuX8(parent,uav_name,simu_id,multiplex);
    56     } else {
    57         parent->Err("UAV type %s unknown\n",uav_type.c_str());
    58         return NULL;
     35    } else */ if (uav_type == "hds_x4") {
     36    parent->Err("UAV type %s not yet implemented\n", uav_type.c_str());
     37    return NULL;
     38  } else if (uav_type == "hds_x8") {
     39    return new HdsX8(parent, uav_name, multiplex);
     40  } else if (uav_type == "xair") {
     41    return new XAir(parent, uav_name, multiplex);
     42  } else if (uav_type == "hds_xufo") {
     43    parent->Err("UAV type %s not yet implemented\n", uav_type.c_str());
     44    return NULL;
     45  } else if (uav_type.compare(0, 7, "x4_simu") == 0) {
     46    int simu_id = 0;
     47    if (uav_type.size() > 7) {
     48      simu_id = atoi(uav_type.substr(7, uav_type.size() - 7).c_str());
    5949    }
     50    return new SimuX4(parent, uav_name, simu_id, multiplex);
     51  } else if (uav_type.compare(0, 7, "x8_simu") == 0) {
     52    int simu_id = 0;
     53    if (uav_type.size() > 7) {
     54      simu_id = atoi(uav_type.substr(7, uav_type.size() - 7).c_str());
     55    }
     56    return new SimuX8(parent, uav_name, simu_id, multiplex);
     57  } else {
     58    parent->Err("UAV type %s unknown\n", uav_type.c_str());
     59    return NULL;
     60  }
    6061}

  • trunk/lib/FlairMeta/src/UavFactory.h

    r10 r15  
    2121#include <Uav.h>
    2222
    23 flair::meta::Uav* CreateUav(flair::core::FrameworkManager* parent,std::string uav_name,std::string uav_type,flair::filter::UavMultiplex *multiplex=NULL);
     23flair::meta::Uav *CreateUav(flair::core::FrameworkManager *parent,
     24                            std::string uav_name, std::string uav_type,
     25                            flair::filter::UavMultiplex *multiplex = NULL);
    2426
    2527#endif // UAVFACTORY

  • trunk/lib/FlairMeta/src/UavStateMachine.cpp

    r10 r15  
    5151using namespace flair::meta;
    5252
    53 UavStateMachine::UavStateMachine(Uav* inUav,uint16_t ds3Port):
    54         Thread(getFrameworkManager(),"UavStateMachine",50),
    55         uav(inUav),failSafeMode(true),flagConnectionLost(false),flagBatteryLow(false),flagZTrajectoryFinished(false) {
    56     altitudeState=AltitudeState_t::Stopped;
    57     uav->UseDefaultPlot();
    58 
    59     Tab *uavTab=new Tab(getFrameworkManager()->GetTabWidget(),"uav",0);
    60     buttonslayout=new GridLayout(uavTab->NewRow(),"buttons");
    61     button_kill=new PushButton(buttonslayout->NewRow(),"kill");
    62     button_start_log=new PushButton(buttonslayout->NewRow(),"start_log");
    63     button_stop_log=new PushButton(buttonslayout->LastRowLastCol(),"stop_log");
    64     button_take_off=new PushButton(buttonslayout->NewRow(),"take_off");
    65     button_land=new PushButton(buttonslayout->LastRowLastCol(),"land");
    66 
    67     Tab *lawTab=new Tab(getFrameworkManager()->GetTabWidget(),"control laws");
    68     TabWidget *tabWidget=new TabWidget(lawTab->NewRow(),"laws");
    69     setupLawTab=new Tab(tabWidget,"Setup");
    70     graphLawTab=new Tab(tabWidget,"Graphes");
    71 
    72     uRoll=new NestedSat(setupLawTab->At(0,0),"u_roll");
    73     uRoll->ConvertSatFromDegToRad();
    74     uRoll->UseDefaultPlot(graphLawTab->NewRow());
    75 
    76     uPitch=new NestedSat(setupLawTab->At(0,1),"u_pitch");
    77     uPitch->ConvertSatFromDegToRad();
    78     uPitch->UseDefaultPlot(graphLawTab->LastRowLastCol());
    79 
    80     uYaw=new Pid(setupLawTab->At(0,2),"u_yaw");
    81     uYaw->UseDefaultPlot(graphLawTab->LastRowLastCol());
    82 
    83     uZ=new PidThrust(setupLawTab->At(1,2),"u_z");
    84     uZ->UseDefaultPlot(graphLawTab->LastRowLastCol());
    85 
    86     getFrameworkManager()->AddDeviceToLog(uZ);
    87     uZ->AddDeviceToLog(uRoll);
    88     uZ->AddDeviceToLog(uPitch);
    89     uZ->AddDeviceToLog(uYaw);
    90 
    91     joy=new MetaDualShock3(getFrameworkManager(),"dualshock3",ds3Port,30);
    92     uav->GetAhrs()->AddPlot(joy->GetReferenceOrientation(),DataPlot::Blue);
    93 
    94     altitudeMode=AltitudeMode_t::Manual;
    95     orientationMode=OrientationMode_t::Manual;
    96     thrustMode=ThrustMode_t::Default;
    97     torqueMode=TorqueMode_t::Default;
    98 
    99     GroupBox* reglagesGroupbox=new GroupBox(uavTab->NewRow(),"takeoff/landing");
    100     desiredTakeoffAltitude=new DoubleSpinBox(reglagesGroupbox->NewRow(),"desired takeoff altitude"," m",0,5,0.1,2,1);
    101     desiredLandingAltitude=new DoubleSpinBox(reglagesGroupbox->LastRowLastCol(),"desired landing altitude"," m",0,1,0.1,1);
    102     altitudeTrajectory=new TrajectoryGenerator1D(uavTab->NewRow(),"alt cons","m");
    103     uav->GetMetaUsRangeFinder()->GetZPlot()->AddCurve(altitudeTrajectory->Matrix()->Element(0),DataPlot::Green);
    104     uav->GetMetaUsRangeFinder()->GetVzPlot()->AddCurve(altitudeTrajectory->Matrix()->Element(1),DataPlot::Green);
    105 }
    106 
    107 UavStateMachine::~UavStateMachine() {
    108 }
    109 
    110 void UavStateMachine::AddDeviceToControlLawLog(const IODevice* device) {
    111     uZ->AddDeviceToLog(device);
    112 }
    113 
    114 void UavStateMachine::AddDataToControlLawLog(const core::io_data* data) {
    115     uZ->AddDataToLog(data);
    116 }
    117 
    118 const TargetController *UavStateMachine::GetJoystick(void) const {
    119     return joy;
    120 }
     53UavStateMachine::UavStateMachine(Uav *inUav, uint16_t ds3Port)
     54    : Thread(getFrameworkManager(), "UavStateMachine", 50), uav(inUav),
     55      failSafeMode(true), flagConnectionLost(false), flagBatteryLow(false),
     56      flagZTrajectoryFinished(false) {
     57  altitudeState = AltitudeState_t::Stopped;
     58  uav->UseDefaultPlot();
     59
     60  Tab *uavTab = new Tab(getFrameworkManager()->GetTabWidget(), "uav", 0);
     61  buttonslayout = new GridLayout(uavTab->NewRow(), "buttons");
     62  button_kill = new PushButton(buttonslayout->NewRow(), "kill");
     63  button_start_log = new PushButton(buttonslayout->NewRow(), "start_log");
     64  button_stop_log = new PushButton(buttonslayout->LastRowLastCol(), "stop_log");
     65  button_take_off = new PushButton(buttonslayout->NewRow(), "take_off");
     66  button_land = new PushButton(buttonslayout->LastRowLastCol(), "land");
     67
     68  Tab *lawTab = new Tab(getFrameworkManager()->GetTabWidget(), "control laws");
     69  TabWidget *tabWidget = new TabWidget(lawTab->NewRow(), "laws");
     70  setupLawTab = new Tab(tabWidget, "Setup");
     71  graphLawTab = new Tab(tabWidget, "Graphes");
     72
     73  uRoll = new NestedSat(setupLawTab->At(0, 0), "u_roll");
     74  uRoll->ConvertSatFromDegToRad();
     75  uRoll->UseDefaultPlot(graphLawTab->NewRow());
     76
     77  uPitch = new NestedSat(setupLawTab->At(0, 1), "u_pitch");
     78  uPitch->ConvertSatFromDegToRad();
     79  uPitch->UseDefaultPlot(graphLawTab->LastRowLastCol());
     80
     81  uYaw = new Pid(setupLawTab->At(0, 2), "u_yaw");
     82  uYaw->UseDefaultPlot(graphLawTab->LastRowLastCol());
     83
     84  uZ = new PidThrust(setupLawTab->At(1, 2), "u_z");
     85  uZ->UseDefaultPlot(graphLawTab->LastRowLastCol());
     86
     87  getFrameworkManager()->AddDeviceToLog(uZ);
     88  uZ->AddDeviceToLog(uRoll);
     89  uZ->AddDeviceToLog(uPitch);
     90  uZ->AddDeviceToLog(uYaw);
     91
     92  joy = new MetaDualShock3(getFrameworkManager(), "dualshock3", ds3Port, 30);
     93  uav->GetAhrs()->AddPlot(joy->GetReferenceOrientation(), DataPlot::Blue);
     94
     95  altitudeMode = AltitudeMode_t::Manual;
     96  orientationMode = OrientationMode_t::Manual;
     97  thrustMode = ThrustMode_t::Default;
     98  torqueMode = TorqueMode_t::Default;
     99
     100  GroupBox *reglagesGroupbox =
     101      new GroupBox(uavTab->NewRow(), "takeoff/landing");
     102  desiredTakeoffAltitude =
     103      new DoubleSpinBox(reglagesGroupbox->NewRow(), "desired takeoff altitude",
     104                        " m", 0, 5, 0.1, 2, 1);
     105  desiredLandingAltitude =
     106      new DoubleSpinBox(reglagesGroupbox->LastRowLastCol(),
     107                        "desired landing altitude", " m", 0, 1, 0.1, 1);
     108  altitudeTrajectory =
     109      new TrajectoryGenerator1D(uavTab->NewRow(), "alt cons", "m");
     110  uav->GetMetaUsRangeFinder()->GetZPlot()->AddCurve(
     111      altitudeTrajectory->Matrix()->Element(0), DataPlot::Green);
     112  uav->GetMetaUsRangeFinder()->GetVzPlot()->AddCurve(
     113      altitudeTrajectory->Matrix()->Element(1), DataPlot::Green);
     114}
     115
     116UavStateMachine::~UavStateMachine() {}
     117
     118void UavStateMachine::AddDeviceToControlLawLog(const IODevice *device) {
     119  uZ->AddDeviceToLog(device);
     120}
     121
     122void UavStateMachine::AddDataToControlLawLog(const core::io_data *data) {
     123  uZ->AddDataToLog(data);
     124}
     125
     126const TargetController *UavStateMachine::GetJoystick(void) const { return joy; }
    121127
    122128const Quaternion &UavStateMachine::GetCurrentQuaternion(void) const {
    123             return currentQuaternion;
     129  return currentQuaternion;
    124130}
    125131
    126132const Vector3D &UavStateMachine::GetCurrentAngularSpeed(void) const {
    127     return currentAngularSpeed;
    128 }
    129 
    130 const Uav *UavStateMachine::GetUav(void) const {
    131     return uav;
    132 }
    133 
    134 void UavStateMachine::AltitudeValues(float &altitude,float &verticalSpeed) const{
    135     FailSafeAltitudeValues(altitude, verticalSpeed);
    136 }
    137 
    138 void UavStateMachine::FailSafeAltitudeValues(float &altitude,float &verticalSpeed) const {
    139     altitude=uav->GetMetaUsRangeFinder()->z();
    140     verticalSpeed=uav->GetMetaUsRangeFinder()->Vz();
     133  return currentAngularSpeed;
     134}
     135
     136const Uav *UavStateMachine::GetUav(void) const { return uav; }
     137
     138void UavStateMachine::AltitudeValues(float &altitude,
     139                                     float &verticalSpeed) const {
     140  FailSafeAltitudeValues(altitude, verticalSpeed);
     141}
     142
     143void UavStateMachine::FailSafeAltitudeValues(float &altitude,
     144                                             float &verticalSpeed) const {
     145  altitude = uav->GetMetaUsRangeFinder()->z();
     146  verticalSpeed = uav->GetMetaUsRangeFinder()->Vz();
    141147}
    142148
    143149void UavStateMachine::Run() {
    144     WarnUponSwitches(true);
    145     uav->StartSensors();
    146 
    147     if(getFrameworkManager()->ErrorOccured()==true) {
    148         SafeStop();
    149     }
    150 
    151     while(!ToBeStopped()) {
    152         SecurityCheck();
    153 
    154         //get controller inputs
    155         CheckJoystick();
    156         CheckPushButton();
    157 
    158         if(IsPeriodSet()) {
    159             WaitPeriod();
    160         } else {
    161             WaitUpdate(uav->GetAhrs());
    162         }
    163         needToComputeDefaultTorques=true;
    164         needToComputeDefaultThrust=true;
    165 
    166         SignalEvent(Event_t::EnteringControlLoop);
    167 
    168         ComputeOrientation();
    169         ComputeAltitude();
    170 
    171         //compute thrust and torques to apply
    172         ComputeTorques();
    173         ComputeThrust();//logs are added to uz, so it must be updated at last
    174 
    175         // Set torques for roll, pitch and yaw angles (value between -1 and 1). Set thrust (value between 0 and 1)
    176         uav->GetUavMultiplex()->SetRoll(-currentTorques.roll);
    177         uav->GetUavMultiplex()->SetPitch(-currentTorques.pitch);
    178         uav->GetUavMultiplex()->SetYaw(-currentTorques.yaw);
    179         uav->GetUavMultiplex()->SetThrust(-currentThrust);//on raisonne en negatif sur l'altitude, a revoir avec les equations
    180         uav->GetUavMultiplex()->SetRollTrim(joy->RollTrim());
    181         uav->GetUavMultiplex()->SetPitchTrim(joy->PitchTrim());
    182         uav->GetUavMultiplex()->SetYawTrim(0);
    183         uav->GetUavMultiplex()->Update(GetTime());
    184     }
    185 
    186     WarnUponSwitches(false);
     150  WarnUponSwitches(true);
     151  uav->StartSensors();
     152
     153  if (getFrameworkManager()->ErrorOccured() == true) {
     154    SafeStop();
     155  }
     156
     157  while (!ToBeStopped()) {
     158    SecurityCheck();
     159
     160    // get controller inputs
     161    CheckJoystick();
     162    CheckPushButton();
     163
     164    if (IsPeriodSet()) {
     165      WaitPeriod();
     166    } else {
     167      WaitUpdate(uav->GetAhrs());
     168    }
     169    needToComputeDefaultTorques = true;
     170    needToComputeDefaultThrust = true;
     171
     172    SignalEvent(Event_t::EnteringControlLoop);
     173
     174    ComputeOrientation();
     175    ComputeAltitude();
     176
     177    // compute thrust and torques to apply
     178    ComputeTorques();
     179    ComputeThrust(); // logs are added to uz, so it must be updated at last
     180
     181    // Set torques for roll, pitch and yaw angles (value between -1 and 1). Set
     182    // thrust (value between 0 and 1)
     183    uav->GetUavMultiplex()->SetRoll(-currentTorques.roll);
     184    uav->GetUavMultiplex()->SetPitch(-currentTorques.pitch);
     185    uav->GetUavMultiplex()->SetYaw(-currentTorques.yaw);
     186    uav->GetUavMultiplex()->SetThrust(-currentThrust); // on raisonne en negatif
     187                                                       // sur l'altitude, a
     188                                                       // revoir avec les
     189                                                       // equations
     190    uav->GetUavMultiplex()->SetRollTrim(joy->RollTrim());
     191    uav->GetUavMultiplex()->SetPitchTrim(joy->PitchTrim());
     192    uav->GetUavMultiplex()->SetYawTrim(0);
     193    uav->GetUavMultiplex()->Update(GetTime());
     194  }
     195
     196  WarnUponSwitches(false);
    187197}
    188198
    189199void UavStateMachine::ComputeOrientation(void) {
    190     if (failSafeMode) {
    191         GetDefaultOrientation()->GetQuaternionAndAngularRates(currentQuaternion, currentAngularSpeed);
     200  if (failSafeMode) {
     201    GetDefaultOrientation()->GetQuaternionAndAngularRates(currentQuaternion,
     202                                                          currentAngularSpeed);
     203  } else {
     204    GetOrientation()->GetQuaternionAndAngularRates(currentQuaternion,
     205                                                   currentAngularSpeed);
     206  }
     207}
     208
     209const AhrsData *UavStateMachine::GetOrientation(void) const {
     210  return GetDefaultOrientation();
     211}
     212
     213const AhrsData *UavStateMachine::GetDefaultOrientation(void) const {
     214  return uav->GetAhrs()->GetDatas();
     215}
     216
     217void UavStateMachine::ComputeAltitude(void) {
     218  if (failSafeMode) {
     219    FailSafeAltitudeValues(currentAltitude, currentVerticalSpeed);
     220  } else {
     221    AltitudeValues(currentAltitude, currentVerticalSpeed);
     222  }
     223}
     224
     225void UavStateMachine::ComputeReferenceAltitude(float &refAltitude,
     226                                               float &refVerticalVelocity) {
     227  if (altitudeMode == AltitudeMode_t::Manual) {
     228    GetDefaultReferenceAltitude(refAltitude, refVerticalVelocity);
     229  } else {
     230    GetReferenceAltitude(refAltitude, refVerticalVelocity);
     231  }
     232}
     233
     234void UavStateMachine::GetDefaultReferenceAltitude(float &refAltitude,
     235                                                  float &refVerticalVelocity) {
     236  float zc, dzc;
     237
     238  switch (altitudeState) {
     239  // initiate a takeoff: increase motor speed in open loop (see ComputeThrust)
     240  // until we detect a take off of 0.03m (hard coded value) above the ground.
     241  case AltitudeState_t::TakingOff: {
     242    if (currentAltitude > groundAltitude + 0.03) {
     243      altitudeTrajectory->StartTraj(currentAltitude,
     244                                    desiredTakeoffAltitude->Value());
     245      altitudeState = AltitudeState_t::Stabilized;
     246      SignalEvent(Event_t::Stabilized);
     247    }
     248    break;
     249  }
     250  // landing, only check if we reach desired landing altitude
     251  case AltitudeState_t::StartLanding: {
     252    if (altitudeTrajectory->Position() == desiredLandingAltitude->Value()) {
     253      // The Uav target altitude has reached its landing value (typically 0)
     254      // but the real Uav altitude may not have reach this value yet because of
     255      // command delay. Moreover, it
     256      // may never exactly reach this value if the ground is not perfectly
     257      // leveled (critical case: there's a
     258      // deep and narrow hole right in the sensor line of sight). That's why we
     259      // have a 2 phases landing strategy.
     260      altitudeState = AltitudeState_t::FinishLanding;
     261      SignalEvent(Event_t::FinishLanding);
     262      joy->SetLedOFF(1); // DualShock3::led1
     263    }
     264  }
     265  // stabilized: check if z trajectory is finished
     266  case AltitudeState_t::Stabilized: {
     267    if (!altitudeTrajectory->IsRunning() && !flagZTrajectoryFinished) {
     268      SignalEvent(Event_t::ZTrajectoryFinished);
     269      flagZTrajectoryFinished = true;
     270    }
     271    if (flagZTrajectoryFinished && desiredTakeoffAltitude->ValueChanged()) {
     272      flagZTrajectoryFinished = false;
     273      altitudeTrajectory->StartTraj(currentAltitude,
     274                                    desiredTakeoffAltitude->Value());
     275      joy->SetZRef(0);
     276    }
     277  }
     278  }
     279
     280  // Récupère les consignes (du joystick dans l'implémentation par défaut). La
     281  // consigne joystick est une vitesse ("delta_z", dzc). le zc est calculé par
     282  // la manette
     283  zc = joy->ZRef(); // a revoir, la position offset devrait se calculer dans le
     284                    // generator
     285  dzc = joy->DzRef();
     286
     287  // z control law
     288  altitudeTrajectory->SetPositionOffset(zc);
     289  altitudeTrajectory->SetSpeedOffset(dzc);
     290
     291  altitudeTrajectory->Update(GetTime());
     292  refAltitude = altitudeTrajectory->Position();
     293  refVerticalVelocity = altitudeTrajectory->Speed();
     294}
     295
     296void UavStateMachine::GetReferenceAltitude(float &refAltitude,
     297                                           float &refVerticalVelocity) {
     298  Thread::Warn("Default GetReferenceAltitude method is not overloaded => "
     299               "switching back to safe mode\n");
     300  EnterFailSafeMode();
     301};
     302
     303void UavStateMachine::ComputeThrust(void) {
     304  if (altitudeMode == AltitudeMode_t::Manual) {
     305    currentThrust = ComputeDefaultThrust();
     306  } else {
     307    currentThrust = ComputeCustomThrust();
     308  }
     309}
     310
     311float UavStateMachine::ComputeDefaultThrust(void) {
     312  if (needToComputeDefaultThrust) {
     313    // compute desired altitude
     314    float refAltitude, refVerticalVelocity;
     315    ComputeReferenceAltitude(refAltitude, refVerticalVelocity);
     316
     317    switch (altitudeState) {
     318    case AltitudeState_t::TakingOff: {
     319      // The progressive increase in motor speed is used to evaluate the motor
     320      // speed that compensate the uav weight. This value
     321      // will be used as an offset for altitude control afterwards
     322      uZ->OffsetStepUp();
     323      break;
     324    }
     325    case AltitudeState_t::StartLanding:
     326    case AltitudeState_t::Stabilized: {
     327      float p_error = currentAltitude - refAltitude;
     328      float d_error = currentVerticalSpeed - refVerticalVelocity;
     329      uZ->SetValues(p_error, d_error);
     330      break;
     331    }
     332    // decrease motor speed in open loop until value offset_g , uav should have
     333    // already landed or be very close to at this point
     334    case AltitudeState_t::FinishLanding: {
     335      if (uZ->OffsetStepDown() == false) {
     336        StopMotors();
     337      }
     338      break;
     339    }
     340    }
     341    uZ->Update(GetTime());
     342
     343    savedDefaultThrust = uZ->Output();
     344    needToComputeDefaultThrust = false;
     345  }
     346
     347  return savedDefaultThrust;
     348}
     349
     350float UavStateMachine::ComputeCustomThrust(void) {
     351  Thread::Warn("Default GetThrust method is not overloaded => switching back "
     352               "to safe mode\n");
     353  EnterFailSafeMode();
     354  return ComputeDefaultThrust();
     355}
     356
     357const AhrsData *UavStateMachine::ComputeReferenceOrientation(void) {
     358  if (orientationMode == OrientationMode_t::Manual) {
     359    return GetDefaultReferenceOrientation();
     360  } else {
     361    return GetReferenceOrientation();
     362  }
     363}
     364
     365const AhrsData *UavStateMachine::GetDefaultReferenceOrientation(void) const {
     366  // We directly control yaw, pitch, roll angles
     367  return joy->GetReferenceOrientation();
     368}
     369
     370const AhrsData *UavStateMachine::GetReferenceOrientation(void) {
     371  Thread::Warn("Default GetReferenceOrientation method is not overloaded => "
     372               "switching back to safe mode\n");
     373  EnterFailSafeMode();
     374  return GetDefaultReferenceOrientation();
     375}
     376
     377void UavStateMachine::ComputeTorques(void) {
     378  if (torqueMode == TorqueMode_t::Default) {
     379    ComputeDefaultTorques(currentTorques);
     380  } else {
     381    ComputeCustomTorques(currentTorques);
     382  }
     383}
     384
     385void UavStateMachine::ComputeDefaultTorques(Euler &torques) {
     386  if (needToComputeDefaultTorques) {
     387    const AhrsData *refOrientation = ComputeReferenceOrientation();
     388    Quaternion refQuaternion;
     389    Vector3D refAngularRates;
     390    refOrientation->GetQuaternionAndAngularRates(refQuaternion,
     391                                                 refAngularRates);
     392    Euler refAngles = refQuaternion.ToEuler();
     393    Euler currentAngles = currentQuaternion.ToEuler();
     394
     395    uYaw->SetValues(currentAngles.YawDistanceFrom(refAngles.yaw),
     396                    currentAngularSpeed.z - refAngularRates.z);
     397    uYaw->Update(GetTime());
     398    torques.yaw = uYaw->Output();
     399
     400    uPitch->SetValues(refAngles.pitch, currentAngles.pitch,
     401                      currentAngularSpeed.y);
     402    uPitch->Update(GetTime());
     403    torques.pitch = uPitch->Output();
     404
     405    uRoll->SetValues(refAngles.roll, currentAngles.roll, currentAngularSpeed.x);
     406    uRoll->Update(GetTime());
     407    torques.roll = uRoll->Output();
     408
     409    savedDefaultTorques = torques;
     410    needToComputeDefaultTorques = false;
     411  } else {
     412    torques = savedDefaultTorques;
     413  }
     414}
     415
     416void UavStateMachine::ComputeCustomTorques(Euler &torques) {
     417  Thread::Warn("Default ComputeCustomTorques method is not overloaded => "
     418               "switching back to safe mode\n");
     419  EnterFailSafeMode();
     420  ComputeDefaultTorques(torques);
     421}
     422
     423void UavStateMachine::TakeOff(void) {
     424  flagZTrajectoryFinished = false;
     425
     426  if (altitudeState == AltitudeState_t::Stopped) { // &&
     427    // GetBatteryMonitor()->IsBatteryLow()==false)
     428    // The uav always takes off in fail safe mode
     429    EnterFailSafeMode();
     430    joy->SetLedOFF(4); // DualShock3::led4
     431    joy->SetLedOFF(1); // DualShock3::led1
     432    joy->Rumble(0x70);
     433    joy->SetZRef(0);
     434
     435    uZ->SetOffset(); // positionne l'offset de compensation de la gravité à sa
     436                     // valeur initiale (station sol)
     437
     438    uav->GetUavMultiplex()->LockUserInterface();
     439    // Active les moteurs. Pouvoir les désactiver permet de pouvoir observer les
     440    // consignes moteurs
     441    // sans les faire tourner effectivement (en déplaçant à la main le drone)
     442    uav->GetBldc()->SetEnabled(true);
     443    groundAltitude = currentAltitude;
     444    altitudeState = AltitudeState_t::TakingOff;
     445
     446    SignalEvent(Event_t::TakingOff);
     447  } else {
     448    joy->ErrorNotify();
     449  }
     450}
     451
     452void UavStateMachine::Land(void) {
     453  if (altitudeMode != AltitudeMode_t::Manual) {
     454    SetAltitudeMode(AltitudeMode_t::Manual);
     455  }
     456  if (altitudeState == AltitudeState_t::Stabilized) {
     457    joy->SetLedOFF(4); // DualShock3::led4
     458    joy->Rumble(0x70);
     459
     460    altitudeTrajectory->StopTraj();
     461    joy->SetZRef(0);
     462    altitudeTrajectory->StartTraj(
     463        currentAltitude,
     464        desiredLandingAltitude->Value()); // shouldn't it be groundAltitude?
     465    SignalEvent(Event_t::StartLanding);
     466    altitudeState = AltitudeState_t::StartLanding;
     467  } else {
     468    joy->ErrorNotify();
     469  }
     470}
     471
     472void UavStateMachine::SignalEvent(Event_t event) {
     473  switch (event) {
     474  case Event_t::StartLanding:
     475    Thread::Info("Altitude: entering 'StartLanding' state\n");
     476    break;
     477  case Event_t::Stopped:
     478    Thread::Info("Altitude: entering 'Stopped' state\n");
     479    break;
     480  case Event_t::TakingOff:
     481    Thread::Info("Altitude: taking off\n");
     482    break;
     483  case Event_t::Stabilized:
     484    Thread::Info("Altitude: entering 'Stabilized' state\n");
     485    break;
     486  case Event_t::FinishLanding:
     487    Thread::Info("Altitude: entering 'FinishLanding' state\n");
     488    break;
     489  case Event_t::EmergencyStop:
     490    Thread::Info("Emergency stop!\n");
     491    break;
     492  }
     493}
     494
     495void UavStateMachine::EmergencyStop(void) {
     496  if (altitudeState != AltitudeState_t::Stopped) {
     497    StopMotors();
     498    EnterFailSafeMode();
     499    joy->Rumble(0x70);
     500    SignalEvent(Event_t::EmergencyStop);
     501  }
     502}
     503
     504void UavStateMachine::StopMotors(void) {
     505  joy->FlashLed(1, 10, 10); // DualShock3::led1
     506  uav->GetBldc()->SetEnabled(false);
     507  uav->GetUavMultiplex()->UnlockUserInterface();
     508  SignalEvent(Event_t::Stopped);
     509  altitudeState = AltitudeState_t::Stopped;
     510  uav->GetAhrs()->UnlockUserInterface();
     511
     512  uZ->SetValues(0, 0);
     513  uZ->Reset();
     514}
     515
     516GridLayout *UavStateMachine::GetButtonsLayout(void) const {
     517  return buttonslayout;
     518}
     519
     520void UavStateMachine::SecurityCheck(void) {
     521  MandatorySecurityCheck();
     522  ExtraSecurityCheck();
     523}
     524
     525void UavStateMachine::MandatorySecurityCheck(void) {
     526  if (getFrameworkManager()->ConnectionLost() && !flagConnectionLost) {
     527    flagConnectionLost = true;
     528    Thread::Err("Connection lost\n");
     529    EnterFailSafeMode();
     530    if (altitudeState == AltitudeState_t::Stopped) {
     531      SafeStop();
    192532    } else {
    193         GetOrientation()->GetQuaternionAndAngularRates(currentQuaternion, currentAngularSpeed);
    194     }
    195 }
    196 
    197 const AhrsData *UavStateMachine::GetOrientation(void) const {
    198     return GetDefaultOrientation();
    199 }
    200 
    201 const AhrsData *UavStateMachine::GetDefaultOrientation(void) const {
    202     return uav->GetAhrs()->GetDatas();
    203 }
    204 
    205 void UavStateMachine::ComputeAltitude(void) {
    206     if (failSafeMode) {
    207         FailSafeAltitudeValues(currentAltitude, currentVerticalSpeed);
    208     } else {
    209         AltitudeValues(currentAltitude,currentVerticalSpeed);
    210     }
    211 }
    212 
    213 void UavStateMachine::ComputeReferenceAltitude(float &refAltitude, float &refVerticalVelocity) {
    214     if (altitudeMode==AltitudeMode_t::Manual) {
    215         GetDefaultReferenceAltitude(refAltitude, refVerticalVelocity);
    216     } else {
    217         GetReferenceAltitude(refAltitude, refVerticalVelocity);
    218     }
    219 }
    220 
    221 void UavStateMachine::GetDefaultReferenceAltitude(float &refAltitude, float &refVerticalVelocity) {
    222     float zc,dzc;
    223 
    224     switch(altitudeState) {
    225         //initiate a takeoff: increase motor speed in open loop (see ComputeThrust) until we detect a take off of 0.03m (hard coded value) above the ground.
    226         case AltitudeState_t::TakingOff: {
    227             if(currentAltitude>groundAltitude+0.03) {
    228                 altitudeTrajectory->StartTraj(currentAltitude,desiredTakeoffAltitude->Value());
    229                 altitudeState=AltitudeState_t::Stabilized;
    230                 SignalEvent(Event_t::Stabilized);
    231             }
    232             break;
    233         }
    234         //landing, only check if we reach desired landing altitude
    235         case AltitudeState_t::StartLanding: {
    236             if(altitudeTrajectory->Position()==desiredLandingAltitude->Value()) {
    237                 //The Uav target altitude has reached its landing value (typically 0)
    238                 // but the real Uav altitude may not have reach this value yet because of command delay. Moreover, it
    239                 // may never exactly reach this value if the ground is not perfectly leveled (critical case: there's a
    240                 // deep and narrow hole right in the sensor line of sight). That's why we have a 2 phases landing strategy.
    241                 altitudeState=AltitudeState_t::FinishLanding;
    242                 SignalEvent(Event_t::FinishLanding);
    243                 joy->SetLedOFF(1);//DualShock3::led1
    244             }
    245         }
    246         //stabilized: check if z trajectory is finished
    247         case AltitudeState_t::Stabilized: {
    248             if(!altitudeTrajectory->IsRunning() && !flagZTrajectoryFinished) {
    249                 SignalEvent(Event_t::ZTrajectoryFinished);
    250                 flagZTrajectoryFinished=true;
    251             }
    252             if(flagZTrajectoryFinished && desiredTakeoffAltitude->ValueChanged()) {
    253                 flagZTrajectoryFinished=false;
    254                 altitudeTrajectory->StartTraj(currentAltitude,desiredTakeoffAltitude->Value());
    255                 joy->SetZRef(0);
    256             }
    257         }
    258     }
    259 
    260     //Récupère les consignes (du joystick dans l'implémentation par défaut). La consigne joystick est une vitesse ("delta_z", dzc). le zc est calculé par la manette
    261     zc=joy->ZRef();//a revoir, la position offset devrait se calculer dans le generator
    262     dzc=joy->DzRef();
    263 
    264     //z control law
    265     altitudeTrajectory->SetPositionOffset(zc);
    266     altitudeTrajectory->SetSpeedOffset(dzc);
    267 
    268     altitudeTrajectory->Update(GetTime());
    269     refAltitude=altitudeTrajectory->Position();
    270     refVerticalVelocity=altitudeTrajectory->Speed();
    271 }
    272 
    273 void UavStateMachine::GetReferenceAltitude(float &refAltitude, float &refVerticalVelocity) {
    274     Thread::Warn("Default GetReferenceAltitude method is not overloaded => switching back to safe mode\n");
     533      Land();
     534    }
     535  }
     536  if ((altitudeState == AltitudeState_t::TakingOff ||
     537       altitudeState == AltitudeState_t::Stabilized) &&
     538      uav->GetBatteryMonitor()->IsBatteryLow() && !flagBatteryLow) {
     539    flagBatteryLow = true;
     540    Printf("Battery Low\n");
    275541    EnterFailSafeMode();
    276 };
    277 
    278 void UavStateMachine::ComputeThrust(void) {
    279     if (altitudeMode==AltitudeMode_t::Manual) {
    280         currentThrust=ComputeDefaultThrust();
    281     } else {
    282         currentThrust=ComputeCustomThrust();
    283     }
    284 }
    285 
    286 float UavStateMachine::ComputeDefaultThrust(void) {
    287     if(needToComputeDefaultThrust) {
    288         //compute desired altitude
    289         float refAltitude, refVerticalVelocity;
    290         ComputeReferenceAltitude(refAltitude, refVerticalVelocity);
    291 
    292         switch(altitudeState) {
    293             case AltitudeState_t::TakingOff: {
    294                 //The progressive increase in motor speed is used to evaluate the motor speed that compensate the uav weight. This value
    295                 //will be used as an offset for altitude control afterwards
    296                 uZ->OffsetStepUp();
    297                 break;
    298             }
    299             case AltitudeState_t::StartLanding:
    300             case AltitudeState_t::Stabilized: {
    301                 float p_error=currentAltitude-refAltitude;
    302                 float d_error=currentVerticalSpeed-refVerticalVelocity;
    303                 uZ->SetValues(p_error,d_error);
    304                 break;
    305             }
    306             //decrease motor speed in open loop until value offset_g , uav should have already landed or be very close to at this point
    307             case AltitudeState_t::FinishLanding: {
    308                 if(uZ->OffsetStepDown()==false) {
    309                     StopMotors();
    310                 }
    311                 break;
    312             }
    313         }
    314         uZ->Update(GetTime());
    315 
    316         savedDefaultThrust=uZ->Output();
    317         needToComputeDefaultThrust=false;
    318     }
    319 
    320     return savedDefaultThrust;
    321 }
    322 
    323 float UavStateMachine::ComputeCustomThrust(void) {
    324     Thread::Warn("Default GetThrust method is not overloaded => switching back to safe mode\n");
    325     EnterFailSafeMode();
    326     return ComputeDefaultThrust();
    327 }
    328 
    329 const AhrsData* UavStateMachine::ComputeReferenceOrientation(void) {
    330     if(orientationMode==OrientationMode_t::Manual) {
    331         return GetDefaultReferenceOrientation();
    332     } else {
    333         return GetReferenceOrientation();
    334     }
    335 }
    336 
    337 const AhrsData* UavStateMachine::GetDefaultReferenceOrientation(void) const {
    338     // We directly control yaw, pitch, roll angles
    339     return joy->GetReferenceOrientation();
    340 }
    341 
    342 const AhrsData* UavStateMachine::GetReferenceOrientation(void) {
    343     Thread::Warn("Default GetReferenceOrientation method is not overloaded => switching back to safe mode\n");
    344     EnterFailSafeMode();
    345     return GetDefaultReferenceOrientation();
    346 }
    347 
    348 void UavStateMachine::ComputeTorques(void) {
    349     if(torqueMode==TorqueMode_t::Default) {
    350         ComputeDefaultTorques(currentTorques);
    351     } else {
    352         ComputeCustomTorques(currentTorques);
    353     }
    354 }
    355 
    356 void UavStateMachine::ComputeDefaultTorques(Euler &torques) {
    357     if(needToComputeDefaultTorques) {
    358         const AhrsData *refOrientation=ComputeReferenceOrientation();
    359         Quaternion refQuaternion;
    360         Vector3D refAngularRates;
    361         refOrientation->GetQuaternionAndAngularRates(refQuaternion,refAngularRates);
    362         Euler refAngles=refQuaternion.ToEuler();
    363         Euler currentAngles=currentQuaternion.ToEuler();
    364 
    365         uYaw->SetValues(currentAngles.YawDistanceFrom(refAngles.yaw),currentAngularSpeed.z-refAngularRates.z);
    366         uYaw->Update(GetTime());
    367         torques.yaw=uYaw->Output();
    368 
    369         uPitch->SetValues(refAngles.pitch,currentAngles.pitch,currentAngularSpeed.y);
    370         uPitch->Update(GetTime());
    371         torques.pitch=uPitch->Output();
    372 
    373         uRoll->SetValues(refAngles.roll,currentAngles.roll,currentAngularSpeed.x);
    374         uRoll->Update(GetTime());
    375         torques.roll=uRoll->Output();
    376 
    377         savedDefaultTorques=torques;
    378         needToComputeDefaultTorques=false;
    379     } else {
    380         torques=savedDefaultTorques;
    381     }
    382 }
    383 
    384 void UavStateMachine::ComputeCustomTorques(Euler &torques) {
    385     Thread::Warn("Default ComputeCustomTorques method is not overloaded => switching back to safe mode\n");
    386     EnterFailSafeMode();
    387     ComputeDefaultTorques(torques);
    388 }
    389 
    390 void UavStateMachine::TakeOff(void) {
    391     flagZTrajectoryFinished=false;
    392 
    393     if(altitudeState==AltitudeState_t::Stopped) {// && GetBatteryMonitor()->IsBatteryLow()==false)
    394         //The uav always takes off in fail safe mode
    395         EnterFailSafeMode();
    396         joy->SetLedOFF(4);//DualShock3::led4
    397         joy->SetLedOFF(1);//DualShock3::led1
    398         joy->Rumble(0x70);
    399         joy->SetZRef(0);
    400 
    401         uZ->SetOffset(); // positionne l'offset de compensation de la gravité à sa valeur initiale (station sol)
    402 
    403         uav->GetUavMultiplex()->LockUserInterface();
    404         //Active les moteurs. Pouvoir les désactiver permet de pouvoir observer les consignes moteurs
    405         // sans les faire tourner effectivement (en déplaçant à la main le drone)
    406         uav->GetBldc()->SetEnabled(true);
    407         groundAltitude=currentAltitude;
    408         altitudeState=AltitudeState_t::TakingOff;
    409 
    410         SignalEvent(Event_t::TakingOff);
    411     } else {
     542    Land();
     543  }
     544}
     545
     546void UavStateMachine::CheckJoystick(void) {
     547  GenericCheckJoystick();
     548  ExtraCheckJoystick();
     549}
     550
     551void UavStateMachine::GenericCheckJoystick(void) {
     552  static bool isEmergencyStopButtonPressed = false;
     553  static bool isTakeOffButtonPressed = false;
     554  static bool isSafeModeButtonPressed = false;
     555
     556  if (joy->IsButtonPressed(1)) { // select
     557    if (!isEmergencyStopButtonPressed) {
     558      isEmergencyStopButtonPressed = true;
     559      Thread::Info("Emergency stop from joystick\n");
     560      EmergencyStop();
     561    }
     562  } else
     563    isEmergencyStopButtonPressed = false;
     564
     565  if (joy->IsButtonPressed(0)) { // start
     566    if (!isTakeOffButtonPressed) {
     567      isTakeOffButtonPressed = true;
     568      switch (altitudeState) {
     569      case AltitudeState_t::Stopped:
     570        TakeOff();
     571        break;
     572      case AltitudeState_t::Stabilized:
     573        Land();
     574        break;
     575      default:
    412576        joy->ErrorNotify();
    413     }
    414 }
    415 
    416 void UavStateMachine::Land(void) {
    417     if (altitudeMode!=AltitudeMode_t::Manual) {
    418         SetAltitudeMode(AltitudeMode_t::Manual);
    419     }
    420     if(altitudeState==AltitudeState_t::Stabilized) {
    421         joy->SetLedOFF(4);//DualShock3::led4
    422         joy->Rumble(0x70);
    423 
    424         altitudeTrajectory->StopTraj();
    425         joy->SetZRef(0);
    426         altitudeTrajectory->StartTraj(currentAltitude,desiredLandingAltitude->Value()); //shouldn't it be groundAltitude?
    427         SignalEvent(Event_t::StartLanding);
    428         altitudeState=AltitudeState_t::StartLanding;
    429     } else {
    430         joy->ErrorNotify();
    431     }
    432 }
    433 
    434 void UavStateMachine::SignalEvent(Event_t event) {
    435     switch(event) {
    436     case Event_t::StartLanding:
    437         Thread::Info("Altitude: entering 'StartLanding' state\n");
    438577        break;
    439     case Event_t::Stopped:
    440         Thread::Info("Altitude: entering 'Stopped' state\n");
    441         break;
    442     case Event_t::TakingOff:
    443         Thread::Info("Altitude: taking off\n");
    444         break;
    445     case Event_t::Stabilized:
    446         Thread::Info("Altitude: entering 'Stabilized' state\n");
    447         break;
    448     case Event_t::FinishLanding:
    449         Thread::Info("Altitude: entering 'FinishLanding' state\n");
    450         break;
    451     case Event_t::EmergencyStop:
    452         Thread::Info("Emergency stop!\n");
    453         break;
    454     }
    455 }
    456 
    457 void UavStateMachine::EmergencyStop(void) {
    458     if(altitudeState!=AltitudeState_t::Stopped) {
    459         StopMotors();
    460         EnterFailSafeMode();
    461         joy->Rumble(0x70);
    462         SignalEvent(Event_t::EmergencyStop);
    463     }
    464 }
    465 
    466 void UavStateMachine::StopMotors(void)
    467 {
    468     joy->FlashLed(1,10,10);//DualShock3::led1
    469     uav->GetBldc()->SetEnabled(false);
    470     uav->GetUavMultiplex()->UnlockUserInterface();
    471     SignalEvent(Event_t::Stopped);
    472     altitudeState=AltitudeState_t::Stopped;
    473     uav->GetAhrs()->UnlockUserInterface();
    474 
    475     uZ->SetValues(0,0);
    476     uZ->Reset();
    477 }
    478 
    479 GridLayout* UavStateMachine::GetButtonsLayout(void) const {
    480     return buttonslayout;
    481 }
    482 
    483 void UavStateMachine::SecurityCheck(void) {
    484     MandatorySecurityCheck();
    485     ExtraSecurityCheck();
    486 }
    487 
    488 void UavStateMachine::MandatorySecurityCheck(void) {
    489     if(getFrameworkManager()->ConnectionLost() && !flagConnectionLost) {
    490         flagConnectionLost=true;
    491         Thread::Err("Connection lost\n");
    492         EnterFailSafeMode();
    493         if(altitudeState==AltitudeState_t::Stopped) {
    494             SafeStop();
    495         } else {
    496             Land();
    497         }
    498     }
    499     if((altitudeState==AltitudeState_t::TakingOff || altitudeState==AltitudeState_t::Stabilized) && uav->GetBatteryMonitor()->IsBatteryLow() && !flagBatteryLow) {
    500         flagBatteryLow=true;
    501         Printf("Battery Low\n");
    502         EnterFailSafeMode();
    503         Land();
    504     }
    505 }
    506 
    507 void UavStateMachine::CheckJoystick(void) {
    508     GenericCheckJoystick();
    509     ExtraCheckJoystick();
    510 }
    511 
    512 void UavStateMachine::GenericCheckJoystick(void) {
    513     static bool isEmergencyStopButtonPressed=false;
    514     static bool isTakeOffButtonPressed=false;
    515     static bool isSafeModeButtonPressed=false;
    516 
    517    if(joy->IsButtonPressed(1)) { //select
    518         if (!isEmergencyStopButtonPressed) {
    519             isEmergencyStopButtonPressed=true;
    520             Thread::Info("Emergency stop from joystick\n");
    521             EmergencyStop();
    522         }
    523     } else isEmergencyStopButtonPressed=false;
    524 
    525     if(joy->IsButtonPressed(0)) { //start
    526         if (!isTakeOffButtonPressed) {
    527             isTakeOffButtonPressed=true;
    528             switch(altitudeState) {
    529                 case AltitudeState_t::Stopped:
    530                     TakeOff();
    531                     break;
    532                 case AltitudeState_t::Stabilized:
    533                     Land();
    534                     break;
    535                 default:
    536                     joy->ErrorNotify();
    537                     break;
    538             }
    539         }
    540     } else isTakeOffButtonPressed=false;
    541 
    542     //cross
    543     //gsanahuj:conflict with Majd programs.
    544     //check if l1,l2,r1 and r2 are not pressed
    545     //to allow a combination in user program
    546     if(joy->IsButtonPressed(5) && !joy->IsButtonPressed(6) && !joy->IsButtonPressed(7) && !joy->IsButtonPressed(9) && !joy->IsButtonPressed(10)) {
    547         if (!isSafeModeButtonPressed) {
    548             isSafeModeButtonPressed=true;
    549             Thread::Info("Entering fail safe mode\n");
    550             EnterFailSafeMode();
    551         }
    552     } else isSafeModeButtonPressed=false;
     578      }
     579    }
     580  } else
     581    isTakeOffButtonPressed = false;
     582
     583  // cross
     584  // gsanahuj:conflict with Majd programs.
     585  // check if l1,l2,r1 and r2 are not pressed
     586  // to allow a combination in user program
     587  if (joy->IsButtonPressed(5) && !joy->IsButtonPressed(6) &&
     588      !joy->IsButtonPressed(7) && !joy->IsButtonPressed(9) &&
     589      !joy->IsButtonPressed(10)) {
     590    if (!isSafeModeButtonPressed) {
     591      isSafeModeButtonPressed = true;
     592      Thread::Info("Entering fail safe mode\n");
     593      EnterFailSafeMode();
     594    }
     595  } else
     596    isSafeModeButtonPressed = false;
    553597}
    554598
    555599void UavStateMachine::CheckPushButton(void) {
    556     GenericCheckPushButton();
    557     ExtraCheckPushButton();
     600  GenericCheckPushButton();
     601  ExtraCheckPushButton();
    558602}
    559603
    560604void UavStateMachine::GenericCheckPushButton(void) {
    561     if(button_kill->Clicked()==true) SafeStop();
    562     if(button_take_off->Clicked()==true) TakeOff();
    563     if(button_land->Clicked()==true) Land();
    564     if(button_start_log->Clicked()==true) getFrameworkManager()->StartLog();
    565     if(button_stop_log->Clicked()==true) getFrameworkManager()->StopLog();
     605  if (button_kill->Clicked() == true)
     606    SafeStop();
     607  if (button_take_off->Clicked() == true)
     608    TakeOff();
     609  if (button_land->Clicked() == true)
     610    Land();
     611  if (button_start_log->Clicked() == true)
     612    getFrameworkManager()->StartLog();
     613  if (button_stop_log->Clicked() == true)
     614    getFrameworkManager()->StopLog();
    566615}
    567616
    568617void UavStateMachine::EnterFailSafeMode(void) {
    569     SetAltitudeMode(AltitudeMode_t::Manual);
    570     SetOrientationMode(OrientationMode_t::Manual);
    571     SetThrustMode(ThrustMode_t::Default);
    572     SetTorqueMode(TorqueMode_t::Default);
    573 
    574     GetDefaultOrientation()->GetQuaternionAndAngularRates(currentQuaternion, currentAngularSpeed);
    575     joy->SetYawRef(currentQuaternion);
     618  SetAltitudeMode(AltitudeMode_t::Manual);
     619  SetOrientationMode(OrientationMode_t::Manual);
     620  SetThrustMode(ThrustMode_t::Default);
     621  SetTorqueMode(TorqueMode_t::Default);
     622
     623  GetDefaultOrientation()->GetQuaternionAndAngularRates(currentQuaternion,
     624                                                        currentAngularSpeed);
     625  joy->SetYawRef(currentQuaternion);
     626  uYaw->Reset();
     627  uPitch->Reset();
     628  uRoll->Reset();
     629
     630  failSafeMode = true;
     631  SignalEvent(Event_t::EnteringFailSafeMode);
     632}
     633
     634bool UavStateMachine::ExitFailSafeMode(void) {
     635  // only exit fail safe mode if in Stabilized altitude state
     636  // gsanahuj: pour la demo inaugurale on ne peut pas etre en failsafe
     637  // le ruban perturbe l'us
     638  /*
     639    if (altitudeState!=AltitudeState_t::Stabilized) {
     640        return false;
     641    } else*/ {
     642    failSafeMode = false;
     643    return true;
     644  }
     645}
     646
     647bool UavStateMachine::SetTorqueMode(TorqueMode_t const &newTorqueMode) {
     648  if ((newTorqueMode == TorqueMode_t::Custom) && (failSafeMode)) {
     649    if (!ExitFailSafeMode())
     650      return false;
     651  }
     652  // When transitionning from Custom to Default torque mode, we should reset the
     653  // default control laws
     654  if ((torqueMode == TorqueMode_t::Custom) &&
     655      (newTorqueMode == TorqueMode_t::Default)) {
    576656    uYaw->Reset();
    577657    uPitch->Reset();
    578658    uRoll->Reset();
    579 
    580     failSafeMode=true;
    581     SignalEvent(Event_t::EnteringFailSafeMode);
    582 }
    583 
    584 bool UavStateMachine::ExitFailSafeMode(void) {
    585     // only exit fail safe mode if in Stabilized altitude state
    586     //gsanahuj: pour la demo inaugurale on ne peut pas etre en failsafe
    587     //le ruban perturbe l'us
    588     /*
    589     if (altitudeState!=AltitudeState_t::Stabilized) {
    590         return false;
    591     } else*/ {
    592         failSafeMode=false;
    593         return true;
    594     }
    595 }
    596 
    597 bool UavStateMachine::SetTorqueMode(TorqueMode_t const &newTorqueMode) {
    598     if ((newTorqueMode==TorqueMode_t::Custom) && (failSafeMode)) {
    599         if (!ExitFailSafeMode()) return false;
    600     }
    601     //When transitionning from Custom to Default torque mode, we should reset the default control laws
    602     if ((torqueMode==TorqueMode_t::Custom) && (newTorqueMode==TorqueMode_t::Default)) {
    603         uYaw->Reset();
    604         uPitch->Reset();
    605         uRoll->Reset();
    606     }
    607     torqueMode=newTorqueMode;
    608     return true;
     659  }
     660  torqueMode = newTorqueMode;
     661  return true;
    609662}
    610663
    611664bool UavStateMachine::SetAltitudeMode(AltitudeMode_t const &newAltitudeMode) {
    612     if ((newAltitudeMode==AltitudeMode_t::Custom) && (failSafeMode)) {
    613         if (!ExitFailSafeMode()) return false;
    614     }
    615     altitudeMode=newAltitudeMode;
    616     GotoAltitude(desiredTakeoffAltitude->Value());
    617 
    618     return true;
     665  if ((newAltitudeMode == AltitudeMode_t::Custom) && (failSafeMode)) {
     666    if (!ExitFailSafeMode())
     667      return false;
     668  }
     669  altitudeMode = newAltitudeMode;
     670  GotoAltitude(desiredTakeoffAltitude->Value());
     671
     672  return true;
    619673}
    620674
    621675bool UavStateMachine::GotoAltitude(float desiredAltitude) {
    622     if (altitudeMode!=AltitudeMode_t::Manual) {
    623         return false;
    624     }
    625     altitudeTrajectory->StartTraj(uav->GetMetaUsRangeFinder()->z(),desiredAltitude);
    626     return true;
    627 }
    628 
    629 bool UavStateMachine::SetOrientationMode(OrientationMode_t const &newOrientationMode) {
    630     if ((newOrientationMode==OrientationMode_t::Custom) && (failSafeMode)) {
    631         if (!ExitFailSafeMode()) return false;
    632     }
    633     //When transitionning from Custom to Manual mode we must reset to yaw reference to the current absolute yaw angle,
    634     // overwise the Uav will abruptly change orientation
    635     if ((orientationMode==OrientationMode_t::Custom) && (newOrientationMode==OrientationMode_t::Manual)) {
    636         joy->SetYawRef(currentQuaternion);
    637     }
    638     orientationMode=newOrientationMode;
    639     return true;
     676  if (altitudeMode != AltitudeMode_t::Manual) {
     677    return false;
     678  }
     679  altitudeTrajectory->StartTraj(uav->GetMetaUsRangeFinder()->z(),
     680                                desiredAltitude);
     681  return true;
     682}
     683
     684bool UavStateMachine::SetOrientationMode(
     685    OrientationMode_t const &newOrientationMode) {
     686  if ((newOrientationMode == OrientationMode_t::Custom) && (failSafeMode)) {
     687    if (!ExitFailSafeMode())
     688      return false;
     689  }
     690  // When transitionning from Custom to Manual mode we must reset to yaw
     691  // reference to the current absolute yaw angle,
     692  // overwise the Uav will abruptly change orientation
     693  if ((orientationMode == OrientationMode_t::Custom) &&
     694      (newOrientationMode == OrientationMode_t::Manual)) {
     695    joy->SetYawRef(currentQuaternion);
     696  }
     697  orientationMode = newOrientationMode;
     698  return true;
    640699}
    641700
    642701bool UavStateMachine::SetThrustMode(ThrustMode_t const &newThrustMode) {
    643     if ((newThrustMode==ThrustMode_t::Custom) && (failSafeMode)) {
    644         if (!ExitFailSafeMode()) return false;
    645     }
    646     thrustMode=newThrustMode;
    647     return true;
    648 }
     702  if ((newThrustMode == ThrustMode_t::Custom) && (failSafeMode)) {
     703    if (!ExitFailSafeMode())
     704      return false;
     705  }
     706  thrustMode = newThrustMode;
     707  return true;
     708}

  • trunk/lib/FlairMeta/src/UavStateMachine.h

    r10 r15  
    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     }
    53 }
    54 
    55 namespace flair { namespace meta {
    56 
    57  /*! \class UavStateMachine
     28namespace core {
     29class FrameworkManager;
     30class AhrsData;
     31class io_data;
     32}
     33namespace gui {
     34class PushButton;
     35class GridLayout;
     36class Tab;
     37class DoubleSpinBox;
     38}
     39namespace filter {
     40class ControlLaw;
     41class NestedSat;
     42class Pid;
     43class PidThrust;
     44class TrajectoryGenerator1D;
     45}
     46namespace sensor {
     47class TargetController;
     48}
     49namespace meta {
     50class MetaDualShock3;
     51class Uav;
     52}
     53}
     54
     55namespace flair {
     56namespace meta {
     57
     58/*! \class UavStateMachine
    5859*
    5960* \brief State machine for UAV
    6061*
    61 * thread synchronized with ahrs, unless a period is set with SetPeriodUS or SetPeriodMS
     62* thread synchronized with ahrs, unless a period is set with SetPeriodUS or
     63*SetPeriodMS
    6264*/
    6365
    64 class UavStateMachine : public core::Thread
    65 {
    66     protected:
    67         enum class AltitudeMode_t {
    68             Manual,
    69             Custom
    70         };
    71         const AltitudeMode_t &GetAltitudeMode(void) const {
    72             return altitudeMode;
    73         }
    74         bool SetAltitudeMode(const AltitudeMode_t &altitudeMode);
    75 
    76         //uses TrajectoryGenerator1D *altitudeTrajectory to go to desiredAltitude
    77         //available in mode AltitudeMode_t::Manual
    78         //return true if goto is possible
    79         bool GotoAltitude(float desiredAltitude);
    80 
    81         enum class OrientationMode_t {
    82             Manual,
    83             Custom
    84         };
    85         const OrientationMode_t &GetOrientationMode(void) const {
    86             return orientationMode;
    87         }
    88         bool SetOrientationMode(const OrientationMode_t &orientationMode);
    89 
    90         enum class ThrustMode_t {
    91             Default,
    92             Custom
    93         };
    94         const ThrustMode_t &GetThrustMode() const {
    95             return thrustMode;
    96         }
    97         bool SetThrustMode(const ThrustMode_t &thrustMode);
    98 
    99         enum class TorqueMode_t {
    100             Default,
    101             Custom
    102         };
    103         const TorqueMode_t &GetTorqueMode(void) const {
    104             return torqueMode;
    105         }
    106         bool SetTorqueMode(const TorqueMode_t &torqueMode);
    107 
    108         enum class Event_t {
    109             EnteringFailSafeMode,
    110             EnteringControlLoop,
    111             StartLanding,
    112             FinishLanding,
    113             Stopped,
    114             TakingOff,
    115             EmergencyStop,
    116             Stabilized, //as soon as uav is 3cm far from the ground
    117             ZTrajectoryFinished,
    118         };
    119 
    120         UavStateMachine(meta::Uav* uav,uint16_t ds3Port=20000);
    121         ~UavStateMachine();
    122 
    123         const core::Quaternion &GetCurrentQuaternion(void) const;
    124 
    125         const core::Vector3D &GetCurrentAngularSpeed(void) const;
    126 
    127         const meta::Uav *GetUav(void) const;
    128 
    129         void Land(void);
    130         void TakeOff(void);
    131         void EmergencyStop(void);
    132         //! Used to signal an event
    133         /*!
    134             \param event the event which occured
    135         */
    136         virtual void SignalEvent(Event_t event);
    137 
    138         virtual const core::AhrsData *GetOrientation(void) const;
    139         const core::AhrsData *GetDefaultOrientation(void) const;
    140 
    141         virtual void AltitudeValues(float &z,float &dz) const;//in uav coordinate!
    142         void EnterFailSafeMode(void);
    143         bool ExitFailSafeMode(void);
    144         void FailSafeAltitudeValues(float &z,float &dz) const;//in uav coordinate!
    145 
    146         gui::GridLayout *GetButtonsLayout(void) const;
    147         virtual void ExtraSecurityCheck(void){};
    148         virtual void ExtraCheckJoystick(void){};
    149         virtual void ExtraCheckPushButton(void){};
    150 
    151         void GetDefaultReferenceAltitude(float &refAltitude, float &refVerticalVelocity);
    152         virtual void GetReferenceAltitude(float &refAltitude, float &refVerticalVelocity);
    153         //float GetDefaultThrustOffset(void);
    154         const core::AhrsData *GetDefaultReferenceOrientation(void) const;
    155         virtual const core::AhrsData *GetReferenceOrientation(void);
    156 
    157         /*!
    158         * \brief Compute Custom Torques
    159         *
    160         * Reimplement this method to use TorqueMode_t::Custom. \n
    161         * This method is called internally by UavStateMachine, do not call it by yourself. \n
    162         * See GetTorques if you need torques values.
    163         *
    164         * \param torques custom torques
    165         */
    166         virtual void ComputeCustomTorques(core::Euler &torques);
    167 
    168         /*!
    169         * \brief Compute Default Torques
    170         *
    171         * This method is called internally by UavStateMachine when using TorqueMode_t::Default. \n
    172         * Torques are only computed once by loop, other calls to this method will use previously computed torques.
    173         *
    174         * \param torques default torques
    175         */
    176         void ComputeDefaultTorques(core::Euler &torques);
    177 
    178         /*!
    179         * \brief Get Torques
    180         *
    181         * \return torques current torques
    182         */
    183         //const core::Euler &GetTorques() const;
    184 
    185         /*!
    186         * \brief Compute Custom Thrust
    187         *
    188         * Reimplement this method to use ThrustMode_t::Custom. \n
    189         * This method is called internally by UavStateMachine, do not call it by yourself. \n
    190         * See GetThrust if you need thrust value.
    191         *
    192         * \return custom Thrust
    193         */
    194         virtual float ComputeCustomThrust(void);
    195 
    196         /*!
    197         * \brief Compute Default Thrust
    198         *
    199         * This method is called internally by UavStateMachine when using ThrustMode_t::Default. \n
    200         * Thrust is only computed once by loop, other calls to this method will use previously computed thrust.
    201         *
    202         * \return default thrust
    203         */
    204         float ComputeDefaultThrust(void);
    205 
    206         /*!
    207         * \brief Get Thrust
    208         *
    209         * \return current thrust
    210         */
    211         //float GetThrust() const;
    212 
    213         /*!
    214         * \brief Add an IODevice to the control law logs
    215         *
    216         * The IODevice will be automatically logged among the Uz logs,
    217         * if logging is enabled (see IODevice::SetDataToLog, FrameworkManager::StartLog
    218         * and FrameworkManager::AddDeviceToLog). \n
    219         *
    220         * \param device IODevice to log
    221         */
    222         void AddDeviceToControlLawLog(const core::IODevice* device);
    223 
    224         /*!
    225         * \brief Add an io_data to the control law logs
    226         *
    227         * The io_data will be automatically logged among the Uz logs,
    228         * if logging is enabled (see IODevice::SetDataToLog, FrameworkManager::StartLog
    229         * and FrameworkManager::AddDeviceToLog). \n
    230         *
    231         * \param data io_data to log
    232         */
    233         void AddDataToControlLawLog(const core::io_data* data);
    234 
    235         const sensor::TargetController *GetJoystick(void) const;
    236 
    237         gui::Tab *setupLawTab,*graphLawTab;
    238 
    239     private:
    240         /*!
    241         \enum AltitudeState_t
    242         \brief States of the altitude state machine
    243         */
    244         enum class AltitudeState_t {
    245             Stopped,        /*!< the uav motors are stopped */
    246             TakingOff,      /*!< take off initiated. Motors accelerate progressively until the UAV lift up */
    247             Stabilized,     /*!< the uav is actively maintaining its altitude */
    248             StartLanding,   /*!< landing initiated. Altitude is required to reach the landing altitude (0 by default) */
    249             FinishLanding   /*!< motors are gradually stopped */
    250         };
    251         AltitudeState_t altitudeState;
    252         void ProcessAltitudeFiniteStateMachine();
    253         void ComputeReferenceAltitude(float &refAltitude, float &refVerticalVelocity);
    254 
    255 
    256         float groundAltitude; // effective altitude when the uav leaves the ground
    257         float currentAltitude,currentVerticalSpeed;
    258 
    259         bool failSafeMode;
    260         void SecurityCheck(void);
    261         void MandatorySecurityCheck(void);
    262         void CheckJoystick();
    263         void GenericCheckJoystick();
    264         void CheckPushButton(void);
    265         void GenericCheckPushButton(void);
    266         void Run(void);
    267         void StopMotors(void);
    268 
    269         meta::Uav *uav;
    270 
    271         core::Quaternion currentQuaternion;
    272         core::Vector3D currentAngularSpeed;
    273 
    274         const core::AhrsData *ComputeReferenceOrientation(void);
    275 
    276         void ComputeOrientation(void);
    277         void ComputeAltitude(void);
    278 
    279         void ComputeTorques(void);
    280         core::Euler currentTorques,savedDefaultTorques;
    281         bool needToComputeDefaultTorques;
    282 
    283         void ComputeThrust(void);
    284         float currentThrust,savedDefaultThrust;
    285         bool needToComputeDefaultThrust;
    286 
    287         gui::PushButton *button_kill,*button_take_off,*button_land,*button_start_log,*button_stop_log;
    288         gui::GridLayout *buttonslayout;
    289         gui::DoubleSpinBox *desiredTakeoffAltitude,*desiredLandingAltitude;
    290         AltitudeMode_t altitudeMode;
    291         OrientationMode_t orientationMode;
    292         ThrustMode_t thrustMode;
    293         TorqueMode_t torqueMode;
    294         bool flagBatteryLow;
    295         bool flagConnectionLost;
    296         bool flagZTrajectoryFinished;
    297         filter::NestedSat *uRoll,*uPitch;
    298         filter::Pid *uYaw;
    299         filter::PidThrust *uZ;
    300 
    301         MetaDualShock3 *joy;
    302         filter::TrajectoryGenerator1D *altitudeTrajectory;
     66class UavStateMachine : public core::Thread {
     67protected:
     68  enum class AltitudeMode_t { Manual, Custom };
     69  const AltitudeMode_t &GetAltitudeMode(void) const { return altitudeMode; }
     70  bool SetAltitudeMode(const AltitudeMode_t &altitudeMode);
     71
     72  // uses TrajectoryGenerator1D *altitudeTrajectory to go to desiredAltitude
     73  // available in mode AltitudeMode_t::Manual
     74  // return true if goto is possible
     75  bool GotoAltitude(float desiredAltitude);
     76
     77  enum class OrientationMode_t { Manual, Custom };
     78  const OrientationMode_t &GetOrientationMode(void) const {
     79    return orientationMode;
     80  }
     81  bool SetOrientationMode(const OrientationMode_t &orientationMode);
     82
     83  enum class ThrustMode_t { Default, Custom };
     84  const ThrustMode_t &GetThrustMode() const { return thrustMode; }
     85  bool SetThrustMode(const ThrustMode_t &thrustMode);
     86
     87  enum class TorqueMode_t { Default, Custom };
     88  const TorqueMode_t &GetTorqueMode(void) const { return torqueMode; }
     89  bool SetTorqueMode(const TorqueMode_t &torqueMode);
     90
     91  enum class Event_t {
     92    EnteringFailSafeMode,
     93    EnteringControlLoop,
     94    StartLanding,
     95    FinishLanding,
     96    Stopped,
     97    TakingOff,
     98    EmergencyStop,
     99    Stabilized, // as soon as uav is 3cm far from the ground
     100    ZTrajectoryFinished,
     101  };
     102
     103  UavStateMachine(meta::Uav *uav, uint16_t ds3Port = 20000);
     104  ~UavStateMachine();
     105
     106  const core::Quaternion &GetCurrentQuaternion(void) const;
     107
     108  const core::Vector3D &GetCurrentAngularSpeed(void) const;
     109
     110  const meta::Uav *GetUav(void) const;
     111
     112  void Land(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);
     120
     121  virtual const core::AhrsData *GetOrientation(void) const;
     122  const core::AhrsData *GetDefaultOrientation(void) const;
     123
     124  virtual void AltitudeValues(float &z, float &dz) const; // in uav coordinate!
     125  void EnterFailSafeMode(void);
     126  bool ExitFailSafeMode(void);
     127  void FailSafeAltitudeValues(float &z, float &dz) const; // in uav coordinate!
     128
     129  gui::GridLayout *GetButtonsLayout(void) const;
     130  virtual void ExtraSecurityCheck(void){};
     131  virtual void ExtraCheckJoystick(void){};
     132  virtual void ExtraCheckPushButton(void){};
     133
     134  void GetDefaultReferenceAltitude(float &refAltitude,
     135                                   float &refVerticalVelocity);
     136  virtual void GetReferenceAltitude(float &refAltitude,
     137                                    float &refVerticalVelocity);
     138  // float GetDefaultThrustOffset(void);
     139  const core::AhrsData *GetDefaultReferenceOrientation(void) const;
     140  virtual const core::AhrsData *GetReferenceOrientation(void);
     141
     142  /*!
     143  * \brief Compute Custom Torques
     144  *
     145  * Reimplement this method to use TorqueMode_t::Custom. \n
     146  * This method is called internally by UavStateMachine, do not call it by
     147  *yourself. \n
     148  * See GetTorques if you need torques values.
     149  *
     150  * \param torques custom torques
     151  */
     152  virtual void ComputeCustomTorques(core::Euler &torques);
     153
     154  /*!
     155  * \brief Compute Default Torques
     156  *
     157  * This method is called internally by UavStateMachine when using
     158  *TorqueMode_t::Default. \n
     159  * Torques are only computed once by loop, other calls to this method will use
     160  *previously computed torques.
     161  *
     162  * \param torques default torques
     163  */
     164  void ComputeDefaultTorques(core::Euler &torques);
     165
     166  /*!
     167  * \brief Get Torques
     168  *
     169  * \return torques current torques
     170  */
     171  // const core::Euler &GetTorques() const;
     172
     173  /*!
     174  * \brief Compute Custom Thrust
     175  *
     176  * Reimplement this method to use ThrustMode_t::Custom. \n
     177  * This method is called internally by UavStateMachine, do not call it by
     178  *yourself. \n
     179  * See GetThrust if you need thrust value.
     180  *
     181  * \return custom Thrust
     182  */
     183  virtual float ComputeCustomThrust(void);
     184
     185  /*!
     186  * \brief Compute Default Thrust
     187  *
     188  * This method is called internally by UavStateMachine when using
     189  *ThrustMode_t::Default. \n
     190  * Thrust is only computed once by loop, other calls to this method will use
     191  *previously computed thrust.
     192  *
     193  * \return default thrust
     194  */
     195  float ComputeDefaultThrust(void);
     196
     197  /*!
     198  * \brief Get Thrust
     199  *
     200  * \return current thrust
     201  */
     202  // float GetThrust() const;
     203
     204  /*!
     205  * \brief Add an IODevice to the control law logs
     206  *
     207  * The IODevice will be automatically logged among the Uz logs,
     208  * if logging is enabled (see IODevice::SetDataToLog,
     209  *FrameworkManager::StartLog
     210  * and FrameworkManager::AddDeviceToLog). \n
     211  *
     212  * \param device IODevice to log
     213  */
     214  void AddDeviceToControlLawLog(const core::IODevice *device);
     215
     216  /*!
     217  * \brief Add an io_data to the control law logs
     218  *
     219  * The io_data will be automatically logged among the Uz logs,
     220  * if logging is enabled (see IODevice::SetDataToLog,
     221  *FrameworkManager::StartLog
     222  * and FrameworkManager::AddDeviceToLog). \n
     223  *
     224  * \param data io_data to log
     225  */
     226  void AddDataToControlLawLog(const core::io_data *data);
     227
     228  const sensor::TargetController *GetJoystick(void) const;
     229
     230  gui::Tab *setupLawTab, *graphLawTab;
     231
     232private:
     233  /*!
     234  \enum AltitudeState_t
     235  \brief States of the altitude state machine
     236  */
     237  enum class AltitudeState_t {
     238    Stopped,      /*!< the uav motors are stopped */
     239    TakingOff,    /*!< take off initiated. Motors accelerate progressively until
     240                     the UAV lift up */
     241    Stabilized,   /*!< the uav is actively maintaining its altitude */
     242    StartLanding, /*!< landing initiated. Altitude is required to reach the
     243                     landing altitude (0 by default) */
     244    FinishLanding /*!< motors are gradually stopped */
     245  };
     246  AltitudeState_t altitudeState;
     247  void ProcessAltitudeFiniteStateMachine();
     248  void ComputeReferenceAltitude(float &refAltitude, float &refVerticalVelocity);
     249
     250  float groundAltitude; // effective altitude when the uav leaves the ground
     251  float currentAltitude, currentVerticalSpeed;
     252
     253  bool failSafeMode;
     254  void SecurityCheck(void);
     255  void MandatorySecurityCheck(void);
     256  void CheckJoystick();
     257  void GenericCheckJoystick();
     258  void CheckPushButton(void);
     259  void GenericCheckPushButton(void);
     260  void Run(void);
     261  void StopMotors(void);
     262
     263  meta::Uav *uav;
     264
     265  core::Quaternion currentQuaternion;
     266  core::Vector3D currentAngularSpeed;
     267
     268  const core::AhrsData *ComputeReferenceOrientation(void);
     269
     270  void ComputeOrientation(void);
     271  void ComputeAltitude(void);
     272
     273  void ComputeTorques(void);
     274  core::Euler currentTorques, savedDefaultTorques;
     275  bool needToComputeDefaultTorques;
     276
     277  void ComputeThrust(void);
     278  float currentThrust, savedDefaultThrust;
     279  bool needToComputeDefaultThrust;
     280
     281  gui::PushButton *button_kill, *button_take_off, *button_land,
     282      *button_start_log, *button_stop_log;
     283  gui::GridLayout *buttonslayout;
     284  gui::DoubleSpinBox *desiredTakeoffAltitude, *desiredLandingAltitude;
     285  AltitudeMode_t altitudeMode;
     286  OrientationMode_t orientationMode;
     287  ThrustMode_t thrustMode;
     288  TorqueMode_t torqueMode;
     289  bool flagBatteryLow;
     290  bool flagConnectionLost;
     291  bool flagZTrajectoryFinished;
     292  filter::NestedSat *uRoll, *uPitch;
     293  filter::Pid *uYaw;
     294  filter::PidThrust *uZ;
     295
     296  MetaDualShock3 *joy;
     297  filter::TrajectoryGenerator1D *altitudeTrajectory;
    303298};
    304 
    305299};
    306300};

  • trunk/lib/FlairMeta/src/XAir.cpp

    r10 r15  
    3535using namespace flair::actuator;
    3636
    37 namespace flair { namespace meta {
     37namespace flair {
     38namespace meta {
    3839
    39 XAir::XAir(FrameworkManager* parent,string uav_name,filter::UavMultiplex *multiplex) : Uav(parent,uav_name,multiplex) {
    40     RTDM_I2cPort* i2cport=new RTDM_I2cPort(parent,"rtdm_i2c","rti2c3");
    41     RTDM_SerialPort* imu_port=new RTDM_SerialPort(parent,"imu_port","rtser1");
     40XAir::XAir(FrameworkManager *parent, string uav_name,
     41           filter::UavMultiplex *multiplex)
     42    : Uav(parent, uav_name, multiplex) {
     43  RTDM_I2cPort *i2cport = new RTDM_I2cPort(parent, "rtdm_i2c", "rti2c3");
     44  RTDM_SerialPort *imu_port = new RTDM_SerialPort(parent, "imu_port", "rtser1");
    4245
    43     if(multiplex==NULL) SetMultiplex(new X4X8Multiplex(parent,"motors",X4X8Multiplex::X8));
     46  if (multiplex == NULL)
     47    SetMultiplex(new X4X8Multiplex(parent, "motors", X4X8Multiplex::X8));
    4448
    45     SetBldc(new AfroBldc(GetUavMultiplex(),GetUavMultiplex()->GetLayout(),"motors",GetUavMultiplex()->MotorsCount(),i2cport));
    46     SetUsRangeFinder(new Srf08(parent,"SRF08",i2cport,0x70,60));
    47     SetAhrs(new Gx3_25_ahrs(parent,"imu",imu_port,Gx3_25_imu::EulerAnglesAndAngularRates,70));
    48     Tab* bat_tab=new Tab(parent->GetTabWidget(),"battery");
    49     SetBatteryMonitor(new BatteryMonitor(bat_tab->NewRow(),"battery"));
    50     GetBatteryMonitor()->SetBatteryValue(12);
     49  SetBldc(new AfroBldc(GetUavMultiplex(), GetUavMultiplex()->GetLayout(),
     50                       "motors", GetUavMultiplex()->MotorsCount(), i2cport));
     51  SetUsRangeFinder(new Srf08(parent, "SRF08", i2cport, 0x70, 60));
     52  SetAhrs(new Gx3_25_ahrs(parent, "imu", imu_port,
     53                          Gx3_25_imu::EulerAnglesAndAngularRates, 70));
     54  Tab *bat_tab = new Tab(parent->GetTabWidget(), "battery");
     55  SetBatteryMonitor(new BatteryMonitor(bat_tab->NewRow(), "battery"));
     56  GetBatteryMonitor()->SetBatteryValue(12);
    5157
    52 /*
    53         if(VRPNType==Auto || VRPNType==AutoSerialPort)
    54         {
    55             RTDM_SerialPort* vrpn_port=new RTDM_SerialPort(parent,"vrpn_port","rtser3");
     58  /*
     59          if(VRPNType==Auto || VRPNType==AutoSerialPort)
     60          {
     61              RTDM_SerialPort* vrpn_port=new
     62     RTDM_SerialPort(parent,"vrpn_port","rtser3");
    5663
    57             vrpnclient=new VrpnClient(parent,"vrpn",vrpn_port,10000,80);
    58             uav_vrpn=new MetaVrpnObject(vrpnclient,uav_name,VRPNSerialObjectId);
    59         }
    60 */
    61     SetVerticalCamera(new Ps3Eye(parent,"camv",0,50));
     64              vrpnclient=new VrpnClient(parent,"vrpn",vrpn_port,10000,80);
     65              uav_vrpn=new
     66     MetaVrpnObject(vrpnclient,uav_name,VRPNSerialObjectId);
     67          }
     68  */
     69  SetVerticalCamera(new Ps3Eye(parent, "camv", 0, 50));
    6270}
    6371
    64 XAir::~XAir() {
    65 
    66 }
     72XAir::~XAir() {}
    6773
    6874void XAir::StartSensors(void) {
    69     ((Gx3_25_ahrs*)GetAhrs())->Start();
    70     ((Srf08*)GetUsRangeFinder())->Start();
    71     ((Ps3Eye *)GetVerticalCamera())->Start();
    72     Uav::StartSensors();
     75  ((Gx3_25_ahrs *)GetAhrs())->Start();
     76  ((Srf08 *)GetUsRangeFinder())->Start();
     77  ((Ps3Eye *)GetVerticalCamera())->Start();
     78  Uav::StartSensors();
    7379}
    7480

  • trunk/lib/FlairMeta/src/XAir.h

    r10 r15  
    1616#include "Uav.h"
    1717
    18 namespace flair
    19 {
    20 namespace meta
    21 {
    22     /*! \class XAir
    23     *
    24     * \brief Class defining an ardrone2 uav
    25     */
    26     class XAir : public Uav {
    27         public:
    28             XAir(core::FrameworkManager* parent,std::string uav_name,filter::UavMultiplex *multiplex=NULL);
    29             ~XAir();
    30             void StartSensors(void);
     18namespace flair {
     19namespace meta {
     20/*! \class XAir
     21*
     22* \brief Class defining an ardrone2 uav
     23*/
     24class XAir : public Uav {
     25public:
     26  XAir(core::FrameworkManager *parent, std::string uav_name,
     27       filter::UavMultiplex *multiplex = NULL);
     28  ~XAir();
     29  void StartSensors(void);
    3130
    32         private:
    33 
    34     };
     31private:
     32};
    3533} // end namespace meta
    3634} // end namespace flair

  • trunk/lib/FlairMeta/src/unexported/MetaDualShock3_impl.h

    r10 r15  
    1616#include <IODevice.h>
    1717
    18 namespace flair
    19 {
    20     namespace meta
    21     {
    22         class MetaDualShock3;
    23     }
    24     namespace filter
    25     {
    26         class JoyReference;
    27     }
     18namespace flair {
     19namespace meta {
     20class MetaDualShock3;
     21}
     22namespace filter {
     23class JoyReference;
     24}
    2825}
    2926
    … …  
    3229* \brief Classe intégrant la manette DualShock3 et les consignes joystick
    3330*/
    34 class MetaDualShock3_impl: public flair::core::IODevice
    35 {
    36     public:
    37         MetaDualShock3_impl(flair::meta::MetaDualShock3* self,std::string name);
    38         ~MetaDualShock3_impl();
    39         flair::filter::JoyReference *joy_ref;
     31class MetaDualShock3_impl : public flair::core::IODevice {
     32public:
     33  MetaDualShock3_impl(flair::meta::MetaDualShock3 *self, std::string name);
     34  ~MetaDualShock3_impl();
     35  flair::filter::JoyReference *joy_ref;
    4036
    41     private:
    42         void UpdateFrom(const flair::core::io_data *data);
    43         flair::meta::MetaDualShock3* self;
    44         bool joy_init;
    45         bool wasRollTrimUpButtonPressed,wasRollTrimDownButtonPressed,wasPitchTrimUpButtonPressed,wasPitchTrimDownButtonPressed;
     37private:
     38  void UpdateFrom(const flair::core::io_data *data);
     39  flair::meta::MetaDualShock3 *self;
     40  bool joy_init;
     41  bool wasRollTrimUpButtonPressed, wasRollTrimDownButtonPressed,
     42      wasPitchTrimUpButtonPressed, wasPitchTrimDownButtonPressed;
    4643};
    4744
Note: See TracChangeset for help on using the changeset viewer.