Changeset 38 in flair-src for trunk/lib/FlairMeta/src/UavStateMachine.cpp


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

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

File:
1 edited

Legend:

Unmodified
Added
Removed
  • 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;
Note: See TracChangeset for help on using the changeset viewer.