Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_VideoTX: Feat: Add RC Switch to VTX Power Mapping

Browse Source 
This patch does both enhance and simplify the vtx power selection via RC switch.

The RC Option VTX Power expects a 6 position switch
and is therefore useless if any other sort of switch is used.

The typical setup found on a radio is a 3 position switch and up until now,
position 1 results in pit mode which is not accessible if armed and
typically pit mode is tied to arm state through option bits.
So Position 1 is lost for no reason!
Position 3 is the MAX POWER value and Position 2 is some arbitrary value
in between.

This patch makes use of the same concept as the flight-mode switch with
configurable positional values.

It also removes obscure code to find power levels that also seems to
have code duplicates.
This commit is contained in:
Patrick Menschel 2024-12-04 15:32:45 +01:00
parent 10209a2a13
commit 0ef0f383c1
2 changed files with 80 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

102
libraries/AP_VideoTX/AP_VideoTX.cpp
View File

@ -75,6 +75,42 @@ const AP_Param::GroupInfo AP_VideoTX::var_info[] = {
// @Range: 25 1000
AP_GROUPINFO("MAX_POWER", 7, AP_VideoTX, _max_power_mw, 800),
// @Param: VTX_POWER1
// @DisplayName: VTX_POWER1
// @Description: The VTX Power for switch position 1 (910 to 1230 and above 2049)
// @Range: 25 1000
AP_GROUPINFO("VTX_POWER1", 8, AP_VideoTX, _vtx_power_1, 0),
// @Param: VTX_POWER2
// @DisplayName: VTX_POWER2
// @Description: The VTX Power for switch position 2 (1231 to 1360)
// @Range: 25 1000
AP_GROUPINFO("VTX_POWER2", 9, AP_VideoTX, _vtx_power_2, 25),
// @Param: VTX_POWER3
// @DisplayName: VTX_POWER3
// @Description: The VTX Power for switch position 3 (1361 to 1490)
// @Range: 25 1000
AP_GROUPINFO("VTX_POWER3", 10, AP_VideoTX, _vtx_power_3, 100),
// @Param: VTX_POWER4
// @DisplayName: VTX_POWER4
// @Description: The VTX Power for switch position 4 (1491 to 1620)
// @Range: 25 1000
AP_GROUPINFO("VTX_POWER4", 11, AP_VideoTX, _vtx_power_4, 200),
// @Param: VTX_POWER5
// @DisplayName: VTX_POWER5
// @Description: The VTX Power for switch position 5 (1621 to 1749)
// @Range: 25 1000
AP_GROUPINFO("VTX_POWER5", 12, AP_VideoTX, _vtx_power_5, 400),
// @Param: VTX_POWER6
// @DisplayName: VTX_POWER6
// @Description: The VTX Power for switch position 6 (1750 to 2049)
// @Range: 25 1000
AP_GROUPINFO("VTX_POWER6", 13, AP_VideoTX, _vtx_power_6, 800),
AP_GROUPEND
};
@ -508,36 +544,46 @@ void AP_VideoTX::change_power(int8_t position)
if (!_enabled || position < 0 || position > 5) {
return;
}
// first find out how many possible levels there are
uint8_t num_active_levels = 0;
for (uint8_t i = 0; i < VTX_MAX_POWER_LEVELS; i++) {
if (_power_levels[i].active != PowerActive::Inactive && _power_levels[i].mw <= _max_power_mw) {
num_active_levels++;
}
}
// iterate through to find the level
uint16_t level = constrain_int16(roundf((num_active_levels * (position + 1)/ 6.0f) - 1), 0, num_active_levels - 1);
debug("looking for pos %d power level %d from %d", position, level, num_active_levels);
uint16_t power = 0;
for (uint8_t i = 0, j = 0; i < num_active_levels; i++, j++) {
while (j < VTX_MAX_POWER_LEVELS-1 && _power_levels[j].active == PowerActive::Inactive) {
j++;
}
if (i == level) {
power = _power_levels[j].mw;
debug("selected power %dmw", power);
break;
}
}
if (power == 0) {
if (!hal.util->get_soft_armed()) { // don't allow pitmode to be entered if already armed
set_configured_options(get_configured_options() | uint8_t(VideoOptions::VTX_PITMODE));
}
if (position == 0){
power = _vtx_power_1;
} else if (position == 1){
power = _vtx_power_2;
} else if (position == 2){
power = _vtx_power_3;
} else if (position == 3){
power = _vtx_power_4;
} else if (position == 4){
power = _vtx_power_5;
} else if (position == 5){
power = _vtx_power_6;
} else {
if (has_option(VideoOptions::VTX_PITMODE)) {
set_configured_options(get_configured_options() & ~uint8_t(VideoOptions::VTX_PITMODE));
}
power = 0;
}
debug("selected power %dmw", power);
if ((power == 0)
&& !(has_option(VideoOptions::VTX_PITMODE_ON_DISARM)
|| has_option(VideoOptions::VTX_PITMODE_UNTIL_ARM)
)
&& (!hal.util->get_soft_armed())
){
/**
* If power is 0 this indicates pitmode by RC switch but there are two constraints.
* 1. Pitmode must not be controlled by ARM state.
* 2. It is not allowed to enter pitmode if the vehicle is already armed for safety.
* If none of this is the case set the VTX_PITMODE flag.
*/
set_configured_options(get_configured_options() | uint8_t(VideoOptions::VTX_PITMODE));
} else if (!(has_option(VideoOptions::VTX_PITMODE_ON_DISARM)
|| has_option(VideoOptions::VTX_PITMODE_UNTIL_ARM)
)
&& (has_option(VideoOptions::VTX_PITMODE))
) {
// pitmode is not controlled by arm state and we need to disable it
set_configured_options(get_configured_options() & ~uint8_t(VideoOptions::VTX_PITMODE));
}
if (power > 0){
// The configured power may not be 0.
set_configured_power_mw(power);
}
}

6
libraries/AP_VideoTX/AP_VideoTX.h
View File

@ -188,6 +188,12 @@ private:
AP_Int16 _power_mw;
uint16_t _current_power;
AP_Int16 _max_power_mw;
AP_Int16 _vtx_power_1;
AP_Int16 _vtx_power_2;
AP_Int16 _vtx_power_3;
AP_Int16 _vtx_power_4;
AP_Int16 _vtx_power_5;
AP_Int16 _vtx_power_6;
// frequency band
AP_Int8 _band;