Motor Control Library

Diff: Components/PowerStep01/PowerStep01.cpp

Revision:

5:e7dca8c6ae9f

Parent:

4:f48e8d87553e

Child:

7:9d772e2a9dbe

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Components/PowerStep01/PowerStep01.cpp	Fri Mar 24 10:24:39 2017 +0000
@@ -0,0 +1,1593 @@
+/**
+ ******************************************************************************
+ * @file    PowerStep01.cpp
+ * @author  IPC Rennes
+ * @version V1.0.0
+ * @date    March 18th, 2016
+ * @brief   Powerstep01 motor driver (Microstepping controller with power MOSFETs)
+ * @note    (C) COPYRIGHT 2016 STMicroelectronics
+ ******************************************************************************
+ * @attention
+ *
+ * <h2><center>&copy; COPYRIGHT(c) 2016 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.
+ *
+ ******************************************************************************
+ */
+
+/* Includes ------------------------------------------------------------------*/
+#include "PowerStep01.h"
+
+/* Definitions ---------------------------------------------------------------*/
+
+/* Error of bad SPI transaction. */
+#define POWERSTEP01_ERROR_1        (POWERSTEP01_ERROR_BASE|0x0001)
+
+/* Variables  ----------------------------------------------------------------*/
+
+/* Number of devices. */
+uint8_t PowerStep01::numberOfDevices = 0;
+
+/* ISR flags used to restart an interrupted SPI transfer when an error is reported. */
+bool PowerStep01::spiPreemptionByIsr = FALSE;
+bool PowerStep01::isrFlag = FALSE;
+
+/* SPI Transmission for Daisy-Chain Configuration. */
+uint8_t PowerStep01::spiTxBursts[POWERSTEP01_CMD_ARG_MAX_NB_BYTES][MAX_NUMBER_OF_DEVICES];
+uint8_t PowerStep01::spiRxBursts[POWERSTEP01_CMD_ARG_MAX_NB_BYTES][MAX_NUMBER_OF_DEVICES];
+
+
+/* Methods -------------------------------------------------------------------*/
+
+/**********************************************************
+ * @brief Starts the Powerstep01 library
+ * @param[in] pInit pointer to the initialization data
+ * @retval COMPONENT_OK in case of success.
+ **********************************************************/
+status_t PowerStep01::Powerstep01_Init(void* pInit)
+{ 
+  
+  /* configure the step clock */
+  Powerstep01_Board_StepClockInit();
+  
+  /* Standby-reset deactivation */
+  Powerstep01_Board_ReleaseReset();
+  
+  /* Let a delay after reset */
+  Powerstep01_Board_Delay(1);
+
+  if (pInit == 0)
+  {
+    // Set all registers to their predefined values 
+    // from powerstep01_target_config.h 
+    Powerstep01_SetRegisterToPredefinedValues();
+  }
+  else
+  {
+    Powerstep01_SetDeviceParamsToGivenValues((powerstep01_init_u_t*) pInit);
+  }
+  
+  // Put the Powerstep01 in HiZ state
+  Powerstep01_CmdHardHiZ();
+  
+  Powerstep01_FetchAndClearAllStatus();
+
+  return COMPONENT_OK;
+}
+
+/**********************************************************
+ * @brief Read id
+ * @param[in] id pointer to the identifier to be read.
+ * @retval COMPONENT_OK in case of success.
+ **********************************************************/
+status_t PowerStep01::Powerstep01_ReadID(uint8_t *id)
+{
+  *id = deviceInstance;
+
+  return COMPONENT_OK;
+}
+
+/**********************************************************
+ * @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 PowerStep01::Powerstep01_AttachErrorHandler(void (*callback)(uint16_t error))
+{
+  errorHandlerCallback = (void (*)(uint16_t error)) callback;
+}
+
+/**********************************************************
+ * @brief  Issues the get_status command to the Powerstep01 device
+ * @retval Status Register value
+ * @note Once the get_status 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 Powerstep01_ReadStatusRegister
+ * or Powerstep01_CmdGetParam).
+ **********************************************************/
+uint16_t PowerStep01::Powerstep01_CmdGetStatus(void)
+{
+  uint16_t status = 0;
+  uint32_t loop;
+  uint8_t spiIndex = numberOfDevices - deviceInstance - 1;
+  bool itDisable = FALSE;  
+ 
+  do
+  {
+    spiPreemptionByIsr = FALSE;
+    if (itDisable)
+    {
+      /* re-enable Powerstep01_Board_EnableIrq if disable in previous iteration */
+      Powerstep01_Board_EnableIrq();
+      itDisable = FALSE;
+    }    
+    for (loop = 0; loop < numberOfDevices; loop++)
+    {
+       spiTxBursts[0][loop] = POWERSTEP01_NOP;
+       spiTxBursts[1][loop] = POWERSTEP01_NOP;
+       spiTxBursts[2][loop] = POWERSTEP01_NOP;
+       spiTxBursts[3][loop] = POWERSTEP01_NOP;
+       spiRxBursts[0][loop] = 0;
+       spiRxBursts[1][loop] = 0;
+       spiRxBursts[2][loop] = 0;
+       spiRxBursts[3][loop] = 0;       
+    }
+    spiTxBursts[0][spiIndex] = POWERSTEP01_GET_STATUS;
+    /* Disable interruption before checking */
+    /* pre-emption by ISR and SPI transfers*/
+    Powerstep01_Board_DisableIrq();
+    itDisable = TRUE;
+  } while (spiPreemptionByIsr); // check pre-emption by ISR  
+  for (loop = 0; loop < POWERSTEP01_CMD_ARG_NB_BYTES_GET_STATUS + POWERSTEP01_RSP_NB_BYTES_GET_STATUS; loop++)
+  {
+     Powerstep01_WriteBytes(&spiTxBursts[loop][0], &spiRxBursts[loop][0]);
+  }
+  status = (spiRxBursts[1][spiIndex] << 8) | (spiRxBursts[2][spiIndex]);
+  /* re-enable Powerstep01_Board_EnableIrq after SPI transfers*/
+  Powerstep01_Board_EnableIrq();  
+
+  return (status);
+}
+
+/**********************************************************
+ * @brief  Requests the motor to move to the home position (ABS_POSITION = 0)
+ * @retval None
+ **********************************************************/
+void PowerStep01::Powerstep01_CmdGoHome(void)
+{
+  Powerstep01_SendCommand(POWERSTEP01_GO_HOME, 0);
+} 
+
+/**********************************************************
+ * @brief  Requests the motor to move to the mark position 
+ * @retval None
+ **********************************************************/
+void PowerStep01::Powerstep01_CmdGoMark(void)
+{
+    Powerstep01_SendCommand(POWERSTEP01_GO_MARK, 0); 
+}
+
+/**********************************************************
+ * @brief  Requests the motor to move to the specified position 
+ * @param[in] targetPosition absolute position in steps
+ * @retval None
+ **********************************************************/
+void PowerStep01::Powerstep01_CmdGoTo(int32_t targetPosition)
+{
+  Powerstep01_SendCommand(POWERSTEP01_GO_TO, targetPosition);
+}
+
+/******************************************************//**
+ * @brief Issues PowerStep01 Go To Dir command
+ * @param[in] direction movement direction
+ * @param[in] abs_pos absolute position where requested to move
+ * @retval None
+ *********************************************************/
+void PowerStep01::Powerstep01_CmdGoToDir(motorDir_t direction,
+                                         int32_t abs_pos)
+{
+  Powerstep01_SendCommand((uint8_t)POWERSTEP01_GO_TO_DIR|
+                          (uint8_t)direction, abs_pos);  
+}
+
+/******************************************************//**
+ * @brief Issues PowerStep01 Go Until command
+ * @param[in] action ACTION_RESET or ACTION_COPY
+ * @param[in] direction movement direction
+ * @param[in] speed in steps/tick
+ * @retval None
+ *********************************************************/
+void PowerStep01::Powerstep01_CmdGoUntil(motorAction_t action,
+                            motorDir_t direction,
+                            uint32_t speed)
+{
+  Powerstep01_SendCommand(
+    (uint8_t)POWERSTEP01_GO_UNTIL|(uint8_t)action|(uint8_t)direction,
+    speed);
+}
+
+/**********************************************************
+ * @brief Immediatly stops the motor and disable the power bridge
+ * @retval None
+ * @note The hard_hiz command immediately disables the power bridges
+ * (high impedance state) and raises the HiZ flag. 
+ * When the motor is stopped, a hard_hiz command forces the bridges 
+ * to enter high impedance state.
+ * This command can be given anytime and is immediately executed.
+ *********************************************************/
+void PowerStep01::Powerstep01_CmdHardHiZ(void)
+{
+  Powerstep01_SendCommand(POWERSTEP01_HARD_HIZ, 0);    
+}
+
+/**********************************************************
+ * @brief  Immediatly stops the motor and disable the power bridge
+ * @retval None
+ * @note The hard_stop command causes an immediate motor stop with
+ * infinite deceleration.
+ * When the motor is in high impedance state, a hard_stop command
+ * forces the bridges to exit high impedance state; no motion is performed.
+ * This command can be given anytime and is immediately executed.
+ * This command keeps the BUSY flag low until the motor is stopped.
+ **********************************************************/
+void PowerStep01::Powerstep01_CmdHardStop(void) 
+{
+  Powerstep01_SendCommand(POWERSTEP01_HARD_STOP, 0);
+}
+
+/**********************************************************
+ * @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 PowerStep01::Powerstep01_CmdMove(motorDir_t direction,
+                                      uint32_t stepCount)
+{
+  Powerstep01_SendCommand((uint8_t)POWERSTEP01_MOVE|(uint8_t)direction,
+    stepCount);
+}
+
+/**********************************************************
+ * @brief  Issues the Nop command to the Powerstep01 device
+ * @retval None
+ **********************************************************/
+void PowerStep01::Powerstep01_CmdNop(void)
+{
+  Powerstep01_SendCommand(POWERSTEP01_NOP, 0);
+}
+
+/******************************************************//**
+ * @brief Issues PowerStep01 Release SW command
+ * @param[in] action type of action to undertake when the SW
+ * input is forced high
+ * @param[in] direction movement direction
+ * @retval None
+ *********************************************************/
+void PowerStep01::Powerstep01_CmdReleaseSw(motorAction_t action,
+                                           motorDir_t direction)
+{
+   Powerstep01_SendCommand((uint8_t)POWERSTEP01_RELEASE_SW|
+                           (uint8_t)action|
+                           (uint8_t)direction,
+                           0);
+}
+
+/******************************************************//**
+ * @brief Issues PowerStep01 Reset Device command
+ * @param[in] deviceId (from 0 to MAX_NUMBER_OF_DEVICES-1 )
+ * @retval None
+ *********************************************************/
+void PowerStep01::Powerstep01_CmdResetDevice(void)
+{
+  Powerstep01_SendCommand(POWERSTEP01_RESET_DEVICE, 0);         
+}
+
+/******************************************************//**
+ * @brief Issues PowerStep01 Reset Pos command
+ * @param[in] deviceId (from 0 to MAX_NUMBER_OF_DEVICES-1 )
+ * @retval None
+ *********************************************************/
+void PowerStep01::Powerstep01_CmdResetPos(void)
+{
+  Powerstep01_SendCommand(POWERSTEP01_RESET_POS, 0);       
+}
+
+/**********************************************************
+ * @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
+ * @param[in] speed in steps/s
+ * @retval None
+ **********************************************************/
+void PowerStep01::Powerstep01_CmdRun(motorDir_t direction, uint32_t speed)
+{
+  Powerstep01_SendCommand((uint8_t)POWERSTEP01_RUN|(uint8_t)direction, speed);
+}
+
+/**********************************************************
+ * @brief Stops the motor by using the device deceleration
+ * and disables the power bridges
+ * @retval None
+ * @note The soft_hiz command disables the power bridges
+ * (high impedance state) after a deceleration to zero.
+ * The deceleration value used is the one stored in the DEC register.
+ * When bridges are disabled, the HiZ flag is raised.
+ * When the motor is stopped, a soft_hiz command forces the bridges
+ * to enter high impedance state.
+ * This command can be given anytime and is immediately executed.
+ * This command keeps the BUSY flag low until the motor is stopped.
+ *********************************************************/
+void PowerStep01::Powerstep01_CmdSoftHiZ(void)
+{
+  Powerstep01_SendCommand(POWERSTEP01_SOFT_HIZ, 0);           
+}
+
+/**********************************************************
+ * @brief  Stops the motor by using the device deceleration
+ * @retval None
+ * @note The soft_stop command causes an immediate deceleration
+ * to zero speed and a consequent motor stop.
+ * The deceleration value used is the one stored in the DEC register.
+ * When the motor is in high impedance state, a soft_stop
+ * command forces the bridges to exit from high impedance state.
+ * No motion is performed.
+ * This command can be given anytime and is immediately executed.
+ * This command keeps the BUSY flag low until the motor is stopped.
+ **********************************************************/
+void PowerStep01::Powerstep01_CmdSoftStop(void)
+{   
+  Powerstep01_SendCommand(POWERSTEP01_SOFT_STOP, 0);
+}
+
+/******************************************************//**
+ * @brief Issues PowerStep01 Step Clock command
+ * @param[in] direction Movement direction (FORWARD, BACKWARD)
+ * @retval None
+ *********************************************************/
+void PowerStep01::Powerstep01_CmdStepClock(motorDir_t direction)
+{
+  Powerstep01_SendCommand((uint8_t)POWERSTEP01_STEP_CLOCK|(uint8_t)direction,
+                          0);  
+}
+
+/**********************************************************
+ * @brief Error handler which calls the user callback (if defined)
+ * @param[in] error Number of the error
+ * @retval None
+ **********************************************************/
+void PowerStep01::Powerstep01_ErrorHandler(uint16_t error)
+{
+  if (errorHandlerCallback != 0)
+  {
+    (void) errorHandlerCallback(error);
+  }
+  else   
+  {
+    /* Aborting the program. */
+    exit(EXIT_FAILURE);
+  }
+}
+
+/******************************************************//**
+ * @brief Fetch and clear status flags of all devices 
+ * by issuing a GET_STATUS command simultaneously  
+ * to all devices.
+ * Then, the fetched status of each device can be retrieved
+ * by using the Powerstep01_GetFetchedStatus function
+ * provided there is no other calls to functions which 
+ * use the SPI in between.
+ * @retval None
+ *********************************************************/
+void PowerStep01::Powerstep01_FetchAndClearAllStatus(void)
+{
+  uint8_t loop;
+
+  for (loop = 0; loop < numberOfDevices; loop++)
+  {
+     spiTxBursts[0][loop] = POWERSTEP01_GET_STATUS;
+     spiTxBursts[1][loop] = POWERSTEP01_NOP;
+     spiTxBursts[2][loop] = POWERSTEP01_NOP;
+     spiTxBursts[3][loop] = POWERSTEP01_NOP;
+     spiRxBursts[0][loop] = 0;
+     spiRxBursts[1][loop] = 0;
+     spiRxBursts[2][loop] = 0;
+     spiRxBursts[3][loop] = 0;
+  }
+  for (loop = 0; 
+       loop < POWERSTEP01_CMD_ARG_NB_BYTES_GET_STATUS + 
+              POWERSTEP01_RSP_NB_BYTES_GET_STATUS; 
+       loop++)
+  {
+     Powerstep01_WriteBytes(&spiTxBursts[loop][0], &spiRxBursts[loop][0]);
+  }
+}
+
+/******************************************************//**
+ * @brief Get the value of the STATUS register which was 
+ * fetched by using Powerstep01_FetchAndClearAllStatus.
+ * The fetched values are available  as long as there
+ * no other calls to functions which use the SPI.
+ * @retval Last fetched value of the STATUS register
+ *********************************************************/
+uint16_t PowerStep01::Powerstep01_GetFetchedStatus(void)
+{
+  uint16_t status = 0;
+  if (numberOfDevices > deviceInstance)
+  {
+    uint8_t spiIndex = numberOfDevices - deviceInstance - 1;
+    status = (spiRxBursts[1][spiIndex] << 8) | (spiRxBursts[2][spiIndex]);
+  }
+  return (status);
+}
+
+/**********************************************************
+ * @brief Returns the FW version of the library
+ * @retval POWERSTEP01_FW_VERSION
+ **********************************************************/
+uint32_t PowerStep01::Powerstep01_GetFwVersion(void)
+{
+  return (POWERSTEP01_FW_VERSION);
+}
+
+/**********************************************************
+ * @brief  Returns the mark position  device
+ * @retval Mark register value converted in a 32b signed integer 
+ **********************************************************/
+int32_t PowerStep01::Powerstep01_GetMark(void)
+{
+  return Powerstep01_ConvertPosition(Powerstep01_CmdGetParam(POWERSTEP01_MARK));
+}
+
+/**********************************************************
+ * @brief  Returns the ABS_POSITION device
+ * @retval ABS_POSITION register value converted in a 32b signed integer
+ **********************************************************/
+int32_t PowerStep01::Powerstep01_GetPosition(void)
+{
+  return Powerstep01_ConvertPosition(
+    Powerstep01_CmdGetParam(POWERSTEP01_ABS_POS));
+}
+
+/**********************************************************
+ * @brief Checks if the device is busy
+ * by reading the Busy flag bit of its status Register
+ * This operation clears the status register
+ * @retval true if device is busy, false zero
+ *********************************************************/
+bool PowerStep01::Powerstep01_IsDeviceBusy(void)
+{
+  if(!(Powerstep01_CmdGetStatus() & POWERSTEP01_STATUS_BUSY)) 
+  {
+    return TRUE;
+  }
+  else 
+  {
+    return FALSE;
+  }
+}
+
+/**********************************************************
+ * @brief  Reads the Status Register value
+ * @retval Status register value
+ * @note The status register flags are not cleared 
+ * at the difference with Powerstep01_CmdGetStatus()
+ **********************************************************/
+uint16_t PowerStep01::Powerstep01_ReadStatusRegister(void)
+{
+  return (Powerstep01_CmdGetParam(POWERSTEP01_STATUS));
+}
+
+/**********************************************************
+ * @brief  Set the stepping mode 
+ * @param[in] stepMode from full step to 1/128 microstep
+ * as specified in enum motorStepMode_t
+ * @retval None
+ **********************************************************/
+bool PowerStep01::Powerstep01_SelectStepMode(motorStepMode_t stepMode)
+{
+  uint8_t stepModeRegister;
+  powerstep01_StepSel_t powerstep01StepMode;
+
+  switch (stepMode)
+  {
+    case STEP_MODE_FULL:
+      powerstep01StepMode = POWERSTEP01_STEP_SEL_1;
+      break;
+    case STEP_MODE_HALF:
+      powerstep01StepMode = POWERSTEP01_STEP_SEL_1_2;
+      break;    
+    case STEP_MODE_1_4:
+      powerstep01StepMode = POWERSTEP01_STEP_SEL_1_4;
+      break;        
+    case STEP_MODE_1_8:
+      powerstep01StepMode = POWERSTEP01_STEP_SEL_1_8;
+      break;
+    case STEP_MODE_1_16:
+      powerstep01StepMode = POWERSTEP01_STEP_SEL_1_16;
+      break;        
+    case STEP_MODE_1_32:
+      powerstep01StepMode = POWERSTEP01_STEP_SEL_1_32;
+      break;
+    case STEP_MODE_1_64:
+      powerstep01StepMode = POWERSTEP01_STEP_SEL_1_64;
+      break;
+    case STEP_MODE_1_128:
+      powerstep01StepMode = POWERSTEP01_STEP_SEL_1_128;
+      break;
+    default:
+      return false;
+  }
+  
+  /* Set the powerstep01 in HiZ state */
+  Powerstep01_CmdHardHiZ();  
+  
+  /* Read Step mode register and clear STEP_SEL field */
+  stepModeRegister = (uint8_t)(0xF8 & Powerstep01_CmdGetParam(POWERSTEP01_STEP_MODE)) ;
+  
+  /* Apply new step mode */
+  Powerstep01_CmdSetParam(POWERSTEP01_STEP_MODE, stepModeRegister | (uint8_t)powerstep01StepMode);
+
+  /* Reset abs pos register */
+  Powerstep01_CmdResetPos();
+
+  return true;
+}
+
+/**********************************************************
+ * @brief  Set current position to be the Home position (ABS pos set to 0)
+ * @retval None
+ **********************************************************/
+void PowerStep01::Powerstep01_SetHome(void)
+{
+  Powerstep01_CmdSetParam(POWERSTEP01_ABS_POS, 0);
+}
+
+/**********************************************************
+ * @brief  Sets current position to be the Mark position 
+ * @retval None
+ **********************************************************/
+void PowerStep01::Powerstep01_SetMark(void)
+{
+  Powerstep01_CmdSetParam(POWERSTEP01_MARK,
+    Powerstep01_CmdGetParam(POWERSTEP01_ABS_POS));
+}                                                  
+
+/**********************************************************
+ * @brief  Locks until the device state becomes Inactive
+ * @retval None
+ **********************************************************/
+void PowerStep01::Powerstep01_WaitWhileActive(void)
+{
+  /* Wait while motor is running */
+  while (Powerstep01_IsDeviceBusy() != 0);
+}
+
+/**
+ * @brief To and from register parameter conversion functions
+ */
+
+/**********************************************************
+ * @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 PowerStep01::Powerstep01_ConvertPosition(uint32_t abs_position_reg)
+{
+  int32_t operation_result;
+
+  if (abs_position_reg & POWERSTEP01_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 & POWERSTEP01_ABS_POS_VALUE_MASK);
+    operation_result = -operation_result;
+  } 
+  else 
+  {
+    operation_result = (int32_t) abs_position_reg;
+  }
+  return operation_result;
+}
+
+/**
+ * @brief Functions to initialize the registers
+ */
+
+/**********************************************************
+ * @brief Set the parameters of the device to values of initPrm structure
+ * @param[in] initPrm structure containing values to initialize the device
+ * parameters
+ * @retval None.
+ **********************************************************/
+void PowerStep01::Powerstep01_SetDeviceParamsToGivenValues(
+  powerstep01_init_u_t *initPrm)
+{
+  Powerstep01_CmdSetParam(POWERSTEP01_ABS_POS, 0);
+  Powerstep01_CmdSetParam(POWERSTEP01_EL_POS, 0);
+  Powerstep01_CmdSetParam(POWERSTEP01_MARK, 0);
+  Powerstep01_CmdSetParam(POWERSTEP01_ACC,
+    acc_dec_steps_s2_to_reg_val(initPrm->cm.cp.acceleration));
+  Powerstep01_CmdSetParam(POWERSTEP01_DEC,
+    acc_dec_steps_s2_to_reg_val(initPrm->cm.cp.deceleration));
+  Powerstep01_CmdSetParam(POWERSTEP01_MAX_SPEED,
+    max_spd_steps_s_to_reg_val(initPrm->cm.cp.maxSpeed));
+  Powerstep01_CmdSetParam(POWERSTEP01_MIN_SPEED,
+    initPrm->cm.cp.lowSpeedOptimization|
+    max_spd_steps_s_to_reg_val(initPrm->cm.cp.minSpeed));
+  Powerstep01_CmdSetParam(POWERSTEP01_FS_SPD,
+    initPrm->cm.cp.boostMode|
+    fs_spd_steps_s_to_reg_val(initPrm->cm.cp.fullStepSpeed));
+  Powerstep01_CmdSetParam(POWERSTEP01_OCD_TH,
+    stall_ocd_th_to_reg_val(initPrm->cm.cp.ocdThreshold));
+  Powerstep01_CmdSetParam(POWERSTEP01_STEP_MODE,
+    (uint8_t)initPrm->cm.cp.syncClockSelection|
+    (uint8_t)initPrm->cm.cp.cmVmSelection|
+    (uint8_t)(uint8_t)initPrm->cm.cp.stepMode);
+  Powerstep01_CmdSetParam(POWERSTEP01_ALARM_EN,
+    initPrm->cm.cp.alarmsSelection);
+  Powerstep01_CmdSetParam(POWERSTEP01_GATECFG1,
+    (uint16_t)initPrm->cm.cp.iGate|
+    (uint16_t)initPrm->cm.cp.tcc|
+    (uint16_t)initPrm->cm.cp.tBoost|
+    (uint16_t)initPrm->cm.cp.wdEn);
+  Powerstep01_CmdSetParam(POWERSTEP01_GATECFG2,
+    (uint16_t)initPrm->cm.cp.tBlank|
+    (uint16_t)initPrm->cm.cp.tdt);  
+  if (initPrm->cm.cp.cmVmSelection == POWERSTEP01_CM_VM_VOLTAGE)
+  {
+    //Voltage mode
+    Powerstep01_CmdSetParam(POWERSTEP01_INT_SPD,
+      int_spd_steps_s_to_reg_val(
+        initPrm->vm.intersectSpeed));
+    Powerstep01_CmdSetParam(POWERSTEP01_K_THERM,
+      k_therm_comp_to_reg_val(
+        initPrm->vm.thermalCompensationFactor)); 
+    Powerstep01_CmdSetParam(POWERSTEP01_STALL_TH,
+      stall_ocd_th_to_reg_val(
+        initPrm->vm.stallThreshold));
+    Powerstep01_CmdSetParam(POWERSTEP01_KVAL_HOLD,
+      k_val_perc_to_reg_val(
+        initPrm->vm.kvalHold));
+    Powerstep01_CmdSetParam(POWERSTEP01_KVAL_RUN,
+      k_val_perc_to_reg_val(
+        initPrm->vm.kvalRun));
+    Powerstep01_CmdSetParam(POWERSTEP01_KVAL_ACC,
+      k_val_perc_to_reg_val(
+        initPrm->vm.kvalAcc));
+    Powerstep01_CmdSetParam(POWERSTEP01_KVAL_DEC,
+      k_val_perc_to_reg_val(
+        initPrm->vm.kvalDec));
+    Powerstep01_CmdSetParam(POWERSTEP01_ST_SLP,
+      bemf_slope_perc_to_reg_val(
+        initPrm->vm.startSlope));
+    Powerstep01_CmdSetParam(POWERSTEP01_FN_SLP_ACC,
+      bemf_slope_perc_to_reg_val(
+        initPrm->vm.accelerationFinalSlope));
+    Powerstep01_CmdSetParam(POWERSTEP01_FN_SLP_DEC,
+      bemf_slope_perc_to_reg_val(
+        initPrm->vm.decelerationFinalSlope));
+    Powerstep01_CmdSetParam(POWERSTEP01_CONFIG,
+      (uint16_t)initPrm->vm.oscClkSel| 
+      (uint16_t)initPrm->vm.swMode | 
+      (uint16_t)initPrm->vm.enVsComp| 
+      (uint16_t)initPrm->vm.ocSd| 
+      (uint16_t)initPrm->vm.uvloVal| 
+      (uint16_t)initPrm->vm.vccVal| 
+      (uint16_t)initPrm->vm.fPwmInt| 
+      (uint16_t)initPrm->vm.fPwmDec);
+  }
+  else
+  {
+    // Current mode
+        Powerstep01_CmdSetParam(POWERSTEP01_TVAL_HOLD,
+          t_val_ref_voltage_to_reg_val(
+            initPrm->cm.tvalHold));
+        Powerstep01_CmdSetParam(POWERSTEP01_TVAL_RUN,
+          t_val_ref_voltage_to_reg_val(
+            initPrm->cm.tvalRun));
+        Powerstep01_CmdSetParam(POWERSTEP01_TVAL_ACC,
+          t_val_ref_voltage_to_reg_val(
+            initPrm->cm.tvalAcc));
+        Powerstep01_CmdSetParam(POWERSTEP01_TVAL_DEC,
+          t_val_ref_voltage_to_reg_val(
+            initPrm->cm.tvalDec));
+        Powerstep01_CmdSetParam(POWERSTEP01_T_FAST,
+          (uint8_t)initPrm->cm.toffFast|
+          (uint8_t)initPrm->cm.fastStep);
+        Powerstep01_CmdSetParam(POWERSTEP01_TON_MIN,
+          t_min_time_to_reg_val(
+            initPrm->cm.tonMin));
+        Powerstep01_CmdSetParam(POWERSTEP01_TOFF_MIN,
+          t_min_time_to_reg_val(
+            initPrm->cm.toffMin));       
+        Powerstep01_CmdSetParam(POWERSTEP01_CONFIG,
+          (uint16_t)initPrm->cm.oscClkSel| 
+          (uint16_t)initPrm->cm.swMode| 
+          (uint16_t)initPrm->cm.tqReg| 
+          (uint16_t)initPrm->cm.ocSd| 
+          (uint16_t)initPrm->cm.uvloVal| 
+          (uint16_t)initPrm->cm.vccVal|
+          (uint16_t)initPrm->cm.tsw|
+          (uint16_t)initPrm->cm.predEn);
+  }
+}
+
+/**********************************************************
+ * @brief Sets the registers of the Powerstep01 to their predefined values 
+ * from powerstep01_target_config.h
+ * @retval None
+ **********************************************************/
+void PowerStep01::Powerstep01_SetRegisterToPredefinedValues(void)
+{
+  powerstep01_CmVm_t cmVm;
+  
+  Powerstep01_CmdSetParam(
+    POWERSTEP01_ABS_POS,
+    0);
+  Powerstep01_CmdSetParam(
+    POWERSTEP01_EL_POS,
+    0);
+  Powerstep01_CmdSetParam(
+    POWERSTEP01_MARK,
+    0);
+  switch (deviceInstance)
+  {
+    case 0:
+      cmVm = POWERSTEP01_CONF_PARAM_CM_VM_DEVICE_0;
+      Powerstep01_CmdSetParam(POWERSTEP01_ACC,
+        acc_dec_steps_s2_to_reg_val(
+          POWERSTEP01_CONF_PARAM_ACC_DEVICE_0));
+      Powerstep01_CmdSetParam(POWERSTEP01_DEC,
+        acc_dec_steps_s2_to_reg_val(
+          POWERSTEP01_CONF_PARAM_DEC_DEVICE_0));
+      Powerstep01_CmdSetParam(POWERSTEP01_MAX_SPEED,
+        max_spd_steps_s_to_reg_val(
+          POWERSTEP01_CONF_PARAM_MAX_SPEED_DEVICE_0));
+      Powerstep01_CmdSetParam(POWERSTEP01_MIN_SPEED,
+        POWERSTEP01_CONF_PARAM_LSPD_BIT_DEVICE_0|
+        min_spd_steps_s_to_reg_val(
+          POWERSTEP01_CONF_PARAM_MIN_SPEED_DEVICE_0));
+      Powerstep01_CmdSetParam(POWERSTEP01_FS_SPD,
+        POWERSTEP01_CONF_PARAM_BOOST_MODE_DEVICE_0|
+        fs_spd_steps_s_to_reg_val(
+          POWERSTEP01_CONF_PARAM_FS_SPD_DEVICE_0)); 
+      Powerstep01_CmdSetParam(POWERSTEP01_OCD_TH,
+        (uint8_t)POWERSTEP01_CONF_PARAM_OCD_TH_DEVICE_0);
+      Powerstep01_CmdSetParam(POWERSTEP01_STEP_MODE,
+        (uint8_t)POWERSTEP01_CONF_PARAM_SYNC_MODE_DEVICE_0 |
+        (uint8_t)POWERSTEP01_CONF_PARAM_CM_VM_DEVICE_0|
+        (uint8_t)POWERSTEP01_CONF_PARAM_STEP_MODE_DEVICE_0);
+      Powerstep01_CmdSetParam(POWERSTEP01_ALARM_EN,
+        POWERSTEP01_CONF_PARAM_ALARM_EN_DEVICE_0);
+      Powerstep01_CmdSetParam(POWERSTEP01_GATECFG1,
+        (uint16_t)POWERSTEP01_CONF_PARAM_IGATE_DEVICE_0 | 
+        (uint16_t)POWERSTEP01_CONF_PARAM_TCC_DEVICE_0   | 
+        (uint16_t)POWERSTEP01_CONF_PARAM_TBOOST_DEVICE_0|
+        (uint16_t)POWERSTEP01_CONF_PARAM_WD_EN_DEVICE_0);
+      Powerstep01_CmdSetParam(POWERSTEP01_GATECFG2,
+        (uint16_t)POWERSTEP01_CONF_PARAM_TBLANK_DEVICE_0 | 
+        (uint16_t)POWERSTEP01_CONF_PARAM_TDT_DEVICE_0);
+      // Voltage mode
+      if (cmVm == POWERSTEP01_CM_VM_VOLTAGE)
+      {
+        Powerstep01_CmdSetParam(POWERSTEP01_INT_SPD,
+          int_spd_steps_s_to_reg_val(
+            POWERSTEP01_CONF_PARAM_INT_SPD_DEVICE_0));
+        Powerstep01_CmdSetParam(POWERSTEP01_K_THERM,
+          k_therm_comp_to_reg_val(
+            POWERSTEP01_CONF_PARAM_K_THERM_DEVICE_0));
+        Powerstep01_CmdSetParam(POWERSTEP01_STALL_TH,
+          stall_ocd_th_to_reg_val(
+            POWERSTEP01_CONF_PARAM_STALL_TH_DEVICE_0));
+        Powerstep01_CmdSetParam(POWERSTEP01_KVAL_HOLD,
+          k_val_perc_to_reg_val(
+            POWERSTEP01_CONF_PARAM_KVAL_HOLD_DEVICE_0));
+        Powerstep01_CmdSetParam(POWERSTEP01_KVAL_RUN,
+          k_val_perc_to_reg_val(
+            POWERSTEP01_CONF_PARAM_KVAL_RUN_DEVICE_0));
+        Powerstep01_CmdSetParam(POWERSTEP01_KVAL_ACC,
+          k_val_perc_to_reg_val(
+            POWERSTEP01_CONF_PARAM_KVAL_ACC_DEVICE_0));
+        Powerstep01_CmdSetParam(POWERSTEP01_KVAL_DEC,
+          k_val_perc_to_reg_val(
+            POWERSTEP01_CONF_PARAM_KVAL_DEC_DEVICE_0));
+        Powerstep01_CmdSetParam(POWERSTEP01_ST_SLP,
+          bemf_slope_perc_to_reg_val(
+            POWERSTEP01_CONF_PARAM_ST_SLP_DEVICE_0));
+        Powerstep01_CmdSetParam(POWERSTEP01_FN_SLP_ACC,
+          bemf_slope_perc_to_reg_val(
+            POWERSTEP01_CONF_PARAM_FN_SLP_ACC_DEVICE_0));  
+        Powerstep01_CmdSetParam(POWERSTEP01_FN_SLP_DEC,
+          bemf_slope_perc_to_reg_val(
+            POWERSTEP01_CONF_PARAM_FN_SLP_DEC_DEVICE_0));
+        Powerstep01_CmdSetParam(POWERSTEP01_CONFIG,
+          (uint16_t)POWERSTEP01_CONF_PARAM_CLOCK_SETTING_DEVICE_0 | 
+          (uint16_t)POWERSTEP01_CONF_PARAM_SW_MODE_DEVICE_0       | 
+          (uint16_t)POWERSTEP01_CONF_PARAM_VS_COMP_DEVICE_0       | 
+          (uint16_t)POWERSTEP01_CONF_PARAM_OC_SD_DEVICE_0         | 
+          (uint16_t)POWERSTEP01_CONF_PARAM_UVLOVAL_DEVICE_0       | 
+          (uint16_t)POWERSTEP01_CONF_PARAM_VCCVAL_DEVICE_0        | 
+          (uint16_t)POWERSTEP01_CONF_PARAM_PWM_DIV_DEVICE_0       | 
+          (uint16_t)POWERSTEP01_CONF_PARAM_PWM_MUL_DEVICE_0);
+      }
+      else
+      {
+        // Current mode
+        Powerstep01_CmdSetParam(POWERSTEP01_TVAL_HOLD,
+          t_val_ref_voltage_to_reg_val(
+            POWERSTEP01_CONF_PARAM_TVAL_HOLD_DEVICE_0));
+        Powerstep01_CmdSetParam(POWERSTEP01_TVAL_RUN,
+          t_val_ref_voltage_to_reg_val(
+            POWERSTEP01_CONF_PARAM_TVAL_RUN_DEVICE_0));
+        Powerstep01_CmdSetParam(POWERSTEP01_TVAL_ACC,
+          t_val_ref_voltage_to_reg_val(
+            POWERSTEP01_CONF_PARAM_TVAL_ACC_DEVICE_0));
+        Powerstep01_CmdSetParam(POWERSTEP01_TVAL_DEC,
+          t_val_ref_voltage_to_reg_val(
+            POWERSTEP01_CONF_PARAM_TVAL_DEC_DEVICE_0));
+        Powerstep01_CmdSetParam(POWERSTEP01_T_FAST,
+          (uint8_t)POWERSTEP01_CONF_PARAM_TOFF_FAST_DEVICE_0 |
+          (uint8_t)POWERSTEP01_CONF_PARAM_FAST_STEP_DEVICE_0);
+        Powerstep01_CmdSetParam(POWERSTEP01_TON_MIN,
+          t_min_time_to_reg_val(
+            POWERSTEP01_CONF_PARAM_TON_MIN_DEVICE_0));
+        Powerstep01_CmdSetParam(POWERSTEP01_TOFF_MIN,
+          t_min_time_to_reg_val(
+            POWERSTEP01_CONF_PARAM_TOFF_MIN_DEVICE_0));
+        Powerstep01_CmdSetParam(POWERSTEP01_CONFIG,
+          (uint16_t)POWERSTEP01_CONF_PARAM_CLOCK_SETTING_DEVICE_0 | 
+          (uint16_t)POWERSTEP01_CONF_PARAM_SW_MODE_DEVICE_0       | 
+          (uint16_t)POWERSTEP01_CONF_PARAM_TQ_REG_DEVICE_0        | 
+          (uint16_t)POWERSTEP01_CONF_PARAM_OC_SD_DEVICE_0         | 
+          (uint16_t)POWERSTEP01_CONF_PARAM_UVLOVAL_DEVICE_0       | 
+          (uint16_t)POWERSTEP01_CONF_PARAM_VCCVAL_DEVICE_0        | 
+          (uint16_t)POWERSTEP01_CONF_PARAM_TSW_DEVICE_0           |
+          (uint16_t)POWERSTEP01_CONF_PARAM_PRED_DEVICE_0);          
+      }
+      break;
+#if (MAX_NUMBER_OF_DEVICES > 1)
+   case 1:
+      cmVm = POWERSTEP01_CONF_PARAM_CM_VM_DEVICE_1;
+      Powerstep01_CmdSetParam(POWERSTEP01_ACC,
+        acc_dec_steps_s2_to_reg_val(
+          POWERSTEP01_CONF_PARAM_ACC_DEVICE_1));
+      Powerstep01_CmdSetParam(POWERSTEP01_DEC,
+        acc_dec_steps_s2_to_reg_val(
+          POWERSTEP01_CONF_PARAM_DEC_DEVICE_1));
+      Powerstep01_CmdSetParam(POWERSTEP01_MAX_SPEED,
+        max_spd_steps_s_to_reg_val(
+          POWERSTEP01_CONF_PARAM_MAX_SPEED_DEVICE_1));
+      Powerstep01_CmdSetParam(POWERSTEP01_MIN_SPEED,
+        POWERSTEP01_CONF_PARAM_LSPD_BIT_DEVICE_1|
+        min_spd_steps_s_to_reg_val(
+          POWERSTEP01_CONF_PARAM_MIN_SPEED_DEVICE_1));
+      Powerstep01_CmdSetParam(POWERSTEP01_FS_SPD,
+        POWERSTEP01_CONF_PARAM_BOOST_MODE_DEVICE_1|
+        fs_spd_steps_s_to_reg_val(
+          POWERSTEP01_CONF_PARAM_FS_SPD_DEVICE_1)); 
+      Powerstep01_CmdSetParam(POWERSTEP01_OCD_TH,
+        (uint8_t)POWERSTEP01_CONF_PARAM_OCD_TH_DEVICE_1);
+      Powerstep01_CmdSetParam(POWERSTEP01_STEP_MODE,
+        (uint8_t)POWERSTEP01_CONF_PARAM_SYNC_MODE_DEVICE_1 |
+        (uint8_t)POWERSTEP01_CONF_PARAM_CM_VM_DEVICE_1|
+        (uint8_t)POWERSTEP01_CONF_PARAM_STEP_MODE_DEVICE_1);
+      Powerstep01_CmdSetParam(POWERSTEP01_ALARM_EN,
+        POWERSTEP01_CONF_PARAM_ALARM_EN_DEVICE_1);
+      Powerstep01_CmdSetParam(POWERSTEP01_GATECFG1,
+        (uint16_t)POWERSTEP01_CONF_PARAM_IGATE_DEVICE_1 | 
+        (uint16_t)POWERSTEP01_CONF_PARAM_TCC_DEVICE_1   | 
+        (uint16_t)POWERSTEP01_CONF_PARAM_TBOOST_DEVICE_1|
+        (uint16_t)POWERSTEP01_CONF_PARAM_WD_EN_DEVICE_1);
+      Powerstep01_CmdSetParam(POWERSTEP01_GATECFG2,
+        (uint16_t)POWERSTEP01_CONF_PARAM_TBLANK_DEVICE_1 | 
+        (uint16_t)POWERSTEP01_CONF_PARAM_TDT_DEVICE_1);
+      // Voltage mode
+      if (cmVm == POWERSTEP01_CM_VM_VOLTAGE)
+      {
+        Powerstep01_CmdSetParam(POWERSTEP01_INT_SPD,
+          int_spd_steps_s_to_reg_val(
+            POWERSTEP01_CONF_PARAM_INT_SPD_DEVICE_1));
+        Powerstep01_CmdSetParam(POWERSTEP01_K_THERM,
+          k_therm_comp_to_reg_val(
+            POWERSTEP01_CONF_PARAM_K_THERM_DEVICE_1));
+        Powerstep01_CmdSetParam(POWERSTEP01_STALL_TH,
+          stall_ocd_th_to_reg_val(
+            POWERSTEP01_CONF_PARAM_STALL_TH_DEVICE_1));
+        Powerstep01_CmdSetParam(POWERSTEP01_KVAL_HOLD,
+          k_val_perc_to_reg_val(
+            POWERSTEP01_CONF_PARAM_KVAL_HOLD_DEVICE_1));
+        Powerstep01_CmdSetParam(POWERSTEP01_KVAL_RUN,
+          k_val_perc_to_reg_val(
+            POWERSTEP01_CONF_PARAM_KVAL_RUN_DEVICE_1));
+        Powerstep01_CmdSetParam(POWERSTEP01_KVAL_ACC,
+          k_val_perc_to_reg_val(
+            POWERSTEP01_CONF_PARAM_KVAL_ACC_DEVICE_1));
+        Powerstep01_CmdSetParam(POWERSTEP01_KVAL_DEC,
+          k_val_perc_to_reg_val(
+            POWERSTEP01_CONF_PARAM_KVAL_DEC_DEVICE_1));
+        Powerstep01_CmdSetParam(POWERSTEP01_ST_SLP,
+          bemf_slope_perc_to_reg_val(
+            POWERSTEP01_CONF_PARAM_ST_SLP_DEVICE_1));
+        Powerstep01_CmdSetParam(POWERSTEP01_FN_SLP_ACC,
+          bemf_slope_perc_to_reg_val(
+            POWERSTEP01_CONF_PARAM_FN_SLP_ACC_DEVICE_1));  
+        Powerstep01_CmdSetParam(POWERSTEP01_FN_SLP_DEC,
+          bemf_slope_perc_to_reg_val(
+            POWERSTEP01_CONF_PARAM_FN_SLP_DEC_DEVICE_1));
+        Powerstep01_CmdSetParam(POWERSTEP01_CONFIG,
+          (uint16_t)POWERSTEP01_CONF_PARAM_CLOCK_SETTING_DEVICE_1 | 
+          (uint16_t)POWERSTEP01_CONF_PARAM_SW_MODE_DEVICE_1       | 
+          (uint16_t)POWERSTEP01_CONF_PARAM_VS_COMP_DEVICE_1       | 
+          (uint16_t)POWERSTEP01_CONF_PARAM_OC_SD_DEVICE_1         | 
+          (uint16_t)POWERSTEP01_CONF_PARAM_UVLOVAL_DEVICE_1       | 
+          (uint16_t)POWERSTEP01_CONF_PARAM_VCCVAL_DEVICE_1        | 
+          (uint16_t)POWERSTEP01_CONF_PARAM_PWM_DIV_DEVICE_1       | 
+          (uint16_t)POWERSTEP01_CONF_PARAM_PWM_MUL_DEVICE_1);
+      }
+      else
+      {
+        // Current mode
+        Powerstep01_CmdSetParam(POWERSTEP01_TVAL_HOLD,
+          t_val_ref_voltage_to_reg_val(
+            POWERSTEP01_CONF_PARAM_TVAL_HOLD_DEVICE_1));
+        Powerstep01_CmdSetParam(POWERSTEP01_TVAL_RUN,
+          t_val_ref_voltage_to_reg_val(
+            POWERSTEP01_CONF_PARAM_TVAL_RUN_DEVICE_1));
+        Powerstep01_CmdSetParam(POWERSTEP01_TVAL_ACC,
+          t_val_ref_voltage_to_reg_val(
+            POWERSTEP01_CONF_PARAM_TVAL_ACC_DEVICE_1));
+        Powerstep01_CmdSetParam(POWERSTEP01_TVAL_DEC,
+          t_val_ref_voltage_to_reg_val(
+            POWERSTEP01_CONF_PARAM_TVAL_DEC_DEVICE_1));
+        Powerstep01_CmdSetParam(POWERSTEP01_T_FAST,
+          (uint8_t)POWERSTEP01_CONF_PARAM_TOFF_FAST_DEVICE_1 |
+          (uint8_t)POWERSTEP01_CONF_PARAM_FAST_STEP_DEVICE_1);
+        Powerstep01_CmdSetParam(POWERSTEP01_TON_MIN,
+          t_min_time_to_reg_val(
+            POWERSTEP01_CONF_PARAM_TON_MIN_DEVICE_1));
+        Powerstep01_CmdSetParam(POWERSTEP01_TOFF_MIN,
+          t_min_time_to_reg_val(
+            POWERSTEP01_CONF_PARAM_TOFF_MIN_DEVICE_1));
+        Powerstep01_CmdSetParam(POWERSTEP01_CONFIG,
+          (uint16_t)POWERSTEP01_CONF_PARAM_CLOCK_SETTING_DEVICE_1 | 
+          (uint16_t)POWERSTEP01_CONF_PARAM_SW_MODE_DEVICE_1       | 
+          (uint16_t)POWERSTEP01_CONF_PARAM_TQ_REG_DEVICE_1        | 
+          (uint16_t)POWERSTEP01_CONF_PARAM_OC_SD_DEVICE_1         | 
+          (uint16_t)POWERSTEP01_CONF_PARAM_UVLOVAL_DEVICE_1       | 
+          (uint16_t)POWERSTEP01_CONF_PARAM_VCCVAL_DEVICE_1        | 
+          (uint16_t)POWERSTEP01_CONF_PARAM_TSW_DEVICE_1           |
+          (uint16_t)POWERSTEP01_CONF_PARAM_PRED_DEVICE_1);          
+      }
+      break;     
+#endif
+#if (MAX_NUMBER_OF_DEVICES > 2)
+   case 2:
+      cmVm = POWERSTEP01_CONF_PARAM_CM_VM_DEVICE_2;
+      Powerstep01_CmdSetParam(POWERSTEP01_ACC,
+        acc_dec_steps_s2_to_reg_val(
+          POWERSTEP01_CONF_PARAM_ACC_DEVICE_2));
+      Powerstep01_CmdSetParam(POWERSTEP01_DEC,
+        acc_dec_steps_s2_to_reg_val(
+          POWERSTEP01_CONF_PARAM_DEC_DEVICE_2));
+      Powerstep01_CmdSetParam(POWERSTEP01_MAX_SPEED,
+        max_spd_steps_s_to_reg_val(
+          POWERSTEP01_CONF_PARAM_MAX_SPEED_DEVICE_2));
+      Powerstep01_CmdSetParam(POWERSTEP01_MIN_SPEED,
+        POWERSTEP01_CONF_PARAM_LSPD_BIT_DEVICE_2|
+        min_spd_steps_s_to_reg_val(
+          POWERSTEP01_CONF_PARAM_MIN_SPEED_DEVICE_2));
+      Powerstep01_CmdSetParam(POWERSTEP01_FS_SPD,
+        POWERSTEP01_CONF_PARAM_BOOST_MODE_DEVICE_2|
+        fs_spd_steps_s_to_reg_val(
+          POWERSTEP01_CONF_PARAM_FS_SPD_DEVICE_2)); 
+      Powerstep01_CmdSetParam(POWERSTEP01_OCD_TH,
+        (uint8_t)POWERSTEP01_CONF_PARAM_OCD_TH_DEVICE_2);
+      Powerstep01_CmdSetParam(POWERSTEP01_STEP_MODE,
+        (uint8_t)POWERSTEP01_CONF_PARAM_SYNC_MODE_DEVICE_2 |
+        (uint8_t)POWERSTEP01_CONF_PARAM_CM_VM_DEVICE_2|
+        (uint8_t)POWERSTEP01_CONF_PARAM_STEP_MODE_DEVICE_2);
+      Powerstep01_CmdSetParam(POWERSTEP01_ALARM_EN,
+        POWERSTEP01_CONF_PARAM_ALARM_EN_DEVICE_2);
+      Powerstep01_CmdSetParam(POWERSTEP01_GATECFG1,
+        (uint16_t)POWERSTEP01_CONF_PARAM_IGATE_DEVICE_2 | 
+        (uint16_t)POWERSTEP01_CONF_PARAM_TCC_DEVICE_2   | 
+        (uint16_t)POWERSTEP01_CONF_PARAM_TBOOST_DEVICE_2|
+        (uint16_t)POWERSTEP01_CONF_PARAM_WD_EN_DEVICE_2);
+      Powerstep01_CmdSetParam(POWERSTEP01_GATECFG2,
+        (uint16_t)POWERSTEP01_CONF_PARAM_TBLANK_DEVICE_2 | 
+        (uint16_t)POWERSTEP01_CONF_PARAM_TDT_DEVICE_2);
+      // Voltage mode
+      if (cmVm == POWERSTEP01_CM_VM_VOLTAGE)
+      {
+        Powerstep01_CmdSetParam(POWERSTEP01_INT_SPD,
+          int_spd_steps_s_to_reg_val(
+            POWERSTEP01_CONF_PARAM_INT_SPD_DEVICE_2));
+        Powerstep01_CmdSetParam(POWERSTEP01_K_THERM,
+          k_therm_comp_to_reg_val(
+            POWERSTEP01_CONF_PARAM_K_THERM_DEVICE_2));
+        Powerstep01_CmdSetParam(POWERSTEP01_STALL_TH,
+          stall_ocd_th_to_reg_val(
+            POWERSTEP01_CONF_PARAM_STALL_TH_DEVICE_2));
+        Powerstep01_CmdSetParam(POWERSTEP01_KVAL_HOLD,
+          k_val_perc_to_reg_val(
+            POWERSTEP01_CONF_PARAM_KVAL_HOLD_DEVICE_2));
+        Powerstep01_CmdSetParam(POWERSTEP01_KVAL_RUN,
+          k_val_perc_to_reg_val(
+            POWERSTEP01_CONF_PARAM_KVAL_RUN_DEVICE_2));
+        Powerstep01_CmdSetParam(POWERSTEP01_KVAL_ACC,
+          k_val_perc_to_reg_val(
+            POWERSTEP01_CONF_PARAM_KVAL_ACC_DEVICE_2));
+        Powerstep01_CmdSetParam(POWERSTEP01_KVAL_DEC,
+          k_val_perc_to_reg_val(
+            POWERSTEP01_CONF_PARAM_KVAL_DEC_DEVICE_2));
+        Powerstep01_CmdSetParam(POWERSTEP01_ST_SLP,
+          bemf_slope_perc_to_reg_val(
+            POWERSTEP01_CONF_PARAM_ST_SLP_DEVICE_2));
+        Powerstep01_CmdSetParam(POWERSTEP01_FN_SLP_ACC,
+          bemf_slope_perc_to_reg_val(
+            POWERSTEP01_CONF_PARAM_FN_SLP_ACC_DEVICE_2));  
+        Powerstep01_CmdSetParam(POWERSTEP01_FN_SLP_DEC,
+          bemf_slope_perc_to_reg_val(
+            POWERSTEP01_CONF_PARAM_FN_SLP_DEC_DEVICE_2));
+        Powerstep01_CmdSetParam(POWERSTEP01_CONFIG,
+          (uint16_t)POWERSTEP01_CONF_PARAM_CLOCK_SETTING_DEVICE_2 | 
+          (uint16_t)POWERSTEP01_CONF_PARAM_SW_MODE_DEVICE_2       | 
+          (uint16_t)POWERSTEP01_CONF_PARAM_VS_COMP_DEVICE_2       | 
+          (uint16_t)POWERSTEP01_CONF_PARAM_OC_SD_DEVICE_2         | 
+          (uint16_t)POWERSTEP01_CONF_PARAM_UVLOVAL_DEVICE_2       | 
+          (uint16_t)POWERSTEP01_CONF_PARAM_VCCVAL_DEVICE_2        | 
+          (uint16_t)POWERSTEP01_CONF_PARAM_PWM_DIV_DEVICE_2       | 
+          (uint16_t)POWERSTEP01_CONF_PARAM_PWM_MUL_DEVICE_2);
+      }
+      else
+      {
+        // Current mode
+        Powerstep01_CmdSetParam(POWERSTEP01_TVAL_HOLD,
+          t_val_ref_voltage_to_reg_val(
+            POWERSTEP01_CONF_PARAM_TVAL_HOLD_DEVICE_2));
+        Powerstep01_CmdSetParam(POWERSTEP01_TVAL_RUN,
+          t_val_ref_voltage_to_reg_val(
+            POWERSTEP01_CONF_PARAM_TVAL_RUN_DEVICE_2));
+        Powerstep01_CmdSetParam(POWERSTEP01_TVAL_ACC,
+          t_val_ref_voltage_to_reg_val(
+            POWERSTEP01_CONF_PARAM_TVAL_ACC_DEVICE_2));
+        Powerstep01_CmdSetParam(POWERSTEP01_TVAL_DEC,
+          t_val_ref_voltage_to_reg_val(
+            POWERSTEP01_CONF_PARAM_TVAL_DEC_DEVICE_2));
+        Powerstep01_CmdSetParam(POWERSTEP01_T_FAST,
+          (uint8_t)POWERSTEP01_CONF_PARAM_TOFF_FAST_DEVICE_2 |
+          (uint8_t)POWERSTEP01_CONF_PARAM_FAST_STEP_DEVICE_2);
+        Powerstep01_CmdSetParam(POWERSTEP01_TON_MIN,
+          t_min_time_to_reg_val(
+            POWERSTEP01_CONF_PARAM_TON_MIN_DEVICE_2));
+        Powerstep01_CmdSetParam(POWERSTEP01_TOFF_MIN,
+          t_min_time_to_reg_val(
+            POWERSTEP01_CONF_PARAM_TOFF_MIN_DEVICE_2));
+        Powerstep01_CmdSetParam(POWERSTEP01_CONFIG,
+          (uint16_t)POWERSTEP01_CONF_PARAM_CLOCK_SETTING_DEVICE_2 | 
+          (uint16_t)POWERSTEP01_CONF_PARAM_SW_MODE_DEVICE_2       | 
+          (uint16_t)POWERSTEP01_CONF_PARAM_TQ_REG_DEVICE_2        | 
+          (uint16_t)POWERSTEP01_CONF_PARAM_OC_SD_DEVICE_2         | 
+          (uint16_t)POWERSTEP01_CONF_PARAM_UVLOVAL_DEVICE_2       | 
+          (uint16_t)POWERSTEP01_CONF_PARAM_VCCVAL_DEVICE_2        | 
+          (uint16_t)POWERSTEP01_CONF_PARAM_TSW_DEVICE_2           |
+          (uint16_t)POWERSTEP01_CONF_PARAM_PRED_DEVICE_2);          
+      }
+      break;
+#endif
+    default: ;
+  }
+}
+
+/**
+  * @brief Functions to get and set parameters using digital or analog values
+  */
+
+/**********************************************************
+ * @brief  Issues the GetParam command to the Powerstep01 device
+ * @param[in] parameter Register adress (POWERSTEP01_ABS_POS,
+ * POWERSTEP01_MARK,...)
+ * @retval Register value
+ **********************************************************/
+uint32_t PowerStep01::Powerstep01_CmdGetParam(powerstep01_Registers_t param)
+{
+
+  uint32_t spiRxData;
+  uint32_t loop;
+  uint8_t maxArgumentNbBytes = 0;
+  uint8_t spiIndex = numberOfDevices - deviceInstance - 1;
+  bool itDisable = FALSE;
+  
+  do
+  {
+    spiPreemptionByIsr = FALSE;
+    if (itDisable)
+    {
+      /* re-enable Powerstep01_Board_EnableIrq if disable in previous iteration */
+      Powerstep01_Board_EnableIrq();
+      itDisable = FALSE;
+    }
+    for (loop = 0; loop < numberOfDevices; loop++)
+    {
+      spiTxBursts[0][loop] = POWERSTEP01_NOP;
+      spiTxBursts[1][loop] = POWERSTEP01_NOP;
+      spiTxBursts[2][loop] = POWERSTEP01_NOP;
+      spiTxBursts[3][loop] = POWERSTEP01_NOP;
+      spiRxBursts[0][loop] = 0;
+      spiRxBursts[1][loop] = 0;
+      spiRxBursts[2][loop] = 0;
+      spiRxBursts[3][loop] = 0;    
+    }
+    switch (param)
+    {
+      case POWERSTEP01_ABS_POS: 
+      case POWERSTEP01_MARK:
+      case POWERSTEP01_SPEED:
+        spiTxBursts[0][spiIndex] = ((uint8_t)POWERSTEP01_GET_PARAM )| (param);
+        maxArgumentNbBytes = 3;
+        break;
+      case POWERSTEP01_EL_POS:
+      case POWERSTEP01_ACC:
+      case POWERSTEP01_DEC:
+      case POWERSTEP01_MAX_SPEED:
+      case POWERSTEP01_MIN_SPEED:
+      case POWERSTEP01_FS_SPD:
+      case POWERSTEP01_INT_SPD:
+      case POWERSTEP01_CONFIG:
+      case POWERSTEP01_GATECFG1:
+      case POWERSTEP01_STATUS:
+        spiTxBursts[1][spiIndex] = ((uint8_t)POWERSTEP01_GET_PARAM )| (param);
+        maxArgumentNbBytes = 2;
+        break;
+    default:
+        spiTxBursts[2][spiIndex] = ((uint8_t)POWERSTEP01_GET_PARAM )| (param);
+        maxArgumentNbBytes = 1;
+    }
+    /* Disable interruption before checking */
+    /* pre-emption by ISR and SPI transfers*/
+    Powerstep01_Board_DisableIrq();
+    itDisable = TRUE;
+  } while (spiPreemptionByIsr); // check pre-emption by ISR
+  for (loop = POWERSTEP01_CMD_ARG_MAX_NB_BYTES-1-maxArgumentNbBytes;
+       loop < POWERSTEP01_CMD_ARG_MAX_NB_BYTES;
+       loop++)
+  {
+     Powerstep01_WriteBytes(&spiTxBursts[loop][0],
+                           &spiRxBursts[loop][0]);
+  }
+  spiRxData = ((uint32_t)spiRxBursts[1][spiIndex] << 16)|
+               (spiRxBursts[2][spiIndex] << 8) |
+               (spiRxBursts[3][spiIndex]);    
+  /* re-enable Powerstep01_Board_EnableIrq after SPI transfers*/
+  Powerstep01_Board_EnableIrq();
+  return (spiRxData);
+}
+
+/**********************************************************
+ * @brief  Issues the SetParam command to the PowerStep01 device
+ * @param[in] parameter Register adress (POWERSTEP01_ABS_POS,
+ * POWERSTEP01_MARK,...)
+ * @param[in] value Value to set in the register
+ * @retval None
+ **********************************************************/
+void PowerStep01::Powerstep01_CmdSetParam(powerstep01_Registers_t param,
+  uint32_t value)
+{
+  uint32_t loop;
+  uint8_t maxArgumentNbBytes = 0;
+  uint8_t spiIndex = numberOfDevices - deviceInstance - 1;
+  bool itDisable = FALSE;
+  
+  do
+  {
+    spiPreemptionByIsr = FALSE;
+    if (itDisable)
+    {
+      /* re-enable Powerstep01_Board_EnableIrq if disable in previous iteration */
+      Powerstep01_Board_EnableIrq();
+      itDisable = FALSE;
+    }  
+    for (loop = 0;loop < numberOfDevices; loop++)
+    {
+      spiTxBursts[0][loop] = POWERSTEP01_NOP;
+      spiTxBursts[1][loop] = POWERSTEP01_NOP;
+      spiTxBursts[2][loop] = POWERSTEP01_NOP;
+      spiTxBursts[3][loop] = POWERSTEP01_NOP;
+    }
+    switch (param)
+    {
+      case POWERSTEP01_ABS_POS: ;
+      case POWERSTEP01_MARK:
+        spiTxBursts[0][spiIndex] = ((uint8_t)POWERSTEP01_SET_PARAM )| (param);
+        spiTxBursts[1][spiIndex] = (uint8_t)(value >> 16);
+        spiTxBursts[2][spiIndex] = (uint8_t)(value >> 8);
+        maxArgumentNbBytes = 3;
+        break;
+      case POWERSTEP01_EL_POS:
+      case POWERSTEP01_ACC:
+      case POWERSTEP01_DEC:
+      case POWERSTEP01_MAX_SPEED:
+      case POWERSTEP01_MIN_SPEED:
+      case POWERSTEP01_FS_SPD:
+      case POWERSTEP01_INT_SPD:
+      case POWERSTEP01_CONFIG:
+      case POWERSTEP01_GATECFG1:
+        spiTxBursts[1][spiIndex] = ((uint8_t)POWERSTEP01_SET_PARAM )| (param);
+        spiTxBursts[2][spiIndex] = (uint8_t)(value >> 8);
+        maxArgumentNbBytes = 2;
+        break;
+      default:
+        spiTxBursts[2][spiIndex] = ((uint8_t)POWERSTEP01_SET_PARAM )| (param);
+        maxArgumentNbBytes = 1;
+    }
+    spiTxBursts[3][spiIndex] = (uint8_t)(value);
+    /* Disable interruption before checking */
+    /* pre-emption by ISR and SPI transfers*/
+    Powerstep01_Board_DisableIrq();
+    itDisable = TRUE;
+  } while (spiPreemptionByIsr); // check pre-emption by ISR  
+  /* SPI transfer */
+  for (loop = POWERSTEP01_CMD_ARG_MAX_NB_BYTES - 1 - maxArgumentNbBytes;
+       loop < POWERSTEP01_CMD_ARG_MAX_NB_BYTES;
+       loop++)
+  {
+    Powerstep01_WriteBytes(&spiTxBursts[loop][0],&spiRxBursts[loop][0]);
+  }
+  /* re-enable Powerstep01_Board_EnableIrq after SPI transfers*/
+  Powerstep01_Board_EnableIrq();
+}
+
+/**********************************************************
+ * @brief Issues PowerStep01 Get Parameter command and convert the result to
+ * float value
+ * @param[in] param PowerStep01 register address
+ * @retval The parameter's float value.
+ *********************************************************/
+float PowerStep01::Powerstep01_GetAnalogValue(powerstep01_Registers_t param)
+{
+  bool voltageMode = ((POWERSTEP01_CM_VM_CURRENT&Powerstep01_CmdGetParam(POWERSTEP01_STEP_MODE))==0);
+  uint32_t registerValue = Powerstep01_CmdGetParam((powerstep01_Registers_t) param);
+  float value;
+  switch (param)
+  {
+    case POWERSTEP01_ABS_POS:
+    case POWERSTEP01_MARK:
+      value = (float) Powerstep01_ConvertPosition(registerValue);
+      break;
+    case POWERSTEP01_ACC:
+    case POWERSTEP01_DEC:
+      value = acc_dec_reg_val_to_steps_s2(registerValue);
+      break;
+    case POWERSTEP01_SPEED:
+      value = speed_reg_val_to_steps_s(registerValue);
+      break;
+    case POWERSTEP01_MAX_SPEED:
+      value = max_spd_reg_val_to_steps_s(registerValue);
+      break;
+    case POWERSTEP01_MIN_SPEED:
+      registerValue &= POWERSTEP01_MIN_SPEED_MASK;
+      value = min_spd_reg_val_to_steps_s(registerValue);
+      break;      
+    case POWERSTEP01_FS_SPD:
+      registerValue &= POWERSTEP01_FS_SPD_MASK;
+      value = fs_spd_reg_val_to_steps_s(registerValue);
+      break;
+    case POWERSTEP01_INT_SPD:
+      value = int_spd_reg_val_to_steps_s(registerValue);
+      break;
+    case POWERSTEP01_K_THERM:
+      value = k_therm_reg_val_to_comp(registerValue);
+      break;
+    case POWERSTEP01_OCD_TH:
+    case POWERSTEP01_STALL_TH:
+      value = stall_ocd_reg_val_to_th(registerValue);
+      break;
+    case POWERSTEP01_KVAL_HOLD:  //POWERSTEP01_TVAL_HOLD
+    case POWERSTEP01_KVAL_RUN:   //POWERSTEP01_TVAL_RUN
+    case POWERSTEP01_KVAL_ACC:   //POWERSTEP01_TVAL_ACC
+    case POWERSTEP01_KVAL_DEC:   //POWERSTEP01_TVAL_DEC
+      if (voltageMode!=FALSE)  value = k_val_reg_val_to_perc(registerValue);
+      else value = k_val_reg_val_to_perc(registerValue);     
+      break;
+    case POWERSTEP01_ST_SLP:
+      if (voltageMode==FALSE) 
+      {
+        break;
+      }      
+    case POWERSTEP01_FN_SLP_ACC: //POWERSTEP01_TON_MIN
+    case POWERSTEP01_FN_SLP_DEC: //POWERSTEP01_TOFF_MIN
+      if (voltageMode!=FALSE) value = bemf_slope_reg_val_to_perc(registerValue);
+      else value = t_min_reg_val_to_time(registerValue);
+      break;
+    default:
+      value = (float) registerValue;
+  }
+  return value;
+}
+
+/******************************************************//**
+ * @brief Put commands in queue before synchronous sending
+ * done by calling Powerstep01_SendQueuedCommands.
+ * Any call to functions that use the SPI between the calls of 
+ * Powerstep01_QueueCommands and Powerstep01_SendQueuedCommands 
+ * will corrupt the queue.
+ * A command for each device of the daisy chain must be 
+ * specified before calling Powerstep01_SendQueuedCommands.
+ * @param[in] command Command to queue (all Powerstep01 commmands 
+ * except POWERSTEP01_SET_PARAM, POWERSTEP01_GET_PARAM, 
+ * POWERSTEP01_GET_STATUS)
+ * @param[in] value argument of the command to queue
+ * @retval None
+ *********************************************************/
+void PowerStep01::Powerstep01_QueueCommands(uint8_t command, int32_t value)
+{
+  if (numberOfDevices > deviceInstance)
+  {
+    uint8_t spiIndex = numberOfDevices - deviceInstance - 1;
+    
+    switch (command & DAISY_CHAIN_COMMAND_MASK)
+    {
+      case POWERSTEP01_RUN: ;
+      case POWERSTEP01_MOVE: ;
+      case POWERSTEP01_GO_TO: ;
+      case POWERSTEP01_GO_TO_DIR: ;
+      case POWERSTEP01_GO_UNTIL: ;
+      case POWERSTEP01_GO_UNTIL_ACT_CPY:
+       spiTxBursts[0][spiIndex] = command;
+       spiTxBursts[1][spiIndex] = (uint8_t)(value >> 16);
+       spiTxBursts[2][spiIndex] = (uint8_t)(value >> 8);
+       spiTxBursts[3][spiIndex] = (uint8_t)(value);
+       break;
+      default:
+       spiTxBursts[0][spiIndex] = POWERSTEP01_NOP;
+       spiTxBursts[1][spiIndex] = POWERSTEP01_NOP;
+       spiTxBursts[2][spiIndex] = POWERSTEP01_NOP;
+       spiTxBursts[3][spiIndex] = command;
+    }
+  }
+}
+
+/**********************************************************
+ * @brief  Sends a command to the device via the SPI
+ * @param[in] command Command to send (all Powerstep01 commmands 
+ * except POWERSTEP01_SET_PARAM, POWERSTEP01_GET_PARAM, 
+ * POWERSTEP01_GET_STATUS)
+ * @param[in] value arguments to send on 32 bits
+ * @retval None
+ **********************************************************/
+void PowerStep01::Powerstep01_SendCommand(uint8_t command, uint32_t value)
+{
+  uint32_t loop;
+  uint8_t maxArgumentNbBytes = 0;
+  bool itDisable = FALSE;
+  uint8_t spiIndex = numberOfDevices - deviceInstance - 1;
+  
+  do
+  {
+    spiPreemptionByIsr = FALSE;
+    if (itDisable)
+    {
+      /* re-enable Powerstep01_Board_EnableIrq if disable in previous iteration */
+      Powerstep01_Board_EnableIrq();
+      itDisable = FALSE;
+    }    
+    for (loop = 0; loop < numberOfDevices; loop++)
+    {
+        spiTxBursts[0][loop] = POWERSTEP01_NOP;
+        spiTxBursts[1][loop] = POWERSTEP01_NOP;
+        spiTxBursts[2][loop] = POWERSTEP01_NOP;
+        spiTxBursts[3][loop] = POWERSTEP01_NOP;   
+    }
+    switch (command & DAISY_CHAIN_COMMAND_MASK)
+    {
+      case POWERSTEP01_GO_TO:
+      case POWERSTEP01_GO_TO_DIR:
+        value = value & POWERSTEP01_ABS_POS_VALUE_MASK;
+      case POWERSTEP01_RUN:
+      case POWERSTEP01_MOVE:
+      case POWERSTEP01_GO_UNTIL:
+      case POWERSTEP01_GO_UNTIL_ACT_CPY:
+        spiTxBursts[0][spiIndex] = command;
+        spiTxBursts[1][spiIndex] = (uint8_t)(value >> 16);
+        spiTxBursts[2][spiIndex] = (uint8_t)(value >> 8);
+        spiTxBursts[3][spiIndex] = (uint8_t)(value);
+        maxArgumentNbBytes = 3;
+        break;
+    default:
+        spiTxBursts[0][spiIndex] = POWERSTEP01_NOP;
+        spiTxBursts[1][spiIndex] = POWERSTEP01_NOP;
+        spiTxBursts[2][spiIndex] = POWERSTEP01_NOP;
+        spiTxBursts[3][spiIndex] = command;
+    }
+    /* Disable interruption before checking */
+    /* pre-emption by ISR and SPI transfers*/
+    Powerstep01_Board_DisableIrq();
+    itDisable = TRUE;
+  } while (spiPreemptionByIsr); // check pre-emption by ISR
+  for (loop = POWERSTEP01_CMD_ARG_MAX_NB_BYTES - 1 - maxArgumentNbBytes; 
+       loop < POWERSTEP01_CMD_ARG_MAX_NB_BYTES; 
+       loop++)
+  {
+     Powerstep01_WriteBytes(&spiTxBursts[loop][0], &spiRxBursts[loop][0]);
+  }
+  /* re-enable Powerstep01_Board_EnableIrq after SPI transfers*/
+  Powerstep01_Board_EnableIrq();
+}
+
+/******************************************************//**
+ * @brief Sends commands stored previously in the queue by 
+ * Powerstep01_QueueCommands
+ * @retval None
+ *********************************************************/
+void PowerStep01::Powerstep01_SendQueuedCommands(void)
+{
+  uint8_t loop;
+  
+  for (loop = 0; 
+       loop < POWERSTEP01_CMD_ARG_MAX_NB_BYTES; 
+       loop++)
+  {
+     Powerstep01_WriteBytes(&spiTxBursts[loop][0], &spiRxBursts[loop][0]);
+  }
+}
+
+/**********************************************************
+ * @brief Issues the SetParam command to the PowerStep01
+ * @param[in] param PowerStep01 Register address
+ * @param[in] value Float value to convert and set into the register
+ * @retval TRUE if param is valid, FALSE otherwise
+ *********************************************************/
+bool PowerStep01::Powerstep01_SetAnalogValue(powerstep01_Registers_t param, float value)
+{
+  uint32_t registerValue;
+  bool result = TRUE;
+  bool voltageMode = ((POWERSTEP01_CM_VM_CURRENT&Powerstep01_CmdGetParam(POWERSTEP01_STEP_MODE))==0);
+  if ((value < 0)&&((param != POWERSTEP01_ABS_POS)&&(param != POWERSTEP01_MARK)))
+  {
+    result = FALSE;
+  }
+  switch (param)
+  {
+    case POWERSTEP01_EL_POS:
+      if ((value > (POWERSTEP01_ELPOS_STEP_MASK|POWERSTEP01_ELPOS_MICROSTEP_MASK))||
+          ((value!=0)&&(value < (1<<(7-(POWERSTEP01_STEP_MODE_STEP_SEL&Powerstep01_CmdGetParam(POWERSTEP01_STEP_MODE))))))) result = FALSE;
+      else registerValue = ((uint32_t) value)&(POWERSTEP01_ELPOS_STEP_MASK|POWERSTEP01_ELPOS_MICROSTEP_MASK);
+      break;        
+    case POWERSTEP01_ABS_POS:
+    case POWERSTEP01_MARK:
+      if (value < 0)
+      {
+        value=-value;
+        if (((uint32_t)value)<=(POWERSTEP01_MAX_POSITION+1))
+          registerValue = (POWERSTEP01_ABS_POS_VALUE_MASK+1-(uint32_t)value)&POWERSTEP01_ABS_POS_VALUE_MASK;
+        else result = FALSE;  
+      }
+      else
+      {
+        if (((uint32_t)value)<=POWERSTEP01_MAX_POSITION)
+          registerValue = ((uint32_t) value)&POWERSTEP01_ABS_POS_VALUE_MASK;
+        else result = FALSE; 
+      }
+      break;
+    case POWERSTEP01_ACC:
+    case POWERSTEP01_DEC:
+      if (value > POWERSTEP01_ACC_DEC_MAX_VALUE) result = FALSE;
+      else registerValue = acc_dec_steps_s2_to_reg_val(value);
+      break;
+    case POWERSTEP01_MAX_SPEED:
+      if (value > POWERSTEP01_MAX_SPEED_MAX_VALUE) result = FALSE;
+      else registerValue = max_spd_steps_s_to_reg_val(value);
+      break;
+    case POWERSTEP01_MIN_SPEED:
+      if (value > POWERSTEP01_MIN_SPEED_MAX_VALUE) result = FALSE;
+      else registerValue = (POWERSTEP01_LSPD_OPT&Powerstep01_CmdGetParam(param))|min_spd_steps_s_to_reg_val(value);
+      break;      
+    case POWERSTEP01_FS_SPD:
+      if (value > POWERSTEP01_FS_SPD_MAX_VALUE) result = FALSE;
+      else registerValue = (POWERSTEP01_BOOST_MODE&Powerstep01_CmdGetParam(param))|fs_spd_steps_s_to_reg_val(value);
+      break;
+    case POWERSTEP01_INT_SPD:
+      if (value > POWERSTEP01_INT_SPD_MAX_VALUE) result = FALSE;
+      else registerValue = int_spd_steps_s_to_reg_val(value);
+      break;
+    case POWERSTEP01_K_THERM:
+      if ((value < POWERSTEP01_K_THERM_MIN_VALUE)||
+          (value > POWERSTEP01_K_THERM_MAX_VALUE)) result = FALSE;
+      else registerValue = k_therm_comp_to_reg_val(value);
+      break;
+    case POWERSTEP01_OCD_TH:
+    case POWERSTEP01_STALL_TH:
+      if (value > POWERSTEP01_STALL_OCD_TH_MAX_VALUE) result = FALSE;
+      else registerValue = stall_ocd_th_to_reg_val(value);
+      break;
+    case POWERSTEP01_KVAL_HOLD:  //POWERSTEP01_TVAL_HOLD
+    case POWERSTEP01_KVAL_RUN:   //POWERSTEP01_TVAL_RUN
+    case POWERSTEP01_KVAL_ACC:   //POWERSTEP01_TVAL_ACC
+    case POWERSTEP01_KVAL_DEC:   //POWERSTEP01_TVAL_DEC
+      if (voltageMode==FALSE)
+      {
+        if (value > POWERSTEP01_TVAL_MAX_VALUE) result = FALSE;
+        else registerValue = t_val_ref_voltage_to_reg_val(value);
+      }
+      else
+      {
+        if (value > POWERSTEP01_KVAL_MAX_VALUE) result = FALSE;
+        else registerValue = k_val_perc_to_reg_val(value);
+      }
+      break;
+    case POWERSTEP01_ST_SLP:
+      if (voltageMode==FALSE) 
+      {
+        result = FALSE;
+        break;
+      }
+    case POWERSTEP01_FN_SLP_ACC: //POWERSTEP01_TON_MIN
+    case POWERSTEP01_FN_SLP_DEC: //POWERSTEP01_TOFF_MIN
+      if (voltageMode==FALSE)
+      {
+        if (value>POWERSTEP01_TOFF_TON_MIN_MAX_VALUE) result = FALSE;
+        else registerValue = t_min_time_to_reg_val(value);
+      }
+      else
+      {
+        if (value > POWERSTEP01_SLP_MAX_VALUE) result = FALSE;
+        else registerValue = bemf_slope_perc_to_reg_val(value);
+      }
+      break;
+    default:
+      result = FALSE;
+  }
+  if (result!=FALSE)
+  {
+    Powerstep01_CmdSetParam(param, registerValue);
+  }
+  return result;
+}
+
+/**********************************************************
+ * @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 PowerStep01::Powerstep01_WriteBytes(uint8_t *pByteToTransmit, uint8_t *pReceivedByte)
+{
+  if (Powerstep01_Board_SpiWriteBytes(pByteToTransmit, pReceivedByte) != 0)
+  {
+    Powerstep01_ErrorHandler(POWERSTEP01_ERROR_1);
+  }
+  if (isrFlag)
+  {
+    spiPreemptionByIsr = TRUE;
+  }
+}
+
+/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/