Skip to content

Commit

Permalink
Add DSHOT support and associated changes
Browse files Browse the repository at this point in the history
  • Loading branch information
@avtoku
avtoku committed Jun 30, 2024
1 parent 553e706 commit f90bc5f
Show file tree
Hide file tree
Showing 23 changed files with 749 additions and 734 deletions.
134 changes: 22 additions & 112 deletions common/Varmint.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -390,22 +390,35 @@ float Varmint::rc_read(uint8_t chan)
ever_read = true;
}

if ((chan < PWM_CHANNELS) & ever_read)
if ((chan < RC_PACKET_CHANNELS) & ever_read)
return p.chan[chan];
return -1; // out of range or no data in p
}

///////////////////////////////////////////////////////////////////////////////////////////////
// PWM

// legacy, all channels are pwm and set to the same 'refresh_rate'
void Varmint::pwm_init(uint32_t refresh_rate, uint16_t idle_pwm)
{
for (int ch = 0; ch < PWM_CHANNELS; ch++)
pwm_.disable(ch);
for (int ch = 0; ch < PWM_CHANNELS; ch++)
pwm_.set_rate(ch, refresh_rate);
{
pwm_.setRate(ch, refresh_rate);
if(idle_pwm==0) pwm_.writeUs(ch, 0); // OFF
else pwm_.write(ch, 0.0); // Channel minimum value
}
for (int ch = 0; ch < PWM_CHANNELS; ch++)
pwm_.enable(ch);
}

void Varmint::pwm_init(const float *rate, uint32_t channels)
{
pwm_.updateConfig(rate,channels);
}


void Varmint::pwm_disable(void)
{
for (int ch = 0; ch < PWM_CHANNELS; ch++)
Expand All @@ -416,6 +429,13 @@ void Varmint::pwm_write(uint8_t ch, float value)
pwm_.write(ch, value);
}

void Varmint::pwm_write(float *value, uint32_t channels)
{
pwm_.write(value, channels);
}



///////////////////////////////////////////////////////////////////////////////////////////////
// LEDs
void Varmint::led0_on() // Red LED
Expand Down Expand Up @@ -491,113 +511,3 @@ bool Varmint::memory_write(const void *src, size_t len)

///////////////////////////////////////////////////////////////////////////////////////////////
// Helper functions (not part of parent class)

//uint32_t Varmint::pwm_init_timers(uint32_t servo_pwm_period_us)
//{
// {
// TIM_MasterConfigTypeDef sMasterConfig = {0};
// TIM_OC_InitTypeDef sConfigOC = {0};
// TIM_BreakDeadTimeConfigTypeDef sBreakDeadTimeConfig = {0};
//
// htim1.Instance = TIM1;
// htim1.Init.Prescaler = (SERVO_PWM_CLK_DIV);
// htim1.Init.CounterMode = TIM_COUNTERMODE_UP;
// htim1.Init.Period = servo_pwm_period_us;
// htim1.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
// htim1.Init.RepetitionCounter = 0;
// htim1.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_ENABLE;
// if (HAL_TIM_PWM_Init(&htim1) != HAL_OK)
// return DRIVER_HAL_ERROR;
// sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
// sMasterConfig.MasterOutputTrigger2 = TIM_TRGO2_RESET;
// sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
// if (HAL_TIMEx_MasterConfigSynchronization(&htim1, &sMasterConfig) != HAL_OK)
// return DRIVER_HAL_ERROR;
// sConfigOC.OCMode = TIM_OCMODE_PWM1;
// sConfigOC.Pulse = (SERVO_PWM_CENTER);
// sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
// sConfigOC.OCNPolarity = TIM_OCNPOLARITY_HIGH;
// sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
// sConfigOC.OCIdleState = TIM_OCIDLESTATE_RESET;
// sConfigOC.OCNIdleState = TIM_OCNIDLESTATE_RESET;
// if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_1) != HAL_OK)
// return DRIVER_HAL_ERROR;
// if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_2) != HAL_OK)
// return DRIVER_HAL_ERROR;
// if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_3) != HAL_OK)
// return DRIVER_HAL_ERROR;
// if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_4) != HAL_OK)
// return DRIVER_HAL_ERROR;
// sBreakDeadTimeConfig.OffStateRunMode = TIM_OSSR_DISABLE;
// sBreakDeadTimeConfig.OffStateIDLEMode = TIM_OSSI_DISABLE;
// sBreakDeadTimeConfig.LockLevel = TIM_LOCKLEVEL_OFF;
// sBreakDeadTimeConfig.DeadTime = 0;
// sBreakDeadTimeConfig.BreakState = TIM_BREAK_DISABLE;
// sBreakDeadTimeConfig.BreakPolarity = TIM_BREAKPOLARITY_HIGH;
// sBreakDeadTimeConfig.BreakFilter = 0;
// sBreakDeadTimeConfig.Break2State = TIM_BREAK2_DISABLE;
// sBreakDeadTimeConfig.Break2Polarity = TIM_BREAK2POLARITY_HIGH;
// sBreakDeadTimeConfig.Break2Filter = 0;
// sBreakDeadTimeConfig.AutomaticOutput = TIM_AUTOMATICOUTPUT_DISABLE;
// if (HAL_TIMEx_ConfigBreakDeadTime(&htim1, &sBreakDeadTimeConfig) != HAL_OK)
// return DRIVER_HAL_ERROR;
// HAL_TIM_MspPostInit(&htim1);
// }
//#if defined(htim3)
// {
// TIM_MasterConfigTypeDef sMasterConfig = {0};
// TIM_OC_InitTypeDef sConfigOC = {0};
// htim3.Instance = TIM3;
// htim3.Init.Prescaler = (SERVO_PWM_CLK_DIV);
// htim3.Init.CounterMode = TIM_COUNTERMODE_UP;
// htim3.Init.Period = servo_pwm_period_us;
// htim3.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
// htim3.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_ENABLE;
// if (HAL_TIM_PWM_Init(&htim3) != HAL_OK)
// return DRIVER_HAL_ERROR;
// sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
// sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
// if (HAL_TIMEx_MasterConfigSynchronization(&htim3, &sMasterConfig) != HAL_OK)
// return DRIVER_HAL_ERROR;
// sConfigOC.OCMode = TIM_OCMODE_PWM1;
// sConfigOC.Pulse = (SERVO_PWM_CENTER);
// sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
// sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
// if (HAL_TIM_PWM_ConfigChannel(&htim3, &sConfigOC, TIM_CHANNEL_1) != HAL_OK)
// return DRIVER_HAL_ERROR;
// if (HAL_TIM_PWM_ConfigChannel(&htim3, &sConfigOC, TIM_CHANNEL_2) != HAL_OK)
// return DRIVER_HAL_ERROR;
// HAL_TIM_MspPostInit(&htim3);
// }
//#endif
// {
// TIM_MasterConfigTypeDef sMasterConfig = {0};
// TIM_OC_InitTypeDef sConfigOC = {0};
// htim4.Instance = TIM4;
// htim4.Init.Prescaler = (SERVO_PWM_CLK_DIV);
// htim4.Init.CounterMode = TIM_COUNTERMODE_UP;
// htim4.Init.Period = servo_pwm_period_us;
// htim4.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
// htim4.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_ENABLE;
// if (HAL_TIM_PWM_Init(&htim4) != HAL_OK)
// return DRIVER_HAL_ERROR;
// sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
// sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
// if (HAL_TIMEx_MasterConfigSynchronization(&htim4, &sMasterConfig) != HAL_OK)
// return DRIVER_HAL_ERROR;
// sConfigOC.OCMode = TIM_OCMODE_PWM1;
// sConfigOC.Pulse = (SERVO_PWM_CENTER);
// sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
// sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
// if (HAL_TIM_PWM_ConfigChannel(&htim4, &sConfigOC, TIM_CHANNEL_1) != HAL_OK)
// return DRIVER_HAL_ERROR;
// if (HAL_TIM_PWM_ConfigChannel(&htim4, &sConfigOC, TIM_CHANNEL_2) != HAL_OK)
// return DRIVER_HAL_ERROR;
// if (HAL_TIM_PWM_ConfigChannel(&htim4, &sConfigOC, TIM_CHANNEL_3) != HAL_OK)
// return DRIVER_HAL_ERROR;
// if (HAL_TIM_PWM_ConfigChannel(&htim4, &sConfigOC, TIM_CHANNEL_4) != HAL_OK)
// return DRIVER_HAL_ERROR;
// HAL_TIM_MspPostInit(&htim4);
// }
// return DRIVER_OK;
//}
3 changes: 1 addition & 2 deletions common/drivers/Driver.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -47,8 +47,7 @@
class Driver
{
public:
// virtual bool startDma (void)=0; // Called in response to a data ready signal
// virtual void endDma (void)=0; // Called when DMA read is complete

virtual bool display(void) = 0;

uint16_t rxFifoCount(void)
Expand Down
117 changes: 71 additions & 46 deletions common/drivers/Pwm.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -46,57 +46,82 @@
// 600 600kbit/s 1.25 0.625 1.67 26.72
// 1200 1200kbit/s 0.625 0.313 0.83 13.28

typedef enum
{
PWM_STD,
PWM_DSHOT,
} PwmProtocol;

//typedef struct __attribute__((__packed__))
//{
// TIM_HandleTypeDef *htim;
// uint16_t channel;
// uint16_t min;
// uint16_t center;
// uint16_t max;
//} PwmChannelCfg;

typedef struct __attribute__((__packed__))
__attribute__((section(".data"))) PwmBlockStructure pwm_init[PWM_TIMER_BLOCKS] = PWM_INIT_DEFINE;

__attribute__((section("my_dma_buffers")))
__attribute__((aligned(32))) static uint32_t pwm_dma_buf[PWM_TIMER_BLOCKS][PWM_DMA_BUFFER_LEN] = {0};


void Pwm::updateConfig(const float *rate, uint32_t channels)
{
TIM_TypeDef *instance;
PwmProtocol protocol;
uint32_t period_counts;
} PwmBlockCfg;
channels = (channels<PWM_CHANNELS)?channels:PWM_CHANNELS;

for(uint32_t ch=0;ch<channels;ch++)
HAL_TIM_PWM_Stop(htim_[ch], chan_[ch]);

__attribute__((section(".data"))) PwmChannelCfg pwm_ch[PWM_CHANNELS] = PWM_CHANNELS_DEFINE;
__attribute__((section(".data"))) PwmBlockCfg pwm_blk[PWM_BLOCKS] = PWM_BLOCKS_DEFINE;
for(uint32_t ch=0;ch<channels;ch++)
setRate(ch,rate[ch]);

for(uint32_t ch=0;ch<channels;ch++)
HAL_TIM_PWM_Start(htim_[ch], chan_[ch]);
}

//#define PWM_CLOCK (2e8) // 200 MHz

uint32_t Pwm::init(void)
{
chan_ = pwm_ch;
for(uint8_t bk=0;bk<PWM_BLOCKS;bk++)
block_ = pwm_init;
dmaBuf_ = pwm_dma_buf;

// Clear lookup table
for(uint32_t output_index = 0; output_index<PWM_CHANNELS; output_index++)
{
htim_[output_index] = nullptr;
chan_[output_index] = PWM_CHAN_IGNORE;
blockIndex_[output_index] = (uint32_t)(-1);
}

// Fill-in lookup tables
for(uint32_t bk=0;bk<PWM_TIMER_BLOCKS;bk++)
{
TIM_HandleTypeDef *htim;
if(pwm_blk[bk].instance==TIM1) htim = &htim1;
else if(pwm_blk[bk].instance==TIM3) htim = &htim3;
else if(pwm_blk[bk].instance==TIM4) htim = &htim4;
else if(pwm_blk[bk].instance==TIM8) htim = &htim8;

uint32_t prescaler; // yields 1us per count with a 200MHz clock input
if(pwm_blk[bk].protocol==PWM_DSHOT)
prescaler = 0; // 5ns per count.
else // PWM_STD
prescaler = 199; // 1us per count
for( uint32_t ch=0; ch<4; ch++)
{
uint32_t output_index = block_[bk].chan[ch];
if( output_index<PWM_CHANNELS )
{
blockIndex_[output_index] = bk;
htim_[output_index] = block_[bk].htim;
chan_[output_index] = (uint32_t)ch<<2; // Note: TIM_CHANNEL_x = (x-1)*4; up to x==4
}
}
}

for(uint32_t bk=0;bk<PWM_TIMER_BLOCKS;bk++)
{
TIM_MasterConfigTypeDef sMasterConfig = {0};
TIM_OC_InitTypeDef sConfigOC = {0};

htim->Instance = pwm_blk[bk].instance;
htim->Init.Prescaler = prescaler;
TIM_HandleTypeDef *htim = block_[bk].htim;

if(htim == &htim1) htim->Instance = TIM1;
else if(htim == &htim3) htim->Instance = TIM3;
else if(htim == &htim4) htim->Instance = TIM4;
else if(htim == &htim8) htim->Instance = TIM8;
else return DRIVER_HAL_ERROR;

if((block_[bk].rate >= 150000) && (block_[bk].type == PWM_DSHOT)) // DSHOT
{
htim->Init.Prescaler = 0;
htim->Init.Period = (uint64_t)200000000/block_[bk].rate;
}
else if((block_[bk].type == PWM_STANDARD) && (block_[bk].rate < 490))
{
htim->Init.Prescaler = 199;
htim->Init.Period = (uint64_t)1000000/block_[bk].rate;
}
else
return DRIVER_HAL_ERROR;

htim->Init.CounterMode = TIM_COUNTERMODE_UP;
htim->Init.Period = pwm_blk[bk].period_counts;
htim->Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
htim->Init.RepetitionCounter = 0;
htim->Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_ENABLE;
Expand All @@ -108,24 +133,24 @@ uint32_t Pwm::init(void)
if (HAL_TIMEx_MasterConfigSynchronization(htim, &sMasterConfig) != HAL_OK)
return DRIVER_HAL_ERROR;
sConfigOC.OCMode = TIM_OCMODE_PWM1;
// sConfigOC.Pulse = (SERVO_PWM_CENTER);
// sConfigOC.Pulse = 1500;
sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
sConfigOC.OCNPolarity = TIM_OCNPOLARITY_HIGH;
sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
sConfigOC.OCIdleState = TIM_OCIDLESTATE_RESET;
sConfigOC.OCNIdleState = TIM_OCNIDLESTATE_RESET;

for(uint8_t ch=0;ch<PWM_CHANNELS;ch++)
for(uint32_t ch=0;ch<4;ch++)
{
if(pwm_ch[ch].htim==htim)
if(block_[bk].chan[ch]<PWM_CHANNELS)
{
sConfigOC.Pulse = pwm_ch[ch].center;
if (HAL_TIM_PWM_ConfigChannel(htim, &sConfigOC, pwm_ch[ch].channel) != HAL_OK)
sConfigOC.Pulse = 0; // default to flat line output
if (HAL_TIM_PWM_ConfigChannel(htim, &sConfigOC,(uint32_t)ch*4) != HAL_OK)
return DRIVER_HAL_ERROR;
}
}

if( (pwm_blk[bk].instance==TIM1) || (pwm_blk[bk].instance==TIM8))
if( (htim==&htim1) || (htim==&htim8))
{
TIM_BreakDeadTimeConfigTypeDef sBreakDeadTimeConfig = {0};
sBreakDeadTimeConfig.OffStateRunMode = TIM_OSSR_DISABLE;
Expand All @@ -144,7 +169,7 @@ uint32_t Pwm::init(void)
}

HAL_TIM_MspPostInit(htim);

}

return DRIVER_OK;
}
Loading

0 comments on commit f90bc5f

Please sign in to comment.