定深模式源码分析 | Felix’s Micro Space

type

date

slug

功能分析

定深模式控制过程：根据油门输入，确定Z轴爬升率，并对水面和水底两个临界工况的爬升率做特殊处理。最后根据得到的爬升率确定Z轴上目标位置。

如何实现深度调节：通过油门Throttle输入，通过算法确定期望深度。

水面和水底判断：有两种方案，一种配备压力传感器结合实时水深和油门临界值做判断，另一种未装载压力传感器，单纯通过油门临界值判断。

水面和水底处理：水面控制在-15cm处，水底控制在底部向上10cm处。

源码分析 - `void ModeAlthold::control_depth()`

根据油门通道控制输入，获取定深模式下Z轴期望爬升率；

限制目标爬升率在 -sub.get_pilot_speed_dn()和 g.pilot_speed_up之间；

get_pilot_speed_dn()判断配置的下降速度，若为0，则返回上升速度 pilot_speed_up的绝对值，若为非 0 值，则返回pilot_speed_dn；

b. 与升降相关的参数包括PILOT_SPEED_DN、PILOT_SPEED_UP、PILOT_ACCEL_Z，分别是下降速度、上身速度和垂直加速度。可以通过QGC|设置|PILOT 路径查看。

油门控制在中间位置存在死区，当油门控制的爬升率低于0.05，认为是误操作，深度不发生变化。

水面和水底特殊情况处理

水面情况：水面surface_depth 为 -10cm，设置的水深低于水面5cm。通过宏定义MIN，保证在控制处于死区时，控制深度低于-15，保证处于水面下。
水底情况：设置高于底部10cm，防止底部碰撞。

定深模式在深度控制是通过油门控制爬升率确定控制高度。

另外对两部分展开讲讲，水面和水底判断方法。

油门输入确定指令爬升率

根据油门输入，输出爬升率。主要做好油门控制死区管理。通过油门输入在上下可操作区间做线性映射获取比例，乘以最大的PILOT_SPEED_DN、PILOT_SPEED_UP配置速度。

举一个具体数值例子说明

failsafe.pilot_input = false (no failsafe condition)

channel_throttle->get_control_mid() = 500.0f (midpoint of the throttle control)

g.throttle_deadzone = 100 (deadzone value)

gain = 1.0f (gain factor)

get_pilot_speed_dn() = -200.0f cm/s (maximum downward speed)

g.pilot_speed_up = 300.0f cm/s (maximum upward speed)

throttle_control = 700.0f (current throttle input)

水面和底部判断

ArduSub/surface_bottom_detector.cpp 完成水面和水底检测。核心是调用update_surface_and_bottom_detector接口完成。

ArduSub/surface_bottom_detector.cpp 函数接口说明

Static Variables

static uint32_t bottom_detector_count = 0;

static uint32_t surface_detector_count = 0;

static float current_depth = 0;

void Sub::update_surface_and_bottom_detector()

This function checks if the submarine has hit the bottom or the surface and updates the ap.at_bottom and ap.at_surface flags accordingly. It is called at the MAIN_LOOP_RATE .

Parameters: None

Returns: None

Functionality:

Only updates when motors are armed.

Retrieves the current velocity and checks if the submarine is stationary in the vertical axis.

Uses the external pressure sensor (if available) to get the current depth.

Updates the ap.at_surface flag based on the current depth and predefined surface depth.

Increments counters to determine if the submarine has reached the bottom or surface based on throttle limits and velocity.

Resets the flags if the submarine is moving or if the criteria are not met.

void Sub::set_surfaced(bool at_surface)

This function sets the ap.at_surface flag and logs the event.

Parameters:

bool at_surface : Indicates whether the submarine is at the surface.

Returns: None

Functionality:

Checks if the state has changed.

Updates the ap.at_surface flag and resets the surface_detector_count .

Logs the event using LOGGER_WRITE_EVENT .

void Sub::set_bottomed(bool at_bottom)

This function sets the ap.at_bottom flag and logs the event.

Parameters:

bool at_bottom : Indicates whether the submarine is at the bottom.

Returns: None

Functionality:

Checks if the state has changed.

Updates the ap.at_bottom flag and resets the bottom_detector_count .

Logs the event using LOGGER_WRITE_EVENT .

以下内容是对 Sub::update_surface_and_bottom_detector() 接口详细解释。

只有电机解锁才检测潜航器位置。

第一种方案是通过外部传感器压力计获取入水深度。

处于水面判断逻辑：当前深度 current_depth 高于设置的水面高度。缺省的水面高度通过参数配置，默认的高度在 config.h 文件中通过宏定义设置为-10cm。

处于底部判断逻辑：油门推力处于低限，但是机器静止，即下潜受阻，认为触底。对于水下设备，油门推力0.0-0.5-1.0三个值对应下降最大加速度-静止-向上最大加速度。映射关系不同于飞行设备（0.0-0.5-1.0三个值对应静止-中等向上加速度-向上最大加速度）。油门处于低限0指向下最大加速度，即下潜最大加速度。潜航器的油门控制通过如下源码详细说明(详见)：