更新task_3.py，增强爬坡和下坡阶段的监测功能，添加姿态判断参数，优化稳定性检测逻辑，使用滑动窗口记录高度和俯仰角，提升代码可读性和执行效率。

task_3/task_3.py

@@ -5,6 +5,7 @@ import toml
 import copy
 import math
 import lcm
+import numpy as np
 
 # 添加父目录到路径，以便能够导入utils
 sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
@@ -103,36 +104,73 @@ def run_task_3(ctrl, msg):
         max_iterations = 250  # 最大循环次数，作为安全保障
         min_iterations = 150  # 最小循环次数，作为安全保障
 
+        # 姿态判断参数
+        pitch_threshold = 0.05  # 俯仰角阈值（弧度）
+        angular_rate_threshold = 0.03  # 角速度阈值（弧度/秒）
+
         # 阶段控制
         climbing_detected = False  # 是否检测到正在爬坡
+        
+        # 高度变化记录
+        height_window = []
+        pitch_window = []
+        window_size = 8
+        
+        # 记录起始姿态和高度
+        start_height = ctrl.odo_msg.xyz[2]
+        info(f"开始监测爬坡过程，初始高度: {start_height:.3f}", "监测")
+        
         for i in range(max_iterations):
             # 发送控制命令维持心跳
             ctrl.Send_cmd(msg)
             
+            # 获取当前状态数据
+            vz = ctrl.odo_msg.vxyz[2]  # Z轴速度
+            current_height = ctrl.odo_msg.xyz[2]  # 当前高度
+            current_pitch = ctrl.odo_msg.rpy[1]  # 当前俯仰角
+            pitch_rate = ctrl.odo_msg.omegaBody[1]  # 俯仰角速度
+            vbody_z = ctrl.odo_msg.vBody[2]  # 机体坐标系Z速度
+            
+            # 更新滑动窗口数据
+            height_window.append(current_height)
+            pitch_window.append(current_pitch)
+            if len(height_window) > window_size:
+                height_window.pop(0)
+                pitch_window.pop(0)
+            
             # 每10次迭代打印一次当前信息
             if i % 10 == 0:
-                # 获取当前Z轴位置和速度
+                info(f"当前Z轴速度={vz:.3f}, 当前高度={current_height:.3f}, 俯仰角={current_pitch:.3f}, 角速度={pitch_rate:.3f}", "监测")
-                current_vz = ctrl.odo_msg.vxyz[2]  # z轴速度
-                info(f"当前Z轴速度={current_vz:.3f}", "监测")
             
-            # 获取z轴速度
+            # 检测是否开始爬坡阶段
-            vz = ctrl.odo_msg.vxyz[2]
-            
-            # 检测是否开始爬坡阶段 - 使用z轴速度判断
             if not climbing_detected and vz > climb_speed_threshold:
                 climbing_detected = True
-                info(f"检测到开始爬坡，Z轴速度: {vz:.3f}, 当前高度: {ctrl.odo_msg.xyz[2]:.3f}", "监测")
+                info(f"检测到开始爬坡，Z轴速度: {vz:.3f}, 当前高度: {current_height:.3f}, 俯仰角: {current_pitch:.3f}", "监测")
             
-            # 只有在检测到爬坡后，才开始监控Z轴是否停止增加
+            # 多条件判断是否完成爬坡
-            if i > min_iterations and climbing_detected:
+            if i > min_iterations and climbing_detected and len(height_window) == window_size:
-                # 如果Z轴速度接近于0或者为负，表示已经停止爬升或开始下降
+                # 计算高度和俯仰角的稳定性
-                if abs(vz) < z_speed_threshold: #  or vz < 0:
+                height_std = np.std(height_window)  # 高度标准差
+                pitch_std = np.std(pitch_window)    # 俯仰角标准差
+                
+                # 多条件综合判断
+                position_stable = abs(vz) < z_speed_threshold  # 垂直速度稳定
+                attitude_stable = abs(current_pitch) < pitch_threshold  # 俯仰角接近水平
+                angular_rate_stable = abs(pitch_rate) < angular_rate_threshold  # 角速度稳定
+                height_stable = height_std < 0.01  # 高度变化小
+                pitch_stable = pitch_std < 0.01  # 俯仰角变化小
+                vbody_stable = abs(vbody_z) < 0.01  # 机体Z方向速度稳定
+                
+                # 综合判断条件
+                if (position_stable and attitude_stable and angular_rate_stable) or \
+                   (position_stable and height_stable and pitch_stable) or \
+                   (vbody_stable and attitude_stable and height_stable):
                     stable_count += 1
                     if stable_count >= stable_threshold:
-                        info(f"Z轴速度趋近于0，停止循环。当前速度: {vz:.3f}", "监测")
+                        info(f"检测到已完成爬坡：\n  - Z轴速度: {vz:.3f}\n  - 俯仰角: {current_pitch:.3f}\n  - 角速度: {pitch_rate:.3f}\n  - 当前高度: {current_height:.3f}\n  - 上升了: {current_height - start_height:.3f}米", "监测")
                         break
                 else:
-                    # 如果Z轴仍有明显上升速度，重置稳定计数
+                    # 重置稳定计数
                     stable_count = 0
             
             time.sleep(0.2)
@@ -217,40 +255,73 @@ def run_task_3(ctrl, msg):
         min_iterations = 150  # 最小循环次数，确保有足够的时间开始动作
         start_height = ctrl.odo_msg.xyz[2]  # 记录起始高度
         
+        # 姿态判断参数
+        pitch_threshold = 0.05  # 俯仰角阈值（弧度）
+        angular_rate_threshold = 0.03  # 角速度阈值（弧度/秒）
+        
         # 阶段控制
         descending_detected = False  # 是否检测到正在下坡
         flat_ground_detected = False  # 是否检测到已到达平地
         
+        # 高度变化记录
+        height_window = []
+        pitch_window = []
+        window_size = 8
+        
         info(f"开始监测下坡过程，初始高度: {start_height}", "监测")
         
         for i in range(max_iterations):
             # 发送控制命令维持心跳
             ctrl.Send_cmd(msg)
             
-            # 获取z轴速度和当前高度
+            # 获取当前状态数据
-            vz = ctrl.odo_msg.vxyz[2]
+            vz = ctrl.odo_msg.vxyz[2]  # Z轴速度
-            current_height = ctrl.odo_msg.xyz[2]
+            current_height = ctrl.odo_msg.xyz[2]  # 当前高度
+            current_pitch = ctrl.odo_msg.rpy[1]  # 当前俯仰角
+            pitch_rate = ctrl.odo_msg.omegaBody[1]  # 俯仰角速度
+            vbody_z = ctrl.odo_msg.vBody[2]  # 机体坐标系Z速度
+            
+            # 更新滑动窗口数据
+            height_window.append(current_height)
+            pitch_window.append(current_pitch)
+            if len(height_window) > window_size:
+                height_window.pop(0)
+                pitch_window.pop(0)
             
             # 每10次迭代打印一次当前信息
             if observe and i % 10 == 0:
-                info(f"当前Z轴速度={vz:.3f}, 当前高度={current_height:.3f}", "监测")
+                info(f"当前Z轴速度={vz:.3f}, 当前高度={current_height:.3f}, 俯仰角={current_pitch:.3f}, 角速度={pitch_rate:.3f}", "监测")
             
-            # 检测是否开始下坡阶段 - 使用z轴速度判断（负值表示下降）
+            # 检测是否开始下坡阶段
             if not descending_detected and vz < descent_speed_threshold:
                 descending_detected = True
-                info(f"检测到开始下坡，Z轴速度: {vz:.3f}, 当前高度: {current_height:.3f}", "监测")
+                info(f"检测到开始下坡，Z轴速度: {vz:.3f}, 当前高度: {current_height:.3f}, 俯仰角: {current_pitch:.3f}", "监测")
             
-            # 只有在检测到下坡后，才开始监控是否到达平地
+            # 多条件判断是否到达平地
-            if i > min_iterations and descending_detected:
+            if i > min_iterations and descending_detected and len(height_window) == window_size:
-                # 如果Z轴速度接近于0，表示已经停止下降（到达平地）
+                # 计算高度和俯仰角的稳定性
-                if abs(vz) < z_speed_threshold:
+                height_std = np.std(height_window)  # 高度标准差
+                pitch_std = np.std(pitch_window)    # 俯仰角标准差
+                
+                # 多条件综合判断
+                position_stable = abs(vz) < z_speed_threshold  # 垂直速度稳定
+                attitude_stable = abs(current_pitch) < pitch_threshold  # 俯仰角接近水平
+                angular_rate_stable = abs(pitch_rate) < angular_rate_threshold  # 角速度稳定
+                height_stable = height_std < 0.01  # 高度变化小
+                pitch_stable = pitch_std < 0.01  # 俯仰角变化小
+                vbody_stable = abs(vbody_z) < 0.01  # 机体Z方向速度稳定
+                
+                # 综合判断条件
+                if (position_stable and attitude_stable and angular_rate_stable) or \
+                   (position_stable and height_stable and pitch_stable) or \
+                   (vbody_stable and attitude_stable and height_stable):
                     stable_count += 1
                     if stable_count >= stable_threshold:
-                        info(f"检测到已到达平地，Z轴速度趋近于0，停止循环。当前速度: {vz:.3f}, 当前高度: {current_height:.3f}, 下降了: {start_height - current_height:.3f}米", "监测")
+                        info(f"检测到已到达平地：\n  - Z轴速度: {vz:.3f}\n  - 俯仰角: {current_pitch:.3f}\n  - 角速度: {pitch_rate:.3f}\n  - 高度: {current_height:.3f}\n  - 下降了: {start_height - current_height:.3f}米", "监测")
                         flat_ground_detected = True
                         break
                 else:
-                    # 如果Z轴仍有明显下降速度，重置稳定计数
+                    # 重置稳定计数
                     stable_count = 0
             
             time.sleep(0.2)