text: /****************************************************************************
   * include/dsp.h
   *
   * Licensed to the Apache Software Foundation (ASF) under one or more
   * contributor license agreements.  See the NOTICE file distributed with
   * this work for additional information regarding copyright ownership.  The
   * ASF licenses this file to you under the Apache License, Version 2.0 (the
   * "License"); you may not use this file except in compliance with the
   * License.  You may obtain a copy of the License at
   *
   *   http://www.apache.org/licenses/LICENSE-2.0
   *
   * Unless required by applicable law or agreed to in writing, software
   * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
   * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.  See the
   * License for the specific language governing permissions and limitations
   * under the License.
   *
   ****************************************************************************/
  
  #ifndef __INCLUDE_DSP_H
  #define __INCLUDE_DSP_H
  
  /****************************************************************************
   * Included Files
   ****************************************************************************/
  
  #include <nuttx/compiler.h>
  #include <stddef.h>
  #include <stdint.h>
  #include <string.h>
  #include <stdbool.h>
  #include <math.h>
  
  #include <assert.h>
  
  /****************************************************************************
   * Pre-processor Definitions
   ****************************************************************************/
  
  /* Disable DEBUGASSERT macro if LIBDSP debug is not enabled */
  
  #ifdef CONFIG_LIBDSP_DEBUG
  #  ifndef CONFIG_DEBUG_ASSERTIONS
  #    warning "Need CONFIG_DEBUG_ASSERTIONS to work properly"
  #  endif
  #else
  #  undef DEBUGASSERT
  #  define DEBUGASSERT(x)
  #endif
  
  #ifndef CONFIG_LIBDSP_PRECISION
  #  define CONFIG_LIBDSP_PRECISION 0
  #endif
  
  /* Phase rotation direction */
  
  #define DIR_CW   (1.0f)
  #define DIR_CCW  (-1.0f)
  
  /* Some math constants ******************************************************/
  
  #define SQRT3_BY_TWO_F     (0.866025f)
  #define SQRT3_BY_THREE_F   (0.57735f)
  #define ONE_BY_SQRT3_F     (0.57735f)
  #define TWO_BY_SQRT3_F     (1.15470f)
  
  /* Some lib constants *******************************************************/
  
  /* Motor electrical angle is in range 0.0 to 2*PI */
  
  #define MOTOR_ANGLE_E_MAX    (2.0f*M_PI_F)
  #define MOTOR_ANGLE_E_MIN    (0.0f)
  #define MOTOR_ANGLE_E_RANGE  (MOTOR_ANGLE_E_MAX - MOTOR_ANGLE_E_MIN)
  
  /* Motor mechanical angle is in range 0.0 to 2*PI */
  
  #define MOTOR_ANGLE_M_MAX    (2.0f*M_PI_F)
  #define MOTOR_ANGLE_M_MIN    (0.0f)
  #define MOTOR_ANGLE_M_RANGE  (MOTOR_ANGLE_M_MAX - MOTOR_ANGLE_M_MIN)
  
  /* Some useful macros *******************************************************/
  
  /****************************************************************************
   * Name: LP_FILTER
   *
   * Description:
   *   Simple single-pole digital low pass filter:
   *     Y(n) = (1-beta)*Y(n-1) + beta*X(n) = (beta * (Y(n-1) - X(n)))
   *
   *     filter - (0.0 - 1.0) where 1.0 gives unfiltered values
   *     filter = T * (2*PI) * f_c
   *
   *     phase shift = -arctan(f_in/f_c)
   *
   *     T    - period at which the digital filter is being calculated
   *     f_in - input frequency of the filter
   *     f_c  - cutoff frequency of the filter
   *
   * REFERENCE: https://www.embeddedrelated.com/showarticle/779.php
   *
   ****************************************************************************/
  
  #define LP_FILTER(val, sample, filter) val -= (filter * (val - sample))
  
  /****************************************************************************
   * Name: SVM3_BASE_VOLTAGE_GET
   *
   * Description:
   *  Get maximum voltage for SVM3 without overmodulation
   *
   *  Notes:
   *   max possible phase voltage for 3-phase power inwerter:
   *     Vd = (2/3)*Vdc
   *   max phase reference voltage according to SVM modulation diagram:
   *     Vrefmax = Vd * cos(30*) = SQRT3_BY_2 * Vd
   *   which give us:
   *     Vrefmax = SQRT3_BY_3 * Vdc
   *
   *   Vdc - bus voltage
   *
   ****************************************************************************/
  
  #define SVM3_BASE_VOLTAGE_GET(vbus) (vbus * SQRT3_BY_THREE_F)
  
  /****************************************************************************
   * Public Types
   ****************************************************************************/
  
  /* This structure represents phase angle.
   * Besides angle value it also stores sine and cosine values for given angle.
   */
  
  struct phase_angle_s
  {
    float   angle;               /* Phase angle in radians <0, 2PI> */
    float   sin;                 /* Phase angle sine */
    float   cos;                 /* Phase angle cosine */
  };
  
  typedef struct phase_angle_s phase_angle_t;
  
  /* This structure stores motor angles and corresponding sin and cos values
   *
   * th_el = th_m * pole_pairs
   * th_m = th_el/pole_pairs
   *
   * where:
   *   th_el      - motor electrical angle
   *   th_m       - motor mechanical angle
   *   pole_pairs - motor pole pairs
   *
   *  NOTE: pole_pairs = poles_total/2
   */
  
  struct motor_angle_s
  {
    phase_angle_t angle_el;      /* Electrical angle */
    float         anglem;        /* Mechanical angle in radians <0, 2PI> */
    float         one_by_p;      /* Aux variable */
    uint8_t       p;             /* Number of the motor pole pairs */
    int8_t        i;             /* Pole counter */
  };
  
  /* Float number saturaton */
  
  struct float_sat_s
  {
    float min;                    /* Lower limit */
    float max;                    /* Upper limit */
  };
  
  typedef struct float_sat_s float_sat_t;
  
  /* PI/PID controller state structure */
  
  struct pid_controller_s
  {
    float       out;              /* Controller output */
    float_sat_t sat;              /* Output saturation */
    float       err;              /* Current error value */
    float       err_prev;         /* Previous error value */
    float       KP;               /* Proportional coefficient */
    float       KI;               /* Integral coefficient */
    float       KD;               /* Derivative coefficient */
    float       part[3];          /* 0 - proporitonal part
                                   * 1 - integral part
                                   * 2 - derivative part
                                   */
  };
  
  typedef struct pid_controller_s pid_controller_t;
  
  /* This structure represents the ABC frame (3 phase vector) */
  
  struct abc_frame_s
  {
    float a;                     /* A component */
    float b;                     /* B component */
    float c;                     /* C component */
  };
  
  typedef struct abc_frame_s abc_frame_t;
  
  /* This structure represents the alpha-beta frame (2 phase vector) */
  
  struct ab_frame_s
  {
    float a;                     /* Alpha component */
    float b;                     /* Beta component */
  };
  
  typedef struct ab_frame_s ab_frame_t;
  
  /* This structure represent the direct-quadrature frame */
  
  struct dq_frame_s
  {
    float d;                     /* Driect component */
    float q;                     /* Quadrature component */
  };
  
  typedef struct dq_frame_s dq_frame_t;
  
  /* Space Vector Modulation data for 3-phase system */
  
  struct svm3_state_s
  {
    uint8_t     sector;          /* Current space vector sector */
    float       d_u;             /* Duty cycle for phase U */
    float       d_v;             /* Duty cycle for phase V */
    float       d_w;             /* Duty cycle for phase W */
    float       d_max;           /* Duty cycle max */
    float       d_min;           /* Duty cycle min */
  };
  
  /* Motor open-loop control data */
  
  struct openloop_data_s
  {
    float max;           /* Open-loop max speed */
    float angle;         /* Open-loop current angle normalized to <0.0, 2PI> */
    float per;           /* Open-loop control execution period */
  };
  
  /* Common motor observer structure */
  
  struct motor_observer_s
  {
    float angle;               /* Estimated observer angle */
    float speed;               /* Estimated observer speed */
    float per;                 /* Observer execution period */
  
    float angle_err;           /* Observer angle error.
                                * This can be used to gradually eliminate
                                * error between openloop angle and observer
                                * angle
                                */
  
    /* There are different types of motor observers which different
     * sets of private data.
     */
  
    void *so;                  /* Speed estimation observer data */
    void *ao;                  /* Angle estimation observer data */
  };
  
  /* Speed observer division method data */
  
  struct motor_sobserver_div_s
  {
    float angle_diff;             /* Mechanical angle difference */
    float angle_acc;              /* Accumulated mechanical angle */
    float angle_prev;             /* Previous mechanical angle */
    float one_by_dt;              /* Frequency of observer execution */
    float cntr;                   /* Sample counter */
    float samples;                /* Number of samples for observer */
    float filter;                 /* Low-pass filter for final omega */
  };
  
  /* Speed observer PLL method data */
  #if 0
  struct motor_sobserver_pll_s
  {
    /* TODO */
  };
  #endif
  
  /* Motor Sliding Mode Observer private data */
  
  struct motor_observer_smo_s
  {
    float k_slide;        /* Bang-bang controller gain */
    float err_max;        /* Linear mode threshold */
    float F;              /* Current observer F gain (1-Ts*R/L) */
    float G;              /* Current observer G gain (Ts/L) */
    float emf_lp_filter1; /* Adaptive first low pass EMF filter */
    float emf_lp_filter2; /* Adaptive second low pass EMF filter */
    ab_frame_t emf;       /* Estimated back-EMF */
    ab_frame_t emf_f;     /* Fitlered estimated back-EMF */
    ab_frame_t z;         /* Correction factor */
    ab_frame_t i_est;     /* Estimated idq current */
    ab_frame_t v_err;     /* v_err = v_ab - emf */
    ab_frame_t i_err;     /* i_err = i_est - i_dq */
    ab_frame_t sign;      /* Bang-bang controller sign */
  };
  
  /* Motor physical parameters.
   * This data structure was designed to work with BLDC/PMSM motors,
   * but probably can be used to describe different types of motors.
   */
  
  struct motor_phy_params_s
  {
    uint8_t p;                   /* Number of the motor pole pairs */
  
    float   res;                 /* Phase-to-neutral temperature compensated
                                  * resistance
                                  */
    float   res_base;            /* Phase-to-neutral base resistance */
    float   res_alpha;           /* Temperature coefficient of resistance */
    float   res_temp_ref;        /* Reference temperature of alpha */
    float   ind;                 /* Average phase-to-neutral inductance */
    float   one_by_ind;          /* Inverse phase-to-neutral inductance */
  };
  
  /* Field oriented control (FOC) data
   * REVISIT:
   */
  
  struct foc_data_s
  {
    abc_frame_t      v_abc;    /* Voltage in ABC frame */
    ab_frame_t       v_ab;     /* Voltage in alpha-beta frame */
    dq_frame_t       v_dq;     /* Voltage in dq frame */
    ab_frame_t       v_ab_mod; /* Modulation voltage normalized to
                                * magnitude (0.0, 1.0)
                                */
  
    abc_frame_t      i_abc;    /* Current in ABC frame */
    ab_frame_t       i_ab;     /* Current in apha-beta frame */
    dq_frame_t       i_dq;     /* Current in dq frame */
    dq_frame_t       i_dq_err; /* DQ current error */
  
    dq_frame_t       i_dq_ref; /* Current dq reference frame */
    pid_controller_t id_pid;   /* Current d-axis component PI controller */
    pid_controller_t iq_pid;   /* Current q-axis component PI controller */
  
    float vdq_mag_max;         /* Maximum dq voltage magnitude */
    float vab_mod_scale;       /* Voltage alpha-beta modulation scale */
  };
  
  /****************************************************************************
   * Public Functions Prototypes
   ****************************************************************************/
  
  #undef EXTERN
  #if defined(__cplusplus)
  #define EXTERN extern "C"
  extern "C"
  {
  #else
  #define EXTERN extern
  #endif
  
  /* Math functions */
  
  float fast_sin(float angle);
  float fast_sin2(float angle);
  float fast_cos(float angle);
  float fast_cos2(float angle);
  float fast_atan2(float y, float x);
  
  void f_saturate(FAR float *val, float min, float max);
  
  float vector2d_mag(float x, float y);
  void vector2d_saturate(FAR float *x, FAR float *y, float max);
  
  void dq_saturate(FAR dq_frame_t *dq, float max);
  float dq_mag(FAR dq_frame_t *dq);
  
  /* PID controller functions */
  
  void pid_controller_init(FAR pid_controller_t *pid,
                           float KP, float KI, float KD);
  void pi_controller_init(FAR pid_controller_t *pid,
                          float KP, float KI);
  void pid_saturation_set(FAR pid_controller_t *pid, float min, float max);
  void pi_saturation_set(FAR pid_controller_t *pid, float min, float max);
  void pid_integral_reset(FAR pid_controller_t *pid);
  void pi_integral_reset(FAR pid_controller_t *pid);
  float pi_controller(FAR pid_controller_t *pid, float err);
  float pid_controller(FAR pid_controller_t *pid, float err);
  
  /* Transformation functions */
  
  void clarke_transform(FAR abc_frame_t *abc, FAR ab_frame_t *ab);
  void inv_clarke_transform(FAR ab_frame_t *ab, FAR abc_frame_t *abc);
  void park_transform(FAR phase_angle_t *angle, FAR ab_frame_t *ab,
                      FAR dq_frame_t *dq);
  void inv_park_transform(FAR phase_angle_t *angle, FAR dq_frame_t *dq,
                          FAR ab_frame_t *ab);
  
  /* Phase angle related functions */
  
  void angle_norm(FAR float *angle, float per, float bottom, float top);
  void angle_norm_2pi(FAR float *angle, float bottom, float top);
  void phase_angle_update(FAR struct phase_angle_s *angle, float val);
  
  /* 3-phase system space vector modulation */
  
  void svm3_init(FAR struct svm3_state_s *s, float min, float max);
  void svm3(FAR struct svm3_state_s *s, FAR ab_frame_t *ab);
  void svm3_current_correct(FAR struct svm3_state_s *s,
                            int32_t *c0, int32_t *c1, int32_t *c2);
  
  /* Field Oriented control */
  
  void foc_vbase_update(FAR struct foc_data_s *foc, float vbase);
  void foc_idq_ref_set(FAR struct foc_data_s *data, float d, float q);
  
  void foc_init(FAR struct foc_data_s *data,
                float id_kp, float id_ki, float iq_kp, float iq_ki);
  void foc_process(FAR struct foc_data_s *foc,
                   FAR abc_frame_t *i_abc,
                   FAR phase_angle_t *angle);
  
  /* BLDC/PMSM motor observers */
  
  void motor_observer_init(FAR struct motor_observer_s *observer,
                           FAR void *ao, FAR void *so, float per);
  float motor_observer_speed_get(FAR struct motor_observer_s *o);
  float motor_observer_angle_get(FAR struct motor_observer_s *o);
  
  void motor_observer_smo_init(FAR struct motor_observer_smo_s *smo,
                               float kslide, float err_max);
  void motor_observer_smo(FAR struct motor_observer_s *o,
                          FAR ab_frame_t *i_ab, FAR ab_frame_t *v_ab,
                          FAR struct motor_phy_params_s *phy, float dir);
  
  void motor_sobserver_div_init(FAR struct motor_sobserver_div_s *so,
                                uint8_t samples, float filer, float per);
  void motor_sobserver_div(FAR struct motor_observer_s *o,
                           float angle, float dir);
  
  /* Motor openloop control */
  
  void motor_openloop_init(FAR struct openloop_data_s *op,
                           float max, float per);
  void motor_openloop(FAR struct openloop_data_s *op, float speed, float dir);
  float motor_openloop_angle_get(FAR struct openloop_data_s *op);
  
  /* Motor angle */
  
  void motor_angle_init(FAR struct motor_angle_s *angle, uint8_t p);
  void motor_angle_e_update(FAR struct motor_angle_s *angle,
                            float angle_new, float dir);
  void motor_angle_m_update(FAR struct motor_angle_s *angle,
                            float angle_new, float dir);
  float motor_angle_m_get(FAR struct motor_angle_s *angle);
  float motor_angle_e_get(FAR struct motor_angle_s *angle);
  
  /* Motor physical parameters functions */
  
  void motor_phy_params_init(FAR struct motor_phy_params_s *phy, uint8_t poles,
                              float res, float ind);
  void motor_phy_params_temp_set(FAR struct motor_phy_params_s *phy,
                                 float res_alpha, float res_temp_ref);
  
  #undef EXTERN
  #if defined(__cplusplus)
  }
  #endif
  
  #endif /* __INCLUDE_DSP_H */
  