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 okbool 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 alarmvoid 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 flagsvoid 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; } } }}
- 快闪绿灯条件
原文地址：https://www.cnblogs.com/BlogsOfLei/p/9709608.html
时间： 2024-10-14 04:19:18