PX4飞控之PWM输出控制

PX4飞控之PWM输出控制

多旋翼电调如好盈XRotor，DJI通用电调等都支持PWM信号来传输控制信号。常用的400Hz电调信号对应周期2500us，一般使用高电平时间1000us~2000us为有效信号区间，即1000us为最低输出，2000us为最高输出，锁定（停转）信号一般取900us（或950us，太低电调会判断成无信号，太高容易误判成有输出而意外启动）。一般电调也都支持有效信区间设置，可以在典型值附近自定义。
为了保证安全，电调上电后需要先检测到锁定信号，即900us左右的高电平时间，上电后检测到锁定信号电调会长叫一声（这个声音通过电机发出来的，电动机跟扬声器原理上很像，接上声音信号放放音乐也是毫无压力的==），从锁定信号到最低输出信号（1000us）需要有加速过程（即分多次慢慢增加到1000us），加速完成后，进入开启模式，这时就可以在有效信号区间内直接输出给定值。如果电调上电后飞控直接输出有效信号或从锁定信号直接跳变到有效信号都会导致电调进入报警模式（急促“嘀嘀嘀”报警声），而且控制器的输出信号区间（如-1~1）也需要量化到电调的有效区间，因此需要有一个电调控制程序来启动电调和量化有效信号。分享下PX4的电调控制程序。

Firmware\src\modules\systemlib\pwm_limit\pwm_limit.c

---

• pwm_limit中使用状态机来标志电调控制的不同阶段

enum {
    PWM_LIMIT_STATE_INIT = 0,  // 启动时初始化状态，等待一定时间后自动切换到锁定状态
    PWM_LIMIT_STATE_OFF,       // 锁定状态
    PWM_LIMIT_STATE_RAMP,      // 加速状态
    PWM_LIMIT_STATE_ON         // 开启状态，此时可以在有效区间内给任意控制信号
};

• 初始化：将state切到初始化状态，重置解锁时间

void pwm_limit_init(pwm_limit_t *limit)
{
    // 初始化状态机，重置解锁时间
    limit->state = PWM_LIMIT_STATE_INIT;
    limit->time_armed = 0;
    return;
}

• 状态机切换

    switch (limit->state) {
    case PWM_LIMIT_STATE_INIT:

        if (armed) {

            /* set arming time for the first call */
            if (limit->time_armed == 0) {
                limit->time_armed = hrt_absolute_time();
            }

            // 开机等待50ms后再进入锁定状态，在锁定状态下才能进入其他状态
            if (hrt_elapsed_time(&limit->time_armed) >= INIT_TIME_US) {
                limit->state = PWM_LIMIT_STATE_OFF;
            }
        }

        break;

    case PWM_LIMIT_STATE_OFF:
        if (armed) {
            // 解锁后进入加速状态
            limit->state = PWM_LIMIT_STATE_RAMP;

            /* reset arming time, used for ramp timing */
            // 记录下开始加速的时间
            limit->time_armed = hrt_absolute_time();
        }

        break;

    case PWM_LIMIT_STATE_RAMP:
        if (!armed) {
            // 加速状态中检测到飞机锁定(disarm)，则切换回锁定状态
            limit->state = PWM_LIMIT_STATE_OFF;

        } else if (hrt_elapsed_time(&limit->time_armed) >= RAMP_TIME_US) {
            // 加速时间到达500ms后，切换到开启状态
            limit->state = PWM_LIMIT_STATE_ON;
        }

        break;

    case PWM_LIMIT_STATE_ON:
        if (!armed) {
            // 飞机锁定，切换到锁定状态
            limit->state = PWM_LIMIT_STATE_OFF;
        }

        break;

    default:
        break;
    }

• 不同状态下的输出值

  /* then set effective_pwm based on state */
  switch (local_limit_state) {
  case PWM_LIMIT_STATE_OFF:
  case PWM_LIMIT_STATE_INIT:
      // 锁定状态和初始化状态下，所有通道都输出锁定的PWM值(900us高电平时间)
      for (unsigned i = 0; i < num_channels; i++) {
          effective_pwm[i] = disarmed_pwm[i];
      }
  
      break;
  
  case PWM_LIMIT_STATE_RAMP: {
          // 计算进入加速状态到现在的时间
          hrt_abstime diff = hrt_elapsed_time(&limit->time_armed);
  
          // 转换成百分比(整型计算，先放大PROGRESS_INT_SCALING倍，后面再除回来)
          progress = diff * PROGRESS_INT_SCALING / RAMP_TIME_US;
  
          if (progress > PROGRESS_INT_SCALING) {
              progress = PROGRESS_INT_SCALING;
          }
  
          for (unsigned i = 0; i < num_channels; i++) {
  
              float control_value = output[i];
  
              /* check for invalid / disabled channels */
              if (!isfinite(control_value)) {
                  effective_pwm[i] = disarmed_pwm[i];
                  continue;
              }
  
              uint16_t ramp_min_pwm;
  
              /* if a disarmed pwm value was set, blend between disarmed and min */
              if (disarmed_pwm[i] > 0) {
  
                  /* safeguard against overflows */
                  unsigned disarmed = disarmed_pwm[i];
  
                  if (disarmed > min_pwm[i]) {
                      // 预防参数设置错误导致锁定PWM值大于最低PWM
                      disarmed = min_pwm[i];
                  }
  
                  // 计算锁定PWM值和有效输出的最低PWM值之间的差值
                  unsigned disarmed_min_diff = min_pwm[i] - disarmed;
                  // 计算当前最小值
                  ramp_min_pwm = disarmed + (disarmed_min_diff * progress) / PROGRESS_INT_SCALING;
  
              } else {
  
                  /* no disarmed pwm value set, choose min pwm */
                  ramp_min_pwm = min_pwm[i];
              }
  
              if (reverse_mask & (1 << i)) {
                  control_value = -1.0f * control_value;
              }
  
              // 量化控制器输出
              effective_pwm[i] = control_value * (max_pwm[i] - ramp_min_pwm) / 2 + (max_pwm[i] + ramp_min_pwm) / 2;
  
              /* last line of defense against invalid inputs */
              if (effective_pwm[i] < ramp_min_pwm) {
                  effective_pwm[i] = ramp_min_pwm;
  
              } else if (effective_pwm[i] > max_pwm[i]) {
                  effective_pwm[i] = max_pwm[i];
              }
          }
      }
      break;
  
  case PWM_LIMIT_STATE_ON:
  
      for (unsigned i = 0; i < num_channels; i++) {
  
          float control_value = output[i];
  
          /* check for invalid / disabled channels */
          if (!isfinite(control_value)) {
              effective_pwm[i] = disarmed_pwm[i];
              continue;
          }
  
          if (reverse_mask & (1 << i)) {
              control_value = -1.0f * control_value;
          }
  
          // 开启状态，直接量化控制器输出到有效区间
          effective_pwm[i] = control_value * (max_pwm[i] - min_pwm[i]) / 2 + (max_pwm[i] + min_pwm[i]) / 2;
  
          /* last line of defense against invalid inputs */
          if (effective_pwm[i] < min_pwm[i]) {
              effective_pwm[i] = min_pwm[i];
  
          } else if (effective_pwm[i] > max_pwm[i]) {
              effective_pwm[i] = max_pwm[i];
          }
  
      }
  
      break;
  
  default:
      break;
  }