Ardupilot(PX4)飛控驅動蜂鳴器和RGB細節
阿新 • • 發佈：2018-09-26
initial 控制 text ict notify ble default 參數 neither
飛控代碼細節
-
任務調用頻率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;
-
-