controlA_Follow.c
4.3 KB
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
#include "controlA_Follow.h"
#include "math_utils.h"
#include "isoCalculate.h"
// float yawY_Pre = 0;
// float yawY_actPre = 0;
// LowPassFSTD_s argA_lowPass = {0};
LowPassFSTDRing_s argA_lowPassRing = {0};
float yawPre = 360.0f;
float yawPre_s1 = 0; // 计划偏航角 - 死区
float errPre_s1 = 0; // 计划偏航偏差 - 死区
float a = 1.0f / ((CONTROL_A_DEADBAND + CONTROL_A_TRANSITION_BAND) - (CONTROL_A_DEADBAND - CONTROL_A_TRANSITION_BAND));
float b = ((CONTROL_A_DEADBAND - CONTROL_A_TRANSITION_BAND) / ((CONTROL_A_DEADBAND - CONTROL_A_TRANSITION_BAND) - (CONTROL_A_DEADBAND + CONTROL_A_TRANSITION_BAND)));
int FirstDeadBand = 0;
int FirstOutBand = 0;
// static void addToWatch()
// {
// gen_OBS.vaule[3] = yawPre;
// }
float ControlA_Follow_YawErrOut(float raw_A, float imu_Ang_Yaw)
{
// 控制量A的角度
float angA = raw_A * 57.2957795130f;
// 控制量A的绝对值角度
float absOfA = fabsf(angA);
// A和当前偏航角的融合目标(跟随模式下的目标航向角),但是没有过零处理
float yawTargetRaw = imu_Ang_Yaw - angA;
// A和当前偏航角的融合目标(跟随模式下的目标航向角),带过零处理,为真实目标航向角
float yawTargetTrue = keepIn180UpDown(yawTargetRaw);
// 输出用于Yaw的误差
float yawErr;
// 当前yaw计划和yaw目标的误差
float errC_T = shortARCCalculate(yawPre, yawTargetTrue);
// 当控制量A位于死区
if (absOfA <= (CONTROL_A_DEADBAND - CONTROL_A_TRANSITION_BAND))
// if (absOfA <= (CONTROL_A_DEADBAND))
{
if (FirstDeadBand == 0)
{
// yawPre = yawTargetTrue;
FirstDeadBand = 1;
}
yawPre = LowPassFilterSTDRing_Process_EX(&argA_lowPassRing, yawTargetTrue, yawPre);
yawErr = -shortARCCalculate(imu_Ang_Yaw, yawPre);
}
// 当控制量A位于过渡段
else if ((absOfA > (CONTROL_A_DEADBAND - CONTROL_A_TRANSITION_BAND)) && (absOfA <= (CONTROL_A_DEADBAND + CONTROL_A_TRANSITION_BAND)))
{
// LowPass - 死区低通滤波
FirstDeadBand = 0;
float yawPre_s2_LP = LowPassFilterSTDRing_Process_EX(&argA_lowPassRing, yawTargetTrue, yawPre);
float preErr_s2_LP = shortARCCalculate(yawPre, yawPre_s2_LP);
// OutPlan - 死区外偏航规划
float err_s2_C_T = shortARCCalculate(yawPre, yawTargetTrue);
float yawPre_s2_OP = 0.0f;
float preErr_s2_OP = 0.0f;
if (((errC_T) > 0))
{
yawPre_s2_OP = yawPre + _constraint(CONTROL_A_STEP * (expf((fabsf(errC_T)) / 30.0f)), 0, CONTROL_A_STEP_MAX);
yawPre_s2_OP = keepIn180UpDown(yawPre_s2_OP);
preErr_s2_OP = shortARCCalculate(yawPre, yawPre_s2_OP);
}
else
{
yawPre_s2_OP = yawPre - _constraint(CONTROL_A_STEP * (expf((fabsf(errC_T)) / 30.0f)), 0, CONTROL_A_STEP_MAX);
yawPre_s2_OP = keepIn180UpDown(yawPre_s2_OP);
preErr_s2_OP = shortARCCalculate(yawPre, yawPre_s2_OP);
}
// 线性融合
float boost = _constraint((a * fabsf(err_s2_C_T) + b), 0.0f, 0.9999f);
yawPre = yawPre + ((1.0f - boost) * preErr_s2_LP + boost * preErr_s2_OP);
yawPre = keepIn180UpDown(yawPre);
yawErr = -shortARCCalculate(imu_Ang_Yaw, yawPre);
}
else
// 当控制量A位于过渡段外
{
// 持续刷新死区低通滤波的时间
TimeFlash(&(argA_lowPassRing.timeS));
// 状态标记
// FirstDeadBand = 0;
// if (FirstOutBand == 0)
// {
// FirstOutBand = 1;
// }
// 计算偏航规划的短弧
float errC_T = shortARCCalculate(yawPre, yawTargetTrue);
// 核心处理部分
if ((errC_T) > 0)
{
yawPre = yawPre + _constraint(CONTROL_A_STEP * (expf((fabsf(errC_T)) / 30.0f)), 0, CONTROL_A_STEP_MAX);
}
else
{
yawPre = yawPre - _constraint(CONTROL_A_STEP * (expf((fabsf(errC_T)) / 30.0f)), 0, CONTROL_A_STEP_MAX);
}
// 限制偏航轴的范围(过零处理)
yawPre = keepIn180UpDown(yawPre);
// 短弧计算
yawErr = -shortARCCalculate(imu_Ang_Yaw, yawPre);
}
// addToWatch();
return yawErr / 57.2957795130f;
}
void controlA_Follow_Init()
{
LowPassFilterSTDRing_Init(&argA_lowPassRing, CONTROL_A_DEADBAND_LP_TIME, 180.0f);
}