text: /****************************************************************************
   * apps/industry/foc/fixed16/foc_align.c
   * This file implements angle sensor alignment routine for fixed16
   *
   * 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.
   *
   ****************************************************************************/
  
  /****************************************************************************
   * Included Files
   ****************************************************************************/
  
  #include <nuttx/config.h>
  
  #include <assert.h>
  #include <errno.h>
  #include <stdlib.h>
  
  #include "industry/foc/foc_common.h"
  #include "industry/foc/foc_log.h"
  
  #include "industry/foc/fixed16/foc_align.h"
  
  /****************************************************************************
   * Pre-processor Definitions
   ****************************************************************************/
  
  /* Align to -90deg = 270deg */
  
  #define FOC_ALIGN_ANGLE        (b16muli(b16HALFPI, 3))
  
  /* Direction alignment */
  
  #define ALIGN_DIR_ANGLE_STEP   (ftob16(0.001f))
  #define ALIGN_DIR_ANGLE_HOLD_0 (ftob16(0.0f))
  #define ALIGN_DIR_ANGLE_HOLD_1 (b16divi(b16PI, 3))
  #define ALIGN_DIR_ANGLE_HOLD_2 (b16muli(b16divi(b16PI, 3), 2))
  #define ALIGN_DIR_ANGLE_HOLD_3 (b16PI)
  #define ALIGN_DIR_ANGLE_HOLD_4 (b16muli(b16divi(b16PI, 3), 4))
  #define ALIGN_DIR_HOLD_CNTR    (10)
  
  /* IDLE steps */
  
  #define IDLE_STEPS             (500)
  
  /* Index alignment */
  
  #define ALIGN_INDEX_ANGLE_SIGN   (-b16muli(b16PI, 2))
  #define ALIGN_INDEX_ANGLE_STEP   (ftob16(0.001f))
  #define ALIGN_INDEX_ANGLE_NOZERO (ftob16(0.1f))
  
  /****************************************************************************
   * Private Data Types
   ****************************************************************************/
  
  /* Align stage */
  
  enum foc_align_run_stage_e
  {
    FOC_ALIGN_RUN_INIT,
  #ifdef CONFIG_INDUSTRY_FOC_ALIGN_INDEX
    FOC_ALIGN_RUN_INDEX,
  #endif
    FOC_ALIGN_RUN_OFFSET,
    FOC_ALIGN_RUN_DIR,
    FOC_ALIGN_RUN_IDLE,
    FOC_ALIGN_RUN_DONE
  };
  
  /* Align routine private data */
  
  struct foc_align_b16_s
  {
    struct foc_routine_align_cfg_b16_s   cfg;
    struct foc_routine_aling_final_b16_s final;
    int                                  cntr;
    int                                  stage;
    int                                  dir_step;
  #ifdef CONFIG_INDUSTRY_FOC_ALIGN_INDEX
    int                                  index_step;
    b16_t                                index_angle;
  #endif
    b16_t                                dir_angle;
    b16_t                                angle_last;
    b16_t                                angle_now;
    b16_t                                diff_cw;
    b16_t                                diff_ccw;
  };
  
  /****************************************************************************
   * Private Function Prototypes
   ****************************************************************************/
  
  int foc_routine_align_init_b16(FAR foc_routine_b16_t *r);
  void foc_routine_align_deinit_b16(FAR foc_routine_b16_t *r);
  int foc_routine_align_cfg_b16(FAR foc_routine_b16_t *r, FAR void *cfg);
  int foc_routine_align_run_b16(FAR foc_routine_b16_t *r,
                                FAR struct foc_routine_in_b16_s *in,
                                FAR struct foc_routine_out_b16_s *out);
  int foc_routine_align_final_b16(FAR foc_routine_b16_t *r, FAR void *data);
  
  /****************************************************************************
   * Public Data
   ****************************************************************************/
  
  struct foc_routine_ops_b16_s g_foc_routine_align_b16 =
  {
    .init   = foc_routine_align_init_b16,
    .deinit = foc_routine_align_deinit_b16,
    .cfg    = foc_routine_align_cfg_b16,
    .run    = foc_routine_align_run_b16,
    .final  = foc_routine_align_final_b16,
  };
  
  /****************************************************************************
   * Private Functions
   ****************************************************************************/
  
  #ifdef CONFIG_INDUSTRY_FOC_ALIGN_INDEX
  /****************************************************************************
   * Name: foc_align_index_run_b16
   *
   * Description:
   *   Run index alignment routine
   *
   * Input Parameter:
   *   align - pointer to FOC align routine
   *   in    - pointer to FOC routine input data
   *   out   - pointer to FOC routine output data
   *
   ****************************************************************************/
  
  static int foc_align_index_run_b16(FAR struct foc_align_b16_s *align,
                                     FAR struct foc_routine_in_b16_s *in,
                                     FAR struct foc_routine_out_b16_s *out)
  {
    static b16_t dir         = DIR_CW_B16;
    b16_t        sign        = 0;
    int          ret         = FOC_ROUTINE_RUN_NOTDONE;
  
    /* Get mechanical angles */
  
    align->angle_last = align->angle_now;
    align->angle_now  = in->angle_m;
  
    /* Normalize angle to <-M_PI, M_PI> range */
  
    align->angle_now = align->angle_now - b16PI;
    angle_norm_2pi_b16(&align->angle_now, -b16PI, b16PI);
  
    /* The product of the previous angle with an angle now gives us
     * information about the encoder overflow.
     * This procedure assumes that the encoder index signal resets the
     * encoder value to zero.
     */
  
    sign = b16mulb16(align->angle_now, align->angle_last);
  
    /* Move the motor shaft to approach index signal from both direction.
     * After that, the encoder zero should be aligned with the encoder index.
     */
  
    if (align->index_step == 0)
      {
        /* Move the motor shaft away from zero */
  
        align->index_angle += b16mulb16(dir, ALIGN_INDEX_ANGLE_STEP);
  
        if (align->angle_now < ALIGN_INDEX_ANGLE_NOZERO &&
            align->angle_now > -ALIGN_INDEX_ANGLE_NOZERO)
          {
            align->index_step += 1;
          }
      }
  
    else if (align->index_step == 1)
      {
        /* Move the motor shaft to zero in positive direction */
  
        align->index_angle += b16mulb16(dir, ALIGN_INDEX_ANGLE_STEP);
  
        if (sign < 0 && sign < ALIGN_INDEX_ANGLE_SIGN)
          {
            dir = b16mulb16(DIR_CCW_B16, dir);
            align->index_step += 1;
          }
      }
  
    else if (align->index_step == 2)
      {
        /* Move the motor shaft away from zero */
  
        align->index_angle += b16mulb16(dir, ALIGN_INDEX_ANGLE_STEP);
  
        if (align->angle_now < ALIGN_INDEX_ANGLE_NOZERO &&
            align->angle_now > -ALIGN_INDEX_ANGLE_NOZERO)
          {
            align->index_step += 1;
          }
      }
  
    else if (align->index_step == 3)
      {
        /* Move the motor shaft to zero in negative direction */
  
        align->index_angle += b16mulb16(dir, ALIGN_INDEX_ANGLE_STEP);
  
        if (sign < 0 && sign < ALIGN_INDEX_ANGLE_SIGN)
          {
            dir = b16mulb16(DIR_CCW_B16, dir);
            align->index_step += 1;
          }
      }
  
    else if (align->index_step == 4)
      {
        /* Done */
  
        ret = FOC_ROUTINE_RUN_DONE;
      }
  
    /* Get output */
  
    if (ret == FOC_ROUTINE_RUN_DONE)
      {
        /* Force IDLE mode */
  
        out->dq_ref.q   = 0;
        out->dq_ref.d   = 0;
        out->vdq_comp.q = 0;
        out->vdq_comp.d = 0;
        out->angle      = 0;
        out->foc_mode   = FOC_HANDLER_MODE_IDLE;
  
        /* Reset data */
  
        align->angle_last = 0;
        align->angle_now  = 0;
        align->cntr       = 0;
      }
    else
      {
        /* Force DQ voltage vector */
  
        out->dq_ref.q   = align->cfg.volt;
        out->dq_ref.d   = 0;
        out->vdq_comp.q = 0;
        out->vdq_comp.d = 0;
        out->angle      = align->index_angle;
        out->foc_mode   = FOC_HANDLER_MODE_VOLTAGE;
  
        /* Increase counter */
  
        align->cntr += 1;
      }
  
    return ret;
  }
  #endif
  
  /****************************************************************************
   * Name: foc_align_offset_run_b16
   *
   * Description:
   *   Run offset alignment routine
   *
   * Input Parameter:
   *   align - pointer to FOC align routine
   *   in    - pointer to FOC routine input data
   *   out   - pointer to FOC routine output data
   *
   ****************************************************************************/
  
  static int foc_align_offset_run_b16(FAR struct foc_align_b16_s *align,
                                      FAR struct foc_routine_in_b16_s *in,
                                      FAR struct foc_routine_out_b16_s *out)
  {
    int ret = FOC_ROUTINE_RUN_NOTDONE;
  
    /* Reset Align angle sensor */
  
    if (align->cntr > align->cfg.offset_steps)
      {
        /* Align angle sensor */
  
        if (align->cfg.cb.zero != NULL)
          {
            ret = align->cfg.cb.zero(align->cfg.cb.priv, in->angle);
            if (ret < 0)
              {
                FOCLIBERR("ERROR: align offset callback failed %d!\n", ret);
                goto errout;
              }
          }
  
        /* Offset align done */
  
        align->cntr = 0;
        ret         = FOC_ROUTINE_RUN_DONE;
  
        /* Force IDLE mode */
  
        out->dq_ref.q   = 0;
        out->dq_ref.d   = 0;
        out->vdq_comp.q = 0;
        out->vdq_comp.d = 0;
        out->angle      = 0;
        out->foc_mode   = FOC_HANDLER_MODE_IDLE;
      }
    else
      {
        /* Force DQ voltage vector */
  
        out->dq_ref.q   = align->cfg.volt;
        out->dq_ref.d   = 0;
        out->vdq_comp.q = 0;
        out->vdq_comp.d = 0;
        out->angle      = FOC_ALIGN_ANGLE;
        out->foc_mode   = FOC_HANDLER_MODE_VOLTAGE;
  
        /* Increase counter */
  
        align->cntr += 1;
      }
  
    /* Store offset if done */
  
    if (ret == FOC_ROUTINE_RUN_DONE)
      {
        align->final.offset = in->angle;
      }
  
  errout:
    return ret;
  }
  
  /****************************************************************************
   * Name: foc_align_dir_move_b16
   ****************************************************************************/
  
  static void foc_align_dir_move_b16(FAR struct foc_align_b16_s *align,
                                     b16_t dir, b16_t dest)
  {
    bool next = false;
  
    DEBUGASSERT(align);
  
    /* Update angle */
  
    align->dir_angle += b16mulb16(dir, ALIGN_DIR_ANGLE_STEP);
  
    /* Check destination */
  
    if (dir == DIR_CW_B16)
      {
        if (align->dir_angle >= dest)
          {
            next = true;
          }
      }
  
    else if (dir == DIR_CCW_B16)
      {
        if (align->dir_angle <= dest)
          {
            next = true;
          }
      }
    else
      {
        DEBUGASSERT(0);
      }
  
    /* Next step */
  
    if (next == true)
      {
        align->dir_step += 1;
      }
  }
  
  /****************************************************************************
   * Name: foc_align_dir_hold_b16
   ****************************************************************************/
  
  static void foc_align_dir_hold_b16(FAR struct foc_align_b16_s *align,
                                     b16_t dir, bool last, bool diff)
  {
    DEBUGASSERT(align);
  
    /* Lock angle */
  
    align->dir_angle = align->dir_angle;
  
    /* Increase counter */
  
    align->cntr += 1;
  
    if (align->cntr > ALIGN_DIR_HOLD_CNTR)
      {
        /* Next step */
  
        align->dir_step += 1;
  
        /* Accumulate diff */
  
        if (diff == true)
          {
            if (dir == DIR_CW_B16)
              {
                align->diff_cw += (align->angle_now - align->angle_last);
              }
            else if (dir == DIR_CCW_B16)
              {
                align->diff_ccw += (align->angle_now - align->angle_last);
              }
            else
              {
                DEBUGASSERT(0);
              }
          }
  
        /* Store last angle */
  
        if (last == true)
          {
            align->angle_last = align->angle_now;
          }
  
        /* Reset counter */
  
        align->cntr = 0;
      }
  }
  
  /****************************************************************************
   * Name: foc_align_dir_run_b16
   *
   * Description:
   *   Run direction alignment routine
   *
   * Input Parameter:
   *   align - pointer to FOC align routine
   *   in    - pointer to FOC routine input data
   *   out   - pointer to FOC routine output data
   *
   ****************************************************************************/
  
  int foc_align_dir_run_b16(FAR struct foc_align_b16_s *align,
                            FAR struct foc_routine_in_b16_s *in,
                            FAR struct foc_routine_out_b16_s *out)
  {
    int  ret  = FOC_ROUTINE_RUN_NOTDONE;
    bool last = false;
  
    /* Store angle now */
  
    align->angle_now = in->angle;
  
    /* Handle dir align */
  
    if (align->dir_step == 0)
      {
        /* Start with position 0 */
  
        align->dir_angle = ALIGN_DIR_ANGLE_HOLD_0;
  
        /* Step 0 - hold position 0 */
  
        foc_align_dir_hold_b16(align, DIR_CW_B16, true, true);
      }
  
    else if (align->dir_step == 1)
      {
        /* Step 1 - move motor in CW direction to position 1 */
  
        foc_align_dir_move_b16(align, DIR_CW_B16, ALIGN_DIR_ANGLE_HOLD_1);
      }
  
    else if (align->dir_step == 2)
      {
        /* Step 2 - hold position 1 */
  
        foc_align_dir_hold_b16(align, DIR_CW_B16, true, false);
      }
  
    else if (align->dir_step == 3)
      {
        /* Step 3 - move motor in CW direction to position 2 */
  
        foc_align_dir_move_b16(align, DIR_CW_B16, ALIGN_DIR_ANGLE_HOLD_2);
      }
  
    else if (align->dir_step == 4)
      {
        /* Step 4 - hold position 2 */
  
        foc_align_dir_hold_b16(align, DIR_CW_B16, true, true);
      }
  
    else if (align->dir_step == 5)
      {
        /* Step 5 - move motor in CW direction to position 3 */
  
        foc_align_dir_move_b16(align, DIR_CW_B16, ALIGN_DIR_ANGLE_HOLD_3);
      }
  
    else if (align->dir_step == 6)
      {
        /* Step 6 - hold position 3 */
  
        foc_align_dir_hold_b16(align, DIR_CW_B16, true, true);
      }
  
    else if (align->dir_step == 6)
      {
        /* Step 6 - move motor in CW direction to position 4 */
  
        foc_align_dir_move_b16(align, DIR_CW_B16, ALIGN_DIR_ANGLE_HOLD_4);
      }
  
    else if (align->dir_step == 7)
      {
        /* Step 7 - hold position 4 */
  
        foc_align_dir_hold_b16(align, DIR_CW_B16, true, true);
      }
  
    else if (align->dir_step == 8)
      {
        /* Step 8 - move motor in CCW direction to position 3 */
  
        foc_align_dir_move_b16(align, DIR_CCW_B16, ALIGN_DIR_ANGLE_HOLD_3);
      }
  
    else if (align->dir_step == 9)
      {
        /* Step 9 - hold position 3 */
  
        foc_align_dir_hold_b16(align, DIR_CCW_B16, true, true);
      }
  
    else if (align->dir_step == 10)
      {
        /* Step 10 - move motor in CCW direction to position 2 */
  
        foc_align_dir_move_b16(align, DIR_CCW_B16, ALIGN_DIR_ANGLE_HOLD_2);
      }
  
    else if (align->dir_step == 11)
      {
        /* Step 11 - hold position 2 */
  
        foc_align_dir_hold_b16(align, DIR_CCW_B16, true, true);
      }
  
    else if (align->dir_step == 12)
      {
        /* Step 12 - move motor in CCW direction to position 1 */
  
        foc_align_dir_move_b16(align, DIR_CCW_B16, ALIGN_DIR_ANGLE_HOLD_1);
      }
  
    else if (align->dir_step == 13)
      {
        /* Step 13 - hold position 1 */
  
        foc_align_dir_hold_b16(align, DIR_CCW_B16, true, true);
      }
  
    else if (align->dir_step == 14)
      {
        /* Step 14 - move motor in CCW direction to position 0 */
  
        foc_align_dir_move_b16(align, DIR_CCW_B16, ALIGN_DIR_ANGLE_HOLD_0);
      }
  
    else if (align->dir_step == 15)
      {
        /* Step 15 - hold position 0 */
  
        foc_align_dir_hold_b16(align, DIR_CCW_B16, true, true);
      }
  
    else
      {
        /* Set flag */
  
        last = true;
  
        /* Get sensor direction according to sampled data */
  
        if (align->diff_cw > 0 && align->diff_ccw < 0)
          {
            align->final.dir = DIR_CW_B16;
          }
  
        else if (align->diff_cw < 0 && align->diff_ccw > 0)
          {
            align->final.dir = DIR_CCW_B16;
          }
  
        else
          {
            /* Invalid data */
  
            FOCLIBERR("ERROR: direction align failed !\n");
  
            align->final.dir = DIR_NONE_B16;
            ret              = -EINVAL;
  
            goto errout;
          }
  
        /* Align angle sensor */
  
        if (align->cfg.cb.dir != NULL)
          {
            ret = align->cfg.cb.dir(align->cfg.cb.priv, align->final.dir);
            if (ret < 0)
              {
                FOCLIBERR("ERROR: align dir callback failed %d!\n", ret);
                goto errout;
              }
          }
  
        /* Direction alignment done */
  
        ret = FOC_ROUTINE_RUN_DONE;
      }
  
    /* Force DQ voltage vector */
  
    out->dq_ref.q   = align->cfg.volt;
    out->dq_ref.d   = 0;
    out->vdq_comp.q = 0;
    out->vdq_comp.d = 0;
    out->angle      = align->dir_angle;
    out->foc_mode   = FOC_HANDLER_MODE_VOLTAGE;
  
  errout:
  
    /* Force current to zero if alignment done */
  
    if (last == true)
      {
        out->dq_ref.q = 0;
        out->dq_ref.d = 0;
      }
  
    return ret;
  }
  
  /****************************************************************************
   * Name: foc_align_idle_run_b16
   *
   * Description:
   *   Force IDLE state
   *
   * Input Parameter:
   *   align - pointer to FOC align routine
   *   in    - pointer to FOC routine input data
   *   out   - pointer to FOC routine output data
   *
   ****************************************************************************/
  
  static int foc_align_idle_run_b16(FAR struct foc_align_b16_s *align,
                                      FAR struct foc_routine_in_b16_s *in,
                                      FAR struct foc_routine_out_b16_s *out)
  {
    int ret = FOC_ROUTINE_RUN_NOTDONE;
  
    /* Get output */
  
    out->dq_ref.q   = 0;
    out->dq_ref.d   = 0;
    out->vdq_comp.q = 0;
    out->vdq_comp.d = 0;
    out->angle      = 0;
    out->foc_mode   = FOC_HANDLER_MODE_IDLE;
  
    /* Increase counter */
  
    align->cntr += 1;
  
    if (align->cntr > IDLE_STEPS)
      {
        /* Done */
  
        ret = FOC_ROUTINE_RUN_DONE;
  
        /* Reset counter */
  
        align->cntr = 0;
      }
  
    return ret;
  }
  
  /****************************************************************************
   * Name: foc_routine_align_init_b16
   *
   * Description:
   *   Initialize the FOC align routine (fixed16)
   *
   * Input Parameter:
   *   r - pointer to FOC routine
   *
   ****************************************************************************/
  
  int foc_routine_align_init_b16(FAR foc_routine_b16_t *r)
  {
    int ret = OK;
  
    DEBUGASSERT(r);
  
    /* Connect angle data */
  
    r->data = zalloc(sizeof(struct foc_align_b16_s));
    if (r->data == NULL)
      {
        ret = -ENOMEM;
        goto errout;
      }
  
  errout:
    return ret;
  }
  
  /****************************************************************************
   * Name: foc_routine_align_deinit_b16
   *
   * Description:
   *   Deinitialize the FOC align routine (fixed16)
   *
   * Input Parameter:
   *   r - pointer to FOC routine
   *
   ****************************************************************************/
  
  void foc_routine_align_deinit_b16(FAR foc_routine_b16_t *r)
  {
    DEBUGASSERT(r);
  
    if (r->data)
      {
        /* Free routine data */
  
        free(r->data);
      }
  }
  
  /****************************************************************************
   * Name: foc_routine_align_cfg_b16
   *
   * Description:
   *   Configure the FOC align routine (fixed16)
   *
   * Input Parameter:
   *   r   - pointer to FOC routine
   *   cfg - pointer to align routine configuration data
   *
   ****************************************************************************/
  
  int foc_routine_align_cfg_b16(FAR foc_routine_b16_t *r, FAR void *cfg)
  {
    FAR struct foc_align_b16_s *a   = NULL;
    int                         ret = OK;
  
    DEBUGASSERT(r);
    DEBUGASSERT(cfg);
  
    /* Get aling data */
  
    DEBUGASSERT(r->data);
    a = r->data;
  
    /* Copy configuration */
  
    memcpy(&a->cfg, cfg, sizeof(struct foc_routine_align_cfg_b16_s));
  
    /* Verify configuration */
  
    if (a->cfg.volt <= 0)
      {
        ret = -EINVAL;
        goto errout;
      }
  
    if (a->cfg.offset_steps <= 0)
      {
        ret = -EINVAL;
        goto errout;
      }
  
    if (a->cfg.cb.zero == NULL || a->cfg.cb.dir == NULL ||
        a->cfg.cb.priv == NULL)
      {
        FOCLIBWARN("WARN: no align cb data!\n");
      }
  
  errout:
    return ret;
  }
  
  /****************************************************************************
   * Name: foc_routine_align_run_b16
   *
   * Description:
   *   Run the FOC align routine step (fixed16)
   *
   * Input Parameter:
   *   r   - pointer to FOC routine
   *   in  - pointer to FOC routine input data
   *   out - pointer to FOC routine output data
   *
   ****************************************************************************/
  
  int foc_routine_align_run_b16(FAR foc_routine_b16_t *r,
                                FAR struct foc_routine_in_b16_s *in,
                                FAR struct foc_routine_out_b16_s *out)
  {
    FAR struct foc_align_b16_s *a   = NULL;
    int                         ret = FOC_ROUTINE_RUN_NOTDONE;
  
    DEBUGASSERT(r);
    DEBUGASSERT(in);
    DEBUGASSERT(out);
  
    /* Get aling data */
  
    DEBUGASSERT(r->data);
    a = r->data;
  
    /* Force IDLE state at default */
  
    out->dq_ref.q   = 0;
    out->dq_ref.d   = 0;
    out->vdq_comp.q = 0;
    out->vdq_comp.d = 0;
    out->angle      = 0;
    out->foc_mode   = FOC_HANDLER_MODE_IDLE;
  
    /* Handle alignment stage */
  
    switch (a->stage)
      {
        case FOC_ALIGN_RUN_INIT:
          {
            a->stage += 1;
            ret = FOC_ROUTINE_RUN_NOTDONE;
  
            break;
          }
  
  #ifdef CONFIG_INDUSTRY_FOC_ALIGN_INDEX
        case FOC_ALIGN_RUN_INDEX:
          {
            /* Align zero procedure */
  
            ret = foc_align_index_run_b16(a, in, out);
            if (ret < 0)
              {
                goto errout;
              }
  
            if (ret == FOC_ROUTINE_RUN_DONE)
              {
                FOCLIBLOG("ALIGN INDEX done!\n");
  
                a->stage += 1;
                ret = FOC_ROUTINE_RUN_NOTDONE;
              }
  
            break;
          }
  #endif
  
      case FOC_ALIGN_RUN_OFFSET:
        {
          /* Align zero procedure */
  
          ret = foc_align_offset_run_b16(a, in, out);
          if (ret < 0)
            {
              goto errout;
            }
  
          if (ret == FOC_ROUTINE_RUN_DONE)
            {
              FOCLIBLOG("ALIGN OFFSET done!\n");
  
              a->stage += 1;
              ret = FOC_ROUTINE_RUN_NOTDONE;
            }
  
          break;
        }
  
        case FOC_ALIGN_RUN_DIR:
          {
            /* Align direction procedure */
  
            ret = foc_align_dir_run_b16(a, in, out);
            if (ret < 0)
              {
                goto errout;
              }
  
            if (ret == FOC_ROUTINE_RUN_DONE)
              {
                FOCLIBLOG("ALIGN DIR done!\n");
  
                a->stage += 1;
                ret = FOC_ROUTINE_RUN_NOTDONE;
              }
  
            break;
          }
  
        case FOC_ALIGN_RUN_IDLE:
          {
            /* De-energetize motor */
  
            ret = foc_align_idle_run_b16(a, in, out);
            if (ret < 0)
              {
                goto errout;
              }
  
            if (ret == FOC_ROUTINE_RUN_DONE)
              {
                FOCLIBLOG("ALIGN IDLE done!\n");
  
                a->stage += 1;
                ret = FOC_ROUTINE_RUN_NOTDONE;
              }
  
            break;
          }
  
        case FOC_ALIGN_RUN_DONE:
          {
            ret = FOC_ROUTINE_RUN_DONE;
  
            break;
          }
  
        default:
          {
            FOCLIBERR("ERROR: invalid alignment stage %d\n", a->stage);
            ret = -EINVAL;
            goto errout;
          }
      }
  
  errout:
    return ret;
  }
  
  /****************************************************************************
   * Name: foc_routine_align_final_b16
   *
   * Description:
   *   Finalize the FOC routine data (fixed16)
   *
   * Input Parameter:
   *   r    - pointer to FOC routine
   *   data - pointer to FOC align routine final data
   *
   ****************************************************************************/
  
  int foc_routine_align_final_b16(FAR foc_routine_b16_t *r, FAR void *data)
  {
    FAR struct foc_align_b16_s *a = NULL;
  
    DEBUGASSERT(r);
    DEBUGASSERT(data);
  
    /* Get aling data */
  
    DEBUGASSERT(r->data);
    a = r->data;
  
    /* Get final data */
  
    memcpy(data, &a->final, sizeof(struct foc_routine_aling_final_b16_s));
  
    return OK;
  }
  