Ardupilot(PX4)飞控驱动蜂鸣器和RGB细节

Ardupilot(PX4)飞控驱动蜂鸣器细节飞控代码细节
  • 任务调用频率50HZ(20ms),buzzer.update()

  • 内部将频率减少到10HZ(100ms)

  • 单响(SINGLE_BUZZ) 响1次,100ms

    解锁事件错误,电池故障提醒,其中加锁只响一次,其他两种会一直间断响

  • 双响(DOUBLE_BUZZ) 100ms间断响两次

    vehicle_lost间断响,找飞机时使用

  • 解锁(ARMING_BUZZ) 响声 3S

    解锁常响3S一次

  • BARO_GLITCH,100ms间断响5次(飞控代码暂时没用到这部分)

  • EKF_BAD,四个音调越来越短 :ekf_bad错误

    300ms on >> 100ms off >> 200ms on >> 100ms off >>100ms on >>100ms off >>100ms on >>100ms off

    注意: ARM代表解锁,DISARM加锁

  • 代码如下void Buzzer::update()
    {
      // return immediately if disabled
      if (!AP_Notify::flags.external_leds) {
          return;
      }

      // check for arming failed event
      if (AP_Notify::events.arming_failed) {
          // arming failed buzz
          play_pattern(SINGLE_BUZZ);
      }

      // reduce 50hz call down to 10hz
      _counter++;
      if (_counter < 5) {
          return;
      }
      _counter = 0;

      // complete currently played pattern
      if (_pattern != NONE) {
          _pattern_counter++;
          switch (_pattern) {
              case SINGLE_BUZZ:
                  // buzz for 10th of a second
                  if (_pattern_counter == 1) {
                      on(true);
                  }else{
                      on(false);
                      _pattern = NONE;
                  }
                  return;
              case DOUBLE_BUZZ:
                  // buzz for 10th of a second
                  switch (_pattern_counter) {
                      case 1:
                          on(true);
                          break;
                      case 2:
                          on(false);
                          break;
                      case 3:
                          on(true);
                          break;
                      case 4:
                      default:
                          on(false);
                          _pattern = NONE;
                          break;
                  }
                  return;
              case ARMING_BUZZ:
                  // record start time
                  if (_pattern_counter == 1) {
                      _arming_buzz_start_ms = AP_HAL::millis();
                      on(true);
                  } else {
                      // turn off buzzer after 3 seconds
                      if (AP_HAL::millis() - _arming_buzz_start_ms >= BUZZER_ARMING_BUZZ_MS) {
                          _arming_buzz_start_ms = 0;
                          on(false);
                          _pattern = NONE;
                      }
                  }
                  return;
              case BARO_GLITCH:
                  // four fast tones
                  switch (_pattern_counter) {
                      case 1:
                      case 3:
                      case 5:
                      case 7:
                      case 9:
                          on(true);
                          break;
                      case 2:
                      case 4:
                      case 6:
                      case 8:
                          on(false);
                          break;
                      case 10:
                          on(false);
                          _pattern = NONE;
                          break;
                      default:
                          // do nothing
                          break;
                  }
                  return;
              case EKF_BAD:
                  // four tones getting shorter)
                  switch (_pattern_counter) {
                      case 1:
                      case 5:
                      case 8:
                      case 10:
                          on(true);
                          break;
                      case 4:
                      case 7:
                      case 9:
                          on(false);
                          break;
                      case 11:
                          on(false);
                          _pattern = NONE;
                          break;
                      default:
                          // do nothing
                          break;
                  }
                  return;
              default:
                  // do nothing
                  break;
          }
      }

      // check if armed status has changed
      if (_flags.armed != AP_Notify::flags.armed) {
          _flags.armed = AP_Notify::flags.armed;
          if (_flags.armed) {
              // double buzz when armed
              play_pattern(ARMING_BUZZ);
          }else{
              // single buzz when disarmed
              play_pattern(SINGLE_BUZZ);
          }
          return;
      }

      // check ekf bad
      if (_flags.ekf_bad != AP_Notify::flags.ekf_bad) {
          _flags.ekf_bad = AP_Notify::flags.ekf_bad;
          if (_flags.ekf_bad) {
              // ekf bad warning buzz
              play_pattern(EKF_BAD);
          }
          return;
      }

      // if vehicle lost was enabled, starting beep
      if (AP_Notify::flags.vehicle_lost) {
          play_pattern(DOUBLE_BUZZ);
      }

      // if battery failsafe constantly single buzz
      if (AP_Notify::flags.failsafe_battery) {
          play_pattern(SINGLE_BUZZ);
      }
    }
    从mavlink协议里面获取这些标志位
  • 加锁解锁状态的获取:

    心跳包,条件base_mode&MAV_MODE_FLAG_SAFETY_ARMED==MAV_MODE_FLAG_SAFETY_ARMED,如果为真,则在加锁状态,如果为假,则在解锁状态。

  • EKF_BAD

    飞控代码条件

    if (!ekf_position_ok() && !optflow_position_ok()) return true;

    return compass_variance >= g.fs_ekf_thresh && vel_variance >= g.fs_ekf_thresh;

    上述条件返回真,说明EKF_BAD

    g.fs_ekf_thresh为用于设置参数, compass_variance 和vel_variance 可通过mavlink 协议中的MAVLINK_MSG_ID_EKF_STATUS_REPORT消息获得。以下代码程序中的判断条件可以从该消息中获取。

    bool Copter::ekf_position_ok()
    {
      if (!ahrs.have_inertial_nav()) {
          // do not allow navigation with dcm position
          return false;
      }

      // with EKF use filter status and ekf check
      nav_filter_status filt_status = inertial_nav.get_filter_status();

      // if disarmed we accept a predicted horizontal position
      if (!motors->armed()) {
          return ((filt_status.flags.horiz_pos_abs || filt_status.flags.pred_horiz_pos_abs));
      } else {
          // once armed we require a good absolute position and EKF must not be in const_pos_mode
          return (filt_status.flags.horiz_pos_abs && !filt_status.flags.const_pos_mode);
      }
    }

    // optflow_position_ok - returns true if optical flow based position estimate is ok
    bool Copter::optflow_position_ok()
    {
    #if OPTFLOW != ENABLED && VISUAL_ODOMETRY_ENABLED != ENABLED
      return false;
    #else
      // return immediately if EKF not used
      if (!ahrs.have_inertial_nav()) {
          return false;
      }

      // return immediately if neither optflow nor visual odometry is enabled
      bool enabled = false;
    #if OPTFLOW == ENABLED
      if (optflow.enabled()) {
          enabled = true;
      }
    #endif
    #if VISUAL_ODOMETRY_ENABLED == ENABLED
      if (g2.visual_odom.enabled()) {
          enabled = true;
      }
    #endif
      if (!enabled) {
          return false;
      }

      // get filter status from EKF
      nav_filter_status filt_status = inertial_nav.get_filter_status();

      // if disarmed we accept a predicted horizontal relative position
      if (!motors->armed()) {
          return (filt_status.flags.pred_horiz_pos_rel);
      } else {
          return (filt_status.flags.horiz_pos_rel && !filt_status.flags.const_pos_mode);
      }
    #endif
    }
  •   vehicle_lost,飞行器丢失,找飞机使用// check for pilot stick input to trigger lost vehicle alarm
    void Copter::lost_vehicle_check()
    {
      static uint8_t soundalarm_counter;

      // disable if aux switch is setup to vehicle alarm as the two could interfere
      if (check_if_auxsw_mode_used(AUXSW_LOST_COPTER_SOUND)) {
          return;
      }

      // ensure throttle is down, motors not armed, pitch and roll rc at max. Note: rc1=roll rc2=pitch
      if (ap.throttle_zero && !motors->armed() && (channel_roll->get_control_in() > 4000) && (channel_pitch->get_control_in() > 4000)) {
          if (soundalarm_counter >= LOST_VEHICLE_DELAY) {
              if (AP_Notify::flags.vehicle_lost == false) {
                  AP_Notify::flags.vehicle_lost = true;
                  gcs_send_text(MAV_SEVERITY_NOTICE,"Locate Copter alarm");
              }
          } else {
              soundalarm_counter++;
          }
      } else {
          soundalarm_counter = 0;
          if (AP_Notify::flags.vehicle_lost == true) {
              AP_Notify::flags.vehicle_lost = false;
          }
      }
    }
    Ardupilot(PX4)飞控驱动RGB全彩灯细节IO控制LED逻辑
  • 闪烁时间: 快闪分配255,中等时间204,慢闪时间102

  • 50HZ的频率调用rgbled.update_colours()函数,内部降频为10HZ

  • 判断usb连接采用中等时间闪烁,为usb供电解压

  • LED闪烁分10小步骤

  • 闪烁逻辑

    • 系统初始化,奇数次亮红灯,偶数次亮蓝灯,返回

    • 成功保存电调校准和遥控器校准,0-3-6亮红灯,1-4-7亮蓝灯,2-5-8亮绿灯,9-灭灯,返回

    • 电台故障,电源报警,ekf_bad,GPS_glitch(GPS误差大),leak_detected,返回

      • 0-1-2-3-4亮黄灯

      • 5-6-7-8-9

        • leak_detected,全亮紫色

        • ekf_bad,红灯

        • gps_glitch,蓝灯

        • 电台和电源故障,灭灯

    • 加锁状态

      • GPS固定类型大于GPS_OK_FIX_3D,亮绿灯,否则亮蓝灯

    • 解锁状态

      • 解锁校验失败,0-1-4-5,黄灯(红灯和绿灯亮),2-3-6-7-8-9,全灭

    • 解锁校验成功

      • 快闪绿灯条件

        AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D_DGPS && AP_Notify::flags.pre_arm_gps_check;

        0-上述条件成立,开绿灯

        1-上述条件成立,关绿灯

        2-上述条件成立,开绿灯

        3-上述条件成立,关绿灯

        4-关红灯,上述条件成立,关蓝灯,亮绿灯,不成立亮蓝灯,关绿灯

        5-上述条件成立,关绿灯

        6-上述条件成立,亮绿灯

        7-上述条件成立,关绿灯

        8-条件成立,关绿灯

        9-全关

        代码如下
        // _scheduled_update - updates _red, _green, _blue according to notify flags
        void RGBLed::update_colours(void)
        {
          uint8_t brightness = _led_bright;

          switch (pNotify->_rgb_led_brightness) {
          case RGB_LED_OFF:
              brightness = _led_off;
              break;
          case RGB_LED_LOW:
              brightness = _led_dim;
              break;
          case RGB_LED_MEDIUM:
              brightness = _led_medium;
              break;
          case RGB_LED_HIGH:
              brightness = _led_bright;
              break;
          }

          // slow rate from 50Hz to 10hz
          counter++;
          if (counter < 5) {
              return;
          }

          // reset counter
          counter = 0;

          // move forward one step
          step++;
          if (step >= 10) {
              step = 0;
          }

          // use dim light when connected through USB
          if (hal.gpio->usb_connected() && brightness > _led_dim) {
              brightness = _led_dim;
          }

          // initialising pattern
          if (AP_Notify::flags.initialising) {
              if (step & 1) {
                  // odd steps display red light
                  _red_des = brightness;
                  _blue_des = _led_off;
                  _green_des = _led_off;
              } else {
                  // even display blue light
                  _red_des = _led_off;
                  _blue_des = brightness;
                  _green_des = _led_off;
              }

              // exit so no other status modify this pattern
              return;
          }
           
          // save trim and esc calibration pattern
          if (AP_Notify::flags.save_trim || AP_Notify::flags.esc_calibration) {
              switch(step) {
                  case 0:
                  case 3:
                  case 6:
                      // red on
                      _red_des = brightness;
                      _blue_des = _led_off;
                      _green_des = _led_off;
                      break;

                  case 1:
                  case 4:
                  case 7:
                      // blue on
                      _red_des = _led_off;
                      _blue_des = brightness;
                      _green_des = _led_off;
                      break;

                  case 2:
                  case 5:
                  case 8:
                      // green on
                      _red_des = _led_off;
                      _blue_des = _led_off;
                      _green_des = brightness;
                      break;

                  case 9:
                      // all off
                      _red_des = _led_off;
                      _blue_des = _led_off;
                      _green_des = _led_off;
                      break;
              }
              // exit so no other status modify this pattern
              return;
          }

          // radio and battery failsafe patter: flash yellow
          // gps failsafe pattern : flashing yellow and blue
          // ekf_bad pattern : flashing yellow and red
          if (AP_Notify::flags.failsafe_radio || AP_Notify::flags.failsafe_battery ||
                  AP_Notify::flags.ekf_bad || AP_Notify::flags.gps_glitching || AP_Notify::flags.leak_detected) {
              switch(step) {
                  case 0:
                  case 1:
                  case 2:
                  case 3:
                  case 4:
                      // yellow on
                      _red_des = brightness;
                      _blue_des = _led_off;
                      _green_des = brightness;
                      break;
                  case 5:
                  case 6:
                  case 7:
                  case 8:
                  case 9:
                      if (AP_Notify::flags.leak_detected) {
                          // purple if leak detected
                          _red_des = brightness;
                          _blue_des = brightness;
                          _green_des = brightness;
                      } else if (AP_Notify::flags.ekf_bad) {
                          // red on if ekf bad
                          _red_des = brightness;
                          _blue_des = _led_off;
                          _green_des = _led_off;
                      } else if (AP_Notify::flags.gps_glitching) {
                          // blue on gps glitch
                          _red_des = _led_off;
                          _blue_des = brightness;
                          _green_des = _led_off;
                      }else{
                          // all off for radio or battery failsafe
                          _red_des = _led_off;
                          _blue_des = _led_off;
                          _green_des = _led_off;
                      }
                      break;
              }
              // exit so no other status modify this pattern
              return;
          }

          // solid green or blue if armed
          if (AP_Notify::flags.armed) {
              // solid green if armed with GPS 3d lock
              if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D) {
                  _red_des = _led_off;
                  _blue_des = _led_off;
                  _green_des = brightness;
              }else{
                  // solid blue if armed with no GPS lock
                  _red_des = _led_off;
                  _blue_des = brightness;
                  _green_des = _led_off;
              }
              return;
          }else{
              // double flash yellow if failing pre-arm checks
              if (!AP_Notify::flags.pre_arm_check) {
                  switch(step) {
                      case 0:
                      case 1:
                      case 4:
                      case 5:
                          // yellow on
                          _red_des = brightness;
                          _blue_des = _led_off;
                          _green_des = brightness;
                          break;
                      case 2:
                      case 3:
                      case 6:
                      case 7:
                      case 8:
                      case 9:
                          // all off
                          _red_des = _led_off;
                          _blue_des = _led_off;
                          _green_des = _led_off;
                          break;
                  }
              }else{
                  // fast flashing green if disarmed with GPS 3D lock and DGPS
                  // slow flashing green if disarmed with GPS 3d lock (and no DGPS)
                  // flashing blue if disarmed with no gps lock or gps pre_arm checks have failed
                  bool fast_green = AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D_DGPS && AP_Notify::flags.pre_arm_gps_check;
                  switch(step) {
                      case 0:
                          if (fast_green) {
                              _green_des = brightness;
                          }
                          break;
                      case 1:
                          if (fast_green) {
                              _green_des = _led_off;
                          }
                          break;
                      case 2:
                          if (fast_green) {
                              _green_des = brightness;
                          }
                          break;
                      case 3:
                          if (fast_green) {
                              _green_des = _led_off;
                          }
                          break;
                      case 4:
                          _red_des = _led_off;
                          if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D && AP_Notify::flags.pre_arm_gps_check) {
                              // flashing green if disarmed with GPS 3d lock
                              _blue_des = _led_off;
                              _green_des = brightness;
                          }else{
                              // flashing blue if disarmed with no gps lock
                              _blue_des = brightness;
                              _green_des = _led_off;
                          }
                          break;
                      case 5:
                          if (fast_green) {
                              _green_des = _led_off;
                          }
                          break;

                      case 6:
                          if (fast_green) {
                              _green_des = brightness;
                          }
                          break;

                      case 7:
                          if (fast_green) {
                              _green_des = _led_off;
                          }
                          break;
                      case 8:
                          if (fast_green) {
                              _green_des = brightness;
                          }
                          break;
                      case 9:
                          // all off
                          _red_des = _led_off;
                          _blue_des = _led_off;
                          _green_des = _led_off;
                          break;
                  }
              }
          }
        }
      •  

  •  

    相关内容推荐