Changeset 38 in flair-src for trunk/lib

Timestamp:

Jun 23, 2016, 10:15:30 AM (9 years ago)

Author:

Bayard Gildas

Message:

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

Location:

trunk/lib

Files:

• FlairCore/src/IODevice.cpp (modified) (2 diffs)
• FlairCore/src/IODevice.h (modified) (1 diff)
• FlairCore/src/Thread_impl.cpp (modified) (2 diffs)
• FlairMeta/src/MetaDualShock3.cpp (modified) (5 diffs)
• FlairMeta/src/MetaDualShock3.h (modified) (3 diffs)
• FlairMeta/src/MetaDualShock3_impl.cpp (modified) (6 diffs)
• FlairMeta/src/UavStateMachine.cpp (modified) (11 diffs)
• FlairMeta/src/UavStateMachine.h (modified) (4 diffs)
• FlairMeta/src/unexported/MetaDualShock3_impl.h (modified) (2 diffs)
• FlairSensorActuator/src/EmulatedController.cpp (added)
• FlairSensorActuator/src/EmulatedController.h (added)
• FlairSensorActuator/src/TargetController.cpp (modified) (2 diffs)
• FlairSensorActuator/src/TargetEthController.cpp (modified) (1 diff)

trunk/lib/FlairCore/src/IODevice.cpp

@@ -27 +27 @@
 namespace core {
 
-IODevice::IODevice(const Object *parent, string name)
-    : Object(parent, name, "IODevice") {
-  pimpl_ = new IODevice_impl(this);
+IODevice::IODevice(const Object* parent,string name): Object(parent,name,"IODevice") {
+    pimpl_=new IODevice_impl(this);
+    lastUpdate=GetTime();
 }
 
@@ -48 +48 @@
   }
 
-  if (data != NULL) {
-    if (getFrameworkManager()->IsLogging() == true)
-      pimpl_->WriteLog(data->DataTime());
+    if(data!=NULL) {
+        lastUpdate=data->DataTime();
+        if(getFrameworkManager()->IsLogging()==true) pimpl_->WriteLog(data->DataTime());
 
     data->pimpl_->Circle();

trunk/lib/FlairCore/src/IODevice.h

@@ -114 +114 @@
   virtual DataType const &GetOutputDataType() const;
 
-protected:
-  /*!
-  * \brief Process the childs of type IODevice, and log if needed
-  *
-  * This method must be called after computing datas;
-  * generally at the end of the reimplemented UpdateFrom or after acquiring
-  *datas in case of a sensor. \n
-  * It will call UpdateFrom methods of each child of type IODevice,
-  * and log all datas (this IODevice and its childs)
-  * if logging is enabled (see SetDataToLog(), AddDeviceToLog(),
-  * FrameworkManager::StartLog and FrameworkManager::AddDeviceToLog). \n
-  * If a thread is waiting on this IODevice (see Thread::WaitUpdate), it will be
-  *resumed.
-  *
-  * \param data data to process
-  */
-  void ProcessUpdate(io_data *data);
+            Time lastUpdate;
+        protected:
+            /*!
+            * \brief Process the childs of type IODevice, and log if needed
+            *
+            * This method must be called after computing datas;
+            * generally at the end of the reimplemented UpdateFrom or after acquiring datas in case of a sensor. \n
+            * It will call UpdateFrom methods of each child of type IODevice,
+            * and log all datas (this IODevice and its childs)
+            * if logging is enabled (see SetDataToLog(), AddDeviceToLog(),
+            * FrameworkManager::StartLog and FrameworkManager::AddDeviceToLog). \n
+            * If a thread is waiting on this IODevice (see Thread::WaitUpdate), it will be resumed.
+            *
+            * \param data data to process
+            */
+            void ProcessUpdate(io_data* data);
 
 private:

trunk/lib/FlairCore/src/Thread_impl.cpp

@@ -28 +28 @@
 #else
 #include <sys/resource.h>
+#include <unistd.h>
+#include <sys/syscall.h>
 #endif
 
@@ -153 +155 @@
 }
 #else
-void *Thread_impl::main_nrt(void *arg) {
-  Thread_impl *caller = (Thread_impl *)arg;
+void* Thread_impl::main_nrt(void * arg)
+{
+    Thread_impl *caller = (Thread_impl*)arg;
+/*string th_name=getFrameworkManager()->ObjectName()+ "-" + caller->self->ObjectName();
+caller->self->Info("pthread '%s' created with TID %x\n",th_name.c_str(),(pid_t)syscall(SYS_gettid));*/
 
   caller->self->Run();

trunk/lib/FlairMeta/src/MetaDualShock3.cpp

@@ -18 +18 @@
 #include "MetaDualShock3.h"
 #include "MetaDualShock3_impl.h"
+#include "TargetController.h"
 #include "JoyReference.h"
 #include <Tab.h>
@@ -30 +31 @@
 using namespace flair::filter;
 using namespace flair::gui;
+using namespace flair::sensor;
 
 namespace flair {
@@ -35 +37 @@
 
 MetaDualShock3::MetaDualShock3(FrameworkManager *parent, string name,
-                               uint16_t port, uint8_t priority)
-    : TargetEthController(parent, name, port, priority) {
+                               TargetController *controller)
+    : controller(controller),IODevice((IODevice*)controller, name) {
   pimpl_ = new MetaDualShock3_impl(this, name);
   parent->AddDeviceToLog(pimpl_->joy_ref);
-  Start();
+  controller->Start();
 }
 
@@ -49 +51 @@
 
 void MetaDualShock3::ErrorNotify(void) {
-  TargetEthController::FlashLed(4, 10, 0);
-  TargetEthController::Rumble(0xff, 20, 0, 0);
+    controller->FlashLed(4,10,0);
+    controller->Rumble(0xff,20,0,0);
 }
 
-void MetaDualShock3::Rumble(uint8_t left_force, uint8_t left_timeout,
-                            uint8_t right_force, uint8_t right_timeout) {
-  TargetEthController::Rumble(left_force, left_timeout, right_force,
-                              right_timeout);
+void MetaDualShock3::Rumble(uint8_t left_force,uint8_t left_timeout,uint8_t right_force,uint8_t right_timeout) {
+    controller->Rumble(left_force,left_timeout,right_force,right_timeout);
 }
 
 void MetaDualShock3::SetLedON(unsigned int ledId) {
-  TargetEthController::SetLedOn(ledId);
+    controller->SetLedOn(ledId);
 }
 
 void MetaDualShock3::SetLedOFF(unsigned int ledId) {
-  TargetEthController::SetLedOff(ledId);
+    controller->SetLedOff(ledId);
 }
 
-void MetaDualShock3::FlashLed(unsigned int ledId, uint8_t on_timeout,
-                              uint8_t off_timeout) {
-  TargetEthController::FlashLed(ledId, on_timeout, off_timeout);
+void MetaDualShock3::FlashLed(unsigned int ledId,uint8_t on_timeout,uint8_t off_timeout) {
+    controller->FlashLed(ledId,on_timeout,off_timeout);
 }
 
@@ -91 +90 @@
 
 float MetaDualShock3::PitchTrim(void) const {
-  return pimpl_->joy_ref->PitchTrim();
+    return pimpl_->joy_ref->PitchTrim();
+}
+
+void MetaDualShock3::UpdateFrom(const flair::core::io_data *data) {
+    pimpl_->UpdateFrom(data);
 }
 

trunk/lib/FlairMeta/src/MetaDualShock3.h

@@ -14 +14 @@
 #define METADUALSHOCK3_H
 
-#include <TargetEthController.h>
+#include <TargetController.h>
+#include <IODevice.h>
 
 namespace flair {
@@ -35 +36 @@
 * \brief Classe intégrant la manette MetaDualShock3
 */
-class MetaDualShock3 : public sensor::TargetEthController {
+class MetaDualShock3 : public core::IODevice {
   friend class ::MetaDualShock3_impl;
 
 public:
   MetaDualShock3(core::FrameworkManager *parent, std::string name,
-                 uint16_t port, uint8_t priority);
+                 sensor::TargetController *controller);
   ~MetaDualShock3();
   core::AhrsData *GetReferenceOrientation(void) const;
@@ -66 +67 @@
   void FlashLed(unsigned int ledId, uint8_t on_timeout, uint8_t off_timeout);
 
-private:
-  class MetaDualShock3_impl *pimpl_;
-};
+        private:
+            class MetaDualShock3_impl* pimpl_;
+            void UpdateFrom(const flair::core::io_data *data);
+            sensor::TargetController *controller;
+    };
 } // end namespace meta
 } // end namespace flair

trunk/lib/FlairMeta/src/MetaDualShock3_impl.cpp

@@ -29 +29 @@
 using namespace flair::meta;
 
-MetaDualShock3_impl::MetaDualShock3_impl(MetaDualShock3 *self, string name)
-    : IODevice((IODevice *)self, name) {
-  joy_ref = new JoyReference(self->GetTab()->NewRow(), "consignes joy");
+MetaDualShock3_impl::MetaDualShock3_impl(MetaDualShock3 *self, string name) {
+  joy_ref=new JoyReference(self->controller->GetTab()->NewRow(),"consignes joy");
   this->self = self;
   joy_init = false;
@@ -50 +49 @@
 
   // up
-  if (self->IsButtonPressed(12)) {
+  if (self->controller->IsButtonPressed(12)) {
     if (!wasPitchTrimDownButtonPressed) {
       joy_ref->PitchTrimDown();
@@ -60 +59 @@
 
   // down
-  if (self->IsButtonPressed(13)) {
+  if (self->controller->IsButtonPressed(13)) {
     if (!wasPitchTrimUpButtonPressed) {
       joy_ref->PitchTrimUp();
@@ -70 +69 @@
 
   // right
-  if (self->IsButtonPressed(15)) {
+  if (self->controller->IsButtonPressed(15)) {
     if (!wasRollTrimUpButtonPressed) {
       joy_ref->RollTrimUp();
@@ -80 +79 @@
 
   // left
-  if (self->IsButtonPressed(14)) {
+  if (self->controller->IsButtonPressed(14)) {
     if (!wasRollTrimDownButtonPressed) {
       joy_ref->RollTrimDown();
@@ -107 +106 @@
 
   if (!joy_init) {
-    self->TargetEthController::FlashLed(1, 10, 10);
+    self->controller->FlashLed(1, 10, 10);
     joy_init = true;
   }

trunk/lib/FlairMeta/src/UavStateMachine.cpp

@@ -51 +51 @@
 using namespace flair::meta;
 
-UavStateMachine::UavStateMachine(Uav *inUav, uint16_t ds3Port)
-    : Thread(getFrameworkManager(), "UavStateMachine", 50), uav(inUav),
-      failSafeMode(true), flagConnectionLost(false), flagBatteryLow(false),
-      flagZTrajectoryFinished(false) {
-  altitudeState = AltitudeState_t::Stopped;
-  uav->UseDefaultPlot();
+UavStateMachine::UavStateMachine(Uav* inUav,TargetController *controller):
+        Thread(getFrameworkManager(),"UavStateMachine",50),
+        uav(inUav),controller(controller),failSafeMode(true),flagConnectionLost(false),flagBatteryLow(false),flagCriticalSensorLost(false),flagZTrajectoryFinished(false),safeToFly(true){
+    altitudeState=AltitudeState_t::Stopped;
+    uav->UseDefaultPlot();
 
   Tab *uavTab = new Tab(getFrameworkManager()->GetTabWidget(), "uav", 0);
@@ -90 +89 @@
   uZ->AddDeviceToLog(uYaw);
 
-  joy = new MetaDualShock3(getFrameworkManager(), "dualshock3", ds3Port, 30);
-  uav->GetAhrs()->AddPlot(joy->GetReferenceOrientation(), DataPlot::Blue);
+    joy=new MetaDualShock3(getFrameworkManager(),"uav high level controller",controller);
+    uav->GetAhrs()->AddPlot(joy->GetReferenceOrientation(),DataPlot::Blue);
 
   altitudeMode = AltitudeMode_t::Manual;
@@ -124 +123 @@
 }
 
-const TargetController *UavStateMachine::GetJoystick(void) const { return joy; }
+const TargetController *UavStateMachine::GetJoystick(void) const {
+    return controller;
+}
 
 const Quaternion &UavStateMachine::GetCurrentQuaternion(void) const {
@@ -253 +254 @@
       // The Uav target altitude has reached its landing value (typically 0)
       // but the real Uav altitude may not have reach this value yet because of
-      // command delay. Moreover, it
-      // may never exactly reach this value if the ground is not perfectly
-      // leveled (critical case: there's a
+      // command delay. Moreover, it may never exactly reach this value if the 
+      // ground is not perfectly leveled (critical case: there's a
       // deep and narrow hole right in the sensor line of sight). That's why we
       // have a 2 phases landing strategy.
@@ -424 +424 @@
   flagZTrajectoryFinished = false;
 
-  if (altitudeState == AltitudeState_t::Stopped) { // &&
-    // GetBatteryMonitor()->IsBatteryLow()==false)
-    // The uav always takes off in fail safe mode
-    EnterFailSafeMode();
-    joy->SetLedOFF(4); // DualShock3::led4
-    joy->SetLedOFF(1); // DualShock3::led1
-    joy->Rumble(0x70);
-    joy->SetZRef(0);
+    if((altitudeState==AltitudeState_t::Stopped) && safeToFly) {// && GetBatteryMonitor()->IsBatteryLow()==false)
+        //The uav always takes off in fail safe mode
+        EnterFailSafeMode();
+        joy->SetLedOFF(4);//DualShock3::led4
+        joy->SetLedOFF(1);//DualShock3::led1
+        joy->Rumble(0x70);
+        joy->SetZRef(0);
 
     uZ->SetOffset(); // positionne l'offset de compensation de la gravité à sa
@@ -458 +457 @@
     joy->Rumble(0x70);
 
-    altitudeTrajectory->StopTraj();
-    joy->SetZRef(0);
-    altitudeTrajectory->StartTraj(
-        currentAltitude,
-        desiredLandingAltitude->Value()); // shouldn't it be groundAltitude?
-    SignalEvent(Event_t::StartLanding);
-    altitudeState = AltitudeState_t::StartLanding;
-  } else {
-    joy->ErrorNotify();
-  }
+        altitudeTrajectory->StopTraj();
+        joy->SetZRef(0);
+        altitudeTrajectory->StartTraj(currentAltitude,desiredLandingAltitude->Value()); //shouldn't it be groundAltitude?
+        SignalEvent(Event_t::StartLanding);
+        altitudeState=AltitudeState_t::StartLanding;
+    } else if (altitudeState==AltitudeState_t::TakingOff) {
+        EmergencyLand();
+    } else {
+        joy->ErrorNotify();
+    }
+}
+
+void UavStateMachine::EmergencyLand(void) {
+    //Gradually decrease motor speed
+    //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)
+    altitudeState=AltitudeState_t::FinishLanding;
+    safeToFly=false;
+Printf("Emergency landing!\n");
 }
 
@@ -494 +501 @@
 
 void UavStateMachine::EmergencyStop(void) {
-  if (altitudeState != AltitudeState_t::Stopped) {
-    StopMotors();
-    EnterFailSafeMode();
-    joy->Rumble(0x70);
-    SignalEvent(Event_t::EmergencyStop);
-  }
+    if(altitudeState!=AltitudeState_t::Stopped) {
+        StopMotors();
+        EnterFailSafeMode();
+        joy->Rumble(0x70);
+        SignalEvent(Event_t::EmergencyStop);
+    }
+    safeToFly=false;
 }
 
@@ -534 +542 @@
     }
   }
-  if ((altitudeState == AltitudeState_t::TakingOff ||
-       altitudeState == AltitudeState_t::Stabilized) &&
-      uav->GetBatteryMonitor()->IsBatteryLow() && !flagBatteryLow) {
-    flagBatteryLow = true;
-    Printf("Battery Low\n");
-    EnterFailSafeMode();
-    Land();
-  }
+    if((altitudeState==AltitudeState_t::TakingOff || altitudeState==AltitudeState_t::Stabilized) && uav->GetBatteryMonitor()->IsBatteryLow() && !flagBatteryLow) {
+        flagBatteryLow=true;
+        Thread::Err("Low Battery\n");
+        EnterFailSafeMode();
+        Land();
+    }
+    Time now=GetTime();
+    if ((altitudeState==AltitudeState_t::Stopped) && (now-uav->GetAhrs()->lastUpdate>(Time)100*1000*1000)) { //100ms
+        flagCriticalSensorLost=true;
+        Thread::Err("Critical sensor lost\n");
+        EnterFailSafeMode();
+        EmergencyLand(); 
+    }
 }
 
@@ -554 +567 @@
   static bool isSafeModeButtonPressed = false;
 
-  if (joy->IsButtonPressed(1)) { // select
+  if (controller->IsButtonPressed(1)) { // select
     if (!isEmergencyStopButtonPressed) {
       isEmergencyStopButtonPressed = true;
@@ -563 +576 @@
     isEmergencyStopButtonPressed = false;
 
-  if (joy->IsButtonPressed(0)) { // start
+  if (controller->IsButtonPressed(0)) { // start
     if (!isTakeOffButtonPressed) {
       isTakeOffButtonPressed = true;
@@ -585 +598 @@
   // check if l1,l2,r1 and r2 are not pressed
   // to allow a combination in user program
-  if (joy->IsButtonPressed(5) && !joy->IsButtonPressed(6) &&
-      !joy->IsButtonPressed(7) && !joy->IsButtonPressed(9) &&
-      !joy->IsButtonPressed(10)) {
+  if (controller->IsButtonPressed(5) && !controller->IsButtonPressed(6) &&
+      !controller->IsButtonPressed(7) && !controller->IsButtonPressed(9) &&
+      !controller->IsButtonPressed(10)) {
     if (!isSafeModeButtonPressed) {
       isSafeModeButtonPressed = true;

trunk/lib/FlairMeta/src/UavStateMachine.h

@@ -101 +101 @@
   };
 
-  UavStateMachine(meta::Uav *uav, uint16_t ds3Port = 20000);
-  ~UavStateMachine();
+        UavStateMachine(meta::Uav* uav, sensor::TargetController* controller);
+        ~UavStateMachine();
 
   const core::Quaternion &GetCurrentQuaternion(void) const;
@@ -110 +110 @@
   const meta::Uav *GetUav(void) const;
 
-  void Land(void);
-  void TakeOff(void);
-  void EmergencyStop(void);
-  //! Used to signal an event
-  /*!
-      \param event the event which occured
-  */
-  virtual void SignalEvent(Event_t event);
+        void Land(void);
+        void EmergencyLand(void);
+        void TakeOff(void);
+        void EmergencyStop(void);
+        //! Used to signal an event
+        /*!
+            \param event the event which occured
+        */
+        virtual void SignalEvent(Event_t event);
 
   virtual const core::AhrsData *GetOrientation(void) const;
@@ -262 +263 @@
 
   meta::Uav *uav;
+  sensor::TargetController *controller;
 
   core::Quaternion currentQuaternion;
@@ -289 +291 @@
   bool flagBatteryLow;
   bool flagConnectionLost;
+  bool flagCriticalSensorLost;
   bool flagZTrajectoryFinished;
+  bool safeToFly;
   filter::NestedSat *uRoll, *uPitch;
   filter::Pid *uYaw;

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

@@ -14 +14 @@
 #define METADUALSHOCK3_IMPL_H
 
-#include <IODevice.h>
+#include <MetaDualShock3.h>
 
 namespace flair {
@@ -29 +29 @@
 * \brief Classe intégrant la manette DualShock3 et les consignes joystick
 */
-class MetaDualShock3_impl : public flair::core::IODevice {
+class MetaDualShock3_impl {
 public:
   MetaDualShock3_impl(flair::meta::MetaDualShock3 *self, std::string name);
   ~MetaDualShock3_impl();
   flair::filter::JoyReference *joy_ref;
+  void UpdateFrom(const flair::core::io_data *data);
 
 private:
-  void UpdateFrom(const flair::core::io_data *data);
   flair::meta::MetaDualShock3 *self;
   bool joy_init;

trunk/lib/FlairSensorActuator/src/TargetController.cpp

@@ -34 +34 @@
 namespace sensor {
 
-TargetController::TargetController(const FrameworkManager *parent, string name,
-                                   uint8_t priority)
-    : Thread(parent, name, priority), IODevice(parent, name) {
-  main_tab = new Tab(getFrameworkManager()->GetTabWidget(), name);
-  TabWidget *tab = new TabWidget(main_tab->NewRow(), name);
-  setup_tab = new Tab(tab, "Reglages");
+TargetController::TargetController(const FrameworkManager* parent,string name,uint8_t priority) :
+    Thread(parent,name,priority),IODevice(parent,name) {
+    main_tab=new Tab(getFrameworkManager()->GetTabWidget(),name);
+    TabWidget* tab=new TabWidget(main_tab->NewRow(),name);
+    setup_tab=new Tab(tab,"Reglages");
+    SetPeriodMS(20);; //50Hz
 }
 
@@ -150 +150 @@
           }
         }
-      } else {
-        // Thread::Info("Debug: relax...\n");
-        usleep(20000); // 20ms
       }
+      WaitPeriod();
     }
   }

trunk/lib/FlairSensorActuator/src/TargetEthController.cpp

@@ -80 +80 @@
   bool fullDatagramReceived = false;
 
-  do {
-    received =
-        dataSocket->RecvMessage(receiveFrameBuffer + receiveCurrentPosition,
-                                bytesToReceive, TIME_NONBLOCK);
-    if (received > 0) {
-      bytesToReceive -= received;
-      if (bytesToReceive == 0) {
-        // a full datagram has been read in receiveFrameBuffer
-        fullDatagramReceived = true;
-        receiveCurrentPosition = 0;
-        // we swap the data and reception buffers to avoid copy
-        char *swapFrameBuffer = dataFrameBuffer;
-        dataFrameBuffer = receiveFrameBuffer;
-        receiveFrameBuffer = swapFrameBuffer;
-      }
-    }
-  } while (!received < 0);
+    do {
+        received=dataSocket->RecvMessage(receiveFrameBuffer+receiveCurrentPosition,bytesToReceive,TIME_NONBLOCK);
+        if (received>0) {
+            receiveCurrentPosition+=received;
+            bytesToReceive-=received;
+            if (bytesToReceive==0) {
+                //a full datagram has been read in receiveFrameBuffer
+                fullDatagramReceived=true;
+                //we swap the data and reception buffers to avoid copy
+                char *swapFrameBuffer=dataFrameBuffer;
+                dataFrameBuffer=receiveFrameBuffer;
+                receiveFrameBuffer=swapFrameBuffer;
+                receiveCurrentPosition=0;
+                bytesToReceive=dataFrameSize;
+            }
+        }
+    } while (!(received<0));
 
   return fullDatagramReceived;