From 4c50f41eb52f57a7ed28ec2acfe1f3066d8f05ec Mon Sep 17 00:00:00 2001 From: but0n Date: Mon, 4 Sep 2017 21:53:48 +0800 Subject: [PATCH] feat: (Module) Totally refactor --- libs/module/avm_i2c.c | 20 ++++- libs/module/avm_i2c.h | 7 +- libs/module/avm_motor.c | 18 +++- libs/module/avm_motor.h | 9 +- libs/module/avm_mpu6050.c | 69 ++++++++------- libs/module/avm_mpu6050.h | 21 +++-- libs/module/avm_pid.c | 28 ++++++- libs/module/avm_pid.h | 4 +- libs/module/avm_uart.c | 105 +++++++++++------------ libs/module/avm_uart.h | 27 ++---- src/main.c | 171 ++++---------------------------------- 11 files changed, 198 insertions(+), 281 deletions(-) diff --git a/libs/module/avm_i2c.c b/libs/module/avm_i2c.c index 857daca..dbc536a 100644 --- a/libs/module/avm_i2c.c +++ b/libs/module/avm_i2c.c @@ -1,9 +1,21 @@ -#include "avm_i2c.h" +#include -#include "stm32f10x.h" -#include "avm_bit.h" +static unsigned char avm_i2c_init(void *arg); -void delay_us(volatile unsigned int nus) { +avm_module_t avm_i2c_module_st = { + 0, + NULL, + avm_i2c_init, + NULL, + NULL +}; + +unsigned char avm_i2c_init(void *arg) { + IIC_init(); + return 0; +} + +static void delay_custom(volatile unsigned int nus) { for(nus *= 4; nus; nus--); } diff --git a/libs/module/avm_i2c.h b/libs/module/avm_i2c.h index fc5c388..35116af 100644 --- a/libs/module/avm_i2c.h +++ b/libs/module/avm_i2c.h @@ -11,11 +11,12 @@ #define READ_SDA BIT_ADDR(&(GPIOB->IDR), SDA_PINNUM) -#define IIC_DELAY() delay_us(1) +#define IIC_DELAY() delay_custom(1) + +extern avm_module_t avm_i2c_module_st; - -void delay_us(volatile unsigned int nus); +// void delay_us(volatile unsigned int nus); void IIC_init(); void IIC_Start(); diff --git a/libs/module/avm_motor.c b/libs/module/avm_motor.c index b9b0d5e..8b01e24 100644 --- a/libs/module/avm_motor.c +++ b/libs/module/avm_motor.c @@ -1,5 +1,19 @@ -#include "avm_motor.h" -#include "stm32f10x.h" +#include + +static unsigned char avm_motor_init(void *); + +avm_module_t avm_motor_module_st = { + 0, + NULL, + avm_motor_init, + NULL, + NULL +}; + +static unsigned char avm_motor_init(void *arg) { + MOTOR_SETTING(); + return 0; +} //A7 void motor_PWM_Init(unsigned short arr, unsigned short psc) { diff --git a/libs/module/avm_motor.h b/libs/module/avm_motor.h index d482139..6b9afbb 100644 --- a/libs/module/avm_motor.h +++ b/libs/module/avm_motor.h @@ -1,6 +1,8 @@ #ifndef _AVM_MOTOR_H_ #define _AVM_MOTOR_H_ +extern avm_module_t avm_motor_module_st; + #define MOTOR_NORMAL_STARTUP @@ -19,19 +21,20 @@ #define MOTOR_SETTING() do {\ motor_PWM_Init(36000,40);\ MOTOR_ALL = THROTTLE_MIN;\ - delay(4000);\ + delay_ms(4000);\ MOTOR_ALL = THROTTLE_MID;\ } while(0) #else #define MOTOR_SETTING() do {\ motor_PWM_Init(36000,40);\ MOTOR_ALL = THROTTLE_MAX;\ - delay(3000);\ + delay_ms(3000);\ MOTOR_ALL = THROTTLE_MIN;\ - delay(8000);\ + delay_ms(8000);\ } while(0) #endif + void motor_PWM_Init(unsigned short arr, unsigned short psc); //72MHz / (arr + 1)*(psc + 1) #endif diff --git a/libs/module/avm_mpu6050.c b/libs/module/avm_mpu6050.c index 84c6076..21a6644 100644 --- a/libs/module/avm_mpu6050.c +++ b/libs/module/avm_mpu6050.c @@ -1,9 +1,22 @@ -#include "avm_mpu6050.h" +#include + +static unsigned char avm_mpu_init(void *); + +avm_module_t avm_mpu_module_st = { + 0, + NULL, + avm_mpu_init, + NULL, + NULL +}; + +static unsigned char avm_mpu_init(void *arg) { + // Delay is required after MPU6050 powered up, At least 7ms + delay_ms(7); + MPU_init(); + return 0; +} -#include "avm_bit.h" -#include "stm32f10x.h" -#include "avm_i2c.h" -#include "avm_uart.h" void MPU_Sigle_Write(unsigned char reg_addr, unsigned char reg_data) { IIC_Start(); @@ -76,26 +89,26 @@ void MPU6050_getStructData(pSixAxis cache) { cache->aZ += A_Z_OFFSET; #endif } -void MPU6050_debug(pSixAxis cache) { - uart_Float2Char((float)cache->gX); - uart_sendData(' '); - uart_Float2Char((float)cache->gY); - uart_sendData(' '); - uart_Float2Char((float)cache->gZ); - uart_sendData(' '); - - uart_Float2Char((float)cache->aX); - uart_sendData(' '); - uart_Float2Char((float)cache->aY); - uart_sendData(' '); - uart_Float2Char((float)cache->aZ); - uart_sendData(' '); - uart_sendData(0x0D); - uart_sendData(0x0A); -} +// void MPU6050_debug(pSixAxis cache) { +// uart_Float2Char((float)cache->gX); +// uart_sendData(' '); +// uart_Float2Char((float)cache->gY); +// uart_sendData(' '); +// uart_Float2Char((float)cache->gZ); +// uart_sendData(' '); +// +// uart_Float2Char((float)cache->aX); +// uart_sendData(' '); +// uart_Float2Char((float)cache->aY); +// uart_sendData(' '); +// uart_Float2Char((float)cache->aZ); +// uart_sendData(' '); +// uart_sendData(0x0D); +// uart_sendData(0x0A); +// } float g_Yaw, g_Pitch, g_Roll; - +SixAxis avm_euler; void IMU_Comput(SixAxis cache) { static float g_q0 = 1, g_q1 = 0, g_q2 = 0, g_q3 = 0; //Quaternion @@ -106,7 +119,7 @@ void IMU_Comput(SixAxis cache) { float vx, vy, vz; float ex, ey, ez; - norm = _sqrt(cache.aX*cache.aX + cache.aY*cache.aY + cache.aZ*cache.aZ); //取模 + norm = sqrt(cache.aX*cache.aX + cache.aY*cache.aY + cache.aZ*cache.aZ); //取模 //向量化 cache.aX = cache.aX / norm; @@ -140,13 +153,13 @@ void IMU_Comput(SixAxis cache) { g_q3 += (g_q0 * cache.gZ + g_q1 * cache.gY - g_q2 * cache.gX) * halfT; //正常化四元 - norm = _sqrt(g_q0*g_q0 + g_q1*g_q1 + g_q2*g_q2 + g_q3*g_q3); + norm = sqrt(g_q0*g_q0 + g_q1*g_q1 + g_q2*g_q2 + g_q3*g_q3); g_q0 = g_q0 / norm; g_q1 = g_q1 / norm; g_q2 = g_q2 / norm; g_q3 = g_q3 / norm; - g_Pitch = _asin(-2 * g_q1 * g_q3 + 2 * g_q0 * g_q2) * 57.3; - g_Roll = _atan2(2 * g_q2 * g_q3 + 2 * g_q0 * g_q1, -2 * g_q1*g_q1 - 2 * g_q2*g_q2 + 1) * 57.3; - g_Yaw = _atan2(2 * (g_q1 * g_q2 + g_q0 * g_q3), g_q0*g_q0 + g_q1*g_q1 - g_q2*g_q2 - g_q3*g_q3) * 57.3; + g_Pitch = asin(-2 * g_q1 * g_q3 + 2 * g_q0 * g_q2) * 57.3; + g_Roll = atan2(2 * g_q2 * g_q3 + 2 * g_q0 * g_q1, -2 * g_q1*g_q1 - 2 * g_q2*g_q2 + 1) * 57.3; + g_Yaw = atan2(2 * (g_q1 * g_q2 + g_q0 * g_q3), g_q0*g_q0 + g_q1*g_q1 - g_q2*g_q2 - g_q3*g_q3) * 57.3; } diff --git a/libs/module/avm_mpu6050.h b/libs/module/avm_mpu6050.h index 8062a8b..5cba61f 100644 --- a/libs/module/avm_mpu6050.h +++ b/libs/module/avm_mpu6050.h @@ -39,17 +39,19 @@ void delay(volatile unsigned int count); //=_=_=_=_=_=_=_=_=_=_=_=_=_=_=_=_=_=_=_=_=_=_=_=_=_=_=_=_=_=_=_=_=_ #define IMU_SOFTWARE_FIXED -#define G_X_OFFSET 5.2439f -#define G_Y_OFFSET -0.7926f -#define G_Z_OFFSET -0.3048f +#define G_X_OFFSET 1.5853f +#define G_Y_OFFSET 0.7926f +#define G_Z_OFFSET 2.0121f #define A_X_OFFSET 0 -#define A_Y_OFFSET 11.3f -#define A_Z_OFFSET 0 +#define A_Y_OFFSET 0 +#define A_Z_OFFSET 0.2f #define IMU_ADDRESS 0x68 #define IMU_NOT_CONNECTED (MPU_Sigle_Read(WHO_AM_I)!=IMU_ADDRESS) +extern avm_module_t avm_mpu_module_st; + typedef struct{ float gX; float gY; @@ -63,11 +65,8 @@ typedef struct{ #define Ki 0.002f //积分增益支配率 #define halfT 0.001f //采样周期的一半 -float g_Yaw, g_Pitch, g_Roll; - -extern double _asin (double); -extern double _atan2 (double,double); -extern double _sqrt (double); +extern float g_Yaw, g_Pitch, g_Roll; +extern SixAxis avm_euler; void MPU_Sigle_Write(unsigned char reg_addr, unsigned char reg_data); @@ -75,7 +74,7 @@ unsigned char MPU_Sigle_Read(unsigned reg_addr); short MPU_GetData(unsigned char REG_Addr); void MPU_init(); void MPU6050_getStructData(pSixAxis cache); -void MPU6050_debug(pSixAxis cache); +// void MPU6050_debug(pSixAxis cache); void IMU_Comput(SixAxis); #endif diff --git a/libs/module/avm_pid.c b/libs/module/avm_pid.c index 038755e..3bd2324 100644 --- a/libs/module/avm_pid.c +++ b/libs/module/avm_pid.c @@ -1,6 +1,28 @@ -#include "stm32f10x.h" -#include "avm_pid.h" -#include "avm_motor.h" +#include + +// float InnerLast; //保存内环旧值以便后向差分 +// float OutterLast; //保存外环旧值以便后向差分 +// float *Feedback; //反馈数据, 实时的角度数据 +// float *Gyro; //角速度 +// float Error; //误差值 +// float p; //比例项(内环外环共用) +// float i; //积分项(仅用于外环) +// float d; //微分项(内环外环共用) +// short output; //PID输出, 用来修改PWM值, 2字节 +// __IO uint16_t *Channel1; //PWM输出, 通道1 +// __IO uint16_t *Channel2; //PWM输出, 通道2 + +pid_st avm_pid = { + .InnerLast = 0, + .OutterLast = 0, + .Feedback = &g_Roll, + .p = 0, + .i = 0, + .d = 0, + .Channel1 = &MOTOR2, + .Channel2 = &MOTOR4, + .Gyro = &avm_euler.gX, +}; void pid_SingleAxis(pid_pst temp, float setPoint) { temp->Error = *temp->Feedback - setPoint; diff --git a/libs/module/avm_pid.h b/libs/module/avm_pid.h index eef0328..bd5d365 100644 --- a/libs/module/avm_pid.h +++ b/libs/module/avm_pid.h @@ -1,8 +1,6 @@ #ifndef _AVM_PID_H_ #define _AVM_PID_H_ -extern float g_Yaw, g_Pitch, g_Roll; //eular - #define OUTTER_LOOP_KP 0 //0.257 * 0.83 0.255 #define OUTTER_LOOP_KI 0 #define OUTTER_LOOP_KD 0 @@ -31,6 +29,8 @@ typedef struct { __IO uint16_t *Channel2; } pid_st, *pid_pst; +extern pid_st avm_pid; + void pid_SingleAxis(pid_pst package, float setPoint); void pid(float setPoint, float d); diff --git a/libs/module/avm_uart.c b/libs/module/avm_uart.c index 750811b..e0d73f8 100644 --- a/libs/module/avm_uart.c +++ b/libs/module/avm_uart.c @@ -1,8 +1,21 @@ -#include "avm_uart.h" -#include "stm32f10x.h" +#include "avm_core.h" -int top = -1; //Stack Pointer -char gCmdCache[CMD_MAX_LENGTH]; +static unsigned char avm_uart_init(void *conf); +static unsigned int avm_uart_conf[] = {72, 115200}; + +avm_module_t avm_uart_module_st = { + 0, + &avm_uart_conf[0], + avm_uart_init, + NULL, + NULL +}; + + +static unsigned char avm_uart_init(void *conf) { + uart_init(((unsigned int *)conf)[0], ((unsigned int *)conf)[1]); + return 0; +} void uart_init(unsigned int pclk2, unsigned int bound) { @@ -39,65 +52,44 @@ void uart_init(unsigned int pclk2, unsigned int bound) { void USART1_IRQHandler(void) { if(USART1->SR & USART_SR_RXNE) { - char cmd = USART1->DR; // Read this register to clear RXNE flag + const char cmd = USART1->DR; // 读取串口接收寄存器来清除 RXNE 标志 switch (cmd) { - case 0x0D: - case 0x0A: - UART_CR(); - uart_sendStr("Handle Command:\t"); - uart_sendStr(gCmdCache); - UART_CR(); - clrCache(); + case TOKEN: + MPU6050_getStructData(&avm_euler); + IMU_Comput(avm_euler); + pid_SingleAxis(&avm_pid, 0); + + uart_Float2Char(g_Pitch); + uart_sendStr("@"); + + uart_Float2Char(g_Roll); + uart_sendStr("@"); + + uart_Float2Char(g_Yaw); + uart_sendStr("@"); + + uart_short2char(*avm_pid.Channel1); + uart_sendStr("@"); + + uart_short2char(*avm_pid.Channel2); + uart_sendStr("\n\r"); break; - case 0x08: - case 0x7F: - pop = '\0'; - uart_sendData(0x7F); - uart_sendData(0x08); - break; - case '$': - clrCache(); - default: - if(STACK_OVERFLOW) - break; - push(cmd); - uart_sendData(cmd); + case '>': // Jump to bootloader + uart_sendStr("Running bootloader..."); + jump2ISP(); + // NOTE: running bootloader + default: //其它按键 break; } } } -void uart_decode() { -// The Last Item Of CMD Cache Array -// | 7 | 6 | 5 | 4 | 3 | 2 | 1 | 0 |-bit-| -// Each bit is a switch of IR Devices - char k; - // unsigned char stagement; - while(top > -1) { // While CMD Cache Not Empty - k = pop; - if(ISLEGAL_NUM(k)) { // Handle Numbers - gCmdCache[CMD_MAX_LENGTH - 1] |= 1 << (k - '0'); // Store number argument to the top of cmd stack - } else { // Handle Key Token - if(k == TOKEN_SEND) { // Send data - - } - else if(k == TOKEN_LEARN) { // decode and store data - - } - } - - - - - } -} - void uart_sendData(unsigned char data) { USART1->DR = data; while((USART1->SR & 0x40) == 0); } -void uart_showData(short k) { +void uart_short2char(short k) { char cache[] = "00000"; int i = 4; unsigned short bit[] = {10000, 1000, 100, 10, 1}; @@ -108,6 +100,17 @@ void uart_showData(short k) { uart_sendStr(cache); } +void uart_int2char(unsigned int k) { + char cache[] = "0000000000"; // Max value is 4294967295 + unsigned char i = 9; + const unsigned int bit[] = {1000000000, 100000000, 10000000, 1000000, 100000, 10000, 1000, 100, 10, 1}; + + do { + cache[i] += (char)(k / bit[i] % 10); + } while(i--); + uart_sendStr(cache); +} + unsigned char uart_Float2Char(float value) { unsigned char IntegerPart; float DecimalPart; diff --git a/libs/module/avm_uart.h b/libs/module/avm_uart.h index 7273586..b069aa4 100644 --- a/libs/module/avm_uart.h +++ b/libs/module/avm_uart.h @@ -1,35 +1,24 @@ #ifndef _AVM_UART_H_ #define _AVM_UART_H_ + void uart_init(unsigned int pclk2, unsigned int bound); void uart_sendData(unsigned char data); -void uart_showData(short k); +void uart_short2char(short k); +void uart_int2char(unsigned int k); unsigned char uart_Float2Char(float value); -void uart_sendStr(char * cmd); +void uart_sendStr(char *cmd); void USART1_IRQHandler(void); -void uart_decode(); + +extern avm_module_t avm_uart_module_st; #define UART_CR() do {\ uart_sendData(0x0D);\ uart_sendData(0x0A);\ } while(0) + #define UART_CLEAR() uart_sendStr("\033[H\033[J") -#define CMD_MAX_LENGTH 32 -#define pop gCmdCache[top--] -#define push(s) gCmdCache[++top] = s -#define clrCache() {\ - while (top > -1) {\ - pop = '\0';\ - }\ -} -#define STACK_OVERFLOW (top == CMD_MAX_LENGTH - 1) - -// decode -#define TOKEN_SEND 'S' -#define TOKEN_LEARN 'L' -#define CMD_NUM_MAX '8' -#define CMD_NUM_MIN '1' -#define ISLEGAL_NUM(k) (((k) >= CMD_NUM_MIN) && ((k) <= CMD_NUM_MAX)) +#define TOKEN '$' #endif diff --git a/src/main.c b/src/main.c index 8be3094..ea8e191 100644 --- a/src/main.c +++ b/src/main.c @@ -1,159 +1,20 @@ -#include -#include "stm32f10x.h" -#include "avm_bit.h" -#include "avm_mpu6050.h" -#include "avm_motor.h" -#include "avm_uart.h" -#include "avm_wifi.h" -#include "avm_pid.h" -#include "avm_tty.h" - -#define ISP_ADDR 0x1FFFF000 - -void jump2ISP() { - __set_MSP(*(unsigned int *)ISP_ADDR); - ((void (*)(void))*((unsigned int *)(ISP_ADDR + 4)))(); -} - - -double _asin(double i) {return asin(i);} -double _atan2(double i,double k) {return atan2(i,k);} -double _sqrt(double i) {return sqrt(i);} - - -//ms -void delay(unsigned int t) { - SysTick->LOAD = 9000 * t; - SysTick->VAL = 0; - SysTick->CTRL = 0x01; - for(unsigned int tmp = SysTick->CTRL;(tmp&0x01)&&(!(tmp&SysTick_CTRL_COUNTFLAG));tmp = SysTick->CTRL); - SysTick->CTRL = 0; - SysTick->VAL = 0; -} - - -#define DEBUG_BLDC //Config - -#if defined (DEBUG_MPU6050_EULER) || defined (DEBUG_MPU6050_SOURCEDATA) || defined (DEBUG_BLDC) - SixAxis sourceData; -#else -#error Which Debug type are you using? Define DEBUG_XXXX in your compiler options. -#endif - -// float InnerLast; //保存内环旧值以便后向差分 -// float OutterLast; //保存外环旧值以便后向差分 -// float *Feedback; //反馈数据, 实时的角度数据 -// float *Gyro; //角速度 -// float Error; //误差值 -// float p; //比例项(内环外环共用) -// float i; //积分项(仅用于外环) -// float d; //微分项(内环外环共用) -// short output; //PID输出, 用来修改PWM值, 2字节 -// __IO uint16_t *Channel1; //PWM输出, 通道1 -// __IO uint16_t *Channel2; //PWM输出, 通道2 -#ifdef DEBUG_BLDC - pid_st g_pid_roll = { - .InnerLast = 0, - .OutterLast = 0, - .Feedback = &g_Roll, - .i = 0, - .Channel1 = &MOTOR2, - .Channel2 = &MOTOR4, - .Gyro = &sourceData.gX, - }; -#endif - -void mpu_task() { - while(1) { - MPU6050_getStructData(&sourceData); - IMU_Comput(sourceData); - // vTaskDelay(10); - } -} - -#ifdef DEBUG_BLDC - void pid_task() { - while(1) { - pid_SingleAxis(&g_pid_roll, 0); - // vTaskDelay(10); - } - } -#endif - -void uart_task() { - while(1) { - uart_sendStr("Pitch Angle: "); - uart_Float2Char(g_Pitch); - - uart_sendStr("; Roll Angle: "); - uart_Float2Char(g_Roll); - - uart_sendStr("; Yaw Angle: "); - uart_Float2Char(g_Yaw); - - UART_CR(); - - // vTaskDelay(100); - } -} - -void uart_debugPID() { - while(1) { - TTY_CLEAR(); - - TTY_RED(); - uart_sendStr(" Motor占空比: "); - TTY_NONE(); - TTY_BLUE(); - uart_showData(*g_pid_roll.Channel1); - uart_sendStr("\t"); - uart_showData(*g_pid_roll.Channel2); - TTY_NONE(); - - uart_sendStr("\n\rRoll:\t"); - uart_Float2Char(*g_pid_roll.Feedback); - - uart_sendStr("\tGyro:\t"); - uart_Float2Char(*g_pid_roll.Gyro); - - uart_sendStr("\n\rP:\t"); - uart_Float2Char(g_pid_roll.p); - - uart_sendStr("\n\rI:\t"); - uart_Float2Char(g_pid_roll.i); - - uart_sendStr("\n\rD:\t"); - uart_Float2Char(g_pid_roll.d); - - uart_sendStr("\n\r=_=_=_=_=_=_=_=_=_=_=_=_=_=_=_=_=_=_=_=_=_=_=_=_=\n\rInner Cache:\t"); - uart_Float2Char(g_pid_roll.InnerLast); - uart_sendStr("\n\rOutter Cache:\t"); - uart_Float2Char(g_pid_roll.OutterLast); - - uart_sendStr("\n\rOutput:\t\t"); - TTY_RED(); - uart_showData(g_pid_roll.output); - TTY_NONE(); - uart_sendStr("\n\r"); - // vTaskDelay(100); - } -} +#include int main() { - #ifdef DEBUG_BLDC - //Brushless motor auto init - MOTOR_SETTING(); - #endif + // #ifdef DEBUG_BLDC + // //Brushless motor auto init + // MOTOR_SETTING(); + // #endif + // + // uart_sendStr("Config MPU6050..."); + // UART_CR(); + // MPU_init(); + // uart_sendStr("MPU6050 Connect Success!"); + // UART_CR(); + avm_core_module_st.init_module(NULL); + // avm_uart_module_st.init_module(avm_uart_module_st.confs); + // uart_init(72, 115200); - uart_init(72, 115200); - uart_sendStr("Config MPU6050..."); - UART_CR(); - MPU_init(); - uart_sendStr("MPU6050 Connect Success!"); - UART_CR(); - - // xTaskCreate(uart_task, "UART_TASK", 100, NULL, 1, NULL); - // xTaskCreate(mpu_task, "MPU_TASK", 100, NULL, 3, NULL); - // xTaskCreate(pid_task, "PID_TASK", 100, NULL, 2, NULL); - while(1); + while(1) { + } }