[v3.6.2] [remove feature]: easondrone_msgs::ControlCommand::Idle

HuaYuXiao · Aug 5, 2024 · 78b5432 · 78b5432
1 parent fdbebce
commit 78b5432
Show file tree

Hide file tree

Showing 4 changed files with 20 additions and 32 deletions.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -5,6 +5,10 @@ All notable changes to this project will be documented in this file.
 The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.1.0/),
 and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0.html).
 
+## [v3.6.2] - 2024-08-05
+- [remove feature]: check for geofence
+- [remove feature]: `easondrone_msgs::ControlCommand::Idle`
+
 ## [v3.6.1] - 2024-08-05
 - [new feature]: support `VINS-Fusion`: `/vins_estimator/odometry` to `/mavros/vision_pose/pose`
 - [remove support]: `cartographer`, `outdoor`

diff --git a/package.xml b/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>px4ctrl</name>
-  <version>3.6.1</version>
+  <version>3.6.2</version>
   <description>
     A ROS package to estimate and control drone
   </description>

diff --git a/src/Utilities/terminal_control.cpp b/src/Utilities/terminal_control.cpp
@@ -46,7 +46,7 @@ int main(int argc, char **argv){
             ("/easondrone/control_command", 10);
 
     // 初始化命令 - Idle模式 电机怠速旋转 等待来自上层的控制指令
-    Command_to_pub.Mode                                = easondrone_msgs::ControlCommand::Idle;
+    Command_to_pub.Mode                                = easondrone_msgs::ControlCommand::Move;
     Command_to_pub.Reference_State.Move_mode           = easondrone_msgs::PositionReference::XYZ_POS;
     Command_to_pub.Reference_State.Move_frame          = easondrone_msgs::PositionReference::ENU_FRAME;
 
@@ -67,7 +67,7 @@ int main(int argc, char **argv){
 }
 
 void mainloop(){
-    int Control_Mode = 0;
+    int Control_Mode = 6;
     bool valid_Control_Mode = false;
     int Move_mode = 0;
     bool valid_move_mode = false;
@@ -83,11 +83,10 @@ void mainloop(){
         while (!valid_Control_Mode){
             cout << "-------- EasonDrone Terminal--------" << endl;
             cout << "Please choose the Command.Mode: " << endl;
-            cout << "0 for IDLE, 1 for TAKEOFF, 2 for HOLD, 3 for LAND, " << endl;
-            cout << "4 for MOVE, 5 for DISARM, 6 - ARM & OFFBOARD" << endl;
+            cout << "1 - Takeoff, 2 - Hover,  3 - Land, " << endl;
+            cout << "4 - Move,    5 - Disarm, 6 - Arm & Offboard" << endl;
             if (cin >> Control_Mode) {
-                if (Control_Mode == 0 ||
-                    Control_Mode == 1 ||
+                if (Control_Mode == 1 ||
                     Control_Mode == 2 ||
                     Control_Mode == 3 ||
                     Control_Mode == 4 ||
@@ -110,14 +109,6 @@ void mainloop(){
         valid_Control_Mode = false;
 
         switch (Control_Mode){
-            case easondrone_msgs::ControlCommand::Idle:{
-                Command_to_pub.header.stamp = ros::Time::now();
-                Command_to_pub.Mode = easondrone_msgs::ControlCommand::Idle;
-                easondrone_ctrl_pub_.publish(Command_to_pub);
-
-                break;
-            }
-
             case 6:{
                 Command_to_pub.header.stamp = ros::Time::now();
                 Command_to_pub.Mode = easondrone_msgs::ControlCommand::OFFBOARD_ARM;
@@ -159,10 +150,12 @@ void mainloop(){
                             Move_mode == 2 ||
                             Move_mode == 3) {
                             valid_move_mode = true;
-                        } else {
+                        }
+                        else {
                             cout << "Invalid input! Please enter a valid Move_mode." << endl;
                         }
-                    } else {
+                    }
+                    else {
                         // Clear error flags
                         cin.clear();
                         // Discard invalid input
@@ -177,10 +170,12 @@ void mainloop(){
                     if (cin >> Move_frame) {
                         if (Move_frame == 0 || Move_frame == 1) {
                             valid_move_frame = true;
-                        } else {
+                        }
+                        else {
                             cout << "Invalid input! Please enter 0 or 1." << endl;
                         }
-                    } else {
+                    }
+                    else {
                         // Clear error flags
                         cin.clear();
                         // Discard invalid input

diff --git a/src/px4ctrl.cpp b/src/px4ctrl.cpp
@@ -68,7 +68,7 @@ int main(int argc, char **argv){
     cout << "Land_speed       : "<< Land_speed_ <<" [m/s] "<<endl;
 
     // 初始化命令- 默认设置：Idle模式 电机怠速旋转 等待来自上层的控制指令
-    Command_Now.Mode                                = easondrone_msgs::ControlCommand::Idle;
+    Command_Now.Mode                                = easondrone_msgs::ControlCommand::Move;
     Command_Now.Reference_State.Move_mode           = easondrone_msgs::PositionReference::XYZ_POS;
     Command_Now.Reference_State.Move_frame          = easondrone_msgs::PositionReference::ENU_FRAME;
 
@@ -90,11 +90,6 @@ int main(int argc, char **argv){
         ros::spinOnce();
 
         switch (Command_Now.Mode){
-            // 【Idle】
-            case easondrone_msgs::ControlCommand::Idle:{
-                break;
-            }
-
             // 怠速旋转，此时可以切入offboard模式，但不会起飞
             case easondrone_msgs::ControlCommand::OFFBOARD_ARM:{
                 ROS_INFO("FSM_EXEC_STATE: OFFBOARD & ARM");
@@ -183,10 +178,6 @@ int main(int argc, char **argv){
                     }
                 }
 
-//                if(_DroneState.landed){
-//                    Command_Now.Mode = easondrone_msgs::ControlCommand::Idle;
-//                }
-
                 break;
             }
 
@@ -225,9 +216,7 @@ int main(int argc, char **argv){
         }
 
         //执行控制
-        if(Command_Now.Mode != easondrone_msgs::ControlCommand::Idle){
-            _ControlOutput = pos_controller_cascade_pid.pos_controller(_DroneState, Command_Now.Reference_State, dt);
-        }
+        _ControlOutput = pos_controller_cascade_pid.pos_controller(_DroneState, Command_Now.Reference_State, dt);
 
         throttle_sp[0] = _ControlOutput.Throttle[0];
         throttle_sp[1] = _ControlOutput.Throttle[1];