nuttx/libs/libdsp/lib_motor.c

/****************************************************************************
 * control/lib_motor.c
 *
 *   Copyright (C) 2018 Gregory Nutt. All rights reserved.
 *   Author: Mateusz Szafoni <raiden00@railab.me>
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name NuttX nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

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

#include <dsp.h>

/****************************************************************************
 * Pre-processor Definitions
 ****************************************************************************/

#define POLE_CNTR_THR 0.0f

/****************************************************************************
 * Private Functions
 ****************************************************************************/

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

/****************************************************************************
 * Name: motor_openloop_init
 *
 * Description:
 *  Initialize open-loop data
 *
 * Input Parameters:
 *   op  - (in/out) pointer to the openlooop data structure
 *   max - (in) maximum openloop speed
 *   per - (in) period of the open-loop control
 *
 * Returned Value:
 *   None
 *
 ****************************************************************************/

void motor_openloop_init(FAR struct openloop_data_s *op, float max, float per)
{
  DEBUGASSERT(op != NULL);
  DEBUGASSERT(max > 0.0f);
  DEBUGASSERT(per > 0.0f);

  /* Reset openloop structure */

  memset(op, 0, sizeof(struct openloop_data_s));

  /* Initialize data */

  op->max = max;
  op->per = per;
}

/****************************************************************************
 * Name: motor_openloop
 *
 * 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(FAR struct openloop_data_s *op, float speed, float dir)
{
  DEBUGASSERT(op != NULL);
  DEBUGASSERT(speed >= 0.0f);
  DEBUGASSERT(dir == DIR_CW || dir == DIR_CCW);

  float phase_step = 0.0f;

  /* REVISIT: what should we do if speed is greater than max open-loop speed?
   *          Saturate speed or stop motor ?
   */

  if (speed > op->max)
    {
      speed = op->max;
    }

  /* Get phase step */

  phase_step = dir * speed * op->per;

  /* Update open-loop angle */

  op->angle += phase_step;

  /* Normalize the open-looop angle to 0.0 - 2PI range */

  angle_norm_2pi(&op->angle, MOTOR_ANGLE_E_MIN, MOTOR_ANGLE_E_MAX);
}

/****************************************************************************
 * Name: motor_openloop_angle_get
 *
 * 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
 *
 ****************************************************************************/

float motor_openloop_angle_get(FAR struct openloop_data_s *op)
{
  DEBUGASSERT(op != NULL);

  return op->angle;
}

/* In a multipolar electrical machines, mechanical angle is not equal
 * to mechanical angle which can be described as:
 *
 *   electrical angle = (p) * mechanical angle
 *   where p - number of poles pair
 *
 *
 * electrical
 * angle:
 *
 * poles = 4
 * i =                0    1    2    3    0    1    2    3    0
 *            2PI  ______________________________________________
 *                    /|   /|   /|   /|   /|   /|   /|   /|   /|
 *                   / |  / |  / |  / |  / |  / |  / |  / |  / |
 *                  /  | /  | /  | /  | /  | /  | /  | /  | /  |
 *            0    /___|/___|/___|/___|/___|/___|/___|/___|/___|/
 *                 .                  .                   .
 * mechanical      .                  .                   .
 * angle:          .                  .                   .
 *                 .                  .                   .
 *            2PI  .__________________.___________________.______
 *                 .                  o                   o
 *                 .             o    o              o    o
 *                 .        o         o         o         o
 *                 .   o              o    o              o     o
 *            0    o__________________o___________________o______
 *
 */

/****************************************************************************
 * Name: motor_angle_init
 *
 * Description:
 *   Initialize motor angle structure
 *
 * Input Parameters:
 *   angle - (in/out) pointer to the motor angle strucutre
 *   p     - (in) number of the motor pole pairs
 *
 * Returned Value:
 *   None
 *
 ****************************************************************************/

void motor_angle_init(FAR struct motor_angle_s *angle, uint8_t p)
{
  DEBUGASSERT(angle != NULL);
  DEBUGASSERT(p > 0);

  /* Reset structure */

  memset(angle, 0, sizeof(struct motor_angle_s));

  /* Store pole pairs */

  angle->p = p;
  angle->one_by_p = (float)1.0f/p;

  /* Initialize angle with 0.0 */

  phase_angle_update(&angle->angle_el, 0.0f);
}

/****************************************************************************
 * Name: motor_angle_e_update
 *
 * Description:
 *   Update motor angle structure using electrical motor angle.
 *
 * Input Parameters:
 *   angle     - (in/out) pointer to the motor angle structure
 *   angle_new - (in) new motor electrical angle in range <0.0, 2PI>
 *
 * Returned Value:
 *   None
 *
 ****************************************************************************/

void motor_angle_e_update(FAR struct motor_angle_s *angle, float angle_new, float dir)
{
  DEBUGASSERT(angle != NULL);
  DEBUGASSERT(angle_new >= 0.0f && angle_new <= MOTOR_ANGLE_E_MAX);
  DEBUGASSERT(dir == DIR_CW || dir == DIR_CCW);

  /* Check if we crossed electrical angle boundaries */

  if (dir == DIR_CW &&
      angle_new - angle->angle_el.angle < -POLE_CNTR_THR)
    {
      /* For CW direction - previous angle is greater than current angle */

      angle->i += 1;
    }

  else if (dir == DIR_CCW &&
           angle_new - angle->angle_el.angle > POLE_CNTR_THR)
    {
      /* For CCW direction - previous angle is lower than current angle */

      angle->i -= 1;
    }

  /* Reset pole counter if needed */

  if (angle->i >= angle->p)
    {
      angle->i = 0;
    }

  else if (angle->i < 0)
    {
      angle->i = angle->p-1;
    }

  /* Update electrical angle structure */

  phase_angle_update(&angle->angle_el, angle_new);

  /* Calculate mechanical angle.
   * One electrical angle rotation is equal to one mechanical rotation divided
   * by number of motor pole pairs.
   */

  angle->anglem = (MOTOR_ANGLE_E_RANGE * angle->i +
                   angle->angle_el.angle) * angle->one_by_p;

  /* Normalize mechanical angle to <0, 2PI> and store */

  angle_norm_2pi(&angle->anglem, MOTOR_ANGLE_M_MIN, MOTOR_ANGLE_M_MAX);
}

/****************************************************************************
 * Name: motor_angle_m_update
 *
 * Description:
 *   Update motor angle structure using mechanical motor angle
 *
 * Input Parameters:
 *   angle     - (in/out) pointer to the motor angle structure
 *   angle_new - (in) new motor mechanical angle in range <0.0, 2PI>
 *
 * Returned Value:
 *   None
 *
 ****************************************************************************/

void motor_angle_m_update(FAR struct motor_angle_s *angle, float angle_new, float dir)
{
  DEBUGASSERT(angle != NULL);
  DEBUGASSERT(angle_new >= 0.0f && angle_new <= MOTOR_ANGLE_E_MAX);
  DEBUGASSERT(dir == DIR_CW || dir == DIR_CCW);

  float angle_el = 0.0f;

  /* Store new mechanical angle */

  angle->anglem = angle_new;

  /* Update pole counter */

  angle->i = (uint8_t)(angle->anglem * angle->p/MOTOR_ANGLE_M_MAX);

  /* Get electrical angle */

  angle_el = angle->anglem * angle->p - MOTOR_ANGLE_E_MAX * angle->i;

  /* Update electrical angle structure */

  phase_angle_update(&angle->angle_el, angle_el);
}

/****************************************************************************
 * Name: motor_angle_m_get
 *
 * Description:
 *   Get motor mechanical angle
 *
 * Input Parameters:
 *   angle - (in/out) pointer to the motor angle structure
 *
 * Returned Value:
 *   Return motor mechanical angle
 *
 ****************************************************************************/

float motor_angle_m_get(FAR struct motor_angle_s *angle)
{
  DEBUGASSERT(angle != NULL);

  return angle->anglem;
}

/****************************************************************************
 * Name: motor_angle_e_get
 *
 * Description:
 *   Get motor electrical angle
 *
 * Input Parameters:
 *   angle - (in/out) pointer to the motor angle structure
 *
 * Returned Value:
 *   Return motor electrical angle
 *
 ****************************************************************************/

float motor_angle_e_get(FAR struct motor_angle_s *angle)
{
  DEBUGASSERT(angle != NULL);

  return angle->angle_el.angle;
}

/****************************************************************************
 * Name: motor_phy_params_init
 *
 * Description:
 *   Initialize motor physical parameters
 *
 * Input Parameters:
 *   phy   - (in/out) pointer to the motor physical parameters
 *   poles - (in) number of the motor pole pairs
 *   res   - (in) average phase-to-neutral base motor resistance
 *                (without temperature compensation)
 *   ind   - (in) average phase-to-neutral motor inductance
 *
 * Returned Value:
 *   None
 *
 ****************************************************************************/

void motor_phy_params_init(FAR struct motor_phy_params_s *phy, uint8_t poles,
                            float res, float ind)
{
  DEBUGASSERT(phy != NULL);

  phy->p          = poles;
  phy->res_base   = res;
  phy->ind        = ind;
  phy->one_by_ind = 1.0f/ind;

  /* Initialize with zeros */

  phy->res          = 0.0f;
  phy->res_alpha    = 0.0f;
  phy->res_temp_ref = 0.0f;
}

/****************************************************************************
 * Name: motor_phy_params_temp_set
 * Description:
 *   Initialize motor physical temperature parameters
 *
 * Input Parameters:
 *   phy          - (in/out) pointer to the motor physical parameters
 *   res_alpha    - (in) temperature coefficient of the winding resistance
 *   res_temp_ref - (in) reference temperature for alpha
 *
 * Returned Value:
 *   None
 *
 ****************************************************************************/

void motor_phy_params_temp_set(FAR struct motor_phy_params_s *phy,
                               float res_alpha, float res_temp_ref)
{
  DEBUGASSERT(phy != NULL);

  phy->res_alpha    = res_alpha;
  phy->res_temp_ref = res_temp_ref;
}