mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-20 06:23:34 +08:00
[vtx] Add VTX driver with Tramp and SmartAudio support
This commit is contained in:
committed by
Niklas Hauser
parent
fad4450d7d
commit
c0c265cd1f
@@ -0,0 +1,41 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2025 PX4 Development Team. All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
# are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# 2. Redistributions in binary form must reproduce the above copyright
|
||||
# notice, this list of conditions and the following disclaimer in
|
||||
# the documentation and/or other materials provided with the
|
||||
# distribution.
|
||||
# 3. Neither the name PX4 nor the names of its contributors may be
|
||||
# used to endorse or promote products derived from this software
|
||||
# without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
############################################################################
|
||||
px4_add_module(
|
||||
MODULE drivers__vtxtable
|
||||
MAIN vtxtable
|
||||
COMPILE_FLAGS
|
||||
SRCS
|
||||
VtxTable.cpp
|
||||
DEPENDS
|
||||
crc
|
||||
)
|
||||
@@ -0,0 +1,28 @@
|
||||
menuconfig DRIVERS_VTXTABLE
|
||||
bool "VTX Table"
|
||||
default n
|
||||
---help---
|
||||
Manages the configuration of VTX frequencies and power levels.
|
||||
|
||||
if DRIVERS_VTXTABLE
|
||||
|
||||
config VTXTABLE_CONFIG_FILE
|
||||
string "VTX Configuration File Path"
|
||||
default "/fs/microsd/vtx_config"
|
||||
---help---
|
||||
Allows saving/loading the VTX table and aux config
|
||||
from a file system.
|
||||
|
||||
config VTXTABLE_USE_STORAGE
|
||||
bool
|
||||
default y if VTXTABLE_CONFIG_FILE != ""
|
||||
default n if !(VTXTABLE_CONFIG_FILE != "")
|
||||
|
||||
config VTXTABLE_AUX_MAP
|
||||
string "VTX Auxiliary Map"
|
||||
default n
|
||||
---help---
|
||||
Enable the AUX map for mapping AUX channel ranges to VTX
|
||||
table settings.
|
||||
|
||||
endif # DRIVERS_VTXTABLE
|
||||
@@ -0,0 +1,369 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2025 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include "VtxTable.hpp"
|
||||
|
||||
#include <px4_platform_common/log.h>
|
||||
#include <px4_platform_common/module.h>
|
||||
#ifdef CONFIG_VTXTABLE_USE_STORAGE
|
||||
#include <px4_platform_common/workqueue.h>
|
||||
#endif
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
static vtx::Config vtxtable_data;
|
||||
|
||||
vtx::Config &vtxtable()
|
||||
{
|
||||
return vtxtable_data;
|
||||
}
|
||||
|
||||
#ifdef CONFIG_VTXTABLE_USE_STORAGE
|
||||
int vtxtable_store(const char *filename)
|
||||
{
|
||||
if (filename == nullptr) { filename = CONFIG_VTXTABLE_CONFIG_FILE; }
|
||||
|
||||
const int rv = vtxtable().store(filename);
|
||||
|
||||
if (rv < 0) {
|
||||
PX4_ERR("%s is not accessible!", filename);
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
PX4_INFO("saved to %s", filename);
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
int vtxtable_load(const char *filename)
|
||||
{
|
||||
if (filename == nullptr) { filename = CONFIG_VTXTABLE_CONFIG_FILE; }
|
||||
|
||||
const int rv = vtxtable().load(filename);
|
||||
|
||||
if (rv < 0) {
|
||||
switch (rv) {
|
||||
case -ENOENT:
|
||||
PX4_ERR("%s not found!", filename);
|
||||
break;
|
||||
|
||||
case -EPROTO:
|
||||
PX4_ERR("VTX config serialization format is unsupported in %s!", filename);
|
||||
break;
|
||||
|
||||
case -EBADMSG:
|
||||
PX4_ERR("VTX config has a corrupt CRC in %s!", filename);
|
||||
break;
|
||||
|
||||
case -EMSGSIZE:
|
||||
PX4_ERR("VTX config in %s is incomplete!", filename);
|
||||
break;
|
||||
|
||||
default:
|
||||
PX4_ERR("Loading VTX config from %s failed!", filename);
|
||||
break;
|
||||
}
|
||||
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
PX4_INFO("loaded from %s", filename);
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
static struct work_s storing_work;
|
||||
#endif
|
||||
|
||||
static void vtxtable_autosave()
|
||||
{
|
||||
#ifdef CONFIG_VTXTABLE_USE_STORAGE
|
||||
work_queue(LPWORK, &storing_work, [](void *) { vtxtable_store(nullptr); }, nullptr, USEC2TICK(1_s));
|
||||
#endif
|
||||
}
|
||||
|
||||
static int vtxtable_print_usage(const char *reason)
|
||||
{
|
||||
if (reason) {
|
||||
PX4_WARN("%s\n", reason);
|
||||
}
|
||||
|
||||
PRINT_MODULE_DESCRIPTION(
|
||||
R"DESCR_STR(
|
||||
### Description
|
||||
Manages the VTX frequency, power level and RC mapping table for VTX configuration.
|
||||
|
||||
)DESCR_STR");
|
||||
|
||||
PRINT_MODULE_USAGE_NAME("vtxtable", "driver");
|
||||
|
||||
PRINT_MODULE_USAGE_COMMAND_DESCR("status", "Shows the current VTX table configuration.");
|
||||
|
||||
PRINT_MODULE_USAGE_COMMAND_DESCR("name", "Sets the VTX table name: <string>");
|
||||
|
||||
PRINT_MODULE_USAGE_COMMAND_DESCR("bands", "Sets the number of bands: <int>");
|
||||
PRINT_MODULE_USAGE_COMMAND_DESCR("band", "Sets the band frequencies: <1-index> <name> <letter> <attribute> <frequencies...>");
|
||||
PRINT_MODULE_USAGE_COMMAND_DESCR("channels", "Sets the number of channels: <int>");
|
||||
|
||||
PRINT_MODULE_USAGE_COMMAND_DESCR("powerlevels", "Sets number of power levels: <int>");
|
||||
PRINT_MODULE_USAGE_COMMAND_DESCR("powervalues", "Sets the power level values: <int...>");
|
||||
PRINT_MODULE_USAGE_COMMAND_DESCR("powerlabels", "Sets the power level labels: <3 chars...>");
|
||||
|
||||
#ifdef CONFIG_VTXTABLE_AUX_MAP
|
||||
PRINT_MODULE_USAGE_COMMAND_DESCR("<int>", "Sets an entry in the mapping table: <0-index> <aux channel> <band> <channel> <power level> <start range> <end range>");
|
||||
#endif
|
||||
PRINT_MODULE_USAGE_COMMAND_DESCR("clear", "Clears the VTX table configuration.");
|
||||
|
||||
#ifdef CONFIG_VTXTABLE_USE_STORAGE
|
||||
PRINT_MODULE_USAGE_COMMAND_DESCR("save", "Saves the VTX config to a file: <file>");
|
||||
PRINT_MODULE_USAGE_COMMAND_DESCR("load", "Loads the VTX config from a file: <file>");
|
||||
#endif
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
void vtxtable_print_status()
|
||||
{
|
||||
if (vtxtable().bands() == 0) {
|
||||
PX4_INFO("VTX table: <not configured>");
|
||||
|
||||
} else {
|
||||
PX4_INFO("VTX table %ux%u: %s", vtxtable().bands(), vtxtable().channels(), vtxtable().name());
|
||||
vtxtable_print_frequencies();
|
||||
}
|
||||
|
||||
if (vtxtable().power_levels() == 0) {
|
||||
PX4_INFO("Power levels: <not configured>");
|
||||
|
||||
} else {
|
||||
PX4_INFO("Power levels:");
|
||||
vtxtable_print_power_levels();
|
||||
}
|
||||
|
||||
#ifdef CONFIG_VTXTABLE_AUX_MAP
|
||||
|
||||
if (vtxtable().map_size() == 0) {
|
||||
PX4_INFO("RC mapping: <not configured>");
|
||||
|
||||
} else {
|
||||
PX4_INFO("RC mapping:");
|
||||
vtxtable_print_aux_map();
|
||||
}
|
||||
|
||||
#endif
|
||||
}
|
||||
|
||||
int vtxtable_custom_command(int argc, char *argv[])
|
||||
{
|
||||
if (!strcmp(argv[0], "status")) {
|
||||
vtxtable_print_status();
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
if (!strcmp(argv[0], "name")) {
|
||||
if (argc < 2) {
|
||||
return vtxtable_print_usage("not enough arguments");
|
||||
}
|
||||
|
||||
vtxtable_autosave();
|
||||
return vtxtable().set_name(argv[1]) ? PX4_OK : PX4_ERROR;
|
||||
}
|
||||
|
||||
if (!strcmp(argv[0], "band")) {
|
||||
if (argc < 5) {
|
||||
return vtxtable_print_usage("not enough arguments");
|
||||
}
|
||||
|
||||
const size_t band = atoi(argv[1]) - 1;
|
||||
vtxtable().set_band_name(band, argv[2]);
|
||||
vtxtable().set_band_letter(band, argv[3][0]);
|
||||
vtxtable().set_band_attribute(band,
|
||||
argv[4][0] == 'F' ? vtx::Config::BandAttribute::FACTORY : vtx::Config::BandAttribute::CUSTOM);
|
||||
|
||||
for (int i = 5; i < argc; i++) {
|
||||
vtxtable().set_frequency(band, i - 5, atoi(argv[i]));
|
||||
}
|
||||
|
||||
vtxtable_autosave();
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
if (!strcmp(argv[0], "bands")) {
|
||||
if (argc < 2) {
|
||||
return vtxtable_print_usage("not enough arguments");
|
||||
}
|
||||
|
||||
vtxtable_autosave();
|
||||
return vtxtable().set_bands(atoi(argv[1])) ? PX4_OK : PX4_ERROR;
|
||||
}
|
||||
|
||||
if (!strcmp(argv[0], "channels")) {
|
||||
if (argc < 2) {
|
||||
return vtxtable_print_usage("not enough arguments");
|
||||
}
|
||||
|
||||
vtxtable_autosave();
|
||||
return vtxtable().set_channels(atoi(argv[1])) ? PX4_OK : PX4_ERROR;
|
||||
}
|
||||
|
||||
if (!strcmp(argv[0], "powervalues")) {
|
||||
uint8_t levels{1};
|
||||
|
||||
for (; levels < argc; levels++) {
|
||||
vtxtable().set_power_value(levels - 1, atoi(argv[levels]));
|
||||
}
|
||||
|
||||
vtxtable_autosave();
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
if (!strcmp(argv[0], "powerlabels")) {
|
||||
uint8_t levels{1};
|
||||
|
||||
for (; levels < argc; levels++) {
|
||||
vtxtable().set_power_label(levels - 1, argv[levels]);
|
||||
}
|
||||
|
||||
vtxtable_autosave();
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
if (!strcmp(argv[0], "powerlevels")) {
|
||||
if (argc < 2) {
|
||||
return vtxtable_print_usage("not enough arguments");
|
||||
}
|
||||
|
||||
vtxtable_autosave();
|
||||
return vtxtable().set_power_levels(atoi(argv[1])) ? PX4_OK : PX4_ERROR;
|
||||
}
|
||||
|
||||
#ifdef CONFIG_VTXTABLE_AUX_MAP
|
||||
char *endptr {};
|
||||
strtol(argv[0], &endptr, 10);
|
||||
|
||||
if (endptr != argv[0]) {
|
||||
if (argc < 7) {
|
||||
return vtxtable_print_usage("not enough arguments");
|
||||
}
|
||||
|
||||
vtxtable_autosave();
|
||||
return vtxtable().set_map_entry(
|
||||
atoi(argv[0]), atoi(argv[1]) + 4 - 1, atoi(argv[2]),
|
||||
atoi(argv[3]), atoi(argv[4]), atoi(argv[5]), atoi(argv[6])) ? PX4_OK : PX4_ERROR;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
if (!strcmp(argv[0], "clear")) {
|
||||
vtxtable().clear();
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
#ifdef CONFIG_VTXTABLE_USE_STORAGE
|
||||
const char *const filename = (argc == 2) ? argv[1] : nullptr;
|
||||
|
||||
if (!strcmp(argv[0], "save")) {
|
||||
return vtxtable_store(filename);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[0], "load")) {
|
||||
return vtxtable_load(filename);
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
return vtxtable_print_usage("unknown command");
|
||||
}
|
||||
|
||||
void vtxtable_print_frequencies()
|
||||
{
|
||||
for (uint8_t b = 0; b < vtxtable().bands(); b++) {
|
||||
PX4_INFO_RAW("INFO [vtxtable] %c: %-*s=", vtxtable().band_letter(b), vtx::Config::NAME_LENGTH, vtxtable().band_name(b));
|
||||
|
||||
for (uint8_t c = 0; c < vtxtable().channels(); c++) {
|
||||
PX4_INFO_RAW(" %4hu", vtxtable().frequency(b, c));
|
||||
}
|
||||
|
||||
if (vtxtable().band_attribute(b) == vtx::Config::BandAttribute::CUSTOM) {
|
||||
PX4_INFO_RAW(" CUSTOM\n");
|
||||
|
||||
} else {
|
||||
PX4_INFO_RAW("\n");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void vtxtable_print_power_levels()
|
||||
{
|
||||
for (uint8_t p = 0; p < vtxtable().power_levels(); p++) {
|
||||
PX4_INFO(" %u: %2hi = %s", p + 1, vtxtable().power_value(p), vtxtable().power_label(p));
|
||||
}
|
||||
}
|
||||
|
||||
#ifdef CONFIG_VTXTABLE_AUX_MAP
|
||||
void vtxtable_print_aux_map()
|
||||
{
|
||||
for (uint8_t i = 0; i < vtx::Config::MAP_LENGTH; i++) {
|
||||
uint8_t rc_channel{}, band{}, channel{};
|
||||
int8_t power_level{};
|
||||
uint16_t start_range{}, end_range{};
|
||||
vtxtable().map_entry(i, &rc_channel, &band, &channel, &power_level, &start_range, &end_range);
|
||||
|
||||
if (!start_range && !end_range) { break; }
|
||||
|
||||
if (!band && !channel) {
|
||||
if (power_level == -1) {
|
||||
PX4_INFO(" %2u: Ch%-2u, %4hu - %4hu = pit mode",
|
||||
i, rc_channel, start_range, end_range);
|
||||
|
||||
} else {
|
||||
PX4_INFO(" %2u: Ch%-2u, %4hu - %4hu = %s",
|
||||
i, rc_channel, start_range, end_range,
|
||||
vtxtable().power_label(power_level - 1));
|
||||
}
|
||||
|
||||
} else {
|
||||
PX4_INFO(" %2u: Ch%-2u, %4hu - %4hu = %u + %u",
|
||||
i, rc_channel, start_range, end_range,
|
||||
band, channel);
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
extern "C" __EXPORT int vtxtable_main(int argc, char *argv[])
|
||||
{
|
||||
if (argc <= 1) {
|
||||
return vtxtable_print_usage("missing command");
|
||||
}
|
||||
argc--; argv++;
|
||||
return vtxtable_custom_command(argc, argv);
|
||||
}
|
||||
@@ -0,0 +1,53 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2025 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "config.h"
|
||||
|
||||
/**
|
||||
* @author Niklas Hauser <niklas@auterion.com>
|
||||
*/
|
||||
|
||||
extern vtx::Config &vtxtable();
|
||||
|
||||
#ifdef CONFIG_VTXTABLE_USE_STORAGE
|
||||
extern int vtxtable_store(const char *filename);
|
||||
extern int vtxtable_load(const char *filename);
|
||||
#endif
|
||||
|
||||
extern void vtxtable_print_frequencies();
|
||||
extern void vtxtable_print_power_levels();
|
||||
#ifdef CONFIG_VTXTABLE_AUX_MAP
|
||||
extern void vtxtable_print_aux_map();
|
||||
#endif
|
||||
@@ -0,0 +1,545 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2025 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
#include <cstddef>
|
||||
#include <cstdint>
|
||||
#include <cstring>
|
||||
#include <math.h>
|
||||
#include <errno.h>
|
||||
#include <fcntl.h>
|
||||
#include <containers/LockGuard.hpp>
|
||||
#include <board_config.h>
|
||||
extern "C" {
|
||||
#include <lib/crc/crc.h>
|
||||
}
|
||||
|
||||
namespace vtx
|
||||
{
|
||||
|
||||
/**
|
||||
* Class storing the VTX frequencies and power levels and the map from RC channels to VTX settings.
|
||||
* Everything is 0-indexed.
|
||||
* @author Niklas Hauser <niklas@auterion.com>
|
||||
*/
|
||||
class Config
|
||||
{
|
||||
public:
|
||||
static constexpr size_t BANDS{24};
|
||||
static constexpr size_t CHANNELS{16};
|
||||
static constexpr size_t POWER_LEVELS{16};
|
||||
static constexpr size_t MAP_LENGTH{160};
|
||||
static constexpr size_t NAME_LENGTH{16};
|
||||
static constexpr size_t BAND_NAME_LENGTH{12};
|
||||
static constexpr size_t POWER_LABEL_LENGTH{4};
|
||||
enum class BandAttribute : uint8_t {
|
||||
FACTORY = 0,
|
||||
CUSTOM = 1,
|
||||
};
|
||||
// Allows to detect changes in the configuration
|
||||
using ChangeType = uint16_t;
|
||||
inline ChangeType get_change() { return _change_value; }
|
||||
|
||||
constexpr Config() = default;
|
||||
inline ~Config()
|
||||
{
|
||||
pthread_mutex_destroy(&_mutex);
|
||||
}
|
||||
|
||||
#ifdef CONFIG_VTXTABLE_AUX_MAP
|
||||
// ================================== RC MAP ==================================
|
||||
inline int set_map_entry(uint8_t index, uint8_t rc_channel, uint8_t band, uint8_t channel,
|
||||
int8_t power_level, uint16_t start_range, uint16_t end_range)
|
||||
{
|
||||
if (index >= MAP_LENGTH) {
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
LockGuard lg{_mutex};
|
||||
_data.rc_map[index].rc_channel = rc_channel;
|
||||
_data.rc_map[index].band = band;
|
||||
_data.rc_map[index].channel = channel;
|
||||
_data.rc_map[index].power_level = power_level;
|
||||
_data.rc_map[index].start_range = start_range;
|
||||
_data.rc_map[index].end_range = end_range;
|
||||
_change_value++;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
inline int map_entry(uint8_t index, uint8_t *rc_channel, uint8_t *band, uint8_t *channel,
|
||||
int8_t *power_level, uint16_t *start_range, uint16_t *end_range) const
|
||||
{
|
||||
if (index >= MAP_LENGTH) {
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
LockGuard lg{_mutex};
|
||||
|
||||
if (rc_channel) { *rc_channel = _data.rc_map[index].rc_channel; }
|
||||
|
||||
if (band) { *band = _data.rc_map[index].band; }
|
||||
|
||||
if (channel) { *channel = _data.rc_map[index].channel; }
|
||||
|
||||
if (power_level) { *power_level = _data.rc_map[index].power_level; }
|
||||
|
||||
if (start_range) { *start_range = _data.rc_map[index].start_range; }
|
||||
|
||||
if (end_range) { *end_range = _data.rc_map[index].end_range; }
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
inline int map_lookup(uint16_t *rc_values, size_t rc_count,
|
||||
int8_t *band, int8_t *channel, int8_t *power_level) const
|
||||
{
|
||||
LockGuard lg{_mutex};
|
||||
|
||||
for (uint8_t i = 0; i < MAP_LENGTH; i++) {
|
||||
if (_data.rc_map[i].rc_channel >= rc_count || _data.rc_map[i].is_empty()) {
|
||||
continue;
|
||||
}
|
||||
|
||||
const uint16_t pwm_value = rc_values[_data.rc_map[i].rc_channel];
|
||||
|
||||
if (pwm_value >= _data.rc_map[i].start_range &&
|
||||
pwm_value < _data.rc_map[i].end_range) {
|
||||
if (_data.rc_map[i].band) { *band = _data.rc_map[i].band; }
|
||||
|
||||
if (_data.rc_map[i].channel) { *channel = _data.rc_map[i].channel; }
|
||||
|
||||
if (_data.rc_map[i].power_level) { *power_level = _data.rc_map[i].power_level; }
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
inline int map_clear()
|
||||
{
|
||||
LockGuard lg{_mutex};
|
||||
memset(&_data.rc_map, 0, sizeof(_data.rc_map));
|
||||
_change_value++;
|
||||
return 0;
|
||||
}
|
||||
|
||||
inline size_t map_size() const
|
||||
{
|
||||
LockGuard lg{_mutex};
|
||||
size_t count{};
|
||||
|
||||
for (uint_fast8_t i = 0; i < MAP_LENGTH; i++) {
|
||||
if (!_data.rc_map[i].is_empty()) {
|
||||
count++;
|
||||
}
|
||||
}
|
||||
|
||||
return count;
|
||||
}
|
||||
#endif
|
||||
|
||||
// ================================ VTX TABLE =================================
|
||||
inline const char *name() const
|
||||
{
|
||||
LockGuard lg{_mutex};
|
||||
return _data.table.name;
|
||||
}
|
||||
inline bool set_name(const char *name)
|
||||
{
|
||||
if (!name) { return false; }
|
||||
|
||||
LockGuard lg{_mutex};
|
||||
strncpy(_data.table.name, name, NAME_LENGTH);
|
||||
_data.table.name[NAME_LENGTH] = 0;
|
||||
_change_value++;
|
||||
return true;
|
||||
}
|
||||
inline uint16_t frequency(uint8_t band, uint8_t channel) const
|
||||
{
|
||||
LockGuard lg{_mutex};
|
||||
|
||||
if (band >= _data.table.bands || channel >= _data.table.channels) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
return _data.table.frequency[band][channel];
|
||||
}
|
||||
inline bool set_frequency(uint8_t band, uint8_t channel, uint16_t frequency)
|
||||
{
|
||||
if (band >= BANDS || channel >= CHANNELS) {
|
||||
return false;
|
||||
}
|
||||
|
||||
LockGuard lg{_mutex};
|
||||
_data.table.frequency[band][channel] = frequency;
|
||||
_change_value++;
|
||||
return true;
|
||||
}
|
||||
inline bool find_band_channel(uint16_t frequency, uint8_t *band, uint8_t *channel) const
|
||||
{
|
||||
LockGuard lg{_mutex};
|
||||
|
||||
for (uint8_t bandi = 0; bandi < _data.table.bands; bandi++) {
|
||||
for (uint8_t channeli = 0; channeli < _data.table.channels; channeli++) {
|
||||
if (_data.table.frequency[bandi][channeli] == frequency) {
|
||||
*band = bandi;
|
||||
*channel = channeli;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
inline size_t channels() const
|
||||
{
|
||||
LockGuard lg{_mutex};
|
||||
return _data.table.channels;
|
||||
}
|
||||
inline bool set_channels(size_t channels)
|
||||
{
|
||||
if (channels > CHANNELS) {
|
||||
return false;
|
||||
}
|
||||
|
||||
LockGuard lg{_mutex};
|
||||
_data.table.channels = channels;
|
||||
_change_value++;
|
||||
return true;
|
||||
}
|
||||
|
||||
inline size_t bands() const
|
||||
{
|
||||
LockGuard lg{_mutex};
|
||||
return _data.table.bands;
|
||||
}
|
||||
inline bool set_bands(size_t bands)
|
||||
{
|
||||
if (bands > BANDS) {
|
||||
return false;
|
||||
}
|
||||
|
||||
LockGuard lg{_mutex};
|
||||
_data.table.bands = bands;
|
||||
_change_value++;
|
||||
return true;
|
||||
}
|
||||
|
||||
inline const char *band_name(uint8_t band) const
|
||||
{
|
||||
LockGuard lg{_mutex};
|
||||
|
||||
if (band >= _data.table.bands) {
|
||||
return "?";
|
||||
}
|
||||
|
||||
return const_cast<const char *>(_data.table.band[band]);
|
||||
}
|
||||
inline bool set_band_name(uint8_t band, const char *name)
|
||||
{
|
||||
if (band >= BANDS || !name) {
|
||||
return false;
|
||||
}
|
||||
|
||||
LockGuard lg{_mutex};
|
||||
strncpy(_data.table.band[band], name, BAND_NAME_LENGTH);
|
||||
_data.table.band[band][BAND_NAME_LENGTH] = 0;
|
||||
_change_value++;
|
||||
return true;
|
||||
}
|
||||
|
||||
inline char band_letter(uint8_t band) const
|
||||
{
|
||||
LockGuard lg{_mutex};
|
||||
|
||||
if (band >= _data.table.bands) {
|
||||
return '?';
|
||||
}
|
||||
|
||||
const char c = _data.table.letter[band];
|
||||
return c ? c : '?';
|
||||
}
|
||||
inline bool set_band_letter(uint8_t band, char c)
|
||||
{
|
||||
if (band >= BANDS) {
|
||||
return false;
|
||||
}
|
||||
|
||||
LockGuard lg{_mutex};
|
||||
_data.table.letter[band] = c;
|
||||
_change_value++;
|
||||
return true;
|
||||
}
|
||||
|
||||
inline BandAttribute band_attribute(uint8_t band) const
|
||||
{
|
||||
LockGuard lg{_mutex};
|
||||
|
||||
if (band >= _data.table.bands) {
|
||||
return BandAttribute::FACTORY;
|
||||
}
|
||||
|
||||
return _data.table.attribute[band];
|
||||
}
|
||||
inline bool set_band_attribute(uint8_t band, BandAttribute attribute)
|
||||
{
|
||||
if (band >= BANDS) {
|
||||
return false;
|
||||
}
|
||||
|
||||
LockGuard lg{_mutex};
|
||||
_data.table.attribute[band] = attribute;
|
||||
_change_value++;
|
||||
return true;
|
||||
}
|
||||
|
||||
inline size_t power_levels() const
|
||||
{
|
||||
LockGuard lg{_mutex};
|
||||
return _data.table.power_levels;
|
||||
}
|
||||
inline bool set_power_levels(size_t levels)
|
||||
{
|
||||
if (levels > POWER_LEVELS) {
|
||||
return false;
|
||||
}
|
||||
|
||||
LockGuard lg{_mutex};
|
||||
_data.table.power_levels = levels;
|
||||
_change_value++;
|
||||
return true;
|
||||
}
|
||||
|
||||
inline int16_t power_value(uint8_t level) const
|
||||
{
|
||||
LockGuard lg{_mutex};
|
||||
|
||||
if (level >= _data.table.power_levels) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
return _data.table.power_value[level];
|
||||
}
|
||||
inline bool set_power_value(uint8_t level, int16_t value)
|
||||
{
|
||||
if (level >= POWER_LEVELS) {
|
||||
return false;
|
||||
}
|
||||
|
||||
LockGuard lg{_mutex};
|
||||
_data.table.power_value[level] = value;
|
||||
_change_value++;
|
||||
return true;
|
||||
}
|
||||
inline const char *power_label(uint8_t level) const
|
||||
{
|
||||
LockGuard lg{_mutex};
|
||||
|
||||
if (level >= _data.table.power_levels) {
|
||||
return "?";
|
||||
}
|
||||
|
||||
return _data.table.power_label[level];
|
||||
}
|
||||
inline bool set_power_label(uint8_t level, const char *label)
|
||||
{
|
||||
if (level >= POWER_LEVELS || !label) {
|
||||
return false;
|
||||
}
|
||||
|
||||
LockGuard lg{_mutex};
|
||||
strncpy(_data.table.power_label[level], label, POWER_LABEL_LENGTH);
|
||||
_data.table.power_label[level][POWER_LABEL_LENGTH] = 0;
|
||||
_change_value++;
|
||||
return true;
|
||||
}
|
||||
|
||||
inline void table_clear()
|
||||
{
|
||||
LockGuard lg{_mutex};
|
||||
_data.table = {};
|
||||
_change_value++;
|
||||
}
|
||||
|
||||
inline void clear()
|
||||
{
|
||||
LockGuard lg{_mutex};
|
||||
_data = {};
|
||||
_change_value++;
|
||||
}
|
||||
|
||||
#ifdef CONFIG_VTXTABLE_USE_STORAGE
|
||||
inline int store(const char *filename) const
|
||||
{
|
||||
LockGuard lg{_mutex};
|
||||
int fd = open(filename, O_WRONLY | O_CREAT | O_TRUNC);
|
||||
|
||||
if (fd < 0) {
|
||||
return errno;
|
||||
}
|
||||
|
||||
// first write the magic/version
|
||||
const uint64_t magic = MAGIC;
|
||||
int written = write(fd, &magic, sizeof(MAGIC));
|
||||
|
||||
// Then write the rest of the data
|
||||
written += write(fd, &_data, sizeof(Storage));
|
||||
|
||||
// finally compute and write the crc16
|
||||
const uint16_t crc = crc16_signature(CRC16_INITIAL, sizeof(Storage), (const uint8_t *)&_data);
|
||||
written += write(fd, &crc, sizeof(crc));
|
||||
|
||||
close(fd);
|
||||
return (written == FILE_SIZE) ? 0 : -EMSGSIZE;
|
||||
}
|
||||
|
||||
inline int load(const char *filename)
|
||||
{
|
||||
if (access(filename, R_OK)) {
|
||||
return -ENOENT;
|
||||
}
|
||||
|
||||
LockGuard lg{_mutex};
|
||||
int fd = open(filename, O_RDONLY);
|
||||
|
||||
if (fd < 0) {
|
||||
return errno;
|
||||
}
|
||||
|
||||
// Check file size first
|
||||
const off_t file_size = lseek(fd, 0, SEEK_END);
|
||||
|
||||
if (file_size != FILE_SIZE) {
|
||||
close(fd);
|
||||
return -EMSGSIZE;
|
||||
}
|
||||
|
||||
// Check the version next
|
||||
lseek(fd, 0, SEEK_SET);
|
||||
uint64_t magic{};
|
||||
const int read_size = read(fd, &magic, sizeof(magic));
|
||||
|
||||
if (read_size != sizeof(magic) || magic != MAGIC) {
|
||||
close(fd);
|
||||
return -EPROTO;
|
||||
}
|
||||
|
||||
// Compute and check crc16 of the data in chunks
|
||||
const size_t CHUNK{128};
|
||||
uint8_t buffer[CHUNK];
|
||||
size_t remaining{sizeof(Storage)};
|
||||
uint16_t computed_crc{0xffff};
|
||||
|
||||
while (remaining > 0) {
|
||||
const int r = read(fd, buffer, (remaining < CHUNK) ? remaining : CHUNK);
|
||||
|
||||
if (r <= 0) {
|
||||
close(fd);
|
||||
return -EIO;
|
||||
}
|
||||
|
||||
computed_crc = crc16_signature(computed_crc, r, buffer) ^ CRC16_OUTPUT_XOR;
|
||||
remaining -= r;
|
||||
}
|
||||
|
||||
// read the stored crc16
|
||||
uint16_t stored_crc{};
|
||||
const int r = read(fd, &stored_crc, sizeof(stored_crc));
|
||||
|
||||
if (r != sizeof(stored_crc)) {
|
||||
close(fd);
|
||||
return -EIO;
|
||||
}
|
||||
|
||||
if (computed_crc != stored_crc) {
|
||||
close(fd);
|
||||
return -EBADMSG;
|
||||
}
|
||||
|
||||
// now read into the data structure directly
|
||||
lseek(fd, sizeof(magic), SEEK_SET);
|
||||
read(fd, &_data, sizeof(Storage));
|
||||
close(fd);
|
||||
|
||||
_change_value++;
|
||||
return 0;
|
||||
}
|
||||
#endif
|
||||
|
||||
private:
|
||||
#ifdef CONFIG_VTXTABLE_AUX_MAP
|
||||
struct MapEntry {
|
||||
uint8_t rc_channel;
|
||||
uint8_t band;
|
||||
uint8_t channel;
|
||||
int8_t power_level;
|
||||
uint16_t start_range;
|
||||
uint16_t end_range;
|
||||
constexpr bool is_empty() const { return !start_range && !end_range; }
|
||||
} __attribute__((packed));
|
||||
#endif
|
||||
|
||||
struct Table {
|
||||
uint16_t frequency[BANDS][CHANNELS] {};
|
||||
int16_t power_value[POWER_LEVELS] {};
|
||||
char band[BANDS][BAND_NAME_LENGTH + 1] {};
|
||||
char power_label[POWER_LEVELS][POWER_LABEL_LENGTH + 1] {};
|
||||
char name[NAME_LENGTH + 1] {};
|
||||
char letter[BANDS] {};
|
||||
BandAttribute attribute[BANDS] {};
|
||||
uint8_t bands{};
|
||||
uint8_t channels{};
|
||||
uint8_t power_levels{};
|
||||
} __attribute__((packed));
|
||||
|
||||
struct Storage {
|
||||
Table table{};
|
||||
#ifdef CONFIG_VTXTABLE_AUX_MAP
|
||||
MapEntry rc_map[MAP_LENGTH];
|
||||
#endif
|
||||
} __attribute__((packed));
|
||||
|
||||
#ifdef CONFIG_VTXTABLE_USE_STORAGE
|
||||
static constexpr uint16_t VERSION {3};
|
||||
static constexpr uint64_t MAGIC{0x767478'636e66'0000ull | VERSION}; // "vtxcnf" + version magic
|
||||
static constexpr size_t FILE_SIZE{sizeof(Storage) + sizeof(MAGIC) + sizeof(uint16_t)};
|
||||
#endif
|
||||
|
||||
mutable pthread_mutex_t _mutex = PTHREAD_MUTEX_INITIALIZER;
|
||||
Storage _data{};
|
||||
ChangeType _change_value{};
|
||||
};
|
||||
|
||||
}
|
||||
Reference in New Issue
Block a user