1 files changed, 11 insertions, 21 deletions

diff --git a/firmware/target/arm/tms320dm320/mrobe-500/powermgmt-mr500.c b/firmware/target/arm/tms320dm320/mrobe-500/powermgmt-mr500.c
index f3746c8c98..654ec96591 100755
--- a/firmware/target/arm/tms320dm320/mrobe-500/powermgmt-mr500.c
+++ b/firmware/target/arm/tms320dm320/mrobe-500/powermgmt-mr500.c
@@ -25,7 +25,7 @@
 #include "tsc2100.h"
 #include "kernel.h"
 
-unsigned short current_voltage = 3910;
+static unsigned short current_voltage = 3910;
 const unsigned short battery_level_dangerous[BATTERY_TYPES_COUNT] =
 {
     0
@@ -47,38 +47,28 @@ const unsigned short percent_to_volt_charge[11] =
 {
     100, 300, 400, 500, 600, 700, 800, 900, 1000, 1200, 1320,
 };
+
 void read_battery_inputs(void)
 {
-    short tsadc = tsc2100_readreg(TSADC_PAGE, TSADC_ADDRESS);
-    short adscm = (tsadc&TSADC_ADSCM_MASK)>>TSADC_ADSCM_SHIFT;
-    if (adscm == 0xb) /* battery is available */
-    {
-        current_voltage = tsc2100_readreg(0, 5); /* BAT1 */
-        tsc2100_readreg(0, 6); /* BAT2 */
-        tsc2100_readreg(0, 7); /* AUX */
-        /* reset the TSC2100 to read touches */
-        tsadc &= ~(TSADC_PSTCM|TSADC_ADST|TSADC_ADSCM_MASK);
-        tsadc |= TSADC_PSTCM|(0x2<<TSADC_ADSCM_SHIFT);
-        tsc2100_writereg(TSADC_PAGE, TSADC_ADDRESS, tsadc);
-        tsc2100_writereg(TSSTAT_PAGE, TSSTAT_ADDRESS, 2<<TSSTAT_PINTDAV_SHIFT);
-    }
+    short dummy1, dummy2;
+    tsc2100_read_volt(&current_voltage, &dummy1, &dummy2);
+
+    /* Set the TSC2100 back to read touches */
+    tsc2100_set_mode(0x01);
 }
     
 /* Returns battery voltage from ADC [millivolts] */
 unsigned int battery_adc_voltage(void)
 {
     static unsigned last_tick = 0;
-    short tsadc = tsc2100_readreg(TSADC_PAGE, TSADC_ADDRESS);
+
     if (TIME_BEFORE(last_tick+2*HZ, current_tick))
     {
-        tsadc &= ~(TSADC_PSTCM|TSADC_ADST|TSADC_ADSCM_MASK);
-        tsadc |= 0xb<<TSADC_ADSCM_SHIFT;
-        tsc2100_writereg(TSADC_PAGE, TSADC_ADDRESS, tsadc&(~(1<<15)));
-        tsc2100_writereg(TSSTAT_PAGE, TSSTAT_ADDRESS, 2<<TSSTAT_PINTDAV_SHIFT);
+        /* Set the TSC2100 to read voltages */
+        tsc2100_set_mode(0x0B);
         last_tick = current_tick;
     }
-    else
-        read_battery_inputs();
+        
     return current_voltage;
 }