PWM转换成ANGLE

贾专家

已于 2023-12-13 11:42:35 修改

阅读量174

点赞数 1

分类专栏： APM 文章标签：学习 c++

于 2023-12-13 11:37:58 首次发布

本文链接：https://blog.csdn.net/weixin_44355245/article/details/134965410

版权

APM 专栏收录该内容

6 篇文章

订阅专栏

一、简介

本文章讲述的是自稳模式下，飞控接收到接收机的滚转、俯仰、油门和偏航的输入值后，如何将其转换成目标角度。
本代码为Ardupilot-4.3.7

二、自稳模式代码的输入信号部分

1、get_pilot_desired_lean_angles

在这里插入图片描述

get_pilot_desired_lean_angles函数是将输入量转换成目标滚转角和目标俯仰角，其中滚转通道和俯仰通道的输入范围是-4500~+4500，该函数具体的内容如下：

void Mode::get_pilot_desired_lean_angles(float &roll_out_cd, float &pitch_out_cd, float angle_max_cd, float angle_limit_cd) const
{
    // throttle failsafe check
    // 判断遥控器是否断连
    if (copter.failsafe.radio || !copter.ap.rc_receiver_present) {
        roll_out_cd = 0.0;
        pitch_out_cd = 0.0;
        return;
    }

    //transform pilot's normalised roll or pitch stick input into a roll and pitch euler angle command
    float roll_out_deg;
    float pitch_out_deg;
    // channel_roll->get_control_in()和channel_pitch->get_control_in()的范围是-4500~4500，ROLL_PITCH_YAW_INPUT_MAX设置为4500
    // 第一个和第二个参数的数值范围最终为-1~+1
    // angle_max_cd和angle_limit_cd默认值为3000，从地面站参数ANGLE_MAX可以知道			
    rc_input_to_roll_pitch(channel_roll->get_control_in()*(1.0/ROLL_PITCH_YAW_INPUT_MAX), channel_pitch->get_control_in()*(1.0/ROLL_PITCH_YAW_INPUT_MAX), angle_max_cd * 0.01,  angle_limit_cd * 0.01, roll_out_deg, pitch_out_deg);

    // Convert to centi-degrees
    // 将目标倾斜角的单位从度转换成弧度
    roll_out_cd = roll_out_deg * 100.0;
    pitch_out_cd = pitch_out_deg * 100.0;
}

可以看到，此函数关键的内容是rc_input_to_roll_pitch函数，此函数是真正的将滚转通道和俯仰通道的值转换成目标倾斜角。输入的第一个参数是滚转通道的数值/4500，输入的第二个参数是俯仰通道的数值/4500，第三个和第四个参数默认值为3000 * 0.01，roll_out_deg和pitch_out_deg是以度为单位输出的目标倾斜角，具体的函数体内容如下所示：

void rc_input_to_roll_pitch(float roll_in_unit, float pitch_in_unit, float angle_max_deg, float angle_limit_deg, float &roll_out_deg, float &pitch_out_deg)
{
	// 将最大角度进行限幅，最大不能超过85°，最大角度默认为30°
	// 并将最大角度angle_max_deg从度转换成弧度rc_2_rad 
    angle_max_deg = MIN(angle_max_deg, 85.0);
    float rc_2_rad = radians(angle_max_deg);

    // fetch roll and pitch stick positions and convert them to normalised horizontal thrust
    // pitch_in_unit输入值（范围-1~1）乘上最大弧度角rc_2_rad，目的将滚转和俯仰的位置归一到（-30~+30）中，并转换成推力向量
    Vector2f thrust;
    thrust.x = - tanf(rc_2_rad * pitch_in_unit);
    thrust.y = tanf(rc_2_rad * roll_in_unit);

	
    // calculate the horizontal thrust limit based on the angle limit
    // 得到限幅范围
    angle_limit_deg = constrain_float(angle_limit_deg, 10.0f, angle_max_deg);
    float thrust_limit = tanf(radians(angle_limit_deg)); 

    // apply horizontal thrust limit
    // 对滚转和俯仰进行限幅
    thrust.limit_length(thrust_limit);

    // Conversion from angular thrust vector to euler angles.
    // 将推力向量进行反正切处理，转换成弧度
    float pitch_rad = - atanf(thrust.x);
    float roll_rad = atanf(cosf(pitch_rad) * thrust.y); // ？？？？

    // Convert to degrees
    // 将弧度转换成角度
    roll_out_deg = degrees(roll_rad);
    pitch_out_deg = degrees(pitch_rad);
}