static void *COMM_UDP_thread_func(void *arg) {
DBG(D_COMM, D_DEBUG, "COMM UDP eth rx thread started\n");
time last_tick = SYS_get_time_ms();
while (TRUE) {
u16_t len;
while (ETH_SPI_state() != ETH_UP) {
DBG(D_COMM, D_DEBUG, "COMM UDP eth not up, sleep\n");
OS_thread_sleep(1000);
}
if (ETH_SPI_state() == ETH_UP) {
bool pkt = ETH_SPI_read(ecomm.rx_frame, &len, 100);
if (ETH_SPI_state() != ETH_UP) {
DBG(D_COMM, D_DEBUG, "COMM UDP eth down while waiting for frame\n");
continue;
} else {
if (pkt) {
// got packet, handle it
bool fin = FALSE;
int res = R_COMM_OK;
int i = 0;
DBG(D_COMM, D_DEBUG, "COMM UDP rx frame, len %i\n", len);
while (i < len - UDP_DATA_P && !fin && res == R_COMM_OK) {
res = ecomm.driver.phy.up_rx_f(&ecomm.driver, ecomm.rx_frame[i + UDP_DATA_P],
(unsigned char*)&fin);
i++;
}
DBG(D_COMM, D_DEBUG, "COMM UDP rx frame comm stack res:%i fin:%i\n", res, fin);
}
// check if tick is needed
if (SYS_get_time_ms() - last_tick >= COMM_RESEND_TICK(0) / 2) {
last_tick = SYS_get_time_ms();
//DBG(D_COMM, D_DEBUG, "COMM UDP eth rx tmo, tick\n");
comm_tick(&ecomm.driver, COMM_cb_get_tick_count());
}
} // if eth still is up
} // if eth is up
} // while true
return NULL ;
}
static void app_rover_tick(void) {
if (APP_pair_status() != PAIRING_OK) {
return;
}
if (APP_pair_status() == PAIRING_OK) {
if (last_ctrl == 0) {
last_ctrl = SYS_get_time_ms();
} else {
if (SYS_get_time_ms() - last_ctrl > 2000) {
// no message from controller in a while, consider us unpaired
DBG(D_APP, D_WARN, "UNPAIR due to controller silence\n");
APP_set_paired_state(FALSE);
return;
}
}
}
if (app_rover_remote_req & APP_ROVER_REMOTE_REQ_RADAR_REPORT) {
// dispatch radar report
app_rover_dispatch_radar_report();
}
}
void TIMER_irq() {
if (TIM_GetITStatus(STM32_SYSTEM_TIMER, TIM_IT_Update) != RESET) {
TIM_ClearITPendingBit(STM32_SYSTEM_TIMER, TIM_IT_Update);
q++;
if ((q & 0xffff) < 0x1ff) {
gpio_enable(PORTF, PIN6);
} else {
gpio_disable(PORTF, PIN6);
}
bool ms_update = SYS_timer();
if (ms_update) {
TRACE_MS_TICK(SYS_get_time_ms() & 0xff);
}
#ifdef CONFIG_TASK_QUEUE
TASK_timer();
#endif
#ifdef CONFIG_OS
if (ms_update) {
__os_time_tick(SYS_get_time_ms());
}
#endif
}
}
void TIMER_irq() {
if (TIM_GetITStatus(STM32_SYSTEM_TIMER, TIM_IT_Update) != RESET) {
TIM_ClearITPendingBit(STM32_SYSTEM_TIMER, TIM_IT_Update);
#ifdef CONFIG_CNC
CNC_timer();
#endif
bool ms_update = SYS_timer();
if (ms_update) {
TRACE_MS_TICK(SYS_get_time_ms() & 0xff);
}
TASK_timer();
#ifdef CONFIG_LED
LED_SHIFT_tick();
LED_tick();
#endif
if (ms_update) {
__os_time_tick(SYS_get_time_ms());
}
CLI_timer();
}
}
void TASK_dump(u8_t io) {
ioprint(io, "TASK SYSTEM\n-----------\n");
print_task(io, task_sys.current, " current");
char lst[sizeof(TASK_DUMP_OUTPUT)];
memcpy(lst, TASK_DUMP_OUTPUT, sizeof(TASK_DUMP_OUTPUT));
char* p = (char*)strchr(lst, '_');
task* ct = (task *)task_sys.head;
int ix = 1;
while (ct) {
sprint(p, "%02i", ix++);
print_task(io, ct, lst);
ct = ct->_next;
}
print_task(io, (task *)task_sys.last, " last ");
ioprint(io, " pool bitmap ");
for (ix = 0; ix < sizeof(task_pool.mask)/sizeof(task_pool.mask[0]); ix++) {
ioprint(io, "%032b ", task_pool.mask[ix]);
}
ioprint(io, "\n");
for (ix = 0; ix < sizeof(task_pool.mask)/sizeof(task_pool.mask[0]); ix++) {
int bit;
for (bit = 0; bit < 32; bit++) {
if ((task_pool.mask[ix] & (1<<bit)) == 0) {
print_task(io, &task_pool.task[ix*32 + bit], " ");
}
}
}
ioprint(io, "\n");
ioprint(io, " timers\n");
char lst2[sizeof(TASK_TIM_DUMP_OUTPUT)];
memcpy(lst2, TASK_TIM_DUMP_OUTPUT, sizeof(TASK_TIM_DUMP_OUTPUT));
p = (char*)strchr(lst2, '_');
task_timer* tt = task_sys.first_timer;
ix = 1;
time now = SYS_get_time_ms();
while (tt) {
sprint(p, "%02i", ix++);
print_timer(io, tt, lst2, now);
tt = tt->_next;
}
}
void MOTOR_control(s16_t ohori, s16_t overi) {
s16_t motor_ctrl[2];
// update control timestamp
motor.ts_last_control = SYS_get_time_ms();
// check configuration for axis inversion
if (APP_cfg_get_val(CFG_CONTROL) & CFG_CONTROL_JOY_H_INVERT) {
ohori = -ohori;
}
if (APP_cfg_get_val(CFG_CONTROL) & CFG_CONTROL_JOY_V_INVERT) {
overi = -overi;
}
// squash control vector around origin
s16_t hori = motor_lin_squash(ohori, MOTOR_CONTROL_VECTOR_SQUASH);
s16_t veri = motor_lin_squash(overi, MOTOR_CONTROL_VECTOR_SQUASH);
#ifdef DBG_MOTOR
print("MOTOR CTRL[(%+03i/%+03i) -> %+03i/%+03i]",
ohori, overi, hori, veri);
#endif
motor_translate(hori, veri, motor_ctrl);
// adjust steering according to configuration
s16_t adj = APP_cfg_get_val(CFG_STEER_ADJUST);
s16_t left = motor_ctrl[0];
s16_t right = motor_ctrl[1];
#ifdef DBG_MOTOR
print(" TRANS[%+03i/%+03i]",
left, right);
#endif
if (adj < 0) {
adj = -adj;
adj >>= 2;
adj = 128-adj;
right = (s16_t)((s32_t)(adj*right)/128);
} else if (adj > 0) {
void TASK_timer() {
task_timer *cur_timer = task_sys.first_timer;
if (cur_timer == NULL || task_sys.tim_lock) {
return;
}
task_timer *old_timer = NULL;
while (cur_timer && cur_timer->start_time <= SYS_get_time_ms()) {
#ifndef CONFIG_TASK_NONCRITICAL_TIMER
enter_critical();
TQ_ENTER_CRITICAL;
#endif
if (((cur_timer->task->flags & (TASK_RUN | TASK_WAIT)) == 0) && cur_timer->alive) {
// expired, schedule for run
TRACE_TASK_TIMER(cur_timer->_ix);
#ifndef CONFIG_TASK_NONCRITICAL_TIMER
TQ_EXIT_CRITICAL;
#endif
TASK_run(cur_timer->task, cur_timer->arg, cur_timer->arg_p);
#ifndef CONFIG_TASK_NONCRITICAL_TIMER
TQ_ENTER_CRITICAL;
#endif
}
old_timer = cur_timer;
cur_timer = cur_timer->_next;
task_sys.first_timer = cur_timer;
if (old_timer->recurrent_time && old_timer->alive) {
// recurrent, reinsert
old_timer->start_time += old_timer->recurrent_time; // need to set this before inserting for sorting
task_insert_timer(old_timer, old_timer->start_time);
} else {
old_timer->alive = FALSE;
}
#ifndef CONFIG_TASK_NONCRITICAL_TIMER
TQ_EXIT_CRITICAL;
exit_critical();
#endif
}
}
void TASK_start_timer(task *task, task_timer* timer, u32_t arg, void *arg_p, time start_time, time recurrent_time,
const char *name) {
#ifndef CONFIG_TASK_NONCRITICAL_TIMER
enter_critical();
TQ_ENTER_CRITICAL;
#endif
task_sys.tim_lock = TRUE;
ASSERT(timer->alive == FALSE);
timer->_ix = _g_timer_ix++;
timer->arg = arg;
timer->arg_p = arg_p;
timer->task = task;
timer->start_time = start_time;
timer->recurrent_time = recurrent_time;
timer->alive = TRUE;
timer->name = name;
task_insert_timer(timer, SYS_get_time_ms() + start_time);
task_sys.tim_lock = FALSE;
#ifndef CONFIG_TASK_NONCRITICAL_TIMER
TQ_EXIT_CRITICAL;
exit_critical();
#endif
}
static void app_rover_handle_rx(comm_arg *rx, u16_t len, u8_t *data, bool already_received) {
last_ctrl = SYS_get_time_ms();
u8_t reply[REPLY_MAX_LEN];
u8_t reply_ix = 0;
reply[reply_ix++] = ACK_OK;
switch (data[0]) {
case CMD_CONTROL: {
// general rover control
u8_t act = (u8_t)data[6];
u8_t sr = (u8_t)data[7];
reply[reply_ix++] = sr;
if (!already_received) {
remote->motor_ctrl[0] = (s8_t)data[1];
remote->motor_ctrl[1] = (s8_t)data[2];
remote->pan = (s8_t)data[3];
remote->tilt = (s8_t)data[4];
remote->radar = (s8_t)data[5];
// action_mask
remote->light_ir = (act & SPYBOT_ACTION_LIGHT_IR) != 0;
remote->light_white = (act & SPYBOT_ACTION_LIGHT_WHITE) != 0;
remote->beep = (act & SPYBOT_ACTION_BEEP) != 0;
}
// status_mask
if (sr & SPYBOT_SR_ACC) {
reply[reply_ix++] = remote->lsm_acc[0];
reply[reply_ix++] = remote->lsm_acc[1];
reply[reply_ix++] = remote->lsm_acc[2];
}
if (sr & SPYBOT_SR_HEADING) {
reply[reply_ix++] = remote->lsm_heading;
}
if (sr & SPYBOT_SR_TEMP) {
reply[reply_ix++] = remote->temp;
}
if (sr & SPYBOT_SR_BATT) {
reply[reply_ix++] = remote->batt;
}
if ((sr & SPYBOT_SR_RADAR) && !already_received) {
app_rover_set_remote_req(APP_ROVER_REMOTE_REQ_RADAR_REPORT | APP_REMOTE_REQ_URGENT);
}
COMRAD_reply(reply, reply_ix);
#ifdef CONFIG_SPYBOT_MOTOR
MOTOR_control(remote->motor_ctrl[0], remote->motor_ctrl[1]);
#endif
#ifdef CONFIG_SPYBOT_SERVO
SERVO_control(SERVO_CAM_PAN, remote->pan);
SERVO_control(SERVO_CAM_TILT, remote->tilt);
SERVO_control_radar(remote->radar);
#endif
break;
}
case CMD_SET_CONFIG: {
// sets volatile static configuration on rover side
int data_ix = 1;
while (data_ix < COMM_LNK_MAX_DATA && data[data_ix] != CFG_STOP) {
spybot_cfg cfg_type = data[data_ix];
s16_t cfg_val = (data[data_ix + 1] << 8) | (data[data_ix + 2]);
APP_cfg_set(cfg_type, cfg_val);
data_ix += 3;
DBG(D_APP, D_DEBUG, "rover remote update cfg %i = %i\n", cfg_type, cfg_val);
}
reply[reply_ix++] = 1; // ok
COMRAD_reply(reply, reply_ix);
break;
}
case CMD_STORE_CONFIG:
// stores current volatile static configuration
#ifdef CONFIG_M24M01
CFG_EE_set_config(app_cfg);
CFG_EE_store_config();
reply[reply_ix++] = 1; // ok
COMRAD_reply(reply, reply_ix);
DBG(D_APP, D_DEBUG, "rover stored config\n");
#else
COMRAD_reply(REPLY_DENY, 1);
#endif
break;
case CMD_LOAD_CONFIG:
// loads latest stored configuration (and will start using it by
// copying it to volatile static configuration)
#ifdef CONFIG_M24M01
CFG_EE_load_config();
COMRAD_reply(reply, reply_ix);
DBG(D_APP, D_DEBUG, "rover loaded config\n");
#else
COMRAD_reply(REPLY_DENY, 1);
#endif
break;
case CMD_GET_CONFIG: {
#ifdef CONFIG_M24M01
configuration_t cfg;
int res = CFG_EE_get_config(&cfg);
if (res == CFG_OK) {
reply[reply_ix++] = 1; // ok, have config
reply[reply_ix++] = app_cfg->main.steer_adjust;
reply[reply_ix++] = app_cfg->main.radar_adjust;
reply[reply_ix++] = app_cfg->main.cam_pan_adjust;
reply[reply_ix++] = app_cfg->main.cam_tilt_adjust;
reply[reply_ix++] = app_cfg->main.control;
DBG(D_APP, D_DEBUG, "rover returned config\n");
} else {
reply[reply_ix++] = 0; // nok, no config yet - try later
DBG(D_APP, D_DEBUG, "rover no config to return\n");
}
#else
reply[reply_ix++] = 0; // nok, no config ever
#endif
COMRAD_reply(reply, reply_ix);
break;
}
case CMD_LSM_MAG_EXTREMES:
// todo
break;
case CMD_LSM_ACC_EXTREMES:
// todo
break;
default:
APP_handle_unknown_msg(rx, len, data, already_received);
break;
}
}
