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;
}
}
}
}
Ardupilot(PX4)飞控驱动蜂鸣器和RGB细节的更多相关文章
- PX4/PixHawk无人机飞控应用开发
最近做的一个国防背景的field UAV项目,细节不能多谈,简单写点技术体会. 1.PX4/Pixhawk飞控软件架构简介 PX4是目前最流行的开源飞控板之一.PX4的软件系统实际上就是一个firmw ...
- 【30集iCore3_ADP出厂源代码(ARM部分)讲解视频】30-3 底层驱动之LED_蜂鸣器
视频简介: 该视频介绍iCore3应用开发平台出厂源代码中GPIO的配置方法 及如何点亮LED和驱动蜂鸣器发声. 源视频包下载地址: http://pan.baidu.com/s/1nvpYMff ...
- Android深度探索--HAL与驱动开发----第八章读书笔记
通过蜂鸣器的实现原理,实现一个完整的蜂呜器驱动,可以打开和关闭蜂鸣器. PWM驱动的实现方式不同于LED驱动, PWM 驱动将由多个文件组成.这也是大多数 Linux 驱动的标准实现方式． 刚开始是L ...
- 2440裸机驱动之PWM开发
原文http://blog.chinaunix.net/uid-14114479-id-3125685.html ARM驱动蜂鸣器的方式有两种:一种是PWM输出口直接驱动,另一种是利用IO定时翻转电平 ...
- STM32开发指南-蜂鸣器实验
另一种I/O作为输出的应用,利用一个I/O来控制板载的有源蜂鸣器,实现蜂鸣器控制. PS:有源蜂鸣器自带了震荡电路,一通电就会发声:无源蜂鸣器则没有自带震荡电路,必须外部提供2~5Khz左右的方波驱动 ...
- 基于ARM-contexA9-蜂鸣器pwm驱动开发
上次,我们写过一个蜂鸣器叫的程序,但是那个程序仅仅只是驱动蜂鸣器,用电平1和0来驱动而已,跟驱动LED其实没什么两样.我们先来回顾一下蜂鸣器的硬件还有相关的寄存器吧: 还是和以前一样的步骤: 1.看电 ...
- Linux ALSA声卡驱动之一：ALSA架构简介
声明:本博内容均由http://blog.csdn.net/droidphone原创,转载请注明出处,谢谢! 一. 概述 ALSA是Advanced Linux Sound Architecture ...
- linux学习--字符设备驱动
目录 1.字符设备驱动抽象结构 2.设备号及设备节点 2.1 设备号分配与管理 2.2 设备节点的生成 3.打开设备文件 linux驱动有基本的接口进行注册和卸载,这里不再做详细说明,本文主要关注li ...
- Linux ALSA声卡驱动之一：ALS…
声明:本博内容均由http://blog.csdn.net/droidphone原创,转载请注明出处,谢谢! 一. 概述 ALSA是Advanced Linux Sound Architecture ...
随机推荐
- 在WPF（core版本）中引用外部字体不可用问题说明
这几天使用WPF写软件,想引用外部字体,于是下载了字体文件: 然后在App.xaml中添加了如下代码: <FontFamily x:Key="Digital-7 Mono"& ...
- 7-46 jmu-python-求单词长度 (10 分)
输入n个单词,计算每个单词长度.对单词长度排序,分行输出单词长度及其单词. 输入格式: 行1:单词个数n 分行输入n个单词 输出格式: 分行输出单词长度及其单词.(单词长度,单词)用元组表示 输入样例 ...
- MVC07
1. 讲解ASP.net MVC的I/O操作 新建一个控制台程序,输入代码如下 using System; using System.IO; namespace IO { class Program ...
- 微信h5页面audio标签在ios下不能自动播放
背景介绍:在一个h5页面中,当用户提交表单到后台,后台返回的结果成功的话,开始自动播放背景音乐 出现的问题:在安卓手机上正常,iOS中没有反应 后来网上一番搜索后了解到时因为iOS不允许自动播放音乐, ...
- 全面认识HBase架构（建议收藏）
在网上看过很多HBaes架构相关的文章,内容深浅不一,直到发现了一篇MapR官网的文章https://mapr.com/blog/in-depth-look-hbase-architecture/#. ...
- Ubuntu系统下环境安装遇到依赖冲突问题
问题场景:在ubuntu系统下使用docker拉了一个python3.6的镜像,要在该容器中安装vim结果总是报已安装某些依赖的版本不满足要求 解决方法: 1.安装aptitude apt-get i ...
- docker 学习之路 将docker容器变为镜像并上传
环境 ubunt 16.4 去hub.docker.com上注册一个账号,并在账号中注册一个公有public或者私有仓库private 步骤如下 如上图 点击该处进入创建docker库页面 除了名字之 ...
- PG归并排序算法详解
前言 归并排序算法是连接算法中比较复杂的算法,相比嵌套循环与Hash匹配而言.本节会通过实例来说明该算法在PG中的具体实现. 在PG中,通过状态机来实现--归并-连接.当然这里的完整流程是排序--归并 ...
- js中 navigator 对象
Navigator 对象包含有关浏览器的信息. 很多时候我们需要在判断网页所处的浏览器和平台,Navigator为我们提供了便利 Navigator常见的对象属性如下: 属性 描述 appCodeNa ...
- kafka原理解析
两张图读懂kafka应用: Kafka 中的术语 broker:中间的kafka cluster,存储消息,是由多个server组成的集群. topic:kafka给消息提供的分类方式.broker用 ...