industry/foc: add support for CORDIC

include/industry/foc/fixed16/foc_cordic.h

@@ -33,16 +33,20 @@
  * Public Function Prototypes
  ****************************************************************************/
 
+#ifdef CONFIG_INDUSTRY_FOC_CORDIC_DQSAT
 /****************************************************************************
  * Name: foc_cordic_dqsat_b16
  ****************************************************************************/
 
 int foc_cordic_dqsat_b16(int fd, FAR dq_frame_b16_t *dq_ref, b16_t mag_max);
+#endif
 
+#ifdef CONFIG_INDUSTRY_FOC_CORDIC_ANGLE
 /****************************************************************************
  * Name: foc_cordic_angle_b16
  ****************************************************************************/
 
 int foc_cordic_angle_b16(int fd, FAR phase_angle_b16_t *angle, b16_t a);
+#endif
 
 #endif /* __INDUSTRY_FOC_FIXED16_FOC_CORDIC_H */

include/industry/foc/float/foc_cordic.h

@@ -33,16 +33,20 @@
  * Public Function Prototypes
  ****************************************************************************/
 
+#ifdef CONFIG_INDUSTRY_FOC_CORDIC_DQSAT
 /****************************************************************************
  * Name: foc_cordic_dqsat_f32
  ****************************************************************************/
 
 int foc_cordic_dqsat_f32(int fd, FAR dq_frame_f32_t *dq_ref, float mag_max);
+#endif
 
+#ifdef CONFIG_INDUSTRY_FOC_CORDIC_ANGLE
 /****************************************************************************
  * Name: foc_cordic_angle_f32
  ****************************************************************************/
 
 int foc_cordic_angle_f32(int fd, FAR phase_angle_f32_t *angle, float a);
+#endif
 
 #endif /* __INDUSTRY_FOC_FLOAT_FOC_CORDIC_H */

industry/foc/Kconfig

@@ -14,11 +14,26 @@ if INDUSTRY_FOC
 
 config INDUSTRY_FOC_CORDIC
 	bool "Enable CORDIC support"
-	depends on EXPERIMENTAL
 	default n
 	---help---
 		Enable support for CORDIC hardware acceleration
 
+if INDUSTRY_FOC_CORDIC
+
+config INDUSTRY_FOC_CORDIC_DEVPATH
+	string "CORDIC device path"
+	default "/dev/cordic0"
+
+config INDUSTRY_FOC_CORDIC_ANGLE
+	bool "Enable CORDIC for phase angle"
+	default n
+
+config INDUSTRY_FOC_CORDIC_DQSAT
+	bool "Enable CORDIC for dq saturation"
+	default n
+
+endif # INDUSTRY_FOC_CORDIC
+
 config INDUSTRY_FOC_FIXED16
 	bool "Enable support for fixed16"
 	default n

industry/foc/fixed16/foc_cordic.c

@@ -24,14 +24,25 @@
 
 #include <nuttx/config.h>
 
-#include <assert.h>
+#include <sys/ioctl.h>
 
+#include <assert.h>
+#include <fcntl.h>
+#include <debug.h>
+#include <errno.h>
+
+#include <nuttx/math/cordic.h>
+#include <nuttx/math/math_ioctl.h>
+
+#include "industry/foc/foc_log.h"
 #include "industry/foc/fixed16/foc_cordic.h"
 
 /****************************************************************************
  * Public Functions
  ****************************************************************************/
 
+#ifdef CONFIG_INDUSTRY_FOC_CORDIC_DQSAT
+
 /****************************************************************************
  * Name: foc_cordic_dqsat_b16
  *
@@ -47,14 +58,70 @@
 
 int foc_cordic_dqsat_b16(int fd, FAR dq_frame_b16_t *dq, b16_t mag_max)
 {
+  struct cordic_calc_s io;
+  b16_t                dnorm   = 0;
+  b16_t                qnorm   = 0;
+  b16_t                dqscale = 0;
+  b16_t                mag     = 0;
+  b16_t                tmp     = 0;
+  int                  ret     = OK;
+
   DEBUGASSERT(dq);
 
-  /* TODO: */
+  /* Normalize DQ to [-1, 1] */
 
-  ASSERT(0);
+  if (dq->d > dq->q)
+    {
+      dqscale = b16divb16(b16ONE, dq->d);
+    }
+  else
+    {
+      dqscale = b16divb16(b16ONE, dq->q);
+    }
+
+  dnorm = b16mulb16(dq->d, dqscale);
+  qnorm = b16mulb16(dq->d, dqscale);
+
+  /* Get modulus */
+
+  io.func      = CORDIC_CALC_FUNC_MOD;
+  io.res2_incl = false;
+  io.arg1      = b16toq31(dnorm);
+  io.arg2      = b16toq31(qnorm);
+  io.res1      = 0;
+  io.res2      = 0;
+
+  ret = ioctl(fd, MATHIOC_CORDIC_CALC, (unsigned long)((uintptr_t)&io));
+  if (ret < 0)
+    {
+      FOCLIBERR("ERROR: MATHIOC_CORDIC_CALC failed, errno=%d\n", errno);
+    }
+
+  /* Get real magnitude */
+
+  mag = b16mulb16(q31tof(io.res1), dqscale);
+
+  /* Magnitude bottom limit */
+
+  if (mag < 1)
+    {
+      mag = 1;
+    }
+
+  if (mag > mag_max)
+    {
+      /* Saturate vector */
+
+      tmp = b16divb16(mag_max, mag);
+      dq->d = b16mulb16(dq->d, tmp);
+      dq->q = b16mulb16(dq->q, tmp);
+    }
 
   return OK;
 }
+#endif  /* CONFIG_INDUSTRY_FOC_CORDIC_DQSAT */
+
+#ifdef CONFIG_INDUSTRY_FOC_CORDIC_ANGLE
 
 /****************************************************************************
  * Name: foc_cordic_angle_b16
@@ -71,11 +138,46 @@ int foc_cordic_dqsat_b16(int fd, FAR dq_frame_b16_t *dq, b16_t mag_max)
 
 int foc_cordic_angle_b16(int fd, FAR phase_angle_b16_t *angle, b16_t a)
 {
+  const b16_t          onebypi = b16divb16(b16ONE, b16PI);
+  struct cordic_calc_s io;
+  b16_t                anorm   = 0;
+  int                  ret     = OK;
+
   DEBUGASSERT(angle);
 
-  /* TODO: */
+  /* Copy angle */
 
-  ASSERT(0);
+  anorm = a;
+
+  /* Normalize angle to [-PI, PI] */
+
+  angle_norm_2pi_b16(&anorm, -b16PI, b16PI);
+
+  /* Normalize angle to [-1, 1] */
+
+  anorm = b16mulb16(anorm, onebypi);
+
+  /* Get cosine and sine from single call */
+
+  io.func      = CORDIC_CALC_FUNC_COS;
+  io.res2_incl = true;
+  io.arg1      = b16toq31(anorm);
+  io.arg2      = b16toq31(b16ONE);
+  io.res1      = 0;
+  io.res2      = 0;
+
+  ret = ioctl(fd, MATHIOC_CORDIC_CALC, (unsigned long)((uintptr_t)&io));
+  if (ret < 0)
+    {
+      FOCLIBERR("ERROR: MATHIOC_CORDIC_CALC failed, errno=%d\n", errno);
+    }
+
+  /* Fill phase angle struct */
+
+  angle->angle = a;
+  angle->cos   = q31tob16(io.res1);
+  angle->sin   = q31tob16(io.res2);
 
   return OK;
 }
+#endif  /* CONFIG_INDUSTRY_FOC_CORDIC_ANGLE */

industry/foc/fixed16/foc_handler.c

@@ -26,6 +26,7 @@
 
 #include <assert.h>
 #include <errno.h>
+#include <fcntl.h>
 
 #include "industry/foc/foc_log.h"
 #include "industry/foc/foc_common.h"
@@ -104,6 +105,18 @@ int foc_handler_init_b16(FAR foc_handler_b16_t *h,
       goto errout;
     }
 
+#ifdef CONFIG_INDUSTRY_FOC_CORDIC
+  /* Open CORDIC device */
+
+  h->fd = open(CONFIG_INDUSTRY_FOC_CORDIC_DEVPATH, O_RDWR);
+  if (h->fd < 0)
+    {
+      FOCLIBERR("ERROR: failed to open CORDIC device %s %d\n",
+                CONFIG_INDUSTRY_FOC_CORDIC_DEVPATH, errno);
+      goto errout;
+    }
+#endif
+
 errout:
   return ret;
 }
@@ -133,6 +146,12 @@ int foc_handler_deinit_b16(FAR foc_handler_b16_t *h)
 
   h->ops.ctrl->deinit(h);
 
+#ifdef CONFIG_INDUSTRY_FOC_CORDIC
+  /* Close CORDIC device */
+
+  close(h->fd);
+#endif
+
   /* Reset data */
 
   memset(h, 0, sizeof(foc_handler_b16_t));

industry/foc/fixed16/foc_picontrol.c

@@ -235,7 +235,7 @@ static void foc_control_input_set_b16(FAR foc_handler_b16_t *h,
 
   /* Update phase angle */
 
-#ifndef CONFIG_INDUSTRY_FOC_CORDIC
+#ifndef CONFIG_INDUSTRY_FOC_CORDIC_ANGLE
   phase_angle_update_b16(&foc->angle, angle);
 #else
   foc_cordic_angle_b16(h->fd, &foc->angle, angle);
@@ -281,7 +281,7 @@ static void foc_control_voltage_run_b16(FAR foc_handler_b16_t *h,
 
   /* Saturate voltage DQ vector */
 
-#ifndef CONFIG_INDUSTRY_FOC_CORDIC
+#ifndef CONFIG_INDUSTRY_FOC_CORDIC_DQSAT
   dq_saturate_b16(dq_ref, mag_max);
 #else
   foc_cordic_dqsat_b16(h->fd, dq_ref, mag_max);
@@ -343,7 +343,7 @@ static void foc_control_current_run_b16(FAR foc_handler_b16_t *h,
 
   /* Saturate voltage DQ vector */
 
-#ifndef CONFIG_INDUSTRY_FOC_CORDIC
+#ifndef CONFIG_INDUSTRY_FOC_CORDIC_DQSAT
   dq_saturate_b16(dq_ref, mag_max);
 #else
   foc_cordic_dqsat_b16(h->fd, dq_ref, mag_max);

industry/foc/float/foc_cordic.c

@@ -24,14 +24,25 @@
 
 #include <nuttx/config.h>
 
-#include <assert.h>
+#include <sys/ioctl.h>
 
+#include <assert.h>
+#include <fcntl.h>
+#include <debug.h>
+#include <errno.h>
+
+#include <nuttx/math/cordic.h>
+#include <nuttx/math/math_ioctl.h>
+
+#include "industry/foc/foc_log.h"
 #include "industry/foc/float/foc_cordic.h"
 
 /****************************************************************************
  * Public Functions
  ****************************************************************************/
 
+#ifdef CONFIG_INDUSTRY_FOC_CORDIC_DQSAT
+
 /****************************************************************************
  * Name: foc_cordic_dqsat_f32
  *
@@ -47,14 +58,70 @@
 
 int foc_cordic_dqsat_f32(int fd, FAR dq_frame_f32_t *dq, float mag_max)
 {
+  struct cordic_calc_s io;
+  float                dnorm   = 0.0f;
+  float                qnorm   = 0.0f;
+  float                dqscale = 0.0f;
+  float                mag     = 0.0f;
+  float                tmp     = 0.0f;
+  int                  ret     = OK;
+
   DEBUGASSERT(dq);
 
-  /* TODO: */
+  /* Normalize DQ to [-1, 1] */
 
-  ASSERT(0);
+  if (dq->d > dq->q)
+    {
+      dqscale = (1.0f / dq->d);
+    }
+  else
+    {
+      dqscale = (1.0f / dq->q);
+    }
+
+  dnorm = dq->d * dqscale;
+  qnorm = dq->d * dqscale;
+
+  /* Get modulus */
+
+  io.func      = CORDIC_CALC_FUNC_MOD;
+  io.res2_incl = false;
+  io.arg1      = ftoq31(dnorm);
+  io.arg2      = ftoq31(qnorm);
+  io.res1      = 0;
+  io.res2      = 0;
+
+  ret = ioctl(fd, MATHIOC_CORDIC_CALC, (unsigned long)((uintptr_t)&io));
+  if (ret < 0)
+    {
+      FOCLIBERR("ERROR: MATHIOC_CORDIC_CALC failed, errno=%d\n", errno);
+    }
+
+  /* Get real magnitude */
+
+  mag = q31tof(io.res1) * dqscale;
+
+  /* Magnitude bottom limit */
+
+  if (mag < 1e-10f)
+    {
+      mag = 1e-10f;
+    }
+
+  if (mag > mag_max)
+    {
+      /* Saturate vector */
+
+      tmp = mag_max / mag;
+      dq->d *= tmp;
+      dq->q *= tmp;
+    }
 
   return OK;
 }
+#endif  /* CONFIG_INDUSTRY_FOC_CORDIC_DQSAT */
+
+#ifdef CONFIG_INDUSTRY_FOC_CORDIC_ANGLE
 
 /****************************************************************************
  * Name: foc_cordic_angle_f32
@@ -71,11 +138,46 @@ int foc_cordic_dqsat_f32(int fd, FAR dq_frame_f32_t *dq, float mag_max)
 
 int foc_cordic_angle_f32(int fd, FAR phase_angle_f32_t *angle, float a)
 {
+  const float          onebypi = (1.0f / M_PI_F);
+  struct cordic_calc_s io;
+  float                anorm   = 0.0f;
+  int                  ret     = OK;
+
   DEBUGASSERT(angle);
 
-  /* TODO: */
+  /* Copy angle */
 
-  ASSERT(0);
+  anorm = a;
+
+  /* Normalize angle to [-PI, PI] */
+
+  angle_norm_2pi(&anorm, -M_PI_F, M_PI_F);
+
+  /* Normalize angle to [-1, 1] */
+
+  anorm = anorm * onebypi;
+
+  /* Get cosine and sine from single call */
+
+  io.func      = CORDIC_CALC_FUNC_COS;
+  io.res2_incl = true;
+  io.arg1      = ftoq31(anorm);
+  io.arg2      = ftoq31(1.0f);
+  io.res1      = 0;
+  io.res2      = 0;
+
+  ret = ioctl(fd, MATHIOC_CORDIC_CALC, (unsigned long)((uintptr_t)&io));
+  if (ret < 0)
+    {
+      FOCLIBERR("ERROR: MATHIOC_CORDIC_CALC failed, errno=%d\n", errno);
+    }
+
+  /* Fill phase angle struct */
+
+  angle->angle = a;
+  angle->cos   = q31tof(io.res1);
+  angle->sin   = q31tof(io.res2);
 
   return OK;
 }
+#endif  /* CONFIG_INDUSTRY_FOC_CORDIC_ANGLE */

industry/foc/float/foc_handler.c

@@ -26,6 +26,7 @@
 
 #include <assert.h>
 #include <errno.h>
+#include <fcntl.h>
 
 #include "industry/foc/foc_log.h"
 #include "industry/foc/foc_common.h"
@@ -104,6 +105,18 @@ int foc_handler_init_f32(FAR foc_handler_f32_t *h,
       goto errout;
     }
 
+#ifdef CONFIG_INDUSTRY_FOC_CORDIC
+  /* Open CORDIC device */
+
+  h->fd = open(CONFIG_INDUSTRY_FOC_CORDIC_DEVPATH, O_RDWR);
+  if (h->fd < 0)
+    {
+      FOCLIBERR("ERROR: failed to open CORDIC device %s %d\n",
+                CONFIG_INDUSTRY_FOC_CORDIC_DEVPATH, errno);
+      goto errout;
+    }
+#endif
+
 errout:
   return ret;
 }
@@ -133,6 +146,12 @@ int foc_handler_deinit_f32(FAR foc_handler_f32_t *h)
 
   h->ops.ctrl->deinit(h);
 
+#ifdef CONFIG_INDUSTRY_FOC_CORDIC
+  /* Close CORDIC device */
+
+  close(h->fd);
+#endif
+
   /* Reset data */
 
   memset(h, 0, sizeof(foc_handler_f32_t));

industry/foc/float/foc_picontrol.c

@@ -235,7 +235,7 @@ static void foc_control_input_set_f32(FAR foc_handler_f32_t *h,
 
   /* Update phase angle */
 
-#ifndef CONFIG_INDUSTRY_FOC_CORDIC
+#ifndef CONFIG_INDUSTRY_FOC_CORDIC_ANGLE
   phase_angle_update(&foc->angle, angle);
 #else
   foc_cordic_angle_f32(h->fd, &foc->angle, angle);
@@ -281,7 +281,7 @@ static void foc_control_voltage_run_f32(FAR foc_handler_f32_t *h,
 
   /* Saturate voltage DQ vector */
 
-#ifndef CONFIG_INDUSTRY_FOC_CORDIC
+#ifndef CONFIG_INDUSTRY_FOC_CORDIC_DQSAT
   dq_saturate(dq_ref, mag_max);
 #else
   foc_cordic_dqsat_f32(h->fd, dq_ref, mag_max);
@@ -344,7 +344,7 @@ static void foc_control_current_run_f32(FAR foc_handler_f32_t *h,
 
   /* Saturate voltage DQ vector */
 
-#ifndef CONFIG_INDUSTRY_FOC_CORDIC
+#ifndef CONFIG_INDUSTRY_FOC_CORDIC_DQSAT
   dq_saturate(dq_ref, mag_max);
 #else
   foc_cordic_dqsat_f32(h->fd, dq_ref, mag_max);