Skip to content

Commit

Permalink
[Spinal][DShot] workaround to use Dshot and ESC telemetry in Spinal v…
Browse files Browse the repository at this point in the history 
…ersion 4.1
  • Loading branch information
@sugihara-16
sugihara-16 committed Oct 4, 2024
1 parent c8857f9 commit 43387bb
Show file tree
Hide file tree
Showing 390 changed files with 309,801 additions and 683 deletions.
5 changes: 0 additions & 5 deletions aerial_robot_nerve/spinal/mcu_project/boards/stm32H7/Inc/stm32h7xx_it.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -56,14 +56,9 @@ void DebugMon_Handler(void);
void DMA1_Stream0_IRQHandler(void);
void DMA1_Stream1_IRQHandler(void);
void DMA1_Stream2_IRQHandler(void);
void DMA1_Stream3_IRQHandler(void);
void FDCAN1_IT0_IRQHandler(void);
void USART1_IRQHandler(void);
void TIM8_BRK_TIM12_IRQHandler(void);
void DMA2_Stream0_IRQHandler(void);
void DMA2_Stream1_IRQHandler(void);
void DMA2_Stream2_IRQHandler(void);
void DMA2_Stream3_IRQHandler(void);
void ETH_IRQHandler(void);
void ETH_WKUP_IRQHandler(void);
/* USER CODE BEGIN EFP */
Expand Down
4 changes: 2 additions & 2 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=1AC19A2B3B1B1D3F999102F78524A977
8DF89ED150041C4CBC7CB9A9CAA90856=1AC19A2B3B1B1D3F999102F78524A977
2F62501ED4689FB349E356AB974DBE57=11CAAD06F69AD2CF77BFF756BCD6A7A1
8DF89ED150041C4CBC7CB9A9CAA90856=11CAAD06F69AD2CF77BFF756BCD6A7A1
DC22A860405A8BF2F2C095E5B6529F12=60EEDAEEFD85B73FAEEE0D0107B4210E
eclipse.preferences.version=1
8 changes: 0 additions & 8 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 @@ -190,14 +190,6 @@ PROVIDE ( _end = . );
*(.GpsRxBufferSection)
. = ABSOLUTE(0x24044800);
*(.EscRxBufferSection)
. = ABSOLUTE(0x24045000);
*(.DShotBufferSection1)
. = ABSOLUTE(0x24045080);
*(.DShotBufferSection2)
. = ABSOLUTE(0x24045100);
*(.DShotBufferSection3)
. = ABSOLUTE(0x24045180);
*(.DShotBufferSection4)
} >RAM_D1 AT> FLASH


Expand Down
202 changes: 12 additions & 190 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,8 +49,6 @@

#include <Spine/spine.h>

#include "dshot_esc/dshot.h"

/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
Expand Down Expand Up @@ -79,18 +77,12 @@ 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 @@ -118,9 +110,6 @@ Baro baro_;
GPS gps_;
BatteryStatus battery_status_;

DShot dshot_;


StateEstimate estimator_;
FlightControl controller_;

Expand All @@ -140,7 +129,6 @@ 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 @@ -205,7 +193,6 @@ 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 @@ -252,9 +239,7 @@ 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)
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);
controller_.init(&htim1, &htim4, &estimator_, &battery_status_, &nh_, &flightControlMutexHandle);

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

Expand Down Expand Up @@ -415,10 +400,10 @@ void SystemClock_Config(void)
{
Error_Handler();
}
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.PeriphClockSelection = RCC_PERIPHCLK_USART3|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 @@ -718,9 +703,9 @@ static void MX_TIM1_Init(void)

/* USER CODE END TIM1_Init 1 */
htim1.Instance = TIM1;
htim1.Init.Prescaler = 2-1;
htim1.Init.CounterMode = TIM_COUNTERMODE_UP;
htim1.Init.Period = 1-1;
htim1.Init.Prescaler = 3;
htim1.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED1;
htim1.Init.Period = 50000;
htim1.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
htim1.Init.RepetitionCounter = 0;
htim1.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
Expand All @@ -745,7 +730,7 @@ static void MX_TIM1_Init(void)
Error_Handler();
}
sConfigOC.OCMode = TIM_OCMODE_PWM1;
sConfigOC.Pulse = 0;
sConfigOC.Pulse = 1000;
sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
sConfigOC.OCNPolarity = TIM_OCNPOLARITY_HIGH;
sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
Expand All @@ -755,6 +740,7 @@ 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 @@ -924,7 +910,7 @@ static void MX_USART3_UART_Init(void)

/* USER CODE END USART3_Init 1 */
huart3.Instance = USART3;
huart3.Init.BaudRate = 115200;
huart3.Init.BaudRate = 19200;
huart3.Init.WordLength = UART_WORDLENGTH_8B;
huart3.Init.StopBits = UART_STOPBITS_1;
huart3.Init.Parity = UART_PARITY_NONE;
Expand All @@ -934,7 +920,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_HalfDuplex_Init(&huart3) != HAL_OK)
if (HAL_UART_Init(&huart3) != HAL_OK)
{
Error_Handler();
}
Expand All @@ -956,54 +942,6 @@ 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 @@ -1012,7 +950,6 @@ 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 @@ -1024,21 +961,6 @@ 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 @@ -1072,14 +994,6 @@ 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 @@ -1116,8 +1030,6 @@ 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 @@ -1390,96 +1302,6 @@ 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 43387bb

Please sign in to comment.