Changeset 159 in flair-src for trunk/demos/Gps/uav/src/DemoGps.cpp


Ignore:
Timestamp:
03/05/17 10:52:49 (7 years ago)
Author:
Sanahuja Guillaume
Message:

cosmetic changes

File:
1 edited

Legend:

Unmodified
Added
Removed

  • trunk/demos/Gps/uav/src/DemoGps.cpp

    r137 r159  
    3939using namespace flair::meta;
    4040
    41 DemoGps::DemoGps(TargetController *controller): UavStateMachine(controller), behaviourMode(BehaviourMode_t::Default) {
    42                 Uav* uav=GetUav();
    43     startCircle=new PushButton(GetButtonsLayout()->NewRow(),"start_circle");
    44     stopCircle=new PushButton(GetButtonsLayout()->LastRowLastCol(),"stop_circle");
    45 
    46     //RTDM_SerialPort *serialport=new RTDM_SerialPort(getFrameworkManager(),"gps_serial","rtser2");
    47     //gps=new Mb800("gps",serialport,(NmeaGps::NMEAFlags_t)(NmeaGps::GGA|NmeaGps::VTG),40);
    48     gps=new SimuGps("gps",(NmeaGps::NMEAFlags_t)(NmeaGps::GGA|NmeaGps::VTG),0,40);
    49     gps->UseDefaultPlot();
    50     getFrameworkManager()->AddDeviceToLog(gps);
    51     //gps->Start();
    52 
    53     circle=new TrajectoryGenerator2DCircle(gps->GetLayout()->NewRow(),"circle");
    54     //todo: add graphs in gps plot
    55     /*
    56     uav->GetVrpnObject()->xPlot()->AddCurve(circle->Matrix()->Element(0,0),DataPlot::Blue);
    57     uav->GetVrpnObject()->yPlot()->AddCurve(circle->Matrix()->Element(0,1),DataPlot::Blue);
    58     uav->GetVrpnObject()->VxPlot()->AddCurve(circle->Matrix()->Element(1,0),DataPlot::Blue);
    59     uav->GetVrpnObject()->VyPlot()->AddCurve(circle->Matrix()->Element(1,1),DataPlot::Blue);
    60     uav->GetVrpnObject()->XyPlot()->AddCurve(circle->Matrix()->Element(0,1),circle->Matrix()->Element(0,0),DataPlot::Blue,"circle");*/
    61 
    62     uX=new Pid(setupLawTab->At(1,0),"u_x");
    63     uX->UseDefaultPlot(graphLawTab->NewRow());
    64     uY=new Pid(setupLawTab->At(1,1),"u_y");
    65     uY->UseDefaultPlot(graphLawTab->LastRowLastCol());
    66 
    67     customReferenceOrientation= new AhrsData(this,"reference");
    68     uav->GetAhrs()->AddPlot(customReferenceOrientation,DataPlot::Yellow);
    69     AddDataToControlLawLog(customReferenceOrientation);
    70 
    71     customOrientation=new AhrsData(this,"orientation");
     41DemoGps::DemoGps(TargetController* controller)
     42    : UavStateMachine(controller)
     43    , behaviourMode(BehaviourMode_t::Default) {
     44  Uav* uav = GetUav();
     45  startCircle = new PushButton(GetButtonsLayout()->NewRow(), "start_circle");
     46  stopCircle = new PushButton(GetButtonsLayout()->LastRowLastCol(), "stop_circle");
     47
     48  // RTDM_SerialPort *serialport=new RTDM_SerialPort(getFrameworkManager(),"gps_serial","rtser2");
     49  // gps=new Mb800("gps",serialport,(NmeaGps::NMEAFlags_t)(NmeaGps::GGA|NmeaGps::VTG),40);
     50  gps = new SimuGps("gps", (NmeaGps::NMEAFlags_t)(NmeaGps::GGA | NmeaGps::VTG), 0,0, 40);
     51
     52  gps->UseDefaultPlot();
     53  getFrameworkManager()->AddDeviceToLog(gps);
     54  gps->Start();
     55
     56  circle = new TrajectoryGenerator2DCircle(gps->GetLayout()->NewRow(), "circle");
     57  // todo: add graphs in gps plot
     58  /*
     59  uav->GetVrpnObject()->xPlot()->AddCurve(circle->Matrix()->Element(0,0),DataPlot::Blue);
     60  uav->GetVrpnObject()->yPlot()->AddCurve(circle->Matrix()->Element(0,1),DataPlot::Blue);
     61  uav->GetVrpnObject()->VxPlot()->AddCurve(circle->Matrix()->Element(1,0),DataPlot::Blue);
     62  uav->GetVrpnObject()->VyPlot()->AddCurve(circle->Matrix()->Element(1,1),DataPlot::Blue);
     63  uav->GetVrpnObject()->XyPlot()->AddCurve(circle->Matrix()->Element(0,1),circle->Matrix()->Element(0,0),DataPlot::Blue,"circle");*/
     64
     65  uX = new Pid(setupLawTab->At(1, 0), "u_x");
     66  uX->UseDefaultPlot(graphLawTab->NewRow());
     67  uY = new Pid(setupLawTab->At(1, 1), "u_y");
     68  uY->UseDefaultPlot(graphLawTab->LastRowLastCol());
     69
     70  customReferenceOrientation = new AhrsData(this, "reference");
     71  uav->GetAhrs()->AddPlot(customReferenceOrientation, DataPlot::Yellow);
     72  AddDataToControlLawLog(customReferenceOrientation);
     73
     74  customOrientation = new AhrsData(this, "orientation");
    7275}
    7376
    … …  
    7578}
    7679
    77 AhrsData *DemoGps::GetReferenceOrientation(void) {
    78     Vector2D pos_err, vel_err; // in Uav coordinate system
    79     float yaw_ref;
    80     Euler refAngles;
    81 
    82     PositionValues(pos_err, vel_err, yaw_ref);
    83 
    84     refAngles.yaw=yaw_ref;
    85 
    86     uX->SetValues(pos_err.x, vel_err.x);
    87     uX->Update(GetTime());
    88     refAngles.pitch=uX->Output();
    89 
    90     uY->SetValues(pos_err.y, vel_err.y);
    91     uY->Update(GetTime());
    92     refAngles.roll=-uY->Output();
    93 
    94     customReferenceOrientation->SetQuaternionAndAngularRates(refAngles.ToQuaternion(),Vector3D(0,0,0));
    95 
    96     return customReferenceOrientation;
    97 }
    98 
    99 void DemoGps::PositionValues(Vector2D &pos_error,Vector2D &vel_error,float &yaw_ref) {
    100     Vector3D uav_pos,uav_vel;
    101     Vector2D uav_2Dpos,uav_2Dvel;
    102 
    103 //TODO GPS position and circle center
    104    // GetUav()->GetVrpnObject()->GetPosition(uav_pos);
    105     //GetUav()->GetVrpnObject()->GetSpeed(uav_vel);
    106 
    107     uav_pos.To2Dxy(uav_2Dpos);
    108     uav_vel.To2Dxy(uav_2Dvel);
    109 
    110     if (behaviourMode==BehaviourMode_t::PositionHold) {
    111         pos_error=uav_2Dpos-posHold;
    112         vel_error=uav_2Dvel;
    113         yaw_ref=yawHold;
    114     } else { //Circle
    115         Vector2D circle_pos,circle_vel;
    116         Vector2D target_2Dpos;
    117 
    118         circle->SetCenter(target_2Dpos);
    119 
    120         //circle reference
    121         circle->Update(GetTime());
    122         circle->GetPosition(circle_pos);
    123         circle->GetSpeed(circle_vel);
    124 
    125         //error in optitrack frame
    126         pos_error=uav_2Dpos-circle_pos;
    127         vel_error=uav_2Dvel-circle_vel;
    128         yaw_ref=atan2(target_2Dpos.y-uav_pos.y,target_2Dpos.x-uav_pos.x);
     80AhrsData* DemoGps::GetReferenceOrientation(void) {
     81  Vector2D pos_err, vel_err; // in Uav coordinate system
     82  float yaw_ref;
     83  Euler refAngles;
     84
     85  PositionValues(pos_err, vel_err, yaw_ref);
     86
     87  refAngles.yaw = yaw_ref;
     88
     89  uX->SetValues(pos_err.x, vel_err.x);
     90  uX->Update(GetTime());
     91  refAngles.pitch = uX->Output();
     92
     93  uY->SetValues(pos_err.y, vel_err.y);
     94  uY->Update(GetTime());
     95  refAngles.roll = -uY->Output();
     96
     97  customReferenceOrientation->SetQuaternionAndAngularRates(refAngles.ToQuaternion(), Vector3D(0, 0, 0));
     98
     99  return customReferenceOrientation;
     100}
     101
     102void DemoGps::PositionValues(Vector2D& pos_error, Vector2D& vel_error, float& yaw_ref) {
     103  Vector3D uav_pos, uav_vel;
     104  Vector2D uav_2Dpos, uav_2Dvel;
     105
     106  // TODO GPS position and circle center
     107  // GetUav()->GetVrpnObject()->GetPosition(uav_pos);
     108  // GetUav()->GetVrpnObject()->GetSpeed(uav_vel);
     109
     110  uav_pos.To2Dxy(uav_2Dpos);
     111  uav_vel.To2Dxy(uav_2Dvel);
     112
     113  if(behaviourMode == BehaviourMode_t::PositionHold) {
     114    pos_error = uav_2Dpos - posHold;
     115    vel_error = uav_2Dvel;
     116    yaw_ref = yawHold;
     117  } else { // Circle
     118    Vector2D circle_pos, circle_vel;
     119    Vector2D target_2Dpos;
     120
     121    circle->SetCenter(target_2Dpos);
     122
     123    // circle reference
     124    circle->Update(GetTime());
     125    circle->GetPosition(circle_pos);
     126    circle->GetSpeed(circle_vel);
     127
     128    // error in optitrack frame
     129    pos_error = uav_2Dpos - circle_pos;
     130    vel_error = uav_2Dvel - circle_vel;
     131    yaw_ref = atan2(target_2Dpos.y - uav_pos.y, target_2Dpos.x - uav_pos.x);
     132  }
     133
     134  // error in uav frame
     135  Quaternion currentQuaternion = GetCurrentQuaternion();
     136  Euler currentAngles; // in vrpn frame
     137  currentQuaternion.ToEuler(currentAngles);
     138  pos_error.Rotate(-currentAngles.yaw);
     139  vel_error.Rotate(-currentAngles.yaw);
     140}
     141
     142void DemoGps::SignalEvent(Event_t event) {
     143  UavStateMachine::SignalEvent(event);
     144  switch(event) {
     145  case Event_t::TakingOff:
     146    behaviourMode = BehaviourMode_t::Default;
     147    break;
     148  case Event_t::EnteringControlLoop:
     149    if((behaviourMode == BehaviourMode_t::Circle) && (!circle->IsRunning())) {
     150      GpsPositionHold();
    129151    }
    130 
    131     //error in uav frame
    132     Quaternion currentQuaternion=GetCurrentQuaternion();
    133     Euler currentAngles;//in vrpn frame
    134     currentQuaternion.ToEuler(currentAngles);
    135     pos_error.Rotate(-currentAngles.yaw);
    136     vel_error.Rotate(-currentAngles.yaw);
    137 }
    138 
    139 void DemoGps::SignalEvent(Event_t event) {
    140     UavStateMachine::SignalEvent(event);
    141     switch(event) {
    142     case Event_t::TakingOff:
    143         behaviourMode=BehaviourMode_t::Default;
    144         break;
    145     case Event_t::EnteringControlLoop:
    146         if ((behaviourMode==BehaviourMode_t::Circle) && (!circle->IsRunning())) {
    147             GpsPositionHold();
    148         }
    149         break;
    150     case Event_t::EnteringFailSafeMode:
    151         behaviourMode=BehaviourMode_t::Default;
    152         break;
    153     }
     152    break;
     153  case Event_t::EnteringFailSafeMode:
     154    behaviourMode = BehaviourMode_t::Default;
     155    break;
     156  }
    154157}
    155158
    156159void DemoGps::ExtraSecurityCheck(void) {
    157 
    158160}
    159161
    160162void DemoGps::ExtraCheckPushButton(void) {
    161     if(startCircle->Clicked() && (behaviourMode!=BehaviourMode_t::Circle)) {
    162         StartCircle();
    163     }
    164     if(stopCircle->Clicked() && (behaviourMode==BehaviourMode_t::Circle)) {
    165         StopCircle();
    166     }
     163  if(startCircle->Clicked() && (behaviourMode != BehaviourMode_t::Circle)) {
     164    StartCircle();
     165  }
     166  if(stopCircle->Clicked() && (behaviourMode == BehaviourMode_t::Circle)) {
     167    StopCircle();
     168  }
    167169}
    168170
    169171void DemoGps::ExtraCheckJoystick(void) {
    170     //R1 and Circle
    171     if(GetJoystick()->IsButtonPressed(9) && GetJoystick()->IsButtonPressed(4) && (behaviourMode!=BehaviourMode_t::Circle)) {
    172         StartCircle();
    173     }
    174 
    175     //R1 and Cross
    176     if(GetJoystick()->IsButtonPressed(9) && GetJoystick()->IsButtonPressed(5) && (behaviourMode==BehaviourMode_t::Circle)) {
    177         StopCircle();
    178     }
     172  // R1 and Circle
     173  if(GetJoystick()->IsButtonPressed(9) && GetJoystick()->IsButtonPressed(4) &&
     174     (behaviourMode != BehaviourMode_t::Circle)) {
     175    StartCircle();
     176  }
     177
     178  // R1 and Cross
     179  if(GetJoystick()->IsButtonPressed(9) && GetJoystick()->IsButtonPressed(5) &&
     180     (behaviourMode == BehaviourMode_t::Circle)) {
     181    StopCircle();
     182  }
    179183}
    180184
    181185void DemoGps::StartCircle(void) {
    182     if (SetOrientationMode(OrientationMode_t::Custom)) {
    183         Thread::Info("DemoGps: start circle\n");
    184     } else {
    185         Thread::Warn("DemoGps: could not start circle\n");
    186         return;
    187     }
    188     Vector3D uav_pos;
    189     Vector2D uav_2Dpos,target_2Dpos;
    190 
    191     circle->SetCenter(target_2Dpos);
    192 
    193     //todo get uav and circle pos by gps
    194     uav_pos.To2Dxy(uav_2Dpos);
    195     circle->StartTraj(uav_2Dpos);
    196 
    197     uX->Reset();
    198     uY->Reset();
    199     behaviourMode=BehaviourMode_t::Circle;
     186  if(SetOrientationMode(OrientationMode_t::Custom)) {
     187    Thread::Info("DemoGps: start circle\n");
     188  } else {
     189    Thread::Warn("DemoGps: could not start circle\n");
     190    return;
     191  }
     192  Vector3D uav_pos;
     193  Vector2D uav_2Dpos, target_2Dpos;
     194
     195  circle->SetCenter(target_2Dpos);
     196
     197  // todo get uav and circle pos by gps
     198  uav_pos.To2Dxy(uav_2Dpos);
     199  circle->StartTraj(uav_2Dpos);
     200
     201  uX->Reset();
     202  uY->Reset();
     203  behaviourMode = BehaviourMode_t::Circle;
    200204}
    201205
    202206void DemoGps::StopCircle(void) {
    203     circle->FinishTraj();
    204     //GetJoystick()->Rumble(0x70);
    205     Thread::Info("DemoGps: finishing circle\n");
     207  circle->FinishTraj();
     208  // GetJoystick()->Rumble(0x70);
     209  Thread::Info("DemoGps: finishing circle\n");
    206210}
    207211
    208212void DemoGps::GpsPositionHold(void) {
    209213
    210 
    211 //tood set yawHold and posHold
    212 
    213 
    214     uX->Reset();
    215     uY->Reset();
    216     behaviourMode=BehaviourMode_t::PositionHold;
    217     SetOrientationMode(OrientationMode_t::Custom);
    218     Thread::Info("DemoGps: holding position\n");
    219 }
     214  // tood set yawHold and posHold
     215
     216  uX->Reset();
     217  uY->Reset();
     218  behaviourMode = BehaviourMode_t::PositionHold;
     219  SetOrientationMode(OrientationMode_t::Custom);
     220  Thread::Info("DemoGps: holding position\n");
     221}
Note: See TracChangeset for help on using the changeset viewer.