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

Passage du "framework" à flair

File:
1 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() {}
Note: See TracChangeset for help on using the changeset viewer.