From 4be6cfccabac8955293e63d0e1b94eebdee86af9 Mon Sep 17 00:00:00 2001
From: Bertrand Songis <bsongis@gmail.com>
Date: Mon, 6 Jun 2016 21:59:49 +0200
Subject: [PATCH] [Taranis] Crossfire "Flight mode" telemetry sensor now OK

---
 radio/src/gui/taranis/lcd.cpp             |  3 +++
 radio/src/telemetry/crossfire.cpp         | 13 +++++++++----
 radio/src/telemetry/telemetry_sensors.cpp | 15 ++++++++++-----
 radio/src/telemetry/telemetry_sensors.h   |  1 +
 4 files changed, 23 insertions(+), 9 deletions(-)

diff --git a/radio/src/gui/taranis/lcd.cpp b/radio/src/gui/taranis/lcd.cpp
index c4845870d..a30ba1e1c 100644
--- a/radio/src/gui/taranis/lcd.cpp
+++ b/radio/src/gui/taranis/lcd.cpp
@@ -1055,6 +1055,9 @@ void putsTelemetryChannelValue(coord_t x, coord_t y, uint8_t channel, int32_t va
       }
     }
   }
+  else if (telemetrySensor.unit == UNIT_TEXT) {
+    lcdDrawSizedText(x, att & DBLSIZE ? y+1 : y, telemetryItem.text, sizeof(telemetryItem.text), att & ~DBLSIZE);
+  }
   else {
     LcdFlags flags = att;
     if (telemetrySensor.prec > 0) {
diff --git a/radio/src/telemetry/crossfire.cpp b/radio/src/telemetry/crossfire.cpp
index 192003d95..45c45b2b6 100644
--- a/radio/src/telemetry/crossfire.cpp
+++ b/radio/src/telemetry/crossfire.cpp
@@ -78,7 +78,7 @@ const CrossfireSensor crossfireSensors[] = {
   {ATTITUDE_ID,    0, ZSTR_PITCH,       UNIT_RADIANS,       3},
   {ATTITUDE_ID,    1, ZSTR_ROLL,        UNIT_RADIANS,       3},
   {ATTITUDE_ID,    2, ZSTR_YAW,         UNIT_RADIANS,       3},
-  {FLIGHT_MODE_ID, 2, ZSTR_FLIGHT_MODE, UNIT_TEXT,          0},
+  {FLIGHT_MODE_ID, 0, ZSTR_FLIGHT_MODE, UNIT_TEXT,          0},
   {0,              0, "UNKNOWN",        UNIT_RAW,           0},
 };
 
@@ -184,9 +184,14 @@ void processCrossfireTelemetryFrame()
       break;
 
     case FLIGHT_MODE_ID:
-      // TODO
+    {
+      const CrossfireSensor & sensor = crossfireSensors[FLIGHT_MODE_INDEX];
+      for (int i=0; i<min<int>(16, telemetryRxBuffer[1]-2); i+=4) {
+        uint32_t value = *((uint32_t *)&telemetryRxBuffer[3+i]);
+        setTelemetryValue(TELEM_PROTO_CROSSFIRE, sensor.id, 0, sensor.subId, value, sensor.unit, i);
+      }
       break;
-
+    }
   }
 }
 
@@ -237,4 +242,4 @@ void crossfireSetDefault(int index, uint8_t id, uint8_t subId)
   }
 
   storageDirty(EE_MODEL);
-}
\ No newline at end of file
+}
diff --git a/radio/src/telemetry/telemetry_sensors.cpp b/radio/src/telemetry/telemetry_sensors.cpp
index 63ac6f7b7..3654ddc4d 100644
--- a/radio/src/telemetry/telemetry_sensors.cpp
+++ b/radio/src/telemetry/telemetry_sensors.cpp
@@ -2,7 +2,7 @@
  * Copyright (C) OpenTX
  *
  * Based on code named
- *   th9x - http://code.google.com/p/th9x 
+ *   th9x - http://code.google.com/p/th9x
  *   er9x - http://code.google.com/p/er9x
  *   gruvin9x - http://code.google.com/p/gruvin9x
  *
@@ -163,6 +163,11 @@ void TelemetryItem::setValue(const TelemetrySensor & sensor, int32_t val, uint32
       newVal = (newVal * sensor.custom.offset) / sensor.custom.ratio;
     }
   }
+  else if (unit == UNIT_TEXT) {
+    *((uint32_t*)&text[prec]) = newVal;
+    lastReceived = now();
+    return;
+  }
   else {
     newVal = sensor.getValue(newVal, unit, prec);
     if (sensor.autoOffset) {
@@ -474,7 +479,7 @@ void setTelemetryValue(TelemetryProtocol protocol, uint16_t id, uint8_t subId, u
   if (available || !allowNewSensors) {
     return;
   }
-  
+
   int index = availableTelemetryIndex();
   if (index >= 0) {
     switch (protocol) {
@@ -541,12 +546,12 @@ const UnitConversionRule unitConversionTable[] = {
   /* unitFrom     unitTo                    multiplier   divisor */
   { UNIT_METERS,            UNIT_FEET,             105,   32},
   { UNIT_METERS_PER_SECOND, UNIT_FEET_PER_SECOND,  105,   32},
-   
+
   { UNIT_KTS, UNIT_KMH,                           1852, 1000}, // 1 knot = 1.85200 kilometers per hour
   { UNIT_KTS, UNIT_MPH,                           1151, 1000}, // 1 knot = 1.15077945 miles per hour
   { UNIT_KTS, UNIT_METERS_PER_SECOND,             1000, 1944}, // 1 knot = 0.514444444 meters / second (divide with 1.94384449)
   { UNIT_KTS, UNIT_FEET_PER_SECOND,               1688, 1000}, // 1 knot = 1.68780986 feet per second
-  
+
   { UNIT_KMH, UNIT_KTS,                           1000, 1852}, // 1 km/h = 0.539956803 knots (divide with 1.85200)
   { UNIT_KMH, UNIT_MPH,                           1000, 1609}, // 1 km/h = 0.621371192 miles per hour (divide with 1.60934400)
   { UNIT_KMH, UNIT_METERS_PER_SECOND,               10,   36}, // 1 km/h = 0.277777778 meters / second (divide with 3.6)
@@ -577,7 +582,7 @@ int32_t convertTelemetryValue(int32_t value, uint8_t unit, uint8_t prec, uint8_t
       ++p;
     }
   }
-  
+
   for (int i=destPrec; i<prec; i++)
     value /= 10;
 
diff --git a/radio/src/telemetry/telemetry_sensors.h b/radio/src/telemetry/telemetry_sensors.h
index 4baac0475..9f0f4b086 100644
--- a/radio/src/telemetry/telemetry_sensors.h
+++ b/radio/src/telemetry/telemetry_sensors.h
@@ -70,6 +70,7 @@ class TelemetryItem
         // pilot latitude is stored in max
         // distFromEarthAxis is stored in value
       } gps;
+      char text[16];
     };
 
     static uint8_t now()