-
Notifications
You must be signed in to change notification settings - Fork 1
/
Copy pathAno_MotorCtrl.c
141 lines (115 loc) · 2.59 KB
/
Ano_MotorCtrl.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
#include "Ano_MotorCtrl.h"
#include "Ano_Math.h"
#include "ANO_RC.h"
#include "Drv_icm20602.h"
#include "Drv_spl06.h"
#include "Ano_Imu.h"
#include "Drv_pwm_out.h"
#include "Ano_MotionCal.h"
#include "Ano_Filter.h"
#include "Ano_Navigate.h"
/*
四轴：
机头
m2 m1
\ /
\ /
/ \
/ \
m3 m4
机尾
*/
s16 motor[MOTORSNUM];
s16 motor_step[MOTORSNUM];
//float motor_lpf[MOTORSNUM];
static u16 motor_prepara_cnt;
_mc_st mc;
u16 IDLING;//10*Ano_Parame.set.idle_speed_pwm //200
// 电机控制任务
void Motor_Ctrl_Task(u8 dT_ms)
{
u8 i;
// if(flag.taking_off) // type(flag) :标志结构体 taking_off:u8 taking_off;
// {
// flag.motor_preparation = 1; u8 motor_preparation;
// motor_prepara_cnt = 0;
// }
if(flag.unlock_sta)
{
IDLING = 10*LIMIT(Ano_Parame.set.idle_speed_pwm,0,30);
if(flag.motor_preparation == 0) // 电机没准备好
{
motor_prepara_cnt += dT_ms;
if(flag.motor_preparation == 0)
{
if(motor_prepara_cnt<300)
{
motor[m1] = IDLING;
}
else if(motor_prepara_cnt<600)
{
motor[m2] = IDLING;
}
else if(motor_prepara_cnt<900)
{
motor[m3] = IDLING;
}
else if(motor_prepara_cnt<1200)
{
motor[m4] = IDLING;
}
else
{
flag.motor_preparation = 1;
motor_prepara_cnt = 0;
}
}
}
}
else
{
flag.motor_preparation = 0;
}
if(flag.motor_preparation == 1) // 电机准备好了
{
motor_step[m1] = mc.ct_val_thr +mc.ct_val_yaw -mc.ct_val_rol +mc.ct_val_pit; // m1 = thr + yaw - rol + pit
motor_step[m2] = mc.ct_val_thr -mc.ct_val_yaw +mc.ct_val_rol +mc.ct_val_pit; // m2 = thr - yaw + rol + pit
motor_step[m3] = mc.ct_val_thr +mc.ct_val_yaw +mc.ct_val_rol -mc.ct_val_pit; // m3 = thr + yaw + rol - pit
motor_step[m4] = mc.ct_val_thr -mc.ct_val_yaw -mc.ct_val_rol -mc.ct_val_pit; // m4 = thr - yaw - rol - pit
for(i=0;i<MOTORSNUM;i++)
{
motor_step[i] = LIMIT(motor_step[i],IDLING,1000);
//motor_lpf[i] += 0.5f *(motor_step[i] - motor_lpf[i]) ;
}
}
for(i=0;i<MOTORSNUM;i++)
{
if(flag.unlock_sta)
{
if(flag.motor_preparation == 1)
{
motor[i] = LIMIT(motor_step[i],IDLING,999);
}
}
else
{
motor[i] = 0;
}
}
//配置输出
for(u8 i =0;i<4;i++)
{
SetPwm(motor);
}
//#define Cali_Set_ESC
#ifdef Cali_Set_ESC
//配置输出
for(u8 i =0;i<4;i++)
{
motor[i] = CH_N[CH_THR]+500;
SetPwm(motor);
}
#endif
//test
// SetPwm(motor);
}