C++ chTimeNow函数代码示例

void
app_cfg_idle()
{
  if ((chTimeNow() - last_idle) > S2ST(2)) {
    app_cfg_flush();
    last_idle = chTimeNow();
  }
}

/**
 * @brief   Current timestamp in milliseconds.
 * @note    The resolution is in milliseconds, but the precision may not be.
 *
 * @return
 *          The current timestamp, with a resolution of one millisecond.
 */
uint32_t uros_lld_threading_gettimestampmsec(void) {

#if CH_FREQUENCY == 1000
  return (uint32_t)chTimeNow();
#else
  return (((uint32_t)chTimeNow() - 1) * 1000) / CH_FREQUENCY + 1;
#endif
}

void SpeedMetersConfig(SpeedMeter_callback_t _left_cb, SpeedMeter_callback_t _right_cb){
  left_cb = _left_cb;
  right_cb = _right_cb;
     
  extStart(&EXTD1, &extcfg);
  
  tl1 = tl2 = chTimeNow();
  tr1 = tr2 = chTimeNow();
}

void Keys_t::ITask() {
    chThdSleepMilliseconds(KEYS_POLL_PERIOD_MS);
    if(App.PThd == nullptr) return;
    // Check keys
    for(uint8_t i=0; i<KEYS_CNT; i++) {
        bool PressedNow = !PinIsSet(KeyData[i].PGpio, KeyData[i].Pin);
        // Check if just pressed
        if(PressedNow and !Key[i].IsPressed) {
            chSysLock();
            Key[i].IsPressed = true;
            Key[i].IsLongPress = false;
            Key[i].IsRepeating = false;
            chEvtSignalI(App.PThd, KeyData[i].EvtMskPress);
            // Reset timers
            RepeatTimer = chTimeNow();
            LongPressTimer = chTimeNow();
            chSysUnlock();
        }
        // Check if just released
        else if(!PressedNow and Key[i].IsPressed) {
            Key[i].IsPressed = false;
            if(!Key[i].IsLongPress) {
                chSysLock();
                chEvtSignalI(App.PThd, KeyData[i].EvtMskRelease);
                chSysUnlock();
            }
        }
        // Check if still pressed
        else if(PressedNow and Key[i].IsPressed) {
            // Check if long press
            if(!Key[i].IsLongPress) {
                if(TimeElapsed(&LongPressTimer, KEY_LONGPRESS_DELAY_MS)) {
                    Key[i].IsLongPress = true;
                    chSysLock();
                    chEvtSignalI(App.PThd, KeyData[i].EvtMskLongPress);
                    chSysUnlock();
                }
            }
            // Check if repeat
            if(!Key[i].IsRepeating) {
                if(TimeElapsed(&RepeatTimer, KEYS_KEY_BEFORE_REPEAT_DELAY_MS)) {
                    Key[i].IsRepeating = true;
                    chSysLock();
                    chEvtSignalI(App.PThd, KeyData[i].EvtMskRepeat);
                    chSysUnlock();
                }
            }
            else {
                if(TimeElapsed(&RepeatTimer, KEY_REPEAT_PERIOD_MS)) {
                    chSysLock();
                    chEvtSignalI(App.PThd, KeyData[i].EvtMskRepeat);
                    chSysUnlock();
                }
            }
        } // if still pressed
    } // for
}

static state_t do_state_pad(instance_data_t *data)
{
    state_estimation_trust_barometer = true;
    if(chTimeNow() > 10000 && data->state.a > IGNITION_ACCEL) {
        data->t_launch = chTimeNow();
        return STATE_IGNITION;
    } else {
        return STATE_PAD;
    }
}

RadInputValue inputEncoderProcessor(RadInputState* state)
{
  RadInputValue v;
  v.encoder.delta = state->encoder.value - state->encoder.last_value;
  state->encoder.last_value = state->encoder.value;
  v.encoder.rate =
      (float)v.encoder.delta /
      (chTimeNow() - state->encoder.last_time) * S2ST(1);
  state->encoder.last_time = chTimeNow();
  return v;
}

u32_t sys_now(void) {

#if CH_FREQUENCY == 1000
  return (u32_t)chTimeNow();
#elif (CH_FREQUENCY / 1000) >= 1 && (CH_FREQUENCY % 1000) == 0
  return ((u32_t)chTimeNow() - 1) / (CH_FREQUENCY / 1000) + 1;
#elif (1000 / CH_FREQUENCY) >= 1 && (1000 % CH_FREQUENCY) == 0
  return ((u32_t)chTimeNow() - 1) * (1000 / CH_FREQUENCY) + 1;
#else
  return (u32_t)(((u64_t)(chTimeNow() - 1) * 1000) / CH_FREQUENCY) + 1;
#endif
}

int advp_send(uint8_t *frame, d7_ti time, uint8_t bg_channel, uint8_t fg_channel)
{
    systime_t now;
    systime_t end;
    uint8_t bg_frame[FG_FRAME_SIZE];
    bg_param_t conf;
    int error;
    int step = 0;

    //min advp time is needed because the first send takes some time
    if(time < MIN_ADVP_TIME) time = MIN_ADVP_TIME;

    //configure the background send procedure
    bg_set_subnet(bg_frame, data_proc_conf.my_subnet);
    conf.channel = fg_channel;

    data_proc_conf.pack_class = BACKGROUND_CLASS;
    data_proc_conf.channel = bg_channel;
    data_proc_config(&data_proc_conf);

    now = chTimeNow();
    end = now + d7_ti_to_systime(time);

    //keep sending packets with the remaining time
    while(now < (end - (PROT_SEND_TIME)))
    {
        conf.time = systime_to_d7_ti(end - now - (PROT_SEND_TIME - SAFE_ADVP_WINDOW));
        bg_set_advp(bg_frame,&conf);
        data_add_frame(bg_frame);
        if(!step){
            error = data_send_packet();
            step = 1;
        } else
            error = data_send_packet_protected();
        if(error){
        }

        now = chTimeNow();
    }

    //prepare the foreground send procedure
    data_proc_conf.pack_class = FOREGROUND_CLASS;
    data_proc_conf.channel = fg_channel;
    data_proc_config(&data_proc_conf);
    data_add_frame(frame);
    now = chTimeNow();
    //if(now > end) 
    //    return -(now - end);
    //chThdSleepMilliseconds(end-now);

    return data_send_packet_protected();
}

void writeToFlash(void) {
    flashState.version = FLASH_DATA_VERSION;
    scheduleMsg(&logger, "FLASH_DATA_VERSION=%d", flashState.version);
    crc result = flashStateCrc(&flashState);
    flashState.value = result;
    scheduleMsg(&logger, "Reseting flash=%d", FLASH_USAGE);
    flashErase(FLASH_ADDR, FLASH_USAGE);
    scheduleMsg(&logger, "Flashing with CRC=%d", result);
    time_t now = chTimeNow();
    result = flashWrite(FLASH_ADDR, (const char *) &flashState, FLASH_USAGE);
    scheduleMsg(&logger, "Flash programmed in (ms): %d", chTimeNow() - now);
    scheduleMsg(&logger, "Flashed: %d", result);
}

u32_t sys_arch_mbox_fetch(sys_mbox_t *mbox, void **msg, u32_t timeout) {
  systime_t time, tmo;

  chSysLock();
  tmo = timeout > 0 ? (systime_t)timeout : TIME_INFINITE;
  time = chTimeNow();
  if (chMBFetchS(*mbox, (msg_t *)msg, tmo) != RDY_OK)
    time = SYS_ARCH_TIMEOUT;
  else
    time = chTimeNow() - time;
  chSysUnlock();
  return time;
}

u32_t sys_arch_sem_wait(sys_sem_t *sem, u32_t timeout) {
  systime_t time, tmo;

  chSysLock();
  tmo = timeout > 0 ? (systime_t)timeout : TIME_INFINITE;
  time = chTimeNow();
  if (chSemWaitTimeoutS(*sem, tmo) != RDY_OK)
    time = SYS_ARCH_TIMEOUT;
  else
    time = chTimeNow() - time;
  chSysUnlock();
  return time;
}

void inputGinputFetcher(RadInputConfig* config, RadInputState* state)
{
  if (state->is_enabled == 1) {
    // Initialization
    ginputGetMouse(0);
    state->is_enabled = 2;
    state->encoder.last_time = chTimeNow();
  }

  ginputGetMouseStatus(0, &config->ginput.event);

  uint8_t is_down = (config->ginput.event.current_buttons & config->ginput.button_mask) ? 1 : 0;
  state->encoder.value = config->ginput.event.x / 2;
  if (abs((int)state->encoder.value - state->encoder.last_value) > 50)
    state->encoder.last_value = state->encoder.value;

  // Modifying the state of the next channel: button channel
  RadInputState* state_button = state + 1;
  if (is_down)
  {
    if (!state_button->button.is_down) {
      state_button->button.is_down = 1;
      state_button->button.times++;
    }
  } else {
    state_button->button.is_down = 0;
  }
}

msg_t qeipub_node(void *arg) {
	Node node("qeipub");
	Publisher<tQEIMsg> qei_pub;
	systime_t time;
	tQEIMsg *msgp;

	(void) arg;
	chRegSetThreadName("qeipub");

	qeiStart(&QEI_DRIVER, &qeicfg);
	qeiEnable (&QEI_DRIVER);

	node.advertise(qei_pub, "qei"MOTOR_ID_STRING);

	for (;;) {
		time = chTimeNow();
		int16_t delta = qeiUpdate(&QEI_DRIVER);

		if (qei_pub.alloc(msgp)) {
			msgp->timestamp.sec = 0;
			msgp->timestamp.nsec = 0;
			msgp->delta = delta;
			qei_pub.publish(*msgp);
		}

		time += MS2ST(50);
		chThdSleepUntil(time);
	}

	return CH_SUCCESS;
}

bool LSM303calibrator::trig(void){
  switch(state){
  case LSM303_CAL_SLEEP:

    chSysLock();
    last_point_timestamp = chTimeNow();
    sample = 0;
    point = 0;
    state = LSM303_CAL_COLLECTING;
    chSysUnlock();

    mavlink_dbg_print(MAV_SEVERITY_INFO, "MAG: calibration started");
    chThdSleep(1);
    mavlink_system_struct.state = MAV_STATE_CALIBRATING;
    return CH_SUCCESS;
    break;

  case LSM303_CAL_WAIT_NEXT:
    mavlink_dbg_print(MAV_SEVERITY_NOTICE, "MAG: new position reached");
    state = LSM303_CAL_COLLECTING;
    return CH_SUCCESS;
    break;

  default:
    return CH_FAILED;
    break;
  }
}

int main(void)
{
	halInit();
	chSysInit();

	mcu_conf();

	for (int i = 0; i < 10; i++)
	{
		chThdSleepMilliseconds(100);
		palTogglePad(TEST_LED_PORT3, TEST_LED_PIN3);
	}

	ap.start();
	appInit();

	ph.StartAutoIdle();
	ph.setFunctionTable(&ph_ft);
	enable_watchdog();
	ph.RequestData(STARTUP);

	while (TRUE)
	{
		Scheduler::Play();
		sysTime = chTimeNow();
		ph.HandlePacketLoop();
	}

	return 1;
}