source: rc_autosar_rh850/trunk/swc-application/common/swcs/DriveManager.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: 9.0 KB
Line 
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: DriveManager.c 122 2015-07-17 01:31:28Z ertl-honda $
41 */
42
43/*
44 * CDD SW-C DriveManager用の記述
45 */
46
47#include "Rte_DriveManager.h"
48#include "ModelCarControl.h"
49#include "t_syslog.h"
50
51/*
52 * ステアリングのニュートラル値
53 */
54static IDT_Angle steer_neutral = STEER_NEUTRAL_INIT;
55
56/*
57 * 車速のゲイン
58 */
59static uint8 speed_gain = SPEED_GAIN_INIT;
60
61/*
62 * ステアリングニュートラルや車速ゲイン調整時のACK通知時間
63 */
64#define ACK_CNT_MAX 8U
65
66/*
67 * ステアリングニュートラルや車速ゲイン調整時のACK通知用変数
68 */
69static uint8 acklr_cnt = 0U;
70static uint8 ackr_cnt = 0U;
71static uint8 ackl_cnt = 0U;
72
73/*
74 * 現在値
75 * ランナブル間で共有するために共有変数として宣言
76 */
77static IDT_Angle cur_angle;
78static IDT_Speed cur_speed;
79static IDT_Adjust cur_steern;
80static IDT_Adjust cur_gain;
81static IDT_Brake cur_brake;
82
83/*
84 * DrivingControlランナブル
85 */
86void
87DrivingControl(void)
88{
89 Std_ReturnType ercd;
90
91 /* IfDriving情報取得 */
92 ercd = Rte_Read_DrivingIn_angle(&cur_angle);
93 ercd += Rte_Read_DrivingIn_speed(&cur_speed);
94 ercd += Rte_Read_DrivingIn_steern(&cur_steern);
95 ercd += Rte_Read_DrivingIn_gain(&cur_gain);
96 ercd += Rte_Read_DrivingIn_brake(&cur_brake);
97
98 /* 1つでも取得に失敗したら後続の処理は実行しない */
99 if (ercd != RTE_E_OK) {
100 syslog(LOG_INFO, "## Error: Rte_Read_DrivingIn()");
101 return;
102 }
103
104 /* ステアリングニュートラル調整 */
105 switch (cur_steern) {
106 case CONTCMD_STEERN_I:
107 steer_neutral = STEER_NEUTRAL_INIT;
108 acklr_cnt = ACK_CNT_MAX;
109 break;
110 case CONTCMD_STEERN_R:
111 /* 右(-)へ調整 */
112 if (steer_neutral > STEER_NEUTRAL_MIN) {
113 steer_neutral--;
114 }
115 ackr_cnt = ACK_CNT_MAX;
116 break;
117 case CONTCMD_STEERN_L:
118 /* 左(+)へ調整 */
119 if (steer_neutral < STEER_NEUTRAL_MAX) {
120 steer_neutral++;
121 }
122 ackl_cnt = ACK_CNT_MAX;
123 break;
124 default:
125 /* 要求なし */
126 break;
127 }
128
129 /* ゲイン調整 */
130 switch (cur_gain) {
131 case CONTCMD_GAIN_I:
132 speed_gain = SPEED_GAIN_INIT;
133 acklr_cnt = ACK_CNT_MAX;
134 break;
135 case CONTCMD_GAIN_U:
136 /* ゲインを上げる */
137 if (speed_gain < SPEED_GAIN_MAX) {
138 speed_gain++;
139 }
140 ackr_cnt = ACK_CNT_MAX;
141 break;
142 case CONTCMD_GAIN_D:
143 /* ゲインを下げる */
144 if (speed_gain > SPEED_GAIN_MIN) {
145 speed_gain--;
146 }
147 ackl_cnt = ACK_CNT_MAX;
148 break;
149 default:
150 /* 要求なし */
151 break;
152 }
153
154 /* 操舵角送信(-63〜64) */
155 Rte_Call_AngleClt_OpSetSteerAngle(cur_angle + steer_neutral);
156
157 /* 車速送信(-63〜64) */
158 Rte_Call_SpeedClt_OpSetDriveSpeed((cur_speed * speed_gain) / SPEED_GAIN_MID);
159
160 /* ブレーキ送信 */
161 Rte_Call_BrakeClt_OpSetBrake(cur_brake);
162}
163
164/*
165 * 初回起動時のボディ系の確認の点滅終了までのカウント値
166 */
167#define START_CNT_MAX ((INIT_BODY_BLINK_CYCLE_MS*INIT_BODY_BLINK_COUNT*2)/BODYCONTROL_RUNNABLE_CYCLE_MS)
168
169/*
170 * BodyControlランナブル
171 */
172void
173BodyControl(void)
174{
175 Std_ReturnType ercd;
176 IDT_Lamp headlamp;
177 IDT_Lamp foglamp;
178 IDT_Lamp hazard;
179 IDT_Lamp winker_r;
180 IDT_Lamp winker_l;
181 static uint8 start_cnt = 0U;
182 static IDT_Lamp toggle_headlamp = FALSE;
183 static IDT_Lamp toggle_foglamp = FALSE;
184 static IDT_Lamp toggle_winker_r = FALSE;
185 static IDT_Lamp toggle_winker_l = FALSE;
186 static IDT_Lamp toggle_hazard = FALSE;
187 static IDT_Lamp pre_headlamp = FALSE;
188 static IDT_Lamp pre_foglamp = FALSE;
189 static IDT_Lamp pre_hazard = FALSE;
190 static IDT_Lamp pre_winker_r = FALSE;
191 static IDT_Lamp pre_winker_l = FALSE;
192
193 /* 初回起動時処理 */
194 if (start_cnt <= START_CNT_MAX) {
195 /* 左右のWinker，全Lamp点滅(head，fog，brake，back) */
196 if ((((start_cnt / (START_CNT_MAX/(INIT_BODY_BLINK_COUNT*2))) % 2U) == 0U)) {
197 Rte_Write_LampOut_headlamp(TRUE);
198 Rte_Write_LampOut_foglamp(TRUE);
199 Rte_Write_LampOut_backlamp(TRUE);
200 Rte_Write_LampOut_brakelamp(TRUE);
201 Rte_Write_WinkerOut_state(WINKER_LR_ON);
202 Rte_Write_BuzzerOut_state(BUZZER_ON);
203 }
204 else {
205 Rte_Write_LampOut_headlamp(FALSE);
206 Rte_Write_LampOut_foglamp(FALSE);
207 Rte_Write_LampOut_backlamp(FALSE);
208 Rte_Write_LampOut_brakelamp(FALSE);
209 Rte_Write_WinkerOut_state(WINKER_OFF);
210 Rte_Write_BuzzerOut_state(BUZZER_OFF);
211 }
212 /* 起動カウントインクリメント */
213 start_cnt++;
214 }
215 /* 通常起動時処理 */
216 else {
217 /* IfBody情報取得 */
218 ercd = Rte_Read_BodyIn_headlamp(&headlamp);
219 ercd += Rte_Read_BodyIn_foglamp(&foglamp);
220 ercd += Rte_Read_BodyIn_hazard(&hazard);
221 ercd += Rte_Read_BodyIn_winker_r(&winker_r);
222 ercd += Rte_Read_BodyIn_winker_l(&winker_l);
223
224 /* 1つでも取得に失敗したら後続の処理は実行しない */
225 if (ercd != RTE_E_OK) {
226 syslog(LOG_INFO, "## Error: Rte_Read_BodyIn()");
227 return;
228 }
229
230 /* FALSE→TRUE遷移時に各トグル切り替え */
231 if ((pre_headlamp == FALSE) && (headlamp == TRUE)) {
232 toggle_headlamp = (toggle_headlamp == FALSE) ? TRUE : FALSE;
233 }
234 if ((pre_foglamp == FALSE) && (foglamp == TRUE)) {
235 toggle_foglamp = (toggle_foglamp == FALSE) ? TRUE : FALSE;
236 }
237 if ((pre_hazard == FALSE) && (hazard == TRUE)) {
238 toggle_hazard = (toggle_hazard == FALSE) ? TRUE : FALSE;
239 }
240 if ((pre_winker_r == FALSE) && (winker_r == TRUE)) {
241 toggle_winker_r = (toggle_winker_r == FALSE) ? TRUE : FALSE;
242 }
243 if ((pre_winker_l == FALSE) && (winker_l == TRUE)) {
244 toggle_winker_l = (toggle_winker_l == FALSE) ? TRUE : FALSE;
245 }
246
247 /*
248 * ランプ状態判定
249 */
250 /* ヘッドランプ状態判定 */
251 if (toggle_headlamp == TRUE) {
252 Rte_Write_LampOut_headlamp(TRUE);
253 }
254 else {
255 Rte_Write_LampOut_headlamp(FALSE);
256 }
257
258 /* フォグランプ状態判定 */
259 if (toggle_foglamp == TRUE) {
260 ercd += Rte_Write_LampOut_foglamp(TRUE);
261 }
262 else {
263 ercd += Rte_Write_LampOut_foglamp(FALSE);
264 }
265
266 /* ブレーキランプ状態判定 */
267 if (cur_brake == TRUE) {
268 Rte_Write_LampOut_brakelamp(TRUE);
269 }
270 else {
271 Rte_Write_LampOut_brakelamp(FALSE);
272 }
273
274 /* バックランプ状態判定 */
275 if (cur_speed < 0) {
276 Rte_Write_LampOut_backlamp(TRUE);
277 }
278 else {
279 Rte_Write_LampOut_backlamp(FALSE);
280 }
281
282 /*
283 * ウィンカー情報解析
284 */
285 /* ゲイン，ニュートラル調整中は方向指示 */
286 if ((acklr_cnt != 0U) || (ackr_cnt != 0U) || (ackl_cnt != 0U)) {
287 if (acklr_cnt != 0U) {
288 acklr_cnt--;
289 Rte_Write_WinkerOut_state((acklr_cnt == 0)? WINKER_OFF : WINKER_LR_ON);
290 }
291 if (ackr_cnt != 0U) {
292 ackr_cnt--;
293 Rte_Write_WinkerOut_state((ackr_cnt == 0)? WINKER_OFF : WINKER_R_ON);
294 }
295 if (ackl_cnt != 0U) {
296 ackl_cnt--;
297 Rte_Write_WinkerOut_state((ackl_cnt == 0)? WINKER_OFF : WINKER_L_ON);
298 }
299 }
300 else {
301 /* 左右点滅 */
302 if ((toggle_hazard == TRUE) ||
303 ((toggle_winker_r == TRUE) && (toggle_winker_l == TRUE))) {
304 Rte_Write_WinkerOut_state(WINKER_LR_BLINK);
305 }
306 /* 右のみ点滅 */
307 else if (toggle_winker_r == TRUE) {
308 Rte_Write_WinkerOut_state(WINKER_R_BLINK);
309 }
310 /* 左のみ点滅 */
311 else if (toggle_winker_l == TRUE) {
312 Rte_Write_WinkerOut_state(WINKER_L_BLINK);
313 }
314 /* 消灯 */
315 else {
316 Rte_Write_WinkerOut_state(WINKER_OFF);
317 }
318 }
319
320 /*
321 * Buzzer情報解析
322 */
323 /* 車速がマイナスか判定 */
324 if (cur_speed < 0) {
325 Rte_Write_BuzzerOut_state(BUZZER_INTERVAL_ON);
326 }
327 else {
328 Rte_Write_BuzzerOut_state(BUZZER_OFF);
329 }
330
331 /* 前回値保存 */
332 pre_headlamp = headlamp;
333 pre_foglamp = foglamp;
334 pre_hazard = hazard;
335 pre_winker_r = winker_r;
336 pre_winker_l = winker_l;
337 }
338}
Note: See TracBrowser for help on using the repository browser.