source: rc_os_nios2/atk2-sc1_1.3.2/obj/nios2_dev_rc/rc_car.c@ 128

Last change on this file since 128 was 128, checked in by ertl-honda, 9 years ago

追加．
File size: 38.3 KB
Line 
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 */
139static volatile uint8 command_tbl[8]; /* コマンド引渡しテーブル */
140
141/*
142 * ファイル名，行番号の参照用の変数
143 */
144extern const char8 *fatal_file_name; /* ファイル名 */
145extern 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
167void
168set_led(uint8 pattern){
169 sil_wrw_iop((void *)LED_PIO_BASE, pattern);
170}
171
172uint8
173get_dipsw(void){
174 return (uint8)sil_rew_iop((void *)DIPSW_PIO_BASE);
175}
176
177volatile boolean IsBodyOnECU = FALSE;
178volatile boolean IsOpeOnECU = FALSE;
179volatile boolean IsCntOnECU = FALSE;
180
181void
182appmode_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 */
197sint32
198main(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 */
268typedef 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 */
290volatile BODY_CONT_INFO g_body_cont_info;
291
292void
293set_tlu01(unsigned int pattern){
294 sil_wrw_iop((void *)TLU01_PIO_BASE, pattern);
295}
296
297unsigned int
298get_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
317static 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
328void
329tlu01_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 */
354void
355tlu01_init(void)
356{
357 /* 全消灯 */
358 sil_wrw_iop((void *)TLU01_PIO_BASE, 0x00);
359}
360
361#define BLINK_CYCLE_MS 500
362
363volatile int buzzer_state = 0;
364
365void
366UnpackBodyContCanMsg(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
380TASK(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
446ALARMCALLBACK(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 */
486volatile boolean UpDateCntrolCmd = FALSE;
487
488/*
489 * 操作タスク
490 */
491TASK(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
523float g_steer_neutral = STEER_NEUTRAL_INIT;
524float g_angle = 0;
525
526typedef enum{
527 SHIFT_N,
528 SHIFT_D,
529 SHIFT_R,
530}SHIFT_STATE;
531
532/*
533 * 制御関連の変数
534 */
535volatile boolean g_blinker_l_blink = FALSE;
536volatile boolean g_blinker_r_blink = FALSE;
537volatile boolean g_isBrake = FALSE;
538volatile int g_accelerator = 0;
539volatile int g_speed_gain = SPEED_GAIN_INIT;
540volatile SHIFT_STATE g_ShiftState = SHIFT_N;
541
542/*
543 * PS3コントローラのアナログボタンのパラメータ
544 */
545#define PS3ANALOG_MAX 63
546#define PS3ANALOG_MIN -64
547
548
549void
550AdjustSteerNeturalL(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 */
562void
563AdjustSteerNeturalR(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 */
576void
577onPS3AnalogL_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 */
601void
602onPS3AnalogL_UD(void)
603{
604
605}
606
607/*
608 * 右アナログコントローラの左右動作
609 */
610void
611onPS3AnalogR_LR(void)
612{
613
614}
615
616/*
617 * 右アナログコントローラの上下動作
618 * 上 - 方向
619 * 下 + 方向
620 */
621void
622onPS3AnalogR_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 */
648void
649onPS3ButtonCrossEdgeOn(void) {
650 g_isBrake = TRUE;
651// SetDriveSpeed(0);
652 g_body_cont_info.breaklamp = BODY_CONT_ON;
653}
654
655
656void
657onPS3ButtonCrossEdgeOff(void) {
658 g_isBrake = FALSE;
659 g_body_cont_info.breaklamp = BODY_CONT_OFF;
660}
661
662/*
663 * △ : ハザード
664 */
665void
666onPS3ButtonTriangleEdgeOn(void) {
667 TOGGLE_ON_OFF(g_body_cont_info.hazardlamp);
668}
669
670void
671onPS3ButtonTriangleEdgeOff(void) {
672
673}
674
675/*
676 * ○ : メインヘッドライト(ヘッドライト1) 切り替え
677 */
678void
679onPS3ButtonNoughtEdgeOn(void) {
680 TOGGLE_ON_OFF(g_body_cont_info.headlight1);
681}
682
683void
684onPS3ButtonNoughtEdgeOff(void) {
685
686}
687
688/*
689 * □ : ギア切り替え
690 */
691void
692onPS3ButtonSquareEdgeOn(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
702void
703onPS3ButtonSquareEdgeOff(void) {
704
705}
706
707/*
708 * 左ボタン : ステアリングのニュートラルを調整
709 */
710void
711onPS3ButtonLeftEdgeOn(void) {
712 AdjustSteerNeturalL();
713 g_body_cont_info.blinker_l = BODY_CONT_ON;
714}
715
716void
717onPS3ButtonLeftEdgeOff(void) {
718 g_body_cont_info.blinker_l = (g_blinker_l_blink)? BODY_CONT_BLINK : BODY_CONT_OFF;
719}
720
721/*
722 * 右ボタン : ステアリングのニュートラルを調整
723 */
724void
725onPS3ButtonRightEdgeOn(void) {
726 AdjustSteerNeturalR();
727 g_body_cont_info.blinker_r = BODY_CONT_ON;
728}
729
730void
731onPS3ButtonRightEdgeOff(void) {
732 g_body_cont_info.blinker_r = (g_blinker_r_blink)? BODY_CONT_BLINK : BODY_CONT_OFF;
733}
734
735/*
736 * 上ボタン : ステアリングをニュートラルにする
737 */
738void
739onPS3ButtonUpEdgeOn(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
762void
763onPS3ButtonUpEdgeOff(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 */
771void
772onPS3ButtonDownEdgeOn(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
802void
803onPS3ButtonDownEdgeOff(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 */
814void
815onPS3ButtonL1EdgeOn(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
831void
832onPS3ButtonL1EdgeOff(void) {
833
834}
835
836void
837onPS3ButtonR1EdgeOn(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
853void
854onPS3ButtonR1EdgeOff(void) {
855
856}
857
858/*
859 * L2/R2 : モータ制御のゲインを変更
860 */
861void
862onPS3ButtonL2EdgeOn(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
873void
874onPS3ButtonL2EdgeOff(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
880void
881onPS3ButtonR2EdgeOn(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
893void
894onPS3ButtonR2EdgeOff(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 */
910void
911onMsgPS3(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 */
998boolean g_body_can_msg_cyclic = FALSE;
999
1000
1001void
1002body_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 */
1046void
1047body_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}
1080void
1081rcb3_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
1086void
1087com_error(){
1088 syslog(LOG_NOTICE, "com error");
1089}
1090
1091/*
1092 * 制御タスク
1093 */
1094TASK(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
1168void
1169servo_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
1180void
1181RCPowerTrainTaskWait_20msec(void){
1182 ClearEvent(RCPowerTrainEvt);
1183 WaitEvent(RCPowerTrainEvt);
1184 ClearEvent(RCPowerTrainEvt);
1185 WaitEvent(RCPowerTrainEvt);
1186 ClearEvent(RCPowerTrainEvt);
1187}
1188
1189void
1190esc_setspeed(int speed){
1191 sil_wrw_iop((void *)PWM2_ADDR, M100NSEC_TO_PWMCNT(speed));
1192}
1193
1194void
1195esc_dbrak(void){
1196 esc_setspeed(ESC_BREAK);
1197}
1198
1199void
1200esc_rbrak(void){
1201 esc_setspeed(ESC_DTH);
1202 RCPowerTrainTaskWait_20msec();
1203 RCPowerTrainTaskWait_20msec();
1204 esc_setspeed(ESC_BREAK);
1205}
1206
1207void
1208esc_neutral(void){
1209 esc_setspeed(ESC_N);
1210}
1211
1212void
1213esc_reverse(void){
1214 esc_neutral();
1215 RCPowerTrainTaskWait_20msec();
1216 RCPowerTrainTaskWait_20msec();
1217}
1218
1219void
1220esc_change_drive_to_reverse(void){
1221 esc_setspeed(ESC_BREAK);
1222 RCPowerTrainTaskWait_20msec();
1223 esc_neutral();
1224 RCPowerTrainTaskWait_20msec();
1225}
1226
1227TASK(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/*
1253SERVO_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 */
1358static uint8 led0_enable = FALSE;
1359static uint8 led7_enable = FALSE;
1360
1361void
1362set_led7(boolean enable){
1363 led7_enable = enable;
1364}
1365
1366void
1367set_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
1375int led_duty[8];
1376
1377ALARMCALLBACK(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 */
1446void
1447can_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 */
1463void
1464StartupHook(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 */
1477void
1478ShutdownHook(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 */
1499static uint8
1500GetCommand(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 */
1526TASK(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
1609void
1610ErrorHook(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
1742void
1743PreTaskHook(void)
1744{
1745} /* PreTaskHook */
1746#endif /* CFG_USE_PRETASKHOOK */
1747
1748/*
1749 * ポストタスクフックルーチン
1750 *
1751 * 空ルーチンを呼出す
1752 */
1753#ifdef CFG_USE_POSTTASKHOOK
1754void
1755PostTaskHook(void)
1756{
1757} /* PostTaskHook */
1758#endif /* CFG_USE_POSTTASKHOOK */
1759
1760
1761/*
1762 * プロテクションフックルーチン
1763 */
1764#ifdef CFG_USE_PROTECTIONHOOK
1765ProtectionReturnType
1766ProtectionHook(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 */
Note: See TracBrowser for help on using the repository browser.