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_RCTelemetry_config.h"
17 :
18 : #if AP_GHST_TELEM_ENABLED
19 :
20 : #include "AP_GHST_Telem.h"
21 : #include <AP_VideoTX/AP_VideoTX.h>
22 : #include <AP_HAL/utility/sparse-endian.h>
23 : #include <AP_BattMonitor/AP_BattMonitor.h>
24 : #include <AP_GPS/AP_GPS.h>
25 : #include <GCS_MAVLink/GCS.h>
26 : #include <AP_RCProtocol/AP_RCProtocol_GHST.h>
27 : #include <AP_SerialManager/AP_SerialManager.h>
28 : #include <AP_Compass/AP_Compass.h>
29 : #include <AP_Baro/AP_Baro.h>
30 : #include <AP_AHRS/AP_AHRS.h>
31 : #include <AP_Notify/AP_Notify.h>
32 : #include <math.h>
33 : #include <stdio.h>
34 : #include <AP_HAL/AP_HAL.h>
35 : #include <AP_Vehicle/AP_Vehicle_Type.h>
36 :
37 : //#define GHST_DEBUG
38 : #ifdef GHST_DEBUG
39 : # define debug(fmt, args...) hal.console->printf("GHST: " fmt "\n", ##args)
40 : #else
41 : # define debug(fmt, args...) do {} while(0)
42 : #endif
43 :
44 : extern const AP_HAL::HAL& hal;
45 :
46 : AP_GHST_Telem *AP_GHST_Telem::singleton;
47 :
48 0 : AP_GHST_Telem::AP_GHST_Telem() : AP_RCTelemetry(0)
49 : {
50 0 : singleton = this;
51 0 : }
52 :
53 0 : AP_GHST_Telem::~AP_GHST_Telem(void)
54 : {
55 0 : singleton = nullptr;
56 0 : }
57 :
58 0 : bool AP_GHST_Telem::init(void)
59 : {
60 : // sanity check that we are using a UART for RC input
61 0 : if (!AP::serialmanager().have_serial(AP_SerialManager::SerialProtocol_RCIN, 0)) {
62 0 : return false;
63 : }
64 :
65 0 : return AP_RCTelemetry::init();
66 : }
67 :
68 : /*
69 : setup ready for passthrough telem
70 : */
71 0 : void AP_GHST_Telem::setup_wfq_scheduler(void)
72 : {
73 : // initialize packet weights for the WFQ scheduler
74 : // priority[i] = 1/_scheduler.packet_weight[i]
75 : // rate[i] = LinkRate * ( priority[i] / (sum(priority[1-n])) )
76 :
77 : // CRSF telemetry rate is 150Hz (4ms) max, so these rates must fit
78 0 : add_scheduler_entry(50, 120); // Attitude and compass 8Hz
79 0 : add_scheduler_entry(200, 1000); // VTX parameters 1Hz
80 0 : add_scheduler_entry(1300, 500); // battery 2Hz
81 0 : add_scheduler_entry(550, 280); // GPS 3Hz
82 0 : add_scheduler_entry(550, 280); // GPS2 3Hz
83 0 : }
84 :
85 0 : void AP_GHST_Telem::update_custom_telemetry_rates(AP_RCProtocol_GHST::RFMode rf_mode)
86 : {
87 0 : if (is_high_speed_telemetry(rf_mode)) {
88 : // standard telemetry for high data rates
89 0 : set_scheduler_entry(BATTERY, 1000, 1000); // 1Hz
90 0 : set_scheduler_entry(ATTITUDE, 1000, 1000); // 1Hz
91 : // custom telemetry for high data rates
92 0 : set_scheduler_entry(GPS, 550, 500); // 2.0Hz
93 0 : set_scheduler_entry(GPS2, 550, 500); // 2.0Hz
94 : } else {
95 : // standard telemetry for low data rates
96 0 : set_scheduler_entry(BATTERY, 1000, 2000); // 0.5Hz
97 0 : set_scheduler_entry(ATTITUDE, 1000, 3000); // 0.33Hz
98 : // GHST custom telemetry for low data rates
99 0 : set_scheduler_entry(GPS, 550, 1000); // 1.0Hz
100 0 : set_scheduler_entry(GPS2, 550, 1000); // 1.0Hz
101 : }
102 0 : }
103 :
104 0 : bool AP_GHST_Telem::process_rf_mode_changes()
105 : {
106 0 : const AP_RCProtocol_GHST::RFMode current_rf_mode = get_rf_mode();
107 0 : uint32_t now = AP_HAL::millis();
108 :
109 0 : AP_RCProtocol_GHST* ghost = AP::ghost();
110 0 : AP_HAL::UARTDriver* uart = nullptr;
111 0 : if (ghost != nullptr) {
112 0 : uart = ghost->get_UART();
113 : }
114 :
115 0 : if (uart == nullptr) {
116 0 : return true;
117 : }
118 :
119 0 : if (!ghost->is_detected()) {
120 0 : return false;
121 : }
122 : // not ready yet
123 0 : if (!uart->is_initialized()) {
124 0 : return false;
125 : }
126 : #if !defined (STM32H7)
127 : // warn the user if their setup is sub-optimal, H7 does not need DMA on serial port
128 0 : if (_telem_bootstrap_msg_pending && !uart->is_dma_enabled()) {
129 0 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "%s: running on non-DMA serial port", get_protocol_string());
130 : }
131 : #endif
132 : // note if option was set to show LQ in place of RSSI
133 0 : bool current_lq_as_rssi_active = rc().option_is_enabled(RC_Channels::Option::USE_CRSF_LQ_AS_RSSI);
134 0 : if(_telem_bootstrap_msg_pending || _noted_lq_as_rssi_active != current_lq_as_rssi_active){
135 0 : _noted_lq_as_rssi_active = current_lq_as_rssi_active;
136 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s: RSSI now displays %s", get_protocol_string(), current_lq_as_rssi_active ? " as LQ" : "normally");
137 : }
138 0 : _telem_bootstrap_msg_pending = false;
139 :
140 0 : const bool is_high_speed = is_high_speed_telemetry(current_rf_mode);
141 0 : if ((now - _telem_last_report_ms > 5000)) {
142 : // report an RF mode change or a change in telemetry rate if we haven't done so in the last 5s
143 0 : if (!rc().option_is_enabled(RC_Channels::Option::SUPPRESS_CRSF_MESSAGE) && (_rf_mode != current_rf_mode)) {
144 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s: Link rate %dHz, Telemetry %s",
145 : get_protocol_string(), ghost->get_link_rate(), _enable_telemetry ? "ON" : "OFF");
146 : }
147 : // tune the scheduler based on telemetry speed high/low transitions
148 0 : if (_telem_is_high_speed != is_high_speed) {
149 0 : update_custom_telemetry_rates(current_rf_mode);
150 : }
151 0 : _telem_is_high_speed = is_high_speed;
152 0 : _rf_mode = current_rf_mode;
153 0 : _telem_last_avg_rate = _scheduler.avg_packet_rate;
154 0 : if (_telem_last_report_ms == 0) { // only want to show bootstrap messages once
155 0 : _telem_bootstrap_msg_pending = true;
156 : }
157 0 : _telem_last_report_ms = now;
158 : }
159 0 : return true;
160 : }
161 :
162 0 : AP_RCProtocol_GHST::RFMode AP_GHST_Telem::get_rf_mode() const
163 : {
164 0 : AP_RCProtocol_GHST* ghost = AP::ghost();
165 0 : if (ghost == nullptr) {
166 0 : return AP_RCProtocol_GHST::RFMode::RF_MODE_UNKNOWN;
167 : }
168 :
169 0 : return static_cast<AP_RCProtocol_GHST::RFMode>(ghost->get_link_status().rf_mode);
170 : }
171 :
172 0 : bool AP_GHST_Telem::is_high_speed_telemetry(const AP_RCProtocol_GHST::RFMode rf_mode) const
173 : {
174 0 : return rf_mode == AP_RCProtocol_GHST::RFMode::GHST_RF_MODE_RACE || rf_mode == AP_RCProtocol_GHST::RFMode::GHST_RF_MODE_RACE250;
175 : }
176 :
177 0 : uint16_t AP_GHST_Telem::get_telemetry_rate() const
178 : {
179 0 : return get_avg_packet_rate();
180 : }
181 :
182 : // WFQ scheduler
183 0 : bool AP_GHST_Telem::is_packet_ready(uint8_t idx, bool queue_empty)
184 : {
185 0 : return _enable_telemetry;
186 : }
187 :
188 : // WFQ scheduler
189 0 : void AP_GHST_Telem::process_packet(uint8_t idx)
190 : {
191 : // send packet
192 0 : switch (idx) {
193 0 : case ATTITUDE:
194 0 : calc_attitude();
195 0 : break;
196 0 : case BATTERY: // BATTERY
197 0 : calc_battery();
198 0 : break;
199 0 : case GPS: // GPS
200 0 : calc_gps();
201 0 : break;
202 0 : case GPS2: // GPS secondary info
203 0 : calc_gps2();
204 0 : break;
205 0 : default:
206 0 : break;
207 : }
208 0 : }
209 :
210 : // Process a frame from the GHST protocol decoder
211 0 : bool AP_GHST_Telem::_process_frame(AP_RCProtocol_GHST::FrameType frame_type, void* data) {
212 0 : switch (frame_type) {
213 : // this means we are connected to an RC receiver and can send telemetry
214 0 : case AP_RCProtocol_GHST::GHST_UL_RC_CHANS_RSSI: {
215 0 : process_rf_mode_changes();
216 0 : _enable_telemetry = AP::ghost()->is_telemetry_supported();
217 0 : break;
218 : }
219 0 : default:
220 0 : break;
221 : }
222 0 : return true;
223 : }
224 :
225 : // process any changed settings and schedule for transmission
226 0 : void AP_GHST_Telem::update()
227 : {
228 0 : }
229 :
230 0 : void AP_GHST_Telem::process_pending_requests()
231 : {
232 0 : _pending_request.frame_type = 0;
233 0 : }
234 :
235 : // prepare battery data
236 0 : void AP_GHST_Telem::calc_battery()
237 : {
238 : debug("BATTERY");
239 0 : const AP_BattMonitor &_battery = AP::battery();
240 :
241 0 : _telem.battery.voltage = htole16(uint16_t(roundf(_battery.voltage(0) * 100.0f)));
242 :
243 : float current;
244 0 : if (!_battery.current_amps(current, 0)) {
245 0 : current = 0;
246 : }
247 0 : _telem.battery.current = htole16(uint16_t(roundf(current * 100.0f)));
248 :
249 : float used_mah;
250 0 : if (!_battery.consumed_mah(used_mah, 0)) {
251 0 : used_mah = 0;
252 : }
253 :
254 0 : _telem.battery.consumed = htole16(uint16_t(roundf(used_mah * 100.0f)));;
255 0 : _telem.battery.rx_voltage = htole16(uint16_t(roundf(hal.analogin->board_voltage() * 10)));
256 :
257 0 : _telem_size = sizeof(BatteryFrame);
258 0 : _telem_type = AP_RCProtocol_GHST::GHST_DL_PACK_STAT;
259 :
260 0 : _telem_pending = true;
261 0 : }
262 :
263 : // prepare gps data
264 0 : void AP_GHST_Telem::calc_gps()
265 : {
266 : debug("GPS");
267 0 : const Location &loc = AP::gps().location(0); // use the first gps instance (same as in send_mavlink_gps_raw)
268 :
269 0 : _telem.gps.latitude = htole32(loc.lat);
270 0 : _telem.gps.longitude = htole32(loc.lng);
271 0 : _telem.gps.altitude = htole16(constrain_int16(loc.alt / 100, 0, 5000) + 1000);
272 :
273 0 : _telem_size = sizeof(AP_GHST_Telem::GPSFrame);
274 0 : _telem_type = AP_RCProtocol_GHST::GHST_DL_GPS_PRIMARY;
275 :
276 0 : _telem_pending = true;
277 0 : }
278 :
279 0 : void AP_GHST_Telem::calc_gps2()
280 : {
281 : debug("GPS2");
282 0 : _telem.gps2.groundspeed = htole16(roundf(AP::gps().ground_speed() * 100000 / 3600));
283 0 : _telem.gps2.gps_heading = htole16(roundf(AP::gps().ground_course() * 100.0f));
284 0 : _telem.gps2.satellites = AP::gps().num_sats();
285 :
286 0 : AP_AHRS &_ahrs = AP::ahrs();
287 0 : WITH_SEMAPHORE(_ahrs.get_semaphore());
288 0 : Location loc;
289 :
290 0 : if (_ahrs.get_location(loc) && _ahrs.home_is_set()) {
291 0 : const Location &home_loc = _ahrs.get_home();
292 0 : _telem.gps2.home_dist = home_loc.get_distance(loc) / 10; // 10m
293 0 : _telem.gps2.home_heading = loc.get_bearing_to(home_loc) / 10; // deci-degrees
294 : } else {
295 0 : _telem.gps2.home_dist = 0;
296 0 : _telem.gps2.home_heading = 0;
297 : }
298 :
299 0 : AP_GPS_FixType status = AP::gps().status();
300 0 : _telem.gps2.flags = status >= AP_GPS_FixType::FIX_2D ? 0x1 : 0;
301 :
302 0 : _telem_size = sizeof(AP_GHST_Telem::GPSSecondaryFrame);
303 0 : _telem_type = AP_RCProtocol_GHST::GHST_DL_GPS_SECONDARY;
304 :
305 0 : _telem_pending = true;
306 0 : }
307 :
308 : // prepare attitude data
309 0 : void AP_GHST_Telem::calc_attitude()
310 : {
311 : debug("MAGBARO");
312 0 : AP_AHRS &_ahrs = AP::ahrs();
313 0 : WITH_SEMAPHORE(_ahrs.get_semaphore());
314 :
315 0 : float heading = AP::compass().calculate_heading(_ahrs.get_rotation_body_to_ned());
316 0 : _telem.sensor.compass_heading = htole16(degrees(wrap_PI(heading)));
317 :
318 0 : float alt = AP::baro().get_altitude();
319 0 : _telem.sensor.baro_alt = htole16(roundf(alt));
320 0 : _telem.sensor.vario = 0;
321 0 : _telem.sensor.flags = 3;
322 0 : _telem_size = sizeof(AP_GHST_Telem::SensorFrame);
323 0 : _telem_type = AP_RCProtocol_GHST::GHST_DL_MAGBARO;
324 :
325 0 : _telem_pending = true;
326 0 : }
327 :
328 : /*
329 : fetch GHST frame data
330 : if is_tx_active is true then this will be a request for telemetry after receiving an RC frame
331 : */
332 0 : bool AP_GHST_Telem::_get_telem_data(AP_RCProtocol_GHST::Frame* data, bool is_tx_active)
333 : {
334 0 : memset(&_telem, 0, sizeof(TelemetryPayload));
335 0 : _is_tx_active = is_tx_active;
336 :
337 0 : run_wfq_scheduler();
338 0 : if (!_telem_pending) {
339 0 : return false;
340 : }
341 0 : memcpy(data->payload, &_telem, _telem_size);
342 0 : data->device_address = AP_RCProtocol_GHST::GHST_ADDRESS_GHST_RECEIVER;
343 0 : data->length = _telem_size + 2;
344 0 : data->type = _telem_type;
345 :
346 0 : _telem_pending = false;
347 0 : return true;
348 : }
349 :
350 : /*
351 : fetch data for an external transport, such as GHST
352 : */
353 0 : bool AP_GHST_Telem::process_frame(AP_RCProtocol_GHST::FrameType frame_type, void* data)
354 : {
355 0 : if (!get_singleton()) {
356 0 : return false;
357 : }
358 0 : return singleton->_process_frame(frame_type, data);
359 : }
360 :
361 : /*
362 : fetch data for an external transport, such as GHST
363 : */
364 0 : bool AP_GHST_Telem::get_telem_data(AP_RCProtocol_GHST::Frame* data, bool is_tx_active)
365 : {
366 0 : if (!get_singleton()) {
367 0 : return false;
368 : }
369 0 : return singleton->_get_telem_data(data, is_tx_active);
370 : }
371 :
372 0 : AP_GHST_Telem *AP_GHST_Telem::get_singleton(void) {
373 0 : if (!singleton && !hal.util->get_soft_armed()) {
374 : // if telem data is requested when we are disarmed and don't
375 : // yet have a AP_GHST_Telem object then try to allocate one
376 0 : NEW_NOTHROW AP_GHST_Telem();
377 : // initialize the passthrough scheduler
378 0 : if (singleton) {
379 0 : singleton->init();
380 : }
381 : }
382 0 : return singleton;
383 : }
384 :
385 : namespace AP {
386 0 : AP_GHST_Telem *ghost_telem() {
387 0 : return AP_GHST_Telem::get_singleton();
388 : }
389 : };
390 :
391 : #endif // AP_GHST_TELEM_ENABLED
|
