Changeset 38 in flair-src for trunk


Ignore:
Timestamp:
Jun 23, 2016, 10:15:30 AM (9 years ago)
Author:
Bayard Gildas
Message:

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

Location:
trunk
Files:
2 added
17 edited

Legend:

Unmodified
Added
Removed

  • trunk/demos/CircleFollower/uav/src/CircleFollower.cpp

    r23 r38  
    1313
    1414#include "CircleFollower.h"
     15#include <TargetController.h>
    1516#include <Uav.h>
    1617#include <GridLayout.h>
    … …  
    3334using namespace flair::core;
    3435using namespace flair::gui;
     36using namespace flair::sensor;
    3537using namespace flair::filter;
    3638using namespace flair::meta;
    3739
    38 CircleFollower::CircleFollower(Uav* uav): UavStateMachine(uav), behaviourMode(BehaviourMode_t::Default), vrpnLost(false) {
     40CircleFollower::CircleFollower(Uav* uav,TargetController *controller): UavStateMachine(uav,controller), behaviourMode(BehaviourMode_t::Default), vrpnLost(false) {
    3941    uav->SetupVRPNAutoIP(uav->ObjectName());
    4042

  • trunk/demos/CircleFollower/uav/src/CircleFollower.h

    r23 r38  
    2727        class MetaVrpnObject;
    2828    }
     29    namespace sensor {
     30        class TargetController;
     31    }
    2932}
    3033
    3134class CircleFollower : public flair::meta::UavStateMachine {
    3235    public:
    33         CircleFollower(flair::meta::Uav* uav);
     36        CircleFollower(flair::meta::Uav* uav,flair::sensor::TargetController *controller);
    3437        ~CircleFollower();
    3538

  • trunk/demos/CircleFollower/uav/src/main.cpp

    r23 r38  
    1717#include <stdio.h>
    1818#include <tclap/CmdLine.h>
     19#include <TargetEthController.h>
    1920
    2021using namespace TCLAP;
    … …  
    2223using namespace flair::core;
    2324using namespace flair::meta;
     25using namespace flair::sensor;
    2426
    2527string uav_type;
    2628string log_path;
    2729int port;
     30int ds3port;
    2831string xml_file;
    2932string name;
    … …  
    4346
    4447    Uav* drone=CreateUav(manager,name,uav_type);
    45     CircleFollower* demo=new CircleFollower(drone);
     48    TargetEthController *controller=new TargetEthController(manager,"Dualshock3",ds3port);
     49    CircleFollower* demo=new CircleFollower(drone,controller);
    4650
    4751    demo->Start();
    … …  
    7478        cmd.add( addressArg );
    7579
     80        ValueArg<int> ds3portArg("d","ds3_port","port pour ds3",false,20000,"int");
     81        cmd.add( ds3portArg );
     82
    7683        cmd.parse( argc, argv );
    7784
    … …  
    7986        log_path = logsArg.getValue();
    8087        port=portArg.getValue();
     88        ds3port=ds3portArg.getValue();
    8189        xml_file = xmlArg.getValue();
    8290        name=nameArg.getValue();

  • trunk/demos/SimpleFleet/uav/src/SimpleFleet.cpp

    r33 r38  
    1313
    1414#include "SimpleFleet.h"
     15#include <TargetController.h>
    1516#include <Uav.h>
    1617#include <GridLayout.h>
    … …  
    4950
    5051
    51 SimpleFleet::SimpleFleet(flair::meta::Uav* uav,string broadcast,uint16_t ds3port): UavStateMachine(uav,ds3port), behaviourMode(BehaviourMode_t::Default), vrpnLost(false) {
     52SimpleFleet::SimpleFleet(flair::meta::Uav* uav,string broadcast,TargetController *controller): UavStateMachine(uav,controller), behaviourMode(BehaviourMode_t::Default), vrpnLost(false) {
    5253    uav->SetupVRPNAutoIP(uav->ObjectName());
    5354

  • trunk/demos/SimpleFleet/uav/src/SimpleFleet.h

    r33 r38  
    3434class SimpleFleet : public flair::meta::UavStateMachine {
    3535    public:
    36         SimpleFleet(flair::meta::Uav* uav,std::string broadcast,uint16_t ds3port);
     36        SimpleFleet(flair::meta::Uav* uav,std::string broadcast,flair::sensor::TargetController *controller);
    3737        ~SimpleFleet();
    3838

  • trunk/demos/SimpleFleet/uav/src/main.cpp

    r33 r38  
    1717#include <stdio.h>
    1818#include <tclap/CmdLine.h>
     19#include <TargetEthController.h>
    1920
    2021using namespace TCLAP;
    … …  
    2223using namespace flair::core;
    2324using namespace flair::meta;
     25using namespace flair::sensor;
    2426
    2527string uav_type;
    … …  
    4547
    4648    Uav* drone=CreateUav(manager,name,uav_type);
    47     SimpleFleet* demo=new SimpleFleet(drone,broadcast,ds3port);
     49    TargetEthController *controller=new TargetEthController(manager,"Dualshock3",ds3port);
     50    SimpleFleet* demo=new SimpleFleet(drone,broadcast,controller);
    4851
    4952    demo->Start();

  • trunk/lib/FlairCore/src/IODevice.cpp

    r15 r38  
    2727namespace core {
    2828
    29 IODevice::IODevice(const Object *parent, string name)
    30     : Object(parent, name, "IODevice") {
    31   pimpl_ = new IODevice_impl(this);
     29IODevice::IODevice(const Object* parent,string name): Object(parent,name,"IODevice") {
     30    pimpl_=new IODevice_impl(this);
     31    lastUpdate=GetTime();
    3232}
    3333
    … …  
    4848  }
    4949
    50   if (data != NULL) {
    51     if (getFrameworkManager()->IsLogging() == true)
    52       pimpl_->WriteLog(data->DataTime());
     50    if(data!=NULL) {
     51        lastUpdate=data->DataTime();
     52        if(getFrameworkManager()->IsLogging()==true) pimpl_->WriteLog(data->DataTime());
    5353
    5454    data->pimpl_->Circle();

  • trunk/lib/FlairCore/src/IODevice.h

    r15 r38  
    114114  virtual DataType const &GetOutputDataType() const;
    115115
    116 protected:
    117   /*!
    118   * \brief Process the childs of type IODevice, and log if needed
    119   *
    120   * This method must be called after computing datas;
    121   * generally at the end of the reimplemented UpdateFrom or after acquiring
    122   *datas in case of a sensor. \n
    123   * It will call UpdateFrom methods of each child of type IODevice,
    124   * and log all datas (this IODevice and its childs)
    125   * if logging is enabled (see SetDataToLog(), AddDeviceToLog(),
    126   * FrameworkManager::StartLog and FrameworkManager::AddDeviceToLog). \n
    127   * If a thread is waiting on this IODevice (see Thread::WaitUpdate), it will be
    128   *resumed.
    129   *
    130   * \param data data to process
    131   */
    132   void ProcessUpdate(io_data *data);
     116            Time lastUpdate;
     117        protected:
     118            /*!
     119            * \brief Process the childs of type IODevice, and log if needed
     120            *
     121            * This method must be called after computing datas;
     122            * generally at the end of the reimplemented UpdateFrom or after acquiring datas in case of a sensor. \n
     123            * It will call UpdateFrom methods of each child of type IODevice,
     124            * and log all datas (this IODevice and its childs)
     125            * if logging is enabled (see SetDataToLog(), AddDeviceToLog(),
     126            * FrameworkManager::StartLog and FrameworkManager::AddDeviceToLog). \n
     127            * If a thread is waiting on this IODevice (see Thread::WaitUpdate), it will be resumed.
     128            *
     129            * \param data data to process
     130            */
     131            void ProcessUpdate(io_data* data);
    133132
    134133private:

  • trunk/lib/FlairCore/src/Thread_impl.cpp

    r20 r38  
    2828#else
    2929#include <sys/resource.h>
     30#include <unistd.h>
     31#include <sys/syscall.h>
    3032#endif
    3133
    … …  
    153155}
    154156#else
    155 void *Thread_impl::main_nrt(void *arg) {
    156   Thread_impl *caller = (Thread_impl *)arg;
     157void* Thread_impl::main_nrt(void * arg)
     158{
     159    Thread_impl *caller = (Thread_impl*)arg;
     160/*string th_name=getFrameworkManager()->ObjectName()+ "-" + caller->self->ObjectName();
     161caller->self->Info("pthread '%s' created with TID %x\n",th_name.c_str(),(pid_t)syscall(SYS_gettid));*/
    157162
    158163  caller->self->Run();

  • 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;

  • trunk/lib/FlairSensorActuator/src/TargetController.cpp

    r15 r38  
    3434namespace sensor {
    3535
    36 TargetController::TargetController(const FrameworkManager *parent, string name,
    37                                    uint8_t priority)
    38     : Thread(parent, name, priority), IODevice(parent, name) {
    39   main_tab = new Tab(getFrameworkManager()->GetTabWidget(), name);
    40   TabWidget *tab = new TabWidget(main_tab->NewRow(), name);
    41   setup_tab = new Tab(tab, "Reglages");
     36TargetController::TargetController(const FrameworkManager* parent,string name,uint8_t priority) :
     37    Thread(parent,name,priority),IODevice(parent,name) {
     38    main_tab=new Tab(getFrameworkManager()->GetTabWidget(),name);
     39    TabWidget* tab=new TabWidget(main_tab->NewRow(),name);
     40    setup_tab=new Tab(tab,"Reglages");
     41    SetPeriodMS(20);; //50Hz
    4242}
    4343
    … …  
    150150          }
    151151        }
    152       } else {
    153         // Thread::Info("Debug: relax...\n");
    154         usleep(20000); // 20ms
    155152      }
     153      WaitPeriod();
    156154    }
    157155  }

  • trunk/lib/FlairSensorActuator/src/TargetEthController.cpp

    r15 r38  
    8080  bool fullDatagramReceived = false;
    8181
    82   do {
    83     received =
    84         dataSocket->RecvMessage(receiveFrameBuffer + receiveCurrentPosition,
    85                                 bytesToReceive, TIME_NONBLOCK);
    86     if (received > 0) {
    87       bytesToReceive -= received;
    88       if (bytesToReceive == 0) {
    89         // a full datagram has been read in receiveFrameBuffer
    90         fullDatagramReceived = true;
    91         receiveCurrentPosition = 0;
    92         // we swap the data and reception buffers to avoid copy
    93         char *swapFrameBuffer = dataFrameBuffer;
    94         dataFrameBuffer = receiveFrameBuffer;
    95         receiveFrameBuffer = swapFrameBuffer;
    96       }
    97     }
    98   } while (!received < 0);
     82    do {
     83        received=dataSocket->RecvMessage(receiveFrameBuffer+receiveCurrentPosition,bytesToReceive,TIME_NONBLOCK);
     84        if (received>0) {
     85            receiveCurrentPosition+=received;
     86            bytesToReceive-=received;
     87            if (bytesToReceive==0) {
     88                //a full datagram has been read in receiveFrameBuffer
     89                fullDatagramReceived=true;
     90                //we swap the data and reception buffers to avoid copy
     91                char *swapFrameBuffer=dataFrameBuffer;
     92                dataFrameBuffer=receiveFrameBuffer;
     93                receiveFrameBuffer=swapFrameBuffer;
     94                receiveCurrentPosition=0;
     95                bytesToReceive=dataFrameSize;
     96            }
     97        }
     98    } while (!(received<0));
    9999
    100100  return fullDatagramReceived;
Note: See TracChangeset for help on using the changeset viewer.