From 6d854ffeef2ee5873d6ea942d6630ec9a51ddfe9 Mon Sep 17 00:00:00 2001
From: StaY <stephane.yang@ensea.fr>
Date: Fri, 20 Oct 2023 14:26:57 +0200
Subject: [PATCH] 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 | 1 +
.../src/Com_SerialWritingThread.cpp | 17 +++++----
.../src/TestDroneRemoteControl.cpp | 36 +++++++++++++++++++
3 files changed, 47 insertions(+), 7 deletions(-)
diff --git a/Com_server/fly_monitor/include/Data_Drone.h b/Com_server/fly_monitor/include/Data_Drone.h
index 5d66160..b53d3cd 100644
--- a/Com_server/fly_monitor/include/Data_Drone.h
+++ b/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)
diff --git a/Com_server/fly_monitor/src/Com_SerialWritingThread.cpp b/Com_server/fly_monitor/src/Com_SerialWritingThread.cpp
index 97e4596..d4a595e 100644
--- a/Com_server/fly_monitor/src/Com_SerialWritingThread.cpp
+++ b/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;
}
}
diff --git a/Com_server/fly_monitor/src/TestDroneRemoteControl.cpp b/Com_server/fly_monitor/src/TestDroneRemoteControl.cpp
index d0eda71..5c41aad 100644
--- a/Com_server/fly_monitor/src/TestDroneRemoteControl.cpp
+++ b/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
--
GitLab