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


Ignore:
Timestamp:
06/23/16 10:15:30 (8 years ago)
Author:
Bayard Gildas
Message:

Modif. pour ajour manette émulée (EmulatedController)

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

Legend:

Unmodified
Added
Removed

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

    r15 r38  
    1818#include "MetaDualShock3.h"
    1919#include "MetaDualShock3_impl.h"
     20#include "TargetController.h"
    2021#include "JoyReference.h"
    2122#include <Tab.h>
    … …  
    3031using namespace flair::filter;
    3132using namespace flair::gui;
     33using namespace flair::sensor;
    3234
    3335namespace flair {
    … …  
    3537
    3638MetaDualShock3::MetaDualShock3(FrameworkManager *parent, string name,
    37                                uint16_t port, uint8_t priority)
    38     : TargetEthController(parent, name, port, priority) {
     39                               TargetController *controller)
     40    : controller(controller),IODevice((IODevice*)controller, name) {
    3941  pimpl_ = new MetaDualShock3_impl(this, name);
    4042  parent->AddDeviceToLog(pimpl_->joy_ref);
    41   Start();
     43  controller->Start();
    4244}
    4345
    … …  
    4951
    5052void MetaDualShock3::ErrorNotify(void) {
    51   TargetEthController::FlashLed(4, 10, 0);
    52   TargetEthController::Rumble(0xff, 20, 0, 0);
     53    controller->FlashLed(4,10,0);
     54    controller->Rumble(0xff,20,0,0);
    5355}
    5456
    55 void 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);
     57void MetaDualShock3::Rumble(uint8_t left_force,uint8_t left_timeout,uint8_t right_force,uint8_t right_timeout) {
     58    controller->Rumble(left_force,left_timeout,right_force,right_timeout);
    5959}
    6060
    6161void MetaDualShock3::SetLedON(unsigned int ledId) {
    62   TargetEthController::SetLedOn(ledId);
     62    controller->SetLedOn(ledId);
    6363}
    6464
    6565void MetaDualShock3::SetLedOFF(unsigned int ledId) {
    66   TargetEthController::SetLedOff(ledId);
     66    controller->SetLedOff(ledId);
    6767}
    6868
    69 void MetaDualShock3::FlashLed(unsigned int ledId, uint8_t on_timeout,
    70                               uint8_t off_timeout) {
    71   TargetEthController::FlashLed(ledId, on_timeout, off_timeout);
     69void MetaDualShock3::FlashLed(unsigned int ledId,uint8_t on_timeout,uint8_t off_timeout) {
     70    controller->FlashLed(ledId,on_timeout,off_timeout);
    7271}
    7372
    … …  
    9190
    9291float MetaDualShock3::PitchTrim(void) const {
    93   return pimpl_->joy_ref->PitchTrim();
     92    return pimpl_->joy_ref->PitchTrim();
     93}
     94
     95void MetaDualShock3::UpdateFrom(const flair::core::io_data *data) {
     96    pimpl_->UpdateFrom(data);
    9497}
    9598

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

    r15 r38  
    1414#define METADUALSHOCK3_H
    1515
    16 #include <TargetEthController.h>
     16#include <TargetController.h>
     17#include <IODevice.h>
    1718
    1819namespace flair {
    … …  
    3536* \brief Classe intégrant la manette MetaDualShock3
    3637*/
    37 class MetaDualShock3 : public sensor::TargetEthController {
     38class MetaDualShock3 : public core::IODevice {
    3839  friend class ::MetaDualShock3_impl;
    3940
    4041public:
    4142  MetaDualShock3(core::FrameworkManager *parent, std::string name,
    42                  uint16_t port, uint8_t priority);
     43                 sensor::TargetController *controller);
    4344  ~MetaDualShock3();
    4445  core::AhrsData *GetReferenceOrientation(void) const;
    … …  
    6667  void FlashLed(unsigned int ledId, uint8_t on_timeout, uint8_t off_timeout);
    6768
    68 private:
    69   class MetaDualShock3_impl *pimpl_;
    70 };
     69        private:
     70            class MetaDualShock3_impl* pimpl_;
     71            void UpdateFrom(const flair::core::io_data *data);
     72            sensor::TargetController *controller;
     73    };
    7174} // end namespace meta
    7275} // end namespace flair

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

    r15 r38  
    2929using namespace flair::meta;
    3030
    31 MetaDualShock3_impl::MetaDualShock3_impl(MetaDualShock3 *self, string name)
    32     : IODevice((IODevice *)self, name) {
    33   joy_ref = new JoyReference(self->GetTab()->NewRow(), "consignes joy");
     31MetaDualShock3_impl::MetaDualShock3_impl(MetaDualShock3 *self, string name) {
     32  joy_ref=new JoyReference(self->controller->GetTab()->NewRow(),"consignes joy");
    3433  this->self = self;
    3534  joy_init = false;
    … …  
    5049
    5150  // up
    52   if (self->IsButtonPressed(12)) {
     51  if (self->controller->IsButtonPressed(12)) {
    5352    if (!wasPitchTrimDownButtonPressed) {
    5453      joy_ref->PitchTrimDown();
    … …  
    6059
    6160  // down
    62   if (self->IsButtonPressed(13)) {
     61  if (self->controller->IsButtonPressed(13)) {
    6362    if (!wasPitchTrimUpButtonPressed) {
    6463      joy_ref->PitchTrimUp();
    … …  
    7069
    7170  // right
    72   if (self->IsButtonPressed(15)) {
     71  if (self->controller->IsButtonPressed(15)) {
    7372    if (!wasRollTrimUpButtonPressed) {
    7473      joy_ref->RollTrimUp();
    … …  
    8079
    8180  // left
    82   if (self->IsButtonPressed(14)) {
     81  if (self->controller->IsButtonPressed(14)) {
    8382    if (!wasRollTrimDownButtonPressed) {
    8483      joy_ref->RollTrimDown();
    … …  
    107106
    108107  if (!joy_init) {
    109     self->TargetEthController::FlashLed(1, 10, 10);
     108    self->controller->FlashLed(1, 10, 10);
    110109    joy_init = true;
    111110  }

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

    r15 r38  
    5151using namespace flair::meta;
    5252
    53 UavStateMachine::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();
     53UavStateMachine::UavStateMachine(Uav* inUav,TargetController *controller):
     54        Thread(getFrameworkManager(),"UavStateMachine",50),
     55        uav(inUav),controller(controller),failSafeMode(true),flagConnectionLost(false),flagBatteryLow(false),flagCriticalSensorLost(false),flagZTrajectoryFinished(false),safeToFly(true){
     56    altitudeState=AltitudeState_t::Stopped;
     57    uav->UseDefaultPlot();
    5958
    6059  Tab *uavTab = new Tab(getFrameworkManager()->GetTabWidget(), "uav", 0);
    … …  
    9089  uZ->AddDeviceToLog(uYaw);
    9190
    92   joy = new MetaDualShock3(getFrameworkManager(), "dualshock3", ds3Port, 30);
    93   uav->GetAhrs()->AddPlot(joy->GetReferenceOrientation(), DataPlot::Blue);
     91    joy=new MetaDualShock3(getFrameworkManager(),"uav high level controller",controller);
     92    uav->GetAhrs()->AddPlot(joy->GetReferenceOrientation(),DataPlot::Blue);
    9493
    9594  altitudeMode = AltitudeMode_t::Manual;
    … …  
    124123}
    125124
    126 const TargetController *UavStateMachine::GetJoystick(void) const { return joy; }
     125const TargetController *UavStateMachine::GetJoystick(void) const {
     126    return controller;
     127}
    127128
    128129const Quaternion &UavStateMachine::GetCurrentQuaternion(void) const {
    … …  
    253254      // The Uav target altitude has reached its landing value (typically 0)
    254255      // 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
     256      // command delay. Moreover, it may never exactly reach this value if the
     257      // ground is not perfectly leveled (critical case: there's a
    258258      // deep and narrow hole right in the sensor line of sight). That's why we
    259259      // have a 2 phases landing strategy.
    … …  
    424424  flagZTrajectoryFinished = false;
    425425
    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);
     426    if((altitudeState==AltitudeState_t::Stopped) && safeToFly) {// && GetBatteryMonitor()->IsBatteryLow()==false)
     427        //The uav always takes off in fail safe mode
     428        EnterFailSafeMode();
     429        joy->SetLedOFF(4);//DualShock3::led4
     430        joy->SetLedOFF(1);//DualShock3::led1
     431        joy->Rumble(0x70);
     432        joy->SetZRef(0);
    434433
    435434    uZ->SetOffset(); // positionne l'offset de compensation de la gravité à sa
    … …  
    458457    joy->Rumble(0x70);
    459458
    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   }
     459        altitudeTrajectory->StopTraj();
     460        joy->SetZRef(0);
     461        altitudeTrajectory->StartTraj(currentAltitude,desiredLandingAltitude->Value()); //shouldn't it be groundAltitude?
     462        SignalEvent(Event_t::StartLanding);
     463        altitudeState=AltitudeState_t::StartLanding;
     464    } else if (altitudeState==AltitudeState_t::TakingOff) {
     465        EmergencyLand();
     466    } else {
     467        joy->ErrorNotify();
     468    }
     469}
     470
     471void UavStateMachine::EmergencyLand(void) {
     472    //Gradually decrease motor speed
     473    //Called if landing is required during take off (motors are accelerating but Uav did not actually left the ground yet), or if critical sensors have been lost (attitude is lost)
     474    altitudeState=AltitudeState_t::FinishLanding;
     475    safeToFly=false;
     476Printf("Emergency landing!\n");
    470477}
    471478
    … …  
    494501
    495502void UavStateMachine::EmergencyStop(void) {
    496   if (altitudeState != AltitudeState_t::Stopped) {
    497     StopMotors();
    498     EnterFailSafeMode();
    499     joy->Rumble(0x70);
    500     SignalEvent(Event_t::EmergencyStop);
    501   }
     503    if(altitudeState!=AltitudeState_t::Stopped) {
     504        StopMotors();
     505        EnterFailSafeMode();
     506        joy->Rumble(0x70);
     507        SignalEvent(Event_t::EmergencyStop);
     508    }
     509    safeToFly=false;
    502510}
    503511
    … …  
    534542    }
    535543  }
    536   if ((altitudeState == AltitudeState_t::TakingOff ||
    537        altitudeState == AltitudeState_t::Stabilized) &&
    538       uav->GetBatteryMonitor()->IsBatteryLow() && !flagBatteryLow) {
    539     flagBatteryLow = true;
    540     Printf("Battery Low\n");
    541     EnterFailSafeMode();
    542     Land();
    543   }
     544    if((altitudeState==AltitudeState_t::TakingOff || altitudeState==AltitudeState_t::Stabilized) && uav->GetBatteryMonitor()->IsBatteryLow() && !flagBatteryLow) {
     545        flagBatteryLow=true;
     546        Thread::Err("Low Battery\n");
     547        EnterFailSafeMode();
     548        Land();
     549    }
     550    Time now=GetTime();
     551    if ((altitudeState==AltitudeState_t::Stopped) && (now-uav->GetAhrs()->lastUpdate>(Time)100*1000*1000)) { //100ms
     552        flagCriticalSensorLost=true;
     553        Thread::Err("Critical sensor lost\n");
     554        EnterFailSafeMode();
     555        EmergencyLand();
     556    }
    544557}
    545558
    … …  
    554567  static bool isSafeModeButtonPressed = false;
    555568
    556   if (joy->IsButtonPressed(1)) { // select
     569  if (controller->IsButtonPressed(1)) { // select
    557570    if (!isEmergencyStopButtonPressed) {
    558571      isEmergencyStopButtonPressed = true;
    … …  
    563576    isEmergencyStopButtonPressed = false;
    564577
    565   if (joy->IsButtonPressed(0)) { // start
     578  if (controller->IsButtonPressed(0)) { // start
    566579    if (!isTakeOffButtonPressed) {
    567580      isTakeOffButtonPressed = true;
    … …  
    585598  // check if l1,l2,r1 and r2 are not pressed
    586599  // 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)) {
     600  if (controller->IsButtonPressed(5) && !controller->IsButtonPressed(6) &&
     601      !controller->IsButtonPressed(7) && !controller->IsButtonPressed(9) &&
     602      !controller->IsButtonPressed(10)) {
    590603    if (!isSafeModeButtonPressed) {
    591604      isSafeModeButtonPressed = true;

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

    r15 r38  
    101101  };
    102102
    103   UavStateMachine(meta::Uav *uav, uint16_t ds3Port = 20000);
    104   ~UavStateMachine();
     103        UavStateMachine(meta::Uav* uav, sensor::TargetController* controller);
     104        ~UavStateMachine();
    105105
    106106  const core::Quaternion &GetCurrentQuaternion(void) const;
    … …  
    110110  const meta::Uav *GetUav(void) const;
    111111
    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);
     112        void Land(void);
     113        void EmergencyLand(void);
     114        void TakeOff(void);
     115        void EmergencyStop(void);
     116        //! Used to signal an event
     117        /*!
     118            \param event the event which occured
     119        */
     120        virtual void SignalEvent(Event_t event);
    120121
    121122  virtual const core::AhrsData *GetOrientation(void) const;
    … …  
    262263
    263264  meta::Uav *uav;
     265  sensor::TargetController *controller;
    264266
    265267  core::Quaternion currentQuaternion;
    … …  
    289291  bool flagBatteryLow;
    290292  bool flagConnectionLost;
     293  bool flagCriticalSensorLost;
    291294  bool flagZTrajectoryFinished;
     295  bool safeToFly;
    292296  filter::NestedSat *uRoll, *uPitch;
    293297  filter::Pid *uYaw;

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

    r15 r38  
    1414#define METADUALSHOCK3_IMPL_H
    1515
    16 #include <IODevice.h>
     16#include <MetaDualShock3.h>
    1717
    1818namespace flair {
    … …  
    2929* \brief Classe intégrant la manette DualShock3 et les consignes joystick
    3030*/
    31 class MetaDualShock3_impl : public flair::core::IODevice {
     31class MetaDualShock3_impl {
    3232public:
    3333  MetaDualShock3_impl(flair::meta::MetaDualShock3 *self, std::string name);
    3434  ~MetaDualShock3_impl();
    3535  flair::filter::JoyReference *joy_ref;
     36  void UpdateFrom(const flair::core::io_data *data);
    3637
    3738private:
    38   void UpdateFrom(const flair::core::io_data *data);
    3939  flair::meta::MetaDualShock3 *self;
    4040  bool joy_init;
Note: See TracChangeset for help on using the changeset viewer.