ArduPilot开源飞控之AP_Mount

ArduPilot开源飞控之AP_Mount_Servo

1. 源由
2. 框架设计
- 2.1 公有成员
- 2.2 受保护成员
- - 私有成员:
3. 重要方法
- 3.1 AP_Mount_Servo::init
- 3.2 AP_Mount_Servo::update
- 3.3 AP_Mount_Servo::has_roll_control
- 3.4 AP_Mount_Servo::has_pitch_control
- 3.5 AP_Mount_Servo::has_pan_control
4. 辅助函数
- 4.1 AP_Mount_Servo::update_angle_outputs
- 4.2 AP_Mount_Servo::move_servo
- 4.3 AP_Mount_Servo::get_attitude_quaternion
4. 总结
5. 参考资料

1. 源由

AP_Mount_Servo是PWM舵机控制云台的设备后台程序。通常舵机直接接受PWM信号控制，因此该云台设备驱动类也是最为基础的。

2. 框架设计

继承自 AP_Mount_Backend
用于控制基于舵机的云台
- 舵机控制: 允许在滚转、俯仰和偏航方向上的运动。
- 稳定: 区分需要外部稳定的舵机云台和自稳定的无刷PWM云台。
- 四元数: 使用四元数表示和获取姿态。
其中关于角度更新部分在AP_Mount_Backend::calculate_poi完成

2.1 公有成员

构造函数:
```
AP_Mount_Servo(AP_Mount &frontend, AP_Mount_Params &params, bool requires_stab, uint8_t instance)
```
- 参数:
  - AP_Mount &frontend: 前端云台对象的引用。
  - AP_Mount_Params &params: 云台参数的引用。
  - bool requires_stab: 指示是否需要稳定的布尔值。
  - uint8_t instance: 实例号。
- 初始化列表:
  - 使用frontend、params和instance初始化基类AP_Mount_Backend。
  - 将requires_stabilization设置为requires_stab。
  - 将舵机功能索引（_roll_idx、_tilt_idx、_pan_idx、_open_idx）初始化为SRV_Channel::k_none。
方法:
- void init() override;: 初始化实例。
- void update() override;: 定期更新云台位置。
- bool has_roll_control() const override;: 如果云台可以控制滚转，则返回true。
- bool has_pitch_control() const override;: 如果云台可以控制俯仰，则返回true。
- bool has_pan_control() const override;: 如果云台可以控制偏航（平移），则返回true。

2.2 受保护成员

方法:
- bool get_attitude_quaternion(Quaternion& att_quat) override;: 获取姿态的四元数，并在成功时返回true。

私有成员:

方法:
- void update_angle_outputs(const MountTarget& angle_rad);: 根据地球坐标系的目标更新机身坐标系的角度输出。
- void move_servo(uint8_t rc, int16_t angle, int16_t angle_min, int16_t angle_max);: 将具有给定功能ID的舵机移动到指定角度。所有角度都是机身坐标系的角度，单位为度*10。
成员:
- const bool requires_stabilization;: 指示舵机云台是否需要稳定。
- SRV_Channel::Aux_servo_function_t _roll_idx;: 滚转舵机功能索引。
- SRV_Channel::Aux_servo_function_t _tilt_idx;: 俯仰舵机功能索引。
- SRV_Channel::Aux_servo_function_t _pan_idx;: 偏航舵机功能索引。
- SRV_Channel::Aux_servo_function_t _open_idx;: 打开舵机功能索引。
- Vector3f _angle_bf_output_rad;: 最终的机身坐标系输出角度，单位为弧度。

3. 重要方法

3.1 AP_Mount_Servo::init

该初始化例程在AP_Mount::init中调用。

AP_Mount_Servo::init()
├── Check if _instance == 0
│   ├── True
│   │   ├── Set _roll_idx to SRV_Channel::k_mount_roll
│   │   ├── Set _tilt_idx to SRV_Channel::k_mount_tilt
│   │   ├── Set _pan_idx to SRV_Channel::k_mount_pan
│   │   └── Set _open_idx to SRV_Channel::k_mount_open
│   └── False (this must be the 2nd mount)
│       ├── Set _roll_idx to SRV_Channel::k_mount2_roll
│       ├── Set _tilt_idx to SRV_Channel::k_mount2_tilt
│       ├── Set _pan_idx to SRV_Channel::k_mount2_pan
│       └── Set _open_idx to SRV_Channel::k_mount2_open
└── Call AP_Mount_Backend::init()

3.2 AP_Mount_Servo::update

该定时更新例程在AP_Mount::update中调用。

AP_Mount_Servo::update()
|
|-- set_rctargeting_on_rcinput_change()
|
|-- auto mount_mode = get_mode()
|
|-- switch (mount_mode)
|   |
|   |-- case MAV_MOUNT_MODE_RETRACT
|   |   |-- _angle_bf_output_rad = _params.retract_angles.get() * DEG_TO_RAD
|   |   |-- mnt_target.angle_rad.set(_angle_bf_output_rad, false)
|   |   |-- mnt_target.target_type = MountTargetType::ANGLE
|   |
|   |-- case MAV_MOUNT_MODE_NEUTRAL
|   |   |-- _angle_bf_output_rad = _params.neutral_angles.get() * DEG_TO_RAD
|   |   |-- mnt_target.angle_rad.set(_angle_bf_output_rad, false)
|   |   |-- mnt_target.target_type = MountTargetType::ANGLE
|   |
|   |-- case MAV_MOUNT_MODE_MAVLINK_TARGETING
|   |   |-- // Do nothing, targets are set elsewhere
|   |
|   |-- case MAV_MOUNT_MODE_RC_TARGETING
|   |   |-- MountTarget rc_target
|   |   |-- get_rc_target(mnt_target.target_type, rc_target)
|   |   |-- switch (mnt_target.target_type)
|   |       |-- case MountTargetType::ANGLE
|   |       |   |-- mnt_target.angle_rad = rc_target
|   |       |-- case MountTargetType::RATE
|   |           |-- mnt_target.rate_rads = rc_target
|   |
|   |-- case MAV_MOUNT_MODE_GPS_POINT
|   |   |-- if (get_angle_target_to_roi(mnt_target.angle_rad))
|   |       |-- mnt_target.target_type = MountTargetType::ANGLE
|   |
|   |-- case MAV_MOUNT_MODE_HOME_LOCATION
|   |   |-- if (get_angle_target_to_home(mnt_target.angle_rad))
|   |       |-- mnt_target.target_type = MountTargetType::ANGLE
|   |
|   |-- case MAV_MOUNT_MODE_SYSID_TARGET
|   |   |-- if (get_angle_target_to_sysid(mnt_target.angle_rad))
|   |       |-- mnt_target.target_type = MountTargetType::ANGLE
|   |
|   |-- default
|       |-- // Do nothing
|
|-- switch (mnt_target.target_type)
|   |
|   |-- case MountTargetType::RATE
|   |   |-- update_angle_target_from_rate(mnt_target.rate_rads, mnt_target.angle_rad)
|   |   |-- FALLTHROUGH
|   |
|   |-- case MountTargetType::ANGLE
|       |-- if ((mount_mode != MAV_MOUNT_MODE_RETRACT) & (mount_mode != MAV_MOUNT_MODE_NEUTRAL))
|           |-- update_angle_outputs(mnt_target.angle_rad)
|
|-- const bool mount_open = (mount_mode == MAV_MOUNT_MODE_RETRACT) ? 0 : 1
|-- move_servo(_open_idx, mount_open, 0, 1)
|
|-- move_servo(_roll_idx, degrees(_angle_bf_output_rad.x)*10, _params.roll_angle_min*10, _params.roll_angle_max*10)
|-- move_servo(_tilt_idx, degrees(_angle_bf_output_rad.y)*10, _params.pitch_angle_min*10, _params.pitch_angle_max*10)
|-- move_servo(_pan_idx,  degrees(_angle_bf_output_rad.z)*10, _params.yaw_angle_min*10, _params.yaw_angle_max*10)

3.3 AP_Mount_Servo::has_roll_control

是否分配roll控制的RC辅助通道。

// returns true if this mount can control its roll
bool AP_Mount_Servo::has_roll_control() const
{
    return SRV_Channels::function_assigned(_roll_idx) && roll_range_valid();
}

3.4 AP_Mount_Servo::has_pitch_control

是否分配pitch控制的RC辅助通道。

bool AP_Mount_Servo::has_pitch_control() const
{
    return SRV_Channels::function_assigned(_tilt_idx) && pitch_range_valid();
}

3.5 AP_Mount_Servo::has_pan_control

是否分配yaw控制的RC辅助通道。

// returns true if this mount can control its pan (required for multicopters)
bool AP_Mount_Servo::has_pan_control() const
{
    return SRV_Channels::function_assigned(_pan_idx) && yaw_range_valid();
}

4. 辅助函数

4.1 AP_Mount_Servo::update_angle_outputs

根据飞机姿态动态调整摄像头角度。

update_angle_outputs
├── 定义并初始化变量
│   ├── ahrs = AP::ahrs()
│   ├── yaw_bf_rad = constrain_float(angle_rad.get_bf_yaw(), radians(_params.yaw_angle_min), radians(_params.yaw_angle_max))
│   └── _angle_bf_output_rad = {angle_rad.roll, angle_rad.pitch, yaw_bf_rad}
├── 检查是否需要稳定化
│   ├── if (!requires_stabilization)
│   │   └── return
├── 获取姿态角度
│   ├── ahrs_angle_rad = {ahrs.get_roll(), ahrs.get_pitch()}
│   ├── 检查是否有pan控制
│   │   └── if (has_pan_control())
│   │       └── ahrs_angle_rad.rotate(-yaw_bf_rad)
├── 添加姿态角度修正
│   ├── _angle_bf_output_rad.x -= ahrs_angle_rad.x
│   ├── _angle_bf_output_rad.y -= ahrs_angle_rad.y
├── 计算导引滤波
│   ├── 获取陀螺仪数据
│   │   └── gyro = ahrs.get_gyro()
│   ├── 计算roll角度变化率
│   │   └── if (!is_zero(_params.roll_stb_lead) && fabsf(ahrs.get_pitch()) < M_PI/3.0f)
│   │       └── roll_rate = gyro.x + (ahrs.sin_pitch() / ahrs.cos_pitch()) * (gyro.y * ahrs.sin_roll() + gyro.z * ahrs.cos_roll())
│   │       └── _angle_bf_output_rad.x -= roll_rate * _params.roll_stb_lead
│   ├── 计算pitch角度变化率
│   │   └── if (!is_zero(_params.pitch_stb_lead))
│   │       └── pitch_rate = ahrs.cos_pitch() * gyro.y - ahrs.sin_roll() * gyro.z
│   │       └── _angle_bf_output_rad.y -= pitch_rate * _params.pitch_stb_lead

4.2 AP_Mount_Servo::move_servo

根据角度计算PWM值，控制伺服器运动(前提就是PWM与角度关系是线性的)。

// move_servo - moves servo with the given id to the specified angle.  all angles are in degrees * 10
void AP_Mount_Servo::move_servo(uint8_t function_idx, int16_t angle, int16_t angle_min, int16_t angle_max)
{
	SRV_Channels::move_servo((SRV_Channel::Aux_servo_function_t)function_idx, angle, angle_min, angle_max);
}

4.3 AP_Mount_Servo::get_attitude_quaternion

获取四元数组。

// get attitude as a quaternion.  returns true on success
bool AP_Mount_Servo::get_attitude_quaternion(Quaternion& att_quat)
{
    // No feedback from gimbal so simply report demanded servo angles (which is
    // not the same as target angles).
    float roll_rad = 0.0f;
    float pitch_rad = 0.0f;
    float yaw_rad = 0.0f;
    if (has_roll_control()) {
        roll_rad = constrain_float(_angle_bf_output_rad.x, radians(_params.roll_angle_min), radians(_params.roll_angle_max));
    }
    if (has_pitch_control()) {
        pitch_rad = constrain_float(_angle_bf_output_rad.y, radians(_params.pitch_angle_min), radians(_params.pitch_angle_max));
    }
    if (has_pan_control()) {
        yaw_rad = constrain_float(_angle_bf_output_rad.z, radians(_params.yaw_angle_min), radians(_params.yaw_angle_max));
    }

    // convert to quaternion
    att_quat.from_euler(roll_rad, pitch_rad, yaw_rad);
    return true;
}

4. 总结

AP_Mount_Servo是通过PWM控制舵机，从而调整云台角度。该云台设备驱动类是最基本或者说最为原始的，电子件最少得云台控制程序。

大部分某宝上的都是这类云台。

在这里插入图片描述

5. 参考资料

【1】ArduPilot开源飞控系统之简单介绍
【2】ArduPilot之开源代码Task介绍
【3】ArduPilot飞控启动&运行过程简介
【4】ArduPilot之开源代码Library&Sketches设计
【5】ArduPilot之开源代码Sensor Drivers设计
【6】ArduPilot开源飞控之AP_Mount
【7】ArduPilot开源飞控之AP_Mount_Backend