File content as of revision 15:40470df81d9a:

/**
  ******************************************************************************
  * @file    l6474_class.cpp
  * @author  IPC Rennes
  * @version V1.5.0
  * @date    November 12, 2014
  * @brief   L6474 driver (fully integrated microstepping motor driver)
  * @note    (C) COPYRIGHT 2014 STMicroelectronics
  ******************************************************************************
  * @attention
  *
  * <h2><center>&copy; COPYRIGHT(c) 2014 STMicroelectronics</center></h2>
  *
  * 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 of STMicroelectronics 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 HOLDER 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.
  *
  ******************************************************************************
  */


/* Generated with Stm32CubeTOO -----------------------------------------------*/


/* Revision ------------------------------------------------------------------*/
/*
    Repository:       http://svn.x-nucleodev.codex.cro.st.com/svnroot/X-NucleoDev
    Branch/Trunk/Tag: trunk
    Based on:         X-CUBE-SPN1/trunk/Drivers/BSP/Components/l6474/l6474.c
    Revision:         0
*/


/* Includes ------------------------------------------------------------------*/

#include "l6474_class.h"
#include "l6474.h"

    
/* Definitions ---------------------------------------------------------------*/

/* Error while initialising the SPI. */
#define L6474_ERROR_0        (0x8000)   

/* Error of bad SPI transaction. */
#define L6474_ERROR_1        (0x8001)
    
/* Maximum number of steps. */
#define MAX_STEPS            (0x7FFFFFFF)

/* Maximum frequency of the PWMs in Hz. */
#define L6474_MAX_PWM_FREQ   (10000)

/* Minimum frequency of the PWMs in Hz. */
#define L6474_MIN_PWM_FREQ   (2)


/* Variables  ----------------------------------------------------------------*/

/* Number of devices. */
uint8_t L6474::numberOfDevices = 0;

/* ISR flags used to restart an interrupted SPI transfer when an error is reported. */
bool L6474::spiPreemtionByIsr = FALSE;
bool L6474::isrFlag = FALSE;

/* SPI Transmission for Daisy-Chain Configuration. */
uint8_t L6474::spiTxBursts[L6474_CMD_ARG_MAX_NB_BYTES][MAX_NUMBER_OF_DEVICES];
uint8_t L6474::spiRxBursts[L6474_CMD_ARG_MAX_NB_BYTES][MAX_NUMBER_OF_DEVICES];


/* Methods -------------------------------------------------------------------*/

/**********************************************************
 * @brief  Attaches a user callback to the error Handler.
 * The call back will be then called each time the library 
 * detects an error
 * @param[in] callback Name of the callback to attach 
 * to the error Hanlder
 * @retval None
 **********************************************************/
void L6474::L6474_AttachErrorHandler(void (*callback)(uint16_t error))
{
  errorHandlerCallback = (void (*)(uint16_t error)) callback;
}

/**********************************************************
 * @brief Starts the L6474 library
 * @retval COMPONENT_OK in case of success
 **********************************************************/
DrvStatusTypeDef L6474::L6474_Init(MOTOR_InitTypeDef *L6474_Init)
{
  /* Initialise the PWMs used for the Step clocks ----------------------------*/
  L6474_PwmInit();

  /* Initialise the L6474s ------------------------------------------------*/
  
  /* Standby-reset deactivation */
  L6474_ReleaseReset();
  
  /* Let a delay after reset */
  L6474_Delay(1); 

  /* Set all registers and context variables to the predefined values from l6474_target_config.h */
  L6474_SetDeviceParamsToPredefinedValues();
  
  /* Disable L6474 powerstage */
  L6474_CmdDisable();

  /* Get Status to clear flags after start up */
  L6474_CmdGetStatus();
  
  return COMPONENT_OK;
}

/**********************************************************
 * @brief Read id
 * @param id pointer to the identifier to be read.
 * @retval COMPONENT_OK in case of success
 **********************************************************/
DrvStatusTypeDef L6474::L6474_ReadID(uint8_t *id)
{
  *id = deviceInstance;

  return COMPONENT_OK;
}

/**********************************************************
 * @brief Returns the acceleration of the specified device
 * @retval Acceleration in pps^2
 **********************************************************/
uint16_t L6474::L6474_GetAcceleration(void)
{
  return (devicePrm.acceleration);
}            

/**********************************************************
 * @brief Returns the current speed of the specified device
 * @retval Speed in pps
 **********************************************************/
uint16_t L6474::L6474_GetCurrentSpeed(void)
{
  return devicePrm.speed;
}

/**********************************************************
 * @brief Returns the deceleration of the specified device
 * @retval Deceleration in pps^2
 **********************************************************/
uint16_t L6474::L6474_GetDeceleration(void)
{                                                  
  return (devicePrm.deceleration);
}          

/**********************************************************
 * @brief Returns the device state
 * @retval State (ACCELERATING, DECELERATING, STEADY or INACTIVE)
 **********************************************************/
motorState_t L6474::L6474_GetDeviceState(void)
{
  return devicePrm.motionState;
}

/**********************************************************
 * @brief Returns the FW version of the library
 * @param None
 * @retval L6474_FW_VERSION
 **********************************************************/
uint8_t L6474::L6474_GetFwVersion(void)
{
  return (L6474_FW_VERSION);
}

/**********************************************************
 * @brief  Returns the mark position  of the specified device
 * @retval Mark register value converted in a 32b signed integer 
 **********************************************************/
int32_t L6474::L6474_GetMark(void)
{
  return L6474_ConvertPosition(L6474_CmdGetParam(L6474_MARK));
}

/**********************************************************
 * @brief  Returns the max speed of the specified device
 * @retval maxSpeed in pps
 **********************************************************/
uint16_t L6474::L6474_GetMaxSpeed(void)
{                                                  
  return (devicePrm.maxSpeed);
}

/**********************************************************
 * @brief  Returns the min speed of the specified device
 * @retval minSpeed in pps
 **********************************************************/
uint16_t L6474::L6474_GetMinSpeed(void)
{                                                  
  return (devicePrm.minSpeed);
}                                                     

/**********************************************************
 * @brief  Returns the ABS_POSITION of the specified device
 * @retval ABS_POSITION register value converted in a 32b signed integer
 **********************************************************/
int32_t L6474::L6474_GetPosition(void)
{
  return L6474_ConvertPosition(L6474_CmdGetParam(L6474_ABS_POS));
}

/**********************************************************
 * @brief  Requests the motor to move to the home position (ABS_POSITION = 0)
 * @retval None
 **********************************************************/
void L6474::L6474_GoHome(void)
{
  L6474_GoTo(0);
} 
  
/**********************************************************
 * @brief  Requests the motor to move to the mark position 
 * @retval None
 **********************************************************/
void L6474::L6474_GoMark(void)
{
    uint32_t mark;

    mark = L6474_ConvertPosition(L6474_CmdGetParam(L6474_MARK));
    L6474_GoTo(mark);  
}

/**********************************************************
 * @brief  Requests the motor to move to the specified position 
 * @param[in] targetPosition absolute position in steps
 * @retval None
 **********************************************************/
void L6474::L6474_GoTo(int32_t targetPosition)
{
  motorDir_t direction;
  int32_t steps;
  
  /* Eventually deactivate motor */
  if (devicePrm.motionState != INACTIVE) 
  {
    L6474_HardStop();
  }

  /* Get current position */
  devicePrm.currentPosition = L6474_ConvertPosition(L6474_CmdGetParam(L6474_ABS_POS));
  
  /* Compute the number of steps to perform */
  steps = targetPosition - devicePrm.currentPosition;
  
  if (steps >= 0) 
  {
    devicePrm.stepsToTake = steps;
    direction = FORWARD;
  } 
  else 
  {
    devicePrm.stepsToTake = -steps;
    direction = BACKWARD;
  }
  
  if (steps != 0) 
  {
    devicePrm.commandExecuted = MOVE_CMD;
        
    /* Direction setup */
    L6474_SetDirection(direction);

    L6474_ComputeSpeedProfile(devicePrm.stepsToTake);
    
    /* Motor activation */
    L6474_StartMovement();
  }  
}

/**********************************************************
 * @brief  Immediatly stops the motor and disable the power bridge
 * @retval None
 **********************************************************/
void L6474::L6474_HardStop(void) 
{
  /* Disable corresponding PWM */
  L6474_PwmStop();

  /* Set inactive state */
  devicePrm.motionState = INACTIVE;
  devicePrm.commandExecuted = NO_CMD;
  devicePrm.stepsToTake = MAX_STEPS;  
}

/**********************************************************
 * @brief  Moves the motor of the specified number of steps
 * @param[in] direction FORWARD or BACKWARD
 * @param[in] stepCount Number of steps to perform
 * @retval None
 **********************************************************/
void L6474::L6474_Move(motorDir_t direction, uint32_t stepCount)
{
  /* Eventually deactivate motor */
  if (devicePrm.motionState != INACTIVE) 
  {
    L6474_HardStop();
  }
  
  if (stepCount != 0) 
  {
    devicePrm.stepsToTake = stepCount;
    
    devicePrm.commandExecuted = MOVE_CMD;
    
    devicePrm.currentPosition = L6474_ConvertPosition(L6474_CmdGetParam(L6474_ABS_POS));
    
    /* Direction setup */
    L6474_SetDirection(direction);

    L6474_ComputeSpeedProfile(stepCount);
    
    /* Motor activation */
    L6474_StartMovement();
  }  
}

/**********************************************************
 * @brief  Runs the motor. It will accelerate from the min 
 * speed up to the max speed by using the device acceleration.
 * @param[in] direction FORWARD or BACKWARD
 * @retval None
 **********************************************************/
void L6474::L6474_Run(motorDir_t direction)
{
  /* Eventually deactivate motor */
  if (devicePrm.motionState != INACTIVE) 
  {
    L6474_HardStop();
  }
  
  /* Direction setup */
  L6474_SetDirection(direction);

  devicePrm.commandExecuted = RUN_CMD;

  /* Motor activation */
  L6474_StartMovement(); 
}

/**********************************************************
 * @brief  Changes the acceleration of the specified device
 * @param[in] newAcc New acceleration to apply in pps^2
 * @retval true if the command is successfully executed, else false
 * @note The command is not performed is the device is executing 
 * a MOVE or GOTO command (but it can be used during a RUN command)
 **********************************************************/
bool L6474::L6474_SetAcceleration(uint16_t newAcc)
{                                                  
  bool cmdExecuted = FALSE;
  if ((newAcc != 0)&&
      ((devicePrm.motionState == INACTIVE)||
       (devicePrm.commandExecuted == RUN_CMD)))
  {
    devicePrm.acceleration = newAcc;
    cmdExecuted = TRUE;
  }    
  return cmdExecuted;
}            

/**********************************************************
 * @brief  Changes the deceleration of the specified device
 * @param[in] newDec New deceleration to apply in pps^2
 * @retval true if the command is successfully executed, else false
 * @note The command is not performed is the device is executing 
 * a MOVE or GOTO command (but it can be used during a RUN command)
 **********************************************************/
bool L6474::L6474_SetDeceleration(uint16_t newDec)
{                                                  
  bool cmdExecuted = FALSE;
  if ((newDec != 0)&& 
      ((devicePrm.motionState == INACTIVE)||
       (devicePrm.commandExecuted == RUN_CMD)))
  {
    devicePrm.deceleration = newDec;
    cmdExecuted = TRUE;
  }      
  return cmdExecuted;
}        

/**********************************************************
 * @brief  Set current position to be the Home position (ABS pos set to 0)
 * @retval None
 **********************************************************/
void L6474::L6474_SetHome(void)
{
  L6474_CmdSetParam(L6474_ABS_POS, 0);
}
 
/**********************************************************
 * @brief  Sets current position to be the Mark position 
 * @retval None
 **********************************************************/
void L6474::L6474_SetMark(void)
{
  uint32_t mark = L6474_CmdGetParam(L6474_ABS_POS);
  L6474_CmdSetParam(L6474_MARK, mark);
}

/**********************************************************
 * @brief  Changes the max speed of the specified device
 * @param[in] newMaxSpeed New max speed  to apply in pps
 * @retval true if the command is successfully executed, else false
 * @note The command is not performed is the device is executing 
 * a MOVE or GOTO command (but it can be used during a RUN command).
 **********************************************************/
bool L6474::L6474_SetMaxSpeed(uint16_t newMaxSpeed)
{                                                  
  bool cmdExecuted = FALSE;
  if ((newMaxSpeed >= L6474_MIN_PWM_FREQ)&&
      (newMaxSpeed <= L6474_MAX_PWM_FREQ) &&
      (devicePrm.minSpeed <= newMaxSpeed) &&
      ((devicePrm.motionState == INACTIVE)||
       (devicePrm.commandExecuted == RUN_CMD)))
  {
    devicePrm.maxSpeed = newMaxSpeed;
    cmdExecuted = TRUE;
  }
  return cmdExecuted;
}                                                     

/**********************************************************
 * @brief  Changes the min speed of the specified device
 * @param[in] newMinSpeed New min speed  to apply in pps
 * @retval true if the command is successfully executed, else false
 * @note The command is not performed is the device is executing 
 * a MOVE or GOTO command (but it can be used during a RUN command).
 **********************************************************/
bool L6474::L6474_SetMinSpeed(uint16_t newMinSpeed)
{                                                  
  bool cmdExecuted = FALSE;
  if ((newMinSpeed >= L6474_MIN_PWM_FREQ)&&
      (newMinSpeed <= L6474_MAX_PWM_FREQ) &&
      (newMinSpeed <= devicePrm.maxSpeed) && 
      ((devicePrm.motionState == INACTIVE)||
       (devicePrm.commandExecuted == RUN_CMD)))
  {
    devicePrm.minSpeed = newMinSpeed;
    cmdExecuted = TRUE;
  }  
  return cmdExecuted;
}                 

/**********************************************************
 * @brief  Stops the motor by using the device deceleration
 * @retval true if the command is successfully executed, else false
 * @note The command is not performed is the device is in INACTIVE state.
 **********************************************************/
bool L6474::L6474_SoftStop(void)
{   
  bool cmdExecuted = FALSE;
  if (devicePrm.motionState != INACTIVE)
  {
    devicePrm.commandExecuted = SOFT_STOP_CMD;
    cmdExecuted = TRUE;
  }
  return (cmdExecuted);
}

/**********************************************************
 * @brief  Locks until the device state becomes Inactive
 * @retval None
 **********************************************************/
void L6474::L6474_WaitWhileActive(void)
{
  /* Wait while motor is running */
  while (L6474_GetDeviceState() != INACTIVE);
}

/**********************************************************
 * @brief  Issue the Disable command to the L6474 of the specified device
 * @retval None
 **********************************************************/
void L6474::L6474_CmdDisable(void)
{
  L6474_SendCommand(L6474_DISABLE);
}

/**********************************************************
 * @brief  Issues the Enable command to the L6474 of the specified device
 * @retval None
 **********************************************************/
void L6474::L6474_CmdEnable(void)
{
  L6474_SendCommand(L6474_ENABLE);
}

/**********************************************************
 * @brief  Issues the GetParam command to the L6474 of the specified device
 * @param[in] parameter Register adress (L6474_ABS_POS, L6474_MARK,...)
 * @retval Register value
 **********************************************************/
uint32_t L6474::L6474_CmdGetParam(L6474_Registers_t parameter)
{
  uint32_t i;
  uint32_t spiRxData;
  uint8_t maxArgumentNbBytes = 0;
  uint8_t spiIndex = numberOfDevices - deviceInstance - 1;
  bool itDisable = FALSE;  
  
  do
  {
    spiPreemtionByIsr = FALSE;
    if (itDisable)
    {
      /* re-enable L6474_EnableIrq if disable in previous iteration */
      L6474_EnableIrq();
      itDisable = FALSE;
    }
  
    for (i = 0; i < numberOfDevices; i++)
    {
      spiTxBursts[0][i] = L6474_NOP;
      spiTxBursts[1][i] = L6474_NOP;
      spiTxBursts[2][i] = L6474_NOP;
      spiTxBursts[3][i] = L6474_NOP;
      spiRxBursts[1][i] = 0;
      spiRxBursts[2][i] = 0;
      spiRxBursts[3][i] = 0;    
    }

    switch (parameter)
    {
      case L6474_ABS_POS: ;
      case L6474_MARK:
        spiTxBursts[0][spiIndex] = ((uint8_t)L6474_GET_PARAM )| (parameter);
        maxArgumentNbBytes = 3;
        break;
      case L6474_EL_POS: ;
      case L6474_CONFIG: ;
      case L6474_STATUS:
        spiTxBursts[1][spiIndex] = ((uint8_t)L6474_GET_PARAM )| (parameter);
        maxArgumentNbBytes = 2;
        break;
      default:
        spiTxBursts[2][spiIndex] = ((uint8_t)L6474_GET_PARAM )| (parameter);
        maxArgumentNbBytes = 1;
    }
    
    /* Disable interruption before checking */
    /* pre-emption by ISR and SPI transfers*/
    L6474_DisableIrq();
    itDisable = TRUE;
  } while (spiPreemtionByIsr); // check pre-emption by ISR
    
  for (i = L6474_CMD_ARG_MAX_NB_BYTES-1-maxArgumentNbBytes;
       i < L6474_CMD_ARG_MAX_NB_BYTES;
       i++)
  {
     L6474_WriteBytes(&spiTxBursts[i][0], &spiRxBursts[i][0]);
  }
  
  spiRxData = ((uint32_t)spiRxBursts[1][spiIndex] << 16) |
              (spiRxBursts[2][spiIndex] << 8) |
              (spiRxBursts[3][spiIndex]);
  
  /* re-enable L6474_EnableIrq after SPI transfers*/
  L6474_EnableIrq();
    
  return (spiRxData);
}

/**********************************************************
 * @brief  Issues the GetStatus command to the L6474 of the specified device
 * @retval Status Register value
 * @note Once the GetStatus command is performed, the flags of the status register
 * are reset. This is not the case when the status register is read with the
 * GetParam command (via the functions L6474ReadStatusRegister or L6474_CmdGetParam).
 **********************************************************/
uint16_t L6474::L6474_CmdGetStatus(void)
{
  uint32_t i;
  uint16_t status;
  uint8_t spiIndex = numberOfDevices - deviceInstance - 1;
  bool itDisable = FALSE;  
  
  do
  {
    spiPreemtionByIsr = FALSE;
    if (itDisable)
    {
      /* re-enable L6474_EnableIrq if disable in previous iteration */
      L6474_EnableIrq();
      itDisable = FALSE;
    }

    for (i = 0; i < numberOfDevices; i++)
    {
       spiTxBursts[0][i] = L6474_NOP;
       spiTxBursts[1][i] = L6474_NOP;
       spiTxBursts[2][i] = L6474_NOP;
       spiRxBursts[1][i] = 0;
       spiRxBursts[2][i] = 0;
    }
    spiTxBursts[0][spiIndex] = L6474_GET_STATUS;

    /* Disable interruption before checking */
    /* pre-emption by ISR and SPI transfers*/
    L6474_DisableIrq();
    itDisable = TRUE;
  } while (spiPreemtionByIsr); // check pre-emption by ISR

  for (i = 0; i < L6474_CMD_ARG_NB_BYTES_GET_STATUS + L6474_RSP_NB_BYTES_GET_STATUS; i++)
  {
     L6474_WriteBytes(&spiTxBursts[i][0], &spiRxBursts[i][0]);
  }
  status = (spiRxBursts[1][spiIndex] << 8) | (spiRxBursts[2][spiIndex]);
  
  /* re-enable L6474_EnableIrq after SPI transfers*/
  L6474_EnableIrq();
  
  return (status);
}

/**********************************************************
 * @brief  Issues the Nop command to the L6474 of the specified device
 * @retval None
 **********************************************************/
void L6474::L6474_CmdNop(void)
{
  L6474_SendCommand(L6474_NOP);
}

/**********************************************************
 * @brief  Issues the SetParam command to the L6474 of the specified device
 * @param[in] parameter Register adress (L6474_ABS_POS, L6474_MARK,...)
 * @param[in] value Value to set in the register
 * @retval None
 **********************************************************/
void L6474::L6474_CmdSetParam(L6474_Registers_t parameter, uint32_t value)
{
  uint32_t i;
  uint8_t maxArgumentNbBytes = 0;
  uint8_t spiIndex = numberOfDevices - deviceInstance - 1;
  bool itDisable = FALSE;  
  do
  {
    spiPreemtionByIsr = FALSE;
    if (itDisable)
    {
      /* re-enable L6474_EnableIrq if disable in previous iteration */
      L6474_EnableIrq();
      itDisable = FALSE;
    }

    for (i = 0; i < numberOfDevices; i++)
    {
      spiTxBursts[0][i] = L6474_NOP;
      spiTxBursts[1][i] = L6474_NOP;
      spiTxBursts[2][i] = L6474_NOP;
      spiTxBursts[3][i] = L6474_NOP;
    }

    switch (parameter)
    {
      case L6474_ABS_POS: ;
      case L6474_MARK:
          spiTxBursts[0][spiIndex] = parameter;
          spiTxBursts[1][spiIndex] = (uint8_t)(value >> 16);
          spiTxBursts[2][spiIndex] = (uint8_t)(value >> 8);
          maxArgumentNbBytes = 3;
          break;
      case L6474_EL_POS: ;
      case L6474_CONFIG:
          spiTxBursts[1][spiIndex] = parameter;
          spiTxBursts[2][spiIndex] = (uint8_t)(value >> 8);
          maxArgumentNbBytes = 2;
          break;
      default:
          spiTxBursts[2][spiIndex] = parameter;
          maxArgumentNbBytes = 1;
          break;
    }
    spiTxBursts[3][spiIndex] = (uint8_t)(value);
    
    /* Disable interruption before checking */
    /* pre-emption by ISR and SPI transfers*/
    L6474_DisableIrq();
    itDisable = TRUE;
  } while (spiPreemtionByIsr); // check pre-emption by ISR
 
  /* SPI transfer */
  for (i = L6474_CMD_ARG_MAX_NB_BYTES-1-maxArgumentNbBytes;
       i < L6474_CMD_ARG_MAX_NB_BYTES;
       i++)
  {
     L6474_WriteBytes(&spiTxBursts[i][0],&spiRxBursts[i][0]);
  }
  /* re-enable L6474_EnableIrq after SPI transfers*/
  L6474_EnableIrq();
}

/**********************************************************
 * @brief  Reads the Status Register value
 * @retval Status register valued
 * @note The status register flags are not cleared 
 * at the difference with L6474CmdGetStatus()
 **********************************************************/
uint16_t L6474::L6474_ReadStatusRegister(void)
{
  return (L6474_CmdGetParam(L6474_STATUS));
}

/**********************************************************
 * @brief  Set the stepping mode 
 * @param[in] stepMod from full step to 1/16 microstep as specified in enum motorStepMode_t
 * @retval None
 **********************************************************/
void L6474::L6474_SelectStepMode(motorStepMode_t stepMod)
{
  uint8_t stepModeRegister;
  L6474_STEP_SEL_t l6474StepMod;
  
  switch (stepMod)
  {
    case STEP_MODE_FULL:
      l6474StepMod = L6474_STEP_SEL_1;
      break;
    case STEP_MODE_HALF:
      l6474StepMod = L6474_STEP_SEL_1_2;
      break;    
    case STEP_MODE_1_4:
      l6474StepMod = L6474_STEP_SEL_1_4;
      break;        
    case STEP_MODE_1_8:
      l6474StepMod = L6474_STEP_SEL_1_8;
      break;       
    case STEP_MODE_1_16:
    default:
      l6474StepMod = L6474_STEP_SEL_1_16;
      break;       
  }

  /* Eventually deactivate motor */
  if (devicePrm.motionState != INACTIVE) 
  {
    L6474_HardStop();
  }
  
  /* Read Step mode register and clear STEP_SEL field */
  stepModeRegister = (uint8_t)(0xF8 & L6474_CmdGetParam(L6474_STEP_MODE)) ;
  
  /* Apply new step mode */
  L6474_CmdSetParam(L6474_STEP_MODE, stepModeRegister | (uint8_t)l6474StepMod);

  /* Reset abs pos register */
  L6474_SetHome();
}

/**********************************************************
 * @brief  Get the direction
 * @param  None
 * @retval direction FORWARD or BACKWARD
 **********************************************************/
motorDir_t L6474::L6474_GetDirection(void)
{
  return devicePrm.direction;
}

/**********************************************************
 * @brief  Specifies the direction 
 * @param[in] dir FORWARD or BACKWARD
 * @note The direction change is only applied if the device 
 * is in INACTIVE state
 * @retval None
 **********************************************************/
void L6474::L6474_SetDirection(motorDir_t direction)
{
  if (devicePrm.motionState == INACTIVE)
  {
    devicePrm.direction = direction;
    L6474_SetDirectionGpio(direction);
  }
}

/**********************************************************
 * @brief  Updates the current speed of the device
 * @param[in] newSpeed in pps
 * @retval None
 **********************************************************/
void L6474::L6474_ApplySpeed(uint16_t newSpeed)
{
  if (newSpeed < L6474_MIN_PWM_FREQ)
  {
    newSpeed = L6474_MIN_PWM_FREQ;  
  }
  if (newSpeed > L6474_MAX_PWM_FREQ)
  {
    newSpeed = L6474_MAX_PWM_FREQ;
  }
  
  devicePrm.speed = newSpeed;

  L6474_PwmSetFreq(newSpeed);
}

/**********************************************************
 * @brief  Computes the speed profile according to the number of steps to move
 * @param[in] nbSteps number of steps to perform
 * @retval None
 * @note Using the acceleration and deceleration of the device,
 * this function determines the duration in steps of the acceleration,
 * steady and deceleration phases.
 * If the total number of steps to perform is big enough, a trapezoidal move
 * is performed (i.e. there is a steady phase where the motor runs at the maximum
 * speed.
 * Else, a triangular move is performed (no steady phase: the maximum speed is never
 * reached.
 **********************************************************/
void L6474::L6474_ComputeSpeedProfile(uint32_t nbSteps)
{
  uint32_t reqAccSteps; 
  uint32_t reqDecSteps;
   
  /* compute the number of steps to get the targeted speed */
  uint16_t minSpeed = devicePrm.minSpeed;
  reqAccSteps = (devicePrm.maxSpeed - minSpeed);
  reqAccSteps *= (devicePrm.maxSpeed + minSpeed);
  reqDecSteps = reqAccSteps;
  reqAccSteps /= (uint32_t)devicePrm.acceleration;
  reqAccSteps /= 2;

  /* compute the number of steps to stop */
  reqDecSteps /= (uint32_t)devicePrm.deceleration;
  reqDecSteps /= 2;

  if(( reqAccSteps + reqDecSteps ) > nbSteps)
  { 
    /* Triangular move  */
    /* reqDecSteps = (Pos * Dec) /(Dec+Acc) */
    uint32_t dec = devicePrm.deceleration;
    uint32_t acc = devicePrm.acceleration;
    
    reqDecSteps =  ((uint32_t) dec * nbSteps) / (acc + dec);
    if (reqDecSteps > 1)
    {
      reqAccSteps = reqDecSteps - 1;
      if(reqAccSteps == 0)
      {
        reqAccSteps = 1;
      }      
    }
    else
    {
      reqAccSteps = 0;
    }
    devicePrm.endAccPos = reqAccSteps;
    devicePrm.startDecPos = reqDecSteps;
  }
  else
  {  
    /* Trapezoidal move */
    /* accelerating phase to endAccPos */
    /* steady phase from  endAccPos to startDecPos */
    /* decelerating from startDecPos to stepsToTake*/
    devicePrm.endAccPos = reqAccSteps;
    devicePrm.startDecPos = nbSteps - reqDecSteps - 1;
  }
}

/**********************************************************
 * @brief  Converts the ABS_POSITION register value to a 32b signed integer
 * @param[in] abs_position_reg value of the ABS_POSITION register
 * @retval operation_result 32b signed integer corresponding to the absolute position 
 **********************************************************/
int32_t L6474::L6474_ConvertPosition(uint32_t abs_position_reg)
{
  int32_t operation_result;

  if (abs_position_reg & L6474_ABS_POS_SIGN_BIT_MASK) 
  {
    /* Negative register value */
    abs_position_reg = ~abs_position_reg;
    abs_position_reg += 1;

    operation_result = (int32_t) (abs_position_reg & L6474_ABS_POS_VALUE_MASK);
    operation_result = -operation_result;
  } 
  else 
  {
    operation_result = (int32_t) abs_position_reg;
  }

  return operation_result;
}

/**********************************************************
 * @brief Error handler which calls the user callback (if defined)
 * @param[in] error Number of the error
 * @retval None
 **********************************************************/
void L6474::L6474_ErrorHandler(uint16_t error)
{
  if (errorHandlerCallback != 0)
  {
    (void) errorHandlerCallback(error);
  }
  else   
  {
    /* Aborting the program. */
    exit(EXIT_FAILURE);
  }
}

/**********************************************************
 * @brief  Sends a command without arguments to the L6474 via the SPI
 * @param[in] param Command to send 
 * @retval None
 **********************************************************/
void L6474::L6474_SendCommand(uint8_t param)
{
  uint32_t i;
  bool itDisable = FALSE;
  uint8_t spiIndex = numberOfDevices - deviceInstance - 1;
  
  do
  {
    spiPreemtionByIsr = FALSE;
    if (itDisable)
    {
      /* re-enable L6474_EnableIrq if disable in previous iteration */
      L6474_EnableIrq();
      itDisable = FALSE;
    }
  
    for (i = 0; i < numberOfDevices; i++)
    {
      spiTxBursts[3][i] = L6474_NOP;     
    }
    spiTxBursts[3][spiIndex] = param;
    
    /* Disable interruption before checking */
    /* pre-emption by ISR and SPI transfers*/
    L6474_DisableIrq();
    itDisable = TRUE;
  } while (spiPreemtionByIsr); // check pre-emption by ISR

  L6474_WriteBytes(&spiTxBursts[3][0], &spiRxBursts[3][0]); 
  
  /* re-enable L6474_EnableIrq after SPI transfers*/
  L6474_EnableIrq();
}

/**********************************************************
 * @brief  Sets the registers of the L6474 to their predefined values 
 * from l6474_target_config.h
 * @retval None
 **********************************************************/
void L6474::L6474_SetRegisterToPredefinedValues(void)
{
  L6474_CmdSetParam(
                    L6474_ABS_POS,
                    0);
  L6474_CmdSetParam(
                    L6474_EL_POS,
                    0);
  L6474_CmdSetParam(
                    L6474_MARK,
                    0);
  switch (deviceInstance)
  {
    case 0:
      L6474_CmdSetParam(
                        L6474_TVAL,
                        L6474_Tval_Current_to_Par(L6474_CONF_PARAM_TVAL_DEVICE_0));
      L6474_CmdSetParam(
                        L6474_T_FAST,
                        (uint8_t)L6474_CONF_PARAM_TOFF_FAST_DEVICE_0 |
                        (uint8_t)L6474_CONF_PARAM_FAST_STEP_DEVICE_0);
      L6474_CmdSetParam(
                        L6474_TON_MIN,
                        L6474_Tmin_Time_to_Par(L6474_CONF_PARAM_TON_MIN_DEVICE_0)
                        );
      L6474_CmdSetParam(
                        L6474_TOFF_MIN,
                        L6474_Tmin_Time_to_Par(L6474_CONF_PARAM_TOFF_MIN_DEVICE_0));
      L6474_CmdSetParam(
                        L6474_OCD_TH,
                        L6474_CONF_PARAM_OCD_TH_DEVICE_0);
      L6474_CmdSetParam(
                        L6474_STEP_MODE,
                        (uint8_t)L6474_CONF_PARAM_STEP_SEL_DEVICE_0 |
                        (uint8_t)L6474_CONF_PARAM_SYNC_SEL_DEVICE_0);
      L6474_CmdSetParam(
                        L6474_ALARM_EN,
                        L6474_CONF_PARAM_ALARM_EN_DEVICE_0);
      L6474_CmdSetParam(
                        L6474_CONFIG,
                        (uint16_t)L6474_CONF_PARAM_CLOCK_SETTING_DEVICE_0 |
                        (uint16_t)L6474_CONF_PARAM_TQ_REG_DEVICE_0 |
                        (uint16_t)L6474_CONF_PARAM_OC_SD_DEVICE_0 |
                        (uint16_t)L6474_CONF_PARAM_SR_DEVICE_0 |
                        (uint16_t)L6474_CONF_PARAM_TOFF_DEVICE_0);
      break;
    case 1:
      L6474_CmdSetParam(
                        L6474_TVAL,
                        L6474_Tval_Current_to_Par(L6474_CONF_PARAM_TVAL_DEVICE_1));
      L6474_CmdSetParam(
                        L6474_T_FAST,
                        (uint8_t)L6474_CONF_PARAM_TOFF_FAST_DEVICE_1 |
                        (uint8_t)L6474_CONF_PARAM_FAST_STEP_DEVICE_1);
      L6474_CmdSetParam(
                        L6474_TON_MIN,
                        L6474_Tmin_Time_to_Par(L6474_CONF_PARAM_TON_MIN_DEVICE_1));
      L6474_CmdSetParam(
                        L6474_TOFF_MIN,
                        L6474_Tmin_Time_to_Par(L6474_CONF_PARAM_TOFF_MIN_DEVICE_1));
      L6474_CmdSetParam(
                        L6474_OCD_TH,
                        L6474_CONF_PARAM_OCD_TH_DEVICE_1);
      L6474_CmdSetParam(
                        L6474_STEP_MODE,
                        (uint8_t)L6474_CONF_PARAM_STEP_SEL_DEVICE_1 |
                        (uint8_t)L6474_CONF_PARAM_SYNC_SEL_DEVICE_1);
      L6474_CmdSetParam(
                        L6474_ALARM_EN,
                        L6474_CONF_PARAM_ALARM_EN_DEVICE_1);
      L6474_CmdSetParam(
                        L6474_CONFIG,
                        (uint16_t)L6474_CONF_PARAM_CLOCK_SETTING_DEVICE_1 |
                        (uint16_t)L6474_CONF_PARAM_TQ_REG_DEVICE_1 |
                        (uint16_t)L6474_CONF_PARAM_OC_SD_DEVICE_1 |
                        (uint16_t)L6474_CONF_PARAM_SR_DEVICE_1 |
                        (uint16_t)L6474_CONF_PARAM_TOFF_DEVICE_1);
      break;
    case 2:
      L6474_CmdSetParam(
                        L6474_TVAL,
                        L6474_Tval_Current_to_Par(L6474_CONF_PARAM_TVAL_DEVICE_2));
      L6474_CmdSetParam(
                        L6474_T_FAST,
                        (uint8_t)L6474_CONF_PARAM_TOFF_FAST_DEVICE_2 |
                        (uint8_t)L6474_CONF_PARAM_FAST_STEP_DEVICE_2);
      L6474_CmdSetParam(
                        L6474_TON_MIN,
                        L6474_Tmin_Time_to_Par(L6474_CONF_PARAM_TON_MIN_DEVICE_2));
      L6474_CmdSetParam(
                        L6474_TOFF_MIN,
                        L6474_Tmin_Time_to_Par(L6474_CONF_PARAM_TOFF_MIN_DEVICE_2));
      L6474_CmdSetParam(
                        L6474_OCD_TH,
                        L6474_CONF_PARAM_OCD_TH_DEVICE_2);
      L6474_CmdSetParam(
                        L6474_STEP_MODE,
                        (uint8_t)L6474_CONF_PARAM_STEP_SEL_DEVICE_2 |
                        (uint8_t)L6474_CONF_PARAM_SYNC_SEL_DEVICE_2);
      L6474_CmdSetParam(
                        L6474_ALARM_EN,
                        L6474_CONF_PARAM_ALARM_EN_DEVICE_2);
      L6474_CmdSetParam(
                        L6474_CONFIG,
                        (uint16_t)L6474_CONF_PARAM_CLOCK_SETTING_DEVICE_2 |
                        (uint16_t)L6474_CONF_PARAM_TQ_REG_DEVICE_2 |
                        (uint16_t)L6474_CONF_PARAM_OC_SD_DEVICE_2 |
                        (uint16_t)L6474_CONF_PARAM_SR_DEVICE_2 |
                        (uint16_t)L6474_CONF_PARAM_TOFF_DEVICE_2);
      break;
    default: ;
  }
}

/**********************************************************
 * @brief  Sets the parameters of the device to predefined values
 * from l6474_target_config.h
 * @param None
 * @retval None
 **********************************************************/
void L6474::L6474_SetDeviceParamsToPredefinedValues(void)
{
  switch (deviceInstance)
  {
    case 0:
      devicePrm.acceleration = L6474_CONF_PARAM_ACC_DEVICE_0;
      devicePrm.deceleration = L6474_CONF_PARAM_DEC_DEVICE_0;
      devicePrm.maxSpeed = L6474_CONF_PARAM_MAX_SPEED_DEVICE_0;
      devicePrm.minSpeed = L6474_CONF_PARAM_MIN_SPEED_DEVICE_0;
      break;

    case 1:
      devicePrm.acceleration = L6474_CONF_PARAM_ACC_DEVICE_1;
      devicePrm.deceleration = L6474_CONF_PARAM_DEC_DEVICE_1;
      devicePrm.maxSpeed = L6474_CONF_PARAM_MAX_SPEED_DEVICE_1;
      devicePrm.minSpeed = L6474_CONF_PARAM_MIN_SPEED_DEVICE_1;
      break;

    case 2:
      devicePrm.acceleration = L6474_CONF_PARAM_ACC_DEVICE_2;
      devicePrm.deceleration = L6474_CONF_PARAM_DEC_DEVICE_2;
      devicePrm.maxSpeed = L6474_CONF_PARAM_MAX_SPEED_DEVICE_2;
      devicePrm.minSpeed = L6474_CONF_PARAM_MIN_SPEED_DEVICE_2;
      break;
  }

  devicePrm.accu = 0;
  devicePrm.currentPosition = 0;
  devicePrm.endAccPos = 0;
  devicePrm.relativePos = 0;
  devicePrm.startDecPos = 0;
  devicePrm.stepsToTake = 0;
  devicePrm.speed = 0;
  devicePrm.commandExecuted = NO_CMD;
  devicePrm.direction = FORWARD;
  devicePrm.motionState = INACTIVE;
  
  L6474_SetRegisterToPredefinedValues();
}

/**********************************************************
 * @brief Initialises the bridge parameters to start the movement
 * and enable the power bridge
 * @retval None
 **********************************************************/
void L6474::L6474_StartMovement(void)  
{
  /* Enable L6474 powerstage */
  L6474_CmdEnable();
  if (devicePrm.endAccPos != 0)
  {
    devicePrm.motionState = ACCELERATING;
  }
  else
  {
    devicePrm.motionState = DECELERATING;    
  }
  devicePrm.accu = 0;
  devicePrm.relativePos = 0;
  L6474_ApplySpeed(devicePrm.minSpeed);
}

/**********************************************************
 * @brief  Handles the device state machine at each ste
 * @retval None
 * @note Must only be called by the timer ISR
 **********************************************************/
void L6474::L6474_StepClockHandler(void)
{
  /* Set isr flag */
  isrFlag = TRUE;
  
  /* Incrementation of the relative position */
  devicePrm.relativePos++;

  switch (devicePrm.motionState) 
  {
    case ACCELERATING: 
    {
        uint32_t relPos = devicePrm.relativePos;
        uint32_t endAccPos = devicePrm.endAccPos;
        uint16_t speed = devicePrm.speed;
        uint32_t acc = ((uint32_t)devicePrm.acceleration << 16);
        
        if ((devicePrm.commandExecuted == SOFT_STOP_CMD)||
            ((devicePrm.commandExecuted != RUN_CMD)&&  
             (relPos == devicePrm.startDecPos)))
        {
          devicePrm.motionState = DECELERATING;
          devicePrm.accu = 0;
        }
        else if ((speed >= devicePrm.maxSpeed)||
                 ((devicePrm.commandExecuted != RUN_CMD)&&
                  (relPos == endAccPos)))
        {
          devicePrm.motionState = STEADY;
        }
        else
        {
          bool speedUpdated = FALSE;
          /* Go on accelerating */
          if (speed == 0) speed =1;
          devicePrm.accu += acc / speed;
          while (devicePrm.accu >= (0X10000L))
          {
            devicePrm.accu -= (0X10000L);
            speed +=1;
            speedUpdated = TRUE;
          }
          
          if (speedUpdated)
          {
            if (speed > devicePrm.maxSpeed)
            {
              speed = devicePrm.maxSpeed;
            }    
            devicePrm.speed = speed;
            L6474_ApplySpeed(devicePrm.speed);
          }
        }
        break;
    }
    case STEADY: 
    {
      uint16_t maxSpeed = devicePrm.maxSpeed;
      uint32_t relativePos = devicePrm.relativePos;
      if  ((devicePrm.commandExecuted == SOFT_STOP_CMD)||
           ((devicePrm.commandExecuted != RUN_CMD)&&
            (relativePos >= (devicePrm.startDecPos))) ||
           ((devicePrm.commandExecuted == RUN_CMD)&&
            (devicePrm.speed > maxSpeed)))
      {
        devicePrm.motionState = DECELERATING;
        devicePrm.accu = 0;
      }
      else if ((devicePrm.commandExecuted == RUN_CMD)&&
               (devicePrm.speed < maxSpeed))
      {
        devicePrm.motionState = ACCELERATING;
        devicePrm.accu = 0;
      }
      break;
    }
    case DECELERATING: 
    {
      uint32_t relativePos = devicePrm.relativePos;
      uint16_t speed = devicePrm.speed;
      uint32_t deceleration = ((uint32_t)devicePrm.deceleration << 16);
      if (((devicePrm.commandExecuted == SOFT_STOP_CMD)&&(speed <=  devicePrm.minSpeed))||
          ((devicePrm.commandExecuted != RUN_CMD)&&
           (relativePos >= devicePrm.stepsToTake)))
      {
        /* Motion process complete */
        L6474_HardStop();
      }
      else if ((devicePrm.commandExecuted == RUN_CMD)&&
               (speed <= devicePrm.maxSpeed))
      {
        devicePrm.motionState = STEADY;
      }
      else
      {
        /* Go on decelerating */
        if (speed > devicePrm.minSpeed)
        {
          bool speedUpdated = FALSE;
          if (speed == 0) speed =1;
          devicePrm.accu += deceleration / speed;
          while (devicePrm.accu >= (0X10000L))
          {
            devicePrm.accu -= (0X10000L);
            if (speed > 1)
            {  
              speed -=1;
            }
            speedUpdated = TRUE;
          }
        
          if (speedUpdated)
          {
            if (speed < devicePrm.minSpeed)
            {
              speed = devicePrm.minSpeed;
            }  
            devicePrm.speed = speed;
            L6474_ApplySpeed(devicePrm.speed);
          }
        }
      }
      break;
    }
    default: 
    {
      break;
    }
  }  
  /* Set isr flag */
  isrFlag = FALSE;
}

/**********************************************************
 * @brief Converts mA in compatible values for TVAL register 
 * @param[in] Tval
 * @retval TVAL values
 **********************************************************/
uint8_t L6474::L6474_Tval_Current_to_Par(double Tval)
{
  return ((uint8_t)(((Tval - 31.25)/31.25)+0.5));
}

/**********************************************************
 * @brief Convert time in us in compatible values 
 * for TON_MIN register
 * @param[in] Tmin
 * @retval TON_MIN values
 **********************************************************/
uint8_t L6474::L6474_Tmin_Time_to_Par(double Tmin)
{
  return ((uint8_t)(((Tmin - 0.5)*2)+0.5));
}

/**********************************************************
 * @brief  Write and receive a byte via SPI
 * @param[in] pByteToTransmit pointer to the byte to transmit
 * @param[in] pReceivedByte pointer to the received byte
 * @retval None
 **********************************************************/
void L6474::L6474_WriteBytes(uint8_t *pByteToTransmit, uint8_t *pReceivedByte)
{
  if (L6474_SpiWriteBytes(pByteToTransmit, pReceivedByte) != 0)
  {
    L6474_ErrorHandler(L6474_ERROR_1);
  }
  
  if (isrFlag)
  {
    spiPreemtionByIsr = TRUE;
  }
}

/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/