1 | /*
|
---|
2 | * Copyright (C) 2014-2015 by Embedded and Real-Time Systems Laboratory
|
---|
3 | * Graduate School of Information Science, Nagoya Univ., JAPAN
|
---|
4 | * Copyright (C) 2014-2015 by FUJI SOFT INCORPORATED, JAPAN
|
---|
5 | *
|
---|
6 | * 上記著作権者は,以下の(1)〜(4)の条件を満たす場合に限り,本ソフトウェ
|
---|
7 | * ア(本ソフトウェアを改変したものを含む.以下同じ)を使用・複製・改
|
---|
8 | * 変・再配布(以下,利用と呼ぶ)することを無償で許諾する.
|
---|
9 | * (1) 本ソフトウェアをソースコードの形で利用する場合には,上記の著作
|
---|
10 | * 権表示,この利用条件および下記の無保証規定が,そのままの形でソー
|
---|
11 | * スコード中に含まれていること.
|
---|
12 | * (2) 本ソフトウェアを,ライブラリ形式など,他のソフトウェア開発に使
|
---|
13 | * 用できる形で再配布する場合には,再配布に伴うドキュメント(利用
|
---|
14 | * 者マニュアルなど)に,上記の著作権表示,この利用条件および下記
|
---|
15 | * の無保証規定を掲載すること.
|
---|
16 | * (3) 本ソフトウェアを,機器に組み込むなど,他のソフトウェア開発に使
|
---|
17 | * 用できない形で再配布する場合には,次のいずれかの条件を満たすこ
|
---|
18 | * と.
|
---|
19 | * (a) 再配布に伴うドキュメント(利用者マニュアルなど)に,上記の著
|
---|
20 | * 作権表示,この利用条件および下記の無保証規定を掲載すること.
|
---|
21 | * (b) 再配布の形態を,別に定める方法によって,TOPPERSプロジェクトに
|
---|
22 | * 報告すること.
|
---|
23 | * (4) 本ソフトウェアの利用により直接的または間接的に生じるいかなる損
|
---|
24 | * 害からも,上記著作権者およびTOPPERSプロジェクトを免責すること.
|
---|
25 | * また,本ソフトウェアのユーザまたはエンドユーザからのいかなる理
|
---|
26 | * 由に基づく請求からも,上記著作権者およびTOPPERSプロジェクトを
|
---|
27 | * 免責すること.
|
---|
28 | *
|
---|
29 | * 本ソフトウェアは,AUTOSAR(AUTomotive Open System ARchitecture)仕
|
---|
30 | * 様に基づいている.上記の許諾は,AUTOSARの知的財産権を許諾するもので
|
---|
31 | * はない.AUTOSARは,AUTOSAR仕様に基づいたソフトウェアを商用目的で利
|
---|
32 | * 用する者に対して,AUTOSARパートナーになることを求めている.
|
---|
33 | *
|
---|
34 | * 本ソフトウェアは,無保証で提供されているものである.上記著作権者お
|
---|
35 | * よびTOPPERSプロジェクトは,本ソフトウェアに関して,特定の使用目的
|
---|
36 | * に対する適合性も含めて,いかなる保証も行わない.また,本ソフトウェ
|
---|
37 | * アの利用により直接的または間接的に生じたいかなる損害に関しても,そ
|
---|
38 | * の責任を負わない.
|
---|
39 | *
|
---|
40 | * $Id: ModelCarControl.h 49 2015-04-24 05:15:03Z honda $
|
---|
41 | */
|
---|
42 |
|
---|
43 | /*
|
---|
44 | * CDD SW-C OperationManager用の記述
|
---|
45 | */
|
---|
46 |
|
---|
47 | #include "Rte_OperationManager.h"
|
---|
48 | #include "ModelCarControl.h"
|
---|
49 | #include "rcb3.h"
|
---|
50 | #include "t_syslog.h"
|
---|
51 |
|
---|
52 |
|
---|
53 | /* ボディ系情報を送信する回数(20ms * 6 = 120ms) */
|
---|
54 | #define BODY_SEND_COUNT 6U
|
---|
55 |
|
---|
56 | /*
|
---|
57 | * OperationControlランナブル
|
---|
58 | */
|
---|
59 | void
|
---|
60 | OperationControl(void)
|
---|
61 | {
|
---|
62 | Std_ReturnType ercd;
|
---|
63 | IDT_RCB3 rcb3_bstate;
|
---|
64 |
|
---|
65 | IDT_Angle angle;
|
---|
66 | IDT_Speed speed;
|
---|
67 | IDT_Adjust steern;
|
---|
68 | IDT_Adjust gain;
|
---|
69 | IDT_Brake brake = FALSE;
|
---|
70 | IDT_Lamp headlamp = FALSE;
|
---|
71 | IDT_Lamp foglamp = FALSE;
|
---|
72 | IDT_Lamp hazard = FALSE;
|
---|
73 | IDT_Lamp winker_r = FALSE;
|
---|
74 | IDT_Lamp winker_l = FALSE;
|
---|
75 |
|
---|
76 |
|
---|
77 | #ifdef TOPPERS_WITH_DISP
|
---|
78 | IDT_Disp disp;
|
---|
79 | IDT_Cycle cycle;
|
---|
80 | IDT_Switch differ = FALSE;
|
---|
81 | IDT_Switch swich_angle = FALSE;
|
---|
82 | IDT_Switch swich_speed = FALSE;
|
---|
83 | IDT_Switch pwm_angle = FALSE;
|
---|
84 | IDT_Switch pwm_speed = FALSE;
|
---|
85 | #endif /* TOPPERS_WITH_DISP */
|
---|
86 |
|
---|
87 | /* RCB3コントローラから送られて来たRCB3コマンドの取得 */
|
---|
88 | ercd = Rte_Call_RCB3CommandClt_OpGetIfRCB3Command(&rcb3_bstate);
|
---|
89 | if (ercd != RTE_E_OK) {
|
---|
90 | syslog(LOG_INFO, "## Error: Rte_Call_RCB3CommandClt_OpGetIfRCB3Command()");
|
---|
91 | }
|
---|
92 | else if (rcb3_bstate != NOT_RECEIVE_COMMAND) {
|
---|
93 | /* 各ON/OFF切り替え情報 */
|
---|
94 | if ((rcb3_bstate & RCB3_MSG_BUTTON_NOUGHT) != 0U) {
|
---|
95 | headlamp = TRUE;
|
---|
96 | }
|
---|
97 | if ((rcb3_bstate & RCB3_MSG_BUTTON_SQUARE) != 0U) {
|
---|
98 | foglamp = TRUE;
|
---|
99 | }
|
---|
100 | if ((rcb3_bstate & RCB3_MSG_BUTTON_TRIANGLE) != 0U) {
|
---|
101 | hazard = TRUE;
|
---|
102 | }
|
---|
103 | if ((rcb3_bstate & RCB3_MSG_BUTTON_R1) != 0U) {
|
---|
104 | winker_r = TRUE;
|
---|
105 | }
|
---|
106 | if ((rcb3_bstate & RCB3_MSG_BUTTON_L1) != 0U) {
|
---|
107 | winker_l = TRUE;
|
---|
108 | }
|
---|
109 |
|
---|
110 | /* ステアリング(-63〜64に調整) */
|
---|
111 | angle = (-1) * ((IDT_Angle) ((rcb3_bstate >> 16U) & 0xFFU) - 64);
|
---|
112 |
|
---|
113 | /* 前進,後退(-63〜64に調整) */
|
---|
114 | speed = (-1) * ((IDT_Speed) ((rcb3_bstate >> 24U) & 0xFFU) - 64);
|
---|
115 |
|
---|
116 | /* ステアリング調整 */
|
---|
117 | if ((rcb3_bstate & RCB3_MSG_BUTTON_UP) != 0U) {
|
---|
118 | steern = CONTCMD_STEERN_I; /* ステアリングニュートラルを初期値に */
|
---|
119 | }
|
---|
120 | else if ((rcb3_bstate & RCB3_MSG_BUTTON_RIGHT) != 0U) {
|
---|
121 | steern = CONTCMD_STEERN_R; /* ステアリングのニュートラルを右に調整 */
|
---|
122 | }
|
---|
123 | else if ((rcb3_bstate & RCB3_MSG_BUTTON_LEFT) != 0U) {
|
---|
124 | steern = CONTCMD_STEERN_L; /* ステアリングのニュートラルを左に調整 */
|
---|
125 | }
|
---|
126 | else {
|
---|
127 | steern = 0U; /* 要求なし */
|
---|
128 | }
|
---|
129 |
|
---|
130 | /* ゲイン調整 */
|
---|
131 | if ((rcb3_bstate & RCB3_MSG_BUTTON_DOWN) != 0U) {
|
---|
132 | gain = CONTCMD_GAIN_I; /* モータゲインを初期値にする */
|
---|
133 | }
|
---|
134 | else if ((rcb3_bstate & RCB3_MSG_BUTTON_R2) != 0U) {
|
---|
135 | gain = CONTCMD_GAIN_U; /* モータゲインを1段階上げる */
|
---|
136 | }
|
---|
137 | else if ((rcb3_bstate & RCB3_MSG_BUTTON_L2) != 0U) {
|
---|
138 | gain = CONTCMD_GAIN_D; /* モータゲインを1段階下げる */
|
---|
139 | }
|
---|
140 | else {
|
---|
141 | gain = 0U; /* 要求なし */
|
---|
142 | }
|
---|
143 |
|
---|
144 | /* ブレーキ */
|
---|
145 | if ((rcb3_bstate & RCB3_MSG_BUTTON_CROSS) != 0U) {
|
---|
146 | brake = TRUE;
|
---|
147 | }
|
---|
148 |
|
---|
149 | /* IfDrivingへ送信 */
|
---|
150 | Rte_Write_DrivingOut_angle(angle);
|
---|
151 | Rte_Write_DrivingOut_speed(speed);
|
---|
152 | Rte_Write_DrivingOut_steern(steern);
|
---|
153 | Rte_Write_DrivingOut_gain(gain);
|
---|
154 | Rte_Write_DrivingOut_brake(brake);
|
---|
155 |
|
---|
156 | /* IfBodyへの送信 */
|
---|
157 | Rte_Write_BodyOut_headlamp(headlamp);
|
---|
158 | Rte_Write_BodyOut_foglamp(foglamp);
|
---|
159 | Rte_Write_BodyOut_hazard(hazard);
|
---|
160 | Rte_Write_BodyOut_winker_r(winker_r);
|
---|
161 | Rte_Write_BodyOut_winker_l(winker_l);
|
---|
162 | }
|
---|
163 |
|
---|
164 |
|
---|
165 | #ifdef TOPPERS_WITH_DISP
|
---|
166 | /* 状態表示要求 */
|
---|
167 | ercd = Rte_Call_DisplayCommandClt_OpGetDisplayCommand(&disp);
|
---|
168 | if (ercd != RTE_E_OK) {
|
---|
169 | syslog(LOG_INFO, "## Error: Rte_Call_DisplayCommandClt_OpGetDisplayCommand()");
|
---|
170 | }
|
---|
171 | else if (disp != NOT_RECEIVE_COMMAND) {
|
---|
172 | /* 表示周期 */
|
---|
173 | cycle = disp & 0xFFFFU;
|
---|
174 |
|
---|
175 | /* 差分がある場合のみ表示するかどうか */
|
---|
176 | if ((disp & 0x00010000U) != 0U) {
|
---|
177 | differ = TRUE;
|
---|
178 | }
|
---|
179 |
|
---|
180 | /* 各ON/OFF切り替え情報 */
|
---|
181 | if ((disp & 0x00020000U) != 0U) {
|
---|
182 | swich_angle = TRUE;
|
---|
183 | }
|
---|
184 | if ((disp & 0x00040000U) != 0U) {
|
---|
185 | swich_speed = TRUE;
|
---|
186 | }
|
---|
187 | if ((disp & 0x00080000U) != 0U) {
|
---|
188 | pwm_angle = TRUE;
|
---|
189 | }
|
---|
190 | if ((disp & 0x00100000U) != 0U) {
|
---|
191 | pwm_speed = TRUE;
|
---|
192 | }
|
---|
193 |
|
---|
194 | /* IfDisplayModeへ送信 */
|
---|
195 | Rte_Write_DisplayModeOut_cycle(cycle);
|
---|
196 | Rte_Write_DisplayModeOut_differ(differ);
|
---|
197 | Rte_Write_DisplayModeOut_angle(swich_angle);
|
---|
198 | Rte_Write_DisplayModeOut_speed(swich_speed);
|
---|
199 | Rte_Write_DisplayModeOut_pwm_angle(pwm_angle);
|
---|
200 | Rte_Write_DisplayModeOut_pwm_speed(pwm_speed);
|
---|
201 | }
|
---|
202 | #endif /* TOPPERS_WITH_DISP */
|
---|
203 | }
|
---|