feat: (Module) Totally refactor

avem-labs · Sep 4, 2017 · 4c50f41 · 4c50f41
1 parent 771807a
commit 4c50f41
Show file tree

Hide file tree

Showing 11 changed files with 194 additions and 277 deletions.
diff --git a/libs/module/avm_i2c.c b/libs/module/avm_i2c.c
@@ -1,9 +1,21 @@
-#include "avm_i2c.h"
+#include <avm_core.h>
 
-#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
@@ -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
@@ -1,5 +1,19 @@
-#include "avm_motor.h"
-#include "stm32f10x.h"
+#include <avm_core.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) {

diff --git 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
@@ -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) {
- 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
@@ -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,19 +65,16 @@ 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);
 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
@@ -1,6 +1,28 @@
-#include "stm32f10x.h"
-#include "avm_pid.h"
-#include "avm_motor.h"
+#include <avm_core.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;

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