mirror of https://github.com/rusefi/bldc.git
Merge pull request #71 from paltatech/powerdesigns-dev
Powerdesigns dev
This commit is contained in:
commit
a546cc4dd5
|
@ -0,0 +1,14 @@
|
||||||
|
language: cpp
|
||||||
|
compiler: gcc
|
||||||
|
cache: apt
|
||||||
|
sudo: required
|
||||||
|
dist: trusty
|
||||||
|
addons:
|
||||||
|
apt:
|
||||||
|
packages:
|
||||||
|
- gcc-arm-none-eabi
|
||||||
|
- libnewlib-arm-none-eabi
|
||||||
|
env:
|
||||||
|
|
||||||
|
script:
|
||||||
|
- cd build_all && ./rebuild_all
|
|
@ -0,0 +1,131 @@
|
||||||
|
/**
|
||||||
|
******************************************************************************
|
||||||
|
* @file stm32f4xx_iwdg.h
|
||||||
|
* @author MCD Application Team
|
||||||
|
* @version V1.6.0
|
||||||
|
* @date 10-July-2015
|
||||||
|
* @brief This file contains all the functions prototypes for the IWDG
|
||||||
|
* firmware library.
|
||||||
|
******************************************************************************
|
||||||
|
* @attention
|
||||||
|
*
|
||||||
|
* <h2><center>© COPYRIGHT 2015 STMicroelectronics</center></h2>
|
||||||
|
*
|
||||||
|
* Licensed under MCD-ST Liberty SW License Agreement V2, (the "License");
|
||||||
|
* You may not use this file except in compliance with the License.
|
||||||
|
* You may obtain a copy of the License at:
|
||||||
|
*
|
||||||
|
* http://www.st.com/software_license_agreement_liberty_v2
|
||||||
|
*
|
||||||
|
* Unless required by applicable law or agreed to in writing, software
|
||||||
|
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||||
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||||
|
* See the License for the specific language governing permissions and
|
||||||
|
* limitations under the License.
|
||||||
|
*
|
||||||
|
******************************************************************************
|
||||||
|
*/
|
||||||
|
|
||||||
|
/* Define to prevent recursive inclusion -------------------------------------*/
|
||||||
|
#ifndef __STM32F4xx_IWDG_H
|
||||||
|
#define __STM32F4xx_IWDG_H
|
||||||
|
|
||||||
|
#ifdef __cplusplus
|
||||||
|
extern "C" {
|
||||||
|
#endif
|
||||||
|
|
||||||
|
/* Includes ------------------------------------------------------------------*/
|
||||||
|
#include "stm32f4xx.h"
|
||||||
|
|
||||||
|
/** @addtogroup STM32F4xx_StdPeriph_Driver
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
|
||||||
|
/** @addtogroup IWDG
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
|
||||||
|
/* Exported types ------------------------------------------------------------*/
|
||||||
|
/* Exported constants --------------------------------------------------------*/
|
||||||
|
|
||||||
|
/** @defgroup IWDG_Exported_Constants
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
|
||||||
|
/** @defgroup IWDG_WriteAccess
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
#define IWDG_WriteAccess_Enable ((uint16_t)0x5555)
|
||||||
|
#define IWDG_WriteAccess_Disable ((uint16_t)0x0000)
|
||||||
|
#define IS_IWDG_WRITE_ACCESS(ACCESS) (((ACCESS) == IWDG_WriteAccess_Enable) || \
|
||||||
|
((ACCESS) == IWDG_WriteAccess_Disable))
|
||||||
|
/**
|
||||||
|
* @}
|
||||||
|
*/
|
||||||
|
|
||||||
|
/** @defgroup IWDG_prescaler
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
#define IWDG_Prescaler_4 ((uint8_t)0x00)
|
||||||
|
#define IWDG_Prescaler_8 ((uint8_t)0x01)
|
||||||
|
#define IWDG_Prescaler_16 ((uint8_t)0x02)
|
||||||
|
#define IWDG_Prescaler_32 ((uint8_t)0x03)
|
||||||
|
#define IWDG_Prescaler_64 ((uint8_t)0x04)
|
||||||
|
#define IWDG_Prescaler_128 ((uint8_t)0x05)
|
||||||
|
#define IWDG_Prescaler_256 ((uint8_t)0x06)
|
||||||
|
#define IS_IWDG_PRESCALER(PRESCALER) (((PRESCALER) == IWDG_Prescaler_4) || \
|
||||||
|
((PRESCALER) == IWDG_Prescaler_8) || \
|
||||||
|
((PRESCALER) == IWDG_Prescaler_16) || \
|
||||||
|
((PRESCALER) == IWDG_Prescaler_32) || \
|
||||||
|
((PRESCALER) == IWDG_Prescaler_64) || \
|
||||||
|
((PRESCALER) == IWDG_Prescaler_128)|| \
|
||||||
|
((PRESCALER) == IWDG_Prescaler_256))
|
||||||
|
/**
|
||||||
|
* @}
|
||||||
|
*/
|
||||||
|
|
||||||
|
/** @defgroup IWDG_Flag
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
#define IWDG_FLAG_PVU ((uint16_t)0x0001)
|
||||||
|
#define IWDG_FLAG_RVU ((uint16_t)0x0002)
|
||||||
|
#define IS_IWDG_FLAG(FLAG) (((FLAG) == IWDG_FLAG_PVU) || ((FLAG) == IWDG_FLAG_RVU))
|
||||||
|
#define IS_IWDG_RELOAD(RELOAD) ((RELOAD) <= 0xFFF)
|
||||||
|
/**
|
||||||
|
* @}
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @}
|
||||||
|
*/
|
||||||
|
|
||||||
|
/* Exported macro ------------------------------------------------------------*/
|
||||||
|
/* Exported functions --------------------------------------------------------*/
|
||||||
|
|
||||||
|
/* Prescaler and Counter configuration functions ******************************/
|
||||||
|
void IWDG_WriteAccessCmd(uint16_t IWDG_WriteAccess);
|
||||||
|
void IWDG_SetPrescaler(uint8_t IWDG_Prescaler);
|
||||||
|
void IWDG_SetReload(uint16_t Reload);
|
||||||
|
void IWDG_ReloadCounter(void);
|
||||||
|
|
||||||
|
/* IWDG activation function ***************************************************/
|
||||||
|
void IWDG_Enable(void);
|
||||||
|
|
||||||
|
/* Flag management function ***************************************************/
|
||||||
|
FlagStatus IWDG_GetFlagStatus(uint16_t IWDG_FLAG);
|
||||||
|
|
||||||
|
#ifdef __cplusplus
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#endif /* __STM32F4xx_IWDG_H */
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @}
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @}
|
||||||
|
*/
|
||||||
|
|
||||||
|
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
|
|
@ -75,10 +75,6 @@
|
||||||
/* Includes ------------------------------------------------------------------*/
|
/* Includes ------------------------------------------------------------------*/
|
||||||
#include "misc.h"
|
#include "misc.h"
|
||||||
|
|
||||||
#ifndef assert_param
|
|
||||||
#define assert_param(expr) ((void)0)
|
|
||||||
#endif
|
|
||||||
|
|
||||||
/** @addtogroup STM32F4xx_StdPeriph_Driver
|
/** @addtogroup STM32F4xx_StdPeriph_Driver
|
||||||
* @{
|
* @{
|
||||||
*/
|
*/
|
||||||
|
|
|
@ -106,9 +106,6 @@
|
||||||
#include "stm32f4xx_adc.h"
|
#include "stm32f4xx_adc.h"
|
||||||
#include "stm32f4xx_rcc.h"
|
#include "stm32f4xx_rcc.h"
|
||||||
|
|
||||||
#ifndef assert_param
|
|
||||||
#define assert_param(expr) ((void)0)
|
|
||||||
#endif
|
|
||||||
|
|
||||||
/** @addtogroup STM32F4xx_StdPeriph_Driver
|
/** @addtogroup STM32F4xx_StdPeriph_Driver
|
||||||
* @{
|
* @{
|
||||||
|
|
|
@ -124,10 +124,6 @@
|
||||||
#include "stm32f4xx_dma.h"
|
#include "stm32f4xx_dma.h"
|
||||||
#include "stm32f4xx_rcc.h"
|
#include "stm32f4xx_rcc.h"
|
||||||
|
|
||||||
#ifndef assert_param
|
|
||||||
#define assert_param(expr) ((void)0)
|
|
||||||
#endif
|
|
||||||
|
|
||||||
/** @addtogroup STM32F4xx_StdPeriph_Driver
|
/** @addtogroup STM32F4xx_StdPeriph_Driver
|
||||||
* @{
|
* @{
|
||||||
*/
|
*/
|
||||||
|
|
|
@ -67,10 +67,6 @@
|
||||||
/* Includes ------------------------------------------------------------------*/
|
/* Includes ------------------------------------------------------------------*/
|
||||||
#include "stm32f4xx_exti.h"
|
#include "stm32f4xx_exti.h"
|
||||||
|
|
||||||
#ifndef assert_param
|
|
||||||
#define assert_param(expr) ((void)0)
|
|
||||||
#endif
|
|
||||||
|
|
||||||
/** @addtogroup STM32F4xx_StdPeriph_Driver
|
/** @addtogroup STM32F4xx_StdPeriph_Driver
|
||||||
* @{
|
* @{
|
||||||
*/
|
*/
|
||||||
|
|
|
@ -83,12 +83,6 @@
|
||||||
#define FLASH_OPTCR_BFB2 ((uint32_t)0x00000010)
|
#define FLASH_OPTCR_BFB2 ((uint32_t)0x00000010)
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifndef assert_param
|
|
||||||
#define assert_param(expr) ((void)0)
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#define STM32F40_41xxx
|
|
||||||
|
|
||||||
/** @addtogroup STM32F4xx_StdPeriph_Driver
|
/** @addtogroup STM32F4xx_StdPeriph_Driver
|
||||||
* @{
|
* @{
|
||||||
*/
|
*/
|
||||||
|
|
|
@ -0,0 +1,266 @@
|
||||||
|
/**
|
||||||
|
******************************************************************************
|
||||||
|
* @file stm32f4xx_iwdg.c
|
||||||
|
* @author MCD Application Team
|
||||||
|
* @version V1.6.0
|
||||||
|
* @date 10-July-2015
|
||||||
|
* @brief This file provides firmware functions to manage the following
|
||||||
|
* functionalities of the Independent watchdog (IWDG) peripheral:
|
||||||
|
* + Prescaler and Counter configuration
|
||||||
|
* + IWDG activation
|
||||||
|
* + Flag management
|
||||||
|
*
|
||||||
|
@verbatim
|
||||||
|
===============================================================================
|
||||||
|
##### IWDG features #####
|
||||||
|
===============================================================================
|
||||||
|
[..]
|
||||||
|
The IWDG can be started by either software or hardware (configurable
|
||||||
|
through option byte).
|
||||||
|
|
||||||
|
The IWDG is clocked by its own dedicated low-speed clock (LSI) and
|
||||||
|
thus stays active even if the main clock fails.
|
||||||
|
Once the IWDG is started, the LSI is forced ON and cannot be disabled
|
||||||
|
(LSI cannot be disabled too), and the counter starts counting down from
|
||||||
|
the reset value of 0xFFF. When it reaches the end of count value (0x000)
|
||||||
|
a system reset is generated.
|
||||||
|
The IWDG counter should be reloaded at regular intervals to prevent
|
||||||
|
an MCU reset.
|
||||||
|
|
||||||
|
The IWDG is implemented in the VDD voltage domain that is still functional
|
||||||
|
in STOP and STANDBY mode (IWDG reset can wake-up from STANDBY).
|
||||||
|
|
||||||
|
IWDGRST flag in RCC_CSR register can be used to inform when a IWDG
|
||||||
|
reset occurs.
|
||||||
|
|
||||||
|
Min-max timeout value @32KHz (LSI): ~125us / ~32.7s
|
||||||
|
The IWDG timeout may vary due to LSI frequency dispersion. STM32F4xx
|
||||||
|
devices provide the capability to measure the LSI frequency (LSI clock
|
||||||
|
connected internally to TIM5 CH4 input capture). The measured value
|
||||||
|
can be used to have an IWDG timeout with an acceptable accuracy.
|
||||||
|
For more information, please refer to the STM32F4xx Reference manual
|
||||||
|
|
||||||
|
##### How to use this driver #####
|
||||||
|
===============================================================================
|
||||||
|
[..]
|
||||||
|
(#) Enable write access to IWDG_PR and IWDG_RLR registers using
|
||||||
|
IWDG_WriteAccessCmd(IWDG_WriteAccess_Enable) function
|
||||||
|
|
||||||
|
(#) Configure the IWDG prescaler using IWDG_SetPrescaler() function
|
||||||
|
|
||||||
|
(#) Configure the IWDG counter value using IWDG_SetReload() function.
|
||||||
|
This value will be loaded in the IWDG counter each time the counter
|
||||||
|
is reloaded, then the IWDG will start counting down from this value.
|
||||||
|
|
||||||
|
(#) Start the IWDG using IWDG_Enable() function, when the IWDG is used
|
||||||
|
in software mode (no need to enable the LSI, it will be enabled
|
||||||
|
by hardware)
|
||||||
|
|
||||||
|
(#) Then the application program must reload the IWDG counter at regular
|
||||||
|
intervals during normal operation to prevent an MCU reset, using
|
||||||
|
IWDG_ReloadCounter() function.
|
||||||
|
|
||||||
|
@endverbatim
|
||||||
|
******************************************************************************
|
||||||
|
* @attention
|
||||||
|
*
|
||||||
|
* <h2><center>© COPYRIGHT 2015 STMicroelectronics</center></h2>
|
||||||
|
*
|
||||||
|
* Licensed under MCD-ST Liberty SW License Agreement V2, (the "License");
|
||||||
|
* You may not use this file except in compliance with the License.
|
||||||
|
* You may obtain a copy of the License at:
|
||||||
|
*
|
||||||
|
* http://www.st.com/software_license_agreement_liberty_v2
|
||||||
|
*
|
||||||
|
* Unless required by applicable law or agreed to in writing, software
|
||||||
|
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||||
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||||
|
* See the License for the specific language governing permissions and
|
||||||
|
* limitations under the License.
|
||||||
|
*
|
||||||
|
******************************************************************************
|
||||||
|
*/
|
||||||
|
|
||||||
|
/* Includes ------------------------------------------------------------------*/
|
||||||
|
#include "stm32f4xx_iwdg.h"
|
||||||
|
|
||||||
|
/** @addtogroup STM32F4xx_StdPeriph_Driver
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
|
||||||
|
/** @defgroup IWDG
|
||||||
|
* @brief IWDG driver modules
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
|
||||||
|
/* Private typedef -----------------------------------------------------------*/
|
||||||
|
/* Private define ------------------------------------------------------------*/
|
||||||
|
|
||||||
|
/* KR register bit mask */
|
||||||
|
#define KR_KEY_RELOAD ((uint16_t)0xAAAA)
|
||||||
|
#define KR_KEY_ENABLE ((uint16_t)0xCCCC)
|
||||||
|
|
||||||
|
/* Private macro -------------------------------------------------------------*/
|
||||||
|
/* Private variables ---------------------------------------------------------*/
|
||||||
|
/* Private function prototypes -----------------------------------------------*/
|
||||||
|
/* Private functions ---------------------------------------------------------*/
|
||||||
|
|
||||||
|
/** @defgroup IWDG_Private_Functions
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
|
||||||
|
/** @defgroup IWDG_Group1 Prescaler and Counter configuration functions
|
||||||
|
* @brief Prescaler and Counter configuration functions
|
||||||
|
*
|
||||||
|
@verbatim
|
||||||
|
===============================================================================
|
||||||
|
##### Prescaler and Counter configuration functions #####
|
||||||
|
===============================================================================
|
||||||
|
|
||||||
|
@endverbatim
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Enables or disables write access to IWDG_PR and IWDG_RLR registers.
|
||||||
|
* @param IWDG_WriteAccess: new state of write access to IWDG_PR and IWDG_RLR registers.
|
||||||
|
* This parameter can be one of the following values:
|
||||||
|
* @arg IWDG_WriteAccess_Enable: Enable write access to IWDG_PR and IWDG_RLR registers
|
||||||
|
* @arg IWDG_WriteAccess_Disable: Disable write access to IWDG_PR and IWDG_RLR registers
|
||||||
|
* @retval None
|
||||||
|
*/
|
||||||
|
void IWDG_WriteAccessCmd(uint16_t IWDG_WriteAccess)
|
||||||
|
{
|
||||||
|
/* Check the parameters */
|
||||||
|
assert_param(IS_IWDG_WRITE_ACCESS(IWDG_WriteAccess));
|
||||||
|
IWDG->KR = IWDG_WriteAccess;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Sets IWDG Prescaler value.
|
||||||
|
* @param IWDG_Prescaler: specifies the IWDG Prescaler value.
|
||||||
|
* This parameter can be one of the following values:
|
||||||
|
* @arg IWDG_Prescaler_4: IWDG prescaler set to 4
|
||||||
|
* @arg IWDG_Prescaler_8: IWDG prescaler set to 8
|
||||||
|
* @arg IWDG_Prescaler_16: IWDG prescaler set to 16
|
||||||
|
* @arg IWDG_Prescaler_32: IWDG prescaler set to 32
|
||||||
|
* @arg IWDG_Prescaler_64: IWDG prescaler set to 64
|
||||||
|
* @arg IWDG_Prescaler_128: IWDG prescaler set to 128
|
||||||
|
* @arg IWDG_Prescaler_256: IWDG prescaler set to 256
|
||||||
|
* @retval None
|
||||||
|
*/
|
||||||
|
void IWDG_SetPrescaler(uint8_t IWDG_Prescaler)
|
||||||
|
{
|
||||||
|
/* Check the parameters */
|
||||||
|
assert_param(IS_IWDG_PRESCALER(IWDG_Prescaler));
|
||||||
|
IWDG->PR = IWDG_Prescaler;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Sets IWDG Reload value.
|
||||||
|
* @param Reload: specifies the IWDG Reload value.
|
||||||
|
* This parameter must be a number between 0 and 0x0FFF.
|
||||||
|
* @retval None
|
||||||
|
*/
|
||||||
|
void IWDG_SetReload(uint16_t Reload)
|
||||||
|
{
|
||||||
|
/* Check the parameters */
|
||||||
|
assert_param(IS_IWDG_RELOAD(Reload));
|
||||||
|
IWDG->RLR = Reload;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Reloads IWDG counter with value defined in the reload register
|
||||||
|
* (write access to IWDG_PR and IWDG_RLR registers disabled).
|
||||||
|
* @param None
|
||||||
|
* @retval None
|
||||||
|
*/
|
||||||
|
void IWDG_ReloadCounter(void)
|
||||||
|
{
|
||||||
|
IWDG->KR = KR_KEY_RELOAD;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @}
|
||||||
|
*/
|
||||||
|
|
||||||
|
/** @defgroup IWDG_Group2 IWDG activation function
|
||||||
|
* @brief IWDG activation function
|
||||||
|
*
|
||||||
|
@verbatim
|
||||||
|
===============================================================================
|
||||||
|
##### IWDG activation function #####
|
||||||
|
===============================================================================
|
||||||
|
|
||||||
|
@endverbatim
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Enables IWDG (write access to IWDG_PR and IWDG_RLR registers disabled).
|
||||||
|
* @param None
|
||||||
|
* @retval None
|
||||||
|
*/
|
||||||
|
void IWDG_Enable(void)
|
||||||
|
{
|
||||||
|
IWDG->KR = KR_KEY_ENABLE;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @}
|
||||||
|
*/
|
||||||
|
|
||||||
|
/** @defgroup IWDG_Group3 Flag management function
|
||||||
|
* @brief Flag management function
|
||||||
|
*
|
||||||
|
@verbatim
|
||||||
|
===============================================================================
|
||||||
|
##### Flag management function #####
|
||||||
|
===============================================================================
|
||||||
|
|
||||||
|
@endverbatim
|
||||||
|
* @{
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Checks whether the specified IWDG flag is set or not.
|
||||||
|
* @param IWDG_FLAG: specifies the flag to check.
|
||||||
|
* This parameter can be one of the following values:
|
||||||
|
* @arg IWDG_FLAG_PVU: Prescaler Value Update on going
|
||||||
|
* @arg IWDG_FLAG_RVU: Reload Value Update on going
|
||||||
|
* @retval The new state of IWDG_FLAG (SET or RESET).
|
||||||
|
*/
|
||||||
|
FlagStatus IWDG_GetFlagStatus(uint16_t IWDG_FLAG)
|
||||||
|
{
|
||||||
|
FlagStatus bitstatus = RESET;
|
||||||
|
/* Check the parameters */
|
||||||
|
assert_param(IS_IWDG_FLAG(IWDG_FLAG));
|
||||||
|
if ((IWDG->SR & IWDG_FLAG) != (uint32_t)RESET)
|
||||||
|
{
|
||||||
|
bitstatus = SET;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
bitstatus = RESET;
|
||||||
|
}
|
||||||
|
/* Return the flag status */
|
||||||
|
return bitstatus;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @}
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @}
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @}
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @}
|
||||||
|
*/
|
||||||
|
|
||||||
|
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
|
|
@ -74,10 +74,6 @@
|
||||||
#define RCC_BDCR_LSEMOD ((uint32_t)0x00000008)
|
#define RCC_BDCR_LSEMOD ((uint32_t)0x00000008)
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifndef assert_param
|
|
||||||
#define assert_param(expr) ((void)0)
|
|
||||||
#endif
|
|
||||||
|
|
||||||
/** @addtogroup STM32F4xx_StdPeriph_Driver
|
/** @addtogroup STM32F4xx_StdPeriph_Driver
|
||||||
* @{
|
* @{
|
||||||
*/
|
*/
|
||||||
|
@ -261,7 +257,7 @@ void RCC_DeInit(void)
|
||||||
RCC->PLLI2SCFGR = 0x20003000;
|
RCC->PLLI2SCFGR = 0x20003000;
|
||||||
#endif /* STM32F40_41xxx || STM32F427_437xx || STM32F429_439xx || STM32F401xx || STM32F411xE || STM32F446xx || STM32F469_479xx */
|
#endif /* STM32F40_41xxx || STM32F427_437xx || STM32F429_439xx || STM32F401xx || STM32F411xE || STM32F446xx || STM32F469_479xx */
|
||||||
|
|
||||||
#if defined(STM32F40_41xxx) || defined(STM32F427_437xx) || defined(STM32F429_439xx) || defined(STM32F446xx) || defined(STM32F469_479xx)
|
#if defined(STM32F427_437xx) || defined(STM32F429_439xx) || defined(STM32F446xx) || defined(STM32F469_479xx)
|
||||||
/* Reset PLLSAICFGR register, only available for STM32F42xxx/43xxx/446xx/469xx/479xx devices */
|
/* Reset PLLSAICFGR register, only available for STM32F42xxx/43xxx/446xx/469xx/479xx devices */
|
||||||
RCC->PLLSAICFGR = 0x24003000;
|
RCC->PLLSAICFGR = 0x24003000;
|
||||||
#endif /* STM32F40_41xxx || STM32F427_437xx || STM32F429_439xx || STM32F446xx || STM32F469_479xx */
|
#endif /* STM32F40_41xxx || STM32F427_437xx || STM32F429_439xx || STM32F446xx || STM32F469_479xx */
|
||||||
|
@ -827,7 +823,7 @@ void RCC_PLLSAIConfig(uint32_t PLLSAIM, uint32_t PLLSAIN, uint32_t PLLSAIP, uint
|
||||||
}
|
}
|
||||||
#endif /* STM32F446xx */
|
#endif /* STM32F446xx */
|
||||||
|
|
||||||
#if defined(STM32F40_41xxx) || defined(STM32F427_437xx) || defined(STM32F429_439xx) || defined(STM32F401xx) || defined(STM32F411xE)
|
#if defined(STM32F427_437xx) || defined(STM32F429_439xx) || defined(STM32F401xx) || defined(STM32F411xE)
|
||||||
/**
|
/**
|
||||||
* @brief Configures the PLLSAI clock multiplication and division factors.
|
* @brief Configures the PLLSAI clock multiplication and division factors.
|
||||||
*
|
*
|
||||||
|
@ -1660,7 +1656,7 @@ void RCC_I2SCLKConfig(uint32_t RCC_I2SCLKSource)
|
||||||
}
|
}
|
||||||
#endif /* STM32F40_41xxx || STM32F427_437xx || STM32F429_439xx || STM32F401xx || STM32F411xE || STM32F469_479xx */
|
#endif /* STM32F40_41xxx || STM32F427_437xx || STM32F429_439xx || STM32F401xx || STM32F411xE || STM32F469_479xx */
|
||||||
|
|
||||||
#if defined(STM32F40_41xxx) || defined(STM32F427_437xx) || defined(STM32F429_439xx) || defined(STM32F469_479xx)
|
#if defined(STM32F427_437xx) || defined(STM32F429_439xx) || defined(STM32F469_479xx)
|
||||||
/**
|
/**
|
||||||
* @brief Configures SAI1BlockA clock source selection.
|
* @brief Configures SAI1BlockA clock source selection.
|
||||||
*
|
*
|
||||||
|
|
|
@ -50,10 +50,6 @@
|
||||||
#include "stm32f4xx_syscfg.h"
|
#include "stm32f4xx_syscfg.h"
|
||||||
#include "stm32f4xx_rcc.h"
|
#include "stm32f4xx_rcc.h"
|
||||||
|
|
||||||
#ifndef assert_param
|
|
||||||
#define assert_param(expr) ((void)0)
|
|
||||||
#endif
|
|
||||||
|
|
||||||
/** @addtogroup STM32F4xx_StdPeriph_Driver
|
/** @addtogroup STM32F4xx_StdPeriph_Driver
|
||||||
* @{
|
* @{
|
||||||
*/
|
*/
|
||||||
|
|
|
@ -119,10 +119,6 @@
|
||||||
#include "stm32f4xx_tim.h"
|
#include "stm32f4xx_tim.h"
|
||||||
#include "stm32f4xx_rcc.h"
|
#include "stm32f4xx_rcc.h"
|
||||||
|
|
||||||
#ifndef assert_param
|
|
||||||
#define assert_param(expr) ((void)0)
|
|
||||||
#endif
|
|
||||||
|
|
||||||
/** @addtogroup STM32F4xx_StdPeriph_Driver
|
/** @addtogroup STM32F4xx_StdPeriph_Driver
|
||||||
* @{
|
* @{
|
||||||
*/
|
*/
|
||||||
|
|
|
@ -84,10 +84,6 @@
|
||||||
#include "stm32f4xx_wwdg.h"
|
#include "stm32f4xx_wwdg.h"
|
||||||
#include "stm32f4xx_rcc.h"
|
#include "stm32f4xx_rcc.h"
|
||||||
|
|
||||||
#ifndef assert_param
|
|
||||||
#define assert_param(expr) ((void)0)
|
|
||||||
#endif
|
|
||||||
|
|
||||||
/** @addtogroup STM32F4xx_StdPeriph_Driver
|
/** @addtogroup STM32F4xx_StdPeriph_Driver
|
||||||
* @{
|
* @{
|
||||||
*/
|
*/
|
||||||
|
|
|
@ -7,6 +7,7 @@ STM32SRC = ${CHIBIOS}/ext/stdperiph_stm32f4/src/misc.c \
|
||||||
${CHIBIOS}/ext/stdperiph_stm32f4/src/stm32f4xx_rcc.c \
|
${CHIBIOS}/ext/stdperiph_stm32f4/src/stm32f4xx_rcc.c \
|
||||||
${CHIBIOS}/ext/stdperiph_stm32f4/src/stm32f4xx_syscfg.c \
|
${CHIBIOS}/ext/stdperiph_stm32f4/src/stm32f4xx_syscfg.c \
|
||||||
${CHIBIOS}/ext/stdperiph_stm32f4/src/stm32f4xx_tim.c \
|
${CHIBIOS}/ext/stdperiph_stm32f4/src/stm32f4xx_tim.c \
|
||||||
|
${CHIBIOS}/ext/stdperiph_stm32f4/src/stm32f4xx_iwdg.c \
|
||||||
${CHIBIOS}/ext/stdperiph_stm32f4/src/stm32f4xx_wwdg.c
|
${CHIBIOS}/ext/stdperiph_stm32f4/src/stm32f4xx_wwdg.c
|
||||||
|
|
||||||
STM32INC = ${CHIBIOS}/ext/stdperiph_stm32f4/inc
|
STM32INC = ${CHIBIOS}/ext/stdperiph_stm32f4/inc
|
||||||
|
|
|
@ -59,7 +59,7 @@
|
||||||
#ifdef __cplusplus
|
#ifdef __cplusplus
|
||||||
extern "C" {
|
extern "C" {
|
||||||
#endif /* __cplusplus */
|
#endif /* __cplusplus */
|
||||||
|
|
||||||
/** @addtogroup Library_configuration_section
|
/** @addtogroup Library_configuration_section
|
||||||
* @{
|
* @{
|
||||||
*/
|
*/
|
||||||
|
@ -125,6 +125,29 @@
|
||||||
#define STM32F407xx
|
#define STM32F407xx
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
#if defined(STM32F439xx) || defined(STM32F429xx)
|
||||||
|
#define STM32F429_439xx
|
||||||
|
|
||||||
|
#elif defined(STM32F437xx) || defined(STM32F427xx)
|
||||||
|
#define STM32F427_437xx
|
||||||
|
|
||||||
|
#elif defined(STM32F405xx) || defined(STM32F415xx) || \
|
||||||
|
defined(STM32F407xx) || defined(STM32F417xx)
|
||||||
|
#define STM32F40_41xxx
|
||||||
|
|
||||||
|
#elif defined(STM32F401xC) || defined(STM32F401xE)
|
||||||
|
#define STM32F401xx
|
||||||
|
|
||||||
|
#elif defined(STM32F411xE)
|
||||||
|
#define STM32F411xx
|
||||||
|
|
||||||
|
#elif defined(STM32F2XX)
|
||||||
|
|
||||||
|
#else
|
||||||
|
#error "STM32F2xx/F4xx device not specified"
|
||||||
|
#endif
|
||||||
|
|
||||||
|
|
||||||
#if defined(STM32F405xx)
|
#if defined(STM32F405xx)
|
||||||
#include "stm32f405xx.h"
|
#include "stm32f405xx.h"
|
||||||
#elif defined(STM32F415xx)
|
#elif defined(STM32F415xx)
|
||||||
|
@ -210,6 +233,32 @@ typedef enum
|
||||||
#include "stm32f4xx_hal.h"
|
#include "stm32f4xx_hal.h"
|
||||||
#endif /* USE_HAL_DRIVER */
|
#endif /* USE_HAL_DRIVER */
|
||||||
|
|
||||||
|
|
||||||
|
/* Uncomment the line below to expanse the "assert_param" macro in the
|
||||||
|
Standard Peripheral Library drivers code */
|
||||||
|
//#define USE_FULL_ASSERT 1
|
||||||
|
|
||||||
|
/* Exported macro ------------------------------------------------------------*/
|
||||||
|
#ifdef USE_FULL_ASSERT
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief The assert_param macro is used for function's parameters check.
|
||||||
|
* @param expr: If expr is false, it calls assert_failed function
|
||||||
|
* which reports the name of the source file and the source
|
||||||
|
* line number of the call that failed.
|
||||||
|
* If expr is true, it returns no value.
|
||||||
|
* @retval None
|
||||||
|
*/
|
||||||
|
#define assert_param(expr) ((expr) ? (void)0 : assert_failed((uint8_t *)__FILE__, __LINE__))
|
||||||
|
/* Exported functions ------------------------------------------------------- */
|
||||||
|
void assert_failed(uint8_t* file, uint32_t line);
|
||||||
|
#else
|
||||||
|
#ifndef assert_param
|
||||||
|
#define assert_param(expr) ((void)0)
|
||||||
|
#endif
|
||||||
|
#endif /* USE_FULL_ASSERT */
|
||||||
|
|
||||||
|
|
||||||
#ifdef __cplusplus
|
#ifdef __cplusplus
|
||||||
}
|
}
|
||||||
#endif /* __cplusplus */
|
#endif /* __cplusplus */
|
||||||
|
|
|
@ -73,7 +73,7 @@ static char *ch_ltoa(char *p, long num, unsigned radix) {
|
||||||
}
|
}
|
||||||
|
|
||||||
#if CHPRINTF_USE_FLOAT
|
#if CHPRINTF_USE_FLOAT
|
||||||
static const long pow10[FLOAT_PRECISION] = {
|
static const long ch_pow10[FLOAT_PRECISION] = {
|
||||||
10, 100, 1000, 10000, 100000, 1000000, 10000000, 100000000, 1000000000
|
10, 100, 1000, 10000, 100000, 1000000, 10000000, 100000000, 1000000000
|
||||||
};
|
};
|
||||||
|
|
||||||
|
@ -82,7 +82,7 @@ static char *ftoa(char *p, double num, unsigned long precision) {
|
||||||
|
|
||||||
if ((precision == 0) || (precision > FLOAT_PRECISION))
|
if ((precision == 0) || (precision > FLOAT_PRECISION))
|
||||||
precision = FLOAT_PRECISION;
|
precision = FLOAT_PRECISION;
|
||||||
precision = pow10[precision - 1];
|
precision = ch_pow10[precision - 1];
|
||||||
|
|
||||||
l = (long)num;
|
l = (long)num;
|
||||||
p = long_to_string_with_divisor(p, l, 10, 0);
|
p = long_to_string_with_divisor(p, l, 10, 0);
|
||||||
|
|
|
@ -295,6 +295,7 @@ static void wakeup(void *p) {
|
||||||
/* Falls through. */
|
/* Falls through. */
|
||||||
case CH_STATE_WTCOND:
|
case CH_STATE_WTCOND:
|
||||||
#endif
|
#endif
|
||||||
|
/* Falls through. */
|
||||||
case CH_STATE_QUEUED:
|
case CH_STATE_QUEUED:
|
||||||
/* States requiring dequeuing.*/
|
/* States requiring dequeuing.*/
|
||||||
(void) queue_dequeue(tp);
|
(void) queue_dequeue(tp);
|
||||||
|
|
2
Makefile
2
Makefile
|
@ -226,7 +226,7 @@ AOPT =
|
||||||
TOPT = -mthumb -DTHUMB
|
TOPT = -mthumb -DTHUMB
|
||||||
|
|
||||||
# Define C warning options here
|
# Define C warning options here
|
||||||
CWARN = -Wall -Wextra -Wundef -Wstrict-prototypes
|
CWARN = -Wall -Wextra -Wundef -Wstrict-prototypes -Wshadow
|
||||||
|
|
||||||
# Define C++ warning options here
|
# Define C++ warning options here
|
||||||
CPPWARN = -Wall -Wextra -Wundef
|
CPPWARN = -Wall -Wextra -Wundef
|
||||||
|
|
90
README.md
90
README.md
|
@ -1,2 +1,90 @@
|
||||||
|
# VESC firmware
|
||||||
|
|
||||||
|
[![License: GPL v3](https://img.shields.io/badge/License-GPLv3-blue.svg)](https://www.gnu.org/licenses/gpl-3.0)
|
||||||
|
[![Travis CI Status](https://travis-ci.com/paltatech/bldc.svg?branch=powerdesigns-dev)](https://travis-ci.com/paltatech/bldc)
|
||||||
|
[![Codacy Badge](https://api.codacy.com/project/badge/Grade/92eefb23a0c24b3cbed011b14ca0ffc9)](https://www.codacy.com/app/nitrousnrg/bldc?utm_source=github.com&utm_medium=referral&utm_content=paltatech/bldc&utm_campaign=Badge_Grade)
|
||||||
|
[![Contributors](https://img.shields.io/github/contributors/vedderb/bldc.svg)](https://github.com/vedderb/bldc/graphs/contributors)
|
||||||
|
[![Watchers](https://img.shields.io/github/watchers/vedderb/bldc.svg)](https://github.com/vedderb/bldc/watchers)
|
||||||
|
[![Stars](https://img.shields.io/github/stars/vedderb/bldc.svg)](https://github.com/vedderb/bldc/stargazers)
|
||||||
|
[![Forks](https://img.shields.io/github/forks/vedderb/bldc.svg)](https://github.com/vedderb/bldc/network/members)
|
||||||
|
|
||||||
|
An open source motor controller firmware.
|
||||||
|
|
||||||
This is the source code for the VESC DC/BLDC/FOC controller. Read more at
|
This is the source code for the VESC DC/BLDC/FOC controller. Read more at
|
||||||
http://vesc-project.com/
|
[http://vesc-project.com/](http://vesc-project.com/)
|
||||||
|
|
||||||
|
## Supported boards
|
||||||
|
|
||||||
|
All of them!
|
||||||
|
|
||||||
|
Make sure you select your board in [conf_general.h](conf_general.h)
|
||||||
|
|
||||||
|
|
||||||
|
```c
|
||||||
|
//#define HW_VERSION_40
|
||||||
|
//#define HW_VERSION_45
|
||||||
|
//#define HW_VERSION_46 // Also for 4.7
|
||||||
|
//#define HW_VERSION_48
|
||||||
|
//#define HW_VERSION_49
|
||||||
|
//#define HW_VERSION_410 // Also for 4.11 and 4.12
|
||||||
|
#define HW_VERSION_60
|
||||||
|
//#define HW_VERSION_R2
|
||||||
|
//#define HW_VERSION_VICTOR_R1A
|
||||||
|
//#define HW_VERSION_DAS_RS
|
||||||
|
//#define HW_VERSION_PALTA
|
||||||
|
//#define HW_VERSION_RH
|
||||||
|
//#define HW_VERSION_TP
|
||||||
|
//#define HW_VERSION_75_300
|
||||||
|
//#define HW_VERSION_MINI4
|
||||||
|
//#define HW_VERSION_DAS_MINI
|
||||||
|
```
|
||||||
|
There are also many other options that can be changed in conf_general.h
|
||||||
|
|
||||||
|
|
||||||
|
## Prerequisites
|
||||||
|
|
||||||
|
On an Ubuntu machine, install the gcc-arm-embedded toolchain
|
||||||
|
|
||||||
|
|
||||||
|
```bash
|
||||||
|
sudo add-apt-repository ppa:team-gcc-arm-embedded/ppa
|
||||||
|
sudo apt update
|
||||||
|
sudo apt install gcc-arm-embedded
|
||||||
|
```
|
||||||
|
|
||||||
|
Add udev rules to use the stlink v2 programmer without being root
|
||||||
|
|
||||||
|
|
||||||
|
```bash
|
||||||
|
wget vedder.se/Temp/49-stlinkv2.rules
|
||||||
|
sudo mv 49-stlinkv2.rules /etc/udev/rules.d/
|
||||||
|
sudo udevadm trigger
|
||||||
|
```
|
||||||
|
|
||||||
|
Build and flash the [bootloader](https://github.com/vedderb/bldc-bootloader)
|
||||||
|
|
||||||
|
|
||||||
|
## Build
|
||||||
|
|
||||||
|
Clone and build the firmware
|
||||||
|
|
||||||
|
```bash
|
||||||
|
git clone https://github.com/vedderb/bldc.git vesc_firmware
|
||||||
|
cd vesc_firmware
|
||||||
|
make
|
||||||
|
```
|
||||||
|
|
||||||
|
Flash it using an STLink SWD debugger
|
||||||
|
|
||||||
|
```bash
|
||||||
|
make upload
|
||||||
|
```
|
||||||
|
|
||||||
|
## Contribute
|
||||||
|
|
||||||
|
Head to the [forums](https://vesc-project.com/forum) to get involved and improve this project.
|
||||||
|
|
||||||
|
|
||||||
|
## License
|
||||||
|
|
||||||
|
The software is released under the GNU General Public License version 3.0
|
||||||
|
|
|
@ -287,13 +287,13 @@ static THD_FUNCTION(ppm_thread, arg) {
|
||||||
}
|
}
|
||||||
|
|
||||||
if (send_current && config.multi_esc) {
|
if (send_current && config.multi_esc) {
|
||||||
float current = mc_interface_get_tot_current_directional_filtered();
|
float current_filtered = mc_interface_get_tot_current_directional_filtered();
|
||||||
|
|
||||||
for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
|
for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
|
||||||
can_status_msg *msg = comm_can_get_status_msg_index(i);
|
can_status_msg *msg = comm_can_get_status_msg_index(i);
|
||||||
|
|
||||||
if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < MAX_CAN_AGE) {
|
if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < MAX_CAN_AGE) {
|
||||||
comm_can_set_current(msg->id, current);
|
comm_can_set_current(msg->id, current_filtered);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
4
buffer.c
4
buffer.c
|
@ -106,7 +106,7 @@ void buffer_append_float32_auto(uint8_t* buffer, float number, int32_t *index) {
|
||||||
|
|
||||||
uint32_t res = ((e & 0xFF) << 23) | (sig_i & 0x7FFFFF);
|
uint32_t res = ((e & 0xFF) << 23) | (sig_i & 0x7FFFFF);
|
||||||
if (sig < 0) {
|
if (sig < 0) {
|
||||||
res |= 1 << 31;
|
res |= 1U << 31;
|
||||||
}
|
}
|
||||||
|
|
||||||
buffer_append_uint32(buffer, res, index);
|
buffer_append_uint32(buffer, res, index);
|
||||||
|
@ -157,7 +157,7 @@ float buffer_get_float32_auto(const uint8_t *buffer, int32_t *index) {
|
||||||
|
|
||||||
int e = (res >> 23) & 0xFF;
|
int e = (res >> 23) & 0xFF;
|
||||||
uint32_t sig_i = res & 0x7FFFFF;
|
uint32_t sig_i = res & 0x7FFFFF;
|
||||||
bool neg = res & (1 << 31);
|
bool neg = res & (1U << 31);
|
||||||
|
|
||||||
float sig = 0.0;
|
float sig = 0.0;
|
||||||
if (e != 0 || sig_i != 0) {
|
if (e != 0 || sig_i != 0) {
|
||||||
|
|
Binary file not shown.
|
@ -217,3 +217,20 @@ cd $FWPATH
|
||||||
make clean
|
make clean
|
||||||
cd $DIR
|
cd $DIR
|
||||||
|
|
||||||
|
#################### HW PALTA ########################
|
||||||
|
|
||||||
|
COPYDIR=PALTA
|
||||||
|
rm $COPYDIR/*
|
||||||
|
|
||||||
|
# default
|
||||||
|
cd $FWPATH
|
||||||
|
touch conf_general.h
|
||||||
|
make -j8 build_args='-DHW_SOURCE=\"hw_palta.c\" -DHW_HEADER=\"hw_palta.h\"'
|
||||||
|
cd $DIR
|
||||||
|
cp $FWPATH/build/BLDC_4_ChibiOS.bin $COPYDIR/VESC_default.bin
|
||||||
|
|
||||||
|
# Clean
|
||||||
|
cd $FWPATH
|
||||||
|
make clean
|
||||||
|
cd $DIR
|
||||||
|
|
||||||
|
|
|
@ -692,6 +692,9 @@ static THD_FUNCTION(cancom_read_thread, arg) {
|
||||||
chEvtRegister(&HW_CAN_DEV.rxfull_event, &el, 0);
|
chEvtRegister(&HW_CAN_DEV.rxfull_event, &el, 0);
|
||||||
|
|
||||||
while(!chThdShouldTerminateX()) {
|
while(!chThdShouldTerminateX()) {
|
||||||
|
// Feed watchdog
|
||||||
|
timeout_feed_WDT(THREAD_CANBUS);
|
||||||
|
|
||||||
if (chEvtWaitAnyTimeout(ALL_EVENTS, MS2ST(10)) == 0) {
|
if (chEvtWaitAnyTimeout(ALL_EVENTS, MS2ST(10)) == 0) {
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
|
|
|
@ -472,7 +472,6 @@ void commands_process_packet(unsigned char *data, unsigned int len) {
|
||||||
mcconf.m_dc_f_sw = buffer_get_float32_auto(data, &ind);
|
mcconf.m_dc_f_sw = buffer_get_float32_auto(data, &ind);
|
||||||
mcconf.m_ntc_motor_beta = buffer_get_float32_auto(data, &ind);
|
mcconf.m_ntc_motor_beta = buffer_get_float32_auto(data, &ind);
|
||||||
mcconf.m_out_aux_mode = data[ind++];
|
mcconf.m_out_aux_mode = data[ind++];
|
||||||
|
|
||||||
mcconf.si_motor_poles = data[ind++];
|
mcconf.si_motor_poles = data[ind++];
|
||||||
mcconf.si_gear_ratio = buffer_get_float32_auto(data, &ind);
|
mcconf.si_gear_ratio = buffer_get_float32_auto(data, &ind);
|
||||||
mcconf.si_wheel_diameter = buffer_get_float32_auto(data, &ind);
|
mcconf.si_wheel_diameter = buffer_get_float32_auto(data, &ind);
|
||||||
|
@ -1342,6 +1341,12 @@ void commands_apply_mcconf_hw_limits(mc_configuration *mcconf) {
|
||||||
utils_truncate_number(&mcconf->l_temp_fet_start, HW_LIM_TEMP_FET);
|
utils_truncate_number(&mcconf->l_temp_fet_start, HW_LIM_TEMP_FET);
|
||||||
utils_truncate_number(&mcconf->l_temp_fet_end, HW_LIM_TEMP_FET);
|
utils_truncate_number(&mcconf->l_temp_fet_end, HW_LIM_TEMP_FET);
|
||||||
#endif
|
#endif
|
||||||
|
#ifdef HW_LIM_FOC_CTRL_LOOP_FREQ
|
||||||
|
if (mcconf->foc_sample_v0_v7 == true)
|
||||||
|
utils_truncate_number(&mcconf->foc_f_sw, HW_LIM_FOC_CTRL_LOOP_FREQ); //control loop executes twice per pwm cycle when sampling in v0 and v7
|
||||||
|
else
|
||||||
|
utils_truncate_number(&mcconf->foc_f_sw, HW_LIM_FOC_CTRL_LOOP_FREQ*2);
|
||||||
|
#endif
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -344,6 +344,7 @@ bool conf_general_store_app_configuration(app_configuration *conf) {
|
||||||
mc_interface_lock();
|
mc_interface_lock();
|
||||||
|
|
||||||
RCC_APB1PeriphClockCmd(RCC_APB1Periph_WWDG, DISABLE);
|
RCC_APB1PeriphClockCmd(RCC_APB1Periph_WWDG, DISABLE);
|
||||||
|
timeout_configure_IWDT_slowest();
|
||||||
|
|
||||||
bool is_ok = true;
|
bool is_ok = true;
|
||||||
uint8_t *conf_addr = (uint8_t*)conf;
|
uint8_t *conf_addr = (uint8_t*)conf;
|
||||||
|
@ -363,6 +364,7 @@ bool conf_general_store_app_configuration(app_configuration *conf) {
|
||||||
}
|
}
|
||||||
|
|
||||||
RCC_APB1PeriphClockCmd(RCC_APB1Periph_WWDG, ENABLE);
|
RCC_APB1PeriphClockCmd(RCC_APB1Periph_WWDG, ENABLE);
|
||||||
|
timeout_configure_IWDT();
|
||||||
|
|
||||||
chThdSleepMilliseconds(100);
|
chThdSleepMilliseconds(100);
|
||||||
mc_interface_unlock();
|
mc_interface_unlock();
|
||||||
|
@ -411,6 +413,7 @@ bool conf_general_store_mc_configuration(mc_configuration *conf) {
|
||||||
mc_interface_lock();
|
mc_interface_lock();
|
||||||
|
|
||||||
RCC_APB1PeriphClockCmd(RCC_APB1Periph_WWDG, DISABLE);
|
RCC_APB1PeriphClockCmd(RCC_APB1Periph_WWDG, DISABLE);
|
||||||
|
timeout_configure_IWDT_slowest();
|
||||||
|
|
||||||
bool is_ok = true;
|
bool is_ok = true;
|
||||||
uint8_t *conf_addr = (uint8_t*)conf;
|
uint8_t *conf_addr = (uint8_t*)conf;
|
||||||
|
@ -430,6 +433,7 @@ bool conf_general_store_mc_configuration(mc_configuration *conf) {
|
||||||
}
|
}
|
||||||
|
|
||||||
RCC_APB1PeriphClockCmd(RCC_APB1Periph_WWDG, ENABLE);
|
RCC_APB1PeriphClockCmd(RCC_APB1Periph_WWDG, ENABLE);
|
||||||
|
timeout_configure_IWDT();
|
||||||
|
|
||||||
chThdSleepMilliseconds(100);
|
chThdSleepMilliseconds(100);
|
||||||
mc_interface_unlock();
|
mc_interface_unlock();
|
||||||
|
@ -799,6 +803,61 @@ bool conf_general_measure_flux_linkage(float current, float duty,
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/* Calculate DTG register */
|
||||||
|
uint8_t conf_general_calculate_deadtime(float deadtime_ns, float core_clock_freq) {
|
||||||
|
uint8_t DTG = 0;
|
||||||
|
float timebase = 1/(core_clock_freq/1000000.0)*1000.0;
|
||||||
|
|
||||||
|
if (deadtime_ns <= (timebase * 127.0) )
|
||||||
|
DTG = deadtime_ns / timebase;
|
||||||
|
else {
|
||||||
|
if (deadtime_ns <= ((63.0 + 64.0)*2.0*timebase) ) {
|
||||||
|
DTG = deadtime_ns / (2.0*timebase) - 64.0;
|
||||||
|
DTG |= 0x80;
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
if (deadtime_ns <= ((31.0 + 32.0)*8.0*timebase) ) {
|
||||||
|
DTG = deadtime_ns / (8.0*timebase) - 32.0;
|
||||||
|
DTG |= 0xC0;
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
if (deadtime_ns <= ((31.0 + 32)*16*timebase) ) {
|
||||||
|
DTG = deadtime_ns / (16.0*timebase) - 32.0;
|
||||||
|
DTG |= 0xE0;
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
// Deadtime requested is longer than max achievable. Set deadtime at
|
||||||
|
// longest possible value
|
||||||
|
DTG = 0xFF;
|
||||||
|
assert_param(1); //catch this
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return DTG;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Try to measure the motor flux linkage using open loop FOC control.
|
||||||
|
*
|
||||||
|
* @param current
|
||||||
|
* The Q-axis current to spin up the motor.
|
||||||
|
*
|
||||||
|
* @param duty
|
||||||
|
* Duty cycle % to measure at
|
||||||
|
*
|
||||||
|
* @param erpm_per_sec
|
||||||
|
* Acceleration rate
|
||||||
|
*
|
||||||
|
* @param res
|
||||||
|
* The motor phase resistance.
|
||||||
|
*
|
||||||
|
* @param linkage
|
||||||
|
* The calculated flux linkage.
|
||||||
|
*
|
||||||
|
* @return
|
||||||
|
* True for success, false otherwise.
|
||||||
|
*/
|
||||||
bool conf_general_measure_flux_linkage_openloop(float current, float duty,
|
bool conf_general_measure_flux_linkage_openloop(float current, float duty,
|
||||||
float erpm_per_sec, float res, float *linkage) {
|
float erpm_per_sec, float res, float *linkage) {
|
||||||
bool result = false;
|
bool result = false;
|
||||||
|
@ -1357,4 +1416,3 @@ int conf_general_detect_apply_all_foc_can(bool detect_can, float max_power_loss,
|
||||||
|
|
||||||
return res;
|
return res;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -70,8 +70,8 @@
|
||||||
//#define HW_SOURCE "hw_410.c" // Also for 4.11 and 4.12
|
//#define HW_SOURCE "hw_410.c" // Also for 4.11 and 4.12
|
||||||
//#define HW_HEADER "hw_410.h" // Also for 4.11 and 4.12
|
//#define HW_HEADER "hw_410.h" // Also for 4.11 and 4.12
|
||||||
|
|
||||||
#define HW_SOURCE "hw_60.c"
|
//#define HW_SOURCE "hw_60.c"
|
||||||
#define HW_HEADER "hw_60.h"
|
//#define HW_HEADER "hw_60.h"
|
||||||
|
|
||||||
//#define HW_SOURCE "hw_r2.c"
|
//#define HW_SOURCE "hw_r2.c"
|
||||||
//#define HW_HEADER "hw_r2.h"
|
//#define HW_HEADER "hw_r2.h"
|
||||||
|
@ -82,8 +82,8 @@
|
||||||
//#define HW_SOURCE "hw_das_rs.c"
|
//#define HW_SOURCE "hw_das_rs.c"
|
||||||
//#define HW_HEADER "hw_das_rs.h"
|
//#define HW_HEADER "hw_das_rs.h"
|
||||||
|
|
||||||
//#define HW_SOURCE "hw_palta.c"
|
#define HW_SOURCE "hw_palta.c"
|
||||||
//#define HW_HEADER "hw_palta.h"
|
#define HW_HEADER "hw_palta.h"
|
||||||
|
|
||||||
//#define HW_SOURCE "hw_rh.c"
|
//#define HW_SOURCE "hw_rh.c"
|
||||||
//#define HW_HEADER "hw_rh.h"
|
//#define HW_HEADER "hw_rh.h"
|
||||||
|
@ -203,6 +203,16 @@
|
||||||
*/
|
*/
|
||||||
#define BLDC_SPEED_CONTROL_CURRENT 1
|
#define BLDC_SPEED_CONTROL_CURRENT 1
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Run the FOC loop once every N ADC ISR requests. This way the pwm frequency is
|
||||||
|
* detached from the FOC calculation, which because it takes ~25usec it can't work
|
||||||
|
* at >40khz. To set a 100kHz pwm FOC_CONTROL_LOOP_FREQ_DIVIDER can be set at 3
|
||||||
|
* so it skips 2 ISR calls and execute the control loop in the 3rd call.
|
||||||
|
*/
|
||||||
|
#ifndef FOC_CONTROL_LOOP_FREQ_DIVIDER
|
||||||
|
#define FOC_CONTROL_LOOP_FREQ_DIVIDER 1
|
||||||
|
#endif
|
||||||
|
|
||||||
// Global configuration variables
|
// Global configuration variables
|
||||||
extern bool conf_general_permanent_nrf_found;
|
extern bool conf_general_permanent_nrf_found;
|
||||||
|
|
||||||
|
@ -218,6 +228,7 @@ bool conf_general_detect_motor_param(float current, float min_rpm, float low_dut
|
||||||
float *int_limit, float *bemf_coupling_k, int8_t *hall_table, int *hall_res);
|
float *int_limit, float *bemf_coupling_k, int8_t *hall_table, int *hall_res);
|
||||||
bool conf_general_measure_flux_linkage(float current, float duty,
|
bool conf_general_measure_flux_linkage(float current, float duty,
|
||||||
float min_erpm, float res, float *linkage);
|
float min_erpm, float res, float *linkage);
|
||||||
|
uint8_t conf_general_calculate_deadtime(float deadtime_ns, float core_clock_freq);
|
||||||
bool conf_general_measure_flux_linkage_openloop(float current, float duty,
|
bool conf_general_measure_flux_linkage_openloop(float current, float duty,
|
||||||
float erpm_per_sec, float res, float *linkage);
|
float erpm_per_sec, float res, float *linkage);
|
||||||
int conf_general_autodetect_apply_sensors_foc(float current,
|
int conf_general_autodetect_apply_sensors_foc(float current,
|
||||||
|
|
|
@ -85,7 +85,11 @@ typedef enum {
|
||||||
FAULT_CODE_DRV,
|
FAULT_CODE_DRV,
|
||||||
FAULT_CODE_ABS_OVER_CURRENT,
|
FAULT_CODE_ABS_OVER_CURRENT,
|
||||||
FAULT_CODE_OVER_TEMP_FET,
|
FAULT_CODE_OVER_TEMP_FET,
|
||||||
FAULT_CODE_OVER_TEMP_MOTOR
|
FAULT_CODE_OVER_TEMP_MOTOR,
|
||||||
|
FAULT_CODE_GATE_DRIVER_OVER_VOLTAGE,
|
||||||
|
FAULT_CODE_GATE_DRIVER_UNDER_VOLTAGE,
|
||||||
|
FAULT_CODE_MCU_UNDER_VOLTAGE,
|
||||||
|
FAULT_CODE_BOOTING_FROM_WATCHDOG_RESET
|
||||||
} mc_fault_code;
|
} mc_fault_code;
|
||||||
|
|
||||||
typedef enum {
|
typedef enum {
|
||||||
|
@ -606,6 +610,7 @@ typedef struct {
|
||||||
float current;
|
float current;
|
||||||
float current_filtered;
|
float current_filtered;
|
||||||
float voltage;
|
float voltage;
|
||||||
|
float gate_driver_voltage;
|
||||||
float duty;
|
float duty;
|
||||||
float rpm;
|
float rpm;
|
||||||
int tacho;
|
int tacho;
|
||||||
|
|
21
eeprom.c
21
eeprom.c
|
@ -331,16 +331,23 @@ uint16_t EE_WriteVariable(uint16_t VirtAddress, uint16_t Data)
|
||||||
{
|
{
|
||||||
uint16_t Status = 0;
|
uint16_t Status = 0;
|
||||||
|
|
||||||
/* Write the variable virtual address and value in the EEPROM */
|
/* Return error if MCU VDD is below 2.9V */
|
||||||
Status = EE_VerifyPageFullWriteVariable(VirtAddress, Data);
|
if (PWR->CSR & PWR_CSR_PVDO)
|
||||||
|
|
||||||
/* In case the EEPROM active page is full */
|
|
||||||
if (Status == PAGE_FULL)
|
|
||||||
{
|
{
|
||||||
/* Perform Page transfer */
|
Status = FLASH_ERROR_PROGRAM;
|
||||||
Status = EE_PageTransfer(VirtAddress, Data);
|
|
||||||
}
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
/* Write the variable virtual address and value in the EEPROM */
|
||||||
|
Status = EE_VerifyPageFullWriteVariable(VirtAddress, Data);
|
||||||
|
|
||||||
|
/* In case the EEPROM active page is full */
|
||||||
|
if (Status == PAGE_FULL)
|
||||||
|
{
|
||||||
|
/* Perform Page transfer */
|
||||||
|
Status = EE_PageTransfer(VirtAddress, Data);
|
||||||
|
}
|
||||||
|
}
|
||||||
/* Return last operation status */
|
/* Return last operation status */
|
||||||
return Status;
|
return Status;
|
||||||
}
|
}
|
||||||
|
|
|
@ -23,6 +23,7 @@
|
||||||
#include "stm32f4xx_conf.h"
|
#include "stm32f4xx_conf.h"
|
||||||
#include "utils.h"
|
#include "utils.h"
|
||||||
#include "mc_interface.h"
|
#include "mc_interface.h"
|
||||||
|
#include "timeout.h"
|
||||||
#include "hw.h"
|
#include "hw.h"
|
||||||
#include <string.h>
|
#include <string.h>
|
||||||
|
|
||||||
|
@ -90,6 +91,7 @@ uint16_t flash_helper_erase_new_app(uint32_t new_app_size) {
|
||||||
mc_interface_release_motor();
|
mc_interface_release_motor();
|
||||||
utils_sys_lock_cnt();
|
utils_sys_lock_cnt();
|
||||||
RCC_APB1PeriphClockCmd(RCC_APB1Periph_WWDG, DISABLE);
|
RCC_APB1PeriphClockCmd(RCC_APB1Periph_WWDG, DISABLE);
|
||||||
|
timeout_configure_IWDT_slowest();
|
||||||
|
|
||||||
for (int i = 0;i < NEW_APP_SECTORS;i++) {
|
for (int i = 0;i < NEW_APP_SECTORS;i++) {
|
||||||
if (new_app_size > flash_addr[NEW_APP_BASE + i]) {
|
if (new_app_size > flash_addr[NEW_APP_BASE + i]) {
|
||||||
|
@ -103,6 +105,7 @@ uint16_t flash_helper_erase_new_app(uint32_t new_app_size) {
|
||||||
}
|
}
|
||||||
|
|
||||||
RCC_APB1PeriphClockCmd(RCC_APB1Periph_WWDG, ENABLE);
|
RCC_APB1PeriphClockCmd(RCC_APB1Periph_WWDG, ENABLE);
|
||||||
|
timeout_configure_IWDT();
|
||||||
utils_sys_unlock_cnt();
|
utils_sys_unlock_cnt();
|
||||||
|
|
||||||
return FLASH_COMPLETE;
|
return FLASH_COMPLETE;
|
||||||
|
@ -116,6 +119,7 @@ uint16_t flash_helper_write_new_app_data(uint32_t offset, uint8_t *data, uint32_
|
||||||
mc_interface_release_motor();
|
mc_interface_release_motor();
|
||||||
utils_sys_lock_cnt();
|
utils_sys_lock_cnt();
|
||||||
RCC_APB1PeriphClockCmd(RCC_APB1Periph_WWDG, DISABLE);
|
RCC_APB1PeriphClockCmd(RCC_APB1Periph_WWDG, DISABLE);
|
||||||
|
timeout_configure_IWDT_slowest();
|
||||||
|
|
||||||
for (uint32_t i = 0;i < len;i++) {
|
for (uint32_t i = 0;i < len;i++) {
|
||||||
uint16_t res = FLASH_ProgramByte(flash_addr[NEW_APP_BASE] + offset + i, data[i]);
|
uint16_t res = FLASH_ProgramByte(flash_addr[NEW_APP_BASE] + offset + i, data[i]);
|
||||||
|
@ -125,6 +129,8 @@ uint16_t flash_helper_write_new_app_data(uint32_t offset, uint8_t *data, uint32_
|
||||||
}
|
}
|
||||||
|
|
||||||
RCC_APB1PeriphClockCmd(RCC_APB1Periph_WWDG, ENABLE);
|
RCC_APB1PeriphClockCmd(RCC_APB1Periph_WWDG, ENABLE);
|
||||||
|
timeout_configure_IWDT();
|
||||||
|
|
||||||
utils_sys_unlock_cnt();
|
utils_sys_unlock_cnt();
|
||||||
|
|
||||||
return FLASH_COMPLETE;
|
return FLASH_COMPLETE;
|
||||||
|
@ -147,6 +153,7 @@ void flash_helper_jump_to_bootloader(void) {
|
||||||
|
|
||||||
// Disable watchdog
|
// Disable watchdog
|
||||||
RCC_APB1PeriphClockCmd(RCC_APB1Periph_WWDG, DISABLE);
|
RCC_APB1PeriphClockCmd(RCC_APB1Periph_WWDG, DISABLE);
|
||||||
|
timeout_configure_IWDT_slowest();
|
||||||
|
|
||||||
chSysDisable();
|
chSysDisable();
|
||||||
|
|
||||||
|
|
|
@ -159,7 +159,7 @@ void gpdrive_init(volatile mc_configuration *configuration) {
|
||||||
TIM_BDTRInitStructure.TIM_OSSRState = TIM_OSSRState_Enable;
|
TIM_BDTRInitStructure.TIM_OSSRState = TIM_OSSRState_Enable;
|
||||||
TIM_BDTRInitStructure.TIM_OSSIState = TIM_OSSIState_Enable;
|
TIM_BDTRInitStructure.TIM_OSSIState = TIM_OSSIState_Enable;
|
||||||
TIM_BDTRInitStructure.TIM_LOCKLevel = TIM_LOCKLevel_OFF;
|
TIM_BDTRInitStructure.TIM_LOCKLevel = TIM_LOCKLevel_OFF;
|
||||||
TIM_BDTRInitStructure.TIM_DeadTime = HW_DEAD_TIME_VALUE;
|
TIM_BDTRInitStructure.TIM_DeadTime = conf_general_calculate_deadtime(HW_DEAD_TIME_NSEC, SYSTEM_CORE_CLOCK);
|
||||||
TIM_BDTRInitStructure.TIM_Break = TIM_Break_Disable;
|
TIM_BDTRInitStructure.TIM_Break = TIM_Break_Disable;
|
||||||
TIM_BDTRInitStructure.TIM_BreakPolarity = TIM_BreakPolarity_High;
|
TIM_BDTRInitStructure.TIM_BreakPolarity = TIM_BreakPolarity_High;
|
||||||
TIM_BDTRInitStructure.TIM_AutomaticOutput = TIM_AutomaticOutput_Disable;
|
TIM_BDTRInitStructure.TIM_AutomaticOutput = TIM_AutomaticOutput_Disable;
|
||||||
|
|
|
@ -230,7 +230,7 @@
|
||||||
#define READ_HALL3() palReadPad(HW_HALL_ENC_GPIO3, HW_HALL_ENC_PIN3)
|
#define READ_HALL3() palReadPad(HW_HALL_ENC_GPIO3, HW_HALL_ENC_PIN3)
|
||||||
|
|
||||||
// Override dead time. See the stm32f4 reference manual for calculating this value.
|
// Override dead time. See the stm32f4 reference manual for calculating this value.
|
||||||
#define HW_DEAD_TIME_VALUE 110
|
#define HW_DEAD_TIME_NSEC 660.0
|
||||||
|
|
||||||
// Default setting overrides
|
// Default setting overrides
|
||||||
#ifndef MCCONF_L_MAX_VOLTAGE
|
#ifndef MCCONF_L_MAX_VOLTAGE
|
||||||
|
|
|
@ -110,7 +110,7 @@
|
||||||
// Voltage on ADC channel
|
// Voltage on ADC channel
|
||||||
#define ADC_VOLTS(ch) ((float)ADC_Value[ch] / 4096.0 * V_REG)
|
#define ADC_VOLTS(ch) ((float)ADC_Value[ch] / 4096.0 * V_REG)
|
||||||
|
|
||||||
#define HW_DEAD_TIME_VALUE 20
|
#define HW_DEAD_TIME_NSEC 120
|
||||||
|
|
||||||
// Double samples in beginning and end for positive current measurement.
|
// Double samples in beginning and end for positive current measurement.
|
||||||
// Useful when the shunt sense traces have noise that causes offset.
|
// Useful when the shunt sense traces have noise that causes offset.
|
||||||
|
|
|
@ -201,7 +201,7 @@
|
||||||
#define READ_HALL2() palReadPad(HW_HALL_ENC_GPIO2, HW_HALL_ENC_PIN2)
|
#define READ_HALL2() palReadPad(HW_HALL_ENC_GPIO2, HW_HALL_ENC_PIN2)
|
||||||
#define READ_HALL3() palReadPad(HW_HALL_ENC_GPIO3, HW_HALL_ENC_PIN3)
|
#define READ_HALL3() palReadPad(HW_HALL_ENC_GPIO3, HW_HALL_ENC_PIN3)
|
||||||
|
|
||||||
#define HW_DEAD_TIME_VALUE 20
|
#define HW_DEAD_TIME_NSEC 120.0
|
||||||
|
|
||||||
// Default setting overrides
|
// Default setting overrides
|
||||||
#ifndef MCCONF_DEFAULT_MOTOR_TYPE
|
#ifndef MCCONF_DEFAULT_MOTOR_TYPE
|
||||||
|
|
|
@ -20,12 +20,34 @@
|
||||||
#include "ch.h"
|
#include "ch.h"
|
||||||
#include "hal.h"
|
#include "hal.h"
|
||||||
#include "stm32f4xx_conf.h"
|
#include "stm32f4xx_conf.h"
|
||||||
|
#include "stm32f4xx_rcc.h"
|
||||||
#include "utils.h"
|
#include "utils.h"
|
||||||
#include "terminal.h"
|
#include "terminal.h"
|
||||||
#include "commands.h"
|
#include "commands.h"
|
||||||
|
|
||||||
|
#include "hw_palta_fpga_bitstream.c" //this file ONLY contains the fpga binary blob
|
||||||
|
|
||||||
|
// Defines
|
||||||
|
#define SPI_SW_MISO_GPIO HW_SPI_PORT_MISO
|
||||||
|
#define SPI_SW_MISO_PIN HW_SPI_PIN_MISO
|
||||||
|
#define SPI_SW_MOSI_GPIO HW_SPI_PORT_MOSI
|
||||||
|
#define SPI_SW_MOSI_PIN HW_SPI_PIN_MOSI
|
||||||
|
#define SPI_SW_SCK_GPIO HW_SPI_PORT_SCK
|
||||||
|
#define SPI_SW_SCK_PIN HW_SPI_PIN_SCK
|
||||||
|
#define SPI_SW_FPGA_CS_GPIO GPIOB
|
||||||
|
#define SPI_SW_FPGA_CS_PIN 7
|
||||||
|
|
||||||
|
#define PALTA_FPGA_CLK_PORT GPIOC
|
||||||
|
#define PALTA_FPGA_CLK_PIN 9
|
||||||
|
#define PALTA_FPGA_RESET_PORT GPIOB
|
||||||
|
#define PALTA_FPGA_RESET_PIN 4
|
||||||
|
#define BITSTREAM_SIZE 104090 //ice40up5k
|
||||||
|
//#define BITSTREAM_SIZE 71338 //ice40LP1K
|
||||||
|
|
||||||
// Variables
|
// Variables
|
||||||
static volatile bool i2c_running = false;
|
static volatile bool i2c_running = false;
|
||||||
|
//extern unsigned char FPGA_bitstream[BITSTREAM_SIZE];
|
||||||
|
|
||||||
|
|
||||||
// I2C configuration
|
// I2C configuration
|
||||||
static const I2CConfig i2cfg = {
|
static const I2CConfig i2cfg = {
|
||||||
|
@ -36,8 +58,24 @@ static const I2CConfig i2cfg = {
|
||||||
|
|
||||||
// Private functions
|
// Private functions
|
||||||
static void terminal_cmd_reset_oc(int argc, const char **argv);
|
static void terminal_cmd_reset_oc(int argc, const char **argv);
|
||||||
|
static void spi_transfer(uint8_t *in_buf, const uint8_t *out_buf, int length);
|
||||||
|
static void spi_begin(void);
|
||||||
|
static void spi_end(void);
|
||||||
|
static void spi_delay(void);
|
||||||
|
void hw_palta_init_FPGA_CLK(void);
|
||||||
|
void hw_palta_setup_dac(void);
|
||||||
|
void hw_palta_configure_brownout(uint8_t);
|
||||||
|
void hw_palta_configure_VDD_undervoltage(void);
|
||||||
|
|
||||||
void hw_init_gpio(void) {
|
void hw_init_gpio(void) {
|
||||||
|
|
||||||
|
// Set Brown out to keep mcu under reset until VDD reaches 2.7V
|
||||||
|
hw_palta_configure_brownout(OB_BOR_LEVEL3);
|
||||||
|
|
||||||
|
// Configure Programmable voltage detector to interrupt the cpu
|
||||||
|
// when VDD is below 2.9V.
|
||||||
|
hw_palta_configure_VDD_undervoltage();
|
||||||
|
|
||||||
// GPIO clock enable
|
// GPIO clock enable
|
||||||
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);
|
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);
|
||||||
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE);
|
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE);
|
||||||
|
@ -48,7 +86,7 @@ void hw_init_gpio(void) {
|
||||||
palSetPadMode(GPIOB, 2,
|
palSetPadMode(GPIOB, 2,
|
||||||
PAL_MODE_OUTPUT_PUSHPULL |
|
PAL_MODE_OUTPUT_PUSHPULL |
|
||||||
PAL_STM32_OSPEED_HIGHEST);
|
PAL_STM32_OSPEED_HIGHEST);
|
||||||
palSetPadMode(GPIOB, 1,
|
palSetPadMode(GPIOB, 11,
|
||||||
PAL_MODE_OUTPUT_PUSHPULL |
|
PAL_MODE_OUTPUT_PUSHPULL |
|
||||||
PAL_STM32_OSPEED_HIGHEST);
|
PAL_STM32_OSPEED_HIGHEST);
|
||||||
|
|
||||||
|
@ -59,12 +97,23 @@ void hw_init_gpio(void) {
|
||||||
|
|
||||||
ENABLE_GATE();
|
ENABLE_GATE();
|
||||||
|
|
||||||
// OC latch
|
// FPGA SPI port
|
||||||
palSetPadMode(PALTA_OC_CLR_PORT, PALTA_OC_CLR_PIN,
|
palSetPadMode(SPI_SW_MISO_GPIO, SPI_SW_MISO_PIN, PAL_MODE_INPUT);
|
||||||
PAL_MODE_OUTPUT_PUSHPULL |
|
palSetPadMode(SPI_SW_SCK_GPIO, SPI_SW_SCK_PIN, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
|
||||||
PAL_STM32_OSPEED_HIGHEST);
|
palSetPadMode(SPI_SW_FPGA_CS_GPIO, SPI_SW_FPGA_CS_PIN, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
|
||||||
|
palSetPadMode(SPI_SW_MOSI_GPIO, SPI_SW_MOSI_PIN, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
|
||||||
|
|
||||||
hw_palta_reset_oc();
|
// Set FPGA SS to '0' to make it start in slave mode
|
||||||
|
palClearPad(SPI_SW_FPGA_CS_GPIO, SPI_SW_FPGA_CS_PIN);
|
||||||
|
|
||||||
|
// FPGA RESET
|
||||||
|
palSetPadMode(PALTA_FPGA_RESET_PORT, PALTA_FPGA_RESET_PIN, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
|
||||||
|
palClearPad(PALTA_FPGA_RESET_PORT, PALTA_FPGA_RESET_PIN);
|
||||||
|
chThdSleep(1);
|
||||||
|
palSetPad(PALTA_FPGA_RESET_PORT, PALTA_FPGA_RESET_PIN);
|
||||||
|
|
||||||
|
//output a 12MHz clock on MCO2
|
||||||
|
hw_palta_init_FPGA_CLK();
|
||||||
|
|
||||||
// GPIOA Configuration: Channel 1 to 3 as alternate function push-pull
|
// GPIOA Configuration: Channel 1 to 3 as alternate function push-pull
|
||||||
palSetPadMode(GPIOA, 8, PAL_MODE_ALTERNATE(GPIO_AF_TIM1) |
|
palSetPadMode(GPIOA, 8, PAL_MODE_ALTERNATE(GPIO_AF_TIM1) |
|
||||||
|
@ -101,14 +150,23 @@ void hw_init_gpio(void) {
|
||||||
palSetPadMode(GPIOA, 2, PAL_MODE_INPUT_ANALOG);
|
palSetPadMode(GPIOA, 2, PAL_MODE_INPUT_ANALOG);
|
||||||
palSetPadMode(GPIOA, 3, PAL_MODE_INPUT_ANALOG);
|
palSetPadMode(GPIOA, 3, PAL_MODE_INPUT_ANALOG);
|
||||||
|
|
||||||
palSetPadMode(GPIOB, 0, PAL_MODE_INPUT_ANALOG);
|
#ifdef PALTA_USE_DAC
|
||||||
|
hw_palta_setup_dac();
|
||||||
|
#else
|
||||||
|
palSetPadMode(GPIOA, 4, PAL_MODE_INPUT_ANALOG); //Temperature bridge A
|
||||||
|
palSetPadMode(GPIOA, 5, PAL_MODE_INPUT_ANALOG); //Temperature bridge B
|
||||||
|
#endif
|
||||||
|
palSetPadMode(GPIOA, 6, PAL_MODE_INPUT_ANALOG); //Temperature bridge C
|
||||||
|
|
||||||
|
palSetPadMode(GPIOB, 0, PAL_MODE_INPUT_ANALOG); //Accel 2
|
||||||
|
palSetPadMode(GPIOB, 1, PAL_MODE_INPUT_ANALOG); //Gate driver supply voltage
|
||||||
|
|
||||||
palSetPadMode(GPIOC, 0, PAL_MODE_INPUT_ANALOG);
|
palSetPadMode(GPIOC, 0, PAL_MODE_INPUT_ANALOG);
|
||||||
palSetPadMode(GPIOC, 1, PAL_MODE_INPUT_ANALOG);
|
palSetPadMode(GPIOC, 1, PAL_MODE_INPUT_ANALOG);
|
||||||
palSetPadMode(GPIOC, 2, PAL_MODE_INPUT_ANALOG);
|
palSetPadMode(GPIOC, 2, PAL_MODE_INPUT_ANALOG);
|
||||||
palSetPadMode(GPIOC, 3, PAL_MODE_INPUT_ANALOG);
|
palSetPadMode(GPIOC, 3, PAL_MODE_INPUT_ANALOG);
|
||||||
palSetPadMode(GPIOC, 4, PAL_MODE_INPUT_ANALOG);
|
palSetPadMode(GPIOC, 4, PAL_MODE_INPUT_ANALOG); //Motor temp
|
||||||
palSetPadMode(GPIOC, 5, PAL_MODE_INPUT_ANALOG);
|
palSetPadMode(GPIOC, 5, PAL_MODE_INPUT_ANALOG); //Accel 1
|
||||||
|
|
||||||
// Register terminal callbacks
|
// Register terminal callbacks
|
||||||
terminal_register_command_callback(
|
terminal_register_command_callback(
|
||||||
|
@ -116,6 +174,9 @@ void hw_init_gpio(void) {
|
||||||
"Reset latched overcurrent fault.",
|
"Reset latched overcurrent fault.",
|
||||||
0,
|
0,
|
||||||
terminal_cmd_reset_oc);
|
terminal_cmd_reset_oc);
|
||||||
|
|
||||||
|
// Send bitstream over SPI to configure FPGA
|
||||||
|
hw_palta_configure_FPGA();
|
||||||
}
|
}
|
||||||
|
|
||||||
void hw_setup_adc_channels(void) {
|
void hw_setup_adc_channels(void) {
|
||||||
|
@ -124,7 +185,8 @@ void hw_setup_adc_channels(void) {
|
||||||
ADC_RegularChannelConfig(ADC1, ADC_Channel_10, 2, ADC_SampleTime_15Cycles);
|
ADC_RegularChannelConfig(ADC1, ADC_Channel_10, 2, ADC_SampleTime_15Cycles);
|
||||||
ADC_RegularChannelConfig(ADC1, ADC_Channel_8, 3, ADC_SampleTime_15Cycles);
|
ADC_RegularChannelConfig(ADC1, ADC_Channel_8, 3, ADC_SampleTime_15Cycles);
|
||||||
ADC_RegularChannelConfig(ADC1, ADC_Channel_14, 4, ADC_SampleTime_15Cycles);
|
ADC_RegularChannelConfig(ADC1, ADC_Channel_14, 4, ADC_SampleTime_15Cycles);
|
||||||
ADC_RegularChannelConfig(ADC1, ADC_Channel_Vrefint, 5, ADC_SampleTime_15Cycles);
|
//ADC_RegularChannelConfig(ADC1, ADC_Channel_Vrefint, 5, ADC_SampleTime_15Cycles);
|
||||||
|
ADC_RegularChannelConfig(ADC1, ADC_Channel_9, 5, ADC_SampleTime_15Cycles);
|
||||||
|
|
||||||
// ADC2 regular channels
|
// ADC2 regular channels
|
||||||
ADC_RegularChannelConfig(ADC2, ADC_Channel_1, 1, ADC_SampleTime_15Cycles);
|
ADC_RegularChannelConfig(ADC2, ADC_Channel_1, 1, ADC_SampleTime_15Cycles);
|
||||||
|
@ -152,6 +214,69 @@ void hw_setup_adc_channels(void) {
|
||||||
ADC_InjectedChannelConfig(ADC3, ADC_Channel_12, 3, ADC_SampleTime_15Cycles);
|
ADC_InjectedChannelConfig(ADC3, ADC_Channel_12, 3, ADC_SampleTime_15Cycles);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void hw_palta_setup_dac(void) {
|
||||||
|
// GPIOA clock enable
|
||||||
|
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);
|
||||||
|
|
||||||
|
// DAC Periph clock enable
|
||||||
|
RCC_APB1PeriphClockCmd(RCC_APB1Periph_DAC, ENABLE);
|
||||||
|
|
||||||
|
// DAC channel 1 & 2 (DAC_OUT1 = PA.4)(DAC_OUT2 = PA.5) configuration
|
||||||
|
palSetPadMode(GPIOA, 4, PAL_MODE_INPUT_ANALOG);
|
||||||
|
palSetPadMode(GPIOA, 5, PAL_MODE_INPUT_ANALOG);
|
||||||
|
|
||||||
|
// Enable both DAC channels with output buffer disabled to achieve rail-to-rail output
|
||||||
|
DAC->CR |= DAC_CR_EN1 | DAC_CR_BOFF1 | DAC_CR_EN2 | DAC_CR_BOFF2;
|
||||||
|
|
||||||
|
// Set DAC channels at 1.65V
|
||||||
|
hw_palta_DAC1_setdata(0x800);
|
||||||
|
hw_palta_DAC2_setdata(0x800);
|
||||||
|
}
|
||||||
|
|
||||||
|
void hw_palta_DAC1_setdata(uint16_t data) {
|
||||||
|
DAC->DHR12R1 = data;
|
||||||
|
}
|
||||||
|
|
||||||
|
void hw_palta_DAC2_setdata(uint16_t data) {
|
||||||
|
DAC->DHR12R2 = data;
|
||||||
|
}
|
||||||
|
|
||||||
|
void hw_palta_configure_brownout(uint8_t BOR_level) {
|
||||||
|
/* Get BOR Option Bytes */
|
||||||
|
if((FLASH_OB_GetBOR() & 0x0C) != BOR_level)
|
||||||
|
{
|
||||||
|
/* Unlocks the option bytes block access */
|
||||||
|
FLASH_OB_Unlock();
|
||||||
|
|
||||||
|
/* Select the desired V(BOR) Level -------------------------------------*/
|
||||||
|
FLASH_OB_BORConfig(BOR_level);
|
||||||
|
|
||||||
|
/* Launch the option byte loading */
|
||||||
|
FLASH_OB_Launch();
|
||||||
|
|
||||||
|
/* Locks the option bytes block access */
|
||||||
|
FLASH_OB_Lock();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void hw_palta_configure_VDD_undervoltage(void) {
|
||||||
|
|
||||||
|
// partially configured in mcuconf.h -> STM32_PVD_ENABLE and STM32_PLS
|
||||||
|
|
||||||
|
// Connect EXTI Line to pin
|
||||||
|
EXTI_InitTypeDef EXTI_InitStructure;
|
||||||
|
|
||||||
|
// Configure EXTI Line
|
||||||
|
EXTI_InitStructure.EXTI_Line = EXTI_Line16; //Connected to Programmable Voltage Detector
|
||||||
|
EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt;
|
||||||
|
EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Rising;
|
||||||
|
EXTI_InitStructure.EXTI_LineCmd = ENABLE;
|
||||||
|
EXTI_Init(&EXTI_InitStructure);
|
||||||
|
|
||||||
|
// Enable and set EXTI Line Interrupt to the highest priority
|
||||||
|
nvicEnableVector(PVD_IRQn, 0);
|
||||||
|
}
|
||||||
|
|
||||||
void hw_start_i2c(void) {
|
void hw_start_i2c(void) {
|
||||||
i2cAcquireBus(&HW_I2C_DEV);
|
i2cAcquireBus(&HW_I2C_DEV);
|
||||||
|
|
||||||
|
@ -246,17 +371,112 @@ void hw_try_restore_i2c(void) {
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void hw_palta_reset_oc(void) {
|
|
||||||
palClearPad(PALTA_OC_CLR_PORT, PALTA_OC_CLR_PIN);
|
|
||||||
chThdSleep(1);
|
|
||||||
palSetPad(PALTA_OC_CLR_PORT, PALTA_OC_CLR_PIN);
|
|
||||||
}
|
|
||||||
|
|
||||||
static void terminal_cmd_reset_oc(int argc, const char **argv) {
|
static void terminal_cmd_reset_oc(int argc, const char **argv) {
|
||||||
(void)argc;
|
(void)argc;
|
||||||
(void)argv;
|
(void)argv;
|
||||||
|
|
||||||
hw_palta_reset_oc();
|
hw_palta_configure_FPGA();
|
||||||
commands_printf("Palta OC latch reset done!");
|
commands_printf("Palta OC latch reset done!");
|
||||||
commands_printf(" ");
|
commands_printf(" ");
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// Software SPI for FPGA control
|
||||||
|
static void spi_transfer(uint8_t *in_buf, const uint8_t *out_buf, int length) {
|
||||||
|
for (int i = 0;i < length;i++) {
|
||||||
|
uint8_t send = out_buf ? out_buf[i] : 0xFF;
|
||||||
|
uint8_t recieve = 0;
|
||||||
|
|
||||||
|
for (int bit = 0;bit < 8;bit++) {
|
||||||
|
palWritePad(HW_SPI_PORT_MOSI, HW_SPI_PIN_MOSI, send >> 7);
|
||||||
|
send <<= 1;
|
||||||
|
|
||||||
|
spi_delay();
|
||||||
|
palSetPad(SPI_SW_SCK_GPIO, SPI_SW_SCK_PIN);
|
||||||
|
spi_delay();
|
||||||
|
|
||||||
|
/*
|
||||||
|
int r1, r2, r3;
|
||||||
|
r1 = palReadPad(SPI_SW_MISO_GPIO, SPI_SW_MISO_PIN);
|
||||||
|
__NOP();
|
||||||
|
r2 = palReadPad(SPI_SW_MISO_GPIO, SPI_SW_MISO_PIN);
|
||||||
|
__NOP();
|
||||||
|
r3 = palReadPad(SPI_SW_MISO_GPIO, SPI_SW_MISO_PIN);
|
||||||
|
|
||||||
|
recieve <<= 1;
|
||||||
|
if (utils_middle_of_3_int(r1, r2, r3)) {
|
||||||
|
recieve |= 1;
|
||||||
|
}
|
||||||
|
*/
|
||||||
|
|
||||||
|
palClearPad(SPI_SW_SCK_GPIO, SPI_SW_SCK_PIN);
|
||||||
|
spi_delay();
|
||||||
|
}
|
||||||
|
|
||||||
|
if (in_buf) {
|
||||||
|
in_buf[i] = recieve;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void hw_palta_init_FPGA_CLK(void) {
|
||||||
|
/* Configure PLLI2S prescalers */
|
||||||
|
/* PLLI2S_VCO : VCO_192M */
|
||||||
|
/* SAI_CLK(first level) = PLLI2S_VCO/PLLI2SQ = 192/4 = 48 Mhz */
|
||||||
|
RCC->PLLI2SCFGR = (192 << 6) | (4 << 28);
|
||||||
|
/* Enable PLLI2S Clock */
|
||||||
|
RCC_PLLI2SCmd(ENABLE);
|
||||||
|
|
||||||
|
/* Wait till PLLI2S is ready */
|
||||||
|
while(RCC_GetFlagStatus(RCC_FLAG_PLLI2SRDY) == RESET)
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
/* Configure MCO2 pin(PC9) in alternate function */
|
||||||
|
palSetPadMode(PALTA_FPGA_CLK_PORT, PALTA_FPGA_CLK_PIN, PAL_MODE_ALTERNATE(GPIO_AF_MCO) |
|
||||||
|
PAL_STM32_OTYPE_PUSHPULL |
|
||||||
|
PAL_STM32_OSPEED_HIGHEST |
|
||||||
|
PAL_STM32_PUDR_PULLUP);
|
||||||
|
|
||||||
|
// HSE clock selected to output on MCO2 pin(PA8) 48MHz/4 = 12MHz
|
||||||
|
RCC_MCO2Config(RCC_MCO2Source_PLLI2SCLK, RCC_MCO2Div_4);
|
||||||
|
}
|
||||||
|
|
||||||
|
char hw_palta_configure_FPGA(void) {
|
||||||
|
spi_begin();
|
||||||
|
palSetPad(SPI_SW_SCK_GPIO, SPI_SW_SCK_PIN);
|
||||||
|
palClearPad(PALTA_FPGA_RESET_PORT, PALTA_FPGA_RESET_PIN);
|
||||||
|
chThdSleep(10);
|
||||||
|
palSetPad(PALTA_FPGA_RESET_PORT, PALTA_FPGA_RESET_PIN);
|
||||||
|
chThdSleep(20);
|
||||||
|
|
||||||
|
spi_transfer(0, FPGA_bitstream, BITSTREAM_SIZE);
|
||||||
|
|
||||||
|
//include 49 extra spi clock cycles, dummy bytes
|
||||||
|
uint8_t dummy = 0;
|
||||||
|
spi_transfer(0, &dummy, 7);
|
||||||
|
|
||||||
|
spi_end();
|
||||||
|
|
||||||
|
// CDONE LED should be set by now
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
static void spi_begin(void) {
|
||||||
|
palClearPad(SPI_SW_FPGA_CS_GPIO, SPI_SW_FPGA_CS_PIN);
|
||||||
|
}
|
||||||
|
|
||||||
|
static void spi_end(void) {
|
||||||
|
palSetPad(SPI_SW_FPGA_CS_GPIO, SPI_SW_FPGA_CS_PIN);
|
||||||
|
}
|
||||||
|
|
||||||
|
static void spi_delay(void) {
|
||||||
|
__NOP();
|
||||||
|
__NOP();
|
||||||
|
__NOP();
|
||||||
|
__NOP();
|
||||||
|
|
||||||
|
__NOP();
|
||||||
|
__NOP();
|
||||||
|
__NOP();
|
||||||
|
__NOP();
|
||||||
|
}
|
||||||
|
|
|
@ -22,6 +22,9 @@
|
||||||
|
|
||||||
#define HW_NAME "PALTA"
|
#define HW_NAME "PALTA"
|
||||||
|
|
||||||
|
#define PALTA_USE_DAC
|
||||||
|
#define HW_VERSION_PALTA
|
||||||
|
|
||||||
// HW properties
|
// HW properties
|
||||||
#define HW_HAS_3_SHUNTS
|
#define HW_HAS_3_SHUNTS
|
||||||
#define HW_HAS_PHASE_SHUNTS
|
#define HW_HAS_PHASE_SHUNTS
|
||||||
|
@ -35,11 +38,8 @@
|
||||||
|
|
||||||
#define LED_GREEN_ON() palSetPad(GPIOB, 2)
|
#define LED_GREEN_ON() palSetPad(GPIOB, 2)
|
||||||
#define LED_GREEN_OFF() palClearPad(GPIOB, 2)
|
#define LED_GREEN_OFF() palClearPad(GPIOB, 2)
|
||||||
#define LED_RED_ON() palSetPad(GPIOB, 1)
|
#define LED_RED_ON() palSetPad(GPIOB, 11)
|
||||||
#define LED_RED_OFF() palClearPad(GPIOB, 1)
|
#define LED_RED_OFF() palClearPad(GPIOB, 11)
|
||||||
|
|
||||||
#define PALTA_OC_CLR_PORT GPIOB
|
|
||||||
#define PALTA_OC_CLR_PIN 5
|
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* ADC Vector
|
* ADC Vector
|
||||||
|
@ -73,11 +73,12 @@
|
||||||
#define ADC_IND_CURR2 4
|
#define ADC_IND_CURR2 4
|
||||||
#define ADC_IND_CURR3 5
|
#define ADC_IND_CURR3 5
|
||||||
#define ADC_IND_VIN_SENS 11
|
#define ADC_IND_VIN_SENS 11
|
||||||
|
#define ADC_IND_VOUT_GATE_DRV 12
|
||||||
#define ADC_IND_EXT 10
|
#define ADC_IND_EXT 10
|
||||||
#define ADC_IND_EXT2 6
|
#define ADC_IND_EXT2 6
|
||||||
#define ADC_IND_TEMP_MOS 8
|
#define ADC_IND_TEMP_MOS 8
|
||||||
#define ADC_IND_TEMP_MOTOR 9
|
#define ADC_IND_TEMP_MOTOR 9
|
||||||
#define ADC_IND_VREFINT 12
|
//#define ADC_IND_VREFINT 12
|
||||||
|
|
||||||
// ADC macros and settings
|
// ADC macros and settings
|
||||||
|
|
||||||
|
@ -86,16 +87,16 @@
|
||||||
#define V_REG 3.3
|
#define V_REG 3.3
|
||||||
#endif
|
#endif
|
||||||
#ifndef VIN_R1
|
#ifndef VIN_R1
|
||||||
#define VIN_R1 294400.0 //TF RevB = 134.81V/V
|
#define VIN_R1 184.0 //TF since RevC = 185V/V
|
||||||
#endif
|
#endif
|
||||||
#ifndef VIN_R2
|
#ifndef VIN_R2
|
||||||
#define VIN_R2 2200.0
|
#define VIN_R2 1.0
|
||||||
#endif
|
#endif
|
||||||
#ifndef CURRENT_AMP_GAIN
|
#ifndef CURRENT_AMP_GAIN
|
||||||
#define CURRENT_AMP_GAIN 8.0
|
#define CURRENT_AMP_GAIN 0.003761 //Transfer Function [V/A] for ISB-425-A
|
||||||
#endif
|
#endif
|
||||||
#ifndef CURRENT_SHUNT_RES
|
#ifndef CURRENT_SHUNT_RES
|
||||||
#define CURRENT_SHUNT_RES 0.000415 //TF 300A/V
|
#define CURRENT_SHUNT_RES 1.000 // Unity gain so we use a single transfer function defined as CURRENT_AMP_GAIN
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
// Input voltage
|
// Input voltage
|
||||||
|
@ -111,6 +112,9 @@
|
||||||
// Voltage on ADC channel
|
// Voltage on ADC channel
|
||||||
#define ADC_VOLTS(ch) ((float)ADC_Value[ch] / 4096.0 * V_REG)
|
#define ADC_VOLTS(ch) ((float)ADC_Value[ch] / 4096.0 * V_REG)
|
||||||
|
|
||||||
|
// Gate driver power supply output voltage
|
||||||
|
#define GET_GATE_DRIVER_SUPPLY_VOLTAGE() ((float)ADC_VOLTS(ADC_IND_VOUT_GATE_DRV) * 11.0)
|
||||||
|
|
||||||
// Double samples in beginning and end for positive current measurement.
|
// Double samples in beginning and end for positive current measurement.
|
||||||
// Useful when the shunt sense traces have noise that causes offset.
|
// Useful when the shunt sense traces have noise that causes offset.
|
||||||
#ifndef CURR1_DOUBLE_SAMPLE
|
#ifndef CURR1_DOUBLE_SAMPLE
|
||||||
|
@ -131,8 +135,8 @@
|
||||||
#define HW_UART_GPIO_AF GPIO_AF_USART3
|
#define HW_UART_GPIO_AF GPIO_AF_USART3
|
||||||
#define HW_UART_TX_PORT GPIOB
|
#define HW_UART_TX_PORT GPIOB
|
||||||
#define HW_UART_TX_PIN 10
|
#define HW_UART_TX_PIN 10
|
||||||
#define HW_UART_RX_PORT GPIOB
|
#define HW_UART_RX_PORT GPIOD
|
||||||
#define HW_UART_RX_PIN 11
|
#define HW_UART_RX_PIN 9 //Freeing B11 for fault LED. PD9 not available in LQFT64 package
|
||||||
|
|
||||||
// ICU Peripheral for servo decoding
|
// ICU Peripheral for servo decoding
|
||||||
#define HW_USE_SERVO_TIM4
|
#define HW_USE_SERVO_TIM4
|
||||||
|
@ -199,10 +203,10 @@
|
||||||
#define HW_SPI_PIN_MISO 11
|
#define HW_SPI_PIN_MISO 11
|
||||||
|
|
||||||
// Measurement macros
|
// Measurement macros
|
||||||
#define ADC_V_L1 ADC_Value[ADC_IND_SENS1]
|
#define ADC_V_L1 (ADC_Value[ADC_IND_SENS1]-2048) //phase voltages are centered in 1.65V
|
||||||
#define ADC_V_L2 ADC_Value[ADC_IND_SENS2]
|
#define ADC_V_L2 (ADC_Value[ADC_IND_SENS2]-2048)
|
||||||
#define ADC_V_L3 ADC_Value[ADC_IND_SENS3]
|
#define ADC_V_L3 (ADC_Value[ADC_IND_SENS3]-2048)
|
||||||
#define ADC_V_ZERO (ADC_Value[ADC_IND_VIN_SENS] / 2)
|
#define ADC_V_ZERO 0 //(ADC_Value[ADC_IND_VIN_SENS] / 2)
|
||||||
|
|
||||||
// Macros
|
// Macros
|
||||||
#define READ_HALL1() palReadPad(HW_HALL_ENC_GPIO1, HW_HALL_ENC_PIN1)
|
#define READ_HALL1() palReadPad(HW_HALL_ENC_GPIO1, HW_HALL_ENC_PIN1)
|
||||||
|
@ -210,7 +214,10 @@
|
||||||
#define READ_HALL3() palReadPad(HW_HALL_ENC_GPIO3, HW_HALL_ENC_PIN3)
|
#define READ_HALL3() palReadPad(HW_HALL_ENC_GPIO3, HW_HALL_ENC_PIN3)
|
||||||
|
|
||||||
// Override dead time. See the stm32f4 reference manual for calculating this value.
|
// Override dead time. See the stm32f4 reference manual for calculating this value.
|
||||||
#define HW_DEAD_TIME_VALUE 181
|
#define HW_DEAD_TIME_NSEC 1400.0
|
||||||
|
#define HW_GATE_DRIVER_SUPPLY_MAX_VOLTAGE 16.0
|
||||||
|
#define HW_GATE_DRIVER_SUPPLY_MIN_VOLTAGE 14.0
|
||||||
|
|
||||||
|
|
||||||
// Default setting overrides
|
// Default setting overrides
|
||||||
#ifndef MCCONF_DEFAULT_MOTOR_TYPE
|
#ifndef MCCONF_DEFAULT_MOTOR_TYPE
|
||||||
|
@ -220,17 +227,24 @@
|
||||||
#define MCCONF_FOC_SAMPLE_V0_V7 true // Run control loop in both v0 and v7 (requires phase shunts)
|
#define MCCONF_FOC_SAMPLE_V0_V7 true // Run control loop in both v0 and v7 (requires phase shunts)
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
// Setting limits (TODO: Configure these)
|
// Execute FOC loop once every "FOC_CONTROL_LOOP_FREQ_DIVIDER" ADC ISR calls
|
||||||
//#define HW_LIM_CURRENT -100.0, 100.0
|
#define FOC_CONTROL_LOOP_FREQ_DIVIDER 1
|
||||||
//#define HW_LIM_CURRENT_IN -100.0, 100.0
|
|
||||||
//#define HW_LIM_CURRENT_ABS 0.0, 150.0
|
// Setting limits
|
||||||
//#define HW_LIM_VIN 6.0, 57.0
|
#define HW_LIM_CURRENT -200.0, 200.0
|
||||||
//#define HW_LIM_ERPM -200e3, 200e3
|
#define HW_LIM_CURRENT_IN -100.0, 100.0
|
||||||
//#define HW_LIM_DUTY_MIN 0.0, 0.1
|
#define HW_LIM_CURRENT_ABS 0.0, 230.0
|
||||||
//#define HW_LIM_DUTY_MAX 0.0, 1.0
|
#define HW_LIM_VIN 6.0, 85.0
|
||||||
//#define HW_LIM_TEMP_FET -40.0, 110.0
|
#define HW_LIM_ERPM -100e3, 100e3
|
||||||
|
#define HW_LIM_DUTY_MIN 0.0, 0.1
|
||||||
|
#define HW_LIM_DUTY_MAX 0.0, 1.0
|
||||||
|
#define HW_LIM_TEMP_FET -40.0, 110.0
|
||||||
|
#define HW_LIM_FOC_CTRL_LOOP_FREQ 10000.0, 30000.0 //at around 38kHz the RTOS starts crashing (26us FOC ISR)
|
||||||
|
|
||||||
|
|
||||||
// HW-specific functions
|
// HW-specific functions
|
||||||
void hw_palta_reset_oc(void);
|
char hw_palta_configure_FPGA(void);
|
||||||
|
void hw_palta_DAC1_setdata(uint16_t data);
|
||||||
|
void hw_palta_DAC2_setdata(uint16_t data);
|
||||||
|
|
||||||
#endif /* HW_PALTA_H_ */
|
#endif /* HW_PALTA_H_ */
|
||||||
|
|
|
@ -0,0 +1 @@
|
||||||
|
const unsigned char FPGA_bitstream[71338] = {0};
|
|
@ -59,3 +59,16 @@ CH_IRQ_HANDLER(TIM8_CC_IRQHandler) {
|
||||||
TIM_ClearITPendingBit(TIM8, TIM_IT_CC1);
|
TIM_ClearITPendingBit(TIM8, TIM_IT_CC1);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
CH_IRQ_HANDLER(PVD_IRQHandler) {
|
||||||
|
if (EXTI_GetITStatus(EXTI_Line16) != RESET) {
|
||||||
|
// Log the fault. Supply voltage dropped below 2.9V,
|
||||||
|
// could corrupt an ongoing flash programming
|
||||||
|
mc_interface_fault_stop(FAULT_CODE_MCU_UNDER_VOLTAGE);
|
||||||
|
|
||||||
|
// Clear the PVD pending bit
|
||||||
|
EXTI_ClearITPendingBit(EXTI_Line16);
|
||||||
|
EXTI_ClearFlag(EXTI_Line16);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
|
@ -74,8 +74,8 @@ static THD_FUNCTION(led_thread, arg) {
|
||||||
for (int i = 0;i < 50;i++) {
|
for (int i = 0;i < 50;i++) {
|
||||||
scale -= 0.02;
|
scale -= 0.02;
|
||||||
uint32_t color = scale_color(COLOR_RED, scale);
|
uint32_t color = scale_color(COLOR_RED, scale);
|
||||||
for (int i = 0;i < (int)fault;i++) {
|
for (int j = 0;j < (int)fault;j++) {
|
||||||
set_led_wrapper(i, color);
|
set_led_wrapper(j, color);
|
||||||
}
|
}
|
||||||
chThdSleepMilliseconds(10);
|
chThdSleepMilliseconds(10);
|
||||||
}
|
}
|
||||||
|
|
16
main.c
16
main.c
|
@ -171,10 +171,19 @@ static THD_FUNCTION(timer_thread, arg) {
|
||||||
|
|
||||||
for(;;) {
|
for(;;) {
|
||||||
packet_timerfunc();
|
packet_timerfunc();
|
||||||
|
timeout_feed_WDT(THREAD_TIMER);
|
||||||
chThdSleepMilliseconds(1);
|
chThdSleepMilliseconds(1);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/* When assertions enabled halve PWM frequency. The control loop ISR runs 40% slower */
|
||||||
|
void assert_failed(uint8_t* file, uint32_t line) {
|
||||||
|
commands_printf("Wrong parameters value: file %s on line %d\r\n", file, line);
|
||||||
|
mc_interface_release_motor();
|
||||||
|
while(1)
|
||||||
|
chThdSleepMilliseconds(1);
|
||||||
|
}
|
||||||
|
|
||||||
int main(void) {
|
int main(void) {
|
||||||
halInit();
|
halInit();
|
||||||
chSysInit();
|
chSysInit();
|
||||||
|
@ -197,6 +206,7 @@ int main(void) {
|
||||||
|
|
||||||
mc_configuration mcconf;
|
mc_configuration mcconf;
|
||||||
conf_general_read_mc_configuration(&mcconf);
|
conf_general_read_mc_configuration(&mcconf);
|
||||||
|
|
||||||
mc_interface_init(&mcconf);
|
mc_interface_init(&mcconf);
|
||||||
|
|
||||||
commands_init();
|
commands_init();
|
||||||
|
@ -228,9 +238,6 @@ int main(void) {
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
timeout_init();
|
|
||||||
timeout_configure(appconf.timeout_msec, appconf.timeout_brake_current);
|
|
||||||
|
|
||||||
#if WS2811_ENABLE
|
#if WS2811_ENABLE
|
||||||
ws2811_init();
|
ws2811_init();
|
||||||
#if !WS2811_TEST
|
#if !WS2811_TEST
|
||||||
|
@ -306,6 +313,9 @@ int main(void) {
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
timeout_init();
|
||||||
|
timeout_configure(appconf.timeout_msec, appconf.timeout_brake_current);
|
||||||
|
|
||||||
for(;;) {
|
for(;;) {
|
||||||
chThdSleepMilliseconds(10);
|
chThdSleepMilliseconds(10);
|
||||||
|
|
||||||
|
|
|
@ -65,6 +65,7 @@ static volatile float m_watt_seconds_charged;
|
||||||
static volatile float m_position_set;
|
static volatile float m_position_set;
|
||||||
static volatile float m_temp_fet;
|
static volatile float m_temp_fet;
|
||||||
static volatile float m_temp_motor;
|
static volatile float m_temp_motor;
|
||||||
|
static volatile float m_gate_driver_voltage;
|
||||||
|
|
||||||
// Sampling variables
|
// Sampling variables
|
||||||
#define ADC_SAMPLE_MAX_LEN 2000
|
#define ADC_SAMPLE_MAX_LEN 2000
|
||||||
|
@ -78,6 +79,7 @@ __attribute__((section(".ram4"))) static volatile uint8_t m_status_samples[ADC_S
|
||||||
__attribute__((section(".ram4"))) static volatile int16_t m_curr_fir_samples[ADC_SAMPLE_MAX_LEN];
|
__attribute__((section(".ram4"))) static volatile int16_t m_curr_fir_samples[ADC_SAMPLE_MAX_LEN];
|
||||||
__attribute__((section(".ram4"))) static volatile int16_t m_f_sw_samples[ADC_SAMPLE_MAX_LEN];
|
__attribute__((section(".ram4"))) static volatile int16_t m_f_sw_samples[ADC_SAMPLE_MAX_LEN];
|
||||||
__attribute__((section(".ram4"))) static volatile int8_t m_phase_samples[ADC_SAMPLE_MAX_LEN];
|
__attribute__((section(".ram4"))) static volatile int8_t m_phase_samples[ADC_SAMPLE_MAX_LEN];
|
||||||
|
|
||||||
static volatile int m_sample_len;
|
static volatile int m_sample_len;
|
||||||
static volatile int m_sample_int;
|
static volatile int m_sample_int;
|
||||||
static volatile debug_sampling_mode m_sample_mode;
|
static volatile debug_sampling_mode m_sample_mode;
|
||||||
|
@ -122,6 +124,7 @@ void mc_interface_init(mc_configuration *configuration) {
|
||||||
m_last_adc_duration_sample = 0.0;
|
m_last_adc_duration_sample = 0.0;
|
||||||
m_temp_fet = 0.0;
|
m_temp_fet = 0.0;
|
||||||
m_temp_motor = 0.0;
|
m_temp_motor = 0.0;
|
||||||
|
m_gate_driver_voltage = 0.0;
|
||||||
|
|
||||||
m_sample_len = 1000;
|
m_sample_len = 1000;
|
||||||
m_sample_int = 1;
|
m_sample_int = 1;
|
||||||
|
@ -343,6 +346,10 @@ const char* mc_interface_fault_to_string(mc_fault_code fault) {
|
||||||
case FAULT_CODE_ABS_OVER_CURRENT: return "FAULT_CODE_ABS_OVER_CURRENT"; break;
|
case FAULT_CODE_ABS_OVER_CURRENT: return "FAULT_CODE_ABS_OVER_CURRENT"; break;
|
||||||
case FAULT_CODE_OVER_TEMP_FET: return "FAULT_CODE_OVER_TEMP_FET"; break;
|
case FAULT_CODE_OVER_TEMP_FET: return "FAULT_CODE_OVER_TEMP_FET"; break;
|
||||||
case FAULT_CODE_OVER_TEMP_MOTOR: return "FAULT_CODE_OVER_TEMP_MOTOR"; break;
|
case FAULT_CODE_OVER_TEMP_MOTOR: return "FAULT_CODE_OVER_TEMP_MOTOR"; break;
|
||||||
|
case FAULT_CODE_GATE_DRIVER_OVER_VOLTAGE: return "FAULT_CODE_GATE_DRIVER_OVER_VOLTAGE"; break;
|
||||||
|
case FAULT_CODE_GATE_DRIVER_UNDER_VOLTAGE: return "FAULT_CODE_GATE_DRIVER_UNDER_VOLTAGE"; break;
|
||||||
|
case FAULT_CODE_MCU_UNDER_VOLTAGE: return "FAULT_CODE_MCU_UNDER_VOLTAGE"; break;
|
||||||
|
case FAULT_CODE_BOOTING_FROM_WATCHDOG_RESET: return "FAULT_CODE_BOOTING_FROM_WATCHDOG_RESET"; break;
|
||||||
default: return "FAULT_UNKNOWN"; break;
|
default: return "FAULT_UNKNOWN"; break;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -1190,6 +1197,7 @@ void mc_interface_fault_stop(mc_fault_code fault) {
|
||||||
fdata.current = mc_interface_get_tot_current();
|
fdata.current = mc_interface_get_tot_current();
|
||||||
fdata.current_filtered = mc_interface_get_tot_current_filtered();
|
fdata.current_filtered = mc_interface_get_tot_current_filtered();
|
||||||
fdata.voltage = GET_INPUT_VOLTAGE();
|
fdata.voltage = GET_INPUT_VOLTAGE();
|
||||||
|
fdata.gate_driver_voltage = m_gate_driver_voltage;
|
||||||
fdata.duty = mc_interface_get_duty_cycle_now();
|
fdata.duty = mc_interface_get_duty_cycle_now();
|
||||||
fdata.rpm = mc_interface_get_rpm();
|
fdata.rpm = mc_interface_get_rpm();
|
||||||
fdata.tacho = mc_interface_get_tachometer_value(false);
|
fdata.tacho = mc_interface_get_tachometer_value(false);
|
||||||
|
@ -1303,6 +1311,16 @@ void mc_interface_mc_timer_isr(void) {
|
||||||
mc_interface_fault_stop(FAULT_CODE_DRV);
|
mc_interface_fault_stop(FAULT_CODE_DRV);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#ifdef HW_VERSION_PALTA
|
||||||
|
if( m_gate_driver_voltage > HW_GATE_DRIVER_SUPPLY_MAX_VOLTAGE) {
|
||||||
|
mc_interface_fault_stop(FAULT_CODE_GATE_DRIVER_OVER_VOLTAGE);
|
||||||
|
}
|
||||||
|
|
||||||
|
if( m_gate_driver_voltage < HW_GATE_DRIVER_SUPPLY_MIN_VOLTAGE) {
|
||||||
|
mc_interface_fault_stop(FAULT_CODE_GATE_DRIVER_UNDER_VOLTAGE);
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
// Watt and ah counters
|
// Watt and ah counters
|
||||||
const float f_samp = mc_interface_get_sampling_frequency_now();
|
const float f_samp = mc_interface_get_sampling_frequency_now();
|
||||||
if (fabsf(current) > 1.0) {
|
if (fabsf(current) > 1.0) {
|
||||||
|
@ -1498,6 +1516,9 @@ static void update_override_limits(volatile mc_configuration *conf) {
|
||||||
|
|
||||||
UTILS_LP_FAST(m_temp_fet, NTC_TEMP(ADC_IND_TEMP_MOS), 0.1);
|
UTILS_LP_FAST(m_temp_fet, NTC_TEMP(ADC_IND_TEMP_MOS), 0.1);
|
||||||
UTILS_LP_FAST(m_temp_motor, NTC_TEMP_MOTOR(conf->m_ntc_motor_beta), 0.1);
|
UTILS_LP_FAST(m_temp_motor, NTC_TEMP_MOTOR(conf->m_ntc_motor_beta), 0.1);
|
||||||
|
#ifdef HW_VERSION_PALTA
|
||||||
|
UTILS_LP_FAST(m_gate_driver_voltage, GET_GATE_DRIVER_SUPPLY_VOLTAGE(), 0.01);
|
||||||
|
#endif
|
||||||
|
|
||||||
const float l_current_min_tmp = conf->l_current_min * conf->l_current_min_scale;
|
const float l_current_min_tmp = conf->l_current_min * conf->l_current_min_scale;
|
||||||
const float l_current_max_tmp = conf->l_current_max * conf->l_current_max_scale;
|
const float l_current_max_tmp = conf->l_current_max * conf->l_current_max_scale;
|
||||||
|
|
|
@ -91,8 +91,9 @@ extern volatile uint16_t ADC_Value[];
|
||||||
extern volatile int ADC_curr_norm_value[];
|
extern volatile int ADC_curr_norm_value[];
|
||||||
|
|
||||||
// Common fixed parameters
|
// Common fixed parameters
|
||||||
#ifndef HW_DEAD_TIME_VALUE
|
#ifndef HW_DEAD_TIME_NSEC
|
||||||
#define HW_DEAD_TIME_VALUE 60 // Dead time
|
#define HW_DEAD_TIME_NSEC 360.0 // Dead time
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
|
||||||
#endif /* MC_INTERFACE_H_ */
|
#endif /* MC_INTERFACE_H_ */
|
||||||
|
|
13
mcpwm.c
13
mcpwm.c
|
@ -30,6 +30,7 @@
|
||||||
#include "utils.h"
|
#include "utils.h"
|
||||||
#include "ledpwm.h"
|
#include "ledpwm.h"
|
||||||
#include "terminal.h"
|
#include "terminal.h"
|
||||||
|
#include "timeout.h"
|
||||||
#include "encoder.h"
|
#include "encoder.h"
|
||||||
|
|
||||||
// Structs
|
// Structs
|
||||||
|
@ -263,7 +264,7 @@ void mcpwm_init(volatile mc_configuration *configuration) {
|
||||||
TIM_BDTRInitStructure.TIM_OSSRState = TIM_OSSRState_Enable;
|
TIM_BDTRInitStructure.TIM_OSSRState = TIM_OSSRState_Enable;
|
||||||
TIM_BDTRInitStructure.TIM_OSSIState = TIM_OSSIState_Enable;
|
TIM_BDTRInitStructure.TIM_OSSIState = TIM_OSSIState_Enable;
|
||||||
TIM_BDTRInitStructure.TIM_LOCKLevel = TIM_LOCKLevel_OFF;
|
TIM_BDTRInitStructure.TIM_LOCKLevel = TIM_LOCKLevel_OFF;
|
||||||
TIM_BDTRInitStructure.TIM_DeadTime = HW_DEAD_TIME_VALUE;
|
TIM_BDTRInitStructure.TIM_DeadTime = conf_general_calculate_deadtime(HW_DEAD_TIME_NSEC, SYSTEM_CORE_CLOCK);
|
||||||
TIM_BDTRInitStructure.TIM_Break = TIM_Break_Disable;
|
TIM_BDTRInitStructure.TIM_Break = TIM_Break_Disable;
|
||||||
TIM_BDTRInitStructure.TIM_BreakPolarity = TIM_BreakPolarity_High;
|
TIM_BDTRInitStructure.TIM_BreakPolarity = TIM_BreakPolarity_High;
|
||||||
TIM_BDTRInitStructure.TIM_AutomaticOutput = TIM_AutomaticOutput_Disable;
|
TIM_BDTRInitStructure.TIM_AutomaticOutput = TIM_AutomaticOutput_Disable;
|
||||||
|
@ -468,11 +469,9 @@ void mcpwm_init(volatile mc_configuration *configuration) {
|
||||||
chThdCreateStatic(timer_thread_wa, sizeof(timer_thread_wa), NORMALPRIO, timer_thread, NULL);
|
chThdCreateStatic(timer_thread_wa, sizeof(timer_thread_wa), NORMALPRIO, timer_thread, NULL);
|
||||||
chThdCreateStatic(rpm_thread_wa, sizeof(rpm_thread_wa), NORMALPRIO, rpm_thread, NULL);
|
chThdCreateStatic(rpm_thread_wa, sizeof(rpm_thread_wa), NORMALPRIO, rpm_thread, NULL);
|
||||||
|
|
||||||
// WWDG configuration
|
// Check if the system has resumed from IWDG reset
|
||||||
RCC_APB1PeriphClockCmd(RCC_APB1Periph_WWDG, ENABLE);
|
if (timeout_had_IWDG_reset())
|
||||||
WWDG_SetPrescaler(WWDG_Prescaler_1);
|
mc_interface_fault_stop(FAULT_CODE_BOOTING_FROM_WATCHDOG_RESET);
|
||||||
WWDG_SetWindowValue(255);
|
|
||||||
WWDG_Enable(100);
|
|
||||||
|
|
||||||
// Reset tachometers again
|
// Reset tachometers again
|
||||||
tachometer = 0;
|
tachometer = 0;
|
||||||
|
@ -1748,7 +1747,7 @@ void mcpwm_adc_int_handler(void *p, uint32_t flags) {
|
||||||
update_timer_attempt();
|
update_timer_attempt();
|
||||||
|
|
||||||
// Reset the watchdog
|
// Reset the watchdog
|
||||||
WWDG_SetCounter(100);
|
timeout_feed_WDT(THREAD_MCPWM);
|
||||||
|
|
||||||
const float input_voltage = GET_INPUT_VOLTAGE();
|
const float input_voltage = GET_INPUT_VOLTAGE();
|
||||||
int ph1, ph2, ph3;
|
int ph1, ph2, ph3;
|
||||||
|
|
67
mcpwm_foc.c
67
mcpwm_foc.c
|
@ -279,7 +279,7 @@ void mcpwm_foc_init(volatile mc_configuration *configuration) {
|
||||||
TIM_BDTRInitStructure.TIM_OSSRState = TIM_OSSRState_Enable;
|
TIM_BDTRInitStructure.TIM_OSSRState = TIM_OSSRState_Enable;
|
||||||
TIM_BDTRInitStructure.TIM_OSSIState = TIM_OSSIState_Enable;
|
TIM_BDTRInitStructure.TIM_OSSIState = TIM_OSSIState_Enable;
|
||||||
TIM_BDTRInitStructure.TIM_LOCKLevel = TIM_LOCKLevel_OFF;
|
TIM_BDTRInitStructure.TIM_LOCKLevel = TIM_LOCKLevel_OFF;
|
||||||
TIM_BDTRInitStructure.TIM_DeadTime = HW_DEAD_TIME_VALUE;
|
TIM_BDTRInitStructure.TIM_DeadTime = conf_general_calculate_deadtime(HW_DEAD_TIME_NSEC, SYSTEM_CORE_CLOCK);
|
||||||
TIM_BDTRInitStructure.TIM_Break = TIM_Break_Disable;
|
TIM_BDTRInitStructure.TIM_Break = TIM_Break_Disable;
|
||||||
TIM_BDTRInitStructure.TIM_BreakPolarity = TIM_BreakPolarity_High;
|
TIM_BDTRInitStructure.TIM_BreakPolarity = TIM_BreakPolarity_High;
|
||||||
TIM_BDTRInitStructure.TIM_AutomaticOutput = TIM_AutomaticOutput_Disable;
|
TIM_BDTRInitStructure.TIM_AutomaticOutput = TIM_AutomaticOutput_Disable;
|
||||||
|
@ -449,11 +449,9 @@ void mcpwm_foc_init(volatile mc_configuration *configuration) {
|
||||||
timer_thd_stop = false;
|
timer_thd_stop = false;
|
||||||
chThdCreateStatic(timer_thread_wa, sizeof(timer_thread_wa), NORMALPRIO, timer_thread, NULL);
|
chThdCreateStatic(timer_thread_wa, sizeof(timer_thread_wa), NORMALPRIO, timer_thread, NULL);
|
||||||
|
|
||||||
// WWDG configuration
|
// Check if the system has resumed from IWDG reset
|
||||||
RCC_APB1PeriphClockCmd(RCC_APB1Periph_WWDG, ENABLE);
|
if (timeout_had_IWDG_reset())
|
||||||
WWDG_SetPrescaler(WWDG_Prescaler_1);
|
mc_interface_fault_stop(FAULT_CODE_BOOTING_FROM_WATCHDOG_RESET);
|
||||||
WWDG_SetWindowValue(255);
|
|
||||||
WWDG_Enable(100);
|
|
||||||
|
|
||||||
m_init_done = true;
|
m_init_done = true;
|
||||||
}
|
}
|
||||||
|
@ -1044,9 +1042,9 @@ void mcpwm_foc_encoder_detect(float current, bool print, float *offset, float *r
|
||||||
for (int i = 0; i < it_rat; i++) {
|
for (int i = 0; i < it_rat; i++) {
|
||||||
float phase_old = m_phase_now_encoder;
|
float phase_old = m_phase_now_encoder;
|
||||||
float phase_ovr_tmp = m_phase_now_override;
|
float phase_ovr_tmp = m_phase_now_override;
|
||||||
for (float i = phase_ovr_tmp; i < phase_ovr_tmp + (2.0 / 3.0) * M_PI;
|
for (float j = phase_ovr_tmp; j < phase_ovr_tmp + (2.0 / 3.0) * M_PI;
|
||||||
i += (2.0 * M_PI) / 500.0) {
|
j += (2.0 * M_PI) / 500.0) {
|
||||||
m_phase_now_override = i;
|
m_phase_now_override = j;
|
||||||
chThdSleepMilliseconds(1);
|
chThdSleepMilliseconds(1);
|
||||||
}
|
}
|
||||||
utils_norm_angle_rad((float*)&m_phase_now_override);
|
utils_norm_angle_rad((float*)&m_phase_now_override);
|
||||||
|
@ -1072,9 +1070,9 @@ void mcpwm_foc_encoder_detect(float current, bool print, float *offset, float *r
|
||||||
for (int i = 0; i < it_rat; i++) {
|
for (int i = 0; i < it_rat; i++) {
|
||||||
float phase_old = m_phase_now_encoder;
|
float phase_old = m_phase_now_encoder;
|
||||||
float phase_ovr_tmp = m_phase_now_override;
|
float phase_ovr_tmp = m_phase_now_override;
|
||||||
for (float i = phase_ovr_tmp; i > phase_ovr_tmp - (2.0 / 3.0) * M_PI;
|
for (float j = phase_ovr_tmp; j > phase_ovr_tmp - (2.0 / 3.0) * M_PI;
|
||||||
i -= (2.0 * M_PI) / 500.0) {
|
j -= (2.0 * M_PI) / 500.0) {
|
||||||
m_phase_now_override = i;
|
m_phase_now_override = j;
|
||||||
chThdSleepMilliseconds(1);
|
chThdSleepMilliseconds(1);
|
||||||
}
|
}
|
||||||
utils_norm_angle_rad((float*)&m_phase_now_override);
|
utils_norm_angle_rad((float*)&m_phase_now_override);
|
||||||
|
@ -1133,14 +1131,14 @@ void mcpwm_foc_encoder_detect(float current, bool print, float *offset, float *r
|
||||||
|
|
||||||
chThdSleepMilliseconds(100);
|
chThdSleepMilliseconds(100);
|
||||||
|
|
||||||
float diff = utils_angle_difference_rad(m_phase_now_encoder, m_phase_now_override);
|
float angle_diff = utils_angle_difference_rad(m_phase_now_encoder, m_phase_now_override);
|
||||||
float s, c;
|
float s, c;
|
||||||
sincosf(diff, &s, &c);
|
sincosf(angle_diff, &s, &c);
|
||||||
s_sum += s;
|
s_sum += s;
|
||||||
c_sum += c;
|
c_sum += c;
|
||||||
|
|
||||||
if (print) {
|
if (print) {
|
||||||
commands_printf("%.2f", (double)(diff * 180.0 / M_PI));
|
commands_printf("%.2f", (double)(angle_diff * 180.0 / M_PI));
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -1155,14 +1153,14 @@ void mcpwm_foc_encoder_detect(float current, bool print, float *offset, float *r
|
||||||
|
|
||||||
chThdSleepMilliseconds(100);
|
chThdSleepMilliseconds(100);
|
||||||
|
|
||||||
float diff = utils_angle_difference_rad(m_phase_now_encoder, m_phase_now_override);
|
float angle_diff = utils_angle_difference_rad(m_phase_now_encoder, m_phase_now_override);
|
||||||
float s, c;
|
float s, c;
|
||||||
sincosf(diff, &s, &c);
|
sincosf(angle_diff, &s, &c);
|
||||||
s_sum += s;
|
s_sum += s;
|
||||||
c_sum += c;
|
c_sum += c;
|
||||||
|
|
||||||
if (print) {
|
if (print) {
|
||||||
commands_printf("%.2f", (double)(diff * 180.0 / M_PI));
|
commands_printf("%.2f", (double)(angle_diff * 180.0 / M_PI));
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -1419,9 +1417,7 @@ bool mcpwm_foc_measure_res_ind(float *res, float *ind) {
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
if (i_last < 0.01) {
|
i_last = (m_conf->l_current_max / 2.0);
|
||||||
i_last = (m_conf->l_current_max / 2.0);
|
|
||||||
}
|
|
||||||
|
|
||||||
*res = mcpwm_foc_measure_resistance(i_last, 200);
|
*res = mcpwm_foc_measure_resistance(i_last, 200);
|
||||||
*ind = mcpwm_foc_measure_inductance_current(i_last, 200, 0);
|
*ind = mcpwm_foc_measure_inductance_current(i_last, 200, 0);
|
||||||
|
@ -1477,8 +1473,8 @@ bool mcpwm_foc_hall_detect(float current, uint8_t *hall_table) {
|
||||||
|
|
||||||
// Forwards
|
// Forwards
|
||||||
for (int i = 0;i < 3;i++) {
|
for (int i = 0;i < 3;i++) {
|
||||||
for (int i = 0;i < 360;i++) {
|
for (int j = 0;j < 360;j++) {
|
||||||
m_phase_now_override = (float)i * M_PI / 180.0;
|
m_phase_now_override = (float)j * M_PI / 180.0;
|
||||||
chThdSleepMilliseconds(5);
|
chThdSleepMilliseconds(5);
|
||||||
|
|
||||||
int hall = read_hall();
|
int hall = read_hall();
|
||||||
|
@ -1492,8 +1488,8 @@ bool mcpwm_foc_hall_detect(float current, uint8_t *hall_table) {
|
||||||
|
|
||||||
// Reverse
|
// Reverse
|
||||||
for (int i = 0;i < 3;i++) {
|
for (int i = 0;i < 3;i++) {
|
||||||
for (int i = 360;i >= 0;i--) {
|
for (int j = 360;j >= 0;j--) {
|
||||||
m_phase_now_override = (float)i * M_PI / 180.0;
|
m_phase_now_override = (float)j * M_PI / 180.0;
|
||||||
chThdSleepMilliseconds(5);
|
chThdSleepMilliseconds(5);
|
||||||
|
|
||||||
int hall = read_hall();
|
int hall = read_hall();
|
||||||
|
@ -1568,6 +1564,12 @@ void mcpwm_foc_adc_int_handler(void *p, uint32_t flags) {
|
||||||
(void)p;
|
(void)p;
|
||||||
(void)flags;
|
(void)flags;
|
||||||
|
|
||||||
|
static int skip = 0;
|
||||||
|
if (++skip == FOC_CONTROL_LOOP_FREQ_DIVIDER)
|
||||||
|
skip = 0;
|
||||||
|
else
|
||||||
|
return;
|
||||||
|
|
||||||
TIM12->CNT = 0;
|
TIM12->CNT = 0;
|
||||||
|
|
||||||
bool is_v7 = !(TIM1->CR1 & TIM_CR1_DIR);
|
bool is_v7 = !(TIM1->CR1 & TIM_CR1_DIR);
|
||||||
|
@ -1585,7 +1587,7 @@ void mcpwm_foc_adc_int_handler(void *p, uint32_t flags) {
|
||||||
}
|
}
|
||||||
|
|
||||||
// Reset the watchdog
|
// Reset the watchdog
|
||||||
WWDG_SetCounter(100);
|
timeout_feed_WDT(THREAD_MCPWM);
|
||||||
|
|
||||||
#ifdef AD2S1205_SAMPLE_GPIO
|
#ifdef AD2S1205_SAMPLE_GPIO
|
||||||
// force a position sample in the AD2S1205 resolver IC (falling edge)
|
// force a position sample in the AD2S1205 resolver IC (falling edge)
|
||||||
|
@ -1950,6 +1952,19 @@ void mcpwm_foc_adc_int_handler(void *p, uint32_t flags) {
|
||||||
float c, s;
|
float c, s;
|
||||||
utils_fast_sincos_better(m_motor_state.phase, &s, &c);
|
utils_fast_sincos_better(m_motor_state.phase, &s, &c);
|
||||||
|
|
||||||
|
#ifdef HW_VERSION_PALTA
|
||||||
|
// rotate alpha-beta 30 degrees to compensate for line-to-line phase voltage sensing
|
||||||
|
float x_tmp = m_motor_state.v_alpha;
|
||||||
|
float y_tmp = m_motor_state.v_beta;
|
||||||
|
|
||||||
|
m_motor_state.v_alpha = x_tmp*COS_MINUS_30_DEG - y_tmp*SIN_MINUS_30_DEG;
|
||||||
|
m_motor_state.v_beta = x_tmp*SIN_MINUS_30_DEG + y_tmp*COS_MINUS_30_DEG;
|
||||||
|
|
||||||
|
// compensate voltage amplitude
|
||||||
|
m_motor_state.v_alpha *= ONE_BY_SQRT3;
|
||||||
|
m_motor_state.v_beta *= ONE_BY_SQRT3;
|
||||||
|
#endif
|
||||||
|
|
||||||
// Park transform
|
// Park transform
|
||||||
float vd_tmp = c * m_motor_state.v_alpha + s * m_motor_state.v_beta;
|
float vd_tmp = c * m_motor_state.v_alpha + s * m_motor_state.v_beta;
|
||||||
float vq_tmp = c * m_motor_state.v_beta - s * m_motor_state.v_alpha;
|
float vq_tmp = c * m_motor_state.v_beta - s * m_motor_state.v_alpha;
|
||||||
|
|
|
@ -93,8 +93,8 @@
|
||||||
#define STM32_I2SSRC STM32_I2SSRC_CKIN
|
#define STM32_I2SSRC STM32_I2SSRC_CKIN
|
||||||
#define STM32_PLLI2SN_VALUE 192
|
#define STM32_PLLI2SN_VALUE 192
|
||||||
#define STM32_PLLI2SR_VALUE 5
|
#define STM32_PLLI2SR_VALUE 5
|
||||||
#define STM32_PVD_ENABLE FALSE
|
#define STM32_PVD_ENABLE TRUE
|
||||||
#define STM32_PLS STM32_PLS_LEV0
|
#define STM32_PLS STM32_PLS_LEV7
|
||||||
#define STM32_BKPRAM_ENABLE FALSE
|
#define STM32_BKPRAM_ENABLE FALSE
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
|
|
@ -13,12 +13,6 @@
|
||||||
#include "stm32f4xx_syscfg.h"
|
#include "stm32f4xx_syscfg.h"
|
||||||
#include "stm32f4xx_tim.h"
|
#include "stm32f4xx_tim.h"
|
||||||
#include "stm32f4xx_wwdg.h"
|
#include "stm32f4xx_wwdg.h"
|
||||||
|
#include "stm32f4xx_iwdg.h"
|
||||||
#ifdef USE_FULL_ASSERT
|
|
||||||
#define assert_param(expr) ((expr) ? (void)0 : assert_failed((uint8_t *)__FILE__, __LINE__))
|
|
||||||
void assert_failed(uint8_t* file, uint32_t line);
|
|
||||||
#else
|
|
||||||
#define assert_param(expr) ((void)0)
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
|
@ -121,6 +121,9 @@ void terminal_process_string(char *str) {
|
||||||
commands_printf("Current : %.1f", (double)fault_vec[i].current);
|
commands_printf("Current : %.1f", (double)fault_vec[i].current);
|
||||||
commands_printf("Current filtered : %.1f", (double)fault_vec[i].current_filtered);
|
commands_printf("Current filtered : %.1f", (double)fault_vec[i].current_filtered);
|
||||||
commands_printf("Voltage : %.2f", (double)fault_vec[i].voltage);
|
commands_printf("Voltage : %.2f", (double)fault_vec[i].voltage);
|
||||||
|
#ifdef HW_VERSION_PALTA
|
||||||
|
commands_printf("Gate drv voltage : %.2f", (double)fault_vec[i].gate_driver_voltage);
|
||||||
|
#endif
|
||||||
commands_printf("Duty : %.3f", (double)fault_vec[i].duty);
|
commands_printf("Duty : %.3f", (double)fault_vec[i].duty);
|
||||||
commands_printf("RPM : %.1f", (double)fault_vec[i].rpm);
|
commands_printf("RPM : %.1f", (double)fault_vec[i].rpm);
|
||||||
commands_printf("Tacho : %d", fault_vec[i].tacho);
|
commands_printf("Tacho : %d", fault_vec[i].tacho);
|
||||||
|
@ -178,6 +181,9 @@ void terminal_process_string(char *str) {
|
||||||
commands_printf("Current 2 sample: %u\n", current2_samp);
|
commands_printf("Current 2 sample: %u\n", current2_samp);
|
||||||
} else if (strcmp(argv[0], "volt") == 0) {
|
} else if (strcmp(argv[0], "volt") == 0) {
|
||||||
commands_printf("Input voltage: %.2f\n", (double)GET_INPUT_VOLTAGE());
|
commands_printf("Input voltage: %.2f\n", (double)GET_INPUT_VOLTAGE());
|
||||||
|
#ifdef HW_VERSION_PALTA
|
||||||
|
commands_printf("Gate driver power supply output voltage: %.2f\n", (double)GET_GATE_DRIVER_SUPPLY_VOLTAGE());
|
||||||
|
#endif
|
||||||
} else if (strcmp(argv[0], "param_detect") == 0) {
|
} else if (strcmp(argv[0], "param_detect") == 0) {
|
||||||
// Use COMM_MODE_DELAY and try to figure out the motor parameters.
|
// Use COMM_MODE_DELAY and try to figure out the motor parameters.
|
||||||
if (argc == 4) {
|
if (argc == 4) {
|
||||||
|
|
143
timeout.c
143
timeout.c
|
@ -19,23 +19,41 @@
|
||||||
|
|
||||||
#include "timeout.h"
|
#include "timeout.h"
|
||||||
#include "mc_interface.h"
|
#include "mc_interface.h"
|
||||||
|
#include "stm32f4xx_conf.h"
|
||||||
|
|
||||||
// Private variables
|
// Private variables
|
||||||
static volatile systime_t timeout_msec;
|
static volatile systime_t timeout_msec;
|
||||||
static volatile systime_t last_update_time;
|
static volatile systime_t last_update_time;
|
||||||
static volatile float timeout_brake_current;
|
static volatile float timeout_brake_current;
|
||||||
static volatile bool has_timeout;
|
static volatile bool has_timeout;
|
||||||
|
static volatile uint32_t feed_counter[MAX_THREADS_MONITOR];
|
||||||
|
|
||||||
// Threads
|
// Threads
|
||||||
static THD_WORKING_AREA(timeout_thread_wa, 512);
|
static THD_WORKING_AREA(timeout_thread_wa, 512);
|
||||||
static THD_FUNCTION(timeout_thread, arg);
|
static THD_FUNCTION(timeout_thread, arg);
|
||||||
|
|
||||||
|
void timeout_init_IWDT(void);
|
||||||
|
|
||||||
void timeout_init(void) {
|
void timeout_init(void) {
|
||||||
timeout_msec = 1000;
|
timeout_msec = 1000;
|
||||||
last_update_time = 0;
|
last_update_time = 0;
|
||||||
timeout_brake_current = 0.0;
|
timeout_brake_current = 0.0;
|
||||||
has_timeout = false;
|
has_timeout = false;
|
||||||
|
|
||||||
|
// WWDG configuration
|
||||||
|
RCC_APB1PeriphClockCmd(RCC_APB1Periph_WWDG, ENABLE);
|
||||||
|
// Timeout = t_PCLK1 (ms) * 4096 * 2^(WDGTB [1:0]) * (T[5:0] + 1)
|
||||||
|
// CLK1 = 42MHz
|
||||||
|
|
||||||
|
// Window set between 6.24ms and 12.48ms. Mcu will reset if watchdog is fed outside the window
|
||||||
|
WWDG_SetPrescaler(WWDG_Prescaler_2);
|
||||||
|
WWDG_SetWindowValue(127);
|
||||||
|
WWDG_Enable(127); //0x7F
|
||||||
|
|
||||||
|
timeout_init_IWDT();
|
||||||
|
|
||||||
|
chThdSleepMilliseconds(10);
|
||||||
|
|
||||||
chThdCreateStatic(timeout_thread_wa, sizeof(timeout_thread_wa), NORMALPRIO, timeout_thread, NULL);
|
chThdCreateStatic(timeout_thread_wa, sizeof(timeout_thread_wa), NORMALPRIO, timeout_thread, NULL);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -60,6 +78,102 @@ float timeout_get_brake_current(void) {
|
||||||
return timeout_brake_current;
|
return timeout_brake_current;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void timeout_feed_WDT(uint8_t index) {
|
||||||
|
++feed_counter[index];
|
||||||
|
}
|
||||||
|
|
||||||
|
void timeout_init_IWDT(void) {
|
||||||
|
/* Enable write access to IWDG_PR and IWDG_RLR registers */
|
||||||
|
IWDG_WriteAccessCmd(IWDG_WriteAccess_Enable);
|
||||||
|
|
||||||
|
/* IWDG counter clock: LSI/4 */
|
||||||
|
IWDG_SetPrescaler(IWDG_Prescaler_4);
|
||||||
|
|
||||||
|
/* Set counter reload value to obtain 12ms IWDG TimeOut.
|
||||||
|
*
|
||||||
|
* LSI timer per datasheet is 32KHz typical, but 17KHz min
|
||||||
|
* and 47KHz max over the complete range of operating conditions,
|
||||||
|
* so reload time must ensure watchdog will work correctly under
|
||||||
|
* all conditions.
|
||||||
|
*
|
||||||
|
* Timeout threads runs every 10ms. Take 20% margin so wdt should
|
||||||
|
* be fed every 12ms. The worst condition occurs when the wdt clock
|
||||||
|
* runs at the max freq (47KHz) due to oscillator tolerances.
|
||||||
|
*
|
||||||
|
* t_IWDG(ms) = t_LSI(ms) * 4 * 2^(IWDG_PR[2:0]) * (IWDG_RLR[11:0] + 1)
|
||||||
|
* t_LSI(ms) [MAX] = 0.021276ms
|
||||||
|
* 12ms = 0.0212765 * 4 * 1 * (140 + 1)
|
||||||
|
|
||||||
|
Counter Reload Value = 140
|
||||||
|
|
||||||
|
When LSI clock runs the slowest, the IWDG will expire every 33.17ms
|
||||||
|
*/
|
||||||
|
IWDG_SetReload(140);
|
||||||
|
|
||||||
|
IWDG_ReloadCounter();
|
||||||
|
|
||||||
|
/* Enable IWDG (the LSI oscillator will be enabled by hardware) */
|
||||||
|
IWDG_Enable();
|
||||||
|
}
|
||||||
|
|
||||||
|
void timeout_configure_IWDT_slowest(void) {
|
||||||
|
while(((IWDG->SR & IWDG_SR_RVU) != 0) || ((IWDG->SR & IWDG_SR_PVU) != 0))
|
||||||
|
{
|
||||||
|
// Continue to kick the dog
|
||||||
|
IWDG_ReloadCounter();
|
||||||
|
}
|
||||||
|
// Unlock register
|
||||||
|
IWDG_WriteAccessCmd(IWDG_WriteAccess_Enable);
|
||||||
|
// Update configuration
|
||||||
|
IWDG_SetReload(1400);
|
||||||
|
IWDG_SetPrescaler(IWDG_Prescaler_256);
|
||||||
|
// Wait for the new configuration to be taken into account
|
||||||
|
while(((IWDG->SR & IWDG_SR_RVU) != 0) || ((IWDG->SR & IWDG_SR_PVU) != 0))
|
||||||
|
{
|
||||||
|
// Continue to kick the dog
|
||||||
|
IWDG_ReloadCounter();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void timeout_configure_IWDT(void) {
|
||||||
|
while(((IWDG->SR & IWDG_SR_RVU) != 0) || ((IWDG->SR & IWDG_SR_PVU) != 0))
|
||||||
|
{
|
||||||
|
// Continue to kick the dog
|
||||||
|
IWDG_ReloadCounter();
|
||||||
|
}
|
||||||
|
// Unlock register
|
||||||
|
IWDG_WriteAccessCmd(IWDG_WriteAccess_Enable);
|
||||||
|
// Update configuration
|
||||||
|
IWDG_SetReload(140);
|
||||||
|
IWDG_SetPrescaler(IWDG_Prescaler_4);
|
||||||
|
// Wait for the new configuration to be taken into account
|
||||||
|
while(((IWDG->SR & IWDG_SR_RVU) != 0) || ((IWDG->SR & IWDG_SR_PVU) != 0))
|
||||||
|
{
|
||||||
|
// Continue to kick the dog
|
||||||
|
IWDG_ReloadCounter();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
bool timeout_had_IWDG_reset(void) {
|
||||||
|
// Check if the system has resumed from IWDG reset
|
||||||
|
if (RCC_GetFlagStatus(RCC_FLAG_IWDGRST) != RESET) {
|
||||||
|
/* IWDGRST flag set */
|
||||||
|
/* Clear reset flags */
|
||||||
|
RCC_ClearFlag();
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Check if the system has resumed from WWDG reset
|
||||||
|
if (RCC_GetFlagStatus(RCC_FLAG_WWDGRST) != RESET) {
|
||||||
|
/* IWDGRST flag set */
|
||||||
|
/* Clear reset flags */
|
||||||
|
RCC_ClearFlag();
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
static THD_FUNCTION(timeout_thread, arg) {
|
static THD_FUNCTION(timeout_thread, arg) {
|
||||||
(void)arg;
|
(void)arg;
|
||||||
|
|
||||||
|
@ -74,6 +188,35 @@ static THD_FUNCTION(timeout_thread, arg) {
|
||||||
has_timeout = false;
|
has_timeout = false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
bool threads_ok = true;
|
||||||
|
|
||||||
|
// Monitored threads (foc, can, timer) must report at least one iteration,
|
||||||
|
// otherwise the watchdog won't be feed and MCU will reset. All threads should
|
||||||
|
// be monitored
|
||||||
|
if(feed_counter[THREAD_MCPWM] < MIN_THREAD_ITERATIONS)
|
||||||
|
threads_ok = false;
|
||||||
|
#if CAN_ENABLE
|
||||||
|
if(feed_counter[THREAD_CANBUS] < MIN_THREAD_ITERATIONS)
|
||||||
|
threads_ok = false;
|
||||||
|
#endif
|
||||||
|
if(feed_counter[THREAD_TIMER] < MIN_THREAD_ITERATIONS)
|
||||||
|
threads_ok = false;
|
||||||
|
|
||||||
|
for( int i = 0; i < MAX_THREADS_MONITOR; i++)
|
||||||
|
feed_counter[i] = 0;
|
||||||
|
|
||||||
|
if (threads_ok == true) {
|
||||||
|
// Feed WDT's
|
||||||
|
WWDG_SetCounter(127); // must reload in >6.24ms and <12.48ms
|
||||||
|
IWDG_ReloadCounter(); // must reload in <12ms
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
// not reloading the watchdog will produce a reset.
|
||||||
|
// This can be checked from the GUI logs as
|
||||||
|
// "FAULT_CODE_BOOTING_FROM_WATCHDOG_RESET"
|
||||||
|
}
|
||||||
|
|
||||||
chThdSleepMilliseconds(10);
|
chThdSleepMilliseconds(10);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
15
timeout.h
15
timeout.h
|
@ -24,12 +24,27 @@
|
||||||
#include "chtypes.h"
|
#include "chtypes.h"
|
||||||
#include "chsystypes.h"
|
#include "chsystypes.h"
|
||||||
|
|
||||||
|
#define MAX_THREADS_MONITOR 10
|
||||||
|
#define MIN_THREAD_ITERATIONS 1
|
||||||
|
|
||||||
|
typedef enum {
|
||||||
|
THREAD_MCPWM = 0,
|
||||||
|
THREAD_CANBUS,
|
||||||
|
THREAD_TIMER,
|
||||||
|
THREAD_USB,
|
||||||
|
THREAD_APP
|
||||||
|
} WWDT_THREAD_TYPES;
|
||||||
|
|
||||||
// Functions
|
// Functions
|
||||||
void timeout_init(void);
|
void timeout_init(void);
|
||||||
void timeout_configure(systime_t timeout, float brake_current);
|
void timeout_configure(systime_t timeout, float brake_current);
|
||||||
void timeout_reset(void);
|
void timeout_reset(void);
|
||||||
bool timeout_has_timeout(void);
|
bool timeout_has_timeout(void);
|
||||||
systime_t timeout_get_timeout_msec(void);
|
systime_t timeout_get_timeout_msec(void);
|
||||||
|
void timeout_configure_IWDT(void);
|
||||||
|
void timeout_configure_IWDT_slowest(void);
|
||||||
|
bool timeout_had_IWDG_reset(void);
|
||||||
|
void timeout_feed_WDT(uint8_t index);
|
||||||
float timeout_get_brake_current(void);
|
float timeout_get_brake_current(void);
|
||||||
|
|
||||||
#endif /* TIMEOUT_H_ */
|
#endif /* TIMEOUT_H_ */
|
||||||
|
|
4
utils.h
4
utils.h
|
@ -83,5 +83,9 @@ uint32_t utils_crc32c(uint8_t *data, uint32_t len);
|
||||||
#define ONE_BY_SQRT3 (0.57735026919)
|
#define ONE_BY_SQRT3 (0.57735026919)
|
||||||
#define TWO_BY_SQRT3 (2.0f * 0.57735026919)
|
#define TWO_BY_SQRT3 (2.0f * 0.57735026919)
|
||||||
#define SQRT3_BY_2 (0.86602540378)
|
#define SQRT3_BY_2 (0.86602540378)
|
||||||
|
#define COS_30_DEG (0.86602540378)
|
||||||
|
#define SIN_30_DEG (0.5)
|
||||||
|
#define COS_MINUS_30_DEG (0.86602540378)
|
||||||
|
#define SIN_MINUS_30_DEG (-0.5)
|
||||||
|
|
||||||
#endif /* UTILS_H_ */
|
#endif /* UTILS_H_ */
|
||||||
|
|
2
ws2811.c
2
ws2811.c
|
@ -74,7 +74,7 @@ void ws2811_init(void) {
|
||||||
}
|
}
|
||||||
|
|
||||||
// Generate gamma correction table
|
// Generate gamma correction table
|
||||||
for (int i = 0;i < 256;i++) {
|
for (i = 0;i < 256;i++) {
|
||||||
gamma_table[i] = (int)roundf(powf((float)i / 255.0, 1.0 / 0.45) * 255.0);
|
gamma_table[i] = (int)roundf(powf((float)i / 255.0, 1.0 / 0.45) * 255.0);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue