/****************************************************************************
 * libs/libdsp/lib_pmsm_model.c
 *
 * 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.
 *
 ****************************************************************************/

/* Permanent magnet synchronous motor model (PMSM)
 *
 * 1) Flux equation:
 *
 *   lambda_q = L_q * i_q
 *   lambda_d = L_d * i_d + lampda_m
 *
 * 2) Voltage equation:
 *
 *   a) D-part:
 *
 *       id    Rs          Ld
 *    o-->----/\/\/\/\----mmmmmm----+
 *    ^                             |
 *    .                             |
 *    .                          .-----.
 *    .                          |  ^  |
 * vd .                          |  |  |
 *    .                          .-----.
 *    .                             | ed
 *    .                             |
 *    o-----------------------------+
 *
 *   ed = -we * lamda_q
 *   ed = -we * (Lq * iq)
 *
 *   vd = (Rs * id) + (d/dt * (Ld * id)) - (Lq * we * iq)
 *   Ld * (d/dt * id) = vd - (Rs * id) + (we * Lq * iq)
 *   (d/dt * id) = (vd - (Rs * id) + (we * Lq * iq)) / Ld
 *
 *   b) Q-part:
 *
 *       iq    Rs          Lq
 *    o-->----/\/\/\/\----mmmmmm----+
 *    ^                             |
 *    .                             |
 *    .                          .-----.
 *    .                          |  ^  |
 * vq .                          |  |  |
 *    .                          .-----.
 *    .                             | eq
 *    .                             |
 *    o-----------------------------+
 *
 *   eq = we * lamda_d
 *   eq = we * (Lq * iq + lamda_m)
 *
 *   vq = (Rs * iq) + (d/dt * (Lq * iq)) + (we * (Ld * id + lambda_m))
 *   Lq * (d/dt * iq) = vq - (Rs * iq) - (we * (Ld * id + lambda_m))
 *   (d/dt * iq) = (vq - (Rs * iq) - (we * (Ld * id + lambda_m))) / Lq
 *
 * 3) Torque equation:
 *
 *   Te = (3/2) * p * (lambda_d * i_q - lambda_q * i_d)
 *   Te = (3/2) * p * (lambda_m * i_q + (L_d - L_q) * i_q * i_d)
 *   Te = (3/2) * p * i_q * (lambda_m  + (L_d - L_q) * i_d)
 *
 * 4) Electromechanical power equation:
 *
 *   Pem = wm * Te
 *   Pem = (3/2) * wm * (lamda_d * i_q - lamda_q * i_d)
 *
 * 5) The general mechanical equation for the motor:
 *
 *   Te = Tl + Td + B * wm + J * (d/dt) * wm
 *   we = p * wm = (P/2) * wm
 *   p  = (P/2)
 *
 *   assume no friction:
 *
 *     Te = Tl + J * (d/dt) * wm
 *     (d/dt) * wm = (Te - Tl) / J
 *
 *  where:
 *   B        - viscous frictions coefficient
 *   J        - interia of the shaft and the load system
 *   Td       - dry friction
 *   Tl       - load torque
 *   Te       - electromagnetic torque
 *   Pe       - electromagnetical power
 *   we       - electrical velociti of the motor
 *   wm       - mechanical velocity of the rotor
 *   lambda_m - flux linkage
 *   P        - Number of poles
 *   p        - pole pairs
 */

/****************************************************************************
 * Included Files
 ****************************************************************************/

#include <nuttx/config.h>

#include <assert.h>
#include <dsp.h>
#include <string.h>

/****************************************************************************
 * Public Functions
 ****************************************************************************/

/****************************************************************************
 * Name: pmsm_model_initialize
 *
 * Description:
 *   Initialzie FOC model
 *
 ****************************************************************************/

int pmsm_model_initialize(FAR struct pmsm_model_f32_s *model,
                          FAR struct pmsm_phy_params_f32_s *phy,
                          float per)
{
  DEBUGASSERT(model);
  DEBUGASSERT(phy);
  DEBUGASSERT(per > 0.0f);

  /* Copy motor model parameters */

  memcpy(&model->phy, phy, sizeof(struct pmsm_phy_params_f32_s));

  /* Initialize controller period */

  model->per = per;

  return OK;
}

/****************************************************************************
 * Name: pmsm_model_elec
 *
 * Description:
 *  Update motor model electrical state
 *
 ****************************************************************************/

int pmsm_model_elec(FAR struct pmsm_model_f32_s *model,
                    FAR ab_frame_f32_t *vab)
{
  float tmp1 = 0.0f;
  float tmp2 = 0.0f;
  float tmp3 = 0.0f;
  float tmp4 = 0.0f;
  float tmp5 = 0.0f;
  float tmp6 = 0.0f;

  DEBUGASSERT(model);
  DEBUGASSERT(vab);

  /* Copy alpha-beta voltage */

  model->state.v_ab.a = vab->a;
  model->state.v_ab.b = vab->b;

  /* Inverse Clarke transform - get abc voltage */

  inv_clarke_transform(&model->state.v_ab,
                       &model->state.v_abc);

  /* Park transform - get DQ voltage */

  park_transform(&model->state.angle.angle_el,
                 &model->state.v_ab,
                 &model->state.v_dq);

  /* q current */

  tmp1 = model->phy.motor.res * model->state.i_dq.q;
  tmp2 = model->phy.ind_d * model->state.i_dq.d;
  tmp3 = tmp2 + model->phy.motor.flux_link;
  tmp4 = model->state.omega_e * tmp3;
  tmp5 = model->state.v_dq.q - tmp1 - tmp4;
  tmp6 = model->per * tmp5;

  model->iq_int += (tmp6 * model->phy.one_by_indq);

  /* d current */

  tmp1 = model->phy.motor.res * model->state.i_dq.d;
  tmp2 = model->phy.ind_q * model->state.i_dq.q;
  tmp3 = tmp2 * model->state.omega_e;
  tmp4 = model->state.v_dq.d - tmp1 + tmp3;
  tmp5 = model->per * tmp4;

  model->id_int += (tmp5 * model->phy.one_by_indd);

  /* Store currents */

  model->state.i_dq.q = model->iq_int;
  model->state.i_dq.d = model->id_int;

  /* Inverse Park transform - get alpha-beta current */

  inv_park_transform(&model->state.angle.angle_el,
                     &model->state.i_dq,
                     &model->state.i_ab);

  /* Inverse Clarke transform - get abc current */

  inv_clarke_transform(&model->state.i_ab,
                       &model->state.i_abc);

  return OK;
}

/****************************************************************************
 * Name: pmsm_model_mech
 *
 * Description:
 *  Update motor model mechanical state
 *
 ****************************************************************************/

int pmsm_model_mech(FAR struct pmsm_model_f32_s *model, float load)
{
  float angle = 0.0f;
  float dir   = 0.0f;
  float te    = 0.0f;
  float tmp1  = 0.0f;
  float tmp2  = 0.0f;
  float tmp3  = 0.0f;
  float tmp4  = 0.0f;
  float tmp5  = 0.0f;

  DEBUGASSERT(model);

  /* Get electrical torque developed by the motor */

  tmp1 = model->phy.ind_d - model->phy.ind_q;
  tmp2 = tmp1 * model->state.i_dq.d;
  tmp3 = model->phy.motor.flux_link - tmp2;
  tmp4 = 1.5f * model->phy.motor.p;
  tmp5 = tmp4 * model->state.i_dq.q;

  te =  tmp5 * tmp3;

  /* Get new mechanical velocity */

  tmp1 = te - load;
  tmp2 = model->per * tmp1 ;
  tmp3 = tmp2 * model->phy.one_by_iner;

  model->state.omega_m = (model->state.omega_m + tmp3);

  /* Get new electrical velocity */

  model->state.omega_e = model->state.omega_m * model->phy.motor.p;

  /* Get rotation direction */

  dir = (model->state.omega_e > 0 ? DIR_CW : DIR_CCW);

  /* Update electrical angle */

  tmp1 = model->state.omega_e * model->per;

  angle = model->state.angle.angle_el.angle + tmp1;

  /* Update with mechanical angel */

  motor_angle_e_update(&model->state.angle, angle, dir);

  return OK;
}