From 1c1bc32ff2c5373c2c9d0746bf68e126a994f78e Mon Sep 17 00:00:00 2001 From: HarukiKozukapenguin Date: Fri, 4 Aug 2023 15:48:41 +0900 Subject: [PATCH 1/5] [DEBUG][aerial_robot_control] set setTargetAcc=0 in setTargetXyFromCurrentState --- .../include/aerial_robot_control/flight_navigation.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/aerial_robot_control/include/aerial_robot_control/flight_navigation.h b/aerial_robot_control/include/aerial_robot_control/flight_navigation.h index 5a463a8f9..75e3ad571 100644 --- a/aerial_robot_control/include/aerial_robot_control/flight_navigation.h +++ b/aerial_robot_control/include/aerial_robot_control/flight_navigation.h @@ -485,6 +485,10 @@ namespace aerial_robot_navigation // set the velocty to zero target_vel_.setX(0); target_vel_.setY(0); + + // set the acceleration to zero + setTargetAccX(0); + setTargetAccY(0); } void setTargetZFromCurrentState() From 6b211a201007ef4c022d2b5f7aff387b7be9882c Mon Sep 17 00:00:00 2001 From: HarukiKozukapenguin Date: Fri, 4 Aug 2023 15:52:38 +0900 Subject: [PATCH 2/5] [DEBUG][aerial_robot_control] set XyControlMode=POS_CONTROL_MODE in landing and halt --- .../include/aerial_robot_control/flight_navigation.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/aerial_robot_control/include/aerial_robot_control/flight_navigation.h b/aerial_robot_control/include/aerial_robot_control/flight_navigation.h index 75e3ad571..3a0c96287 100644 --- a/aerial_robot_control/include/aerial_robot_control/flight_navigation.h +++ b/aerial_robot_control/include/aerial_robot_control/flight_navigation.h @@ -404,6 +404,7 @@ namespace aerial_robot_navigation if(!teleop_flag_) return; setNaviState(LAND_STATE); + setXyControlMode(POS_CONTROL_MODE); setTargetXyFromCurrentState(); setTargetYawFromCurrentState(); @@ -418,6 +419,7 @@ namespace aerial_robot_navigation force_landing_flag_ = true; setNaviState(STOP_STATE); + setXyControlMode(POS_CONTROL_MODE); setTargetXyFromCurrentState(); setTargetYawFromCurrentState(); setTargetPosZ(estimator_->getLandingHeight()); From 661e12878786534d8798ce7c5c934ab55f8c9fa2 Mon Sep 17 00:00:00 2001 From: HarukiKozukapenguin Date: Sat, 5 Aug 2023 18:27:04 +0900 Subject: [PATCH 3/5] [DEBUG] set XyControlMode=POS_CONTROL_MODE in joystick control, and normal_land case --- aerial_robot_control/src/flight_navigation.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/aerial_robot_control/src/flight_navigation.cpp b/aerial_robot_control/src/flight_navigation.cpp index 8443551cb..7561ea2bc 100644 --- a/aerial_robot_control/src/flight_navigation.cpp +++ b/aerial_robot_control/src/flight_navigation.cpp @@ -408,6 +408,7 @@ void BaseNavigator::joyStickControl(const sensor_msgs::JoyConstPtr & joy_msg) setNaviState(LAND_STATE); //update + setXyControlMode(POS_CONTROL_MODE) setTargetXyFromCurrentState(); setTargetYawFromCurrentState(); setTargetPosZ(estimator_->getLandingHeight()); @@ -638,6 +639,7 @@ void BaseNavigator::update() if(normal_land && !force_att_control_flag_) { setNaviState(LAND_STATE); + setXyControlMode(POS_CONTROL_MODE); setTargetXyFromCurrentState(); setTargetYawFromCurrentState(); setTargetPosZ(estimator_->getLandingHeight()); From 291919b97e18f5954a35c77328b74563586b9bd6 Mon Sep 17 00:00:00 2001 From: HarukiKozukapenguin Date: Sat, 5 Aug 2023 18:49:12 +0900 Subject: [PATCH 4/5] [DEBUG][aerial_robot_control] delete setTargetXyFromCurrentState for this setting is enough if it is called in setTargetXyFromCurrentState() Revert "[DEBUG][aerial_robot_control] set XyControlMode=POS_CONTROL_MODE in landing and halt" This reverts commit 6b211a201007ef4c022d2b5f7aff387b7be9882c. Revert "[DEBUG] set XyControlMode=POS_CONTROL_MODE in joystick control, and normal_land case" This reverts commit 661e12878786534d8798ce7c5c934ab55f8c9fa2. --- .../include/aerial_robot_control/flight_navigation.h | 2 -- aerial_robot_control/src/flight_navigation.cpp | 2 -- 2 files changed, 4 deletions(-) diff --git a/aerial_robot_control/include/aerial_robot_control/flight_navigation.h b/aerial_robot_control/include/aerial_robot_control/flight_navigation.h index 3a0c96287..75e3ad571 100644 --- a/aerial_robot_control/include/aerial_robot_control/flight_navigation.h +++ b/aerial_robot_control/include/aerial_robot_control/flight_navigation.h @@ -404,7 +404,6 @@ namespace aerial_robot_navigation if(!teleop_flag_) return; setNaviState(LAND_STATE); - setXyControlMode(POS_CONTROL_MODE); setTargetXyFromCurrentState(); setTargetYawFromCurrentState(); @@ -419,7 +418,6 @@ namespace aerial_robot_navigation force_landing_flag_ = true; setNaviState(STOP_STATE); - setXyControlMode(POS_CONTROL_MODE); setTargetXyFromCurrentState(); setTargetYawFromCurrentState(); setTargetPosZ(estimator_->getLandingHeight()); diff --git a/aerial_robot_control/src/flight_navigation.cpp b/aerial_robot_control/src/flight_navigation.cpp index 7561ea2bc..8443551cb 100644 --- a/aerial_robot_control/src/flight_navigation.cpp +++ b/aerial_robot_control/src/flight_navigation.cpp @@ -408,7 +408,6 @@ void BaseNavigator::joyStickControl(const sensor_msgs::JoyConstPtr & joy_msg) setNaviState(LAND_STATE); //update - setXyControlMode(POS_CONTROL_MODE) setTargetXyFromCurrentState(); setTargetYawFromCurrentState(); setTargetPosZ(estimator_->getLandingHeight()); @@ -639,7 +638,6 @@ void BaseNavigator::update() if(normal_land && !force_att_control_flag_) { setNaviState(LAND_STATE); - setXyControlMode(POS_CONTROL_MODE); setTargetXyFromCurrentState(); setTargetYawFromCurrentState(); setTargetPosZ(estimator_->getLandingHeight()); From 2cebc7af4e5807b2f08f8117eee538f2e1f28288 Mon Sep 17 00:00:00 2001 From: HarukiKozukapenguin Date: Sat, 5 Aug 2023 19:02:08 +0900 Subject: [PATCH 5/5] [DEBUG][aerial_robot_control] set XyControlMode=POS_CONTROL_MODE in the call of setTargetXyFromCurrentState() --- .../include/aerial_robot_control/flight_navigation.h | 1 + 1 file changed, 1 insertion(+) diff --git a/aerial_robot_control/include/aerial_robot_control/flight_navigation.h b/aerial_robot_control/include/aerial_robot_control/flight_navigation.h index 75e3ad571..65ee522cc 100644 --- a/aerial_robot_control/include/aerial_robot_control/flight_navigation.h +++ b/aerial_robot_control/include/aerial_robot_control/flight_navigation.h @@ -478,6 +478,7 @@ namespace aerial_robot_navigation void setTargetXyFromCurrentState() { + setXyControlMode(POS_CONTROL_MODE); tf::Vector3 pos_cog = estimator_->getPos(Frame::COG, estimate_mode_); target_pos_.setX(pos_cog.x()); target_pos_.setY(pos_cog.y());