feat: (Module) Totally refactor

libs/module/avm_i2c.c

@@ -1,9 +1,21 @@
-#include "avm_i2c.h"
+#include <avm_core.h>
 
-#include "stm32f10x.h"
+static unsigned char avm_i2c_init(void *arg);
-#include "avm_bit.h"
 
-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--);
 }
 

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();

libs/module/avm_motor.c

@@ -1,5 +1,19 @@
-#include "avm_motor.h"
+#include <avm_core.h>
-#include "stm32f10x.h"
+
+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) {

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

libs/module/avm_mpu6050.c

@@ -1,9 +1,22 @@
-#include "avm_mpu6050.h"
+#include <avm_core.h>
+
+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) {
+// void MPU6050_debug(pSixAxis cache) {
-    uart_Float2Char((float)cache->gX);
+//     uart_Float2Char((float)cache->gX);
-    uart_sendData(' ');
+//     uart_sendData(' ');
-    uart_Float2Char((float)cache->gY);
+//     uart_Float2Char((float)cache->gY);
-    uart_sendData(' ');
+//     uart_sendData(' ');
-    uart_Float2Char((float)cache->gZ);
+//     uart_Float2Char((float)cache->gZ);
-    uart_sendData(' ');
+//     uart_sendData(' ');
-
+//
-    uart_Float2Char((float)cache->aX);
+//     uart_Float2Char((float)cache->aX);
-    uart_sendData(' ');
+//     uart_sendData(' ');
-    uart_Float2Char((float)cache->aY);
+//     uart_Float2Char((float)cache->aY);
-    uart_sendData(' ');
+//     uart_sendData(' ');
-    uart_Float2Char((float)cache->aZ);
+//     uart_Float2Char((float)cache->aZ);
-    uart_sendData(' ');
+//     uart_sendData(' ');
-    uart_sendData(0x0D);
+//     uart_sendData(0x0D);
-    uart_sendData(0x0A);
+//     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_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_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_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;
 }

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_X_OFFSET 1.5853f
-#define G_Y_OFFSET -0.7926f
+#define G_Y_OFFSET 0.7926f
-#define G_Z_OFFSET -0.3048f
+#define G_Z_OFFSET 2.0121f
 
 #define A_X_OFFSET 0
-#define A_Y_OFFSET 11.3f
+#define A_Y_OFFSET 0
-#define A_Z_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 float g_Yaw, g_Pitch, g_Roll;
-
+extern SixAxis avm_euler;
-extern double _asin (double);
-extern double _atan2 (double,double);
-extern double _sqrt (double);
 
 
 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

libs/module/avm_pid.c

@@ -1,6 +1,28 @@
-#include "stm32f10x.h"
+#include <avm_core.h>
-#include "avm_pid.h"
+
-#include "avm_motor.h"
+// 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;

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);
 

libs/module/avm_uart.c

@@ -1,8 +1,21 @@
-#include "avm_uart.h"
+#include "avm_core.h"
-#include "stm32f10x.h"
 
-int top = -1;	//Stack Pointer
+static unsigned char avm_uart_init(void *conf);
-char gCmdCache[CMD_MAX_LENGTH];
+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 TOKEN:
-			case 0x0A:
+				MPU6050_getStructData(&avm_euler);
-				UART_CR();
+				IMU_Comput(avm_euler);
-				uart_sendStr("Handle Command:\t");
+                pid_SingleAxis(&avm_pid, 0);
-				uart_sendStr(gCmdCache);
+
-				UART_CR();
+				uart_Float2Char(g_Pitch);
-				clrCache();
+				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 '>':	// Jump to bootloader
-			case 0x7F:
+				uart_sendStr("Running bootloader...");
-				pop = '\0';
+				jump2ISP();
-				uart_sendData(0x7F);
+				// NOTE: running bootloader
-				uart_sendData(0x08);
+			default:	//其它按键
-				break;
-			case '$':
-				clrCache();
-			default:
-				if(STACK_OVERFLOW)
-					break;
-				push(cmd);
-				uart_sendData(cmd);
 				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;

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 TOKEN           '$'
-#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))
 
 #endif

src/main.c

@@ -1,159 +1,20 @@
-#include <math.h>
+#include <avm_core.h>
-#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);
-	}
-}
 
 int main() {
-	#ifdef DEBUG_BLDC
+	// #ifdef DEBUG_BLDC
-		//Brushless motor auto init
+	// 	//Brushless motor auto init
-	    MOTOR_SETTING();
+	//     MOTOR_SETTING();
-	#endif
+	// #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);
+	while(1) {
-    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);
 }