fly_monitor update: separate manual control script (control with altitude) and...

fly_monitor update: separate manual control script (control with altitude) and manual direct script (control directly the gas)

Com_server/fly_monitor/include/Data_Drone.h

@@ -161,6 +161,7 @@ class Drone
       int remote_z=0;
       int remote_r=0;
       */
+      int enable_altitude_control = 1;// Flag that chooses between altitude control or throttle control in manual mode
       
       // Variables used in offboard mode
       // with message SET_POSITION_TARGET_LOCAL_NED (#84)

Com_server/fly_monitor/src/Com_SerialWritingThread.cpp

@@ -177,18 +177,21 @@ int Serial_Port_WritingThread::main_loop(struct timeval *front_msg, struct timev
       if(drone1.motors == ARM)
       {
          // Control in altitude
-         ///*
-         drone1.command_directControl(xx,yy,zz,rr);
-         //*/
+         if(drone1.enable_altitude_control == 1)
+            drone1.command_directControl(xx,yy,zz,rr);
          
          // Control directly
-         /*
-         drone1.command_directControl(xx,yy,zz_direct,rr);
-         //*/
+         if(drone1.enable_altitude_control == 0)
+            drone1.command_directControl(xx,yy,zz_direct,rr);
       }
       else
       {
-         std::cout << "x = " << xx << ", y = " << yy << ", z = " << zz << ", r = " << rr << std::endl;
+         if(drone1.enable_altitude_control == 1)
+            std::cout << "x = " << xx << ", y = " << yy << ", z = " << zz << ", r = " << rr << std::endl;
+         
+         if(drone1.enable_altitude_control == 0)
+            std::cout << "x = " << xx << ", y = " << yy << ", z = " << zz_direct << ", r = " << rr << std::endl;
+         
          std::cout << "Drone is not armed, can't control. You can type 'reset' to stop this print." << std::endl;
       }
    }

Com_server/fly_monitor/src/TestDroneRemoteControl.cpp

@@ -555,6 +555,40 @@ int command_loop()
          drone1.command_directControl(0.f,0.f,0.5f,0.f);
       }
       
+      drone1.enable_altitude_control = 1;
+      cout << "\nSet mode to position control" << endl;
+      drone1.flight_mode = FlightMode::Posctl;
+      drone1.command_setMode(FlightMode::Posctl);
+      sleep(2);
+   }
+   else if(buffer_command=="manual_manual")
+   {
+      /*
+      while (!drone1.calibration_check())
+      {
+         cout << "Waiting for system to be ready\n";
+         sleep_for(seconds(1));
+      }
+      //*/
+      cout << "System is ready\n";
+      
+      // x, y, z, r = pitch, roll, throttle, yaw
+      
+      for (unsigned i = 0; i < 20; i++)
+      {
+         drone1.command_directControl(0.f,0.f,0.5f,0.f);
+      }
+      
+      cout << "Arm" << endl;
+      drone1.command_arm(1);
+      sleep(1);
+      
+      for (unsigned i = 0; i < 20; i++)
+      {
+         drone1.command_directControl(0.f,0.f,0.5f,0.f);
+      }
+      
+      drone1.enable_altitude_control = 0;
       cout << "\nSet mode to position control" << endl;
       drone1.flight_mode = FlightMode::Posctl;
       drone1.command_setMode(FlightMode::Posctl);
@@ -747,6 +781,8 @@ int command_loop()
       cout << " k             terminate flight immediately, may cause crash landing" << endl;
       cout << " !k            reset the drone after a flight termination" << endl;
       cout << " manual        set the drone to 'position control' mode, it will be controlled by shared memory" << endl;
+      cout << " manual_manual similar script as manual but the altitude is not controlled, " << endl;
+      cout << "               the gas power is directly linked to the throttle axis" << endl;
       cout << " manual_test   same as manual mode but the drone will not move (no arming)" << endl;
       cout << " tone          make the autopilot play a certain tone" << endl;
       cout << " c             playground for printing" << endl; // c for check