一、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 ×tamp_sample);-
发布推理设定点
void publishThrustSetpoint(const hrt_abstime ×tamp_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*/
