Skip to content

Commit 551023e

Browse files
JcZouJcZou
committed
Cleanup drv_actuator
1 parent 8ef4753 commit 551023e

File tree

1 file changed

+10
-127
lines changed

1 file changed

+10
-127
lines changed

‎target/sieon/s1/drivers/drv_actuator.c‎

Lines changed: 10 additions & 127 deletions
Original file line numberDiff line numberDiff line change
@@ -57,7 +57,7 @@ typedef struct {
5757
uint16_t wait_ms;
5858
bool active;
5959
} dshot_cmd_state_t;
60-
static volatile dshot_cmd_state_t dshot_cmds[MAIN_OUT_CHAN_NUM + AUX_OUT_CHAN_NUM] = { 0 };
60+
static volatile dshot_cmd_state_t dshot_cmds[MAIN_OUT_CHAN_NUM] = { 0 };
6161

6262
/* Channels serviced by the periodic actuator loop. */
6363
static volatile uint16_t main_polled_mask = 0;
@@ -66,7 +66,7 @@ static volatile uint16_t aux_polled_mask = 0;
6666
/* One DMA stream is used per timer, with 4 CCR values updated on each event. */
6767

6868
#define DSHOT_BITS 16U
69-
#define DSHOT_TIMER_COUNT 4U
69+
#define DSHOT_TIMER_COUNT 3U
7070
#define DSHOT_TIMER_CHANNELS 4U
7171
#define DSHOT_FRAME_ROWS 17U
7272
#define DSHOT_DMA_ROWS 18U
@@ -86,7 +86,6 @@ static const struct dshot_dma_map dshot_dma_map[DSHOT_TIMER_COUNT] = {
8686
{ DMA2, LL_DMA_STREAM_2, LL_DMAMUX1_REQ_TIM1_UP, TIM1 },
8787
{ DMA2, LL_DMA_STREAM_3, LL_DMAMUX1_REQ_TIM4_UP, TIM4 },
8888
{ DMA2, LL_DMA_STREAM_4, LL_DMAMUX1_REQ_TIM5_UP, TIM5 },
89-
{ DMA2, LL_DMA_STREAM_5, LL_DMAMUX1_REQ_TIM8_UP, TIM8 },
9089
};
9190

9291
static void _dshot_clear_dma_flags(DMA_TypeDef* dma, uint32_t stream)
@@ -681,21 +680,14 @@ static rt_err_t main_pwm_config(actuator_dev_t dev, const struct actuator_config
681680
if (cfg->protocol == ACT_PROTOCOL_PWM) {
682681
DRV_DBG("main out configured: pwm frequency:%d\n", cfg->pwm_config.pwm_freq);
683682

684-
LL_RCC_ClocksTypeDef rcc_clocks;
685-
LL_RCC_GetSystemClocksFreq(&rcc_clocks);
686-
uint32_t psc_apb1 = (rcc_clocks.PCLK1_Frequency * 2) / PWM_TIMER_FREQUENCY - 1;
687-
uint32_t psc_apb2 = (rcc_clocks.PCLK2_Frequency * 2) / PWM_TIMER_FREQUENCY - 1;
688-
LL_TIM_SetPrescaler(TIM1, psc_apb2);
689-
LL_TIM_SetPrescaler(TIM4, psc_apb1);
690-
LL_TIM_SetPrescaler(TIM5, psc_apb1);
691-
692683
if (__main_set_pwm_frequency(cfg->pwm_config.pwm_freq) != RT_EOK) {
693684
return RT_ERROR;
694685
}
695686
} else if (cfg->protocol == ACT_PROTOCOL_DSHOT) {
696-
DRV_DBG("main out configured: dshot speed:%d\n", cfg->dshot_config.speed);
697687
uint16_t speed = cfg->dshot_config.speed;
698-
if (speed > 0) {
688+
DRV_DBG("main out configured: dshot speed:%d\n", cfg->dshot_config.speed);
689+
690+
if (speed == 150 || speed == 300 || speed == 600) {
699691
LL_RCC_ClocksTypeDef rcc_clocks;
700692
LL_RCC_GetSystemClocksFreq(&rcc_clocks);
701693
uint32_t psc_apb1 = (rcc_clocks.PCLK1_Frequency * 2) / DSHOT_TIMER_FREQUENCY - 1;
@@ -708,32 +700,10 @@ static rt_err_t main_pwm_config(actuator_dev_t dev, const struct actuator_config
708700
LL_TIM_SetAutoReload(TIM1, new_arr - 1);
709701
LL_TIM_SetAutoReload(TIM4, new_arr - 1);
710702
LL_TIM_SetAutoReload(TIM5, new_arr - 1);
711-
/* DShot updates CCR through DMA burst, so preload must stay enabled. */
712-
LL_TIM_OC_EnablePreload(TIM1, LL_TIM_CHANNEL_CH1);
713-
LL_TIM_OC_EnablePreload(TIM1, LL_TIM_CHANNEL_CH2);
714-
LL_TIM_OC_EnablePreload(TIM1, LL_TIM_CHANNEL_CH3);
715-
LL_TIM_OC_EnablePreload(TIM1, LL_TIM_CHANNEL_CH4);
716-
LL_TIM_OC_EnablePreload(TIM4, LL_TIM_CHANNEL_CH1);
717-
LL_TIM_OC_EnablePreload(TIM4, LL_TIM_CHANNEL_CH2);
718-
LL_TIM_OC_EnablePreload(TIM4, LL_TIM_CHANNEL_CH3);
719-
LL_TIM_OC_EnablePreload(TIM4, LL_TIM_CHANNEL_CH4);
720-
LL_TIM_OC_EnablePreload(TIM5, LL_TIM_CHANNEL_CH1);
721-
LL_TIM_OC_EnablePreload(TIM5, LL_TIM_CHANNEL_CH2);
722-
LL_TIM_OC_EnablePreload(TIM5, LL_TIM_CHANNEL_CH3);
723-
LL_TIM_OC_EnablePreload(TIM5, LL_TIM_CHANNEL_CH4);
724-
TIM1->CCR1 = 0;
725-
TIM1->CCR2 = 0;
726-
TIM1->CCR3 = 0;
727-
TIM1->CCR4 = 0;
728-
TIM4->CCR1 = 0;
729-
TIM4->CCR2 = 0;
730-
TIM4->CCR3 = 0;
731-
TIM4->CCR4 = 0;
732-
TIM5->CCR1 = 0;
733-
TIM5->CCR2 = 0;
734-
TIM5->CCR3 = 0;
735-
TIM5->CCR4 = 0;
703+
736704
_setup_dshot_dma();
705+
} else {
706+
return RT_EINVAL;
737707
}
738708
}
739709
/* update device configuration */
@@ -747,31 +717,11 @@ static rt_err_t aux_pwm_config(actuator_dev_t dev, const struct actuator_configu
747717
if (cfg->protocol == ACT_PROTOCOL_PWM) {
748718
DRV_DBG("aux out configured: pwm frequency:%d\n", cfg->pwm_config.pwm_freq);
749719

750-
LL_RCC_ClocksTypeDef rcc_clocks;
751-
LL_RCC_GetSystemClocksFreq(&rcc_clocks);
752-
uint32_t psc_apb2 = (rcc_clocks.PCLK2_Frequency * 2) / PWM_TIMER_FREQUENCY - 1;
753-
LL_TIM_SetPrescaler(TIM8, psc_apb2);
754-
755720
if (__aux_set_pwm_frequency(cfg->pwm_config.pwm_freq) != RT_EOK) {
756721
return RT_ERROR;
757722
}
758-
} else if (cfg->protocol == ACT_PROTOCOL_DSHOT) {
759-
DRV_DBG("aux out configured: dshot speed:%d\n", cfg->dshot_config.speed);
760-
uint16_t speed = cfg->dshot_config.speed;
761-
if (speed > 0) {
762-
LL_RCC_ClocksTypeDef rcc_clocks;
763-
LL_RCC_GetSystemClocksFreq(&rcc_clocks);
764-
uint32_t psc_apb2 = (rcc_clocks.PCLK2_Frequency * 2) / DSHOT_TIMER_FREQUENCY - 1;
765-
LL_TIM_SetPrescaler(TIM8, psc_apb2);
766-
767-
uint32_t new_arr = DSHOT_TIMER_FREQUENCY / ((uint32_t)speed * 1000U);
768-
LL_TIM_SetAutoReload(TIM8, new_arr - 1);
769-
LL_TIM_OC_EnablePreload(TIM8, LL_TIM_CHANNEL_CH1);
770-
LL_TIM_OC_EnablePreload(TIM8, LL_TIM_CHANNEL_CH2);
771-
TIM8->CCR1 = 0;
772-
TIM8->CCR2 = 0;
773-
_setup_dshot_dma();
774-
}
723+
} else {
724+
return RT_EINVAL;
775725
}
776726
/* update device configuration */
777727
dev->config = *cfg;
@@ -912,30 +862,6 @@ static rt_err_t aux_pwm_control(actuator_dev_t dev, int cmd, void* arg)
912862

913863
DRV_DBG("aux out disabled\n");
914864
break;
915-
case ACT_CMD_SET_PROTOCOL: {
916-
uint8_t protocol = *(uint8_t*)arg;
917-
if (protocol == ACT_PROTOCOL_DSHOT || protocol == ACT_PROTOCOL_PWM) {
918-
aux_protocol = protocol;
919-
dev->config.protocol = protocol;
920-
ret = RT_EOK;
921-
} else {
922-
ret = RT_EINVAL;
923-
}
924-
break;
925-
}
926-
case ACT_CMD_DSHOT_SEND: {
927-
struct dshot_command* c = (struct dshot_command*)arg;
928-
if (c == RT_NULL) {
929-
ret = RT_EINVAL;
930-
break;
931-
}
932-
if (dev->config.protocol != ACT_PROTOCOL_DSHOT) {
933-
ret = RT_EINVAL;
934-
break;
935-
}
936-
ret = _dshot_send_command(c->chan_mask, c->value, c->repeat ? c->repeat : 1, c->wait_ms, true);
937-
break;
938-
}
939865
default:
940866
ret = RT_EINVAL;
941867
break;
@@ -1065,49 +991,6 @@ static rt_size_t aux_pwm_write(actuator_dev_t dev, rt_uint16_t chan_sel, const r
1065991
index++;
1066992
}
1067993
}
1068-
} else if (aux_protocol == ACT_PROTOCOL_DSHOT) {
1069-
uint32_t current_arr = LL_TIM_GetAutoReload(TIM8);
1070-
uint16_t packed_sel_aux = 0;
1071-
1072-
aux_polled_mask |= chan_sel;
1073-
1074-
for (uint8_t i = 0; i < AUX_OUT_CHAN_NUM; i++) {
1075-
if (chan_sel & (1 << i)) {
1076-
uint16_t dshot_val;
1077-
val = *index;
1078-
uint8_t global_idx = MAIN_OUT_CHAN_NUM + i;
1079-
1080-
if (dshot_cmds[global_idx].active) {
1081-
if (dshot_cmds[global_idx].repeat > 0) {
1082-
dshot_val = dshot_cmds[global_idx].cmd;
1083-
dshot_cmds[global_idx].repeat--;
1084-
if (dshot_cmds[global_idx].repeat == 0) {
1085-
dshot_cmds[global_idx].wait_start_ms = systime_now_ms();
1086-
if (dshot_cmds[global_idx].wait_ms == 0) {
1087-
dshot_cmds[global_idx].active = false;
1088-
}
1089-
}
1090-
} else {
1091-
dshot_val = DSHOT_CMD_MOTOR_STOP;
1092-
if ((systime_now_ms() - dshot_cmds[global_idx].wait_start_ms) >= dshot_cmds[global_idx].wait_ms) {
1093-
dshot_cmds[global_idx].active = false;
1094-
}
1095-
}
1096-
} else {
1097-
float norm_throttle = (val > 1000) ? ((float)(val - 1000) / 1000.0f) : 0.0f;
1098-
dshot_val = dshot_throttle_to_value(norm_throttle);
1099-
}
1100-
1101-
uint16_t frame = dshot_pack_frame(dshot_val, dev->config.dshot_config.telem_req);
1102-
_dshot_pack(global_idx, frame, current_arr);
1103-
packed_sel_aux |= (1u << i);
1104-
index++;
1105-
}
1106-
}
1107-
if (packed_sel_aux & 0x0003) {
1108-
_dshot_clean_cache(3);
1109-
_dshot_fire_tim(3);
1110-
}
1111994
} else {
1112995
/* unsupported protocol */
1113996
return 0;

0 commit comments

Comments
 (0)