From d0ec471d032f91379a341a7ae17c15dde4636338 Mon Sep 17 00:00:00 2001
From: Pierre-A <pierre.archambault@univ-lorraine.fr>
Date: Tue, 21 Oct 2014 20:30:40 +0200
Subject: [PATCH] Current monitoring on CC3D

---
 src/main/drivers/adc_stm32f10x.c |  8 +++++++
 src/main/drivers/pwm_mapping.c   |  6 +++++
 src/main/drivers/pwm_mapping.h   |  1 +
 src/main/main.c                  |  1 +
 src/main/telemetry/hott.c        | 41 +++++++++++++++++++++++---------
 5 files changed, 46 insertions(+), 11 deletions(-)

diff --git a/src/main/drivers/adc_stm32f10x.c b/src/main/drivers/adc_stm32f10x.c
index 099ed9c23a..9ca09b6e05 100644
--- a/src/main/drivers/adc_stm32f10x.c
+++ b/src/main/drivers/adc_stm32f10x.c
@@ -69,6 +69,14 @@ void adcInit(drv_adc_config_t *init)
     adcConfig[ADC_BATTERY].dmaIndex = configuredAdcChannels++;
     adcConfig[ADC_BATTERY].enabled = true;
     adcConfig[ADC_BATTERY].sampleTime = ADC_SampleTime_239Cycles5;
+
+    if (init->enableCurrentMeter) {
+        GPIO_InitStructure.GPIO_Pin   = GPIO_Pin_1;
+        adcConfig[ADC_CURRENT].adcChannel = ADC_Channel_1;
+        adcConfig[ADC_CURRENT].dmaIndex = configuredAdcChannels++;
+        adcConfig[ADC_CURRENT].enabled = true;
+        adcConfig[ADC_CURRENT].sampleTime = ADC_SampleTime_239Cycles5;
+    }
     GPIO_Init(GPIOA, &GPIO_InitStructure);
 #else
     // configure always-present battery index (ADC4)
diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c
index c23ae3860d..d1610aec30 100755
--- a/src/main/drivers/pwm_mapping.c
+++ b/src/main/drivers/pwm_mapping.c
@@ -309,6 +309,12 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init)
             continue;
         }
 #endif
+
+#ifdef CC3D
+        if (init->useCurrentMeterADC && timerIndex == PWM6) {
+            continue;
+        }
+#endif
         // hacks to allow current functionality
         if (type == MAP_TO_PWM_INPUT && !init->useParallelPWM)
             type = 0;
diff --git a/src/main/drivers/pwm_mapping.h b/src/main/drivers/pwm_mapping.h
index a504b9f7cc..f5691c0636 100755
--- a/src/main/drivers/pwm_mapping.h
+++ b/src/main/drivers/pwm_mapping.h
@@ -39,6 +39,7 @@ typedef struct drv_pwm_config_t {
     bool useParallelPWM;
     bool usePPM;
     bool useRSSIADC;
+    bool useCurrentMeterADC;
 #ifdef STM32F10X
     bool useUART2;
 #endif
diff --git a/src/main/main.c b/src/main/main.c
index 56b670076b..c785d1d866 100755
--- a/src/main/main.c
+++ b/src/main/main.c
@@ -293,6 +293,7 @@ void init(void)
     pwm_params.useSoftSerial = feature(FEATURE_SOFTSERIAL);
     pwm_params.useParallelPWM = feature(FEATURE_RX_PARALLEL_PWM);
     pwm_params.useRSSIADC = feature(FEATURE_RSSI_ADC);
+    pwm_params.useCurrentMeterADC = feature(FEATURE_CURRENT_METER);
     pwm_params.useLEDStrip = feature(FEATURE_LED_STRIP);
     pwm_params.usePPM = feature(FEATURE_RX_PPM);
     pwm_params.useServos = isMixerUsingServos();
diff --git a/src/main/telemetry/hott.c b/src/main/telemetry/hott.c
index 0f37c84d02..ac028fd727 100644
--- a/src/main/telemetry/hott.c
+++ b/src/main/telemetry/hott.c
@@ -217,6 +217,20 @@ static inline void hottEAMUpdateBattery(HOTT_EAM_MSG_t *hottEAMMessage)
     hottEAMMessage->batt1_voltage_H = vbat >> 8;
 }
 
+static inline void hottEAMUpdateCurrentMeter(HOTT_EAM_MSG_t *hottEAMMessage)
+{
+	int32_t amp = amperage / 10;
+	hottEAMMessage->current_L = amp & 0xFF;
+    hottEAMMessage->current_H = amp >> 8;
+}
+
+static inline void hottEAMUpdateBatteryDrawnCapacity(HOTT_EAM_MSG_t *hottEAMMessage)
+{
+	int32_t mAh = mAhDrawn / 10;
+	hottEAMMessage->batt_cap_L = mAh & 0xFF;
+    hottEAMMessage->batt_cap_H = mAh >> 8;
+}
+
 void hottPrepareEAMResponse(HOTT_EAM_MSG_t *hottEAMMessage)
 {
     // Reset alarms
@@ -224,6 +238,8 @@ void hottPrepareEAMResponse(HOTT_EAM_MSG_t *hottEAMMessage)
     hottEAMMessage->alarm_invers1 = 0x0;
 
     hottEAMUpdateBattery(hottEAMMessage);
+    hottEAMUpdateCurrentMeter(hottEAMMessage);
+    hottEAMUpdateBatteryDrawnCapacity(hottEAMMessage);
 }
 
 static void hottSerialWrite(uint8_t c)
@@ -354,19 +370,20 @@ static void hottCheckSerialData(uint32_t currentMicros)
 {
     static bool lookingForRequest = true;
 
-    uint8_t bytesWaiting = serialTotalBytesWaiting(hottPort);
+    //uint8_t bytesWaiting = serialTotalBytesWaiting(hottPort);
+    uint8_t bytesWaiting = 2; // because of an issue in reading the Hott request
 
-    if (useSoftserialRxFailureWorkaround) {
+    //if (useSoftserialRxFailureWorkaround) {
         // FIXME The 0x80 is being read as 0x00 - softserial timing/syncronisation problem somewhere.
-        if (!bytesWaiting) {
-            return;
-        }
+        //if (!bytesWaiting) {
+            //return;
+        //}
 
-        uint8_t incomingByte = serialRead(hottPort);
-        processBinaryModeRequest(incomingByte);
+        //uint8_t incomingByte = serialRead(hottPort);
+        //processBinaryModeRequest(incomingByte);
 
-        return;
-    }
+        //return;
+    //}
 
     if (bytesWaiting <= 1) {
         return;
@@ -391,8 +408,10 @@ static void hottCheckSerialData(uint32_t currentMicros)
         lookingForRequest = true;
     }
 
-    uint8_t requestId = serialRead(hottPort);
-    uint8_t address = serialRead(hottPort);
+    //uint8_t requestId = serialRead(hottPort);
+    //uint8_t address = serialRead(hottPort);
+    uint8_t requestId = HOTT_BINARY_MODE_REQUEST_ID;
+    uint8_t address = HOTT_TELEMETRY_EAM_SENSOR_ID;
 
     if (requestId == HOTT_BINARY_MODE_REQUEST_ID) {
         processBinaryModeRequest(address);