void fin_task(intptr_t unused) {
ter_tsk(BT_RECV_TASK);
ter_tsk(MAIN_TASK);
ter_tsk(AVOID_TASK);
ter_tsk(COLOR_TASK);
ter_tsk(WALK_TASK);
ev3_motor_stop(sensors->LEFT_P, true);
ev3_motor_stop(sensors->RIGHT_P, true);
}
/** ＴＥＬＮＥＴデーモン・タスクの停止
* @param tskno Telnetタスク番号( 0〜(TELNET_DAEMON_TASK_COUNT-1) )
****************************************************************************** */
ER TELNET_Stop( ID tskno )
{
ER errcd;
errcd = ter_tsk( TELNET_DAEMON_FIRST_TASK + tskno ); /* タスク停止 */
return( errcd );
}
ER GPSRec_Close(void)
{
if (!bGPSRecOpened)
return E_SYS;
#if (RECEIVE_FROM_UART2)
if (uart2_close() == E_OK)
#else
if (uart_close() == E_OK)
#endif
debug_msg(("UART2: close success\r\n"));
else
debug_err(("UART2: close fail!\r\n"));
debug_msg("Terminate GPS receive task\r\n");
ter_tsk(GPSRECEIVE_ID);
loc_cpu();
bGPSRecOpened = FALSE;
unl_cpu();
return E_OK;
}
// メインタスク
void main_task(VP_INT exinf)
{
serial_ctl_por(TASK_PORTID, (IOCTL_CRLF | IOCTL_FCSND | IOCTL_FCRCV));
syslog(LOG_NOTICE,"Sample program starts (exinf = %d)", exinf);
std::atexit(finish);
try
{
for (;;)
{
for (ID semid = 1; semid <= 5; semid++)
p_fork[semid - 1] = new fork(semid);
for (ID tskid = 1; tskid <= 5; tskid++)
act_tsk(tskid);
char c;
do
{
serial_rea_dat(TASK_PORTID, &c, 1);
if (c == 'a')
std::abort();
} while (c != 'q' && c != 'Q');
for (ID tskid = 1; tskid <= 5; tskid++)
{
ter_tsk(tskid);
}
for (ID semid = 1; semid <= 5; semid++)
{
delete p_fork[semid - 1];
p_fork[semid - 1] = 0;
}
do
{
syslog(LOG_NOTICE, "restart? [y|n] ");
serial_rea_dat(TASK_PORTID, &c, 1);
} while (c != 'y' && c != 'n');
if (c == 'n')
break;
}
syslog(LOG_NOTICE, "multitask test succeeded");
}
catch (std::bad_alloc&)
{
syslog(LOG_NOTICE, "multitask test failed");
}
std::exit(0);
}
void
task2(intptr_t exinf)
{
ER_UINT ercd;
check_point(42);
ercd = loc_mtx(MTX4);
check_ercd(ercd, E_OK);
check_point(46);
ercd = unl_mtx(MTX4);
check_ercd(ercd, E_OK);
check_point(55);
ercd = wup_tsk(TASK4);
check_ercd(ercd, E_OK);
check_point(57);
ercd = wup_tsk(TASK5);
check_ercd(ercd, E_OK);
check_point(67);
ercd = wup_tsk(TASK5);
check_ercd(ercd, E_OK);
check_point(70);
ercd = wup_tsk(TASK3);
check_ercd(ercd, E_OK);
check_point(72);
ercd = wup_tsk(TASK1);
check_ercd(ercd, E_OK);
check_point(75);
ercd = wup_tsk(TASK4);
check_ercd(ercd, E_OK);
check_point(77);
ercd = wup_tsk(TASK5);
check_ercd(ercd, E_OK);
check_point(104);
ercd = ini_mtx(MTX1);
check_ercd(ercd, E_OK);
check_point(106);
ercd = ter_tsk(TASK1);
check_ercd(ercd, E_OK);
check_finish(107);
check_point(0);
}
/**
Close Play Sound task
Close Play Sound task.
Return value is listed below:
E_SYS: Task is already closed
E_OK: No error
@param void
@return ER
*/
ER LensCtrl_Close(void)
{
if (bLensCtrlOpened == FALSE)
{
return E_SYS;
}
bLensCtrlOpened = FALSE;
ter_tsk(LENSCTRLTSK_ID);
return E_OK;
}
void
task2(intptr_t exinf)
{
ER ercd;
check_point(5);
check_state(false, false, TIPM_ENAALL, false, false, true);
check_point(6);
ercd = ter_tsk(TASK1);
check_ercd(ercd, E_OK);
check_point(7);
ercd = act_tsk(TASK1);
check_ercd(ercd, E_OK);
check_point(8);
ercd = ext_tsk();
check_ercd(ercd, E_OK);
check_point(0);
}
/*
* メインタスク
*/
void main_task(intptr_t exinf)
{
char c;
ID tskid = TASK1;
int_t tskno = 1;
ER_UINT ercd;
PRI tskpri;
#ifndef TASK_LOOP
volatile ulong_t i;
SYSTIM stime1, stime2;
#endif /* TASK_LOOP */
HRTCNT hrtcnt1, hrtcnt2;
SVC_PERROR(syslog_msk_log(LOG_UPTO(LOG_INFO), LOG_UPTO(LOG_EMERG)));
syslog(LOG_NOTICE, "Sample program starts (exinf = %d).", (int_t) exinf);
/*
* シリアルポートの初期化
*
* システムログタスクと同じシリアルポートを使う場合など，シリアル
* ポートがオープン済みの場合にはここでE_OBJエラーになるが，支障は
* ない．
*/
ercd = serial_opn_por(TASK_PORTID);
if (ercd < 0 && MERCD(ercd) != E_OBJ) {
syslog(LOG_ERROR, "%s (%d) reported by `serial_opn_por'.",
itron_strerror(ercd), SERCD(ercd));
}
SVC_PERROR(serial_ctl_por(TASK_PORTID,
(IOCTL_CRLF | IOCTL_FCSND | IOCTL_FCRCV)));
/*
* ループ回数の設定
*
* 並行実行されるタスク内での空ループの回数（task_loop）は，空ルー
* プの実行時間が約0.4秒になるように設定する．この設定のために，
* LOOP_REF回の空ループの実行時間を，その前後でget_timを呼ぶことで
* 測定し，その測定結果から空ループの実行時間が0.4秒になるループ回
* 数を求め，task_loopに設定する．
*
* LOOP_REFは，デフォルトでは1,000,000に設定しているが，想定したよ
* り遅いプロセッサでは，サンプルプログラムの実行開始に時間がかか
* りすぎるという問題を生じる．逆に想定したより速いプロセッサでは，
* LOOP_REF回の空ループの実行時間が短くなり，task_loopに設定する値
* の誤差が大きくなるという問題がある．
*
* そこで，そのようなターゲットでは，target_test.hで，LOOP_REFを適
* 切な値に定義するのが望ましい．
*
* また，task_loopの値を固定したい場合には，その値をTASK_LOOPにマ
* クロ定義する．TASK_LOOPがマクロ定義されている場合，上記の測定を
* 行わずに，TASK_LOOPに定義された値を空ループの回数とする．
*
* ターゲットによっては，空ループの実行時間の1回目の測定で，本来よ
* りも長めになるものがある．このようなターゲットでは，MEASURE_TWICE
* をマクロ定義することで，1回目の測定結果を捨てて，2回目の測定結果
* を使う．
*/
#ifdef TASK_LOOP
task_loop = TASK_LOOP;
#else /* TASK_LOOP */
#ifdef MEASURE_TWICE
task_loop = LOOP_REF;
SVC_PERROR(get_tim(&stime1));
for (i = 0; i < task_loop; i++);
SVC_PERROR(get_tim(&stime2));
#endif /* MEASURE_TWICE */
task_loop = LOOP_REF;
SVC_PERROR(get_tim(&stime1));
for (i = 0; i < task_loop; i++);
SVC_PERROR(get_tim(&stime2));
task_loop = LOOP_REF * 400LU / (ulong_t)(stime2 - stime1) * 1000LU;
#endif /* TASK_LOOP */
/*
* タスクの起動
*/
SVC_PERROR(act_tsk(TASK1));
SVC_PERROR(act_tsk(TASK2));
SVC_PERROR(act_tsk(TASK3));
/*
* メインループ
*/
do {
SVC_PERROR(serial_rea_dat(TASK_PORTID, &c, 1));
switch (c) {
case 'e':
case 's':
case 'S':
case 'd':
case 'y':
case 'Y':
case 'z':
case 'Z':
message[tskno-1] = c;
break;
case '1':
tskno = 1;
tskid = TASK1;
break;
case '2':
tskno = 2;
tskid = TASK2;
break;
case '3':
tskno = 3;
tskid = TASK3;
break;
case 'a':
syslog(LOG_INFO, "#act_tsk(%d)", tskno);
SVC_PERROR(act_tsk(tskid));
break;
case 'A':
syslog(LOG_INFO, "#can_act(%d)", tskno);
SVC_PERROR(ercd = can_act(tskid));
if (ercd >= 0) {
syslog(LOG_NOTICE, "can_act(%d) returns %d", tskno, ercd);
}
break;
case 't':
syslog(LOG_INFO, "#ter_tsk(%d)", tskno);
SVC_PERROR(ter_tsk(tskid));
break;
case '>':
syslog(LOG_INFO, "#chg_pri(%d, HIGH_PRIORITY)", tskno);
SVC_PERROR(chg_pri(tskid, HIGH_PRIORITY));
break;
case '=':
syslog(LOG_INFO, "#chg_pri(%d, MID_PRIORITY)", tskno);
SVC_PERROR(chg_pri(tskid, MID_PRIORITY));
break;
case '<':
syslog(LOG_INFO, "#chg_pri(%d, LOW_PRIORITY)", tskno);
SVC_PERROR(chg_pri(tskid, LOW_PRIORITY));
break;
case 'G':
syslog(LOG_INFO, "#get_pri(%d, &tskpri)", tskno);
SVC_PERROR(ercd = get_pri(tskid, &tskpri));
if (ercd >= 0) {
syslog(LOG_NOTICE, "priority of task %d is %d", tskno, tskpri);
}
break;
case 'w':
syslog(LOG_INFO, "#wup_tsk(%d)", tskno);
SVC_PERROR(wup_tsk(tskid));
break;
case 'W':
syslog(LOG_INFO, "#can_wup(%d)", tskno);
SVC_PERROR(ercd = can_wup(tskid));
if (ercd >= 0) {
syslog(LOG_NOTICE, "can_wup(%d) returns %d", tskno, ercd);
}
break;
case 'l':
syslog(LOG_INFO, "#rel_wai(%d)", tskno);
SVC_PERROR(rel_wai(tskid));
break;
case 'u':
syslog(LOG_INFO, "#sus_tsk(%d)", tskno);
SVC_PERROR(sus_tsk(tskid));
break;
case 'm':
syslog(LOG_INFO, "#rsm_tsk(%d)", tskno);
SVC_PERROR(rsm_tsk(tskid));
break;
case 'x':
syslog(LOG_INFO, "#ras_ter(%d)", tskno);
SVC_PERROR(ras_ter(tskid));
break;
case 'r':
syslog(LOG_INFO, "#rot_rdq(three priorities)");
SVC_PERROR(rot_rdq(HIGH_PRIORITY));
SVC_PERROR(rot_rdq(MID_PRIORITY));
SVC_PERROR(rot_rdq(LOW_PRIORITY));
break;
case 'c':
syslog(LOG_INFO, "#sta_cyc(1)");
SVC_PERROR(sta_cyc(CYCHDR1));
break;
case 'C':
syslog(LOG_INFO, "#stp_cyc(1)");
SVC_PERROR(stp_cyc(CYCHDR1));
break;
case 'b':
syslog(LOG_INFO, "#sta_alm(1, 5000000)");
SVC_PERROR(sta_alm(ALMHDR1, 5000000));
break;
case 'B':
syslog(LOG_INFO, "#stp_alm(1)");
SVC_PERROR(stp_alm(ALMHDR1));
break;
case 'V':
hrtcnt1 = fch_hrt();
hrtcnt2 = fch_hrt();
syslog(LOG_NOTICE, "hrtcnt1 = %tu, hrtcnt2 = %tu",
hrtcnt1, hrtcnt2);
break;
case 'o':
#ifdef TOPPERS_SUPPORT_OVRHDR
syslog(LOG_INFO, "#sta_ovr(%d, 2000000)", tskno);
SVC_PERROR(sta_ovr(tskid, 2000000));
#else /* TOPPERS_SUPPORT_OVRHDR */
syslog(LOG_NOTICE, "sta_ovr is not supported.");
#endif /* TOPPERS_SUPPORT_OVRHDR */
break;
case 'O':
#ifdef TOPPERS_SUPPORT_OVRHDR
syslog(LOG_INFO, "#stp_ovr(%d)", tskno);
SVC_PERROR(stp_ovr(tskid));
#else /* TOPPERS_SUPPORT_OVRHDR */
syslog(LOG_NOTICE, "stp_ovr is not supported.");
#endif /* TOPPERS_SUPPORT_OVRHDR */
break;
case 'v':
SVC_PERROR(syslog_msk_log(LOG_UPTO(LOG_INFO),
LOG_UPTO(LOG_EMERG)));
break;
case 'q':
SVC_PERROR(syslog_msk_log(LOG_UPTO(LOG_NOTICE),
LOG_UPTO(LOG_EMERG)));
break;
#ifdef BIT_KERNEL
case ' ':
SVC_PERROR(loc_cpu());
{
extern ER bit_kernel(void);
SVC_PERROR(ercd = bit_kernel());
if (ercd >= 0) {
syslog(LOG_NOTICE, "bit_kernel passed.");
}
}
SVC_PERROR(unl_cpu());
break;
#endif /* BIT_KERNEL */
default:
break;
}
} while (c != '\003' && c != 'Q');
syslog(LOG_NOTICE, "Sample program ends.");
SVC_PERROR(ext_ker());
assert(0);
}
void
task1(intptr_t exinf)
{
ER_UINT ercd;
T_RTSK rtsk;
T_RMTX rmtx;
test_start(__FILE__);
set_bit_func(bit_kernel);
check_point(1);
ercd = act_tsk(TASK2);
check_ercd(ercd, E_OK);
ercd = sta_alm(ALM1, TEST_TIME_CP);
check_ercd(ercd, E_OK);
ercd = slp_tsk();
check_ercd(ercd, E_OK);
check_point(4);
ercd = loc_cpu();
check_ercd(ercd, E_OK);
ercd = ras_ter(TASK2);
check_ercd(ercd, E_CTX);
ercd = unl_cpu();
check_ercd(ercd, E_OK);
check_point(5);
ercd = ras_ter(0U);
check_ercd(ercd, E_ID);
ercd = ras_ter(TNUM_TSKID + 1);
check_ercd(ercd, E_ID);
ercd = ras_ter(TASK1);
check_ercd(ercd, E_ILUSE);
ercd = ras_ter(TASK3);
check_ercd(ercd, E_OBJ);
check_point(6);
ercd = ref_tsk(TASK2, &rtsk);
check_ercd(ercd, E_OK);
check_assert(rtsk.tskstat == TTS_RDY);
check_point(7);
ercd = ras_ter(TASK2);
check_ercd(ercd, E_OK);
check_point(8);
ercd = ref_tsk(TASK2, &rtsk);
check_ercd(ercd, E_OK);
check_assert(rtsk.tskstat == TTS_DMT);
check_point(9);
ercd = act_tsk(TASK3);
check_ercd(ercd, E_OK);
check_point(12);
ercd = ref_tsk(TASK3, &rtsk);
check_ercd(ercd, E_OK);
check_assert(rtsk.tskstat == TTS_WAI);
ercd = ref_mtx(MTX1, &rmtx);
check_ercd(ercd, E_OK);
check_assert(rmtx.htskid == TASK3);
check_assert(rmtx.wtskid == TSK_NONE);
check_point(13);
ercd = ras_ter(TASK3);
check_ercd(ercd, E_OK);
check_point(14);
ercd = ref_tsk(TASK3, &rtsk);
check_ercd(ercd, E_OK);
check_assert(rtsk.tskstat == TTS_DMT);
ercd = ref_mtx(MTX1, &rmtx);
check_ercd(ercd, E_OK);
check_assert(rmtx.htskid == TSK_NONE);
check_assert(rmtx.wtskid == TSK_NONE);
check_point(15);
ercd = act_tsk(TASK3);
check_ercd(ercd, E_OK);
check_point(18);
ercd = act_tsk(TASK4);
check_ercd(ercd, E_OK);
check_point(20);
ercd = ref_tsk(TASK3, &rtsk);
check_ercd(ercd, E_OK);
check_assert(rtsk.tskstat == TTS_WAI);
ercd = ref_mtx(MTX1, &rmtx);
check_ercd(ercd, E_OK);
check_assert(rmtx.htskid == TASK3);
check_assert(rmtx.wtskid == TASK4);
check_point(21);
ercd = ras_ter(TASK3);
check_ercd(ercd, E_OK);
check_point(24);
ercd = act_tsk(TASK2);
check_ercd(ercd, E_OK);
ercd = act_tsk(TASK2);
check_ercd(ercd, E_OK);
check_point(25);
ercd = ref_tsk(TASK2, &rtsk);
check_ercd(ercd, E_OK);
check_assert(rtsk.tskstat == TTS_RDY);
check_assert(rtsk.actcnt == 1U);
check_point(26);
ercd = ras_ter(TASK2);
check_ercd(ercd, E_OK);
check_point(27);
ercd = ref_tsk(TASK2, &rtsk);
check_ercd(ercd, E_OK);
check_assert(rtsk.tskstat == TTS_RDY);
check_assert(rtsk.actcnt == 0U);
check_point(28);
ercd = act_tsk(TASK3);
check_ercd(ercd, E_OK);
check_point(30);
ercd = act_tsk(TASK3);
check_ercd(ercd, E_OK);
check_point(31);
ercd = ref_tsk(TASK3, &rtsk);
check_ercd(ercd, E_OK);
check_assert(rtsk.tskstat == TTS_WAI);
check_assert(rtsk.actcnt == 1U);
check_point(32);
ercd = ras_ter(TASK3);
check_ercd(ercd, E_OK);
check_point(34);
ercd = act_tsk(TASK2);
check_ercd(ercd, E_OK);
ercd = slp_tsk();
check_ercd(ercd, E_OK);
check_point(36);
ercd = ref_tsk(TASK2, &rtsk);
check_ercd(ercd, E_OK);
check_assert(rtsk.tskstat == TTS_RDY);
check_assert(rtsk.raster == false);
check_assert(rtsk.dister == true);
check_point(37);
ercd = ras_ter(TASK2);
check_ercd(ercd, E_OK);
check_point(38);
ercd = ref_tsk(TASK2, &rtsk);
check_ercd(ercd, E_OK);
check_assert(rtsk.tskstat == TTS_RDY);
check_assert(rtsk.raster == true);
check_assert(rtsk.dister == true);
check_point(39);
ercd = ter_tsk(TASK2);
check_ercd(ercd, E_OK);
ercd = act_tsk(TASK2);
check_ercd(ercd, E_OK);
check_point(40);
ercd = ref_tsk(TASK2, &rtsk);
check_ercd(ercd, E_OK);
check_assert(rtsk.tskstat == TTS_RDY);
check_assert(rtsk.raster == false);
check_assert(rtsk.dister == false);
ercd = tslp_tsk(TEST_TIME_CP);
check_ercd(ercd, E_TMOUT);
check_point(42);
ercd = ref_tsk(TASK2, &rtsk);
check_ercd(ercd, E_OK);
check_assert(rtsk.tskstat == TTS_WAI);
check_assert(rtsk.raster == false);
check_assert(rtsk.dister == true);
check_point(43);
ercd = ras_ter(TASK2);
check_ercd(ercd, E_OK);
check_point(44);
ercd = ref_tsk(TASK2, &rtsk);
check_ercd(ercd, E_OK);
check_assert(rtsk.tskstat == TTS_RDY);
check_assert(rtsk.raster == true);
check_assert(rtsk.dister == true);
ercd = slp_tsk();
check_ercd(ercd, E_OK);
check_point(46);
ercd = act_tsk(TASK3);
check_ercd(ercd, E_OK);
check_point(48);
ercd = ref_tsk(TASK3, &rtsk);
check_ercd(ercd, E_OK);
check_assert(rtsk.tskstat == TTS_WAI);
check_assert(rtsk.raster == false);
check_assert(rtsk.dister == true);
check_point(49);
ercd = ras_ter(TASK3);
check_ercd(ercd, E_OK);
check_point(52);
ercd = ref_tsk(TASK3, &rtsk);
check_ercd(ercd, E_OK);
check_assert(rtsk.tskstat == TTS_DMT);
check_point(53);
ercd = act_tsk(TASK3);
check_ercd(ercd, E_OK);
check_point(55);
ercd = ref_tsk(TASK3, &rtsk);
check_ercd(ercd, E_OK);
check_assert(rtsk.tskstat == TTS_SUS);
check_point(56);
ercd = ras_ter(TASK3);
check_ercd(ercd, E_OK);
check_point(58);
ercd = ref_tsk(TASK3, &rtsk);
check_ercd(ercd, E_OK);
check_assert(rtsk.tskstat == TTS_DMT);
check_finish(59);
check_point(0);
}
/* メインタスク */
void main_task(intptr_t unused)
{
signed char forward; /* 前後進命令 */
signed char turn; /* 旋回命令 */
signed char pwm_L, pwm_R; /* 左右モータPWM出力 */
/* LCD画面表示 */
ev3_lcd_fill_rect(0, 0, EV3_LCD_WIDTH, EV3_LCD_HEIGHT, EV3_LCD_WHITE);
ev3_lcd_draw_string("ミヤウチ", 0, CALIB_FONT_HEIGHT*1);
/* センサー入力ポートの設定 */
ev3_sensor_config(sonar_sensor, ULTRASONIC_SENSOR);
ev3_sensor_config(color_sensor, COLOR_SENSOR);
ev3_color_sensor_get_reflect(color_sensor); /* 反射率モード */
ev3_sensor_config(touch_sensor, TOUCH_SENSOR);
ev3_sensor_config(gyro_sensor, GYRO_SENSOR);
/* モーター出力ポートの設定 */
ev3_motor_config(left_motor, LARGE_MOTOR);
ev3_motor_config(right_motor, LARGE_MOTOR);
ev3_motor_config(tail_motor, LARGE_MOTOR);
ev3_motor_reset_counts(tail_motor);
//キャリブレイト 白取得
printf("Press the touch sensor to measure light intensity of WHITE.\n");
while(!ev3_touch_sensor_is_pressed(touch_sensor));
while(ev3_touch_sensor_is_pressed(touch_sensor));
int white = ev3_color_sensor_get_reflect(color_sensor);
printf("WHITE light intensity: %d.\n", white);
//キャリブレイト 黒取得
printf("Press the touch sensor to measure light intensity of BLACK.\n");
while(!ev3_touch_sensor_is_pressed(touch_sensor));
while(ev3_touch_sensor_is_pressed(touch_sensor));
int black = ev3_color_sensor_get_reflect(color_sensor);
printf("BLACK light intensity: %d.\n", black);
//PID制御
float lasterror = 0, integral = 0;
float midpoint = (white - black) / 2 + black;
/* Open Bluetooth file */
bt = ev3_serial_open_file(EV3_SERIAL_BT);
assert(bt != NULL);
/* Bluetooth通信タスクの起動 */
act_tsk(BT_TASK);
ev3_led_set_color(LED_ORANGE); /* 初期化完了通知 */
/* スタート待機 */
while(1)
{
//尻尾さげる
tail_control(TAIL_ANGLE_STAND_UP);
if (bt_cmd == 1)
{
//リモートスタート
break;
}
if (ev3_touch_sensor_is_pressed(touch_sensor) == 1)
{
//タッチセンサが押された
break;
}
tslp_tsk(1);
}
/* 走行モーターエンコーダーリセット */
ev3_motor_reset_counts(left_motor);
ev3_motor_reset_counts(right_motor);
/* ジャイロセンサーリセット */
ev3_gyro_sensor_reset(gyro_sensor);
balancer.init(GYRO_OFFSET); // <1>
/* スタート（LED緑色） */
ev3_led_set_color(LED_GREEN);
while(1)
{
int32_t motor_ang_l, motor_ang_r;
int gyro, volt;
if (ev3_button_is_pressed(BACK_BUTTON)) break;
tail_control(TAIL_ANGLE_DRIVE); /* バランス走行用角度に制御 */
forward = 30;
float error = midpoint - ev3_color_sensor_get_reflect(color_sensor);
integral = error + integral * 0.5;
turn = 0.07 * error + 0.3 * integral + 1 * (error - lasterror);
// float steer = 0.07 * error + 0.3 * integral + 1 * (error - lasterror);
// ev3_motor_steer(left_motor, right_motor, 10, steer);
lasterror = error;
tslp_tsk(1);
/* 倒立振子制御API に渡すパラメータを取得する */
motor_ang_l = ev3_motor_get_counts(left_motor);
motor_ang_r = ev3_motor_get_counts(right_motor);
gyro = ev3_gyro_sensor_get_rate(gyro_sensor);
volt = ev3_battery_voltage_mV();
/* 倒立振子制御APIを呼び出し、倒立走行するための */
/* 左右モータ出力値を得る */
balancer.setCommand(forward, turn); // <1>
balancer.update(gyro, motor_ang_r, motor_ang_l, volt); // <2>
pwm_L = balancer.getPwmRight(); // <3>
pwm_R = balancer.getPwmLeft(); // <3>
/* EV3ではモーター停止時のブレーキ設定が事前にできないため */
/* 出力0時に、その都度設定する */
if (pwm_L == 0)
{
ev3_motor_stop(left_motor, true);
}
else
{
ev3_motor_set_power(left_motor, (int)pwm_L);
}
if (pwm_R == 0)
{
ev3_motor_stop(right_motor, true);
}
else
{
ev3_motor_set_power(right_motor, (int)pwm_R);
}
tslp_tsk(4); /* 4msec周期起動 */
}
ev3_motor_stop(left_motor, false);
ev3_motor_stop(right_motor, false);
ter_tsk(BT_TASK);
fclose(bt);
ext_tsk();
}
void task1( unsigned int arg )
{
ER ercd;
int tests = 0;
CYG_TEST_INFO( "Task 1 running" );
ercd = dis_dsp();
CYG_TEST_CHECK( E_OK == ercd, "dis_dsp bad ercd" );
ercd = sta_tsk( 2, 22222 );
CYG_TEST_CHECK( E_OK == ercd, "sta_tsk bad ercd" );
ercd = chg_pri( 2, 5 );
CYG_TEST_CHECK( E_OK == ercd, "chg_pri bad ercd" );
ercd = ena_dsp();
CYG_TEST_CHECK( E_OK == ercd, "ena_dsp bad ercd" );
ercd = dly_tsk( 10 );
CYG_TEST_CHECK( E_OK == ercd, "dly_tsk bad ercd" );
#ifdef CYGPKG_UITRON_MEMPOOLFIXED_CREATE_DELETE
tests++;
#ifdef CYGSEM_UITRON_BAD_PARAMS_RETURN_ERRORS
ercd = del_mpf( -6 );
CYG_TEST_CHECK( E_ID == ercd, "del_mpf bad ercd !E_ID" );
ercd = del_mpf( 99 );
CYG_TEST_CHECK( E_ID == ercd, "del_mpf bad ercd !E_ID" );
ercd = cre_mpf( -6, &t_cmpf );
CYG_TEST_CHECK( E_ID == ercd, "cre_mpf bad ercd !E_ID" );
ercd = cre_mpf( 99, &t_cmpf );
CYG_TEST_CHECK( E_ID == ercd, "cre_mpf bad ercd !E_ID" );
#endif // we can test bad param error returns
// try a pre-existing object
// [first get a valid block from it for the freeing test later]
ercd = pget_blf( &vp, 3 );
CYG_TEST_CHECK( E_OK == ercd, "pget_blf bad ercd" );
ercd = cre_mpf( 3, &t_cmpf );
CYG_TEST_CHECK( E_OBJ == ercd, "cre_mpf bad ercd !E_OBJ" );
// delete it so we can play
ercd = del_mpf( 3 );
CYG_TEST_CHECK( E_OK == ercd, "del_mpf bad ercd" );
// check it is deleted
ercd = rel_blf( 3, vp ); // vp did come from this pool
CYG_TEST_CHECK( E_NOEXS == ercd, "rel_blf bad ercd !E_NOEXS" );
ercd = pget_blf( &vp, 3 );
CYG_TEST_CHECK( E_NOEXS == ercd, "pget_blf bad ercd !E_NOEXS" );
ercd = tget_blf( &vp, 3, 10 );
CYG_TEST_CHECK( E_NOEXS == ercd, "tget_blf bad ercd !E_NOEXS" );
ercd = get_blf( &vp, 3 );
CYG_TEST_CHECK( E_NOEXS == ercd, "get_blf bad ercd !E_NOEXS" );
ercd = ref_mpf( &t_rmpf, 3 );
CYG_TEST_CHECK( E_NOEXS == ercd, "ref_mpf bad ercd !E_NOEXS" );
#ifdef CYGSEM_UITRON_BAD_PARAMS_RETURN_ERRORS
// now try creating it (badly)
#ifndef CYGSEM_UITRON_PARAMS_NULL_IS_GOOD_PTR
ercd = cre_mpf( 3, NULL );
CYG_TEST_CHECK( E_PAR == ercd, "cre_mpf bad ercd !E_PAR" );
#endif
ercd = cre_mpf( 3, NADR );
CYG_TEST_CHECK( E_PAR == ercd, "cre_mpf bad ercd !E_PAR" );
t_cmpf.mpfatr = 0xfff;
ercd = cre_mpf( 3, &t_cmpf );
CYG_TEST_CHECK( E_RSATR == ercd, "cre_mpf bad ercd !E_RSATR" );
#endif // we can test bad param error returns
t_cmpf.mpfatr = 0;
t_cmpf.mpfcnt = 10000;
t_cmpf.blfsz = 100;
ercd = cre_mpf( 3, &t_cmpf );
CYG_TEST_CHECK( E_NOMEM == ercd, "cre_mpf bad ercd" );
t_cmpf.mpfcnt = 100;
t_cmpf.blfsz = 100000;
ercd = cre_mpf( 3, &t_cmpf );
CYG_TEST_CHECK( E_NOMEM == ercd, "cre_mpf bad ercd" );
// now create it well
t_cmpf.mpfatr = 0;
t_cmpf.mpfcnt = 10;
t_cmpf.blfsz = 100;
ercd = cre_mpf( 3, &t_cmpf );
CYG_TEST_CHECK( E_OK == ercd, "cre_mpf bad ercd" );
// and check we can use it
ercd = pget_blf( &vp, 3 );
CYG_TEST_CHECK( E_OK == ercd, "pget_blf bad ercd" );
ercd = tget_blf( &vp, 3, 10 );
CYG_TEST_CHECK( E_OK == ercd, "tget_blf bad ercd" );
ercd = get_blf( &vp, 3 );
CYG_TEST_CHECK( E_OK == ercd, "get_blf bad ercd" );
ercd = rel_blf( 3, vp ); // vp did come from new pool
CYG_TEST_CHECK( E_OK == ercd, "rel_blf bad ercd" );
ercd = rel_blf( 3, vp ); // vp already freed
CYG_TEST_CHECK( E_PAR == ercd, "rel_blf bad ercd !E_PAR" );
ercd = ref_mpf( &t_rmpf, 3 );
CYG_TEST_CHECK( E_OK == ercd, "ref_mpf bad ercd" );
// In order to wait on the pools, we must first consume all they have:
while ( E_OK == (ercd = pget_blf( &vp, 1 )) ) /* nothing */;
CYG_TEST_CHECK( E_TMOUT == ercd, "pget_blf bad ercd !E_TMOUT" );
while ( E_OK == (ercd = tget_blf( &vp, 2, 1 )) ) /* nothing */;
CYG_TEST_CHECK( E_TMOUT == ercd, "tget_blf bad ercd !E_TMOUT" );
// now wait while task 2 deletes the wait objects
ercd = wup_tsk( 2 );
CYG_TEST_CHECK( E_OK == ercd, "wup_tsk bad ercd" );
ercd = get_blf( &vp, 1 );
CYG_TEST_CHECK( E_DLT == ercd, "get_blf bad ercd !E_DLT" );
ercd = wup_tsk( 2 );
CYG_TEST_CHECK( E_OK == ercd, "wup_tsk bad ercd" );
ercd = tget_blf( &vp, 2, 20 );
CYG_TEST_CHECK( E_DLT == ercd, "tget_blf bad ercd !E_DLT" );
// check they are deleted
ercd = get_blf( &vp, 1 );
CYG_TEST_CHECK( E_NOEXS == ercd, "get_blf bad ercd !E_NOEXS" );
ercd = pget_blf( &vp, 2 );
CYG_TEST_CHECK( E_NOEXS == ercd, "pget_blf bad ercd !E_NOEXS" );
// re-create and do it again
t_cmpf.mpfcnt = 90;
t_cmpf.blfsz = 20;
ercd = cre_mpf( 1, &t_cmpf );
CYG_TEST_CHECK( E_OK == ercd, "cre_mpf bad ercd" );
t_cmpf.mpfcnt = 5;
t_cmpf.blfsz = 200;
ercd = cre_mpf( 2, &t_cmpf );
CYG_TEST_CHECK( E_OK == ercd, "cre_mpf bad ercd" );
// In order to wait on the pools, we must first consume all they have:
while ( E_OK == (ercd = pget_blf( &vp, 1 )) ) /* nothing */;
CYG_TEST_CHECK( E_TMOUT == ercd, "pget_blf bad ercd !E_TMOUT" );
while ( E_OK == (ercd = tget_blf( &vp, 2, 1 )) ) /* nothing */;
CYG_TEST_CHECK( E_TMOUT == ercd, "tget_blf bad ercd !E_TMOUT" );
// now wait while task 2 deletes the wait objects
ercd = wup_tsk( 2 );
CYG_TEST_CHECK( E_OK == ercd, "wup_tsk bad ercd" );
ercd = get_blf( &vp, 1 );
CYG_TEST_CHECK( E_DLT == ercd, "get_blf bad ercd !E_DLT" );
ercd = wup_tsk( 2 );
CYG_TEST_CHECK( E_OK == ercd, "wup_tsk bad ercd" );
ercd = tget_blf( &vp, 2, 10 );
CYG_TEST_CHECK( E_DLT == ercd, "tget_blf bad ercd !E_DLT" );
// check they are deleted
ercd = tget_blf( &vp, 1, 1 );
CYG_TEST_CHECK( E_NOEXS == ercd, "get_blf bad ercd !E_NOEXS" );
ercd = get_blf( &vp, 2 );
CYG_TEST_CHECK( E_NOEXS == ercd, "pget_blf bad ercd !E_NOEXS" );
CYG_TEST_PASS("create/delete fixed mempools");
#endif // CYGPKG_UITRON_MEMPOOLFIXED_CREATE_DELETE
#ifdef CYGPKG_UITRON_MEMPOOLVAR_CREATE_DELETE
tests++;
#ifdef CYGSEM_UITRON_BAD_PARAMS_RETURN_ERRORS
ercd = del_mpl( -6 );
CYG_TEST_CHECK( E_ID == ercd, "del_mpl bad ercd !E_ID" );
ercd = del_mpl( 99 );
CYG_TEST_CHECK( E_ID == ercd, "del_mpl bad ercd !E_ID" );
ercd = cre_mpl( -6, &t_cmpl );
CYG_TEST_CHECK( E_ID == ercd, "cre_mpl bad ercd !E_ID" );
ercd = cre_mpl( 99, &t_cmpl );
CYG_TEST_CHECK( E_ID == ercd, "cre_mpl bad ercd !E_ID" );
#endif // we can test bad param error returns
// try a pre-existing object
// [first get a valid block from it for the freeing test later]
ercd = pget_blk( &vp, 3, 100 );
CYG_TEST_CHECK( E_OK == ercd, "pget_blk bad ercd" );
ercd = cre_mpl( 3, &t_cmpl );
CYG_TEST_CHECK( E_OBJ == ercd, "cre_mpl bad ercd !E_OBJ" );
// delete it so we can play
ercd = del_mpl( 3 );
CYG_TEST_CHECK( E_OK == ercd, "del_mpl bad ercd" );
// check it is deleted
ercd = rel_blk( 3, vp ); // vp did come from this pool
CYG_TEST_CHECK( E_NOEXS == ercd, "rel_blk bad ercd !E_NOEXS" );
ercd = pget_blk( &vp, 3, 100 );
CYG_TEST_CHECK( E_NOEXS == ercd, "pget_blk bad ercd !E_NOEXS" );
ercd = tget_blk( &vp, 3, 100, 10 );
CYG_TEST_CHECK( E_NOEXS == ercd, "tget_blk bad ercd !E_NOEXS" );
ercd = get_blk( &vp, 3, 100 );
CYG_TEST_CHECK( E_NOEXS == ercd, "get_blk bad ercd !E_NOEXS" );
ercd = ref_mpl( &t_rmpl, 3 );
CYG_TEST_CHECK( E_NOEXS == ercd, "ref_mpl bad ercd !E_NOEXS" );
#ifdef CYGSEM_UITRON_BAD_PARAMS_RETURN_ERRORS
// now try creating it (badly)
#ifndef CYGSEM_UITRON_PARAMS_NULL_IS_GOOD_PTR
ercd = cre_mpl( 3, NULL );
CYG_TEST_CHECK( E_PAR == ercd, "cre_mpl bad ercd !E_PAR" );
#endif
ercd = cre_mpl( 3, NADR );
CYG_TEST_CHECK( E_PAR == ercd, "cre_mpl bad ercd !E_PAR" );
t_cmpl.mplatr = 0xfff;
ercd = cre_mpl( 3, &t_cmpl );
CYG_TEST_CHECK( E_RSATR == ercd, "cre_mpl bad ercd !E_RSATR" );
#endif // we can test bad param error returns
t_cmpl.mplatr = 0;
t_cmpl.mplsz = 100000000;
ercd = cre_mpl( 3, &t_cmpl );
CYG_TEST_CHECK( E_NOMEM == ercd, "cre_mpl bad ercd" );
// now create it well
t_cmpl.mplatr = 0;
t_cmpl.mplsz = 1000;
ercd = cre_mpl( 3, &t_cmpl );
CYG_TEST_CHECK( E_OK == ercd, "cre_mpl bad ercd" );
// and check we can use it
ercd = pget_blk( &vp, 3, 100 );
CYG_TEST_CHECK( E_OK == ercd, "pget_blk bad ercd" );
ercd = pget_blk( &vp, 3, 100000000 ); // way too large
CYG_TEST_CHECK( E_TMOUT == ercd, "pget_blk bad ercd !E_TMOUT" );
ercd = tget_blk( &vp, 3, 100, 10 );
CYG_TEST_CHECK( E_OK == ercd, "tget_blk bad ercd" );
ercd = get_blk( &vp, 3, 100 );
CYG_TEST_CHECK( E_OK == ercd, "get_blk bad ercd" );
ercd = rel_blk( 3, vp ); // vp did come from new pool
CYG_TEST_CHECK( E_OK == ercd, "rel_blk bad ercd" );
ercd = rel_blk( 3, vp ); // vp already freed
CYG_TEST_CHECK( E_PAR == ercd, "rel_blk bad ercd !E_PAR" );
ercd = ref_mpl( &t_rmpl, 3 );
CYG_TEST_CHECK( E_OK == ercd, "ref_mpl bad ercd" );
// In order to wait on the pools, we must first consume all they have:
while ( E_OK == (ercd = pget_blk( &vp, 1, 100 )) ) /* nothing */;
CYG_TEST_CHECK( E_TMOUT == ercd, "pget_blk bad ercd !E_TMOUT" );
while ( E_OK == (ercd = tget_blk( &vp, 2, 100, 1 )) ) /* nothing */;
CYG_TEST_CHECK( E_TMOUT == ercd, "tget_blk bad ercd !E_TMOUT" );
// now wait while task 2 deletes the wait objects
ercd = wup_tsk( 2 );
CYG_TEST_CHECK( E_OK == ercd, "wup_tsk bad ercd" );
ercd = get_blk( &vp, 1, 200 );
CYG_TEST_CHECK( E_DLT == ercd, "get_blk bad ercd !E_DLT" );
ercd = wup_tsk( 2 );
CYG_TEST_CHECK( E_OK == ercd, "wup_tsk bad ercd" );
ercd = tget_blk( &vp, 2, 100, 20 );
CYG_TEST_CHECK( E_DLT == ercd, "tget_blk bad ercd !E_DLT" );
// check they are deleted
ercd = get_blk( &vp, 1, 200 );
CYG_TEST_CHECK( E_NOEXS == ercd, "get_blk bad ercd !E_NOEXS" );
ercd = pget_blk( &vp, 2, 20 );
CYG_TEST_CHECK( E_NOEXS == ercd, "pget_blk bad ercd !E_NOEXS" );
// re-create and do it again
ercd = cre_mpl( 1, &t_cmpl );
CYG_TEST_CHECK( E_OK == ercd, "cre_mpl bad ercd" );
ercd = cre_mpl( 2, &t_cmpl );
CYG_TEST_CHECK( E_OK == ercd, "cre_mpl bad ercd" );
// In order to wait on the pools, we must first consume all they have:
while ( E_OK == (ercd = pget_blk( &vp, 1, 20 )) ) /* nothing */;
CYG_TEST_CHECK( E_TMOUT == ercd, "pget_blk bad ercd !E_TMOUT" );
while ( E_OK == (ercd = tget_blk( &vp, 2, 400, 1 )) ) /* nothing */;
CYG_TEST_CHECK( E_TMOUT == ercd, "tget_blk bad ercd !E_TMOUT" );
// now wait while task 2 deletes the wait objects
ercd = wup_tsk( 2 );
CYG_TEST_CHECK( E_OK == ercd, "wup_tsk bad ercd" );
ercd = get_blk( &vp, 1, 200 );
CYG_TEST_CHECK( E_DLT == ercd, "get_blk bad ercd !E_DLT" );
ercd = wup_tsk( 2 );
CYG_TEST_CHECK( E_OK == ercd, "wup_tsk bad ercd" );
ercd = tget_blk( &vp, 2, 500, 20 );
CYG_TEST_CHECK( E_DLT == ercd, "tget_blk bad ercd !E_DLT" );
// check they are deleted
ercd = tget_blk( &vp, 1, 200, 1 );
CYG_TEST_CHECK( E_NOEXS == ercd, "get_blk bad ercd !E_NOEXS" );
ercd = get_blk( &vp, 2, 20 );
CYG_TEST_CHECK( E_NOEXS == ercd, "pget_blk bad ercd !E_NOEXS" );
CYG_TEST_PASS("create/delete variable mempools");
#endif // CYGPKG_UITRON_MEMPOOLVAR_CREATE_DELETE
ercd = ter_tsk( 2 );
CYG_TEST_CHECK( E_OK == ercd, "ter_tsk bad ercd" );
ercd = dly_tsk( 5 );
CYG_TEST_CHECK( E_OK == ercd, "dly_tsk bad ercd" );
// all done
if ( 0 == tests ) {
CYG_TEST_NA( "No objects have create/delete enabled" );
}
else {
CYG_TEST_EXIT( "All done" );
}
ext_tsk();
}
static ER _dmloader_rmv_ldm(ID ldm_can_id) {
// TODO: hard coded & should check ldm_can status
if (ldm_can_id != 1) return E_ID;
T_LDM_CAN *ldm_can = &ldm_cans[ldm_can_id - 1];
ER ercd;
ercd = loc_mtx(DMLOADER_MTX);
if(ercd != E_OK) {
syslog(LOG_ERROR, "%s(): Acquire mutex failed.", __FUNCTION__);
goto error_exit;
}
if (ldm_can->status != LDM_CAN_RUNNING) {
syslog(LOG_ERROR, "%s(): LDM container is not running.", __FUNCTION__);
ercd = E_OBJ;
goto error_exit;
}
// Destroy all EV3CYCs
destroy_all_ev3cyc();
// Deletion
for(SIZE i = 0; i < ldm_can->cfg_entry_num && ercd == E_OK; ++i) {
MOD_CFG_ENTRY *ent = &ldm_can->cfg_table[i];
switch(ent->sfncd) {
case TSFN_CRE_TSK: {
syslog(LOG_DEBUG, "%s(): RMV MOD_CFG_ENTRY TSFN_CRE_TSK", __FUNCTION__);
ID tskid = *(ID*)ent->retvalptr;
ter_tsk(tskid);
ercd = del_tsk(tskid);
assert(ercd == E_OK);
break; }
case TSFN_CRE_SEM: {
syslog(LOG_DEBUG, "%s(): RMV MOD_CFG_ENTRY TSFN_CRE_SEM", __FUNCTION__);
ID semid = *(ID*)ent->retvalptr;
ercd = del_sem(semid);
assert(ercd == E_OK);
break; }
case TSFN_CRE_FLG: {
syslog(LOG_DEBUG, "%s(): RMV MOD_CFG_ENTRY TSFN_CRE_FLG", __FUNCTION__);
ID flgid = *(ID*)ent->retvalptr;
ercd = del_flg(flgid);
assert(ercd == E_OK);
break; }
default:
syslog(LOG_ERROR, "%s(): Unsupported static function code %d.", __FUNCTION__, ent->sfncd);
ercd = E_OBJ;
goto error_exit;
}
}
ldm_can->status = LDM_CAN_FREE;
// TODO: clean ldm_can
error_exit:
unl_mtx(DMLOADER_MTX);
return ercd;
}
static int __ui_ter_tsk(struct pt_regs *regs)
{
ID tskid = __xn_reg_arg1(regs);
return ter_tsk(tskid);
}
/*
* メインタスク
*/
void main_task(intptr_t exinf)
{
char c;
ID tskid = TASK1;
int_t tskno = 1;
ER_UINT ercd;
PRI tskpri;
#ifndef TASK_LOOP
volatile ulong_t i;
SYSTIM stime1, stime2;
#endif /* TASK_LOOP */
#ifdef TOPPERS_SUPPORT_GET_UTM
SYSUTM utime1, utime2;
#endif /* TOPPERS_SUPPORT_GET_UTM */
T_CTSK ctsk;
T_DTEX dtex;
ID TASK3 = -1;
SVC_PERROR(syslog_msk_log(LOG_UPTO(LOG_INFO), LOG_UPTO(LOG_EMERG)));
syslog(LOG_NOTICE, "Sample program starts (exinf = %d).", (int_t) exinf);
/*
* シリアルポートの初期化
*
* システムログタスクと同じシリアルポートを使う場合など，シリアル
* ポートがオープン済みの場合にはここでE_OBJエラーになるが，支障は
* ない．
*/
ercd = serial_opn_por(TASK_PORTID);
if (ercd < 0 && MERCD(ercd) != E_OBJ) {
syslog(LOG_ERROR, "%s (%d) reported by `serial_opn_por'.",
itron_strerror(ercd), SERCD(ercd));
}
SVC_PERROR(serial_ctl_por(TASK_PORTID,
(IOCTL_CRLF | IOCTL_FCSND | IOCTL_FCRCV)));
/*
* ループ回数の設定
*
* 並行実行されるタスク内での空ループの回数（task_loop）は，空ルー
* プの実行時間が約0.4秒になるように設定する．この設定のために，
* LOOP_REF回の空ループの実行時間を，その前後でget_timを呼ぶことで
* 測定し，その測定結果から空ループの実行時間が0.4秒になるループ回
* 数を求め，task_loopに設定する．
*
* LOOP_REFは，デフォルトでは1,000,000に設定しているが，想定したよ
* り遅いプロセッサでは，サンプルプログラムの実行開始に時間がかか
* りすぎるという問題を生じる．逆に想定したより速いプロセッサでは，
* LOOP_REF回の空ループの実行時間が短くなり，task_loopに設定する値
* の誤差が大きくなるという問題がある．
*
* そこで，そのようなターゲットでは，target_test.hで，LOOP_REFを適
* 切な値に定義するのが望ましい．
*
* また，task_loopの値を固定したい場合には，その値をTASK_LOOPにマ
* クロ定義する．TASK_LOOPがマクロ定義されている場合，上記の測定を
* 行わずに，TASK_LOOPに定義された値を空ループの回数とする．
*
* ターゲットによっては，空ループの実行時間の1回目の測定で，本来よ
* りも長めになるものがある．このようなターゲットでは，MEASURE_TWICE
* をマクロ定義することで，1回目の測定結果を捨てて，2回目の測定結果
* を使う．
*
* タスク例外処理ルーチン内での空ループの回数（tex_loop）は，
* task_loopの4分の1の値（空ループの実行時間が0.1秒になるループ回
* 数）に設定する．
*/
#ifdef TASK_LOOP
task_loop = TASK_LOOP;
#else /* TASK_LOOP */
#ifdef MEASURE_TWICE
task_loop = LOOP_REF;
SVC_PERROR(get_tim(&stime1));
for (i = 0; i < task_loop; i++);
SVC_PERROR(get_tim(&stime2));
#endif /* MEASURE_TWICE */
task_loop = LOOP_REF;
SVC_PERROR(get_tim(&stime1));
for (i = 0; i < task_loop; i++);
SVC_PERROR(get_tim(&stime2));
task_loop = LOOP_REF * 400UL / (stime2 - stime1);
#endif /* TASK_LOOP */
tex_loop = task_loop / 4;
/*
* タスクの起動
*/
SVC_PERROR(act_tsk(TASK1));
SVC_PERROR(act_tsk(TASK2));
/*
* メインループ
*/
do {
SVC_PERROR(serial_rea_dat(TASK_PORTID, &c, 1));
switch (c) {
case 'e':
case 's':
case 'S':
case 'd':
case 'y':
case 'Y':
case 'z':
case 'Z':
message[tskno-1] = c;
break;
case '1':
tskno = 1;
tskid = TASK1;
break;
case '2':
tskno = 2;
tskid = TASK2;
break;
case '3':
tskno = 3;
tskid = TASK3;
break;
case 'a':
syslog(LOG_INFO, "#act_tsk(%d)", tskno);
SVC_PERROR(act_tsk(tskid));
break;
case 'A':
syslog(LOG_INFO, "#can_act(%d)", tskno);
SVC_PERROR(ercd = can_act(tskid));
if (ercd >= 0) {
syslog(LOG_NOTICE, "can_act(%d) returns %d", tskno, ercd);
}
break;
case 't':
syslog(LOG_INFO, "#ter_tsk(%d)", tskno);
SVC_PERROR(ter_tsk(tskid));
break;
case '>':
syslog(LOG_INFO, "#chg_pri(%d, HIGH_PRIORITY)", tskno);
SVC_PERROR(chg_pri(tskid, HIGH_PRIORITY));
break;
case '=':
syslog(LOG_INFO, "#chg_pri(%d, MID_PRIORITY)", tskno);
SVC_PERROR(chg_pri(tskid, MID_PRIORITY));
break;
case '<':
syslog(LOG_INFO, "#chg_pri(%d, LOW_PRIORITY)", tskno);
SVC_PERROR(chg_pri(tskid, LOW_PRIORITY));
break;
case 'G':
syslog(LOG_INFO, "#get_pri(%d, &tskpri)", tskno);
SVC_PERROR(ercd = get_pri(tskid, &tskpri));
if (ercd >= 0) {
syslog(LOG_NOTICE, "priority of task %d is %d", tskno, tskpri);
}
break;
case 'w':
syslog(LOG_INFO, "#wup_tsk(%d)", tskno);
SVC_PERROR(wup_tsk(tskid));
break;
case 'W':
syslog(LOG_INFO, "#can_wup(%d)", tskno);
SVC_PERROR(ercd = can_wup(tskid));
if (ercd >= 0) {
syslog(LOG_NOTICE, "can_wup(%d) returns %d", tskno, ercd);
}
break;
case 'l':
syslog(LOG_INFO, "#rel_wai(%d)", tskno);
SVC_PERROR(rel_wai(tskid));
break;
case 'u':
syslog(LOG_INFO, "#sus_tsk(%d)", tskno);
SVC_PERROR(sus_tsk(tskid));
break;
case 'm':
syslog(LOG_INFO, "#rsm_tsk(%d)", tskno);
SVC_PERROR(rsm_tsk(tskid));
break;
case 'x':
syslog(LOG_INFO, "#ras_tex(%d, 0x0001U)", tskno);
SVC_PERROR(ras_tex(tskid, 0x0001U));
break;
case 'X':
syslog(LOG_INFO, "#ras_tex(%d, 0x0002U)", tskno);
SVC_PERROR(ras_tex(tskid, 0x0002U));
break;
case 'r':
syslog(LOG_INFO, "#rot_rdq(three priorities)");
SVC_PERROR(rot_rdq(HIGH_PRIORITY));
SVC_PERROR(rot_rdq(MID_PRIORITY));
SVC_PERROR(rot_rdq(LOW_PRIORITY));
break;
case 'c':
syslog(LOG_INFO, "#sta_cyc(1)");
SVC_PERROR(sta_cyc(CYCHDR1));
break;
case 'C':
syslog(LOG_INFO, "#stp_cyc(1)");
SVC_PERROR(stp_cyc(CYCHDR1));
break;
case 'b':
syslog(LOG_INFO, "#sta_alm(1, 5000)");
SVC_PERROR(sta_alm(ALMHDR1, 5000));
break;
case 'B':
syslog(LOG_INFO, "#stp_alm(1)");
SVC_PERROR(stp_alm(ALMHDR1));
break;
case '@':
ctsk.tskatr = TA_NULL;
ctsk.exinf = 3;
ctsk.task = task;
ctsk.itskpri = MID_PRIORITY;
ctsk.stksz = STACK_SIZE;
ctsk.stk = NULL;
SVC_PERROR(TASK3 = acre_tsk(&ctsk));
dtex.texatr = TA_NULL;
dtex.texrtn = tex_routine;
SVC_PERROR(def_tex(TASK3, &dtex));
syslog(LOG_NOTICE, "task3 is created with tskid = %d.",
(int_t) TASK3);
break;
case '!':
syslog(LOG_INFO, "#del_tsk(%d)", tskno);
SVC_PERROR(del_tsk(tskid));
break;
case 'V':
#ifdef TOPPERS_SUPPORT_GET_UTM
SVC_PERROR(get_utm(&utime1));
SVC_PERROR(get_utm(&utime2));
syslog(LOG_NOTICE, "utime1 = %ld, utime2 = %ld",
(ulong_t) utime1, (ulong_t) utime2);
#else /* TOPPERS_SUPPORT_GET_UTM */
syslog(LOG_NOTICE, "get_utm is not supported.");
#endif /* TOPPERS_SUPPORT_GET_UTM */
break;
case 'v':
SVC_PERROR(syslog_msk_log(LOG_UPTO(LOG_INFO),
LOG_UPTO(LOG_EMERG)));
break;
case 'q':
SVC_PERROR(syslog_msk_log(LOG_UPTO(LOG_NOTICE),
LOG_UPTO(LOG_EMERG)));
break;
#ifdef BIT_KERNEL
case ' ':
SVC_PERROR(loc_cpu());
{
extern ER bit_kernel(void);
SVC_PERROR(ercd = bit_kernel());
if (ercd >= 0) {
syslog(LOG_NOTICE, "bit_kernel passed.");
}
}
SVC_PERROR(unl_cpu());
break;
#endif /* BIT_KERNEL */
case '$':
syslog(LOG_INFO, "#%d#twai_sem(10000)", tskno);
SVC_PERROR(twai_sem(TEST_SEM, 10000));
break;
default:
break;
}
} while (c != '\003' && c != 'Q');
syslog(LOG_NOTICE, "Sample program ends.");
SVC_PERROR(ext_ker());
assert(0);
}
/*
* タスクコマンド
*/
static UW
task_command(B *command)
{
INT point=0;
B cmd=-1;
UW w;
int no, count;
BOOL result=TRUE;
ER ercd = E_OK;
if(current_tskid == MONTASK){ /* モニタ自体の制御はできない */
printf(" Can't control the monitor!!\n");
return 0;
}
count = sizeof(mon_task) / sizeof(struct SUBCOMMAND_TABLE);
if(command[point]){
for(no = 0 ; no < count ; no++){
if(compare_word(mon_task[no].subcommand, command, 0)){
skip_word(command, &point);
cmd = mon_task[no].type;
break;
}
}
}
switch(cmd){
case MONTASK_ACTIVE: /* 起動要求(act_tsk) */
ercd = act_tsk(current_tskid);
printf(" execute act_tsk(%d)", current_tskid);
break;
case MONTASK_TERM: /* 強制終了(ter_tsk) */
ercd = ter_tsk(current_tskid);
printf(" execute ter_tsk(%d)", current_tskid);
break;
case MONTASK_SUSPEND: /* 待ち要求(sus_tsk) */
ercd = sus_tsk(current_tskid);
printf(" execute sus_tsk(%d)", current_tskid);
break;
case MONTASK_RESUME: /* 待ち再開(rsm_tsk) */
ercd = rsm_tsk(current_tskid);
printf(" execute rsm_tsk(%d)", current_tskid);
break;
case MONTASK_RELEASE: /* 待ち解除(rel_wai) */
ercd = rel_wai(current_tskid);
printf(" execute rel_wai(%d)", current_tskid);
break;
case MONTASK_WAKEUP: /* タスク起床(wup_tsk) */
ercd = wup_tsk(current_tskid);
printf(" execute wup_tsk(%d)", current_tskid);
break;
case MONTASK_PRIORITY: /* 優先度変更(chg_pri) */
if(get_value(command, &point, &w, DEC_BASE)){
ercd = chg_pri(current_tskid, w);
printf(" execute chg_pri(%d, %d)", current_tskid, w);
}
else
result = FALSE;
break;
default:
result = FALSE;
break;
}
if(result){
printf(" :: result = %s !\n", (VP_INT)itron_strerror(ercd));
tslp_tsk(100); /* スイッチング */
}
return 0;
}
void
task3(intptr_t exinf)
{
ER_UINT ercd;
check_point(3);
ercd = snd_mbf(MBF1, string2, 26);
check_ercd(ercd, E_OK);
check_point(7);
ercd = snd_mbf(MBF1, string3, 10);
check_ercd(ercd, E_OK);
check_point(9);
ercd = rcv_mbf(MBF1, buf1);
check_ercd(ercd, 10);
check_assert(strncmp(buf1, string3, 10) == 0);
check_point(10);
ercd = act_tsk(TASK2);
check_ercd(ercd, E_OK);
check_point(12);
ercd = wup_tsk(TASK1);
check_ercd(ercd, E_OK);
check_point(14);
ercd = ter_tsk(TASK2);
check_ercd(ercd, E_OK);
check_point(17);
ercd = snd_mbf(MBF1, string2, 26);
check_ercd(ercd, E_OK);
check_point(22);
ercd = snd_mbf(MBF1, string3, 10);
check_ercd(ercd, E_OK);
check_point(25);
ercd = rcv_mbf(MBF1, buf1);
check_ercd(ercd, 10);
check_assert(strncmp(buf1, string3, 10) == 0);
check_point(26);
ercd = wup_tsk(TASK2);
check_ercd(ercd, E_OK);
check_point(28);
ercd = wup_tsk(TASK1);
check_ercd(ercd, E_OK);
check_point(30);
ercd = rel_wai(TASK2);
check_ercd(ercd, E_OK);
check_point(33);
ercd = snd_mbf(MBF1, string2, 26);
check_ercd(ercd, E_OK);
check_point(40);
ercd = snd_mbf(MBF1, string3, 10);
check_ercd(ercd, E_OK);
check_point(44);
ercd = rcv_mbf(MBF1, buf1);
check_ercd(ercd, 10);
check_assert(strncmp(buf1, string3, 10) == 0);
check_point(45);
ercd = wup_tsk(TASK2);
check_ercd(ercd, E_OK);
check_point(47);
ercd = wup_tsk(TASK1);
check_ercd(ercd, E_OK);
check_point(49);
ercd = sta_alm(ALM1, 1000U);
check_ercd(ercd, E_OK);
ercd = slp_tsk();
check_ercd(ercd, E_OK);
check_point(53);
ercd = snd_mbf(MBF1, string2, 26);
check_ercd(ercd, E_OK);
check_point(57);
ercd = snd_mbf(MBF1, string3, 10);
check_ercd(ercd, E_OK);
check_point(59);
ercd = rcv_mbf(MBF1, buf1);
check_ercd(ercd, 10);
check_assert(strncmp(buf1, string3, 10) == 0);
check_point(60);
ercd = wup_tsk(TASK2);
check_ercd(ercd, E_OK);
check_point(62);
ercd = wup_tsk(TASK1);
check_ercd(ercd, E_OK);
check_point(64);
ercd = slp_tsk();
check_ercd(ercd, E_OK);
check_point(0);
}
/* メインタスク */
void main_task(intptr_t unused)
{
signed char forward; /* 前後進命令 */
signed char turn; /* 旋回命令 */
signed char pwm_L, pwm_R; /* 左右モータPWM出力 */
static int turn = 0;
/* LCD画面表示 */
ev3_lcd_fill_rect(0, 0, EV3_LCD_WIDTH, EV3_LCD_HEIGHT, EV3_LCD_WHITE);
ev3_lcd_draw_string("ETミヤウチ", 0, CALIB_FONT_HEIGHT*1);
/* センサー入力ポートの設定 */
ev3_sensor_config(sonar_sensor, ULTRASONIC_SENSOR);
ev3_sensor_config(color_sensor, COLOR_SENSOR);
ev3_color_sensor_get_reflect(color_sensor); /* 反射率モード */
ev3_sensor_config(touch_sensor, TOUCH_SENSOR);
ev3_sensor_config(gyro_sensor, GYRO_SENSOR);
/* モーター出力ポートの設定 */
ev3_motor_config(left_motor, LARGE_MOTOR);
ev3_motor_config(right_motor, LARGE_MOTOR);
ev3_motor_config(tail_motor, LARGE_MOTOR);
ev3_motor_reset_counts(tail_motor);
/* Open Bluetooth file */
bt = ev3_serial_open_file(EV3_SERIAL_BT);
assert(bt != NULL);
/* Bluetooth通信タスクの起動 */
act_tsk(BT_TASK);
ev3_led_set_color(LED_ORANGE); /* 初期化完了通知 */
/* スタート待機 */
while(1)
{
tail_control(TAIL_ANGLE_STAND_UP); /* 完全停止用角度に制御 */
if (bt_cmd == 1)
{
break; /* リモートスタート */
}
if (ev3_touch_sensor_is_pressed(touch_sensor) == 1)
{
break; /* タッチセンサが押された */
}
tslp_tsk(10); /* 10msecウェイト */
}
/* 走行モーターエンコーダーリセット */
ev3_motor_reset_counts(left_motor);
ev3_motor_reset_counts(right_motor);
/* ジャイロセンサーリセット */
ev3_gyro_sensor_reset(gyro_sensor);
balancer.init(GYRO_OFFSET); // <1>
ev3_led_set_color(LED_GREEN); /* スタート通知 */
/**
* Main loop for the self-balance control algorithm
*/
while(1)
{
int32_t motor_ang_l, motor_ang_r;
int gyro, volt;
turn = (ev3_color_sensor_get_reflect(color_sensor) - (LIGHT_WHITE + LIGHT_BLACK)/2) * KP;
if (ev3_button_is_pressed(BACK_BUTTON)) break;
tail_control(TAIL_ANGLE_DRIVE); /* バランス走行用角度に制御 */
if (sonar_alert() == 1) /* 障害物検知 */
{
forward = turn = 0; /* 障害物を検知したら停止 */
}
else
{
forward = 30; /* 前進命令 */
if (100 < turn) {
turn = 100.0;
} else if (turn < -100) {
turn = -100.0;
}
}
/* 倒立振子制御API に渡すパラメータを取得する */
motor_ang_l = ev3_motor_get_counts(left_motor);
motor_ang_r = ev3_motor_get_counts(right_motor);
gyro = ev3_gyro_sensor_get_rate(gyro_sensor);
volt = ev3_battery_voltage_mV();
/* 倒立振子制御APIを呼び出し、倒立走行するための */
/* 左右モータ出力値を得る */
balancer.setCommand(forward, turn); // <1>
balancer.update(gyro, motor_ang_r, motor_ang_l, volt); // <2>
pwm_L = balancer.getPwmRight(); // <3>
pwm_R = balancer.getPwmLeft(); // <3>
/* EV3ではモーター停止時のブレーキ設定が事前にできないため */
/* 出力0時に、その都度設定する */
if (pwm_L == 0)
{
ev3_motor_stop(left_motor, true);
}
else
{
ev3_motor_set_power(left_motor, (int)pwm_L);
}
if (pwm_R == 0)
{
ev3_motor_stop(right_motor, true);
}
else
{
ev3_motor_set_power(right_motor, (int)pwm_R);
}
tslp_tsk(4); /* 4msec周期起動 */
}
ev3_motor_stop(left_motor, false);
ev3_motor_stop(right_motor, false);
ter_tsk(BT_TASK);
fclose(bt);
ext_tsk();
}
/*
* main task
*/
void main_task(intptr_t exinf)
{
char_t c;
ID tskid = TASK1;
int_t tskno = 1;
ER_UINT ercd;
PRI tskpri;
#ifndef TASK_LOOP
volatile ulong_t i;
SYSTIM stime1, stime2;
#endif /* TASK_LOOP */
#ifdef TOPPERS_SUPPORT_GET_UTM
SYSUTM utime1, utime2;
#endif /* TOPPERS_SUPPORT_GET_UTM */
SVC_PERROR(syslog_msk_log(LOG_UPTO(LOG_INFO), LOG_UPTO(LOG_EMERG)));
syslog(LOG_NOTICE, "Sample program starts (exinf = %d).", (int_t) exinf);
/*
* initialize the serial port
*
* If the same serial port is shared between main task and system
* log task. As this serial port is already opened in system log
* task, serial_opn_por will return E_OBJ which is not a real
* error.
*/
ercd = serial_opn_por(TASK_PORTID);
if (ercd < 0 && MERCD(ercd) != E_OBJ) {
syslog(LOG_ERROR, "%s (%d) reported by `serial_opn_por'.",
itron_strerror(ercd), SERCD(ercd));
}
SVC_PERROR(serial_ctl_por(TASK_PORTID,
(IOCTL_CRLF | IOCTL_FCSND | IOCTL_FCRCV)));
/*
* adjustment of task_loop.
*
* If TASK_LOOP is defined, the adjustment of task_loop will not be
* done.
*
* If MEASURE_TWICE is defined, 2 measurements are made. The 2nd
* result is adopted as the 1st is a little longer than the 2nd.
*/
#ifdef TASK_LOOP
task_loop = TASK_LOOP;
#else /* TASK_LOOP */
#ifdef MEASURE_TWICE
task_loop = LOOP_REF;
SVC_PERROR(get_tim(&stime1));
for (i = 0; i < task_loop; i++);
SVC_PERROR(get_tim(&stime2));
#endif /* MEASURE_TWICE */
task_loop = LOOP_REF;
SVC_PERROR(get_tim(&stime1));
for (i = 0; i < task_loop; i++);
SVC_PERROR(get_tim(&stime2));
task_loop = LOOP_REF * 400UL / (stime2 - stime1);
#endif /* TASK_LOOP */
tex_loop = task_loop / 5;
/*
* activate the target tasks
*/
SVC_PERROR(act_tsk(TASK1));
SVC_PERROR(act_tsk(TASK2));
SVC_PERROR(act_tsk(TASK3));
/*
* main loop
*/
do {
SVC_PERROR(serial_rea_dat(TASK_PORTID, &c, 1));
switch (c) {
case 'e':
case 's':
case 'S':
case 'd':
case 'y':
case 'Y':
case 'z':
case 'Z':
message[tskno-1] = c;
break;
case '1':
tskno = 1;
tskid = TASK1;
break;
case '2':
tskno = 2;
tskid = TASK2;
break;
case '3':
tskno = 3;
tskid = TASK3;
break;
case 'a':
syslog(LOG_INFO, "#act_tsk(%d)", tskno);
SVC_PERROR(act_tsk(tskid));
break;
case 'A':
syslog(LOG_INFO, "#can_act(%d)", tskno);
SVC_PERROR(ercd = can_act(tskid));
if (ercd >= 0) {
syslog(LOG_NOTICE, "can_act(%d) returns %d", tskno, ercd);
}
break;
case 't':
syslog(LOG_INFO, "#ter_tsk(%d)", tskno);
SVC_PERROR(ter_tsk(tskid));
break;
case '>':
syslog(LOG_INFO, "#chg_pri(%d, HIGH_PRIORITY)", tskno);
SVC_PERROR(chg_pri(tskid, HIGH_PRIORITY));
break;
case '=':
syslog(LOG_INFO, "#chg_pri(%d, MID_PRIORITY)", tskno);
SVC_PERROR(chg_pri(tskid, MID_PRIORITY));
break;
case '<':
syslog(LOG_INFO, "#chg_pri(%d, LOW_PRIORITY)", tskno);
SVC_PERROR(chg_pri(tskid, LOW_PRIORITY));
break;
case 'G':
syslog(LOG_INFO, "#get_pri(%d, &tskpri)", tskno);
SVC_PERROR(ercd = get_pri(tskid, &tskpri));
if (ercd >= 0) {
syslog(LOG_NOTICE, "priority of task %d is %d", tskno, tskpri);
}
break;
case 'w':
syslog(LOG_INFO, "#wup_tsk(%d)", tskno);
SVC_PERROR(wup_tsk(tskid));
break;
case 'W':
syslog(LOG_INFO, "#can_wup(%d)", tskno);
SVC_PERROR(ercd = can_wup(tskid));
if (ercd >= 0) {
syslog(LOG_NOTICE, "can_wup(%d) returns %d", tskno, ercd);
}
break;
case 'l':
syslog(LOG_INFO, "#rel_wai(%d)", tskno);
SVC_PERROR(rel_wai(tskid));
break;
case 'u':
syslog(LOG_INFO, "#sus_tsk(%d)", tskno);
SVC_PERROR(sus_tsk(tskid));
break;
case 'm':
syslog(LOG_INFO, "#rsm_tsk(%d)", tskno);
SVC_PERROR(rsm_tsk(tskid));
break;
case 'x':
syslog(LOG_INFO, "#ras_tex(%d, 0x0001U)", tskno);
SVC_PERROR(ras_tex(tskid, 0x0001U));
break;
case 'X':
syslog(LOG_INFO, "#ras_tex(%d, 0x0002U)", tskno);
SVC_PERROR(ras_tex(tskid, 0x0002U));
break;
case 'r':
syslog(LOG_INFO, "#rot_rdq(three priorities)");
SVC_PERROR(rot_rdq(HIGH_PRIORITY));
SVC_PERROR(rot_rdq(MID_PRIORITY));
SVC_PERROR(rot_rdq(LOW_PRIORITY));
break;
case 'c':
syslog(LOG_INFO, "#sta_cyc(1)");
SVC_PERROR(sta_cyc(CYCHDR1));
break;
case 'C':
syslog(LOG_INFO, "#stp_cyc(1)");
SVC_PERROR(stp_cyc(CYCHDR1));
break;
case 'b':
syslog(LOG_INFO, "#sta_alm(1, 5000)");
SVC_PERROR(sta_alm(ALMHDR1, 5000));
break;
case 'B':
syslog(LOG_INFO, "#stp_alm(1)");
SVC_PERROR(stp_alm(ALMHDR1));
break;
case 'V':
#ifdef TOPPERS_SUPPORT_GET_UTM
SVC_PERROR(get_utm(&utime1));
SVC_PERROR(get_utm(&utime2));
syslog(LOG_NOTICE, "utime1 = %ld, utime2 = %ld",
(ulong_t) utime1, (ulong_t) utime2);
#else /* TOPPERS_SUPPORT_GET_UTM */
syslog(LOG_NOTICE, "get_utm is not supported.");
#endif /* TOPPERS_SUPPORT_GET_UTM */
break;
case 'v':
SVC_PERROR(syslog_msk_log(LOG_UPTO(LOG_INFO),
LOG_UPTO(LOG_EMERG)));
break;
case 'q':
SVC_PERROR(syslog_msk_log(LOG_UPTO(LOG_NOTICE),
LOG_UPTO(LOG_EMERG)));
break;
#ifdef BIT_KERNEL
case ' ':
SVC_PERROR(loc_cpu());
{
extern ER bit_kernel(void);
SVC_PERROR(ercd = bit_kernel());
if (ercd >= 0) {
syslog(LOG_NOTICE, "bit_kernel passed.");
}
}
SVC_PERROR(unl_cpu());
break;
#endif /* BIT_KERNEL */
default:
break;
}
} while (c != '\003' && c != 'Q');
syslog(LOG_NOTICE, "Sample program ends.");
SVC_PERROR(ext_ker());
assert(0);
}
/*
* メインタスク
*/
void main_task(intptr_t exinf)
{
char_t c;
ID tskid = TASK1;
int_t tskno = 1;
ER_UINT ercd;
PRI tskpri;
#ifndef TASK_LOOP
volatile ulong_t i;
SYSTIM stime1, stime2;
#endif /* TASK_LOOP */
#ifdef TOPPERS_SUPPORT_GET_UTM
SYSUTM utime1, utime2;
#endif /* TOPPERS_SUPPORT_GET_UTM */
SVC_PERROR(syslog_msk_log(LOG_UPTO(LOG_INFO), LOG_UPTO(LOG_EMERG)));
syslog(LOG_NOTICE, "Sample program starts (exinf = %d).", (int_t) exinf);
/*
* シリアルポートの初期化
*
* システムログタスクと同じシリアルポートを使う場合など，シリアル
* ポートがオープン済みの場合にはここでE_OBJエラーになるが，支障は
* ない．
*/
ercd = serial_opn_por(TASK_PORTID);
if (ercd < 0 && MERCD(ercd) != E_OBJ) {
syslog(LOG_ERROR, "%s (%d) reported by `serial_opn_por'.",
itron_strerror(ercd), SERCD(ercd));
}
SVC_PERROR(serial_ctl_por(TASK_PORTID,
(IOCTL_CRLF | IOCTL_FCSND | IOCTL_FCRCV)));
/*
* ループ回数の設定
*
* TASK_LOOPがマクロ定義されている場合，測定せずに，TASK_LOOPに定
* 義された値を，タスク内でのループ回数とする．
*
* MEASURE_TWICEがマクロ定義されている場合，1回目の測定結果を捨て
* て，2回目の測定結果を使う．1回目の測定は長めの時間が出るため．
*/
#ifdef TASK_LOOP
task_loop = TASK_LOOP;
#else /* TASK_LOOP */
#ifdef MEASURE_TWICE
task_loop = LOOP_REF;
SVC_PERROR(get_tim(&stime1));
for (i = 0; i < task_loop; i++);
SVC_PERROR(get_tim(&stime2));
#endif /* MEASURE_TWICE */
task_loop = LOOP_REF;
SVC_PERROR(get_tim(&stime1));
for (i = 0; i < task_loop; i++);
SVC_PERROR(get_tim(&stime2));
task_loop = LOOP_REF * 400UL / (stime2 - stime1);
#endif /* TASK_LOOP */
tex_loop = task_loop / 5;
/*
* タスクの起動
*/
SVC_PERROR(act_tsk(TASK1));
SVC_PERROR(act_tsk(TASK2));
SVC_PERROR(act_tsk(TASK3));
/*
* メインループ
*/
do {
SVC_PERROR(serial_rea_dat(TASK_PORTID, &c, 1));
switch (c) {
case 'e':
case 's':
case 'S':
case 'd':
case 'y':
case 'Y':
case 'z':
case 'Z':
message[tskno-1] = c;
break;
case '1':
tskno = 1;
tskid = TASK1;
break;
case '2':
tskno = 2;
tskid = TASK2;
break;
case '3':
tskno = 3;
tskid = TASK3;
break;
case 'a':
syslog(LOG_INFO, "#act_tsk(%d)", tskno);
SVC_PERROR(act_tsk(tskid));
break;
case 'A':
syslog(LOG_INFO, "#can_act(%d)", tskno);
SVC_PERROR(ercd = can_act(tskid));
if (ercd >= 0) {
syslog(LOG_NOTICE, "can_act(%d) returns %d", tskno, ercd);
}
break;
case 't':
syslog(LOG_INFO, "#ter_tsk(%d)", tskno);
SVC_PERROR(ter_tsk(tskid));
break;
case '>':
syslog(LOG_INFO, "#chg_pri(%d, HIGH_PRIORITY)", tskno);
SVC_PERROR(chg_pri(tskid, HIGH_PRIORITY));
break;
case '=':
syslog(LOG_INFO, "#chg_pri(%d, MID_PRIORITY)", tskno);
SVC_PERROR(chg_pri(tskid, MID_PRIORITY));
break;
case '<':
syslog(LOG_INFO, "#chg_pri(%d, LOW_PRIORITY)", tskno);
SVC_PERROR(chg_pri(tskid, LOW_PRIORITY));
break;
case 'G':
syslog(LOG_INFO, "#get_pri(%d, &tskpri)", tskno);
SVC_PERROR(ercd = get_pri(tskid, &tskpri));
if (ercd >= 0) {
syslog(LOG_NOTICE, "priority of task %d is %d", tskno, tskpri);
}
break;
case 'w':
syslog(LOG_INFO, "#wup_tsk(%d)", tskno);
SVC_PERROR(wup_tsk(tskid));
break;
case 'W':
syslog(LOG_INFO, "#can_wup(%d)", tskno);
SVC_PERROR(ercd = can_wup(tskid));
if (ercd >= 0) {
syslog(LOG_NOTICE, "can_wup(%d) returns %d", tskno, ercd);
}
break;
case 'l':
syslog(LOG_INFO, "#rel_wai(%d)", tskno);
SVC_PERROR(rel_wai(tskid));
break;
case 'u':
syslog(LOG_INFO, "#sus_tsk(%d)", tskno);
SVC_PERROR(sus_tsk(tskid));
break;
case 'm':
syslog(LOG_INFO, "#rsm_tsk(%d)", tskno);
SVC_PERROR(rsm_tsk(tskid));
break;
case 'x':
syslog(LOG_INFO, "#ras_tex(%d, 0x0001U)", tskno);
SVC_PERROR(ras_tex(tskid, 0x0001U));
break;
case 'X':
syslog(LOG_INFO, "#ras_tex(%d, 0x0002U)", tskno);
SVC_PERROR(ras_tex(tskid, 0x0002U));
break;
case 'r':
syslog(LOG_INFO, "#rot_rdq(three priorities)");
SVC_PERROR(rot_rdq(HIGH_PRIORITY));
SVC_PERROR(rot_rdq(MID_PRIORITY));
SVC_PERROR(rot_rdq(LOW_PRIORITY));
break;
case 'c':
syslog(LOG_INFO, "#sta_cyc(1)");
SVC_PERROR(sta_cyc(CYCHDR1));
break;
case 'C':
syslog(LOG_INFO, "#stp_cyc(1)");
SVC_PERROR(stp_cyc(CYCHDR1));
break;
case 'b':
syslog(LOG_INFO, "#sta_alm(1, 5000)");
SVC_PERROR(sta_alm(ALMHDR1, 5000));
break;
case 'B':
syslog(LOG_INFO, "#stp_alm(1)");
SVC_PERROR(stp_alm(ALMHDR1));
break;
case 'V':
#ifdef TOPPERS_SUPPORT_GET_UTM
SVC_PERROR(get_utm(&utime1));
SVC_PERROR(get_utm(&utime2));
syslog(LOG_NOTICE, "utime1 = %ld, utime2 = %ld",
(ulong_t) utime1, (ulong_t) utime2);
#else /* TOPPERS_SUPPORT_GET_UTM */
syslog(LOG_NOTICE, "get_utm is not supported.");
#endif /* TOPPERS_SUPPORT_GET_UTM */
break;
case 'v':
SVC_PERROR(syslog_msk_log(LOG_UPTO(LOG_INFO),
LOG_UPTO(LOG_EMERG)));
break;
case 'q':
SVC_PERROR(syslog_msk_log(LOG_UPTO(LOG_NOTICE),
LOG_UPTO(LOG_EMERG)));
break;
#ifdef BIT_KERNEL
case ' ':
SVC_PERROR(loc_cpu());
{
extern ER bit_kernel(void);
SVC_PERROR(ercd = bit_kernel());
if (ercd >= 0) {
syslog(LOG_NOTICE, "bit_kernel passed.");
}
}
SVC_PERROR(unl_cpu());
break;
#endif /* BIT_KERNEL */
default:
break;
}
} while (c != '\003' && c != 'Q');
syslog(LOG_NOTICE, "Sample program ends.");
SVC_PERROR(ext_ker());
assert(0);
}
void task1( unsigned int arg )
{
ER ercd;
int tests = 0;
CYG_TEST_INFO( "Task 1 running" );
ercd = dis_dsp();
CYG_TEST_CHECK( E_OK == ercd, "dis_dsp bad ercd" );
ercd = sta_tsk( 2, 22222 );
CYG_TEST_CHECK( E_OK == ercd, "sta_tsk bad ercd" );
ercd = chg_pri( 2, 5 );
CYG_TEST_CHECK( E_OK == ercd, "chg_pri bad ercd" );
ercd = ena_dsp();
CYG_TEST_CHECK( E_OK == ercd, "ena_dsp bad ercd" );
ercd = dly_tsk( 10 );
CYG_TEST_CHECK( E_OK == ercd, "dly_tsk bad ercd" );
#ifdef CYGPKG_UITRON_SEMAS_CREATE_DELETE
tests++;
#ifdef CYGSEM_UITRON_BAD_PARAMS_RETURN_ERRORS
ercd = del_sem( -6 );
CYG_TEST_CHECK( E_ID == ercd, "del_sem bad ercd !E_ID" );
ercd = del_sem( 99 );
CYG_TEST_CHECK( E_ID == ercd, "del_sem bad ercd !E_ID" );
ercd = cre_sem( -6, &t_csem );
CYG_TEST_CHECK( E_ID == ercd, "cre_sem bad ercd !E_ID" );
ercd = cre_sem( 99, &t_csem );
CYG_TEST_CHECK( E_ID == ercd, "cre_sem bad ercd !E_ID" );
#endif // we can test bad param error returns
// try a pre-existing object
ercd = cre_sem( 3, &t_csem );
CYG_TEST_CHECK( E_OBJ == ercd, "cre_sem bad ercd !E_OBJ" );
// delete it so we can play
ercd = del_sem( 3 );
CYG_TEST_CHECK( E_OK == ercd, "del_sem bad ercd" );
// check it is deleted
ercd = sig_sem( 3 );
CYG_TEST_CHECK( E_NOEXS == ercd, "sig_sem bad ercd !E_NOEXS" );
ercd = preq_sem( 3 );
CYG_TEST_CHECK( E_NOEXS == ercd, "preq_sem bad ercd !E_NOEXS" );
ercd = twai_sem( 3, 10 );
CYG_TEST_CHECK( E_NOEXS == ercd, "twai_sem bad ercd !E_NOEXS" );
ercd = wai_sem( 3 );
CYG_TEST_CHECK( E_NOEXS == ercd, "wai_sem bad ercd !E_NOEXS" );
ercd = ref_sem( &t_rsem, 3 );
CYG_TEST_CHECK( E_NOEXS == ercd, "ref_sem bad ercd !E_NOEXS" );
#ifdef CYGSEM_UITRON_BAD_PARAMS_RETURN_ERRORS
// now try creating it (badly)
#ifndef CYGSEM_UITRON_PARAMS_NULL_IS_GOOD_PTR
ercd = cre_sem( 3, NULL );
CYG_TEST_CHECK( E_PAR == ercd, "cre_sem bad ercd !E_PAR" );
#endif
ercd = cre_sem( 3, NADR );
CYG_TEST_CHECK( E_PAR == ercd, "cre_sem bad ercd !E_PAR" );
t_csem.sematr = 0xfff;
ercd = cre_sem( 3, &t_csem );
CYG_TEST_CHECK( E_RSATR == ercd, "cre_sem bad ercd !E_RSATR" );
t_csem.sematr = 0;
#endif // we can test bad param error returns
ercd = cre_sem( 3, &t_csem );
CYG_TEST_CHECK( E_OK == ercd, "cre_sem bad ercd" );
// and check we can use it
ercd = sig_sem( 3 );
CYG_TEST_CHECK( E_OK == ercd, "sig_sem bad ercd" );
ercd = wai_sem( 3 );
CYG_TEST_CHECK( E_OK == ercd, "wai_sem bad ercd" );
ercd = preq_sem( 3 );
CYG_TEST_CHECK( E_TMOUT == ercd, "preq_sem bad ercd !E_TMOUT" );
ercd = twai_sem( 3, 2 );
CYG_TEST_CHECK( E_TMOUT == ercd, "twai_sem bad ercd !E_TMOUT" );
ercd = ref_sem( &t_rsem, 3 );
CYG_TEST_CHECK( E_OK == ercd, "ref_sem bad ercd" );
// now wait while task 2 deletes the wait objects
ercd = wup_tsk( 2 );
CYG_TEST_CHECK( E_OK == ercd, "wup_tsk bad ercd" );
ercd = wai_sem( 1 );
CYG_TEST_CHECK( E_DLT == ercd, "wai_sem bad ercd !E_DLT" );
ercd = wup_tsk( 2 );
CYG_TEST_CHECK( E_OK == ercd, "wup_tsk bad ercd" );
ercd = twai_sem( 2, 20 );
CYG_TEST_CHECK( E_DLT == ercd, "twai_sem bad ercd !E_DLT" );
// check they are deleted
ercd = sig_sem( 1 );
CYG_TEST_CHECK( E_NOEXS == ercd, "sig_sem bad ercd !E_NOEXS" );
ercd = sig_sem( 2 );
CYG_TEST_CHECK( E_NOEXS == ercd, "sig_sem bad ercd !E_NOEXS" );
// re-create and do it again
ercd = cre_sem( 1, &t_csem );
CYG_TEST_CHECK( E_OK == ercd, "cre_sem bad ercd" );
ercd = cre_sem( 2, &t_csem );
CYG_TEST_CHECK( E_OK == ercd, "cre_sem bad ercd" );
// now wait while task 2 deletes the wait objects again
ercd = wup_tsk( 2 );
CYG_TEST_CHECK( E_OK == ercd, "wup_tsk bad ercd" );
ercd = wai_sem( 1 );
CYG_TEST_CHECK( E_DLT == ercd, "wai_sem bad ercd !E_DLT" );
ercd = wup_tsk( 2 );
CYG_TEST_CHECK( E_OK == ercd, "wup_tsk bad ercd" );
ercd = twai_sem( 2, 20 );
CYG_TEST_CHECK( E_DLT == ercd, "twai_sem bad ercd !E_DLT" );
// check they are deleted
ercd = sig_sem( 1 );
CYG_TEST_CHECK( E_NOEXS == ercd, "sig_sem bad ercd !E_NOEXS" );
ercd = sig_sem( 2 );
CYG_TEST_CHECK( E_NOEXS == ercd, "sig_sem bad ercd !E_NOEXS" );
CYG_TEST_PASS("create/delete semaphores");
#endif // CYGPKG_UITRON_SEMAS_CREATE_DELETE
#ifdef CYGPKG_UITRON_FLAGS_CREATE_DELETE
tests++;
#ifdef CYGSEM_UITRON_BAD_PARAMS_RETURN_ERRORS
ercd = del_flg( -6 );
CYG_TEST_CHECK( E_ID == ercd, "del_flg bad ercd !E_ID" );
ercd = del_flg( 99 );
CYG_TEST_CHECK( E_ID == ercd, "del_flg bad ercd !E_ID" );
ercd = cre_flg( -6, &t_cflg );
CYG_TEST_CHECK( E_ID == ercd, "cre_flg bad ercd !E_ID" );
ercd = cre_flg( 99, &t_cflg );
CYG_TEST_CHECK( E_ID == ercd, "cre_flg bad ercd !E_ID" );
#endif // we can test bad param error returns
// try a pre-existing object
ercd = cre_flg( 3, &t_cflg );
CYG_TEST_CHECK( E_OBJ == ercd, "cre_flg bad ercd !E_OBJ" );
// delete it so we can play
ercd = del_flg( 3 );
CYG_TEST_CHECK( E_OK == ercd, "del_flg bad ercd" );
// check it is deleted
ercd = set_flg( 3, 0x6789 );
CYG_TEST_CHECK( E_NOEXS == ercd, "set_flg bad ercd !E_NOEXS" );
ercd = clr_flg( 3, 0x9876 );
CYG_TEST_CHECK( E_NOEXS == ercd, "clr_flg bad ercd !E_NOEXS" );
ercd = pol_flg( &scratch, 3, 0xdddd, TWF_ANDW );
CYG_TEST_CHECK( E_NOEXS == ercd, "pol_flg bad ercd !E_NOEXS" );
ercd = twai_flg( &scratch, 3, 0x4444, TWF_ORW, 10 );
CYG_TEST_CHECK( E_NOEXS == ercd, "twai_flg bad ercd !E_NOEXS" );
ercd = wai_flg( &scratch, 3, 0xbbbb, TWF_ANDW | TWF_CLR );
CYG_TEST_CHECK( E_NOEXS == ercd, "wai_flg bad ercd !E_NOEXS" );
ercd = ref_flg( &t_rflg, 3 );
CYG_TEST_CHECK( E_NOEXS == ercd, "ref_flg bad ercd !E_NOEXS" );
#ifdef CYGSEM_UITRON_BAD_PARAMS_RETURN_ERRORS
// now try creating it (badly)
#ifndef CYGSEM_UITRON_PARAMS_NULL_IS_GOOD_PTR
ercd = cre_flg( 3, NULL );
CYG_TEST_CHECK( E_PAR == ercd, "cre_flg bad ercd !E_PAR" );
#endif
ercd = cre_flg( 3, NADR );
CYG_TEST_CHECK( E_PAR == ercd, "cre_flg bad ercd !E_PAR" );
t_cflg.flgatr = 0xfff;
ercd = cre_flg( 3, &t_cflg );
CYG_TEST_CHECK( E_RSATR == ercd, "cre_flg bad ercd !E_RSATR" );
#endif // we can test bad param error returns
// now create it well
t_cflg.flgatr = 0;
t_cflg.iflgptn = 0;
ercd = cre_flg( 3, &t_cflg );
CYG_TEST_CHECK( E_OK == ercd, "cre_flg bad ercd" );
// and check we can use it
ercd = clr_flg( 3, 0x7256 );
CYG_TEST_CHECK( E_OK == ercd, "clr_flg bad ercd" );
ercd = set_flg( 3, 0xff );
CYG_TEST_CHECK( E_OK == ercd, "set_flg bad ercd" );
ercd = wai_flg( &scratch, 3, 0xaa, TWF_ANDW | TWF_CLR );
CYG_TEST_CHECK( E_OK == ercd, "wai_flg bad ercd" );
ercd = pol_flg( &scratch, 3, 0xaa, TWF_ANDW | TWF_CLR );
CYG_TEST_CHECK( E_TMOUT == ercd, "pol_flg bad ercd !E_TMOUT" );
ercd = twai_flg( &scratch, 3, 0xaa, TWF_ANDW | TWF_CLR, 2 );
CYG_TEST_CHECK( E_TMOUT == ercd, "twai_flg bad ercd !E_TMOUT" );
ercd = ref_flg( &t_rflg, 3 );
CYG_TEST_CHECK( E_OK == ercd, "ref_flg bad ercd" );
CYG_TEST_CHECK( 0 == t_rflg.flgptn, "ref_flg bad ercd" );
// now create it again with a preset pattern and check that we can
// detect that pattern:
ercd = del_flg( 3 );
CYG_TEST_CHECK( E_OK == ercd, "del_flg bad ercd" );
t_cflg.flgatr = 0;
t_cflg.iflgptn = 0x1234;
ercd = cre_flg( 3, &t_cflg );
CYG_TEST_CHECK( E_OK == ercd, "cre_flg bad ercd" );
// and check we can use it
ercd = wai_flg( &scratch, 3, 0x1200, TWF_ANDW );
CYG_TEST_CHECK( E_OK == ercd, "wai_flg bad ercd" );
ercd = pol_flg( &scratch, 3, 0x0034, TWF_ANDW );
CYG_TEST_CHECK( E_OK == ercd, "pol_flg bad ercd" );
ercd = twai_flg( &scratch, 3, 0x1004, TWF_ANDW, 10 );
CYG_TEST_CHECK( E_OK == ercd, "twai_flg bad ercd" );
ercd = pol_flg( &scratch, 3, 0xffedcb, TWF_ORW );
CYG_TEST_CHECK( E_TMOUT == ercd, "pol_flg bad ercd !E_TMOUT" );
ercd = ref_flg( &t_rflg, 3 );
CYG_TEST_CHECK( E_OK == ercd, "ref_flg bad ercd" );
CYG_TEST_CHECK( 0x1234 == t_rflg.flgptn, "ref_flg bad ercd" );
ercd = clr_flg( 3, 0 );
ercd = ref_flg( &t_rflg, 3 );
CYG_TEST_CHECK( E_OK == ercd, "ref_flg bad ercd" );
CYG_TEST_CHECK( 0 == t_rflg.flgptn, "ref_flg bad ercd" );
// now wait while task 2 deletes the wait objects
ercd = wup_tsk( 2 );
CYG_TEST_CHECK( E_OK == ercd, "wup_tsk bad ercd" );
ercd = wai_flg( &scratch, 1, 0xaa, TWF_ANDW );
CYG_TEST_CHECK( E_DLT == ercd, "wai_flg bad ercd !E_DLT" );
ercd = wup_tsk( 2 );
CYG_TEST_CHECK( E_OK == ercd, "wup_tsk bad ercd" );
ercd = twai_flg( &scratch, 2, 0x55, TWF_ANDW, 20 );
CYG_TEST_CHECK( E_DLT == ercd, "twai_flg bad ercd !E_DLT" );
// check they are deleted
ercd = set_flg( 1, 0x22 );
CYG_TEST_CHECK( E_NOEXS == ercd, "set_flg bad ercd !E_NOEXS" );
ercd = clr_flg( 2, 0xdd );
CYG_TEST_CHECK( E_NOEXS == ercd, "clr_flg bad ercd !E_NOEXS" );
// re-create and do it again
t_cflg.iflgptn = 0x5555;
ercd = cre_flg( 1, &t_cflg );
CYG_TEST_CHECK( E_OK == ercd, "cre_flg bad ercd" );
t_cflg.iflgptn = 0;
ercd = cre_flg( 2, &t_cflg );
CYG_TEST_CHECK( E_OK == ercd, "cre_flg bad ercd" );
// now wait while task 2 deletes the wait objects again
ercd = wup_tsk( 2 );
CYG_TEST_CHECK( E_OK == ercd, "wup_tsk bad ercd" );
ercd = wai_flg( &scratch, 1, 0xaaaa, TWF_ORW | TWF_CLR );
CYG_TEST_CHECK( E_DLT == ercd, "wai_flg bad ercd !E_DLT" );
ercd = wup_tsk( 2 );
CYG_TEST_CHECK( E_OK == ercd, "wup_tsk bad ercd" );
ercd = twai_flg( &scratch, 2, 0xffff, TWF_ORW, 20 );
CYG_TEST_CHECK( E_DLT == ercd, "twai_flg bad ercd !E_DLT" );
// check they are deleted
ercd = clr_flg( 1, 0xd00d );
CYG_TEST_CHECK( E_NOEXS == ercd, "clr_flg bad ercd !E_NOEXS" );
ercd = set_flg( 2, 0xfff00 );
CYG_TEST_CHECK( E_NOEXS == ercd, "set_flg bad ercd !E_NOEXS" );
CYG_TEST_PASS("create/delete flags");
#endif // CYGPKG_UITRON_FLAGS_CREATE_DELETE
#ifdef CYGPKG_UITRON_MBOXES_CREATE_DELETE
tests++;
#ifdef CYGSEM_UITRON_BAD_PARAMS_RETURN_ERRORS
ercd = del_mbx( -6 );
CYG_TEST_CHECK( E_ID == ercd, "del_mbx bad ercd !E_ID" );
ercd = del_mbx( 99 );
CYG_TEST_CHECK( E_ID == ercd, "del_mbx bad ercd !E_ID" );
ercd = cre_mbx( -6, &t_cmbx );
CYG_TEST_CHECK( E_ID == ercd, "cre_mbx bad ercd !E_ID" );
ercd = cre_mbx( 99, &t_cmbx );
CYG_TEST_CHECK( E_ID == ercd, "cre_mbx bad ercd !E_ID" );
#endif // we can test bad param error returns
// try a pre-existing object
ercd = cre_mbx( 3, &t_cmbx );
CYG_TEST_CHECK( E_OBJ == ercd, "cre_mbx bad ercd !E_OBJ" );
// delete it so we can play
ercd = del_mbx( 3 );
CYG_TEST_CHECK( E_OK == ercd, "del_mbx bad ercd" );
// check it is deleted
ercd = snd_msg( 3, t_msg );
CYG_TEST_CHECK( E_NOEXS == ercd, "snd_msg bad ercd !E_NOEXS" );
ercd = rcv_msg( &msg, 3 );
CYG_TEST_CHECK( E_NOEXS == ercd, "rcv_msg bad ercd !E_NOEXS" );
ercd = trcv_msg( &msg, 3, 10 );
CYG_TEST_CHECK( E_NOEXS == ercd, "trcv_msg bad ercd !E_NOEXS" );
ercd = prcv_msg( &msg, 3 );
CYG_TEST_CHECK( E_NOEXS == ercd, "prcv_msg bad ercd !E_NOEXS" );
ercd = ref_mbx( &t_rmbx, 3 );
CYG_TEST_CHECK( E_NOEXS == ercd, "ref_mbx bad ercd !E_NOEXS" );
#ifdef CYGSEM_UITRON_BAD_PARAMS_RETURN_ERRORS
// now try creating it (badly)
#ifndef CYGSEM_UITRON_PARAMS_NULL_IS_GOOD_PTR
ercd = cre_mbx( 3, NULL );
CYG_TEST_CHECK( E_PAR == ercd, "cre_mbx bad ercd !E_PAR" );
#endif
ercd = cre_mbx( 3, NADR );
CYG_TEST_CHECK( E_PAR == ercd, "cre_mbx bad ercd !E_PAR" );
t_cmbx.mbxatr = 0xfff;
ercd = cre_mbx( 3, &t_cmbx );
CYG_TEST_CHECK( E_RSATR == ercd, "cre_mbx bad ercd !E_RSATR" );
t_cmbx.mbxatr = 0;
#endif // we can test bad param error returns
ercd = cre_mbx( 3, &t_cmbx );
CYG_TEST_CHECK( E_OK == ercd, "cre_mbx bad ercd" );
// and check we can use it
ercd = snd_msg( 3, t_msg );
CYG_TEST_CHECK( E_OK == ercd, "snd_msg bad ercd" );
ercd = rcv_msg( &msg, 3 );
CYG_TEST_CHECK( E_OK == ercd, "rcv_msg bad ercd" );
ercd = trcv_msg( &msg, 3, 2 );
CYG_TEST_CHECK( E_TMOUT == ercd, "trcv_msg bad ercd !E_TMOUT" );
ercd = prcv_msg( &msg, 3 );
CYG_TEST_CHECK( E_TMOUT == ercd, "prcv_msg bad ercd !E_TMOUT" );
ercd = ref_mbx( &t_rmbx, 3 );
CYG_TEST_CHECK( E_OK == ercd, "ref_mbx bad ercd" );
// now wait while task 2 deletes the wait objects
ercd = wup_tsk( 2 );
CYG_TEST_CHECK( E_OK == ercd, "wup_tsk bad ercd" );
ercd = rcv_msg( &msg, 1 );
CYG_TEST_CHECK( E_DLT == ercd, "wai_mbx bad ercd !E_DLT" );
ercd = wup_tsk( 2 );
CYG_TEST_CHECK( E_OK == ercd, "wup_tsk bad ercd" );
ercd = trcv_msg( &msg, 2, 20 );
CYG_TEST_CHECK( E_DLT == ercd, "twai_mbx bad ercd !E_DLT" );
// check they are deleted
ercd = snd_msg( 1, t_msg );
CYG_TEST_CHECK( E_NOEXS == ercd, "snd_msg bad ercd !E_NOEXS" );
ercd = snd_msg( 2, t_msg );
CYG_TEST_CHECK( E_NOEXS == ercd, "snd_msg bad ercd !E_NOEXS" );
// re-create and do it again
ercd = cre_mbx( 1, &t_cmbx );
CYG_TEST_CHECK( E_OK == ercd, "cre_mbx bad ercd" );
ercd = cre_mbx( 2, &t_cmbx );
CYG_TEST_CHECK( E_OK == ercd, "cre_mbx bad ercd" );
// now wait while task 2 deletes the wait objects again
ercd = wup_tsk( 2 );
CYG_TEST_CHECK( E_OK == ercd, "wup_tsk bad ercd" );
ercd = rcv_msg( &msg, 1 );
CYG_TEST_CHECK( E_DLT == ercd, "wai_mbx bad ercd !E_DLT" );
ercd = wup_tsk( 2 );
CYG_TEST_CHECK( E_OK == ercd, "wup_tsk bad ercd" );
ercd = trcv_msg( &msg, 2, 20 );
CYG_TEST_CHECK( E_DLT == ercd, "twai_mbx bad ercd !E_DLT" );
// check they are deleted
ercd = snd_msg( 1, t_msg );
CYG_TEST_CHECK( E_NOEXS == ercd, "snd_msg bad ercd !E_NOEXS" );
ercd = snd_msg( 2, t_msg );
CYG_TEST_CHECK( E_NOEXS == ercd, "snd_msg bad ercd !E_NOEXS" );
CYG_TEST_PASS("create/delete mboxes");
#endif // CYGPKG_UITRON_MBOXES_CREATE_DELETE
ercd = ter_tsk( 2 );
CYG_TEST_CHECK( E_OK == ercd, "ter_tsk bad ercd" );
ercd = dly_tsk( 5 );
CYG_TEST_CHECK( E_OK == ercd, "dly_tsk bad ercd" );
// all done
if ( 0 == tests ) {
CYG_TEST_NA( "No objects have create/delete enabled" );
}
else {
CYG_TEST_EXIT( "All done" );
}
ext_tsk();
}
void task1( unsigned int arg )
{
ER ercd;
int i;
T_RSYS rsys;
CYG_TEST_INFO( "Task 1 running" );
// check initial state
ercd = ref_sys( &rsys );
CYG_TEST_CHECK( E_OK == ercd, "ref_sys bad ercd" );
CYG_TEST_CHECK( TSS_TSK == rsys.sysstat, "system state not TSS_TSK" );
// disable intrs and check state
ercd = loc_cpu();
CYG_TEST_CHECK( E_OK == ercd, "loc_cpu bad ercd" );
ercd = ref_sys( &rsys );
CYG_TEST_CHECK( E_OK == ercd, "ref_sys bad ercd" );
CYG_TEST_CHECK( TSS_LOC == rsys.sysstat, "system state not TSS_LOC" );
#ifdef CYGSEM_UITRON_BAD_PARAMS_RETURN_ERRORS
// try an illegal op
ercd = dly_tsk( 10 );
CYG_TEST_CHECK( E_CTX == ercd, "dly_tsk bad ercd !E_CTX" );
#endif // CYGSEM_UITRON_BAD_PARAMS_RETURN_ERRORS
// enable intrs and check state and a legal sleep
ercd = unl_cpu();
CYG_TEST_CHECK( E_OK == ercd, "unl_cpu bad ercd" );
ercd = ref_sys( &rsys );
CYG_TEST_CHECK( E_OK == ercd, "ref_sys bad ercd" );
CYG_TEST_CHECK( TSS_TSK == rsys.sysstat, "system state not TSS_TSK" );
ercd = dly_tsk( 1 );
CYG_TEST_CHECK( E_OK == ercd, "dly_tsk bad ercd" );
// disable intrs and try scheduler illegal ops
ercd = loc_cpu();
CYG_TEST_CHECK( E_OK == ercd, "loc_cpu bad ercd" );
ercd = ref_sys( &rsys );
CYG_TEST_CHECK( E_OK == ercd, "ref_sys bad ercd" );
CYG_TEST_CHECK( TSS_LOC == rsys.sysstat, "system state not TSS_LOC" );
#ifdef CYGSEM_UITRON_BAD_PARAMS_RETURN_ERRORS
ercd = dis_dsp();
CYG_TEST_CHECK( E_CTX == ercd, "dis_dsp bad ercd !E_CTX" );
ercd = ena_dsp();
CYG_TEST_CHECK( E_CTX == ercd, "ena_dsp bad ercd !E_CTX" );
#endif // CYGSEM_UITRON_BAD_PARAMS_RETURN_ERRORS
// enable again and check state
ercd = unl_cpu();
CYG_TEST_CHECK( E_OK == ercd, "unl_cpu bad ercd" );
ercd = ref_sys( &rsys );
CYG_TEST_CHECK( E_OK == ercd, "ref_sys bad ercd" );
CYG_TEST_CHECK( TSS_TSK == rsys.sysstat, "system state not TSS_TSK" );
// disable the scheduler and check state
ercd = dis_dsp();
CYG_TEST_CHECK( E_OK == ercd, "dis_dsp bad ercd" );
ercd = ref_sys( &rsys );
CYG_TEST_CHECK( E_OK == ercd, "ref_sys bad ercd" );
CYG_TEST_CHECK( TSS_DDSP == rsys.sysstat, "system state not TSS_DDSP" );
// disable intrs and check state
ercd = loc_cpu();
CYG_TEST_CHECK( E_OK == ercd, "loc_cpu bad ercd" );
ercd = ref_sys( &rsys );
CYG_TEST_CHECK( E_OK == ercd, "ref_sys bad ercd" );
CYG_TEST_CHECK( TSS_LOC == rsys.sysstat, "system state not TSS_LOC" );
// then unlock and check state
ercd = unl_cpu();
CYG_TEST_CHECK( E_OK == ercd, "unl_cpu bad ercd" );
ercd = ref_sys( &rsys );
CYG_TEST_CHECK( E_OK == ercd, "ref_sys bad ercd" );
CYG_TEST_CHECK( TSS_TSK == rsys.sysstat, "system state not TSS_TSK" );
CYG_TEST_PASS( "Interrupt dis/enabling and interactions" );
// and now we can do the rest of the test
#ifdef CYGSEM_UITRON_BAD_PARAMS_RETURN_ERRORS
ercd = rel_wai( 2 );
CYG_TEST_CHECK( E_OBJ == ercd, "rel_wai bad ercd !E_OBJ" );
ercd = rel_wai( 1 );
CYG_TEST_CHECK( E_OBJ == ercd, "rel_wai(me) bad ercd !E_OBJ" );
ercd = rel_wai( -6 );
CYG_TEST_CHECK( E_ID == ercd, "rel_wai bad ercd !E_ID" );
ercd = rel_wai( 99 );
CYG_TEST_CHECK( E_ID == ercd, "rel_wai bad ercd !E_ID" );
#endif // we can test bad param error returns
ercd = dis_dsp();
CYG_TEST_CHECK( E_OK == ercd, "dis_dsp bad ercd" );
ercd = sta_tsk( 2, 22222 );
CYG_TEST_CHECK( E_OK == ercd, "sta_tsk bad ercd" );
ercd = chg_pri( 2, 5 );
CYG_TEST_CHECK( E_OK == ercd, "chg_pri bad ercd" );
ercd = ena_dsp();
CYG_TEST_CHECK( E_OK == ercd, "ena_dsp bad ercd" );
#ifdef CYGSEM_UITRON_BAD_PARAMS_RETURN_ERRORS
ercd = rel_wai( 2 );
CYG_TEST_CHECK( E_OBJ == ercd, "rel_wai bad ercd !E_OBJ" );
ercd = rel_wai( 1 );
CYG_TEST_CHECK( E_OBJ == ercd, "rel_wai(me) bad ercd !E_OBJ" );
#endif // we can test bad param error returns
ercd = wai_sem( 1 );
CYG_TEST_CHECK( E_RLWAI == ercd, "wai_sem bad ercd !E_RLWAI" );
ercd = twai_sem( 1, 20 );
CYG_TEST_CHECK( E_RLWAI == ercd, "twai_sem bad ercd !E_RLWAI" );
ercd = wai_flg( &scratch, 1, 9999, 0 );
CYG_TEST_CHECK( E_RLWAI == ercd, "wai_flg bad ercd !E_RLWAI" );
ercd = twai_flg( &scratch, 1, 9999, 0, 10 );
CYG_TEST_CHECK( E_RLWAI == ercd, "twai_flg bad ercd !E_RLWAI" );
ercd = rcv_msg( &t_msg, 1 );
CYG_TEST_CHECK( E_RLWAI == ercd, "rcv_msg bad ercd !E_RLWAI" );
ercd = trcv_msg( &t_msg, 1, 10 );
CYG_TEST_CHECK( E_RLWAI == ercd, "trcv_msg bad ercd !E_RLWAI" );
// these are loops so as to consume the whole of the mempool
// in order to wait at the end
for ( i = 0; i < 10; i++ )
if ( E_OK != (ercd = get_blf( &vp, 3 ) ) )
break;
CYG_TEST_CHECK( E_RLWAI == ercd, "get_blf bad ercd !E_RLWAI" );
for ( i = 0; i < 10; i++ )
if ( E_OK != (ercd = tget_blf( &vp, 3, 10 ) ) )
break;
CYG_TEST_CHECK( E_RLWAI == ercd, "tget_blf bad ercd !E_RLWAI" );
for ( i = 0; i < 10; i++ )
if ( E_OK != (ercd = get_blk( &vp, 1, 1000 ) ) )
break;
CYG_TEST_CHECK( E_RLWAI == ercd, "get_blk bad ercd !E_RLWAI" );
for ( i = 0; i < 10; i++ )
if ( E_OK != (ercd = tget_blk( &vp, 1, 1000, 10 ) ) )
break;
CYG_TEST_CHECK( E_RLWAI == ercd, "tget_blk bad ercd !E_RLWAI" );
ercd = dly_tsk( 10 );
CYG_TEST_CHECK( E_RLWAI == ercd, "dly_tsk bad ercd !E_RLWAI" );
ercd = tslp_tsk( 10 );
CYG_TEST_CHECK( E_RLWAI == ercd, "tslp_tsk bad ercd !E_RLWAI" );
ercd = slp_tsk();
CYG_TEST_CHECK( E_RLWAI == ercd, "slp_tsk bad ercd !E_RLWAI" );
ercd = ter_tsk( 2 );
CYG_TEST_CHECK( E_OK == ercd, "ter_tsk bad ercd" );
ercd = dly_tsk( 10 );
CYG_TEST_CHECK( E_OK == ercd, "dly_tsk bad ercd" );
CYG_TEST_PASS("release wait: various waiting calls");
// all done
CYG_TEST_EXIT( "All done" );
ext_tsk();
}
/*
* メインタスク
*/
void main_task(VP_INT exinf)
{
char c;
ID tskid = TASK1;
volatile UW i;
INT tskno = 1;
ER_UINT ercd;
PRI tskpri;
SYSTIM stime1, stime2;
#ifndef OMIT_VGET_TIM
SYSUTIM utime1, utime2;
#endif /* OMIT_VGET_TIM */
vmsk_log(LOG_UPTO(LOG_INFO), LOG_UPTO(LOG_EMERG));
syslog(LOG_NOTICE, "Sample program starts (exinf = %d).", (int)exinf);
syscall(serial_ctl_por(TASK_PORTID,
(IOCTL_CRLF | IOCTL_FCSND | IOCTL_FCRCV)));
/*
* ループ回数の設定
*/
task_loop = LOOP_REF;
get_tim(&stime1);
for (i = 0; i < task_loop; i++);
get_tim(&stime2);
task_loop = LOOP_REF * 400 / (stime2 - stime1);
tex_loop = task_loop / 5;
/*
* タスクの起動
*/
act_tsk(TASK1);
act_tsk(TASK2);
act_tsk(TASK3);
/*
* メインループ
*/
do {
syscall(serial_rea_dat(TASK_PORTID, &c, 1));
switch (c) {
case 'e':
case 's':
case 'S':
case 'd':
case 'y':
case 'Y':
case 'z':
case 'Z':
message[tskno-1] = c;
break;
case '1':
tskno = 1;
tskid = TASK1;
break;
case '2':
tskno = 2;
tskid = TASK2;
break;
case '3':
tskno = 3;
tskid = TASK3;
break;
case 'a':
syslog(LOG_INFO, "#act_tsk(%d)", tskno);
syscall(act_tsk(tskid));
break;
case 'A':
syslog(LOG_INFO, "#can_act(%d)", tskno);
syscall(ercd = can_act(tskid));
if (ercd >= 0) {
syslog(LOG_NOTICE, "can_act(%d) returns %d",
tskno, ercd);
}
break;
case 't':
syslog(LOG_INFO, "#ter_tsk(%d)", tskno);
syscall(ter_tsk(tskid));
break;
case '>':
syslog(LOG_INFO, "#chg_pri(%d, HIGH_PRIORITY)", tskno);
chg_pri(tskid, HIGH_PRIORITY);
break;
case '=':
syslog(LOG_INFO, "#chg_pri(%d, MID_PRIORITY)", tskno);
chg_pri(tskid, MID_PRIORITY);
break;
case '<':
syslog(LOG_INFO, "#chg_pri(%d, LOW_PRIORITY)", tskno);
chg_pri(tskid, LOW_PRIORITY);
break;
case 'G':
syslog(LOG_INFO, "#get_pri(%d, &tskpri)", tskno);
syscall(ercd = get_pri(tskid, &tskpri));
if (ercd >= 0) {
syslog(LOG_NOTICE, "priority of task %d is %d",
tskno, tskpri);
}
break;
case 'w':
syslog(LOG_INFO, "#wup_tsk(%d)", tskno);
syscall(wup_tsk(tskid));
break;
case 'W':
syslog(LOG_INFO, "#can_wup(%d)", tskno);
syscall(ercd = can_wup(tskid));
if (ercd >= 0) {
syslog(LOG_NOTICE, "can_wup(%d) returns %d",
tskno, ercd);
}
break;
case 'l':
syslog(LOG_INFO, "#rel_wai(%d)", tskno);
syscall(rel_wai(tskid));
break;
case 'u':
syslog(LOG_INFO, "#sus_tsk(%d)", tskno);
syscall(sus_tsk(tskid));
break;
case 'm':
syslog(LOG_INFO, "#rsm_tsk(%d)", tskno);
syscall(rsm_tsk(tskid));
break;
case 'M':
syslog(LOG_INFO, "#frsm_tsk(%d)", tskno);
syscall(frsm_tsk(tskid));
break;
case 'x':
syslog(LOG_INFO, "#ras_tex(%d, 0x0001)", tskno);
syscall(ras_tex(tskid, 0x0001));
break;
case 'X':
syslog(LOG_INFO, "#ras_tex(%d, 0x0002)", tskno);
syscall(ras_tex(tskid, 0x0002));
break;
case 'r':
syslog(LOG_INFO, "#rot_rdq(three priorities)");
rot_rdq(HIGH_PRIORITY);
rot_rdq(MID_PRIORITY);
rot_rdq(LOW_PRIORITY);
break;
case 'c':
sta_cyc(CYCHDR1);
break;
case 'C':
stp_cyc(CYCHDR1);
break;
#ifndef OMIT_VGET_TIM
case 'V':
syscall(vxget_tim(&utime1));
syscall(vxget_tim(&utime2));
syslog(LOG_NOTICE, "utime1 = %d, utime2 = %d",
(UINT) utime1, (UINT) utime2);
break;
#endif /* OMIT_VGET_TIM */
case 'v':
vmsk_log(LOG_UPTO(LOG_INFO), LOG_UPTO(LOG_EMERG));
break;
case 'q':
vmsk_log(LOG_UPTO(LOG_NOTICE), LOG_UPTO(LOG_EMERG));
break;
default:
break;
}
} while (c != '\003' && c != 'Q');
syslog(LOG_NOTICE, "Sample program ends.");
kernel_exit();
}
void
task1(intptr_t exinf)
{
ER_UINT ercd;
test_start(__FILE__);
set_bit_func(bit_kernel);
check_point(1);
ercd = act_tsk(TASK2);
check_ercd(ercd, E_OK);
ercd = act_tsk(TASK3);
check_ercd(ercd, E_OK);
ercd = tslp_tsk(1000U);
check_ercd(ercd, E_TMOUT);
check_point(4);
ercd = ter_tsk(TASK2);
check_ercd(ercd, E_OK);
ercd = tslp_tsk(1000U);
check_ercd(ercd, E_TMOUT);
check_point(5);
ercd = rcv_mbf(MBF1, buf1);
check_ercd(ercd, 26);
check_assert(strncmp(buf1, string2, 26) == 0);
ercd = act_tsk(TASK2);
check_ercd(ercd, E_OK);
ercd = tslp_tsk(1000U);
check_ercd(ercd, E_TMOUT);
check_point(8);
ercd = ter_tsk(TASK2);
check_ercd(ercd, E_OK);
ercd = slp_tsk();
check_ercd(ercd, E_OK);
check_point(13);
ercd = snd_mbf(MBF1, string3, 10);
check_ercd(ercd, E_OK);
check_point(15);
ercd = rcv_mbf(MBF1, buf1);
check_ercd(ercd, 10);
check_assert(strncmp(buf1, string3, 10) == 0);
ercd = act_tsk(TASK2);
check_ercd(ercd, E_OK);
ercd = tslp_tsk(1000U);
check_ercd(ercd, E_TMOUT);
check_point(18);
ercd = rel_wai(TASK2);
check_ercd(ercd, E_OK);
ercd = tslp_tsk(1000U);
check_ercd(ercd, E_TMOUT);
check_point(20);
ercd = rcv_mbf(MBF1, buf1);
check_ercd(ercd, 26);
check_assert(strncmp(buf1, string2, 26) == 0);
ercd = wup_tsk(TASK2);
check_ercd(ercd, E_OK);
ercd = tslp_tsk(1000U);
check_ercd(ercd, E_TMOUT);
check_point(23);
ercd = rel_wai(TASK2);
check_ercd(ercd, E_OK);
ercd = slp_tsk();
check_ercd(ercd, E_OK);
check_point(29);
ercd = snd_mbf(MBF1, string3, 10);
check_ercd(ercd, E_OK);
check_point(31);
ercd = rcv_mbf(MBF1, buf1);
check_ercd(ercd, 10);
check_assert(strncmp(buf1, string3, 10) == 0);
ercd = tslp_tsk(1000U);
check_ercd(ercd, E_TMOUT);
check_point(34);
ercd = sta_alm(ALM1, 1000U);
check_ercd(ercd, E_OK);
ercd = slp_tsk();
check_ercd(ercd, E_OK);
check_point(36);
ercd = tslp_tsk(1000U);
check_ercd(ercd, E_TMOUT);
check_point(38);
ercd = rcv_mbf(MBF1, buf1);
check_ercd(ercd, 26);
check_assert(strncmp(buf1, string2, 26) == 0);
ercd = wup_tsk(TASK2);
check_ercd(ercd, E_OK);
ercd = tslp_tsk(1000U);
check_ercd(ercd, E_TMOUT);
check_point(41);
ercd = sta_alm(ALM1, 1000U);
check_ercd(ercd, E_OK);
ercd = slp_tsk();
check_ercd(ercd, E_OK);
check_point(48);
ercd = snd_mbf(MBF1, string3, 10);
check_ercd(ercd, E_OK);
check_point(51);
ercd = rcv_mbf(MBF1, buf1);
check_ercd(ercd, 10);
check_assert(strncmp(buf1, string3, 10) == 0);
ercd = wup_tsk(TASK3);
check_ercd(ercd, E_OK);
ercd = slp_tsk();
check_ercd(ercd, E_OK);
check_point(63);
ercd = snd_mbf(MBF1, string3, 10);
check_ercd(ercd, E_OK);
check_point(65);
ercd = rcv_mbf(MBF1, buf1);
check_ercd(ercd, 10);
check_assert(strncmp(buf1, string3, 10) == 0);
ercd = slp_tsk();
check_ercd(ercd, E_OK);
check_point(0);
}
