ArduPilot开源代码之AP_VideoTX

news2024/11/25 20:51:43

ArduPilot开源代码之AP_VideoTX

  • 1. 源由
  • 2. AP_VideoTX子模块
    • 2.1 AP_VideoTX
      • 2.1.1 AP_VideoTX::init
      • 2.1.1 AP_VideoTX::update
    • 2.2 AP_Tramp
      • 2.2.1 AP_Tramp::init
      • 2.2.2 AP_Tramp::update
      • 2.2.3 AP_Tramp::process_requests
    • 2.3 AP_SmartAudio
      • 2.3.1 AP_SmartAudio::init
      • 2.3.2 AP_SmartAudio::loop
      • 2.3.3 AP_SmartAudio::read_response
      • 2.3.4 AP_SmartAudio::parse_response_buffer
  • 3. 设计问题
    • 3.1 逻辑疑点
    • 3.2 Tramp图传应用问题
  • 4. 参考资料

1. 源由

模拟图传系统是FPV早期作为视频影像的一个重要手段。虽然目前逐步的被高清数字图传所替代,但是模拟图传作为高性价比,尤其是作为经济实惠的产品,为大众所喜爱。

结合ArduPilot系统,模拟图传部分抽象为AP_VideoTX模块,这里就这个展开一些简单的功能和代码介绍,以便更加理解在实际应用过程中需要注意的一些问题。

2. AP_VideoTX子模块

AP_VideoTX子模块总体来看可以分为三大部分:

  1. AP_VideoTX:整合的模拟图传业务子系统
  2. AP_Tramp: ImmersionRC Tramp协议
  3. AP_SmartAudio:TBS (Team Blacksheep) Smart Audio协议,大致有V1/V2/V21三个协议版本

子模块的启动从AP_Vehicle::setup中调用

AP_Vehicle::setup
 ├──> <AP_VIDEOTX_ENABLED> AP_VideoTX::init
 ├──> <AP_SMARTAUDIO_ENABLED> AP_SmartAudio::init
 └──> <AP_TRAMP_ENABLED> AP_Tramp::init

子模块任务涉及:AP_VideoTX::update/AP_Tramp::update

const AP_Scheduler::Task AP_Vehicle::scheduler_tasks[] = {
    ... ... 
#if AP_VIDEOTX_ENABLED
    SCHED_TASK_CLASS(AP_VideoTX,   &vehicle.vtx,            update,                    2, 100, 220),
#endif
#if AP_TRAMP_ENABLED
    SCHED_TASK_CLASS(AP_Tramp,     &vehicle.tramp,          update,                   50,  50, 225),
#endif
    ... ... 
};

注:这里AP_SmartAudio在init过程中会创建线程来处理类似AP_Tramp::update的工作。

2.1 AP_VideoTX

2.1.1 AP_VideoTX::init

初始化模拟图传系统的关键参数:

  • _power_mw:功率(mW), 功率表索引(_current_power)
  • _current_band:频段(A/B/E/F/R)
  • _current_channel:频道(1 ~ 8)
  • _current_frequency:只读,目前代码写死
  • _current_options:模拟图传硬件特性,比如Pit Mode等
bool AP_VideoTX::init(void)
{
    if (_initialized) {
        return false;
    }

    // PARAMETER_CONVERSION - Added: Sept-2022
    _options.convert_parameter_width(AP_PARAM_INT16);

    // find the index into the power table //选择表格中功率值或者向下最接近的功率值
    for (uint8_t i = 0; i < VTX_MAX_POWER_LEVELS; i++) {
        if (_power_mw <= _power_levels[i].mw) {
            if (_power_mw != _power_levels[i].mw) {
                if (i > 0) {
                    _current_power = i - 1;
                }
                _power_mw.set_and_save(get_power_mw());
            } else {
                _current_power = i;
            }
            break;
        }
    }
    _current_band = _band;
    _current_channel = _channel;
    _current_frequency = _frequency_mhz;
    _current_options = _options;
    _current_enabled = _enabled;
    _initialized = true;

    return true;
}

2.1.1 AP_VideoTX::update

VTX业务子模块,一直对功率数据/特性数据的一致性进行检查。

void AP_VideoTX::update(void)
{
    if (!_enabled) {
        return;
    }

#if HAL_CRSF_TELEM_ENABLED  //这个是用于VTX将信息通过CRSF模块的电传回遥控器
    AP_CRSF_Telem* crsf = AP::crsf_telem();

    if (crsf != nullptr) {
        crsf->update();
    }
#endif
    // manipulate pitmode if pitmode-on-disarm or power-on-arm is set
    if (has_option(VideoOptions::VTX_PITMODE_ON_DISARM) || has_option(VideoOptions::VTX_PITMODE_UNTIL_ARM)) {
        if (hal.util->get_soft_armed() && has_option(VideoOptions::VTX_PITMODE)) {
            _options.set(_options & ~uint8_t(VideoOptions::VTX_PITMODE));
        } else if (!hal.util->get_soft_armed() && !has_option(VideoOptions::VTX_PITMODE)
            && has_option(VideoOptions::VTX_PITMODE_ON_DISARM)) {
            _options.set(_options | uint8_t(VideoOptions::VTX_PITMODE));
        }
    }
    // check that the requested power is actually allowed // 检查VTX功率数据一致性,如果不一致,则使用上一次有效功率数据
    // reset if not
    if (_power_mw != get_power_mw()) {
        if (_power_levels[find_current_power()].active == PowerActive::Inactive) {
            // reset to something we know works
            debug("power reset to %dmw from %dmw", get_power_mw(), _power_mw.get());
            _power_mw.set_and_save(get_power_mw());
        }
    }
}

2.2 AP_Tramp

2.2.1 AP_Tramp::init

Tramp协议在AP_Vehicle有一个任务会定期执行,因此协议层面仅打开串口端口。

bool AP_Tramp::init(void)
{
    if (AP::vtx().get_enabled() == 0) {
        debug("protocol is not active");
        return false;
    }

    // init uart
    port = AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_Tramp, 0);
    if (port != nullptr) {
        port->configure_parity(0);
        port->set_stop_bits(1);
        port->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE);
        port->set_options((port->get_options() & ~AP_HAL::UARTDriver::OPTION_RXINV));

        port->begin(AP_TRAMP_UART_BAUD, AP_TRAMP_UART_BUFSIZE_RX, AP_TRAMP_UART_BUFSIZE_TX);
        debug("port opened");

        return true;
    }
    return false;
}

2.2.2 AP_Tramp::update

类似驱动本地数据有效性的检查和状态同步,同时调用串口数据处理流程

void AP_Tramp::update()
{
    if (port == nullptr) {  //端口无效时,无需进行协议方面的解析
        return;
    }

    AP_VideoTX& vtx = AP::vtx();

    //当有数据需要更新时,进行更新,并设置更新尝试的最大次数VTX_TRAMP_MAX_RETRIES
    if (vtx.have_params_changed() && retry_count == 0) {
        // check changes in the order they will be processed
        if (vtx.update_frequency() || vtx.update_band() || vtx.update_channel()) {
            if (vtx.update_frequency()) {
                vtx.update_configured_channel_and_band();
            } else {
                vtx.update_configured_frequency();
            }
            set_frequency(vtx.get_configured_frequency_mhz());
        }
        else if (vtx.update_power()) {
            retry_count = VTX_TRAMP_MAX_RETRIES;
        }
        else if (vtx.update_options()) {
            retry_count = VTX_TRAMP_MAX_RETRIES;
        }
    }

    //串口数据处理流程
    process_requests();
}

2.2.3 AP_Tramp::process_requests

通过TrampStatus状态机的方式进行处理:

  • TRAMP_STATUS_OFFLINE
  • TRAMP_STATUS_INIT
  • TRAMP_STATUS_ONLINE_MONITOR_FREQPWRPIT
  • TRAMP_STATUS_ONLINE_MONITOR_TEMP
  • TRAMP_STATUS_ONLINE_CONFIG

在这里插入图片描述

void AP_Tramp::process_requests()
{
    if (port == nullptr) {
        return;
    }

    bool configUpdateRequired = false;

    // Read response from device //收到一包完整的Tramp协议报文
    const char replyCode = receive_response();
    const uint32_t now = AP_HAL::micros();

#ifdef TRAMP_DEBUG
    if (replyCode != 0) {
        debug("receive response '%c'", replyCode);
    }
#endif

    // Act on state
    switch (status) {
    case TrampStatus::TRAMP_STATUS_OFFLINE: {
        // Offline, check for response
        if (replyCode == 'r') {
            // Device replied to reset? request, enter init
            set_status(TrampStatus::TRAMP_STATUS_INIT);
        } else if ((now - last_time_us) >= TRAMP_MIN_REQUEST_PERIOD_US) {
            // Min request period exceeded, issue another reset?
            send_query('r');

            // Update last time
            last_time_us = now;
        }
        break;
    }
    case TrampStatus::TRAMP_STATUS_INIT: {
        // Initializing, check for response
        if (replyCode == 'v') {
            // Device replied to freq / power / pit query, enter online
            set_status(TrampStatus::TRAMP_STATUS_ONLINE_MONITOR_FREQPWRPIT);
        } else if ((now - last_time_us) >= TRAMP_MIN_REQUEST_PERIOD_US) {
            // Min request period exceeded, issue another query
            send_query('v');

            // Update last time
            last_time_us = now;
        }
        break;
    }
    case TrampStatus::TRAMP_STATUS_ONLINE_MONITOR_FREQPWRPIT: {
        // Note after config a status update request is made, a new status
        // request is made, this request is handled above and should prevent
        // subsequent config updates if the config is now correct
        if (retry_count > 0 && ((now - last_time_us) >= TRAMP_MIN_REQUEST_PERIOD_US)) {
            AP_VideoTX& vtx = AP::vtx();
            // Config retries remain and min request period exceeded, check freq
            if (!is_race_lock_enabled() && vtx.update_frequency()) {
                // Freq can be and needs to be updated, issue request
                send_command('F', vtx.get_configured_frequency_mhz());

                // Set flag
                configUpdateRequired = true;
            } else if (!is_race_lock_enabled() && vtx.update_power()) {
                // Power can be and needs to be updated, issue request
                send_command('P', vtx.get_configured_power_mw());

                // Set flag
                configUpdateRequired = true;
            } else if (vtx.update_options()) {
                // Pit mode needs to be updated, issue request
                send_command('I', vtx.has_option(AP_VideoTX::VideoOptions::VTX_PITMODE) ? 0 : 1);

                // Set flag
                configUpdateRequired = true;
            }

            if (configUpdateRequired) {
                // Update required, decrement retry count
                retry_count--;

                // Update last time
                last_time_us = now;

                // Advance state
                set_status(TrampStatus::TRAMP_STATUS_ONLINE_CONFIG);
            } else {
                // No update required, reset retry count
                retry_count = 0;
            }
        }

        /* Was a config update made? */
        if (!configUpdateRequired) {
            /* No, look to continue monitoring */
            if ((now - last_time_us) >= TRAMP_STATUS_REQUEST_PERIOD_US) {
                // Request period exceeded, issue freq/power/pit query
                send_query('v');

                // Update last time
                last_time_us = now;
            } else if (replyCode == 'v') {
                // Got reply, issue temp query
                send_query('s');

                // Wait for reply
                set_status(TrampStatus::TRAMP_STATUS_ONLINE_MONITOR_TEMP);

                // Update last time
                last_time_us = now;
            }
        }

        break;
    }
    case TrampStatus::TRAMP_STATUS_ONLINE_MONITOR_TEMP: {
        // Check request time
        if (replyCode == 's') {
            // Got reply, return to request freq/power/pit
            set_status(TrampStatus::TRAMP_STATUS_ONLINE_MONITOR_TEMP);
        } else if ((now - last_time_us) >= TRAMP_MIN_REQUEST_PERIOD_US) {
            // Timed out after min request period, return to request freq/power/pit query
            set_status(TrampStatus::TRAMP_STATUS_ONLINE_MONITOR_FREQPWRPIT);
        }
        break;
    }
    case TrampStatus::TRAMP_STATUS_ONLINE_CONFIG: {
        // Param should now be set, check time
        if ((now - last_time_us) >= TRAMP_MIN_REQUEST_PERIOD_US) {
            // Min request period exceeded, re-query
            send_query('v');

            // Advance state
            set_status(TrampStatus::TRAMP_STATUS_ONLINE_MONITOR_FREQPWRPIT);

            // Update last time
            last_time_us = now;
        }
        break;
    }
    default:
        // Invalid state, reset
        set_status(TrampStatus::TRAMP_STATUS_OFFLINE);
        break;
    }
}

2.3 AP_SmartAudio

2.3.1 AP_SmartAudio::init

SmartAudio串口协议除了端口初始化,还开了一个线程。该做法与Tramp协议不太一样。如能统一,从设计的角度看就比较整齐美观了,当然历史问题导致了当前的设计,能用就是最好的。

bool AP_SmartAudio::init()
{
    debug("SmartAudio init");

    if (AP::vtx().get_enabled()==0) {
        debug("SmartAudio protocol it's not active");
        return false;
    }

    // init uart
    _port = AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_SmartAudio, 0);
    if (_port!=nullptr) {
        _port->configure_parity(0);
        _port->set_stop_bits(AP::vtx().has_option(AP_VideoTX::VideoOptions::VTX_SA_ONE_STOP_BIT) ? 1 : 2);
        _port->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE);
        _port->set_options((_port->get_options() & ~AP_HAL::UARTDriver::OPTION_RXINV)
            | AP_HAL::UARTDriver::OPTION_HDPLEX | AP_HAL::UARTDriver::OPTION_PULLDOWN_TX | AP_HAL::UARTDriver::OPTION_PULLDOWN_RX);
        if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_SmartAudio::loop, void),
                                          "SmartAudio",
                                          768, AP_HAL::Scheduler::PRIORITY_IO, -1)) {
            return false;
        }

        return true;
    }
    return false;
}

2.3.2 AP_SmartAudio::loop

飞控与VTX图传上电时间可能存在不同步问题,所以

loop依次步骤:

  1. 发送SMARTAUDIO_CMD_GET_SETTINGS命令
  2. read_response/parse_response_buffer对反馈报文解析
  3. 如果超时调整波特率设置,以避免时序上的不同步问题(重置状态)
  4. 有参数变化,准备并发送SMARTAUDIO_CMD_SET_FREQUENCY/SMARTAUDIO_CMD_SET_CHANNEL/SMARTAUDIO_CMD_SET_POWER/SMARTAUDIO_CMD_SET_MODE命令
void AP_SmartAudio::loop()
{
    AP_VideoTX &vtx = AP::vtx();

    while (!hal.scheduler->is_system_initialized()) {
        hal.scheduler->delay(100);
    }

    // allocate response buffer
    uint8_t _response_buffer[AP_SMARTAUDIO_MAX_PACKET_SIZE];

    // initialise uart (this must be called from within tick b/c the UART begin must be called from the same thread as it is used from)
    _port->begin(_smartbaud, AP_SMARTAUDIO_UART_BUFSIZE_RX, AP_SMARTAUDIO_UART_BUFSIZE_TX);


    while (true) {
        // now time to control loop switching
        uint32_t now = AP_HAL::millis();

        // when pending request and last request sended is timeout, take another packet to send
        if (!_is_waiting_response) {
            // command to process
            Packet current_command;

            // repeatedly initialize UART until we know what the VTX is
            if (!_initialised) {
                // request settings every second
                if (requests_queue.is_empty() && !hal.util->get_soft_armed() && now - _last_request_sent_ms > 1000) {
                    request_settings();
                }
            }

            if (requests_queue.pop(current_command)) {
                // send the popped command from bugger
                send_request(current_command.frame, current_command.frame_size);

                now = AP_HAL::millis();
                // it takes roughly 15ms to send a request, don't turn around and try and read until
                // this time has elapsed
                hal.scheduler->delay(20);

                _last_request_sent_ms = now;

                // next loop we expect a response
                _is_waiting_response = true;
            }
        }

        // nothing going on so give CPU to someone else
        if (!_is_waiting_response || !_initialised) {
            hal.scheduler->delay(100);
        }

        // On my Unify Pro32 the SmartAudio response is sent exactly 100ms after the request
        // and the initial response is 40ms long so we should wait at least 140ms before giving up
        if (now - _last_request_sent_ms < 200 && _is_waiting_response) {

            // setup scheduler delay to 50 ms again after response processes
            if (!read_response(_response_buffer)) {
                hal.scheduler->delay(10);
            } else {
                // successful response, wait another 100ms to give the VTX a chance to recover
                // before sending another command. This is needed on the Atlatl v1.
                hal.scheduler->delay(100);
            }

        } else if (_is_waiting_response) { // timeout
            // process autobaud routine
            update_baud_rate();
            _port->discard_input();
            _inline_buffer_length = 0;
            _is_waiting_response = false;
            debug("response timeout");
        } else if (_initialised) {
            if (AP::vtx().have_params_changed() ||_vtx_power_change_pending
                || _vtx_freq_change_pending || _vtx_options_change_pending) {
                update_vtx_params();
                set_configuration_pending(true);
                vtx.set_configuration_finished(false);
                // we've tried to update something, re-request the settings so that they
                // are reflected correctly
                request_settings();
            } else if (is_configuration_pending()) {
                AP::vtx().announce_vtx_settings();
                set_configuration_pending(false);
                vtx.set_configuration_finished(true);
            }
        }
    }
}

2.3.3 AP_SmartAudio::read_response

读取串口报文,并按照报文封装进行校验解析;将合法报文内容送parse_response_buffer进行进一步业务解析。

bool AP_SmartAudio::read_response(uint8_t *response_buffer)
{
    int16_t incoming_bytes_count = _port->available();

    const uint8_t response_header_size= sizeof(FrameHeader);

    // check if it is a response in the wire
    if (incoming_bytes_count <= 0) {
        return false;
    }

    // wait until we have enough bytes to read a header
    if (incoming_bytes_count < response_header_size && _inline_buffer_length == 0) {
        return false;
    }

    // now have at least the header, read it if necessary
    if (_inline_buffer_length == 0) {
        uint8_t b = _port->read();
        // didn't see a sync byte, discard and go around again
        if (b != SMARTAUDIO_SYNC_BYTE) {
            return false;
        }
        response_buffer[_inline_buffer_length++] = b;

        b = _port->read();
        // didn't see a header byte, discard and reset
        if (b != SMARTAUDIO_HEADER_BYTE) {
            _inline_buffer_length = 0;
            return false;
        }

        response_buffer[_inline_buffer_length++] = b;

        // read the rest of the header
        for (; _inline_buffer_length < response_header_size; _inline_buffer_length++) {
            b = _port->read();
            response_buffer[_inline_buffer_length] = b;
        }

        FrameHeader* header = (FrameHeader*)response_buffer;
        incoming_bytes_count -= response_header_size;

        // implementations that ignore the CRC also appear to not account for it in the frame length
        if (ignore_crc()) {
            header->length++;
        }
        _packet_size = header->length;
    }

    // read the rest of the packet
    for (uint8_t i= 0; i < incoming_bytes_count && _inline_buffer_length < _packet_size + response_header_size; i++) {
        uint8_t response_in_bytes = _port->read();

        // check for overflow
        if (_inline_buffer_length >= AP_SMARTAUDIO_MAX_PACKET_SIZE) {
            _inline_buffer_length = 0;
            _is_waiting_response = false;
            return false;
        }

        response_buffer[_inline_buffer_length++] = response_in_bytes;
    }

    // didn't get the whole packet
    if (_inline_buffer_length < _packet_size + response_header_size) {
        return false;
    }

#ifdef SA_DEBUG
    print_bytes_to_hex_string("read_response():", response_buffer, _inline_buffer_length);
#endif
    _is_waiting_response = false;

    bool correct_parse = parse_response_buffer(response_buffer);
    response_buffer = nullptr;
    _inline_buffer_length=0;
    _packet_size = 0;
    _packets_rcvd++;
    // reset the lost packets to 0
    _packets_sent =_packets_rcvd;
    return correct_parse;
}

2.3.4 AP_SmartAudio::parse_response_buffer

握手报文协议解析

  • SMARTAUDIO_RSP_GET_SETTINGS_V1
  • SMARTAUDIO_RSP_GET_SETTINGS_V2
  • SMARTAUDIO_RSP_GET_SETTINGS_V21

SMARTAUDIO_CMD_SET_FREQUENCY/SMARTAUDIO_CMD_SET_CHANNEL/SMARTAUDIO_CMD_SET_POWER/SMARTAUDIO_CMD_SET_MODE命令反馈报文核对

  • SMARTAUDIO_RSP_SET_FREQUENCY
  • SMARTAUDIO_RSP_SET_CHANNEL
  • SMARTAUDIO_RSP_SET_POWER
  • SMARTAUDIO_RSP_SET_MODE
bool  AP_SmartAudio::parse_response_buffer(const uint8_t *buffer)
{
    const FrameHeader *header = (const FrameHeader *)buffer;
    const uint8_t fullFrameLength = sizeof(FrameHeader) + header->length;
    const uint8_t headerPayloadLength = fullFrameLength - 1; // subtract crc byte from length
    const uint8_t *startPtr = buffer + 2;
    const uint8_t *endPtr = buffer + headerPayloadLength;

    if ((crc8_dvb_s2_update(0x00, startPtr, headerPayloadLength-2)!=*(endPtr) && !ignore_crc())
        || header->headerByte != SMARTAUDIO_HEADER_BYTE
        || header->syncByte != SMARTAUDIO_SYNC_BYTE) {
        debug("parse_response_buffer() failed - invalid CRC or header");
        return false;
    }
    // SEND TO GCS A MESSAGE TO UNDERSTAND WHATS HAPPENING
    AP_VideoTX& vtx = AP::vtx();
    Settings settings {};

    switch (header->command) {
    case SMARTAUDIO_RSP_GET_SETTINGS_V1:
        _protocol_version = SMARTAUDIO_SPEC_PROTOCOL_v1;
        unpack_settings(&settings, (const SettingsResponseFrame *)buffer);
        settings.version = SMARTAUDIO_SPEC_PROTOCOL_v1;
        print_settings(&settings);
        update_vtx_settings(settings);
        break;

    case SMARTAUDIO_RSP_GET_SETTINGS_V2:
        _protocol_version = SMARTAUDIO_SPEC_PROTOCOL_v2;
        unpack_settings(&settings, (const SettingsResponseFrame *)buffer);
        settings.version = SMARTAUDIO_SPEC_PROTOCOL_v2;
        print_settings(&settings);
        update_vtx_settings(settings);
        break;

    case SMARTAUDIO_RSP_GET_SETTINGS_V21:
        _protocol_version = SMARTAUDIO_SPEC_PROTOCOL_v21;
        unpack_settings(&settings, (const SettingsExtendedResponseFrame *)buffer);
        settings.version = SMARTAUDIO_SPEC_PROTOCOL_v21;
        print_settings(&settings);
        update_vtx_settings(settings);
        break;

    case SMARTAUDIO_RSP_SET_FREQUENCY: {
        const U16ResponseFrame *resp = (const U16ResponseFrame *)buffer;
        unpack_frequency(&settings, resp->payload);
        vtx.set_frequency_mhz(settings.frequency);
        vtx.set_configured_frequency_mhz(vtx.get_frequency_mhz());
        vtx.update_configured_channel_and_band();
        debug("Frequency was set to %d", settings.frequency);
    }
        break;

    case SMARTAUDIO_RSP_SET_CHANNEL: {
        const U8ResponseFrame *resp = (const U8ResponseFrame *)buffer;
        vtx.set_band(resp->payload / VTX_MAX_CHANNELS);
        vtx.set_channel(resp->payload % VTX_MAX_CHANNELS);
        vtx.set_configured_channel(vtx.get_channel());
        vtx.set_configured_band(vtx.get_band());
        vtx.update_configured_frequency();
        debug("Channel was set to %d", resp->payload);
    }
        break;

    case SMARTAUDIO_RSP_SET_POWER: {
        const U16ResponseFrame *resp = (const U16ResponseFrame *)buffer;
        const uint8_t power = resp->payload & 0xFF;
        switch (_protocol_version) {
        case SMARTAUDIO_SPEC_PROTOCOL_v21:
            if (vtx.get_configured_power_dbm() != power) {
                vtx.update_power_dbm(vtx.get_configured_power_dbm(), AP_VideoTX::PowerActive::Inactive);
            }
            vtx.set_power_dbm(power);
            vtx.set_configured_power_mw(vtx.get_power_mw());
            break;
        case SMARTAUDIO_SPEC_PROTOCOL_v2:
            if (vtx.get_configured_power_level() != power) {
                vtx.update_power_dbm(vtx.get_configured_power_dbm(), AP_VideoTX::PowerActive::Inactive);
            }
            vtx.set_power_level(power);
            vtx.set_configured_power_mw(vtx.get_power_mw());
            break;
        default:
            if (vtx.get_configured_power_dac() != power) {
                vtx.update_power_dbm(vtx.get_configured_power_dbm(), AP_VideoTX::PowerActive::Inactive);
            }
            vtx.set_power_dac(power);
            vtx.set_configured_power_mw(vtx.get_power_mw());
            break;
        }
        debug("Power was set to %d", power);
    }
        break;

    case SMARTAUDIO_RSP_SET_MODE: {
        vtx.set_options(vtx.get_configured_options()); // easiest to just make them match
        debug("Mode was set to 0x%x", buffer[4]);
    }
        break;

    default:
        return false;
    }
    return true;
}

3. 设计问题

3.1 逻辑疑点

  1. SmartAudio和Tramp协议目前是公用一张PowerLevel的表格;

  2. 该表格采用了SmartAudio规范;
    在这里插入图片描述

  3. SmartAudio规范并不能涵盖当前市场上VTX产品所有的功率范围;

3.2 Tramp图传应用问题

由于逻辑上的疑点,最终导致两个问题点:

注:详细讨论可见【14】【15】

  1. 支持Tramp协议的模块无法对SmartAudio规范外的其他特殊功率值进行配置。

比如:PandaRC VT5804ML1 600mW图传功率设置就不再当前代码支持范围。

在这里插入图片描述

  1. 当前代码在6段波段开关选择过程中与上述PowerLevel表格并不一致,在Tramp协议使用时会导致严重的偏差。

在这里插入图片描述

4. 参考资料

【1】ArduPilot开源飞控系统之简单介绍
【2】ArduPilot之开源代码框架
【3】ArduPilot飞控之ubuntu22.04-SITL安装
【4】ArduPilot飞控之ubuntu22.04-Gazebo模拟
【5】ArduPilot飞控之Mission Planner模拟
【6】ArduPilot飞控AOCODARC-H7DUAL固件编译
【7】ArduPilot之开源代码Library&Sketches设计
【8】ArduPilot之开源代码Sensor Drivers设计
【9】ArduPilot之开源代码基础知识&Threading概念
【10】ArduPilot之开源代码UARTs and the Console使用
【11】ArduPilot飞控启动&运行过程简介
【11】ArduPilot之开源代码Task介绍
【12】ArduPilot开源代码之AP_Param
【13】ArduPilot开源代码之AP_Scheduler
【14】How to set ImmersionRC Tramp VTX configuration?
【15】Why analog VTX doesn’t support 600mW power level?

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.coloradmin.cn/o/647326.html

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈,一经查实,立即删除!

相关文章

requests库的使用

文章目录 get 请求post 请求get 请求和 post 请求的区别response1. res.headers2. status_code3. json get 请求 参数类型作用urlstr发起请求的地址params字典url为基准地址&#xff0c;不包含查询参数&#xff1b;使用此参数会自动对 params 字典编码&#xff0c;然后和url拼…

【Spring】@PropertySource 配置文件自定义加密、自定义Yaml文件加载

文章目录 前言参考目录实现步骤1、包结构2、Maven3、自定义配置文件4、application 文件5、自定义数据库配置 MyDataSource6、加密配置 EncryptYamlProperties7、自定义读取yaml配置 MyPropertySourceFactory8、测试加密解密9、自定义 Properties 文件读取10、测试自定义配置读…

走近科学之《JVM 的秘密》

JVM JVM、内存模型、类加载机制、对象的创建、垃圾回收机制、对象内存分配策略、JVM调优等。 1 简介 JVM 即 java 虚拟机&#xff08;Java Virtual Machine&#xff09;&#xff0c;JVM是一种用于计算设备的规范&#xff0c;是一个虚构出来的计算机。是通过在实际计算机上仿真…

msvcr120.dll丢失的解决方法-一键解决提示msvcr120.dll丢失问题

msvcr120.dll是的动态链接库文件之一。它在Windows操作系统中发挥着重要的作用&#xff0c;它提供了应用程序所需的各种功能和方法。 该文件返回编译后的代码所需的支持库。msvcr120.dll包含用于C / C编译器生成的应用程序所需的重要功能&#xff0c;包括数学函数&#xff0c;…

Selenium定位不到元素怎么办?一定要这么做

目录 1、frame/iframe表单嵌套 selenium自动化测试视频教程&#xff1a; 2、页面跳转到新标签页&#xff0c;或弹出警告框等 3、页面元素失去焦点导致脚本运行不稳定 4、使用Xpath或CSS定位 6、元素被遮挡&#xff0c;不可用&#xff0c;不可见 2023最新的Selenium自动化…

linux部署rabbitmq开启mqtt插件由于监听1883端口导致重启rabbitmq失败的解决方法

linux部署rabbitmq开启mqtt插件由于监听1883端口导致重启rabbitmq失败的解决方法 第一步&#xff1a;部署rabbitmq 部署rabbitmq请移步&#xff08;在这里可以找到erlang和rabbitmq适配的版本并下载安装包&#xff09;&#xff1a; https://blog.csdn.net/char1otte/article/de…

mysql:索引原理与慢查询优化

一 索引的原理 1. 索引原理 索引的目的在于提高查询效率&#xff0c;与我们查阅图书所用的目录是一个道理&#xff1a;先定位到章&#xff0c;然后定位到该章下的一个小节&#xff0c;然后找到页数。相似的例子还有&#xff1a;查字典&#xff0c;查火车车次&#xff0c;飞机…

【Shiro】第一章 权限概述

目录 1、什么是权限 2、认证概念 3、授权概念​​​​​​​ 1、什么是权限 权限管理&#xff0c;一般指根据系统设置的安全策略或者安全规则&#xff0c;用户可以访问而且只能访问自己被授权的资源&#xff0c;不多不少。权限管理几乎出现在任何系统里面&#xff0c;只要…

玩转ChatGPT:R代码Debug一例

一、写在前面 今天家里领导发来求助&#xff0c;说是用GPT-3.5写一个 计算mRNA干性指数 的R代码&#xff0c;运行报错。让我用GPT-4帮忙Debug一哈。 搞了半小时&#xff0c;还是有亿点感悟&#xff0c;写段文字记录记录。 二、踩坑过程 &#xff08;1&#xff09;先看原始的…

渗透测试报告怎么写?记得收藏好哦

目录 1、准备好渗透测试记录 2、撰写渗透测试报告书 报告书的撰写建议 1、重点 2、图表重于文字 3、结果与建议 总结&#xff1a; 1、准备好渗透测试记录 测试记录是执行过程的日志&#xff0c;在每日测试工作结束后&#xff0c;应将当日的成果做成记录&#xff0c;虽然…

【Spring学习之更简单的读取和存储Bean对象】属性注入,set注入,构造方法注入

前言&#xff1a; &#x1f49e;&#x1f49e;今天我们依然是学习Spring&#xff0c;这里我们会更加了解Spring的知识&#xff0c;知道Spring是怎么更加简单的读取和存储Bean对象的。也会让大家对Spring更加了解。 &#x1f49f;&#x1f49f;前路漫漫&#xff0c;希望大家坚持…

现在的00后,卷死了呀....

都说00后躺平了&#xff0c;但是有一说一&#xff0c;该卷的还是卷。这不&#xff0c;三月份春招我们公司来了个00后&#xff0c;工作没两年&#xff0c;跳槽到我们公司起薪23K&#xff0c;都快接近我了。 后来才知道人家是个卷王&#xff0c;从早干到晚就差搬张床到工位睡觉了…

数据科学之数据可视化——Tableau可视化气泡图

大家好&#xff0c;我是大鹏&#xff0c;今天给大家分享一个新的一个知识“气泡图”。 气泡图就是用气泡的大小和颜色表示不同的数据。

【RabbitMQ教程】第三章 —— RabbitMQ - 发布确认

&#x1f4a7; 【 R a b b i t M Q 教程】第三章—— R a b b i t M Q − 发布确认 \color{#FF1493}{【RabbitMQ教程】第三章 —— RabbitMQ - 发布确认} 【RabbitMQ教程】第三章——RabbitMQ−发布确认&#x1f4a7; &#x1f337; 仰望天空&#xff0c;妳我亦是行人…

青大数据结构【2020】【三分析计算】

关键字&#xff1a; 无相连通图、Prim算法最小生成树、哈希函数、线性探测法、平均查找长度 1.对于一个带权连通无向图G&#xff0c;可以采用Prim算法构造出从某个顶点v出发的最小生成树&#xff0c;问该最小生成树是否一定包含从顶点v到其他所有顶点的最短路径。如果回答是&a…

docker 网络理论知识点 - CNM 和命名空间

Network 目录 1 network namespace1.1 动手小实验 2 回到 docker2.1 driver and docker02.2 network2.3 网桥 docker0 3 总结 1 network namespace 1.1 动手小实验 网络命名空间。linux kernel 提供的网络虚拟化的功能。创建多个隔离的网络空间。每个空间内 firewall, ether …

Matplotlib学习

文章目录 Matplotlib曲线图的绘制饼图的绘制直方图的绘制散点图的绘制 Matplotlib 在深度学习的实验中&#xff0c;图形的绘制和数据的可视化非常重要。Matplotlib是用于绘制图形的库&#xff0c;使用Matplotlib可以轻松地绘制图形和实现数据的可视化。这里&#xff0c;我们来…

Linux学习之文件信息和文件类型

使用ls -l可以看到当前目录下除隐藏文件之外的文件。 我们拿下边这行信息解释一下&#xff1a; -rw-r--r-- 1 root root 10562254 Mar 9 00:08 cmake-3.25.3.tar.gz Linux中“一切皆文件”&#xff0c;首先需要明确这点&#xff0c;因为对于不同的文件类型&#xff0c;后边的…

前后端分离项目之修改存储信息

本文章基于&#xff1a;前后端分离项目之登录页面(前后端请求、响应和连接数据库)_小俱的一步步的博客-CSDN博客 目录 一、编辑者操作步骤 二、代码实现步骤 以下以存储学生信息为例 一、编辑者操作步骤 1.在前端“编辑”按钮&#xff0c;点击时弹出弹框&#xff0c;出现…

某大厂测试开发面试总结,大家可以参考一下

目录 前言 1、RecyclerView和ListView的区别 2、技术选型的依据 3、原生monkey的原理 4、monkey和monkeyRunner区别 5、appium和uiautomator的关系或者Airtest和uiautomator的区别 6、Android进程间通信方式 7、内存溢出与内存泄露的区别及内存泄漏的原因 8、性能数据收…