Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 :
16 : #include "AP_VideoTX.h"
17 :
18 : #if AP_VIDEOTX_ENABLED
19 :
20 : #include <AP_RCTelemetry/AP_CRSF_Telem.h>
21 : #include <GCS_MAVLink/GCS.h>
22 :
23 : #include <AP_HAL/AP_HAL.h>
24 :
25 : extern const AP_HAL::HAL& hal;
26 :
27 : AP_VideoTX *AP_VideoTX::singleton;
28 :
29 : const AP_Param::GroupInfo AP_VideoTX::var_info[] = {
30 :
31 : // @Param: ENABLE
32 : // @DisplayName: Is the Video Transmitter enabled or not
33 : // @Description: Toggles the Video Transmitter on and off
34 : // @Values: 0:Disable,1:Enable
35 : AP_GROUPINFO_FLAGS("ENABLE", 1, AP_VideoTX, _enabled, 0, AP_PARAM_FLAG_ENABLE),
36 :
37 : // @Param: POWER
38 : // @DisplayName: Video Transmitter Power Level
39 : // @Description: Video Transmitter Power Level. Different VTXs support different power levels, the power level chosen will be rounded down to the nearest supported power level
40 : // @Range: 1 1000
41 : AP_GROUPINFO("POWER", 2, AP_VideoTX, _power_mw, 0),
42 :
43 : // @Param: CHANNEL
44 : // @DisplayName: Video Transmitter Channel
45 : // @Description: Video Transmitter Channel
46 : // @User: Standard
47 : // @Range: 0 7
48 : AP_GROUPINFO("CHANNEL", 3, AP_VideoTX, _channel, 0),
49 :
50 : // @Param: BAND
51 : // @DisplayName: Video Transmitter Band
52 : // @Description: Video Transmitter Band
53 : // @User: Standard
54 : // @Values: 0:Band A,1:Band B,2:Band E,3:Airwave,4:RaceBand,5:Low RaceBand,6:1G3 Band A,7:1G3 Band B,8:Band X,9:3G3 Band A,10:3G3 Band B
55 : AP_GROUPINFO("BAND", 4, AP_VideoTX, _band, 0),
56 :
57 : // @Param: FREQ
58 : // @DisplayName: Video Transmitter Frequency
59 : // @Description: Video Transmitter Frequency. The frequency is derived from the setting of BAND and CHANNEL
60 : // @User: Standard
61 : // @ReadOnly: True
62 : // @Range: 1000 6000
63 : AP_GROUPINFO("FREQ", 5, AP_VideoTX, _frequency_mhz, 0),
64 :
65 : // @Param: OPTIONS
66 : // @DisplayName: Video Transmitter Options
67 : // @Description: Video Transmitter Options. Pitmode puts the VTX in a low power state. Unlocked enables certain restricted frequencies and power levels. Do not enable the Unlocked option unless you have appropriate permissions in your jurisdiction to transmit at high power levels. One stop-bit may be required for VTXs that erroneously mimic iNav behaviour.
68 : // @User: Advanced
69 : // @Bitmask: 0:Pitmode,1:Pitmode until armed,2:Pitmode when disarmed,3:Unlocked,4:Add leading zero byte to requests,5:Use 1 stop-bit in SmartAudio,6:Ignore CRC in SmartAudio,7:Ignore status updates in CRSF and blindly set VTX options
70 : AP_GROUPINFO("OPTIONS", 6, AP_VideoTX, _options, 0),
71 :
72 : // @Param: MAX_POWER
73 : // @DisplayName: Video Transmitter Max Power Level
74 : // @Description: Video Transmitter Maximum Power Level. Different VTXs support different power levels, this prevents the power aux switch from requesting too high a power level. The switch supports 6 power levels and the selected power will be a subdivision between 0 and this setting.
75 : // @Range: 25 1000
76 : AP_GROUPINFO("MAX_POWER", 7, AP_VideoTX, _max_power_mw, 800),
77 :
78 : // @Param: TYPES
79 : // @DisplayName: Allowed VTX control transports
80 : // @Description: Bitmask of the control transports permitted to manage the VTX. AP_VideoTX represents a single VTX, so when more than one transport is present (for example a CRSF VTX on the receiver and an MSP VTX on the goggles) this selects which one owns it. Clear a transport's bit to stop it taking control of the VTX.
81 : // @Bitmask: 0:CRSF,1:SmartAudio,2:Tramp,3:MSP
82 : // @User: Advanced
83 : AP_GROUPINFO("TYPES", 8, AP_VideoTX, _types_allowed, uint8_t(CRSF | SmartAudio | Tramp | MSP)),
84 :
85 : AP_GROUPEND
86 : };
87 :
88 : //#define VTX_DEBUG
89 : #ifdef VTX_DEBUG
90 : # define debug(fmt, args...) hal.console->printf("VTX: " fmt "\n", ##args)
91 : #else
92 : # define debug(fmt, args...) do {} while(0)
93 : #endif
94 :
95 : extern const AP_HAL::HAL& hal;
96 :
97 : const char * AP_VideoTX::band_names[] = {"A","B","E","F","R","L","1G3_A","1G3_B","X","3G3_A","3G3_B"};
98 :
99 : const uint16_t AP_VideoTX::VIDEO_CHANNELS[AP_VideoTX::MAX_BANDS][VTX_MAX_CHANNELS] =
100 : {
101 : { 5865, 5845, 5825, 5805, 5785, 5765, 5745, 5725}, /* Band A */
102 : { 5733, 5752, 5771, 5790, 5809, 5828, 5847, 5866}, /* Band B */
103 : { 5705, 5685, 5665, 5645, 5885, 5905, 5925, 5945}, /* Band E */
104 : { 5740, 5760, 5780, 5800, 5820, 5840, 5860, 5880}, /* Airwave */
105 : { 5658, 5695, 5732, 5769, 5806, 5843, 5880, 5917}, /* Race */
106 : { 5362, 5399, 5436, 5473, 5510, 5547, 5584, 5621}, /* LO Race */
107 : { 1080, 1120, 1160, 1200, 1240, 1280, 1320, 1360}, /* Band 1G3_A */
108 : { 1080, 1120, 1160, 1200, 1258, 1280, 1320, 1360}, /* Band 1G3_B */
109 : { 4990, 5020, 5050, 5080, 5110, 5140, 5170, 5200}, /* Band X */
110 : { 3330, 3350, 3370, 3390, 3410, 3430, 3450, 3470}, /* Band 3G3_A */
111 : { 3170, 3190, 3210, 3230, 3250, 3270, 3290, 3310} /* Band 3G3_B */
112 : };
113 :
114 : // mapping of power level to milliwatt to dbm
115 : // valid power levels from SmartAudio spec, the adjacent levels might be the actual values
116 : // so these are marked as level + 0x10 and will be switched if a dbm message proves it
117 : AP_VideoTX::PowerLevel AP_VideoTX::_power_levels[VTX_MAX_POWER_LEVELS] = {
118 : // level, mw, dbm, dac
119 : { 0xFF, 0, 0, 0 }, // only in SA 2.1
120 : { 0, 25, 14, 7 },
121 : { 0x11, 100, 20, 0xFF }, // only in SA 2.1
122 : { 1, 200, 23, 16 },
123 : { 0x12, 400, 26, 0xFF }, // only in SA 2.1
124 : { 2, 500, 27, 25 },
125 : { 0x12, 600, 28, 0xFF }, // Tramp lies above power levels and always returns 25/100/200/400/600
126 : { 3, 800, 29, 40 },
127 : { 0x13, 1000, 30, 0xFF }, // only in SA 2.1
128 : { 0xFF, 0, 0, 0XFF, PowerActive::Inactive } // slot reserved for a custom power level
129 : };
130 :
131 2098 : AP_VideoTX::AP_VideoTX()
132 : {
133 2098 : if (singleton) {
134 0 : AP_HAL::panic("Too many VTXs");
135 : return;
136 : }
137 2098 : singleton = this;
138 :
139 2098 : AP_Param::setup_object_defaults(this, var_info);
140 : }
141 :
142 458 : AP_VideoTX::~AP_VideoTX(void)
143 : {
144 458 : singleton = nullptr;
145 458 : }
146 :
147 2086 : bool AP_VideoTX::init(void)
148 : {
149 2086 : if (_initialized) {
150 0 : return false;
151 : }
152 :
153 : // find the index into the power table
154 2106 : for (uint8_t i = 0; i < VTX_MAX_POWER_LEVELS; i++) {
155 2106 : if (_power_mw <= _power_levels[i].mw) {
156 2086 : if (_power_mw != _power_levels[i].mw) {
157 0 : if (i > 0) {
158 0 : _current_power = i - 1;
159 : }
160 0 : _power_mw.set_and_save(get_power_mw());
161 : } else {
162 2086 : _current_power = i;
163 : }
164 2086 : break;
165 : }
166 : }
167 2086 : _current_band = _band;
168 2086 : _current_channel = _channel;
169 2086 : _current_frequency = _frequency_mhz;
170 2086 : _current_options = _options;
171 2086 : _current_enabled = _enabled;
172 2086 : _initialized = true;
173 :
174 2086 : return true;
175 : }
176 :
177 0 : bool AP_VideoTX::get_band_and_channel(uint16_t freq, VideoBand& band, uint8_t& channel)
178 : {
179 0 : for (uint8_t i = 0; i < AP_VideoTX::MAX_BANDS; i++) {
180 0 : for (uint8_t j = 0; j < VTX_MAX_CHANNELS; j++) {
181 0 : if (VIDEO_CHANNELS[i][j] == freq) {
182 0 : band = VideoBand(i);
183 0 : channel = j;
184 0 : return true;
185 : }
186 : }
187 : }
188 0 : return false;
189 : }
190 :
191 : // set the current power
192 10 : void AP_VideoTX::set_configured_power_mw(uint16_t power)
193 : {
194 10 : _power_mw.set_and_save_ifchanged(power);
195 10 : }
196 :
197 22 : uint8_t AP_VideoTX::find_current_power() const
198 : {
199 98 : for (uint8_t i = 0; i < VTX_MAX_POWER_LEVELS; i++) {
200 98 : if (_power_mw == _power_levels[i].mw) {
201 22 : return i;
202 : }
203 : }
204 0 : return 0;
205 : }
206 :
207 : // set the power in dbm, rounding appropriately
208 0 : void AP_VideoTX::set_power_dbm(uint8_t power, PowerActive active)
209 : {
210 0 : if (power == _power_levels[_current_power].dbm
211 0 : && _power_levels[_current_power].active == active) {
212 0 : return;
213 : }
214 :
215 0 : for (uint8_t i = 0; i < VTX_MAX_POWER_LEVELS; i++) {
216 0 : if (power == _power_levels[i].dbm) {
217 0 : _current_power = i;
218 0 : _power_levels[i].active = active;
219 : debug("learned power %ddbm", power);
220 : // now unlearn the "other" power level since we have no other way of guessing
221 : // the supported levels
222 0 : if ((_power_levels[i].level & 0xF0) == 0x10) {
223 0 : _power_levels[i].level = _power_levels[i].level & 0xF;
224 : }
225 0 : if (i > 0 && _power_levels[i-1].level == _power_levels[i].level) {
226 : debug("invalidated power %dwm, level %d is now %dmw", _power_levels[i-1].mw, _power_levels[i].level, _power_levels[i].mw);
227 0 : _power_levels[i-1].level = 0xFF;
228 0 : _power_levels[i-1].active = PowerActive::Inactive;
229 0 : } else if (i < VTX_MAX_POWER_LEVELS-1 && _power_levels[i+1].level == _power_levels[i].level) {
230 : debug("invalidated power %dwm, level %d is now %dmw", _power_levels[i+1].mw, _power_levels[i].level, _power_levels[i].mw);
231 0 : _power_levels[i+1].level = 0xFF;
232 0 : _power_levels[i+1].active = PowerActive::Inactive;
233 : }
234 0 : return;
235 : }
236 : }
237 : // learn the non-standard power
238 0 : _current_power = update_power_dbm(power, active);
239 : }
240 :
241 : // add an active power setting in dbm
242 6 : uint8_t AP_VideoTX::update_power_dbm(uint8_t power, PowerActive active)
243 : {
244 24 : for (uint8_t i = 0; i < VTX_MAX_POWER_LEVELS; i++) {
245 24 : if (power == _power_levels[i].dbm) {
246 6 : if (_power_levels[i].active != active) {
247 6 : _power_levels[i].active = active;
248 : debug("%s power %ddbm", active == PowerActive::Active ? "learned" : "invalidated", power);
249 : }
250 6 : return i;
251 : }
252 : }
253 : // handed a non-standard value, use the last slot
254 0 : _power_levels[VTX_MAX_POWER_LEVELS-1].dbm = power;
255 0 : _power_levels[VTX_MAX_POWER_LEVELS-1].level = 255;
256 0 : _power_levels[VTX_MAX_POWER_LEVELS-1].dac = 255;
257 0 : _power_levels[VTX_MAX_POWER_LEVELS-1].mw = uint16_t(roundf(powf(10, power * 0.1f)));
258 0 : _power_levels[VTX_MAX_POWER_LEVELS-1].active = active;
259 : debug("non-standard power %ddbm -> %dmw", power, _power_levels[VTX_MAX_POWER_LEVELS-1].mw);
260 0 : return VTX_MAX_POWER_LEVELS-1;
261 : }
262 :
263 : // add all active power setting in dbm
264 0 : void AP_VideoTX::update_all_power_dbm(uint8_t nlevels, const uint8_t power[])
265 : {
266 0 : for (uint8_t i = 0; i < nlevels; i++) {
267 0 : update_power_dbm(power[i], PowerActive::Active);
268 : }
269 : // invalidate the remaining ones
270 0 : for (uint8_t i = 0; i < VTX_MAX_POWER_LEVELS; i++) {
271 0 : if (_power_levels[i].active == PowerActive::Unknown) {
272 0 : _power_levels[i].active = PowerActive::Inactive;
273 : }
274 : }
275 0 : }
276 :
277 : // mark the level matching the given mW as active, stashing a non-standard
278 : // value in the custom slot; does not change the currently selected power
279 0 : void AP_VideoTX::update_power_mw(uint16_t power_mw, PowerActive active)
280 : {
281 0 : for (uint8_t i = 0; i < ARRAY_SIZE(_power_levels); i++) {
282 0 : if (power_mw == _power_levels[i].mw) {
283 0 : _power_levels[i].active = active;
284 0 : return;
285 : }
286 : }
287 0 : if (power_mw == 0) {
288 0 : return;
289 : }
290 : // non-standard value: use the custom slot
291 0 : PowerLevel &slot = _power_levels[ARRAY_SIZE(_power_levels) - 1];
292 0 : slot.mw = power_mw;
293 0 : slot.dbm = uint8_t(roundf(10.0f * log10f(float(power_mw))));
294 0 : slot.level = 255;
295 0 : slot.dac = 255;
296 0 : slot.active = active;
297 : }
298 :
299 : // number of active power levels
300 0 : uint8_t AP_VideoTX::get_num_power_levels() const
301 : {
302 0 : uint8_t count = 0;
303 0 : for (uint8_t i = 0; i < ARRAY_SIZE(_power_levels); i++) {
304 0 : if (_power_levels[i].active == PowerActive::Active) {
305 0 : count++;
306 : }
307 : }
308 0 : return count;
309 : }
310 :
311 : // mW for a one based index into the active levels (ascending), 0 if unknown
312 10 : uint16_t AP_VideoTX::get_power_mw_for_index(uint8_t index) const
313 : {
314 10 : uint8_t count = 0;
315 80 : for (uint8_t i = 0; i < ARRAY_SIZE(_power_levels); i++) {
316 76 : if (_power_levels[i].active == PowerActive::Active && ++count == index) {
317 6 : return _power_levels[i].mw;
318 : }
319 : }
320 4 : return 0;
321 : }
322 :
323 : // one based index into the active levels for a mW value, 0 if not matched
324 125 : uint8_t AP_VideoTX::get_power_index_for_mw(uint16_t power_mw) const
325 : {
326 125 : uint8_t count = 0;
327 1240 : for (uint8_t i = 0; i < ARRAY_SIZE(_power_levels); i++) {
328 1142 : if (_power_levels[i].active == PowerActive::Active) {
329 81 : count++;
330 81 : if (_power_levels[i].mw == power_mw) {
331 27 : return count;
332 : }
333 : }
334 : }
335 98 : return 0;
336 : }
337 :
338 : // set the power in mw
339 0 : void AP_VideoTX::set_power_mw(uint16_t power)
340 : {
341 0 : for (uint8_t i = 0; i < VTX_MAX_POWER_LEVELS; i++) {
342 0 : if (power == _power_levels[i].mw) {
343 0 : _current_power = i;
344 0 : return;
345 : }
346 : }
347 : // power 0 (pit mode) should always match the built-in 0mW entry; bail
348 : // here so it can never reach log10f below
349 0 : if (power == 0) {
350 0 : return;
351 : }
352 : // non-standard value (e.g. Tramp 2500mW): stash it in the custom slot
353 0 : PowerLevel &slot = _power_levels[VTX_MAX_POWER_LEVELS - 1];
354 0 : slot.mw = power;
355 0 : slot.dbm = uint8_t(roundf(10.0f * log10f(float(power))));
356 0 : slot.level = 255;
357 0 : slot.dac = 255;
358 0 : slot.active = PowerActive::Active;
359 0 : _current_power = VTX_MAX_POWER_LEVELS - 1;
360 : }
361 :
362 : // set the power "level"
363 0 : void AP_VideoTX::set_power_level(uint8_t level, PowerActive active)
364 : {
365 0 : if (level == _power_levels[_current_power].level
366 0 : && _power_levels[_current_power].active == active) {
367 0 : return;
368 : }
369 :
370 0 : for (uint8_t i = 0; i < VTX_MAX_POWER_LEVELS; i++) {
371 0 : if (level == _power_levels[i].level) {
372 0 : _current_power = i;
373 0 : _power_levels[i].active = active;
374 : debug("learned power level %d: %dmw", level, get_power_mw());
375 0 : break;
376 : }
377 : }
378 : }
379 :
380 : // set the power dac
381 0 : void AP_VideoTX::set_power_dac(uint16_t power, PowerActive active)
382 : {
383 0 : if (power == _power_levels[_current_power].dac
384 0 : && _power_levels[_current_power].active == active) {
385 0 : return;
386 : }
387 :
388 0 : for (uint8_t i = 0; i < VTX_MAX_POWER_LEVELS; i++) {
389 0 : if (power == _power_levels[i].dac) {
390 0 : _current_power = i;
391 0 : _power_levels[i].active = active;
392 : debug("learned power %dmw", get_power_mw());
393 : }
394 : }
395 : }
396 :
397 : // set the current channel
398 0 : void AP_VideoTX::set_enabled(bool enabled)
399 : {
400 0 : _current_enabled = enabled;
401 0 : if (!_enabled.configured()) {
402 0 : _enabled.set_and_save(enabled);
403 : }
404 0 : }
405 :
406 0 : void AP_VideoTX::set_power_is_current()
407 : {
408 0 : set_power_dbm(get_configured_power_dbm());
409 0 : }
410 :
411 0 : void AP_VideoTX::set_freq_is_current()
412 : {
413 0 : _current_frequency = _frequency_mhz;
414 0 : _current_band = _band;
415 0 : _current_channel = _channel;
416 0 : }
417 :
418 : // periodic update
419 299997 : void AP_VideoTX::update(void)
420 : {
421 299997 : if (!_enabled) {
422 299801 : return;
423 : }
424 :
425 : // manipulate pitmode if pitmode-on-disarm or power-on-arm is set
426 196 : if (has_option(VideoOptions::VTX_PITMODE_ON_DISARM) || has_option(VideoOptions::VTX_PITMODE_UNTIL_ARM)) {
427 0 : if (hal.util->get_soft_armed() && has_option(VideoOptions::VTX_PITMODE)) {
428 0 : _options.set(_options & ~uint8_t(VideoOptions::VTX_PITMODE));
429 0 : } else if (!hal.util->get_soft_armed() && !has_option(VideoOptions::VTX_PITMODE)
430 0 : && has_option(VideoOptions::VTX_PITMODE_ON_DISARM)) {
431 0 : _options.set(_options | uint8_t(VideoOptions::VTX_PITMODE));
432 : }
433 : }
434 : // check that the requested power is actually allowed
435 : // reset if not
436 196 : if (_power_mw != get_power_mw()) {
437 22 : if (_power_levels[find_current_power()].active == PowerActive::Inactive) {
438 : // reset to something we know works
439 : debug("power reset to %dmw from %dmw", get_power_mw(), _power_mw.get());
440 0 : _power_mw.set_and_save(get_power_mw());
441 : }
442 : }
443 : }
444 :
445 0 : bool AP_VideoTX::update_options() const
446 : {
447 0 : if (!_defaults_set) {
448 0 : return false;
449 : }
450 : // check pitmode
451 0 : if ((_options & uint8_t(VideoOptions::VTX_PITMODE))
452 0 : != (_current_options & uint8_t(VideoOptions::VTX_PITMODE))) {
453 0 : return true;
454 : }
455 :
456 : #if HAL_CRSF_TELEM_ENABLED
457 : // using CRSF so unlock is not an option
458 0 : if (AP::crsf_telem() != nullptr) {
459 0 : return false;
460 : }
461 : #endif
462 : // check unlock only
463 0 : if ((_options & uint8_t(VideoOptions::VTX_UNLOCKED)) != 0
464 0 : && (_current_options & uint8_t(VideoOptions::VTX_UNLOCKED)) == 0) {
465 0 : return true;
466 : }
467 :
468 : // ignore everything else
469 0 : return false;
470 : }
471 :
472 0 : bool AP_VideoTX::update_power() const {
473 0 : if (!_defaults_set || _power_mw == get_power_mw() || get_pitmode()) {
474 0 : return false;
475 : }
476 : // check that the requested power is actually allowed
477 0 : for (uint8_t i = 0; i < VTX_MAX_POWER_LEVELS; i++) {
478 0 : if (_power_mw == _power_levels[i].mw
479 0 : && _power_levels[i].active != PowerActive::Inactive) {
480 0 : return true;
481 : }
482 : }
483 : // Tramp accepts arbitrary mW; the table check above is SmartAudio-only
484 0 : if (_power_mw > 0 && is_provider_enabled(VTXType::Tramp)) {
485 0 : return true;
486 : }
487 : // asked for something unsupported - only SA2.1 allows this and will have already provided a list
488 0 : return false;
489 : }
490 :
491 1748 : bool AP_VideoTX::have_params_changed() const
492 : {
493 : return _enabled
494 1748 : && (update_power()
495 0 : || update_band()
496 0 : || update_channel()
497 0 : || update_frequency()
498 3496 : || update_options());
499 : }
500 :
501 : // update the configured frequency to match the channel and band
502 0 : void AP_VideoTX::update_configured_frequency()
503 : {
504 0 : _frequency_mhz.set_and_save(get_frequency_mhz(_band, _channel));
505 0 : }
506 :
507 : // update the configured channel and band to match the frequency
508 0 : void AP_VideoTX::update_configured_channel_and_band()
509 : {
510 : VideoBand band;
511 : uint8_t channel;
512 0 : if (get_band_and_channel(_frequency_mhz, band, channel)) {
513 0 : _band.set_and_save(band);
514 0 : _channel.set_and_save(channel);
515 : } else {
516 0 : update_configured_frequency();
517 : }
518 0 : }
519 :
520 : // set the current configured values if not currently set in storage
521 : // this is necessary so that the current settings can be seen
522 10 : bool AP_VideoTX::set_defaults()
523 : {
524 10 : if (_defaults_set) {
525 8 : return false;
526 : }
527 :
528 : // check that our current view of frequency matches band/channel
529 : // if not then force one to be correct
530 2 : uint16_t calced_freq = get_frequency_mhz(_current_band, _current_channel);
531 2 : if (_current_frequency != calced_freq) {
532 2 : if (_current_frequency > 0) {
533 : VideoBand band;
534 : uint8_t channel;
535 0 : if (get_band_and_channel(_current_frequency, band, channel)) {
536 0 : _current_band = band;
537 0 : _current_channel = channel;
538 : } else {
539 0 : _current_frequency = calced_freq;
540 : }
541 : } else {
542 2 : _current_frequency = calced_freq;
543 : }
544 : }
545 :
546 2 : if (!_options.configured()) {
547 2 : _options.set_and_save(_current_options);
548 : }
549 2 : if (!_channel.configured()) {
550 0 : _channel.set_and_save(_current_channel);
551 : }
552 2 : if (!_band.configured()) {
553 0 : _band.set_and_save(_current_band);
554 : }
555 2 : if (!_power_mw.configured()) {
556 0 : _power_mw.set_and_save(get_power_mw());
557 : }
558 2 : if (!_frequency_mhz.configured()) {
559 0 : _frequency_mhz.set_and_save(_current_frequency);
560 : }
561 :
562 : // Now check that the user didn't screw up by selecting incompatible options
563 2 : if (_frequency_mhz != get_frequency_mhz(_band, _channel)) {
564 0 : if (_frequency_mhz > 0) {
565 0 : update_configured_channel_and_band();
566 : } else {
567 0 : update_configured_frequency();
568 : }
569 : }
570 :
571 2 : _defaults_set = true;
572 :
573 2 : announce_vtx_settings();
574 :
575 2 : return true;
576 : }
577 :
578 10 : void AP_VideoTX::announce_vtx_settings() const
579 : {
580 : // Output a friendly message so the user knows the VTX has been detected
581 10 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "VTX: %s%d %dMHz, PWR: %dmW",
582 : band_names[_band.get()], _channel.get() + 1, _frequency_mhz.get(),
583 : has_option(VideoOptions::VTX_PITMODE) ? 0 : _power_mw.get());
584 10 : }
585 :
586 : // change the video power based on switch input
587 : // 6-pos range is in the middle of the available range
588 0 : void AP_VideoTX::change_power(int8_t position)
589 : {
590 0 : if (!_enabled || position < 0 || position > 5) {
591 0 : return;
592 : }
593 : // first find out how many possible levels there are
594 0 : uint8_t num_active_levels = 0;
595 0 : for (uint8_t i = 0; i < VTX_MAX_POWER_LEVELS; i++) {
596 0 : if (_power_levels[i].active != PowerActive::Inactive && _power_levels[i].mw <= _max_power_mw) {
597 0 : num_active_levels++;
598 : }
599 : }
600 : // iterate through to find the level
601 0 : uint16_t level = constrain_int16(roundf((num_active_levels * (position + 1)/ 6.0f) - 1), 0, num_active_levels - 1);
602 : debug("looking for pos %d power level %d from %d", position, level, num_active_levels);
603 0 : uint16_t power = 0;
604 0 : for (uint8_t i = 0, j = 0; i < num_active_levels; i++, j++) {
605 0 : while (j < VTX_MAX_POWER_LEVELS-1 && _power_levels[j].active == PowerActive::Inactive) {
606 0 : j++;
607 : }
608 0 : if (i == level) {
609 0 : power = _power_levels[j].mw;
610 : debug("selected power %dmw", power);
611 0 : break;
612 : }
613 : }
614 :
615 0 : if (position == 5 && power < _max_power_mw) {
616 0 : power = _max_power_mw;
617 : debug("selected power %dmw", power);
618 : }
619 :
620 0 : if (power == 0) {
621 0 : if (!hal.util->get_soft_armed()) { // don't allow pitmode to be entered if already armed
622 0 : set_configured_options(get_configured_options() | uint8_t(VideoOptions::VTX_PITMODE));
623 : }
624 : } else {
625 0 : if (has_option(VideoOptions::VTX_PITMODE)) {
626 0 : set_configured_options(get_configured_options() & ~uint8_t(VideoOptions::VTX_PITMODE));
627 : }
628 0 : set_configured_power_mw(power);
629 : }
630 : }
631 :
632 : namespace AP {
633 6872 : AP_VideoTX& vtx() {
634 6872 : return *AP_VideoTX::get_singleton();
635 : }
636 : };
637 :
638 : #endif
|
