u32_t sys_arch_sem_wait(sys_sem_t *sem, u32_t timeout) {
systime_t tmo;
u32_t time;
osalSysLock();
tmo = timeout > 0 ? MS2ST((systime_t)timeout) : TIME_INFINITE;
time = (u32_t)ST2MS(osalOsGetSystemTimeX());
if (chSemWaitTimeoutS(*sem, tmo) != MSG_OK)
time = SYS_ARCH_TIMEOUT;
else
time = (u32_t)ST2MS(osalOsGetSystemTimeX()) - time;
chSchRescheduleS();
osalSysUnlock();
return time;
}
msg_t FsWriterThread::main() {
BaseSequentialStream *chp = (BaseSequentialStream*)&SD1;
chprintf(chp, "Filesystem thread online.\r\n");
while (!fs.connect()) chThdSleepMilliseconds(10);
while (!fs.mount()) chThdSleepMilliseconds(10);
while (!fs.openNew()) chThdSleepMilliseconds(10);
fsReady = true;
while(true) {
uint32_t count = buf.count;
if (count > 3000) count = 3000 + 0.0625*(count-3000);
rb_remove(&buf, count, writebuf);
fs.write(writebuf, count);
fs.sync();
if (fsInfoMessageStream.ready()) {
protocol::message::fs_info_message_t m;
m.time = ST2MS(chibios_rt::System::getTime());
fs.getFn(m.fname);
m.fsize = fs.getFileSize();
fsInfoMessageStream.publish(m);
}
yield();
}
// Should never get here
fs.umount();
fs.disconnect();
}
void rotenc_push_cb(EXTDriver *extp, expchannel_t channel)
{
(void)extp;
(void)channel;
if(ST2MS(chVTTimeElapsedSinceX(rotenc_press_time)) > 500) {
chSysLockFromISR();
chEvtBroadcastFlagsI(&rotenc_es, ROTENC_PRESS_FLAG);
rotenc_press_time = chVTGetSystemTimeX();
chSysUnlockFromISR();
}
}
u32_t sys_arch_mbox_fetch(sys_mbox_t *mbox, void **msg, u32_t timeout) {
systime_t tmo, start, remaining;
osalSysLock();
tmo = timeout > 0 ? MS2ST((systime_t)timeout) : TIME_INFINITE;
start = osalOsGetSystemTimeX();
if (chMBFetchS(*mbox, (msg_t *)msg, tmo) != MSG_OK) {
osalSysUnlock();
return SYS_ARCH_TIMEOUT;
}
remaining = osalOsGetSystemTimeX() - start;
osalSysUnlock();
return (u32_t)ST2MS(remaining);
}
u32_t sys_arch_sem_wait(sys_sem_t *sem, u32_t timeout) {
systime_t tmo, start, remaining;
osalSysLock();
tmo = timeout > 0 ? MS2ST((systime_t)timeout) : TIME_INFINITE;
start = osalOsGetSystemTimeX();
if (chSemWaitTimeoutS(*sem, tmo) != MSG_OK) {
osalSysUnlock();
return SYS_ARCH_TIMEOUT;
}
remaining = osalOsGetSystemTimeX() - start;
osalSysUnlock();
return (u32_t)ST2MS(remaining);
}
/**
*
* @brief Suspends execution of current thread for a regular interval.
*
* @param[in] previous_ms pointer to system time of last execution,
* must have been initialized with PIOS_Thread_Systime() on first invocation
* @param[in] increment_ms time of regular interval in milliseconds
*
*/
void PIOS_Thread_Sleep_Until(uint32_t *previous_ms, uint32_t increment_ms)
{
systime_t future = MS2ST(*previous_ms) + MS2ST(increment_ms);
chSysLock();
systime_t now = chTimeNow();
int mustDelay =
now < MS2ST(*previous_ms) ?
(now < future && future < MS2ST(*previous_ms)) :
(now < future || future < MS2ST(*previous_ms));
if (mustDelay)
chThdSleepS(future - now);
chSysUnlock();
*previous_ms = ST2MS(future);
}
u32_t sys_arch_mbox_fetch(sys_mbox_t *mbox, void **msg, u32_t timeout) {
u32_t time;
systime_t tmo;
osalSysLock();
tmo = timeout > 0 ? MS2ST((systime_t)timeout) : TIME_INFINITE;
time = (u32_t)osalOsGetSystemTimeX();
if (chMBFetchS(*mbox, (msg_t *)msg, tmo) != MSG_OK)
time = SYS_ARCH_TIMEOUT;
else
time = (u32_t)ST2MS(osalOsGetSystemTimeX()) - time;
osalSysUnlock();
return time;
}
static void sp100_wait_measurement(int n) {
/* TODO Signal the error over serial too */
systime_t start_time = chVTGetSystemTime();
while(sp100_status(n) & SP100_STATUS_MEASURE) {
/* If we've been waiting more than 50ms, signal an error and
* continue anyway.
*/
if(ST2MS(chVTGetSystemTime() - start_time) > 50) {
palSetLine(LINE_LED_RED);
return;
}
}
}
systime_t mr_control_time_since_input_update(void) {
return ST2MS(chVTTimeElapsedSinceX(m_rc.last_update_time));
}
healthy &= (*mag)->healthy();
}
return healthy;
}
void RocketSystem::on(const protocol::message::set_arm_state_message_t& m) {
if (state == RocketState::DISARMED) {
return;
}
setArmed(m.armed);
}
void RocketSystem::updateStreams(SensorMeasurements meas, WorldEstimate est, ActuatorSetpoint& sp) {
protocol::message::system_message_t m {
.time = ST2MS(chibios_rt::System::getTime()),
.state = (uint8_t) state,
.motorDC = sp.throttle
};
logger.write(m);
if (systemStream.ready()) {
systemStream.publish(m);
}
}
RocketState RocketSystem::DisarmedState(SensorMeasurements meas, WorldEstimate est, ActuatorSetpoint& sp) {
PulseLED(0,1,0,4); // Green 4 Hz
static bool calibrated = false;
static int calibCount = 0;
static std::array<float, 3> gyrOffsets = unit_config::GYR_OFFSETS;
static DECLARE_THREAD_FUNCTION(visualizerThread, arg) {
(void)arg;
GListener event_listener;
geventListenerInit(&event_listener);
geventAttachSource(&event_listener, (GSourceHandle)¤t_status, 0);
visualizer_keyboard_status_t initial_status = {
.default_layer = 0xFFFFFFFF,
.layer = 0xFFFFFFFF,
.mods = 0xFF,
.leds = 0xFFFFFFFF,
.suspended = false,
};
visualizer_state_t state = {
.status = initial_status,
.current_lcd_color = 0,
#ifdef LCD_ENABLE
.font_fixed5x8 = gdispOpenFont("fixed_5x8"),
.font_dejavusansbold12 = gdispOpenFont("DejaVuSansBold12")
#endif
};
initialize_user_visualizer(&state);
state.prev_lcd_color = state.current_lcd_color;
#ifdef LCD_BACKLIGHT_ENABLE
lcd_backlight_color(
LCD_HUE(state.current_lcd_color),
LCD_SAT(state.current_lcd_color),
LCD_INT(state.current_lcd_color));
#endif
systemticks_t sleep_time = TIME_INFINITE;
systemticks_t current_time = gfxSystemTicks();
while(true) {
systemticks_t new_time = gfxSystemTicks();
systemticks_t delta = new_time - current_time;
current_time = new_time;
bool enabled = visualizer_enabled;
if (!same_status(&state.status, ¤t_status)) {
if (visualizer_enabled) {
if (current_status.suspended) {
stop_all_keyframe_animations();
visualizer_enabled = false;
state.status = current_status;
user_visualizer_suspend(&state);
}
else {
state.status = current_status;
update_user_visualizer_state(&state);
}
state.prev_lcd_color = state.current_lcd_color;
}
}
if (!enabled && state.status.suspended && current_status.suspended == false) {
// Setting the status to the initial status will force an update
// when the visualizer is enabled again
state.status = initial_status;
state.status.suspended = false;
stop_all_keyframe_animations();
user_visualizer_resume(&state);
state.prev_lcd_color = state.current_lcd_color;
}
sleep_time = TIME_INFINITE;
for (int i=0;i<MAX_SIMULTANEOUS_ANIMATIONS;i++) {
if (animations[i]) {
update_keyframe_animation(animations[i], &state, delta, &sleep_time);
}
}
#ifdef LED_ENABLE
gdispGFlush(LED_DISPLAY);
#endif
#ifdef EMULATOR
draw_emulator();
#endif
// The animation can enable the visualizer
// And we might need to update the state when that happens
// so don't sleep
if (enabled != visualizer_enabled) {
sleep_time = 0;
}
systemticks_t after_update = gfxSystemTicks();
unsigned update_delta = after_update - current_time;
if (sleep_time != TIME_INFINITE) {
if (sleep_time > update_delta) {
sleep_time -= update_delta;
}
else {
sleep_time = 0;
}
}
dprintf("Update took %d, last delta %d, sleep_time %d\n", update_delta, delta, sleep_time);
#ifdef PROTOCOL_CHIBIOS
// The gEventWait function really takes milliseconds, even if the documentation says ticks.
// Unfortunately there's no generic ugfx conversion from system time to milliseconds,
// so let's do it in a platform dependent way.
// On windows the system ticks is the same as milliseconds anyway
if (sleep_time != TIME_INFINITE) {
sleep_time = ST2MS(sleep_time);
}
#endif
geventEventWait(&event_listener, sleep_time);
}
#ifdef LCD_ENABLE
gdispCloseFont(state.font_fixed5x8);
gdispCloseFont(state.font_dejavusansbold12);
#endif
return 0;
}
void visualizer_init(void) {
gfxInit();
#ifdef LCD_BACKLIGHT_ENABLE
lcd_backlight_init();
#endif
#ifdef SERIAL_LINK_ENABLE
add_remote_objects(remote_objects, sizeof(remote_objects) / sizeof(remote_object_t*) );
#endif
#ifdef LCD_ENABLE
LCD_DISPLAY = get_lcd_display();
#endif
#ifdef LED_ENABLE
LED_DISPLAY = get_led_display();
#endif
// We are using a low priority thread, the idea is to have it run only
// when the main thread is sleeping during the matrix scanning
gfxThreadCreate(visualizerThreadStack, sizeof(visualizerThreadStack),
VISUALIZER_THREAD_PRIORITY, visualizerThread, NULL);
}
void update_status(bool changed) {
if (changed) {
GSourceListener* listener = geventGetSourceListener((GSourceHandle)¤t_status, NULL);
if (listener) {
geventSendEvent(listener);
}
}
#ifdef SERIAL_LINK_ENABLE
static systime_t last_update = 0;
systime_t current_update = chVTGetSystemTimeX();
systime_t delta = current_update - last_update;
if (changed || delta > MS2ST(10)) {
last_update = current_update;
visualizer_keyboard_status_t* r = begin_write_current_status();
*r = current_status;
end_write_current_status();
}
#endif
}
uint8_t visualizer_get_mods() {
uint8_t mods = get_mods();
#ifndef NO_ACTION_ONESHOT
if (!has_oneshot_mods_timed_out()) {
mods |= get_oneshot_mods();
}
#endif
return mods;
}
void visualizer_update(uint32_t default_state, uint32_t state, uint8_t mods, uint32_t leds) {
// Note that there's a small race condition here, the thread could read
// a state where one of these are set but not the other. But this should
// not really matter as it will be fixed during the next loop step.
// Alternatively a mutex could be used instead of the volatile variables
bool changed = false;
#ifdef SERIAL_LINK_ENABLE
if (is_serial_link_connected ()) {
visualizer_keyboard_status_t* new_status = read_current_status();
if (new_status) {
if (!same_status(¤t_status, new_status)) {
changed = true;
current_status = *new_status;
}
}
}
else {
#else
{
#endif
visualizer_keyboard_status_t new_status = {
.layer = state,
.default_layer = default_state,
.mods = mods,
.leds = leds,
.suspended = current_status.suspended,
};
if (!same_status(¤t_status, &new_status)) {
changed = true;
current_status = new_status;
}
}
update_status(changed);
}
void visualizer_suspend(void) {
current_status.suspended = true;
update_status(true);
}
void visualizer_resume(void) {
current_status.suspended = false;
update_status(true);
}
/**
*
* @brief Returns the current system time.
*
* @returns current system time
*
*/
uint32_t PIOS_Thread_Systime(void)
{
return (uint32_t)ST2MS(chTimeNow());
}
