Arkadi Rafalovich
Personal fork of the library
Fork of
X_NUCLEO_IHM01A1
by 
Arkadi Rafalovich
Diff: Components/l6474/l6474_class.cpp
- Revision:
- 0:2887415a46cd
- Child:
- 1:b38ebb8ea286
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Components/l6474/l6474_class.cpp Wed Oct 14 15:12:18 2015 +0000
@@ -0,0 +1,1348 @@
+/**
+ ******************************************************************************
+ * @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>© 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"
+
+
+/* Private constants ---------------------------------------------------------*/
+
+/// Error while initialising the SPI
+#define L6474_ERROR_0 (0x8000)
+/// Error: 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)
+
+
+/* Private variables --------------------------------------------------------*/
+
+/* Number of devices. */
+uint8_t L6474::numberOfDevices = 0;
+
+
+/* Private function prototypes -----------------------------------------------*/
+
+/******************************************************//**
+ * @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 Attaches a user callback to the flag Interrupt
+ * The call back will be then called each time the status
+ * flag pin will be pulled down due to the occurrence of
+ * a programmed alarms ( OCD, thermal pre-warning or
+ * shutdown, UVLO, wrong command, non-performable command)
+ * @param[in] callback Name of the callback to attach
+ * to the Flag Interrupt
+ * @retval None
+ **********************************************************/
+void L6474::L6474_AttachFlagInterrupt(void (*callback)(void))
+{
+ flagInterruptCallback = (void (*)(void))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 = type;
+
+ 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();
+
+ /* Disable power stage */
+ L6474_CmdDisable();
+
+ /* 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();
+ }
+}
+
+#if 0
+/******************************************************//**
+ * @brief Resets all L6474 devices
+ * @param None
+ * @retval None
+ **********************************************************/
+void L6474::L6474_ResetAllDevices(void)
+{
+ uint8_t loop;
+
+ for (loop = 0; loop < numberOfDevices; loop++)
+ {
+ /* Stop movement and disable power stage*/
+ L6474_HardStop(loop);
+ }
+ L6474_Reset();
+ L6474_Delay(1); // Reset pin must be forced low for at least 10us
+ L6474_ReleaseReset();
+ L6474_Delay(1);
+}
+#endif
+
+/******************************************************//**
+ * @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 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] param Register adress (L6474_ABS_POS, L6474_MARK,...)
+ * @retval Register value
+ **********************************************************/
+uint32_t L6474::L6474_CmdGetParam(uint32_t param)
+{
+ uint32_t i;
+ uint32_t spiRxData;
+ uint8_t maxArgumentNbBytes = 0;
+ bool itDisable = FALSE;
+
+ do
+ {
+ spiPreemtionByIsr = FALSE;
+ if (itDisable)
+ {
+ /* re-enable L6474_EnableIrq if disable in previous iteration */
+ L6474_EnableIrq();
+ itDisable = FALSE;
+ }
+
+ spiTxBursts[0] = L6474_NOP;
+ spiTxBursts[1] = L6474_NOP;
+ spiTxBursts[2] = L6474_NOP;
+ spiTxBursts[3] = L6474_NOP;
+ spiRxBursts[1] = 0;
+ spiRxBursts[2] = 0;
+ spiRxBursts[3] = 0;
+
+ switch (param)
+ {
+ case L6474_ABS_POS: ;
+ case L6474_MARK:
+ spiTxBursts[0] = ((uint8_t)L6474_GET_PARAM )| (param);
+ maxArgumentNbBytes = 3;
+ break;
+ case L6474_EL_POS: ;
+ case L6474_CONFIG: ;
+ case L6474_STATUS:
+ spiTxBursts[1] = ((uint8_t)L6474_GET_PARAM )| (param);
+ maxArgumentNbBytes = 2;
+ break;
+ default:
+ spiTxBursts[2] = ((uint8_t)L6474_GET_PARAM )| (param);
+ 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], &spiRxBursts[i]);
+ }
+
+ spiRxData = ((uint32_t)spiRxBursts[1] << 16) |
+ (spiRxBursts[2] << 8) |
+ (spiRxBursts[3]);
+
+ /* 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 L6474CmdGetParam).
+ **********************************************************/
+uint16_t L6474::L6474_CmdGetStatus(void)
+{
+ uint32_t i;
+ uint16_t status;
+ bool itDisable = FALSE;
+
+ do
+ {
+ spiPreemtionByIsr = FALSE;
+ if (itDisable)
+ {
+ /* re-enable L6474_EnableIrq if disable in previous iteration */
+ L6474_EnableIrq();
+ itDisable = FALSE;
+ }
+
+ spiTxBursts[0] = L6474_NOP;
+ spiTxBursts[1] = L6474_NOP;
+ spiTxBursts[2] = L6474_NOP;
+ spiRxBursts[1] = 0;
+ spiRxBursts[2] = 0;
+ spiTxBursts[0] = 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], &spiRxBursts[i]);
+ }
+ status = (spiRxBursts[1] << 8) | (spiRxBursts[2]);
+
+ /* 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] param Register adress (L6474_ABS_POS, L6474_MARK,...)
+ * @param[in] value Value to set in the register
+ * @retval None
+ **********************************************************/
+void L6474::L6474_CmdSetParam(uint32_t param, uint32_t value)
+{
+ uint32_t i;
+ uint8_t maxArgumentNbBytes = 0;
+ bool itDisable = FALSE;
+ do
+ {
+ spiPreemtionByIsr = FALSE;
+ if (itDisable)
+ {
+ /* re-enable L6474_EnableIrq if disable in previous iteration */
+ L6474_EnableIrq();
+ itDisable = FALSE;
+ }
+
+ spiTxBursts[0] = L6474_NOP;
+ spiTxBursts[1] = L6474_NOP;
+ spiTxBursts[2] = L6474_NOP;
+ spiTxBursts[3] = L6474_NOP;
+
+ switch (param)
+ {
+ case L6474_ABS_POS: ;
+ case L6474_MARK:
+ spiTxBursts[0] = param;
+ spiTxBursts[1] = (uint8_t)(value >> 16);
+ spiTxBursts[2] = (uint8_t)(value >> 8);
+ maxArgumentNbBytes = 3;
+ break;
+ case L6474_EL_POS: ;
+ case L6474_CONFIG:
+ spiTxBursts[1] = param;
+ spiTxBursts[2] = (uint8_t)(value >> 8);
+ maxArgumentNbBytes = 2;
+ break;
+ default:
+ spiTxBursts[2] = param;
+ maxArgumentNbBytes = 1;
+ break;
+ }
+ spiTxBursts[3] = (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],&spiRxBursts[i]);
+ }
+ /* 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 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 dir)
+{
+ if (devicePrm.motionState == INACTIVE)
+ {
+ devicePrm.direction = dir;
+ L6474_SetDirectionGpio(dir);
+ }
+}
+
+/******************************************************//**
+ * @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
+ {
+ while(1)
+ {
+ /* Infinite loop */
+ }
+ }
+}
+
+/******************************************************//**
+ * @brief Handlers of the flag interrupt which calls the user callback (if defined)
+ * @param None
+ * @retval None
+ **********************************************************/
+void L6474::L6474_FlagInterruptHandler(void)
+{
+ if (flagInterruptCallback != 0)
+ {
+ /* Set isr flag */
+ isrFlag = TRUE;
+
+ flagInterruptCallback();
+
+ /* Reset isr flag */
+ isrFlag = FALSE;
+ }
+}
+
+/******************************************************//**
+ * @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)
+{
+ bool itDisable = FALSE;
+
+ do
+ {
+ spiPreemtionByIsr = FALSE;
+ if (itDisable)
+ {
+ /* re-enable L6474_EnableIrq if disable in previous iteration */
+ L6474_EnableIrq();
+ itDisable = FALSE;
+ }
+
+ spiTxBursts[3] = L6474_NOP;
+ spiTxBursts[3] = 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], &spiRxBursts[3]);
+
+ /* 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****/