Refactor missing prototypes 2 (#14170)
[betaflight.git] / src / main / telemetry / hott.c
blobfa0d8a325f7dba29e1eb35b3f5a4380ce0df1281
1 /*
2 * This file is part of Cleanflight and Betaflight.
4 * Cleanflight and Betaflight are free software. You can redistribute
5 * this software and/or modify this software under the terms of the
6 * GNU General Public License as published by the Free Software
7 * Foundation, either version 3 of the License, or (at your option)
8 * any later version.
10 * Cleanflight and Betaflight are distributed in the hope that they
11 * will be useful, but WITHOUT ANY WARRANTY; without even the implied
12 * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
13 * See the GNU General Public License for more details.
15 * You should have received a copy of the GNU General Public License
16 * along with this software.
18 * If not, see <http://www.gnu.org/licenses/>.
22 * telemetry_hott.c
24 * Authors:
25 * Dominic Clifton - Hydra - Software Serial, Electronics, Hardware Integration and debugging, HoTT Code cleanup and fixes, general telemetry improvements.
26 * Carsten Giesen - cGiesen - Baseflight port
27 * Oliver Bayer - oBayer - MultiWii-HoTT, HoTT reverse engineering
28 * Adam Majerczyk - HoTT-for-ardupilot from which some information and ideas are borrowed.
29 * Scavanger & Ziege-One: CMS Textmode addon
31 * https://github.com/obayer/MultiWii-HoTT
32 * https://github.com/oBayer/MultiHoTT-Module
33 * https://code.google.com/p/hott-for-ardupilot
35 * HoTT is implemented in Graupner equipment using a bi-directional protocol over a single wire.
37 * Generally the receiver sends a single request byte out using normal uart signals, then waits a short period for a
38 * multiple byte response and checksum byte before it sends out the next request byte.
39 * Each response byte must be send with a protocol specific delay between them.
41 * Serial ports use two wires but HoTT uses a single wire so some electronics are required so that
42 * the signals don't get mixed up. When cleanflight transmits it should not receive it's own transmission.
44 * Connect as follows:
45 * HoTT TX/RX -> Serial RX (connect directly)
46 * Serial TX -> 1N4148 Diode -(| )-> HoTT TX/RX (connect via diode)
48 * The diode should be arranged to allow the data signals to flow the right way
49 * -(| )- == Diode, | indicates cathode marker.
51 * As noticed by Skrebber the GR-12 (and probably GR-16/24, too) are based on a PIC 24FJ64GA-002, which has 5V tolerant digital pins.
53 * There is a technical discussion (in German) about HoTT here
54 * http://www.rc-network.de/forum/showthread.php/281496-Graupner-HoTT-Telemetrie-Sensoren-Eigenbau-DIY-Telemetrie-Protokoll-entschl%C3%BCsselt/page21
56 #include <stdbool.h>
57 #include <stdint.h>
58 #include <string.h>
60 #include "platform.h"
62 #ifdef USE_TELEMETRY_HOTT
64 #include "build/build_config.h"
65 #include "build/debug.h"
67 #include "common/axis.h"
68 #include "common/maths.h"
69 #include "common/time.h"
71 #include "drivers/serial.h"
72 #include "drivers/time.h"
74 #include "fc/runtime_config.h"
76 #include "flight/position.h"
77 #include "flight/pid.h"
79 #include "io/gps.h"
81 #include "sensors/battery.h"
82 #include "sensors/barometer.h"
83 #include "sensors/sensors.h"
85 #include "telemetry/hott.h"
86 #include "telemetry/telemetry.h"
88 #if defined(USE_HOTT_TEXTMODE) && defined(USE_CMS)
89 #include "scheduler/scheduler.h"
90 #include "io/displayport_hott.h"
92 #define HOTT_TEXTMODE_TASK_PERIOD 1000
93 #define HOTT_TEXTMODE_RX_SCHEDULE 5000
94 #define HOTT_TEXTMODE_TX_DELAY_US 1000
95 #endif
97 //#define HOTT_DEBUG
99 #define HOTT_MESSAGE_PREPARATION_FREQUENCY_5_HZ ((1000 * 1000) / 5)
100 #define HOTT_RX_SCHEDULE 4000
101 #define HOTT_TX_DELAY_US 3000
102 #define MILLISECONDS_IN_A_SECOND 1000
104 static uint32_t rxSchedule = HOTT_RX_SCHEDULE;
105 static uint32_t txDelayUs = HOTT_TX_DELAY_US;
107 static uint32_t lastHoTTRequestCheckAt = 0;
108 static uint32_t lastMessagesPreparedAt = 0;
109 static uint32_t lastHottAlarmSoundTime = 0;
111 static bool hottIsSending = false;
113 static uint8_t *hottMsg = NULL;
114 static uint8_t hottMsgRemainingBytesToSendCount;
115 static uint8_t hottMsgCrc;
117 #define HOTT_CRC_SIZE (sizeof(hottMsgCrc))
119 #define HOTT_BAUDRATE 19200
120 #define HOTT_PORT_MODE MODE_RXTX // must be opened in RXTX so that TX and RX pins are allocated.
122 static serialPort_t *hottPort = NULL;
123 static const serialPortConfig_t *portConfig;
125 static bool hottTelemetryEnabled = false;
126 static portSharing_e hottPortSharing;
128 static HOTT_GPS_MSG_t hottGPSMessage;
129 static HOTT_EAM_MSG_t hottEAMMessage;
131 #if defined(USE_HOTT_TEXTMODE) && defined(USE_CMS)
132 static hottTextModeMsg_t hottTextModeMessage;
133 static bool textmodeIsAlive = false;
134 static int32_t telemetryTaskPeriod = 0;
136 static void initialiseTextmodeMessage(hottTextModeMsg_t *msg)
138 msg->start = HOTT_TEXTMODE_START;
139 msg->esc = HOTT_EAM_SENSOR_TEXT_ID;
140 msg->warning = 0;
141 msg->stop = HOTT_TEXTMODE_STOP;
143 #endif
145 static void initialiseEAMMessage(HOTT_EAM_MSG_t *msg, size_t size)
147 memset(msg, 0, size);
148 msg->start_byte = 0x7C;
149 msg->eam_sensor_id = HOTT_TELEMETRY_EAM_SENSOR_ID;
150 msg->sensor_id = HOTT_EAM_SENSOR_TEXT_ID;
151 msg->stop_byte = 0x7D;
154 #ifdef USE_GPS
155 typedef enum {
156 GPS_FIX_CHAR_NONE = '-',
157 GPS_FIX_CHAR_2D = '2',
158 GPS_FIX_CHAR_3D = '3',
159 GPS_FIX_CHAR_DGPS = 'D'
160 } gpsFixChar_e;
162 static void initialiseGPSMessage(HOTT_GPS_MSG_t *msg, size_t size)
164 memset(msg, 0, size);
165 msg->start_byte = 0x7C;
166 msg->gps_sensor_id = HOTT_TELEMETRY_GPS_SENSOR_ID;
167 msg->sensor_id = HOTT_GPS_SENSOR_TEXT_ID;
168 msg->stop_byte = 0x7D;
170 #endif
172 static void initialiseMessages(void)
174 initialiseEAMMessage(&hottEAMMessage, sizeof(hottEAMMessage));
175 #ifdef USE_GPS
176 initialiseGPSMessage(&hottGPSMessage, sizeof(hottGPSMessage));
177 #endif
178 #if defined(USE_HOTT_TEXTMODE) && defined(USE_CMS)
179 initialiseTextmodeMessage(&hottTextModeMessage);
180 #endif
183 #ifdef USE_GPS
184 STATIC_UNIT_TESTED void addGPSCoordinates(HOTT_GPS_MSG_t *hottGPSMessage, int32_t latitude, int32_t longitude)
186 int16_t deg = latitude / GPS_DEGREES_DIVIDER;
187 int32_t sec = (latitude - (deg * GPS_DEGREES_DIVIDER)) * 6;
188 int8_t min = sec / 1000000L;
189 sec = (sec % 1000000L) / 100L;
190 uint16_t degMin = (deg * 100L) + min;
192 hottGPSMessage->pos_NS = (latitude < 0);
193 hottGPSMessage->pos_NS_dm_L = degMin;
194 hottGPSMessage->pos_NS_dm_H = degMin >> 8;
195 hottGPSMessage->pos_NS_sec_L = sec;
196 hottGPSMessage->pos_NS_sec_H = sec >> 8;
198 deg = longitude / GPS_DEGREES_DIVIDER;
199 sec = (longitude - (deg * GPS_DEGREES_DIVIDER)) * 6;
200 min = sec / 1000000L;
201 sec = (sec % 1000000L) / 100L;
202 degMin = (deg * 100L) + min;
204 hottGPSMessage->pos_EW = (longitude < 0);
205 hottGPSMessage->pos_EW_dm_L = degMin;
206 hottGPSMessage->pos_EW_dm_H = degMin >> 8;
207 hottGPSMessage->pos_EW_sec_L = sec;
208 hottGPSMessage->pos_EW_sec_H = sec >> 8;
211 void hottPrepareGPSResponse(HOTT_GPS_MSG_t *hottGPSMessage)
213 hottGPSMessage->gps_satelites = gpsSol.numSat;
215 if (!STATE(GPS_FIX)) {
216 hottGPSMessage->gps_fix_char = GPS_FIX_CHAR_NONE;
217 return;
220 if (gpsSol.numSat >= GPS_MIN_SAT_COUNT) {
221 hottGPSMessage->gps_fix_char = GPS_FIX_CHAR_3D;
222 } else {
223 hottGPSMessage->gps_fix_char = GPS_FIX_CHAR_2D;
226 addGPSCoordinates(hottGPSMessage, gpsSol.llh.lat, gpsSol.llh.lon);
228 // GPS Speed is returned in cm/s (from io/gps.c) and must be sent in km/h (Hott requirement)
229 const uint16_t speed = (gpsSol.groundSpeed * 36) / 1000;
230 hottGPSMessage->gps_speed_L = speed & 0x00FF;
231 hottGPSMessage->gps_speed_H = speed >> 8;
233 hottGPSMessage->home_distance_L = GPS_distanceToHome & 0x00FF;
234 hottGPSMessage->home_distance_H = GPS_distanceToHome >> 8;
236 int32_t altitudeM = getEstimatedAltitudeCm() / 100;
238 const uint16_t hottGpsAltitude = constrain(altitudeM + HOTT_GPS_ALTITUDE_OFFSET, 0 , UINT16_MAX); // gpsSol.llh.alt in m ; offset = 500 -> O m
240 hottGPSMessage->altitude_L = hottGpsAltitude & 0x00FF;
241 hottGPSMessage->altitude_H = hottGpsAltitude >> 8;
243 hottGPSMessage->home_direction = GPS_directionToHome / 10;
245 #endif
247 static bool shouldTriggerBatteryAlarmNow(void)
249 return ((millis() - lastHottAlarmSoundTime) >= (telemetryConfig()->hottAlarmSoundInterval * MILLISECONDS_IN_A_SECOND));
252 static inline void updateAlarmBatteryStatus(HOTT_EAM_MSG_t *hottEAMMessage)
254 if (shouldTriggerBatteryAlarmNow()) {
255 lastHottAlarmSoundTime = millis();
256 const batteryState_e voltageState = getVoltageState();
257 const batteryState_e consumptionState = getConsumptionState();
258 if (voltageState == BATTERY_WARNING || voltageState == BATTERY_CRITICAL) {
259 hottEAMMessage->warning_beeps = 0x10;
260 hottEAMMessage->alarm_invers1 = HOTT_EAM_ALARM1_FLAG_BATTERY_1;
261 } else if (consumptionState == BATTERY_WARNING || consumptionState == BATTERY_CRITICAL) {
262 hottEAMMessage->warning_beeps = 0x16;
263 hottEAMMessage->alarm_invers1 = HOTT_EAM_ALARM1_FLAG_MAH;
264 } else {
265 hottEAMMessage->warning_beeps = HOTT_EAM_ALARM1_FLAG_NONE;
266 hottEAMMessage->alarm_invers1 = HOTT_EAM_ALARM1_FLAG_NONE;
271 static inline void hottEAMUpdateBattery(HOTT_EAM_MSG_t *hottEAMMessage)
273 const uint16_t volt = getLegacyBatteryVoltage();
274 hottEAMMessage->main_voltage_L = volt & 0xFF;
275 hottEAMMessage->main_voltage_H = volt >> 8;
276 hottEAMMessage->batt1_voltage_L = volt & 0xFF;
277 hottEAMMessage->batt1_voltage_H = volt >> 8;
279 updateAlarmBatteryStatus(hottEAMMessage);
282 static inline void hottEAMUpdateCurrentMeter(HOTT_EAM_MSG_t *hottEAMMessage)
284 const int32_t amp = getAmperage() / 10;
285 hottEAMMessage->current_L = amp & 0xFF;
286 hottEAMMessage->current_H = amp >> 8;
289 static inline void hottEAMUpdateBatteryDrawnCapacity(HOTT_EAM_MSG_t *hottEAMMessage)
291 const int32_t mAh = getMAhDrawn() / 10;
292 hottEAMMessage->batt_cap_L = mAh & 0xFF;
293 hottEAMMessage->batt_cap_H = mAh >> 8;
296 static inline void hottEAMUpdateAltitude(HOTT_EAM_MSG_t *hottEAMMessage)
298 const uint16_t hottEamAltitude = (getEstimatedAltitudeCm() / 100) + HOTT_EAM_OFFSET_HEIGHT;
300 hottEAMMessage->altitude_L = hottEamAltitude & 0x00FF;
301 hottEAMMessage->altitude_H = hottEamAltitude >> 8;
304 #ifdef USE_VARIO
305 static inline void hottEAMUpdateClimbrate(HOTT_EAM_MSG_t *hottEAMMessage)
307 const int32_t vario = getEstimatedVario();
308 hottEAMMessage->climbrate_L = (30000 + vario) & 0x00FF;
309 hottEAMMessage->climbrate_H = (30000 + vario) >> 8;
310 hottEAMMessage->climbrate3s = 120 + (vario / 100);
312 #endif
314 static void hottPrepareEAMResponse(HOTT_EAM_MSG_t *hottEAMMessage)
316 // Reset alarms
317 hottEAMMessage->warning_beeps = 0x0;
318 hottEAMMessage->alarm_invers1 = 0x0;
320 hottEAMUpdateBattery(hottEAMMessage);
321 hottEAMUpdateCurrentMeter(hottEAMMessage);
322 hottEAMUpdateBatteryDrawnCapacity(hottEAMMessage);
323 hottEAMUpdateAltitude(hottEAMMessage);
324 #ifdef USE_VARIO
325 hottEAMUpdateClimbrate(hottEAMMessage);
326 #endif
329 static void hottSerialWrite(uint8_t c)
331 serialWrite(hottPort, c);
334 void freeHoTTTelemetryPort(void)
336 closeSerialPort(hottPort);
337 hottPort = NULL;
338 hottTelemetryEnabled = false;
341 void initHoTTTelemetry(void)
343 portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_HOTT);
345 if (!portConfig) {
346 return;
349 hottPortSharing = determinePortSharing(portConfig, FUNCTION_TELEMETRY_HOTT);
351 #if defined(USE_HOTT_TEXTMODE) && defined(USE_CMS)
352 hottDisplayportRegister();
353 #endif
355 initialiseMessages();
358 static void flushHottRxBuffer(void)
360 while (serialRxBytesWaiting(hottPort) > 0) {
361 serialRead(hottPort);
365 static void workAroundForHottTelemetryOnUsart(serialPort_t *instance, portMode_e mode)
367 closeSerialPort(hottPort);
369 portOptions_e portOptions = telemetryConfig()->telemetry_inverted ? SERIAL_INVERTED : SERIAL_NOT_INVERTED;
371 if (telemetryConfig()->halfDuplex) {
372 portOptions |= SERIAL_BIDIR;
374 // TODO - identifier is set only after opening port
375 hottPort = openSerialPort(instance->identifier, FUNCTION_TELEMETRY_HOTT, NULL, NULL, HOTT_BAUDRATE, mode, portOptions);
378 static bool hottIsUsingHardwareUART(void)
380 return serialType(portConfig->identifier) != SERIALTYPE_SOFTSERIAL;
383 static void hottConfigurePortForTX(void)
385 // FIXME temorary workaround for HoTT not working on Hardware serial ports due to hardware/softserial serial port initialisation differences
386 if (hottIsUsingHardwareUART()) {
387 workAroundForHottTelemetryOnUsart(hottPort, MODE_TX);
388 } else {
389 serialSetMode(hottPort, MODE_TX);
391 hottIsSending = true;
392 hottMsgCrc = 0;
395 static void hottConfigurePortForRX(void)
397 // FIXME temorary workaround for HoTT not working on Hardware serial ports due to hardware/softserial serial port initialisation differences
398 if (hottIsUsingHardwareUART()) {
399 workAroundForHottTelemetryOnUsart(hottPort, MODE_RX);
400 } else {
401 serialSetMode(hottPort, MODE_RX);
403 hottMsg = NULL;
404 hottIsSending = false;
405 flushHottRxBuffer();
408 void configureHoTTTelemetryPort(void)
410 if (!portConfig) {
411 return;
414 portOptions_e portOptions = SERIAL_NOT_INVERTED;
416 if (telemetryConfig()->halfDuplex) {
417 portOptions |= SERIAL_BIDIR;
420 hottPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_HOTT, NULL, NULL, HOTT_BAUDRATE, HOTT_PORT_MODE, portOptions);
422 if (!hottPort) {
423 return;
426 hottConfigurePortForRX();
428 hottTelemetryEnabled = true;
431 static void hottSendResponse(uint8_t *buffer, int length)
433 if (hottIsSending) {
434 return;
437 hottMsg = buffer;
438 hottMsgRemainingBytesToSendCount = length + HOTT_CRC_SIZE;
441 static inline void hottSendGPSResponse(void)
443 hottSendResponse((uint8_t *)&hottGPSMessage, sizeof(hottGPSMessage));
446 static inline void hottSendEAMResponse(void)
448 hottSendResponse((uint8_t *)&hottEAMMessage, sizeof(hottEAMMessage));
451 static void hottPrepareMessages(void)
453 hottPrepareEAMResponse(&hottEAMMessage);
454 #ifdef USE_GPS
455 hottPrepareGPSResponse(&hottGPSMessage);
456 #endif
459 #if defined(USE_HOTT_TEXTMODE) && defined(USE_CMS)
460 static void hottTextmodeStart(void)
462 // Increase menu speed
463 taskInfo_t taskInfo;
464 getTaskInfo(TASK_TELEMETRY, &taskInfo);
465 telemetryTaskPeriod = taskInfo.desiredPeriodUs;
466 rescheduleTask(TASK_TELEMETRY, TASK_PERIOD_HZ(HOTT_TEXTMODE_TASK_PERIOD));
468 rxSchedule = HOTT_TEXTMODE_RX_SCHEDULE;
469 txDelayUs = HOTT_TEXTMODE_TX_DELAY_US;
472 static void hottTextmodeStop(void)
474 // Set back to avoid slow down of the FC
475 if (telemetryTaskPeriod > 0) {
476 rescheduleTask(TASK_TELEMETRY, telemetryTaskPeriod);
477 telemetryTaskPeriod = 0;
480 rxSchedule = HOTT_RX_SCHEDULE;
481 txDelayUs = HOTT_TX_DELAY_US;
484 bool hottTextmodeIsAlive(void)
486 return textmodeIsAlive;
489 void hottTextmodeGrab(void)
491 hottTextModeMessage.esc = HOTT_EAM_SENSOR_TEXT_ID;
494 void hottTextmodeExit(void)
496 hottTextModeMessage.esc = HOTT_TEXTMODE_ESC;
499 void hottTextmodeWriteChar(uint8_t column, uint8_t row, char c)
501 if (column < HOTT_TEXTMODE_DISPLAY_COLUMNS && row < HOTT_TEXTMODE_DISPLAY_ROWS) {
502 hottTextModeMessage.txt[row][column] = c;
506 static void processHottTextModeRequest(const uint8_t cmd)
508 static bool setEscBack = false;
510 if (!textmodeIsAlive) {
511 hottTextmodeStart();
512 textmodeIsAlive = true;
515 if ((cmd & 0xF0) != HOTT_EAM_SENSOR_TEXT_ID) {
516 return;
519 if (setEscBack) {
520 hottTextModeMessage.esc = HOTT_EAM_SENSOR_TEXT_ID;
521 setEscBack = false;
524 if (hottTextModeMessage.esc != HOTT_TEXTMODE_ESC) {
525 hottCmsOpen();
526 } else {
527 setEscBack = true;
530 hottSetCmsKey(cmd & 0x0f, hottTextModeMessage.esc == HOTT_TEXTMODE_ESC);
531 hottSendResponse((uint8_t *)&hottTextModeMessage, sizeof(hottTextModeMessage));
533 #endif
535 static void processBinaryModeRequest(uint8_t address)
537 #if defined(USE_HOTT_TEXTMODE) && defined(USE_CMS)
538 if (textmodeIsAlive) {
539 hottTextmodeStop();
540 textmodeIsAlive = false;
542 #endif
544 #ifdef HOTT_DEBUG
545 static uint8_t hottBinaryRequests = 0;
546 static uint8_t hottGPSRequests = 0;
547 static uint8_t hottEAMRequests = 0;
548 #endif
550 switch (address) {
551 #ifdef USE_GPS
552 case 0x8A:
553 #ifdef HOTT_DEBUG
554 hottGPSRequests++;
555 #endif
556 if (sensors(SENSOR_GPS)) {
557 hottSendGPSResponse();
559 break;
560 #endif
561 case 0x8E:
562 #ifdef HOTT_DEBUG
563 hottEAMRequests++;
564 #endif
565 hottSendEAMResponse();
566 break;
569 #ifdef HOTT_DEBUG
570 hottBinaryRequests++;
571 debug[0] = hottBinaryRequests;
572 #ifdef USE_GPS
573 debug[1] = hottGPSRequests;
574 #endif
575 debug[2] = hottEAMRequests;
576 #endif
579 static void hottCheckSerialData(uint32_t currentMicros)
581 static bool lookingForRequest = true;
583 const uint8_t bytesWaiting = serialRxBytesWaiting(hottPort);
585 if (bytesWaiting <= 1) {
586 return;
589 if (bytesWaiting != 2) {
590 flushHottRxBuffer();
591 lookingForRequest = true;
592 return;
595 if (lookingForRequest) {
596 lastHoTTRequestCheckAt = currentMicros;
597 lookingForRequest = false;
598 return;
599 } else {
600 bool enoughTimePassed = currentMicros - lastHoTTRequestCheckAt >= rxSchedule;
602 if (!enoughTimePassed) {
603 return;
605 lookingForRequest = true;
608 const uint8_t requestId = serialRead(hottPort);
609 const uint8_t address = serialRead(hottPort);
611 if ((requestId == 0) || (requestId == HOTT_BINARY_MODE_REQUEST_ID) || (address == HOTT_TELEMETRY_NO_SENSOR_ID)) {
613 * FIXME the first byte of the HoTT request frame is ONLY either 0x80 (binary mode) or 0x7F (text mode).
614 * The binary mode is read as 0x00 (error reading the upper bit) while the text mode is correctly decoded.
615 * The (requestId == 0) test is a workaround for detecting the binary mode with no ambiguity as there is only
616 * one other valid value (0x7F) for text mode.
617 * The error reading for the upper bit should nevertheless be fixed
619 processBinaryModeRequest(address);
621 #if defined(USE_HOTT_TEXTMODE) && defined(USE_CMS)
622 else if (requestId == HOTTV4_TEXT_MODE_REQUEST_ID) {
623 processHottTextModeRequest(address);
625 #endif
628 static void hottSendTelemetryData(void)
631 if (!hottIsSending) {
632 hottConfigurePortForTX();
633 return;
636 if (hottMsgRemainingBytesToSendCount == 0) {
637 hottConfigurePortForRX();
638 return;
641 --hottMsgRemainingBytesToSendCount;
642 if (hottMsgRemainingBytesToSendCount == 0) {
643 hottSerialWrite(hottMsgCrc++);
644 return;
647 hottMsgCrc += *hottMsg;
648 hottSerialWrite(*hottMsg++);
651 static inline bool shouldPrepareHoTTMessages(uint32_t currentMicros)
653 return currentMicros - lastMessagesPreparedAt >= HOTT_MESSAGE_PREPARATION_FREQUENCY_5_HZ;
656 static inline bool shouldCheckForHoTTRequest(void)
658 if (hottIsSending) {
659 return false;
661 return true;
664 void checkHoTTTelemetryState(void)
666 const bool newTelemetryEnabledValue = telemetryDetermineEnabledState(hottPortSharing);
668 if (newTelemetryEnabledValue == hottTelemetryEnabled) {
669 return;
672 if (newTelemetryEnabledValue) {
673 configureHoTTTelemetryPort();
674 } else {
675 freeHoTTTelemetryPort();
679 void handleHoTTTelemetry(timeUs_t currentTimeUs)
681 static timeUs_t serialTimer;
683 if (!hottTelemetryEnabled) {
684 return;
687 if (shouldPrepareHoTTMessages(currentTimeUs)) {
688 hottPrepareMessages();
689 lastMessagesPreparedAt = currentTimeUs;
692 if (shouldCheckForHoTTRequest()) {
693 hottCheckSerialData(currentTimeUs);
696 if (!hottMsg)
697 return;
699 if (hottIsSending) {
700 if (currentTimeUs - serialTimer < txDelayUs) {
701 return;
704 hottSendTelemetryData();
705 serialTimer = currentTimeUs;
708 #endif