ArduSub程序学习（3）--控制RGB LED的颜色

仗剑走代码

于 2024-07-30 19:40:52 发布

阅读量456

点赞数 17

分类专栏： ardusub pixhawk ardupilot 文章标签：学习 c++ autopilot

本文链接：https://blog.csdn.net/weixin_49839886/article/details/140799943

版权

ardusub 同时被 3 个专栏收录

6 篇文章 0 订阅

订阅专栏

ardupilot

6 篇文章 0 订阅

订阅专栏

pixhawk

1 篇文章 0 订阅

订阅专栏

今天大家动手实践一下控制RGB LED的颜色，程序改起来比较简单，在更改程序之前先来学习一下有关这部分的程序，今天还是以sub4.1程序为例，网上有更改比较老版本的程序，应该是3.5版本的。关于控制RGB LED的颜色的程序主要在libraries/AP_Notify/RGBLed.cpp。

RGBLed.cpp

首先看第一个构造函数，这个函数是用来初始化LED的不同状态（关闭、亮、适中和暗）。这些值会被用来设定LED的亮度级别。例如_led_off(led_off)的作用是将传入构造函数的参数led_off的值赋给类的成员变量_led_off。

RGBLed::RGBLed(uint8_t led_off, uint8_t led_bright, uint8_t led_medium, uint8_t led_dim)
    : _led_off(led_off),//关闭
      _led_bright(led_bright),//亮
      _led_medium(led_medium),//适中
      _led_dim(led_dim)//暗
{
}

_set_rgb

_set_rgb检查输入的RGB值是否和当前设置的颜色不同，如果不同则调用hw_set_rgb（一个低层硬件更新函数）更新硬件，并更新当前颜色值。这样做避免了不必要的硬件调用。

void RGBLed::_set_rgb(uint8_t red, uint8_t green, uint8_t blue)
{
    if (red != _red_curr ||
        green != _green_curr ||
        blue != _blue_curr) {
        // call the hardware update routine
        if (hw_set_rgb(red, green, blue)) {
            _red_curr = red;
            _green_curr = green;
            _blue_curr = blue;
        }
    }
}

rgb_source

返回当前RGB LED的颜色来源类型，比如标准、MAVLink、交通灯模式等。pNotify是一个通知对象，包含了当前系统状态。

RGBLed::rgb_source_t RGBLed::rgb_source() const
{
    return rgb_source_t(pNotify->_rgb_led_override.get());
}

set_rgb

如果RGB LED的来源是MAVLink（意味着外部命令覆盖），则不执行颜色设置；否则调用_set_rgb设置颜色。

void RGBLed::set_rgb(uint8_t red, uint8_t green, uint8_t blue)
{
    if (rgb_source() == mavlink) {
        // don't set if in override mode
        return;
    }
    _set_rgb(red, green, blue);
}

get_brightness

get_brightness函数的作用是根据内部状态和外部条件（如是否通过USB连接），计算并返回当前应该设置的LED亮度值。它通过查阅pNotify对象中的亮度设置以及其他条件（如USB连接状态），来确定具体的亮度值。

定义了一个局部变量brightness，初始值设置为类成员变量_led_bright的值，这个变量代表最亮的亮度。

pNotify是一个指向某个对象的指针，该对象包含有关于LED亮度的信息。_rgb_led_brightness是这个对象的成员变量，表示当前LED的亮度设置。

uint8_t RGBLed::get_brightness(void) const
{
    uint8_t brightness = _led_bright;

    switch (pNotify->_rgb_led_brightness) {
    case RGB_LED_OFF://RGB_LED_OFF: 如果亮度设置为关闭状态(RGB_LED_OFF)，则将brightness设置为_led_off，通常表示完全关闭（亮度为0）。
        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;
    }

    // use dim light when connected through USB
    //这段代码处理一种特殊情况：如果设备通过USB连接（hal.gpio->usb_connected()返回true），且当前亮度高于_led_dim，则强制将亮度降低到_led_dim。这样做的目的是在USB连接时避免过高的亮度，可能是为了节省功耗或避免USB供电过载。
    if (hal.gpio->usb_connected() && brightness > _led_dim) {
        brightness = _led_dim;
    }
    return brightness;
}

get_colour_sequence_obc

返回OBC模式下的颜色序列，主要根据系统是否ARM（武装）状态设置为红色或绿色。

uint32_t RGBLed::get_colour_sequence_obc(void) const
{
    if (AP_Notify::flags.armed) {
        return DEFINE_COLOUR_SEQUENCE_SOLID(RED);
    }
    return DEFINE_COLOUR_SEQUENCE_SOLID(GREEN);
}

get_colour_sequence

get_colour_sequence函数通过读取不同的系统状态标志（如初始化、校准、故障、武装状态等），决定LED的颜色显示模式。每种模式对应不同的颜色序列，通常用于向用户提供关于系统状态的视觉反馈。

uint32_t RGBLed::get_colour_sequence(void) const
{
    // initialising pattern初始化模式
    if (AP_Notify::flags.initialising) {
        return sequence_initialising;
    }

    // save trim or any calibration pattern校准模式
    //检查是否有任何校准过程正在进行，如保存修正、ESC校准、指南针校准或温度校准。如果是，返回sequence_trim_or_esc，表示校准状态的颜色序列。
    if (AP_Notify::flags.save_trim ||
        AP_Notify::flags.esc_calibration ||
        AP_Notify::flags.compass_cal_running ||
        AP_Notify::flags.temp_cal_running) {
        return sequence_trim_or_esc;
    }

    // radio and battery failsafe pattern: 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_gcs ||
        AP_Notify::flags.failsafe_battery ||
        AP_Notify::flags.ekf_bad ||
        AP_Notify::flags.gps_glitching ||
        AP_Notify::flags.leak_detected) {

        if (AP_Notify::flags.leak_detected) {
            // purple if leak detected泄漏检测（leak_detected）：返回sequence_failsafe_leak（通常是紫色）。
            return sequence_failsafe_leak;
        } else if (AP_Notify::flags.ekf_bad) {
            // red on if ekf badEKF错误（ekf_bad）：返回sequence_failsafe_ekf（通常是红色）。
            return sequence_failsafe_ekf;
        } else if (AP_Notify::flags.gps_glitching) {
            // blue on gps glitch GPS故障（gps_glitching）：返回sequence_failsafe_gps_glitching（通常是蓝色）。
            return sequence_failsafe_gps_glitching;
        }
        // all off for radio or battery failsafe其他故障：返回sequence_failsafe_radio_or_battery（通常是黄色）。
        return sequence_failsafe_radio_or_battery;
    }

    // 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) {
            return sequence_armed;
        }
        // solid blue if armed with no GPS lock
        return sequence_armed_nogps;
    }

    // double flash yellow if failing pre-arm checks预启动检查失败
    if (!AP_Notify::flags.pre_arm_check) {
        return sequence_prearm_failing;
    }
    if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D_DGPS && AP_Notify::flags.pre_arm_gps_check) {
        return sequence_disarmed_good_dgps;
    }

    if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D && AP_Notify::flags.pre_arm_gps_check) {
        return sequence_disarmed_good_gps;
    }

    return sequence_disarmed_bad_gps;
}

get_colour_sequence_traffic_light

返回“交通灯”模式的颜色序列，用于指示初始状态、ARM状态和安全开关状态等。

uint32_t RGBLed::get_colour_sequence_traffic_light(void) const
{
    if (AP_Notify::flags.initialising) {
        return DEFINE_COLOUR_SEQUENCE(RED,GREEN,BLUE,RED,GREEN,BLUE,RED,GREEN,BLUE,BLACK);
    }

    if (AP_Notify::flags.armed) {
        return DEFINE_COLOUR_SEQUENCE_SLOW(RED);
    }

    if (hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED) {
        if (!AP_Notify::flags.pre_arm_check) {
            return DEFINE_COLOUR_SEQUENCE_ALTERNATE(YELLOW, BLACK);
        } else {
            return DEFINE_COLOUR_SEQUENCE_SLOW(YELLOW);
        }
    }

    if (!AP_Notify::flags.pre_arm_check) {
        return DEFINE_COLOUR_SEQUENCE_ALTERNATE(GREEN, BLACK);
    }
    return DEFINE_COLOUR_SEQUENCE_SLOW(GREEN);
}

update

RGBLed::update 函数的作用是根据当前的LED模式选择合适的颜色序列，然后根据时间步长和亮度设置LED的颜色。通过这种方式，可以实现多种不同的LED显示效果，例如根据系统状态显示不同的颜色，或者在交通信号灯模式下显示不同的颜色序列。比较重要的一段函数，我下面拆开讲讲。

void RGBLed::update()
{
    uint32_t current_colour_sequence = 0;

    switch (rgb_source()) {
    case mavlink:
        update_override();
        return; // note this is a return not a break!
    case standard:
        current_colour_sequence = get_colour_sequence();
        break;
    case obc:
        current_colour_sequence = get_colour_sequence_obc();
        break;
    case traffic_light:
        current_colour_sequence = get_colour_sequence_traffic_light();
        break;
    }

    const uint8_t brightness = get_brightness();

    uint8_t step = (AP_HAL::millis()/100) % 10;

    // ensure we can't skip a step even with awful timing
    if (step != last_step) {
        step = (last_step+1) % 10;
        last_step = step;
    }

    const uint8_t colour = (current_colour_sequence >> (step*3)) & 7;

    uint8_t red_des = (colour & RED) ? brightness : _led_off;
    uint8_t green_des = (colour & GREEN) ? brightness : _led_off;
    uint8_t blue_des = (colour & BLUE) ? brightness : _led_off;

    set_rgb(red_des, green_des, blue_des);
}

void RGBLed::update()

void: 函数不返回任何值。
RGBLed::: 这是 RGBLed 类的成员函数。
```
uint32_t current_colour_sequence = 0;
```
uint32_t: 32位无符号整型，用于存储当前颜色序列的编码。

根据LED模式选择颜色序列

switch (rgb_source()) {
case mavlink:
    update_override();
    return; // note this is a return not a break!
case standard:
    current_colour_sequence = get_colour_sequence();
    break;
case obc:
    current_colour_sequence = get_colour_sequence_obc();
    break;
case traffic_light:
    current_colour_sequence = get_colour_sequence_traffic_light();
    break;
}

switch (rgb_source()): 根据 rgb_source() 函数返回的LED模式来选择不同的更新逻辑。

mavlink: 如果当前模式是 MAVLink，那么调用 update_override() 函数更新LED状态，然后返回（终止函数执行），不继续后面的代码。
standard: 如果当前模式是标准模式，调用 get_colour_sequence() 函数获取颜色序列，并将其赋值给 current_colour_sequence。
obc: 如果当前模式是 OBC（机载计算机），调用 get_colour_sequence_obc() 函数获取颜色序列，并将其赋值给 current_colour_sequence。
traffic_light: 如果当前模式是交通信号灯模式，调用 get_colour_sequence_traffic_light() 函数获取颜色序列，并将其赋值给 current_colour_sequence。

获取亮度

const uint8_t brightness = get_brightness();

const uint8_t: 8位无符号整型，用于存储LED的亮度值。
get_brightness(): 函数调用返回当前的亮度值，根据系统状态（如是否通过USB连接）来调整亮度。

计算时间步长

uint8_t step = (AP_HAL::millis()/100) % 10;

uint8_t: 8位无符号整型，用于存储时间步长。
AP_HAL::millis(): 获取系统启动以来的时间（毫秒）。
/100: 将时间除以100，将毫秒转换为0.1秒的单位。
% 10: 对结果取模10，将步长限制在0到9之间，用于确定当前显示的颜色。

防止跳过步骤

if (step != last_step) {
    step = (last_step+1) % 10;
    last_step = step;
}

if (step != last_step): 如果当前步长与上一个步长不同。
step = (last_step+1) % 10: 计算下一个步长，并将其限制在0到9之间，防止跳过步骤。
last_step = step: 更新上一个步长为当前步长。

计算颜色

const uint8_t colour = (current_colour_sequence >> (step*3)) & 7;

const uint8_t: 8位无符号整型，用于存储当前颜色。
current_colour_sequence >> (step*3): 将颜色序列右移 (step*3) 位，以提取当前步长对应的颜色。每个颜色占3位（RGB），因此移动的位数为步长乘以3。
& 7: 将结果与7（00000111）进行按位与操作，提取出当前颜色的值。

更新LED颜色

uint8_t red_des = (colour & RED) ? brightness : _led_off;
uint8_t green_des = (colour & GREEN) ? brightness : _led_off;
uint8_t blue_des = (colour & BLUE) ? brightness : _led_off;

uint8_t red_des: 目标红色分量。如果 colour & RED 为真（非零），则设置为当前亮度 brightness，否则设置为 _led_off（LED关闭）。
uint8_t green_des: 目标绿色分量。逻辑与红色分量相同。
uint8_t blue_des: 目标蓝色分量。逻辑与红色分量相同。
```
set_rgb(red_des, green_des, blue_des);
```
set_rgb(): 调用该函数将计算出的目标红色、绿色和蓝色分量设置到LED上。

下面的 handle_led_control、update_override 和 rgb_control 函数大家自己理解理解吧，实在是太多了，我就总结一下吧。

handle_led_control: 处理来自 MAVLink 消息的 LED 控制命令，如果当前模式为 MAVLink 则解码并设置 LED 的控制参数。
update_override: 根据 rate_hz 和当前时间更新 LED 的状态，支持闪烁和固态显示。
rgb_control: 设置 LED 的 RGB 颜色和闪烁频率，用于脚本控制或外部指令。

更改代码

下面我更改程序为了实现控制RGB灯红蓝绿三种颜色依次点亮，并循环执行。

1.首先对 RGBLed 类的方法进行调整。

RGBLed::RGBLed(uint8_t led_off, uint8_t led_bright, uint8_t led_medium, uint8_t led_dim):
    _led_off(led_off),
    _led_bright(led_bright),
    _led_medium(led_medium),
    _led_dim(led_dim),
    _current_color(0) // 初始化当前颜色索引
{
}

我们还要在RGEled.h里面修改 RGBLed 类定义，确保它包括 _current_color 成员变量，并且所有相关方法都正确引用这个变量。大家在RGEled.h里面找到这部分，添加上uint8_t _current_color;

2.对update更改改变颜色控制

这部分直接把update替换为我下面提供的代码

void RGBLed::update()
{
    const uint8_t brightness = get_brightness();

    // 根据当前颜色索引设置RGB颜色
    switch (_current_color) {
    case 0: // Red
        set_rgb(brightness, _led_off, _led_off);
        break;
    case 1: // Green
        set_rgb(_led_off, brightness, _led_off);
        break;
    case 2: // Blue
        set_rgb(_led_off, _led_off, brightness);
        break;
    }

    // 更新当前颜色索引
    _current_color = (_current_color + 1) % 3;
}

// 其余的方法保持不变

在这个修改中：

在 RGBLed 类中添加了一个 _current_color 成员变量，用于跟踪当前的颜色索引。
修改了 update 方法，以便根据 _current_color 的值设置RGB灯的颜色，并在每次调用 update 时更新 _current_color，实现红、蓝、绿三色循环。

这样每次 update 方法被调用时，RGB LED 将依次切换红色、绿色和蓝色，并循环执行。你可以根据需要调整 update 方法的调用频率，以控制颜色切换的速度。

编译生成固件

rm -rf build
./waf configure --board fmuv3
./waf sub

完成后在ardupilot/build/fmuv3/bin找到ardusub.apj下载到pixhawk里即可。

效果展示

pixhawk控制RGBled等效果

仗剑走代码

关注

17
点赞
踩
16

收藏

觉得还不错? 一键收藏
打赏
0
评论
ArduSub程序学习（3）--控制RGB LED的颜色

今天大家动手实践一下控制RGB LED的颜色，程序改起来比较简单，在更改程序之前先来学习一下有关这部分的程序，今天还是以sub4.1程序为例，网上有更改比较老版本的程序，应该是3.5版本的。关于控制RGB LED的颜色的程序主要在libraries/AP_Notify/RGBLed.cpp。
复制链接

扫一扫