From 1cbc7ca204f35cd40ac665814e7715e6ea3fa595 Mon Sep 17 00:00:00 2001
From: Pavel Pisa <ppisa@pikron.com>
Date: Sat, 14 Feb 2015 23:15:02 +0100
Subject: [PATCH] RoCoN: add test for channels mutual current ADC calibration
 and check.

Signed-off-by: Pavel Pisa <ppisa@pikron.com>
---
 sw/app/rocon/appl_pxmc.c  | 13 ++++++++
 sw/app/rocon/appl_pxmc.h  |  2 ++
 sw/app/rocon/appl_tests.c | 63 +++++++++++++++++++++++++++++++++++++++
 3 files changed, 78 insertions(+)

diff --git a/sw/app/rocon/appl_pxmc.c b/sw/app/rocon/appl_pxmc.c
index fdd9ef9..ec520dc 100644
--- a/sw/app/rocon/appl_pxmc.c
+++ b/sw/app/rocon/appl_pxmc.c
@@ -393,6 +393,19 @@ pxmc_rocon_pwm_chan2reg(unsigned chan)
   return pwm_reg;
 }
 
+int pxmc_rocon_pwm_direct_wr(unsigned chan, unsigned pwm, int en)
+{
+  volatile uint32_t *pwm_reg;
+  pwm_reg = pxmc_rocon_pwm_chan2reg(chan);
+
+  if (pwm_reg == &pxmc_rocon_pwm_dummy_reg)
+    return -1;
+
+  *pwm_reg = pwm | (en? 0x4000: 0x8000);
+
+  return 0;
+}
+
 /**
  * pxmc_rocon_pwm3ph_wr - Output of the 3-phase PWM to the hardware
  * @mcs:  Motion controller state information
diff --git a/sw/app/rocon/appl_pxmc.h b/sw/app/rocon/appl_pxmc.h
index 17ce6ce..e9dfcca 100644
--- a/sw/app/rocon/appl_pxmc.h
+++ b/sw/app/rocon/appl_pxmc.h
@@ -41,4 +41,6 @@ typedef struct pxmc_rocon_state_t {
 
 extern int pxmc_rocon_vin_act;
 
+int pxmc_rocon_pwm_direct_wr(unsigned chan, unsigned pwm, int en);
+
 #endif /*_APPL_PXMC_H_*/
diff --git a/sw/app/rocon/appl_tests.c b/sw/app/rocon/appl_tests.c
index 1f00f36..ed8df1a 100644
--- a/sw/app/rocon/appl_tests.c
+++ b/sw/app/rocon/appl_tests.c
@@ -20,6 +20,7 @@
 
 #include "appl_defs.h"
 #include "appl_fpga.h"
+#include "appl_pxmc.h"
 #include "pxmcc_types.h"
 #include "pxmcc_interface.h"
 
@@ -534,6 +535,63 @@ int cmd_do_testtumblebus(cmd_io_t *cmd_io, const struct cmd_des *des, char *para
   return 0;
 }
 
+int cmd_do_testcuradc(cmd_io_t *cmd_io, const struct cmd_des *des, char *param[])
+{
+  char *ps = param[1];
+  long pwm_chan_a;
+  long pwm_chan_b;
+  long pwm_b = 0;
+  pxmc_state_t *mcs;
+  volatile pxmcc_data_t *mcc_data = pxmc_rocon_mcc_data();
+  int i;
+  long cur_a;
+  long cur_b;
+  long pwm_cycle = mcc_data->common.pwm_cycle;
+
+  si_skspace(&ps);
+  if (si_long(&ps, &pwm_chan_a, 0) < 0)
+      return -CMDERR_BADPAR;
+
+  si_skspace(&ps);
+  if (si_ulong(&ps, &pwm_chan_b, 0) < 0)
+        return -CMDERR_BADPAR;
+
+  si_skspace(&ps);
+  if (*ps) {
+    if (si_ulong(&ps, &pwm_b, 0) < 0)
+          return -CMDERR_BADPAR;
+    pxmc_for_each_mcs(i, mcs) {
+      /* PXMS_ENI_m - check if input (IRC) update is enabled */
+      if (mcs->pxms_flg & (PXMS_ENR_m | PXMS_ENO_m)) {
+        pxmc_set_const_out(mcs,0);
+        pxmc_clear_flag(mcs, PXMS_ENO_b);
+      }
+    }
+
+    for (i = 0; i < 16; i++) {
+      if (i == pwm_chan_a) {
+        if (pxmc_rocon_pwm_direct_wr(i, 0, 1) < 0)
+          return -CMDERR_EIO;
+      } else if (i == pwm_chan_b) {
+        if (pxmc_rocon_pwm_direct_wr(i, pwm_b, 1) < 0)
+          return -CMDERR_EIO;
+      } else {
+        pxmc_rocon_pwm_direct_wr(i, 0, 0);
+      }
+    }
+  }
+
+  cur_a = mcc_data->curadc[pwm_chan_a].cur_val;
+  cur_b = mcc_data->curadc[pwm_chan_b].cur_val;
+  if (pwm_b < pwm_cycle)
+    cur_b = (pwm_cycle * cur_b + pwm_cycle / 2) / (pwm_cycle - pwm_b);
+
+  printf("ch %2ld pwm %5ld cur %7ld\n", pwm_chan_a, 0l, cur_a);
+  printf("ch %2ld pwm %5ld cur %7ld\n", pwm_chan_b, pwm_b, cur_b);
+
+  return 0;
+}
+
 cmd_des_t const cmd_des_test_memusage = {0, 0,
                 "memusage", "report memory usage", cmd_do_test_memusage,
 {
@@ -616,6 +674,10 @@ cmd_des_t const cmd_des_testtumblebus = {0, 0,
                                      cmd_do_testtumblebus, {(void *)0}
                                     };
 
+cmd_des_t const cmd_des_testcuradc = {0, 0,
+                                     "testcuradc", "test current adc channel calibration",
+                                     cmd_do_testcuradc, {(void *)0}
+                                    };
 
 cmd_des_t const *const cmd_appl_tests[] =
 {
@@ -636,5 +698,6 @@ cmd_des_t const *const cmd_appl_tests[] =
   &cmd_des_testfncapprox,
   &cmd_des_testtumblefw,
   &cmd_des_testtumblebus,
+  &cmd_des_testcuradc,
   NULL
 };
-- 
2.39.2