void udb_run(void) { uint16_t currentTime; uint16_t nextHeartbeatTime; if (strlen(SILSIM_SERIAL_RC_INPUT_DEVICE) == 0) { udb_pwIn[THROTTLE_INPUT_CHANNEL] = 2000; udb_pwTrim[THROTTLE_INPUT_CHANNEL] = 2000; } nextHeartbeatTime = get_current_milliseconds(); while (1) { if (!handleUDBSockets()) { sleep_milliseconds(1); } currentTime = get_current_milliseconds(); if (currentTime >= nextHeartbeatTime && !(nextHeartbeatTime <= UDB_STEP_TIME && currentTime >= UDB_WRAP_TIME-UDB_STEP_TIME)) { udb_callback_read_sensors(); udb_flags._.radio_on = (sil_radio_on && udb_pwIn[FAILSAFE_INPUT_CHANNEL] >= FAILSAFE_INPUT_MIN && udb_pwIn[FAILSAFE_INPUT_CHANNEL] <= FAILSAFE_INPUT_MAX); LED_GREEN = (udb_flags._.radio_on) ? LED_ON : LED_OFF ; if (udb_heartbeat_counter % 20 == 0) udb_background_callback_periodic(); // Run at 2Hz udb_servo_callback_prepare_outputs(); sil_ui_update(); if (udb_heartbeat_counter % 80 == 0) writeEEPROMFileIfNeeded(); // Run at 0.5Hz udb_heartbeat_counter++; nextHeartbeatTime = nextHeartbeatTime + UDB_STEP_TIME; if (nextHeartbeatTime > UDB_WRAP_TIME) nextHeartbeatTime -= UDB_WRAP_TIME; } process_queued_events(); } }
void udb_run(void) { uint16_t currentTime; static uint16_t nextHeartbeatTime; if (!initialised) { initialised = 1; if (strlen(SILSIM_SERIAL_RC_INPUT_DEVICE) == 0) { udb_pwIn[THROTTLE_INPUT_CHANNEL] = 2000; udb_pwTrim[THROTTLE_INPUT_CHANNEL] = 2000; } nextHeartbeatTime = get_current_milliseconds(); } // while (1) { if (!handleUDBSockets()) { sleep_milliseconds(1); } currentTime = get_current_milliseconds(); if (currentTime >= nextHeartbeatTime && !(nextHeartbeatTime <= UDB_STEP_TIME && currentTime >= UDB_WRAP_TIME-UDB_STEP_TIME)) { udb_callback_read_sensors(); udb_flags._.radio_on = (sil_radio_on && udb_pwIn[FAILSAFE_INPUT_CHANNEL] >= FAILSAFE_INPUT_MIN && udb_pwIn[FAILSAFE_INPUT_CHANNEL] <= FAILSAFE_INPUT_MAX); // LED_GREEN = (udb_flags._.radio_on) ? LED_ON : LED_OFF; if (udb_flags._.radio_on) { led_on(LED_GREEN); } else { led_off(LED_GREEN); } udb_heartbeat_40hz_callback(); // Run at 40Hz udb_heartbeat_callback(); // Run at HEARTBEAT_HZ sil_ui_update(); // if (udb_heartbeat_counter % 80 == 0) if (udb_heartbeat_counter % (2 * HEARTBEAT_HZ) == 0) { writeEEPROMFileIfNeeded(); // Run at 0.5Hz } udb_heartbeat_counter++; nextHeartbeatTime = nextHeartbeatTime + UDB_STEP_TIME; if (nextHeartbeatTime > UDB_WRAP_TIME) nextHeartbeatTime -= UDB_WRAP_TIME; } process_queued_events(); // } }