From f59f85d4a47dfb3c61a904c083dae1c9042a2d7f Mon Sep 17 00:00:00 2001 From: Matthias Badaire Date: Tue, 20 Jan 2015 20:30:25 +0100 Subject: [PATCH] AP_Frsky_Telem: add SBUS support add sbus support using a timer on a thread --- libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp | 614 ++++++++++++++++---- libraries/AP_Frsky_Telem/AP_Frsky_Telem.h | 108 +++- 2 files changed, 587 insertions(+), 135 deletions(-) diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp index 870777755f..70a64a5be9 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp +++ b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp @@ -1,3 +1,4 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- /* Inspired by work done here https://github.com/PX4/Firmware/tree/master/src/drivers/frsky_telemetry from Stefan Rado @@ -18,44 +19,99 @@ /* FRSKY Telemetry library - for the moment it only handle hub port telemetry - the sport reference are only here to simulate the frsky module and use opentx simulator. it will eventually be removed */ - #include - extern const AP_HAL::HAL& hal; -void AP_Frsky_Telem::init(AP_HAL::UARTDriver *port, uint8_t frsky_type) +void AP_Frsky_Telem::init(AP_HAL::UARTDriver *port, enum FrSkyProtocol protocol) { if (port == NULL) { return; } _port = port; - _port->begin(9600); - _initialised = true; -} - -void AP_Frsky_Telem::frsky_send_byte(uint8_t value) -{ - const uint8_t x5E[] = { 0x5D, 0x3E }; - const uint8_t x5D[] = { 0x5D, 0x3D }; - switch (value) { - case 0x5E: - _port->write( x5E, sizeof(x5E)); - break; - - case 0x5D: - _port->write( x5D, sizeof(x5D)); - break; - - default: - _port->write(&value, sizeof(value)); - break; + _protocol = protocol; + if (_protocol == FrSkySPORT) { + _port->begin(57600); + _gps_call = 0; + _fas_call = 0; + _vario_call = 0 ; + _various_call = 0 ; + _gps_data_ready = false; + _battery_data_ready = false; + _baro_data_ready = false; + _mode_data_ready = false; + _sats_data_ready = false; + _sport_status = 0; + hal.scheduler->register_io_process(AP_HAL_MEMBERPROC(&AP_Frsky_Telem::sport_tick)); + }else {// if this is D-port then spec says 9600 baud + _port->begin(9600); + _initialised = true; } } +/* + simple crc implementation for FRSKY telem S-PORT +*/ +void AP_Frsky_Telem::calc_crc (uint8_t byte){ + _crc += byte; //0-1FF + _crc += _crc >> 8; //0-100 + _crc &= 0x00ff; + _crc += _crc >> 8; //0-0FF + _crc &= 0x00ff; +} + +/* + * send the crc at the end of the S-PORT frame + */ +void AP_Frsky_Telem::send_crc() { + frsky_send_byte(0x00ff-_crc); + _crc = 0; +} + + +/* + send 1 byte and do the byte stuffing Frsky stuff + This can send more than 1 byte eventually +*/ +void AP_Frsky_Telem::frsky_send_byte(uint8_t value) +{ + if (_protocol == FrSkyDPORT) { + const uint8_t x5E[] = { 0x5D, 0x3E }; + const uint8_t x5D[] = { 0x5D, 0x3D }; + switch (value) { + case 0x5E: + _port->write( x5E, sizeof(x5E)); + break; + + case 0x5D: + _port->write( x5D, sizeof(x5D)); + break; + + default: + _port->write(&value, sizeof(value)); + break; + } + } else { //SPORT + calc_crc(value); + const uint8_t x7E[] = { 0x7D, 0x5E }; + const uint8_t x7D[] = { 0x7D, 0x5D }; + switch (value) { + case 0x7E: + _port->write( x7E, sizeof(x7E)); + break; + + case 0x7D: + _port->write( x7D, sizeof(x7D)); + break; + + default: + _port->write(&value, sizeof(value)); + break; + } + } +} + /** * Sends a 0x5E start/stop byte. */ @@ -65,61 +121,65 @@ void AP_Frsky_Telem::frsky_send_hub_startstop() _port->write(&c, sizeof(c)); } +/* + add sport protocol for frsky tx module +*/ +void AP_Frsky_Telem::frsky_send_sport_prim() +{ + static const uint8_t c = 0x10; + frsky_send_byte(c); +} + + /** * Sends one data id/value pair. */ void AP_Frsky_Telem::frsky_send_data(uint8_t id, int16_t data) { + static const uint8_t zero = 0x0; + /* Cast data to unsigned, because signed shift might behave incorrectly */ uint16_t udata = data; - frsky_send_hub_startstop(); - frsky_send_byte(id); + if (_protocol == FrSkySPORT) { + frsky_send_sport_prim(); + frsky_send_byte(id); + frsky_send_byte(zero); + } else { + frsky_send_hub_startstop(); + frsky_send_byte(id); + } frsky_send_byte(udata); /* LSB */ frsky_send_byte(udata >> 8); /* MSB */ + if (_protocol == FrSkySPORT) { + //Sport expect 32 bits data but we use only 16 bits data, so we send 0 for MSB + frsky_send_byte(zero); + frsky_send_byte(zero); + send_crc(); + } } -/** - * Sends frame 1 (every 200ms): - * barometer altitude, battery voltage & current +/* + * frsky_send_baro_meters : send altitude in Meters based on ahrs estimate */ -void AP_Frsky_Telem::frsky_send_frame1(uint8_t mode) -{ +void AP_Frsky_Telem::calc_baro_alt(){ struct Location loc; - float battery_amps = _battery.current_amps(); float baro_alt = 0; // in meters bool posok = _ahrs.get_position(loc); if (posok) { baro_alt = loc.alt * 0.01f; // convert to meters - if (!loc.flags.relative_alt) { - baro_alt -= _ahrs.get_home().alt * 0.01f; // subtract home if set - } + if (!loc.flags.relative_alt) { + baro_alt -= _ahrs.get_home().alt * 0.01f; // subtract home if set + } } - const AP_GPS &gps = _ahrs.get_gps(); - - // GPS status is sent as num_sats*10 + status, to fit into a - // uint8_t - uint8_t T2 = gps.num_sats() * 10 + gps.status(); - - frsky_send_data(FRSKY_ID_TEMP1, mode); - frsky_send_data(FRSKY_ID_TEMP2, T2); - /* - Note that this isn't actually barometric altitdue, it is the - AHRS estimate of altitdue above home. - */ - uint16_t baro_alt_meters = (uint16_t)baro_alt; - uint16_t baro_alt_cm = (baro_alt - baro_alt_meters) * 100; - frsky_send_data(FRSKY_ID_BARO_ALT_BP, baro_alt_meters); - frsky_send_data(FRSKY_ID_BARO_ALT_AP, baro_alt_cm); - - frsky_send_data(FRSKY_ID_FUEL, roundf(_battery.capacity_remaining_pct())); - - frsky_send_data(FRSKY_ID_VFAS, roundf(_battery.voltage() * 10.0f)); - frsky_send_data(FRSKY_ID_CURRENT, (battery_amps < 0) ? 0 : roundf(battery_amps * 10.0f)); - + Note that this isn't actually barometric altitude, it is the + inertial nav estimate of altitude above home. + */ + _baro_alt_meters = (int16_t)baro_alt; + _baro_alt_cm = (baro_alt - abs(_baro_alt_meters)) * 100; } /** @@ -131,68 +191,241 @@ float AP_Frsky_Telem::frsky_format_gps(float dec) return (dm_deg * 100.0f) + (dec - dm_deg) * 60; } -/** - * Sends frame 2 (every 1000ms): - * course(heading), latitude, longitude, ground speed, GPS altitude +/* + * prepare lattitude and longitude information stored in member variables */ -void AP_Frsky_Telem::frsky_send_frame2() -{ - // we send the heading based on the ahrs instead of GPS course which is not very usefull - uint16_t course_in_degrees = (_ahrs.yaw_sensor / 100) % 360; - frsky_send_data(FRSKY_ID_GPS_COURS_BP, course_in_degrees); +void AP_Frsky_Telem::calc_gps_position(){ + + _course_in_degrees = (_ahrs.yaw_sensor / 100) % 360; const AP_GPS &gps = _ahrs.get_gps(); - bool posok = (gps.status() >= 3); - if (posok){ - // send formatted frame - float lat = 0, lon = 0, alt = 0, speed= 0; - char lat_ns = 0, lon_ew = 0; + float lat; + float lon ; + float alt ; + float speed; + _pos_gps_ok = (gps.status() >= 3); + if (_pos_gps_ok){ Location loc = gps.location();//get gps instance 0 lat = frsky_format_gps(fabsf(loc.lat/10000000.0)); - uint16_t latdddmm = lat; - uint16_t latmmmm = (lat - latdddmm) * 10000; - lat_ns = (loc.lat < 0) ? 'S' : 'N'; + _latdddmm = lat; + _latmmmm = (lat - _latdddmm) * 10000; + _lat_ns = (loc.lat < 0) ? 'S' : 'N'; lon = frsky_format_gps(fabsf(loc.lng/10000000.0)); - uint16_t londddmm = lon; - uint16_t lonmmmm = (lon - londddmm) * 10000; - lon_ew = (loc.lng < 0) ? 'W' : 'E'; + _londddmm = lon; + _lonmmmm = (lon - _londddmm) * 10000; + _lon_ew = (loc.lng < 0) ? 'W' : 'E'; - alt = loc.alt / 100; - uint16_t alt_gps_meters = alt; - uint16_t alt_gps_cm = (alt - alt_gps_meters) * 100; + alt = loc.alt * 0.01f; + _alt_gps_meters = (int16_t)alt; + _alt_gps_cm = (alt - abs(_alt_gps_meters)) * 100; speed = gps.ground_speed (); - uint16_t speed_in_meter = speed; - uint16_t speed_in_centimeter = (speed - speed_in_meter) * 100; - - - frsky_send_data(FRSKY_ID_GPS_LAT_BP, latdddmm); - frsky_send_data(FRSKY_ID_GPS_LAT_AP, latmmmm); - frsky_send_data(FRSKY_ID_GPS_LAT_NS, lat_ns); - - frsky_send_data(FRSKY_ID_GPS_LONG_BP, londddmm); - frsky_send_data(FRSKY_ID_GPS_LONG_AP, lonmmmm); - frsky_send_data(FRSKY_ID_GPS_LONG_EW, lon_ew); - - frsky_send_data(FRSKY_ID_GPS_SPEED_BP, speed_in_meter); - frsky_send_data(FRSKY_ID_GPS_SPEED_AP, speed_in_centimeter); - - frsky_send_data(FRSKY_ID_GPS_ALT_BP, alt_gps_meters); - frsky_send_data(FRSKY_ID_GPS_ALT_AP, alt_gps_cm); - + _speed_in_meter = speed; + _speed_in_centimeter = (speed - _speed_in_meter) * 100; + } else { + _latdddmm = 0; + _latmmmm = 0; + _lat_ns = 0; + _londddmm = 0; + _lonmmmm = 0; + _alt_gps_meters = 0; + _alt_gps_cm = 0; + _speed_in_meter = 0; + _speed_in_centimeter = 0; } } +/* + * prepare battery information stored in member variables + */ +void AP_Frsky_Telem::calc_battery (){ + _batt_remaining = roundf(_battery.capacity_remaining_pct()); + _batt_volts = roundf(_battery.voltage() * 10.0f); + _batt_amps = roundf(_battery.current_amps() * 10.0f); +} /* - check for input bytes from sport + * prepare sats information stored in member variables */ -void AP_Frsky_Telem::check_sport_input(void) -{ - +void AP_Frsky_Telem::calc_gps_sats (){ + // GPS status is sent as num_sats*10 + status, to fit into a uint8_t + const AP_GPS &gps = _ahrs.get_gps(); + gps_sats = gps.num_sats() * 10 + gps.status(); +} + + + + +/* + * send number of gps satellite and gps status eg: 73 means 7 satellite and 3d lock + */ +void AP_Frsky_Telem::send_gps_sats (){ + frsky_send_data(FRSKY_ID_TEMP2, gps_sats); +} + +/* + * send control_mode as Temperature 1 (TEMP1) + */ +void AP_Frsky_Telem::send_mode (void){ + frsky_send_data(FRSKY_ID_TEMP1, _mode); +} + +/* + * send barometer altitude integer part . Initialize baro altitude + */ +void AP_Frsky_Telem::send_baro_alt_m (void){ + frsky_send_data(FRSKY_ID_BARO_ALT_BP, _baro_alt_meters); +} + +/* + * send barometer altitude decimal part + */ +void AP_Frsky_Telem::send_baro_alt_cm (void){ + frsky_send_data(FRSKY_ID_BARO_ALT_AP, _baro_alt_cm); +} + +/* + * send battery remaining + */ +void AP_Frsky_Telem::send_batt_remain (void){ + frsky_send_data(FRSKY_ID_FUEL, _batt_remaining); +} + +/* + * send battery voltage + */ +void AP_Frsky_Telem::send_batt_volts (void){ + frsky_send_data(FRSKY_ID_VFAS, _batt_volts); +} + +/* + * send current consumptiom + */ +void AP_Frsky_Telem::send_current (void){ + frsky_send_data(FRSKY_ID_CURRENT, _batt_amps); +} + +/* + * send heading in degree based on AHRS and not GPS + */ +void AP_Frsky_Telem::send_heading (void){ + frsky_send_data(FRSKY_ID_GPS_COURS_BP, _course_in_degrees); +} + +/* + * send gps lattitude degree and minute integer part; Initialize gps info + */ +void AP_Frsky_Telem::send_gps_lat_dd (void){ + frsky_send_data(FRSKY_ID_GPS_LAT_BP, _latdddmm); +} + +/* + * send gps lattitude minutes decimal part + */ +void AP_Frsky_Telem::send_gps_lat_mm (void){ + frsky_send_data(FRSKY_ID_GPS_LAT_AP, _latmmmm); +} + +/* + * send gps North / South information + */ +void AP_Frsky_Telem::send_gps_lat_ns (void){ + frsky_send_data(FRSKY_ID_GPS_LAT_NS, _lat_ns); +} + +/* + * send gps longitude degree and minute integer part + */ +void AP_Frsky_Telem::send_gps_lon_dd (void){ + frsky_send_data(FRSKY_ID_GPS_LONG_BP, _londddmm); +} + +/* + * send gps longitude minutes decimal part + */ +void AP_Frsky_Telem::send_gps_lon_mm (void){ + frsky_send_data(FRSKY_ID_GPS_LONG_AP, _lonmmmm); +} + +/* + * send gps East / West information + */ +void AP_Frsky_Telem::send_gps_lon_ew (void){ + frsky_send_data(FRSKY_ID_GPS_LONG_EW, _lon_ew); +} + +/* + * send gps speed integer part + */ +void AP_Frsky_Telem::send_gps_speed_meter (void){ + frsky_send_data(FRSKY_ID_GPS_SPEED_BP, _speed_in_meter); +} + +/* + * send gps speed decimal part + */ +void AP_Frsky_Telem::send_gps_speed_cm (void){ + frsky_send_data(FRSKY_ID_GPS_SPEED_AP, _speed_in_centimeter); +} + +/* + * send gps altitude integer part + */ +void AP_Frsky_Telem::send_gps_alt_meter (void){ + frsky_send_data(FRSKY_ID_GPS_ALT_BP, _alt_gps_meters); +} + +/* + * send gps altitude decimals + */ +void AP_Frsky_Telem::send_gps_alt_cm (void){ + frsky_send_data(FRSKY_ID_GPS_ALT_AP, _alt_gps_cm); +} + +/* + * send frame 1 every 200ms with baro alt, nb sats, batt volts and amp, control_mode + */ + +void AP_Frsky_Telem::send_hub_frame(){ + uint32_t now = hal.scheduler->millis(); + + // send frame1 every 200ms + if (now - _last_frame1_ms > 200) { + _last_frame1_ms = now; + calc_gps_sats(); + send_gps_sats (); + send_mode (); + + calc_battery(); + send_batt_remain (); + send_batt_volts (); + send_current (); + + calc_baro_alt(); + send_baro_alt_m (); + send_baro_alt_cm (); + } + // send frame2 every second + if (now - _last_frame2_ms > 1000) { + _last_frame2_ms = now; + send_heading(); + calc_gps_position(); + if (_pos_gps_ok){ + send_gps_lat_dd (); + send_gps_lat_mm (); + send_gps_lat_ns (); + send_gps_lon_dd (); + send_gps_lon_mm (); + send_gps_lon_ew (); + send_gps_speed_meter (); + send_gps_speed_cm (); + send_gps_alt_meter (); + send_gps_alt_cm (); + } + } } @@ -200,29 +433,168 @@ void AP_Frsky_Telem::check_sport_input(void) send telemetry frames. Should be called at 50Hz. The high rate is to allow this code to poll for serial bytes coming from the receiver for the SPort protocol - */ -void AP_Frsky_Telem::send_frames(uint8_t control_mode, enum FrSkyProtocol protocol) +*/ +void AP_Frsky_Telem::send_frames(uint8_t control_mode) { if (!_initialised) { return; } - if (protocol == FrSkySPORT) { - // check for sport bytes - check_sport_input(); - } - - uint32_t now = hal.scheduler->millis(); - - // send frame1 every 200ms - if (now - _last_frame1_ms > 200) { - _last_frame1_ms = now; - frsky_send_frame1(control_mode); - } - - // send frame2 every second - if (now - _last_frame2_ms > 1000) { - _last_frame2_ms = now; - frsky_send_frame2(); + if (_protocol == FrSkySPORT) { + if (!_mode_data_ready){ + _mode=control_mode; + _mode_data_ready = true; + } + if (!_baro_data_ready){ + calc_baro_alt(); + _baro_data_ready = true; + } + if (!_gps_data_ready){ + calc_gps_position(); + _gps_data_ready = true; + } + if (!_sats_data_ready){ + calc_gps_sats(); + _sats_data_ready = true; + } + if (!_battery_data_ready){ + calc_battery(); + _battery_data_ready = true; + } + } else { + _mode=control_mode; + send_hub_frame(); } } + + +void AP_Frsky_Telem::sport_tick(void){ + if (!_initialised) { + _port->begin(57600); + _initialised = true; + } + + int16_t numc; + numc = _port->available(); + + // check if available is negative + if ( numc < 0 ){ + return; + } + + // this is the constant for hub data frame + if (_port->txspace() < 19) { + return ; + } + + for (int16_t i = 0; i < numc; i++) { + int16_t readbyte = _port->read(); + if (_sport_status == 0){ + if (readbyte == SPORT_START_FRAME){ + _sport_status = 1; + } + } else { + switch (readbyte){ + case DATA_ID_FAS: + if (_battery_data_ready){ + switch (_fas_call){ + case 0: + send_batt_volts(); + break; + case 1: + send_current(); + break; + } + _fas_call++; + if (_fas_call > 1) { + _fas_call = 0; + } + _battery_data_ready = false; + } + break; + case DATA_ID_GPS: + if (_gps_data_ready){ + switch (_gps_call) { + case 0: + send_gps_lat_dd (); + break; + case 1: + send_gps_lat_mm (); + break; + case 2: + send_gps_lat_ns (); + break; + case 3: + send_gps_lon_dd (); + break; + case 4: + send_gps_lon_mm (); + break; + case 5: + send_gps_lon_ew (); + break; + case 6: + send_gps_speed_meter (); + break; + case 7: + send_gps_speed_cm (); + break; + case 8: + send_gps_alt_meter (); + break; + case 9: + send_gps_alt_cm (); + break; + case 10: + send_heading(); + break; + } + + _gps_call++; + if (_gps_call > 10) { + _gps_call = 0; + _gps_data_ready = false; + } + } + break; + case DATA_ID_VARIO: + if (_baro_data_ready){ + switch (_vario_call){ + case 0 : + send_baro_alt_m (); + break; + case 1: + send_baro_alt_cm (); + break; + } + _vario_call ++; + if (_vario_call > 1) { + _vario_call = 0; + _baro_data_ready = false; + } + } + break; + case DATA_ID_SP2UR: + switch (_various_call){ + case 0 : + if ( _sats_data_ready ){ + send_gps_sats (); + _sats_data_ready = false; + } + break; + case 1: + if ( _mode_data_ready ){ + send_mode (); + _mode_data_ready = false; + } + break; + } + _various_call++; + if (_various_call > 1) _various_call = 0; + break; + } + _sport_status = 0; + + } + } +} diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h index 00a98fc758..7912760725 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h +++ b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h @@ -1,3 +1,4 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- /* This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by @@ -57,18 +58,29 @@ #define FRSKY_ID_VOLTS_BP 0x3A #define FRSKY_ID_VOLTS_AP 0x3B +// Default sensor data IDs (Physical IDs + CRC) +#define DATA_ID_VARIO 0x00 // 0 +#define DATA_ID_FLVSS 0xA1 // 1 +#define DATA_ID_FAS 0x22 // 2 +#define DATA_ID_GPS 0x83 // 3 +#define DATA_ID_RPM 0xE4 // 4 +#define DATA_ID_SP2UH 0x45 // 5 +#define DATA_ID_SP2UR 0xC6 // 6 + +#define SPORT_START_FRAME 0x7E + #define HUB_PORT true #define S_PORT false class AP_Frsky_Telem { - public: +public: //constructor AP_Frsky_Telem(AP_AHRS &ahrs, AP_BattMonitor &battery) : - _initialised(false), - _ahrs(ahrs), - _battery(battery) - {} + _initialised(false), + _ahrs(ahrs), + _battery(battery) + {} // these enums must match up with TELEM2_PROTOCOL in vehicle code enum FrSkyProtocol { @@ -76,25 +88,93 @@ class AP_Frsky_Telem FrSkySPORT = 3 }; - void init(AP_HAL::UARTDriver *port, uint8_t frsky_type); - void send_frames(uint8_t control_mode, enum FrSkyProtocol protocol); + void init(AP_HAL::UARTDriver *port, enum FrSkyProtocol protocol); + void send_frames(uint8_t control_mode); - private: - void frsky_send_data(uint8_t id, int16_t data); - void frsky_send_frame1(uint8_t mode); - void frsky_send_frame2(); - void check_sport_input(void); - - float frsky_format_gps(float dec); +private: + + void calc_crc (uint8_t byte); + void send_crc(); + void frsky_send_byte(uint8_t value); void frsky_send_hub_startstop(); + void frsky_send_sport_prim(); + + void frsky_send_data(uint8_t id, int16_t data); + + void calc_baro_alt(); + float frsky_format_gps(float dec); + void calc_gps_position(); + void calc_battery(); + void calc_gps_sats(); + + void send_gps_sats (void); + void send_mode (void); + void send_baro_alt_m (void); + void send_baro_alt_cm (void); + void send_batt_remain (void); + void send_batt_volts (void); + void send_current (void); + void send_prearm_error (void); + void send_heading (void); + void send_gps_lat_dd (void); + void send_gps_lat_mm (void); + void send_gps_lat_ns (void); + void send_gps_lon_dd (void); + void send_gps_lon_mm (void); + void send_gps_lon_ew (void); + void send_gps_speed_meter (void); + void send_gps_speed_cm (void); + void send_gps_alt_meter (void); + void send_gps_alt_cm (void); + + void send_hub_frame(); + void sport_tick (); AP_HAL::UARTDriver *_port; bool _initialised; AP_AHRS &_ahrs; AP_BattMonitor &_battery; + enum FrSkyProtocol _protocol; + + uint16_t _crc; uint32_t _last_frame1_ms; uint32_t _last_frame2_ms; + + bool _battery_data_ready; + uint16_t _batt_remaining; + uint16_t _batt_volts; + uint16_t _batt_amps; + + bool _sats_data_ready; + uint16_t gps_sats; + + bool _gps_data_ready; + bool _pos_gps_ok; + uint16_t _course_in_degrees; + char _lat_ns, _lon_ew; + uint16_t _latdddmm; + uint16_t _latmmmm; + uint16_t _londddmm; + uint16_t _lonmmmm; + uint16_t _alt_gps_meters; + uint16_t _alt_gps_cm ; + int16_t _speed_in_meter; + uint16_t _speed_in_centimeter; + + bool _baro_data_ready; + int16_t _baro_alt_meters ; + uint16_t _baro_alt_cm ; + + bool _mode_data_ready; + uint8_t _mode; + + uint8_t _fas_call ; + uint8_t _gps_call; + uint8_t _vario_call; + uint8_t _various_call; + + uint8_t _sport_status; }; #endif