[incubator-nuttx] branch master updated: libdsp/fixed16: add openloop handler

[ac...@apache.org]

This is an automated email from the ASF dual-hosted git repository.

acassis pushed a commit to branch master
in repository https://gitbox.apache.org/repos/asf/incubator-nuttx.git


The following commit(s) were added to refs/heads/master by this push:
     new 55517a5  libdsp/fixed16: add openloop handler
55517a5 is described below

commit 55517a51e0c4b75cc742a19b933f7e8e1a4b119c
Author: raiden00pl <ra...@railab.me>
AuthorDate: Thu Apr 1 13:41:37 2021 +0200

    libdsp/fixed16: add openloop handler
---
 include/dspb16.h            | 15 ++++++++
 libs/libdsp/lib_motor_b16.c | 91 +++++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 106 insertions(+)

diff --git a/include/dspb16.h b/include/dspb16.h
index 127bea668..8ea6330 100644
--- a/include/dspb16.h
+++ b/include/dspb16.h
@@ -242,6 +242,14 @@ struct svm3_state_b16_s
   b16_t     d_w;             /* Duty cycle for phase W */
 };
 
+/* Motor open-loop control data */
+
+struct openloop_data_b16_s
+{
+  b16_t angle;         /* Open-loop current angle normalized to <0.0, 2PI> */
+  b16_t per;           /* Open-loop control execution period */
+};
+
 /* FOC initialize data */
 
 struct foc_initdata_b16_s
@@ -441,6 +449,13 @@ void foc_vabmod_get_b16(FAR struct foc_data_b16_s *foc,
                         FAR ab_frame_b16_t *v_ab_mod);
 void foc_vdq_mag_max_get_b16(FAR struct foc_data_b16_s *foc, FAR b16_t *max);
 
+/* Motor openloop control */
+
+void motor_openloop_init_b16(FAR struct openloop_data_b16_s *op, b16_t per);
+void motor_openloop_b16(FAR struct openloop_data_b16_s *op, b16_t speed,
+                        b16_t dir);
+b16_t motor_openloop_angle_get_b16(FAR struct openloop_data_b16_s *op);
+
 /* Motor angle */
 
 void motor_angle_init_b16(FAR struct motor_angle_b16_s *angle, uint8_t p);
diff --git a/libs/libdsp/lib_motor_b16.c b/libs/libdsp/lib_motor_b16.c
index f32f484..fca3ff0 100644
--- a/libs/libdsp/lib_motor_b16.c
+++ b/libs/libdsp/lib_motor_b16.c
@@ -35,6 +35,97 @@
  ****************************************************************************/
 
 /****************************************************************************
+ * Name: motor_openloop_init_b16
+ *
+ * Description:
+ *  Initialize open-loop data
+ *
+ * Input Parameters:
+ *   op  - (in/out) pointer to the openloop data structure
+ *   per - (in) period of the open-loop control
+ *
+ * Returned Value:
+ *   None
+ *
+ ****************************************************************************/
+
+void motor_openloop_init_b16(FAR struct openloop_data_b16_s *op, b16_t per)
+{
+  LIBDSP_DEBUGASSERT(op != NULL);
+  LIBDSP_DEBUGASSERT(per > 0);
+
+  /* Reset openloop structure */
+
+  memset(op, 0, sizeof(struct openloop_data_b16_s));
+
+  /* Initialize data */
+
+  op->per = per;
+}
+
+/****************************************************************************
+ * Name: motor_openloop_b16
+ *
+ * Description:
+ *   One step of the open-loop control
+ *
+ * Input Parameters:
+ *   op    - (in/out) pointer to the open-loop data structure
+ *   speed - (in) open-loop speed
+ *   dir   - (in) rotation direction
+ *
+ * Returned Value:
+ *   None
+ *
+ ****************************************************************************/
+
+void motor_openloop_b16(FAR struct openloop_data_b16_s *op, b16_t speed,
+                        b16_t dir)
+{
+  LIBDSP_DEBUGASSERT(op != NULL);
+  LIBDSP_DEBUGASSERT(speed >= 0);
+  LIBDSP_DEBUGASSERT(dir == DIR_CW_B16 || dir == DIR_CCW_B16);
+
+  b16_t phase_step = 0;
+  b16_t tmp = 0;
+
+  /* Get phase step */
+
+  tmp = b16mulb16(dir, speed);
+  phase_step =  b16mulb16(tmp, op->per);
+
+  /* Update open-loop angle */
+
+  op->angle += phase_step;
+
+  /* Normalize the open-loop angle to 0.0 - 2PI range */
+
+  angle_norm_2pi_b16(&op->angle, MOTOR_ANGLE_E_MIN_B16,
+                     MOTOR_ANGLE_E_MAX_B16);
+}
+
+/****************************************************************************
+ * Name: motor_openloop_angle_get_b16
+ *
+ * Description:
+ *   Get angle from open-loop controller
+ *
+ * Input Parameters:
+ *   op    - (in/out) pointer to the open-loop data structure
+ *
+ * Returned Value:
+ *   Return angle from open-loop controller
+ *
+ ****************************************************************************/
+
+b16_t motor_openloop_angle_get_b16(FAR struct openloop_data_b16_s *op)
+{
+  LIBDSP_DEBUGASSERT(op != NULL);
+
+  return op->angle;
+}
+
+/****************************************************************************
  * Name: motor_angle_e_update_b16
  *
  * Description: