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

Timestamp:

Jul 12, 2021, 11:19:21 AM (3 years ago)

Author:

Sanahuja Guillaume

Message:

add AltitudeSensor class
failsafe altitude sensor in changeable

File:

• trunk/lib/FlairMeta/src/UavStateMachine.cpp (modified) (5 diffs)

trunk/lib/FlairMeta/src/UavStateMachine.cpp

@@ -53 +53 @@
 
 UavStateMachine::UavStateMachine(TargetController *controller):
-        Thread(getFrameworkManager(),"UavStateMachine",50),
-        uav(GetUav()),controller(controller),failSafeMode(true),flagConnectionLost(false),flagBatteryLow(false),flagCriticalSensorLost(false),flagZTrajectoryFinished(false),safeToFly(true){
-    altitudeState=AltitudeState_t::Stopped;
-    uav->UseDefaultPlot();
+  Thread(getFrameworkManager(),"UavStateMachine",50),
+  uav(GetUav()),controller(controller),failSafeMode(true),flagConnectionLost(false),flagBatteryLow(false),flagCriticalSensorLost(false),flagZTrajectoryFinished(false),safeToFly(true){
+  altitudeState=AltitudeState_t::Stopped;
+  uav->UseDefaultPlot();
 
   Tab *uavTab = new Tab(getFrameworkManager()->GetTabWidget(), "uav", 0);
@@ -112 +112 @@
   uav->GetMetaUsRangeFinder()->GetVzPlot()->AddCurve(
       altitudeTrajectory->GetMatrix()->Element(1), DataPlot::Green);
+      
+  failSafeAltitudeSensor=uav->GetMetaUsRangeFinder();
 }
 
@@ -125 +127 @@
 
 const TargetController *UavStateMachine::GetTargetController(void) const {
-    return controller;
+  return controller;
 }
 
 MetaDualShock3 *UavStateMachine::GetJoystick(void) const {
-    return joy;
+  return joy;
 }
 
@@ -138 +140 @@
 const Vector3Df &UavStateMachine::GetCurrentAngularSpeed(void) const {
   return currentAngularSpeed;
+}
+
+void UavStateMachine::SetFailSafeAltitudeSensor(AltitudeSensor *altitudeSensor) {
+  Warn("Replacing the FailSafeAltitudeSensor with %s\n",altitudeSensor->ObjectName().c_str());
+  failSafeAltitudeSensor=altitudeSensor;
 }
 
@@ -147 +154 @@
 void UavStateMachine::FailSafeAltitudeValues(float &altitude,
                                              float &verticalSpeed) const {
-  altitude = uav->GetMetaUsRangeFinder()->z();
-  verticalSpeed = uav->GetMetaUsRangeFinder()->Vz();
+  altitude = failSafeAltitudeSensor->z();
+  verticalSpeed = failSafeAltitudeSensor->Vz();
   
+  //a mettre dans le metausrangefinder?
+  //on ne sait pas si failSafeAltitudeSensor est vrpn (dans ce cas pas besoin de rotation)
+  //ou un us range finder
   //+0.04 decalage en z de l'us en simulation
   //faire un reglage