?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
update credits
[librepilot.git] / flight / modules / UAVOHottBridge / uavohottbridge.c
blob60023b759d64de1cfe0ee165fccf16daceb44e7b
1 /**
2 ******************************************************************************
3 * @addtogroup OpenPilotModules OpenPilot Modules
4 * @{
5 * @addtogroup UAVOHoTTBridge UAVO to HoTT Bridge Telemetry Module
6 * @{
7 *
8 * @file uavohottbridge.c
9 * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017.
10 * Tau Labs, http://taulabs.org, Copyright (C) 2013-2014
11 * @brief sends telemery data on HoTT request
12 *
13 * @see The GNU Public License (GPL) Version 3
14 *
15 *****************************************************************************/
16 /*
17 * This program is free software; you can redistribute it and/or modify
18 * it under the terms of the GNU General Public License as published by
19 * the Free Software Foundation; either version 3 of the License, or
20 * (at your option) any later version.
21 *
22 * This program is distributed in the hope that it will be useful, but
23 * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
24 * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
25 * for more details.
26 *
27 * You should have received a copy of the GNU General Public License along
28 * with this program; if not, write to the Free Software Foundation, Inc.,
29 * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
30 *
31 * Additional note on redistribution: The copyright and license notices above
32 * must be maintained in each individual source file that is a derivative work
33 * of this source file; otherwise redistribution is prohibited.
34 */
35
36 #include "openpilot.h"
37 #include "hwsettings.h"
38 #include "taskinfo.h"
39 #include "callbackinfo.h"
40 #include "hottbridgesettings.h"
41 #include "attitudestate.h"
42 #include "barosensor.h"
43 #include "flightbatterystate.h"
44 #include "flightstatus.h"
45 #include "gyrosensor.h"
46 #include "gpspositionsensor.h"
47 #include "gpstime.h"
48 #include "airspeedstate.h"
49 #include "homelocation.h"
50 #include "positionstate.h"
51 #include "systemalarms.h"
52 #include "velocitystate.h"
53 #include "hottbridgestatus.h"
54 #include "hottbridgesettings.h"
55 #include "objectpersistence.h"
56 #include "pios_sensors.h"
57 #include "uavohottbridge.h"
58
59 #include "pios_board_io.h"
60
61 #if defined(PIOS_INCLUDE_HOTT_BRIDGE)
62
63 #if defined(PIOS_HoTT_STACK_SIZE)
64 #define STACK_SIZE_BYTES PIOS_HoTT_STACK_SIZE
65 #else
66 #define STACK_SIZE_BYTES 2048
67 #endif
68 #define TASK_PRIORITY CALLBACK_TASK_AUXILIARY
69
70 static bool module_enabled = false;
71
72 // Private variables
73 static bool module_enabled;
74 static struct telemetrydata *telestate;
75 static HoTTBridgeStatusData status;
76
77 // Private functions
78 static void uavoHoTTBridgeTask(void *parameters);
79 static uint16_t build_VARIO_message(struct hott_vario_message *msg);
80 static uint16_t build_GPS_message(struct hott_gps_message *msg);
81 static uint16_t build_GAM_message(struct hott_gam_message *msg);
82 static uint16_t build_EAM_message(struct hott_eam_message *msg);
83 static uint16_t build_ESC_message(struct hott_esc_message *msg);
84 static uint16_t build_TEXT_message(struct hott_text_message *msg);
85 static uint8_t calc_checksum(uint8_t *data, uint16_t size);
86 static uint8_t generate_warning();
87 static void update_telemetrydata();
88 static void convert_long2gps(int32_t value, uint8_t *dir, uword_t *min, uword_t *sec);
89 static uint8_t scale_float2uint8(float value, float scale, float offset);
90 static int8_t scale_float2int8(float value, float scale, float offset);
91 static uword_t scale_float2uword(float value, float scale, float offset);
92
93 /**
94 * Module start routine automatically called after initialization routine
95 * @return 0 when was successful
96 */
97 static int32_t uavoHoTTBridgeStart(void)
98 {
99 status.TxPackets = 0;
100 status.RxPackets = 0;
101 status.TxFail = 0;
102 status.RxFail = 0;
103
104
105 // Start task
106 if (module_enabled) {
107 xTaskHandle taskHandle;
108
109 xTaskCreate(uavoHoTTBridgeTask, "uavoHoTTBridge", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle);
110 PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_UAVOHOTTBRIDGE, taskHandle);
111 }
112
113 return 0;
114 }
115
116 /**
117 * Module initialization routine
118 * @return 0 when initialization was successful
119 */
120 static int32_t uavoHoTTBridgeInitialize(void)
121 {
122 if (PIOS_COM_HOTT) {
123 module_enabled = true;
124 // HoTT telemetry baudrate is fixed to 19200
125
126 PIOS_COM_ChangeBaud(PIOS_COM_HOTT, 19200);
127 bool param = true;
128 PIOS_COM_Ioctl(PIOS_COM_HOTT, PIOS_IOCTL_USART_SET_HALFDUPLEX, &param);
129 HoTTBridgeStatusInitialize();
130
131 // allocate memory for telemetry data
132 telestate = (struct telemetrydata *)pios_malloc(sizeof(*telestate));
133
134 if (telestate == NULL) {
135 // there is not enough free memory. the module could not run.
136 module_enabled = false;
137 return -1;
138 }
139 } else {
140 module_enabled = false;
141 }
142 return 0;
143 }
144 MODULE_INITCALL(uavoHoTTBridgeInitialize, uavoHoTTBridgeStart);
145
146 /**
147 * Main task. It does not return.
148 */
149 static void uavoHoTTBridgeTask(__attribute__((unused)) void *parameters)
150 {
151 uint8_t rx_buffer[2];
152 uint8_t tx_buffer[HOTT_MAX_MESSAGE_LENGTH];
153 uint16_t message_size;
154
155 // clear all state values
156 memset(telestate, 0, sizeof(*telestate));
157
158 // initialize timer variables
159 portTickType lastSysTime = xTaskGetTickCount();
160 // idle delay between telemetry request and answer
161 uint32_t idledelay = IDLE_TIME;
162 // data delay between transmitted bytes
163 uint32_t datadelay = DATA_TIME;
164
165 // work on hott telemetry. endless loop.
166 while (1) {
167 // clear message size on every loop before processing
168 message_size = 0;
169
170 // shift receiver buffer. make room for one byte.
171 rx_buffer[1] = rx_buffer[0];
172
173 // wait for a byte of telemetry request in data delay interval
174 while (PIOS_COM_ReceiveBuffer(PIOS_COM_HOTT, rx_buffer, 1, 0) == 0) {
175 vTaskDelayUntil(&lastSysTime, datadelay / portTICK_RATE_MS);
176 }
177 // set start trigger point
178 lastSysTime = xTaskGetTickCount();
179
180 // examine received data stream
181 if (rx_buffer[1] == HOTT_BINARY_ID) {
182 // first received byte looks like a binary request. check second received byte for a sensor id.
183 switch (rx_buffer[0]) {
184 case HOTT_VARIO_ID:
185 message_size = build_VARIO_message((struct hott_vario_message *)tx_buffer);
186 break;
187 case HOTT_GPS_ID:
188 message_size = build_GPS_message((struct hott_gps_message *)tx_buffer);
189 break;
190 case HOTT_GAM_ID:
191 message_size = build_GAM_message((struct hott_gam_message *)tx_buffer);
192 break;
193 case HOTT_EAM_ID:
194 message_size = build_EAM_message((struct hott_eam_message *)tx_buffer);
195 break;
196 case HOTT_ESC_ID:
197 message_size = build_ESC_message((struct hott_esc_message *)tx_buffer);
198 break;
199 default:
200 message_size = 0;
201 }
202 } else if (rx_buffer[1] == HOTT_TEXT_ID) {
203 // first received byte looks like a text request. check second received byte for a valid button.
204 switch (rx_buffer[0]) {
205 case HOTT_BUTTON_DEC:
206 case HOTT_BUTTON_INC:
207 case HOTT_BUTTON_SET:
208 case HOTT_BUTTON_NIL:
209 case HOTT_BUTTON_NEXT:
210 case HOTT_BUTTON_PREV:
211 message_size = build_TEXT_message((struct hott_text_message *)tx_buffer);
212 break;
213 default:
214 message_size = 0;
215 }
216 }
217
218 // check if a message is in the transmit buffer.
219 if (message_size > 0) {
220 status.RxPackets++;
221
222 // check idle line before transmit. pause, then check receiver buffer
223 vTaskDelayUntil(&lastSysTime, idledelay / portTICK_RATE_MS);
224
225 if (PIOS_COM_ReceiveBuffer(PIOS_COM_HOTT, rx_buffer, 1, 0) == 0) {
226 // nothing received means idle line. ready to transmit the requested message
227 for (int i = 0; i < message_size; i++) {
228 // send message content with pause between each byte
229 PIOS_COM_SendCharNonBlocking(PIOS_COM_HOTT, tx_buffer[i]);
230 // grab possible incoming loopback data and throw it away
231 PIOS_COM_ReceiveBuffer(PIOS_COM_HOTT, rx_buffer, sizeof(rx_buffer), 0);
232 vTaskDelayUntil(&lastSysTime, datadelay / portTICK_RATE_MS);
233 }
234 status.TxPackets++;
235
236
237 // after transmitting the message, any loopback data needs to be cleaned up.
238 vTaskDelayUntil(&lastSysTime, idledelay / portTICK_RATE_MS);
239 PIOS_COM_ReceiveBuffer(PIOS_COM_HOTT, tx_buffer, message_size, 0);
240 } else {
241 status.RxFail++;
242 }
243 HoTTBridgeStatusSet(&status);
244 }
245 }
246 }
247
248 /**
249 * Build requested answer messages.
250 * \return value sets message size
251 */
252 uint16_t build_VARIO_message(struct hott_vario_message *msg)
253 {
254 update_telemetrydata();
255
256 if (telestate->Settings.Sensor.VARIO == HOTTBRIDGESETTINGS_SENSOR_DISABLED) {
257 return 0;
258 }
259
260 // clear message buffer
261 memset(msg, 0, sizeof(*msg));
262
263 // message header
264 msg->start = HOTT_START;
265 msg->stop = HOTT_STOP;
266 msg->sensor_id = HOTT_VARIO_ID;
267 msg->warning = generate_warning();
268 msg->sensor_text_id = HOTT_VARIO_TEXT_ID;
269
270 // alarm inverse bits. invert display areas on limits
271 msg->alarm_inverse |= (telestate->Settings.Limit.MinHeight > telestate->altitude) ? VARIO_INVERT_ALT : 0;
272 msg->alarm_inverse |= (telestate->Settings.Limit.MaxHeight < telestate->altitude) ? VARIO_INVERT_ALT : 0;
273 msg->alarm_inverse |= (telestate->Settings.Limit.MaxHeight < telestate->altitude) ? VARIO_INVERT_MAX : 0;
274 msg->alarm_inverse |= (telestate->Settings.Limit.MinHeight > telestate->altitude) ? VARIO_INVERT_MIN : 0;
275 msg->alarm_inverse |= (telestate->Settings.Limit.NegDifference1 > telestate->climbrate1s) ? VARIO_INVERT_CR1S : 0;
276 msg->alarm_inverse |= (telestate->Settings.Limit.PosDifference1 < telestate->climbrate1s) ? VARIO_INVERT_CR1S : 0;
277 msg->alarm_inverse |= (telestate->Settings.Limit.NegDifference2 > telestate->climbrate3s) ? VARIO_INVERT_CR3S : 0;
278 msg->alarm_inverse |= (telestate->Settings.Limit.PosDifference2 < telestate->climbrate3s) ? VARIO_INVERT_CR3S : 0;
279 msg->alarm_inverse |= (telestate->Settings.Limit.NegDifference2 > telestate->climbrate10s) ? VARIO_INVERT_CR10S : 0;
280 msg->alarm_inverse |= (telestate->Settings.Limit.PosDifference2 < telestate->climbrate10s) ? VARIO_INVERT_CR10S : 0;
281
282 // altitude relative to ground
283 msg->altitude = scale_float2uword(telestate->altitude, 1, OFFSET_ALTITUDE);
284 msg->min_altitude = scale_float2uword(telestate->min_altitude, 1, OFFSET_ALTITUDE);
285 msg->max_altitude = scale_float2uword(telestate->max_altitude, 1, OFFSET_ALTITUDE);
286
287 // climbrate
288 msg->climbrate = scale_float2uword(telestate->climbrate1s, M_TO_CM, OFFSET_CLIMBRATE);
289 msg->climbrate3s = scale_float2uword(telestate->climbrate3s, M_TO_CM, OFFSET_CLIMBRATE);
290 msg->climbrate10s = scale_float2uword(telestate->climbrate10s, M_TO_CM, OFFSET_CLIMBRATE);
291
292 // compass
293 msg->compass = scale_float2int8(telestate->Attitude.Yaw, DEG_TO_UINT, 0);
294
295 // statusline
296 memcpy(msg->ascii, telestate->statusline, sizeof(msg->ascii));
297
298 // free display characters
299 msg->ascii1 = 0;
300 msg->ascii2 = 0;
301 msg->ascii3 = 0;
302
303 msg->checksum = calc_checksum((uint8_t *)msg, sizeof(*msg));
304 return sizeof(*msg);
305 }
306
307 uint16_t build_GPS_message(struct hott_gps_message *msg)
308 {
309 update_telemetrydata();
310
311 if (telestate->Settings.Sensor.GPS == HOTTBRIDGESETTINGS_SENSOR_DISABLED) {
312 return 0;
313 }
314
315 // clear message buffer
316 memset(msg, 0, sizeof(*msg));
317
318 // message header
319 msg->start = HOTT_START;
320 msg->stop = HOTT_STOP;
321 msg->sensor_id = HOTT_GPS_ID;
322 msg->warning = generate_warning();
323 msg->sensor_text_id = HOTT_GPS_TEXT_ID;
324
325 // alarm inverse bits. invert display areas on limits
326 msg->alarm_inverse1 |= (telestate->Settings.Limit.MaxDistance < telestate->homedistance) ? GPS_INVERT_HDIST : 0;
327 msg->alarm_inverse1 |= (telestate->Settings.Limit.MinSpeed > telestate->GPS.Groundspeed) ? GPS_INVERT_SPEED : 0;
328 msg->alarm_inverse1 |= (telestate->Settings.Limit.MaxSpeed < telestate->GPS.Groundspeed) ? GPS_INVERT_SPEED : 0;
329 msg->alarm_inverse1 |= (telestate->Settings.Limit.MinHeight > telestate->altitude) ? GPS_INVERT_ALT : 0;
330 msg->alarm_inverse1 |= (telestate->Settings.Limit.MaxHeight < telestate->altitude) ? GPS_INVERT_ALT : 0;
331 msg->alarm_inverse1 |= (telestate->Settings.Limit.NegDifference1 > telestate->climbrate1s) ? GPS_INVERT_CR1S : 0;
332 msg->alarm_inverse1 |= (telestate->Settings.Limit.PosDifference1 < telestate->climbrate1s) ? GPS_INVERT_CR1S : 0;
333 msg->alarm_inverse1 |= (telestate->Settings.Limit.NegDifference2 > telestate->climbrate3s) ? GPS_INVERT_CR3S : 0;
334 msg->alarm_inverse1 |= (telestate->Settings.Limit.PosDifference2 < telestate->climbrate3s) ? GPS_INVERT_CR3S : 0;
335 msg->alarm_inverse2 |= (telestate->SysAlarms.Alarm.GPS != SYSTEMALARMS_ALARM_OK) ? GPS_INVERT2_POS : 0;
336
337 // gps direction, groundspeed and postition
338 msg->flight_direction = scale_float2uint8(telestate->GPS.Heading, DEG_TO_UINT, 0);
339 msg->gps_speed = scale_float2uword(telestate->GPS.Groundspeed, MS_TO_KMH, 0);
340 convert_long2gps(telestate->GPS.Latitude, &msg->latitude_ns, &msg->latitude_min, &msg->latitude_sec);
341 convert_long2gps(telestate->GPS.Longitude, &msg->longitude_ew, &msg->longitude_min, &msg->longitude_sec);
342
343 // homelocation distance, course and state
344 msg->distance = scale_float2uword(telestate->homedistance, 1, 0);
345 msg->home_direction = scale_float2uint8(telestate->homecourse, DEG_TO_UINT, 0);
346 msg->ascii5 = (telestate->Home.Set ? 'H' : '-');
347
348 // altitude relative to ground and climb rate
349 msg->altitude = scale_float2uword(telestate->altitude, 1, OFFSET_ALTITUDE);
350 msg->climbrate = scale_float2uword(telestate->climbrate1s, M_TO_CM, OFFSET_CLIMBRATE);
351 msg->climbrate3s = scale_float2uint8(telestate->climbrate3s, 1, OFFSET_CLIMBRATE3S);
352
353 // number of satellites,gps fix and state
354 msg->gps_num_sat = telestate->GPS.Satellites;
355 switch (telestate->GPS.Status) {
356 case GPSPOSITIONSENSOR_STATUS_FIX2D:
357 msg->gps_fix_char = '2';
358 break;
359 case GPSPOSITIONSENSOR_STATUS_FIX3D:
360 case GPSPOSITIONSENSOR_STATUS_FIX3DDGNSS:
361 msg->gps_fix_char = '3';
362 break;
363 default:
364 msg->gps_fix_char = 0;
365 }
366 switch (telestate->SysAlarms.Alarm.GPS) {
367 case SYSTEMALARMS_ALARM_UNINITIALISED:
368 msg->ascii6 = 0;
369 // if there is no gps, show compass flight direction
370 msg->flight_direction = scale_float2int8((telestate->Attitude.Yaw > 0) ? telestate->Attitude.Yaw : 360 + telestate->Attitude.Yaw, DEG_TO_UINT, 0);
371 break;
372 case SYSTEMALARMS_ALARM_OK:
373 msg->ascii6 = '.';
374 break;
375 case SYSTEMALARMS_ALARM_WARNING:
376 msg->ascii6 = '?';
377 break;
378 case SYSTEMALARMS_ALARM_ERROR:
379 case SYSTEMALARMS_ALARM_CRITICAL:
380 msg->ascii6 = '!';
381 break;
382 default:
383 msg->ascii6 = 0;
384 }
385
386 // model angles
387 msg->angle_roll = scale_float2int8(telestate->Attitude.Roll, DEG_TO_UINT, 0);
388 msg->angle_nick = scale_float2int8(telestate->Attitude.Pitch, DEG_TO_UINT, 0);
389 msg->angle_compass = scale_float2int8(telestate->Attitude.Yaw, DEG_TO_UINT, 0);
390
391 // gps time
392 msg->gps_hour = telestate->GPStime.Hour;
393 msg->gps_min = telestate->GPStime.Minute;
394 msg->gps_sec = telestate->GPStime.Second;
395 msg->gps_msec = 0;
396
397 // gps MSL (NN) altitude MSL
398 msg->msl = scale_float2uword(telestate->GPS.Altitude, 1, 0);
399
400 // free display chararacter
401 msg->ascii4 = 0;
402
403 msg->checksum = calc_checksum((uint8_t *)msg, sizeof(*msg));
404 return sizeof(*msg);
405 }
406
407 uint16_t build_GAM_message(struct hott_gam_message *msg)
408 {
409 update_telemetrydata();
410
411 if (telestate->Settings.Sensor.GAM == HOTTBRIDGESETTINGS_SENSOR_DISABLED) {
412 return 0;
413 }
414
415 // clear message buffer
416 memset(msg, 0, sizeof(*msg));
417
418 // message header
419 msg->start = HOTT_START;
420 msg->stop = HOTT_STOP;
421 msg->sensor_id = HOTT_GAM_ID;
422 msg->warning = generate_warning();
423 msg->sensor_text_id = HOTT_GAM_TEXT_ID;
424
425 // alarm inverse bits. invert display areas on limits
426 msg->alarm_inverse2 |= (telestate->Settings.Limit.MaxCurrent < telestate->Battery.Current) ? GAM_INVERT2_CURRENT : 0;
427 msg->alarm_inverse2 |= (telestate->Settings.Limit.MinPowerVoltage > telestate->Battery.Voltage) ? GAM_INVERT2_VOLTAGE : 0;
428 msg->alarm_inverse2 |= (telestate->Settings.Limit.MaxPowerVoltage < telestate->Battery.Voltage) ? GAM_INVERT2_VOLTAGE : 0;
429 msg->alarm_inverse2 |= (telestate->Settings.Limit.MinHeight > telestate->altitude) ? GAM_INVERT2_ALT : 0;
430 msg->alarm_inverse2 |= (telestate->Settings.Limit.MaxHeight < telestate->altitude) ? GAM_INVERT2_ALT : 0;
431 msg->alarm_inverse2 |= (telestate->Settings.Limit.NegDifference1 > telestate->climbrate1s) ? GAM_INVERT2_CR1S : 0;
432 msg->alarm_inverse2 |= (telestate->Settings.Limit.PosDifference1 < telestate->climbrate1s) ? GAM_INVERT2_CR1S : 0;
433 msg->alarm_inverse2 |= (telestate->Settings.Limit.NegDifference2 > telestate->climbrate3s) ? GAM_INVERT2_CR3S : 0;
434 msg->alarm_inverse2 |= (telestate->Settings.Limit.PosDifference2 < telestate->climbrate3s) ? GAM_INVERT2_CR3S : 0;
435
436 // temperatures
437 msg->temperature1 = scale_float2uint8(telestate->Gyro.temperature, 1, OFFSET_TEMPERATURE);
438 msg->temperature2 = scale_float2uint8(telestate->Baro.Temperature, 1, OFFSET_TEMPERATURE);
439
440 // altitude
441 msg->altitude = scale_float2uword(telestate->altitude, 1, OFFSET_ALTITUDE);
442
443 // climbrate
444 msg->climbrate = scale_float2uword(telestate->climbrate1s, M_TO_CM, OFFSET_CLIMBRATE);
445 msg->climbrate3s = scale_float2uint8(telestate->climbrate3s, 1, OFFSET_CLIMBRATE3S);
446
447 // main battery
448 float voltage = (telestate->Battery.Voltage > 0) ? telestate->Battery.Voltage : 0;
449 float current = (telestate->Battery.Current > 0) ? telestate->Battery.Current : 0;
450 float energy = (telestate->Battery.ConsumedEnergy > 0) ? telestate->Battery.ConsumedEnergy : 0;
451 msg->voltage = scale_float2uword(voltage, 10, 0);
452 msg->current = scale_float2uword(current, 10, 0);
453 msg->capacity = scale_float2uword(energy, 0.1f, 0);
454
455 // simulate individual cell voltage
456 uint8_t cell_voltage = (telestate->Battery.Voltage > 0) ? scale_float2uint8(telestate->Battery.Voltage / telestate->Battery.NbCells, 50, 0) : 0;
457 msg->cell1 = (telestate->Battery.NbCells >= 1) ? cell_voltage : 0;
458 msg->cell2 = (telestate->Battery.NbCells >= 2) ? cell_voltage : 0;
459 msg->cell3 = (telestate->Battery.NbCells >= 3) ? cell_voltage : 0;
460 msg->cell4 = (telestate->Battery.NbCells >= 4) ? cell_voltage : 0;
461 msg->cell5 = (telestate->Battery.NbCells >= 5) ? cell_voltage : 0;
462 msg->cell6 = (telestate->Battery.NbCells >= 6) ? cell_voltage : 0;
463
464 msg->min_cell_volt = cell_voltage;
465 msg->min_cell_volt_num = telestate->Battery.NbCells;
466
467 // apply main voltage to batt1 voltage
468 msg->batt1_voltage = msg->voltage;
469
470 // AirSpeed
471 float airspeed = (telestate->Airspeed.TrueAirspeed > 0) ? telestate->Airspeed.TrueAirspeed : 0;
472 msg->speed = scale_float2uword(airspeed, MS_TO_KMH, 0);
473
474 // pressure kPa to 0.1Bar
475 msg->pressure = scale_float2uint8(telestate->Baro.Pressure, 0.1f, 0);
476
477 msg->checksum = calc_checksum((uint8_t *)msg, sizeof(*msg));
478 return sizeof(*msg);
479 }
480
481 uint16_t build_EAM_message(struct hott_eam_message *msg)
482 {
483 update_telemetrydata();
484
485 if (telestate->Settings.Sensor.EAM == HOTTBRIDGESETTINGS_SENSOR_DISABLED) {
486 return 0;
487 }
488
489 // clear message buffer
490 memset(msg, 0, sizeof(*msg));
491
492 // message header
493 msg->start = HOTT_START;
494 msg->stop = HOTT_STOP;
495 msg->sensor_id = HOTT_EAM_ID;
496 msg->warning = generate_warning();
497 msg->sensor_text_id = HOTT_EAM_TEXT_ID;
498
499 // alarm inverse bits. invert display areas on limits
500 msg->alarm_inverse1 |= (telestate->Settings.Limit.MaxUsedCapacity < telestate->Battery.ConsumedEnergy) ? EAM_INVERT_CAPACITY : 0;
501 msg->alarm_inverse1 |= (telestate->Settings.Limit.MaxCurrent < telestate->Battery.Current) ? EAM_INVERT_CURRENT : 0;
502 msg->alarm_inverse1 |= (telestate->Settings.Limit.MinPowerVoltage > telestate->Battery.Voltage) ? EAM_INVERT_VOLTAGE : 0;
503 msg->alarm_inverse1 |= (telestate->Settings.Limit.MaxPowerVoltage < telestate->Battery.Voltage) ? EAM_INVERT_VOLTAGE : 0;
504 msg->alarm_inverse2 |= (telestate->Settings.Limit.MinHeight > telestate->altitude) ? EAM_INVERT2_ALT : 0;
505 msg->alarm_inverse2 |= (telestate->Settings.Limit.MaxHeight < telestate->altitude) ? EAM_INVERT2_ALT : 0;
506 msg->alarm_inverse2 |= (telestate->Settings.Limit.NegDifference1 > telestate->climbrate1s) ? EAM_INVERT2_CR1S : 0;
507 msg->alarm_inverse2 |= (telestate->Settings.Limit.PosDifference1 < telestate->climbrate1s) ? EAM_INVERT2_CR1S : 0;
508 msg->alarm_inverse2 |= (telestate->Settings.Limit.NegDifference2 > telestate->climbrate3s) ? EAM_INVERT2_CR3S : 0;
509 msg->alarm_inverse2 |= (telestate->Settings.Limit.PosDifference2 < telestate->climbrate3s) ? EAM_INVERT2_CR3S : 0;
510
511 // main battery
512 float voltage = (telestate->Battery.Voltage > 0) ? telestate->Battery.Voltage : 0;
513 float current = (telestate->Battery.Current > 0) ? telestate->Battery.Current : 0;
514 float energy = (telestate->Battery.ConsumedEnergy > 0) ? telestate->Battery.ConsumedEnergy : 0;
515 msg->voltage = scale_float2uword(voltage, 10, 0);
516 msg->current = scale_float2uword(current, 10, 0);
517 msg->capacity = scale_float2uword(energy, 0.1f, 0);
518
519 // simulate individual cell voltage
520 uint8_t cell_voltage = (telestate->Battery.Voltage > 0) ? scale_float2uint8(telestate->Battery.Voltage / telestate->Battery.NbCells, 50, 0) : 0;
521 msg->cell1_H = (telestate->Battery.NbCells >= 1) ? cell_voltage : 0;
522 msg->cell2_H = (telestate->Battery.NbCells >= 2) ? cell_voltage : 0;
523 msg->cell3_H = (telestate->Battery.NbCells >= 3) ? cell_voltage : 0;
524 msg->cell4_H = (telestate->Battery.NbCells >= 4) ? cell_voltage : 0;
525 msg->cell5_H = (telestate->Battery.NbCells >= 5) ? cell_voltage : 0;
526 msg->cell6_H = (telestate->Battery.NbCells >= 6) ? cell_voltage : 0;
527 msg->cell7_H = (telestate->Battery.NbCells >= 7) ? cell_voltage : 0;
528
529 // apply main voltage to batt1 voltage
530 msg->batt1_voltage = msg->voltage;
531
532 // AirSpeed
533 float airspeed = (telestate->Airspeed.TrueAirspeed > 0) ? telestate->Airspeed.TrueAirspeed : 0;
534 msg->speed = scale_float2uword(airspeed, MS_TO_KMH, 0);
535
536 // temperatures
537 msg->temperature1 = scale_float2uint8(telestate->Gyro.temperature, 1, OFFSET_TEMPERATURE);
538 msg->temperature2 = scale_float2uint8(telestate->Baro.Temperature, 1, OFFSET_TEMPERATURE);
539
540 // altitude
541 msg->altitude = scale_float2uword(telestate->altitude, 1, OFFSET_ALTITUDE);
542
543 // climbrate
544 msg->climbrate = scale_float2uword(telestate->climbrate1s, M_TO_CM, OFFSET_CLIMBRATE);
545 msg->climbrate3s = scale_float2uint8(telestate->climbrate3s, 1, OFFSET_CLIMBRATE3S);
546
547 // flight time
548 float flighttime = (telestate->Battery.EstimatedFlightTime <= 5999) ? telestate->Battery.EstimatedFlightTime : 5999;
549 msg->electric_min = flighttime / 60;
550 msg->electric_sec = flighttime - 60 * msg->electric_min;
551
552 msg->checksum = calc_checksum((uint8_t *)msg, sizeof(*msg));
553 return sizeof(*msg);
554 }
555
556 uint16_t build_ESC_message(struct hott_esc_message *msg)
557 {
558 update_telemetrydata();
559
560 if (telestate->Settings.Sensor.ESC == HOTTBRIDGESETTINGS_SENSOR_DISABLED) {
561 return 0;
562 }
563
564 // clear message buffer
565 memset(msg, 0, sizeof(*msg));
566
567 // message header
568 msg->start = HOTT_START;
569 msg->stop = HOTT_STOP;
570 msg->sensor_id = HOTT_ESC_ID;
571 msg->warning = 0;
572 msg->sensor_text_id = HOTT_ESC_TEXT_ID;
573
574 // main batterie
575 float voltage = (telestate->Battery.Voltage > 0) ? telestate->Battery.Voltage : 0;
576 float current = (telestate->Battery.Current > 0) ? telestate->Battery.Current : 0;
577 float max_current = (telestate->Battery.PeakCurrent > 0) ? telestate->Battery.PeakCurrent : 0;
578 float energy = (telestate->Battery.ConsumedEnergy > 0) ? telestate->Battery.ConsumedEnergy : 0;
579 msg->batt_voltage = scale_float2uword(voltage, 10, 0);
580 msg->current = scale_float2uword(current, 10, 0);
581 msg->max_current = scale_float2uword(max_current, 10, 0);
582 msg->batt_capacity = scale_float2uword(energy, 0.1f, 0);
583
584 // temperatures
585 msg->temperatureESC = scale_float2uint8(telestate->Gyro.temperature, 1, OFFSET_TEMPERATURE);
586 msg->max_temperatureESC = scale_float2uint8(0, 1, OFFSET_TEMPERATURE);
587 msg->temperatureMOT = scale_float2uint8(telestate->Baro.Temperature, 1, OFFSET_TEMPERATURE);
588 msg->max_temperatureMOT = scale_float2uint8(0, 1, OFFSET_TEMPERATURE);
589
590 msg->checksum = calc_checksum((uint8_t *)msg, sizeof(*msg));
591 return sizeof(*msg);
592 }
593
594 uint16_t build_TEXT_message(struct hott_text_message *msg)
595 {
596 update_telemetrydata();
597
598 // clear message buffer
599 memset(msg, 0, sizeof(*msg));
600
601 // message header
602 msg->start = HOTT_START;
603 msg->stop = HOTT_STOP;
604 msg->sensor_id = HOTT_TEXT_ID;
605
606 msg->checksum = calc_checksum((uint8_t *)msg, sizeof(*msg));
607 return sizeof(*msg);
608 }
609
610 /**
611 * update telemetry data
612 * this is called on every telemetry request
613 * calling interval is 200ms depending on TX
614 * 200ms telemetry request is used as time base for timed calculations (5Hz interval)
615 */
616 void update_telemetrydata()
617 {
618 // update all available data
619 if (HoTTBridgeSettingsHandle() != NULL) {
620 HoTTBridgeSettingsGet(&telestate->Settings);
621 }
622 if (AttitudeStateHandle() != NULL) {
623 AttitudeStateGet(&telestate->Attitude);
624 }
625 if (BaroSensorHandle() != NULL) {
626 BaroSensorGet(&telestate->Baro);
627 }
628 if (FlightBatteryStateHandle() != NULL) {
629 FlightBatteryStateGet(&telestate->Battery);
630 }
631 if (FlightStatusHandle() != NULL) {
632 FlightStatusGet(&telestate->FlightStatus);
633 }
634 if (GPSPositionSensorHandle() != NULL) {
635 GPSPositionSensorGet(&telestate->GPS);
636 }
637 if (AirspeedStateHandle() != NULL) {
638 AirspeedStateGet(&telestate->Airspeed);
639 }
640 if (GPSTimeHandle() != NULL) {
641 GPSTimeGet(&telestate->GPStime);
642 }
643 if (GyroSensorHandle() != NULL) {
644 GyroSensorGet(&telestate->Gyro);
645 }
646 if (HomeLocationHandle() != NULL) {
647 HomeLocationGet(&telestate->Home);
648 }
649 if (PositionStateHandle() != NULL) {
650 PositionStateGet(&telestate->Position);
651 }
652 if (SystemAlarmsHandle() != NULL) {
653 SystemAlarmsGet(&telestate->SysAlarms);
654 }
655 if (VelocityStateHandle() != NULL) {
656 VelocityStateGet(&telestate->Velocity);
657 }
658
659 // send actual climbrate value to ring buffer as mm per 0.2s values
660 uint8_t n = telestate->climbrate_pointer;
661 telestate->climbratebuffer[telestate->climbrate_pointer++] = -telestate->Velocity.Down * 200;
662 telestate->climbrate_pointer %= climbratesize;
663
664 // calculate avarage climbrates in meters per 1, 3 and 10 second(s) based on 200ms interval
665 telestate->climbrate1s = 0;
666 telestate->climbrate3s = 0;
667 telestate->climbrate10s = 0;
668 for (uint8_t i = 0; i < climbratesize; i++) {
669 telestate->climbrate1s += (i < 5) ? telestate->climbratebuffer[n] : 0;
670 telestate->climbrate3s += (i < 15) ? telestate->climbratebuffer[n] : 0;
671 telestate->climbrate10s += (i < 50) ? telestate->climbratebuffer[n] : 0;
672 n += climbratesize - 1;
673 n %= climbratesize;
674 }
675 telestate->climbrate1s = telestate->climbrate1s / 1000;
676 telestate->climbrate3s = telestate->climbrate3s / 1000;
677 telestate->climbrate10s = telestate->climbrate10s / 1000;
678
679 // set altitude offset and clear min/max values when arming
680 if ((telestate->FlightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING) || ((telestate->last_armed != FLIGHTSTATUS_ARMED_ARMED) && (telestate->FlightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED))) {
681 telestate->min_altitude = 0;
682 telestate->max_altitude = 0;
683 }
684 telestate->last_armed = telestate->FlightStatus.Armed;
685
686 // calculate altitude relative to start position
687 telestate->altitude = -telestate->Position.Down;
688
689 // check and set min/max values when armed
690 // and without receiver input for standalone board used as sensor
691 if ((telestate->FlightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) || ((telestate->SysAlarms.Alarm.Attitude == SYSTEMALARMS_ALARM_OK) && (telestate->SysAlarms.Alarm.Receiver != SYSTEMALARMS_ALARM_OK))) {
692 if (telestate->min_altitude > telestate->altitude) {
693 telestate->min_altitude = telestate->altitude;
694 }
695 if (telestate->max_altitude < telestate->altitude) {
696 telestate->max_altitude = telestate->altitude;
697 }
698 }
699
700 // gps home position and course
701 telestate->homedistance = sqrtf(telestate->Position.North * telestate->Position.North + telestate->Position.East * telestate->Position.East);
702 telestate->homecourse = acosf(-telestate->Position.North / telestate->homedistance) / 3.14159265f * 180;
703 if (telestate->Position.East > 0) {
704 telestate->homecourse = 360 - telestate->homecourse;
705 }
706
707 // statusline
708 const char *txt_unknown = "unknown";
709 const char *txt_manual = "Manual";
710 const char *txt_stabilized1 = "Stabilized1";
711 const char *txt_stabilized2 = "Stabilized2";
712 const char *txt_stabilized3 = "Stabilized3";
713 const char *txt_stabilized4 = "Stabilized4";
714 const char *txt_stabilized5 = "Stabilized5";
715 const char *txt_stabilized6 = "Stabilized6";
716 const char *txt_positionhold = "PositionHold";
717 const char *txt_courselock = "CourseLock";
718 const char *txt_velocityroam = "VelocityRoam";
719 const char *txt_homeleash = "HomeLeash";
720 const char *txt_absoluteposition = "AbsolutePosition";
721 const char *txt_returntobase = "ReturnToBase";
722 const char *txt_land = "Land";
723 const char *txt_pathplanner = "PathPlanner";
724 const char *txt_poi = "PointOfInterest";
725 const char *txt_autocruise = "AutoCruise";
726 const char *txt_autotakeoff = "AutoTakeOff";
727 const char *txt_autotune = "Autotune";
728 const char *txt_disarmed = "Disarmed";
729 const char *txt_arming = "Arming";
730 const char *txt_armed = "Armed";
731
732 const char *txt_flightmode;
733 switch (telestate->FlightStatus.FlightMode) {
734 case FLIGHTSTATUS_FLIGHTMODE_MANUAL:
735 txt_flightmode = txt_manual;
736 break;
737 case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
738 txt_flightmode = txt_stabilized1;
739 break;
740 case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
741 txt_flightmode = txt_stabilized2;
742 break;
743 case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
744 txt_flightmode = txt_stabilized3;
745 break;
746 case FLIGHTSTATUS_FLIGHTMODE_STABILIZED4:
747 txt_flightmode = txt_stabilized4;
748 break;
749 case FLIGHTSTATUS_FLIGHTMODE_STABILIZED5:
750 txt_flightmode = txt_stabilized5;
751 break;
752 case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6:
753 txt_flightmode = txt_stabilized6;
754 break;
755 case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
756 txt_flightmode = txt_positionhold;
757 break;
758 case FLIGHTSTATUS_FLIGHTMODE_COURSELOCK:
759 txt_flightmode = txt_courselock;
760 break;
761 case FLIGHTSTATUS_FLIGHTMODE_VELOCITYROAM:
762 txt_flightmode = txt_velocityroam;
763 break;
764 case FLIGHTSTATUS_FLIGHTMODE_HOMELEASH:
765 txt_flightmode = txt_homeleash;
766 break;
767 case FLIGHTSTATUS_FLIGHTMODE_ABSOLUTEPOSITION:
768 txt_flightmode = txt_absoluteposition;
769 break;
770 case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE:
771 txt_flightmode = txt_returntobase;
772 break;
773 case FLIGHTSTATUS_FLIGHTMODE_LAND:
774 txt_flightmode = txt_land;
775 break;
776 case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:
777 txt_flightmode = txt_pathplanner;
778 break;
779 case FLIGHTSTATUS_FLIGHTMODE_POI:
780 txt_flightmode = txt_poi;
781 break;
782 case FLIGHTSTATUS_FLIGHTMODE_AUTOCRUISE:
783 txt_flightmode = txt_autocruise;
784 break;
785 case FLIGHTSTATUS_FLIGHTMODE_AUTOTAKEOFF:
786 txt_flightmode = txt_autotakeoff;
787 break;
788 case FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE:
789 txt_flightmode = txt_autotune;
790 break;
791 default:
792 txt_flightmode = txt_unknown;
793 }
794
795 const char *txt_armstate;
796 switch (telestate->FlightStatus.Armed) {
797 case FLIGHTSTATUS_ARMED_DISARMED:
798 txt_armstate = txt_disarmed;
799 break;
800 case FLIGHTSTATUS_ARMED_ARMING:
801 txt_armstate = txt_arming;
802 break;
803 case FLIGHTSTATUS_ARMED_ARMED:
804 txt_armstate = txt_armed;
805 break;
806 default:
807 txt_armstate = txt_unknown;
808 }
809
810 snprintf(telestate->statusline, sizeof(telestate->statusline), "%12s,%8s", txt_flightmode, txt_armstate);
811 }
812
813 /**
814 * generate warning beeps or spoken announcements
815 */
816 uint8_t generate_warning()
817 {
818 bool gps_ok = (telestate->SysAlarms.Alarm.GPS == SYSTEMALARMS_ALARM_OK);
819
820 // set warning tone with hardcoded priority
821 if ((telestate->Settings.Warning.MinSpeed == HOTTBRIDGESETTINGS_WARNING_ENABLED) &&
822 (telestate->Settings.Limit.MinSpeed > telestate->GPS.Groundspeed * MS_TO_KMH) && gps_ok) {
823 return HOTT_TONE_A; // minimum speed
824 }
825 if ((telestate->Settings.Warning.NegDifference2 == HOTTBRIDGESETTINGS_WARNING_ENABLED) &&
826 (telestate->Settings.Limit.NegDifference2 > telestate->climbrate3s)) {
827 return HOTT_TONE_B; // sink rate 3s
828 }
829 if ((telestate->Settings.Warning.NegDifference1 == HOTTBRIDGESETTINGS_WARNING_ENABLED) &&
830 (telestate->Settings.Limit.NegDifference1 > telestate->climbrate1s)) {
831 return HOTT_TONE_C; // sink rate 1s
832 }
833 if ((telestate->Settings.Warning.MaxDistance == HOTTBRIDGESETTINGS_WARNING_ENABLED) &&
834 (telestate->Settings.Limit.MaxDistance < telestate->homedistance) && gps_ok) {
835 return HOTT_TONE_D; // maximum distance
836 }
837 if ((telestate->Settings.Warning.MinSensor1Temp == HOTTBRIDGESETTINGS_WARNING_ENABLED) &&
838 (telestate->Settings.Limit.MinSensor1Temp > telestate->Gyro.temperature)) {
839 return HOTT_TONE_F; // minimum temperature sensor 1
840 }
841 if ((telestate->Settings.Warning.MinSensor2Temp == HOTTBRIDGESETTINGS_WARNING_ENABLED) &&
842 (telestate->Settings.Limit.MinSensor2Temp > telestate->Baro.Temperature)) {
843 return HOTT_TONE_G; // minimum temperature sensor 2
844 }
845 if ((telestate->Settings.Warning.MaxSensor1Temp == HOTTBRIDGESETTINGS_WARNING_ENABLED) &&
846 (telestate->Settings.Limit.MaxSensor1Temp < telestate->Gyro.temperature)) {
847 return HOTT_TONE_H; // maximum temperature sensor 1
848 }
849 if ((telestate->Settings.Warning.MaxSensor2Temp == HOTTBRIDGESETTINGS_WARNING_ENABLED) &&
850 (telestate->Settings.Limit.MaxSensor2Temp < telestate->Baro.Temperature)) {
851 return HOTT_TONE_I; // maximum temperature sensor 2
852 }
853 if ((telestate->Settings.Warning.MaxSpeed == HOTTBRIDGESETTINGS_WARNING_ENABLED) &&
854 (telestate->Settings.Limit.MaxSpeed < telestate->GPS.Groundspeed * MS_TO_KMH) && gps_ok) {
855 return HOTT_TONE_L; // maximum speed
856 }
857 if ((telestate->Settings.Warning.PosDifference2 == HOTTBRIDGESETTINGS_WARNING_ENABLED) &&
858 (telestate->Settings.Limit.PosDifference2 > telestate->climbrate3s)) {
859 return HOTT_TONE_M; // climb rate 3s
860 }
861 if ((telestate->Settings.Warning.PosDifference1 == HOTTBRIDGESETTINGS_WARNING_ENABLED) &&
862 (telestate->Settings.Limit.PosDifference1 > telestate->climbrate1s)) {
863 return HOTT_TONE_N; // climb rate 1s
864 }
865 if ((telestate->Settings.Warning.MinHeight == HOTTBRIDGESETTINGS_WARNING_ENABLED) &&
866 (telestate->Settings.Limit.MinHeight > telestate->altitude)) {
867 return HOTT_TONE_O; // minimum height
868 }
869 if ((telestate->Settings.Warning.MinPowerVoltage == HOTTBRIDGESETTINGS_WARNING_ENABLED) &&
870 (telestate->Settings.Limit.MinPowerVoltage > telestate->Battery.Voltage)) {
871 return HOTT_TONE_P; // minimum input voltage
872 }
873 if ((telestate->Settings.Warning.MaxUsedCapacity == HOTTBRIDGESETTINGS_WARNING_ENABLED) &&
874 (telestate->Settings.Limit.MaxUsedCapacity < telestate->Battery.ConsumedEnergy)) {
875 return HOTT_TONE_V; // capacity
876 }
877 if ((telestate->Settings.Warning.MaxCurrent == HOTTBRIDGESETTINGS_WARNING_ENABLED) &&
878 (telestate->Settings.Limit.MaxCurrent < telestate->Battery.Current)) {
879 return HOTT_TONE_W; // maximum current
880 }
881 if ((telestate->Settings.Warning.MaxPowerVoltage == HOTTBRIDGESETTINGS_WARNING_ENABLED) &&
882 (telestate->Settings.Limit.MaxPowerVoltage < telestate->Battery.Voltage)) {
883 return HOTT_TONE_X; // maximum input voltage
884 }
885 if ((telestate->Settings.Warning.MaxHeight == HOTTBRIDGESETTINGS_WARNING_ENABLED) &&
886 (telestate->Settings.Limit.MaxHeight < telestate->altitude)) {
887 return HOTT_TONE_Z; // maximum height
888 }
889 // altitude beeps when crossing altitude limits at 20,40,60,80,100,200,400,600,800 and 1000 meters
890 if (telestate->Settings.Warning.AltitudeBeep == HOTTBRIDGESETTINGS_WARNING_ENABLED) {
891 // update altitude when checked for beeps
892 float last = telestate->altitude_last;
893 float actual = telestate->altitude;
894 telestate->altitude_last = telestate->altitude;
895 if (((last < 20) && (actual >= 20)) || ((last > 20) && (actual <= 20))) {
896 return HOTT_TONE_20M;
897 }
898 if (((last < 40) && (actual >= 40)) || ((last > 40) && (actual <= 40))) {
899 return HOTT_TONE_40M;
900 }
901 if (((last < 60) && (actual >= 60)) || ((last > 60) && (actual <= 60))) {
902 return HOTT_TONE_60M;
903 }
904 if (((last < 80) && (actual >= 80)) || ((last > 80) && (actual <= 80))) {
905 return HOTT_TONE_80M;
906 }
907 if (((last < 100) && (actual >= 100)) || ((last > 100) && (actual <= 100))) {
908 return HOTT_TONE_100M;
909 }
910 if (((last < 200) && (actual >= 200)) || ((last > 200) && (actual <= 200))) {
911 return HOTT_TONE_200M;
912 }
913 if (((last < 400) && (actual >= 400)) || ((last > 400) && (actual <= 400))) {
914 return HOTT_TONE_400M;
915 }
916 if (((last < 600) && (actual >= 600)) || ((last > 600) && (actual <= 600))) {
917 return HOTT_TONE_600M;
918 }
919 if (((last < 800) && (actual >= 800)) || ((last > 800) && (actual <= 800))) {
920 return HOTT_TONE_800M;
921 }
922 if (((last < 1000) && (actual >= 1000)) || ((last > 1000) && (actual <= 1000))) {
923 return HOTT_TONE_1000M;
924 }
925 }
926
927 // there is no warning
928 return 0;
929 }
930
931 /**
932 * calculate checksum of data buffer
933 */
934 uint8_t calc_checksum(uint8_t *data, uint16_t size)
935 {
936 uint16_t sum = 0;
937
938 for (int i = 0; i < size; i++) {
939 sum += data[i];
940 }
941 return sum;
942 }
943
944 /**
945 * scale float value with scale and offset to unsigned byte
946 */
947 uint8_t scale_float2uint8(float value, float scale, float offset)
948 {
949 uint16_t temp = (uint16_t)roundf(value * scale + offset);
950 uint8_t result;
951
952 result = (uint8_t)temp & 0xff;
953 return result;
954 }
955
956 /**
957 * scale float value with scale and offset to signed byte (int8_t)
958 */
959 int8_t scale_float2int8(float value, float scale, float offset)
960 {
961 int8_t result = (int8_t)roundf(value * scale + offset);
962
963 return result;
964 }
965
966 /**
967 * scale float value with scale and offset to word
968 */
969 uword_t scale_float2uword(float value, float scale, float offset)
970 {
971 uint16_t temp = (uint16_t)roundf(value * scale + offset);
972 uword_t result;
973
974 result.l = (uint8_t)temp & 0xff;
975 result.h = (uint8_t)(temp >> 8) & 0xff;
976 return result;
977 }
978
979 /**
980 * convert dword gps value into HoTT gps format and write result to given pointers
981 */
982 void convert_long2gps(int32_t value, uint8_t *dir, uword_t *min, uword_t *sec)
983 {
984 // convert gps decigrad value into degrees, minutes and seconds
985 uword_t temp;
986 uint32_t absvalue = abs(value);
987 uint16_t degrees = (absvalue / 10000000);
988 uint32_t seconds = (absvalue - degrees * 10000000) * 6;
989 uint16_t minutes = seconds / 1000000;
990
991 seconds %= 1000000;
992 seconds = seconds / 100;
993 uint16_t degmin = degrees * 100 + minutes;
994 // write results
995 *dir = (value < 0) ? 1 : 0;
996 temp.l = (uint8_t)degmin & 0xff;
997 temp.h = (uint8_t)(degmin >> 8) & 0xff;
998 *min = temp;
999 temp.l = (uint8_t)seconds & 0xff;
1000 temp.h = (uint8_t)(seconds >> 8) & 0xff;
1001 *sec = temp;
1002 }
1003
1004 #endif // PIOS_INCLUDE_HOTT_BRIDGE
1005 /**
1006 * @}
1007 * @}
1008 */