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;
                 }
             }
         }
      }

    •