Line data Source code
1 : /*
2 : * This file is free software: you can redistribute it and/or modify it
3 : * under the terms of the GNU General Public License as published by the
4 : * Free Software Foundation, either version 3 of the License, or
5 : * (at your option) any later version.
6 : *
7 : * This file is distributed in the hope that it will be useful, but
8 : * WITHOUT ANY WARRANTY; without even the implied warranty of
9 : * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
10 : * See the GNU General Public License for more details.
11 : *
12 : * You should have received a copy of the GNU General Public License along
13 : * with this program. If not, see <http://www.gnu.org/licenses/>.
14 : *
15 : * Code by Andrew Tridgell and Siddharth Bharat Purohit
16 : */
17 :
18 : #include "AP_RCProtocol_config.h"
19 :
20 : #if AP_RCPROTOCOL_ENABLED
21 :
22 : #include "AP_RCProtocol.h"
23 : #include <AP_Math/AP_Math.h>
24 : #include <RC_Channel/RC_Channel.h>
25 : #include <AP_Vehicle/AP_Vehicle_Type.h>
26 : #include <AP_Logger/AP_Logger.h>
27 : #include <AP_VideoTX/AP_VideoTX_config.h>
28 :
29 : // for video TX configuration:
30 : #if AP_VIDEOTX_ENABLED
31 : #include <AP_VideoTX/AP_VideoTX.h>
32 : #include "spm_srxl.h"
33 : #endif
34 :
35 :
36 :
37 37692 : AP_RCProtocol_Backend::AP_RCProtocol_Backend(AP_RCProtocol &_frontend) :
38 37692 : frontend(_frontend)
39 37692 : {}
40 :
41 36440332 : bool AP_RCProtocol_Backend::new_input()
42 : {
43 36440332 : bool ret = rc_input_count != last_rc_input_count;
44 36440332 : if (ret) {
45 7501439 : last_rc_input_count = rc_input_count;
46 : }
47 36440332 : return ret;
48 : }
49 :
50 1310180 : uint8_t AP_RCProtocol_Backend::num_channels() const
51 : {
52 1310180 : return _num_channels;
53 : }
54 :
55 119953024 : uint16_t AP_RCProtocol_Backend::read(uint8_t chan)
56 : {
57 119953024 : return _pwm_values[chan];
58 : }
59 :
60 0 : void AP_RCProtocol_Backend::read(uint16_t *pwm, uint8_t n)
61 : {
62 0 : if (n >= MAX_RCIN_CHANNELS) {
63 0 : n = MAX_RCIN_CHANNELS;
64 : }
65 0 : memcpy(pwm, _pwm_values, n*sizeof(pwm[0]));
66 0 : }
67 :
68 : /*
69 : provide input from a backend
70 : */
71 7510171 : void AP_RCProtocol_Backend::add_input(uint8_t num_values, uint16_t *values, bool in_failsafe, int16_t _rssi, int16_t _rx_link_quality)
72 : {
73 7510171 : num_values = MIN(num_values, MAX_RCIN_CHANNELS);
74 7510171 : memcpy(_pwm_values, values, num_values*sizeof(uint16_t));
75 7510171 : _num_channels = num_values;
76 7510171 : rc_frame_count++;
77 7510171 : frontend.set_failsafe_active(in_failsafe);
78 : #if !AP_RC_CHANNEL_ENABLED
79 : // failsafed is sorted out in AP_IOMCU.cpp
80 : in_failsafe = false;
81 : #else
82 7510171 : if (rc().option_is_enabled(RC_Channels::Option::IGNORE_FAILSAFE)) {
83 0 : in_failsafe = false;
84 : }
85 : #endif
86 7510171 : if (!in_failsafe) {
87 7508631 : rc_input_count++;
88 : }
89 7510171 : rssi = _rssi;
90 7510171 : rx_link_quality = _rx_link_quality;
91 7510171 : }
92 :
93 :
94 : /*
95 : decode channels from the standard 11bit format (used by CRSF, SBUS, FPort and FPort2)
96 : must be used on multiples of 8 channels
97 : */
98 2560 : void AP_RCProtocol_Backend::decode_11bit_channels(const uint8_t* data, uint8_t nchannels, uint16_t *values, uint16_t mult, uint16_t div, uint16_t offset)
99 : {
100 : #define CHANNEL_SCALE(x) ((int32_t(x) * mult) / div + offset)
101 5120 : while (nchannels >= 8) {
102 2560 : const Channels11Bit_8Chan* channels = (const Channels11Bit_8Chan*)data;
103 2560 : values[0] = CHANNEL_SCALE(channels->ch0);
104 2560 : values[1] = CHANNEL_SCALE(channels->ch1);
105 2560 : values[2] = CHANNEL_SCALE(channels->ch2);
106 2560 : values[3] = CHANNEL_SCALE(channels->ch3);
107 2560 : values[4] = CHANNEL_SCALE(channels->ch4);
108 2560 : values[5] = CHANNEL_SCALE(channels->ch5);
109 2560 : values[6] = CHANNEL_SCALE(channels->ch6);
110 2560 : values[7] = CHANNEL_SCALE(channels->ch7);
111 :
112 2560 : nchannels -= 8;
113 2560 : data += sizeof(*channels);
114 2560 : values += 8;
115 : }
116 2560 : }
117 :
118 : #if AP_VIDEOTX_ENABLED
119 : // configure the video transmitter, the input values are Spektrum-oriented
120 0 : void AP_RCProtocol_Backend::configure_vtx(uint8_t band, uint8_t channel, uint8_t power, uint8_t pitmode)
121 : {
122 0 : AP_VideoTX& vtx = AP::vtx();
123 : // VTX Band (0 = Fatshark, 1 = Raceband, 2 = E, 3 = B, 4 = A)
124 : // map to TBS band A, B, E, Race, Airwave, LoRace
125 0 : switch (band) {
126 0 : case VTX_BAND_FATSHARK:
127 0 : vtx.set_configured_band(AP_VideoTX::VideoBand::FATSHARK);
128 0 : break;
129 0 : case VTX_BAND_RACEBAND:
130 0 : vtx.set_configured_band(AP_VideoTX::VideoBand::RACEBAND);
131 0 : break;
132 0 : case VTX_BAND_E_BAND:
133 0 : vtx.set_configured_band(AP_VideoTX::VideoBand::BAND_E);
134 0 : break;
135 0 : case VTX_BAND_B_BAND:
136 0 : vtx.set_configured_band(AP_VideoTX::VideoBand::BAND_B);
137 0 : break;
138 0 : case VTX_BAND_A_BAND:
139 0 : vtx.set_configured_band(AP_VideoTX::VideoBand::BAND_A);
140 0 : break;
141 0 : default:
142 0 : break;
143 : }
144 : // VTX Channel (0-7)
145 0 : vtx.set_configured_channel(channel);
146 0 : if (pitmode) {
147 0 : vtx.set_configured_options(vtx.get_options() | uint8_t(AP_VideoTX::VideoOptions::VTX_PITMODE));
148 : } else {
149 0 : vtx.set_configured_options(vtx.get_options() & ~uint8_t(AP_VideoTX::VideoOptions::VTX_PITMODE));
150 : }
151 :
152 0 : switch (power) {
153 0 : case VTX_POWER_1MW_14MW:
154 : case VTX_POWER_15MW_25MW:
155 0 : vtx.set_configured_power_mw(25);
156 0 : break;
157 0 : case VTX_POWER_26MW_99MW:
158 : case VTX_POWER_100MW_299MW:
159 0 : vtx.set_configured_power_mw(100);
160 0 : break;
161 0 : case VTX_POWER_300MW_600MW:
162 0 : vtx.set_configured_power_mw(400);
163 0 : break;
164 0 : case VTX_POWER_601_PLUS:
165 0 : vtx.set_configured_power_mw(800);
166 0 : break;
167 0 : default:
168 0 : break;
169 : }
170 0 : }
171 : #endif // AP_VIDEOTX_ENABLED
172 :
173 : /*
174 : optionally log RC input data
175 : */
176 0 : void AP_RCProtocol_Backend::log_data(AP_RCProtocol::rcprotocol_t prot, uint32_t timestamp, const uint8_t *data, uint8_t len) const
177 : {
178 : #if HAL_LOGGING_ENABLED && AP_RC_CHANNEL_ENABLED
179 :
180 : #if (CONFIG_HAL_BOARD == HAL_BOARD_SITL || CONFIG_HAL_BOARD == HAL_BOARD_LINUX)
181 0 : if (RC_Channels::get_singleton() == nullptr) { // allow running without RC_Channels if we are doing the examples
182 0 : return;
183 : }
184 : #endif
185 0 : if (rc().option_is_enabled(RC_Channels::Option::LOG_RAW_DATA)) {
186 0 : uint32_t u32[10] {};
187 0 : if (len > sizeof(u32)) {
188 0 : len = sizeof(u32);
189 : }
190 0 : memcpy(u32, data, len);
191 : // @LoggerMessage: RCDA
192 : // @Description: Raw RC data
193 : // @Field: TimeUS: Time since system startup
194 : // @Field: TS: data arrival timestamp
195 : // @Field: Prot: Protocol currently being decoded
196 : // @Field: Len: Number of valid bytes in message
197 : // @Field: U0: first quartet of bytes
198 : // @Field: U1: second quartet of bytes
199 : // @Field: U2: third quartet of bytes
200 : // @Field: U3: fourth quartet of bytes
201 : // @Field: U4: fifth quartet of bytes
202 : // @Field: U5: sixth quartet of bytes
203 : // @Field: U6: seventh quartet of bytes
204 : // @Field: U7: eight quartet of bytes
205 : // @Field: U8: ninth quartet of bytes
206 : // @Field: U9: tenth quartet of bytes
207 0 : AP::logger().WriteStreaming("RCDA", "TimeUS,TS,Prot,Len,U0,U1,U2,U3,U4,U5,U6,U7,U8,U9", "QIBBIIIIIIIIII",
208 : AP_HAL::micros64(),
209 : timestamp,
210 : (uint8_t)prot,
211 : len,
212 : u32[0], u32[1], u32[2], u32[3], u32[4],
213 : u32[5], u32[6], u32[7], u32[8], u32[9]);
214 : }
215 : #endif // HAL_LOGGING_ENABLED && AP_RC_CHANNEL_ENABLED
216 : }
217 :
218 : #endif // AP_RCPROTOCOL_ENABLED
|
