一、Px4版本1.14.1机型文件

PX4Autopilotmain\ROMFS\px4fmu_common\init.d\airframes路径下

这个脚本设置了BlueROV2（重型配置）的各种参数和初始化步骤，包括电池设置、通信设置、机架和旋翼配置以及PWM输出功能的映射。通过这些设置，可以确保机器人在启动时加载正确的配置，并按照预期的方式运行

# 加载默认的UUV配置

. ${R}etc/init.d/rc.uuv_defaults

# 设置参数

param	set-default
CBRK_IO_SAFETY	22027
MAV_1_CONFIG	102
BAT1_A_PER_V	37.8798
BAT1_CAPACITY	18000
BAT1_V_DIV	11
BAT1_N_CELLS	4
BAT_V_OFFS_CURR	0.33

# 机架和旋翼配置

param	set-default
CA_AIRFRAME	7
CA_ROTOR_COUNT	8
CA_R_REV	255

# 旋翼0参数

param	set-default
CA_ROTOR0_AX	1
CA_ROTOR0_AY	-1
CA_ROTOR0_AZ	0
CA_ROTOR0_KM	0
CA_ROTOR0_PX	0.5
CA_ROTOR0_PY	0.3
CA_ROTOR0_PZ	0.2

 # 旋翼1参数    

param	set-default
CA_ROTOR1_AX	1
CA_ROTOR1_AY	1
CA_ROTOR1_AZ	0
CA_ROTOR1_KM	0
CA_ROTOR1_PX	0.5
CA_ROTOR1_PY	-0.3
CA_ROTOR1_PZ	0.2

# 旋翼2参数

param	set-default
CA_ROTOR2_AX	1
CA_ROTOR2_AY	1
CA_ROTOR2_AZ	0
CA_ROTOR2_KM	0
CA_ROTOR2_PX	-0.5
CA_ROTOR2_PY	0.3
CA_ROTOR2_PZ	0.2

# 旋翼3参数

param	set-default
CA_ROTOR3_AX	1
CA_ROTOR3_AY	-1
CA_ROTOR3_AZ	0
CA_ROTOR3_KM	0
CA_ROTOR3_PX	-0.5
CA_ROTOR3_PY	-0.3
CA_ROTOR3_PZ	0.2

# 旋翼4参数

param	set-default
CA_ROTOR4_AZ	-1
CA_ROTOR4_KM	0
CA_ROTOR4_PX	0.5
CA_ROTOR4_PY	0.5

# 旋翼5参数

param	set-default
CA_ROTOR5_AZ	1
CA_ROTOR5_KM	0
CA_ROTOR5_PX	0.5
CA_ROTOR5_PY	-0.5

# 旋翼6参数

param	set-default
CA_ROTOR6_AZ	1
CA_ROTOR6_KM	0
CA_ROTOR6_PX	-0.5
CA_ROTOR6_PY	0.5

# 旋翼7参数

param	set-default
CA_ROTOR7_KM	0
CA_ROTOR7_PX	-0.5
CA_ROTOR7_PY	-0.5

# PWM功能映射

param	set-default
PWM_MAIN_FUNC1	101
PWM_MAIN_FUNC2	102
PWM_MAIN_FUNC3	103
PWM_MAIN_FUNC4	104
PWM_MAIN_FUNC5	105
PWM_MAIN_FUNC6	106
PWM_MAIN_FUNC7	107
PWM_MAIN_FUNC8	108

二、Rcs启动脚本

启动脚本是一个神奇的东西，它能够识别出你对应的飞机类型，加载对应的混控器，选择对应的姿态、位置估计程序以及控制程序，初始化你需要的驱动程序。

启动脚本实现了挂载SD卡，设置存储好的飞行控制初始参数（可以通过地面站修改），启动所有外设传感器，启动与地面站通行的Mavlink服务，以及机型的选择后对应启动的控制服务（以uuv为例，启动了ekf2，uuv_att_control，uuv_pos_control等服务），打开navigator服务。    

启动机架代码

# Configure vehicle type specific parameters.

        # Note: rc.vehicle_setup is the entry point for all vehicle type specific setup.

        . ${R}etc/init.d/rc.vehicle_setup

进入rc.vehicle_setup 后，可以看到有一个机型判断

# UUV setup    

#

if [ $VEHICLE_TYPE = uuv ]

then

        # Start standard vtol apps.

        . ${R}etc/init.d/rc.uuv_apps

fi

然后进入rc.uuv.app​​​​​​​​​​​​​​

# Start Control Allocator

#

control_allocator start

          

#

# Start UUV Attitude Controller.

#

uuv_att_control start

          

#

# Start UUV Land Detector.

#

land_detector start rover

可以看到启动控制分配器、UUV姿态控制器和UUV着陆检测器的命令。    

三、然后我们进入到uuv_att_control.h

先来看它的头文件，uuv_att_control.h定义了一个名为 UUVAttitudeControl的类，它继承了ModuleBase、ModuleParams 和 px4::WorkItem。

公有成员函数

• 构造函数和析构函数

UUVAttitudeControl(); ~UUVAttitudeControl(); );

• 任务启动  

static int task_spawn(int argc, char *argv[]);

• 定义命令

static int custom_command(int argc, char *argv[]);

• 打印用法

static int print_usage(const char *reason = nullptr);

• 初始化函数

bool init();

私有成员函数

• 发布扭矩设定点

void publishTorqueSetpoint(const hrt_abstime &timestamp_sample);

• 发布推理设定点

void publishThrustSetpoint(const hrt_abstime &timestamp_sample);

• 运行函数（主要控制逻辑）

void Run() override;

• 参数更新

void parameters_update(bool force = false);

• 姿态控制

void control_attitude_geo(const vehicle_attitude_s &attitude, const vehicle_attitude_setpoint_s &attitude_setpoint, const vehicle_angular_velocity_s &angular_velocity, const vehicle_rates_setpoint_

四、然后我们进入到uuv_att_control.cpp          

1、函数constrain_actuator_commands

用于约束并设置姿态控制和推力控制的命令，以确保这些命令在合理的范围内。具体地说，它处理滚转（roll）、俯仰（pitch）、偏航（yaw）以及在 x、y、z 方向上的推力（thrust）的控制命令。以其中一个举例：​​​​​​​

检查滚转命令（roll_u）
if (PX4_ISFINITE(roll_u)) {
                roll_u = math::constrain(roll_u, -1.0f, 1.0f);4.                _vehicle_torque_setpoint.xyz[0] = roll_u;

        } else {
                _vehicle_torque_setpoint.xyz[0] = 0.0f;
        }

• 首先检查roll_u是否为有限值（不是无穷大或NaN）
• 如果是有限值，则将roll_u约束在 [-1.0, 1.0] 范围内，并设置为车辆的滚转扭矩设定点。
• 如果不是有限值，则将滚转扭矩设定点设置为 0。

2、control_attitude_geo函数，使用比例和微分控制器调节水下机器人的姿态。

通过计算姿态误差和角速度误差，生成控制力矩，并结合推力设定值，生成最终的控制命令。通过 constrain_actuator_commands 函数，将这些命令约束在合理范围内，以确保机器人的稳定性和安全性。

1）从四元数提取姿态和设定值​​​​​​​

Eulerf euler_angles(matrix::Quatf(attitude.q));
        float roll_body = attitude_setpoint.roll_body;
        float pitch_body = attitude_setpoint.pitch_body;
        float yaw_body = attitude_setpoint.yaw_body;
        float roll_rate_desired = rates_setpoint.roll;
        float pitch_rate_desired = rates_setpoint.pitch;
        float yaw_rate_desired = rates_setpoint.yaw;

• 将姿态四元数转换为欧拉角。
• 提取设定的姿态（滚转、俯仰、偏航）和角速度。

2）计算旋转矩阵

/* get attitude setpoint rotational matrix */
        Dcmf rot_des = Eulerf(roll_body, pitch_body, yaw_body);

        /* get current rotation matrix from control state quaternions */
        Quatf q_att(attitude.q);
        Matrix3f rot_att =  matrix::Dcm<float>(q_att);
;

• 将设定的欧拉角转换成旋转矩阵rot_des。

• 将当前姿态四元数转换为旋转矩阵rot_att。

3）计算姿态误差

/* Compute matrix: attitude error */
        Matrix3f e_R = (rot_des.transpose() * rot_att - rot_att.transpose() * rot_des) * 0.5;

        /* vee-map the error to get a vector instead of matrix e_R */
        e_R_vec(0) = e_R(2, 1);  /**< Roll  */
        e_R_vec(1) = e_R(0, 2);  /**< Pitch */
        e_R_vec(2) = e_R(1, 0);  /**< Yaw   */

• e_R(2, 1)表示矩阵e_R的第3行第2列元素。
• 计算姿态误差矩阵e_R该矩阵表示当前姿态与期望姿态之间的误差。
• 使用vee-map将误差矩阵转换为误差向量e_R_vec分别对应滚转、俯仰和偏航的误差。

4）计算角速度误差并应用PD控制器​​​​​​​

/**< P-Control */
        torques(0) = - e_R_vec(0) * _param_roll_p.get();        /**< Roll  */
        torques(1) = - e_R_vec(1) * _param_pitch_p.get();       /**< Pitch */
        torques(2) = - e_R_vec(2) * _param_yaw_p.get();         /**< Yaw   */

        /**< PD-Control */
        torques(0) = torques(0) - omega(0) * _param_roll_d.get();  /**< Roll  */
        torques(1) = torques(1) - omega(1) * _param_pitch_d.get(); /**< Pitch */
        torques(2) = torques(2) - omega(2) * _param_yaw_d.get();   /**< Yaw   */

        float roll_u = torques(0);
        float pitch_u = torques(1);
        float yaw_u = torques(2);

        // take thrust as
        float thrust_x = attitude_setpoint.thrust_body[0];
        float thrust_y = attitude_setpoint.thrust_body[1];
        float thrust_z = attitude_setpoint.thrust_body[2];

• 计算当前角度与设定角度之间的误差omega。
• 通过P控制器计算初步的控制力矩。
• 通过PD控制器（比例微分控制器）调整控制力矩。

五、提取推力设定值并约束控制命令​​​​​​​

constrain_actuator_commands(roll_u, pitch_u, yaw_u, thrust_x, thrust_y, thrust_z);
        /* Geometric Controller END*/