void test_cancel_cb() {
int disable;
int ferr;
short tleid1;
short tleid2;
if (verbose)
printf("cancel-cb-test\n");
ferr = timer_start_cb(4, 4, 4, &tleid1, &cbcan);
assert(ferr == XZFIL_ERR_OK);
ferr = timer_cancel(tleid1);
assert(ferr == XZFIL_ERR_OK);
disable = msg_test_assert_disable();
ferr = timer_cancel(tleid1);
assert(ferr == XZFIL_ERR_NOTFOUND);
msg_test_assert_enable(disable);
ferr = timer_start_cb(1, 1, 1, &tleid1, &cbcan);
assert(ferr == XZFIL_ERR_OK);
usleep(4000);
ferr = timer_cancel(tleid1);
assert(ferr == XZFIL_ERR_OK);
ferr = timer_start_cb(1, 1, 1, &tleid1, &cbcan);
assert(ferr == XZFIL_ERR_OK);
ferr = timer_start_cb(1, 1, 1, &tleid2, &cbcan);
assert(ferr == XZFIL_ERR_OK);
usleep(4000);
ferr = timer_cancel(tleid2);
assert(ferr == XZFIL_ERR_OK);
ferr = timer_cancel(tleid1);
assert(ferr == XZFIL_ERR_OK);
ferr = timer_start_cb(214800, 4, 4, &tleid1, &cbcan); // test CR 6539
assert(ferr == XZFIL_ERR_OK);
ferr = timer_cancel(tleid1);
assert(ferr == XZFIL_ERR_OK);
}
void test_timers_cb() {
int ferr;
int status;
struct timeval t_elapsed;
struct timeval t_start1;
struct timeval t_start2;
struct timeval t_stop;
short tleid1;
short tleid2;
long to_act;
long to_exp;
if (verbose)
printf("timer-cb-test\n");
// check timers
util_time_timer_start(&t_start1);
ferr = timer_start_cb(40, 2, 2, &tleid1, &cb1);
assert(ferr == XZFIL_ERR_OK);
usleep(30000);
util_time_timer_start(&t_start2);
ferr = timer_start_cb(40, 4, 4, &tleid2, &cb2);
assert(ferr == XZFIL_ERR_OK);
status = cv1.wait(true);
TEST_CHK_STATUSOK(status);
if (verbose)
printf("cv1 wait done\n");
util_time_timer_stop(&t_stop);
util_time_elapsed(&t_start1, &t_stop, &t_elapsed);
to_act = t_elapsed.tv_sec * 1000000 + t_elapsed.tv_usec;
to_exp = t1.sre_tleTOVal * 10000;
if (verbose || (to_act < to_exp))
printf("tleid=%d, exp-to=%ld, act-to=%ld\n",
t1.sre_tleId, to_exp, to_act);
assert(to_act >= to_exp);
if (to_tol > 0)
assert(to_act <= (to_exp + to_tol));
status = cv2.wait(true);
TEST_CHK_STATUSOK(status);
if (verbose)
printf("cv2 wait done\n");
util_time_timer_stop(&t_stop);
util_time_elapsed(&t_start2, &t_stop, &t_elapsed);
to_act = t_elapsed.tv_sec * 1000000 + t_elapsed.tv_usec;
to_exp = t2.sre_tleTOVal * 10000;
if (verbose || (to_act < to_exp))
printf("tleid=%d, exp-to=%ld, act-to=%ld\n",
t2.sre_tleId, to_exp, to_act);
assert(to_act >= to_exp);
if (to_tol > 0)
assert(to_act <= (to_exp + to_tol));
}
int sh_speed(int argc, char ** argv) {
uint16_t timeout = 0;
int left;
int right;
int ok;
if (argc < 3) {
cprintf("correct usage:\r\n");
cprintf("\t%c [o] LEFT RIGHT <TIMEOUT>\r\n", argv[0]);
return -1;
}
if (argc > 3) {
timeout = read_uint16(argv[3], &ok);
timer_start_cb(timer_id, timeout, 1);
}
left = read_int16(argv[1], &ok);
right = read_int16(argv[2], &ok);
cprintf("%d %d\r\n",left, right);
// speed_start(speed_l);
// speed_start(speed_r);
speed_setPoint(speed_l, left);
speed_setPoint(speed_r, right);
return 0;
}
//
// Timer callback
//
void timer_callback(int tleid, int toval, short parm1, long parm2) {
int ferr;
if (gv_verbose)
printf("timer_callback \n");
reset_time_counter();
ferr = timer_start_cb(gv_time_refresh_delay, 0, 0, &gv_tleid, &timer_callback);
assert(ferr == XZFIL_ERR_OK);
}
//
// server main
//
int main(int pv_argc, char *pa_argv[]) {
bool lv_done;
int lv_ferr;
int lv_lerr;
BMS_SRE lv_sre;
CALL_COMP_DOVERS(idtmsrv, pv_argc, pa_argv);
do_init(pv_argc, pa_argv);
lv_ferr = msg_mon_process_startup(true); // system messages
assert(lv_ferr == XZFIL_ERR_OK);
msg_mon_enable_mon_messages(true);
lv_ferr = msg_mon_get_my_process_name(ga_name, sizeof(ga_name));
assert(lv_ferr == XZFIL_ERR_OK);
do_shm();
lv_ferr = timer_start_cb(gv_time_refresh_delay, 0, 0, &gv_tleid, &timer_callback);
assert(lv_ferr == XZFIL_ERR_OK);
lv_done = false;
while (!lv_done) {
do {
lv_lerr = XWAIT(LREQ, -1);
lv_lerr = BMSG_LISTEN_((short *) &lv_sre, // sre
0, // listenopts
0); // listenertag
} while (lv_lerr == XSRETYPE_NOWORK);
if (lv_sre.sre_flags & XSRE_MON) {
do_mon_msg(&lv_sre, &lv_done);
} else {
do_req(&lv_sre);
}
}
if (gv_verbose)
printf("server %s shutting down\n", ga_name);
lv_ferr = msg_mon_process_shutdown();
assert(lv_ferr == XZFIL_ERR_OK);
return 0;
}
void sh_roam_init(uint16_t period_ms) {
roam_timer_id = timer_add_cb(roam_cb, NULL);
timer_start_cb(roam_timer_id, period_ms, 0);
}
void sh_speed_arrow_left() {
speed_setPoint(speed_l, 60);
speed_setPoint(speed_r, 60);
timer_start_cb(timer_id, 200, 1);
}
void sh_speed_arrow_right() {
speed_setPoint(speed_l, -60);
speed_setPoint(speed_r, -60);
timer_start_cb(timer_id, 200, 1);
}
void sh_speed_arrow_down() {
speed_setPoint(speed_l, -100);
speed_setPoint(speed_r, 100);
timer_start_cb(timer_id, 200, 1);
}
