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_RSSI_config.h"
17 :
18 : #if AP_RSSI_ENABLED
19 :
20 : #include <AP_RSSI/AP_RSSI.h>
21 : #include <GCS_MAVLink/GCS.h>
22 : #include <RC_Channel/RC_Channel.h>
23 :
24 : #include <utility>
25 :
26 : extern const AP_HAL::HAL& hal;
27 :
28 : #ifndef BOARD_RSSI_DEFAULT
29 : #define BOARD_RSSI_DEFAULT 0
30 : #endif
31 :
32 : #ifndef BOARD_RSSI_ANA_PIN
33 : #define BOARD_RSSI_ANA_PIN -1
34 : #endif
35 :
36 : #ifndef BOARD_RSSI_ANA_PIN_HIGH
37 : #define BOARD_RSSI_ANA_PIN_HIGH 5.0f
38 : #endif
39 :
40 : const AP_Param::GroupInfo AP_RSSI::var_info[] = {
41 :
42 : // @Param: TYPE
43 : // @DisplayName: RSSI Type
44 : // @Description: Radio Receiver RSSI type. If your radio receiver supports RSSI of some kind, set it here, then set its associated RSSI_XXXXX parameters, if any.
45 : // @Values: 0:Disabled,1:AnalogPin,2:RCChannelPwmValue,3:ReceiverProtocol,4:PWMInputPin,5:TelemetryRadioRSSI
46 : // @User: Standard
47 : AP_GROUPINFO_FLAGS("TYPE", 0, AP_RSSI, rssi_type, BOARD_RSSI_DEFAULT, AP_PARAM_FLAG_ENABLE),
48 :
49 : // @Param: ANA_PIN
50 : // @DisplayName: Receiver RSSI sensing pin
51 : // @Description: Pin used to read the RSSI voltage or PWM value. Analog Airspeed ports can be used for Analog inputs (some autopilots provide others also), Non-IOMCU Servo/MotorOutputs can be used for PWM input when configured as "GPIOs". Values for some autopilots are given as examples. Search wiki for "Analog pins" for analog pin or "GPIOs", if PWM input type, to determine pin number.
52 : // @Values: 8:V5 Nano,11:Pixracer,13:Pixhawk ADC4,14:Pixhawk ADC3,15:Pixhawk ADC6/Pixhawk2 ADC,50:AUX1,51:AUX2,52:AUX3,53:AUX4,54:AUX5,55:AUX6,103:Pixhawk SBUS
53 : // @Range: -1 127
54 : // @User: Standard
55 : AP_GROUPINFO("ANA_PIN", 1, AP_RSSI, rssi_analog_pin, BOARD_RSSI_ANA_PIN),
56 :
57 : // @Param: PIN_LOW
58 : // @DisplayName: RSSI pin's lowest voltage
59 : // @Description: RSSI pin's voltage received on the RSSI_ANA_PIN when the signal strength is the weakest. Some radio receivers put out inverted values so this value may be higher than RSSI_PIN_HIGH. When using pin 103, the maximum value of the parameter is 3.3V.
60 : // @Units: V
61 : // @Increment: 0.01
62 : // @Range: 0 5.0
63 : // @User: Standard
64 : AP_GROUPINFO("PIN_LOW", 2, AP_RSSI, rssi_analog_pin_range_low, 0.0f),
65 :
66 : // @Param: PIN_HIGH
67 : // @DisplayName: RSSI pin's highest voltage
68 : // @Description: RSSI pin's voltage received on the RSSI_ANA_PIN when the signal strength is the strongest. Some radio receivers put out inverted values so this value may be lower than RSSI_PIN_LOW. When using pin 103, the maximum value of the parameter is 3.3V.
69 : // @Units: V
70 : // @Increment: 0.01
71 : // @Range: 0 5.0
72 : // @User: Standard
73 : AP_GROUPINFO("PIN_HIGH", 3, AP_RSSI, rssi_analog_pin_range_high, BOARD_RSSI_ANA_PIN_HIGH),
74 :
75 : // @Param: CHANNEL
76 : // @DisplayName: Receiver RSSI channel number
77 : // @Description: The channel number where RSSI will be output by the radio receiver (5 and above).
78 : // @Range: 0 16
79 : // @User: Standard
80 : AP_GROUPINFO("CHANNEL", 4, AP_RSSI, rssi_channel, 0),
81 :
82 : // @Param: CHAN_LOW
83 : // @DisplayName: RSSI PWM low value
84 : // @Description: PWM value that the radio receiver will put on the RSSI_CHANNEL or RSSI_ANA_PIN when the signal strength is the weakest. Some radio receivers output inverted values so this value may be lower than RSSI_CHAN_HIGH
85 : // @Units: PWM
86 : // @Range: 0 2000
87 : // @User: Standard
88 : AP_GROUPINFO("CHAN_LOW", 5, AP_RSSI, rssi_channel_low_pwm_value, 1000),
89 :
90 : // @Param: CHAN_HIGH
91 : // @DisplayName: Receiver RSSI PWM high value
92 : // @Description: PWM value that the radio receiver will put on the RSSI_CHANNEL or RSSI_ANA_PIN when the signal strength is the strongest. Some radio receivers output inverted values so this value may be higher than RSSI_CHAN_LOW
93 : // @Units: PWM
94 : // @Range: 0 2000
95 : // @User: Standard
96 : AP_GROUPINFO("CHAN_HIGH", 6, AP_RSSI, rssi_channel_high_pwm_value, 2000),
97 :
98 : AP_GROUPEND
99 : };
100 :
101 : // Public
102 : // ------
103 :
104 : // constructor
105 2098 : AP_RSSI::AP_RSSI()
106 : {
107 2098 : AP_Param::setup_object_defaults(this, var_info);
108 2098 : if (_singleton) {
109 0 : AP_HAL::panic("Too many RSSI sensors");
110 : }
111 2098 : _singleton = this;
112 2098 : }
113 :
114 : // destructor
115 458 : AP_RSSI::~AP_RSSI(void)
116 : {
117 458 : }
118 :
119 : /*
120 : * Get the AP_RSSI singleton
121 : */
122 1122329 : AP_RSSI *AP_RSSI::get_singleton()
123 : {
124 1122329 : return _singleton;
125 : }
126 :
127 : // Initialize the rssi object and prepare it for use
128 1978 : void AP_RSSI::init()
129 : {
130 : // a pin for reading the receiver RSSI voltage. The scaling by 0.25
131 : // is to take the 0 to 1024 range down to an 8 bit range for MAVLink
132 1978 : rssi_analog_source = hal.analogin->channel(ANALOG_INPUT_NONE);
133 1978 : }
134 :
135 : // Read the receiver RSSI value as a float 0.0f - 1.0f.
136 : // 0.0 represents weakest signal, 1.0 represents maximum signal.
137 600 : float AP_RSSI::read_receiver_rssi()
138 : {
139 600 : switch (RssiType(rssi_type.get())) {
140 600 : case RssiType::TYPE_DISABLED:
141 600 : return 0.0f;
142 0 : case RssiType::ANALOG_PIN:
143 0 : return read_pin_rssi();
144 0 : case RssiType::RC_CHANNEL_VALUE:
145 0 : return read_channel_rssi();
146 0 : case RssiType::RECEIVER: {
147 0 : int16_t rssi = RC_Channels::get_receiver_rssi();
148 0 : if (rssi != -1) {
149 0 : return rssi * (1/255.0);
150 : }
151 0 : return 0.0f;
152 : }
153 0 : case RssiType::PWM_PIN:
154 0 : return read_pwm_pin_rssi();
155 0 : case RssiType::TELEMETRY_RADIO_RSSI:
156 0 : return read_telemetry_radio_rssi();
157 : }
158 : // should never get to here
159 0 : return 0.0f;
160 : }
161 :
162 : // Only valid for RECEIVER type RSSI selections. Returns -1 if protocol does not provide link quality report.
163 0 : float AP_RSSI::read_receiver_link_quality()
164 : {
165 0 : if (RssiType(rssi_type.get()) == RssiType::RECEIVER) {
166 0 : return RC_Channels::get_receiver_link_quality();
167 : }
168 0 : return -1;
169 : }
170 :
171 : // Read the receiver RSSI value as an 8-bit integer
172 : // 0 represents weakest signal, 255 represents maximum signal.
173 52 : uint8_t AP_RSSI::read_receiver_rssi_uint8()
174 : {
175 52 : return read_receiver_rssi() * 255;
176 : }
177 :
178 : // Private
179 : // -------
180 :
181 : // read the RSSI value from an analog pin - returns float in range 0.0 to 1.0
182 0 : float AP_RSSI::read_pin_rssi()
183 : {
184 0 : if (!rssi_analog_source || !rssi_analog_source->set_pin(rssi_analog_pin)) {
185 0 : return 0;
186 : }
187 0 : float current_analog_voltage = rssi_analog_source->voltage_average();
188 :
189 0 : return scale_and_constrain_float_rssi(current_analog_voltage, rssi_analog_pin_range_low, rssi_analog_pin_range_high);
190 : }
191 :
192 : // read the RSSI value from a PWM value on a RC channel
193 0 : float AP_RSSI::read_channel_rssi()
194 : {
195 0 : RC_Channel *c = rc().channel(rssi_channel-1);
196 0 : if (c == nullptr) {
197 0 : return 0.0f;
198 : }
199 0 : uint16_t rssi_channel_value = c->get_radio_in();
200 0 : float channel_rssi = scale_and_constrain_float_rssi(rssi_channel_value, rssi_channel_low_pwm_value, rssi_channel_high_pwm_value);
201 0 : return channel_rssi;
202 : }
203 :
204 : // read the PWM value from a pin
205 0 : float AP_RSSI::read_pwm_pin_rssi()
206 : {
207 : // check if pin has changed and configure interrupt handlers if required:
208 0 : if (!pwm_state.pwm_source.set_pin(rssi_analog_pin, "RSSI")) {
209 : // disabled (either by configuration or failure to attach interrupt)
210 0 : return 0.0f;
211 : }
212 :
213 0 : uint16_t pwm_us = pwm_state.pwm_source.get_pwm_us();
214 :
215 0 : const uint32_t now = AP_HAL::millis();
216 0 : if (pwm_us == 0) {
217 : // no reading; check for timeout:
218 0 : if (now - pwm_state.last_reading_ms > 1000) {
219 : // no reading for a second - something is broken
220 0 : pwm_state.rssi_value = 0.0f;
221 : }
222 : } else {
223 : // a new reading - convert pwm value to rssi value
224 0 : pwm_state.rssi_value = scale_and_constrain_float_rssi(pwm_us, rssi_channel_low_pwm_value, rssi_channel_high_pwm_value);
225 0 : pwm_state.last_reading_ms = now;
226 : }
227 :
228 0 : return pwm_state.rssi_value;
229 : }
230 :
231 0 : float AP_RSSI::read_telemetry_radio_rssi()
232 : {
233 : #if HAL_GCS_ENABLED
234 0 : return GCS_MAVLINK::telemetry_radio_rssi();
235 : #else
236 : return 0;
237 : #endif
238 : }
239 :
240 : // Scale and constrain a float rssi value to 0.0 to 1.0 range
241 0 : float AP_RSSI::scale_and_constrain_float_rssi(float current_rssi_value, float low_rssi_range, float high_rssi_range)
242 : {
243 0 : float rssi_value_range = fabsf(high_rssi_range - low_rssi_range);
244 0 : if (is_zero(rssi_value_range)) {
245 : // User range isn't meaningful, return 0 for RSSI (and avoid divide by zero)
246 0 : return 0.0f;
247 : }
248 : // Note that user-supplied ranges may be inverted and we accommodate that here.
249 : // (Some radio receivers put out inverted ranges for RSSI-type values).
250 0 : bool range_is_inverted = (high_rssi_range < low_rssi_range);
251 : // Constrain to the possible range - values outside are clipped to ends
252 0 : current_rssi_value = constrain_float(current_rssi_value,
253 : range_is_inverted ? high_rssi_range : low_rssi_range,
254 : range_is_inverted ? low_rssi_range : high_rssi_range);
255 :
256 0 : if (range_is_inverted)
257 : {
258 : // Swap values so we can treat them as low->high uniformly in the code that follows
259 0 : current_rssi_value = high_rssi_range + fabsf(current_rssi_value - low_rssi_range);
260 0 : std::swap(low_rssi_range, high_rssi_range);
261 : }
262 :
263 : // Scale the value down to a 0.0 - 1.0 range
264 0 : float rssi_value_scaled = (current_rssi_value - low_rssi_range) / rssi_value_range;
265 : // Make absolutely sure the value is clipped to the 0.0 - 1.0 range. This should handle things if the
266 : // value retrieved falls outside the user-supplied range.
267 0 : return constrain_float(rssi_value_scaled, 0.0f, 1.0f);
268 : }
269 :
270 : AP_RSSI *AP_RSSI::_singleton = nullptr;
271 :
272 : namespace AP {
273 :
274 1121729 : AP_RSSI *rssi()
275 : {
276 1121729 : return AP_RSSI::get_singleton();
277 : }
278 :
279 : };
280 :
281 : #endif // AP_RSSI_ENABLED
|
