From 3ca868a59fc87558400a13f59e6d29ca855437d1 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Sun, 6 Apr 2014 23:34:37 +0100 Subject: [PATCH] Enable HoTT as a telemetry provider. Import cGiensen's HoTT telemetry implementation - untested. --- Makefile | 1 + src/board.h | 3 +- src/cli.c | 5 +- src/cli.h | 13 +++ src/mixer.c | 2 - src/mw.c | 5 + src/serial.c | 6 +- src/telemetry_common.c | 14 ++- src/telemetry_common.h | 2 +- src/telemetry_frsky.c | 2 +- src/telemetry_frsky.h | 2 +- src/telemetry_hott.c | 254 +++++++++++++++++++++++++++++++++++++++++ src/telemetry_hott.h | 204 +++++++++++++++++++++++++++++++++ 13 files changed, 499 insertions(+), 14 deletions(-) create mode 100644 src/cli.h create mode 100644 src/telemetry_hott.c create mode 100644 src/telemetry_hott.h diff --git a/Makefile b/Makefile index 9e50b1ca8..2cee3893d 100644 --- a/Makefile +++ b/Makefile @@ -57,6 +57,7 @@ COMMON_SRC = startup_stm32f10x_md_gcc.S \ spektrum.c \ telemetry_common.c \ telemetry_frsky.c \ + telemetry_hott.c \ drv_gpio.c \ drv_i2c.c \ drv_i2c_soft.c \ diff --git a/src/board.h b/src/board.h index 36ed2c90e..77a84e716 100755 --- a/src/board.h +++ b/src/board.h @@ -94,7 +94,8 @@ typedef enum { typedef enum { - TELEMETRY_PROVIDER_FRSKY = 0 + TELEMETRY_PROVIDER_FRSKY = 0, + TELEMETRY_PROVIDER_HOTT } TelemetryProvider; typedef enum { diff --git a/src/cli.c b/src/cli.c index 4d34bde50..4dd8f6311 100644 --- a/src/cli.c +++ b/src/cli.c @@ -30,6 +30,9 @@ extern const char rcChannelLetters[]; // from mixer.c extern int16_t motor_disarmed[MAX_MOTORS]; +// signal that we're in cli mode +uint8_t cliMode = 0; + // buffer static char cliBuffer[48]; static uint32_t bufferIndex = 0; @@ -133,7 +136,7 @@ const clivalue_t valueTable[] = { { "gps_type", VAR_UINT8, &mcfg.gps_type, 0, 3 }, { "gps_baudrate", VAR_INT8, &mcfg.gps_baudrate, 0, 4 }, { "serialrx_type", VAR_UINT8, &mcfg.serialrx_type, 0, 3 }, - { "telemetry_provider", VAR_UINT8, &mcfg.telemetry_provider, 0, 0 }, + { "telemetry_provider", VAR_UINT8, &mcfg.telemetry_provider, 0, 1 }, { "telemetry_softserial", VAR_UINT8, &mcfg.telemetry_softserial, 0, 2 }, { "telemetry_switch", VAR_UINT8, &mcfg.telemetry_switch, 0, 1 }, { "vbatscale", VAR_UINT8, &mcfg.vbatscale, 10, 200 }, diff --git a/src/cli.h b/src/cli.h new file mode 100644 index 000000000..f21bceb43 --- /dev/null +++ b/src/cli.h @@ -0,0 +1,13 @@ +/* + * cli.h + * + * Created on: 6 Apr 2014 + * Author: Hydra + */ + +#ifndef CLI_H_ +#define CLI_H_ + +extern uint8_t cliMode; + +#endif /* CLI_H_ */ diff --git a/src/mixer.c b/src/mixer.c index 364a1e2ae..d982c8e4d 100755 --- a/src/mixer.c +++ b/src/mixer.c @@ -304,8 +304,6 @@ void writeServos(void) } } -extern uint8_t cliMode; - void writeMotors(void) { uint8_t i; diff --git a/src/mw.c b/src/mw.c index a4bcda037..7214c36ab 100755 --- a/src/mw.c +++ b/src/mw.c @@ -1,6 +1,7 @@ #include "board.h" #include "mw.h" +#include "cli.h" #include "telemetry_common.h" // June 2013 V2.2-dev @@ -206,6 +207,10 @@ void annexCode(void) serialCom(); + if (!cliMode && feature(FEATURE_TELEMETRY)) { + handleTelemetry(); + } + if (sensors(SENSOR_GPS)) { static uint32_t GPSLEDTime; if ((int32_t)(currentTime - GPSLEDTime) >= 0 && (GPS_numSat >= 5)) { diff --git a/src/serial.c b/src/serial.c index ae359c065..719ffe93a 100755 --- a/src/serial.c +++ b/src/serial.c @@ -1,6 +1,7 @@ #include "board.h" #include "mw.h" +#include "cli.h" #include "telemetry_common.h" // Multiwii Serial Protocol 0 @@ -114,8 +115,6 @@ static const char pidnames[] = static uint8_t checksum, indRX, inBuf[INBUF_SIZE]; static uint8_t cmdMSP; -// signal that we're in cli mode -uint8_t cliMode = 0; void serialize32(uint32_t a) { @@ -696,7 +695,4 @@ void serialCom(void) c_state = IDLE; } } - if (!cliMode && feature(FEATURE_TELEMETRY)) { // The first condition should never evaluate to true but I'm putting it here anyway - silpstream - sendTelemetry(); - } } diff --git a/src/telemetry_common.c b/src/telemetry_common.c index 79398530b..828fff630 100644 --- a/src/telemetry_common.c +++ b/src/telemetry_common.c @@ -2,6 +2,7 @@ #include "mw.h" #include "telemetry_frsky.h" +#include "telemetry_hott.h" void initTelemetry(void) { @@ -40,12 +41,21 @@ bool isFrSkyTelemetryEnabled(void) return mcfg.telemetry_provider == TELEMETRY_PROVIDER_FRSKY; } -void sendTelemetry(void) +bool isHoTTTelemetryEnabled(void) +{ + return mcfg.telemetry_provider == TELEMETRY_PROVIDER_HOTT; +} + +void handleTelemetry(void) { if (!isTelemetryEnabled()) return; if (isFrSkyTelemetryEnabled()) { - sendFrSkyTelemetry(); + handleFrSkyTelemetry(); + } + + if (isHoTTTelemetryEnabled()) { + handleHoTTTelemetry(); } } diff --git a/src/telemetry_common.h b/src/telemetry_common.h index 9a4129968..defd43320 100644 --- a/src/telemetry_common.h +++ b/src/telemetry_common.h @@ -11,7 +11,7 @@ // telemetry void initTelemetry(void); void updateTelemetryState(void); -void sendTelemetry(void); +void handleTelemetry(void); bool isTelemetryEnabled(void); #endif /* TELEMETRY_COMMON_H_ */ diff --git a/src/telemetry_frsky.c b/src/telemetry_frsky.c index ade5fdb13..e0724dcdd 100644 --- a/src/telemetry_frsky.c +++ b/src/telemetry_frsky.c @@ -252,7 +252,7 @@ bool hasEnoughTimeLapsedSinceLastTelemetryTransmission(uint32_t currentMillis) return currentMillis - lastCycleTime >= CYCLETIME; } -void sendFrSkyTelemetry(void) +void handleFrSkyTelemetry(void) { if (!canSendFrSkyTelemetry()) { return; diff --git a/src/telemetry_frsky.h b/src/telemetry_frsky.h index 343b09be9..550b92932 100644 --- a/src/telemetry_frsky.h +++ b/src/telemetry_frsky.h @@ -8,7 +8,7 @@ #ifndef TELEMETRY_FRSKY_H_ #define TELEMETRY_FRSKY_H_ -void sendFrSkyTelemetry(void); +void handleFrSkyTelemetry(void); void updateFrSkyTelemetryState(void); #endif /* TELEMETRY_FRSKY_H_ */ diff --git a/src/telemetry_hott.c b/src/telemetry_hott.c new file mode 100644 index 000000000..78f4c0e6a --- /dev/null +++ b/src/telemetry_hott.c @@ -0,0 +1,254 @@ +/* + * telemetry_hott.c + * + * Created on: 6 Apr 2014 + * Author: Hydra/cGiesen + */ + +#include "board.h" +#include "mw.h" + +#include "telemetry_hott.h" + + +const uint8_t kHoTTv4BinaryPacketSize = 45; +const uint8_t kHoTTv4TextPacketSize = 173; + +static uint8_t outBuffer[173]; +static void hottV4SerialWrite(uint8_t c); +static inline void hottV4EnableReceiverMode(void); +static inline void hottV4EnableTransmitterMode(void); + +static void hottV4SendData(uint8_t *data, uint8_t size); +static void hottV4SendGPS(void); +static void hottV4GPSUpdate(void); +static void hottV4SendEAM(void); +static void hottV4EAMUpdateBattery(void); +static void hottV4EAMUpdateTemperatures(void); +bool batteryWarning; + +/* + * Sends HoTTv4 capable GPS telemetry frame. + */ + +void hottV4SendGPS(void) +{ + /** Minimum data set for EAM */ + HoTTV4GPSModule.startByte = 0x7C; + HoTTV4GPSModule.sensorID = HOTTV4_GPS_SENSOR_ID; + HoTTV4GPSModule.sensorTextID = HOTTV4_GPS_SENSOR_TEXT_ID; + HoTTV4GPSModule.endByte = 0x7D; + /** ### */ + + /** Reset alarms */ + HoTTV4GPSModule.alarmTone = 0x0; + HoTTV4GPSModule.alarmInverse1 = 0x0; + + hottV4GPSUpdate(); + + // Clear output buffer + memset(&outBuffer, 0, sizeof(outBuffer)); + + // Copy EAM data to output buffer + memcpy(&outBuffer, &HoTTV4GPSModule, kHoTTv4BinaryPacketSize); + + // Send data from output buffer + hottV4SendData(outBuffer, kHoTTv4BinaryPacketSize); + } + +void hottV4GPSUpdate(void) +{ + //number of Satelites + HoTTV4GPSModule.GPSNumSat=GPS_numSat; + if (f.GPS_FIX > 0) { + /** GPS fix */ + HoTTV4GPSModule.GPS_fix = 0x66; // Displays a 'f' for fix + //latitude + HoTTV4GPSModule.LatitudeNS=(GPS_coord[LAT]<0); + uint8_t deg = GPS_coord[LAT] / 100000; + uint32_t sec = (GPS_coord[LAT] - (deg * 100000)) * 6; + uint8_t min = sec / 10000; + sec = sec % 10000; + uint16_t degMin = (deg * 100) + min; + HoTTV4GPSModule.LatitudeMinLow = degMin; + HoTTV4GPSModule.LatitudeMinHigh = degMin >> 8; + HoTTV4GPSModule.LatitudeSecLow = sec; + HoTTV4GPSModule.LatitudeSecHigh = sec >> 8; + //latitude + HoTTV4GPSModule.longitudeEW=(GPS_coord[LON]<0); + deg = GPS_coord[LON] / 100000; + sec = (GPS_coord[LON] - (deg * 100000)) * 6; + min = sec / 10000; + sec = sec % 10000; + degMin = (deg * 100) + min; + HoTTV4GPSModule.longitudeMinLow = degMin; + HoTTV4GPSModule.longitudeMinHigh = degMin >> 8; + HoTTV4GPSModule.longitudeSecLow = sec; + HoTTV4GPSModule.longitudeSecHigh = sec >> 8; + /** GPS Speed in km/h */ + uint16_t speed = (GPS_speed / 100) * 36; // 0.1m/s * 0.36 = km/h + HoTTV4GPSModule.GPSSpeedLow = speed & 0x00FF; + HoTTV4GPSModule.GPSSpeedHigh = speed >> 8; + /** Distance to home */ + HoTTV4GPSModule.distanceLow = GPS_distanceToHome & 0x00FF; + HoTTV4GPSModule.distanceHigh = GPS_distanceToHome >> 8; + /** Altitude */ + HoTTV4GPSModule.altitudeLow = GPS_altitude & 0x00FF; + HoTTV4GPSModule.altitudeHigh = GPS_altitude >> 8; + /** Altitude */ + HoTTV4GPSModule.HomeDirection = GPS_directionToHome; + } + else + { + HoTTV4GPSModule.GPS_fix = 0x20; // Displays a ' ' to show nothing or clear the old value + } + } + + /** + * Writes cell 1-4 high, low values and if not available + * calculates vbat. + */ + static void hottV4EAMUpdateBattery() { +// HoTTV4ElectricAirModule.cell1L = 0; +// HoTTV4ElectricAirModule.cell1H = 0; + +// HoTTV4ElectricAirModule.cell2L = 0; +// HoTTV4ElectricAirModule.cell2H = 0; + +// HoTTV4ElectricAirModule.cell3L = 0; +// HoTTV4ElectricAirModule.cell3H = 0; + +// HoTTV4ElectricAirModule.cell4L = 0; +// HoTTV4ElectricAirModule.cell4H = 0; + + HoTTV4ElectricAirModule.driveVoltageLow = vbat & 0xFF; + HoTTV4ElectricAirModule.driveVoltageHigh = vbat >> 8; + HoTTV4ElectricAirModule.battery1Low = vbat & 0xFF; + HoTTV4ElectricAirModule.battery1High = vbat >> 8; + +// HoTTV4ElectricAirModule.battery2Low = 0 & 0xFF; +// HoTTV4ElectricAirModule.battery2High = 0 >> 8; + + if (batteryWarning) { + HoTTV4ElectricAirModule.alarmTone = HoTTv4NotificationUndervoltage; + HoTTV4ElectricAirModule.alarmInverse1 |= 0x80; // Invert Voltage display + } + } + + static void hottV4EAMUpdateTemperatures() { + HoTTV4ElectricAirModule.temp1 = 20 + 0; + HoTTV4ElectricAirModule.temp2 = 20; + + //if (HoTTV4ElectricAirModule.temp1 >= (20 + MultiHoTTModuleSettings.alarmTemp1)) { + // HoTTV4ElectricAirModule.alarmTone = HoTTv4NotificationMaxTemperature; + // HoTTV4ElectricAirModule.alarmInverse |= 0x8; // Invert Temp1 display + //} + } + + +/** + * Sends HoTTv4 capable EAM telemetry frame. + */ +void hottV4SendEAM(void) { + /** Minimum data set for EAM */ + HoTTV4ElectricAirModule.startByte = 0x7C; + HoTTV4ElectricAirModule.sensorID = HOTTV4_ELECTRICAL_AIR_SENSOR_ID; + HoTTV4ElectricAirModule.sensorTextID = HOTTV4_ELECTRICAL_AIR_SENSOR_TEXT_ID; + HoTTV4ElectricAirModule.endByte = 0x7D; + /** ### */ + + /** Reset alarms */ + HoTTV4ElectricAirModule.alarmTone = 0x0; + HoTTV4ElectricAirModule.alarmInverse1 = 0x0; + + hottV4EAMUpdateBattery(); + hottV4EAMUpdateTemperatures(); + + HoTTV4ElectricAirModule.current = 0 / 10; + HoTTV4ElectricAirModule.height = OFFSET_HEIGHT + 0; + HoTTV4ElectricAirModule.m2s = OFFSET_M2S; + HoTTV4ElectricAirModule.m3s = OFFSET_M3S; + + // Clear output buffer + memset(&outBuffer, 0, sizeof(outBuffer)); + + // Copy EAM data to output buffer + memcpy(&outBuffer, &HoTTV4ElectricAirModule, kHoTTv4BinaryPacketSize); + + // Send data from output buffer + hottV4SendData(outBuffer, kHoTTv4BinaryPacketSize); +} + +/** + * Expects an array of at least size bytes. All bytes till size will be transmitted + * to the HoTT capable receiver. Last byte will always be treated as checksum and is + * calculated on the fly. + */ +static void hottV4SendData(uint8_t *data, uint8_t size) { + //hottV4Serial.flush(); + + // Protocoll specific waiting time + // to avoid collisions + delay(5); + + if (serialTotalBytesWaiting(core.telemport) == 0) { + hottV4EnableTransmitterMode(); + + uint16_t crc = 0; + uint8_t i; + + for (i = 0; i < (size - 1); i++) { + crc += data[i]; + hottV4SerialWrite(data[i]); + + // Protocoll specific delay between each transmitted byte + delayMicroseconds(HOTTV4_TX_DELAY); + } + + // Write package checksum + hottV4SerialWrite(crc & 0xFF); + + hottV4EnableReceiverMode(); + } +} + +/** + * Writes out given byte to HoTT serial interface. + * If in debug mode, data is also written to UART serial interface. + */ +static void hottV4SerialWrite(uint8_t c) { + //hottV4Serial.write(c); + serialWrite(core.telemport, c); +} + +/** + * Enables RX and disables TX + */ +static inline void hottV4EnableReceiverMode() { + //DDRD &= ~(1 << HOTTV4_RXTX); + //PORTD |= (1 << HOTTV4_RXTX); +} + +/** + * Enabels TX and disables RX + */ +static inline void hottV4EnableTransmitterMode() { + //DDRD |= (1 << HOTTV4_RXTX); +} + +void handleHoTTTelemetry(void) +{ + while (serialTotalBytesWaiting(core.telemport)) { + uint8_t c = serialRead(core.telemport); + switch (c) { + case 0x8A: + if (sensors(SENSOR_GPS)) hottV4SendGPS(); + break; + case 0x8E: + hottV4SendEAM(); + break; + } + } +} + + diff --git a/src/telemetry_hott.h b/src/telemetry_hott.h new file mode 100644 index 000000000..21c6956bc --- /dev/null +++ b/src/telemetry_hott.h @@ -0,0 +1,204 @@ +/* + * telemetry_hott.h + * + * Created on: 6 Apr 2014 + * Author: Hydra + */ + +#ifndef TELEMETRY_HOTT_H_ +#define TELEMETRY_HOTT_H_ + +/* ###### HoTT module specifications ###### */ + +#define HOTTV4_GENERAL_AIR_SENSOR_ID 0xD0 + +#define HOTTV4_ELECTRICAL_AIR_SENSOR_ID 0x8E // Electric Air Sensor ID +#define HOTTV4_ELECTRICAL_AIR_SENSOR_TEXT_ID 0xE0 // Electric Air Module ID + +#define HOTTV4_GPS_SENSOR_ID 0x8A // GPS Sensor ID +#define HOTTV4_GPS_SENSOR_TEXT_ID 0xA0 // GPS Module ID + +#define HOTTV4_RXTX 4 +#define HOTTV4_TX_DELAY 1000 + +#define HOTTV4_BUTTON_DEC 0xEB +#define HOTTV4_BUTTON_INC 0xED +#define HOTTV4_BUTTON_SET 0xE9 +#define HOTTV4_BUTTON_NIL 0x0F +#define HOTTV4_BUTTON_NEXT 0xEE +#define HOTTV4_BUTTON_PREV 0xE7 + +#define OFFSET_HEIGHT 500 +#define OFFSET_M2S 72 +#define OFFSET_M3S 120 + +typedef enum { + HoTTv4NotificationErrorCalibration = 0x01, + HoTTv4NotificationErrorReceiver = 0x02, + HoTTv4NotificationErrorDataBus = 0x03, + HoTTv4NotificationErrorNavigation = 0x04, + HoTTv4NotificationErrorError = 0x05, + HoTTv4NotificationErrorCompass = 0x06, + HoTTv4NotificationErrorSensor = 0x07, + HoTTv4NotificationErrorGPS = 0x08, + HoTTv4NotificationErrorMotor = 0x09, + + HoTTv4NotificationMaxTemperature = 0x0A, + HoTTv4NotificationAltitudeReached = 0x0B, + HoTTv4NotificationWaypointReached = 0x0C, + HoTTv4NotificationNextWaypoint = 0x0D, + HoTTv4NotificationLanding = 0x0E, + HoTTv4NotificationGPSFix = 0x0F, + HoTTv4NotificationUndervoltage = 0x10, + HoTTv4NotificationGPSHold = 0x11, + HoTTv4NotificationGPSHome = 0x12, + HoTTv4NotificationGPSOff = 0x13, + HoTTv4NotificationBeep = 0x14, + HoTTv4NotificationMicrocopter = 0x15, + HoTTv4NotificationCapacity = 0x16, + HoTTv4NotificationCareFreeOff = 0x17, + HoTTv4NotificationCalibrating = 0x18, + HoTTv4NotificationMaxRange = 0x19, + HoTTv4NotificationMaxAltitude = 0x1A, + + HoTTv4Notification20Meter = 0x25, + HoTTv4NotificationMicrocopterOff = 0x26, + HoTTv4NotificationAltitudeOn = 0x27, + HoTTv4NotificationAltitudeOff = 0x28, + HoTTv4Notification100Meter = 0x29, + HoTTv4NotificationCareFreeOn = 0x2E, + HoTTv4NotificationDown = 0x2F, + HoTTv4NotificationUp = 0x30, + HoTTv4NotificationHold = 0x31, + HoTTv4NotificationGPSOn = 0x32, + HoTTv4NotificationFollowing = 0x33, + HoTTv4NotificationStarting = 0x34, + HoTTv4NotificationReceiver = 0x35, +} HoTTv4Notification; + +/*----------------------------------------------------------- +GPS +Receiver -> GPS Sensor (Flightcontrol) +Byte 1: 0x80 = Receiver byte +Byte 2: 0x8A = GPS Sensor byte (witch data Transmitter wants to get) +5ms Idle Line! +5ms delay +-----------------------------------------------------------*/ + +struct { + uint8_t startByte; // Byte 1: 0x7C = Start byte data + uint8_t sensorID; // Byte 2: 0x8A = GPS Sensor + uint8_t alarmTone; // Byte 3: 0…= warning beeps + uint8_t sensorTextID; // Byte 4: 160 0xA0 Sensor ID Neu! + uint8_t alarmInverse1; // Byte 5: 01 inverse status + uint8_t alarmInverse2; // Byte 6: 00 inverse status status 1 = no GPS signal + uint8_t flightDirection; // Byte 7: 119 = fly direction. 1 = 2°; 0° (North), 9 0° (East), 180° (South), 270° (West) + uint8_t GPSSpeedLow; // Byte 8: 8 = GPS speed low byte 8km/h + uint8_t GPSSpeedHigh; // Byte 9: 0 = GPS speed high byte + + // Example: N 48°39'988" + uint8_t LatitudeNS; // Byte 10: 000 = N; 001 = S + // 0x12E7 = 4839 (48°30') + uint8_t LatitudeMinLow; // Byte 11: 231 = 0xE7 + uint8_t LatitudeMinHigh; // Byte 12: 018 = 0x12 + // 0x03DC = 0988 (988") + uint8_t LatitudeSecLow; // Byte 13: 220 = 0xDC + uint8_t LatitudeSecHigh; // Byte 14: 003 = 0x03 + + // Example: E 9°25'9360" + uint8_t longitudeEW; // Byte 15: 000 = E; 001 = W; + // 0x039D = 0925 (09°25') + uint8_t longitudeMinLow; // Byte 16: 157 = 0x9D + uint8_t longitudeMinHigh; // Byte 17: 003 = 0x03 + // 0x2490 = 9360 (9360") + uint8_t longitudeSecLow; // Byte 18: 144 = 0x90 + uint8_t longitudeSecHigh; // Byte 19: 036 = 0x24 + + uint8_t distanceLow; // Byte 20: distance low byte (meter) + uint8_t distanceHigh; // Byte 21: distance high byte + uint8_t altitudeLow; // Byte 22: Altitude low byte (meter) + uint8_t altitudeHigh; // Byte 23: Altitude high byte + uint8_t resolutionLow; // Byte 24: Low Byte m/s resolution 0.01m 48 = 30000 = 0.00m/s (1=0.01m/s) + uint8_t resolutionHigh; // Byte 25: High Byte m/s resolution 0.01m + uint8_t unknow1; // Byte 26: 120 = 0m/3s + uint8_t GPSNumSat; // Byte 27: number of satelites (1 byte) + uint8_t GPSFixChar; // Byte 28: GPS.FixChar. (GPS fix character. display, if DGPS, 2D oder 3D) (1 byte) + uint8_t HomeDirection; // Byte 29: HomeDirection (direction from starting point to Model position) (1 byte) + uint8_t angleXdirection; // Byte 30: angle x-direction (1 byte) + uint8_t angleYdirection; // Byte 31: angle y-direction (1 byte) + uint8_t angleZdirection; // Byte 32: angle z-direction (1 byte) + uint8_t gyroXLow; // Byte 33: gyro x low byte (2 bytes) + uint8_t gyroXHigh; // Byte 34: gyro x high byte + uint8_t gyroYLow; // Byte 35: gyro y low byte (2 bytes) + uint8_t gyroYHigh; // Byte 36: gyro y high byte + uint8_t gyroZLow; // Byte 37: gyro z low byte (2 bytes) + uint8_t gyroZHigh; // Byte 38: gyro z high byte + uint8_t vibration; // Byte 39: vibration (1 bytes) + uint8_t Ascii4; // Byte 40: 00 ASCII Free Character [4] + uint8_t Ascii5; // Byte 41: 00 ASCII Free Character [5] + uint8_t GPS_fix; // Byte 42: 00 ASCII Free Character [6], we use it for GPS FIX + uint8_t version; // Byte 43: 00 version number + uint8_t endByte; // Byte 44: 0x7D Ende byte + uint8_t chksum; // Byte 45: Parity Byte +} HoTTV4GPSModule; + +/*----------------------------------------------------------- +EAM (Electric Air Module) 33620 +EmpfängerElectric Sensor +Byte 1: 80 = Receiver byte +Byte 2: 8E = Electric Sensor byte +5ms Idle Line! + *-----------------------------------------------------------*/ + +struct { + uint8_t startByte; + uint8_t sensorID; + uint8_t alarmTone; // Alarm */ + uint8_t sensorTextID; + uint8_t alarmInverse1; + uint8_t alarmInverse2; + + uint8_t cell1L; // Low Voltage Cell 1-7 in 2mV steps */ + uint8_t cell2L; + uint8_t cell3L; + uint8_t cell4L; + uint8_t cell5L; + uint8_t cell6L; + uint8_t cell7L; + uint8_t cell1H; // High Voltage Cell 1-7 in 2mV steps */ + uint8_t cell2H; + uint8_t cell3H; + uint8_t cell4H; + uint8_t cell5H; + uint8_t cell6H; + uint8_t cell7H; + + uint8_t battery1Low; // Battetry 1 LSB/MSB in 100mv steps; 50 == 5V */ + uint8_t battery1High; // Battetry 1 LSB/MSB in 100mv steps; 50 == 5V */ + uint8_t battery2Low; // Battetry 2 LSB/MSB in 100mv steps; 50 == 5V */ + uint8_t battery2High; // Battetry 2 LSB/MSB in 100mv steps; 50 == 5V */ + + uint8_t temp1; // Temp 1; Offset of 20. 20 == 0C */ + uint8_t temp2; // Temp 2; Offset of 20. 20 == 0C */ + + uint16_t height; // Height. Offset -500. 500 == 0 */ + uint16_t current; // 1 = 0.1A */ + uint8_t driveVoltageLow; + uint8_t driveVoltageHigh; + uint16_t capacity; // mAh */ + uint16_t m2s; // m2s; 0x48 == 0 */ + uint8_t m3s; // m3s; 0x78 == 0 */ + + uint16_t rpm; // RPM. 10er steps; 300 == 3000rpm */ + uint8_t minutes; + uint8_t seconds; + uint8_t speed; + + uint8_t version; + uint8_t endByte; + uint8_t chksum; +} HoTTV4ElectricAirModule; + +void handleHoTTTelemetry(void); + +#endif /* TELEMETRY_HOTT_H_ */