2 * This file is part of Cleanflight.
4 * Cleanflight is free software: you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License as published by
6 * the Free Software Foundation, either version 3 of the License, or
7 * (at your option) any later version.
9 * Cleanflight is distributed in the hope that it will be useful,
10 * but WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 * GNU General Public License for more details.
14 * You should have received a copy of the GNU General Public License
15 * along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
20 * Dominic Clifton/Hydra
22 * Adam Majerczyk (majerczyk.adam@gmail.com)
23 * Texmode add-on by Michi (mamaretti32@gmail.com)
24 * Scavanger & Ziege-One: CMS Textmode addon
31 #define HOTTV4_TEXT_MODE_REQUEST_ID 0x7f
32 #define HOTTV4_BINARY_MODE_REQUEST_ID 0x80
34 #define HOTTV4_BUTTON_DEC 0xB
35 #define HOTTV4_BUTTON_INC 0xD
36 #define HOTTV4_BUTTON_SET 0x9
37 #define HOTTV4_BUTTON_NIL 0xF
38 #define HOTTV4_BUTTON_NEXT 0xE
39 #define HOTTV4_BUTTON_PREV 0x7
41 #define HOTT_EAM_OFFSET_HEIGHT 500
42 #define HOTT_EAM_OFFSET_M2S 72
43 #define HOTT_EAM_OFFSET_M3S 120
44 #define HOTT_EAM_OFFSET_TEMPERATURE 20
46 #define HOTT_GPS_ALTITUDE_OFFSET 500
49 HOTT_EAM_ALARM1_FLAG_NONE
= 0,
50 HOTT_EAM_ALARM1_FLAG_MAH
= (1 << 0),
51 HOTT_EAM_ALARM1_FLAG_BATTERY_1
= (1 << 1),
52 HOTT_EAM_ALARM1_FLAG_BATTERY_2
= (1 << 2),
53 HOTT_EAM_ALARM1_FLAG_TEMPERATURE_1
= (1 << 3),
54 HOTT_EAM_ALARM1_FLAG_TEMPERATURE_2
= (1 << 4),
55 HOTT_EAM_ALARM1_FLAG_ALTITUDE
= (1 << 5),
56 HOTT_EAM_ALARM1_FLAG_CURRENT
= (1 << 6),
57 HOTT_EAM_ALARM1_FLAG_MAIN_VOLTAGE
= (1 << 7),
58 } hottEamAlarm1Flag_e
;
61 HOTT_EAM_ALARM2_FLAG_NONE
= 0,
62 HOTT_EAM_ALARM2_FLAG_MS
= (1 << 0),
63 HOTT_EAM_ALARM2_FLAG_M3S
= (1 << 1),
64 HOTT_EAM_ALARM2_FLAG_ALTITUDE_DUPLICATE
= (1 << 2),
65 HOTT_EAM_ALARM2_FLAG_MS_DUPLICATE
= (1 << 3),
66 HOTT_EAM_ALARM2_FLAG_M3S_DUPLICATE
= (1 << 4),
67 HOTT_EAM_ALARM2_FLAG_UNKNOWN_1
= (1 << 5),
68 HOTT_EAM_ALARM2_FLAG_UNKNOWN_2
= (1 << 6),
69 HOTT_EAM_ALARM2_FLAG_ON_SIGN_OR_TEXT_ACTIVE
= (1 << 7),
70 } hottEamAlarm2Flag_e
;
77 #define HOTT_TEXT_MODE_REQUEST_ID 0x7f
78 #define HOTT_BINARY_MODE_REQUEST_ID 0x80
81 //Id 0x80 is used when no sensor has been found during the bus scan
82 // additionaly meaning?
83 #define HOTT_TELEMETRY_NO_SENSOR_ID 0x80
85 //Graupner #33601 Vario Module
86 #define HOTT_TELEMETRY_VARIO_SENSOR_ID 0x89
88 //Graupner #33600 GPS Module
89 #define HOTT_TELEMETRY_GPS_SENSOR_ID 0x8a
91 //Graupner #337xx Air ESC
92 #define HOTT_TELEMETRY_AIRESC_SENSOR_ID 0x8c
94 //Graupner #33611 General Air Module
95 #define HOTT_TELEMETRY_GAM_SENSOR_ID 0x8d
97 //Graupner #33620 Electric Air Module
98 #define HOTT_TELEMETRY_EAM_SENSOR_ID 0x8e
101 #define HOTT_EAM_SENSOR_TEXT_ID 0xE0 // Electric Air Module ID
102 #define HOTT_GPS_SENSOR_TEXT_ID 0xA0 // GPS Module ID
105 #define HOTT_TEXTMODE_DISPLAY_ROWS 8
106 #define HOTT_TEXTMODE_DISPLAY_COLUMNS 21
107 #define HOTT_TEXTMODE_START 0x7B
108 #define HOTT_TEXTMODE_STOP 0x7D
109 #define HOTT_TEXTMODE_ESC 0x01
110 //Text mode msgs type
111 typedef struct hottTextModeMsg_s
{
112 uint8_t start
; //#01 constant value 0x7b
113 uint8_t esc
; //#02 Low byte: Sensor ID or 0x01 for escape
114 uint8_t warning
; //#03 1=A 2=B ...
115 uint8_t txt
[HOTT_TEXTMODE_DISPLAY_ROWS
][HOTT_TEXTMODE_DISPLAY_COLUMNS
]; //#04 ASCII text to display to
116 // Bit 7 = 1 -> Inverse character display
118 uint8_t stop
; //#171 constant value 0x7d
121 typedef struct HOTT_GAM_MSG_s
{
122 uint8_t start_byte
; //#01 start uint8_t constant value 0x7c
123 uint8_t gam_sensor_id
; //#02 EAM sensort id. constat value 0x8d
124 uint8_t warning_beeps
; //#03 1=A 2=B ... 0x1a=Z 0 = no alarm
125 // Q Min cell voltage sensor 1
126 // R Min Battery 1 voltage sensor 1
127 // J Max Battery 1 voltage sensor 1
128 // F Min temperature sensor 1
129 // H Max temperature sensor 1
130 // S Min Battery 2 voltage sensor 2
131 // K Max Battery 2 voltage sensor 2
132 // G Min temperature sensor 2
133 // I Max temperature sensor 2
135 // V Max capacity mAh
136 // P Min main power voltage
137 // X Max main power voltage
140 // C negative difference m/s too high
141 // A negative difference m/3s too high
142 // N positive difference m/s too high
143 // L positive difference m/3s too high
147 uint8_t sensor_id
; //#04 constant value 0xd0
148 uint8_t alarm_invers1
; //#05 alarm bitmask. Value is displayed inverted
150 // 0 all cell voltage
158 uint8_t alarm_invers2
; //#06 alarm bitmask. Value is displayed inverted
160 // 0 main power current
161 // 1 main power voltage
167 // 7 "ON" sign/text msg active
169 uint8_t cell1
; //#07 cell 1 voltage lower value. 0.02V steps, 124=2.48V
175 uint8_t batt1_L
; //#13 battery 1 voltage LSB value. 0.1V steps. 50 = 5.5V
176 uint8_t batt1_H
; //#14
177 uint8_t batt2_L
; //#15 battery 2 voltage LSB value. 0.1V steps. 50 = 5.5V
178 uint8_t batt2_H
; //#16
179 uint8_t temperature1
; //#17 temperature 1. offset of 20. a value of 20 = 0�C
180 uint8_t temperature2
; //#18 temperature 2. offset of 20. a value of 20 = 0�C
181 uint8_t fuel_procent
; //#19 Fuel capacity in %. Values 0--100
182 // graphical display ranges: 0-25% 50% 75% 100%
183 uint8_t fuel_ml_L
; //#20 Fuel in ml scale. Full = 65535!
184 uint8_t fuel_ml_H
; //#21
185 uint8_t rpm_L
; //#22 RPM in 10 RPM steps. 300 = 3000rpm
187 uint8_t altitude_L
; //#24 altitude in meters. offset of 500, 500 = 0m
188 uint8_t altitude_H
; //#25
189 uint8_t climbrate_L
; //#26 climb rate in 0.01m/s. Value of 30000 = 0.00 m/s
190 uint8_t climbrate_H
; //#27
191 uint8_t climbrate3s
; //#28 climb rate in m/3sec. Value of 120 = 0m/3sec
192 uint8_t current_L
; //#29 current in 0.1A steps
193 uint8_t current_H
; //#30
194 uint8_t main_voltage_L
; //#31 Main power voltage using 0.1V steps
195 uint8_t main_voltage_H
; //#32
196 uint8_t batt_cap_L
; //#33 used battery capacity in 10mAh steps
197 uint8_t batt_cap_H
; //#34
198 uint8_t speed_L
; //#35 (air?) speed in km/h(?) we are using ground speed here per default
199 uint8_t speed_H
; //#36
200 uint8_t min_cell_volt
; //#37 minimum cell voltage in 2mV steps. 124 = 2,48V
201 uint8_t min_cell_volt_num
; //#38 number of the cell with the lowest voltage
202 uint8_t rpm2_L
; //#39 RPM in 10 RPM steps. 300 = 3000rpm
203 uint8_t rpm2_H
; //#40
204 uint8_t general_error_number
;//#41 Voice error == 12. TODO: more docu
205 uint8_t pressure
; //#42 Pressure up to 16bar. 0,1bar scale. 20 = 2bar
206 uint8_t version
; //#43 version number TODO: more info?
207 uint8_t stop_byte
; //#44 stop uint8_t
210 #define HOTT_VARIO_MSG_TEXT_LEN 21
211 typedef struct HOTT_VARIO_MSG_s
{
212 uint8_t start_byte
; //#01 start uint8_t constant value 0x7c
213 uint8_t vario_sensor_id
; //#02 VARIO sensort id. constat value 0x89
214 uint8_t warning_beeps
; //#03 1=A 2=B ...
215 // Q Min cell voltage sensor 1
216 // R Min Battery 1 voltage sensor 1
217 // J Max Battery 1 voltage sensor 1
218 // F Min temperature sensor 1
219 // H Max temperature sensor 1
220 // S Min Battery voltage sensor 2
221 // K Max Battery voltage sensor 2
222 // G Min temperature sensor 2
223 // I Max temperature sensor 2
225 // V Max capacity mAh
226 // P Min main power voltage
227 // X Max main power voltage
232 // C m/s negative difference
233 // A m/3s negative difference
236 uint8_t sensor_id
; //#04 constant value 0x90
237 uint8_t alarm_invers1
; //#05 Inverse display (alarm?) bitmask
239 uint8_t altitude_L
; //#06 Altitude low uint8_t. In meters. A value of 500 means 0m
240 uint8_t altitude_H
; //#07 Altitude high uint8_t
241 uint8_t altitude_max_L
; //#08 Max. measured altitude low uint8_t. In meters. A value of 500 means 0m
242 uint8_t altitude_max_H
; //#09 Max. measured altitude high uint8_t
243 uint8_t altitude_min_L
; //#10 Min. measured altitude low uint8_t. In meters. A value of 500 means 0m
244 uint8_t altitude_min_H
; //#11 Min. measured altitude high uint8_t
245 uint8_t climbrate_L
; //#12 Climb rate in m/s. Steps of 0.01m/s. Value of 30000 = 0.00 m/s
246 uint8_t climbrate_H
; //#13 Climb rate in m/s
247 uint8_t climbrate3s_L
; //#14 Climb rate in m/3s. Steps of 0.01m/3s. Value of 30000 = 0.00 m/3s
248 uint8_t climbrate3s_H
; //#15 Climb rate m/3s low uint8_t
249 uint8_t climbrate10s_L
; //#16 Climb rate m/10s. Steps of 0.01m/10s. Value of 30000 = 0.00 m/10s
250 uint8_t climbrate10s_H
; //#17 Climb rate m/10s low uint8_t
251 uint8_t text_msg
[HOTT_VARIO_MSG_TEXT_LEN
]; //#18 Free ASCII text message
252 uint8_t free_char1
; //#39 Free ASCII character. appears right to home distance
253 uint8_t free_char2
; //#40 Free ASCII character. appears right to home direction
254 uint8_t free_char3
; //#41 Free ASCII character. appears? TODO: Check where this char appears
255 uint8_t compass_direction
; //#42 Compass heading in 2� steps. 1 = 2�
256 uint8_t version
; //#43 version number TODO: more info?
257 uint8_t stop_byte
; //#44 stop uint8_t, constant value 0x7d
260 typedef struct HOTT_EAM_MSG_s
{
261 uint8_t start_byte
; //#01 start uint8_t
262 uint8_t eam_sensor_id
; //#02 EAM sensort id. constat value 0x8e
263 uint8_t warning_beeps
; //#03 1=A 2=B ... or 'A' - 0x40 = 1
264 // Q Min cell voltage sensor 1
265 // R Min Battery 1 voltage sensor 1
266 // J Max Battery 1 voltage sensor 1
267 // F Mim temperature sensor 1
268 // H Max temperature sensor 1
269 // S Min cell voltage sensor 2
270 // K Max cell voltage sensor 2
271 // G Min temperature sensor 2
272 // I Max temperature sensor 2
274 // V Max capacity mAh
275 // P Min main power voltage
276 // X Max main power voltage
279 // C (negative) sink rate m/sec to high
280 // B (negative) sink rate m/3sec to high
281 // N climb rate m/sec to high
282 // M climb rate m/3sec to high
284 uint8_t sensor_id
; //#04 constant value 0xe0
285 uint8_t alarm_invers1
; //#05 alarm bitmask. Value is displayed inverted
294 // 7 Main power voltage
295 uint8_t alarm_invers2
; //#06 alarm bitmask. Value is displayed inverted
299 // 2 Altitude (duplicate?)
300 // 3 m/s (duplicate?)
301 // 4 m/3s (duplicate?)
304 // 7 "ON" sign/text msg active
306 uint8_t cell1_L
; //#07 cell 1 voltage lower value. 0.02V steps, 124=2.48V
307 uint8_t cell2_L
; //#08
308 uint8_t cell3_L
; //#09
309 uint8_t cell4_L
; //#10
310 uint8_t cell5_L
; //#11
311 uint8_t cell6_L
; //#12
312 uint8_t cell7_L
; //#13
313 uint8_t cell1_H
; //#14 cell 1 voltage high value. 0.02V steps, 124=2.48V
314 uint8_t cell2_H
; //#15
315 uint8_t cell3_H
; //#16
316 uint8_t cell4_H
; //#17
317 uint8_t cell5_H
; //#18
318 uint8_t cell6_H
; //#19
319 uint8_t cell7_H
; //#20
321 uint8_t batt1_voltage_L
; //#21 battery 1 voltage lower value in 100mv steps, 50=5V. optionally cell8_L value 0.02V steps
322 uint8_t batt1_voltage_H
; //#22
324 uint8_t batt2_voltage_L
; //#23 battery 2 voltage lower value in 100mv steps, 50=5V. optionally cell8_H value. 0.02V steps
325 uint8_t batt2_voltage_H
; //#24
327 uint8_t temp1
; //#25 Temperature sensor 1. 20=0�, 46=26� - offset of 20.
328 uint8_t temp2
; //#26 temperature sensor 2
330 uint8_t altitude_L
; //#27 Attitude lower value. unit: meters. Value of 500 = 0m
331 uint8_t altitude_H
; //#28
333 uint8_t current_L
; //#29 Current in 0.1 steps
334 uint8_t current_H
; //#30
336 uint8_t main_voltage_L
; //#31 Main power voltage (drive) in 0.1V steps
337 uint8_t main_voltage_H
; //#32
339 uint8_t batt_cap_L
; //#33 used battery capacity in 10mAh steps
340 uint8_t batt_cap_H
; //#34
342 uint8_t climbrate_L
; //#35 climb rate in 0.01m/s. Value of 30000 = 0.00 m/s
343 uint8_t climbrate_H
; //#36
345 uint8_t climbrate3s
; //#37 climbrate in m/3sec. Value of 120 = 0m/3sec
347 uint8_t rpm_L
; //#38 RPM. Steps: 10 U/min
350 uint8_t electric_min
; //#40 Electric minutes. Time does start, when motor current is > 3 A
351 uint8_t electric_sec
; //#41
353 uint8_t speed_L
; //#42 (air?) speed in km/h. Steps 1km/h
354 uint8_t speed_H
; //#43
355 uint8_t stop_byte
; //#44 stop uint8_t
358 //HoTT GPS Sensor response to Receiver (?!not?! Smartbox)
359 typedef struct HOTT_GPS_MSG_s
{
360 uint8_t start_byte
; //#01 constant value 0x7c
361 uint8_t gps_sensor_id
; //#02 constant value 0x8a
362 uint8_t warning_beeps
; //#03 1=A 2=B ...
367 // C (negative) sink rate m/sec to high
368 // B (negative) sink rate m/3sec to high
369 // N climb rate m/sec to high
370 // M climb rate m/3sec to high
371 // D Max home distance
374 uint8_t sensor_id
; //#04 constant (?) value 0xa0
375 uint8_t alarm_invers1
; //#05
377 uint8_t alarm_invers2
; //#06 1 = No GPS signal
380 uint8_t flight_direction
; //#07 flight direction in 2 degreees/step (1 = 2degrees);
381 uint8_t gps_speed_L
; //08 km/h
382 uint8_t gps_speed_H
; //#09
384 uint8_t pos_NS
; //#10 north = 0, south = 1
385 uint8_t pos_NS_dm_L
; //#11 degree minutes ie N48�39�988
386 uint8_t pos_NS_dm_H
; //#12
387 uint8_t pos_NS_sec_L
; //#13 position seconds
388 uint8_t pos_NS_sec_H
; //#14
390 uint8_t pos_EW
; //#15 east = 0, west = 1
391 uint8_t pos_EW_dm_L
; //#16 degree minutes ie. E9�25�9360
392 uint8_t pos_EW_dm_H
; //#17
393 uint8_t pos_EW_sec_L
; //#18 position seconds
394 uint8_t pos_EW_sec_H
; //#19
396 uint8_t home_distance_L
; //#20 meters
397 uint8_t home_distance_H
; //#21
399 uint8_t altitude_L
; //#22 meters. Value of 500 = 0m
400 uint8_t altitude_H
; //#23
402 uint8_t climbrate_L
; //#24 m/s 0.01m/s resolution. Value of 30000 = 0.00 m/s
403 uint8_t climbrate_H
; //#25
405 uint8_t climbrate3s
; //#26 climbrate in m/3s resolution, value of 120 = 0 m/3s
407 uint8_t gps_satelites
;//#27 sat count
408 uint8_t gps_fix_char
; //#28 GPS fix character. display, 'D' = DGPS, '2' = 2D, '3' = 3D, '-' = no fix. Where appears this char???
410 uint8_t home_direction
;//#29 direction from starting point to Model position (2 degree steps)
411 uint8_t angle_roll
; //#30 angle roll in 2 degree steps
412 uint8_t angle_nick
; //#31 angle in 2degree steps
413 uint8_t angle_compass
; //#32 angle in 2degree steps. 1 = 2�, 255 = - 2� (1 uint8_t) North = 0�
415 uint8_t gps_time_h
; //#33 UTC time hours
416 uint8_t gps_time_m
; //#34 UTC time minutes
417 uint8_t gps_time_s
; //#35 UTC time seconds
418 uint8_t gps_time_sss
; //#36 UTC time milliseconds
420 uint8_t msl_altitude_L
;//#37 mean sea level altitude
421 uint8_t msl_altitude_H
;//#38
423 uint8_t vibration
; //#39 vibrations level in %
425 uint8_t free_char1
; //#40 appears right to home distance
426 uint8_t free_char2
; //#41 appears right to home direction
427 uint8_t free_char3
; //#42 GPS ASCII D=DGPS 2=2D 3=3D -=No Fix
428 uint8_t version
; //#43
429 // 0 GPS Graupner #33600
432 uint8_t stop_byte
; //#44 constant value 0x7d
435 typedef struct HOTT_AIRESC_MSG_s
{
436 uint8_t start_byte
; //#01 constant value 0x7c
437 uint8_t gps_sensor_id
; //#02 constant value 0x8c
438 uint8_t warning_beeps
; //#03 1=A 2=B ...
450 uint8_t sensor_id
; //#04 constant value 0xc0
451 uint8_t alarm_invers1
; //#05 TODO: more info
452 uint8_t alarm_invers2
; //#06 TODO: more info
453 uint8_t input_v_L
; //#07 Input voltage low byte
454 uint8_t input_v_H
; //#08
455 uint8_t input_v_min_L
; //#09 Input min. voltage low byte
456 uint8_t input_v_min_H
; //#10
457 uint8_t batt_cap_L
; //#11 battery capacity in 10mAh steps
458 uint8_t batt_cap_H
; //#12
459 uint8_t esc_temp
; //#13 ESC temperature
460 uint8_t esc_max_temp
; //#14 ESC max. temperature
461 uint8_t current_L
; //#15 Current in 0.1 steps
462 uint8_t current_H
; //#16
463 uint8_t current_max_L
; //#17 Current max. in 0.1 steps
464 uint8_t current_max_H
; //#18
465 uint8_t rpm_L
; //#19 RPM in 10U/min steps
467 uint8_t rpm_max_L
; //#21 RPM max
468 uint8_t rpm_max_H
; //#22
469 uint8_t throttle
; //#23 throttle in %
470 uint8_t speed_L
; //#24 Speed
471 uint8_t speed_H
; //#25
472 uint8_t speed_max_L
; //#26 Speed max
473 uint8_t speed_max_H
; //#27
474 uint8_t bec_v
; //#28 BEC voltage
475 uint8_t bec_min_v
; //#29 BEC min. voltage
476 uint8_t bec_current
; //#30 BEC current
477 uint8_t bec_current_max_L
; //#31 BEC max. current
478 uint8_t bec_current_max_H
; //#32 TODO: not really clear why 2 bytes...
479 uint8_t pwm
; //#33 PWM
480 uint8_t bec_temp
; //#34 BEC temperature
481 uint8_t bec_temp_max
; //#35 BEC highest temperature
482 uint8_t motor_temp
; //#36 Motor or external sensor temperature
483 uint8_t motor_temp_max
; //#37 Highest motor or external sensor temperature
484 uint8_t motor_rpm_L
; //#38 Motor or external RPM sensor (without gear)
485 uint8_t motor_rpm_H
; //#39
486 uint8_t motor_timing
; //#40 Motor timing
487 uint8_t motor_timing_adv
; //#41 Motor advanced timing
488 uint8_t motor_highest_current
; //#42 Motor number (1-x) with highest current
489 uint8_t version
; //#43 Version number (highest current motor 1-x)
490 uint8_t stop_byte
; //#44 constant value 0x7d
493 void handleHoTTTelemetry(timeUs_t currentTimeUs
);
494 void checkHoTTTelemetryState(void);
496 void initHoTTTelemetry(void);
497 void configureHoTTTelemetryPort(void);
498 void freeHoTTTelemetryPort(void);
500 #if defined (USE_HOTT_TEXTMODE) && defined (USE_CMS)
501 bool hottTextmodeIsAlive(void);
502 void hottTextmodeGrab(void);
503 void hottTextmodeExit(void);
504 void hottTextmodeWriteChar(uint8_t column
, uint8_t row
, char c
);
507 uint32_t getHoTTTelemetryProviderBaudRate(void);
509 void hottPrepareGPSResponse(HOTT_GPS_MSG_t
*hottGPSMessage
);