diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_SRXL2.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_SRXL2.cpp
index db1891e6b3..57f3bdd770 100644
--- a/libraries/AP_RCProtocol/AP_RCProtocol_SRXL2.cpp
+++ b/libraries/AP_RCProtocol/AP_RCProtocol_SRXL2.cpp
@@ -134,7 +134,7 @@ void AP_RCProtocol_SRXL2::_process_byte(uint32_t timestamp_us, uint8_t byte)
void AP_RCProtocol_SRXL2::update(void)
{
-#if 0 // it's not clear this is actually required
+ // it's not clear this is actually required, perhaps on power loss?
if (frontend.protocol_detected() == AP_RCProtocol::SRXL2) {
uint32_t now = AP_HAL::millis();
// there have been no updates since we were last called
@@ -144,7 +144,6 @@ void AP_RCProtocol_SRXL2::update(void)
_last_run_ms = now;
}
}
-#endif
}
void AP_RCProtocol_SRXL2::capture_scaled_input(const uint16_t *values, bool in_failsafe, int16_t new_rssi)
@@ -251,6 +250,8 @@ void AP_RCProtocol_SRXL2::_change_baud_rate(uint32_t baudrate)
#endif
}
+// SRXL2 library callbacks below
+
// User-provided routine to change the baud rate settings on the given UART:
// uart - the same uint8_t value as the uart parameter passed to srxlInit()
// baudRate - the actual baud rate (currently either 115200 or 400000)
@@ -298,12 +299,10 @@ void srxlReceivedChannelData(SrxlChannelData* pChannelData, bool isFailsafe)
// Return true if you want this bind information set automatically for all other receivers on all SRXL buses.
bool srxlOnBind(SrxlFullID device, SrxlBindData info)
{
- // TODO: Add custom handling of bound data report here if needed
return true;
}
// User-provided callback routine to handle reception of a VTX control packet.
void srxlOnVtx(SrxlVtxData* pVtxData)
{
- //userProvidedHandleVtxData(pVtxData);
}
diff --git a/libraries/AP_RCTelemetry/AP_RCTelemetry.cpp b/libraries/AP_RCTelemetry/AP_RCTelemetry.cpp
new file mode 100644
index 0000000000..06393fa83e
--- /dev/null
+++ b/libraries/AP_RCTelemetry/AP_RCTelemetry.cpp
@@ -0,0 +1,258 @@
+/*
+ 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
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see .
+*/
+
+/*
+ Abstract Telemetry library
+*/
+
+#include "AP_RCTelemetry.h"
+
+#include
+#include
+#include
+#include
+#include
+
+#ifdef TELEM_DEBUG
+# define debug(fmt, args...) hal.console->printf("Telem: " fmt "\n", ##args)
+#else
+# define debug(fmt, args...) do {} while(0)
+#endif
+
+extern const AP_HAL::HAL& hal;
+
+/*
+ setup ready for passthrough telem
+ */
+bool AP_RCTelemetry::init(void)
+{
+#if !APM_BUILD_TYPE(APM_BUILD_UNKNOWN)
+ // make telemetry available to GCS_MAVLINK (used to queue statustext messages from GCS_MAVLINK)
+ // add firmware and frame info to message queue
+ const char* _frame_string = gcs().frame_string();
+ if (_frame_string == nullptr) {
+ queue_message(MAV_SEVERITY_INFO, AP::fwversion().fw_string);
+ } else {
+ char firmware_buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1];
+ snprintf(firmware_buf, sizeof(firmware_buf), "%s %s", AP::fwversion().fw_string, _frame_string);
+ queue_message(MAV_SEVERITY_INFO, firmware_buf);
+ }
+#endif
+ setup_wfq_scheduler();
+
+ return true;
+}
+
+void AP_RCTelemetry::update_avg_packet_rate()
+{
+ uint32_t poll_now = AP_HAL::millis();
+
+ _scheduler.avg_packet_counter++;
+
+ if (poll_now - _scheduler.last_poll_timer > 1000) { //average in last 1000ms
+ // initialize
+ if (_scheduler.avg_packet_rate == 0) _scheduler.avg_packet_rate = _scheduler.avg_packet_counter;
+ // moving average
+ _scheduler.avg_packet_rate = (uint8_t)_scheduler.avg_packet_rate * 0.75f + _scheduler.avg_packet_counter * 0.25f;
+ // reset
+ _scheduler.last_poll_timer = poll_now;
+ _scheduler.avg_packet_counter = 0;
+ debug("avg packet rate %dHz, rates(Hz) %d %d %d %d %d %d %d %d", _scheduler.avg_packet_rate,
+ _scheduler.packet_rate[0],
+ _scheduler.packet_rate[1],
+ _scheduler.packet_rate[2],
+ _scheduler.packet_rate[3],
+ _scheduler.packet_rate[4],
+ _scheduler.packet_rate[5],
+ _scheduler.packet_rate[6],
+ _scheduler.packet_rate[7]);
+ }
+}
+
+/*
+ * WFQ scheduler
+ */
+void AP_RCTelemetry::run_wfq_scheduler(void)
+{
+ update_avg_packet_rate();
+
+ uint32_t now = AP_HAL::millis();
+ uint8_t max_delay_idx = 0;
+
+ float max_delay = 0;
+ float delay = 0;
+ bool packet_ready = false;
+
+ // build message queue for sensor_status_flags
+ check_sensor_status_flags();
+ // build message queue for ekf_status
+ check_ekf_status();
+
+ // dynamic priorities
+ bool queue_empty;
+ {
+ WITH_SEMAPHORE(_statustext.sem);
+ queue_empty = !_statustext.available && _statustext.queue.empty();
+ }
+
+ adjust_packet_weight(queue_empty);
+
+ // search the packet with the longest delay after the scheduled time
+ for (int i=0; i<_time_slots; i++) {
+ // normalize packet delay relative to packet weight
+ delay = (now - _scheduler.packet_timer[i])/static_cast(_scheduler.packet_weight[i]);
+ // use >= so with equal delays we choose the packet with lowest priority
+ // this is ensured by the packets being sorted by desc frequency
+ // apply the rate limiter
+ if (delay >= max_delay && ((now - _scheduler.packet_timer[i]) >= _scheduler.packet_min_period[i])) {
+ packet_ready = is_packet_ready(i, queue_empty);
+
+ if (packet_ready) {
+ max_delay = delay;
+ max_delay_idx = i;
+ }
+ }
+ }
+ now = AP_HAL::millis();
+#ifdef TELEM_DEBUG
+ _scheduler.packet_rate[max_delay_idx] = (_scheduler.packet_rate[max_delay_idx] + 1000 / (now - _scheduler.packet_timer[max_delay_idx])) / 2;
+#endif
+ _scheduler.packet_timer[max_delay_idx] = now;
+ //debug("process_packet(%d): %f", max_delay_idx, max_delay);
+ // send packet
+ process_packet(max_delay_idx);
+}
+
+/*
+ * add message to message cue for transmission through link
+ */
+void AP_RCTelemetry::queue_message(MAV_SEVERITY severity, const char *text)
+{
+ mavlink_statustext_t statustext{};
+
+ statustext.severity = severity;
+ strncpy(statustext.text, text, sizeof(statustext.text));
+
+ // The force push will ensure comm links do not block other comm links forever if they fail.
+ // If we push to a full buffer then we overwrite the oldest entry, effectively removing the
+ // block but not until the buffer fills up.
+ WITH_SEMAPHORE(_statustext.sem);
+ _statustext.queue.push_force(statustext);
+}
+
+/*
+ * add sensor_status_flags information to message cue, normally passed as sys_status mavlink messages to the GCS, for transmission through FrSky link
+ */
+void AP_RCTelemetry::check_sensor_status_flags(void)
+{
+ uint32_t now = AP_HAL::millis();
+
+ const uint32_t _sensor_status_flags = sensor_status_flags();
+
+ if ((now - check_sensor_status_timer) >= 5000) { // prevent repeating any system_status messages unless 5 seconds have passed
+ // only one error is reported at a time (in order of preference). Same setup and displayed messages as Mission Planner.
+ if ((_sensor_status_flags & MAV_SYS_STATUS_SENSOR_GPS) > 0) {
+ queue_message(MAV_SEVERITY_CRITICAL, "Bad GPS Health");
+ check_sensor_status_timer = now;
+ } else if ((_sensor_status_flags & MAV_SYS_STATUS_SENSOR_3D_GYRO) > 0) {
+ queue_message(MAV_SEVERITY_CRITICAL, "Bad Gyro Health");
+ check_sensor_status_timer = now;
+ } else if ((_sensor_status_flags & MAV_SYS_STATUS_SENSOR_3D_ACCEL) > 0) {
+ queue_message(MAV_SEVERITY_CRITICAL, "Bad Accel Health");
+ check_sensor_status_timer = now;
+ } else if ((_sensor_status_flags & MAV_SYS_STATUS_SENSOR_3D_MAG) > 0) {
+ queue_message(MAV_SEVERITY_CRITICAL, "Bad Compass Health");
+ check_sensor_status_timer = now;
+ } else if ((_sensor_status_flags & MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE) > 0) {
+ queue_message(MAV_SEVERITY_CRITICAL, "Bad Baro Health");
+ check_sensor_status_timer = now;
+ } else if ((_sensor_status_flags & MAV_SYS_STATUS_SENSOR_LASER_POSITION) > 0) {
+ queue_message(MAV_SEVERITY_CRITICAL, "Bad LiDAR Health");
+ check_sensor_status_timer = now;
+ } else if ((_sensor_status_flags & MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW) > 0) {
+ queue_message(MAV_SEVERITY_CRITICAL, "Bad OptFlow Health");
+ check_sensor_status_timer = now;
+ } else if ((_sensor_status_flags & MAV_SYS_STATUS_TERRAIN) > 0) {
+ queue_message(MAV_SEVERITY_CRITICAL, "Bad or No Terrain Data");
+ check_sensor_status_timer = now;
+ } else if ((_sensor_status_flags & MAV_SYS_STATUS_GEOFENCE) > 0) {
+ queue_message(MAV_SEVERITY_CRITICAL, "Geofence Breach");
+ check_sensor_status_timer = now;
+ } else if ((_sensor_status_flags & MAV_SYS_STATUS_AHRS) > 0) {
+ queue_message(MAV_SEVERITY_CRITICAL, "Bad AHRS");
+ check_sensor_status_timer = now;
+ } else if ((_sensor_status_flags & MAV_SYS_STATUS_SENSOR_RC_RECEIVER) > 0) {
+ queue_message(MAV_SEVERITY_CRITICAL, "No RC Receiver");
+ check_sensor_status_timer = now;
+ } else if ((_sensor_status_flags & MAV_SYS_STATUS_LOGGING) > 0) {
+ queue_message(MAV_SEVERITY_CRITICAL, "Bad Logging");
+ check_sensor_status_timer = now;
+ }
+ }
+}
+
+/*
+ * add innovation variance information to message cue, normally passed as ekf_status_report mavlink messages to the GCS, for transmission through FrSky link
+ */
+void AP_RCTelemetry::check_ekf_status(void)
+{
+ // get variances
+ bool get_variance;
+ float velVar, posVar, hgtVar, tasVar;
+ Vector3f magVar;
+ Vector2f offset;
+ {
+ AP_AHRS &_ahrs = AP::ahrs();
+ WITH_SEMAPHORE(_ahrs.get_semaphore());
+ get_variance = _ahrs.get_variances(velVar, posVar, hgtVar, magVar, tasVar, offset);
+ }
+ if (get_variance) {
+ uint32_t now = AP_HAL::millis();
+ if ((now - check_ekf_status_timer) >= 10000) { // prevent repeating any ekf_status message unless 10 seconds have passed
+ // multiple errors can be reported at a time. Same setup as Mission Planner.
+ if (velVar >= 1) {
+ queue_message(MAV_SEVERITY_CRITICAL, "Error velocity variance");
+ check_ekf_status_timer = now;
+ }
+ if (posVar >= 1) {
+ queue_message(MAV_SEVERITY_CRITICAL, "Error pos horiz variance");
+ check_ekf_status_timer = now;
+ }
+ if (hgtVar >= 1) {
+ queue_message(MAV_SEVERITY_CRITICAL, "Error pos vert variance");
+ check_ekf_status_timer = now;
+ }
+ if (magVar.length() >= 1) {
+ queue_message(MAV_SEVERITY_CRITICAL, "Error compass variance");
+ check_ekf_status_timer = now;
+ }
+ if (tasVar >= 1) {
+ queue_message(MAV_SEVERITY_CRITICAL, "Error terrain alt variance");
+ check_ekf_status_timer = now;
+ }
+ }
+ }
+}
+
+uint32_t AP_RCTelemetry::sensor_status_flags() const
+{
+ uint32_t present;
+ uint32_t enabled;
+ uint32_t health;
+ gcs().get_sensor_status_flags(present, enabled, health);
+
+ return ~health & enabled & present;
+}
+
diff --git a/libraries/AP_RCTelemetry/AP_RCTelemetry.h b/libraries/AP_RCTelemetry/AP_RCTelemetry.h
new file mode 100644
index 0000000000..43056340f3
--- /dev/null
+++ b/libraries/AP_RCTelemetry/AP_RCTelemetry.h
@@ -0,0 +1,97 @@
+/*
+ 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
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see .
+*/
+#pragma once
+
+#include
+#include
+#include
+
+#define TELEM_PAYLOAD_STATUS_CAPACITY 5 // size of the message buffer queue (max number of messages waiting to be sent)
+
+// for fair scheduler
+#define TELEM_TIME_SLOT_MAX 15
+//#define TELEM_DEBUG
+
+class AP_RCTelemetry {
+public:
+ AP_RCTelemetry(uint8_t time_slots) : _time_slots(time_slots) {}
+ virtual ~AP_RCTelemetry() {};
+
+ /* Do not allow copies */
+ AP_RCTelemetry(const AP_RCTelemetry &other) = delete;
+ AP_RCTelemetry &operator=(const AP_RCTelemetry&) = delete;
+
+ // add statustext message to message queue
+ void queue_message(MAV_SEVERITY severity, const char *text);
+
+ // update error mask of sensors and subsystems. The mask uses the
+ // MAV_SYS_STATUS_* values from mavlink. If a bit is set then it
+ // indicates that the sensor or subsystem is present but not
+ // functioning correctly
+ uint32_t sensor_status_flags() const;
+
+protected:
+ void run_wfq_scheduler();
+ // set an entry in the scheduler table
+ void set_scheduler_entry(uint8_t slot, uint32_t weight, uint32_t min_period_ms) {
+ _scheduler.packet_weight[slot] = weight;
+ _scheduler.packet_min_period[slot] = min_period_ms;
+ }
+ // add an entry to the scheduler table
+ void add_scheduler_entry(uint32_t weight, uint32_t min_period_ms) {
+ set_scheduler_entry(_time_slots++, weight, min_period_ms);
+ }
+ // setup ready for passthrough operation
+ virtual bool init(void);
+
+ uint8_t _time_slots;
+
+ struct
+ {
+ uint32_t last_poll_timer;
+ uint32_t avg_packet_counter;
+ uint32_t packet_timer[TELEM_TIME_SLOT_MAX];
+ uint32_t packet_weight[TELEM_TIME_SLOT_MAX];
+ uint32_t packet_min_period[TELEM_TIME_SLOT_MAX];
+ uint8_t avg_packet_rate;
+#ifdef TELEM_DEBUG
+ uint8_t packet_rate[TELEM_TIME_SLOT_MAX];
+#endif
+ } _scheduler;
+
+ struct {
+ HAL_Semaphore sem;
+ ObjectBuffer queue{TELEM_PAYLOAD_STATUS_CAPACITY};
+ mavlink_statustext_t next;
+ bool available;
+ } _statustext;
+
+private:
+ uint32_t check_sensor_status_timer;
+ uint32_t check_ekf_status_timer;
+
+ // passthrough WFQ scheduler
+ virtual void setup_wfq_scheduler() = 0;
+ virtual bool get_next_msg_chunk(void) = 0;
+ virtual bool is_packet_ready(uint8_t idx, bool queue_empty) = 0;
+ virtual void process_packet(uint8_t idx) = 0;
+ virtual void adjust_packet_weight(bool queue_empty) = 0;
+
+ void update_avg_packet_rate();
+
+ // methods to convert flight controller data to FrSky SPort Passthrough (OpenTX) format
+ void check_sensor_status_flags(void);
+ void check_ekf_status(void);
+};
diff --git a/libraries/AP_RCTelemetry/AP_Spektrum_Telem.cpp b/libraries/AP_RCTelemetry/AP_Spektrum_Telem.cpp
new file mode 100644
index 0000000000..ca263a97ce
--- /dev/null
+++ b/libraries/AP_RCTelemetry/AP_Spektrum_Telem.cpp
@@ -0,0 +1,622 @@
+/*
+ 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
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see .
+*/
+
+/*
+ Spektrum Telemetry library, based on AP_Frsky_Telem.cpp
+ See https://www.spektrumrc.com/ProdInfo/Files/SPM_Telemetry_Developers_Specs.pdf
+*/
+
+#include "AP_Spektrum_Telem.h"
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#ifdef HAVE_AP_BLHELI_SUPPORT
+#include
+#endif
+#include
+
+#if HAL_SPEKTRUM_TELEM_ENABLED
+
+#define MICROSEC_PER_MINUTE 60000000
+#define MAX_TEXTGEN_LEN 13
+
+//#define SPKT_DEBUG
+#ifdef SPKT_DEBUG
+# define debug(fmt, args...) hal.console->printf("SPKT:" fmt "\n", ##args)
+#else
+# define debug(fmt, args...) do {} while(0)
+#endif
+
+extern const AP_HAL::HAL& hal;
+
+AP_Spektrum_Telem *AP_Spektrum_Telem::singleton;
+
+AP_Spektrum_Telem::AP_Spektrum_Telem() : AP_RCTelemetry(0)
+{
+ singleton = this;
+}
+
+AP_Spektrum_Telem::~AP_Spektrum_Telem(void)
+{
+ singleton = nullptr;
+}
+
+bool AP_Spektrum_Telem::init(void)
+{
+ // sanity check that we are using a UART for RC input
+ if (!AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_RCIN, 0)) {
+ return false;
+ }
+ return AP_RCTelemetry::init();
+}
+
+/*
+ setup ready for passthrough telem
+ */
+void AP_Spektrum_Telem::setup_wfq_scheduler(void)
+{
+ // initialize packet weights for the WFQ scheduler
+ // priority[i] = 1/_scheduler.packet_weight[i]
+ // rate[i] = LinkRate * ( priority[i] / (sum(priority[1-n])) )
+
+ // Spektrum telemetry rate is 46Hz, so these rates must fit
+ add_scheduler_entry(50, 100); // qos 10Hz
+ add_scheduler_entry(50, 100); // rpm 10Hz
+ add_scheduler_entry(50, 100); // text, 10Hz
+ add_scheduler_entry(50, 120); // Attitude and compass 8Hz
+ add_scheduler_entry(550, 280); // GPS 3Hz
+ add_scheduler_entry(550, 280); // ESC 3Hz
+ add_scheduler_entry(400, 250); // altitude 4Hz
+ add_scheduler_entry(400, 250); // airspeed 4Hz
+ add_scheduler_entry(700, 500); // GPS status 2Hz
+ add_scheduler_entry(1300, 500); // batt volt 2Hz
+ add_scheduler_entry(1300, 500); // batt curr 2Hz
+ add_scheduler_entry(1300, 500); // batt mah 2Hz
+ add_scheduler_entry(1300, 500); // temp 2Hz
+}
+
+void AP_Spektrum_Telem::adjust_packet_weight(bool queue_empty)
+{
+ if (!queue_empty) {
+ _scheduler.packet_weight[TEXT] = 50; // messages
+ } else {
+ _scheduler.packet_weight[TEXT] = 5000; // messages
+ }
+}
+
+// WFQ scheduler
+bool AP_Spektrum_Telem::is_packet_ready(uint8_t idx, bool queue_empty)
+{
+ bool packet_ready = false;
+ switch (idx) {
+ case TEXT:
+ packet_ready = !queue_empty;
+ break;
+ default:
+ packet_ready = true;
+ break;
+ }
+
+ return packet_ready;
+}
+
+// WFQ scheduler
+void AP_Spektrum_Telem::process_packet(uint8_t idx)
+{
+ // send packet
+ switch (idx) {
+ case QOS: // QOS
+ calc_qos();
+ break;
+ case RPM: // RPM
+ calc_rpm();
+ break;
+ case TEXT: // status text
+ if (repeat_msg_chunk() || get_next_msg_chunk()) {
+ send_msg_chunk(_msg_chunk);
+ }
+ break;
+ case ATTITUDE: // Attitude and compass
+ calc_attandmag();
+ break;
+ case GPS_LOC: // GPS location
+ calc_gps_location();
+ break;
+ case ESC: // ESC
+ calc_esc();
+ break;
+ case ALTITUDE: // altitude
+ calc_altitude();
+ break;
+ case AIRSPEED: // airspeed
+ calc_airspeed();
+ break;
+ case GPS_STATUS: // GPS status
+ calc_gps_status();
+ break;
+ case VOLTAGE: // Battery volts
+ calc_batt_volts(0);
+ break;
+ case AMPS: // Battery current
+ calc_batt_amps(0);
+ break;
+ case MAH: // Battery current & mah
+ calc_batt_mah();
+ break;
+ case TEMP: // temperature
+ calc_temperature(0);
+ break;
+ default:
+ break;
+ }
+}
+
+// whether to repeat the last texgen output
+bool AP_Spektrum_Telem::repeat_msg_chunk(void)
+{
+ if (_msg_chunk.repeats == 0) {
+ return false;
+ }
+
+ // repeat each message chunk 3 times to ensure transmission
+ // on slow links reduce the number of duplicate chunks
+ uint8_t extra_chunks = 2;
+
+ if (_scheduler.avg_packet_rate < 20) {
+ extra_chunks = 0;
+ } else if (_scheduler.avg_packet_rate < 30) {
+ extra_chunks = 1;
+ }
+
+ if (_msg_chunk.repeats++ > extra_chunks) {
+ _msg_chunk.repeats = 0;
+ return false;
+ }
+ return true;
+}
+
+
+// grabs one "chunk" (13 bytes) of the queued message to be transmitted
+bool AP_Spektrum_Telem::get_next_msg_chunk(void)
+{
+ _msg_chunk.repeats++;
+
+ if (!_statustext.available) {
+ WITH_SEMAPHORE(_statustext.sem);
+ if (!_statustext.queue.pop(_statustext.next)) {
+ return false;
+ }
+
+ _statustext.available = true;
+
+ // We're going to display a new message so first clear the screen
+ _msg_chunk.linenumber = 0xFF;
+ _msg_chunk.char_index = 0;
+ return true;
+ }
+
+ uint8_t character = 0;
+ memset(_msg_chunk.chunk, 0, MAX_TEXTGEN_LEN);
+
+ const uint8_t message_len = sizeof(_statustext.next.text);
+ // the message fits in an entire line of text
+ if (message_len < MAX_TEXTGEN_LEN) {
+ memcpy(_msg_chunk.chunk, _statustext.next.text, message_len);
+ _msg_chunk.linenumber = 0;
+ _statustext.available = false;
+ return true;
+ }
+
+ // a following part of multi-line text
+ if (_msg_chunk.linenumber == 0xFF) {
+ _msg_chunk.linenumber = 0;
+ } else if (_msg_chunk.char_index > 0) {
+ _msg_chunk.linenumber++;
+ }
+
+ // skip leading whitespace
+ while (_statustext.next.text[_msg_chunk.char_index] == ' ' && _msg_chunk.char_index < message_len) {
+ _msg_chunk.char_index++;
+ }
+
+ uint8_t space_idx = 0;
+ const uint8_t begin_idx = _msg_chunk.char_index;
+ // can't fit it all on one line so wrap at an appropriate place
+ for (int i = 0; i < MAX_TEXTGEN_LEN && _msg_chunk.char_index < message_len; i++) {
+ character = _statustext.next.text[_msg_chunk.char_index++];
+ // split at the first ':'
+ if (character == ':') {
+ _msg_chunk.chunk[i] = 0;
+ break;
+ }
+ // record the last space if we need to go back there
+ if (character == ' ') {
+ space_idx = _msg_chunk.char_index;
+ }
+
+ _msg_chunk.chunk[i] = character;
+
+ if (!character) {
+ break;
+ }
+ }
+ // still not done, can we break at a word boundary?
+ if (character != 0 && _msg_chunk.char_index < message_len && space_idx > 0) {
+ _msg_chunk.char_index = space_idx;
+ _msg_chunk.chunk[space_idx - begin_idx - 1] = 0;
+ }
+
+ // we've reached the end of the message (string terminated by '\0' or last character of the string has been processed)
+ if (character == 0 || _msg_chunk.char_index == message_len) {
+ _msg_chunk.char_index = 0; // reset index to get ready to process the next message
+ _statustext.available = false;
+ }
+
+ return true;
+}
+
+// prepare qos data - mandatory frame that must be sent periodically
+void AP_Spektrum_Telem::calc_qos()
+{
+ _telem.qos.identifier = TELE_DEVICE_QOS;
+ _telem.qos.sID = 0;
+ _telem.qos.A = 0xFFFF;
+ _telem.qos.B = 0xFFFF;
+ _telem.qos.L = 0xFFFF;
+ _telem.qos.R = 0xFFFF;
+ _telem.qos.F = 0xFFFF;
+ _telem.qos.H = 0xFFFF;
+ _telem.qos.rxVoltage = 0xFFFF;
+ _telem_pending = true;
+}
+
+// prepare rpm data - B/E mandatory frame that must be sent periodically
+void AP_Spektrum_Telem::calc_rpm()
+{
+ const AP_BattMonitor &_battery = AP::battery();
+
+ _telem.rpm.identifier = TELE_DEVICE_RPM;
+ _telem.rpm.sID = 0;
+ // battery voltage in centivolts, can have up to a 12S battery (4.25Vx12S = 51.0V)
+ _telem.rpm.volts = htobe16(((uint16_t)roundf(_battery.voltage(0) * 100.0f)));
+ _telem.rpm.temperature = htobe16(int16_t(roundf(32.0f + AP::baro().get_temperature(0) * 9.0f / 5.0f)));
+ const AP_RPM *rpm = AP::rpm();
+ float rpm_value;
+ if (!rpm || !rpm->get_rpm(0, rpm_value) || rpm_value < 999.0f) {
+ rpm_value = 999.0f;
+ }
+ _telem.rpm.microseconds = htobe16(uint16_t(roundf(MICROSEC_PER_MINUTE / rpm_value)));
+ _telem.rpm.dBm_A = 0x7F;
+ _telem.rpm.dBm_B = 0x7F;
+ _telem_pending = true;
+}
+
+void AP_Spektrum_Telem::send_msg_chunk(const MessageChunk& chunk)
+{
+ memcpy(_telem.textgen.text, chunk.chunk, 13);
+ _telem.textgen.identifier = TELE_DEVICE_TEXTGEN;
+ _telem.textgen.lineNumber = chunk.linenumber;
+ _telem.textgen.sID = 0;
+ _telem_pending = true;
+}
+
+// prepare battery data - B/E but not supported by Spektrum
+void AP_Spektrum_Telem::calc_batt_volts(uint8_t instance)
+{
+ const AP_BattMonitor &_battery = AP::battery();
+
+ // battery voltage in centivolts, can have up to a 12S battery (4.25Vx12S = 51.0V)
+ _telem.hv.volts = htobe16(uint16_t(roundf(_battery.voltage(instance) * 100.0f)));
+ _telem.hv.identifier = TELE_DEVICE_VOLTAGE;
+ _telem.hv.sID = 0;
+ _telem_pending = true;
+}
+
+// prepare battery data - B/E but not supported by Spektrum
+void AP_Spektrum_Telem::calc_batt_amps(uint8_t instance)
+{
+ const AP_BattMonitor &_battery = AP::battery();
+
+ float current;
+ if (!_battery.current_amps(current, instance)) {
+ current = 0;
+ }
+
+ // Range: +/- 150A Resolution: 300A / 2048 = 0.196791 A/count
+ _telem.amps.current = htobe16(int16_t(roundf(current * 2048.0f / 300.0f)));
+ _telem.amps.identifier = TELE_DEVICE_AMPS;
+ _telem.amps.sID = 0;
+ _telem_pending = true;
+}
+
+// prepare battery data - L/E
+void AP_Spektrum_Telem::calc_batt_mah()
+{
+ const AP_BattMonitor &_battery = AP::battery();
+
+ _telem.fpMAH.identifier = TELE_DEVICE_FP_MAH;
+ _telem.fpMAH.sID = 0;
+
+ float current;
+ if (!_battery.current_amps(current, 0)) {
+ current = 0;
+ }
+ _telem.fpMAH.current_A = int16_t(roundf(current * 10.0f)); // Instantaneous current, 0.1A (0-3276.6A)
+
+ float used_mah;
+ if (!_battery.consumed_mah(used_mah, 0)) {
+ used_mah = 0;
+ }
+ _telem.fpMAH.chargeUsed_A = int16_t(roundf(used_mah)); // Integrated mAh used, 1mAh (0-32.766Ah)
+
+ float temp;
+ if (_battery.get_temperature(temp, 0)) {
+ _telem.fpMAH.temp_A = uint16_t(roundf(temp * 10.0f)); // Temperature, 0.1C (0-150C, 0x7FFF indicates not populated)
+ } else {
+ _telem.fpMAH.temp_A = 0x7FFF;
+ }
+
+ if (!_battery.current_amps(current, 1)) {
+ current = 0;
+ }
+ _telem.fpMAH.current_B = int16_t(roundf(current * 10.0f)); // Instantaneous current, 0.1A (0-3276.6A)
+
+ if (!_battery.consumed_mah(used_mah, 1)) {
+ used_mah = 0;
+ }
+ _telem.fpMAH.chargeUsed_B = int16_t(roundf(used_mah)); // Integrated mAh used, 1mAh (0-32.766Ah)
+
+ if (_battery.get_temperature(temp, 1)) {
+ _telem.fpMAH.temp_B = uint16_t(roundf(temp * 10.0f)); // Temperature, 0.1C (0-150C, 0x7FFF indicates not populated)
+ } else {
+ _telem.fpMAH.temp_B = 0x7FFF;
+ }
+
+ _telem_pending = true;
+}
+
+// prepare temperature data - B/E but not supported by Spektrum
+void AP_Spektrum_Telem::calc_temperature(uint8_t instance)
+{
+ _telem.temp.temperature = htobe16(int16_t(roundf(32.0f + AP::baro().get_temperature(instance) * 9.0f / 5.0f)));
+ _telem.temp.identifier = TELE_DEVICE_TEMPERATURE;
+ _telem.temp.sID = 0;
+ _telem_pending = true;
+}
+
+// prepare altitude data - B/E
+void AP_Spektrum_Telem::calc_altitude()
+{
+ _telem.alt.identifier = TELE_DEVICE_ALTITUDE;
+ _telem.alt.sID = 0;
+
+ AP_AHRS &ahrs = AP::ahrs();
+ WITH_SEMAPHORE(ahrs.get_semaphore());
+
+ float alt = 0;
+ ahrs.get_relative_position_D_home(alt);
+ alt = roundf(-alt * 10.0f);
+ _telem.alt.altitude = htobe16(uint16_t(alt)); // .1m increments
+ _max_alt = MAX(alt, _max_alt);
+ _telem.alt.maxAltitude = htobe16(uint16_t(_max_alt)); // .1m increments
+ _telem_pending = true;
+}
+
+// prepare airspeed data - B/E
+void AP_Spektrum_Telem::calc_airspeed()
+{
+ _telem.speed.identifier = TELE_DEVICE_AIRSPEED;
+ _telem.speed.sID = 0;
+
+ AP_AHRS &ahrs = AP::ahrs();
+ WITH_SEMAPHORE(ahrs.get_semaphore());
+
+ const AP_Airspeed *airspeed = ahrs.get_airspeed();
+ float speed = 0.0f;
+ if (airspeed && airspeed->healthy()) {
+ speed = roundf(airspeed->get_airspeed() * 3.6);
+ } else {
+ speed = roundf(AP::ahrs().groundspeed() * 3.6);
+ }
+ _telem.speed.airspeed = htobe16(uint16_t(speed)); // 1 km/h increments
+ _max_speed = MAX(speed, _max_speed);
+ _telem.speed.maxAirspeed = htobe16(uint16_t(_max_speed)); // 1 km/h increments
+ _telem_pending = true;
+}
+
+// prepare attitude and compass data - L/E
+void AP_Spektrum_Telem::calc_attandmag(void)
+{
+ _telem.attMag.identifier = TELE_DEVICE_ATTMAG;
+ _telem.attMag.sID = 0;
+
+ AP_AHRS &_ahrs = AP::ahrs();
+ WITH_SEMAPHORE(_ahrs.get_semaphore());
+
+ // Attitude, 3 axes. Roll is a rotation about the X Axis of the vehicle using the RHR.
+ // Units are 0.1 deg - Pitch is a rotation about the Y Axis of the vehicle using the RHR.
+ // Yaw is a rotation about the Z Axis of the vehicle using the RHR.
+ _telem.attMag.attRoll = _ahrs.roll_sensor / 10;
+ _telem.attMag.attPitch = _ahrs.pitch_sensor / 10;
+ _telem.attMag.attYaw = _ahrs.yaw_sensor / 10;
+ _telem.attMag.heading = (_ahrs.yaw_sensor / 10) % 3600; // Heading, 0.1deg
+
+ const Vector3f& field = AP::compass().get_field();
+
+ _telem.attMag.magX = int16_t(roundf(field.x * 10.0f)); // Units are 0.1mG
+ _telem.attMag.magY = int16_t(roundf(field.y * 10.0f));
+ _telem.attMag.magZ = int16_t(roundf(field.z * 10.0f));
+ _telem_pending = true;
+}
+
+// prepare gps location - L/E
+void AP_Spektrum_Telem::calc_gps_location()
+{
+ const Location &loc = AP::gps().location(0); // use the first gps instance (same as in send_mavlink_gps_raw)
+ const uint32_t u1e8 = 100000000, u1e7 = 10000000, u1e6 = 1000000, u1e5 = 100000, u1e4 = 10000;
+
+ _telem.gpsloc.identifier = TELE_DEVICE_GPS_LOC; // Source device = 0x16
+ _telem.gpsloc.sID = 0; // Secondary ID
+ uint32_t alt = (abs(loc.alt) / 10) % u1e6;
+ _telem.gpsloc.altitudeLow = ((alt % u1e4 / 1000) << 12) | ((alt % 1000 / 100) << 8)
+ | ((alt % 100 / 10) << 4) | (alt % 100); // BCD, meters, format 3.1 (Low order of altitude)
+
+ const float lat = fabsf(loc.lat / 1.0e7f); // BCD, format 4.4, Degrees * 100 + minutes, less than 100 degrees
+ const float lng = fabsf(loc.lng / 1.0e7f); // BCD, format 4.4 , Degrees * 100 + minutes, flag indicates > 99 degrees
+
+ const uint32_t ulat = roundf((int32_t(lat) * 100.0f + (lat - int32_t(lat)) * 60.0f) * 10000.0f);
+ const uint32_t ulng = roundf((int32_t(lng) * 100.0f + (lng - int32_t(lng)) * 60.0f) * 10000.0f);
+
+ _telem.gpsloc.latitude = ((ulat % u1e8 / u1e7) << 28) | ((ulat % u1e7 / u1e6) << 24) | ((ulat % u1e6 / u1e5) << 20) | ((ulat % u1e5 / u1e4) << 16)
+ | ((ulat % u1e4 / 1000) << 12) | ((ulat % 1000 / 100) << 8) | ((ulat % 100 / 10) << 4) | (ulat % 10);
+ _telem.gpsloc.longitude = ((ulng % u1e8 / u1e7) << 28) | ((ulng % u1e7 / u1e6) << 24) | ((ulng % u1e6 / u1e5) << 20) | ((ulng % u1e5 / u1e4) << 16)
+ | ((ulng % u1e4 / 1000) << 12) | ((ulng % 1000 / 100) << 8) | ((ulng % 100 / 10) << 4) | (ulng % 10);
+
+ uint16_t course = uint16_t(roundf(AP::gps().ground_course() * 10.0f));
+ _telem.gpsloc.course = ((course % u1e5 / u1e4) << 12) | ((course % u1e4 / 1000) << 8) | ((course % 1000 / 100) << 4) | (course % 100 / 10); // BCD, 3.1
+ uint16_t hdop = AP::gps().get_hdop(0);
+ _telem.gpsloc.HDOP = ((hdop % 1000 / 100) << 4) | (hdop % 100 / 10); // BCD, format 1.1
+ _telem.gpsloc.GPSflags = 0;
+
+ if (AP::gps().status(0) >= AP_GPS::GPS_OK_FIX_3D) {
+ _telem.gpsloc.GPSflags |= GPS_INFO_FLAGS_3D_FIX;
+ }
+ if (loc.alt < 0) {
+ _telem.gpsloc.GPSflags |= GPS_INFO_FLAGS_NEGATIVE_ALT;
+ }
+ if ((loc.lng / 1e7) > 99) {
+ _telem.gpsloc.GPSflags |= GPS_INFO_FLAGS_LONGITUDE_GREATER_99;
+ }
+ if (loc.lat >= 0) {
+ _telem.gpsloc.GPSflags |= GPS_INFO_FLAGS_IS_NORTH;
+ }
+ if (loc.lng >= 0) {
+ _telem.gpsloc.GPSflags |= GPS_INFO_FLAGS_IS_EAST;
+ }
+ if (AP::gps().status(0) > AP_GPS::NO_FIX) {
+ _telem.gpsloc.GPSflags |= GPS_INFO_FLAGS_GPS_FIX_VALID;
+ }
+ if (AP::gps().status(0) >= AP_GPS::NO_FIX) {
+ _telem.gpsloc.GPSflags |= GPS_INFO_FLAGS_GPS_DATA_RECEIVED;
+ }
+ _telem_pending = true;
+}
+
+// prepare gps status - L/E
+void AP_Spektrum_Telem::calc_gps_status()
+{
+ const Location &loc = AP::gps().location(0);
+
+ _telem.gpsstat.identifier = TELE_DEVICE_GPS_STATS; // Source device = 0x17
+ _telem.gpsstat.sID = 0; // Secondary ID
+
+ uint16_t knots = roundf(AP::gps().ground_speed() * 1.94384f * 10.0f);
+ _telem.gpsstat.speed = ((knots % 10000 / 1000) << 12) | ((knots % 1000 / 100) << 8) | ((knots % 100 / 10) << 4) | (knots % 10); // BCD, knots, format 3.1
+ uint16_t ms;
+ uint8_t h, m, s;
+ AP::rtc().get_system_clock_utc(h, m, s, ms); // BCD, format HH:MM:SS.S, format 6.1
+ _telem.gpsstat.UTC = ((((h / 10) << 4) | (h % 10)) << 20) | ((((m / 10) << 4) | (m % 10)) << 12) | ((((s / 10) << 4) | (s % 10)) << 4) | (ms / 100) ;
+ uint8_t nsats = AP::gps().num_sats();
+ _telem.gpsstat.numSats = ((nsats / 10) << 4) | (nsats % 10); // BCD, 0-99
+ uint32_t alt = (abs(loc.alt) / 100000);
+ _telem.gpsstat.altitudeHigh = ((alt / 10) << 4) | (alt % 10); // BCD, meters, format 2.0 (High order of altitude)
+ _telem_pending = true;
+}
+
+// prepare ESC information - B/E
+void AP_Spektrum_Telem::calc_esc()
+{
+#ifdef HAVE_AP_BLHELI_SUPPORT
+ AP_BLHeli* blh = AP_BLHeli::get_singleton();
+
+ if (blh == nullptr) {
+ return;
+ }
+
+ AP_BLHeli::telem_data td;
+
+ if (!blh->get_telem_data(0, td)) {
+ return;
+ }
+
+ _telem.esc.identifier = TELE_DEVICE_ESC; // Source device = 0x20
+ _telem.esc.sID = 0; // Secondary ID
+ _telem.esc.RPM = htobe16(uint16_t(roundf(blh->get_average_motor_frequency_hz() * 60))); // Electrical RPM, 10RPM (0-655340 RPM) 0xFFFF --> "No data"
+ _telem.esc.voltsInput = htobe16(td.voltage); // Volts, 0.01v (0-655.34V) 0xFFFF --> "No data"
+ _telem.esc.tempFET = htobe16(td.temperature * 10); // Temperature, 0.1C (0-6553.4C) 0xFFFF --> "No data"
+ _telem.esc.currentMotor = htobe16(td.current); // Current, 10mA (0-655.34A) 0xFFFF --> "No data"
+ _telem.esc.tempBEC = 0xFFFF; // Temperature, 0.1C (0-6553.4C) 0xFFFF --> "No data"
+ _telem.esc.currentBEC = 0xFF; // BEC Current, 100mA (0-25.4A) 0xFF ----> "No data"
+ _telem.esc.voltsBEC = 0xFF; // BEC Volts, 0.05V (0-12.70V) 0xFF ----> "No data"
+ _telem.esc.throttle = 0xFF; // 0.5% (0-100%) 0xFF ----> "No data"
+ _telem.esc.powerOut = 0xFF; // Power Output, 0.5% (0-127%) 0xFF ----> "No data"
+ _telem_pending = true;
+#endif
+}
+
+/*
+ fetch Spektrum data for an external transport, such as SRXL2
+ */
+bool AP_Spektrum_Telem::_get_telem_data(uint8_t* data)
+{
+ memset(&_telem, 0, 16);
+ run_wfq_scheduler();
+ if (!_telem_pending) {
+ return false;
+ }
+ memcpy(data, &_telem, 16);
+ _telem_pending = false;
+ return true;
+}
+
+/*
+ fetch data for an external transport, such as SRXL2
+ */
+bool AP_Spektrum_Telem::get_telem_data(uint8_t* data)
+{
+ if (!singleton && !hal.util->get_soft_armed()) {
+ // if telem data is requested when we are disarmed and don't
+ // yet have a AP_Spektrum_Telem object then try to allocate one
+ new AP_Spektrum_Telem();
+ // initialize the passthrough scheduler
+ if (singleton) {
+ singleton->init();
+ }
+ }
+ if (!singleton) {
+ return false;
+ }
+ return singleton->_get_telem_data(data);
+}
+
+namespace AP {
+ AP_Spektrum_Telem *spektrum_telem() {
+ return AP_Spektrum_Telem::get_singleton();
+ }
+};
+
+#endif
\ No newline at end of file
diff --git a/libraries/AP_RCTelemetry/AP_Spektrum_Telem.h b/libraries/AP_RCTelemetry/AP_Spektrum_Telem.h
new file mode 100644
index 0000000000..5ecb15af2c
--- /dev/null
+++ b/libraries/AP_RCTelemetry/AP_Spektrum_Telem.h
@@ -0,0 +1,147 @@
+/*
+ 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
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see .
+*/
+#pragma once
+
+#include
+
+#ifndef HAL_SPEKTRUM_TELEM_ENABLED
+#define HAL_SPEKTRUM_TELEM_ENABLED !HAL_MINIMIZE_FEATURES
+#endif
+
+#if HAL_SPEKTRUM_TELEM_ENABLED
+
+#include
+#include
+#include
+#include "AP_RCTelemetry.h"
+
+#define UINT8 uint8_t
+#define UINT16 uint16_t
+#define UINT32 uint32_t
+#define UINT64 uint64_t
+#define INT8 int8_t
+#define INT16 int16_t
+#define INT32 int32_t
+extern "C" {
+#include "spektrumTelemetrySensors.h"
+}
+#undef UINT8
+#undef UINT16
+#undef UINT32
+#undef UINT64
+#undef INT8
+#undef INT16
+#undef INT32
+
+class AP_Spektrum_Telem : public AP_RCTelemetry {
+public:
+ AP_Spektrum_Telem();
+ ~AP_Spektrum_Telem() override;
+
+ /* Do not allow copies */
+ AP_Spektrum_Telem(const AP_Spektrum_Telem &other) = delete;
+ AP_Spektrum_Telem &operator=(const AP_Spektrum_Telem&) = delete;
+
+ // init - perform required initialisation
+ virtual bool init() override;
+
+ static AP_Spektrum_Telem *get_singleton(void) {
+ return singleton;
+ }
+
+ // get next telemetry data for external consumers of SPort data
+ static bool get_telem_data(uint8_t* data);
+
+private:
+
+ enum SensorType {
+ QOS,
+ RPM,
+ TEXT,
+ ATTITUDE,
+ GPS_LOC,
+ ESC,
+ ALTITUDE,
+ AIRSPEED,
+ GPS_STATUS,
+ VOLTAGE,
+ AMPS,
+ MAH,
+ TEMP,
+ NUM_SENSORS
+ };
+
+ struct MessageChunk
+ {
+ uint8_t chunk[13]; // a "chunk" (13 characters/bytes) at a time of the queued message to be sent
+ uint8_t linenumber;
+ uint8_t char_index; // index of which character to get in the message
+ uint8_t repeats;
+ } _msg_chunk;
+
+ float _max_speed = 0.0f;
+ float _max_alt = 0.0f;
+
+ // passthrough WFQ scheduler
+ // Text Generator
+ bool get_next_msg_chunk(void) override;
+ bool repeat_msg_chunk(void);
+ void send_msg_chunk(const MessageChunk& message);
+ bool is_packet_ready(uint8_t idx, bool queue_empty) override;
+ void process_packet(uint8_t idx) override;
+ void adjust_packet_weight(bool queue_empty) override;
+ // RxV + flight log data
+ void calc_qos();
+ // High-Voltage sensor
+ void calc_batt_volts(uint8_t instance);
+ // Temperature Sensor
+ void calc_temperature(uint8_t instance);
+ // Amps
+ void calc_batt_amps(uint8_t instance);
+ // Flight Battery Capacity (Dual)
+ void calc_batt_mah();
+ // Altitude (Eagle Tree Sensor)
+ void calc_altitude();
+ // Air Speed (Eagle Tree Sensor)
+ void calc_airspeed();
+ // Attitude and Magnetic Compass
+ void calc_attandmag();
+ // GPS Location Data (Eagle Tree)
+ void calc_gps_location();
+ // GPS Status (Eagle Tree)
+ void calc_gps_status();
+ // Electronic Speed Control
+ void calc_esc();
+ // RPM sensor
+ void calc_rpm();
+
+ // setup ready for passthrough operation
+ void setup_wfq_scheduler(void) override;
+
+ // get next telemetry data for external consumers of SPort data (internal function)
+ bool _get_telem_data(uint8_t* data);
+
+ // all Spektrum telemtry packets are big-endian!
+ PACKED UN_TELEMETRY _telem;
+ bool _telem_pending;
+
+ static AP_Spektrum_Telem *singleton;
+};
+
+namespace AP {
+ AP_Spektrum_Telem *spektrum_telem();
+};
+
+#endif
diff --git a/libraries/AP_RCTelemetry/spektrumTelemetrySensors.h b/libraries/AP_RCTelemetry/spektrumTelemetrySensors.h
new file mode 100644
index 0000000000..cb40ece55b
--- /dev/null
+++ b/libraries/AP_RCTelemetry/spektrumTelemetrySensors.h
@@ -0,0 +1,1354 @@
+//////////////////////////////////////////////////////////////////////////////
+//
+// Copyright 2013 by Horizon Hobby, Inc.
+// All Rights Reserved Worldwide.
+//
+// Released to Public Domain
+//
+// This header file may be incorporated into non-Horizon
+// products.
+//
+//////////////////////////////////////////////////////////////////////////////
+//
+// Author: AK
+// Date: 2017-02-24
+// Mods: Sync to Spektrum internal version by matching sequence of
+// structs, formatting, etc. Also changed some structs from
+// having "id" to "identifier." Also redefined "spare" in Rx MAH
+// to provide more bits for "chargeUsed" fields.
+//
+#ifndef TELEMETRY_H
+#define TELEMETRY_H
+
+//////////////////////////////////////////////////////////////////////////////
+//
+// TELEMETRY SENSOR I2C ADDRESSES & DEVICE TYPES
+//
+//////////////////////////////////////////////////////////////////////////////
+
+#define TELE_DEVICE_NODATA (0x00) // No data in packet, but telemetry is alive
+#define TELE_DEVICE_VOLTAGE (0x01) // High-Voltage sensor (INTERNAL)
+#define TELE_DEVICE_TEMPERATURE (0x02) // Temperature Sensor (INTERNAL)
+#define TELE_DEVICE_AMPS (0x03) // Amps (INTERNAL)
+#define TELE_DEVICE_RSV_04 (0x04) // Reserved
+#define TELE_DEVICE_FLITECTRL (0x05) // Flight Controller Status Report
+#define TELE_DEVICE_RSV_06 (0x06) // Reserved
+#define TELE_DEVICE_RSV_07 (0x07) // Reserved
+#define TELE_DEVICE_RSV_08 (0x08) // Reserved
+//#define DO_NOT_USE (0x09) // DO NOT USE!
+#define TELE_DEVICE_PBOX (0x0A) // PowerBox
+#define TELE_DEVICE_LAPTIMER (0x0B) // Lap Timer
+#define TELE_DEVICE_TEXTGEN (0x0C) // Text Generator
+#define TELE_DEVICE_VTX (0x0D) // Video Transmitter Feedback
+#define TELE_DEVICE_RSV_0E (0x0E) // Reserved
+#define TELE_DEVICE_RSV_0F (0x0F) // Reserved
+#define TELE_DEVICE_RSV_10 (0x10) // Reserved
+#define TELE_DEVICE_AIRSPEED (0x11) // Air Speed (Eagle Tree Sensor)
+#define TELE_DEVICE_ALTITUDE (0x12) // Altitude (Eagle Tree Sensor)
+#define TELE_DEVICE_RSV_13 (0x13) // Reserved
+#define TELE_DEVICE_GMETER (0x14) // G-Force (Eagle Tree Sensor)
+#define TELE_DEVICE_JETCAT (0x15) // Turbine interface (Eagle Tree)
+#define TELE_DEVICE_GPS_LOC (0x16) // GPS Location Data (Eagle Tree)
+#define TELE_DEVICE_GPS_STATS (0x17) // GPS Status (Eagle Tree)
+#define TELE_DEVICE_RX_MAH (0x18) // Receiver Pack Capacity (Dual)
+#define TELE_DEVICE_JETCAT_2 (0x19) // Turbine interface, message 2 format (Eagle Tree)
+#define TELE_DEVICE_GYRO (0x1A) // 3-axis gyro
+#define TELE_DEVICE_ATTMAG (0x1B) // Attitude and Magnetic Compass
+#define TELE_DEVICE_TILT (0x1C) // Surface Tilt Sensor
+#define TELE_DEVICE_RSV_1D (0x1D) // Reserved
+#define TELE_DEVICE_AS6X_GAIN (0x1E) // Active AS6X Gains (new mode)
+#define TELE_DEVICE_AS3X_LEGACYGAIN (0x1F) // Active AS3X Gains for legacy mode
+#define TELE_DEVICE_ESC (0x20) // Electronic Speed Control
+#define TELE_DEVICE_RSV_21 (0x21) // Reserved
+#define TELE_DEVICE_FUEL (0x22) // Fuel Flow Meter
+#define TELE_DEVICE_RSV_23 (0x23) // Reserved
+#define TELE_DEVICE_ALPHA6 (0x24) // Alpha6 Stabilizer
+#define TELE_DEVICE_RSV_25 (0x25) // Reserved
+#define TELE_DEVICE_GPS_BINARY (0x26) // GPS, binary format
+#define TELE_DEVICE_RSV_27 (0x27) // Reserved
+#define TELE_DEVICE_RSV_28 (0x28) // Reserved
+#define TELE_DEVICE_RSV_29 (0x29) // Reserved
+#define TELE_DEVICE_RSV_2A (0x2A) // Reserved
+#define TELE_DEVICE_RSV_2B (0x2B) // Reserved
+#define TELE_DEVICE_RSV_2C (0x2C) // Reserved
+#define TELE_DEVICE_RSV_2D (0x2D) // Reserved
+#define TELE_DEVICE_RSV_2E (0x2E) // Reserved
+#define TELE_DEVICE_RSV_2F (0x2F) // Reserved
+//#define DO_NOT_USE (0x30) // Internal ST sensor
+//#define DO_NOT_USE (0x32) // Internal ST sensor
+#define TELE_DEVICE_RSV_33 (0x33) // Reserved
+#define TELE_DEVICE_FP_MAH (0x34) // Flight Battery Capacity (Dual)
+#define TELE_DEVICE_RSV_35 (0x35) // Reserved
+#define TELE_DEVICE_DIGITAL_AIR (0x36) // Digital Inputs & Tank Pressure
+#define TELE_DEVICE_RSV_37 (0x37) // Reserved
+#define TELE_DEVICE_STRAIN (0x38) // Thrust/Strain Gauge
+#define TELE_DEVICE_RSV_39 (0x39) // Reserved
+#define TELE_DEVICE_LIPOMON (0x3A) // 6S Cell Monitor (LiPo taps)
+#define TELE_DEVICE_RSV_3B (0x3B) // Reserved
+#define TELE_DEVICE_RSV_3C (0x3C) // Reserved
+#define TELE_DEVICE_RSV_3D (0x3D) // Reserved
+#define TELE_DEVICE_RSV_3E (0x3E) // Reserved
+#define TELE_DEVICE_LIPOMON_14 (0x3F) // 14S Cell Monitor (LiPo taps)
+#define TELE_DEVICE_VARIO_S (0x40) // Vario
+#define TELE_DEVICE_RSV_41 (0x41) // Reserved
+#define TELE_DEVICE_SMARTBATT (0x42) // Spektrum SMART Battery (multiple structs)
+#define TELE_DEVICE_RSV_43 (0x43) // Reserved
+#define TELE_DEVICE_RSV_44 (0x44) // Reserved
+#define TELE_DEVICE_RSV_45 (0x45) // Reserved
+#define TELE_DEVICE_RSV_46 (0x46) // Reserved
+#define TELE_DEVICE_RSV_47 (0x47) // Reserved
+#define TELE_DEVICE_RSV_48 (0x48) // Reserved
+#define TELE_DEVICE_RSV_49 (0x49) // Reserved
+#define TELE_DEVICE_RSV_4A (0x4A) // Reserved
+#define TELE_DEVICE_RSV_4B (0x4B) // Reserved
+#define TELE_DEVICE_RSV_4C (0x4C) // Reserved
+#define TELE_DEVICE_RSV_4D (0x4D) // Reserved
+#define TELE_DEVICE_RSV_4E (0x4E) // Reserved
+#define TELE_DEVICE_RSV_4F (0x4F) // Reserved
+#define TELE_DEVICE_USER_16SU (0x50) // User-Defined, STRU_TELE_USER_16SU
+#define TELE_DEVICE_RSV_51 (0x51) // Reserved
+#define TELE_DEVICE_USER_16SU32U (0x52) // User-Defined, STRU_TELE_USER_16SU32U
+#define TELE_DEVICE_RSV_53 (0x53) // Reserved
+#define TELE_DEVICE_USER_16SU32S (0x54) // User-Defined, STRU_TELE_USER_16SU32S
+#define TELE_DEVICE_RSV_55 (0x55) // Reserved
+#define TELE_DEVICE_USER_16U32SU (0x56) // User-Defined, STRU_TELE_USER_16U32SU
+#define TELE_DEVICE_RSV_57 (0x57) // Reserved
+#define TELE_DEVICE_RSV_58 (0x58) // Reserved
+#define TELE_DEVICE_MULTICYLINDER (0x59) // Multi-cylinder temp sensor
+#define TELE_DEVICE_RSV_5A (0x5A) // Reserved
+#define TELE_DEVICE_RSV_5B (0x5B) // Reserved
+#define TELE_DEVICE_RSV_5C (0x5C) // Reserved
+#define TELE_DEVICE_RSV_5D (0x5D) // Reserved
+#define TELE_DEVICE_RSV_5E (0x5E) // Reserved
+#define TELE_DEVICE_RSV_5F (0x5F) // Reserved
+#define TELE_DEVICE_VSPEAK (0x60) // Reserved for V-Speak
+#define TELE_DEVICE_SMOKE_EL (0x61) // Reserved for Smoke-EL.de
+#define TELE_DEVICE_CROSSFIRE (0x62) // Reserved for Crossfire devices
+#define TELE_DEVICE_RSV_63 (0x63) // Reserved
+#define TELE_DEVICE_RSV_64 (0x64) // Reserved
+#define TELE_DEVICE_RSV_65 (0x65) // Reserved
+#define TELE_DEVICE_EXTRF (0x66) // Reserved for Generic External RF sources
+#define TELE_DEVICE_RSV_67 (0x67) // Reserved
+#define TELE_DEVICE_RSV_68 (0x68) // Reserved
+#define TELE_DEVICE_RSV_69 (0x69) // Reserved
+#define TELE_DEVICE_RSV_6A (0x6A) // Reserved
+//#define DO_NOT_USE (0x6B) // DO NOT USE!
+#define TELE_DEVICE_RSV_6C (0x6C) // Reserved
+#define TELE_DEVICE_RSV_6D (0x6D) // Reserved
+#define TELE_DEVICE_RSV_6E (0x6E) // Reserved
+#define TELE_DEVICE_RSV_6F (0x6F) // Reserved
+#define TELE_DEVICE_RSV_70 (0x70) // Reserved
+#define TELE_XRF_LINKSTATUS (0x71) // External RF Link Status
+#define TELE_DEVICE_RSV_72 (0x72) // Reserved
+#define TELE_DEVICE_RSV_73 (0x73) // Reserved
+#define TELE_DEVICE_RSV_74 (0x74) // Reserved
+#define TELE_DEVICE_RSV_75 (0x75) // Reserved
+#define TELE_DEVICE_RSV_76 (0x76) // Reserved
+#define TELE_DEVICE_RSV_77 (0x77) // Reserved
+#define TELE_DEVICE_RSV_78 (0x78) // Reserved
+#define TELE_DEVICE_RSV_79 (0x79) // Reserved
+#define TELE_DEVICE_RSV_7A (0x7A) // Reserved
+#define TELE_DEVICE_ALT_ZERO (0x7B) // Pseudo-device setting Altitude "zero"
+#define TELE_DEVICE_RTC (0x7C) // Pseudo-device giving timestamp
+#define TELE_DEVICE_RPM (0x7E) // RPM sensor
+#define TELE_DEVICE_QOS (0x7F) // RxV + flight log data
+#define TELE_DEVICE_MAX (0x7F) // Last address available
+
+#define TELE_DEVICE_SHORTRANGE (0x80) // OR this bit to indicate data is from a short-range telemetry device (e.g. TM1100)
+
+#define TELE_DEVICE_MAX_PROGRAM (0x70) // Last programmable address when using sID
+
+//////////////////////////////////////////////////////////////////////////////
+//
+// TELEMETRY
+// DEVICE-SPECIFIC STRUCTURES
+//
+//////////////////////////////////////////////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////////
+//
+// THIRD-PARTY 16-BIT DATA SIGNED/UNSIGNED
+//
+//////////////////////////////////////////////////////////////////////////////
+//
+typedef struct
+{
+ UINT8 identifier; // Source device = 0x50
+ UINT8 sID; // Secondary ID
+ INT16 sField1, // Signed 16-bit data fields
+ sField2,
+ sField3;
+ UINT16 uField1, // Unsigned 16-bit data fields
+ uField2,
+ uField3,
+ uField4;
+} STRU_TELE_USER_16SU;
+
+//////////////////////////////////////////////////////////////////////////////
+//
+// THIRD-PARTY 16-BIT SIGNED/UNSIGNED AND 32-BIT UNSIGNED
+//
+//////////////////////////////////////////////////////////////////////////////
+//
+typedef struct
+{
+ UINT8 identifier; // Source device = 0x52
+ UINT8 sID; // Secondary ID
+ INT16 sField1, // Signed 16-bit data fields
+ sField2;
+ UINT16 uField1, // Unsigned 16-bit data fields
+ uField2,
+ uField3;
+ UINT32 u32Field; // Unsigned 32-bit data field
+} STRU_TELE_USER_16SU32U;
+
+//////////////////////////////////////////////////////////////////////////////
+//
+// THIRD-PARTY 16-BIT SIGNED/UNSIGNED AND 32-BIT SIGNED
+//
+//////////////////////////////////////////////////////////////////////////////
+//
+typedef struct
+{
+ UINT8 identifier; // Source device = 0x54
+ UINT8 sID; // Secondary ID
+ INT16 sField1, // Signed 16-bit data fields
+ sField2;
+ UINT16 uField1, // Unsigned 16-bit data fields
+ uField2,
+ uField3;
+ INT32 s32Field; // Signed 32-bit data field
+} STRU_TELE_USER_16SU32S;
+
+//////////////////////////////////////////////////////////////////////////////
+//
+// THIRD-PARTY 16-BIT UNSIGNED AND 32-BIT SIGNED/UNSIGNED
+//
+//////////////////////////////////////////////////////////////////////////////
+//
+typedef struct
+{
+ UINT8 identifier; // Source device = 0x56
+ UINT8 sID; // Secondary ID
+ UINT16 uField1; // Unsigned 16-bit data field
+ INT32 s32Field; // Signed 32-bit data field
+ UINT32 u32Field1, // Unsigned 32-bit data fields
+ u32Field2;
+} STRU_TELE_USER_16U32SU;
+
+//////////////////////////////////////////////////////////////////////////////
+//
+// POWERBOX
+//
+//////////////////////////////////////////////////////////////////////////////
+//
+typedef struct
+{
+ UINT8 identifier; // Source device = 0x0A
+ UINT8 sID; // Secondary ID
+ UINT16 volt1; // Volts, 0.01v
+ UINT16 volt2; // Volts, 0.01v
+ UINT16 capacity1; // mAh, 1mAh
+ UINT16 capacity2; // mAh, 1mAh
+ UINT16 spare16_1;
+ UINT16 spare16_2;
+ UINT8 spare;
+ UINT8 alarms; // Alarm bitmask (see below)
+} STRU_TELE_POWERBOX;
+
+#define TELE_PBOX_ALARM_VOLTAGE_1 (0x01)
+#define TELE_PBOX_ALARM_VOLTAGE_2 (0x02)
+#define TELE_PBOX_ALARM_CAPACITY_1 (0x04)
+#define TELE_PBOX_ALARM_CAPACITY_2 (0x08)
+//#define TELE_PBOX_ALARM_RPM (0x10)
+//#define TELE_PBOX_ALARM_TEMPERATURE (0x20)
+#define TELE_PBOX_ALARM_RESERVED_1 (0x40)
+#define TELE_PBOX_ALARM_RESERVED_2 (0x80)
+
+//////////////////////////////////////////////////////////////////////////////
+//
+// VOLTAGE
+//
+//////////////////////////////////////////////////////////////////////////////
+//
+typedef struct
+{
+ UINT8 identifier; // Source device = 0x01
+ UINT8 sID; // Secondary ID
+ UINT16 volts; // 0.01V increments
+} STRU_TELE_HV;
+
+//////////////////////////////////////////////////////////////////////////////
+//
+// TEMPERATURE
+//
+//////////////////////////////////////////////////////////////////////////////
+//
+typedef struct
+{
+ UINT8 identifier; // Source device = 0x02
+ UINT8 sID; // Secondary ID
+ INT16 temperature; // Temperature in degrees Fahrenheit
+} STRU_TELE_TEMP;
+
+//////////////////////////////////////////////////////////////////////////////
+//
+// RX CAPACITY METER
+//
+//////////////////////////////////////////////////////////////////////////////
+//
+typedef struct
+{
+ UINT8 identifier; // Source device = 0x18
+ UINT8 sID; // Secondary ID
+ INT16 current_A; // Instantaneous current, 0.01A (0-328.7A) 7FFF-> no data
+ UINT16 chargeUsed_A; // Integrated mAh used, 0.1mAh (0-3276.6mAh)
+ UINT16 volts_A; // Volts, 0.01V increments (0-16.00V)
+ INT16 current_B; // Instantaneous current, 0.01A (0-328.7A) 7FFF-> no data/sensor B
+ UINT16 chargeUsed_B; // Integrated mAh used, 0.1mAh (0-3276.6mAh)
+ UINT16 volts_B; // Volts, 0.01V increments (0-16.00V)
+ UINT8 alerts, // Bit mapped alert conditions (see below)
+ highCharge; // High nybble is extra bits for chargeUsed_B, Low is for chargeUsed_A
+} STRU_TELE_RX_MAH;
+
+#define RXMAH_PS_ALERT_NONE (0) // No alarms
+#define RXMAH_PS_ALERT_RF_INT (1 << 0) // A or internal Remote failure
+#define RXMAH_PS_ALERT_RF_ANT1 (1 << 1) // B remote power fault
+#define RXMAH_PS_ALERT_RF_ANT2 (1 << 2) // L remote power fault
+#define RXMAH_PS_ALERT_RF_ANT3 (1 << 3) // R remote power fault
+#define RXMAH_PS_ALERT_OVERVOLT_A (1 << 4) // Battery A is over voltage
+#define RXMAH_PS_ALERT_OVERVOLT_B (1 << 5) // Battery A is over voltage
+#define RXMAH_PS_ALERT_RFU1 (1 << 6)
+#define RXMAH_PS_ALERT_RFU2 (1 << 7)
+
+//////////////////////////////////////////////////////////////////////////////
+//
+// HIGH-CURRENT
+//
+//////////////////////////////////////////////////////////////////////////////
+//
+typedef struct
+{
+ UINT8 identifier; // Source device = 0x03
+ UINT8 sID; // Secondary ID
+ INT16 current, // Range: +/- 150A Resolution: 300A / 2048 = 0.196791 A/count
+ dummy; // TBD
+} STRU_TELE_IHIGH;
+
+#define IHIGH_RESOLUTION_FACTOR ((FP32)(0.196791))
+
+//////////////////////////////////////////////////////////////////////////////
+//
+// SIMPLE VARIO
+//
+//////////////////////////////////////////////////////////////////////////////
+//
+typedef struct
+{
+ UINT8 identifier; // Source device = 0x40
+ UINT8 sID; // Secondary ID
+ INT16 altitude; // .1m increments
+ INT16 delta_0250ms, // change in altitude last 250ms, 0.1m/s increments
+ delta_0500ms, // change in altitude last 500ms, 0.1m/s increments
+ delta_1000ms, // change in altitude last 1.0 seconds, 0.1m/s increments
+ delta_1500ms, // change in altitude last 1.5 seconds, 0.1m/s increments
+ delta_2000ms, // change in altitude last 2.0 seconds, 0.1m/s increments
+ delta_3000ms; // change in altitude last 3.0 seconds, 0.1m/s increments
+} STRU_TELE_VARIO_S;
+
+//////////////////////////////////////////////////////////////////////////////
+//
+// ALTIMETER
+//
+//////////////////////////////////////////////////////////////////////////////
+//
+typedef struct
+{
+ UINT8 identifier;
+ UINT8 sID; // Secondary ID
+ INT16 altitude; // .1m increments
+ INT16 maxAltitude; // .1m increments
+} STRU_TELE_ALT; // Eagle Tree Sensor
+
+//////////////////////////////////////////////////////////////////////////////
+//
+// AIRSPEED
+//
+//////////////////////////////////////////////////////////////////////////////
+//
+typedef struct
+{
+ UINT8 identifier;
+ UINT8 sID; // Secondary ID
+ UINT16 airspeed; // 1 km/h increments
+ UINT16 maxAirspeed; // 1 km/h increments
+} STRU_TELE_SPEED; // Eagle Tree Sensor
+
+//////////////////////////////////////////////////////////////////////////////
+//
+// LAP TIMER
+//
+//////////////////////////////////////////////////////////////////////////////
+//
+typedef struct
+{
+ UINT8 identifier;
+ UINT8 sID; // Secondary ID
+ UINT8 lapNumber; // Lap last finished
+ UINT8 gateNumber; // Last gate passed
+ UINT32 lastLapTime; // Time of lap in 1ms increments (NOT duration)
+ UINT32 gateTime; // Duration between last 2 gates
+ UINT8 unused[4];
+} STRU_TELE_LAPTIMER;
+
+//////////////////////////////////////////////////////////////////////////////
+//
+// TEXT GENERATOR
+//
+//////////////////////////////////////////////////////////////////////////////
+//
+typedef struct
+{
+ UINT8 identifier;
+ UINT8 sID; // Secondary ID
+ UINT8 lineNumber; // Line number to display (0 = title, 1-8 for general, 254 = Refresh backlight, 255 = Erase all text on screen)
+ char text[13]; // 0-terminated text when < 13 chars
+} STRU_TELE_TEXTGEN;
+
+//////////////////////////////////////////////////////////////////////////////
+//
+// VIDEO TRANSMITTER (VTX)
+//
+//////////////////////////////////////////////////////////////////////////////
+//
+// VTX spec subject to change. Refer to Spektrum VTX Interfacing document for latest info
+//
+typedef struct
+{
+ UINT8 identifier;
+ UINT8 sID; // Secondary ID
+ UINT8 band; // VTX Band (0 = Fatshark, 1 = Raceband, 2 = E, 3 = B, 4 = A, 5-7 = Reserved)
+ UINT8 channel; // VTX Channel (0-7)
+ UINT8 pit; // Pit/Race mode (0 = Race, 1 = Pit). Race = (normal operating) mode. Pit = (reduced power) mode. When PIT is set, it overrides all other power settings.
+ UINT8 power; // VTX Power (0 = Off, 1 = 1mw to 14mW, 2 = 15mW to 25mW, 3 = 26mW to 99mW, 4 = 100mW to 299mW, 5 = 300mW to 600mW, 6 = 601mW+, 7 = manual control)
+ UINT16 powerDec; // VTX Power as a decimal 1mw/unit
+ UINT8 region; // Region (0 = USA, 1 = EU, 0xFF = Not Provided)
+ UINT8 unused[7]; // reserved
+} STRU_TELE_VTX;
+
+//////////////////////////////////////////////////////////////////////////////
+//
+// ESC
+//
+//////////////////////////////////////////////////////////////////////////////
+//
+// Uses big-endian byte order
+//
+typedef struct
+{
+ UINT8 identifier; // Source device = 0x20
+ UINT8 sID; // Secondary ID
+ UINT16 RPM; // Electrical RPM, 10RPM (0-655340 RPM) 0xFFFF --> "No data"
+ UINT16 voltsInput; // Volts, 0.01v (0-655.34V) 0xFFFF --> "No data"
+ UINT16 tempFET; // Temperature, 0.1C (0-6553.4C) 0xFFFF --> "No data"
+ UINT16 currentMotor; // Current, 10mA (0-655.34A) 0xFFFF --> "No data"
+ UINT16 tempBEC; // Temperature, 0.1C (0-6553.4C) 0xFFFF --> "No data"
+ UINT8 currentBEC; // BEC Current, 100mA (0-25.4A) 0xFF ----> "No data"
+ UINT8 voltsBEC; // BEC Volts, 0.05V (0-12.70V) 0xFF ----> "No data"
+ UINT8 throttle; // 0.5% (0-100%) 0xFF ----> "No data"
+ UINT8 powerOut; // Power Output, 0.5% (0-127%) 0xFF ----> "No data"
+} STRU_TELE_ESC;
+
+//////////////////////////////////////////////////////////////////////////////
+//
+// (Liquid) Fuel Flow/Capacity (Two Tanks/Engines)
+//
+//////////////////////////////////////////////////////////////////////////////
+//
+typedef struct
+{
+ UINT8 identifier; // Source device = 0x22
+ UINT8 sID; // Secondary ID
+ UINT16 fuelConsumed_A; // Integrated fuel consumption, 0.1mL
+ UINT16 flowRate_A; // Instantaneous consumption, 0.01mL/min
+ UINT16 temp_A; // Temperature, 0.1C (0-655.34C)
+ UINT16 fuelConsumed_B; // Integrated fuel consumption, 0.1mL
+ UINT16 flowRate_B; // Instantaneous consumption, 0.01mL/min
+ UINT16 temp_B; // Temperature, 0.1C (0-655.34C)
+ UINT16 spare; // Not used
+} STRU_TELE_FUEL;
+
+//////////////////////////////////////////////////////////////////////////////
+//
+// Battery Current/Capacity (Flight Pack Capacity)
+//
+//////////////////////////////////////////////////////////////////////////////
+//
+// AK 2013-11-19 make struct align with 0x03 device
+//
+typedef struct
+{
+ UINT8 identifier; // Source device = 0x34
+ UINT8 sID; // Secondary ID
+ INT16 current_A; // Instantaneous current, 0.1A (0-3276.6A)
+ INT16 chargeUsed_A; // Integrated mAh used, 1mAh (0-32.766Ah)
+ UINT16 temp_A; // Temperature, 0.1C (0-150C, 0x7FFF indicates not populated)
+ INT16 current_B; // Instantaneous current, 0.1A (0-3276.6A)
+ INT16 chargeUsed_B; // Integrated mAh used, 1mAh (0-32.766Ah)
+ UINT16 temp_B; // Temperature, 0.1C (0-150C, 0x7FFF indicates not populated)
+ UINT16 spare; // Not used
+} STRU_TELE_FP_MAH;
+
+//////////////////////////////////////////////////////////////////////////////
+//
+// Digital Input Status (Retract Status) and Tank Pressure
+//
+//////////////////////////////////////////////////////////////////////////////
+//
+typedef struct
+{
+ UINT8 identifier; // Source device = TELE_DEVICE_DIGITAL_AIR
+ UINT8 sID; // Secondary ID
+ UINT16 digital; // Digital inputs (bit per input)
+ UINT16 spare1;
+ UINT16 pressure[4]; // Tank pressure, 0.1PSI (0-6553.4PSI), 0xFFFF = Not Installed
+ UINT16 spare2;
+} STRU_TELE_DIGITAL_AIR;
+
+//////////////////////////////////////////////////////////////////////////////
+//
+// Thrust/Strain Gauge
+//
+//////////////////////////////////////////////////////////////////////////////
+//
+typedef struct
+{
+ UINT8 identifier; // Source device = 0x38
+ UINT8 sID; // Secondary ID
+ UINT16 strain_A, // Strain sensor A
+ strain_B, // Strain sensor B
+ strain_C, // Strain sensor D
+ strain_D; // Strain sensor C
+} STRU_TELE_STRAIN;
+
+//////////////////////////////////////////////////////////////////////////////
+//
+// 6S LiPo Cell Monitor
+//
+//////////////////////////////////////////////////////////////////////////////
+//
+typedef struct
+{
+ UINT8 identifier; // Source device = 0x3A
+ UINT8 sID; // Secondary ID
+ UINT16 cell[6]; // Voltage across cell 1, .01V steps
+ // 0x7FFF --> cell not present
+ UINT16 temp; // Temperature, 0.1C (0-655.34C)
+} STRU_TELE_LIPOMON;
+
+//////////////////////////////////////////////////////////////////////////////
+//
+// 14S LiPo Cell Monitor
+//
+//////////////////////////////////////////////////////////////////////////////
+//
+typedef struct
+{
+ UINT8 identifier; // Source device = 0x3F
+ UINT8 sID; // Secondary ID
+ UINT8 cell[14]; // Voltage across cell 1, .01V steps, excess of 2.56V
+ // (ie, 3.00V would report 300-256 = 44)
+ // 0xFF --> cell not present
+} STRU_TELE_LIPOMON_14;
+
+//////////////////////////////////////////////////////////////////////////////
+//
+// Smart Battery
+//
+//////////////////////////////////////////////////////////////////////////////
+//
+// Uses little-endian byte order for all multi-byte fields
+//
+typedef struct
+{
+ UINT8 identifier; // Source device = 0x42
+ UINT8 sID; // Secondary ID
+ UINT8 typeChannel; // Upper nybble = Message type; Lower nybble = Battery number (0 or 1)
+ UINT8 msgData[13]; // Message-specific data, determined by upper nybble of typeChannel (see defs below)
+} STRU_SMARTBATT_HEADER;
+
+#define SMARTBATT_MSG_TYPE_MASK_BATTNUMBER (0x0F)
+#define SMARTBATT_MSG_TYPE_MASK_MSGTYPE (0xF0)
+
+#define SMARTBATT_MSG_TYPE_REALTIME (0x00)
+#define SMARTBATT_MSG_TYPE_CELLS_1_6 (0x10)
+#define SMARTBATT_MSG_TYPE_CELLS_7_12 (0x20)
+#define SMARTBATT_MSG_TYPE_CELLS_13_18 (0x30)
+#define SMARTBATT_MSG_TYPE_ID (0x80)
+#define SMARTBATT_MSG_TYPE_LIMITS (0x90)
+
+//...........................................................................
+// Real-time battery data when current sense is available
+typedef struct
+{
+ UINT8 identifier; // Source device = 0x42
+ UINT8 sID; // Secondary ID
+ UINT8 typeChannel; // Msg type = SMARTBATT_MSG_TYPE_REALTIME | Battery number (0 or 1)
+ INT8 temperature_C; // Temperature in degrees C, 1 degree increments (-128 = unavailable)
+ UINT32 dischargeCurrent_mA; // Amount of current being drawn from battery, in mA steps (0xFFFFFFFF = unavailable)
+ UINT16 batteryCapacityUsage_mAh; // Approximate battery capacity usage, in mAh (0xFFFF = unavailable)
+ UINT16 minCellVoltage_mV; // Minimum cell voltage of pack, in mV
+ UINT16 maxCellVoltage_mV; // Maximum cell voltage of pack, in mV
+ UINT8 rfu[2];
+} STRU_SMARTBATT_REALTIME;
+
+//...........................................................................
+// Real-time cell voltage
+typedef struct
+{
+ UINT8 identifier; // Source device = 0x42
+ UINT8 sID; // Secondary ID
+ UINT8 typeChannel; // Msg type = SMARTBATT_MSG_TYPE_CELLS_X_Y | Battery number (0 or 1)
+ INT8 temperature_C; // Temperature in degrees C, 1 degree increments (-128 = unavailable)
+ UINT16 cellVoltage_mV[6]; // Cell voltage of first 6 cells, in mV (0xFFFF = unavailable)
+} STRU_SMARTBATT_CELLS;
+
+//...........................................................................
+// Smart Battery ID and general info
+typedef struct
+{
+ UINT8 identifier; // Source device = 0x42
+ UINT8 sID; // Secondary ID
+ UINT8 typeChannel; // Msg type = SMARTBATT_MSG_TYPE_ID | Battery number (0 or 1)
+ UINT8 chemistry; // 0:LiHv, 1:LiPo, 2:LiIon, 3:LiFe, 4:Pb, 5:Ni-MH/Cd
+ UINT8 numOfCells; // Number of cells in the battery
+ UINT8 manufacturer; // 0:BattGo
+ UINT16 cycles; // Number of charge/discharge cycles recorded (0 = unavailable)
+ UINT8 uniqueID[8]; // Unique battery ID, manufacturer-specific
+ // 0: [0] = lower (first) byte of "Customer ID"
+ // [1-3] = lower 3 bytes of "Special Mark of Battery"
+ // [4-7] = 4-byte "Manufacturing Date"
+} STRU_SMARTBATT_ID;
+
+//...........................................................................
+// Smart Battery Limits
+typedef struct
+{
+ UINT8 identifier; // Source device = 0x42
+ UINT8 sID; // Secondary ID
+ UINT8 typeChannel; // Msg type = SMARTBATT_MSG_TYPE_LIMITS | Battery number (0 or 1)
+ UINT8 rfu;
+ UINT16 fullCapacity_mAh; // Fully charged battery capacity, in mAh
+ UINT16 dischargeCurrentRating; // Rated discharge current, in 0.1C
+ UINT16 overDischarge_mV; // Limit below which battery is likely damaged, in mV
+ UINT16 zeroCapacity_mV; // Voltage at which LVC protection should activate, in mV
+ UINT16 fullyCharged_mV; // Voltage reading expected when fully charged, in mV
+ INT8 minWorkingTemp; // Minimum working temperature in degrees C, 1 degree steps
+ INT8 maxWorkingTemp; // Maximum working temperature in degrees C, 1 degree steps
+} STRU_SMARTBATT_LIMITS;
+
+//////////////////////////////////////////////////////////////////////////////
+//
+// ACCELEROMETER
+//
+//////////////////////////////////////////////////////////////////////////////
+//
+typedef struct
+{
+ UINT8 identifier; // Source device = 0x14
+ UINT8 sID; // Secondary ID
+ INT16 GForceX; // force is reported as .01G increments
+ INT16 GForceY; // Range = +/-4000 (+/- 40G) in Pro model
+ INT16 GForceZ; // Range = +/-800 (+/- 8G) in Standard model
+ INT16 maxGForceX; // abs(max G X-axis) FORE/AFT
+ INT16 maxGForceY; // abs (max G Y-axis) LEFT/RIGHT
+ INT16 maxGForceZ; // max G Z-axis WING SPAR LOAD
+ INT16 minGForceZ; // min G Z-axis WING SPAR LOAD
+} STRU_TELE_G_METER;
+
+//////////////////////////////////////////////////////////////////////////////
+//
+// SURFACE TILT (ATTITUDE) SENSOR
+//
+//////////////////////////////////////////////////////////////////////////////
+//
+typedef struct
+{
+ UINT8 identifier; // Source device = 0x1C TELE_DEVICE_TILT
+ UINT8 sID; // Secondary ID
+ INT16 attQuatX; // Quaternion representing attitude using RHR. X component in Q14.
+ INT16 attQuatY; // Y component in Q14.
+ INT16 attQuatZ; // Z component in Q14.
+ INT16 attQuatW; // W component in Q14.
+ UINT16 spare[3];
+} STRU_TELE_TILT;
+
+//////////////////////////////////////////////////////////////////////////////
+//
+// TURBINE
+//
+//////////////////////////////////////////////////////////////////////////////
+//
+typedef struct
+{
+ UINT8 identifier; // Source device = 0x15
+ UINT8 sID; // Secondary ID
+ UINT8 status; // Table below
+ UINT8 throttle; // (BCD) xx Percent
+ UINT16 packVoltage; // (BCD) xx.yy
+ UINT16 pumpVoltage; // (BCD) xx.yy
+ UINT32 RPM; // (BCD)
+ UINT16 EGT; // (BCD) Temperature, Celsius
+ UINT8 offCondition; // Table below
+ UINT8 spare;
+} STRU_TELE_JETCAT;
+
+enum JETCAT_ECU_TURBINE_STATE { // ECU Status definitions
+ JETCAT_ECU_STATE_OFF = 0x00,
+ JETCAT_ECU_STATE_WAIT_for_RPM = 0x01, // (Stby/Start)
+ JETCAT_ECU_STATE_Ignite = 0x02,
+ JETCAT_ECU_STATE_Accelerate = 0x03,
+ JETCAT_ECU_STATE_Stabilise = 0x04,
+ JETCAT_ECU_STATE_Learn_HI = 0x05,
+ JETCAT_ECU_STATE_Learn_LO = 0x06,
+ JETCAT_ECU_STATE_UNDEFINED = 0x07,
+ JETCAT_ECU_STATE_Slow_Down = 0x08,
+ JETCAT_ECU_STATE_Manual = 0x09,
+ JETCAT_ECU_STATE_AutoOff = 0x10,
+ JETCAT_ECU_STATE_Run = 0x11, // (reg.)
+ JETCAT_ECU_STATE_Accleleration_delay = 0x12,
+ JETCAT_ECU_STATE_SpeedReg = 0x13, // (Speed Ctrl)
+ JETCAT_ECU_STATE_Two_Shaft_Regulate = 0x14, // (only for secondary shaft)
+ JETCAT_ECU_STATE_PreHeat1 = 0x15,
+ JETCAT_ECU_STATE_PreHeat2 = 0x16,
+ JETCAT_ECU_STATE_MainFStart = 0x17,
+ JETCAT_ECU_STATE_NotUsed = 0x18,
+ JETCAT_ECU_STATE_KeroFullOn = 0x19,
+ // undefined states 0x1A-0x1F
+ EVOJET_ECU_STATE_off = 0x20,
+ EVOJET_ECU_STATE_ignt = 0x21,
+ EVOJET_ECU_STATE_acce = 0x22,
+ EVOJET_ECU_STATE_run = 0x23,
+ EVOJET_ECU_STATE_cal = 0x24,
+ EVOJET_ECU_STATE_cool = 0x25,
+ EVOJET_ECU_STATE_fire = 0x26,
+ EVOJET_ECU_STATE_glow = 0x27,
+ EVOJET_ECU_STATE_heat = 0x28,
+ EVOJET_ECU_STATE_idle = 0x29,
+ EVOJET_ECU_STATE_lock = 0x2A,
+ EVOJET_ECU_STATE_rel = 0x2B,
+ EVOJET_ECU_STATE_spin = 0x2C,
+ EVOJET_ECU_STATE_stop = 0x2D,
+ // undefined states 0x2E-0x2F
+ HORNET_ECU_STATE_OFF = 0x30,
+ HORNET_ECU_STATE_SLOWDOWN = 0x31,
+ HORNET_ECU_STATE_COOL_DOWN = 0x32,
+ HORNET_ECU_STATE_AUTO = 0x33,
+ HORNET_ECU_STATE_AUTO_HC = 0x34,
+ HORNET_ECU_STATE_BURNER_ON = 0x35,
+ HORNET_ECU_STATE_CAL_IDLE = 0x36,
+ HORNET_ECU_STATE_CALIBRATE = 0x37,
+ HORNET_ECU_STATE_DEV_DELAY = 0x38,
+ HORNET_ECU_STATE_EMERGENCY = 0x39,
+ HORNET_ECU_STATE_FUEL_HEAT = 0x3A,
+ HORNET_ECU_STATE_FUEL_IGNITE = 0x3B,
+ HORNET_ECU_STATE_GO_IDLE = 0x3C,
+ HORNET_ECU_STATE_PROP_IGNITE = 0x3D,
+ HORNET_ECU_STATE_RAMP_DELAY = 0x3E,
+ HORNET_ECU_STATE_RAMP_UP = 0x3F,
+ HORNET_ECU_STATE_STANDBY = 0x40,
+ HORNET_ECU_STATE_STEADY = 0x41,
+ HORNET_ECU_STATE_WAIT_ACC = 0x42,
+ HORNET_ECU_STATE_ERROR = 0x43,
+ // undefined states 0x44-0x4F
+ XICOY_ECU_STATE_Temp_High = 0x50,
+ XICOY_ECU_STATE_Trim_Low = 0x51,
+ XICOY_ECU_STATE_Set_Idle = 0x52,
+ XICOY_ECU_STATE_Ready = 0x53,
+ XICOY_ECU_STATE_Ignition = 0x54,
+ XICOY_ECU_STATE_Fuel_Ramp = 0x55,
+ XICOY_ECU_STATE_Glow_Test = 0x56,
+ XICOY_ECU_STATE_Running = 0x57,
+ XICOY_ECU_STATE_Stop = 0x58,
+ XICOY_ECU_STATE_Flameout = 0x59,
+ XICOY_ECU_STATE_Speed_Low = 0x5A,
+ XICOY_ECU_STATE_Cooling = 0x5B,
+ XICOY_ECU_STATE_Igniter_Bad = 0x5C,
+ XICOY_ECU_STATE_Starter_F = 0x5D,
+ XICOY_ECU_STATE_Weak_Fuel = 0x5E,
+ XICOY_ECU_STATE_Start_On = 0x5F,
+ XICOY_ECU_STATE_Pre_Heat = 0x60,
+ XICOY_ECU_STATE_Battery = 0x61,
+ XICOY_ECU_STATE_Time_Out = 0x62,
+ XICOY_ECU_STATE_Overload = 0x63,
+ XICOY_ECU_STATE_Igniter_Fail = 0x64,
+ XICOY_ECU_STATE_Burner_On = 0x65,
+ XICOY_ECU_STATE_Starting = 0x66,
+ XICOY_ECU_STATE_SwitchOver = 0x67,
+ XICOY_ECU_STATE_Cal_Pump = 0x68,
+ XICOY_ECU_STATE_Pump_Limit = 0x69,
+ XICOY_ECU_STATE_No_Engine = 0x6A,
+ XICOY_ECU_STATE_Pwr_Boost = 0x6B,
+ XICOY_ECU_STATE_Run_Idle = 0x6C,
+ XICOY_ECU_STATE_Run_Max = 0x6D,
+ // undefined states 0x6e-0x73
+ JETCENT_ECU_STATE_STOP = 0x74,
+ JETCENT_ECU_STATE_GLOW_TEST = 0x75,
+ JETCENT_ECU_STATE_STARTER_TEST = 0x76,
+ JETCENT_ECU_STATE_PRIME_FUEL = 0x77,
+ JETCENT_ECU_STATE_PRIME_BURNER = 0x78,
+ JETCENT_ECU_STATE_MAN_COOL = 0x79,
+ JETCENT_ECU_STATE_AUTO_COOL = 0x7A,
+ JETCENT_ECU_STATE_IGN_HEAT = 0x7B,
+ JETCENT_ECU_STATE_IGNITION = 0x7C,
+ JETCENT_ECU_STATE_PREHEAT = 0x7D,
+ JETCENT_ECU_STATE_SWITCHOVER = 0x7E,
+ JETCENT_ECU_STATE_TO_IDLE = 0x7F,
+ JETCENT_ECU_STATE_RUNNING = 0x80,
+ JETCENT_ECU_STATE_STOP_ERROR = 0x81,
+ // undefined states 0x82-0x8F
+ SWIWIN_ECU_STATE_STOP = 0x90,
+ SWIWIN_ECU_STATE_READY = 0x91,
+ SWIWIN_ECU_STATE_IGNITION = 0x92,
+ SWIWIN_ECU_STATE_PREHEAT = 0x93,
+ SWIWIN_ECU_STATE_FUEL_RAMP = 0x94,
+ SWIWIN_ECU_STATE_RUNNING = 0x95,
+ SWIWIN_ECU_STATE_COOLING = 0x96,
+ SWIWIN_ECU_STATE_RESTART_SWOVER = 0x97,
+ SWIWIN_ECU_STATE_NOTUSED = 0x98,
+ // undefined states 0x99-0x9F
+
+ TURBINE_ECU_MAX_STATE = 0x9F
+};
+
+enum JETCAT_ECU_OFF_CONDITIONS { // ECU off conditions. Valid only when the ECUStatus = JETCAT_ECU_STATE_OFF
+ JETCAT_ECU_OFF_No_Off_Condition_defined = 0,
+ JETCAT_ECU_OFF_Shut_down_via_RC,
+ JETCAT_ECU_OFF_Overtemperature,
+ JETCAT_ECU_OFF_Ignition_timeout,
+ JETCAT_ECU_OFF_Acceleration_time_out,
+ JETCAT_ECU_OFF_Acceleration_too_slow,
+ JETCAT_ECU_OFF_Over_RPM,
+ JETCAT_ECU_OFF_Low_Rpm_Off,
+ JETCAT_ECU_OFF_Low_Battery,
+ JETCAT_ECU_OFF_Auto_Off,
+ JETCAT_ECU_OFF_Low_temperature_Off,
+ JETCAT_ECU_OFF_Hi_Temp_Off,
+ JETCAT_ECU_OFF_Glow_Plug_defective,
+ JETCAT_ECU_OFF_Watch_Dog_Timer,
+ JETCAT_ECU_OFF_Fail_Safe_Off,
+ JETCAT_ECU_OFF_Manual_Off, // (via GSU)
+ JETCAT_ECU_OFF_Power_fail, // (Battery fail)
+ JETCAT_ECU_OFF_Temp_Sensor_fail, // (only during startup)
+ JETCAT_ECU_OFF_Fuel_fail,
+ JETCAT_ECU_OFF_Prop_fail,
+ JETCAT_ECU_OFF_2nd_Engine_fail,
+ JETCAT_ECU_OFF_2nd_Engine_Diff_Too_High,
+ JETCAT_ECU_OFF_2nd_Engine_No_Comm,
+ JETCAT_ECU_MAX_OFF_COND,
+ // Jet Central
+ JETCENT_ECU_OFF_No_Off_Condition_defined = 24, // ECU off conditions. Valid only when the ECUStatus = JETCENT_ECU_STATE_STOP or JETCENT_ECU_STATE_STOP_ERROR or JETCENT_ECU_STATE_RUNNING
+ JETCENT_ECU_OFF_IGNITION_ERROR,
+ JETCENT_ECU_OFF_PREHEAT_ERROR,
+ JETCENT_ECU_OFF_SWITCHOVER_ERROR,
+ JETCENT_ECU_OFF_STARTER_MOTOR_ERROR,
+ JETCENT_ECU_OFF_TO_IDLE_ERROR,
+ JETCENT_ECU_OFF_ACCELERATION_ERROR,
+ JETCENT_ECU_OFF_IGNITER_BAD,
+ JETCENT_ECU_OFF_MIN_PUMP_OK,
+ JETCENT_ECU_OFF_MAX_PUMP_OK,
+ JETCENT_ECU_OFF_LOW_RX_BATTERY,
+ JETCENT_ECU_OFF_LOW_ECU_BATTERY,
+ JETCENT_ECU_OFF_NO_RX,
+ JETCENT_ECU_OFF_TRIM_DOWN,
+ JETCENT_ECU_OFF_TRIM_UP,
+ JETCENT_ECU_OFF_FAILSAFE,
+ JETCENT_ECU_OFF_FULL,
+ JETCENT_ECU_OFF_RX_SETUP_ERROR,
+ JETCENT_ECU_OFF_TEMP_SENSOR_ERROR,
+ JETCENT_ECU_OFF_COM_TURBINE_ERROR,
+ JETCENT_ECU_OFF_MAX_TEMP,
+ JETCENT_ECU_OFF_MAX_AMPS,
+ JETCENT_ECU_OFF_LOW_RPM,
+ JETCENT_ECU_OFF_ERROR_RPM_SENSOR,
+ JETCENT_ECU_OFF_MAX_PUMP,
+ JETCENT_ECU_MAX_OFF_COND
+};
+
+typedef struct
+{
+ UINT8 identifier; // Source device = 0x19
+ UINT8 sID; // Secondary ID
+ UINT16 FuelFlowRateMLMin; // (BCD) mL per Minute
+ UINT32 RestFuelVolumeInTankML; // (BCD) mL remaining in tank
+ UINT8 ECUbatteryPercent; // (BCD) % battery pack capacity remaining
+ // 7 bytes left
+} STRU_TELE_JETCAT2;
+
+//////////////////////////////////////////////////////////////////////////////
+//
+// GPS
+// Packed-BCD Type
+//
+//////////////////////////////////////////////////////////////////////////////
+//
+typedef struct
+{
+ UINT8 identifier; // Source device = 0x16
+ UINT8 sID; // Secondary ID
+ UINT16 altitudeLow; // BCD, meters, format 3.1 (Low order of altitude)
+ UINT32 latitude; // BCD, format 4.4, Degrees * 100 + minutes, less than 100 degrees
+ UINT32 longitude; // BCD, format 4.4 , Degrees * 100 + minutes, flag indicates > 99 degrees
+ UINT16 course; // BCD, 3.1
+ UINT8 HDOP; // BCD, format 1.1
+ UINT8 GPSflags; // see definitions below
+} STRU_TELE_GPS_LOC;
+
+typedef struct
+{
+ UINT8 identifier; // Source device = 0x17
+ UINT8 sID; // Secondary ID
+ UINT16 speed; // BCD, knots, format 3.1
+ UINT32 UTC; // BCD, format HH:MM:SS.S, format 6.1
+ UINT8 numSats; // BCD, 0-99
+ UINT8 altitudeHigh; // BCD, meters, format 2.0 (High order of altitude)
+} STRU_TELE_GPS_STAT;
+
+// GPS flags definitions:
+#define GPS_INFO_FLAGS_IS_NORTH_BIT (0)
+#define GPS_INFO_FLAGS_IS_NORTH (1 << GPS_INFO_FLAGS_IS_NORTH_BIT)
+#define GPS_INFO_FLAGS_IS_EAST_BIT (1)
+#define GPS_INFO_FLAGS_IS_EAST (1 << GPS_INFO_FLAGS_IS_EAST_BIT)
+#define GPS_INFO_FLAGS_LONGITUDE_GREATER_99_BIT (2)
+#define GPS_INFO_FLAGS_LONGITUDE_GREATER_99 (1 << GPS_INFO_FLAGS_LONGITUDE_GREATER_99_BIT)
+#define GPS_INFO_FLAGS_GPS_FIX_VALID_BIT (3)
+#define GPS_INFO_FLAGS_GPS_FIX_VALID (1 << GPS_INFO_FLAGS_GPS_FIX_VALID_BIT)
+#define GPS_INFO_FLAGS_GPS_DATA_RECEIVED_BIT (4)
+#define GPS_INFO_FLAGS_GPS_DATA_RECEIVED (1 << GPS_INFO_FLAGS_GPS_DATA_RECEIVED_BIT)
+#define GPS_INFO_FLAGS_3D_FIX_BIT (5)
+#define GPS_INFO_FLAGS_3D_FIX (1 << GPS_INFO_FLAGS_3D_FIX_BIT)
+#define GPS_INFO_FLAGS_NEGATIVE_ALT_BIT (7)
+#define GPS_INFO_FLAGS_NEGATIVE_ALT (1 << GPS_INFO_FLAGS_NEGATIVE_ALT_BIT)
+
+//////////////////////////////////////////////////////////////////////////////
+//
+// GPS
+// Binary Type
+//
+// NOTE: Data resolution for all fields matches Crossfire EXCEPT speed.
+//
+//////////////////////////////////////////////////////////////////////////////
+//
+typedef struct
+{
+ UINT8 identifier; // Source device = 0x26
+ UINT8 sID; // Secondary ID
+ UINT16 altitude; // m, 1000m offset
+ INT32 latitude; // degree / 10,000,000
+ INT32 longitude; // degree / 10,000,000
+ UINT16 heading; // degree / 10
+ UINT8 groundSpeed; // km/h
+ UINT8 numSats; // count
+} STRU_TELE_GPS_BINARY;
+
+//////////////////////////////////////////////////////////////////////////////
+//
+// AS3X Legacy Gain Report
+//
+//////////////////////////////////////////////////////////////////////////////
+//
+typedef struct
+{
+ UINT8 identifier; // Source device = TELE_DEVICE_AS3X_LEGACYGAIN
+ UINT8 sID; // Secondary ID
+ UINT8 gainRoll; // Configured normal gains per axis
+ UINT8 gainPitch;
+ UINT8 gainYaw;
+ UINT8 headRoll; // Configured heading hold gains per axis
+ UINT8 headPitch;
+ UINT8 headYaw;
+ UINT8 activeRoll; // Active gains per axis (as affected by FM channel)
+ UINT8 activePitch;
+ UINT8 activeYaw;
+ UINT8 flightMode; // bit 7 1 --> FM present in bits 0,1 except 0xFF --> not present
+ UINT8 unused[4];
+} STRU_TELE_AS3X_LEGACY;
+
+//////////////////////////////////////////////////////////////////////////////
+//
+// AS6X Gain Report (AS3X Legacy + more fields)
+//
+//////////////////////////////////////////////////////////////////////////////
+//
+typedef struct
+{
+ UINT8 identifier; // Source device = TELE_DEVICE_AS6X_GAIN
+ UINT8 sID; // Secondary ID
+ UINT8 gainRoll; // Configured normal gains per axis
+ UINT8 gainPitch;
+ UINT8 gainYaw;
+ UINT8 headRoll; // Configured heading hold gains per axis
+ UINT8 headPitch;
+ UINT8 headYaw;
+ UINT8 activeRoll; // Active gains per axis (as affected by FM channel)
+ UINT8 activePitch;
+ UINT8 activeYaw;
+ UINT8 flightMode; // bit 7 1 --> FM present in bits 0,1 except 0xFF --> not present
+ // new fields go here:
+ UINT8 unused[4];
+} STRU_TELE_AS6X_GAIN;
+
+//////////////////////////////////////////////////////////////////////////////
+//
+// GYRO
+//
+//////////////////////////////////////////////////////////////////////////////
+//
+typedef struct
+{
+ UINT8 identifier; // Source device = 0x1A
+ UINT8 sID; // Secondary ID
+ INT16 gyroX; // Rotation rates of the body - Rate is about the X Axis which is defined out the nose of the vehicle.
+ INT16 gyroY; // Units are 0.1 deg/sec - Rate is about the Y Axis which is define out the right wing of the vehicle.
+ INT16 gyroZ; // Rate is about the Z axis which is defined down from the vehicle.
+ INT16 maxGyroX; // Max rates (absolute value)
+ INT16 maxGyroY;
+ INT16 maxGyroZ;
+} STRU_TELE_GYRO;
+
+//////////////////////////////////////////////////////////////////////////////
+//
+// Alpha6 Stabilizer
+//
+//////////////////////////////////////////////////////////////////////////////
+//
+typedef struct
+{
+ UINT8 identifier; // Source device = 0x24
+ UINT8 sID; // Secondary ID
+ UINT16 volts; // 0.01V increments
+ UINT8 state_FM; // Flight Mode and System State (see below)
+ UINT8 gainRoll, // Roll Gain, high bit --> Heading Hold
+ gainPitch, // Pitch Gain
+ gainYaw; // Yaw Gain
+ INT16 attRoll, // Roll Attitude, 0.1degree, RHR
+ attPitch, // Pitch Attitude
+ attYaw; // Yaw Attitude
+ UINT16 spare;
+} STRU_TELE_ALPHA6;
+
+#define GBOX_STATE_BOOT (0x00) // Alpha6 State - Boot
+#define GBOX_STATE_INIT (0x01) // Init
+#define GBOX_STATE_READY (0x02) // Ready
+#define GBOX_STATE_SENSORFAULT (0x03) // Sensor Fault
+#define GBOX_STATE_POWERFAULT (0x04) // Power Fault
+#define GBOX_STATE_MASK (0x0F)
+
+#define GBOX_FMODE_FM0 (0x00) // FM0 through FM4
+#define GBOX_FMODE_FM1 (0x10)
+#define GBOX_FMODE_FM2 (0x20)
+#define GBOX_FMODE_FM3 (0x30)
+#define GBOX_FMODE_FM4 (0x40)
+#define GBOX_FMODE_PANIC (0x50)
+#define GBOX_FMODE_MASK (0xF0)
+
+//////////////////////////////////////////////////////////////////////////////
+//
+// ATTITUDE & MAG COMPASS
+//
+//////////////////////////////////////////////////////////////////////////////
+//
+typedef struct
+{
+ UINT8 identifier; // Source device = 0x1B
+ UINT8 sID; // Secondary ID
+ INT16 attRoll; // Attitude, 3 axes. Roll is a rotation about the X Axis of the vehicle using the RHR.
+ INT16 attPitch; // Units are 0.1 deg - Pitch is a rotation about the Y Axis of the vehicle using the RHR.
+ INT16 attYaw; // Yaw is a rotation about the Z Axis of the vehicle using the RHR.
+ INT16 magX; // Magnetic Compass, 3 axes
+ INT16 magY; // Units are 0.1mG
+ INT16 magZ; //
+ UINT16 heading; // Heading, 0.1deg
+} STRU_TELE_ATTMAG;
+
+//////////////////////////////////////////////////////////////////////////////
+//
+// Altitude "Zero" Message
+//
+//////////////////////////////////////////////////////////////////////////////
+//
+typedef struct
+{
+ UINT8 identifier; // Source device = 0x7B
+ UINT8 sID; // Secondary ID
+ UINT8 spare[2];
+ UINT32 altOffset; // Altitude "zero" log
+} STRU_TELE_ALT_ZERO;
+
+//////////////////////////////////////////////////////////////////////////////
+//
+// Real-Time Clock
+//
+//////////////////////////////////////////////////////////////////////////////
+//
+typedef struct
+{
+ UINT8 identifier; // Source device = 0x7C
+ UINT8 sID; // Secondary ID
+ UINT8 spare[6];
+ UINT64 UTC64; // Linux 64-bit time_t for post-2038 date compatibility
+} STRU_TELE_RTC;
+
+//////////////////////////////////////////////////////////////////////////////
+//
+// V-Speak (Placeholder)
+//
+//////////////////////////////////////////////////////////////////////////////
+//
+typedef struct
+{
+ UINT8 identifier; // Source device = 0x61
+ UINT8 sID; // Secondary ID
+ UINT8 spare[14]; // Format TBD by V-Speak
+} STRU_TELE_V_SPEAK;
+
+//////////////////////////////////////////////////////////////////////////////
+//
+// www.Smoke-EL.de
+//
+//////////////////////////////////////////////////////////////////////////////
+//
+typedef struct
+{
+ UINT8 identifier; // Source device = 0x61
+ UINT8 sID; // Secondary ID
+ UINT16 batteryV; // 0.01V, Range 0.00-70.00V
+ UINT16 countdown; // 0.01s, Range 0.00-30.00s
+ INT16 GForce; // 0.01g, Range = +/-8.00g
+ UINT8 cutoff; // 1 count, Range 0-9
+ UINT8 connected; // 0=not connected, 1=connected, x = TBD
+ UINT16 spare[3];
+} STRU_TELE_SMOKE_EL;
+
+//////////////////////////////////////////////////////////////////////////////
+//
+// MULTI-CYLINDER SENSOR
+//
+//////////////////////////////////////////////////////////////////////////////
+//
+typedef struct
+{
+ UINT8 identifier; // Source device = TELE_DEVICE_MULTICYLINDER
+ UINT8 sID; // Secondary ID
+ UINT8 temperature[9]; // Temperature, 1C increments, Offset = 30C, 0xFF = NO DATA
+ // 0x00 = 30C (86F)
+ // 0x01 = 31C ... (88F)
+ // 0xFE = 284C (543F)
+ // 0xFF = NO SENSOR ATTACHED. Note that sensors must be installed cylinder 1-9 in sequence!
+ UINT8 throttlePct; // Throttle percent (0-100% typical, 0xFF = NO DATA)
+ UINT16 RPM; // 4 RPM increments, Offset = 400RPM, range 404-16776.
+ // 0x000 = 0 RPM
+ // 0x001 = 404 RPM
+ // 0x002 = 408 RPM
+ // 0xFFE = 16776 RPM
+ // 0xFFF = NO SENSOR ATTACHED
+ // NOTE: HI NYBBLE RESERVED, set to 0xF to mark "NO DATA" for now
+ UINT8 batteryV; // Voltage, 0.1V increments, Offset = 3.5V, 0xFF = NO DATA
+ // 0x00 = 3.5V
+ // 0x01 = 3.6V
+ // 0xFE = 28.9V
+ // 0xFF = NO SENSOR ATTACHED
+ UINT8 spare; // 0xFF --> no data
+} STRU_TELE_MULTI_TEMP;
+
+//////////////////////////////////////////////////////////////////////////////
+//
+// Transmitter Frame Data
+//
+//////////////////////////////////////////////////////////////////////////////
+//
+typedef struct
+{
+ UINT8 identifier; // Source device = 0x7D
+ UINT8 sID; // Secondary ID
+ UINT16 chanData[7]; // Channel Data array
+} STRU_TELE_FRAMEDATA;
+
+//////////////////////////////////////////////////////////////////////////////
+//
+// AHRS Monitor
+//
+//////////////////////////////////////////////////////////////////////////////
+//
+typedef struct
+{
+ UINT8 identifier; // Source device = TELE_DEVICE_AHRS
+ UINT8 sID; // Secondary ID
+ INT16 attRoll; // Attitude, 3 axes. Roll is a rotation about the X Axis of the vehicle using the RHR.
+ INT16 attPitch; // Units are 0.1 deg - Pitch is a rotation about the Y Axis of the vehicle using the RHR.
+ INT16 attYaw; // Roll is a rotation about the Z Axis of the vehicle using the RHR.
+ INT16 altitude; // .1m increments
+ UINT8 waypoint; // Waypoint number
+ UINT8 spare8;
+ UINT16 spare16[2];
+} STRU_TELE_AHRS; // AHRS data from rx
+
+//////////////////////////////////////////////////////////////////////////////
+//
+// FLIGHT MODE
+//
+//////////////////////////////////////////////////////////////////////////////
+//
+typedef struct
+{
+ UINT8 identifier; // Source device = 0x05 TELE_DEVICE_FLITECTRL
+ UINT8 sID; // Secondary ID
+ UINT8 fMode, // Current flight mode (low nybble)
+ spare8;
+ UINT16 spare[6]; // Growth
+ // Ideas -
+ // arming status in a bitmap
+ // time in state
+} STRU_TELE_FLITECTRL;
+
+
+//////////////////////////////////////////////////////////////////////////////
+//
+// Crossfire QOS
+//
+//////////////////////////////////////////////////////////////////////////////
+//
+typedef struct
+{
+ UINT8 identifier; // Source device = TELE_XRF_LINKSTATUS
+ UINT8 sID; // Secondary ID
+ UINT8 ant1, // dBm * -1
+ ant2,
+ quality; // %
+ INT8 SNR; // dB
+ UINT8 activeAnt, // ant1=0, ant2=1
+ RFmode, // 4fps=0, 50fps, 150Hz
+ upPower, // 0mW=0, 10mW, 25mW, 100mW, 500mW, 1000mW, 2000mW
+ downlink, // dBm * -1
+ qualityDown; // %
+ INT8 SNRdown; // dB
+} STRU_TELE_XF_QOS;
+
+//////////////////////////////////////////////////////////////////////////////
+//
+// RPM/Volts/Temperature
+//
+//////////////////////////////////////////////////////////////////////////////
+//
+// Uses big-endian byte order
+//
+typedef struct
+{
+ UINT8 identifier; // Source device = 0x7E
+ UINT8 sID; // Secondary ID
+ UINT16 microseconds; // microseconds between pulse leading edges
+ UINT16 volts; // 0.01V increments (typically flight pack voltage)
+ INT16 temperature; // Temperature in degrees F. 0x7FFF = "No Data"
+ INT8 dBm_A, // Avg RSSI in dBm (<-1 = dBm, 0 = no data, >0 = % range) -- (legacy)antenna A in dBm
+ dBm_B; // Avg RSSI in % (<-1 = dBm, 0 = no data, >0 = % range) -- (legacy)antenna B in dBm
+ // Note: Legacy use as antenna A/B dBm values is still supported. If only 1 antenna, set B = A.
+ // The "no data" value is 0, but -1 (0xFF) is treated the same for backwards compatibility
+ UINT16 spare[2];
+} STRU_TELE_RPM;
+
+//////////////////////////////////////////////////////////////////////////////
+//
+// QoS DATA
+//
+//////////////////////////////////////////////////////////////////////////////
+//
+// NOTE: AR6410-series send:
+// id = 7F
+// sID = 0
+// A = 0
+// B = 0
+// L = 0
+// R = 0
+// F = fades
+// H = holds
+// rxV = 0xFFFF
+//
+typedef struct
+{
+ UINT8 identifier; // Source device = 0x7F
+ UINT8 sID; // Secondary ID
+ UINT16 A; // Internal/base receiver fades. 0xFFFF = "No data"
+ UINT16 B; // Remote receiver fades. 0xFFFF = "No data"
+ UINT16 L; // Third receiver fades. 0xFFFF = "No data"
+ UINT16 R; // Fourth receiver fades. 0xFFFF = "No data"
+ UINT16 F; // Frame losses. 0xFFFF = "No data"
+ UINT16 H; // Holds. 0xFFFF = "No data"
+ UINT16 rxVoltage; // Volts, .01V increment. 0xFFFF = "No data"
+} STRU_TELE_QOS;
+
+//////////////////////////////////////////////////////////////////////////////
+//
+// UNION OF ALL DEVICE MESSAGES
+//
+//////////////////////////////////////////////////////////////////////////////
+//
+typedef union
+{
+ UINT16 raw[8];
+ STRU_TELE_QOS qos;
+ STRU_TELE_RPM rpm;
+ STRU_TELE_HV hv;
+ STRU_TELE_TEMP temp;
+ STRU_TELE_IHIGH amps;
+ STRU_TELE_ALT alt;
+ STRU_TELE_SPEED speed;
+ STRU_TELE_ESC escSPM;
+ STRU_TELE_VARIO_S varioSimple;
+ STRU_TELE_G_METER accel;
+ STRU_TELE_JETCAT jetcat;
+ STRU_TELE_JETCAT2 jetcat2;
+ STRU_TELE_GPS_LOC gpsloc;
+ STRU_TELE_GPS_STAT gpsstat;
+ STRU_TELE_GPS_BINARY gpsbin;
+ STRU_TELE_AS3X_LEGACY as3x;
+ STRU_TELE_AS6X_GAIN as6x;
+ STRU_TELE_GYRO gyro;
+ STRU_TELE_ALPHA6 alpha6;
+ STRU_TELE_ATTMAG attMag;
+ STRU_TELE_POWERBOX powerBox;
+ STRU_TELE_RX_MAH rxMAH;
+ STRU_TELE_FP_MAH fpMAH;
+ STRU_TELE_ESC esc;
+ STRU_TELE_FUEL fuel;
+ STRU_TELE_DIGITAL_AIR digAir;
+ STRU_TELE_STRAIN strain;
+ STRU_TELE_LIPOMON lipomon;
+ STRU_TELE_LIPOMON_14 lipomon14;
+ STRU_SMARTBATT_HEADER smartBatt_header;
+ STRU_SMARTBATT_REALTIME smartBatt_realtime;
+ STRU_SMARTBATT_CELLS smartBatt_cells;
+ STRU_SMARTBATT_ID smartBatt_ID;
+ STRU_SMARTBATT_LIMITS smartBatt_limits;
+ STRU_TELE_USER_16SU user_16SU;
+ STRU_TELE_USER_16SU32U user_16SU32U;
+ STRU_TELE_USER_16SU32S user_16SU32S;
+ STRU_TELE_USER_16U32SU user_16U32SU;
+ STRU_TELE_TEXTGEN textgen;
+ STRU_TELE_VTX vtx;
+ STRU_TELE_V_SPEAK vSpeak;
+ STRU_TELE_SMOKE_EL smoke_el;
+ STRU_TELE_MULTI_TEMP multiCylinder;
+ STRU_TELE_FLITECTRL fControl;
+ STRU_TELE_TILT tilt;
+ STRU_TELE_XF_QOS xfire;
+} UN_TELEMETRY; // All telemetry messages
+
+//////////////////////////////////////////////////////////////////
+//
+// sID Field Functionality
+//
+//////////////////////////////////////////////////////////////////
+//
+// if .sID == 0x00 then .identifier = device type (TELE_DEVICE_xxx) and address I2C bus
+// if .sID != 0x00 then .sID = device type and .identifer = address on I2C bus
+
+#endif