fix bug & update

Author: Boshen97

CHANGELOG.md

@@ -45,4 +45,18 @@
 
 其他内容: 后续将进行真机测试,根据真机测试情况将继续完善
 
+****************************************************
+
+修改时间: 2022.08.29
+
+修改人:   李博
+
+修改内容: 
+1. 修复真实高度与经纬度数据获取BUG
+2. 修复机体坐标系下XY速度Z高度控制逻辑BUG
+3. 优化多次调用位置偏移量问题
+测试情况: 仿真测试通过
+
+其他内容: 后续将进行真机测试,根据真机测试情况将继续完善
+
 ****************************************************

Modules/uav_control/include/uav_estimator.h

@@ -64,6 +64,7 @@ class UAV_estimator
         ros::Subscriber uwb_sub;
         ros::Subscriber fake_odom_sub;
         ros::Subscriber px4_global_position_sub;
+        ros::Subscriber px4_rel_alt_sub;
         ros::Subscriber gps_status_sub;
         ros::Subscriber set_local_pose_offset_sub;
         // 发布话题

Modules/uav_control/src/uav_controller.cpp

@@ -524,7 +524,7 @@ void UAV_controller::set_command_des()
                 uav_command.velocity_ref[1] = d_vel_enu[1];
                 pos_des[0] = 0.0;
                 pos_des[1] = 0.0;
-                pos_des[2] = uav_command.position_ref[2];
+                pos_des[2] = uav_pos[2] + uav_command.position_ref[2];
                 vel_des[0] = uav_command.velocity_ref[0];
                 vel_des[1] = uav_command.velocity_ref[1];
                 vel_des[2] = 0.0;

Modules/uav_control/src/uav_estimator.cpp

@@ -63,7 +63,7 @@ UAV_estimator::UAV_estimator(ros::NodeHandle &nh)
         // 【订阅】无人机当前经纬度，来自飞控
         px4_global_position_sub = nh.subscribe<sensor_msgs::NavSatFix>(uav_name + "/mavros/global_position/global", 1, &UAV_estimator::px4_global_pos_cb, this);
         // 【订阅】无人机当前真实高度，来自飞控
-        px4_global_position_sub = nh.subscribe<std_msgs::Float64>(uav_name + "/mavros/global_position/rel_alt", 1, &UAV_estimator::px4_global_rel_alt_cb, this);
+        px4_rel_alt_sub = nh.subscribe<std_msgs::Float64>(uav_name + "/mavros/global_position/rel_alt", 1, &UAV_estimator::px4_global_rel_alt_cb, this);
         // 【订阅】设置ENU坐标系下无人机的位置偏移量  坐标系:ENU系 - 来自地面站/终端窗口
         set_local_pose_offset_sub = nh.subscribe<prometheus_msgs::GPSData>(uav_name + "/prometheus/set_local_offset_pose", 1, &UAV_estimator::set_local_pose_offset_cb, this);
         // 【发布】ENU坐标系下的位置偏移量
@@ -733,8 +733,9 @@ void UAV_estimator::set_local_pose_offset_cb(const prometheus_msgs::GPSData::Con
     // 每个飞机的local_position还是在起飞坐标系
     // 所以发布控制指令也要加上偏差
     offset_pose.uav_id = uav_id;
-    offset_pose.x = enu_offset[0] - uav_state.position[0] + msg->x;
-    offset_pose.y = enu_offset[1] - uav_state.position[1] + msg->y;
+    //重复调用时,避免偏移量错误计算,需要先减掉偏移量
+    offset_pose.x += enu_offset[0] - uav_state.position[0] + msg->x;
+    offset_pose.y += enu_offset[1] - uav_state.position[1] + msg->y;
 
     local_pose_offset_pub.publish(offset_pose);
 }