diff --git a/include/adc_dma.h b/include/adc_dma.h
index 931594aae6dfb263af39a2e835041928ff4ba474..c0750d071f91f3336dfbefcc2dc3e2058bcfe186 100755
--- a/include/adc_dma.h
+++ b/include/adc_dma.h
@@ -5,9 +5,10 @@
extern "C" {
#endif
-#include "stm32f1xx_hal.h"
+#include "stm32f1xx.h"
-typedef enum {ADC_CH1,ADC_CH2,ADC_CH3,ADC_CH4,ADC_CH6,ADC_CH8} adc_ch_t;
+typedef enum {ADC_CH1,ADC_CH2,ADC_CH3,ADC_CH4,ADC_CH5,ADC_CH6,ADC_CH7,ADC_CH8,
+ ADC_CH9,ADC_CH10,ADC_CH11,ADC_CH12,ADC_CH13,ADC_CH14,ADC_CH16,ADC_CH18} adc_ch_t;
void adc_init(void);
void adc_start(void);
diff --git a/include/darwin_registers.h b/include/darwin_registers.h
index 9ed633293ce3c82d4dc77af426e55cee20384ed2..27921e9c06cfb71d1b54b93ab02b3e5254aeb75a 100644
--- a/include/darwin_registers.h
+++ b/include/darwin_registers.h
@@ -155,43 +155,78 @@ typedef enum {
DARWIN_EDIT_LED_PERIOD_H = 0x010B,
DARWIN_MNG_LED_CNTRL = 0x010C, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
// | | | | | toggle | value | internally used
- DARWIN_AUX1_LED_CNTRL = 0x010D, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
- // | | | | color | toggle | value | internally used
- DARWIN_AUX1_LED_COLOR_L = 0x010E, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
- // green value LSB | red value
- DARWIN_AUX1_LED_COLOR_H = 0x010F, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
- // | blue value | green value MSB
- DARWIN_AUX2_LED_CNTRL = 0x0110, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
- // | | | | color | toggle | value | internally used
- DARWIN_AUX2_LED_COLOR_L = 0x0111, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
- // green value LSB | red value
- DARWIN_AUX2_LED_COLOR_H = 0x0112, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
- // | blue value | green value MSB
+ DARWIN_AUX1_LED_CNTRL = 0x010D, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // color | toggle_B | value_B | toggle_G | value_G | toggle_R | value_R | internally used
+ DARWIN_AUX1_LED_COLOR_L = 0x010E, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // green value LSB | red value
+ DARWIN_AUX1_LED_COLOR_H = 0x010F, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // | blue value | green value MSB
+ DARWIN_AUX2_LED_CNTRL = 0x0110, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // color | toggle_B | value_B | toggle_G | value_G | toggle_R | value_R | internally used
+ DARWIN_AUX2_LED_COLOR_L = 0x0111, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // green value LSB | red value
+ DARWIN_AUX2_LED_COLOR_H = 0x0112, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // | blue value | green value MSB
DARWIN_START_PB_CNTRL = 0x0113, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
// | | | | | | value | internally used
DARWIN_MODE_PB_CNTRL = 0x0114, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
// | | | | | | value | internally used
- DARWIN_ADC_CNTRL = 0x0115, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
- // | | | | | | | Enable
+ DARWIN_ADC_CNTRL = 0x0115, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // | | | running | | | start | stop
DARWIN_ADC_PERIOD = 0x0116,
+ DARWIN_ADC_CH1_L = 0x0117,
+ DARWIN_ADC_CH1_H = 0x0118,
+ DARWIN_ADC_CH2_L = 0x0119,
+ DARWIN_ADC_CH2_H = 0x011A,
+ DARWIN_ADC_CH3_L = 0x011B,
+ DARWIN_ADC_CH3_H = 0x011C,
+ DARWIN_ADC_CH4_L = 0x011D,
+ DARWIN_ADC_CH4_H = 0x011E,
+ DARWIN_ADC_CH5_L = 0x011F,
+ DARWIN_ADC_CH5_H = 0x0120,
+ DARWIN_ADC_CH6_L = 0x0121,
+ DARWIN_ADC_CH6_H = 0x0122,
+ DARWIN_ADC_CH7_L = 0x0123,
+ DARWIN_ADC_CH7_H = 0x0124,
+ DARWIN_ADC_CH8_L = 0x0125,
+ DARWIN_ADC_CH8_H = 0x0126,
+ DARWIN_ADC_CH9_L = 0x0127,
+ DARWIN_ADC_CH9_H = 0x0128,
+ DARWIN_ADC_CH10_L = 0x0129,
+ DARWIN_ADC_CH10_H = 0x012A,
+ DARWIN_ADC_CH11_L = 0x012B,
+ DARWIN_ADC_CH11_H = 0x012C,
+ DARWIN_ADC_CH12_L = 0x012D,
+ DARWIN_ADC_CH12_H = 0x012E,
+ DARWIN_ADC_CH13_L = 0x012F,
+ DARWIN_ADC_CH13_H = 0x0130,
+ DARWIN_ADC_CH14_L = 0x0131,
+ DARWIN_ADC_CH14_H = 0x0132,
+ DARWIN_ADC_TEMP_L = 0x0133,
+ DARWIN_ADC_TEMP_H = 0x0134,
+ DARWIN_ADC_CH16_L = 0x0135,
+ DARWIN_ADC_CH16_H = 0x0136,
+ DARWIN_ADC_VREF_L = 0x0137,
+ DARWIN_ADC_VREF_H = 0x0138,
+ DARWIN_ADC_CH18_L = 0x0139,
+ DARWIN_ADC_CH18_H = 0x013A,
+ DARWIN_IMU_CNTRL = 0x013B, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // accel_det | gyro_det | calibrating | running | | start_cal | stop | start
+ DARWIN_IMU_CAL_SAMPLES_L = 0x013C,
+ DARWIN_IMU_CAL_SAMPLES_H = 0x013D,
+ DARWIN_IMU_GYRO_X_L = 0x013E,
+ DARWIN_IMU_GYRO_X_H = 0x013F,
+ DARWIN_IMU_GYRO_Y_L = 0x0140,
+ DARWIN_IMU_GYRO_Y_H = 0x0141,
+ DARWIN_IMU_GYRO_Z_L = 0x0142,
+ DARWIN_IMU_GYRO_Z_H = 0x0143,
+ DARWIN_IMU_ACCEL_X_L = 0x0144,
+ DARWIN_IMU_ACCEL_X_H = 0x0145,
+ DARWIN_IMU_ACCEL_Y_L = 0x0146,
+ DARWIN_IMU_ACCEL_Y_H = 0x0147,
+ DARWIN_IMU_ACCEL_Z_L = 0x0148,
+ DARWIN_IMU_ACCEL_Z_H = 0x0149,
-
- DARWIN_ADC_CH1_L = 0x0112,
- DARWIN_ADC_CH1_H = 0x0113,
- DARWIN_ADC_CH2_L = 0x0114,
- DARWIN_ADC_CH2_H = 0x0115,
- DARWIN_ADC_CH3_L = 0x0116,
- DARWIN_ADC_CH3_H = 0x0117,
- DARWIN_ADC_CH4_L = 0x0118,
- DARWIN_ADC_CH4_H = 0x0119,
- DARWIN_ADC_TEMP_L = 0x011A,
- DARWIN_ADC_TEMP_H = 0x011B,
- DARWIN_ADC_CH6_L = 0x011C,
- DARWIN_ADC_CH6_H = 0x011D,
- DARWIN_ADC_VREF_L = 0x011E,
- DARWIN_ADC_VREF_H = 0x011F,
- DARWIN_ADC_CH8_L = 0x0120,
- DARWIN_ADC_CH8_H = 0x0121,
DARWIN_MM_NUM_SERVOS = 0x0128,
DARWIN_MM_CNTRL = 0x0129, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
// | | | | | Enable power | Enable balance | Enable manager
@@ -318,19 +353,33 @@ typedef enum {
#define GPIO_INT_USED 0x01
#define GPIO_VALUE 0x02
#define GPIO_TOGGLE 0x04
+#define GPIO_VALUE_R 0x02
+#define GPIO_TOGGLE_R 0x04
+#define GPIO_VALUE_G 0x08
+#define GPIO_TOGGLE_G 0x10
+#define GPIO_VALUE_B 0x20
+#define GPIO_TOGGLE_B 0x40
#define GPIO_BLINK 0x08
+#define GPIO_COLOR 0x80
#define GPIO_RED_COLOR 0x001F
#define GPIO_GREEN_COLOR 0x03E0
#define GPIO_BLUE_COLOR 0x7C00
#define ADC_BASE_ADDRESS 0x0110
-#define ADC_MEM_LENGTH 18
-#define ADC_ENABLE 0x01
+#define ADC_MEM_LENGTH 38
+#define ADC_START 0x01
+#define ADC_STOP 0x02
+#define ADC_RUNNING 0x10
-#define ZIGBEE_BASE_ADDRESS 0x0122
-#define ZIGBEE_MEM_LENGTH 6
-#define ZIGBEE_EN_PWR 0x01
-#define ZIGBEE_ENABLE 0x02
+#define IMU_BASE_ADDRESS 0x013B
+#define IMU_MEM_LENGTH 15
+#define IMU_START 0x01
+#define IMU_STOP 0x02
+#define IMU_START_CAL 0x03
+#define IMU_RUNNING 0x10
+#define IMU_CALIBRATING 0x20
+#define IMU_ACCEL_DET 0x40
+#define IMU_GYRO_DET 0x80
#define MANAGER_BASE_ADDRESS 0x0128
#define MANAGER_MEM_LENGTH 82
@@ -354,15 +403,6 @@ typedef enum {
#define ACTION_INT_FLAG 0x08
#define ACTION_STATUS 0x10
-#define GYRO_BASE_ADDRESS 0x017C
-#define GYRO_MEM_LENGTH 6
-#define GYRO_EEPROM_ADDRESS 0x0031
-#define GYRO_EEPROM_LENGTH 2
-#define GYRO_CALIBRATE 0x01
-#define GYRO_EN_CAL_INT 0x02
-#define GYRO_EN_FALL_DET 0x04
-#define GYRO_EN_FALL_DET_INT 0x08
-
#define WALK_BASE_ADDRESS 0x0182
#define WALK_MEM_LENGTH 4
#define WALK_EEPROM_ADDRESS 0x0033
diff --git a/include/gpio.h b/include/gpio.h
index 38aa2e98d178d72c3b57e8318e9bae7752765ec0..fc8bb16776a1775cfa2581e50e2b25785dedc579 100755
--- a/include/gpio.h
+++ b/include/gpio.h
@@ -7,17 +7,34 @@ extern "C" {
#include "stm32f1xx.h"
-typedef enum {LED_TX,LED_RX,LED_2,LED_3,LED_4,LED_5_R,LED_5_G,LED_5_B,LED_6_R,LED_6_G,LED_6_B} led_t;
+#define GPIO_NUM_LEDS 11
-typedef enum {START_PB,MODE_PB} pushbutton_t;
+typedef enum {LED_TX=0,LED_RX=1,LED_2=2,LED_3=3,LED_4=4,LED_5_R=5,LED_5_G=6,LED_5_B=7,LED_6_R=8,LED_6_G=9,LED_6_B=10} led_t;
+
+typedef struct
+{
+ GPIO_TypeDef* port;
+ uint16_t pin;
+ TIM_HandleTypeDef * timer;
+ uint32_t timer_ch;
+ uint16_t control_reg;
+ uint16_t data_reg;
+ uint8_t value_mask;
+ uint8_t toggle_mask;
+ uint8_t blink_mask;
+}led_info_t;
+
+#define GPIO_NUM_PB 2
+
+typedef enum {START_PB=0,MODE_PB=1} pushbutton_t;
void gpio_init(void);
// LED functions
void gpio_set_led(led_t led_id);
void gpio_clear_led(led_t led_id);
void gpio_toggle_led(led_t led_id);
-void gpio_blink_led(led_t led_id, int16_t period_ms);
-void gpio_set_color(led_t led_id, uint8_t value);
+void gpio_blink_led(led_t led_id, uint16_t period_ms);
+void gpio_set_color(led_t led_id, uint16_t value);
// Pushbuttons functions
uint8_t gpio_is_pushbutton_pressed(pushbutton_t pb_id);
void gpio_set_pushbutton_callback(pushbutton_t pb_id,void (*callback)(void));
diff --git a/include/imu.h b/include/imu.h
index 939b4e06c54220ce8c4ace515f31893421a5be82..0a72629d94a0214c03a2cabc1aec74e3ec4ece91 100755
--- a/include/imu.h
+++ b/include/imu.h
@@ -1,14 +1,25 @@
#ifndef _IMU_H
#define _IMU_H
-#include "stm32f1xx_hal.h"
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "stm32f1xx.h"
//public functions
void imu_init(void);
void imu_start(void);
void imu_stop(void);
-
+void imu_set_calibration_samples(uint16_t num_samples);
void imu_start_gyro_cal(void);
void imu_get_gyro_data(int32_t *x, int32_t *y, int32_t *z);
+// operation functions
+uint8_t imu_in_range(unsigned short int address,unsigned short int length);
+void imu_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data);
+
+#ifdef __cplusplus
+}
+#endif
#endif
diff --git a/include/scheduler.h b/include/scheduler.h
index 9f0098e90d305d98d3e3d7df7a41217de24ff0d1..2970a430412faae59d7b903aa320ac604d960600 100644
--- a/include/scheduler.h
+++ b/include/scheduler.h
@@ -7,7 +7,14 @@ extern "C" {
#include "stm32f1xx.h"
+typedef enum {SCH_CH1=0,SCH_CH2=1,SCH_CH3=2,SCH_CH4=3} sch_ch_t;
+
void scheduler_init(void);
+void scheduler_set_period(sch_ch_t channel_id, uint16_t period_ms);
+void scheduler_set_one_shot(sch_ch_t channel_id, uint16_t time_ms);
+void scheduler_set_function(sch_ch_t channel_id, void (*function)(void));
+void scheduler_start(sch_ch_t channel_id);
+void scheduler_stop(sch_ch_t channel_id);
#ifdef __cplusplus
}
diff --git a/src/adc_dma.c b/src/adc_dma.c
index 48e249516a63ce904f2e064b859b55e0aa7f171f..28a66d0689a9d1d61601251585ea93435d943b56 100755
--- a/src/adc_dma.c
+++ b/src/adc_dma.c
@@ -1,24 +1,26 @@
#include "adc_dma.h"
#include "ram.h"
-#define ADC1_CH1 ADC_CHANNEL_0
-#define ADC1_CH2 ADC_CHANNEL_1
-#define ADC1_CH3 ADC_CHANNEL_2
-#define ADC1_CH4 ADC_CHANNEL_3
-#define ADC1_CH5 ADC_CHANNEL_4
-#define ADC1_CH6 ADC_CHANNEL_5
-#define ADC1_CH7 ADC_CHANNEL_6
-#define ADC1_CH8 ADC_CHANNEL_7
-
-#define ADC2_CH1 ADC_CHANNEL_8
-#define ADC2_CH2 ADC_CHANNEL_9
-#define ADC2_CH3 ADC_CHANNEL_10
-#define ADC2_CH4 ADC_CHANNEL_11
-#define ADC2_CH5 ADC_CHANNEL_12
-#define ADC2_CH6 ADC_CHANNEL_13
-#define ADC2_CH7 ADC_CHANNEL_14
-#define ADC2_CH8 ADC_CHANNEL_15
-
+#define ADC1_CH1 ADC_CHANNEL_0
+#define ADC1_CH2 ADC_CHANNEL_1
+#define ADC1_CH3 ADC_CHANNEL_2
+#define ADC1_CH4 ADC_CHANNEL_3
+#define ADC1_CH5 ADC_CHANNEL_4
+#define ADC1_CH6 ADC_CHANNEL_5
+#define ADC1_CH7 ADC_CHANNEL_6
+#define ADC1_CH8 ADC_CHANNEL_TEMPSENSOR
+#define ADC1_CH9 ADC_CHANNEL_VREFINT
+
+#define ADC2_CH1 ADC_CHANNEL_7
+#define ADC2_CH2 ADC_CHANNEL_8
+#define ADC2_CH3 ADC_CHANNEL_9
+#define ADC2_CH4 ADC_CHANNEL_10
+#define ADC2_CH5 ADC_CHANNEL_11
+#define ADC2_CH6 ADC_CHANNEL_12
+#define ADC2_CH7 ADC_CHANNEL_13
+#define ADC2_CH8 ADC_CHANNEL_14
+#define ADC2_CH9 ADC_CHANNEL_15
+
#define ADC1_CH1_PIN GPIO_PIN_0
#define ADC1_CH1_PORT GPIOA
#define ADC1_CH1_ENABLE_PORT_CLK __HAL_RCC_GPIOA_CLK_ENABLE()
@@ -40,52 +42,153 @@
#define ADC1_CH7_PIN GPIO_PIN_6
#define ADC1_CH7_PORT GPIOA
#define ADC1_CH7_ENABLE_PORT_CLK __HAL_RCC_GPIOA_CLK_ENABLE()
-#define ADC1_CH8_PIN GPIO_PIN_7
-#define ADC1_CH8_PORT GPIOA
-#define ADC1_CH8_ENABLE_PORT_CLK __HAL_RCC_GPIOA_CLK_ENABLE()
#define ADC1_ENABLE_CLK __ADC1_CLK_ENABLE()
-#define ADC2_CH1_PIN GPIO_PIN_0
-#define ADC2_CH1_PORT GPIOB
-#define ADC2_CH1_ENABLE_PORT_CLK __HAL_RCC_GPIOB_CLK_ENABLE()
-#define ADC2_CH2_PIN GPIO_PIN_1
+#define ADC2_CH1_PIN GPIO_PIN_7
+#define ADC2_CH1_PORT GPIOA
+#define ADC2_CH1_ENABLE_PORT_CLK __HAL_RCC_GPIOA_CLK_ENABLE()
+#define ADC2_CH2_PIN GPIO_PIN_0
#define ADC2_CH2_PORT GPIOB
#define ADC2_CH2_ENABLE_PORT_CLK __HAL_RCC_GPIOB_CLK_ENABLE()
-#define ADC2_CH3_PIN GPIO_PIN_0
-#define ADC2_CH3_PORT GPIOC
-#define ADC2_CH3_ENABLE_PORT_CLK __HAL_RCC_GPIOC_CLK_ENABLE()
-#define ADC2_CH4_PIN GPIO_PIN_1
+#define ADC2_CH3_PIN GPIO_PIN_1
+#define ADC2_CH3_PORT GPIOB
+#define ADC2_CH3_ENABLE_PORT_CLK __HAL_RCC_GPIOB_CLK_ENABLE()
+#define ADC2_CH4_PIN GPIO_PIN_0
#define ADC2_CH4_PORT GPIOC
#define ADC2_CH4_ENABLE_PORT_CLK __HAL_RCC_GPIOC_CLK_ENABLE()
-#define ADC2_CH5_PIN GPIO_PIN_2
+#define ADC2_CH5_PIN GPIO_PIN_1
#define ADC2_CH5_PORT GPIOC
#define ADC2_CH5_ENABLE_PORT_CLK __HAL_RCC_GPIOC_CLK_ENABLE()
-#define ADC2_CH6_PIN GPIO_PIN_3
+#define ADC2_CH6_PIN GPIO_PIN_2
#define ADC2_CH6_PORT GPIOC
#define ADC2_CH6_ENABLE_PORT_CLK __HAL_RCC_GPIOC_CLK_ENABLE()
-#define ADC2_CH7_PIN GPIO_PIN_4
+#define ADC2_CH7_PIN GPIO_PIN_3
#define ADC2_CH7_PORT GPIOC
#define ADC2_CH7_ENABLE_PORT_CLK __HAL_RCC_GPIOC_CLK_ENABLE()
-#define ADC2_CH8_PIN GPIO_PIN_5
+#define ADC2_CH8_PIN GPIO_PIN_4
#define ADC2_CH8_PORT GPIOC
#define ADC2_CH8_ENABLE_PORT_CLK __HAL_RCC_GPIOC_CLK_ENABLE()
+#define ADC2_CH9_PIN GPIO_PIN_5
+#define ADC2_CH9_PORT GPIOC
+#define ADC2_CH9_ENABLE_PORT_CLK __HAL_RCC_GPIOC_CLK_ENABLE()
#define ADC2_ENABLE_CLK __ADC2_CLK_ENABLE()
#define ADC_DMA DMA1
#define ADC_ENABLE_DMA_CLK __HAL_RCC_DMA1_CLK_ENABLE()
-
#define ADC_DMA_CHANNEL DMA1_Channel1
-
#define ADC_DMA_IRQn DMA1_Channel1_IRQn
#define ADC_DMA_IRQHandler DMA1_Channel1_IRQHandler
+#define ADC_TIMER TIM1
+#define ENABLE_ADC_TIMER_CLK __HAL_RCC_TIM1_CLK_ENABLE()
+#define ADC_TIMER_IRQn TIM1_CC_IRQn
+#define ADC_TIMER_IRQHandler TIM1_IRQHandler
+
+// temperature conversion functions
+#define TEMP_V25 0.76// ADC voltage at 25 degrees in V
+#define TEMP_INV_SLOPE 400.0// ADC slope in mV/degree
+// general voltage conversion
+#define VOLTAGE_DELTA 0.000805664
+
// private variables
ADC_HandleTypeDef hadc1;
ADC_HandleTypeDef hadc2;
DMA_HandleTypeDef hdma_adc1;
+TIM_HandleTypeDef ADC_TIM_Handle;
+
+uint32_t adc_data[9];// temporal buffer to store ADC data before conversion
+uint16_t adc_period_ms;
+
+// private functions
+uint16_t adc_convert_temperature(uint16_t value)
+{
+ float conv_value;
+
+ conv_value=(value*VOLTAGE_DELTA-TEMP_V25)*TEMP_INV_SLOPE+25.0;
+
+ return conv_value*(1<<10);// fixed point format 6 | 10
+}
+
+uint16_t adc_convert_voltage(uint16_t value)
+{
+ float conv_value;
+
+ conv_value=value*VOLTAGE_DELTA;
+
+ return conv_value*(1<<12);
+}
+
// interrupt handlers
+void ADC_TIMER_IRQHandler(void)
+{
+ uint32_t capture;
+
+ if(__HAL_TIM_GET_FLAG(&ADC_TIM_Handle, TIM_FLAG_CC1) != RESET)
+ {
+ if(__HAL_TIM_GET_IT_SOURCE(&ADC_TIM_Handle, TIM_IT_CC1) !=RESET)
+ {
+ __HAL_TIM_CLEAR_IT(&ADC_TIM_Handle, TIM_IT_CC1);
+ capture = HAL_TIM_ReadCapturedValue(&ADC_TIM_Handle, TIM_CHANNEL_1);
+ __HAL_TIM_SET_COMPARE(&ADC_TIM_Handle, TIM_CHANNEL_1, (capture + adc_period_ms));
+ }
+ }
+ /* TIM Update event */
+ if(__HAL_TIM_GET_FLAG(&ADC_TIM_Handle, TIM_FLAG_UPDATE) != RESET)
+ if(__HAL_TIM_GET_IT_SOURCE(&ADC_TIM_Handle, TIM_IT_UPDATE) !=RESET)
+ __HAL_TIM_CLEAR_IT(&ADC_TIM_Handle, TIM_IT_UPDATE);
+ /* TIM Break input event */
+ if(__HAL_TIM_GET_FLAG(&ADC_TIM_Handle, TIM_FLAG_BREAK) != RESET)
+ if(__HAL_TIM_GET_IT_SOURCE(&ADC_TIM_Handle, TIM_IT_BREAK) !=RESET)
+ __HAL_TIM_CLEAR_IT(&ADC_TIM_Handle, TIM_IT_BREAK);
+ /* TIM Trigger detection event */
+ if(__HAL_TIM_GET_FLAG(&ADC_TIM_Handle, TIM_FLAG_TRIGGER) != RESET)
+ if(__HAL_TIM_GET_IT_SOURCE(&ADC_TIM_Handle, TIM_IT_TRIGGER) !=RESET)
+ __HAL_TIM_CLEAR_IT(&ADC_TIM_Handle, TIM_IT_TRIGGER);
+ /* TIM commutation event */
+ if(__HAL_TIM_GET_FLAG(&ADC_TIM_Handle, TIM_FLAG_COM) != RESET)
+ if(__HAL_TIM_GET_IT_SOURCE(&ADC_TIM_Handle, TIM_IT_COM) !=RESET)
+ __HAL_TIM_CLEAR_IT(&ADC_TIM_Handle, TIM_FLAG_COM);
+}
+
+void ADC_DMA_IRQHandler(void)
+{
+ uint16_t value,i;
+
+ /* Transfer Error Interrupt management ***************************************/
+ if(__HAL_DMA_GET_FLAG(&hdma_adc1, __HAL_DMA_GET_TE_FLAG_INDEX(&hdma_adc1)) != RESET)
+ if(__HAL_DMA_GET_IT_SOURCE(&hdma_adc1, DMA_IT_TE) != RESET)
+ __HAL_DMA_CLEAR_FLAG(&hdma_adc1, __HAL_DMA_GET_TE_FLAG_INDEX(&hdma_adc1));
+ /* Half Transfer Complete Interrupt management ******************************/
+ if(__HAL_DMA_GET_FLAG(&hdma_adc1, __HAL_DMA_GET_HT_FLAG_INDEX(&hdma_adc1)) != RESET)
+ if(__HAL_DMA_GET_IT_SOURCE(&hdma_adc1, DMA_IT_HT) != RESET)
+ __HAL_DMA_CLEAR_FLAG(&hdma_adc1, __HAL_DMA_GET_HT_FLAG_INDEX(&hdma_adc1));
+ /* Transfer Complete Interrupt management ***********************************/
+ if(__HAL_DMA_GET_FLAG(&hdma_adc1, __HAL_DMA_GET_TC_FLAG_INDEX(&hdma_adc1)) != RESET)
+ if(__HAL_DMA_GET_IT_SOURCE(&hdma_adc1, DMA_IT_TC) != RESET)
+ {
+ __HAL_DMA_CLEAR_FLAG(&hdma_adc1, __HAL_DMA_GET_TC_FLAG_INDEX(&hdma_adc1));
+ for(i=0;i<9;i++)
+ {
+ value=adc_convert_voltage(adc_data[i]&0x0000FFFF);
+ ram_data[DARWIN_ADC_CH1_L+i*4]=value%256;
+ ram_data[DARWIN_ADC_CH1_H+i*4]=value/256;
+ if(i==7)
+ {
+ value=adc_convert_temperature(adc_data[i]&0x0000FFFF);
+ ram_data[DARWIN_ADC_TEMP_L]=value%256;
+ ram_data[DARWIN_ADC_TEMP_H]=value/256;
+ }
+ else
+ {
+ value=adc_convert_voltage((adc_data[i]&0xFFFF0000)>>16);
+ ram_data[DARWIN_ADC_CH2_L+i*4]=value%256;
+ ram_data[DARWIN_ADC_CH2_H+i*4]=value/256;
+ }
+ }
+ }
+}
// public functions
void adc_init(void)
@@ -93,6 +196,15 @@ void adc_init(void)
GPIO_InitTypeDef GPIO_InitStruct;
ADC_MultiModeTypeDef multimode;
ADC_ChannelConfTypeDef sConfig;
+ TIM_ClockConfigTypeDef sClockSourceConfig;
+ TIM_MasterConfigTypeDef sMasterConfig;
+ TIM_BreakDeadTimeConfigTypeDef sBreakDeadTimeConfig;
+ TIM_OC_InitTypeDef TIM_OCInitStructure;
+ uint32_t capture;
+
+ /* initialize the internal variables */
+ adc_period_ms=360;// equivalent to 10 ms
+ ram_data[DARWIN_ADC_PERIOD]=10;
/* enable clocks */
ADC1_CH1_ENABLE_PORT_CLK;
@@ -102,7 +214,6 @@ void adc_init(void)
ADC1_CH5_ENABLE_PORT_CLK;
ADC1_CH6_ENABLE_PORT_CLK;
ADC1_CH7_ENABLE_PORT_CLK;
- ADC1_CH8_ENABLE_PORT_CLK;
ADC2_CH1_ENABLE_PORT_CLK;
ADC2_CH2_ENABLE_PORT_CLK;
ADC2_CH3_ENABLE_PORT_CLK;
@@ -111,6 +222,7 @@ void adc_init(void)
ADC2_CH6_ENABLE_PORT_CLK;
ADC2_CH7_ENABLE_PORT_CLK;
ADC2_CH8_ENABLE_PORT_CLK;
+ ADC2_CH9_ENABLE_PORT_CLK;
ADC_ENABLE_DMA_CLK;
@@ -120,9 +232,9 @@ void adc_init(void)
hadc1.Init.ScanConvMode = ENABLE;
hadc1.Init.ContinuousConvMode = ENABLE;
hadc1.Init.DiscontinuousConvMode = DISABLE;
- hadc1.Init.ExternalTrigConv = ADC_SOFTWARE_START;
+ hadc1.Init.ExternalTrigConv = ADC_EXTERNALTRIGCONV_T1_CC1;
hadc1.Init.DataAlign = ADC_DATAALIGN_RIGHT;
- hadc1.Init.NbrOfConversion = 8;
+ hadc1.Init.NbrOfConversion = 9;
HAL_ADC_Init(&hadc1);
multimode.Mode = ADC_DUALMODE_REGSIMULT;
@@ -162,9 +274,13 @@ void adc_init(void)
sConfig.Rank = 8;
HAL_ADC_ConfigChannel(&hadc1, &sConfig);
+ sConfig.Channel = ADC1_CH9;
+ sConfig.Rank = 9;
+ HAL_ADC_ConfigChannel(&hadc1, &sConfig);
+
/* configure GPIO */
GPIO_InitStruct.Pin = ADC1_CH1_PIN|ADC1_CH2_PIN|ADC1_CH3_PIN|ADC1_CH4_PIN|
- ADC1_CH5_PIN|ADC1_CH6_PIN|ADC1_CH7_PIN|ADC1_CH8_PIN;
+ ADC1_CH5_PIN|ADC1_CH6_PIN|ADC1_CH7_PIN;
GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
@@ -173,9 +289,9 @@ void adc_init(void)
hadc2.Init.ScanConvMode = ENABLE;
hadc2.Init.ContinuousConvMode = ENABLE;
hadc2.Init.DiscontinuousConvMode = DISABLE;
- hadc2.Init.ExternalTrigConv = ADC_SOFTWARE_START;
+ hadc2.Init.ExternalTrigConv = ADC_EXTERNALTRIGCONV_T1_CC1;
hadc2.Init.DataAlign = ADC_DATAALIGN_RIGHT;
- hadc2.Init.NbrOfConversion = 8;
+ hadc2.Init.NbrOfConversion = 9;
HAL_ADC_Init(&hadc2);
multimode.Mode = ADC_DUALMODE_REGSIMULT;
@@ -215,13 +331,21 @@ void adc_init(void)
sConfig.Rank = 8;
HAL_ADC_ConfigChannel(&hadc2, &sConfig);
+ sConfig.Channel = ADC2_CH9;
+ sConfig.Rank = 9;
+ HAL_ADC_ConfigChannel(&hadc2, &sConfig);
+
/* configure GPIO */
- GPIO_InitStruct.Pin = ADC2_CH3_PIN|ADC2_CH4_PIN|ADC2_CH5_PIN|ADC2_CH6_PIN|
- ADC2_CH7_PIN|ADC2_CH8_PIN;
+ GPIO_InitStruct.Pin = ADC2_CH4_PIN|ADC2_CH5_PIN|ADC2_CH6_PIN|ADC2_CH7_PIN|
+ ADC2_CH8_PIN|ADC2_CH9_PIN;
GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
- GPIO_InitStruct.Pin = ADC2_CH1_PIN|ADC2_CH2_PIN;
+ GPIO_InitStruct.Pin = ADC2_CH2_PIN|ADC2_CH3_PIN;
+ GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
+ HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
+
+ GPIO_InitStruct.Pin = ADC2_CH1_PIN;
GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
@@ -238,18 +362,194 @@ void adc_init(void)
__HAL_LINKDMA(&hadc1,DMA_Handle,hdma_adc1);
+ HAL_NVIC_SetPriority(ADC_DMA_IRQn, 3, 1);
+ HAL_NVIC_EnableIRQ(ADC_DMA_IRQn);
+
+ /* initialize timer */
+ ADC_TIM_Handle.Instance = ADC_TIMER;
+ ADC_TIM_Handle.Init.Prescaler = 1000;
+ ADC_TIM_Handle.Init.CounterMode = TIM_COUNTERMODE_UP;
+ ADC_TIM_Handle.Init.Period = 0xFFFF;
+ ADC_TIM_Handle.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
+ ADC_TIM_Handle.Init.RepetitionCounter = 0;
+ HAL_TIM_Base_Init(&ADC_TIM_Handle);
+ HAL_NVIC_SetPriority(ADC_TIMER_IRQn, 3, 1);
+ HAL_NVIC_EnableIRQ(ADC_TIMER_IRQn);
+ /* use the internal clock */
+ sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
+ HAL_TIM_ConfigClockSource(&ADC_TIM_Handle, &sClockSourceConfig);
+ HAL_TIM_OC_Init(&ADC_TIM_Handle);
+ /* disable master/slave mode */
+ sMasterConfig.MasterOutputTrigger = TIM_TRGO_OC1REF;
+ sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
+ HAL_TIMEx_MasterConfigSynchronization(&ADC_TIM_Handle, &sMasterConfig);
+
+ 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.AutomaticOutput = TIM_AUTOMATICOUTPUT_DISABLE;
+ HAL_TIMEx_ConfigBreakDeadTime(&ADC_TIM_Handle, &sBreakDeadTimeConfig);
+
+ TIM_OCInitStructure.OCMode = TIM_OCMODE_TOGGLE;
+ TIM_OCInitStructure.OCPolarity = TIM_OCPOLARITY_HIGH;
+ TIM_OCInitStructure.OCNPolarity = TIM_OCNPOLARITY_HIGH;
+ TIM_OCInitStructure.OCFastMode = TIM_OCFAST_DISABLE;
+ capture = HAL_TIM_ReadCapturedValue(&ADC_TIM_Handle, TIM_CHANNEL_1);
+ TIM_OCInitStructure.Pulse = capture+adc_period_ms;
+ TIM_OCInitStructure.OCIdleState = TIM_OCIDLESTATE_RESET;
+ TIM_OCInitStructure.OCNIdleState = TIM_OCNIDLESTATE_RESET;
+ HAL_TIM_OC_ConfigChannel(&ADC_TIM_Handle, &TIM_OCInitStructure,TIM_CHANNEL_1);
+
/* calibrate ADC1 */
HAL_ADCEx_Calibration_Start(&hadc1);
HAL_ADCEx_Calibration_Start(&hadc2);
HAL_ADC_Start(&hadc2);
- HAL_ADCEx_MultiModeStart_DMA(&hadc1, (uint32_t *)&ram_data[DARWIN_MIC1_L],16);
-// ADC_Enable(&hadc2);
-// ADC_Enable(&hadc1);
-// __HAL_ADC_CLEAR_FLAG(&hadc1, ADC_FLAG_EOC);
-// /* Enable ADC DMA mode of ADC master */
-// SET_BIT(hadc1.Instance->CR2, ADC_CR2_DMA);
-// /* Start the DMA channel */
-// HAL_DMA_Start_IT(hadc1.DMA_Handle, (uint32_t)&hadc1.Instance->DR, (uint32_t)&ram_data[DARWIN_MIC1_L], 8);
-// SET_BIT(hadc1.Instance->CR2, (ADC_CR2_SWSTART | ADC_CR2_EXTTRIG));
+ HAL_ADCEx_MultiModeStart_DMA(&hadc1,adc_data,9);
+ adc_start();
+}
+
+void adc_start(void)
+{
+ if((ram_data[DARWIN_ADC_CNTRL]&ADC_RUNNING)==0x00)
+ {
+ ram_data[DARWIN_ADC_CNTRL]|=ADC_RUNNING;
+ HAL_TIM_OC_Start_IT(&ADC_TIM_Handle, TIM_CHANNEL_1);
+ }
}
+
+void adc_stop(void)
+{
+ if(ram_data[DARWIN_ADC_CNTRL]&ADC_RUNNING)
+ {
+ ram_data[DARWIN_ADC_CNTRL]&=(~ADC_RUNNING);
+ HAL_TIM_OC_Stop_IT(&ADC_TIM_Handle, TIM_CHANNEL_1);
+ }
+}
+
+void adc_set_period(uint8_t period_ms)
+{
+ adc_period_ms=period_ms*84;
+ ram_data[DARWIN_ADC_PERIOD]=period_ms;
+}
+
+inline uint8_t adc_get_period(void)
+{
+ return ram_data[DARWIN_ADC_PERIOD];
+}
+
+uint16_t adc_get_channel(adc_ch_t channel)
+{
+ uint16_t value;
+
+ switch(channel)
+ {
+ case ADC_CH1: value=ram_data[DARWIN_ADC_CH1_L]+ram_data[DARWIN_ADC_CH1_H]*256;
+ break;
+ case ADC_CH2: value=ram_data[DARWIN_ADC_CH2_L]+ram_data[DARWIN_ADC_CH2_H]*256;
+ break;
+ case ADC_CH3: value=ram_data[DARWIN_ADC_CH3_L]+ram_data[DARWIN_ADC_CH3_H]*256;
+ break;
+ case ADC_CH4: value=ram_data[DARWIN_ADC_CH4_L]+ram_data[DARWIN_ADC_CH4_H]*256;
+ break;
+ case ADC_CH5: value=ram_data[DARWIN_ADC_CH5_L]+ram_data[DARWIN_ADC_CH5_H]*256;
+ break;
+ case ADC_CH6: value=ram_data[DARWIN_ADC_CH6_L]+ram_data[DARWIN_ADC_CH6_H]*256;
+ break;
+ case ADC_CH7: value=ram_data[DARWIN_ADC_CH7_L]+ram_data[DARWIN_ADC_CH7_H]*256;
+ break;
+ case ADC_CH8: value=ram_data[DARWIN_ADC_CH8_L]+ram_data[DARWIN_ADC_CH8_H]*256;
+ break;
+ case ADC_CH9: value=ram_data[DARWIN_ADC_CH9_L]+ram_data[DARWIN_ADC_CH9_H]*256;
+ break;
+ case ADC_CH10: value=ram_data[DARWIN_ADC_CH10_L]+ram_data[DARWIN_ADC_CH10_H]*256;
+ break;
+ case ADC_CH11: value=ram_data[DARWIN_ADC_CH11_L]+ram_data[DARWIN_ADC_CH11_H]*256;
+ break;
+ case ADC_CH12: value=ram_data[DARWIN_ADC_CH12_L]+ram_data[DARWIN_ADC_CH12_H]*256;
+ break;
+ case ADC_CH13: value=ram_data[DARWIN_ADC_CH13_L]+ram_data[DARWIN_ADC_CH13_H]*256;
+ break;
+ case ADC_CH14: value=ram_data[DARWIN_ADC_CH14_L]+ram_data[DARWIN_ADC_CH14_H]*256;
+ break;
+ case ADC_CH16: value=ram_data[DARWIN_ADC_CH16_L]+ram_data[DARWIN_ADC_CH16_H]*256;
+ break;
+ case ADC_CH18: value=ram_data[DARWIN_ADC_CH18_L]+ram_data[DARWIN_ADC_CH18_H]*256;
+ break;
+ default: value=0x0000;
+ }
+
+ return value;
+}
+
+uint16_t adc_get_channel_raw(adc_ch_t channel)
+{
+ uint16_t value;
+
+ switch(channel)
+ {
+ case ADC_CH1: value=adc_data[0]&0x0000FFFF;
+ break;
+ case ADC_CH2: value=(adc_data[0]&0xFFFF0000)>>16;
+ break;
+ case ADC_CH3: value=adc_data[1]&0x0000FFFF;
+ break;
+ case ADC_CH4: value=(adc_data[1]&0xFFFF0000)>>16;
+ break;
+ case ADC_CH5: value=adc_data[2]&0x0000FFFF;
+ break;
+ case ADC_CH6: value=(adc_data[2]&0xFFFF0000)>>16;
+ break;
+ case ADC_CH7: value=adc_data[3]&0x0000FFFF;
+ break;
+ case ADC_CH8: value=(adc_data[3]&0xFFFF0000)>>16;
+ break;
+ case ADC_CH9: value=adc_data[4]&0x0000FFFF;
+ break;
+ case ADC_CH10: value=(adc_data[4]&0xFFFF0000)>>16;
+ break;
+ case ADC_CH11: value=adc_data[5]&0x0000FFFF;
+ break;
+ case ADC_CH12: value=(adc_data[5]&0xFFFF0000)>>16;
+ break;
+ case ADC_CH13: value=adc_data[6]&0x0000FFFF;
+ break;
+ case ADC_CH14: value=(adc_data[6]&0xFFFF0000)>>16;
+ break;
+ case ADC_CH16: value=(adc_data[7]&0xFFFF0000)>>16;
+ break;
+ case ADC_CH18: value=(adc_data[8]&0xFFFF0000)>>16;
+ break;
+ default: value=0x0000;
+ }
+
+ return value;
+}
+
+uint16_t adc_get_temperature(void)
+{
+ return ram_data[DARWIN_ADC_TEMP_L]+ram_data[DARWIN_ADC_TEMP_H]*256;
+}
+
+// operation functions
+uint8_t adc_in_range(unsigned short int address,unsigned short int length)
+{
+ return ram_in_window(ADC_BASE_ADDRESS,ADC_MEM_LENGTH,address,length);
+}
+
+void adc_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data)
+{
+ if(ram_in_range(DARWIN_ADC_CNTRL,address,length))
+ {
+ if(data[DARWIN_ADC_CNTRL-address]&ADC_START)
+ adc_start();
+ else if(data[DARWIN_ADC_CNTRL-address]&ADC_STOP)
+ adc_stop();
+ }
+ if(ram_in_range(DARWIN_ADC_PERIOD,address,length))
+ adc_set_period(data[DARWIN_ADC_PERIOD-address]);
+}
+
diff --git a/src/cm730_fw.c b/src/cm730_fw.c
index eb2a5426efcec08344fa087302d32b5807adc6d3..aa8df078d76384414ce592bdd32e52b34325b638 100755
--- a/src/cm730_fw.c
+++ b/src/cm730_fw.c
@@ -3,6 +3,7 @@
#include "eeprom.h"
#include "ram.h"
#include "adc_dma.h"
+#include "imu.h"
#include "darwin_time.h"
#include "darwin_dyn_slave.h"
#include "darwin_dyn_master.h"
@@ -19,6 +20,8 @@ int main(void)
ram_init();
// initialize adc
adc_init();
+ // initialize imu
+ imu_init();
// initialize time module
darwin_time_init();
/* initialize the dynamixel slave interface */
@@ -27,14 +30,14 @@ int main(void)
/* initialize the dynamixel master interface */
darwin_dyn_master_init();
- darwin_dyn_master_enable_power();
+/* 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();
+ darwin_dyn_master_disable_power();*/
while(1);/* main function does not return */
}
diff --git a/src/darwin_dyn_slave.c b/src/darwin_dyn_slave.c
index e85810e8b2c8adb37e72d428b0bb939d803315d7..7f514c92ad653640d71bd5dd1b4547271ff629e0 100755
--- a/src/darwin_dyn_slave.c
+++ b/src/darwin_dyn_slave.c
@@ -2,7 +2,10 @@
#include "darwin_time.h"
#include "usart3.h"
#include "ram.h"
+#include "eeprom.h"
#include "gpio.h"
+#include "adc_dma.h"
+#include "imu.h"
/* timer for the execution of the dynamixel slave loop */
#define DYN_SLAVE_TIMER TIM7
@@ -32,31 +35,39 @@ unsigned char darwin_on_read(unsigned short int address,unsigned short int lengt
unsigned char darwin_on_write(unsigned short int address,unsigned short int length,unsigned char *data)
{
+ unsigned short int i,j;
+
/* dynamixel slave internal operation registers */
- if(ram_in_range(BIOLOID_ID,address,length))
+ if(ram_in_range(DARWIN_ID,address,length))
{
- dyn_slave_set_address(&darwin_dyn_slave,data[BIOLOID_ID-address]);
- ram_data[BIOLOID_ID]=data[BIOLOID_ID-address];
+ dyn_slave_set_address(&darwin_dyn_slave,data[DARWIN_ID-address]);
+ ram_data[DARWIN_ID]=data[DARWIN_ID-address];
}
- if(ram_in_range(BIOLOID_BAUD_RATE,address,length))
+ if(ram_in_range(DARWIN_BAUD_RATE,address,length))
{
- darwin_comm_init.BaudRate=2000000/(data[BIOLOID_BAUD_RATE-address]+1);
+ darwin_comm_init.BaudRate=2000000/(data[DARWIN_BAUD_RATE-address]+1);
usart3_config(&darwin_dyn_slave_comm,&darwin_comm_init);
- ram_data[BIOLOID_BAUD_RATE]=data[BIOLOID_BAUD_RATE-address];
+ ram_data[DARWIN_BAUD_RATE]=data[DARWIN_BAUD_RATE-address];
}
- if(ram_in_range(BIOLOID_RETURN_DELAY_TIME,address,length))
+ if(ram_in_range(DARWIN_RETURN_DELAY_TIME,address,length))
{
- dyn_slave_set_return_delay(&darwin_dyn_slave,data[BIOLOID_RETURN_DELAY_TIME-address]);
- ram_data[BIOLOID_RETURN_DELAY_TIME]=data[BIOLOID_RETURN_DELAY_TIME-address];
+ dyn_slave_set_return_delay(&darwin_dyn_slave,data[DARWIN_RETURN_DELAY_TIME-address]);
+ ram_data[DARWIN_RETURN_DELAY_TIME]=data[DARWIN_RETURN_DELAY_TIME-address];
}
- if(ram_in_range(BIOLOID_RETURN_LEVEL,address,length))
+ if(ram_in_range(DARWIN_RETURN_LEVEL,address,length))
{
- dyn_slave_set_return_level(&darwin_dyn_slave,data[BIOLOID_RETURN_LEVEL-address]);
- ram_data[BIOLOID_RETURN_LEVEL]=data[BIOLOID_RETURN_LEVEL-address];
+ dyn_slave_set_return_level(&darwin_dyn_slave,data[DARWIN_RETURN_LEVEL-address]);
+ ram_data[DARWIN_RETURN_LEVEL]=data[DARWIN_RETURN_LEVEL-address];
}
// GPIO commands
if(gpio_in_range(address,length))
gpio_process_write_cmd(address,length,data);
+ // ADC commands
+ if(adc_in_range(address,length))
+ adc_process_write_cmd(address,length,data);
+ // IMU commands
+ if(imu_in_range(address,length))
+ imu_process_write_cmd(address,length,data);
// write eeprom
for(i=address,j=0;i<LAST_EEPROM_OFFSET && i<(address+length);i++,j++)
EE_WriteVariable(i,data[j]);
@@ -91,13 +102,13 @@ void darwin_dyn_slave_init(void)
/* 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;
+ darwin_comm_init.BaudRate = 921600;
+ darwin_comm_init.WordLength = UART_WORDLENGTH_8B;
+ darwin_comm_init.StopBits = UART_STOPBITS_1;
+ darwin_comm_init.Parity = UART_PARITY_NONE;
+ darwin_comm_init.Mode = UART_MODE_TX_RX;
+ darwin_comm_init.HwFlowCtl = UART_HWCONTROL_NONE;
+ darwin_comm_init.OverSampling = UART_OVERSAMPLING_16;
priorities.irq_priority=0;
priorities.irq_subpriority=0;
@@ -106,21 +117,21 @@ void darwin_dyn_slave_init(void)
priorities.dma_tx_priority=1;
priorities.dma_tx_subpriority=0;
- usart3_init(&darwin_dyn_slave_comm,&Init,&priorities);
+ usart3_init(&darwin_dyn_slave_comm,&darwin_comm_init,&priorities);
dyn_slave_init(&darwin_dyn_slave,&darwin_dyn_slave_comm,0x01,DYN_VER2);
darwin_dyn_slave.on_read=darwin_on_read;
darwin_dyn_slave.on_write=darwin_on_write;
darwin_dyn_slave.on_ping=darwin_on_ping;
dyn_slave_set_return_delay(&darwin_dyn_slave,ram_data[DARWIN_RETURN_DELAY_TIME]);
- dyn_slave_set_return_level(&darwin_dyn_slave,ram_data[DARWIN_STATUS_RETURN_LEVEL]);
+ dyn_slave_set_return_level(&darwin_dyn_slave,ram_data[DARWIN_RETURN_LEVEL]);
/* initialize timer for the execution of the dynamixel slave loop */
DYN_SLAVE_TIMER_ENABLE_CLK;
- bioloid_dyn_slave_tim_handle.Instance=DYN_SLAVE_TIMER;
- bioloid_dyn_slave_tim_handle.Init.Period = 1000;
- bioloid_dyn_slave_tim_handle.Init.Prescaler = 72;
- bioloid_dyn_slave_tim_handle.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
- bioloid_dyn_slave_tim_handle.Init.CounterMode = TIM_COUNTERMODE_UP;
+ darwin_dyn_slave_tim_handle.Instance=DYN_SLAVE_TIMER;
+ darwin_dyn_slave_tim_handle.Init.Period = 1000;
+ darwin_dyn_slave_tim_handle.Init.Prescaler = 72;
+ darwin_dyn_slave_tim_handle.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
+ darwin_dyn_slave_tim_handle.Init.CounterMode = TIM_COUNTERMODE_UP;
HAL_TIM_Base_Init(&darwin_dyn_slave_tim_handle);
// initialize the timer interrupts
HAL_NVIC_SetPriority(DYN_SLAVE_TIMER_IRQn, 2, 0);
diff --git a/src/darwin_time.c b/src/darwin_time.c
index 1935291b86d03424083e39b4fa9d71e4d2f2197d..fc34615e0878b66607f37fbbc86a13b4fbfb3901 100755
--- a/src/darwin_time.c
+++ b/src/darwin_time.c
@@ -61,7 +61,7 @@ void darwin_time_init(void)
us_timer_Handle.Init.ClockDivision=TIM_CLOCKDIVISION_DIV1;
us_timer_Handle.Init.CounterMode=TIM_COUNTERMODE_UP;
HAL_TIM_Base_Init(&us_timer_Handle);
- __HAL_RCC_TIM8_CLK_ENABLE()
+ __HAL_RCC_TIM8_CLK_ENABLE();
__HAL_TIM_SetCounter(&us_timer_Handle,0);
sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
diff --git a/src/gpio.c b/src/gpio.c
index d42448f6802b89dd12c052d5aba3e40a4d86b96b..d6a64f810b487235c3478e6d70989952223d8cf4 100755
--- a/src/gpio.c
+++ b/src/gpio.c
@@ -75,20 +75,22 @@
TIM_HandleTypeDef GPO_TIM1Handle;
TIM_HandleTypeDef GPO_TIM2Handle;
TIM_HandleTypeDef GPO_TIM3Handle;
-// LED blink periods
-__IO uint16_t led_tx_period;
-__IO uint16_t led_rx_period;
-__IO uint16_t led_2_period;
-__IO uint16_t led_3_period;
-__IO uint16_t led_5_R_color;
-__IO uint16_t led_5_G_color;
-__IO uint16_t led_5_B_color;
-__IO uint16_t led_6_R_color;
-__IO uint16_t led_6_G_color;
-__IO uint16_t led_6_B_color;
// Pushbuttons callbacks
void (*start_pb_callback)(void);
void (*mode_pb_callback)(void);
+// LED's info structures
+const led_info_t leds[GPIO_NUM_LEDS]={{LED_TX_GPIO_PORT,LED_TX_PIN,&GPO_TIM1Handle,TIM_CHANNEL_1,DARWIN_TX_LED_CNTRL,DARWIN_TX_LED_PERIOD_L,GPIO_VALUE,GPIO_TOGGLE,GPIO_BLINK},
+ {LED_RX_GPIO_PORT,LED_RX_PIN,&GPO_TIM1Handle,TIM_CHANNEL_2,DARWIN_RX_LED_CNTRL,DARWIN_RX_LED_PERIOD_L,GPIO_VALUE,GPIO_TOGGLE,GPIO_BLINK},
+ {LED_2_GPIO_PORT,LED_2_PIN,&GPO_TIM1Handle,TIM_CHANNEL_3,DARWIN_PLAY_LED_CNTRL,DARWIN_PLAY_LED_PERIOD_L,GPIO_VALUE,GPIO_TOGGLE,GPIO_BLINK},
+ {LED_3_GPIO_PORT,LED_3_PIN,&GPO_TIM1Handle,TIM_CHANNEL_4,DARWIN_EDIT_LED_CNTRL,DARWIN_EDIT_LED_PERIOD_L,GPIO_VALUE,GPIO_TOGGLE,GPIO_BLINK},
+ {LED_4_GPIO_PORT,LED_4_PIN,0,0,DARWIN_MNG_LED_CNTRL,0x0000,GPIO_VALUE,GPIO_TOGGLE,0x00},
+ {LED_5_R_GPIO_PORT,LED_5_R_PIN,&GPO_TIM2Handle,TIM_CHANNEL_1,DARWIN_AUX1_LED_CNTRL,DARWIN_AUX1_LED_COLOR_L,GPIO_VALUE_R,GPIO_TOGGLE_R,0x00},
+ {LED_5_G_GPIO_PORT,LED_5_G_PIN,&GPO_TIM2Handle,TIM_CHANNEL_2,DARWIN_AUX1_LED_CNTRL,DARWIN_AUX1_LED_COLOR_L,GPIO_VALUE_G,GPIO_TOGGLE_G,0x00},
+ {LED_5_B_GPIO_PORT,LED_5_B_PIN,&GPO_TIM2Handle,TIM_CHANNEL_3,DARWIN_AUX1_LED_CNTRL,DARWIN_AUX1_LED_COLOR_L,GPIO_VALUE_B,GPIO_TOGGLE_B,0x00},
+ {LED_6_R_GPIO_PORT,LED_6_R_PIN,&GPO_TIM3Handle,TIM_CHANNEL_1,DARWIN_AUX2_LED_CNTRL,DARWIN_AUX2_LED_COLOR_L,GPIO_VALUE_R,GPIO_TOGGLE_R,0x00},
+ {LED_6_G_GPIO_PORT,LED_6_G_PIN,&GPO_TIM3Handle,TIM_CHANNEL_2,DARWIN_AUX2_LED_CNTRL,DARWIN_AUX2_LED_COLOR_L,GPIO_VALUE_G,GPIO_TOGGLE_G,0x00},
+ {LED_6_B_GPIO_PORT,LED_6_B_PIN,&GPO_TIM3Handle,TIM_CHANNEL_3,DARWIN_AUX2_LED_CNTRL,DARWIN_AUX2_LED_COLOR_L,GPIO_VALUE_B,GPIO_TOGGLE_B,0x00}};
+uint16_t leds_data[GPIO_NUM_LEDS];
// IRQ handler functions
void GPI_EXTI1_IRQHandler(void)
@@ -123,7 +125,7 @@ void GPO_TIMER1_IRQHandler(void)
{
__HAL_TIM_CLEAR_IT(&GPO_TIM1Handle, TIM_IT_CC1);
capture = HAL_TIM_ReadCapturedValue(&GPO_TIM1Handle, TIM_CHANNEL_1);
- __HAL_TIM_SET_COMPARE(&GPO_TIM1Handle, TIM_CHANNEL_1, (capture + led_tx_period));
+ __HAL_TIM_SET_COMPARE(&GPO_TIM1Handle, TIM_CHANNEL_1, (capture + leds_data[LED_TX]));
HAL_GPIO_TogglePin(LED_TX_GPIO_PORT,LED_TX_PIN);
}
}
@@ -133,7 +135,7 @@ void GPO_TIMER1_IRQHandler(void)
{
__HAL_TIM_CLEAR_IT(&GPO_TIM1Handle, TIM_IT_CC2);
capture = HAL_TIM_ReadCapturedValue(&GPO_TIM1Handle, TIM_CHANNEL_2);
- __HAL_TIM_SET_COMPARE(&GPO_TIM1Handle, TIM_CHANNEL_2, (capture + led_rx_period));
+ __HAL_TIM_SET_COMPARE(&GPO_TIM1Handle, TIM_CHANNEL_2, (capture + leds_data[LED_RX]));
HAL_GPIO_TogglePin(LED_RX_GPIO_PORT,LED_RX_PIN);
}
}
@@ -143,7 +145,7 @@ void GPO_TIMER1_IRQHandler(void)
{
__HAL_TIM_CLEAR_IT(&GPO_TIM1Handle, TIM_IT_CC3);
capture = HAL_TIM_ReadCapturedValue(&GPO_TIM1Handle, TIM_CHANNEL_3);
- __HAL_TIM_SET_COMPARE(&GPO_TIM1Handle, TIM_CHANNEL_3, (capture + led_2_period));
+ __HAL_TIM_SET_COMPARE(&GPO_TIM1Handle, TIM_CHANNEL_3, (capture + leds_data[LED_2]));
HAL_GPIO_TogglePin(LED_2_GPIO_PORT,LED_2_PIN);
}
}
@@ -153,7 +155,7 @@ void GPO_TIMER1_IRQHandler(void)
{
__HAL_TIM_CLEAR_IT(&GPO_TIM1Handle, TIM_IT_CC4);
capture = HAL_TIM_ReadCapturedValue(&GPO_TIM1Handle, TIM_CHANNEL_4);
- __HAL_TIM_SET_COMPARE(&GPO_TIM1Handle, TIM_CHANNEL_4, (capture + led_3_period));
+ __HAL_TIM_SET_COMPARE(&GPO_TIM1Handle, TIM_CHANNEL_4, (capture + leds_data[LED_3]));
HAL_GPIO_TogglePin(LED_3_GPIO_PORT,LED_3_PIN);
}
}
@@ -190,12 +192,12 @@ void GPO_TIMER2_IRQHandler(void)
capture = HAL_TIM_ReadCapturedValue(&GPO_TIM2Handle, TIM_CHANNEL_1);
if(red_phase==0x00)
{
- __HAL_TIM_SET_COMPARE(&GPO_TIM2Handle, TIM_CHANNEL_1, (capture + led_5_R_color));
+ __HAL_TIM_SET_COMPARE(&GPO_TIM2Handle, TIM_CHANNEL_1, (capture + leds_data[LED_5_R]));
red_phase=0x01;
}
else
{
- __HAL_TIM_SET_COMPARE(&GPO_TIM2Handle, TIM_CHANNEL_1, (capture + (31-led_5_R_color)));
+ __HAL_TIM_SET_COMPARE(&GPO_TIM2Handle, TIM_CHANNEL_1, (capture + (31-leds_data[LED_5_R])));
red_phase=0x00;
}
HAL_GPIO_TogglePin(LED_5_R_GPIO_PORT,LED_5_R_PIN);
@@ -209,12 +211,12 @@ void GPO_TIMER2_IRQHandler(void)
capture = HAL_TIM_ReadCapturedValue(&GPO_TIM2Handle, TIM_CHANNEL_2);
if(green_phase==0x00)
{
- __HAL_TIM_SET_COMPARE(&GPO_TIM2Handle, TIM_CHANNEL_2, (capture + led_5_G_color));
+ __HAL_TIM_SET_COMPARE(&GPO_TIM2Handle, TIM_CHANNEL_2, (capture + leds_data[LED_5_G]));
green_phase=0x01;
}
else
{
- __HAL_TIM_SET_COMPARE(&GPO_TIM2Handle, TIM_CHANNEL_2, (capture + (31-led_5_G_color)));
+ __HAL_TIM_SET_COMPARE(&GPO_TIM2Handle, TIM_CHANNEL_2, (capture + (31-leds_data[LED_5_G])));
green_phase=0x00;
}
HAL_GPIO_TogglePin(LED_5_G_GPIO_PORT,LED_5_G_PIN);
@@ -228,12 +230,12 @@ void GPO_TIMER2_IRQHandler(void)
capture = HAL_TIM_ReadCapturedValue(&GPO_TIM2Handle, TIM_CHANNEL_3);
if(blue_phase==0x00)
{
- __HAL_TIM_SET_COMPARE(&GPO_TIM2Handle, TIM_CHANNEL_3, (capture + led_5_B_color));
+ __HAL_TIM_SET_COMPARE(&GPO_TIM2Handle, TIM_CHANNEL_3, (capture + leds_data[LED_5_B]));
blue_phase=0x01;
}
else
{
- __HAL_TIM_SET_COMPARE(&GPO_TIM2Handle, TIM_CHANNEL_3, (capture + (31-led_5_B_color)));
+ __HAL_TIM_SET_COMPARE(&GPO_TIM2Handle, TIM_CHANNEL_3, (capture + (31-leds_data[LED_5_B])));
blue_phase=0x00;
}
HAL_GPIO_TogglePin(LED_5_B_GPIO_PORT,LED_5_B_PIN);
@@ -272,12 +274,12 @@ void GPO_TIMER3_IRQHandler(void)
capture = HAL_TIM_ReadCapturedValue(&GPO_TIM3Handle, TIM_CHANNEL_1);
if(red_phase==0x00)
{
- __HAL_TIM_SET_COMPARE(&GPO_TIM3Handle, TIM_CHANNEL_1, (capture + led_6_R_color));
+ __HAL_TIM_SET_COMPARE(&GPO_TIM3Handle, TIM_CHANNEL_1, (capture + leds_data[LED_6_R]));
red_phase=0x01;
}
else
{
- __HAL_TIM_SET_COMPARE(&GPO_TIM3Handle, TIM_CHANNEL_1, (capture + (31-led_6_R_color)));
+ __HAL_TIM_SET_COMPARE(&GPO_TIM3Handle, TIM_CHANNEL_1, (capture + (31-leds_data[LED_6_R])));
red_phase=0x00;
}
HAL_GPIO_TogglePin(LED_6_R_GPIO_PORT,LED_6_R_PIN);
@@ -291,12 +293,12 @@ void GPO_TIMER3_IRQHandler(void)
capture = HAL_TIM_ReadCapturedValue(&GPO_TIM3Handle, TIM_CHANNEL_2);
if(green_phase==0x00)
{
- __HAL_TIM_SET_COMPARE(&GPO_TIM3Handle, TIM_CHANNEL_2, (capture + led_6_G_color));
+ __HAL_TIM_SET_COMPARE(&GPO_TIM3Handle, TIM_CHANNEL_2, (capture + leds_data[LED_6_G]));
green_phase=0x01;
}
else
{
- __HAL_TIM_SET_COMPARE(&GPO_TIM3Handle, TIM_CHANNEL_2, (capture + (31-led_6_G_color)));
+ __HAL_TIM_SET_COMPARE(&GPO_TIM3Handle, TIM_CHANNEL_2, (capture + (31-leds_data[LED_6_G])));
green_phase=0x00;
}
HAL_GPIO_TogglePin(LED_6_G_GPIO_PORT,LED_6_G_PIN);
@@ -310,12 +312,12 @@ void GPO_TIMER3_IRQHandler(void)
capture = HAL_TIM_ReadCapturedValue(&GPO_TIM3Handle, TIM_CHANNEL_3);
if(blue_phase==0x00)
{
- __HAL_TIM_SET_COMPARE(&GPO_TIM3Handle, TIM_CHANNEL_3, (capture + led_6_B_color));
+ __HAL_TIM_SET_COMPARE(&GPO_TIM3Handle, TIM_CHANNEL_3, (capture + leds_data[LED_6_B]));
blue_phase=0x01;
}
else
{
- __HAL_TIM_SET_COMPARE(&GPO_TIM3Handle, TIM_CHANNEL_3, (capture + (31-led_6_B_color)));
+ __HAL_TIM_SET_COMPARE(&GPO_TIM3Handle, TIM_CHANNEL_3, (capture + (31-leds_data[LED_6_B])));
blue_phase=0x00;
}
HAL_GPIO_TogglePin(LED_6_B_GPIO_PORT,LED_6_B_PIN);
@@ -340,6 +342,60 @@ void GPO_TIMER3_IRQHandler(void)
}
// private functions
+void gpio_change_led_state(led_t led_id,uint16_t address,uint8_t *data)
+{
+ const led_info_t *led=&leds[led_id];
+
+ if(data[led->control_reg-address]&led->toggle_mask)
+ {
+ gpio_toggle_led(led_id);
+ if(ram_data[led->control_reg]&led->value_mask)
+ ram_data[led->control_reg]&=(~led->value_mask);
+ else
+ ram_data[led->control_reg]|=led->value_mask;
+ }
+ else
+ {
+ if(data[led->control_reg-address]&led->value_mask)
+ {
+ gpio_set_led(led_id);
+ ram_data[led->control_reg]|=led->value_mask;
+ }
+ else
+ {
+ gpio_clear_led(led_id);
+ ram_data[led->control_reg]&=(~led->value_mask);
+ }
+ }
+}
+
+void gpio_process_led(led_t led_id,uint16_t address,uint16_t length,uint8_t *data)
+{
+ const led_info_t *led=&leds[led_id];
+ uint16_t value;
+
+ if(ram_in_range(led->control_reg,address,length))
+ {
+ if(!(ram_data[led->control_reg]&GPIO_INT_USED))/* GPIO is not internally used */
+ {
+ ram_read_word(led->data_reg,&value);
+ if(ram_in_range(led->data_reg,address,length))
+ value=(value&0xFF00)+data[led->data_reg-address];
+ if(ram_in_range(led->data_reg+1,address,length))
+ value=(value&0x00FF)+(data[led->data_reg+1-address]<<8);
+ if(data[led->control_reg-address]&led->blink_mask)
+ gpio_blink_led(led_id,value);
+ else if(data[led->control_reg-address]&GPIO_COLOR)
+ gpio_set_color(led_id,value);
+ else
+ {
+ gpio_blink_led(led_id,0x0000);
+ gpio_set_color(led_id,0x0000);
+ gpio_change_led_state(led_id,address,data);
+ }
+ }
+ }
+}
// public functions
void gpio_init(void)
@@ -347,6 +403,7 @@ void gpio_init(void)
GPIO_InitTypeDef GPIO_InitStructure;
TIM_ClockConfigTypeDef sClockSourceConfig;
TIM_MasterConfigTypeDef sMasterConfig;
+ uint8_t i;
/* enable clocks */
ENABLE_LED_RX_GPIO_CLK;
@@ -409,16 +466,8 @@ void gpio_init(void)
HAL_GPIO_Init(MODE_PB_GPIO_PORT, &GPIO_InitStructure);
// initialize private variables
- led_rx_period=0;
- led_rx_period=0;
- led_2_period=0;
- led_3_period=0;
- led_5_R_color=0;
- led_5_G_color=0;
- led_5_B_color=0;
- led_6_R_color=0;
- led_6_G_color=0;
- led_6_B_color=0;
+ for(i=0;i<GPIO_NUM_LEDS;i++)
+ leds_data[i]=0;
start_pb_callback=0;
mode_pb_callback=0;
@@ -501,305 +550,71 @@ void gpio_init(void)
void gpio_set_led(led_t led_id)
{
- switch(led_id)
- {
- case LED_TX:
- HAL_GPIO_WritePin(LED_TX_GPIO_PORT,LED_TX_PIN,GPIO_PIN_RESET);
- break;
- case LED_RX:
- HAL_GPIO_WritePin(LED_RX_GPIO_PORT,LED_RX_PIN,GPIO_PIN_RESET);
- break;
- case LED_2:
- HAL_GPIO_WritePin(LED_2_GPIO_PORT,LED_2_PIN,GPIO_PIN_RESET);
- break;
- case LED_3:
- HAL_GPIO_WritePin(LED_3_GPIO_PORT,LED_3_PIN,GPIO_PIN_RESET);
- break;
- case LED_4:
- HAL_GPIO_WritePin(LED_4_GPIO_PORT,LED_4_PIN,GPIO_PIN_RESET);
- break;
- case LED_5_R:
- HAL_GPIO_WritePin(LED_5_R_GPIO_PORT,LED_5_R_PIN,GPIO_PIN_RESET);
- break;
- case LED_5_G:
- HAL_GPIO_WritePin(LED_5_G_GPIO_PORT,LED_5_G_PIN,GPIO_PIN_RESET);
- break;
- case LED_5_B:
- HAL_GPIO_WritePin(LED_5_B_GPIO_PORT,LED_5_B_PIN,GPIO_PIN_RESET);
- break;
- case LED_6_R:
- HAL_GPIO_WritePin(LED_6_R_GPIO_PORT,LED_6_R_PIN,GPIO_PIN_RESET);
- break;
- case LED_6_G:
- HAL_GPIO_WritePin(LED_6_G_GPIO_PORT,LED_6_G_PIN,GPIO_PIN_RESET);
- break;
- case LED_6_B:
- HAL_GPIO_WritePin(LED_6_B_GPIO_PORT,LED_6_B_PIN,GPIO_PIN_RESET);
- break;
- }
+ HAL_GPIO_WritePin(leds[led_id].port,leds[led_id].pin,GPIO_PIN_RESET);
}
void gpio_clear_led(led_t led_id)
{
- switch(led_id)
- {
- case LED_TX:
- HAL_GPIO_WritePin(LED_TX_GPIO_PORT,LED_TX_PIN,GPIO_PIN_SET);
- break;
- case LED_RX:
- HAL_GPIO_WritePin(LED_RX_GPIO_PORT,LED_RX_PIN,GPIO_PIN_SET);
- break;
- case LED_2:
- HAL_GPIO_WritePin(LED_2_GPIO_PORT,LED_2_PIN,GPIO_PIN_SET);
- break;
- case LED_3:
- HAL_GPIO_WritePin(LED_3_GPIO_PORT,LED_3_PIN,GPIO_PIN_SET);
- break;
- case LED_4:
- HAL_GPIO_WritePin(LED_4_GPIO_PORT,LED_4_PIN,GPIO_PIN_SET);
- break;
- case LED_5_R:
- HAL_GPIO_WritePin(LED_5_R_GPIO_PORT,LED_5_R_PIN,GPIO_PIN_SET);
- break;
- case LED_5_G:
- HAL_GPIO_WritePin(LED_5_G_GPIO_PORT,LED_5_G_PIN,GPIO_PIN_SET);
- break;
- case LED_5_B:
- HAL_GPIO_WritePin(LED_5_B_GPIO_PORT,LED_5_B_PIN,GPIO_PIN_SET);
- break;
- case LED_6_R:
- HAL_GPIO_WritePin(LED_6_R_GPIO_PORT,LED_6_R_PIN,GPIO_PIN_SET);
- break;
- case LED_6_G:
- HAL_GPIO_WritePin(LED_6_G_GPIO_PORT,LED_6_G_PIN,GPIO_PIN_SET);
- break;
- case LED_6_B:
- HAL_GPIO_WritePin(LED_6_B_GPIO_PORT,LED_6_B_PIN,GPIO_PIN_SET);
- break;
- }
+ HAL_GPIO_WritePin(leds[led_id].port,leds[led_id].pin,GPIO_PIN_SET);
}
void gpio_toggle_led(led_t led_id)
{
- switch(led_id)
- {
- case LED_TX:
- HAL_GPIO_TogglePin(LED_TX_GPIO_PORT,LED_TX_PIN);
- break;
- case LED_RX:
- HAL_GPIO_TogglePin(LED_RX_GPIO_PORT,LED_RX_PIN);
- break;
- case LED_2:
- HAL_GPIO_TogglePin(LED_2_GPIO_PORT,LED_2_PIN);
- break;
- case LED_3:
- HAL_GPIO_TogglePin(LED_3_GPIO_PORT,LED_3_PIN);
- break;
- case LED_4:
- HAL_GPIO_TogglePin(LED_4_GPIO_PORT,LED_4_PIN);
- break;
- case LED_5_R:
- HAL_GPIO_TogglePin(LED_5_R_GPIO_PORT,LED_5_R_PIN);
- break;
- case LED_5_G:
- HAL_GPIO_TogglePin(LED_5_G_GPIO_PORT,LED_5_G_PIN);
- break;
- case LED_5_B:
- HAL_GPIO_TogglePin(LED_5_B_GPIO_PORT,LED_5_B_PIN);
- break;
- case LED_6_R:
- HAL_GPIO_TogglePin(LED_6_R_GPIO_PORT,LED_6_R_PIN);
- break;
- case LED_6_G:
- HAL_GPIO_TogglePin(LED_6_G_GPIO_PORT,LED_6_G_PIN);
- break;
- case LED_6_B:
- HAL_GPIO_TogglePin(LED_6_B_GPIO_PORT,LED_6_B_PIN);
- break;
- }
+ HAL_GPIO_TogglePin(leds[led_id].port,leds[led_id].pin);
}
-void gpio_blink_led(led_t led_id, int16_t period_ms)
+void gpio_blink_led(led_t led_id, uint16_t period_ms)
{
TIM_OC_InitTypeDef TIM_OCInitStructure;
- TIM_OCInitStructure.OCMode = TIM_OCMODE_TIMING;
- TIM_OCInitStructure.OCPolarity = TIM_OCPOLARITY_HIGH;
- TIM_OCInitStructure.OCFastMode = TIM_OCFAST_DISABLE;
- switch(led_id)
+ if(led_id>=LED_TX && led_id<=LED_3)
{
- case LED_TX:
- if(period_ms>1)
- {
- led_tx_period=period_ms;
- TIM_OCInitStructure.Pulse = led_tx_period;
- HAL_TIM_OC_ConfigChannel(&GPO_TIM1Handle, &TIM_OCInitStructure,TIM_CHANNEL_1);
- HAL_TIM_OC_Start_IT(&GPO_TIM1Handle, TIM_CHANNEL_1);
- }
- else
- HAL_TIM_OC_Stop_IT(&GPO_TIM1Handle, TIM_CHANNEL_1);
- break;
- case LED_RX:
- if(period_ms>1)
- {
- led_rx_period=period_ms;
- TIM_OCInitStructure.Pulse = led_rx_period;
- HAL_TIM_OC_ConfigChannel(&GPO_TIM1Handle, &TIM_OCInitStructure,TIM_CHANNEL_2);
- HAL_TIM_OC_Start_IT(&GPO_TIM1Handle, TIM_CHANNEL_2);
- }
- else
- HAL_TIM_OC_Stop_IT(&GPO_TIM1Handle, TIM_CHANNEL_2);
- break;
- case LED_2:
- if(period_ms>1)
- {
- led_2_period=period_ms;
- TIM_OCInitStructure.Pulse = led_2_period;
- HAL_TIM_OC_ConfigChannel(&GPO_TIM1Handle, &TIM_OCInitStructure,TIM_CHANNEL_3);
- HAL_TIM_OC_Start_IT(&GPO_TIM1Handle, TIM_CHANNEL_3);
- }
- else
- HAL_TIM_OC_Stop_IT(&GPO_TIM1Handle, TIM_CHANNEL_3);
- break;
- case LED_3:
- if(period_ms>1)
- {
- led_3_period=period_ms;
- TIM_OCInitStructure.Pulse = led_3_period;
- HAL_TIM_OC_ConfigChannel(&GPO_TIM1Handle, &TIM_OCInitStructure,TIM_CHANNEL_4);
- HAL_TIM_OC_Start_IT(&GPO_TIM1Handle, TIM_CHANNEL_4);
- }
- else
- HAL_TIM_OC_Stop_IT(&GPO_TIM1Handle, TIM_CHANNEL_4);
- break;
- default: break;
+ TIM_OCInitStructure.OCMode = TIM_OCMODE_TIMING;
+ TIM_OCInitStructure.OCPolarity = TIM_OCPOLARITY_HIGH;
+ TIM_OCInitStructure.OCFastMode = TIM_OCFAST_DISABLE;
+ if(period_ms>1)
+ {
+ leds_data[led_id]=period_ms;
+ TIM_OCInitStructure.Pulse = period_ms;
+ HAL_TIM_OC_ConfigChannel(leds[led_id].timer, &TIM_OCInitStructure,leds[led_id].timer_ch);
+ HAL_TIM_OC_Start_IT(leds[led_id].timer,leds[led_id].timer_ch);
+ }
+ else
+ HAL_TIM_OC_Stop_IT(leds[led_id].timer,leds[led_id].timer_ch);
}
}
-void gpio_set_color(led_t led_id, uint8_t value)
+void gpio_set_color(led_t led_id, uint16_t value)
{
TIM_OC_InitTypeDef TIM_OCInitStructure;
- TIM_OCInitStructure.OCMode = TIM_OCMODE_TIMING;
- TIM_OCInitStructure.OCPolarity = TIM_OCPOLARITY_HIGH;
- TIM_OCInitStructure.OCFastMode = TIM_OCFAST_DISABLE;
- switch(led_id)
+ if(led_id>=LED_5_R && led_id<=LED_6_B)
{
- case LED_5_R:
- if(value==0)
- {
- HAL_TIM_OC_Stop_IT(&GPO_TIM2Handle, TIM_CHANNEL_1);
- gpio_clear_led(led_id);
- }
- else if(value<31)
- {
- led_5_R_color=value;
- TIM_OCInitStructure.Pulse = led_5_R_color;
- HAL_TIM_OC_ConfigChannel(&GPO_TIM2Handle, &TIM_OCInitStructure,TIM_CHANNEL_1);
- HAL_TIM_OC_Start_IT(&GPO_TIM2Handle, TIM_CHANNEL_1);
- }
- else
- {
- HAL_TIM_OC_Stop_IT(&GPO_TIM2Handle, TIM_CHANNEL_1);
- gpio_set_led(led_id);
- }
- break;
- case LED_5_G:
- if(value==0)
- {
- HAL_TIM_OC_Stop_IT(&GPO_TIM2Handle, TIM_CHANNEL_2);
- gpio_clear_led(led_id);
- }
- else if(value<31)
- {
- led_5_G_color=value;
- TIM_OCInitStructure.Pulse = led_5_G_color;
- HAL_TIM_OC_ConfigChannel(&GPO_TIM2Handle, &TIM_OCInitStructure,TIM_CHANNEL_2);
- HAL_TIM_OC_Start_IT(&GPO_TIM2Handle, TIM_CHANNEL_2);
- }
- else
- {
- HAL_TIM_OC_Stop_IT(&GPO_TIM2Handle, TIM_CHANNEL_2);
- gpio_set_led(led_id);
- }
- break;
- case LED_5_B:
- if(value==0)
- {
- HAL_TIM_OC_Stop_IT(&GPO_TIM2Handle, TIM_CHANNEL_3);
- gpio_clear_led(led_id);
- }
- else if(value<31)
- {
- led_5_B_color=value;
- TIM_OCInitStructure.Pulse = led_5_B_color;
- HAL_TIM_OC_ConfigChannel(&GPO_TIM2Handle, &TIM_OCInitStructure,TIM_CHANNEL_3);
- HAL_TIM_OC_Start_IT(&GPO_TIM2Handle, TIM_CHANNEL_3);
- }
- else
- {
- HAL_TIM_OC_Stop_IT(&GPO_TIM2Handle, TIM_CHANNEL_3);
- gpio_set_led(led_id);
- }
- break;
- case LED_6_R:
- if(value==0)
- {
- HAL_TIM_OC_Stop_IT(&GPO_TIM3Handle, TIM_CHANNEL_1);
- gpio_clear_led(led_id);
- }
- else if(value<31)
- {
- led_6_R_color=value;
- TIM_OCInitStructure.Pulse = led_6_R_color;
- HAL_TIM_OC_ConfigChannel(&GPO_TIM3Handle, &TIM_OCInitStructure,TIM_CHANNEL_1);
- HAL_TIM_OC_Start_IT(&GPO_TIM3Handle, TIM_CHANNEL_1);
- }
- else
- {
- HAL_TIM_OC_Stop_IT(&GPO_TIM3Handle, TIM_CHANNEL_1);
- gpio_set_led(led_id);
- }
- break;
- case LED_6_G:
- if(value==0)
- {
- HAL_TIM_OC_Stop_IT(&GPO_TIM3Handle, TIM_CHANNEL_2);
- gpio_clear_led(led_id);
- }
- else if(value<31)
- {
- led_6_G_color=value;
- TIM_OCInitStructure.Pulse = led_6_G_color;
- HAL_TIM_OC_ConfigChannel(&GPO_TIM3Handle, &TIM_OCInitStructure,TIM_CHANNEL_2);
- HAL_TIM_OC_Start_IT(&GPO_TIM3Handle, TIM_CHANNEL_2);
- }
- else
- {
- HAL_TIM_OC_Stop_IT(&GPO_TIM3Handle, TIM_CHANNEL_2);
- gpio_set_led(led_id);
- }
- break;
- case LED_6_B:
- if(value==0)
- {
- HAL_TIM_OC_Stop_IT(&GPO_TIM3Handle, TIM_CHANNEL_3);
- gpio_clear_led(led_id);
- }
- else if(value<31)
- {
- led_6_B_color=value;
- TIM_OCInitStructure.Pulse = led_6_B_color;
- HAL_TIM_OC_ConfigChannel(&GPO_TIM3Handle, &TIM_OCInitStructure,TIM_CHANNEL_3);
- HAL_TIM_OC_Start_IT(&GPO_TIM3Handle, TIM_CHANNEL_3);
- }
- else
- {
- HAL_TIM_OC_Stop_IT(&GPO_TIM3Handle, TIM_CHANNEL_3);
- gpio_set_led(led_id);
- }
- break;
- default: break;
+ TIM_OCInitStructure.OCMode = TIM_OCMODE_TIMING;
+ TIM_OCInitStructure.OCPolarity = TIM_OCPOLARITY_HIGH;
+ TIM_OCInitStructure.OCFastMode = TIM_OCFAST_DISABLE;
+ if(value==0)
+ {
+ HAL_TIM_OC_Stop_IT(leds[led_id].timer,leds[led_id].timer_ch);
+ gpio_clear_led(led_id);
+ }
+ else if(value<31)
+ {
+ if(led_id==LED_5_R || led_id==LED_6_R)
+ leds_data[led_id]=value&GPIO_RED_COLOR;
+ else if(led_id==LED_5_G || led_id==LED_6_G)
+ leds_data[led_id]=((value&GPIO_GREEN_COLOR)>>5);
+ else
+ leds_data[led_id]=((value&GPIO_BLUE_COLOR)>>10);
+ TIM_OCInitStructure.Pulse = leds_data[led_id];
+ HAL_TIM_OC_ConfigChannel(leds[led_id].timer,&TIM_OCInitStructure,leds[led_id].timer_ch);
+ HAL_TIM_OC_Start_IT(leds[led_id].timer,leds[led_id].timer_ch);
+ }
+ else
+ {
+ HAL_TIM_OC_Stop_IT(leds[led_id].timer,leds[led_id].timer_ch);
+ gpio_set_led(led_id);
+ }
}
}
@@ -864,46 +679,17 @@ void gpio_process_read_cmd(unsigned short int address,unsigned short int length,
void gpio_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data)
{
- uint16_t period;
-
- if(ram_in_range(DARWIN_TX_LED_CNTRL,address,length))
- {
- if(!(ram_data[DARWIN_TX_LED_CNTRL]&GPIO_INT_USED))/* GPIO is not internally used */
- {
- period=led_tx_period;
- if(ram_in_range(DARWIN_TX_LED_PERIOD_L,address,length))
- period=(period&0xFF00)+data[DARWIN_TX_LED_PERIOD_L-address];
- if(ram_in_range(DARWIN_TX_LED_PERIOD_H,address,length))
- period=(period&0x00FF)+(data[DARWIN_TX_LED_PERIOD_H-address]<<8);
- if(data[DARWIN_TX_LED_CNTRL-address]&GPIO_BLINK)
- gpio_blink_led(LED_TX,period);
- else
- {
- gpio_blink_led(LED_TX,0x0000);
- if(data[DARWIN_TX_LED_CNTRL-address]&GPIO_TOGGLE)
- {
- gpio_toggle_led(LED_TX);
- if(ram_data[DARWIN_TX_LED_CNTRL]&GPIO_VALUE)
- ram_data[DARWIN_TX_LED_CNTRL]&=(~GPIO_VALUE);
- else
- ram_data[DARWIN_TX_LED_CNTRL]|=GPIO_VALUE;
- }
- else
- {
- if(data[DARWIN_TX_LED_CNTRL-address]&GPIO_VALUE)
- {
- gpio_set_led(LED_TX);
- ram_data[DARWIN_TX_LED_CNTRL]|=GPIO_VALUE;
- }
- else
- {
- gpio_clear_led(LED_TX);
- ram_data[DARWIN_TX_LED_CNTRL]&=(~GPIO_VALUE);
- }
- }
- }
- }
- }
-
+ /* process standard leds */
+ gpio_process_led(LED_TX,address,length,data);
+ gpio_process_led(LED_RX,address,length,data);
+ gpio_process_led(LED_2,address,length,data);
+ gpio_process_led(LED_3,address,length,data);
+ gpio_process_led(LED_4,address,length,data);
+ gpio_process_led(LED_5_R,address,length,data);
+ gpio_process_led(LED_5_G,address,length,data);
+ gpio_process_led(LED_5_B,address,length,data);
+ gpio_process_led(LED_6_R,address,length,data);
+ gpio_process_led(LED_6_G,address,length,data);
+ gpio_process_led(LED_6_B,address,length,data);
}
diff --git a/src/imu.c b/src/imu.c
index b6d86d239ac1dc69d20f864d86f3f847be861025..02b924b949eeab971aa782a85d8e41c5a6429444 100755
--- a/src/imu.c
+++ b/src/imu.c
@@ -32,11 +32,11 @@
// accelerometer registers
#define ACCEL_ID 0x32
#define ACCEL_WHO_AM_I 0x0F
-#define ACCEL_CTRL_REG1 0x20
-#define ACCEL_CTRL_REG2 0x21
-#define ACCEL_CTRL_REG3 0x22
-#define ACCEL_CTRL_REG4 0x23
-#define ACCEL_CTRL_REG5 0x24
+#define ACCEL_CNTRL_REG1 0x20
+#define ACCEL_CNTRL_REG2 0x21
+#define ACCEL_CNTRL_REG3 0x22
+#define ACCEL_CNTRL_REG4 0x23
+#define ACCEL_CNTRL_REG5 0x24
#define ACCEL_HP_FILTER_RESET 0x25
#define ACCEL_REFERENCE 0x26
#define ACCEL_STATUS_REG 0x27
@@ -59,11 +59,11 @@
// gyroscope registers
#define GYRO_ID 0xD3
#define GYRO_WHO_AM_I 0x0F
-#define GYRO_CTRL_REG1 0x20
-#define GYRO_CTRL_REG2 0x21
-#define GYRO_CTRL_REG3 0x22
-#define GYRO_CTRL_REG4 0x23
-#define GYRO_CTRL_REG5 0x24
+#define GYRO_CNTRL_REG1 0x20
+#define GYRO_CNTRL_REG2 0x21
+#define GYRO_CNTRL_REG3 0x22
+#define GYRO_CNTRL_REG4 0x23
+#define GYRO_CNTRL_REG5 0x24
#define GYRO_REFERENCE 0x25
#define GYRO_OUT_TEMP 0x26
#define GYRO_STATUS_REG 0x27
@@ -73,7 +73,7 @@
#define GYRO_OUT_Y_H 0x2B
#define GYRO_OUT_Z_L 0x2C
#define GYRO_OUT_Z_H 0x2D
-#define GYRO_FIFO_CTRL_REG 0x2E
+#define GYRO_FIFO_CNTRL_REG 0x2E
#define GYRO_FIFO_SRC_REG 0x2F
#define GYRO_INT1_CFG 0x30
#define GYRO_INT1_SRC 0x31
@@ -98,7 +98,7 @@ uint8_t imu_current_device_id;
uint8_t imu_stop_flag;
volatile uint8_t imu_stopped;
// gyroscope variables
-const uint8_t gyro_conf_reg=GYRO_CTRL_REG1;
+const uint8_t gyro_conf_reg=GYRO_CNTRL_REG1;
const uint8_t gyro_conf_len=6;
uint8_t gyro_conf_data[6];
const uint8_t gyro_data_reg=GYRO_OUT_X_L;
@@ -106,9 +106,9 @@ const uint8_t gyro_data_len=6;
int16_t gyro_center[3];
int32_t gyro_data[3];
uint8_t gyro_calibration;
-const uint16_t gyro_cal_num_samples=1024;
+uint8_t gyro_calibration_num_samples;
// accelerometer variables
-const uint8_t accel_conf_reg=ACCEL_CTRL_REG1;
+const uint8_t accel_conf_reg=ACCEL_CNTRL_REG1;
const uint8_t accel_conf_len=5;
uint8_t accel_conf_data[5];
const uint8_t accel_data_reg=ACCEL_OUT_X_L;
@@ -208,12 +208,12 @@ uint8_t imu_accel_detect(void)
imu_spi_get_data(&data);
if(data==ACCEL_ID)
{
- ram_data[DARWIN_IMU_STATUS]|=0x01;
+ ram_data[DARWIN_IMU_CNTRL]|=IMU_ACCEL_DET;
return 0x01;
}
else
{
- ram_data[DARWIN_IMU_STATUS]&=0xFE;
+ ram_data[DARWIN_IMU_CNTRL]&=(~IMU_ACCEL_DET);
return 0x00;
}
}
@@ -276,12 +276,12 @@ uint8_t imu_gyro_detect(void)
imu_spi_get_data(&data);
if(data==GYRO_ID)
{
- ram_data[DARWIN_IMU_STATUS]|=0x02;
+ ram_data[DARWIN_IMU_CNTRL]|=IMU_GYRO_DET;
return 0x01;
}
else
{
- ram_data[DARWIN_IMU_STATUS]&=0xFD;
+ ram_data[DARWIN_IMU_CNTRL]&=(~IMU_GYRO_DET);
return 0x00;
}
}
@@ -361,7 +361,6 @@ void IMU_SPI_IRQHANDLER(void)
}
else
{
-// SPI_WaitOnFlagUntilTimeout(&hspi2,SPI_FLAG_TXE,RESET,SPI_TIMEOUT_VALUE);
__HAL_SPI_DISABLE_IT(&hspi2, SPI_IT_TXE);
}
}
@@ -385,8 +384,6 @@ void IMU_TIMER_IRQHandler(void)
{
// stop the timer to get the imu data
HAL_TIM_Base_Stop_IT(&htim);
-
- ram_clear_bit(DARWIN_IMU_STATUS,2);
imu_stop_flag=0x00;
imu_stopped=0x01;
}
@@ -394,7 +391,7 @@ void IMU_TIMER_IRQHandler(void)
{
switch(state)
{
- case IMU_GET_ACCEL: if(ram_data[DARWIN_IMU_STATUS]&0x02)
+ case IMU_GET_ACCEL: if(ram_data[DARWIN_IMU_CNTRL]&IMU_GYRO_DET)
{
imu_spi_get_data(data);
imu_gyro_convert_data(data,&ram_data[DARWIN_IMU_GYRO_X_L]);
@@ -404,33 +401,32 @@ void IMU_TIMER_IRQHandler(void)
gyro_accum[1]+=gyro_data[1];
gyro_accum[2]+=gyro_data[2];
num_samples++;
- if(num_samples==gyro_cal_num_samples)
+ if(num_samples==gyro_calibration_num_samples)
{
num_samples=0;
- gyro_center[0]=gyro_accum[0]/gyro_cal_num_samples;
- gyro_center[1]=gyro_accum[1]/gyro_cal_num_samples;
- gyro_center[2]=gyro_accum[2]/gyro_cal_num_samples;
+ gyro_center[0]=gyro_accum[0]/gyro_calibration_num_samples;
+ gyro_center[1]=gyro_accum[1]/gyro_calibration_num_samples;
+ gyro_center[2]=gyro_accum[2]/gyro_calibration_num_samples;
gyro_accum[0]=0;
gyro_accum[1]=0;
gyro_accum[2]=0;
- ram_set_bit(DARWIN_IMU_STATUS,3);
- ram_clear_bit(DARWIN_IMU_CONTROL,1);
- if((ram_data[DARWIN_IMU_STATUS]&0x04)==0x00)
+ ram_data[DARWIN_IMU_CNTRL]&=(~IMU_CALIBRATING);
+ if((ram_data[DARWIN_IMU_CNTRL]&IMU_RUNNING)==0x00)
imu_stop_flag=0x01;
gyro_calibration=0x00;
}
}
}
- if(ram_data[DARWIN_IMU_STATUS]&0x01)
+ if(ram_data[DARWIN_IMU_CNTRL]&IMU_ACCEL_DET)
imu_accel_get_data();
state=IMU_GET_GYRO;
break;
- case IMU_GET_GYRO: if(ram_data[DARWIN_IMU_STATUS]&0x01)
+ case IMU_GET_GYRO: if(ram_data[DARWIN_IMU_CNTRL]&IMU_ACCEL_DET)
{
imu_spi_get_data(data);
imu_accel_convert_data(data,&ram_data[DARWIN_IMU_ACCEL_X_L]);
}
- if(ram_data[DARWIN_IMU_STATUS]&0x02)
+ if(ram_data[DARWIN_IMU_CNTRL]&IMU_GYRO_DET)
imu_gyro_get_data();
state=IMU_GET_ACCEL;
break;
@@ -493,6 +489,7 @@ void imu_init(void)
gyro_center[1]=0;
gyro_center[2]=0;
gyro_calibration=0x00;
+ imu_set_calibration_samples(1024);
// configure the SPI module
hspi2.Instance = IMU_SPI;
@@ -527,6 +524,7 @@ void imu_init(void)
__HAL_SPI_ENABLE_IT(&hspi2, SPI_IT_RXNE);
+ // detect the sensors
if(imu_gyro_detect())
{
imu_gyro_get_config();
@@ -537,12 +535,11 @@ void imu_init(void)
imu_accel_get_config();
imu_accel_set_config();
}
- // detect the sensors
}
void imu_start(void)
{
- if((ram_data[DARWIN_IMU_STATUS]&0x04)==0x00)// imu is not running
+ if((ram_data[DARWIN_IMU_CNTRL]&IMU_RUNNING)==0x00)// imu is not running
{
// start the accelerometer
imu_accel_start();
@@ -550,27 +547,33 @@ void imu_start(void)
// start the timer to get the imu data
HAL_TIM_Base_Start_IT(&htim);
- ram_set_bit(DARWIN_IMU_STATUS,2);
- ram_set_bit(DARWIN_IMU_CONTROL,0);
+ ram_data[DARWIN_IMU_CNTRL]|=IMU_RUNNING;
}
}
void imu_stop(void)
{
- if(ram_data[DARWIN_IMU_STATUS]&0x04)
+ if(ram_data[DARWIN_IMU_CNTRL]&IMU_RUNNING)
{
imu_stop_flag=0x01;
while(!imu_stopped);
imu_stopped=0x00;
imu_accel_stop();
imu_gyro_stop();
- ram_clear_bit(DARWIN_IMU_CONTROL,0);
+ ram_data[DARWIN_IMU_CNTRL]&=(~IMU_RUNNING);
}
}
+void imu_set_calibration_samples(uint16_t num_samples)
+{
+ gyro_calibration_num_samples=num_samples;
+ ram_data[DARWIN_IMU_CAL_SAMPLES_L]=gyro_calibration_num_samples%256;
+ ram_data[DARWIN_IMU_CAL_SAMPLES_H]=gyro_calibration_num_samples/256;
+}
+
void imu_start_gyro_cal(void)
{
- if((ram_data[DARWIN_IMU_STATUS]&0x04)==0x00)// imu is not running
+ if((ram_data[DARWIN_IMU_CNTRL]&IMU_CALIBRATING)==0x00)// imu is not running
{
// start the accelerometer
imu_accel_start();
@@ -579,7 +582,7 @@ void imu_start_gyro_cal(void)
// start the timer to get the imu data
HAL_TIM_Base_Start_IT(&htim);
}
- ram_clear_bit(DARWIN_IMU_STATUS,3);
+ ram_data[DARWIN_IMU_CNTRL]|=IMU_CALIBRATING;
gyro_center[0]=0;
gyro_center[1]=0;
gyro_center[2]=0;
@@ -592,3 +595,23 @@ void imu_get_gyro_data(int32_t *x, int32_t *y, int32_t *z)
*y=gyro_data[1];
*z=gyro_data[2];
}
+
+// operation functions
+uint8_t imu_in_range(unsigned short int address,unsigned short int length)
+{
+ return ram_in_window(IMU_BASE_ADDRESS,IMU_MEM_LENGTH,address,length);
+}
+
+void imu_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data)
+{
+ if(ram_in_range(DARWIN_IMU_CNTRL,address,length))
+ {
+ if(data[DARWIN_IMU_CNTRL-address]&IMU_START)
+ imu_start();
+ else if(data[DARWIN_IMU_CNTRL-address]&IMU_STOP)
+ imu_stop();
+ else if(data[DARWIN_IMU_CNTRL-address]&IMU_START_CAL)
+ imu_start_gyro_cal();
+ }
+}
+
diff --git a/src/scheduler.c b/src/scheduler.c
index 8be673dbc9de21f8f7697ab27a1f6542efa92ae0..b8e0d999b713a1120c8a22453d93bec19e06bc0d 100644
--- a/src/scheduler.c
+++ b/src/scheduler.c
@@ -5,7 +5,54 @@
#define SCHEDULER_TIMER_IRQHandler TIM1_IRQHandler
#define SCHEDULER_TIMER_CLK __HAL_RCC_TIM1_CLK_ENABLE()
+typedef void (*scheduler_function)(void);
+
+scheduler_function sch_ch1_fnct;
+scheduler_function sch_ch2_fnct;
+scheduler_function sch_ch3_fnct;
+scheduler_function sch_ch4_fnct;
+uint16_t sch_ch1_period;
+uint16_t sch_ch2_period;
+uint16_t sch_ch3_period;
+uint16_t sch_ch4_period;
+
void scheduler_init(void)
{
-
+ /* initialize internal variables */
+ sch_ch1_fnct=0;
+ sch_ch2_fnct=0;
+ sch_ch3_fnct=0;
+ sch_ch4_fnct=0;
+ sch_ch1_period=0;
+ sch_ch2_period=0;
+ sch_ch3_period=0;
+ sch_ch4_period=0;
+ /* initialize timer 1 */
+
+}
+
+void scheduler_set_period(sch_ch_t channel_id, uint16_t period_ms)
+{
+
}
+
+void scheduler_set_one_shot(sch_ch_t channel_id, uint16_t time_ms)
+{
+
+}
+
+void scheduler_set_function(sch_ch_t channel_id, void (*function)(void))
+{
+
+}
+
+void scheduler_start(sch_ch_t channel_id)
+{
+
+}
+
+void scheduler_stop(sch_ch_t channel_id)
+{
+
+}
+