diff --git a/Makefile b/Makefile
index 3668757dd870d3b7d9d6b798ff83d4b0f3ff7203..b649d633d05cc2c28bd80276995fa2057a508ef4 100755
--- a/Makefile
+++ b/Makefile
@@ -7,12 +7,12 @@ TARGET_FILES=src/cm730_fw.c
TARGET_FILES+=src/system_stm32f1xx.c
TARGET_FILES+=src/gpio.c
TARGET_FILES+=src/ram.c
-TARGET_FILES+=src/time.c
+TARGET_FILES+=src/darwin_time.c
TARGET_FILES+=src/eeprom.c
-TARGET_FILES+=src/comm.c
TARGET_FILES+=src/adc_dma.c
TARGET_FILES+=src/imu.c
-TARGET_FILES+=src/dynamixel_slave_uart_dma.c
+TARGET_FILES+=src/darwin_dyn_slave.c
+TARGET_FILES+=src/darwin_dyn_master.c
TARGET_FILES+=src/stm32f1xx_hal_msp.c
TARGET_PROCESSOR=STM32F103RE
@@ -22,13 +22,17 @@ include $(HAL_PATH)/select_processor.mk
STM32_STARTUP_FILES_PATH = $(HAL_PATH)/startup_code/
STM32_LINKER_SCRIPTS_PATH = ./linker_script
-DYNAMIXEL_LIBRARY_PATH=../../STM32_processor/libraries/dynamixel_base
+UTILS_PATH=../../STM32_processor/libraries/utils
+COMM_PATH=../../STM32_processor/libraries/comm
+USART_PATH=../../STM32_processor/libraries/f1/usart
+DYNAMIXEL_PATH=../../STM32_processor/libraries/dynamixel_base
BUILD_PATH=build
COMPILE_OPTS = -mlittle-endian -mcpu=cortex-m3 -mthumb -mthumb-interwork
COMPILE_OPTS += -Wall -O2 -fno-common -msoft-float
COMPILE_OPTS += -ffreestanding -nostdlib -D$(PROCESSOR_MACRO)
-INCLUDE_DIRS = -I$(HAL_PATH)/include -I$(HAL_PATH)/include/core -I$(HAL_PATH)/include/devices -I$(DYNAMIXEL_LIBRARY_PATH)/include -I./include
+INCLUDE_DIRS = -I$(HAL_PATH)/include -I$(HAL_PATH)/include/core -I$(HAL_PATH)/include/devices
+INCLUDE_DIRS += -I$(UTILS_PATH)/include -I$(COMM_PATH)/include -I$(DYNAMIXEL_PATH)/include -I$(USART_PATH)/include -I./include
TCHAIN_PREFIX=arm-none-eabi-
@@ -40,6 +44,7 @@ ASFLAGS = $(COMPILE_OPTS) -c
LD = $(TCHAIN_PREFIX)gcc
LDFLAGS = -mthumb -mcpu=cortex-m3 -Wl,-Map=$@.map,-cref $(INCLUDE_DIRS) -T $(STM32_LINKER_SCRIPTS_PATH)/darwin.ld --specs=nosys.specs
+EXT_LIB = $(COMM_PATH)/lib/comm_m3.a $(UTILS_PATH)/lib/utils_m3.a $(DYNAMIXEL_PATH)/lib/dynamixel_m3.a
OBJCP = $(TCHAIN_PREFIX)objcopy
OBJCPFLAGS_HEX = -O ihex
@@ -69,6 +74,9 @@ TARGET_FILES+=$(HAL_PATH)/src/stm32f1xx_hal_adc_ex.c
TARGET_FILES+=$(HAL_PATH)/src/stm32f1xx_hal_spi.c
TARGET_FILES+=$(HAL_PATH)/src/stm32f1xx_hal.c
+TARGET_FILES+=$(USART_PATH)/src/usart3.c
+TARGET_FILES+=$(USART_PATH)/src/usart1.c
+
DARWIN_OBJS_TMP = $(notdir $(TARGET_FILES:.c=.o))
DARWIN_OBJS = $(patsubst %,$(BUILD_PATH)/%,$(DARWIN_OBJS_TMP))
@@ -83,9 +91,11 @@ $(BUILD_PATH)/%.o: $(HAL_PATH)/src/devices/%.c
$(CC) -c $(CFLAGS) -o $@ $<
$(BUILD_PATH)/%.o: $(HAL_PATH)/src/%.c
$(CC) -c $(CFLAGS) -o $@ $<
+$(BUILD_PATH)/%.o: $(USART_PATH)/src/%.c
+ $(CC) -c $(CFLAGS) -o $@ $<
$(MAIN_OUT_ELF): mkdir_build $(DARWIN_OBJS) $(BUID_PATH)/$(STARTUP_FILE:.s=.o)
- $(LD) $(LDFLAGS) $(DARWIN_OBJS) $(BUILD_PATH)/$(STARTUP_FILE:.s=.o) $(DYNAMIXEL_LIBRARY_PATH)/lib/dynamixel_soft.a --output $@
+ $(LD) $(LDFLAGS) $(DARWIN_OBJS) $(BUILD_PATH)/$(STARTUP_FILE:.s=.o) $(EXT_LIB) --output $@
$(MAIN_OUT_HEX): $(MAIN_OUT_ELF)
$(OBJCP) $(OBJCPFLAGS_HEX) $< $@
diff --git a/include/comm.h b/include/darwin_comm.h
similarity index 100%
rename from include/comm.h
rename to include/darwin_comm.h
diff --git a/include/darwin_dyn_master.h b/include/darwin_dyn_master.h
new file mode 100644
index 0000000000000000000000000000000000000000..47f387a0fc9f10f2c926cdb6d3fc5ab8f1b43ddf
--- /dev/null
+++ b/include/darwin_dyn_master.h
@@ -0,0 +1,16 @@
+#ifndef _DARWIN_DYN_MASTER_H
+#define _DARWIN_DYN_MASTER_H
+
+#include "stm32f1xx_hal.h"
+#include "stm32_time.h"
+#include "comm.h"
+#include "dynamixel_master.h"
+
+extern TDynamixelMaster darwin_dyn_master;
+
+void darwin_dyn_master_init(void);
+inline void darwin_dyn_master_enable_power(void);
+inline void darwin_dyn_master_disable_power(void);
+
+#endif
+
diff --git a/include/darwin_dyn_slave.h b/include/darwin_dyn_slave.h
new file mode 100644
index 0000000000000000000000000000000000000000..6790437e59a306d7341fe5a0fbd100eaf5463e17
--- /dev/null
+++ b/include/darwin_dyn_slave.h
@@ -0,0 +1,14 @@
+#ifndef _DARWIN_DYN_SLAVE_H
+#define _DARWIN_DYN_SLAVE_H
+
+#include "stm32f1xx_hal.h"
+#include "stm32_time.h"
+#include "comm.h"
+#include "dynamixel_slave.h"
+
+extern TDynamixelSlave darwin_dyn_slave;
+
+void darwin_dyn_slave_init(void);
+
+#endif
+
diff --git a/include/darwin_time.h b/include/darwin_time.h
new file mode 100755
index 0000000000000000000000000000000000000000..6408b196c7d2cce7efd8aba326419713cc384f5b
--- /dev/null
+++ b/include/darwin_time.h
@@ -0,0 +1,11 @@
+#ifndef _DARWIN_TIME_H
+#define _DARWIN_TIME_H
+
+#include "stm32f1xx_hal.h"
+
+void darwin_time_init(void);
+unsigned long long int darwin_time_get_counts(void);
+unsigned int darwin_time_get_counts_per_us(void);
+
+#endif
+
diff --git a/include/dynamixel_master_uart_dma.h b/include/dynamixel_master_uart_dma.h
deleted file mode 100755
index 8a4940052a24a7787f28c4399e59129cefdb664c..0000000000000000000000000000000000000000
--- a/include/dynamixel_master_uart_dma.h
+++ /dev/null
@@ -1,41 +0,0 @@
-#ifndef _DYNAMIXEL_MASTER_DMA_H
-#define _DYNAMIXEL_MASTER_DMA_H
-
-#include "stm32f10x.h"
-#include "dynamixel.h"
-#include "dynamixel2.h"
-
-// dynamixel master functions
-void dyn_master_init(void);
-void dyn_master_reset(void);
-void dyn_master_set_timeout(uint16_t timeout_ms);
-void dyn_master_scan(uint8_t *num,uint8_t *ids);
-void dyn2_master_scan(uint8_t *num,uint8_t *ids);
-inline void dyn_master_enable_power(void);
-inline void dyn_master_disable_power(void);
-uint8_t dyn_master_is_power_enabled(void);
-uint8_t dyn_master_ping(uint8_t id);
-uint8_t dyn2_master_ping(uint8_t id);
-uint8_t dyn_master_read_byte(uint8_t id,uint8_t address,uint8_t *data);
-uint8_t dyn2_master_read_byte(uint8_t id,uint16_t address,uint8_t *data);
-uint8_t dyn_master_read_word(uint8_t id,uint8_t address,uint16_t *data);
-uint8_t dyn2_master_read_word(uint8_t id,uint16_t address,uint16_t *data);
-uint8_t dyn_master_read_table(uint8_t id,uint8_t address,uint8_t length,uint8_t *data);
-uint8_t dyn2_master_read_table(uint8_t id,uint16_t address,uint16_t length,uint8_t *data);
-uint8_t dyn_master_write_byte(uint8_t id, uint8_t address, uint8_t data);
-uint8_t dyn2_master_write_byte(uint8_t id, uint16_t address, uint8_t data);
-uint8_t dyn_master_write_word(uint8_t id, uint8_t address, uint16_t data);
-uint8_t dyn2_master_write_word(uint8_t id, uint16_t address, uint16_t data);
-uint8_t dyn_master_write_table(uint8_t id, uint8_t address, uint8_t length, uint8_t *data);
-uint8_t dyn2_master_write_table(uint8_t id, uint16_t address, uint16_t length, uint8_t *data);
-uint8_t dyn_master_reg_write(uint8_t id, uint8_t address, uint8_t length, uint8_t *data);
-uint8_t dyn2_master_reg_write(uint8_t id, uint16_t address, uint16_t length, uint8_t *data);
-uint8_t dyn_master_action(void);
-uint8_t dyn2_master_action(void);
-uint8_t dyn_master_sync_write(uint8_t num,uint8_t *ids,uint8_t address, uint8_t length, TWriteData *data);
-uint8_t dyn2_master_sync_write(uint8_t num,uint8_t *ids,uint16_t address, uint16_t length, TWriteData *data);
-uint8_t dyn_master_bulk_read(uint8_t num,uint8_t *ids,uint8_t *address, uint8_t *length, TWriteData *data);
-// repeater functions
-uint8_t dyn_master_resend_inst_packet(uint8_t *inst_packet, uint8_t *status_packet);
-
-#endif
diff --git a/include/dynamixel_slave_uart_dma.h b/include/dynamixel_slave_uart_dma.h
deleted file mode 100755
index 58dc177a8437339e8636f99a67d744ad0f4d42f3..0000000000000000000000000000000000000000
--- a/include/dynamixel_slave_uart_dma.h
+++ /dev/null
@@ -1,20 +0,0 @@
-#ifndef _DYNAMIXEL_SLAVE_UART_DMA_H
-#define _DYNAXIXEL_SLAVE_UART_DMA_H
-
-#include "dynamixel.h"
-#include "stm32f1xx_hal.h"
-
-// public functions
-void dyn_slave_init(void);
-inline void dyn_slave_set_address(uint8_t id);
-inline uint8_t dyn_slave_get_address(void);
-inline void dyn_slave_set_return_delay(uint8_t delay);
-inline void dyn_slave_set_return_level(uint8_t level);
-inline uint8_t dyn_slave_get_return_level(void);
-uint8_t dyn_slave_is_packet_ready(void);
-void dyn_slave_get_inst_packet(uint8_t *packet);
-void dyn_slave_send_status_packet(uint8_t error,uint8_t length, uint8_t *data);
-void dyn_slave_resend_status_packet(uint8_t *packet);
-void dyn_slave_reset(void);
-
-#endif
diff --git a/include/time.h b/include/time.h
deleted file mode 100755
index 3e495e8a3f3584f6373b5855269940c225ceac9a..0000000000000000000000000000000000000000
--- a/include/time.h
+++ /dev/null
@@ -1,9 +0,0 @@
-#ifndef _TIME_H
-#define _TIME_H
-
-#include "stm32f1xx_hal.h"
-
-void time_init(void);
-void delay_us(uint32_t us);
-
-#endif
diff --git a/src/adc_dma.c b/src/adc_dma.c
index 684b0fea023d4f5b3a74fa81182b99678f5ad993..e8fc3e7a7d15326128c67f00760e402fa56601db 100755
--- a/src/adc_dma.c
+++ b/src/adc_dma.c
@@ -1,6 +1,5 @@
#include "adc_dma.h"
#include "ram.h"
-#include "gpio.h"
#define ADC1_CH1 ADC_CHANNEL_0
#define ADC1_CH2 ADC_CHANNEL_1
@@ -245,7 +244,6 @@ void adc_init(void)
HAL_ADC_Start(&hadc2);
HAL_ADCEx_MultiModeStart_DMA(&hadc1, (uint32_t *)&ram_data[DARWIN_MIC1_L],16);
- gpio_set_led(LED_RX);
// ADC_Enable(&hadc2);
// ADC_Enable(&hadc1);
// __HAL_ADC_CLEAR_FLAG(&hadc1, ADC_FLAG_EOC);
diff --git a/src/cm730_fw.c b/src/cm730_fw.c
index 871c4cdb5c4205445aab78d5653e693800c2255f..d3f081c996cc5b08adc34bd341195d5c7386386d 100755
--- a/src/cm730_fw.c
+++ b/src/cm730_fw.c
@@ -1,11 +1,11 @@
#include "stm32f1xx_hal.h"
#include "gpio.h"
-#include "time.h"
#include "eeprom.h"
#include "ram.h"
-#include "comm.h"
#include "adc_dma.h"
-#include "dynamixel_slave_uart_dma.h"
+#include "darwin_time.h"
+#include "darwin_dyn_slave.h"
+#include "darwin_dyn_master.h"
//#include "motion_manager.h"
//#include "action.h"
//#include "walking.h"
@@ -19,11 +19,12 @@
void SysTick_Handler(void)
{
HAL_IncTick();
+ HAL_SYSTICK_IRQHandler();
}
int main(void)
{
- uint16_t eeprom_data;
+// uint16_t eeprom_data;
// uint16_t period,count=0;
/* initialize the dynamixel master interface */
@@ -42,33 +43,48 @@ int main(void)
// dyn_master_enable_power();
// delay_ms(1000);
-
-// /* initialize the HAL module */
+ /* initialize the HAL module */
HAL_Init();
+ HAL_Delay(1000);
/* initialize the GPIO module */
gpio_init();
- /* initialize the us delay module */
- time_init();
/* initialize EEPROM */
EE_Init();
// initialize the Dynamixel RAM memory space
ram_init();
// initialize adc
adc_init();
- /* initialize the dynamixel slave interface*/
- dyn_slave_init();
- EE_ReadVariable(DEVICE_ID_OFFSET,&eeprom_data);
- dyn_slave_set_address((uint8_t)eeprom_data);
- EE_ReadVariable(RETURN_DELAY_OFFSET,&eeprom_data);
- dyn_slave_set_return_delay((uint8_t)eeprom_data);
- EE_ReadVariable(RETURN_LEVEL_OFFSET,&eeprom_data);
- dyn_slave_set_return_level((uint8_t)eeprom_data);
- // initlaize the communication module
- comm_init();
+ // initialize time module
+ darwin_time_init();
+ /* initialize the dynamixel slave interface */
+ darwin_dyn_slave_init();
+ /* initialize the dynamixel master interface */
+ darwin_dyn_master_init();
+// EE_ReadVariable(DEVICE_ID_OFFSET,&eeprom_data);
+// dyn_slave_set_address((uint8_t)eeprom_data);
+// EE_ReadVariable(RETURN_DELAY_OFFSET,&eeprom_data);
+// dyn_slave_set_return_delay((uint8_t)eeprom_data);
+// EE_ReadVariable(RETURN_LEVEL_OFFSET,&eeprom_data);
+// dyn_slave_set_return_level((uint8_t)eeprom_data);
+ // initlaize the communication moduleKE
+
+// comm_init();
+
- gpio_blink_led(LED_5_R,1000);
+ darwin_dyn_master_enable_power();
+ HAL_Delay(1000);
+ gpio_set_led(LED_3);
+ if(dyn_master_ping(&darwin_dyn_master,0x01)==DYN_SUCCESS)
+ gpio_set_led(LED_2);
+ else
+ gpio_clear_led(LED_2);
+ darwin_dyn_master_disable_power();
- comm_start();
- while(1); /* main function does not return */
+// comm_start();
+ while(1)/* main function does not return */
+ {
+ HAL_Delay(1000);
+ gpio_toggle_led(LED_4);
+ }
}
diff --git a/src/darwin_dyn_master.c b/src/darwin_dyn_master.c
new file mode 100755
index 0000000000000000000000000000000000000000..b434ad76a9ce070edd67e17c86bf7db86b03224d
--- /dev/null
+++ b/src/darwin_dyn_master.c
@@ -0,0 +1,99 @@
+#include "darwin_dyn_master.h"
+#include "darwin_time.h"
+#include "usart1.h"
+
+#define ENABLE_RX_EN_GPIO_CLK __GPIOB_CLK_ENABLE()
+#define RX_EN_PIN GPIO_PIN_5
+#define RX_EN_GPIO_PORT GPIOB
+
+#define ENABLE_TX_EN_GPIO_CLK __GPIOB_CLK_ENABLE()
+#define TX_EN_PIN GPIO_PIN_4
+#define TX_EN_GPIO_PORT GPIOB
+
+#define POWER_GPIO_CLK __GPIOB_CLK_ENABLE()
+#define POWER_PIN GPIO_PIN_8
+#define POWER_GPIO_PORT GPIOB
+
+/* private variables */
+TDynamixelMaster darwin_dyn_master;
+TTime darwin_dyn_master_timer;
+TComm darwin_dyn_master_comm;
+
+// private functions
+void darwin_dyn_master_set_rx_mode(void)
+{
+ HAL_GPIO_WritePin(TX_EN_GPIO_PORT,TX_EN_PIN,GPIO_PIN_RESET);
+ HAL_GPIO_WritePin(RX_EN_GPIO_PORT,RX_EN_PIN,GPIO_PIN_SET);
+}
+
+void darwin_dyn_master_set_tx_mode(void)
+{
+ HAL_GPIO_WritePin(RX_EN_GPIO_PORT,RX_EN_PIN,GPIO_PIN_RESET);
+ HAL_GPIO_WritePin(TX_EN_GPIO_PORT,TX_EN_PIN,GPIO_PIN_SET);
+}
+
+// public functions
+void darwin_dyn_master_init(void)
+{
+ GPIO_InitTypeDef GPIO_InitStructure;
+ TUSART_IRQ_Priorities priorities;
+ UART_InitTypeDef Init;
+
+ // initialize timer structure
+ time_init(&darwin_dyn_master_timer,darwin_time_get_counts_per_us(),darwin_time_get_counts);
+
+ // initialize GPIO
+ ENABLE_RX_EN_GPIO_CLK;
+ ENABLE_TX_EN_GPIO_CLK;
+
+ GPIO_InitStructure.Pin = RX_EN_PIN;
+ GPIO_InitStructure.Mode = GPIO_MODE_OUTPUT_PP;
+ GPIO_InitStructure.Speed = GPIO_SPEED_HIGH;
+ GPIO_InitStructure.Pull = GPIO_NOPULL;
+ HAL_GPIO_Init(RX_EN_GPIO_PORT, &GPIO_InitStructure);
+
+ GPIO_InitStructure.Pin = TX_EN_PIN;
+ HAL_GPIO_Init(TX_EN_GPIO_PORT, &GPIO_InitStructure);
+
+ darwin_dyn_master_set_rx_mode();
+
+ GPIO_InitStructure.Pin = POWER_PIN;
+ GPIO_InitStructure.Mode = GPIO_MODE_OUTPUT_PP;
+ GPIO_InitStructure.Speed = GPIO_SPEED_HIGH;
+ HAL_GPIO_Init(POWER_GPIO_PORT, &GPIO_InitStructure);
+
+ darwin_dyn_master_disable_power();
+
+ /* initialize the comm object */
+ comm_init(&darwin_dyn_master_comm,0x01,&darwin_dyn_master_timer);
+ Init.BaudRate = 1000000;
+ Init.WordLength = UART_WORDLENGTH_8B;
+ Init.StopBits = UART_STOPBITS_1;
+ Init.Parity = UART_PARITY_NONE;
+ Init.Mode = UART_MODE_TX_RX;
+ Init.HwFlowCtl = UART_HWCONTROL_NONE;
+ Init.OverSampling = UART_OVERSAMPLING_16;
+
+ priorities.irq_priority=0;
+ priorities.irq_subpriority=1;
+ priorities.dma_rx_priority=1;
+ priorities.dma_rx_subpriority=3;
+ priorities.dma_tx_priority=1;
+ priorities.dma_tx_subpriority=2;
+
+ usart1_init(&darwin_dyn_master_comm,&Init,&priorities);
+ dyn_master_init(&darwin_dyn_master,&darwin_dyn_master_comm);
+ darwin_dyn_master.set_rx_mode=darwin_dyn_master_set_rx_mode;
+ darwin_dyn_master.set_tx_mode=darwin_dyn_master_set_tx_mode;
+}
+
+inline void darwin_dyn_master_enable_power(void)
+{
+ HAL_GPIO_WritePin(POWER_GPIO_PORT,POWER_PIN,GPIO_PIN_SET);
+}
+
+inline void darwin_dyn_master_disable_power(void)
+{
+ HAL_GPIO_WritePin(POWER_GPIO_PORT,POWER_PIN,GPIO_PIN_RESET);
+}
+
diff --git a/src/darwin_dyn_slave.c b/src/darwin_dyn_slave.c
new file mode 100755
index 0000000000000000000000000000000000000000..00b60f1cd38210ff7949fba23d525827ce045872
--- /dev/null
+++ b/src/darwin_dyn_slave.c
@@ -0,0 +1,53 @@
+#include "darwin_dyn_slave.h"
+#include "darwin_time.h"
+#include "usart3.h"
+
+/* private variables */
+TDynamixelSlave darwin_dyn_slave;
+TTime darwin_dyn_slave_timer;
+TComm darwin_dyn_slave_comm;
+
+// private functions
+unsigned char darwin_on_read(unsigned short int address,unsigned short int length,unsigned char *data)
+{
+ return 0x00;
+}
+
+unsigned char darwin_on_write(unsigned short int address,unsigned short int length,unsigned char *data)
+{
+ return 0x00;
+}
+
+// public functions
+void darwin_dyn_slave_init(void)
+{
+ TUSART_IRQ_Priorities priorities;
+ UART_InitTypeDef Init;
+
+ // initialize timer structure
+ time_init(&darwin_dyn_slave_timer,darwin_time_get_counts_per_us(),darwin_time_get_counts);
+
+ /* initialize the comm object */
+ comm_init(&darwin_dyn_slave_comm,0x01,&darwin_dyn_slave_timer);
+ Init.BaudRate = 921600;
+ Init.WordLength = UART_WORDLENGTH_8B;
+ Init.StopBits = UART_STOPBITS_1;
+ Init.Parity = UART_PARITY_NONE;
+ Init.Mode = UART_MODE_TX_RX;
+ Init.HwFlowCtl = UART_HWCONTROL_NONE;
+ Init.OverSampling = UART_OVERSAMPLING_16;
+
+ priorities.irq_priority=0;
+ priorities.irq_subpriority=0;
+ priorities.dma_rx_priority=1;
+ priorities.dma_rx_subpriority=1;
+ priorities.dma_tx_priority=1;
+ priorities.dma_tx_subpriority=0;
+
+ usart3_init(&darwin_dyn_slave_comm,&Init,&priorities);
+ dyn_slave_init(&darwin_dyn_slave,&darwin_dyn_slave_comm,0x01);
+ darwin_dyn_slave.on_read=darwin_on_read;
+ darwin_dyn_slave.on_write=darwin_on_write;
+ //dyn_slave_set_return_delay(&battery_dyn_slave,ram_data[BATTERY_RETURN_DELAY_TIME]);
+ //dyn_slave_set_return_level(&battery_dyn_slave,ram_data[BATTERY_STATUS_RETURN_LEVEL]);
+}
diff --git a/src/darwin_time.c b/src/darwin_time.c
new file mode 100755
index 0000000000000000000000000000000000000000..d96933101d1c7580eeb9f31fb85d6fe1ac9599e9
--- /dev/null
+++ b/src/darwin_time.c
@@ -0,0 +1,79 @@
+#include "darwin_time.h"
+#include "gpio.h"
+
+TIM_HandleTypeDef us_timer_Handle;
+unsigned long long int counts;
+
+void TIM1_UP_IRQHandler(void)
+{
+ if(__HAL_TIM_GET_FLAG(&us_timer_Handle, TIM_FLAG_UPDATE) != RESET)
+ {
+ if(__HAL_TIM_GET_IT_SOURCE(&us_timer_Handle, TIM_IT_UPDATE) !=RESET)
+ {
+ __HAL_TIM_CLEAR_IT(&us_timer_Handle, TIM_IT_UPDATE);
+ counts+=1;
+ }
+ }
+ if(__HAL_TIM_GET_FLAG(&us_timer_Handle, TIM_FLAG_BREAK) != RESET)
+ {
+ if(__HAL_TIM_GET_IT_SOURCE(&us_timer_Handle, TIM_IT_BREAK) !=RESET)
+ {
+ __HAL_TIM_CLEAR_IT(&us_timer_Handle, TIM_IT_BREAK);
+ }
+ }
+ /* TIM Trigger detection event */
+ if(__HAL_TIM_GET_FLAG(&us_timer_Handle, TIM_FLAG_TRIGGER) != RESET)
+ {
+ if(__HAL_TIM_GET_IT_SOURCE(&us_timer_Handle, TIM_IT_TRIGGER) !=RESET)
+ {
+ __HAL_TIM_CLEAR_IT(&us_timer_Handle, TIM_IT_TRIGGER);
+ }
+ }
+ /* TIM commutation event */
+ if(__HAL_TIM_GET_FLAG(&us_timer_Handle, TIM_FLAG_COM) != RESET)
+ {
+ if(__HAL_TIM_GET_IT_SOURCE(&us_timer_Handle, TIM_IT_COM) !=RESET)
+ {
+ __HAL_TIM_CLEAR_IT(&us_timer_Handle, TIM_FLAG_COM);
+ }
+ }
+}
+
+void darwin_time_init(void)
+{
+ TIM_ClockConfigTypeDef sClockSourceConfig;
+ TIM_MasterConfigTypeDef sMasterConfig;
+
+ us_timer_Handle.Instance=TIM1;
+ us_timer_Handle.Init.Period=0xFFFF;
+ us_timer_Handle.Init.Prescaler=0;
+ us_timer_Handle.Init.ClockDivision=TIM_CLOCKDIVISION_DIV1;
+ us_timer_Handle.Init.CounterMode=TIM_COUNTERMODE_UP;
+ HAL_TIM_Base_Init(&us_timer_Handle);
+ __TIM1_CLK_ENABLE();
+ __HAL_TIM_SetCounter(&us_timer_Handle,0);
+
+ sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
+ HAL_TIM_ConfigClockSource(&us_timer_Handle, &sClockSourceConfig);
+
+ sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
+ sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
+ HAL_TIMEx_MasterConfigSynchronization(&us_timer_Handle, &sMasterConfig);
+
+ // enable interrupts
+ HAL_NVIC_SetPriority(TIM1_UP_IRQn, 3, 3);
+ HAL_NVIC_EnableIRQ(TIM1_UP_IRQn);
+ HAL_TIM_Base_Start_IT(&us_timer_Handle);
+
+ counts=0;
+}
+
+unsigned long long int darwin_time_get_counts(void)
+{
+ return (counts<<16)+__HAL_TIM_GetCounter(&us_timer_Handle);
+}
+
+unsigned int darwin_time_get_counts_per_us(void)
+{
+ return (SystemCoreClock/1000000)+1;
+}
diff --git a/src/dynamixel_master_uart_dma.c b/src/dynamixel_master_uart_dma.c
deleted file mode 100755
index 0f9bb92c2ec7f27556b8049a81cfa255a1dab53d..0000000000000000000000000000000000000000
--- a/src/dynamixel_master_uart_dma.c
+++ /dev/null
@@ -1,953 +0,0 @@
-#include "dynamixel_master_uart_dma.h"
-#include "motion_manager.h"
-#include "time.h"
-#include "ram.h"
-
-#define USART USART1
-#define USART_CLK RCC_APB2Periph_USART1
-#define USART_CLK_INIT RCC_APB2PeriphClockCmd
-#define USART_IRQn USART1_IRQn
-#define USART_IRQHandler USART1_IRQHandler
-
-#define USART_TX_PIN GPIO_Pin_6
-#define USART_TX_GPIO_PORT GPIOB
-#define USART_TX_GPIO_CLK RCC_APB2Periph_GPIOB
-#define USART_TX_SOURCE GPIO_PinSource6
-
-#define USART_RX_PIN GPIO_Pin_7
-#define USART_RX_GPIO_PORT GPIOB
-#define USART_RX_GPIO_CLK RCC_APB2Periph_GPIOB
-#define USART_RX_SOURCE GPIO_PinSource7
-
-#define USART_TX_EN_PIN GPIO_Pin_4
-#define USART_TX_EN_GPIO_PORT GPIOB
-#define USART_TX_EN_GPIO_CLK RCC_APB2Periph_GPIOB
-#define USART_TX_EN_SOURCE GPIO_PinSource4
-
-#define USART_RX_EN_PIN GPIO_Pin_5
-#define USART_RX_EN_GPIO_PORT GPIOB
-#define USART_RX_EN_GPIO_CLK RCC_APB2Periph_GPIOB
-#define USART_RX_EN_SOURCE GPIO_PinSource5
-
-/* DMA configuration */
-#define USART_DR_ADDRESS ((uint32_t)USART1 + 0x04)
-#define USART_DMA DMA1
-#define USART_DMA_CLK RCC_AHBPeriph_DMA1
-
-#define USART_TX_DMA_CHANNEL DMA1_Channel4
-#define USART_TX_DMA_FLAG_GLIF DMA1_FLAG_GL4
-#define USART_TX_DMA_FLAG_TEIF DMA1_FLAG_TE4
-#define USART_TX_DMA_FLAG_HTIF DMA1_FLAG_HT4
-#define USART_TX_DMA_FLAG_TCIF DMA1_FLAG_TC4
-
-#define USART_RX_DMA_CHANNEL DMA1_Channel5
-#define USART_RX_DMA_FLAG_GLIF DMA1_FLAG_GL5
-#define USART_RX_DMA_FLAG_TEIF DMA1_FLAG_TE5
-#define USART_RX_DMA_FLAG_HTIF DMA1_FLAG_HT5
-#define USART_RX_DMA_FLAG_TCIF DMA1_FLAG_TC5
-
-#define USART_DMA_TX_IRQn DMA1_Channel4_IRQn
-#define USART_DMA_RX_IRQn DMA1_Channel5_IRQn
-#define USART_DMA_TX_IRQHandler DMA1_Channel4_IRQHandler
-#define USART_DMA_RX_IRQHandler DMA1_Channel5_IRQHandler
-
-#define POWER_GPIO_CLK RCC_APB2Periph_GPIOB
-#define POWER_PIN GPIO_Pin_8
-#define POWER_GPIO_PORT GPIOB
-#define POWER_SOURCE GPIO_PinSource8
-
-
-#define MAX_BUFFER_LEN 1024
-
-// private variables
-uint8_t dyn_master_version;
-uint16_t dyn_master_timeout;// answer reception timeout
-// input buffer
-uint8_t dyn_master_rx_buffer[MAX_BUFFER_LEN];
-// output buffer
-uint8_t dyn_master_tx_buffer[MAX_BUFFER_LEN];
-// instruction packet ready flag
-volatile uint8_t dyn_master_packet_ready;
-// sending status packet flag
-volatile uint8_t dyn_master_sending_packet;
-// no answer for sync write operation
-uint8_t dyn_master_no_answer;
-// power enabled variable
-uint8_t dyn_master_power_enabled;
-// number of packets to receive
-uint8_t dyn_master_num_read_packets;
-uint8_t dyn_master_packet_length;
-// DMA initialization data structures
-DMA_InitTypeDef DMA_TX_InitStructure;
-DMA_InitTypeDef DMA_RX_InitStructure;
-
-// private functions
-void dyn_master_reset(void)
-{
- /* Reset reception */
- USART_DMACmd(USART, USART_DMAReq_Rx, DISABLE);
- DMA_Cmd(USART_RX_DMA_CHANNEL,DISABLE);
- /* wait until the transaction ends */
- DMA_ClearFlag(USART_RX_DMA_FLAG_GLIF);
- DMA_ClearITPendingBit(USART_RX_DMA_FLAG_GLIF);
- /* clear any pending data */
- USART_ReceiveData(USART);
- /* Reset transmission */
- USART_DMACmd(USART, USART_DMAReq_Tx, DISABLE);
- DMA_Cmd(USART_TX_DMA_CHANNEL,DISABLE);
- DMA_ClearFlag(USART_TX_DMA_FLAG_GLIF);
- DMA_ClearITPendingBit(USART_TX_DMA_FLAG_GLIF);
- USART_ITConfig(USART, USART_IT_TC, DISABLE);
- /* Reset variables */
- dyn_master_packet_ready=0x00;
- dyn_master_sending_packet=0x00;
-}
-
-uint8_t dyn_master_wait_sending(void)
-{
- uint16_t timeout_left=dyn_master_timeout*1000;
-
- while(dyn_master_sending_packet)
- {
- delay_us(100);
- timeout_left-=100;
- if(timeout_left<=0)
- {
- dyn_master_reset();
- return DYN_TIMEOUT;
- }
- }
-
- return DYN_SUCCESS;
-}
-
-uint8_t dyn_master_send(void)
-{
- uint8_t error;
-
- dyn_master_version=1;
- // wait until any previous transmission ends
- if((error=dyn_master_wait_sending())!=DYN_SUCCESS)
- return error;
- // set the DMA transfer
- DMA_SetCurrDataCounter(USART_TX_DMA_CHANNEL,dyn_get_length(dyn_master_tx_buffer)+4);
- DMA_Cmd(USART_TX_DMA_CHANNEL,ENABLE);
- USART_ClearFlag(USART,USART_FLAG_TC);
- USART_ClearITPendingBit(USART,USART_FLAG_TC);
- USART_DMACmd(USART, USART_DMAReq_Tx, ENABLE);
- dyn_master_sending_packet=0x01;
- return error;
-}
-
-uint8_t dyn2_master_send(void)
-{
- uint8_t error;
-
- dyn_master_version=2;
- // wait until any previous transmission ends
- if((error=dyn_master_wait_sending())!=DYN_SUCCESS)
- return error;
- // set the DMA transfer
- DMA_SetCurrDataCounter(USART_TX_DMA_CHANNEL,dyn2_get_length(dyn_master_tx_buffer)+7);
- DMA_Cmd(USART_TX_DMA_CHANNEL,ENABLE);
- USART_ClearFlag(USART,USART_FLAG_TC);
- USART_ClearITPendingBit(USART,USART_FLAG_TC);
- USART_DMACmd(USART, USART_DMAReq_Tx, ENABLE);
- dyn_master_sending_packet=0x01;
- return error;
-}
-
-uint8_t dyn_master_receive(void)
-{
- uint16_t timeout_left=dyn_master_timeout*1000;
-
- // wait for the status packet
- while(!dyn_master_packet_ready)
- {
- delay_us(100);
- timeout_left-=100;
- if(timeout_left<=0)
- {
- dyn_master_reset();
- return DYN_TIMEOUT;
- }
- }
- dyn_master_packet_ready=0x00;
- // check the input packet checksum
- if(dyn_check_checksum(dyn_master_rx_buffer)==0xFF)
- return dyn_get_status_error(dyn_master_rx_buffer);
- else
- {
- dyn_master_reset();
- return DYN_CHECKSUM_ERROR;
- }
-}
-
-uint8_t dyn2_master_receive(void)
-{
- uint16_t timeout_left=dyn_master_timeout*1000;
-
- // wait for the status packet
- while(!dyn_master_packet_ready)
- {
- delay_us(100);
- timeout_left-=100;
- if(timeout_left<=0)
- {
- dyn_master_reset();
- return DYN_TIMEOUT;
- }
- }
- dyn_master_packet_ready=0x00;
- // check the input packet checksum
- if(dyn2_check_checksum(dyn_master_rx_buffer)==0x01)
- return dyn2_get_status_error(dyn_master_rx_buffer);
- else
- {
- dyn_master_reset();
- return DYN_CHECKSUM_ERROR;
- }
-}
-
-void dyn_master_enable_tx(void)
-{
- GPIO_ResetBits(USART_RX_EN_GPIO_PORT, USART_RX_EN_PIN);
- GPIO_SetBits(USART_TX_EN_GPIO_PORT, USART_TX_EN_PIN);
-}
-
-void dyn_master_enable_rx(void)
-{
- GPIO_ResetBits(USART_TX_EN_GPIO_PORT, USART_TX_EN_PIN);
- GPIO_SetBits(USART_RX_EN_GPIO_PORT, USART_RX_EN_PIN);
-}
-
-uint8_t dyn_master_read(uint8_t id,uint8_t address,uint8_t length,uint8_t *data)
-{
- uint8_t error;
-
- /* generate the read packet */
- dyn_init_read_packet(dyn_master_tx_buffer,id,address,length);
- dyn_master_num_read_packets=0x01;
- // enable transmission
- dyn_master_enable_tx();
- // send the data
- if((error=dyn_master_send())!=DYN_SUCCESS)
- {
- dyn_master_enable_rx();
- return error;
- }
- // wait for the transmission to end
- if((error=dyn_master_wait_sending())!=DYN_SUCCESS)
- {
- dyn_master_enable_rx();
- return error;
- }
- dyn_master_enable_rx();
- // wait for the replay within the given timeout
- error=dyn_master_receive();
- if(error==DYN_NO_ERROR)
- dyn_get_read_status_data(dyn_master_rx_buffer,data);// read the data from the status packet
- else
- dyn_master_reset();
- return error;
-}
-
-uint8_t dyn2_master_read(uint8_t id,uint16_t address,uint16_t length,uint8_t *data)
-{
- uint8_t error;
-
- /* generate the read packet */
- dyn2_init_read_packet(dyn_master_tx_buffer,id,address,length);
- dyn_master_num_read_packets=0x01;
- // enable transmission
- dyn_master_enable_tx();
- // send the data
- if((error=dyn2_master_send())!=DYN_SUCCESS)
- {
- dyn_master_enable_rx();
- return error;
- }
- // wait for the transmission to end
- if((error=dyn_master_wait_sending())!=DYN_SUCCESS)
- {
- dyn_master_enable_rx();
- return error;
- }
- dyn_master_enable_rx();
- // wait for the replay within the given timeout
- error=dyn2_master_receive();
- if(error==DYN_NO_ERROR)
- dyn2_get_read_status_data(dyn_master_rx_buffer,data);// read the data from the status packet
- else
- dyn_master_reset();
- return error;
-}
-
-uint8_t dyn_master_write(uint8_t id, uint8_t address, uint8_t length, uint8_t *data)
-{
- uint8_t error;
-
- // generate the read packet for the desired device
- dyn_init_write_packet(dyn_master_tx_buffer,id,address,length,data);
- dyn_master_num_read_packets=0x01;
- // enable transmission
- dyn_master_enable_tx();
- // send the data
- if((error=dyn_master_send())!=DYN_SUCCESS)
- {
- dyn_master_enable_rx();
- return error;
- }
- // wait for the transmission to end
- if((error=dyn_master_wait_sending())!=DYN_SUCCESS)
- {
- dyn_master_enable_rx();
- return error;
- }
- dyn_master_enable_rx();
- // wait for the replay within the given timeout
- error=dyn_master_receive();
- if(error!=DYN_NO_ERROR)
- dyn_master_reset();
- return error;
-}
-
-uint8_t dyn2_master_write(uint8_t id, uint16_t address, uint16_t length, uint8_t *data)
-{
- uint8_t error;
-
- // generate the read packet for the desired device
- dyn2_init_write_packet(dyn_master_tx_buffer,id,address,length,data);
- dyn_master_num_read_packets=0x01;
- // enable transmission
- dyn_master_enable_tx();
- // send the data
- if((error=dyn2_master_send())!=DYN_SUCCESS)
- {
- dyn_master_enable_rx();
- return error;
- }
- // wait for the transmission to end
- if((error=dyn_master_wait_sending())!=DYN_SUCCESS)
- {
- dyn_master_enable_rx();
- return error;
- }
- dyn_master_enable_rx();
- // wait for the replay within the given timeout
- error=dyn2_master_receive();
- if(error!=DYN_NO_ERROR)
- dyn_master_reset();
- return error;
-}
-
-// interrupt handlers
-void USART_IRQHandler(void)
-{
- static uint8_t num_bytes=0;
- uint8_t data;
-
- if(USART_GetITStatus(USART, USART_IT_RXNE) != RESET)
- {
- USART_ClearFlag(USART,USART_FLAG_RXNE);
- data=USART_ReceiveData(USART);
- switch(num_bytes)
- {
- case 0: if(data==0xFF)
- {
- dyn_master_rx_buffer[num_bytes]=data;
- num_bytes++;
- }
- break;
- case 1: if(data==0xFF)
- {
- dyn_master_rx_buffer[num_bytes]=data;
- num_bytes++;
- }
- else num_bytes--;
- break;
- case 2: if(data!=0xFF)
- {
- dyn_master_rx_buffer[num_bytes]=data;
- num_bytes++;
- }
- break;
- case 3: dyn_master_rx_buffer[num_bytes]=data;
- num_bytes=0;
- /* disable USART RX interrupts */
- USART_ITConfig(USART,USART_IT_RXNE,DISABLE);
- /* enable DMA RX */
- DMA_RX_InitStructure.DMA_BufferSize = data;
- DMA_RX_InitStructure.DMA_MemoryBaseAddr = (uint32_t)&dyn_master_rx_buffer[dyn_master_packet_length+4];
- dyn_master_packet_length+=data+4;
- DMA_Init(USART_RX_DMA_CHANNEL,&DMA_RX_InitStructure);
- DMA_Cmd(USART_RX_DMA_CHANNEL,ENABLE);
- USART_DMACmd(USART, USART_DMAReq_Rx, ENABLE);
- break;
- default: break;
- }
- }
- else if(USART_GetITStatus(USART, USART_IT_TC) != RESET)
- {
- USART_ClearFlag(USART,USART_FLAG_TC);
- USART_ITConfig(USART, USART_IT_TC, DISABLE);
- dyn_master_sending_packet=0x00;
- if(dyn_master_no_answer)
- dyn_master_no_answer=0x00;
- else
- {
- /* enable RX interrupts */
- USART_ITConfig(USART, USART_IT_RXNE, ENABLE);
- }
- }
-}
-
-void USART_DMA_TX_IRQHandler(void)
-{
- DMA_Cmd(USART_TX_DMA_CHANNEL,DISABLE);
- DMA_ClearFlag(USART_TX_DMA_FLAG_GLIF);
- DMA_ClearITPendingBit(USART_TX_DMA_FLAG_GLIF);
- USART_DMACmd(USART, USART_DMAReq_Tx, DISABLE);
- USART_ITConfig(USART, USART_IT_TC, ENABLE);
-}
-
-void USART_DMA_RX_IRQHandler(void)
-{
- DMA_Cmd(USART_RX_DMA_CHANNEL,DISABLE);
- DMA_ClearFlag(USART_RX_DMA_FLAG_GLIF);
- DMA_ClearITPendingBit(USART_RX_DMA_FLAG_GLIF);
- USART_DMACmd(USART, USART_DMAReq_Rx, DISABLE);
- dyn_master_num_read_packets--;
- if(dyn_master_num_read_packets==0x00)
- {
- dyn_master_packet_length=0x00;
- dyn_master_packet_ready=0x01;
- }
- else
- {
- /* enable RX interrupts */
- USART_ITConfig(USART, USART_IT_RXNE, ENABLE);
- }
-}
-
-// dynamixel master functions
-void dyn_master_init(void)
-{
- uint16_t i;
- GPIO_InitTypeDef GPIO_InitStructure;
- NVIC_InitTypeDef NVIC_InitStructure;
- USART_InitTypeDef USART_InitStructure;
-
- /* Enable GPIO clock */
- USART_CLK_INIT(USART_CLK, ENABLE);
- RCC_APB1PeriphClockCmd(USART_TX_GPIO_CLK | USART_RX_GPIO_CLK | USART_TX_EN_GPIO_CLK | USART_RX_EN_GPIO_CLK, ENABLE);
- RCC_APB1PeriphClockCmd(POWER_GPIO_CLK, ENABLE);
- // configure the GPIO pins
-
- /* Connect USART pins to AF7 */
- GPIO_PinRemapConfig(GPIO_Remap_USART1, ENABLE);
-
- /* Configure USART Tx and Rx as alternate function push-pull */
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
- GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
- GPIO_InitStructure.GPIO_Pin = USART_TX_PIN;
- GPIO_Init(USART_TX_GPIO_PORT, &GPIO_InitStructure);
-
- GPIO_InitStructure.GPIO_Pin = USART_RX_PIN;
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
- GPIO_Init(USART_RX_GPIO_PORT, &GPIO_InitStructure);
-
- /* configure the control pins */
- GPIO_InitStructure.GPIO_Pin = USART_TX_EN_PIN;
- GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
- GPIO_Init(USART_TX_EN_GPIO_PORT, &GPIO_InitStructure);
- GPIO_InitStructure.GPIO_Pin = USART_RX_EN_PIN;
- GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
- GPIO_Init(USART_RX_EN_GPIO_PORT, &GPIO_InitStructure);
-
- dyn_master_enable_rx();
-
- // initialize the power enable gpio
- GPIO_InitStructure.GPIO_Pin = POWER_PIN;
- GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
- GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
- GPIO_Init(POWER_GPIO_PORT, &GPIO_InitStructure);
-
- dyn_master_disable_power();
-
- dyn_master_timeout=0x00;
- // initialize the buffers
- for(i=0;i<MAX_BUFFER_LEN;i++)
- {
- dyn_master_rx_buffer[i]=0x00;
- dyn_master_tx_buffer[i]=0x00;
- }
- // initialize the flags
- dyn_master_packet_ready=0x00;
- dyn_master_sending_packet=0x00;
- dyn_master_no_answer=0x00;
- dyn_master_num_read_packets=0x00;
- dyn_master_packet_length=0x00;
- dyn_master_version=1;
-
- USART_DeInit(USART);
- USART_StructInit(&USART_InitStructure);
- // configure the serial port
- USART_InitStructure.USART_BaudRate = 1000000;
- USART_InitStructure.USART_WordLength = USART_WordLength_8b;
- USART_InitStructure.USART_StopBits = USART_StopBits_1;
- USART_InitStructure.USART_Parity = USART_Parity_No;
- USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
- USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
- USART_Init(USART, &USART_InitStructure);
- NVIC_InitStructure.NVIC_IRQChannel = USART_IRQn;
- NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
- NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
- NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
- NVIC_Init(&NVIC_InitStructure);
-
- USART_ITConfig(USART, USART_IT_RXNE |
- USART_IT_ORE |
- USART_IT_TXE |
- USART_IT_CTS |
- USART_IT_LBD |
- USART_IT_IDLE |
- USART_IT_PE |
- USART_IT_ERR |
- USART_IT_TC, DISABLE);
-
- /* Clear all USART interrupt flags */
- USART_ClearFlag(USART,USART_FLAG_RXNE |
- USART_FLAG_ORE |
- USART_FLAG_TXE |
- USART_FLAG_CTS |
- USART_FLAG_LBD |
- USART_FLAG_IDLE |
- USART_FLAG_PE |
- USART_FLAG_TC);
-
- /* Clear all USART interrupt pending bits */
- USART_ClearITPendingBit(USART, USART_FLAG_RXNE |
- USART_FLAG_ORE |
- USART_FLAG_TXE |
- USART_FLAG_CTS |
- USART_FLAG_LBD |
- USART_FLAG_IDLE |
- USART_FLAG_PE |
- USART_FLAG_TC);
-
- // configure the DMA channels
- /* Configure TX DMA */
- RCC_AHBPeriphClockCmd(USART_DMA_CLK, ENABLE);
- DMA_DeInit(USART_TX_DMA_CHANNEL);
- DMA_TX_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
- DMA_TX_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
- DMA_TX_InitStructure.DMA_Mode = DMA_Mode_Normal;
- DMA_TX_InitStructure.DMA_PeripheralBaseAddr = (uint32_t) (&(USART->DR));
- DMA_TX_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
- DMA_TX_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
- DMA_TX_InitStructure.DMA_Priority = DMA_Priority_High;
- DMA_TX_InitStructure.DMA_DIR = DMA_DIR_PeripheralDST;
- DMA_TX_InitStructure.DMA_MemoryBaseAddr = (uint32_t)dyn_master_tx_buffer;
- DMA_Init(USART_TX_DMA_CHANNEL,&DMA_TX_InitStructure);
- /* initialize DMA interrupts */
- NVIC_InitStructure.NVIC_IRQChannel = USART_DMA_TX_IRQn;
- NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
- NVIC_InitStructure.NVIC_IRQChannelSubPriority = 2;
- NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
- NVIC_Init(&NVIC_InitStructure);
- DMA_ITConfig(USART_TX_DMA_CHANNEL,DMA_IT_TC,ENABLE);
- DMA_ITConfig(USART_TX_DMA_CHANNEL,DMA_IT_HT | DMA_IT_TE,DISABLE);
-
- /* Configure RX DMA */
- DMA_DeInit(USART_RX_DMA_CHANNEL);
- DMA_RX_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
- DMA_RX_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
- DMA_RX_InitStructure.DMA_Mode = DMA_Mode_Normal;
- DMA_RX_InitStructure.DMA_PeripheralBaseAddr = (uint32_t) (&(USART->DR));
- DMA_RX_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
- DMA_RX_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
- DMA_RX_InitStructure.DMA_Priority = DMA_Priority_High;
- DMA_RX_InitStructure.DMA_DIR = DMA_DIR_PeripheralSRC;
- DMA_RX_InitStructure.DMA_MemoryBaseAddr = (uint32_t)dyn_master_rx_buffer[4];
- DMA_Init(USART_RX_DMA_CHANNEL,&DMA_RX_InitStructure);
- /* initialize DMA interrupts */
- NVIC_InitStructure.NVIC_IRQChannel = USART_DMA_RX_IRQn;
- NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
- NVIC_InitStructure.NVIC_IRQChannelSubPriority = 3;
- NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
- NVIC_Init(&NVIC_InitStructure);
- DMA_ITConfig(USART_RX_DMA_CHANNEL,DMA_IT_TC,ENABLE);
- DMA_ITConfig(USART_RX_DMA_CHANNEL,DMA_IT_HT | DMA_IT_TE,DISABLE);
-
- /* Enable the USART2 */
- USART_Cmd(USART, ENABLE);
-}
-
-void dyn_master_set_timeout(uint16_t timeout_ms)
-{
- // save the desired timeout value
- dyn_master_timeout=timeout_ms;
-}
-
-void dyn_master_scan(uint8_t *num,uint8_t *ids)
-{
- uint8_t i;
-
- for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
- ids[0]=0;
- *num=0;
- for(i=0;i<254;i++)
- {
- if(dyn_master_ping(i)==DYN_NO_ERROR)// the device exists
- {
- ids[*num]=i;
- (*num)++;
- }
- }
-}
-
-void dyn2_master_scan(uint8_t *num,uint8_t *ids)
-{
- uint8_t i;
-
- for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
- ids[0]=0;
- *num=0;
- for(i=0;i<254;i++)
- {
- if(dyn2_master_ping(i)==DYN_NO_ERROR)// the device exists
- {
- ids[*num]=i;
- (*num)++;
- }
- }
-}
-
-inline void dyn_master_enable_power(void)
-{
- GPIO_SetBits(POWER_GPIO_PORT,POWER_PIN);
- dyn_master_power_enabled=0x01;
-}
-
-inline void dyn_master_disable_power(void)
-{
- GPIO_ResetBits(POWER_GPIO_PORT,POWER_PIN);
- dyn_master_power_enabled=0x00;
-}
-
-uint8_t dyn_master_is_power_enabled(void)
-{
- return dyn_master_power_enabled;
-}
-
-uint8_t dyn_master_ping(uint8_t id)
-{
- uint8_t error;
-
- // generate the ping packet for the desired device
- dyn_init_ping_packet(dyn_master_tx_buffer,id);
- dyn_master_num_read_packets=0x01;
- // enable transmission
- dyn_master_enable_tx();
- // send the data
- if((error=dyn_master_send())!=DYN_SUCCESS)
- {
- dyn_master_enable_rx();
- return error;
- }
- // wait for the transmission to end
- if((error=dyn_master_wait_sending())!=DYN_SUCCESS)
- {
- dyn_master_enable_rx();
- return error;
- }
- dyn_master_enable_rx();
- // wait for the replay within the given timeout
- error=dyn_master_receive();
- return error;
-}
-
-uint8_t dyn2_master_ping(uint8_t id)
-{
- uint8_t error;
-
- // generate the ping packet for the desired device
- dyn2_init_ping_packet(dyn_master_tx_buffer,id);
- dyn_master_num_read_packets=0x01;
- // enable transmission
- dyn_master_enable_tx();
- // send the data
- if((error=dyn2_master_send())!=DYN_SUCCESS)
- {
- dyn_master_enable_rx();
- return error;
- }
- // wait for the transmission to end
- if((error=dyn_master_wait_sending())!=DYN_SUCCESS)
- {
- dyn_master_enable_rx();
- return error;
- }
- dyn_master_enable_rx();
- // wait for the replay within the given timeout
- error=dyn2_master_receive();
- return error;
-}
-
-uint8_t dyn_master_read_byte(uint8_t id,uint8_t address,uint8_t *data)
-{
- return dyn_master_read(id,address,1,data);
-}
-
-uint8_t dyn2_master_read_byte(uint8_t id,uint16_t address,uint8_t *data)
-{
- return dyn2_master_read(id,address,1,data);
-}
-
-uint8_t dyn_master_read_word(uint8_t id,uint8_t address,uint16_t *data)
-{
- uint8_t value[2];
- uint8_t error;
-
- error=dyn_master_read(id,address,2,value);
- if(error==DYN_NO_ERROR)
- (*data)=value[0]+value[1]*256;
-
- return error;
-}
-
-uint8_t dyn2_master_read_word(uint8_t id,uint16_t address,uint16_t *data)
-{
- uint8_t value[2];
- uint8_t error;
-
- error=dyn2_master_read(id,address,2,value);
- if(error==DYN_NO_ERROR)
- (*data)=value[0]+value[1]*256;
-
- return error;
-}
-
-uint8_t dyn_master_read_table(uint8_t id,uint8_t address,uint8_t length,uint8_t *data)
-{
- return dyn_master_read(id,address,length,data);
-}
-
-uint8_t dyn2_master_read_table(uint8_t id,uint16_t address,uint16_t length,uint8_t *data)
-{
- return dyn2_master_read(id,address,length,data);
-}
-
-uint8_t dyn_master_write_byte(uint8_t id, uint8_t address, uint8_t data)
-{
- return dyn_master_write(id,address,1,&data);
-}
-
-uint8_t dyn2_master_write_byte(uint8_t id, uint16_t address, uint8_t data)
-{
- return dyn2_master_write(id,address,1,&data);
-}
-
-uint8_t dyn_master_write_word(uint8_t id, uint8_t address, uint16_t data)
-{
- uint8_t value[2];
-
- value[0]=data%256;
- value[1]=data/256;
- return dyn_master_write(id,address,2,value);
-}
-
-uint8_t dyn2_master_write_word(uint8_t id, uint16_t address, uint16_t data)
-{
- uint8_t value[2];
-
- value[0]=data%256;
- value[1]=data/256;
- return dyn2_master_write(id,address,2,value);
-}
-
-uint8_t dyn_master_write_table(uint8_t id, uint8_t address, uint8_t length, uint8_t *data)
-{
- return dyn_master_write(id,address,length,data);
-}
-
-uint8_t dyn2_master_write_table(uint8_t id, uint16_t address, uint16_t length, uint8_t *data)
-{
- return dyn2_master_write(id,address,length,data);
-}
-
-uint8_t dyn_master_reg_write(uint8_t id, uint8_t address, uint8_t length, uint8_t *data)
-{
- uint8_t error;
-
- // generate the ping packet for the desired device
- dyn_init_reg_write_packet(dyn_master_tx_buffer,id,address,length,data);
- dyn_master_num_read_packets=0x01;
- // enable transmission
- dyn_master_enable_tx();
- // send the data
- if((error=dyn_master_send())!=DYN_SUCCESS)
- {
- dyn_master_enable_rx();
- return error;
- }
- // wait for the transmission to end
- if((error=dyn_master_wait_sending())!=DYN_SUCCESS)
- {
- dyn_master_enable_rx();
- return error;
- }
- dyn_master_enable_rx();
- // wait for the replay within the given timeout
- error=dyn_master_receive();
- return error;
-}
-
-uint8_t dyn2_master_reg_write(uint8_t id, uint16_t address, uint16_t length, uint8_t *data)
-{
- uint8_t error;
-
- // generate the ping packet for the desired device
- dyn2_init_reg_write_packet(dyn_master_tx_buffer,id,address,length,data);
- dyn_master_num_read_packets=0x01;
- // enable transmission
- dyn_master_enable_tx();
- // send the data
- if((error=dyn2_master_send())!=DYN_SUCCESS)
- {
- dyn_master_enable_rx();
- return error;
- }
- // wait for the transmission to end
- if((error=dyn_master_wait_sending())!=DYN_SUCCESS)
- {
- dyn_master_enable_rx();
- return error;
- }
- dyn_master_enable_rx();
- // wait for the replay within the given timeout
- error=dyn2_master_receive();
- return error;
-}
-
-uint8_t dyn_master_action(void)
-{
- dyn_master_no_answer=0x01;
- // generate the ping packet for the desired device
- dyn_init_action_packet(dyn_master_tx_buffer);
- // enable transmission
- dyn_master_enable_tx();
- // send the data
- return dyn_master_send();
-}
-
-uint8_t dyn2_master_action(void)
-{
- dyn_master_no_answer=0x01;
- // generate the ping packet for the desired device
- dyn2_init_action_packet(dyn_master_tx_buffer);
- // enable transmission
- dyn_master_enable_tx();
- // send the data
- return dyn2_master_send();
-}
-
-uint8_t dyn_master_sync_write(uint8_t num,uint8_t *ids,uint8_t address, uint8_t length, TWriteData *data)
-{
- dyn_master_no_answer=0x01;
- // generate the sync write packet
- dyn_init_sync_write_packet(dyn_master_tx_buffer,num,ids,address,length,data);
- // enable transmission
- dyn_master_enable_tx();
- // send the data
- return dyn_master_send();
-}
-
-uint8_t dyn2_master_sync_write(uint8_t num,uint8_t *ids,uint16_t address, uint16_t length, TWriteData *data)
-{
- dyn_master_no_answer=0x01;
- // generate the sync write packet
- dyn2_init_sync_write_packet(dyn_master_tx_buffer,num,ids,address,length,data);
- // enable transmission
- dyn_master_enable_tx();
- // send the data
- return dyn2_master_send();
-}
-
-// repeater functions
-uint8_t dyn_master_resend_inst_packet(uint8_t *inst_packet, uint8_t *status_packet)
-{
- uint8_t error;
-
- // copy the contents of the instruction packet to the output buffer
- dyn_copy_packet(inst_packet,dyn_master_tx_buffer);
- dyn_master_num_read_packets=0x01;
- // enable transmission
- dyn_master_enable_tx();
- // send the data
- if((error=dyn_master_send())!=DYN_SUCCESS)
- {
- dyn_master_enable_rx();
- return error;
- }
- // wait for the transmission to end
- if((error=dyn_master_wait_sending())!=DYN_SUCCESS)
- {
- dyn_master_enable_rx();
- return error;
- }
- dyn_master_enable_rx();
- // wait for the replay within the given timeout
- error=dyn_master_receive();
- if(error!=DYN_TIMEOUT)
- {
- // copy the contents of the status of the packet to the output buffer
- dyn_copy_packet(dyn_master_rx_buffer,status_packet);
- }
-
- return error;
-}
-
-uint8_t dyn_master_bulk_read(uint8_t num,uint8_t *ids,uint8_t *address,uint8_t *length,TWriteData *data)
-{
- uint16_t offset=0;
- uint8_t i,error;
-
- dyn_master_num_read_packets=num;
- // generate the sync write packet
- dyn_init_bulk_read_packet(dyn_master_tx_buffer,num,ids,address,length);
- // enable transmission
- dyn_master_enable_tx();
- // send the data
- if((error=dyn_master_send())!=DYN_SUCCESS)
- {
- dyn_master_enable_rx();
- return error;
- }
- // wait for the transmission to end
- if((error=dyn_master_wait_sending())!=DYN_SUCCESS)
- {
- dyn_master_enable_rx();
- return error;
- }
- dyn_master_enable_rx();
- // wait for the replay within the given timeout
- error=dyn_master_receive();
- if(error!=DYN_NO_ERROR)
- {
- dyn_master_reset();
- return error;
- }
- for(i=0;i<num;i++)
- {
- error=dyn_get_status_error(&dyn_master_rx_buffer[offset]);
- if(error!=DYN_NO_ERROR)
- return error;
- offset+=dyn_get_read_status_data(&dyn_master_rx_buffer[offset],data[i].data_addr)+4;
- }
-
- return DYN_SUCCESS;
-}
diff --git a/src/dynamixel_slave_uart_dma.c b/src/dynamixel_slave_uart_dma.c
deleted file mode 100755
index 82ee128e3b3a1b6ac905d1fe4568d579f3894b24..0000000000000000000000000000000000000000
--- a/src/dynamixel_slave_uart_dma.c
+++ /dev/null
@@ -1,316 +0,0 @@
-#include "dynamixel_slave_uart_dma.h"
-
-#define DYN_SLAVE USART3
-#define DYN_SLAVE_ENABLE_CLK __HAL_RCC_USART3_CLK_ENABLE()
-#define DYN_SLAVE_IRQn USART3_IRQn
-#define DYN_SLAVE_IRQHandler USART3_IRQHandler
-
-#define DYN_SLAVE_TX_PIN GPIO_PIN_10
-#define DYN_SLAVE_TX_GPIO_PORT GPIOB
-#define DYN_SLAVE_ENABLE_TX_GPIO_CLK __HAL_RCC_GPIOB_CLK_ENABLE()
-
-#define DYN_SLAVE_RX_PIN GPIO_PIN_11
-#define DYN_SLAVE_RX_GPIO_PORT GPIOB
-#define DYN_SLAVE_ENABLE_RX_GPIO_CLK __HAL_RCC_GPIOB_CLK_ENABLE()
-
-/* DMA configuration */
-#define DYN_SLAVE_DMA DMA1
-#define DYN_SLAVE_ENABLE_DMA_CLK __HAL_RCC_DMA1_CLK_ENABLE()
-
-#define DYN_SLAVE_TX_DMA_CHANNEL DMA1_Channel2
-#define DYN_SLAVE_RX_DMA_CHANNEL DMA1_Channel3
-
-#define DYN_SLAVE_DMA_TX_IRQn DMA1_Channel2_IRQn
-#define DYN_SLAVE_DMA_RX_IRQn DMA1_Channel3_IRQn
-#define DYN_SLAVE_DMA_TX_IRQHandler DMA1_Channel2_IRQHandler
-#define DYN_SLAVE_DMA_RX_IRQHandler DMA1_Channel3_IRQHandler
-
-#define MAX_BUFFER_LEN 1024
-// private variables
-uint8_t dyn_slave_address;// this module slave address
-uint8_t dyn_slave_return_level;// type of response
-uint8_t dyn_slave_return_delay;// delay in the response
-// input buffer
-uint8_t dyn_slave_rx_buffer[MAX_BUFFER_LEN];
-// output buffer
-uint8_t dyn_slave_tx_buffer[MAX_BUFFER_LEN];
-// instruction packet ready flag
-volatile uint8_t dyn_slave_packet_ready;
-// sending status packet flag
-volatile uint8_t dyn_slave_sending_packet;
-// usart communication device handler
-UART_HandleTypeDef UartHandle;
-DMA_HandleTypeDef hdma_tx;
-DMA_HandleTypeDef hdma_rx;
-
-// private functions
-
-// interrupt handlers
-void DYN_SLAVE_IRQHandler(void)
-{
- static uint8_t num_bytes=0;
- uint8_t data;
-
- if(__HAL_UART_GET_FLAG(&UartHandle, UART_FLAG_RXNE) != RESET)
- {
- if(__HAL_UART_GET_IT_SOURCE(&UartHandle, UART_IT_RXNE) !=RESET)
- {
- __HAL_UART_CLEAR_FLAG(&UartHandle,UART_FLAG_RXNE);
- data=(uint8_t)(UartHandle.Instance->DR & (uint8_t)0x00FF);
- switch(num_bytes)
- {
- case 0: if(data==0xFF)
- {
- dyn_slave_rx_buffer[num_bytes]=data;
- num_bytes++;
- }
- break;
- case 1: if(data==0xFF)
- {
- dyn_slave_rx_buffer[num_bytes]=data;
- num_bytes++;
- }
- else num_bytes--;
- break;
- case 2: if(data!=0xFF)
- {
- dyn_slave_rx_buffer[num_bytes]=data;
- num_bytes++;
- }
- break;
- case 3: dyn_slave_rx_buffer[num_bytes]=data;
- num_bytes=0;
- // disable USART RX interrupts
- __HAL_UART_DISABLE_IT(&UartHandle, UART_IT_RXNE);
- // enable DMA RX
- HAL_DMA_Start_IT(UartHandle.hdmarx,(uint32_t)&UartHandle.Instance->DR,(uint32_t)&dyn_slave_rx_buffer[4],data);
- SET_BIT(UartHandle.Instance->CR3, USART_CR3_DMAR);
- break;
- default: break;
- }
- }
- }
- else if(__HAL_UART_GET_FLAG(&UartHandle, UART_FLAG_TC) != RESET)
- {
- if(__HAL_UART_GET_IT_SOURCE(&UartHandle, UART_IT_TC) !=RESET)
- {
- __HAL_UART_CLEAR_FLAG(&UartHandle,UART_FLAG_TC);
- __HAL_UART_DISABLE_IT(&UartHandle, UART_IT_TC);
- dyn_slave_sending_packet=0x00;
- }
- }
-}
-
-void DYN_SLAVE_DMA_TX_IRQHandler(void)
-{
- if(__HAL_DMA_GET_FLAG(UartHandle.hdmatx,__HAL_DMA_GET_TC_FLAG_INDEX(UartHandle.hdmatx)) != RESET)
- {
- if(__HAL_DMA_GET_IT_SOURCE(UartHandle.hdmatx, DMA_IT_TC) != RESET)
- {
- /* Disable the half transfer interrupt */
- __HAL_DMA_DISABLE_IT(UartHandle.hdmatx, DMA_IT_TC);
- /* Clear the transfer complete flag */
- __HAL_DMA_CLEAR_FLAG(UartHandle.hdmatx, __HAL_DMA_GET_TC_FLAG_INDEX(UartHandle.hdmatx));
- CLEAR_BIT(UartHandle.Instance->CR3, (USART_CR3_DMAT | USART_CR3_DMAR));
- HAL_DMA_Abort(UartHandle.hdmatx);
- __HAL_UART_ENABLE_IT(&UartHandle, UART_IT_TC);
- }
- }
- if(__HAL_DMA_GET_FLAG(UartHandle.hdmatx, __HAL_DMA_GET_HT_FLAG_INDEX(UartHandle.hdmatx)) != RESET)
- {
- if(__HAL_DMA_GET_IT_SOURCE(UartHandle.hdmatx, DMA_IT_HT) != RESET)
- {
- /* Disable the half transfer interrupt */
- __HAL_DMA_DISABLE_IT(UartHandle.hdmatx, DMA_IT_HT);
- /* Clear the half transfer complete flag */
- __HAL_DMA_CLEAR_FLAG(UartHandle.hdmatx, __HAL_DMA_GET_HT_FLAG_INDEX(UartHandle.hdmatx));
- }
- }
-}
-
-void DYN_SLAVE_DMA_RX_IRQHandler(void)
-{
- if(__HAL_DMA_GET_FLAG(UartHandle.hdmarx,__HAL_DMA_GET_TC_FLAG_INDEX(UartHandle.hdmarx)) != RESET)
- {
- if(__HAL_DMA_GET_IT_SOURCE(UartHandle.hdmarx, DMA_IT_TC) != RESET)
- {
- /* Disable the transfer complete interrupt */
- __HAL_DMA_DISABLE_IT(UartHandle.hdmarx, DMA_IT_TC);
- /* Clear the transfer complete flag */
- __HAL_DMA_CLEAR_FLAG(UartHandle.hdmarx, __HAL_DMA_GET_TC_FLAG_INDEX(UartHandle.hdmarx));
- CLEAR_BIT(UartHandle.Instance->CR3, (USART_CR3_DMAT | USART_CR3_DMAR));
- HAL_DMA_Abort(UartHandle.hdmarx);
- __HAL_UART_ENABLE_IT(&UartHandle, UART_IT_RXNE);
- dyn_slave_packet_ready=0x01;
- }
- }
- if(__HAL_DMA_GET_FLAG(UartHandle.hdmarx, __HAL_DMA_GET_HT_FLAG_INDEX(UartHandle.hdmarx)) != RESET)
- {
- if(__HAL_DMA_GET_IT_SOURCE(UartHandle.hdmarx, DMA_IT_HT) != RESET)
- {
- /* Disable the half transfer interrupt */
- __HAL_DMA_DISABLE_IT(UartHandle.hdmarx, DMA_IT_HT);
- /* Clear the half transfer complete flag */
- __HAL_DMA_CLEAR_FLAG(UartHandle.hdmarx, __HAL_DMA_GET_HT_FLAG_INDEX(UartHandle.hdmarx));
- }
- }
-}
-
-// public functions
-void dyn_slave_init(void)
-{
- uint16_t i;
- GPIO_InitTypeDef GPIO_InitStructure;
-
- /* Enable GPIO clock */
- DYN_SLAVE_ENABLE_TX_GPIO_CLK;
- DYN_SLAVE_ENABLE_RX_GPIO_CLK;
- DYN_SLAVE_ENABLE_DMA_CLK;
- // configure the GPIO pins
-
- /* Configure USART Tx and Rx as alternate function push-pull */
- GPIO_InitStructure.Pin = DYN_SLAVE_TX_PIN;
- GPIO_InitStructure.Mode = GPIO_MODE_AF_PP;
- GPIO_InitStructure.Pull = GPIO_PULLUP;
- GPIO_InitStructure.Speed = GPIO_SPEED_HIGH;
- HAL_GPIO_Init(DYN_SLAVE_TX_GPIO_PORT, &GPIO_InitStructure);
-
- GPIO_InitStructure.Pin = DYN_SLAVE_RX_PIN;
- GPIO_InitStructure.Mode = GPIO_MODE_INPUT;
- HAL_GPIO_Init(DYN_SLAVE_RX_GPIO_PORT, &GPIO_InitStructure);
-
- // initialize the buffers
- for(i=0;i<MAX_BUFFER_LEN;i++)
- {
- dyn_slave_rx_buffer[i]=0x00;
- dyn_slave_tx_buffer[i]=0x00;
- }
- // initialize the flags
- dyn_slave_packet_ready=0x00;
- dyn_slave_sending_packet=0x00;
-
- UartHandle.Instance = DYN_SLAVE;
-
- UartHandle.Init.BaudRate = 921600;
- UartHandle.Init.WordLength = UART_WORDLENGTH_8B;
- UartHandle.Init.StopBits = UART_STOPBITS_1;
- UartHandle.Init.Parity = UART_PARITY_NONE;
- UartHandle.Init.Mode = UART_MODE_TX_RX;
- UartHandle.Init.HwFlowCtl = UART_HWCONTROL_NONE;
- UartHandle.Init.OverSampling = UART_OVERSAMPLING_16;
- HAL_UART_Init(&UartHandle);
- DYN_SLAVE_ENABLE_CLK;
-
- HAL_NVIC_SetPriority(DYN_SLAVE_IRQn, 0, 0);
- HAL_NVIC_EnableIRQ(DYN_SLAVE_IRQn);
-
- // configure the DMA channels
- hdma_tx.Instance = DYN_SLAVE_TX_DMA_CHANNEL;
- hdma_tx.Init.Direction = DMA_MEMORY_TO_PERIPH;
- hdma_tx.Init.PeriphInc = DMA_PINC_DISABLE;
- hdma_tx.Init.MemInc = DMA_MINC_ENABLE;
- hdma_tx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
- hdma_tx.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;
- hdma_tx.Init.Mode = DMA_NORMAL;
- hdma_tx.Init.Priority = DMA_PRIORITY_LOW;
-
- HAL_DMA_Init(&hdma_tx);
-
- /* Associate the initialized DMA handle to the UART handle */
- __HAL_LINKDMA(&UartHandle, hdmatx, hdma_tx);
-
- HAL_NVIC_SetPriority(DYN_SLAVE_DMA_TX_IRQn, 1, 0);
- HAL_NVIC_EnableIRQ(DYN_SLAVE_DMA_TX_IRQn);
-
- /* Configure the DMA handler for reception process */
- hdma_rx.Instance = DYN_SLAVE_RX_DMA_CHANNEL;
- hdma_rx.Init.Direction = DMA_PERIPH_TO_MEMORY;
- hdma_rx.Init.PeriphInc = DMA_PINC_DISABLE;
- hdma_rx.Init.MemInc = DMA_MINC_ENABLE;
- hdma_rx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
- hdma_rx.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;
- hdma_rx.Init.Mode = DMA_NORMAL;
- hdma_rx.Init.Priority = DMA_PRIORITY_HIGH;
-
- HAL_DMA_Init(&hdma_rx);
-
- /* Associate the initialized DMA handle to the the UART handle */
- __HAL_LINKDMA(&UartHandle, hdmarx, hdma_rx);
-
- HAL_NVIC_SetPriority(DYN_SLAVE_DMA_RX_IRQn, 1, 1);
- HAL_NVIC_EnableIRQ(DYN_SLAVE_DMA_RX_IRQn);
-
- /* enable the rx interrupt */
- __HAL_UART_ENABLE_IT(&UartHandle, UART_IT_RXNE);
-}
-
-inline void dyn_slave_set_address(uint8_t id)
-{
- dyn_slave_address=id;
-}
-
-inline uint8_t dyn_slave_get_address(void)
-{
- return dyn_slave_address;
-}
-
-inline void dyn_slave_set_return_delay(uint8_t delay)
-{
- dyn_slave_return_delay=delay;
-}
-
-inline void dyn_slave_set_return_level(uint8_t level)
-{
- dyn_slave_return_level=level;
-}
-
-inline uint8_t dyn_slave_get_return_level(void)
-{
- return dyn_slave_return_level;
-}
-
-uint8_t dyn_slave_is_packet_ready(void)
-{
- return dyn_slave_packet_ready;
-}
-
-void dyn_slave_get_inst_packet(uint8_t *packet)
-{
- uint8_t i;
-
- for(i=0;i<dyn_get_length(dyn_slave_rx_buffer)+4;i++)
- packet[i]=dyn_slave_rx_buffer[i];
- dyn_slave_packet_ready=0x00;
-}
-
-void dyn_slave_send_status_packet(uint8_t error,uint8_t length, uint8_t *data)
-{
- // wait until the previous transmission has ended (if any)
- while(dyn_slave_sending_packet==0x01);
- // create the status packet
- dyn_init_status_packet(dyn_slave_tx_buffer,dyn_slave_address,error,length,data);
- // set the DMA transfer
- HAL_DMA_Start_IT(UartHandle.hdmatx,(uint32_t)dyn_slave_tx_buffer,(uint32_t)&UartHandle.Instance->DR,dyn_get_length(dyn_slave_tx_buffer)+4);
- /* Clear the TC flag in the SR register by writing 0 to it */
- __HAL_UART_CLEAR_FLAG(&UartHandle,UART_FLAG_TC);
- /* Enable the DMA transfer for transmit request by setting the DMAT bit
- in the UART CR3 register */
- SET_BIT(UartHandle.Instance->CR3, USART_CR3_DMAT);
- dyn_slave_sending_packet=0x01;
-}
-
-void dyn_slave_resend_status_packet(uint8_t *packet)
-{
- // wait until the previous transmission has ended (if any)
- while(dyn_slave_sending_packet==0x01);
- // create the status packet
- dyn_copy_packet(packet,dyn_slave_tx_buffer);
- // set the DMA transfer
- HAL_DMA_Start_IT(UartHandle.hdmatx,(uint32_t)dyn_slave_tx_buffer,(uint32_t)&UartHandle.Instance->DR,dyn_get_length(dyn_slave_tx_buffer)+4);
- /* Clear the TC flag in the SR register by writing 0 to it */
- __HAL_UART_CLEAR_FLAG(&UartHandle,UART_FLAG_TC);
- /* Enable the DMA transfer for transmit request by setting the DMAT bit
- in the UART CR3 register */
- SET_BIT(UartHandle.Instance->CR3, USART_CR3_DMAT);
- dyn_slave_sending_packet=0x01;
-}
diff --git a/src/gpio.c b/src/gpio.c
index 1297921809283193b266a12b49e07dc2c8a0e48c..d115d0557843e5860bf01eb1d73b8a773d77e8f6 100755
--- a/src/gpio.c
+++ b/src/gpio.c
@@ -100,9 +100,9 @@ void GPI_EXTI1_IRQHandler(void)
if(start_pb_callback!=0)
start_pb_callback();
}
- if(__HAL_GPIO_EXTI_GET_IT(START_PB_PIN) != RESET)
+ if(__HAL_GPIO_EXTI_GET_IT(MODE_PB_PIN) != RESET)
{
- __HAL_GPIO_EXTI_CLEAR_IT(START_PB_PIN);
+ __HAL_GPIO_EXTI_CLEAR_IT(MODE_PB_PIN);
if(mode_pb_callback!=0)
mode_pb_callback();
}
@@ -152,6 +152,22 @@ void GPO_TIMER1_IRQHandler(void)
HAL_GPIO_TogglePin(LED_3_GPIO_PORT,LED_3_PIN);
}
}
+ /* TIM Update event */
+ if(__HAL_TIM_GET_FLAG(&GPO_TIM1Handle, TIM_FLAG_UPDATE) != RESET)
+ if(__HAL_TIM_GET_IT_SOURCE(&GPO_TIM1Handle, TIM_IT_UPDATE) !=RESET)
+ __HAL_TIM_CLEAR_IT(&GPO_TIM1Handle, TIM_IT_UPDATE);
+ /* TIM Break input event */
+ if(__HAL_TIM_GET_FLAG(&GPO_TIM1Handle, TIM_FLAG_BREAK) != RESET)
+ if(__HAL_TIM_GET_IT_SOURCE(&GPO_TIM1Handle, TIM_IT_BREAK) !=RESET)
+ __HAL_TIM_CLEAR_IT(&GPO_TIM1Handle, TIM_IT_BREAK);
+ /* TIM Trigger detection event */
+ if(__HAL_TIM_GET_FLAG(&GPO_TIM1Handle, TIM_FLAG_TRIGGER) != RESET)
+ if(__HAL_TIM_GET_IT_SOURCE(&GPO_TIM1Handle, TIM_IT_TRIGGER) !=RESET)
+ __HAL_TIM_CLEAR_IT(&GPO_TIM1Handle, TIM_IT_TRIGGER);
+ /* TIM commutation event */
+ if(__HAL_TIM_GET_FLAG(&GPO_TIM1Handle, TIM_FLAG_COM) != RESET)
+ if(__HAL_TIM_GET_IT_SOURCE(&GPO_TIM1Handle, TIM_IT_COM) !=RESET)
+ __HAL_TIM_CLEAR_IT(&GPO_TIM1Handle, TIM_FLAG_COM);
}
void GPO_TIMER2_IRQHandler(void)
@@ -198,6 +214,22 @@ void GPO_TIMER2_IRQHandler(void)
HAL_GPIO_TogglePin(LED_4_GPIO_PORT,LED_4_PIN);
}
}
+ /* TIM Update event */
+ if(__HAL_TIM_GET_FLAG(&GPO_TIM2Handle, TIM_FLAG_UPDATE) != RESET)
+ if(__HAL_TIM_GET_IT_SOURCE(&GPO_TIM2Handle, TIM_IT_UPDATE) !=RESET)
+ __HAL_TIM_CLEAR_IT(&GPO_TIM2Handle, TIM_IT_UPDATE);
+ /* TIM Break input event */
+ if(__HAL_TIM_GET_FLAG(&GPO_TIM2Handle, TIM_FLAG_BREAK) != RESET)
+ if(__HAL_TIM_GET_IT_SOURCE(&GPO_TIM2Handle, TIM_IT_BREAK) !=RESET)
+ __HAL_TIM_CLEAR_IT(&GPO_TIM2Handle, TIM_IT_BREAK);
+ /* TIM Trigger detection event */
+ if(__HAL_TIM_GET_FLAG(&GPO_TIM2Handle, TIM_FLAG_TRIGGER) != RESET)
+ if(__HAL_TIM_GET_IT_SOURCE(&GPO_TIM2Handle, TIM_IT_TRIGGER) !=RESET)
+ __HAL_TIM_CLEAR_IT(&GPO_TIM2Handle, TIM_IT_TRIGGER);
+ /* TIM commutation event */
+ if(__HAL_TIM_GET_FLAG(&GPO_TIM2Handle, TIM_FLAG_COM) != RESET)
+ if(__HAL_TIM_GET_IT_SOURCE(&GPO_TIM2Handle, TIM_IT_COM) !=RESET)
+ __HAL_TIM_CLEAR_IT(&GPO_TIM2Handle, TIM_FLAG_COM);
}
void GPO_TIMER3_IRQHandler(void)
@@ -234,6 +266,22 @@ void GPO_TIMER3_IRQHandler(void)
HAL_GPIO_TogglePin(LED_6_B_GPIO_PORT,LED_6_B_PIN);
}
}
+ /* TIM Update event */
+ if(__HAL_TIM_GET_FLAG(&GPO_TIM3Handle, TIM_FLAG_UPDATE) != RESET)
+ if(__HAL_TIM_GET_IT_SOURCE(&GPO_TIM3Handle, TIM_IT_UPDATE) !=RESET)
+ __HAL_TIM_CLEAR_IT(&GPO_TIM3Handle, TIM_IT_UPDATE);
+ /* TIM Break input event */
+ if(__HAL_TIM_GET_FLAG(&GPO_TIM3Handle, TIM_FLAG_BREAK) != RESET)
+ if(__HAL_TIM_GET_IT_SOURCE(&GPO_TIM3Handle, TIM_IT_BREAK) !=RESET)
+ __HAL_TIM_CLEAR_IT(&GPO_TIM3Handle, TIM_IT_BREAK);
+ /* TIM Trigger detection event */
+ if(__HAL_TIM_GET_FLAG(&GPO_TIM3Handle, TIM_FLAG_TRIGGER) != RESET)
+ if(__HAL_TIM_GET_IT_SOURCE(&GPO_TIM3Handle, TIM_IT_TRIGGER) !=RESET)
+ __HAL_TIM_CLEAR_IT(&GPO_TIM3Handle, TIM_IT_TRIGGER);
+ /* TIM commutation event */
+ if(__HAL_TIM_GET_FLAG(&GPO_TIM3Handle, TIM_FLAG_COM) != RESET)
+ if(__HAL_TIM_GET_IT_SOURCE(&GPO_TIM3Handle, TIM_IT_COM) !=RESET)
+ __HAL_TIM_CLEAR_IT(&GPO_TIM3Handle, TIM_FLAG_COM);
}
// private functions
@@ -242,6 +290,8 @@ void GPO_TIMER3_IRQHandler(void)
void gpio_init(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
+ TIM_ClockConfigTypeDef sClockSourceConfig;
+ TIM_MasterConfigTypeDef sMasterConfig;
/* enable clocks */
ENABLE_LED_RX_GPIO_CLK;
@@ -330,7 +380,12 @@ void gpio_init(void)
// initialize the timer interrupts
HAL_NVIC_SetPriority(GPO_TIMER1_IRQn, 3, 1);
HAL_NVIC_EnableIRQ(GPO_TIMER1_IRQn);
+ sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
+ HAL_TIM_ConfigClockSource(&GPO_TIM1Handle, &sClockSourceConfig);
HAL_TIM_OC_Init(&GPO_TIM1Handle);
+ sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
+ sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
+ HAL_TIMEx_MasterConfigSynchronization(&GPO_TIM1Handle, &sMasterConfig);
/* LED's timer configuration */
GPO_TIM2Handle.Instance=GPO_TIMER2;
@@ -344,7 +399,12 @@ void gpio_init(void)
// initialize the timer interrupts
HAL_NVIC_SetPriority(GPO_TIMER2_IRQn, 3, 2);
HAL_NVIC_EnableIRQ(GPO_TIMER2_IRQn);
+ sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
+ HAL_TIM_ConfigClockSource(&GPO_TIM2Handle, &sClockSourceConfig);
HAL_TIM_OC_Init(&GPO_TIM2Handle);
+ sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
+ sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
+ HAL_TIMEx_MasterConfigSynchronization(&GPO_TIM2Handle, &sMasterConfig);
/* LED's timer configuration */
GPO_TIM3Handle.Instance=GPO_TIMER3;
@@ -353,14 +413,21 @@ void gpio_init(void)
GPO_TIM3Handle.Init.ClockDivision = TIM_CLOCKDIVISION_DIV2;
GPO_TIM3Handle.Init.CounterMode = TIM_COUNTERMODE_UP;
GPO_TIM3Handle.Init.RepetitionCounter=0;
- HAL_TIM_Base_Init(&GPO_TIM3Handle);
+ HAL_TIM_OC_Init(&GPO_TIM3Handle);
ENABLE_GPO_TIMER3_CLK;
// initialize the timer interrupts
HAL_NVIC_SetPriority(GPO_TIMER3_IRQn, 3, 3);
HAL_NVIC_EnableIRQ(GPO_TIMER3_IRQn);
- HAL_TIM_OC_Init(&GPO_TIM3Handle);
+ sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
+ sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
+ HAL_TIMEx_MasterConfigSynchronization(&GPO_TIM3Handle, &sMasterConfig);
/* enable external interrupts */
+ // clear any pending interrupts
+ if(__HAL_GPIO_EXTI_GET_IT(START_PB_PIN) != RESET)
+ __HAL_GPIO_EXTI_CLEAR_IT(START_PB_PIN);
+ if(__HAL_GPIO_EXTI_GET_IT(MODE_PB_PIN) != RESET)
+ __HAL_GPIO_EXTI_CLEAR_IT(MODE_PB_PIN);
HAL_NVIC_SetPriority(GPI_EXTI1_IRQn, 3, 0);
HAL_NVIC_EnableIRQ(GPI_EXTI1_IRQn);
@@ -514,6 +581,7 @@ void gpio_blink_led(led_t led_id, int16_t period_ms)
TIM_OCInitStructure.OCMode = TIM_OCMODE_TIMING;
TIM_OCInitStructure.OCPolarity = TIM_OCPOLARITY_HIGH;
+ TIM_OCInitStructure.OCFastMode = TIM_OCFAST_DISABLE;
switch(led_id)
{
case LED_TX:
diff --git a/src/stm32f1xx_hal_msp.c b/src/stm32f1xx_hal_msp.c
index 50572cd3a3360ed995622b55282d7ba59e913dd4..4fb94c95217881f52dfa4605e7fd149d37c13ae0 100755
--- a/src/stm32f1xx_hal_msp.c
+++ b/src/stm32f1xx_hal_msp.c
@@ -84,6 +84,7 @@ void HAL_MspInit(void)
RCC_PeriphCLKInitTypeDef PeriphClkInit;
HAL_NVIC_SetPriorityGrouping(NVIC_PRIORITYGROUP_2);
+ HAL_NVIC_SetPriority(SysTick_IRQn, 0, 3);
RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
RCC_OscInitStruct.HSEState = RCC_HSE_ON;
@@ -103,6 +104,10 @@ void HAL_MspInit(void)
PeriphClkInit.PeriphClockSelection = RCC_PERIPHCLK_ADC;
PeriphClkInit.AdcClockSelection = RCC_ADCPCLK2_DIV6;
HAL_RCCEx_PeriphCLKConfig(&PeriphClkInit);
+
+ __HAL_RCC_AFIO_CLK_ENABLE();
+
+ __HAL_AFIO_REMAP_SWJ_DISABLE();
}
/**
diff --git a/src/time.c b/src/time.c
deleted file mode 100755
index 5b0cbee6b1bfe6fd45f9eec883154d8b826ded73..0000000000000000000000000000000000000000
--- a/src/time.c
+++ /dev/null
@@ -1,33 +0,0 @@
-#include "time.h"
-
-uint32_t clocks_per_us;
-TIM_HandleTypeDef us_timer_Handle;
-
-// public functions
-void time_init(void)
-{
- us_timer_Handle.Instance=TIM1;
- us_timer_Handle.Init.Period = 0xFFFF;
- us_timer_Handle.Init.Prescaler = 0;
- us_timer_Handle.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
- us_timer_Handle.Init.CounterMode = TIM_COUNTERMODE_UP;
- us_timer_Handle.Init.RepetitionCounter=0;
- HAL_TIM_Base_Init(&us_timer_Handle);
- __HAL_RCC_TIM1_CLK_ENABLE();
- __HAL_TIM_SET_COUNTER(&us_timer_Handle,0);
- clocks_per_us=(SystemCoreClock/1000000)+1;
-}
-
-void delay_us(uint32_t us) // microseconds
-{
- uint32_t clocks,current;
-
- clocks=clocks_per_us*us;
- __HAL_TIM_SET_COUNTER(&us_timer_Handle,0);
- HAL_TIM_Base_Start(&us_timer_Handle);
- do{
- current=__HAL_TIM_GET_COUNTER(&us_timer_Handle);
- }while(current<clocks);
- HAL_TIM_Base_Stop(&us_timer_Handle);
-}
-