static void
run_demo(struct demo_gui *gui, struct gui_input *input)
{
struct control_window *control = &gui->control;
struct show_window *show = &gui->show;
gui->running = update_control(control, &gui->stack, input, &gui->config);
if (show->hook.flags & GUI_PANEL_ACTIVE)
show->hook.flags = control->show_flags|GUI_PANEL_ACTIVE;
else show->hook.flags = control->show_flags;
update_show(show, &gui->stack, input, gui->ms);
if (show->hook.flags & GUI_PANEL_HIDDEN)
control->show_flags |= GUI_PANEL_HIDDEN;
}
void loop(void) {
if((millis()-timer)>=20) // Main loop runs at 50Hz
{
if(loop_counter++ > 50) {
loop_counter = 0;
//Serial.println(millis() - timer);
}
updateTime();
update_messages();
update_motors();
update_inertial_sensors();
update_control();
}
}
static void
add_control_item(HippoCanvasContextWin *canvas_win,
HippoCanvasItem *item)
{
RegisteredControlItem *citem = g_new0(RegisteredControlItem, 1);
citem->item = item;
g_object_ref(citem->item);
canvas_win->control_items = g_slist_prepend(canvas_win->control_items, citem);
update_control(canvas_win, citem);
g_signal_connect(G_OBJECT(item), "notify::control",
G_CALLBACK(on_item_control_changed),
canvas_win);
}
static void
on_item_control_changed(HippoCanvasItem *item,
GParamSpec *arg,
void *data)
{
HippoCanvasContextWin *canvas_win = HIPPO_CANVAS_CONTEXT_WIN(data);
RegisteredControlItem *citem;
GSList *link;
citem = NULL;
for (link = canvas_win->control_items;
link != NULL;
link = link->next) {
citem = (RegisteredControlItem*) link->data;
if (citem->item == item) {
update_control(canvas_win, citem);
return;
}
}
g_warning("got control changed for an unregistered control item");
}
static msg_t control_thread(void* arg)
{
(void)arg;
chRegSetThreadName("motor");
EventListener listener;
chEvtRegisterMask(&_setpoint_update_event, &listener, ALL_EVENTS);
uint64_t timestamp_hnsec = motor_rtctl_timestamp_hnsec();
while (1) {
/*
* Control loop period adapts to comm period.
*/
const uint32_t comm_period = motor_rtctl_get_comm_period_hnsec();
unsigned control_period_ms = IDLE_CONTROL_PERIOD_MSEC;
if (comm_period > 0) {
control_period_ms = comm_period / HNSEC_PER_MSEC;
}
if (control_period_ms < 1) {
control_period_ms = 1;
} else if (control_period_ms > IDLE_CONTROL_PERIOD_MSEC) {
control_period_ms = IDLE_CONTROL_PERIOD_MSEC;
}
/*
* Thread priority - maximum if the motor is running, normal otherwise
*/
const tprio_t desired_thread_priority = (comm_period > 0) ? HIGHPRIO : NORMALPRIO;
if (desired_thread_priority != chThdGetPriority()) {
const tprio_t old = chThdSetPriority(desired_thread_priority);
lowsyslog("Motor: Priority changed: %i --> %i\n", (int)old, (int)desired_thread_priority);
}
/*
* The event must be set only when the mutex is unlocked.
* Otherwise this thread will take control, stumble upon the locked mutex, return the control
* to the thread that holds the mutex, unlock the mutex, then proceed.
*/
chEvtWaitAnyTimeout(ALL_EVENTS, MS2ST(control_period_ms));
chMtxLock(&_mutex);
const uint64_t new_timestamp_hnsec = motor_rtctl_timestamp_hnsec();
const uint32_t dt_hnsec = new_timestamp_hnsec - timestamp_hnsec;
const float dt = dt_hnsec / (float)HNSEC_PER_SEC;
timestamp_hnsec = new_timestamp_hnsec;
assert(dt > 0);
update_filters(dt);
update_setpoint_ttl(dt_hnsec / HNSEC_PER_MSEC);
update_control(comm_period, dt);
poll_beep();
chMtxUnlock();
watchdog_reset(_watchdog_id);
}
assert_always(0);
return 0;
}
