From 863156116f0df10b5c2adce259e0aa4b4a551312 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Anton=20St=C3=A5lheim?= <anton.stalheim@gmail.com>
Date: Wed, 24 Aug 2016 21:22:21 +0200
Subject: [PATCH 1/2] Added PID values over smartport telemetry

---
 src/main/telemetry/smartport.c | 36 ++++++++++++++++++++++++++++++++++
 1 file changed, 36 insertions(+)

diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c
index 4396b0c0a6..ad0415fe62 100644
--- a/src/main/telemetry/smartport.c
+++ b/src/main/telemetry/smartport.c
@@ -61,6 +61,9 @@
 #include "fc/runtime_config.h"
 
 #include "config/config.h"
+#include "config/config_profile.h"
+extern profile_t *currentProfile;
+extern controlRateConfig_t *currentControlRateProfile;
 
 enum
 {
@@ -314,7 +317,9 @@ void handleSmartPortTelemetry(void)
         smartPortIdCnt++;
 
         int32_t tmpi;
+        uint32_t tmp2;
         static uint8_t t1Cnt = 0;
+        static uint8_t t2Cnt = 0;
 
         switch(id) {
 #ifdef GPS
@@ -464,6 +469,37 @@ void handleSmartPortTelemetry(void)
                     smartPortSendPackage(id, 0);
                     smartPortHasRequest = 0;
                 }
+                else {
+                    switch (t2Cnt) {
+                        case 0:
+                            tmp2 = currentProfile->pidProfile.P8[ROLL];
+                            tmp2 += (currentProfile->pidProfile.P8[PITCH]<<8);
+                            tmp2 += (currentProfile->pidProfile.P8[YAW]<<16);
+                        break;
+                        case 1:
+                            tmp2 = currentProfile->pidProfile.I8[ROLL];
+                            tmp2 += (currentProfile->pidProfile.I8[PITCH]<<8);
+                            tmp2 += (currentProfile->pidProfile.I8[YAW]<<16);
+                        break;
+                        case 2:
+                            tmp2 = currentProfile->pidProfile.D8[ROLL];
+                            tmp2 += (currentProfile->pidProfile.D8[PITCH]<<8);
+                            tmp2 += (currentProfile->pidProfile.D8[YAW]<<16);
+                        break;
+                        case 3:
+                            tmp2 = currentControlRateProfile->rates[FD_ROLL];
+                            tmp2 += (currentControlRateProfile->rates[FD_PITCH]<<8);
+                            tmp2 += (currentControlRateProfile->rates[FD_YAW]<<16);
+                        break;
+                    }
+                    tmp2 += t2Cnt<<24;
+                    t2Cnt++;
+                    if (t2Cnt == 4) {
+                        t2Cnt = 0;
+                    }
+                    smartPortSendPackage(id, tmp2);
+                    smartPortHasRequest = 0;
+                }
                 break;
 #ifdef GPS
             case FSSP_DATAID_GPS_ALT    :

From 206098e127719282afd367bdcacefa099563daa6 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Anton=20St=C3=A5lheim?= <anton.stalheim@gmail.com>
Date: Thu, 25 Aug 2016 12:55:14 +0200
Subject: [PATCH 2/2] Added pidAsTelemtry as option by CLI parameter

---
 src/main/config/config.c       | 1 +
 src/main/io/serial_cli.c       | 1 +
 src/main/telemetry/smartport.c | 3 ++-
 src/main/telemetry/telemetry.h | 1 +
 4 files changed, 5 insertions(+), 1 deletion(-)

diff --git a/src/main/config/config.c b/src/main/config/config.c
index 1a0fc0330d..23beba43a4 100755
--- a/src/main/config/config.c
+++ b/src/main/config/config.c
@@ -266,6 +266,7 @@ void resetTelemetryConfig(telemetryConfig_t *telemetryConfig)
     telemetryConfig->frsky_vfas_precision = 0;
     telemetryConfig->frsky_vfas_cell_voltage = 0;
     telemetryConfig->hottAlarmSoundInterval = 5;
+    telemetryConfig->pidValuesAsTelemetry = 0;
 }
 #endif
 
diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c
index 98a81b221b..00486a6470 100644
--- a/src/main/io/serial_cli.c
+++ b/src/main/io/serial_cli.c
@@ -746,6 +746,7 @@ const clivalue_t valueTable[] = {
     { "frsky_vfas_precision",       VAR_UINT8  | MASTER_VALUE,  &masterConfig.telemetryConfig.frsky_vfas_precision, .config.minmax = { FRSKY_VFAS_PRECISION_LOW,  FRSKY_VFAS_PRECISION_HIGH } },
     { "frsky_vfas_cell_voltage",    VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP,  &masterConfig.telemetryConfig.frsky_vfas_cell_voltage, .config.lookup = { TABLE_OFF_ON } },
     { "hott_alarm_sound_interval",  VAR_UINT8  | MASTER_VALUE,  &masterConfig.telemetryConfig.hottAlarmSoundInterval, .config.minmax = { 0,  120 } },
+    { "pid_values_as_telemetry",    VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP,  &masterConfig.telemetryConfig.pidValuesAsTelemetry, .config.lookup = {TABLE_OFF_ON } },
 #endif
 
     { "battery_capacity",           VAR_UINT16 | MASTER_VALUE,  &masterConfig.batteryConfig.batteryCapacity, .config.minmax = { 0,  20000 } },
diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c
index ad0415fe62..fcf26b010f 100644
--- a/src/main/telemetry/smartport.c
+++ b/src/main/telemetry/smartport.c
@@ -469,7 +469,8 @@ void handleSmartPortTelemetry(void)
                     smartPortSendPackage(id, 0);
                     smartPortHasRequest = 0;
                 }
-                else {
+
+                else if (telemetryConfig->pidValuesAsTelemetry){
                     switch (t2Cnt) {
                         case 0:
                             tmp2 = currentProfile->pidProfile.P8[ROLL];
diff --git a/src/main/telemetry/telemetry.h b/src/main/telemetry/telemetry.h
index b61aa68bd4..1d45ed5b9e 100644
--- a/src/main/telemetry/telemetry.h
+++ b/src/main/telemetry/telemetry.h
@@ -44,6 +44,7 @@ typedef struct telemetryConfig_s {
     uint8_t frsky_vfas_precision;
     uint8_t frsky_vfas_cell_voltage;
     uint8_t hottAlarmSoundInterval;
+    uint8_t pidValuesAsTelemetry;
 } telemetryConfig_t;
 
 void telemetryInit(void);