模拟图传系统是FPV早期作为视频影像的一个重要手段。虽然目前逐步的被高清数字图传所替代,但是模拟图传作为高性价比,尤其是作为经济实惠的产品,为大众所喜爱。
结合ArduPilot系统,模拟图传部分抽象为AP_VideoTX模块,这里就这个展开一些简单的功能和代码介绍,以便更加理解在实际应用过程中需要注意的一些问题。
AP_VideoTX子模块总体来看可以分为三大部分:
子模块的启动从AP_Vehicle::setup中调用
AP_Vehicle::setup
├──> AP_VideoTX::init
├──> AP_SmartAudio::init
└──> 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的工作。
初始化模拟图传系统的关键参数:
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;
}
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());
}
}
}
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;
}
类似驱动本地数据有效性的检查和状态同步,同时调用串口数据处理流程
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();
}
通过TrampStatus状态机的方式进行处理:
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;
}
}
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;
}
飞控与VTX图传上电时间可能存在不同步问题,所以
loop依次步骤:
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);
}
}
}
}
读取串口报文,并按照报文封装进行校验解析;将合法报文内容送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;
}
握手报文协议解析
SMARTAUDIO_CMD_SET_FREQUENCY/SMARTAUDIO_CMD_SET_CHANNEL/SMARTAUDIO_CMD_SET_POWER/SMARTAUDIO_CMD_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;
}
由于逻辑上的疑点,最终导致两个问题点:
注:详细讨论可见【14】【15】
比如:PandaRC VT5804ML1 600mW图传功率设置就不再当前代码支持范围。
【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?