source: rc_autosar_rh850/trunk/os-application/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: 8.6 KB
Line 
1/*
2 * TOPPERS Software
3 * Toyohashi Open Platform for Embedded Real-Time Systems
4 *
5 * Copyright (C) 2014-2015 by Center for Embedded Computing Systems
6 * Graduate School of Information Science, Nagoya Univ., JAPAN
7 * Copyright (C) 2014-2015 by FUJI SOFT INCORPORATED, JAPAN
8 *
9 * 上記著作権者は，以下の(1)〜(4)の条件を満たす場合に限り，本ソフトウェ
10 * ア（本ソフトウェアを改変したものを含む．以下同じ）を使用・複製・改
11 * 変・再配布（以下，利用と呼ぶ）することを無償で許諾する．
12 * (1) 本ソフトウェアをソースコードの形で利用する場合には，上記の著作
13 * 権表示，この利用条件および下記の無保証規定が，そのままの形でソー
14 * スコード中に含まれていること．
15 * (2) 本ソフトウェアを，ライブラリ形式など，他のソフトウェア開発に使
16 * 用できる形で再配布する場合には，再配布に伴うドキュメント（利用
17 * 者マニュアルなど）に，上記の著作権表示，この利用条件および下記
18 * の無保証規定を掲載すること．
19 * (3) 本ソフトウェアを，機器に組み込むなど，他のソフトウェア開発に使
20 * 用できない形で再配布する場合には，次のいずれかの条件を満たすこ
21 * と．
22 * (a) 再配布に伴うドキュメント（利用者マニュアルなど）に，上記の著
23 * 作権表示，この利用条件および下記の無保証規定を掲載すること．
24 * (b) 再配布の形態を，別に定める方法によって，TOPPERSプロジェクトに
25 * 報告すること．
26 * (4) 本ソフトウェアの利用により直接的または間接的に生じるいかなる損
27 * 害からも，上記著作権者およびTOPPERSプロジェクトを免責すること．
28 * また，本ソフトウェアのユーザまたはエンドユーザからのいかなる理
29 * 由に基づく請求からも，上記著作権者およびTOPPERSプロジェクトを
30 * 免責すること．
31 *
32 * 本ソフトウェアは，AUTOSAR（AUTomotive Open System ARchitecture）仕
33 * 様に基づいている．上記の許諾は，AUTOSARの知的財産権を許諾するもので
34 * はない．AUTOSARは，AUTOSAR仕様に基づいたソフトウェアを商用目的で利
35 * 用する者に対して，AUTOSARパートナーになることを求めている．
36 *
37 * 本ソフトウェアは，無保証で提供されているものである．上記著作権者お
38 * よびTOPPERSプロジェクトは，本ソフトウェアに関して，特定の使用目的
39 * に対する適合性も含めて，いかなる保証も行わない．また，本ソフトウェ
40 * アの利用により直接的または間接的に生じたいかなる損害に関しても，そ
41 * の責任を負わない．
42 *
43 * $Id: DriveManager.c 122 2015-07-17 01:31:28Z ertl-honda $
44 */
45
46/*
47 * 制御系関連
48 */
49
50#include "Os.h"
51#include "ModelCarControl.h"
52#include "t_syslog.h"
53
54/*
55 * ステアリングのニュートラル値
56 */
57sint8 steer_neutral = STEER_NEUTRAL_INIT;
58
59/*
60 * 車速のゲイン
61 */
62sint8 speed_gain = SPEED_GAIN_INIT;
63
64/*
65 * ステアリングニュートラルや車速ゲイン調整時のACK通知時間
66 */
67#define ACK_CNT_MAX 8U
68
69/*
70 * ステアリングニュートラルや車速ゲイン調整時のACK通知用変数
71 */
72static uint8 acklr_cnt = 0U;
73static uint8 ackr_cnt = 0U;
74static uint8 ackl_cnt = 0U;
75
76/*
77 * 操舵角と車速の決定
78 */
79void
80DrivingControl(void)
81{
82 /* ステアリングニュートラル調整 */
83 switch (g_contcmd.steern) {
84 case CONTCMD_STEERN_I:
85 steer_neutral = STEER_NEUTRAL_INIT;
86 acklr_cnt = ACK_CNT_MAX;
87 break;
88 case CONTCMD_STEERN_R:
89 /* 右(-)へ調整 */
90 if (steer_neutral > STEER_NEUTRAL_MIN) {
91 steer_neutral--;
92 }
93 ackr_cnt = ACK_CNT_MAX;
94 break;
95 case CONTCMD_STEERN_L:
96 /* 左(+)へ調整 */
97 if (steer_neutral < STEER_NEUTRAL_MAX) {
98 steer_neutral++;
99 }
100 ackl_cnt = ACK_CNT_MAX;
101 break;
102 default:
103 /* 要求なし */
104 break;
105 }
106
107 /* ゲイン調整 */
108 switch (g_contcmd.gain) {
109 case CONTCMD_GAIN_I:
110 speed_gain = SPEED_GAIN_INIT;
111 acklr_cnt = ACK_CNT_MAX;
112 break;
113 case CONTCMD_GAIN_U:
114 /* ゲインを上げる */
115 if (speed_gain < SPEED_GAIN_MAX) {
116 speed_gain++;
117 }
118 ackr_cnt = ACK_CNT_MAX;
119 break;
120 case CONTCMD_GAIN_D:
121 /* ゲインを下げる */
122 if (speed_gain > SPEED_GAIN_MIN) {
123 speed_gain--;
124 }
125 ackl_cnt = ACK_CNT_MAX;
126 break;
127 default:
128 /* 要求なし */
129 break;
130 }
131
132 /* 操舵角決定 */
133 SetSteerAngle(g_contcmd.angle + steer_neutral);
134
135 /* 車速決定 */
136 SetDriveSpeed((g_contcmd.speed * speed_gain) / SPEED_GAIN_MID);
137
138 /* ブレーキ状態設定 */
139 SetBrake(g_contcmd.brake);
140}
141
142/*
143 * 初回起動時のボディ系の確認の点滅終了までのカウント値
144 */
145#define START_CNT_MAX (INIT_BODY_BLINK_CYCLE_MS*INIT_BODY_BLINK_COUNT*2)/DRIVEMANAGER_TASK_CYCLE_MS
146
147void
148BodyControl(void)
149{
150 static uint8 start_cnt = 0U;
151 static boolean toggle_headlamp = FALSE;
152 static boolean toggle_foglamp = FALSE;
153 static boolean toggle_hazard = FALSE;
154 static boolean toggle_winker_r = FALSE;
155 static boolean toggle_winker_l = FALSE;
156 static boolean pre_headlamp = FALSE;
157 static boolean pre_foglamp = FALSE;
158 static boolean pre_hazard = FALSE;
159 static boolean pre_winker_r = FALSE;
160 static boolean pre_winker_l = FALSE;
161 static uint32 canmsg_snd_cycle = 0;
162
163 /* 初回起動時処理 */
164 if (start_cnt <= START_CNT_MAX) {
165 /* 左右のWinker，全Light点滅(head，fog，brake，back) */
166 if ((((start_cnt / (START_CNT_MAX/(INIT_BODY_BLINK_COUNT*2))) % 2U) == 0U)
167 && (start_cnt != START_CNT_MAX)) {
168 g_bodycmd.headlamp = TRUE;
169 g_bodycmd.foglamp = TRUE;
170 g_bodycmd.brakelamp = TRUE;
171 g_bodycmd.backlamp = TRUE;
172 g_bodycmd.winker = WINKER_LR_ON;
173 g_bodycmd.buzzer = BUZZER_ON;
174 canmsg_snd_cycle = 0;
175 }
176 else {
177 g_bodycmd.headlamp = FALSE;
178 g_bodycmd.foglamp = FALSE;
179 g_bodycmd.brakelamp = FALSE;
180 g_bodycmd.backlamp = FALSE;
181 g_bodycmd.winker = WINKER_OFF;
182 g_bodycmd.buzzer = BUZZER_OFF;
183 canmsg_snd_cycle = 0;
184 }
185 /* 起動カウントインクリメント */
186 start_cnt++;
187 }
188 /* 通常起動時処理 */
189 else {
190 /* FALSE→TRUE遷移時に各トグル切り替え */
191 if ((pre_headlamp == FALSE) && (g_contcmd.headlamp == TRUE)) {
192 toggle_headlamp = (toggle_headlamp == FALSE) ? TRUE : FALSE;
193 }
194 if ((pre_foglamp == FALSE) && (g_contcmd.foglamp == TRUE)) {
195 toggle_foglamp = (toggle_foglamp == FALSE) ? TRUE : FALSE;
196 }
197 if ((pre_hazard == FALSE) && (g_contcmd.hazard == TRUE)) {
198 toggle_hazard = (toggle_hazard == FALSE) ? TRUE : FALSE;
199 }
200 if ((pre_winker_r == FALSE) && (g_contcmd.winker_r == TRUE)) {
201 toggle_winker_r = (toggle_winker_r == FALSE) ? TRUE : FALSE;
202 }
203 if ((pre_winker_l == FALSE) && (g_contcmd.winker_l == TRUE)) {
204 toggle_winker_l = (toggle_winker_l == FALSE) ? TRUE : FALSE;
205 }
206
207 /*
208 * ランプ状態判定
209 */
210 /* ヘッドランプ状態判定 */
211 if (toggle_headlamp == TRUE) {
212 g_bodycmd.headlamp = TRUE;
213 }
214 else {
215 g_bodycmd.headlamp = FALSE;
216 }
217
218 /* フォグランプ状態判定 */
219 if (toggle_foglamp == TRUE) {
220 g_bodycmd.foglamp = TRUE;
221 }
222 else {
223 g_bodycmd.foglamp = FALSE;
224 }
225
226 /* ブレーキランプ状態判定 */
227 if (g_contcmd.brake == TRUE) {
228 g_bodycmd.brakelamp = TRUE;
229 }
230 else {
231 g_bodycmd.brakelamp = FALSE;
232 }
233
234 /* バックランプ状態判定 */
235 if (g_contcmd.speed < 0) {
236 g_bodycmd.backlamp = TRUE;
237 }
238 else {
239 g_bodycmd.backlamp = FALSE;
240 }
241
242 /*
243 * ウィンカー状態判定
244 */
245 /* ゲイン，ニュートラル調整中は方向指示 */
246 if ((acklr_cnt != 0U) || (ackr_cnt != 0U) || (ackl_cnt != 0U)) {
247 if (acklr_cnt != 0U) {
248 acklr_cnt--;
249 g_bodycmd.winker = (acklr_cnt == 0)? WINKER_OFF : WINKER_LR_ON;
250 }
251 if (ackr_cnt != 0U) {
252 ackr_cnt--;
253 g_bodycmd.winker = (ackr_cnt == 0)? WINKER_OFF : WINKER_R_ON;
254 }
255 if (ackl_cnt != 0U) {
256 ackl_cnt--;
257 g_bodycmd.winker = (ackl_cnt == 0)? WINKER_OFF : WINKER_L_ON;
258 }
259 }
260 else {
261 /* 左右点滅 */
262 if ((toggle_hazard == TRUE) ||
263 ((toggle_winker_r == TRUE) && (toggle_winker_l == TRUE))) {
264 g_bodycmd.winker = WINKER_LR_BLINK;
265 }
266 /* 右のみ点滅 */
267 else if (toggle_winker_r == TRUE) {
268 g_bodycmd.winker = WINKER_R_BLINK;
269 }
270 /* 左のみ点滅 */
271 else if (toggle_winker_l == TRUE) {
272 g_bodycmd.winker = WINKER_L_BLINK;
273 }
274 /* 消灯 */
275 else {
276 g_bodycmd.winker = WINKER_OFF;
277 }
278 }
279
280 /*
281 * ブザー状態判定
282 */
283 if (g_contcmd.speed < 0) {
284 g_bodycmd.buzzer = BUZZER_INTERVAL_ON;
285 }
286 else {
287 g_bodycmd.buzzer = BUZZER_OFF;
288 }
289
290 /* 前回値保存 */
291 pre_headlamp = g_contcmd.headlamp;
292 pre_foglamp = g_contcmd.foglamp;
293 pre_hazard = g_contcmd.hazard;
294 pre_winker_r = g_contcmd.winker_r;
295 pre_winker_l = g_contcmd.winker_l;
296 }
297
298 /* ボディ系が別ECUの場合はCANで送信 */
299 if (!IsBodyOnECU) {
300 if (canmsg_snd_cycle-- == 0) {
301 SendBodyCmdCanMag();
302 canmsg_snd_cycle = (BODYCMD_CANMSG_SEND_CYCLE_MS/DRIVEMANAGER_TASK_CYCLE_MS);
303 }
304 }
305}
306
307/*
308 * 制御タスク
309 */
310TASK(DriveManagerTask)
311{
312 static boolean executed = FALSE;
313 uint8 can_rx_data[6];
314 uint8 can_rx_size;
315
316 /* 初回起動時のみ実行 */
317 if (executed == FALSE) {
318 syslog(LOG_INFO, "DriveManagerTask : Start!");
319 executed = TRUE;
320 }
321
322 /* 操作系が別ECUの場合はCANで受信 */
323 if (!IsOperatorOnECU){
324 tCanGetRxData(CONTCMD_CANMSG_RXMBOX_ID, CONTCMD_CANMSG_ID,
325 &can_rx_data[0], &can_rx_size);
326 if (can_rx_size != 0) {
327 UnpackContCmdCanMsg(&can_rx_data[0]);
328 }
329 }
330
331 /* 駆動系の制御 */
332 DrivingControl();
333
334 /* ボディ系の制御 */
335 BodyControl();
336
337 TerminateTask();
338}
Note: See TracBrowser for help on using the repository browser.