1 | #include "Os.h"
|
---|
2 | #include "t_syslog.h"
|
---|
3 | #include "t_stdlib.h"
|
---|
4 | #include "sysmod/serial.h"
|
---|
5 | #include "sysmod/syslog.h"
|
---|
6 | #include "rc_car.h"
|
---|
7 |
|
---|
8 | #include "sysmod/banner.h"
|
---|
9 | #include "target_sysmod.h"
|
---|
10 | #include "target_serial.h"
|
---|
11 | #include "prc_sil.h"
|
---|
12 |
|
---|
13 | #include "de0_nano.h"
|
---|
14 | #include "comuart.h"
|
---|
15 | #include "nces_can.h"
|
---|
16 | #include "rcb3.h"
|
---|
17 |
|
---|
18 | /*
|
---|
19 |
|
---|
20 | ○仕様
|
---|
21 |
|
---|
22 | ・コマンド
|
---|
23 | SBXBTの PS3コントローラ/USBゲームパッドUART変換ファームウェア
|
---|
24 | (http://runningele.web.fc2.com/) でサポートしているRCB3に対応
|
---|
25 | している.
|
---|
26 |
|
---|
27 | ・起動時
|
---|
28 | 全てのLEDを0.5秒周期で3秒点滅させる.
|
---|
29 |
|
---|
30 | ・ボタン操舵
|
---|
31 | 左・右 ボタン
|
---|
32 | ・ステアリングのニュートラルを調整.
|
---|
33 | ・ボタンを押すと方向指示器がON,離すとOFF
|
---|
34 | 上ボタン
|
---|
35 | ・ドライブにする.
|
---|
36 | 下ボタン
|
---|
37 | ・リバースににする.
|
---|
38 | L1/R1
|
---|
39 | ・方向指示器をON/OFFする.
|
---|
40 | L2/R2
|
---|
41 | ・モータ制御のゲインを変更
|
---|
42 | ×
|
---|
43 | ・ブレーキ
|
---|
44 | △
|
---|
45 | ・ハザード
|
---|
46 | ○
|
---|
47 | ・メインヘッドライトON/OFF
|
---|
48 | □
|
---|
49 | ・ニュートラルにする.
|
---|
50 | */
|
---|
51 |
|
---|
52 | /*
|
---|
53 | ボディ系コマンドCANメッセージ仕様
|
---|
54 |
|
---|
55 | 送信周期 : 50ms
|
---|
56 | メッセージID : 16
|
---|
57 | メッセージ長 : 11bit
|
---|
58 |
|
---|
59 | offset bits bit assign
|
---|
60 | ヘッドライト1 : 0 : 1 : 1:ON 0:OFF
|
---|
61 | ヘッドライト2 : 1 : 1 : 1:ON 0:OFF
|
---|
62 | ヘッドライト3 : 2 : 1 : 1:ON 0:OFF
|
---|
63 | ブレーキランプ : 3 : 1 : 1:ON 0:OFF
|
---|
64 | バックランプ : 4 : 1 : 1:ON 0:OFF
|
---|
65 | ブザー : 5 : 1 : 1:ON 0:OFF 2:BLINK
|
---|
66 | ウィンカーL : 8 : 2 : 1:ON 0:OFF 2:BLINK
|
---|
67 | ウィンカーR : 10 : 2 : 1:ON 0:OFF 2:BLINK
|
---|
68 | ハザードランプ : 12 : 1 : 1:ON 0:OFF 2:BLINK
|
---|
69 |
|
---|
70 |
|
---|
71 | 制御系コマンドCANメッセージ仕様
|
---|
72 |
|
---|
73 | 送信周期 : メッセージ到達毎
|
---|
74 | メッセージID : 3
|
---|
75 | メッセージ長 : 64bit
|
---|
76 |
|
---|
77 | */
|
---|
78 |
|
---|
79 | /*
|
---|
80 | * 各種パラメータ定義
|
---|
81 | */
|
---|
82 |
|
---|
83 | /*
|
---|
84 | * CAN関連 : ボディ系コマンド用CANメッセージID
|
---|
85 | */
|
---|
86 | #define BODY_CAN_MSG_ID 16
|
---|
87 |
|
---|
88 | /*
|
---|
89 | * CAN関連 : 制御系コマンド用CANメッセージID
|
---|
90 | */
|
---|
91 | #define CONT_CMD_CAN_MSG_ID 3
|
---|
92 |
|
---|
93 | /*
|
---|
94 | * CAN関連 : ボディ系コマンド用CAN送受信メールボックスID
|
---|
95 | */
|
---|
96 | #define BODY_CAN_MSG_RMB 1
|
---|
97 | #define BODY_CAN_MSG_TMB 1
|
---|
98 |
|
---|
99 | /*
|
---|
100 | * CAN関連 : 制御系コマンド用CANメッセージ用CAN送受信メールボックスID
|
---|
101 | */
|
---|
102 | #define CONT_CMD_CAN_MSG_RMB 0
|
---|
103 | #define CONT_CMD_CAN_MSG_TMB 0
|
---|
104 |
|
---|
105 |
|
---|
106 | /*
|
---|
107 | * 制御系 : ニュートラル時のステアリング角度の初期値
|
---|
108 | */
|
---|
109 | #define STEER_NEUTRAL_INIT 0
|
---|
110 |
|
---|
111 | /*
|
---|
112 | * 制御系 : 起動時のボディー系の接続テスト用設定
|
---|
113 | */
|
---|
114 | #define INIT_BODY_BLINK_COUNT 3
|
---|
115 | #define INIT_BODY_BLINK_CYCLE_MS 500
|
---|
116 |
|
---|
117 | /*
|
---|
118 | * 制御系 : 起動周期
|
---|
119 | */
|
---|
120 | #define RC_CONTRL_TASK_CYCLE_MS 10
|
---|
121 |
|
---|
122 | /*
|
---|
123 | * 制御系 : CAN送信周期
|
---|
124 | */
|
---|
125 | #define BODY_CAN_SEND_CYCLE_MS 500
|
---|
126 |
|
---|
127 |
|
---|
128 | #define SPEED_GAIN_INIT 10
|
---|
129 | #define SPEED_GAIN_MIN 1
|
---|
130 | #define SPEED_GAIN_MAX 20
|
---|
131 |
|
---|
132 | /*
|
---|
133 | * 未整理
|
---|
134 | */
|
---|
135 |
|
---|
136 | /*
|
---|
137 | * 内部データバッファ
|
---|
138 | */
|
---|
139 | static volatile uint8 command_tbl[8]; /* コマンド引渡しテーブル */
|
---|
140 |
|
---|
141 | /*
|
---|
142 | * ファイル名,行番号の参照用の変数
|
---|
143 | */
|
---|
144 | extern const char8 *fatal_file_name; /* ファイル名 */
|
---|
145 | extern sint32 fatal_line_num; /* 行番号 */
|
---|
146 |
|
---|
147 | /*
|
---|
148 | * APIエラーログマクロ
|
---|
149 | *
|
---|
150 | * ErrorHookが有効の場合はErrorHookから
|
---|
151 | * エラーログを出力し, ErrorHookが無効の場合は
|
---|
152 | * 以下のマクロよりエラーログ出力を行う
|
---|
153 | */
|
---|
154 | #if defined(CFG_USE_ERRORHOOK)
|
---|
155 | #define error_log(api) (api)
|
---|
156 | #else /* !defined( CFG_USE_ERRORHOOK ) */
|
---|
157 | #define error_log(api) \
|
---|
158 | { \
|
---|
159 | StatusType ercd; \
|
---|
160 | ercd = api; /* 各API実行 */ \
|
---|
161 | if (ercd != E_OK) { \
|
---|
162 | syslog(LOG_INFO, "Error:%d", atk2_strerror(ercd)); \
|
---|
163 | } \
|
---|
164 | }
|
---|
165 | #endif /* defined( CFG_USE_ERRORHOOK ) */
|
---|
166 |
|
---|
167 | void
|
---|
168 | set_led(uint8 pattern){
|
---|
169 | sil_wrw_iop((void *)LED_PIO_BASE, pattern);
|
---|
170 | }
|
---|
171 |
|
---|
172 | uint8
|
---|
173 | get_dipsw(void){
|
---|
174 | return (uint8)sil_rew_iop((void *)DIPSW_PIO_BASE);
|
---|
175 | }
|
---|
176 |
|
---|
177 | volatile boolean IsBodyOnECU = FALSE;
|
---|
178 | volatile boolean IsOpeOnECU = FALSE;
|
---|
179 | volatile boolean IsCntOnECU = FALSE;
|
---|
180 |
|
---|
181 | void
|
---|
182 | appmode_error(void){
|
---|
183 | volatile int i;
|
---|
184 | uint8 pattern = 0;
|
---|
185 | while(1){
|
---|
186 | for(i = 0; i < 1000000; i++);
|
---|
187 | set_led(pattern);
|
---|
188 | pattern = ~pattern;
|
---|
189 | }
|
---|
190 | }
|
---|
191 |
|
---|
192 | /*
|
---|
193 | * ユーザメイン関数
|
---|
194 | *
|
---|
195 | * アプリケーションモードの判断と,カーネル起動
|
---|
196 | */
|
---|
197 | sint32
|
---|
198 | main(void)
|
---|
199 | {
|
---|
200 | AppModeType crt_app_mode;
|
---|
201 | uint8 dipsw;
|
---|
202 |
|
---|
203 | crt_app_mode = AppMode_ALL;
|
---|
204 |
|
---|
205 | dipsw = get_dipsw();
|
---|
206 |
|
---|
207 | switch (dipsw & 0x7) {
|
---|
208 | case 0x00:
|
---|
209 | appmode_error();
|
---|
210 | break;
|
---|
211 | case 0x01:
|
---|
212 | crt_app_mode = AppMode_Body;
|
---|
213 | IsBodyOnECU = TRUE;
|
---|
214 | break;
|
---|
215 | case 0x02:
|
---|
216 | crt_app_mode = AppMode_Cnt;
|
---|
217 | IsCntOnECU = TRUE;
|
---|
218 | break;
|
---|
219 | case 0x04:
|
---|
220 | crt_app_mode = AppMode_Ope;
|
---|
221 | IsOpeOnECU = TRUE;
|
---|
222 | break;
|
---|
223 | case 0x01|0x02:
|
---|
224 | crt_app_mode = AppMode_Body_Cnt;
|
---|
225 | IsBodyOnECU = TRUE;
|
---|
226 | IsCntOnECU = TRUE;
|
---|
227 | break;
|
---|
228 | case 0x01|0x04:
|
---|
229 | crt_app_mode = AppMode_Body_Ope;
|
---|
230 | IsBodyOnECU = TRUE;
|
---|
231 | IsOpeOnECU = TRUE;
|
---|
232 | break;
|
---|
233 | case 0x02|0x04:
|
---|
234 | crt_app_mode = AppMode_Cnt_Ope;
|
---|
235 | IsCntOnECU = TRUE;
|
---|
236 | IsOpeOnECU = TRUE;
|
---|
237 | break;
|
---|
238 | case 0x07:
|
---|
239 | crt_app_mode = AppMode_ALL;
|
---|
240 | IsBodyOnECU = TRUE;
|
---|
241 | IsCntOnECU = TRUE;
|
---|
242 | IsOpeOnECU = TRUE;
|
---|
243 | break;
|
---|
244 | default:
|
---|
245 | appmode_error();
|
---|
246 | break;
|
---|
247 | }
|
---|
248 |
|
---|
249 | /*
|
---|
250 | * カーネル起動
|
---|
251 | */
|
---|
252 | StartOS(crt_app_mode);
|
---|
253 |
|
---|
254 | while (1) {
|
---|
255 | }
|
---|
256 | }
|
---|
257 |
|
---|
258 | //////////////////////////////////////////////////////////////////////////
|
---|
259 | /*
|
---|
260 | *
|
---|
261 | * ボディー制御関連
|
---|
262 | *
|
---|
263 | */
|
---|
264 |
|
---|
265 | /*
|
---|
266 | * ボディーECUへの状態指示構造体
|
---|
267 | */
|
---|
268 | typedef struct {
|
---|
269 | uint8 headlight1;
|
---|
270 | uint8 headlight2;
|
---|
271 | uint8 headlight3;
|
---|
272 | uint8 breaklamp;
|
---|
273 | uint8 backlamp;
|
---|
274 | uint8 buzzer;
|
---|
275 | uint8 blinker_l;
|
---|
276 | uint8 blinker_r;
|
---|
277 | uint8 hazardlamp;
|
---|
278 | }BODY_CONT_INFO;
|
---|
279 |
|
---|
280 | /*
|
---|
281 | * ボディーのアイテムのON/OFFマクロ
|
---|
282 | */
|
---|
283 | #define BODY_CONT_ON 1
|
---|
284 | #define BODY_CONT_OFF 0
|
---|
285 | #define BODY_CONT_BLINK 2
|
---|
286 |
|
---|
287 | /*
|
---|
288 | * ボディーECUへの状態指示用変数
|
---|
289 | */
|
---|
290 | volatile BODY_CONT_INFO g_body_cont_info;
|
---|
291 |
|
---|
292 | void
|
---|
293 | set_tlu01(unsigned int pattern){
|
---|
294 | sil_wrw_iop((void *)TLU01_PIO_BASE, pattern);
|
---|
295 | }
|
---|
296 |
|
---|
297 | unsigned int
|
---|
298 | get_tlu01(void){
|
---|
299 | return (sil_rew_iop((void *)TLU01_PIO_BASE));
|
---|
300 | }
|
---|
301 |
|
---|
302 | /*
|
---|
303 | * tlu01_set() で指示する値
|
---|
304 | */
|
---|
305 | #define BLINKER_L_NO 0
|
---|
306 | #define BLINKER_R_NO 1
|
---|
307 | #define BREAK_LAMP_NO 2
|
---|
308 | #define HEAD_LAMP1_NO 3
|
---|
309 | #define HEAD_LAMP2_NO 4 /* 未実装 */
|
---|
310 | #define HEAD_LAMP3_NO 5 /* 未実装 */
|
---|
311 | #define BACK_LAMP_NO 6
|
---|
312 | #define BUZZER_NO 7
|
---|
313 |
|
---|
314 | #define TLU01_PORT_MIN 0
|
---|
315 | #define TLU01_PORT_MAX 7
|
---|
316 |
|
---|
317 | static const uint8 tlu01_pinfo[8] ={
|
---|
318 | 0x01,
|
---|
319 | 0x40,
|
---|
320 | 0x08|0x20, //BREAK
|
---|
321 | 0x04|0x10,
|
---|
322 | 0x10,
|
---|
323 | 0x00,
|
---|
324 | 0x80,
|
---|
325 | 0x02, //BUZZER
|
---|
326 | };
|
---|
327 |
|
---|
328 | void
|
---|
329 | tlu01_set(uint8 no, boolean on)
|
---|
330 | {
|
---|
331 | uint32 wk;
|
---|
332 | uint16 pattern;
|
---|
333 |
|
---|
334 | if (no > TLU01_PORT_MAX) {
|
---|
335 | return;
|
---|
336 | }
|
---|
337 |
|
---|
338 | pattern = tlu01_pinfo[no - 1];
|
---|
339 |
|
---|
340 | wk = get_tlu01();
|
---|
341 | if (on) {
|
---|
342 | wk |= pattern;
|
---|
343 | }
|
---|
344 | else {
|
---|
345 | wk &= ~pattern;
|
---|
346 | }
|
---|
347 | set_tlu01(wk);
|
---|
348 | }
|
---|
349 |
|
---|
350 |
|
---|
351 | /*
|
---|
352 | * TLU01との接続の初期化
|
---|
353 | */
|
---|
354 | void
|
---|
355 | tlu01_init(void)
|
---|
356 | {
|
---|
357 | /* 全消灯 */
|
---|
358 | sil_wrw_iop((void *)TLU01_PIO_BASE, 0x00);
|
---|
359 | }
|
---|
360 |
|
---|
361 | #define BLINK_CYCLE_MS 500
|
---|
362 |
|
---|
363 | volatile int buzzer_state = 0;
|
---|
364 |
|
---|
365 | void
|
---|
366 | UnpackBodyContCanMsg(uint8 *p_rx_data)
|
---|
367 | {
|
---|
368 | g_body_cont_info.headlight1 = (p_rx_data[0] & 0x80)? BODY_CONT_ON : BODY_CONT_OFF;
|
---|
369 | g_body_cont_info.headlight2 = (p_rx_data[0] & 0x40)? BODY_CONT_ON : BODY_CONT_OFF;
|
---|
370 | g_body_cont_info.headlight3 = (p_rx_data[0] & 0x20)? BODY_CONT_ON : BODY_CONT_OFF;
|
---|
371 | g_body_cont_info.breaklamp = (p_rx_data[0] & 0x10)? BODY_CONT_ON : BODY_CONT_OFF;
|
---|
372 | g_body_cont_info.backlamp = (p_rx_data[0] & 0x08)? BODY_CONT_ON : BODY_CONT_OFF;
|
---|
373 |
|
---|
374 | g_body_cont_info.buzzer = (p_rx_data[0] & 0x06) >> 1;
|
---|
375 | g_body_cont_info.blinker_l = (p_rx_data[1] & 0xc0) >> 6;
|
---|
376 | g_body_cont_info.blinker_r = (p_rx_data[1] & 0x30) >> 4;
|
---|
377 | g_body_cont_info.hazardlamp = (p_rx_data[1] & 0x08)? BODY_CONT_ON : BODY_CONT_OFF;
|
---|
378 | }
|
---|
379 |
|
---|
380 | TASK(RCBodyTask){
|
---|
381 | int cnt = 0;
|
---|
382 | uint32 pattern;
|
---|
383 | boolean blink_on = FALSE;
|
---|
384 | uint8 rx_data[8];
|
---|
385 | uint8 rx_size;
|
---|
386 |
|
---|
387 | syslog(LOG_NOTICE, "RCBodyTask : Start!");
|
---|
388 |
|
---|
389 | tlu01_init();
|
---|
390 |
|
---|
391 | while(1){
|
---|
392 | WaitEvent(RCBodyEvt); /* 10msの作業時間待ち */
|
---|
393 | ClearEvent(RCBodyEvt);
|
---|
394 |
|
---|
395 | /*
|
---|
396 | * 制御系が別ECUならCANからメッセージを取得する.
|
---|
397 | */
|
---|
398 | if (!IsCntOnECU) {
|
---|
399 | NcanGetRxData(0, BODY_CAN_MSG_RMB, &rx_data[0], &rx_size );
|
---|
400 | if (rx_size != 0) {
|
---|
401 | /* 受信メッセージの解析 */
|
---|
402 | UnpackBodyContCanMsg(rx_data);
|
---|
403 | }
|
---|
404 | }
|
---|
405 |
|
---|
406 | pattern = 0;
|
---|
407 |
|
---|
408 | /* ON/OFF処理 */
|
---|
409 | pattern |= (g_body_cont_info.headlight1 == BODY_CONT_ON)? tlu01_pinfo[HEAD_LAMP1_NO] : 0x00;
|
---|
410 | pattern |= (g_body_cont_info.breaklamp == BODY_CONT_ON)? tlu01_pinfo[BREAK_LAMP_NO] : 0x00;
|
---|
411 | pattern |= (g_body_cont_info.backlamp == BODY_CONT_ON)? tlu01_pinfo[BACK_LAMP_NO] : 0x00;
|
---|
412 | if(g_body_cont_info.hazardlamp == BODY_CONT_OFF){
|
---|
413 | pattern |= (g_body_cont_info.blinker_l == BODY_CONT_ON)? tlu01_pinfo[BLINKER_L_NO] : 0x00;
|
---|
414 | pattern |= (g_body_cont_info.blinker_r == BODY_CONT_ON)? tlu01_pinfo[BLINKER_R_NO] : 0x00;
|
---|
415 | }
|
---|
416 |
|
---|
417 | if(blink_on == TRUE){
|
---|
418 | /* 点滅処理 */
|
---|
419 | if (g_body_cont_info.hazardlamp == BODY_CONT_ON){
|
---|
420 | pattern |= tlu01_pinfo[BLINKER_L_NO]|tlu01_pinfo[BLINKER_R_NO];
|
---|
421 | }else{
|
---|
422 | if (g_body_cont_info.blinker_l == BODY_CONT_BLINK){
|
---|
423 | pattern |= tlu01_pinfo[BLINKER_L_NO];
|
---|
424 | }
|
---|
425 | if (g_body_cont_info.blinker_r == BODY_CONT_BLINK){
|
---|
426 | pattern |= tlu01_pinfo[BLINKER_R_NO];
|
---|
427 | }
|
---|
428 | }
|
---|
429 | }
|
---|
430 |
|
---|
431 | if(cnt++ == BLINK_CYCLE_MS/10){
|
---|
432 | cnt = 0;
|
---|
433 | blink_on = (blink_on)? FALSE : TRUE;
|
---|
434 | }
|
---|
435 |
|
---|
436 | if(buzzer_state == 1){
|
---|
437 | set_tlu01(pattern|tlu01_pinfo[BUZZER_NO]);
|
---|
438 | }
|
---|
439 | else{
|
---|
440 | set_tlu01(pattern);
|
---|
441 | }
|
---|
442 | }
|
---|
443 | }
|
---|
444 |
|
---|
445 |
|
---|
446 | ALARMCALLBACK(BuzzerCycAlmCb)
|
---|
447 | {
|
---|
448 | static uint8 buzzer_on = 0;
|
---|
449 | static uint8 pre_buzzer = BODY_CONT_OFF;
|
---|
450 | static uint32 cnt = 0;
|
---|
451 |
|
---|
452 | if (pre_buzzer != g_body_cont_info.buzzer){
|
---|
453 | pre_buzzer = g_body_cont_info.buzzer;
|
---|
454 | cnt = 0;
|
---|
455 | }
|
---|
456 |
|
---|
457 | if(g_body_cont_info.buzzer == BODY_CONT_ON){
|
---|
458 | cnt++;
|
---|
459 | if (cnt < 500) {
|
---|
460 | if(buzzer_on == 0){
|
---|
461 | buzzer_on = 1;
|
---|
462 | buzzer_state = 1;
|
---|
463 | set_tlu01(get_tlu01() | tlu01_pinfo[BUZZER_NO]);
|
---|
464 | }else{
|
---|
465 | buzzer_on = 0;
|
---|
466 | buzzer_state = 0;
|
---|
467 | set_tlu01(get_tlu01() & ~tlu01_pinfo[BUZZER_NO]);
|
---|
468 | }
|
---|
469 | }
|
---|
470 | if(cnt > 1000){
|
---|
471 | cnt = 0;
|
---|
472 | }
|
---|
473 | }
|
---|
474 |
|
---|
475 | }
|
---|
476 |
|
---|
477 |
|
---|
478 | ///////////////////////////////////////////////////////////////////////////////////////////////////////
|
---|
479 | /*
|
---|
480 | * 操作系関連
|
---|
481 | */
|
---|
482 |
|
---|
483 | /*
|
---|
484 | * 制御系へのコマンド更新通知用変数(同一ECU時)
|
---|
485 | */
|
---|
486 | volatile boolean UpDateCntrolCmd = FALSE;
|
---|
487 |
|
---|
488 | /*
|
---|
489 | * 操作タスク
|
---|
490 | */
|
---|
491 | TASK(RCOperatorTask)
|
---|
492 | {
|
---|
493 | unsigned char c;
|
---|
494 | syslog(LOG_INFO, "RCOperatorTask : Start!");
|
---|
495 |
|
---|
496 | comuart_init();
|
---|
497 |
|
---|
498 | while(1){
|
---|
499 | WaitCOMUartEvent();
|
---|
500 | while(comuart_receive(&c, 1)) {
|
---|
501 | if (rcb3_AddReceivedByte(c)) {
|
---|
502 | UpDateCntrolCmd = TRUE;
|
---|
503 | // syslog(LOG_NOTICE, "%d,%d,%d,%d,%d,%d", g_rcb3_rmsg[1], g_rcb3_rmsg[2], g_rcb3_rmsg[3], g_rcb3_rmsg[4], g_rcb3_rmsg[5], g_rcb3_rmsg[6]);
|
---|
504 | /*
|
---|
505 | * RCControlTaskが別ECUの場合はCANで送信
|
---|
506 | */
|
---|
507 | if (!IsCntOnECU) {
|
---|
508 | NcanSetTxData(0, CONT_CMD_CAN_MSG_TMB, CONT_CMD_CAN_MSG_ID, &g_rcb3_rmsg[0], RCB3_MSG_LEN);
|
---|
509 | }
|
---|
510 | }
|
---|
511 | }
|
---|
512 | }
|
---|
513 | }
|
---|
514 |
|
---|
515 | ///////////////////////////////////////////////////////////////////////////////////////////////////////
|
---|
516 | /*
|
---|
517 | *
|
---|
518 | * 制御系関連
|
---|
519 | *
|
---|
520 | */
|
---|
521 |
|
---|
522 |
|
---|
523 | float g_steer_neutral = STEER_NEUTRAL_INIT;
|
---|
524 | float g_angle = 0;
|
---|
525 |
|
---|
526 | typedef enum{
|
---|
527 | SHIFT_N,
|
---|
528 | SHIFT_D,
|
---|
529 | SHIFT_R,
|
---|
530 | }SHIFT_STATE;
|
---|
531 |
|
---|
532 | /*
|
---|
533 | * 制御関連の変数
|
---|
534 | */
|
---|
535 | volatile boolean g_blinker_l_blink = FALSE;
|
---|
536 | volatile boolean g_blinker_r_blink = FALSE;
|
---|
537 | volatile boolean g_isBrake = FALSE;
|
---|
538 | volatile int g_accelerator = 0;
|
---|
539 | volatile int g_speed_gain = SPEED_GAIN_INIT;
|
---|
540 | volatile SHIFT_STATE g_ShiftState = SHIFT_N;
|
---|
541 |
|
---|
542 | /*
|
---|
543 | * PS3コントローラのアナログボタンのパラメータ
|
---|
544 | */
|
---|
545 | #define PS3ANALOG_MAX 63
|
---|
546 | #define PS3ANALOG_MIN -64
|
---|
547 |
|
---|
548 |
|
---|
549 | void
|
---|
550 | AdjustSteerNeturalL(void)
|
---|
551 | {
|
---|
552 | if(g_steer_neutral < 30) {
|
---|
553 | g_steer_neutral++;
|
---|
554 | g_angle++;
|
---|
555 | syslog(LOG_NOTICE, "Adjust steer neutral = %d.%d", (int)g_steer_neutral, (int)(g_steer_neutral*100) % 100);
|
---|
556 | }
|
---|
557 | }
|
---|
558 |
|
---|
559 | /*
|
---|
560 | * ステアリングニュートラルを右側に
|
---|
561 | */
|
---|
562 | void
|
---|
563 | AdjustSteerNeturalR(void)
|
---|
564 | {
|
---|
565 | if(g_steer_neutral > -30) {
|
---|
566 | g_steer_neutral--;
|
---|
567 | g_angle--;
|
---|
568 | syslog(LOG_NOTICE, "Adjust steer neutral = %d.%d", (int)g_steer_neutral, (int)(g_steer_neutral*100) % 100);
|
---|
569 | }
|
---|
570 | }
|
---|
571 |
|
---|
572 | /*
|
---|
573 | * 左アナログコントローラの左右動作
|
---|
574 | * ステアリングを操作
|
---|
575 | */
|
---|
576 | void
|
---|
577 | onPS3AnalogL_LR(void)
|
---|
578 | {
|
---|
579 | float angle;
|
---|
580 | float cur_analog = p_g_cur_ps3analog[NO_PS3ANALOG_L_LR];
|
---|
581 |
|
---|
582 | if ((cur_analog <= PS3ANALOG_MAX) && (cur_analog >= PS3ANALOG_MIN)) {
|
---|
583 | /* 左 30 から 右 -30 範囲 */
|
---|
584 | angle = -(cur_analog/2);
|
---|
585 | angle += g_steer_neutral;
|
---|
586 | if (angle != g_angle) {
|
---|
587 | g_angle = angle;
|
---|
588 | if(g_angle > 0){
|
---|
589 | syslog(LOG_NOTICE, "Steer angle = %d.%d", (int)g_angle, (int)(g_angle*100) % 100);
|
---|
590 | }
|
---|
591 | else {
|
---|
592 | syslog(LOG_NOTICE, "Steer angle = %d.%d", (int)g_angle, (int)(-g_angle*100) % 100);
|
---|
593 | }
|
---|
594 | }
|
---|
595 | }
|
---|
596 | }
|
---|
597 |
|
---|
598 | /*
|
---|
599 | * 左アナログコントローラの上下動作
|
---|
600 | */
|
---|
601 | void
|
---|
602 | onPS3AnalogL_UD(void)
|
---|
603 | {
|
---|
604 |
|
---|
605 | }
|
---|
606 |
|
---|
607 | /*
|
---|
608 | * 右アナログコントローラの左右動作
|
---|
609 | */
|
---|
610 | void
|
---|
611 | onPS3AnalogR_LR(void)
|
---|
612 | {
|
---|
613 |
|
---|
614 | }
|
---|
615 |
|
---|
616 | /*
|
---|
617 | * 右アナログコントローラの上下動作
|
---|
618 | * 上 - 方向
|
---|
619 | * 下 + 方向
|
---|
620 | */
|
---|
621 | void
|
---|
622 | onPS3AnalogR_UD(void)
|
---|
623 | {
|
---|
624 | float cur_analog = p_g_cur_ps3analog[NO_PS3ANALOG_R_UD];
|
---|
625 | g_accelerator = cur_analog;
|
---|
626 |
|
---|
627 | if(cur_analog <= 0){
|
---|
628 | g_body_cont_info.backlamp = BODY_CONT_OFF;
|
---|
629 | g_body_cont_info.buzzer = BODY_CONT_OFF;
|
---|
630 | }
|
---|
631 | else {
|
---|
632 | // g_accelerator = -cur_analog;
|
---|
633 | // syslog(LOG_NOTICE, "Curanalog = %d.%d", (int)cur_analog, (int)(-cur_analog*100) % 100);
|
---|
634 | g_body_cont_info.backlamp = BODY_CONT_ON;
|
---|
635 | g_body_cont_info.buzzer = BODY_CONT_ON;
|
---|
636 | }
|
---|
637 | }
|
---|
638 |
|
---|
639 | /*
|
---|
640 | * PS3コントローラのデジタルボタン毎の処理
|
---|
641 | */
|
---|
642 |
|
---|
643 | #define TOGGLE_ON_OFF(item) item = (item == BODY_CONT_ON)? BODY_CONT_OFF : BODY_CONT_ON;
|
---|
644 |
|
---|
645 | /*
|
---|
646 | * × : ブレーキ
|
---|
647 | */
|
---|
648 | void
|
---|
649 | onPS3ButtonCrossEdgeOn(void) {
|
---|
650 | g_isBrake = TRUE;
|
---|
651 | // SetDriveSpeed(0);
|
---|
652 | g_body_cont_info.breaklamp = BODY_CONT_ON;
|
---|
653 | }
|
---|
654 |
|
---|
655 |
|
---|
656 | void
|
---|
657 | onPS3ButtonCrossEdgeOff(void) {
|
---|
658 | g_isBrake = FALSE;
|
---|
659 | g_body_cont_info.breaklamp = BODY_CONT_OFF;
|
---|
660 | }
|
---|
661 |
|
---|
662 | /*
|
---|
663 | * △ : ハザード
|
---|
664 | */
|
---|
665 | void
|
---|
666 | onPS3ButtonTriangleEdgeOn(void) {
|
---|
667 | TOGGLE_ON_OFF(g_body_cont_info.hazardlamp);
|
---|
668 | }
|
---|
669 |
|
---|
670 | void
|
---|
671 | onPS3ButtonTriangleEdgeOff(void) {
|
---|
672 |
|
---|
673 | }
|
---|
674 |
|
---|
675 | /*
|
---|
676 | * ○ : メインヘッドライト(ヘッドライト1) 切り替え
|
---|
677 | */
|
---|
678 | void
|
---|
679 | onPS3ButtonNoughtEdgeOn(void) {
|
---|
680 | TOGGLE_ON_OFF(g_body_cont_info.headlight1);
|
---|
681 | }
|
---|
682 |
|
---|
683 | void
|
---|
684 | onPS3ButtonNoughtEdgeOff(void) {
|
---|
685 |
|
---|
686 | }
|
---|
687 |
|
---|
688 | /*
|
---|
689 | * □ : ギア切り替え
|
---|
690 | */
|
---|
691 | void
|
---|
692 | onPS3ButtonSquareEdgeOn(void) {
|
---|
693 | syslog(LOG_NOTICE, "Shift N");
|
---|
694 | g_ShiftState = SHIFT_N;
|
---|
695 | g_body_cont_info.backlamp = BODY_CONT_OFF;
|
---|
696 | g_body_cont_info.buzzer = BODY_CONT_OFF;
|
---|
697 | #if 0
|
---|
698 | TOGGLE_ON_OFF(g_body_cont_info.headlight3);
|
---|
699 | #endif
|
---|
700 | }
|
---|
701 |
|
---|
702 | void
|
---|
703 | onPS3ButtonSquareEdgeOff(void) {
|
---|
704 |
|
---|
705 | }
|
---|
706 |
|
---|
707 | /*
|
---|
708 | * 左ボタン : ステアリングのニュートラルを調整
|
---|
709 | */
|
---|
710 | void
|
---|
711 | onPS3ButtonLeftEdgeOn(void) {
|
---|
712 | AdjustSteerNeturalL();
|
---|
713 | g_body_cont_info.blinker_l = BODY_CONT_ON;
|
---|
714 | }
|
---|
715 |
|
---|
716 | void
|
---|
717 | onPS3ButtonLeftEdgeOff(void) {
|
---|
718 | g_body_cont_info.blinker_l = (g_blinker_l_blink)? BODY_CONT_BLINK : BODY_CONT_OFF;
|
---|
719 | }
|
---|
720 |
|
---|
721 | /*
|
---|
722 | * 右ボタン : ステアリングのニュートラルを調整
|
---|
723 | */
|
---|
724 | void
|
---|
725 | onPS3ButtonRightEdgeOn(void) {
|
---|
726 | AdjustSteerNeturalR();
|
---|
727 | g_body_cont_info.blinker_r = BODY_CONT_ON;
|
---|
728 | }
|
---|
729 |
|
---|
730 | void
|
---|
731 | onPS3ButtonRightEdgeOff(void) {
|
---|
732 | g_body_cont_info.blinker_r = (g_blinker_r_blink)? BODY_CONT_BLINK : BODY_CONT_OFF;
|
---|
733 | }
|
---|
734 |
|
---|
735 | /*
|
---|
736 | * 上ボタン : ステアリングをニュートラルにする
|
---|
737 | */
|
---|
738 | void
|
---|
739 | onPS3ButtonUpEdgeOn(void) {
|
---|
740 | g_steer_neutral = STEER_NEUTRAL_INIT;
|
---|
741 | g_angle = STEER_NEUTRAL_INIT;
|
---|
742 | g_body_cont_info.blinker_l = BODY_CONT_ON;
|
---|
743 | g_body_cont_info.blinker_r = BODY_CONT_ON;
|
---|
744 |
|
---|
745 | #if 0
|
---|
746 | if(g_isBrake){
|
---|
747 | syslog(LOG_NOTICE, "Shift D");
|
---|
748 | g_ShiftState = SHIFT_D;
|
---|
749 | g_body_cont_info.backlamp = BODY_CONT_OFF;
|
---|
750 | g_body_cont_info.buzzer = BODY_CONT_OFF;
|
---|
751 | }else{
|
---|
752 | syslog(LOG_NOTICE, "Shift D : error");
|
---|
753 | }
|
---|
754 | #endif
|
---|
755 | #if 0
|
---|
756 | InitSteerNetural();
|
---|
757 | g_body_cont_info.blinker_l = BODY_CONT_ON;
|
---|
758 | g_body_cont_info.blinker_r = BODY_CONT_ON;
|
---|
759 | #endif
|
---|
760 | }
|
---|
761 |
|
---|
762 | void
|
---|
763 | onPS3ButtonUpEdgeOff(void) {
|
---|
764 | g_body_cont_info.blinker_l = (g_blinker_l_blink)? BODY_CONT_BLINK : BODY_CONT_OFF;
|
---|
765 | g_body_cont_info.blinker_r = (g_blinker_r_blink)? BODY_CONT_BLINK : BODY_CONT_OFF;
|
---|
766 | }
|
---|
767 |
|
---|
768 | /*
|
---|
769 | * 下ボタン : モータ制御のゲインを初期値にする
|
---|
770 | */
|
---|
771 | void
|
---|
772 | onPS3ButtonDownEdgeOn(void) {
|
---|
773 |
|
---|
774 | #if 0
|
---|
775 | if(g_isBrake){
|
---|
776 | syslog(LOG_NOTICE, "Shift R");
|
---|
777 | g_ShiftState = SHIFT_R;
|
---|
778 | g_body_cont_info.backlamp = BODY_CONT_ON;
|
---|
779 | g_body_cont_info.buzzer = BODY_CONT_ON;
|
---|
780 | }else{
|
---|
781 | syslog(LOG_NOTICE, "Shift R : error");
|
---|
782 | }
|
---|
783 | #endif
|
---|
784 |
|
---|
785 | g_speed_gain = SPEED_GAIN_INIT;
|
---|
786 | g_body_cont_info.blinker_l = BODY_CONT_ON;
|
---|
787 | g_body_cont_info.blinker_r = BODY_CONT_ON;
|
---|
788 |
|
---|
789 | #if 0
|
---|
790 | char str[30];
|
---|
791 |
|
---|
792 | g_speed_gain = MOTOR_GAIN_INIT;
|
---|
793 | SetDriveSpeed(g_speed/g_speed_gain);
|
---|
794 | g_body_cont_info.blinker_l = BODY_CONT_ON;
|
---|
795 | g_body_cont_info.blinker_r = BODY_CONT_ON;
|
---|
796 |
|
---|
797 | sprintf(str, "Adjust speed gain = %d \r\n", g_speed_gain);
|
---|
798 | uart_PSendString(RC_OUTPUT_PORT, str);
|
---|
799 | #endif
|
---|
800 | }
|
---|
801 |
|
---|
802 | void
|
---|
803 | onPS3ButtonDownEdgeOff(void) {
|
---|
804 | #if 0
|
---|
805 | g_body_cont_info.blinker_l = (g_blinker_l_blink)? BODY_CONT_BLINK : BODY_CONT_OFF;
|
---|
806 | g_body_cont_info.blinker_r = (g_blinker_r_blink)? BODY_CONT_BLINK : BODY_CONT_OFF;
|
---|
807 | #endif
|
---|
808 | }
|
---|
809 |
|
---|
810 |
|
---|
811 | /*
|
---|
812 | * L1/R1 : 方向指示器のON/OFF
|
---|
813 | */
|
---|
814 | void
|
---|
815 | onPS3ButtonL1EdgeOn(void){
|
---|
816 | if (g_blinker_l_blink) {
|
---|
817 | g_blinker_l_blink = FALSE;
|
---|
818 | /* ボディーへの指示がONの場合は他の機能がONにしているため何もしない */
|
---|
819 | if (!(g_body_cont_info.blinker_l == BODY_CONT_ON)) {
|
---|
820 | g_body_cont_info.blinker_l = BODY_CONT_OFF;
|
---|
821 | }
|
---|
822 | } else {
|
---|
823 | g_blinker_l_blink = TRUE;
|
---|
824 | /* ボディーへの指示がONの場合は他の機能がONにしているため何もしない */
|
---|
825 | if (!(g_body_cont_info.blinker_l == BODY_CONT_ON)) {
|
---|
826 | g_body_cont_info.blinker_l = BODY_CONT_BLINK;
|
---|
827 | }
|
---|
828 | }
|
---|
829 | }
|
---|
830 |
|
---|
831 | void
|
---|
832 | onPS3ButtonL1EdgeOff(void) {
|
---|
833 |
|
---|
834 | }
|
---|
835 |
|
---|
836 | void
|
---|
837 | onPS3ButtonR1EdgeOn(void){
|
---|
838 | if (g_blinker_r_blink) {
|
---|
839 | g_blinker_r_blink = FALSE;
|
---|
840 | /* ボディーへの指示がONの場合は他の機能がONにしているため何もしない */
|
---|
841 | if (!(g_body_cont_info.blinker_r == BODY_CONT_ON)) {
|
---|
842 | g_body_cont_info.blinker_r = BODY_CONT_OFF;
|
---|
843 | }
|
---|
844 | } else {
|
---|
845 | g_blinker_r_blink = TRUE;
|
---|
846 | /* ボディーへの指示がONの場合は他の機能がONにしているため何もしない */
|
---|
847 | if (!(g_body_cont_info.blinker_r == BODY_CONT_ON)) {
|
---|
848 | g_body_cont_info.blinker_r = BODY_CONT_BLINK;
|
---|
849 | }
|
---|
850 | }
|
---|
851 | }
|
---|
852 |
|
---|
853 | void
|
---|
854 | onPS3ButtonR1EdgeOff(void) {
|
---|
855 |
|
---|
856 | }
|
---|
857 |
|
---|
858 | /*
|
---|
859 | * L2/R2 : モータ制御のゲインを変更
|
---|
860 | */
|
---|
861 | void
|
---|
862 | onPS3ButtonL2EdgeOn(void){
|
---|
863 | if(g_speed_gain > SPEED_GAIN_MIN){
|
---|
864 | g_speed_gain--;
|
---|
865 | }
|
---|
866 | g_body_cont_info.blinker_l = BODY_CONT_ON;
|
---|
867 | #if 0
|
---|
868 | AdjustSpeedGainD();
|
---|
869 | g_body_cont_info.blinker_l = BODY_CONT_ON;
|
---|
870 | #endif
|
---|
871 | }
|
---|
872 |
|
---|
873 | void
|
---|
874 | onPS3ButtonL2EdgeOff(void){
|
---|
875 | #if 0
|
---|
876 | g_body_cont_info.blinker_l = (g_blinker_l_blink)? BODY_CONT_BLINK : BODY_CONT_OFF;
|
---|
877 | #endif
|
---|
878 | }
|
---|
879 |
|
---|
880 | void
|
---|
881 | onPS3ButtonR2EdgeOn(void){
|
---|
882 | if (g_speed_gain < SPEED_GAIN_MAX){
|
---|
883 | g_speed_gain++;
|
---|
884 | g_body_cont_info.blinker_r = BODY_CONT_ON;
|
---|
885 | }
|
---|
886 |
|
---|
887 | #if 0
|
---|
888 | AdjustSpeedGainU();
|
---|
889 | g_body_cont_info.blinker_r = BODY_CONT_ON;
|
---|
890 | #endif
|
---|
891 | }
|
---|
892 |
|
---|
893 | void
|
---|
894 | onPS3ButtonR2EdgeOff(void){
|
---|
895 | #if 0
|
---|
896 | g_body_cont_info.blinker_r = (g_blinker_r_blink)? BODY_CONT_BLINK : BODY_CONT_OFF;
|
---|
897 | #endif
|
---|
898 | }
|
---|
899 |
|
---|
900 | /*
|
---|
901 | * ボタンの変化検出のためのマクロ
|
---|
902 | */
|
---|
903 | #define PS3BUTTON_EDGE_ON(no) ((p_g_pre_ps3button[no] == FALSE) && (p_g_cur_ps3button[no] == TRUE))
|
---|
904 | #define PS3BUTTON_EDGE_OFF(no) ((p_g_pre_ps3button[no] == TRUE) && (p_g_cur_ps3button[no] == FALSE))
|
---|
905 | #define PS3ANALOG_CHANGE(no) (p_g_pre_ps3analog[no] != p_g_cur_ps3analog[no])
|
---|
906 |
|
---|
907 | /*
|
---|
908 | * PS3コントローラからのイベントの処理
|
---|
909 | */
|
---|
910 | void
|
---|
911 | onMsgPS3(void) {
|
---|
912 | if (PS3BUTTON_EDGE_OFF(NO_PS3BUTTON_CROSS)){
|
---|
913 | onPS3ButtonCrossEdgeOff();
|
---|
914 | }
|
---|
915 | if (PS3BUTTON_EDGE_ON(NO_PS3BUTTON_TRIANGLE)){
|
---|
916 | onPS3ButtonTriangleEdgeOn();
|
---|
917 | }
|
---|
918 | if (PS3BUTTON_EDGE_OFF(NO_PS3BUTTON_TRIANGLE)){
|
---|
919 | onPS3ButtonTriangleEdgeOff();
|
---|
920 | }
|
---|
921 | if (PS3BUTTON_EDGE_ON(NO_PS3BUTTON_NOUGHT)){
|
---|
922 | onPS3ButtonNoughtEdgeOn();
|
---|
923 | }
|
---|
924 | if (PS3BUTTON_EDGE_OFF(NO_PS3BUTTON_NOUGHT)){
|
---|
925 | onPS3ButtonNoughtEdgeOff();
|
---|
926 | }
|
---|
927 | if (PS3BUTTON_EDGE_ON(NO_PS3BUTTON_SQUARE)){
|
---|
928 | onPS3ButtonSquareEdgeOn();
|
---|
929 | }
|
---|
930 | if (PS3BUTTON_EDGE_OFF(NO_PS3BUTTON_SQUARE)){
|
---|
931 | onPS3ButtonSquareEdgeOff();
|
---|
932 | }
|
---|
933 |
|
---|
934 |
|
---|
935 | if (PS3BUTTON_EDGE_ON(NO_PS3BUTTON_LEFT)){
|
---|
936 | onPS3ButtonLeftEdgeOn();
|
---|
937 | }
|
---|
938 | if (PS3BUTTON_EDGE_OFF(NO_PS3BUTTON_LEFT)){
|
---|
939 | onPS3ButtonLeftEdgeOff();
|
---|
940 | }
|
---|
941 | if (PS3BUTTON_EDGE_ON(NO_PS3BUTTON_RIGHT)){
|
---|
942 | onPS3ButtonRightEdgeOn();
|
---|
943 | }
|
---|
944 | if (PS3BUTTON_EDGE_OFF(NO_PS3BUTTON_RIGHT)){
|
---|
945 | onPS3ButtonRightEdgeOff();
|
---|
946 | }
|
---|
947 | if (PS3BUTTON_EDGE_ON(NO_PS3BUTTON_UP)){
|
---|
948 | onPS3ButtonUpEdgeOn();
|
---|
949 | }
|
---|
950 | if (PS3BUTTON_EDGE_OFF(NO_PS3BUTTON_UP)){
|
---|
951 | onPS3ButtonUpEdgeOff();
|
---|
952 | }
|
---|
953 | if (PS3BUTTON_EDGE_ON(NO_PS3BUTTON_DOWN)){
|
---|
954 | onPS3ButtonDownEdgeOn();
|
---|
955 | }
|
---|
956 | if (PS3BUTTON_EDGE_OFF(NO_PS3BUTTON_DOWN)){
|
---|
957 | onPS3ButtonDownEdgeOff();
|
---|
958 | }
|
---|
959 | if (PS3BUTTON_EDGE_ON(NO_PS3BUTTON_CROSS)){
|
---|
960 | onPS3ButtonCrossEdgeOn();
|
---|
961 | }
|
---|
962 |
|
---|
963 | if (PS3BUTTON_EDGE_ON(NO_PS3BUTTON_L1)){
|
---|
964 | onPS3ButtonL1EdgeOn();
|
---|
965 | }
|
---|
966 | if (PS3BUTTON_EDGE_ON(NO_PS3BUTTON_R1)){
|
---|
967 | onPS3ButtonR1EdgeOn();
|
---|
968 | }
|
---|
969 | if (PS3BUTTON_EDGE_ON(NO_PS3BUTTON_L2)){
|
---|
970 | onPS3ButtonL2EdgeOn();
|
---|
971 | }
|
---|
972 | if (PS3BUTTON_EDGE_OFF(NO_PS3BUTTON_L2)){
|
---|
973 | onPS3ButtonL2EdgeOff();
|
---|
974 | }
|
---|
975 | if (PS3BUTTON_EDGE_ON(NO_PS3BUTTON_R2)){
|
---|
976 | onPS3ButtonR2EdgeOn();
|
---|
977 | }
|
---|
978 | if (PS3BUTTON_EDGE_OFF(NO_PS3BUTTON_R2)){
|
---|
979 | onPS3ButtonR2EdgeOff();
|
---|
980 | }
|
---|
981 | if (PS3ANALOG_CHANGE(NO_PS3ANALOG_L_LR)) {
|
---|
982 | onPS3AnalogL_LR();
|
---|
983 | }
|
---|
984 | if (PS3ANALOG_CHANGE(NO_PS3ANALOG_L_UD)) {
|
---|
985 | onPS3AnalogL_UD();
|
---|
986 | }
|
---|
987 | if (PS3ANALOG_CHANGE(NO_PS3ANALOG_R_LR)) {
|
---|
988 | onPS3AnalogR_LR();
|
---|
989 | }
|
---|
990 | if (PS3ANALOG_CHANGE(NO_PS3ANALOG_R_UD)) {
|
---|
991 | onPS3AnalogR_UD();
|
---|
992 | }
|
---|
993 | }
|
---|
994 |
|
---|
995 | /*
|
---|
996 | * 周期送信とするか
|
---|
997 | */
|
---|
998 | boolean g_body_can_msg_cyclic = FALSE;
|
---|
999 |
|
---|
1000 |
|
---|
1001 | void
|
---|
1002 | body_can_msg_send(void)
|
---|
1003 | {
|
---|
1004 | uint8 tx_data[8];
|
---|
1005 | /* 前回送付の状態 */
|
---|
1006 | static BODY_CONT_INFO g_pre_body_cont_info;
|
---|
1007 |
|
---|
1008 | /* 状態指示に変化が無ければ送信しない */
|
---|
1009 | if (!g_body_can_msg_cyclic){
|
---|
1010 | if((g_body_cont_info.headlight1 == g_pre_body_cont_info.headlight1) &&
|
---|
1011 | (g_body_cont_info.headlight2 == g_pre_body_cont_info.headlight2) &&
|
---|
1012 | (g_body_cont_info.headlight3 == g_pre_body_cont_info.headlight3) &&
|
---|
1013 | (g_body_cont_info.breaklamp == g_pre_body_cont_info.breaklamp) &&
|
---|
1014 | (g_body_cont_info.backlamp == g_pre_body_cont_info.backlamp) &&
|
---|
1015 | (g_body_cont_info.buzzer == g_pre_body_cont_info.buzzer) &&
|
---|
1016 | (g_body_cont_info.blinker_l == g_pre_body_cont_info.blinker_l) &&
|
---|
1017 | (g_body_cont_info.blinker_r == g_pre_body_cont_info.blinker_r) &&
|
---|
1018 | (g_body_cont_info.hazardlamp == g_pre_body_cont_info.hazardlamp)) {
|
---|
1019 | return;
|
---|
1020 | }
|
---|
1021 | }
|
---|
1022 |
|
---|
1023 | /* 送信メッセージを生成 */
|
---|
1024 | tx_data[0] = ((g_body_cont_info.headlight1 << 7) & 0x80) |
|
---|
1025 | ((g_body_cont_info.headlight2 << 6) & 0x40) |
|
---|
1026 | ((g_body_cont_info.headlight3 << 5) & 0x20) |
|
---|
1027 | ((g_body_cont_info.breaklamp << 4) & 0x10) |
|
---|
1028 | ((g_body_cont_info.backlamp << 3) & 0x08) |
|
---|
1029 | ((g_body_cont_info.buzzer << 1) & 0x06);
|
---|
1030 |
|
---|
1031 | tx_data[1] = ((g_body_cont_info.blinker_l << 6) & 0xc0) |
|
---|
1032 | ((g_body_cont_info.blinker_r << 4) & 0x30) |
|
---|
1033 | ((g_body_cont_info.hazardlamp << 3) & 0x08);
|
---|
1034 |
|
---|
1035 | NcanSetTxData(0, BODY_CAN_MSG_RMB, BODY_CAN_MSG_ID, tx_data, 2);
|
---|
1036 |
|
---|
1037 | g_pre_body_cont_info = g_body_cont_info;
|
---|
1038 | }
|
---|
1039 |
|
---|
1040 |
|
---|
1041 | /*
|
---|
1042 | * 起動時のボディーの接続確認
|
---|
1043 | *
|
---|
1044 | * 全LED点滅とブザーを鳴らす.
|
---|
1045 | */
|
---|
1046 | void
|
---|
1047 | body_check(void)
|
---|
1048 | {
|
---|
1049 | int loop, tcnt;
|
---|
1050 | uint8 status = BODY_CONT_ON;
|
---|
1051 |
|
---|
1052 | for(loop = 0; loop < INIT_BODY_BLINK_COUNT*2; loop++) {
|
---|
1053 | g_body_cont_info.headlight1 = status;
|
---|
1054 | g_body_cont_info.headlight2 = status;
|
---|
1055 | g_body_cont_info.headlight3 = status;
|
---|
1056 | g_body_cont_info.breaklamp = status;
|
---|
1057 | g_body_cont_info.backlamp = status;
|
---|
1058 | g_body_cont_info.buzzer = status;
|
---|
1059 | g_body_cont_info.blinker_l = status;
|
---|
1060 | g_body_cont_info.blinker_r = status;
|
---|
1061 |
|
---|
1062 | tcnt = 0;
|
---|
1063 |
|
---|
1064 | if (!IsBodyOnECU){
|
---|
1065 | body_can_msg_send();
|
---|
1066 | }
|
---|
1067 |
|
---|
1068 | while(1) {
|
---|
1069 | WaitEvent(RCOperatorEvt);
|
---|
1070 | ClearEvent(RCOperatorEvt);
|
---|
1071 | tcnt++;
|
---|
1072 | if (tcnt == INIT_BODY_BLINK_CYCLE_MS/10) {
|
---|
1073 | break;
|
---|
1074 | }
|
---|
1075 | }
|
---|
1076 |
|
---|
1077 | status = (status == BODY_CONT_ON)? BODY_CONT_OFF : BODY_CONT_ON;
|
---|
1078 | }
|
---|
1079 | }
|
---|
1080 | void
|
---|
1081 | rcb3_error(){
|
---|
1082 | static int cnt = 0;
|
---|
1083 | syslog(LOG_NOTICE, "error 0x%x : 0x%x, 0x%x, 0x%x, 0x%x, 0x%x, 0x%x, 0x%x, 0x%x \r\n", cnt++, g_rcb3_rmsg[0], g_rcb3_rmsg[1], g_rcb3_rmsg[2], g_rcb3_rmsg[3], g_rcb3_rmsg[4], g_rcb3_rmsg[5], g_rcb3_rmsg[6], g_rcb3_rmsg[7]);
|
---|
1084 | }
|
---|
1085 |
|
---|
1086 | void
|
---|
1087 | com_error(){
|
---|
1088 | syslog(LOG_NOTICE, "com error");
|
---|
1089 | }
|
---|
1090 |
|
---|
1091 | /*
|
---|
1092 | * 制御タスク
|
---|
1093 | */
|
---|
1094 | TASK(RCControlTask)
|
---|
1095 | {
|
---|
1096 | uint32 cycle = 0;
|
---|
1097 | syslog(LOG_INFO, "RCControlTask : Start!");
|
---|
1098 | uint8 rx_data[8];
|
---|
1099 | uint8 rx_size;
|
---|
1100 | int loop;
|
---|
1101 | boolean result;
|
---|
1102 |
|
---|
1103 | body_check();
|
---|
1104 |
|
---|
1105 | while(1){
|
---|
1106 | /* 10m待ち */
|
---|
1107 | WaitEvent(RCControlEvt);
|
---|
1108 | ClearEvent(RCControlEvt);
|
---|
1109 |
|
---|
1110 | /*
|
---|
1111 | * RCOperatorTaskが別ECUの場合はCANで受信
|
---|
1112 | */
|
---|
1113 | if (!IsOpeOnECU){
|
---|
1114 | NcanGetRxData(0, CONT_CMD_CAN_MSG_RMB, &rx_data[0], &rx_size);
|
---|
1115 | if (rx_size != 0) {
|
---|
1116 | result = FALSE;
|
---|
1117 | for (loop = 0; loop < rx_size; loop++){
|
---|
1118 | result = rcb3_AddReceivedByte(rx_data[loop]);
|
---|
1119 | }
|
---|
1120 | if (result) {
|
---|
1121 | onMsgPS3();
|
---|
1122 | }
|
---|
1123 | else {
|
---|
1124 | syslog(LOG_NOTICE, "RCControlTask : Received Can data Error!\r\n");
|
---|
1125 | }
|
---|
1126 | }
|
---|
1127 | }
|
---|
1128 | else {
|
---|
1129 | /*
|
---|
1130 | * 同一ECUの場合
|
---|
1131 | */
|
---|
1132 | if (UpDateCntrolCmd == TRUE){
|
---|
1133 | onMsgPS3();
|
---|
1134 | UpDateCntrolCmd = FALSE;
|
---|
1135 | }
|
---|
1136 | }
|
---|
1137 |
|
---|
1138 | /*
|
---|
1139 | * RCBodyTaskが別ECUの場合はCANで送信
|
---|
1140 | */
|
---|
1141 | if (!IsBodyOnECU) {
|
---|
1142 | if (cycle++ == (BODY_CAN_SEND_CYCLE_MS/RC_CONTRL_TASK_CYCLE_MS)) {
|
---|
1143 | body_can_msg_send();
|
---|
1144 | cycle = 0;
|
---|
1145 | }
|
---|
1146 | }
|
---|
1147 | }
|
---|
1148 | }
|
---|
1149 |
|
---|
1150 | //////////////////////////////////////////////////////////////////////////
|
---|
1151 | /*
|
---|
1152 | *
|
---|
1153 | * パワトレ関係
|
---|
1154 | *
|
---|
1155 | */
|
---|
1156 | #define UEC_TO_PWMCNT(usec) (usec*PWM_CLK_MH)
|
---|
1157 | /* 100nsec 単位との変換 */
|
---|
1158 | #define M100NSEC_TO_PWMCNT(m100nsec) ((m100nsec*PWM_CLK_MH)/10)
|
---|
1159 |
|
---|
1160 | #define SERVO_N 1570
|
---|
1161 | //#define SERVO_RMAX 1830
|
---|
1162 | //#define SERVO_LMAX 1400
|
---|
1163 |
|
---|
1164 |
|
---|
1165 | #define SERVO_RMAX 1870
|
---|
1166 | #define SERVO_LMAX 1330
|
---|
1167 |
|
---|
1168 | void
|
---|
1169 | servo_setangle(int angle) {
|
---|
1170 | sil_wrw_iop((void *)PWM1_ADDR, UEC_TO_PWMCNT(angle));
|
---|
1171 | }
|
---|
1172 |
|
---|
1173 | #define ESC_N 15000
|
---|
1174 | #define ESC_DMAX 14000
|
---|
1175 | #define ESC_RMAX 16500
|
---|
1176 | #define ESC_DTH 14500
|
---|
1177 | #define ESC_RTH 15800
|
---|
1178 | #define ESC_BREAK 16200
|
---|
1179 |
|
---|
1180 | void
|
---|
1181 | RCPowerTrainTaskWait_20msec(void){
|
---|
1182 | ClearEvent(RCPowerTrainEvt);
|
---|
1183 | WaitEvent(RCPowerTrainEvt);
|
---|
1184 | ClearEvent(RCPowerTrainEvt);
|
---|
1185 | WaitEvent(RCPowerTrainEvt);
|
---|
1186 | ClearEvent(RCPowerTrainEvt);
|
---|
1187 | }
|
---|
1188 |
|
---|
1189 | void
|
---|
1190 | esc_setspeed(int speed){
|
---|
1191 | sil_wrw_iop((void *)PWM2_ADDR, M100NSEC_TO_PWMCNT(speed));
|
---|
1192 | }
|
---|
1193 |
|
---|
1194 | void
|
---|
1195 | esc_dbrak(void){
|
---|
1196 | esc_setspeed(ESC_BREAK);
|
---|
1197 | }
|
---|
1198 |
|
---|
1199 | void
|
---|
1200 | esc_rbrak(void){
|
---|
1201 | esc_setspeed(ESC_DTH);
|
---|
1202 | RCPowerTrainTaskWait_20msec();
|
---|
1203 | RCPowerTrainTaskWait_20msec();
|
---|
1204 | esc_setspeed(ESC_BREAK);
|
---|
1205 | }
|
---|
1206 |
|
---|
1207 | void
|
---|
1208 | esc_neutral(void){
|
---|
1209 | esc_setspeed(ESC_N);
|
---|
1210 | }
|
---|
1211 |
|
---|
1212 | void
|
---|
1213 | esc_reverse(void){
|
---|
1214 | esc_neutral();
|
---|
1215 | RCPowerTrainTaskWait_20msec();
|
---|
1216 | RCPowerTrainTaskWait_20msec();
|
---|
1217 | }
|
---|
1218 |
|
---|
1219 | void
|
---|
1220 | esc_change_drive_to_reverse(void){
|
---|
1221 | esc_setspeed(ESC_BREAK);
|
---|
1222 | RCPowerTrainTaskWait_20msec();
|
---|
1223 | esc_neutral();
|
---|
1224 | RCPowerTrainTaskWait_20msec();
|
---|
1225 | }
|
---|
1226 |
|
---|
1227 | TASK(RCPowerTrainTask){
|
---|
1228 | int servo_pwm;
|
---|
1229 | static boolean pre_g_isBrake = FALSE;
|
---|
1230 | static SHIFT_STATE pre_g_ShiftState = SHIFT_N;
|
---|
1231 | static int pre_g_accelerator = 0;
|
---|
1232 | int esc_pwm_100nsec = 0;
|
---|
1233 |
|
---|
1234 | syslog(LOG_INFO, "RCPowerTrainTask : Start!");
|
---|
1235 |
|
---|
1236 | /* 周期設定 : 20m周期 */
|
---|
1237 | sil_wrw_iop((void *)PWM_MAX_ADDR, UEC_TO_PWMCNT(20000));
|
---|
1238 |
|
---|
1239 | /* ニュートラル */
|
---|
1240 | esc_neutral();
|
---|
1241 | servo_setangle(SERVO_N);
|
---|
1242 | syslog(LOG_INFO, "RCPowerTrainTask : neutral");
|
---|
1243 |
|
---|
1244 | while(1){
|
---|
1245 | WaitEvent(RCPowerTrainEvt); /* 10msの作業時間待ち */
|
---|
1246 | ClearEvent(RCPowerTrainEvt);
|
---|
1247 |
|
---|
1248 | /*
|
---|
1249 | * サーボの設定
|
---|
1250 | * 左 32 から 右 -32 範囲
|
---|
1251 | */
|
---|
1252 | /*
|
---|
1253 | SERVO_LMAX SERVO_N SERVO_RMAX
|
---|
1254 | 1400 1570 1830
|
---|
1255 | 32 0 -32
|
---|
1256 | */
|
---|
1257 |
|
---|
1258 | if(g_angle >= 0){
|
---|
1259 | /* 左側操舵の場合 */
|
---|
1260 | servo_pwm = SERVO_N - ((((SERVO_N - SERVO_LMAX) * 100) / (32 * 100)) * g_angle);
|
---|
1261 | }
|
---|
1262 | else {
|
---|
1263 | /* 右側操舵の場合 */
|
---|
1264 | servo_pwm = SERVO_N + ((((SERVO_RMAX - SERVO_N) * 100) / (32 * 100)) * (-g_angle));
|
---|
1265 | }
|
---|
1266 | servo_setangle(servo_pwm);
|
---|
1267 |
|
---|
1268 | if(pre_g_isBrake != g_isBrake) {
|
---|
1269 | /* ブレーキの状態が変化した場合 */
|
---|
1270 | pre_g_isBrake = g_isBrake;
|
---|
1271 | if (g_isBrake == FALSE) {
|
---|
1272 | /* ノンブレーキ状態 */
|
---|
1273 | syslog(LOG_INFO, "To non-braking");
|
---|
1274 | if (g_ShiftState == SHIFT_R) {
|
---|
1275 | esc_reverse();
|
---|
1276 | }
|
---|
1277 | }
|
---|
1278 | else if (g_isBrake == TRUE) {
|
---|
1279 | /* ブレーキ状態 */
|
---|
1280 | syslog(LOG_INFO, "To braking");
|
---|
1281 | if (pre_g_accelerator > 0){
|
---|
1282 | esc_dbrak();
|
---|
1283 | }
|
---|
1284 | else {
|
---|
1285 | esc_rbrak();
|
---|
1286 | }
|
---|
1287 | }
|
---|
1288 | }
|
---|
1289 |
|
---|
1290 | if (g_accelerator < 0){
|
---|
1291 | g_ShiftState = SHIFT_D;
|
---|
1292 | pre_g_ShiftState = SHIFT_D;
|
---|
1293 | }
|
---|
1294 |
|
---|
1295 | if (g_accelerator == 0){
|
---|
1296 | g_ShiftState = SHIFT_N;
|
---|
1297 | }
|
---|
1298 |
|
---|
1299 | if (g_accelerator > 0){
|
---|
1300 | if (pre_g_ShiftState == SHIFT_D){
|
---|
1301 | esc_change_drive_to_reverse();
|
---|
1302 | syslog(LOG_INFO, "RCPowerTrainTask : change drive to reverse");
|
---|
1303 | }
|
---|
1304 | g_ShiftState = SHIFT_R;
|
---|
1305 | pre_g_ShiftState = SHIFT_R;
|
---|
1306 | }
|
---|
1307 | #if 0
|
---|
1308 | if(pre_g_ShiftState != g_ShiftState) {
|
---|
1309 | pre_g_ShiftState = g_ShiftState;
|
---|
1310 | if(g_ShiftState == SHIFT_D){
|
---|
1311 | esc_pwm_100nsec = ESC_DTH;
|
---|
1312 | }else if(g_ShiftState == SHIFT_R){
|
---|
1313 | esc_pwm_100nsec = ESC_RTH;
|
---|
1314 | esc_reverse();
|
---|
1315 | }
|
---|
1316 | }
|
---|
1317 | #endif
|
---|
1318 |
|
---|
1319 | if(g_isBrake == TRUE){
|
---|
1320 | /* ブレーキ状態 */
|
---|
1321 | }else{
|
---|
1322 | /* ノンブレーキ状態 */
|
---|
1323 | if(g_ShiftState == SHIFT_N){
|
---|
1324 | /* ニュートラル */
|
---|
1325 | esc_neutral();
|
---|
1326 | } else {
|
---|
1327 | /* ニュートラル以外 */
|
---|
1328 |
|
---|
1329 | /*
|
---|
1330 | *
|
---|
1331 | */
|
---|
1332 | if(g_accelerator == 0){
|
---|
1333 | esc_pwm_100nsec = ESC_N;
|
---|
1334 | }else{
|
---|
1335 | if (g_ShiftState == SHIFT_D){
|
---|
1336 | esc_pwm_100nsec = ESC_DTH + ((((ESC_DTH - ESC_DMAX)/64) * g_accelerator));
|
---|
1337 | // * g_speed_gain/SPEED_GAIN_INIT
|
---|
1338 | }
|
---|
1339 | if (g_ShiftState == SHIFT_R){
|
---|
1340 | esc_pwm_100nsec = ESC_RTH + ((((ESC_RMAX - ESC_RTH)/64) * g_accelerator));
|
---|
1341 | // * g_speed_gain/SPEED_GAIN_INIT
|
---|
1342 | }
|
---|
1343 | }
|
---|
1344 | esc_setspeed(esc_pwm_100nsec);
|
---|
1345 | }
|
---|
1346 | }
|
---|
1347 | pre_g_accelerator = g_accelerator;
|
---|
1348 | }
|
---|
1349 | }
|
---|
1350 |
|
---|
1351 |
|
---|
1352 |
|
---|
1353 |
|
---|
1354 | //////////////////////////////////////////////////////////////////////////
|
---|
1355 | /*
|
---|
1356 | * LED制御系
|
---|
1357 | */
|
---|
1358 | static uint8 led0_enable = FALSE;
|
---|
1359 | static uint8 led7_enable = FALSE;
|
---|
1360 |
|
---|
1361 | void
|
---|
1362 | set_led7(boolean enable){
|
---|
1363 | led7_enable = enable;
|
---|
1364 | }
|
---|
1365 |
|
---|
1366 | void
|
---|
1367 | set_led0(boolean enable){
|
---|
1368 | led0_enable = enable;
|
---|
1369 | }
|
---|
1370 |
|
---|
1371 | #define PWM_CYCLE_100US 100
|
---|
1372 | #define LED_SHIFT_CYCLE 10
|
---|
1373 | #define PWM_MAX 100
|
---|
1374 |
|
---|
1375 | int led_duty[8];
|
---|
1376 |
|
---|
1377 | ALARMCALLBACK(ActiveLEDCycCb){
|
---|
1378 | static int pwm_cyccnt = 0;
|
---|
1379 | uint8 pattern = 0;
|
---|
1380 | static int center_led = 6;
|
---|
1381 | static int direction = 1; /* 1:right, 0:left */
|
---|
1382 | static int led_shift_cnt = 0;
|
---|
1383 | int i;
|
---|
1384 |
|
---|
1385 | /* 100usec毎に呼び出される */
|
---|
1386 |
|
---|
1387 | /* PWM周期 */
|
---|
1388 | if (pwm_cyccnt++ >= PWM_CYCLE_100US){
|
---|
1389 | pwm_cyccnt = 0;
|
---|
1390 | }
|
---|
1391 |
|
---|
1392 | if (pwm_cyccnt == 0) {
|
---|
1393 | if(led_shift_cnt++ == LED_SHIFT_CYCLE){
|
---|
1394 | led_shift_cnt = 0;
|
---|
1395 |
|
---|
1396 | for(i = 0; i < 7; i++){
|
---|
1397 | int pwm = PWM_MAX;
|
---|
1398 | if (center_led > i){
|
---|
1399 | pwm = PWM_MAX >> ((center_led - i) + 1);
|
---|
1400 | }
|
---|
1401 | else if (center_led < i){
|
---|
1402 | pwm = PWM_MAX >> ((i - center_led) + 1);
|
---|
1403 | }
|
---|
1404 | else if (center_led == i){
|
---|
1405 | pwm = PWM_MAX;
|
---|
1406 | }
|
---|
1407 | led_duty[i] = pwm;
|
---|
1408 | }
|
---|
1409 |
|
---|
1410 | if(direction == 1){
|
---|
1411 | /* right */
|
---|
1412 | if (--center_led <= 1){
|
---|
1413 | direction = 0;
|
---|
1414 | }
|
---|
1415 | }
|
---|
1416 | else {
|
---|
1417 | /* left */
|
---|
1418 | if (++center_led >= 6){
|
---|
1419 | direction = 1;
|
---|
1420 | }
|
---|
1421 | }
|
---|
1422 | }
|
---|
1423 | }
|
---|
1424 |
|
---|
1425 | for(i = 1; i < 7; i++){
|
---|
1426 | if (pwm_cyccnt < led_duty[i]) {
|
---|
1427 | pattern |= 1 << i;
|
---|
1428 | }
|
---|
1429 | }
|
---|
1430 |
|
---|
1431 | if (led0_enable) {
|
---|
1432 | pattern |= 0x01;
|
---|
1433 | }
|
---|
1434 | if (led7_enable) {
|
---|
1435 | pattern |= 0x80;
|
---|
1436 | }
|
---|
1437 |
|
---|
1438 | set_led(pattern);
|
---|
1439 | }
|
---|
1440 |
|
---|
1441 | //////////////////////////////////////////////////////////////////////////
|
---|
1442 |
|
---|
1443 | /*
|
---|
1444 | * CANの初期化ルーチン
|
---|
1445 | */
|
---|
1446 | void
|
---|
1447 | can_init(void){
|
---|
1448 | /*
|
---|
1449 | * モードによらず全てのメールボックスを初期化する
|
---|
1450 | */
|
---|
1451 | NcanInit(0);
|
---|
1452 | NcanSetMailBoxInfo(0, BODY_CAN_MSG_TMB, DIR_SEND, 0, BODY_CAN_MSG_ID, 0, 0);
|
---|
1453 | NcanSetMailBoxInfo(0, CONT_CMD_CAN_MSG_TMB, DIR_SEND, 0, CONT_CMD_CAN_MSG_ID, 0, 0);
|
---|
1454 | NcanSetMailBoxInfo(0, BODY_CAN_MSG_RMB, DIR_RECV, 0, BODY_CAN_MSG_ID, 0, 0);
|
---|
1455 | NcanSetMailBoxInfo(0, CONT_CMD_CAN_MSG_RMB, DIR_RECV, 0, CONT_CMD_CAN_MSG_ID, 0, 0);
|
---|
1456 | NcanEnable(0);
|
---|
1457 | }
|
---|
1458 |
|
---|
1459 | //////////////////////////////////////////////////////////////////////////
|
---|
1460 | /*
|
---|
1461 | * スタートアップフックルーチン
|
---|
1462 | */
|
---|
1463 | void
|
---|
1464 | StartupHook(void)
|
---|
1465 | {
|
---|
1466 | syslog_initialize();
|
---|
1467 | syslog_msk_log(LOG_UPTO(LOG_INFO));
|
---|
1468 | InitSerial();
|
---|
1469 | print_banner();
|
---|
1470 | can_init();
|
---|
1471 | rcb3_Init();
|
---|
1472 | }
|
---|
1473 |
|
---|
1474 | /*
|
---|
1475 | * シャットダウンフックルーチン
|
---|
1476 | */
|
---|
1477 | void
|
---|
1478 | ShutdownHook(StatusType Error)
|
---|
1479 | {
|
---|
1480 | /* 終了ログ出力 */
|
---|
1481 | syslog(LOG_INFO, "");
|
---|
1482 | syslog(LOG_INFO, "Sample System ShutDown");
|
---|
1483 | syslog(LOG_INFO, "ShutDownCode:%s", atk2_strerror(Error));
|
---|
1484 | syslog(LOG_INFO, "");
|
---|
1485 |
|
---|
1486 | if (Error == E_OS_SYS_ASSERT_FATAL) {
|
---|
1487 | syslog(LOG_INFO, "fatal_file_name:%s", fatal_file_name);
|
---|
1488 | syslog(LOG_INFO, "fatal_line_num:%d", fatal_line_num);
|
---|
1489 | }
|
---|
1490 |
|
---|
1491 | TermSerial();
|
---|
1492 | }
|
---|
1493 |
|
---|
1494 | //////////////////////////////////////////////////////////////////////////
|
---|
1495 |
|
---|
1496 | /*
|
---|
1497 | * コマンド受信処理
|
---|
1498 | */
|
---|
1499 | static uint8
|
---|
1500 | GetCommand(void)
|
---|
1501 | {
|
---|
1502 | uint8 command; /* コマンド受信バッファ */
|
---|
1503 |
|
---|
1504 | /*
|
---|
1505 | * コマンドを受信するまでループ
|
---|
1506 | */
|
---|
1507 | command = '\0';
|
---|
1508 | do {
|
---|
1509 | WaitEvent(MainEvt); /* 10msウェイト */
|
---|
1510 | ClearEvent(MainEvt);
|
---|
1511 | RecvPolSerialChar(&command); /* 受信バッファポーリング */
|
---|
1512 | if (command == '\n') {
|
---|
1513 | command = '\0';
|
---|
1514 | }
|
---|
1515 | } while (command == '\0');
|
---|
1516 |
|
---|
1517 |
|
---|
1518 | return(command);
|
---|
1519 | } /* GetCommand */
|
---|
1520 |
|
---|
1521 | /*
|
---|
1522 | * メインタスク
|
---|
1523 | *
|
---|
1524 | * ユーザコマンドの受信と,コマンドごとの処理実行
|
---|
1525 | */
|
---|
1526 | TASK(MainTask)
|
---|
1527 | {
|
---|
1528 | uint8 command;
|
---|
1529 | uint8 task_no;
|
---|
1530 | uint32 i;
|
---|
1531 |
|
---|
1532 | /*
|
---|
1533 | * タスク番号・コマンドバッファ初期化
|
---|
1534 | */
|
---|
1535 | task_no = (uint8) (0);
|
---|
1536 | for (i = 0U; i < (sizeof(command_tbl) / sizeof(command_tbl[0])); i++) {
|
---|
1537 | command_tbl[i] = 0U;
|
---|
1538 | }
|
---|
1539 |
|
---|
1540 | syslog(LOG_INFO, "Main Task : start!");
|
---|
1541 | #if 0
|
---|
1542 | NcanSetMailBoxInfo(0, 0, DIR_RECV, 0, 3, 0, 0);
|
---|
1543 | NcanSetMailBoxInfo(0, 0, DIR_SEND, 0, 4, 0, 0);
|
---|
1544 | NcanEnable(0);
|
---|
1545 | while(1){
|
---|
1546 | WaitEvent(MainEvt); /* 10msの作業時間待ち */
|
---|
1547 | ClearEvent(MainEvt);
|
---|
1548 | NcanGetRxData(0, 0, &rx_data[0], &rx_size );
|
---|
1549 | if (rx_size != 0) {
|
---|
1550 | syslog(LOG_INFO, "Main Task : can receive 0x%x, 0x%x", rx_data[0], rx_data[1]);
|
---|
1551 | }
|
---|
1552 | i++;
|
---|
1553 | if(i == 200){
|
---|
1554 | syslog(LOG_INFO, "Main Task : alive");
|
---|
1555 | i = 0;
|
---|
1556 | tx_data[0] = cnt++;
|
---|
1557 | tx_data[1] = 0x02;
|
---|
1558 | NcanSetTxData(0, 0, 4, tx_data, 2);
|
---|
1559 | }
|
---|
1560 | }
|
---|
1561 | #endif
|
---|
1562 |
|
---|
1563 | /*
|
---|
1564 | * コマンド実行ループ
|
---|
1565 | */
|
---|
1566 | while (1) {
|
---|
1567 | WaitEvent(MainEvt); /* 10msの作業時間待ち */
|
---|
1568 | ClearEvent(MainEvt);
|
---|
1569 |
|
---|
1570 | /*
|
---|
1571 | * 入力コマンド取得
|
---|
1572 | */
|
---|
1573 | syslog(LOG_INFO, "Input Command:");
|
---|
1574 | command = GetCommand();
|
---|
1575 |
|
---|
1576 | /*
|
---|
1577 | * 入力コマンドチェック
|
---|
1578 | */
|
---|
1579 | if ((command <= (uint8) (0x1fU)) || (command >= (uint8) (0x80U))) {
|
---|
1580 | syslog(LOG_INFO, "Not ASCII character");
|
---|
1581 | }
|
---|
1582 | else {
|
---|
1583 | syslog(LOG_INFO, "%c", command);
|
---|
1584 |
|
---|
1585 | /*
|
---|
1586 | * コマンド判別
|
---|
1587 | */
|
---|
1588 | switch (command) {
|
---|
1589 | case '1':
|
---|
1590 | default:
|
---|
1591 | /* 上記のコマンド以外の場合,処理を行わない */
|
---|
1592 | break;
|
---|
1593 | }
|
---|
1594 | }
|
---|
1595 | }
|
---|
1596 |
|
---|
1597 | /*
|
---|
1598 | * ここにはこない
|
---|
1599 | */
|
---|
1600 | syslog(LOG_INFO, "MainTask TERMINATE");
|
---|
1601 | error_log(TerminateTask());
|
---|
1602 | } /* TASK( MainTask ) */
|
---|
1603 |
|
---|
1604 |
|
---|
1605 | /*
|
---|
1606 | * エラーフックルーチン
|
---|
1607 | */
|
---|
1608 | #ifdef CFG_USE_ERRORHOOK
|
---|
1609 | void
|
---|
1610 | ErrorHook(StatusType Error)
|
---|
1611 | {
|
---|
1612 | /*
|
---|
1613 | * エラー要因ごとのパラメータログ出力
|
---|
1614 | */
|
---|
1615 | switch (OSErrorGetServiceId()) {
|
---|
1616 | case OSServiceId_ActivateTask:
|
---|
1617 | syslog(LOG_INFO, "Error:%s=ActivateTask(%d)", atk2_strerror(Error), OSError_ActivateTask_TaskID());
|
---|
1618 | break;
|
---|
1619 | case OSServiceId_TerminateTask:
|
---|
1620 | syslog(LOG_INFO, "Error:%s=TerminateTask()", atk2_strerror(Error));
|
---|
1621 | break;
|
---|
1622 | case OSServiceId_ChainTask:
|
---|
1623 | syslog(LOG_INFO, "Error:%s=ChainTask(%d)", atk2_strerror(Error), OSError_ChainTask_TaskID());
|
---|
1624 | break;
|
---|
1625 | case OSServiceId_Schedule:
|
---|
1626 | syslog(LOG_INFO, "Error:%s=Schedule()", atk2_strerror(Error));
|
---|
1627 | break;
|
---|
1628 | case OSServiceId_GetTaskID:
|
---|
1629 | syslog(LOG_INFO, "Error:%s=GetTaskID(0x%p)", atk2_strerror(Error), OSError_GetTaskID_TaskID());
|
---|
1630 | break;
|
---|
1631 | case OSServiceId_GetTaskState:
|
---|
1632 | syslog(LOG_INFO, "Error:%s=GetTaskState(%d, 0x%p)", atk2_strerror(Error),
|
---|
1633 | OSError_GetTaskState_TaskID(), OSError_GetTaskState_State());
|
---|
1634 | break;
|
---|
1635 | case OSServiceId_EnableAllInterrupts:
|
---|
1636 | syslog(LOG_INFO, "Error:%s=EnableAllInterrupts()", atk2_strerror(Error));
|
---|
1637 | break;
|
---|
1638 | case OSServiceId_DisableAllInterrupts:
|
---|
1639 | syslog(LOG_INFO, "Error:%s=DisableAllInterrupts()", atk2_strerror(Error));
|
---|
1640 | break;
|
---|
1641 | case OSServiceId_ResumeAllInterrupts:
|
---|
1642 | syslog(LOG_INFO, "Error:%s=ResumeAllInterrupts()", atk2_strerror(Error));
|
---|
1643 | break;
|
---|
1644 | case OSServiceId_SuspendAllInterrupts:
|
---|
1645 | syslog(LOG_INFO, "Error:%s=SuspendAllInterrupts()", atk2_strerror(Error));
|
---|
1646 | break;
|
---|
1647 | case OSServiceId_ResumeOSInterrupts:
|
---|
1648 | syslog(LOG_INFO, "Error:%s=ResumeOSInterrupts()", atk2_strerror(Error));
|
---|
1649 | break;
|
---|
1650 | case OSServiceId_SuspendOSInterrupts:
|
---|
1651 | syslog(LOG_INFO, "Error:%s=SuspendOSInterrupts()", atk2_strerror(Error));
|
---|
1652 | break;
|
---|
1653 | case OSServiceId_GetISRID:
|
---|
1654 | syslog(LOG_INFO, "Error:%s=GetISRID()", atk2_strerror(Error));
|
---|
1655 | break;
|
---|
1656 | case OSServiceId_GetResource:
|
---|
1657 | syslog(LOG_INFO, "Error:%s=GetResource(%d)", atk2_strerror(Error), OSError_GetResource_ResID());
|
---|
1658 | break;
|
---|
1659 | case OSServiceId_ReleaseResource:
|
---|
1660 | syslog(LOG_INFO, "Error:%s=ReleaseResource(%d)", atk2_strerror(Error), OSError_ReleaseResource_ResID());
|
---|
1661 | break;
|
---|
1662 | case OSServiceId_SetEvent:
|
---|
1663 | syslog(LOG_INFO, "Error:%s=SetEvent(%d, 0x%x)", atk2_strerror(Error),
|
---|
1664 | OSError_SetEvent_TaskID(), OSError_SetEvent_Mask());
|
---|
1665 | break;
|
---|
1666 | case OSServiceId_ClearEvent:
|
---|
1667 | syslog(LOG_INFO, "Error:%s=ClearEvent(0x%x)", atk2_strerror(Error), OSError_ClearEvent_Mask());
|
---|
1668 | break;
|
---|
1669 | case OSServiceId_GetEvent:
|
---|
1670 | syslog(LOG_INFO, "Error:%s=GetEvent(%d, 0x%p)", atk2_strerror(Error),
|
---|
1671 | OSError_GetEvent_TaskID(), OSError_GetEvent_Event());
|
---|
1672 | break;
|
---|
1673 | case OSServiceId_WaitEvent:
|
---|
1674 | syslog(LOG_INFO, "Error:%s=WaitEvent(0x%x)", atk2_strerror(Error), OSError_WaitEvent_Mask());
|
---|
1675 | break;
|
---|
1676 | case OSServiceId_GetAlarmBase:
|
---|
1677 | syslog(LOG_INFO, "Error:%s=GetAlarmBase(0x%p)", atk2_strerror(Error), OSError_GetAlarmBase_AlarmID());
|
---|
1678 | break;
|
---|
1679 | case OSServiceId_GetAlarm:
|
---|
1680 | syslog(LOG_INFO, "Error:%s=GetAlarm(%d, 0x%p)", atk2_strerror(Error),
|
---|
1681 | OSError_GetAlarm_AlarmID(), OSError_GetAlarm_Tick());
|
---|
1682 | break;
|
---|
1683 | case OSServiceId_SetRelAlarm:
|
---|
1684 | syslog(LOG_INFO, "Error:%s=SetRelAlarm(%d, %d, %d)", atk2_strerror(Error),
|
---|
1685 | OSError_SetRelAlarm_AlarmID(), OSError_SetRelAlarm_increment(), OSError_SetRelAlarm_cycle());
|
---|
1686 | break;
|
---|
1687 | case OSServiceId_SetAbsAlarm:
|
---|
1688 | syslog(LOG_INFO, "Error:%s=SetAbsAlarm(%d, %d, %d)", atk2_strerror(Error),
|
---|
1689 | OSError_SetAbsAlarm_AlarmID(), OSError_SetAbsAlarm_start(), OSError_SetAbsAlarm_cycle());
|
---|
1690 | break;
|
---|
1691 | case OSServiceId_CancelAlarm:
|
---|
1692 | syslog(LOG_INFO, "Error:%s=CancelAlarm(%d)", atk2_strerror(Error), OSError_CancelAlarm_AlarmID());
|
---|
1693 | break;
|
---|
1694 | case OSServiceId_StartScheduleTableRel:
|
---|
1695 | syslog(LOG_INFO, "Error:%s=StartScheduleTableRel(%d, %d)", atk2_strerror(Error),
|
---|
1696 | OSError_StartScheduleTableRel_ScheduleTableID(), OSError_StartScheduleTableRel_Offset());
|
---|
1697 | break;
|
---|
1698 | case OSServiceId_StartScheduleTableAbs:
|
---|
1699 | syslog(LOG_INFO, "Error:%s=StartScheduleTableAbs(%d, %d)", atk2_strerror(Error),
|
---|
1700 | OSError_StartScheduleTableAbs_ScheduleTableID(), OSError_StartScheduleTableAbs_Start());
|
---|
1701 | break;
|
---|
1702 | case OSServiceId_StopScheduleTable:
|
---|
1703 | syslog(LOG_INFO, "Error:%s=StopScheduleTable(%d)", atk2_strerror(Error), OSError_StopScheduleTable_ScheduleTableID());
|
---|
1704 | break;
|
---|
1705 | case OSServiceId_NextScheduleTable:
|
---|
1706 | syslog(LOG_INFO, "Error:%s=NextScheduleTable(%d, %d)", atk2_strerror(Error),
|
---|
1707 | OSError_NextScheduleTable_ScheduleTableID_From(), OSError_NextScheduleTable_ScheduleTableID_To());
|
---|
1708 | break;
|
---|
1709 | case OSServiceId_GetScheduleTableStatus:
|
---|
1710 | syslog(LOG_INFO, "Error:%s=GetScheduleTableStatus(%d, 0x%p)", atk2_strerror(Error),
|
---|
1711 | OSError_GetScheduleTableStatus_ScheduleTableID(), OSError_GetScheduleTableStatus_ScheduleStatus());
|
---|
1712 | break;
|
---|
1713 | case OSServiceId_GetActiveApplicationMode:
|
---|
1714 | syslog(LOG_INFO, "Error:%s=GetActiveApplicationMode()", atk2_strerror(Error));
|
---|
1715 | break;
|
---|
1716 | case OSServiceId_StartOS:
|
---|
1717 | syslog(LOG_INFO, "Error:%s=StartOS()", atk2_strerror(Error));
|
---|
1718 | break;
|
---|
1719 | case OSServiceId_ShutdownOS:
|
---|
1720 | syslog(LOG_INFO, "Error:%s=ShutdownOS()", atk2_strerror(Error));
|
---|
1721 | break;
|
---|
1722 | case OSServiceId_IncrementCounter:
|
---|
1723 | syslog(LOG_INFO, "Error:%s=IncrementCounter(%d)", atk2_strerror(Error), OSError_IncrementCounter_CounterID());
|
---|
1724 | break;
|
---|
1725 | case OSServiceId_TaskMissingEnd:
|
---|
1726 | syslog(LOG_INFO, "Error:%s=MissingEnd()", atk2_strerror(Error));
|
---|
1727 | break;
|
---|
1728 | default:
|
---|
1729 | syslog(LOG_INFO, "Error:%s=UnKnownFunc()", atk2_strerror(Error));
|
---|
1730 | break;
|
---|
1731 | }
|
---|
1732 |
|
---|
1733 | } /* ErrorHook */
|
---|
1734 | #endif /* CFG_USE_ERRORHOOK */
|
---|
1735 |
|
---|
1736 | /*
|
---|
1737 | * プレタスクフックルーチン
|
---|
1738 | *
|
---|
1739 | * 空ルーチンを呼出す
|
---|
1740 | */
|
---|
1741 | #ifdef CFG_USE_PRETASKHOOK
|
---|
1742 | void
|
---|
1743 | PreTaskHook(void)
|
---|
1744 | {
|
---|
1745 | } /* PreTaskHook */
|
---|
1746 | #endif /* CFG_USE_PRETASKHOOK */
|
---|
1747 |
|
---|
1748 | /*
|
---|
1749 | * ポストタスクフックルーチン
|
---|
1750 | *
|
---|
1751 | * 空ルーチンを呼出す
|
---|
1752 | */
|
---|
1753 | #ifdef CFG_USE_POSTTASKHOOK
|
---|
1754 | void
|
---|
1755 | PostTaskHook(void)
|
---|
1756 | {
|
---|
1757 | } /* PostTaskHook */
|
---|
1758 | #endif /* CFG_USE_POSTTASKHOOK */
|
---|
1759 |
|
---|
1760 |
|
---|
1761 | /*
|
---|
1762 | * プロテクションフックルーチン
|
---|
1763 | */
|
---|
1764 | #ifdef CFG_USE_PROTECTIONHOOK
|
---|
1765 | ProtectionReturnType
|
---|
1766 | ProtectionHook(StatusType FatalError)
|
---|
1767 | {
|
---|
1768 | StatusType ercd;
|
---|
1769 |
|
---|
1770 | syslog(LOG_INFO, "");
|
---|
1771 | syslog(LOG_INFO, "ProtectionHook");
|
---|
1772 |
|
---|
1773 | if (FatalError == E_OS_STACKFAULT) {
|
---|
1774 | syslog(LOG_INFO, "E_OS_STACKFAULT");
|
---|
1775 | ercd = PRO_SHUTDOWN;
|
---|
1776 | }
|
---|
1777 | else if (FatalError == E_OS_PROTECTION_EXCEPTION) {
|
---|
1778 | syslog(LOG_INFO, "E_OS_PROTECTION_EXCEPTION");
|
---|
1779 | ercd = PRO_IGNORE;
|
---|
1780 | }
|
---|
1781 | else {
|
---|
1782 | ercd = PRO_SHUTDOWN;
|
---|
1783 | }
|
---|
1784 |
|
---|
1785 | return(ercd);
|
---|
1786 | }
|
---|
1787 | #endif /* CFG_USE_PROTECTIONHOOK */
|
---|