GitHub/Avem
1
0
Fork 0
mirror of https://github.com/avem-labs/Avem.git synced 2023-09-01 15:18:49 +08:00
Code Issues Projects Releases Wiki Activity

feat: (Module) Totally refactor

Browse Source
This commit is contained in:
but0n 2017-09-04 21:53:48 +08:00
parent 771807a349
commit 4c50f41eb5
11 changed files with 198 additions and 281 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
libs/module/avm_i2c.c
View File

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

7
libs/module/avm_i2c.h
View File

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

18
libs/module/avm_motor.c
View File

@ -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) {

9
libs/module/avm_motor.h
View File

@ -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

69
libs/module/avm_mpu6050.c
View File

@ -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;
}

21
libs/module/avm_mpu6050.h
View File

@ -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

28
libs/module/avm_pid.c
View File

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

4
libs/module/avm_pid.h
View File

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

105
libs/module/avm_uart.c
View File

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

27
libs/module/avm_uart.h
View File

@ -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

171
src/main.c
View File

@ -1,159 +1,20 @@
#include <math.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);
}
}
#include <avm_core.h>
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) {
}
}