/*---------------------------------------------------------------------------*
* Routine: sys_mbox_post
*---------------------------------------------------------------------------*
* Description:
* Post the "msg" to the mailbox.
* Inputs:
* sys_mbox_t mbox -- Handle of mailbox
* void *msg -- Pointer to data to post
*---------------------------------------------------------------------------*/
void sys_mbox_post(sys_mbox_t *mbox, void *msg) {
// if (osMessagePut(mbox->id, (uint32_t)msg, osWaitForever) != osOK)
if (snd_dtq(mbox->id, (intptr_t)msg) != E_OK) {
error("sys_mbox_post error\n");
}
}
/*---------------------------------------------------------------------------*
* Routine: sys_mbox_trypost
*---------------------------------------------------------------------------*
* Description:
* Try to post the "msg" to the mailbox. Returns immediately with
* error if cannot.
* Inputs:
* sys_mbox_t mbox -- Handle of mailbox
* void *msg -- Pointer to data to post
* Outputs:
* err_t -- ERR_OK if message posted, else ERR_MEM
* if not.
*---------------------------------------------------------------------------*/
err_t sys_mbox_trypost(sys_mbox_t *mbox, void *msg) {
ER ercd;
// osStatus status = osMessagePut(mbox->id, (uint32_t)msg, 0);
ercd = snd_dtq(mbox->id, (intptr_t)msg);
// return (status == osOK) ? (ERR_OK) : (ERR_MEM);
return (ercd == E_OK) ? (ERR_OK) : (ERR_MEM);
}
void
ppp_cp_output (uint8_t code, uint8_t id, T_NET_BUF *output)
{
T_PPP_CP_HDR *cph;
/* CP ヘッダを設定する */
cph = GET_PPP_CP_HDR(output);
cph->code = code;
cph->id = id;
cph->len = htons(output->len - sizeof(T_PPP_HDR));
/* PPP 出力キューに投入する。*/
if (snd_dtq(DTQ_PPP_OUTPUT, output) != E_OK) {
syscall(rel_net_buf(output));
NET_COUNT_PPP(net_count_ppp.out_err_packets, 1);
}
}
void
if_loop_output_task (intptr_t exinf)
{
T_NET_BUF *output;
ER error;
ID tskid;
get_tid(&tskid);
syslog(LOG_NOTICE, "[LOOP OUTPUT:%d] started.", tskid);
while (true) {
if (rcv_dtq(DTQ_LOOP_OUTPUT, (intptr_t*)&output) == E_OK) {
NET_COUNT_LOOP(net_count_loop.in_octets, output->len);
NET_COUNT_LOOP(net_count_loop.in_packets, 1);
if ((error = snd_dtq(DTQ_LOOP_INPUT, output)) != E_OK)
syslog(LOG_NOTICE, "[LOOP OUTPUT] drop error: %s", itron_strerror(error));
}
}
}
void main_task(intptr_t unused) {
ev3_lcd_set_font(EV3_FONT_MEDIUM);
ev3_font_get_size(EV3_FONT_MEDIUM, &fontw, &fonth);
memfile_t memfile;
ev3_memfile_load("/test.bmp", &memfile);
image_t image;
ev3_image_load(&memfile, &image);
ev3_lcd_draw_image(&image, 0, fonth * 2);
//ev3_sta_cyc(TEST_EV3_CYC1);
// Enable TEST_EV3_CYC2 for 5 seconds
ev3_sta_cyc(TEST_EV3_CYC2);
tslp_tsk(5000);
ev3_stp_cyc(TEST_EV3_CYC2);
syslog(LOG_NOTICE, "TEST DTQ");
intptr_t data = 0xdeadbeef;
assert(snd_dtq(DTQ1, data) == E_OK);
data = 0;
assert(rcv_dtq(DTQ1, &data) == E_OK);
assert(data == 0xdeadbeef);
syslog(LOG_NOTICE, "TEST PDQ");
data = 0xdeadbeef;
assert(snd_pdq(PDQ1, data, 1) == E_OK);
data = 0xdeadbee2;
assert(snd_pdq(PDQ1, data, 2) == E_OK);
PRI datapri;
assert(rcv_pdq(PDQ1, &data, &datapri) == E_OK);
assert(data == 0xdeadbeef && datapri == 1);
assert(rcv_pdq(PDQ1, &data, &datapri) == E_OK);
assert(data == 0xdeadbee2 && datapri == 2);
syslog(LOG_NOTICE, "TEST MTX");
assert(loc_mtx(MTX1) == E_OK);
assert(unl_mtx(MTX1) == E_OK);
syslog(LOG_NOTICE, "TEST DONE");
#if 0
// Register button handlers
ev3_button_set_on_clicked(BACK_BUTTON, button_clicked_handler, BACK_BUTTON);
ev3_button_set_on_clicked(ENTER_BUTTON, button_clicked_handler, ENTER_BUTTON);
ev3_button_set_on_clicked(LEFT_BUTTON, button_clicked_handler, LEFT_BUTTON);
// Configure sensors
ev3_sensor_config(gyro_sensor, GYRO_SENSOR);
// Configure motors
ev3_motor_config(left_motor, LARGE_MOTOR);
ev3_motor_config(right_motor, LARGE_MOTOR);
// Start task for self-balancing
act_tsk(BALANCE_TASK);
// Open Bluetooth file
bt = ev3_serial_open_file(EV3_SERIAL_BT);
assert(bt != NULL);
// Start task for printing message while idle
act_tsk(IDLE_TASK);
while(1) {
uint8_t c = fgetc(bt);
sus_tsk(IDLE_TASK);
switch(c) {
case 'w':
if(motor_control_drive < 0)
motor_control_drive = 0;
else
motor_control_drive += 10;
fprintf(bt, "motor_control_drive: %d\n", motor_control_drive);
break;
case 's':
if(motor_control_drive > 0)
motor_control_drive = 0;
else
motor_control_drive -= 10;
fprintf(bt, "motor_control_drive: %d\n", motor_control_drive);
break;
case 'a':
if(motor_control_steer < 0)
motor_control_steer = 0;
else
motor_control_steer += 10;
fprintf(bt, "motor_control_steer: %d\n", motor_control_steer);
break;
case 'd':
if(motor_control_steer > 0)
motor_control_steer = 0;
else
motor_control_steer -= 10;
fprintf(bt, "motor_control_steer: %d\n", motor_control_steer);
break;
case 'h':
fprintf(bt, "==========================\n");
fprintf(bt, "Usage:\n");
fprintf(bt, "Press 'w' to speed up\n");
fprintf(bt, "Press 's' to speed down\n");
fprintf(bt, "Press 'a' to turn left\n");
fprintf(bt, "Press 'd' to turn right\n");
fprintf(bt, "Press 'i' for idle task\n");
fprintf(bt, "Press 'h' for this message\n");
fprintf(bt, "==========================\n");
break;
case 'i':
fprintf(bt, "Idle task started.\n");
rsm_tsk(IDLE_TASK);
break;
default:
fprintf(bt, "Unknown key '%c' pressed.\n", c);
}
}
#endif
}
