source: rc_autosar_rh850/trunk/os-application/ModelCarControl.c@ 122

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

ID属性の付加．
  • Property svn:keywords set to Id
File size: 11.0 KB
Line 
1/*
2 * TOPPERS Software
3 * Toyohashi Open Platform for Embedded Real-Time Systems
4 * Software
5 *
6 * Copyright (C) 2015 by Center for Embedded Computing Systems
7 * Graduate School of Information Science, Nagoya Univ., JAPAN
8 * Copyright (C) 2014-2015 by FUJI SOFT INCORPORATED, JAPAN
9 *
10 * 上記著作権者は，以下の(1)〜(4)の条件を満たす場合に限り，本ソフトウェ
11 * ア（本ソフトウェアを改変したものを含む．以下同じ）を使用・複製・改
12 * 変・再配布（以下，利用と呼ぶ）することを無償で許諾する．
13 * (1) 本ソフトウェアをソースコードの形で利用する場合には，上記の著作
14 * 権表示，この利用条件および下記の無保証規定が，そのままの形でソー
15 * スコード中に含まれていること．
16 * (2) 本ソフトウェアを，ライブラリ形式など，他のソフトウェア開発に使
17 * 用できる形で再配布する場合には，再配布に伴うドキュメント（利用
18 * 者マニュアルなど）に，上記の著作権表示，この利用条件および下記
19 * の無保証規定を掲載すること．
20 * (3) 本ソフトウェアを，機器に組み込むなど，他のソフトウェア開発に使
21 * 用できない形で再配布する場合には，次のいずれかの条件を満たすこ
22 * と．
23 * (a) 再配布に伴うドキュメント（利用者マニュアルなど）に，上記の著
24 * 作権表示，この利用条件および下記の無保証規定を掲載すること．
25 * (b) 再配布の形態を，別に定める方法によって，TOPPERSプロジェクトに
26 * 報告すること．
27 * (4) 本ソフトウェアの利用により直接的または間接的に生じるいかなる損
28 * 害からも，上記著作権者およびTOPPERSプロジェクトを免責すること．
29 * また，本ソフトウェアのユーザまたはエンドユーザからのいかなる理
30 * 由に基づく請求からも，上記著作権者およびTOPPERSプロジェクトを
31 * 免責すること．
32 *
33 * 本ソフトウェアは，AUTOSAR（AUTomotive Open System ARchitecture）仕
34 * 様に基づいている．上記の許諾は，AUTOSARの知的財産権を許諾するもので
35 * はない．AUTOSARは，AUTOSAR仕様に基づいたソフトウェアを商用目的で利
36 * 用する者に対して，AUTOSARパートナーになることを求めている．
37 *
38 * 本ソフトウェアは，無保証で提供されているものである．上記著作権者お
39 * よびTOPPERSプロジェクトは，本ソフトウェアに関して，特定の使用目的
40 * に対する適合性も含めて，いかなる保証も行わない．また，本ソフトウェ
41 * アの利用により直接的または間接的に生じたいかなる損害に関しても，そ
42 * の責任を負わない．
43 *
44 * $Id: ModelCarControl.c 122 2015-07-17 01:31:28Z ertl-honda $
45 */
46
47/*
48 * RC-CAR制御プログラム
49 */
50
51#include "Os.h"
52#include "t_syslog.h"
53#include "sysmod/serial.h"
54#include "sysmod/banner.h"
55#include "ModelCarControl.h"
56
57/*
58
59○仕様
60
61・概要
62 PS3コントローラでRCカーを制御する．
63
64・PS3コントローラからのコマンド
65 PS3コントローラからの操作要求は，SBDBTのPS3コントローラ/USBゲームパッド
66 UART変換ファームウェア(http://www.runele.com/) により，RCB3形式に変換され，
67 シリアルプロトコルで送られる．
68
69・起動時
70 全てのランプとブザーを0.5秒周期で3秒点滅させる．
71
72・ボタン操舵
73 左・右 ボタン
74 ・ステアリングのニュートラルを調整．
75 ・ボタンを押すと方向指示器がON，離すとOFF
76 上ボタン
77 ・ドライブにする．
78 下ボタン
79 ・リバースににする．
80 L1/R1
81 ・方向指示器をON/OFFする．
82 L2/R2
83 ・モータ制御のゲインを変更
84 ×
85 ・ブレーキ
86 △
87 ・ハザード
88 ○
89 ・メインヘッドライトON/OFF
90 □
91 ・サブメインヘッドライトON/OFF
92
93
94○CANメッセージ仕様
95
96 ●ボディ系コマンドCANメッセージ仕様
97
98 送信周期 : 50ms
99 メッセージID : 16
100 メッセージ長 : 11bit
101 メッセージ :
102 offset bits bit assign
103 ヘッドランプ : 0 : 1 : 0:OFF 1:ON
104 フォグランプ : 1 : 1 : 0:OFF 1:ON
105 ブレーキランプ : 2 : 1 : 0:OFF 1:ON
106 バックランプ : 3 : 1 : 0:OFF 1:ON
107 ブザー : 5 : 2 : 0:OFF 1:ON 2:TOGGLE
108 ウィンカー : 8 : 3 : 0:OFF 1:L_ON 2:R_ON 3:LR_ON 4:L_BLINK 5:R_BLINK 6:LR_BLINK
109
110 ●制御系コマンドCANメッセージ仕様
111
112 送信周期 : メッセージ到達毎
113 メッセージID : 3
114 メッセージ長 : 40bit
115 メッセージ :
116
117 offset bits
118 ステアリング角度 : 0 : 8 : 数値
119 車速 : 8 : 8 : 数値
120 ステアリング
121 ニュートラル : 16 : 8 : 数値
122 車速ゲイン : 24 : 8 : 数値
123 ブレーキ : 32 : 1 : 1:ON 0:OFF
124 ヘッドランプ : 33 : 1 : 1:ON 0:OFF
125 フォグランプ : 34 : 1 : 1:ON 0:OFF
126 Lウィンカー : 35 : 1 : 1:ON 0:OFF
127 Rウィンカー : 36 : 1 : 1:ON 0:OFF
128 ハザード : 37 : 1 : 1:ON 0:OFF
129 */
130
131/*
132 * 各系の動作状況保持変数
133 */
134boolean IsBodyOnECU = FALSE;
135boolean IsOperatorOnECU = FALSE;
136boolean IsControlerOnECU = FALSE;
137
138/*
139 * アプリケーションモードが指定されていない場合の通知処理
140 */
141void
142DoAppModeNone(void)
143{
144 volatile int i;
145
146 led_init();
147 while(1){
148 for(i = 0; i < 1000000; i++);
149 led_out(LED1|LED2|LED3|LED4);
150 for(i = 0; i < 1000000; i++);
151 led_out(LED_OFF);
152 }
153}
154
155/*
156 * アプリケーションモードの取得
157 */
158boolean
159GetAppMode(AppModeType *p_appmode)
160{
161 uint8 dipsw;
162 boolean ret = TRUE;
163
164 switch_dip_init();
165 dipsw = switch_dip_sense();
166
167 switch (dipsw) {
168 case DSW1:
169 *p_appmode = AppMode_Body;
170 break;
171 case DSW2:
172 *p_appmode = AppMode_Cont;
173 break;
174 case DSW3:
175 *p_appmode = AppMode_Ope;
176 break;
177 case DSW1|DSW2:
178 *p_appmode = AppMode_Body_Cont;
179 break;
180 case DSW1|DSW3:
181 *p_appmode = AppMode_Body_Ope;
182 break;
183 case DSW2|DSW3:
184 *p_appmode = AppMode_Cont_Ope;
185 break;
186 case DSW1|DSW2|DSW3:
187 *p_appmode = AppMode_ALL;
188 break;
189 default:
190 ret = FALSE;
191 break;
192 }
193
194 return ret;
195}
196
197/*
198 * ユーザメイン関数
199 *
200 * アプリケーションモードの判断とカーネル起動
201 */
202sint32
203main(void)
204{
205 AppModeType crt_app_mode;
206
207 if(!GetAppMode(&crt_app_mode)){
208 /*
209 * 有効なアプリケーションモードが指定されてない場合
210 */
211 DoAppModeNone();
212 }
213
214 if (crt_app_mode == AppMode_Body) {
215 IsBodyOnECU = TRUE;
216 }
217 else if (crt_app_mode == AppMode_Cont) {
218 IsControlerOnECU = TRUE;
219 }
220 else if (crt_app_mode == AppMode_Ope) {
221 IsOperatorOnECU = TRUE;
222 }
223 else if (crt_app_mode == AppMode_Body_Cont) {
224 IsBodyOnECU = TRUE;
225 IsControlerOnECU = TRUE;
226 }
227 else if (crt_app_mode == AppMode_Body_Ope) {
228 IsBodyOnECU = TRUE;
229 IsOperatorOnECU = TRUE;
230 }
231 else if (crt_app_mode == AppMode_Cont_Ope) {
232 IsControlerOnECU = TRUE;
233 IsOperatorOnECU = TRUE;
234 }
235 else if (crt_app_mode == AppMode_ALL) {
236 IsBodyOnECU = TRUE;
237 IsControlerOnECU = TRUE;
238 IsOperatorOnECU = TRUE;
239 }
240
241 /*
242 * OS起動
243 */
244 StartOS(crt_app_mode);
245
246 while (1) {
247 }
248}
249
250/*
251 * スタートアップフックルーチン
252 */
253void
254StartupHook(void)
255{
256 /*
257 * 共通の初期化
258 */
259 syslog_initialize();
260 syslog_msk_log(LOG_UPTO(LOG_INFO));
261 InitSerial();
262 print_banner();
263 syslog(LOG_INFO, "Model Car Control System Start");
264
265 tCanInit();
266 tCanSetMailBoxInfo(BODYCMD_CANMSG_RXMBOX_ID, 0, BODYCMD_CANMSG_ID, 0xff);
267 tCanSetMailBoxInfo(CONTCMD_CANMSG_RXMBOX_ID, 0, CONTCMD_CANMSG_ID, 0xff);
268 tCanEnable();
269
270 /* LedBlinker関連の初期化 */
271 LedBlinkerInit();
272
273 /* ボディ系に関する初期化 */
274 if(IsBodyOnECU) {
275 BodyControlInit();
276 }
277
278 /* 操作系に関する初期化 */
279 if(IsOperatorOnECU) {
280 OperationInit();
281 }
282
283 /* 制御系に関する初期化 */
284 if(IsControlerOnECU) {
285 RcCarInit();
286 }
287}
288
289/*
290 * シャットダウンフックルーチン
291 */
292void
293ShutdownHook(StatusType Error)
294{
295 /* 終了ログ出力 */
296 syslog(LOG_INFO, "");
297 syslog(LOG_INFO, "Model Car Control System ShutDown");
298 syslog(LOG_INFO, "ShutDownCode:%s", atk2_strerror(Error));
299 syslog(LOG_INFO, "");
300 TermSerial();
301}
302
303/*
304 * ボディ系への状態指示用変数
305 */
306volatile BODYCMD g_bodycmd;
307
308/*
309 * 周期送信とするか
310 */
311boolean g_bodycmd_canmsg_cyclic = FALSE;
312
313/*
314 * ボディ系コマンドをCANメッセージとして送信する
315 */
316void
317SendBodyCmdCanMag(void)
318{
319 uint8 tx_data[2];
320 /* 前回送付の状態 */
321 static BODYCMD g_pre_bodycmd;
322
323 /* 状態指示に変化が無ければ送信しない */
324 if (!g_bodycmd_canmsg_cyclic){
325 if((g_bodycmd.headlamp == g_pre_bodycmd.headlamp) &&
326 (g_bodycmd.foglamp == g_pre_bodycmd.foglamp) &&
327 (g_bodycmd.brakelamp == g_pre_bodycmd.brakelamp)&&
328 (g_bodycmd.backlamp == g_pre_bodycmd.backlamp) &&
329 (g_bodycmd.buzzer == g_pre_bodycmd.buzzer) &&
330 (g_bodycmd.winker == g_pre_bodycmd.winker)) {
331 return;
332 }
333 }
334
335 /* 送信メッセージを生成 */
336 tx_data[0] = ((g_bodycmd.headlamp << 7) & 0x80) |
337 ((g_bodycmd.foglamp << 6) & 0x40) |
338 ((g_bodycmd.brakelamp << 5) & 0x20) |
339 ((g_bodycmd.backlamp << 4) & 0x10) |
340 ((g_bodycmd.buzzer << 2) & 0x0c);
341 tx_data[1] = ((g_bodycmd.winker << 5) & 0xe0);
342
343 tCanSetTxData(BODYCMD_CANMSG_TXMBOX_ID, BODYCMD_CANMSG_ID, tx_data, 2);
344
345 g_pre_bodycmd = g_bodycmd;
346}
347
348/*
349 * ボディ系コマンドのCANメッセージからのアンパック
350 */
351void
352UnpackBodyCmdCanMsg(uint8 *p_rx_data)
353{
354 g_bodycmd.headlamp = (p_rx_data[0] & 0x80)? TRUE : FALSE;
355 g_bodycmd.foglamp = (p_rx_data[0] & 0x40)? TRUE : FALSE;
356 g_bodycmd.brakelamp = (p_rx_data[0] & 0x20)? TRUE : FALSE;
357 g_bodycmd.backlamp = (p_rx_data[0] & 0x10)? TRUE : FALSE;
358 g_bodycmd.buzzer = (p_rx_data[0] & 0x0c) >> 2;
359 g_bodycmd.winker = (p_rx_data[1] & 0xe0) >> 5;
360}
361
362/*
363 * 制御系へのコマンド更新通知用変数(同一ECU時)
364 */
365volatile boolean UpDateContCmd = FALSE;
366
367/*
368 * 制御系への指示用構造体
369 */
370volatile CONTCMD g_contcmd;
371
372/*
373 * 制御系への指示をCANメッセージとして送信
374 */
375void
376SendContCmdCanMag(void)
377{
378 uint8 tx_data[5];
379
380 /* 送信メッセージを生成 */
381 tx_data[0] = g_contcmd.angle;
382 tx_data[1] = g_contcmd.speed;
383 tx_data[2] = g_contcmd.steern;
384 tx_data[3] = g_contcmd.gain;
385 tx_data[4] = ((g_contcmd.brake << 7) & 0x80) |
386 ((g_contcmd.headlamp << 6) & 0x40) |
387 ((g_contcmd.foglamp << 5) & 0x20) |
388 ((g_contcmd.winker_l << 4) & 0x10) |
389 ((g_contcmd.winker_r << 3) & 0x08) |
390 ((g_contcmd.hazard << 2) & 0x04);
391
392 tCanSetTxData(CONTCMD_CANMSG_TXMBOX_ID, CONTCMD_CANMSG_ID, tx_data, 5);
393}
394
395/*
396 * 制御系コマンドのCANメッセージからのアンパック
397 */
398void
399UnpackContCmdCanMsg(uint8 *p_rx_data)
400{
401 g_contcmd.angle = p_rx_data[0];
402 g_contcmd.speed = p_rx_data[1];
403 g_contcmd.steern = p_rx_data[2];
404 g_contcmd.gain = p_rx_data[3];
405
406 g_contcmd.brake = (p_rx_data[4] & 0x80)? TRUE : FALSE;
407 g_contcmd.headlamp = (p_rx_data[4] & 0x40)? TRUE : FALSE;
408 g_contcmd.foglamp = (p_rx_data[4] & 0x20)? TRUE : FALSE;
409 g_contcmd.winker_l = (p_rx_data[4] & 0x10)? TRUE : FALSE;
410 g_contcmd.winker_r = (p_rx_data[4] & 0x08)? TRUE : FALSE;
411 g_contcmd.hazard = (p_rx_data[4] & 0x04)? TRUE : FALSE;
412}
413
414/*
415 * エラーフックルーチン
416 */
417void
418ErrorHook(StatusType Error)
419{
420 TaskType tskid;
421 StatusType ercd;
422
423 /* 多重起動要求エラーは無視 */
424 if ((OSErrorGetServiceId() == OSServiceId_ActivateTask) && (Error == E_OS_LIMIT)) {
425 return;
426 }
427
428 syslog(LOG_NOTICE, "## ErrorHook is called !! (%d)", Error);
429
430 ercd = GetTaskID(&tskid);
431 if (ercd == E_OK) {
432 syslog(LOG_NOTICE, "GetTaskID: %d", (sintptr) tskid);
433 }
434 else {
435 syslog(LOG_NOTICE, "GetTaskID is unavailable.");
436 }
437 syslog(LOG_NOTICE, "GetISRID: %d", (sintptr) GetISRID());
438 syslog(LOG_NOTICE, "OSErrorGetServiceId(): %d", OSErrorGetServiceId());
439
440 ShutdownOS(E_OK);
441
442 return;
443}
444
445/*
446 * プロテクションフックルーチン
447 */
448ProtectionReturnType
449ProtectionHook(StatusType FatalError)
450{
451 StatusType ercd;
452
453 syslog(LOG_INFO, "");
454 syslog(LOG_INFO, "ProtectionHook");
455
456 if (FatalError == E_OS_STACKFAULT) {
457 syslog(LOG_INFO, "E_OS_STACKFAULT");
458 ercd = PRO_SHUTDOWN;
459 }
460 else if (FatalError == E_OS_PROTECTION_EXCEPTION) {
461 syslog(LOG_INFO, "E_OS_PROTECTION_EXCEPTION");
462 ercd = PRO_IGNORE;
463 }
464 else {
465 ercd = PRO_SHUTDOWN;
466 }
467
468 return(ercd);
469}
Note: See TracBrowser for help on using the repository browser.