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