Changeset 105 in flair-src for trunk/demos/PidStandalone/uav/src


Ignore:
Timestamp:
Oct 18, 2016, 9:13:08 AM (8 years ago)
Author:
Bayard Gildas
Message:

Passage du "framework" à flair

Location:
trunk/demos/PidStandalone/uav/src
Files:
3 edited

Legend:

Unmodified
Added
Removed

  • trunk/demos/PidStandalone/uav/src/PidUav.cpp

    r44 r105  
    2929
    3030using namespace std;
    31 using namespace framework::core;
    32 using namespace framework::sensor;
    33 using namespace framework::gui;
    34 using namespace framework::filter;
    35 using namespace framework::meta;
     31using namespace flair::core;
     32using namespace flair::sensor;
     33using namespace flair::gui;
     34using namespace flair::filter;
     35using namespace flair::meta;
    3636
    3737/*
    … …  
    4141** ===================================================================
    4242*/
    43 PidUav::PidUav(Uav* uav,TargetController *controller): behaviourMode(BehaviourMode_t::Default),UavStateMachine(uav,controller) {
    44     my_uRoll=new Pid(setupLawTab->At(1,1),"custom uRoll");
    45     my_uRoll->UseDefaultPlot(graphLawTab->LastRowLastCol());
    46     my_uPitch=new Pid(setupLawTab->At(1,0),"custom uPitch");
    47     my_uPitch->UseDefaultPlot(graphLawTab->NewRow());
     43PidUav::PidUav(Uav *uav, TargetController *controller)
     44    : behaviourMode(BehaviourMode_t::Default),
     45      UavStateMachine(uav, controller) {
     46  my_uRoll = new Pid(setupLawTab->At(1, 1), "custom uRoll");
     47  my_uRoll->UseDefaultPlot(graphLawTab->LastRowLastCol());
     48  my_uPitch = new Pid(setupLawTab->At(1, 0), "custom uPitch");
     49  my_uPitch->UseDefaultPlot(graphLawTab->NewRow());
    4850}
    4951
    5052void PidUav::SignalEvent(Event_t event) {
    51     UavStateMachine::SignalEvent(event);
    52     switch(event) {
    53     case Event_t::TakingOff:
    54         //always take off in default mode
    55         behaviourMode=BehaviourMode_t::Default;
    56         break;
    57     case Event_t::EnteringFailSafeMode:
    58         //return to default mode
    59         Thread::Info("Entering failSafe mode\n");
    60         behaviourMode=BehaviourMode_t::Default;
    61         break;
    62     }
     53  UavStateMachine::SignalEvent(event);
     54  switch (event) {
     55  case Event_t::TakingOff:
     56    // always take off in default mode
     57    behaviourMode = BehaviourMode_t::Default;
     58    break;
     59  case Event_t::EnteringFailSafeMode:
     60    // return to default mode
     61    Thread::Info("Entering failSafe mode\n");
     62    behaviourMode = BehaviourMode_t::Default;
     63    break;
     64  }
    6365}
    6466
    6567bool PidUav::StartCustomMode() {
    66     //ask UavStateMachine to enter in custom torques
    67     if (SetTorqueMode(TorqueMode_t::Custom)) {
    68         Thread::Info("CustomTorques: start\n");
    69         my_uPitch->Reset();
    70         my_uRoll->Reset();
    71         return true;
    72     } else {
    73         Thread::Warn("CustomTorques: could not start\n");
    74         return false;
    75     }
     68  // ask UavStateMachine to enter in custom torques
     69  if (SetTorqueMode(TorqueMode_t::Custom)) {
     70    Thread::Info("CustomTorques: start\n");
     71    my_uPitch->Reset();
     72    my_uRoll->Reset();
     73    return true;
     74  } else {
     75    Thread::Warn("CustomTorques: could not start\n");
     76    return false;
     77  }
    7678}
    7779
    7880void PidUav::StartOscillatingMode() {
    79     if (behaviourMode==BehaviourMode_t::Default) {
    80         if (!StartCustomMode()) return;
    81     }
    82     behaviourMode=BehaviourMode_t::Oscillating;
    83     Thread::Info("Entering oscillating mode\n");
    84     //remove the 'D' effect with a strong 'P'
    85 
     81  if (behaviourMode == BehaviourMode_t::Default) {
     82    if (!StartCustomMode())
     83      return;
     84  }
     85  behaviourMode = BehaviourMode_t::Oscillating;
     86  Thread::Info("Entering oscillating mode\n");
     87  // remove the 'D' effect with a strong 'P'
    8688}
    8789
    8890void PidUav::ExtraCheckJoystick(void) {
    89     static bool wasOscillatingModeButtonPressed=false;
    90     // controller button R1 enters optical flow mode
    91     if(GetJoystick()->IsButtonPressed(9)) { // R1
    92         if (!wasOscillatingModeButtonPressed) {
    93             wasOscillatingModeButtonPressed=true;
    94             if (behaviourMode!=BehaviourMode_t::Oscillating) StartOscillatingMode();
    95         }
    96     } else {
    97         wasOscillatingModeButtonPressed=false;
     91  static bool wasOscillatingModeButtonPressed = false;
     92  // controller button R1 enters optical flow mode
     93  if (GetJoystick()->IsButtonPressed(9)) { // R1
     94    if (!wasOscillatingModeButtonPressed) {
     95      wasOscillatingModeButtonPressed = true;
     96      if (behaviourMode != BehaviourMode_t::Oscillating)
     97        StartOscillatingMode();
    9898    }
     99  } else {
     100    wasOscillatingModeButtonPressed = false;
     101  }
    99102}
    100103
    101104void PidUav::ComputeCustomTorques(Euler &torques) {
    102     ComputeDefaultTorques(torques);
    103     //overload default torques calculation for pitch and roll
    104     const AhrsData *refOrientation=GetDefaultReferenceOrientation();
    105     Quaternion refQuaternion;
    106     Vector3D refAngularRates;
    107     refOrientation->GetQuaternionAndAngularRates(refQuaternion,refAngularRates);
    108     Euler refAngles=refQuaternion.ToEuler();
     105  ComputeDefaultTorques(torques);
     106  // overload default torques calculation for pitch and roll
     107  const AhrsData *refOrientation = GetDefaultReferenceOrientation();
     108  Quaternion refQuaternion;
     109  Vector3D refAngularRates;
     110  refOrientation->GetQuaternionAndAngularRates(refQuaternion, refAngularRates);
     111  Euler refAngles = refQuaternion.ToEuler();
    109112
    110     const AhrsData *currentOrientation=GetDefaultOrientation();
    111     Quaternion currentQuaternion;
    112     Vector3D currentAngularRates;
    113     currentOrientation->GetQuaternionAndAngularRates(currentQuaternion,currentAngularRates);
    114     Euler currentAngles=currentQuaternion.ToEuler();
     113  const AhrsData *currentOrientation = GetDefaultOrientation();
     114  Quaternion currentQuaternion;
     115  Vector3D currentAngularRates;
     116  currentOrientation->GetQuaternionAndAngularRates(currentQuaternion,
     117                                                   currentAngularRates);
     118  Euler currentAngles = currentQuaternion.ToEuler();
    115119
    116     my_uRoll->SetValues(currentAngles.roll-refAngles.roll,currentAngularRates.x);
    117     my_uRoll->Update(GetTime());
    118     torques.roll=my_uRoll->Output();
     120  my_uRoll->SetValues(currentAngles.roll - refAngles.roll,
     121                      currentAngularRates.x);
     122  my_uRoll->Update(GetTime());
     123  torques.roll = my_uRoll->Output();
    119124
    120     my_uPitch->SetValues(currentAngles.pitch-refAngles.pitch,currentAngularRates.y);
    121     my_uPitch->Update(GetTime());
    122     torques.pitch=my_uPitch->Output();
     125  my_uPitch->SetValues(currentAngles.pitch - refAngles.pitch,
     126                       currentAngularRates.y);
     127  my_uPitch->Update(GetTime());
     128  torques.pitch = my_uPitch->Output();
    123129}
    124130
    125 PidUav::~PidUav() {
    126 }
     131PidUav::~PidUav() {}

  • trunk/demos/PidStandalone/uav/src/PidUav.h

    r44 r105  
    1717#include <UavStateMachine.h>
    1818
    19 namespace framework {
    20     namespace core {
    21         class FrameworkManager;
    22         class cvmatrix;
    23     }
    24     namespace gui {
    25         class GroupBox;
    26         class DoubleSpinBox;
    27     }
    28     namespace filter {
    29         class Pid;
    30     }
    31     namespace sensor {
    32         class TargetController;
    33     }
     19namespace flair {
     20  namespace core {
     21    class cvmatrix;
     22  }
     23  namespace gui {
     24    class GroupBox;
     25    class DoubleSpinBox;
     26  }
     27  namespace filter {
     28    class Pid;
     29  }
     30  namespace sensor {
     31    class TargetController;
     32  }
    3433}
    3534
    36 class PidUav : public framework::meta::UavStateMachine {
    37     public:
    38         PidUav(framework::meta::Uav* uav, framework::sensor::TargetController *controller);
    39         ~PidUav();
     35class PidUav : public flair::meta::UavStateMachine {
     36public:
     37  PidUav(flair::meta::Uav *uav,
     38         flair::sensor::TargetController *controller);
     39  ~PidUav();
    4040
    41     protected:
    42         void SignalEvent(Event_t event);
    43         void ExtraCheckJoystick(void);
    44     private:
    45         void ComputeCustomTorques(framework::core::Euler &torques);
    46         bool StartCustomMode();
    47         void StartOscillatingMode();
    48         framework::filter::Pid *my_uPitch,*my_uRoll;
    49         enum class BehaviourMode_t {
    50             Default,
    51             Oscillating
    52         };
    53         BehaviourMode_t behaviourMode;
     41protected:
     42  void SignalEvent(Event_t event);
     43  void ExtraCheckJoystick(void);
     44
     45private:
     46  void ComputeCustomTorques(flair::core::Euler &torques);
     47  bool StartCustomMode();
     48  void StartOscillatingMode();
     49  flair::filter::Pid *my_uPitch, *my_uRoll;
     50  enum class BehaviourMode_t { Default, Oscillating };
     51  BehaviourMode_t behaviourMode;
    5452};
    5553

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

    r44 r105  
    2222using namespace TCLAP;
    2323using namespace std;
    24 using namespace framework::core;
    25 using namespace framework::meta;
    26 using namespace framework::sensor;
     24using namespace flair::core;
     25using namespace flair::meta;
     26using namespace flair::sensor;
    2727
    2828string uav_type;
    … …  
    3535string address;
    3636
    37 void parseOptions(int argc, char** argv);
     37void parseOptions(int argc, char **argv);
    3838
     39int main(int argc, char *argv[]) {
     40  parseOptions(argc, argv);
    3941
    40 int main(int argc, char* argv[]) {
    41     parseOptions(argc,argv);
     42  FrameworkManager *manager;
     43  manager = new FrameworkManager(name);
     44  if (!headless) {
     45    manager->SetupConnection(address, port);
     46  }
     47  manager->SetupUserInterface(xml_file);
     48  manager->SetupLogger(log_path);
    4249
    43     FrameworkManager *manager;
    44     manager= new FrameworkManager(name);
    45     if (!headless) {
    46         manager->SetupConnection(address,port);
    47     }
    48     manager->SetupUserInterface(xml_file);
    49     manager->SetupLogger(log_path);
     50  Uav *drone = CreateUav(manager, name, uav_type);
     51  TargetEthController *controller =
     52      new TargetEthController(manager, "Dualshock3", 20000);
     53  // EmulatedController *controller=new EmulatedController(manager,"Emulated
     54  // Controller");
     55  //    controller->AddStep(10000,"Waiting for AHRS
     56  //    stabilization",0,0.,0.,0.,0.);
     57  //    controller->AddStep(500,"Takeoff",(uint16_t)EmulatedController::ButtonType::start,0.,0.,0.,0.);
     58  //    controller->AddStep(3000,"Waiting for flight
     59  //    stabilization",0,0.,0.,0.,0.);
     60  /*    controller->AddStep(2000,"fly left",0,-0.3,0.,0.,0.);
     61      controller->AddStep(1000,"fly right",0,0.3,0.,0.,0.);*/
     62  //    controller->AddStep(500,"Land",(uint16_t)EmulatedController::ButtonType::start,0.,0.,0.,0.);
     63  //    controller->AddStep(3000,"Waiting for landing",0,0.,0.,0.,0.);
     64  //    controller->AddStep(500,"Stop",(uint16_t)EmulatedController::ButtonType::select,0.,0.,0.,0.);
     65  PidUav *app = new PidUav(drone, controller);
    5066
    51     Uav* drone=CreateUav(manager,name,uav_type);
    52     TargetEthController *controller=new TargetEthController(manager,"Dualshock3",20000);
    53     //EmulatedController *controller=new EmulatedController(manager,"Emulated Controller");
    54 //    controller->AddStep(10000,"Waiting for AHRS stabilization",0,0.,0.,0.,0.);
    55 //    controller->AddStep(500,"Takeoff",(uint16_t)EmulatedController::ButtonType::start,0.,0.,0.,0.);
    56 //    controller->AddStep(3000,"Waiting for flight stabilization",0,0.,0.,0.,0.);
    57 /*    controller->AddStep(2000,"fly left",0,-0.3,0.,0.,0.);
    58     controller->AddStep(1000,"fly right",0,0.3,0.,0.,0.);*/
    59 //    controller->AddStep(500,"Land",(uint16_t)EmulatedController::ButtonType::start,0.,0.,0.,0.);
    60 //    controller->AddStep(3000,"Waiting for landing",0,0.,0.,0.,0.);
    61 //    controller->AddStep(500,"Stop",(uint16_t)EmulatedController::ButtonType::select,0.,0.,0.,0.);
    62     PidUav* app=new PidUav(drone,controller);
     67  app->Start();
     68  app->Join();
    6369
    64     app->Start();
    65     app->Join();
    66 
    67     delete manager;
     70  delete manager;
    6871}
    6972
    70 void parseOptions(int argc, char** argv) {
    71         try {
     73void parseOptions(int argc, char **argv) {
     74  try {
    7275
    73         CmdLine cmd("Command description message", ' ', "0.1");
     76    CmdLine cmd("Command description message", ' ', "0.1");
    7477
    75         ValueArg<string> nameArg("n","name","uav name",true,"x4","string");
    76         cmd.add( nameArg );
     78    ValueArg<string> nameArg("n", "name", "uav name", true, "x4", "string");
     79    cmd.add(nameArg);
    7780
    78         ValueArg<string> typeArg("t","type","uav type: ardrone2, hds_x4, hds_x8, hds_xufo, hds_simu or hds_simux (with x the number of the simulated uav)",true,"hds_x4","string");
    79         cmd.add( typeArg );
     81    ValueArg<string> typeArg(
     82        "t", "type", "uav type: ardrone2, hds_x4, hds_x8, hds_xufo, hds_simu "
     83                     "or hds_simux (with x the number of the simulated uav)",
     84        true, "hds_x4", "string");
     85    cmd.add(typeArg);
    8086
    81         ValueArg<string> xmlArg("x","xml","fichier xml",true,"./reglages.xml","string");
    82         cmd.add( xmlArg );
     87    ValueArg<string> xmlArg("x", "xml", "fichier xml", true, "./reglages.xml",
     88                            "string");
     89    cmd.add(xmlArg);
    8390
    84         ValueArg<string> logsArg("l","logs","repertoire des logs",true,"/media/ram","string");
    85         cmd.add( logsArg );
     91    ValueArg<string> logsArg("l", "logs", "repertoire des logs", true,
     92                             "/media/ram", "string");
     93    cmd.add(logsArg);
    8694
    87         ValueArg<int> portArg("p","port","port pour station sol",true,9000,"int");
    88         cmd.add( portArg );
     95    ValueArg<int> portArg("p", "port", "port pour station sol", true, 9000,
     96                          "int");
     97    cmd.add(portArg);
    8998
    90         ValueArg<string> addressArg("a","address","addresse station sol",true,"127.0.0.1","string");
    91         cmd.add( addressArg );
     99    ValueArg<string> addressArg("a", "address", "addresse station sol", true,
     100                                "127.0.0.1", "string");
     101    cmd.add(addressArg);
    92102
    93         SwitchArg headlessArg("e","headless","headless mode", false);
    94         cmd.add( headlessArg );
     103    SwitchArg headlessArg("e", "headless", "headless mode", false);
     104    cmd.add(headlessArg);
    95105
    96         cmd.parse( argc, argv );
     106    cmd.parse(argc, argv);
    97107
    98         // Get the value parsed by each arg.
    99         uav_type=typeArg.getValue();
    100         log_path = logsArg.getValue();
    101         port=portArg.getValue();
    102         xml_file = xmlArg.getValue();
    103         name=nameArg.getValue();
    104         address=addressArg.getValue();
    105         headless=headlessArg.getValue();
     108    // Get the value parsed by each arg.
     109    uav_type = typeArg.getValue();
     110    log_path = logsArg.getValue();
     111    port = portArg.getValue();
     112    xml_file = xmlArg.getValue();
     113    name = nameArg.getValue();
     114    address = addressArg.getValue();
     115    headless = headlessArg.getValue();
    106116
    107         } catch (ArgException &e)  // catch any exceptions
    108         { cerr << "error: " << e.error() << " for arg " << e.argId() << endl; }
     117  } catch (ArgException &e) // catch any exceptions
     118  {
     119    cerr << "error: " << e.error() << " for arg " << e.argId() << endl;
     120  }
    109121}
Note: See TracChangeset for help on using the changeset viewer.