Changeset 38 in flair-src for trunk/lib/FlairMeta/src/UavStateMachine.cpp
- Timestamp:
- Jun 23, 2016, 10:15:30 AM (9 years ago)
- File:
-
- 1 edited
Legend:
- Unmodified
- Added
- Removed
-
trunk/lib/FlairMeta/src/UavStateMachine.cpp
r15 r38 51 51 using namespace flair::meta; 52 52 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(); 53 UavStateMachine::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(); 59 58 60 59 Tab *uavTab = new Tab(getFrameworkManager()->GetTabWidget(), "uav", 0); … … 90 89 uZ->AddDeviceToLog(uYaw); 91 90 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); 94 93 95 94 altitudeMode = AltitudeMode_t::Manual; … … 124 123 } 125 124 126 const TargetController *UavStateMachine::GetJoystick(void) const { return joy; } 125 const TargetController *UavStateMachine::GetJoystick(void) const { 126 return controller; 127 } 127 128 128 129 const Quaternion &UavStateMachine::GetCurrentQuaternion(void) const { … … 253 254 // The Uav target altitude has reached its landing value (typically 0) 254 255 // 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 258 258 // deep and narrow hole right in the sensor line of sight). That's why we 259 259 // have a 2 phases landing strategy. … … 424 424 flagZTrajectoryFinished = false; 425 425 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); 434 433 435 434 uZ->SetOffset(); // positionne l'offset de compensation de la gravité à sa … … 458 457 joy->Rumble(0x70); 459 458 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 471 void 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; 476 Printf("Emergency landing!\n"); 470 477 } 471 478 … … 494 501 495 502 void 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; 502 510 } 503 511 … … 534 542 } 535 543 } 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 } 544 557 } 545 558 … … 554 567 static bool isSafeModeButtonPressed = false; 555 568 556 if ( joy->IsButtonPressed(1)) { // select569 if (controller->IsButtonPressed(1)) { // select 557 570 if (!isEmergencyStopButtonPressed) { 558 571 isEmergencyStopButtonPressed = true; … … 563 576 isEmergencyStopButtonPressed = false; 564 577 565 if ( joy->IsButtonPressed(0)) { // start578 if (controller->IsButtonPressed(0)) { // start 566 579 if (!isTakeOffButtonPressed) { 567 580 isTakeOffButtonPressed = true; … … 585 598 // check if l1,l2,r1 and r2 are not pressed 586 599 // 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)) { 590 603 if (!isSafeModeButtonPressed) { 591 604 isSafeModeButtonPressed = true;
Note:
See TracChangeset
for help on using the changeset viewer.