Gimbal_DataDeal.c
12.0 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
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "JZsdkLib.h"
#include "BaseConfig.h"
#include "JZsdk_TaskManagement/TaskManagement.h"
#include "Gimbal_SpecialUart/Gimbal_SpecialUart.h"
#include "../Gimbal_Param.h"
#include "Gimbal/Gimbal.h"
#include "Hal_Send/HalSend.h"
#include "JZsdk_uart/JZsdk_Uart.h"
#include "Ircut/H3_ircut/H3_ircut.h"
#include "DeviceInfo/DebugInfo/DebugInfo.h"
#ifdef WIRINGPI_STATUS_ON
#include <wiringPi.h>
#endif
#define PITCH_OFFSET_FILE_PATH "/root/Gimbal_Pitch_BaseOffset"
static T_JZsdkReturnCode Gimbal_DataDeal_Read_PitchBaseOffset(int *offset);
static T_JZsdkReturnCode Gimbal_DataDeal_Write_PitchBaseOffset(int offset);
static int g_Gimbal_Pitch_BaseOffset = 0; //云台俯仰基准偏移量
/******************
*
* 初始化
*
* **************/
T_JZsdkReturnCode Gimbal_DataDeal_init()
{
//由于150s 150t 开发时还没有帧协议,所以是一个独特的控制类型
#if DEVICE_VERSION == JZ_H150S || DEVICE_VERSION == JZ_H150T
int Uart_fd = 0;
//1、串口初始化
JZsdk_Uart_UartEnabled(GIMBAL_UART_NUM, GIMBAL_UART_BITRATE, &Uart_fd);
//2、串口接收初始化
Gimbal_SpecialUart_UartReceive_Init(Uart_fd);
//不需特地初始化
#elif DEVICE_VERSION == JZ_H1T || DEVICE_VERSION == JZ_C1
// 不用去特地初始化t60的云台
#elif DEVICE_VERSION == JZ_H10 || DEVICE_VERSION == JZ_H10T
//H10 H10T需要去读取云台偏移量
Gimbal_DataDeal_Read_PitchBaseOffset(&g_Gimbal_Pitch_BaseOffset);
#elif DEVICE_VERSION == JZ_U3 \
|| DEVICE_VERSION == JZ_U3S || DEVICE_VERSION == JZ_U3D || DEVICE_VERSION == JZ_U30 || DEVICE_VERSION == TF_A1
//设置0度
#else
return JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;
#endif
return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}
T_JZsdkReturnCode Gimbal_DataDeal_SetRealAngle(int angle)
{
int angle_PWM = 0;
#if DEVICE_VERSION == JZ_H150S || DEVICE_VERSION == JZ_H150T
//调整为PWM值 1000~2000
//完整计算公式
//angle_PWM= PITCH_PWM_MIN +(-angle)*(PITCH_PWM_MAX-PITCH_PWM_MIN)/(abs(MAX_PITCH)+abs(MIN_PITCH));
//简化为
angle_PWM= PITCH_PWM_MIN +(angle)*(PITCH_PWM_MAX-PITCH_PWM_MIN)/MIN_PITCH;
unsigned char send_angle[]={0x5a,0x00,0x00,0xa5};
send_angle[1]=(unsigned char )(angle_PWM);
send_angle[2]=(unsigned char )(angle_PWM>>8);
Gimbal_SpecialUart_UartSend(send_angle, 4);
#elif DEVICE_VERSION == JZ_H10
//因为PWM写的时候是按90度写的，所以分母是900，不是设定的600
int base_offset = g_Gimbal_Pitch_BaseOffset;
//最大最小值的偏移量 //正整数时，则总体角度向0偏移 为负数时，向-900便宜
//计算方法 设置最小角度时 为 PwM - 195 - 200 //设置最大角度时 为 PwM - 0 - 200
int base = 200; //0度
//每43.5对饮20度
//完整计算公式
//angle_PWM = base + angle*195/((abs(MAX_PITCH) + abs(MIN_PITCH)))
//上调15度
//angle_PWM = 200 -angle*195/900 -150*195/900; //上调15度
//简化为
//angle_PWM =base + base_offset +angle*195/MIN_PITCH;
angle_PWM =base + base_offset +angle*195/(-900);
#ifdef WIRINGPI_STATUS_ON
H3_ircut_pwm(angle_PWM);
#endif
#elif DEVICE_VERSION == JZ_H10T
int base_offset = g_Gimbal_Pitch_BaseOffset;
int base = 820;
//因为PWM写的时候是按60度写的
angle_PWM = base + base_offset + ( angle * (120) /(MAX_PITCH - MIN_PITCH) );
Ircut_PWM_control(angle_PWM);
#elif DEVICE_VERSION == JZ_U3 || DEVICE_VERSION == JZ_U3S || DEVICE_VERSION == JZ_U3D || DEVICE_VERSION == JZ_U30 \
|| DEVICE_VERSION == TF_A1
//发送角度给串口2
HalSend_type1Send_Set_GimbalPitchAngle(UART_DEV_2, angle, 0, JZ_FLAGCODE_ON);
#elif DEVICE_VERSION == JZ_H1T
//发送角度给串口1
HalSend_type1Send_Set_GimbalPitchAngle(UART_DEV_1, angle, 0, JZ_FLAGCODE_ON);
#elif DEVICE_VERSION == JZ_T40 || DEVICE_VERSION == JZ_T40S
//T40很特殊，需要控制两个子设备，所以要准备帧序列
HalSend_type1Send_Set_GimbalPitchAngle(UART_DEV_2, angle, 0x10, JZ_FLAGCODE_OFF);
//如果处于调试模式
if(DebugInfo_ModeGet() == JZ_FLAGCODE_ON)
{
//获取云台的debug角度
T_GimbalInfo gimbalinfo;
Gimbal_Get_GimbalInfo(&gimbalinfo);
if (gimbalinfo.DebugPitch_Left != 0)
{
HalSend_type1Send_Set_GimbalPitchAngle(UART_DEV_2, (angle+gimbalinfo.DebugPitch_Left), 0x11, JZ_FLAGCODE_OFF);
}
if (gimbalinfo.DebugPitch_Right != 0)
{
HalSend_type1Send_Set_GimbalPitchAngle(UART_DEV_2, (angle+gimbalinfo.DebugPitch_Right), 0x12, JZ_FLAGCODE_OFF);
}
}
#else
return JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;
#endif
return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}
/******************************
*
*
* 设置云台最大最小值
*
*
* *******************************/
T_JZsdkReturnCode Gimbal_DataDeal_SetPitchRange(int Range)
{
if (Range != 0xFF && Range != 0x00)
{
return JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;
}
#if DEVICE_VERSION == JZ_H150S || DEVICE_VERSION == JZ_H150T
uint8_t set_min_motor[]={0x4A,0x00,0x00,0xA4};
uint8_t set_max_motor[]={0x6A,0x00,0x00,0xA6};
if (Range == 0xFF)
{
Gimbal_SpecialUart_UartSend(set_max_motor, 4);
}
else if (Range == 0x00)
{
Gimbal_SpecialUart_UartSend(set_min_motor, 4);
}
#elif DEVICE_VERSION == JZ_H10
//因为PWM写的时候是按90度写的，所以分母是900，不是设定的600
int base_offset = g_Gimbal_Pitch_BaseOffset;
//在最大的角度弄的
if (Range == 0xFF)
{
g_Gimbal_Pitch_BaseOffset++;
}
//在最小的角度弄的
else if (Range == 0x00)
{
g_Gimbal_Pitch_BaseOffset--;
}
// //在最大的角度弄的
// if (Range == 0xFF)
// {
// //先通过角度 得出目前的PWM值
// int angle = Gimbal_Get_PitchRealPitchAngle();
// //int angle_PWM =200 + base_offset +angle*195/MIN_PITCH;
// int angle_PWM =200 + base_offset +angle*195/(-900);
// //通过最大值的计算方式 得出offset值
// //g_Gimbal_Pitch_BaseOffset = angle_PWM - ((MAX_PITCH*195)/(MAX_PITCH - MIN_PITCH)) - base_offset;
// g_Gimbal_Pitch_BaseOffset = angle_PWM - ((MAX_PITCH*195)/(MAX_PITCH - (-900))) - base_offset;
// if (g_Gimbal_Pitch_BaseOffset <= - 100)
// {
// g_Gimbal_Pitch_BaseOffset = - 100;
// }
// else if (g_Gimbal_Pitch_BaseOffset >= 100)
// {
// g_Gimbal_Pitch_BaseOffset = 100;
// }
// }
// //在最小的角度弄的
// else if (Range == 0x00)
// {
// //先通过角度 得出目前的PWM值
// int angle = Gimbal_Get_PitchRealPitchAngle();
// int angle_PWM =200 + base_offset +angle*195/(-900);
// //通过最小值的计算方式 得出offset值
// g_Gimbal_Pitch_BaseOffset = angle_PWM - (((MAX_PITCH - (-900))*195)/(MAX_PITCH - (-900)) ) - base_offset;
// if (g_Gimbal_Pitch_BaseOffset <= - 100) //最小160
// {
// g_Gimbal_Pitch_BaseOffset = - 100;
// }
// else if (g_Gimbal_Pitch_BaseOffset >= 100) //最大420
// {
// g_Gimbal_Pitch_BaseOffset = 100;
// }
// }
JZSDK_LOG_INFO("设置pitch的offset值:%d",g_Gimbal_Pitch_BaseOffset);
//写入偏移量
Gimbal_DataDeal_Write_PitchBaseOffset(g_Gimbal_Pitch_BaseOffset);
//刷新角度
Gimbal_Flush_Pitch();
#elif DEVICE_VERSION == JZ_H10T
//因为PWM写的时候是按90度写的，所以分母是900，不是设定的600
int base_offset = g_Gimbal_Pitch_BaseOffset;
//在最大的角度弄的
if (Range == 0xFF)
{
g_Gimbal_Pitch_BaseOffset++;
}
//在最小的角度弄的
else if (Range == 0x00)
{
g_Gimbal_Pitch_BaseOffset--;
}
// //先通过角度 得出目前的PWM值
// int angle = Gimbal_Get_PitchRealPitchAngle();
// //因为PWM写的时候是按90度写的，所以分母是900，不是设定的600
// int base_offset = g_Gimbal_Pitch_BaseOffset;
// //先通过角度 得出目前的PWM值
// int angle = Gimbal_Get_PitchRealPitchAngle();
// //在最大的角度弄的
// if (Range == 0xFF)
// {
// //先得出目前pwm值
// int angle_PWM = 820 + base_offset + (angle * (120)/(MAX_PITCH - MIN_PITCH)); //820是0度
// //通过最大值的计算方式 得出新的offset值
// //g_Gimbal_Pitch_BaseOffset = angle_PWM - ((MAX_PITCH*195)/(MAX_PITCH - MIN_PITCH)) - base_offset;
// g_Gimbal_Pitch_BaseOffset = angle_PWM - 820 - ((MAX_PITCH*120)/(MAX_PITCH - MIN_PITCH)) - base_offset;
// //下限600，上限830
// if (g_Gimbal_Pitch_BaseOffset <= - 100)
// {
// g_Gimbal_Pitch_BaseOffset = - 100;
// }
// else if (g_Gimbal_Pitch_BaseOffset >= 20)
// {
// g_Gimbal_Pitch_BaseOffset = 20;
// }
// }
// //在最小的角度弄的
// else if (Range == 0x00)
// {
// //先通过角度 得出目前的PWM值
// int angle = Gimbal_Get_PitchRealPitchAngle();
// int angle_PWM = 820 + base_offset + (angle * (120)/(900)); //820是0度
// //通过最小值的计算方式 得出offset值
// g_Gimbal_Pitch_BaseOffset = angle_PWM - 820 - ((MAX_PITCH*120)/(MAX_PITCH - MIN_PITCH)) - base_offset;
// //下限600，上限830
// if (g_Gimbal_Pitch_BaseOffset <= - 100)
// {
// g_Gimbal_Pitch_BaseOffset = - 100;
// }
// else if (g_Gimbal_Pitch_BaseOffset >= 20)
// {
// g_Gimbal_Pitch_BaseOffset = 20;
// }
// }
JZSDK_LOG_INFO("设置pitch的offset值:%d",g_Gimbal_Pitch_BaseOffset);
//写入偏移量
Gimbal_DataDeal_Write_PitchBaseOffset(g_Gimbal_Pitch_BaseOffset);
//刷新角度
Gimbal_Flush_Pitch();
#elif DEVICE_VERSION == JZ_U3 || DEVICE_VERSION == JZ_U3S || DEVICE_VERSION == JZ_U3D || DEVICE_VERSION == JZ_U30 \
|| DEVICE_VERSION == TF_A1
if (Range == 0xFF)
{
HalSend_type1Send_SetGimbalRange(UART_DEV_2, 0, Range);
}
else if (Range == 0x00)
{
HalSend_type1Send_SetGimbalRange(UART_DEV_2, 0, Range);
}
#elif DEVICE_VERSION == JZ_H1T
//好像没必要弄
#else
return JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;
#endif
return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}
static T_JZsdkReturnCode Gimbal_DataDeal_Read_PitchBaseOffset(int *offset)
{
FILE *fp = NULL;
fp = fopen(PITCH_OFFSET_FILE_PATH, "r+b");
if (fp == NULL)
{
fp = fopen(PITCH_OFFSET_FILE_PATH, "w+b");
if (fp == NULL)
{
*offset = 0;
return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}
}
else
{
int ret = fseek(fp, 0, SEEK_SET);
if (ret != 0)
{
JZSDK_LOG_ERROR("Seek count file error, ret: %d.", ret);
}
ret = fread((int *) offset, 1, sizeof(int), fp);
if (ret != sizeof(int))
{
JZSDK_LOG_ERROR("Read Pitch BaseOffset error.");
*offset = 0;
return JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;
}
else
{
JZSDK_LOG_INFO("Pitch BaseOffset=%d",*offset);
return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}
}
if (fp != NULL)
{
fclose(fp);
}
return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}
static T_JZsdkReturnCode Gimbal_DataDeal_Write_PitchBaseOffset(int offset)
{
FILE *fp = NULL;
fp = fopen(PITCH_OFFSET_FILE_PATH, "w+b");
if (fp == NULL)
{
return JZ_ERROR_SYSTEM_MODULE_CODE_FAILURE;
}
int ret = fwrite((int *) &offset, 1, sizeof(unsigned int),fp);
if (ret != sizeof(unsigned int))
{
JZSDK_LOG_ERROR("Pitch BaseOffset error.");
}
else
{
JZSDK_LOG_DEBUG("Pitch BaseOffset=%d\n",offset);
}
fflush(fp);
fclose(fp);
return JZ_ERROR_SYSTEM_MODULE_CODE_SUCCESS;
}