Skip to content

Commit

Permalink
[Spinal][DShot][WIP] workaround to use dshot in Spinal 4.1
Browse files Browse the repository at this point in the history
  • Loading branch information
@sugihara-16
sugihara-16 committed Oct 3, 2024
1 parent 58f071c commit c8857f9
Show file tree
Hide file tree
Showing 10 changed files with 415 additions and 255 deletions.
5 changes: 5 additions & 0 deletions aerial_robot_nerve/spinal/mcu_project/boards/stm32H7/STM32CubeIDE/.project
View file
Original file line number Diff line number Diff line change
Expand Up @@ -67,6 +67,11 @@
<type>1</type>
<locationURI>PARENT-1-PROJECT_LOC/Src/lwip.c</locationURI>
</link>
<link>
<name>Application/User/main.c</name>
<type>1</type>
<locationURI>PARENT-1-PROJECT_LOC/Src/main.c</locationURI>
</link>
<link>
<name>Application/User/main.cpp</name>
<type>1</type>
Expand Down
6 changes: 3 additions & 3 deletions ...nerve/spinal/mcu_project/boards/stm32H7/STM32CubeIDE/.settings/stm32cubeide.project.prefs
View file
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
2F62501ED4689FB349E356AB974DBE57=5D6667793A96760B0F4246ED47BAFDEB
8DF89ED150041C4CBC7CB9A9CAA90856=5D6667793A96760B0F4246ED47BAFDEB
DC22A860405A8BF2F2C095E5B6529F12=6DF979B751C120663785242C4964B6DB
2F62501ED4689FB349E356AB974DBE57=1AC19A2B3B1B1D3F999102F78524A977
8DF89ED150041C4CBC7CB9A9CAA90856=1AC19A2B3B1B1D3F999102F78524A977
DC22A860405A8BF2F2C095E5B6529F12=60EEDAEEFD85B73FAEEE0D0107B4210E
eclipse.preferences.version=1
2 changes: 0 additions & 2 deletions aerial_robot_nerve/spinal/mcu_project/boards/stm32H7/STM32CubeIDE/STM32H743VITX_FLASH.ld
View file
Original file line number Diff line number Diff line change
Expand Up @@ -189,8 +189,6 @@ PROVIDE ( _end = . );
. = ABSOLUTE(0x24044400);
*(.GpsRxBufferSection)
. = ABSOLUTE(0x24044800);
*(.ServoRxBufferSection)
. = ABSOLUTE(0x24044C00);
*(.EscRxBufferSection)
. = ABSOLUTE(0x24045000);
*(.DShotBufferSection1)
Expand Down
203 changes: 190 additions & 13 deletions aerial_robot_nerve/spinal/mcu_project/boards/stm32H7/Src/main.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,6 +49,8 @@

#include <Spine/spine.h>

#include "dshot_esc/dshot.h"

/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
Expand Down Expand Up @@ -77,12 +79,18 @@ SPI_HandleTypeDef hspi1;

TIM_HandleTypeDef htim1;
TIM_HandleTypeDef htim4;
DMA_HandleTypeDef hdma_tim1_ch1;
DMA_HandleTypeDef hdma_tim1_ch2;
DMA_HandleTypeDef hdma_tim1_ch3;
DMA_HandleTypeDef hdma_tim1_ch4;

UART_HandleTypeDef huart1;
UART_HandleTypeDef huart3;
UART_HandleTypeDef huart6;
DMA_HandleTypeDef hdma_usart1_rx;
DMA_HandleTypeDef hdma_usart1_tx;
DMA_HandleTypeDef hdma_usart3_rx;
DMA_HandleTypeDef hdma_usart6_rx;

osThreadId coreTaskHandle;
osThreadId rosSpinTaskHandle;
Expand Down Expand Up @@ -110,6 +118,8 @@ Baro baro_;
GPS gps_;
BatteryStatus battery_status_;

DShot dshot_;


StateEstimate estimator_;
FlightControl controller_;
Expand All @@ -130,6 +140,7 @@ static void MX_USART1_UART_Init(void);
static void MX_USART3_UART_Init(void);
static void MX_TIM1_Init(void);
static void MX_TIM4_Init(void);
static void MX_USART6_UART_Init(void);
void coreTaskFunc(void const * argument);
void rosSpinTaskFunc(void const * argument);
void idleTaskFunc(void const * argument);
Expand Down Expand Up @@ -194,6 +205,7 @@ int main(void)
MX_USART3_UART_Init();
MX_TIM1_Init();
MX_TIM4_Init();
MX_USART6_UART_Init();
/* USER CODE BEGIN 2 */

// workaround for the wired generation of STMCubeMX
Expand Down Expand Up @@ -240,7 +252,9 @@ int main(void)
gps_.init(&huart3, &nh_, LED2_GPIO_Port, LED2_Pin);
battery_status_.init(&hadc1, &nh_);
estimator_.init(&imu_, &baro_, &gps_, &nh_); // imu + baro + gps => att + alt + pos(xy)
controller_.init(&htim1, &htim4, &estimator_, &battery_status_, &nh_, &flightControlMutexHandle);
dshot_.init(DSHOT600, &htim1,TIM_CHANNEL_1, &htim1,TIM_CHANNEL_2, &htim1,TIM_CHANNEL_3, &htim1, TIM_CHANNEL_4);
dshot_.initTelemetry(&huart6);
controller_.init(&htim1, &htim4, &estimator_, &dshot_, &battery_status_, &nh_, &flightControlMutexHandle);

FlashMemory::read(); //IMU calib data (including IMU in neurons)

Expand Down Expand Up @@ -401,10 +415,10 @@ void SystemClock_Config(void)
{
Error_Handler();
}
PeriphClkInitStruct.PeriphClockSelection = RCC_PERIPHCLK_USART3|RCC_PERIPHCLK_FDCAN
|RCC_PERIPHCLK_USART1|RCC_PERIPHCLK_SPI1
|RCC_PERIPHCLK_I2C3|RCC_PERIPHCLK_ADC
|RCC_PERIPHCLK_I2C1;
PeriphClkInitStruct.PeriphClockSelection = RCC_PERIPHCLK_USART3|RCC_PERIPHCLK_USART6
|RCC_PERIPHCLK_FDCAN|RCC_PERIPHCLK_USART1
|RCC_PERIPHCLK_SPI1|RCC_PERIPHCLK_I2C3
|RCC_PERIPHCLK_ADC|RCC_PERIPHCLK_I2C1;
PeriphClkInitStruct.PLL2.PLL2M = 1;
PeriphClkInitStruct.PLL2.PLL2N = 100;
PeriphClkInitStruct.PLL2.PLL2P = 2;
Expand Down Expand Up @@ -473,7 +487,7 @@ static void MX_ADC1_Init(void)
}
/** Configure Regular Channel
*/
sConfig.Channel = ADC_CHANNEL_15;
sConfig.Channel = ADC_CHANNEL_18;
sConfig.Rank = ADC_REGULAR_RANK_1;
sConfig.SamplingTime = ADC_SAMPLETIME_2CYCLES_5;
sConfig.SingleDiff = ADC_SINGLE_ENDED;
Expand Down Expand Up @@ -704,9 +718,9 @@ static void MX_TIM1_Init(void)

/* USER CODE END TIM1_Init 1 */
htim1.Instance = TIM1;
htim1.Init.Prescaler = 3;
htim1.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED1;
htim1.Init.Period = 50000;
htim1.Init.Prescaler = 2-1;
htim1.Init.CounterMode = TIM_COUNTERMODE_UP;
htim1.Init.Period = 1-1;
htim1.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
htim1.Init.RepetitionCounter = 0;
htim1.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
Expand All @@ -731,7 +745,7 @@ static void MX_TIM1_Init(void)
Error_Handler();
}
sConfigOC.OCMode = TIM_OCMODE_PWM1;
sConfigOC.Pulse = 10000;
sConfigOC.Pulse = 0;
sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
sConfigOC.OCNPolarity = TIM_OCNPOLARITY_HIGH;
sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
Expand All @@ -741,7 +755,6 @@ static void MX_TIM1_Init(void)
{
Error_Handler();
}
sConfigOC.Pulse = 0;
if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_2) != HAL_OK)
{
Error_Handler();
Expand Down Expand Up @@ -911,7 +924,7 @@ static void MX_USART3_UART_Init(void)

/* USER CODE END USART3_Init 1 */
huart3.Instance = USART3;
huart3.Init.BaudRate = 19200;
huart3.Init.BaudRate = 115200;
huart3.Init.WordLength = UART_WORDLENGTH_8B;
huart3.Init.StopBits = UART_STOPBITS_1;
huart3.Init.Parity = UART_PARITY_NONE;
Expand All @@ -921,7 +934,7 @@ static void MX_USART3_UART_Init(void)
huart3.Init.OneBitSampling = UART_ONE_BIT_SAMPLE_DISABLE;
huart3.Init.ClockPrescaler = UART_PRESCALER_DIV1;
huart3.AdvancedInit.AdvFeatureInit = UART_ADVFEATURE_NO_INIT;
if (HAL_UART_Init(&huart3) != HAL_OK)
if (HAL_HalfDuplex_Init(&huart3) != HAL_OK)
{
Error_Handler();
}
Expand All @@ -943,6 +956,54 @@ static void MX_USART3_UART_Init(void)

}

/**
* @brief USART6 Initialization Function
* @param None
* @retval None
*/
static void MX_USART6_UART_Init(void)
{

/* USER CODE BEGIN USART6_Init 0 */

/* USER CODE END USART6_Init 0 */

/* USER CODE BEGIN USART6_Init 1 */

/* USER CODE END USART6_Init 1 */
huart6.Instance = USART6;
huart6.Init.BaudRate = 115200;
huart6.Init.WordLength = UART_WORDLENGTH_8B;
huart6.Init.StopBits = UART_STOPBITS_1;
huart6.Init.Parity = UART_PARITY_NONE;
huart6.Init.Mode = UART_MODE_RX;
huart6.Init.HwFlowCtl = UART_HWCONTROL_NONE;
huart6.Init.OverSampling = UART_OVERSAMPLING_16;
huart6.Init.OneBitSampling = UART_ONE_BIT_SAMPLE_DISABLE;
huart6.Init.ClockPrescaler = UART_PRESCALER_DIV1;
huart6.AdvancedInit.AdvFeatureInit = UART_ADVFEATURE_NO_INIT;
if (HAL_UART_Init(&huart6) != HAL_OK)
{
Error_Handler();
}
if (HAL_UARTEx_SetTxFifoThreshold(&huart6, UART_TXFIFO_THRESHOLD_1_8) != HAL_OK)
{
Error_Handler();
}
if (HAL_UARTEx_SetRxFifoThreshold(&huart6, UART_RXFIFO_THRESHOLD_1_8) != HAL_OK)
{
Error_Handler();
}
if (HAL_UARTEx_DisableFifoMode(&huart6) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN USART6_Init 2 */

/* USER CODE END USART6_Init 2 */

}

/**
* Enable DMA controller clock
*/
Expand All @@ -951,6 +1012,7 @@ static void MX_DMA_Init(void)

/* DMA controller clock enable */
__HAL_RCC_DMA1_CLK_ENABLE();
__HAL_RCC_DMA2_CLK_ENABLE();

/* DMA interrupt init */
/* DMA1_Stream0_IRQn interrupt configuration */
Expand All @@ -962,6 +1024,21 @@ static void MX_DMA_Init(void)
/* DMA1_Stream2_IRQn interrupt configuration */
HAL_NVIC_SetPriority(DMA1_Stream2_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(DMA1_Stream2_IRQn);
/* DMA1_Stream3_IRQn interrupt configuration */
HAL_NVIC_SetPriority(DMA1_Stream3_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(DMA1_Stream3_IRQn);
/* DMA2_Stream0_IRQn interrupt configuration */
HAL_NVIC_SetPriority(DMA2_Stream0_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(DMA2_Stream0_IRQn);
/* DMA2_Stream1_IRQn interrupt configuration */
HAL_NVIC_SetPriority(DMA2_Stream1_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(DMA2_Stream1_IRQn);
/* DMA2_Stream2_IRQn interrupt configuration */
HAL_NVIC_SetPriority(DMA2_Stream2_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(DMA2_Stream2_IRQn);
/* DMA2_Stream3_IRQn interrupt configuration */
HAL_NVIC_SetPriority(DMA2_Stream3_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(DMA2_Stream3_IRQn);

}

Expand Down Expand Up @@ -995,6 +1072,14 @@ static void MX_GPIO_Init(void)
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_MEDIUM;
HAL_GPIO_Init(GPIOE, &GPIO_InitStruct);

/*Configure GPIO pin : PA3 */
GPIO_InitStruct.Pin = GPIO_PIN_3;
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
GPIO_InitStruct.Alternate = GPIO_AF7_USART2;
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);

/*Configure GPIO pin : IMUCS_Pin */
GPIO_InitStruct.Pin = IMUCS_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
Expand Down Expand Up @@ -1031,6 +1116,8 @@ static void MX_GPIO_Init(void)
/* USER CODE END Header_coreTaskFunc */
void coreTaskFunc(void const * argument)
{
/* init code for LWIP */
MX_LWIP_Init();
/* USER CODE BEGIN 5 */
#ifdef USE_ETH
/* init code for LWIP */
Expand Down Expand Up @@ -1303,6 +1390,96 @@ void MPU_Config(void)
MPU_InitStruct.IsCacheable = MPU_ACCESS_NOT_CACHEABLE;
MPU_InitStruct.IsBufferable = MPU_ACCESS_BUFFERABLE;

HAL_MPU_ConfigRegion(&MPU_InitStruct);
/** Initializes and configures the Region and the memory to be protected
*/
MPU_InitStruct.Enable = MPU_REGION_ENABLE;
MPU_InitStruct.Number = MPU_REGION_NUMBER6;
MPU_InitStruct.BaseAddress = 0x24044800;
MPU_InitStruct.Size = MPU_REGION_SIZE_1KB;
MPU_InitStruct.SubRegionDisable = 0x0;
MPU_InitStruct.TypeExtField = MPU_TEX_LEVEL1;
MPU_InitStruct.AccessPermission = MPU_REGION_FULL_ACCESS;
MPU_InitStruct.DisableExec = MPU_INSTRUCTION_ACCESS_ENABLE;
MPU_InitStruct.IsShareable = MPU_ACCESS_NOT_SHAREABLE;
MPU_InitStruct.IsCacheable = MPU_ACCESS_NOT_CACHEABLE;
MPU_InitStruct.IsBufferable = MPU_ACCESS_NOT_BUFFERABLE;

HAL_MPU_ConfigRegion(&MPU_InitStruct);
/** Initializes and configures the Region and the memory to be protected
*/
MPU_InitStruct.Enable = MPU_REGION_ENABLE;
MPU_InitStruct.Number = MPU_REGION_NUMBER7;
MPU_InitStruct.BaseAddress = 0x24044C00;
MPU_InitStruct.Size = MPU_REGION_SIZE_1KB;
MPU_InitStruct.SubRegionDisable = 0x0;
MPU_InitStruct.TypeExtField = MPU_TEX_LEVEL1;
MPU_InitStruct.AccessPermission = MPU_REGION_FULL_ACCESS;
MPU_InitStruct.DisableExec = MPU_INSTRUCTION_ACCESS_ENABLE;
MPU_InitStruct.IsShareable = MPU_ACCESS_NOT_SHAREABLE;
MPU_InitStruct.IsCacheable = MPU_ACCESS_NOT_CACHEABLE;
MPU_InitStruct.IsBufferable = MPU_ACCESS_NOT_BUFFERABLE;

HAL_MPU_ConfigRegion(&MPU_InitStruct);
/** Initializes and configures the Region and the memory to be protected
*/
MPU_InitStruct.Enable = MPU_REGION_ENABLE;
MPU_InitStruct.Number = MPU_REGION_NUMBER8;
MPU_InitStruct.BaseAddress = 0x24045000;
MPU_InitStruct.Size = MPU_REGION_SIZE_128B;
MPU_InitStruct.SubRegionDisable = 0x0;
MPU_InitStruct.TypeExtField = MPU_TEX_LEVEL0;
MPU_InitStruct.AccessPermission = MPU_REGION_FULL_ACCESS;
MPU_InitStruct.DisableExec = MPU_INSTRUCTION_ACCESS_ENABLE;
MPU_InitStruct.IsShareable = MPU_ACCESS_NOT_SHAREABLE;
MPU_InitStruct.IsCacheable = MPU_ACCESS_NOT_CACHEABLE;
MPU_InitStruct.IsBufferable = MPU_ACCESS_NOT_BUFFERABLE;

HAL_MPU_ConfigRegion(&MPU_InitStruct);
/** Initializes and configures the Region and the memory to be protected
*/
MPU_InitStruct.Enable = MPU_REGION_ENABLE;
MPU_InitStruct.Number = MPU_REGION_NUMBER9;
MPU_InitStruct.BaseAddress = 0x24045080;
MPU_InitStruct.Size = MPU_REGION_SIZE_128B;
MPU_InitStruct.SubRegionDisable = 0x0;
MPU_InitStruct.TypeExtField = MPU_TEX_LEVEL0;
MPU_InitStruct.AccessPermission = MPU_REGION_FULL_ACCESS;
MPU_InitStruct.DisableExec = MPU_INSTRUCTION_ACCESS_ENABLE;
MPU_InitStruct.IsShareable = MPU_ACCESS_NOT_SHAREABLE;
MPU_InitStruct.IsCacheable = MPU_ACCESS_NOT_CACHEABLE;
MPU_InitStruct.IsBufferable = MPU_ACCESS_NOT_BUFFERABLE;

HAL_MPU_ConfigRegion(&MPU_InitStruct);
/** Initializes and configures the Region and the memory to be protected
*/
MPU_InitStruct.Enable = MPU_REGION_ENABLE;
MPU_InitStruct.Number = MPU_REGION_NUMBER10;
MPU_InitStruct.BaseAddress = 0x24045100;
MPU_InitStruct.Size = MPU_REGION_SIZE_128B;
MPU_InitStruct.SubRegionDisable = 0x0;
MPU_InitStruct.TypeExtField = MPU_TEX_LEVEL0;
MPU_InitStruct.AccessPermission = MPU_REGION_FULL_ACCESS;
MPU_InitStruct.DisableExec = MPU_INSTRUCTION_ACCESS_ENABLE;
MPU_InitStruct.IsShareable = MPU_ACCESS_NOT_SHAREABLE;
MPU_InitStruct.IsCacheable = MPU_ACCESS_NOT_CACHEABLE;
MPU_InitStruct.IsBufferable = MPU_ACCESS_NOT_BUFFERABLE;

HAL_MPU_ConfigRegion(&MPU_InitStruct);
/** Initializes and configures the Region and the memory to be protected
*/
MPU_InitStruct.Enable = MPU_REGION_ENABLE;
MPU_InitStruct.Number = MPU_REGION_NUMBER11;
MPU_InitStruct.BaseAddress = 0x24045180;
MPU_InitStruct.Size = MPU_REGION_SIZE_128B;
MPU_InitStruct.SubRegionDisable = 0x0;
MPU_InitStruct.TypeExtField = MPU_TEX_LEVEL0;
MPU_InitStruct.AccessPermission = MPU_REGION_FULL_ACCESS;
MPU_InitStruct.DisableExec = MPU_INSTRUCTION_ACCESS_ENABLE;
MPU_InitStruct.IsShareable = MPU_ACCESS_NOT_SHAREABLE;
MPU_InitStruct.IsCacheable = MPU_ACCESS_NOT_CACHEABLE;
MPU_InitStruct.IsBufferable = MPU_ACCESS_NOT_BUFFERABLE;

HAL_MPU_ConfigRegion(&MPU_InitStruct);
/* Enables the MPU */
HAL_MPU_Enable(MPU_PRIVILEGED_DEFAULT);
Expand Down
Loading

0 comments on commit c8857f9

Please sign in to comment.