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 : /*
17 : Graupner Hott Telemetry library
18 : Hott telemetry runs at 19200 8N1 on a non-inverted half-duplex UART
19 :
20 : With thanks to Graupner and betaflight
21 : */
22 :
23 : #include "AP_Hott_Telem.h"
24 :
25 : #if HAL_HOTT_TELEM_ENABLED
26 :
27 : #include <AP_AHRS/AP_AHRS.h>
28 : #include <AP_BattMonitor/AP_BattMonitor.h>
29 : #include <AP_Baro/AP_Baro.h>
30 : #include <AP_RPM/AP_RPM.h>
31 : #include <AP_Airspeed/AP_Airspeed.h>
32 : #include <AP_Stats/AP_Stats.h>
33 : #include <AP_GPS/AP_GPS.h>
34 : #include <AP_RTC/AP_RTC.h>
35 : #include <AP_Notify/AP_Notify.h>
36 : #include <AP_Mission/AP_Mission.h>
37 : #include <AP_SerialManager/AP_SerialManager.h>
38 : #include <stdio.h>
39 :
40 : #define PROT_BINARY 0x80
41 : #define PROT_ID_GAM 0x8D
42 : #define PROT_ID_EAM 0x8E
43 : #define PROT_ID_GPS 0x8A
44 : #define PROT_ID_VARIO 0x89
45 :
46 : #define BYTE_DELAY_FIRST_US 4000
47 : #define BYTE_DELAY_US 1200
48 :
49 : extern const AP_HAL::HAL& hal;
50 :
51 : AP_Hott_Telem *AP_Hott_Telem::singleton;
52 :
53 2098 : AP_Hott_Telem::AP_Hott_Telem(void)
54 : {
55 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
56 2096 : if (singleton != nullptr) {
57 0 : AP_HAL::panic("AP_Hott_Telem must be singleton");
58 : }
59 : #endif
60 2098 : singleton = this;
61 2098 : }
62 :
63 : /*
64 : * initialise uart
65 : */
66 2086 : void AP_Hott_Telem::init()
67 : {
68 2086 : const AP_SerialManager &serial_manager = AP::serialmanager();
69 :
70 2086 : uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Hott, 0);
71 2086 : if (uart) {
72 : // register thread
73 0 : if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_Hott_Telem::loop, void),
74 : "Hott",
75 : 1024, AP_HAL::Scheduler::PRIORITY_BOOST, 1)) {
76 0 : DEV_PRINTF("Failed to create Hott thread\n");
77 : }
78 : }
79 2086 : }
80 :
81 : /*
82 : send EAM (Electric Air Model)
83 : */
84 0 : void AP_Hott_Telem::send_EAM(void)
85 : {
86 : // EAM message
87 : struct PACKED {
88 : uint8_t start_byte = 0x7C; //#01 start uint8_t
89 : uint8_t eam_sensor_id = 0x8E;//#02 EAM sensort id. constat value 0x8e
90 : uint8_t warning_beeps;
91 : uint8_t sensor_id = 0xE0;
92 : uint16_t alarms; //#05 alarm bitmask. Value is displayed inverted
93 : uint8_t cell_low[7]; //#07 cell voltage lower value. 0.02V steps, 124=2.48V
94 : uint8_t cell_high[7]; //#14 cell voltage high value. 0.02V steps, 124=2.48V
95 : uint16_t batt1_voltage; //#21 battery 1 voltage in 100mv steps
96 : uint16_t batt2_voltage; //#23 battery 2 voltage in 100mv steps
97 : uint8_t temp1; //#25 Temperature sensor 1. 20=0C, 46=26C - offset of 20.
98 : uint8_t temp2; //#26 temperature sensor 2
99 : uint16_t altitude; //#27 Attitude unit: meters. Value of 500 = 0m
100 : uint16_t current; //#29 Current in 0.1A steps
101 : uint16_t main_voltage; //#31 Main power voltage (drive) in 0.1V steps
102 : uint16_t batt_used; //#33 used battery capacity in 10mAh steps
103 : uint16_t climbrate; //#35 climb rate in 0.01m/s. Value of 30000 = 0.00 m/s
104 : uint8_t climbrate3s; //#37 climbrate in m/3sec. Value of 120 = 0m/3sec
105 : uint16_t rpm; //#38 RPM. Steps: 10 rev/min
106 : uint8_t electric_min; //#40 Electric minutes. Time starts when motor current is > 3 A
107 : uint8_t electric_sec; //#41
108 : uint16_t speed; //#42 speed in km/h. Steps 1km/h
109 : uint8_t stop_byte = 0x7D; //#44 stop
110 0 : } msg {};
111 :
112 : #if AP_BATTERY_ENABLED
113 0 : const AP_BattMonitor &battery = AP::battery();
114 0 : if (battery.num_instances() > 0) {
115 0 : msg.batt1_voltage = uint16_t(battery.voltage(0) * 10);
116 : }
117 0 : if (battery.num_instances() > 1) {
118 0 : msg.batt2_voltage = uint16_t(battery.voltage(1) * 10);
119 : }
120 : float current;
121 0 : if (battery.current_amps(current)) {
122 0 : msg.current = uint16_t(current * 10);
123 : }
124 0 : msg.main_voltage = uint16_t(battery.voltage() * 10);
125 : float used_mah;
126 0 : if (battery.consumed_mah(used_mah)) {
127 0 : msg.batt_used = used_mah * 0.1;
128 : }
129 : #endif // AP_BATTERY_ENABLED
130 :
131 0 : const AP_Baro &baro = AP::baro();
132 0 : msg.temp1 = uint8_t(baro.get_temperature(0) + 20.5);
133 : #if BARO_MAX_INSTANCES > 1
134 0 : if (baro.healthy(1)) {
135 0 : msg.temp2 = uint8_t(baro.get_temperature(1) + 20.5);
136 : }
137 : #endif
138 :
139 0 : AP_AHRS &ahrs = AP::ahrs();
140 0 : float alt = 0;
141 0 : Vector3f vel;
142 : {
143 0 : WITH_SEMAPHORE(ahrs.get_semaphore());
144 0 : ahrs.get_relative_position_D_home(alt);
145 0 : alt = -alt;
146 0 : IGNORE_RETURN(ahrs.get_velocity_NED(vel));
147 0 : }
148 0 : msg.altitude = uint16_t(500.5 + alt);
149 :
150 0 : msg.climbrate = uint16_t(30000.5 + vel.z * -100);
151 0 : msg.climbrate3s = 120 + vel.z * -3;
152 :
153 : #if AP_RPM_ENABLED
154 0 : const AP_RPM *rpm = AP::rpm();
155 : float rpm_value;
156 0 : if (rpm && rpm->get_rpm(0, rpm_value)) {
157 0 : msg.rpm = rpm_value * 0.1;
158 : }
159 : #endif
160 :
161 0 : AP_Stats *stats = AP::stats();
162 0 : if (stats) {
163 0 : uint32_t t = stats->get_flight_time_s();
164 0 : msg.electric_min = t / 60U;
165 0 : msg.electric_sec = t % 60U;
166 : }
167 :
168 : #if AP_AIRSPEED_ENABLED
169 0 : AP_Airspeed *airspeed = AP_Airspeed::get_singleton();
170 0 : if (airspeed && airspeed->healthy()) {
171 0 : msg.speed = uint16_t(airspeed->get_airspeed() * 3.6 + 0.5);
172 : } else {
173 0 : WITH_SEMAPHORE(ahrs.get_semaphore());
174 0 : msg.speed = uint16_t(ahrs.groundspeed() * 3.6 + 0.5);
175 0 : }
176 : #else
177 : WITH_SEMAPHORE(ahrs.get_semaphore());
178 : msg.speed = uint16_t(ahrs.groundspeed() * 3.6 + 0.5);
179 : #endif
180 :
181 0 : send_packet((const uint8_t *)&msg, sizeof(msg));
182 0 : }
183 :
184 : /*
185 : convert from a GPS lat/lon in decimal degrees to degrees plus decimal minutes
186 : */
187 0 : void AP_Hott_Telem::GPS_to_DDM(float decimal, uint8_t &sign, uint16_t &dm, uint16_t &sec) const
188 : {
189 0 : sign = decimal>=0?0:1;
190 0 : decimal = fabsf(decimal);
191 0 : uint8_t deg = uint16_t(decimal);
192 0 : uint8_t min = uint16_t((decimal - deg) * 60);
193 0 : dm = deg*100 + min;
194 0 : sec = (decimal - (deg + min/60.0)) * 60 * 10000 + 0.5;
195 0 : }
196 :
197 : /*
198 : send GPS packet
199 : */
200 0 : void AP_Hott_Telem::send_GPS(void)
201 : {
202 : // GPS message
203 : struct PACKED {
204 : uint8_t start_byte = 0x7c; //#01 constant value 0x7c
205 : uint8_t gps_sensor_id = 0x8a; //#02 constant value 0x8a
206 : uint8_t warning_beeps; //#03
207 : uint8_t sensor_id = 0xA0; //#04 constant (?) value 0xa0
208 : uint16_t alarm; //#05
209 : uint8_t flight_direction; //#07 flight direction in 2 degreees/step (1 = 2degrees);
210 : uint16_t gps_speed_kmh; //#08 km/h
211 : uint8_t pos_NS; //#10 north = 0, south = 1
212 : uint16_t pos_NS_dm; //#11 degree minutes
213 : uint16_t pos_NS_sec; //#13 position seconds
214 : uint8_t pos_EW; //#15 east = 0, west = 1
215 : uint16_t pos_EW_dm; //#16 degree minutes
216 : uint16_t pos_EW_sec; //#18 position seconds
217 : uint16_t home_distance; //#20 meters
218 : uint16_t altitude; //#22 meters. Value of 500 = 0m
219 : uint16_t climbrate; //#24 m/s 0.01m/s resolution. Value of 30000 = 0.00 m/s
220 : uint8_t climbrate3s; //#26 climbrate in m/3s resolution, value of 120 = 0 m/3s
221 : uint8_t gps_satelites; //#27 sat count
222 : uint8_t gps_fix_char; //#28 GPS fix character. display, 'D' = DGPS, '2' = 2D, '3' = 3D, '-' = no fix
223 : uint8_t home_direction; //#29 direction from starting point to Model position (2 degree steps)
224 : int16_t vel_north; //#30 velocity north mm/s
225 : uint8_t speed_acc; //#32 speed accuracy cm/s
226 : uint8_t gps_time_h; //#33 UTC time hours
227 : uint8_t gps_time_m; //#34 UTC time minutes
228 : uint8_t gps_time_s; //#35 UTC time seconds
229 : uint8_t gps_time_hs; //#36 UTC time 0.01s units
230 : int16_t vel_east; //#37 velocity north mm/s
231 : uint8_t horiz_acc; //#39 horizontal accuracy
232 : uint8_t free_char1; //#40 displayed to right of home
233 : uint8_t free_char2; //#41
234 : uint8_t free_char3; //#42 GPS fix character. display, 'D' = DGPS, '2' = 2D, '3' = 3D, '-' = no fix
235 : uint8_t version = 1; //#43 0: GPS Graupner #33600, 1: ArduPilot
236 : uint8_t stop_byte = 0x7d; //#44
237 0 : } msg {};
238 :
239 0 : AP_GPS &gps = AP::gps();
240 0 : Location loc;
241 :
242 : {
243 0 : WITH_SEMAPHORE(gps.get_semaphore());
244 0 : loc = gps.location();
245 0 : msg.flight_direction = uint16_t(gps.ground_course() * 0.5 + 0.5);
246 0 : msg.gps_speed_kmh = uint16_t(gps.ground_speed() * 3.6 + 0.5);
247 : float sacc, hacc;
248 0 : if (gps.speed_accuracy(sacc)) {
249 0 : msg.speed_acc = sacc * 100 + 0.5;
250 : }
251 0 : if (gps.horizontal_accuracy(hacc)) {
252 0 : msg.horiz_acc = hacc * 100 + 0.5;
253 : }
254 0 : msg.gps_satelites = gps.num_sats();
255 0 : }
256 :
257 0 : float lat = loc.lat * 1.0e-7;
258 0 : float lon = loc.lng * 1.0e-7;
259 :
260 : uint16_t dm, sec;
261 0 : GPS_to_DDM(lat, msg.pos_NS, dm, sec);
262 0 : msg.pos_NS_dm = dm;
263 0 : msg.pos_NS_sec = sec;
264 :
265 0 : GPS_to_DDM(lon, msg.pos_EW, dm, sec);
266 0 : msg.pos_EW_dm = dm;
267 0 : msg.pos_EW_sec = sec;
268 :
269 0 : AP_AHRS &ahrs = AP::ahrs();
270 0 : Vector2f home_vec;
271 0 : float alt = 0;
272 0 : Vector3f vel;
273 : {
274 0 : WITH_SEMAPHORE(ahrs.get_semaphore());
275 0 : if (ahrs.get_relative_position_NE_home(home_vec)) {
276 0 : msg.home_distance = home_vec.length();
277 : }
278 0 : ahrs.get_relative_position_D_home(alt);
279 0 : alt = -alt;
280 0 : IGNORE_RETURN(ahrs.get_velocity_NED(vel));
281 0 : }
282 :
283 0 : msg.climbrate = uint16_t(30000.5 + vel.z * -100);
284 0 : msg.climbrate3s = 120 + vel.z * -3;
285 0 : msg.vel_north = vel.x * 1000 + 0.5;
286 0 : msg.vel_east = vel.y * 1000 + 0.5;
287 0 : msg.altitude = uint16_t(500.5 + alt);
288 :
289 0 : msg.gps_fix_char = gps.status_onechar();
290 0 : msg.free_char3 = msg.gps_fix_char;
291 :
292 0 : msg.home_direction = degrees(atan2f(home_vec.y, home_vec.x)) * 0.5 + 0.5;
293 :
294 : #if AP_RTC_ENABLED
295 0 : AP_RTC &rtc = AP::rtc();
296 : {
297 0 : WITH_SEMAPHORE(rtc.get_semaphore());
298 : uint16_t ms;
299 0 : rtc.get_system_clock_utc(msg.gps_time_h, msg.gps_time_m, msg.gps_time_s, ms);
300 0 : }
301 : #endif
302 :
303 0 : send_packet((const uint8_t *)&msg, sizeof(msg));
304 0 : }
305 :
306 : /*
307 : send Vario
308 : */
309 0 : void AP_Hott_Telem::send_Vario(void)
310 : {
311 : // Vario message
312 : struct PACKED {
313 : uint8_t start_byte = 0x7C; //#01 start uint8_t
314 : uint8_t vario_id = 0x89; //#02 ID
315 : uint8_t warning_beeps; //#03 warnings
316 : uint8_t sensor_id = 0x90; //#04 sensor ID
317 : uint8_t inv_status; //#05 status
318 : uint16_t altitude; //#06 Attitude meters. Value of 500 = 0m
319 : uint16_t altitude_max; //#08 Attitude max meters. Value of 500 = 0m
320 : uint16_t altitude_min; //#10 Attitude min meters. Value of 500 = 0m
321 : uint16_t climbrate; //#12 climb rate in 0.01m/s. Value of 30000 = 0.00 m/s
322 : uint16_t climbrate3s; //#14 climb rate in meters per 3s Value of 30000 = 0.00 m/s
323 : uint16_t climbrate10s; //#16 climb rate in meters per 10s. Value of 30000 = 0.00 m/s
324 : char text[3][7]; //#18 #Text display
325 : char ascii3[3]; //#39 3 extra characters
326 : uint8_t yaw; //#42 yaw in 2 degree units, 0 = north
327 : uint8_t version = 1; //#43 protocol version
328 : uint8_t stop_byte = 0x7D; //#44 stop
329 0 : } msg {};
330 :
331 0 : AP_AHRS &ahrs = AP::ahrs();
332 0 : Vector3f vel;
333 0 : float alt = 0;
334 : {
335 0 : WITH_SEMAPHORE(ahrs.get_semaphore());
336 0 : ahrs.get_relative_position_D_home(alt);
337 0 : alt = -alt;
338 0 : IGNORE_RETURN(ahrs.get_velocity_NED(vel));
339 0 : msg.yaw = wrap_360_cd(ahrs.yaw_sensor) * 0.005;
340 0 : }
341 :
342 0 : min_alt = MIN(alt, min_alt);
343 0 : max_alt = MAX(alt, max_alt);
344 :
345 0 : msg.altitude = uint16_t(500.5 + alt);
346 0 : msg.altitude_max = uint16_t(500.5 + max_alt);
347 0 : msg.altitude_min = uint16_t(500.5 + min_alt);
348 :
349 0 : msg.climbrate = 30000.5 + vel.z * -100;
350 0 : msg.climbrate3s = 30000.5 + vel.z * -100*3;
351 0 : msg.climbrate10s = 30000.5 + vel.z * -100*10;
352 :
353 0 : AP_Notify *notify = AP_Notify::get_singleton();
354 0 : char fltmode[5] {};
355 0 : if (notify) {
356 0 : strncpy(fltmode, notify->get_flight_mode_str(), sizeof(fltmode));
357 0 : strncpy(msg.text[0], fltmode, sizeof(msg.text[0]));
358 : }
359 0 : if (hal.util->get_soft_armed()) {
360 0 : strncpy(msg.text[1], "ARMED", sizeof(msg.text[1]));
361 : #if AP_MISSION_ENABLED
362 0 : if (strncmp(fltmode, "AUTO", sizeof(fltmode)) == 0) {
363 0 : char wp[10] {};
364 0 : snprintf(wp, sizeof(wp), "WP %3u", AP::mission().get_current_nav_index());
365 0 : memcpy(msg.text[2], wp, sizeof(msg.text[2]));
366 : }
367 : #endif // AP_MISSION_ENABLED
368 : } else {
369 0 : strncpy(msg.text[1], "DISARM", sizeof(msg.text[1]));
370 0 : const char *ck = AP_Notify::flags.pre_arm_check ? "CK:PASS" : "CK:FAIL";
371 0 : memcpy(msg.text[2], ck, MIN(strlen(ck), sizeof(msg.text[2])));
372 : }
373 :
374 0 : send_packet((const uint8_t *)&msg, sizeof(msg));
375 0 : }
376 :
377 : /*
378 : send a packet out
379 : */
380 0 : void AP_Hott_Telem::send_packet(const uint8_t *b, uint8_t len)
381 : {
382 : // initial delay
383 0 : hal.scheduler->delay_microseconds(BYTE_DELAY_FIRST_US);
384 0 : uint8_t crc = 0;
385 0 : while (len) {
386 0 : uint8_t ob = *b;
387 0 : if (uart->write(ob) == 1) {
388 0 : len--;
389 0 : crc += ob;
390 0 : b++;
391 0 : hal.scheduler->delay_microseconds(BYTE_DELAY_US);
392 : } else {
393 0 : hal.scheduler->delay_microseconds(100);
394 : }
395 : }
396 0 : uart->write(crc);
397 :
398 : // discard any bytes received during the send
399 0 : hal.scheduler->delay_microseconds(BYTE_DELAY_US*2);
400 0 : while (uart->available() != 0) {
401 0 : uart->read();
402 0 : hal.scheduler->delay_microseconds(100);
403 : }
404 0 : }
405 :
406 : /*
407 : thread to process requests
408 : */
409 0 : void AP_Hott_Telem::loop(void)
410 : {
411 0 : uart->begin(19200, 10, 10);
412 0 : uart->set_unbuffered_writes(true);
413 :
414 : while (true) {
415 0 : hal.scheduler->delay_microseconds(1500);
416 0 : uint32_t n = uart->available();
417 0 : if (n < 2) {
418 : // wait for 2 bytes
419 0 : continue;
420 : }
421 0 : if (n > 2) {
422 0 : uart->discard_input();
423 0 : continue;
424 : }
425 :
426 0 : const uint8_t prot_type = uart->read();
427 0 : const uint8_t sensor_id = uart->read();
428 0 : if (prot_type != PROT_BINARY) {
429 : // only do binary protocol for now
430 0 : continue;
431 : }
432 :
433 0 : switch (sensor_id) {
434 0 : case PROT_ID_EAM:
435 0 : send_EAM();
436 0 : break;
437 0 : case PROT_ID_GPS:
438 0 : send_GPS();
439 0 : break;
440 0 : case PROT_ID_VARIO:
441 0 : send_Vario();
442 0 : break;
443 : }
444 0 : }
445 : }
446 :
447 : namespace AP {
448 0 : AP_Hott_Telem *hott_telem() {
449 0 : return AP_Hott_Telem::get_singleton();
450 : }
451 : };
452 :
453 : #endif // HAL_HOTT_TELEM_ENABLED
|
