industry/foc/*/foc_openloop.c: adapt to previous changes

industry/foc/fixed16/foc_openloop.c

@@ -32,7 +32,7 @@
 #include "industry/foc/fixed16/foc_angle.h"
 
 /****************************************************************************
- * Private Function Prototypes
+ * Private Data Types
  ****************************************************************************/
 
 /* Open-loop data */
@@ -41,6 +41,7 @@ struct foc_openloop_b16_s
 {
   struct foc_openloop_cfg_b16_s cfg;
   struct openloop_data_b16_s    data;
+  b16_t                         dir;
 };
 
 /****************************************************************************
@@ -50,9 +51,11 @@ struct foc_openloop_b16_s
 static int foc_angle_ol_init_b16(FAR foc_angle_b16_t *h);
 static void foc_angle_ol_deinit_b16(FAR foc_angle_b16_t *h);
 static int foc_angle_ol_cfg_b16(FAR foc_angle_b16_t *h, FAR void *cfg);
-static void foc_angle_ol_run_b16(FAR foc_angle_b16_t *h,
+static int foc_angle_ol_zero_b16(FAR foc_angle_b16_t *h);
-                                 FAR struct foc_angle_in_b16_s *in,
+static int foc_angle_ol_dir_b16(FAR foc_angle_b16_t *h, b16_t dir);
-                                 FAR struct foc_angle_out_b16_s *out);
+static int foc_angle_ol_run_b16(FAR foc_angle_b16_t *h,
+                                FAR struct foc_angle_in_b16_s *in,
+                                FAR struct foc_angle_out_b16_s *out);
 
 /****************************************************************************
  * Public Data
@@ -63,6 +66,8 @@ struct foc_angle_ops_b16_s g_foc_angle_ol_b16 =
   .init   = foc_angle_ol_init_b16,
   .deinit = foc_angle_ol_deinit_b16,
   .cfg    = foc_angle_ol_cfg_b16,
+  .zero   = foc_angle_ol_zero_b16,
+  .dir    = foc_angle_ol_dir_b16,
   .run    = foc_angle_ol_run_b16,
 };
 
@@ -142,7 +147,7 @@ static int foc_angle_ol_cfg_b16(FAR foc_angle_b16_t *h, FAR void *cfg)
 
   DEBUGASSERT(h);
 
-  /* Get modulation data */
+  /* Get open-loop data */
 
   DEBUGASSERT(h->data);
   ol = h->data;
@@ -156,6 +161,72 @@ static int foc_angle_ol_cfg_b16(FAR foc_angle_b16_t *h, FAR void *cfg)
   DEBUGASSERT(ol->cfg.per > 0);
   motor_openloop_init_b16(&ol->data, ol->cfg.per);
 
+  /* Initialize with CW direction */
+
+  ol->dir = DIR_CW_B16;
+
+  return OK;
+}
+
+/****************************************************************************
+ * Name: foc_angle_ol_zero_b16
+ *
+ * Description:
+ *   Zero the open-loop FOC angle handler (fixed16)
+ *
+ * Input Parameter:
+ *   h   - pointer to FOC angle handler
+ *   dir - sensor direction (1 if normal -1 if inverted)
+ *
+ ****************************************************************************/
+
+static int foc_angle_ol_zero_b16(FAR foc_angle_b16_t *h)
+{
+  FAR struct foc_openloop_b16_s *ol = NULL;
+
+  DEBUGASSERT(h);
+
+  /* Get open-loop data */
+
+  DEBUGASSERT(h->data);
+  ol = h->data;
+
+  /* Reset angle */
+
+  ol->data.angle = 0;
+
+  return OK;
+}
+
+/****************************************************************************
+ * Name: foc_angle_ol_dir_b16
+ *
+ * Description:
+ *   Set the open-loop FOC angle handler direction (fixed16)
+ *
+ * Input Parameter:
+ *   h   - pointer to FOC angle handler
+ *   dir - sensor direction (1 if normal -1 if inverted)
+ *
+ ****************************************************************************/
+
+static int foc_angle_ol_dir_b16(FAR foc_angle_b16_t *h, b16_t dir)
+{
+  FAR struct foc_openloop_b16_s *ol = NULL;
+
+  DEBUGASSERT(h);
+
+  UNUSED(dir);
+
+  /* Get open-loop data */
+
+  DEBUGASSERT(h->data);
+  ol = h->data;
+
+  /* Configure direction */
+
+  ol->dir = dir;
+
   return OK;
 }
 
@@ -172,15 +243,16 @@ static int foc_angle_ol_cfg_b16(FAR foc_angle_b16_t *h, FAR void *cfg)
  *
  ****************************************************************************/
 
-static void foc_angle_ol_run_b16(FAR foc_angle_b16_t *h,
+static int foc_angle_ol_run_b16(FAR foc_angle_b16_t *h,
-                                 FAR struct foc_angle_in_b16_s *in,
+                                FAR struct foc_angle_in_b16_s *in,
-                                 FAR struct foc_angle_out_b16_s *out)
+                                FAR struct foc_angle_out_b16_s *out)
 {
   FAR struct foc_openloop_b16_s *ol = NULL;
+  b16_t tmp = 0;
 
   DEBUGASSERT(h);
 
-  /* Get modulation data */
+  /* Get open-loop data */
 
   DEBUGASSERT(h->data);
   ol = h->data;
@@ -189,7 +261,12 @@ static void foc_angle_ol_run_b16(FAR foc_angle_b16_t *h,
 
   motor_openloop_b16(&ol->data, in->vel, in->dir);
 
+  tmp = motor_openloop_angle_get_b16(&ol->data);
+
   /* Get open-loop angle */
 
-  out->angle = motor_openloop_angle_get_b16(&ol->data);
+  out->type  = FOC_ANGLE_TYPE_ELE;
+  out->angle = b16mulb16(ol->dir, tmp);
+
+  return OK;
 }

industry/foc/float/foc_openloop.c

@@ -32,7 +32,7 @@
 #include "industry/foc/float/foc_angle.h"
 
 /****************************************************************************
- * Private Function Prototypes
+ * Private Data Types
  ****************************************************************************/
 
 /* Open-loop data */
@@ -41,6 +41,7 @@ struct foc_openloop_f32_s
 {
   struct foc_openloop_cfg_f32_s cfg;
   struct openloop_data_f32_s    data;
+  float                         dir;
 };
 
 /****************************************************************************
@@ -50,9 +51,11 @@ struct foc_openloop_f32_s
 static int foc_angle_ol_init_f32(FAR foc_angle_f32_t *h);
 static void foc_angle_ol_deinit_f32(FAR foc_angle_f32_t *h);
 static int foc_angle_ol_cfg_f32(FAR foc_angle_f32_t *h, FAR void *cfg);
-static void foc_angle_ol_run_f32(FAR foc_angle_f32_t *h,
+static int foc_angle_ol_zero_f32(FAR foc_angle_f32_t *h);
-                                 FAR struct foc_angle_in_f32_s *in,
+static int foc_angle_ol_dir_f32(FAR foc_angle_f32_t *h, float dir);
-                                 FAR struct foc_angle_out_f32_s *out);
+static int foc_angle_ol_run_f32(FAR foc_angle_f32_t *h,
+                                FAR struct foc_angle_in_f32_s *in,
+                                FAR struct foc_angle_out_f32_s *out);
 
 /****************************************************************************
  * Public Data
@@ -65,6 +68,8 @@ struct foc_angle_ops_f32_s g_foc_angle_ol_f32 =
   .init   = foc_angle_ol_init_f32,
   .deinit = foc_angle_ol_deinit_f32,
   .cfg    = foc_angle_ol_cfg_f32,
+  .zero   = foc_angle_ol_zero_f32,
+  .dir    = foc_angle_ol_dir_f32,
   .run    = foc_angle_ol_run_f32,
 };
 
@@ -144,7 +149,7 @@ static int foc_angle_ol_cfg_f32(FAR foc_angle_f32_t *h, FAR void *cfg)
 
   DEBUGASSERT(h);
 
-  /* Get modulation data */
+  /* Get open-loop data */
 
   DEBUGASSERT(h->data);
   ol = h->data;
@@ -158,6 +163,72 @@ static int foc_angle_ol_cfg_f32(FAR foc_angle_f32_t *h, FAR void *cfg)
   DEBUGASSERT(ol->cfg.per > 0.0f);
   motor_openloop_init(&ol->data, ol->cfg.per);
 
+  /* Initialize with CW direction */
+
+  ol->dir = DIR_CW;
+
+  return OK;
+}
+
+/****************************************************************************
+ * Name: foc_angle_ol_zero_f32
+ *
+ * Description:
+ *   Zero the open-loop FOC angle handler (float32)
+ *
+ * Input Parameter:
+ *   h   - pointer to FOC angle handler
+ *   dir - sensor direction (1 if normal -1 if inverted)
+ *
+ ****************************************************************************/
+
+static int foc_angle_ol_zero_f32(FAR foc_angle_f32_t *h)
+{
+  FAR struct foc_openloop_f32_s *ol = NULL;
+
+  DEBUGASSERT(h);
+
+  /* Get open-loop data */
+
+  DEBUGASSERT(h->data);
+  ol = h->data;
+
+  /* Reset angle */
+
+  ol->data.angle = 0.0f;
+
+  return OK;
+}
+
+/****************************************************************************
+ * Name: foc_angle_ol_dir_f32
+ *
+ * Description:
+ *   Set the open-loop FOC angle handler direction (float32)
+ *
+ * Input Parameter:
+ *   h   - pointer to FOC angle handler
+ *   dir - sensor direction (1 if normal -1 if inverted)
+ *
+ ****************************************************************************/
+
+static int foc_angle_ol_dir_f32(FAR foc_angle_f32_t *h, float dir)
+{
+  FAR struct foc_openloop_f32_s *ol = NULL;
+
+  DEBUGASSERT(h);
+
+  UNUSED(dir);
+
+  /* Get open-loop data */
+
+  DEBUGASSERT(h->data);
+  ol = h->data;
+
+  /* Configure direction */
+
+  ol->dir = dir;
+
   return OK;
 }
 
@@ -174,7 +245,7 @@ static int foc_angle_ol_cfg_f32(FAR foc_angle_f32_t *h, FAR void *cfg)
  *
  ****************************************************************************/
 
-static void foc_angle_ol_run_f32(FAR foc_angle_f32_t *h,
+static int foc_angle_ol_run_f32(FAR foc_angle_f32_t *h,
                                  FAR struct foc_angle_in_f32_s *in,
                                  FAR struct foc_angle_out_f32_s *out)
 {
@@ -182,7 +253,7 @@ static void foc_angle_ol_run_f32(FAR foc_angle_f32_t *h,
 
   DEBUGASSERT(h);
 
-  /* Get modulation data */
+  /* Get open-loop data */
 
   DEBUGASSERT(h->data);
   ol = h->data;
@@ -193,5 +264,8 @@ static void foc_angle_ol_run_f32(FAR foc_angle_f32_t *h,
 
   /* Get open-loop angle */
 
-  out->angle = motor_openloop_angle_get(&ol->data);
+  out->type  = FOC_ANGLE_TYPE_ELE;
+  out->angle = ol->dir * motor_openloop_angle_get(&ol->data);
+
+  return OK;
 }