无人机飞控姿态控制代码

角度值计算对应的 PWM 脉冲

/**

• @brief 根据输入的角度值计算对应的 PWM 脉冲宽度
• 此函数将一个经过缩放的角度值转换为对应的 PWM 脉冲宽度。输入的角度值会根据舵机的最大角度范围进行约束，
• 并考虑舵机的反转设置。最终根据角度的正负，分别计算出对应的 PWM 值。
• @param scaled_value 经过缩放的角度值
• @return uint16_t 计算得到的 PWM 脉冲宽度，单位为微秒
  */
  uint16_t SRV_Channel::pwm_from_angle(float scaled_value) const
  {
  // 检查最大角度范围是否为 0，如果为 0 则直接返回舵机的中位 PWM 值
  if (high_out == 0) {
  return servo_trim;
  }
  // 如果舵机设置为反转，则将输入的角度值取反
  if (reversed) {
  scaled_value = -scaled_value;
  }
  // 将输入的角度值约束在 -high_out 到 high_out 的范围内
  scaled_value = constrain_float(scaled_value, -high_out, high_out);
  // 根据角度值的正负情况计算 PWM 脉冲宽度
  if (scaled_value > 0) {
  // 当角度值为正时，基于舵机中位值和最大值计算 PWM 脉冲宽度
  return servo_trim + uint16_t( (scaled_value * (float)(servo_max - servo_trim)) / (float)high_out);
  } else {
  // 当角度值为负时，基于舵机中位值和最小值计算 PWM