industrial/foc: add an interface that returns the modulation state

Useful for debugging and demonstrating FOC operation
apache · Oct 5, 2023 · a56f092 · a56f092
1 parent 97e2be0
commit a56f092
Show file tree

Hide file tree

Showing 8 changed files with 108 additions and 12 deletions.
diff --git a/examples/foc/foc_fixed16_thr.c b/examples/foc/foc_fixed16_thr.c
@@ -127,7 +127,9 @@ static int foc_handler_run(FAR struct foc_motor_b16_s *motor,
 
   /* Get FOC handler state */
 
-  foc_handler_state_b16(&motor->handler, &motor->foc_state);
+  foc_handler_state_b16(&motor->handler,
+                        &motor->foc_state,
+                        NULL);
 
   return ret;
 }

diff --git a/examples/foc/foc_float_thr.c b/examples/foc/foc_float_thr.c
@@ -127,7 +127,9 @@ static int foc_handler_run(FAR struct foc_motor_f32_s *motor,
 
   /* Get FOC handler state */
 
-  foc_handler_state_f32(&motor->handler, &motor->foc_state);
+  foc_handler_state_f32(&motor->handler,
+                        &motor->foc_state,
+                        NULL);
 
   return ret;
 }

diff --git a/include/industry/foc/fixed16/foc_handler.h b/include/industry/foc/fixed16/foc_handler.h
@@ -104,6 +104,11 @@ struct foc_modulation_ops_b16_s
   CODE void (*run)(FAR foc_handler_b16_t *h,
                    FAR ab_frame_b16_t *v_ab_mod,
                    FAR b16_t *duty);
+
+  /* Get modulation state */
+
+  CODE void (*state_get)(FAR foc_handler_b16_t *h,
+                         FAR void *state);
 };
 
 /* Current/voltage controller operations */
@@ -235,7 +240,8 @@ void foc_handler_cfg_b16(FAR foc_handler_b16_t *h,
  ****************************************************************************/
 
 void foc_handler_state_b16(FAR foc_handler_b16_t *h,
-                           FAR struct foc_state_b16_s *state);
+                           FAR struct foc_state_b16_s *state,
+                           FAR void *mod_state);
 
 #ifdef CONFIG_INDUSTRY_FOC_HANDLER_PRINT
 /****************************************************************************

diff --git a/include/industry/foc/float/foc_handler.h b/include/industry/foc/float/foc_handler.h
@@ -104,6 +104,11 @@ struct foc_modulation_ops_f32_s
   CODE void (*run)(FAR foc_handler_f32_t *h,
                    FAR ab_frame_f32_t *v_ab_mod,
                    FAR float *duty);
+
+  /* Get modulation state */
+
+  CODE void (*state_get)(FAR foc_handler_f32_t *h,
+                         FAR void *state);
 };
 
 /* Current/voltage controller operations */
@@ -236,7 +241,8 @@ void foc_handler_cfg_f32(FAR foc_handler_f32_t *h,
  ****************************************************************************/
 
 void foc_handler_state_f32(FAR foc_handler_f32_t *h,
-                           FAR struct foc_state_f32_s *state);
+                           FAR struct foc_state_f32_s *state,
+                           FAR void *mod_state);
 
 #ifdef CONFIG_INDUSTRY_FOC_HANDLER_PRINT
 /****************************************************************************

diff --git a/industry/foc/fixed16/foc_handler.c b/industry/foc/fixed16/foc_handler.c
@@ -310,18 +310,25 @@ int foc_handler_run_b16(FAR foc_handler_b16_t *h,
  *   Get FOC handler state (fixed16)
  *
  * Input Parameter:
- *   h     - pointer to FOC handler
- *   state - pointer to FOC state data
+ *   h         - pointer to FOC handler
+ *   state     - pointer to FOC state data
+ *   mod_state - pointer to modulation state data (optional)
  *
  ****************************************************************************/
 
 void foc_handler_state_b16(FAR foc_handler_b16_t *h,
-                           FAR struct foc_state_b16_s *state)
+                           FAR struct foc_state_b16_s *state,
+                           FAR void *mod_state)
 {
   DEBUGASSERT(h);
   DEBUGASSERT(state);
 
   h->ops.ctrl->state_get(h, state);
+
+  if (mod_state)
+    {
+      h->ops.mod->state_get(h, mod_state);
+    }
 }
 
 #ifdef CONFIG_INDUSTRY_FOC_HANDLER_PRINT

diff --git a/industry/foc/fixed16/foc_svm3.c b/industry/foc/fixed16/foc_svm3.c
@@ -73,6 +73,8 @@ static void foc_modulation_vbase_get_b16(FAR foc_handler_b16_t *h,
 static void foc_modulation_run_b16(FAR foc_handler_b16_t *h,
                                    FAR ab_frame_b16_t *v_ab_mod,
                                    FAR b16_t *duty);
+static void foc_modulation_state_b16(FAR foc_handler_b16_t *h,
+                                     FAR void *v_priv);
 
 /****************************************************************************
  * Public Data
@@ -88,6 +90,7 @@ struct foc_modulation_ops_b16_s g_foc_mod_svm3_b16 =
   .current   = foc_modulation_current_b16,
   .vbase_get = foc_modulation_vbase_get_b16,
   .run       = foc_modulation_run_b16,
+  .state_get = foc_modulation_state_b16,
 };
 
 /****************************************************************************
@@ -243,7 +246,7 @@ static void foc_modulation_current_b16(FAR foc_handler_b16_t *h,
 }
 
 /****************************************************************************
- * Name: foc_modulation_b16
+ * Name: foc_modulation_run_b16
  *
  * Description:
  *   Handle the SVM3 modulation (fixed16)
@@ -286,3 +289,33 @@ static void foc_modulation_run_b16(FAR foc_handler_b16_t *h,
   f_saturate_b16(&duty[1], 0, svm->cfg.pwm_duty_max);
   f_saturate_b16(&duty[2], 0, svm->cfg.pwm_duty_max);
 }
+
+/****************************************************************************
+ * Name: foc_modulation_state_b16
+ *
+ * Description:
+ *   Get the SVM3 modulation state (fixed16)
+ *
+ * Input Parameter:
+ *   h     - pointer to FOC handler
+ *   state - pointer to modulation specific data
+ *
+ ****************************************************************************/
+
+static void foc_modulation_state_b16(FAR foc_handler_b16_t *h,
+                                     FAR void *state)
+{
+  FAR struct foc_svm3mod_b16_s *svm = NULL;
+
+  DEBUGASSERT(h);
+  DEBUGASSERT(state);
+
+  /* Get modulation data */
+
+  DEBUGASSERT(h->modulation);
+  svm = h->modulation;
+
+  /* Copy data */
+
+  memcpy(state, &svm->state, sizeof(struct svm3_state_b16_s));
+}
diff --git a/industry/foc/float/foc_handler.c b/industry/foc/float/foc_handler.c
@@ -310,18 +310,25 @@ int foc_handler_run_f32(FAR foc_handler_f32_t *h,
  *   Get FOC handler state (float32)
  *
  * Input Parameter:
- *   h     - pointer to FOC handler
- *   state - pointer to FOC state data
+ *   h         - pointer to FOC handler
+ *   state     - pointer to FOC state data
+ *   mod_state - pointer to modulation state data (optional)
  *
  ****************************************************************************/
 
 void foc_handler_state_f32(FAR foc_handler_f32_t *h,
-                           FAR struct foc_state_f32_s *state)
+                           FAR struct foc_state_f32_s *state,
+                           FAR void *mod_state)
 {
   DEBUGASSERT(h);
   DEBUGASSERT(state);
 
   h->ops.ctrl->state_get(h, state);
+
+  if (mod_state)
+    {
+      h->ops.mod->state_get(h, mod_state);
+    }
 }
 
 #ifdef CONFIG_INDUSTRY_FOC_HANDLER_PRINT

diff --git a/industry/foc/float/foc_svm3.c b/industry/foc/float/foc_svm3.c
@@ -73,6 +73,8 @@ static void foc_modulation_vbase_get_f32(FAR foc_handler_f32_t *h,
 static void foc_modulation_run_f32(FAR foc_handler_f32_t *h,
                                    FAR ab_frame_f32_t *v_ab_mod,
                                    FAR float *duty);
+static void foc_modulation_state_f32(FAR foc_handler_f32_t *h,
+                                     FAR void *v_priv);
 
 /****************************************************************************
  * Public Data
@@ -88,6 +90,7 @@ struct foc_modulation_ops_f32_s g_foc_mod_svm3_f32 =
   .current   = foc_modulation_current_f32,
   .vbase_get = foc_modulation_vbase_get_f32,
   .run       = foc_modulation_run_f32,
+  .state_get = foc_modulation_state_f32,
 };
 
 /****************************************************************************
@@ -243,7 +246,7 @@ static void foc_modulation_current_f32(FAR foc_handler_f32_t *h,
 }
 
 /****************************************************************************
- * Name: foc_modulation_f32
+ * Name: foc_modulation_run_f32
  *
  * Description:
  *   Handle the SVM3 modulation (float32)
@@ -286,3 +289,33 @@ static void foc_modulation_run_f32(FAR foc_handler_f32_t *h,
   f_saturate(&duty[1], 0.0f, svm->cfg.pwm_duty_max);
   f_saturate(&duty[2], 0.0f, svm->cfg.pwm_duty_max);
 }
+
+/****************************************************************************
+ * Name: foc_modulation_state_f32
+ *
+ * Description:
+ *   Get the SVM3 modulation state (float32)
+ *
+ * Input Parameter:
+ *   h     - pointer to FOC handler
+ *   state - pointer to modulation specific data
+ *
+ ****************************************************************************/
+
+static void foc_modulation_state_f32(FAR foc_handler_f32_t *h,
+                                     FAR void *state)
+{
+  FAR struct foc_svm3mod_f32_s *svm = NULL;
+
+  DEBUGASSERT(h);
+  DEBUGASSERT(state);
+
+  /* Get modulation data */
+
+  DEBUGASSERT(h->modulation);
+  svm = h->modulation;
+
+  /* Copy data */
+
+  memcpy(state, &svm->state, sizeof(struct svm3_state_f32_s));
+}