Exemplo n.º 1
0
void
app_cfg_idle()
{
  if ((chTimeNow() - last_idle) > S2ST(2)) {
    app_cfg_flush();
    last_idle = chTimeNow();
  }
}
Exemplo n.º 2
0
/**
 * @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
}
Exemplo n.º 3
0
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();
}
Exemplo n.º 4
0
Arquivo: keys.cpp Projeto: Kreyl/nute
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
}
Exemplo n.º 5
0
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;
    }
}
Exemplo n.º 6
0
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;
}
Exemplo n.º 7
0
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
}
Exemplo n.º 8
0
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();
}
Exemplo n.º 9
0
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);
}
Exemplo n.º 10
0
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;
}
Exemplo n.º 11
0
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;
}
Exemplo n.º 12
0
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;
  }
}
Exemplo n.º 13
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;
}
Exemplo n.º 14
0
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;
  }
}
Exemplo n.º 15
0
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;
}
Exemplo n.º 16
0
static void cmd_rms(BaseSequentialStream *chp) {

  /* 
   * Creating dynamic threads using the heap allocator
  */
  Thread *tp1 = chThdCreateFromHeap(NULL, WA_SIZE, NORMALPRIO-2, thread1, chp);
  Thread *tp2 = chThdCreateFromHeap(NULL, WA_SIZE, NORMALPRIO, thread2, chp);
  Thread *tp3 = chThdCreateFromHeap(NULL, WA_SIZE, NORMALPRIO-1, thread3, chp);
  Thread *tp4 = chThdCreateFromHeap(NULL, WA_SIZE, NORMALPRIO-3, thread4, chp);


  chThdSleepUntil(chTimeNow() + MS2ST(500));

  /*
   * Try to kill threads
  */
  chThdTerminate(tp1);
  chThdTerminate(tp2);
  chThdTerminate(tp3);
  chThdTerminate(tp4);

  /*
   * Wait for the thread to terminate (if it has not terminated
   * already) then get the thread exit message (msg) and returns the
   * terminated thread memory to the heap.
   */
  msg_t msg = chThdWait(tp1);
  msg = chThdWait(tp2);
  msg = chThdWait(tp3);
  msg = chThdWait(tp4);
}
Exemplo n.º 17
0
/**
 * Updates calibration data.
 */
void LSM303calibrator::update(const float *data, uint32_t still_msk){
  switch(state){
  case LSM303_CAL_SLEEP:
    /* nothing to do */
    return;
    break;

  case LSM303_CAL_WAIT_NEXT:
    if ((chTimeNow() - last_point_timestamp) < WAIT_NEXT_TIMEOUT){
      red_blink_mb.post((msg_t)slowblink, TIME_IMMEDIATE);
      return;
    }
    else{
      /* time is out */
      chSysLock();
      sample = 0;
      point = 0;
      state = LSM303_CAL_SLEEP;
      chSysUnlock();
      mavlink_dbg_print(MAV_SEVERITY_INFO, "MAG: wait next point time out");
      mavlink_system_struct.state = MAV_STATE_STANDBY;
    }
    break;

  case LSM303_CAL_COLLECTING:
    collecting(data, still_msk);
    break;

  default:
    chDbgPanic("unhandled case");
    break;
  }
}
Exemplo n.º 18
0
// Implements steps according to the current step interval
// You must call this at least once per step
// returns true if a step occurred
bool AccelStepper::runSpeed()
{
    // Dont do anything unless we actually have a step interval
    if (!_stepInterval)
	return false;

    //unsigned long time = micros();
    unsigned long time = chTimeNow();
    // Gymnastics to detect wrapping of either the nextStepTime and/or the current time
    unsigned long nextStepTime = _lastStepTime + _stepInterval;
    if (   ((nextStepTime >= _lastStepTime) && ((time >= nextStepTime) || (time < _lastStepTime)))
	|| ((nextStepTime < _lastStepTime) && ((time >= nextStepTime) && (time < _lastStepTime))))

    {
	if (_direction == DIRECTION_CW)
	{
	    // Clockwise
	    _currentPos += 1;
	}
	else
	{
	    // Anticlockwise  
	    _currentPos -= 1;
	}
	step(_currentPos & 0x7); // Bottom 3 bits (same as mod 8, but works with + and - numbers) 

	_lastStepTime = time;
	return true;
    }
    else
    {
	return false;
    }
}
Exemplo n.º 19
0
uint32_t get_timestamp(void) {
    systime_t new_stime;
    new_stime = chTimeNow();
    last_timestamp += (((new_stime-last_systime) * 1000) / CH_FREQUENCY);
    last_systime = new_stime;
    return last_timestamp;
}
Exemplo n.º 20
0
static msg_t uart_thread(void *arg) {
	(void)arg;

	chRegSetThreadName("UART");

	uartStart(&HW_UART_DEV, &uart_cfg);
	palSetPadMode(HW_UART_TX_PORT, HW_UART_TX_PIN, PAL_MODE_ALTERNATE(HW_UART_GPIO_AF) |
			PAL_STM32_OSPEED_HIGHEST |
			PAL_STM32_PUDR_PULLUP);
	palSetPadMode(HW_UART_RX_PORT, HW_UART_RX_PIN, PAL_MODE_ALTERNATE(HW_UART_GPIO_AF) |
			PAL_STM32_OSPEED_HIGHEST |
			PAL_STM32_PUDR_PULLUP);

	systime_t time = chTimeNow();

	for(;;) {
		time += MS2ST(1);

		if ((systime_t) ((float) chTimeElapsedSince(last_uart_update_time)
				/ ((float) CH_FREQUENCY / 1000.0)) > (float)TIMEOUT) {
			mcpwm_set_brake_current(-10.0);
		} else {
			set_output(out_received);
		}

		chThdSleepUntil(time);
	}

	return 0;
}
Exemplo n.º 21
0
static msg_t
VexSonarTask( void *arg )
{
    tVexSonarChannel    c;

    (void)arg;

    chRegSetThreadName("sonar");

    gptStart( sonarGpt, &vexSonarGpt );

    while(!chThdShouldTerminate())
        {
        if( vexSonars[nextSonar].flags == (SONAR_INSTALLED | SONAR_ENABLED) )
            {
            // ping sonar
            vexSonarPing(nextSonar);

            // wait for next time slot
            // the timer is set to timeout in 40mS but we need a 10mS gap before any more
            // pings can be sent
            chThdSleepUntil(chTimeNow() + 50);

            // calculate echo time
            vexSonars[nextSonar].time = vexSonars[nextSonar].time_f - vexSonars[nextSonar].time_r;

            // was the time too great ?
            if( vexSonars[nextSonar].time > 35000 )
                vexSonars[nextSonar].time = -1;

            // if we have a valid time calculate real distance
            if( vexSonars[nextSonar].time != -1 )
                {
                vexSonars[nextSonar].distance_cm = vexSonars[nextSonar].time   / 58;
                vexSonars[nextSonar].distance_inch = vexSonars[nextSonar].time / 148;
                }
            else
                {
                vexSonars[nextSonar].distance_cm = -1;
                vexSonars[nextSonar].distance_inch = -1;
                }

            // look for next sonar
            for(c=kVexSonar_1;c<kVexSonar_Num;c++)
                {
                if( ++nextSonar == kVexSonar_Num )
                    nextSonar = kVexSonar_1;

                // we need sonar to be installed and enabled
                if( vexSonars[nextSonar].flags == (SONAR_INSTALLED | SONAR_ENABLED) )
                    break;
                }
            }
        else
            // Nothing enabled, just wait
            chThdSleepMilliseconds(25);
        }

    return (msg_t)0;
}
Exemplo n.º 22
0
/*
 * Encoder publisher node
 */
msg_t encoder_node(void *arg) {
	encoder_node_conf * conf = (encoder_node_conf *) arg;
	r2p::Node node(conf->name);
	r2p::Publisher<r2p::EncoderMsg> enc_pub;
	systime_t time;
	qeidelta_t delta;
	r2p::EncoderMsg *msgp;

	(void) arg;
	chRegSetThreadName(conf->name);

	/* Enable the QEI driver. */
	qeiInit();
	qeiStart(&QEI_DRIVER, &qeicfg);
	qeiEnable (&QEI_DRIVER);

	node.advertise(enc_pub, conf->topic);

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

		if (enc_pub.alloc(msgp)) {
			msgp->delta = delta * conf->t2r;
			enc_pub.publish(*msgp);
		}

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

	return CH_SUCCESS;
}
Exemplo n.º 23
0
/**
 * @brief   Suspends the invoking thread until the system time arrives to the
 *          specified value.
 *
 * @param[in] time      absolute system time
 *
 * @api
 */
void chThdSleepUntil(systime_t time) {

  chSysLock();
  if ((time -= chTimeNow()) > 0)
    chThdSleepS(time);
  chSysUnlock();
}
Exemplo n.º 24
0
/**
 * @brief Shaft position callback used by RPM calculation logic.
 *
 * This callback is invoked on interrupt thread.
 */
static void shaftPositionCallback(ShaftEvents ckpSignalType, int index) {
	itoa10(&shaft_signal_msg_index[1], index);
	if (ckpSignalType == SHAFT_PRIMARY_UP) {
		addWaveChartEvent("crank", "up", shaft_signal_msg_index);
	} else if (ckpSignalType == SHAFT_PRIMARY_DOWN) {
		addWaveChartEvent("crank", "down", shaft_signal_msg_index);
	} else if (ckpSignalType == SHAFT_SECONDARY_UP) {
		addWaveChartEvent("crank2", "up", shaft_signal_msg_index);
	} else if (ckpSignalType == SHAFT_SECONDARY_DOWN) {
		addWaveChartEvent("crank2", "down", shaft_signal_msg_index);
	}


	if (index != 0) {
#if EFI_PROD_CODE || EFI_SIMULATOR
		if (engineConfiguration->analogChartMode == AC_TRIGGER)
			acAddData(getCrankshaftAngle(chTimeNow()), 1000 * ckpSignalType + index);
#endif
		return;
	}
	rpmState.revolutionCounter++;

	time_t now = chTimeNow();

	int hadRpmRecently = isRunning();

	if (hadRpmRecently) {
		if (isNoisySignal(&rpmState, now)) {
			// unexpected state. Noise?
			rpmState.rpm = NOISY_RPM;
		} else {
			int diff = now - rpmState.lastRpmEventTime;
			// 60000 because per minute
			// * 2 because each revolution of crankshaft consists of two camshaft revolutions
			// / 4 because each cylinder sends a signal
			// need to measure time from the previous non-skipped event

			int rpm = (int)(60000 * TICKS_IN_MS / engineConfiguration->rpmMultiplier / diff);
			rpmState.rpm = rpm > UNREALISTIC_RPM ? NOISY_RPM : rpm;
		}
	}
	rpmState.lastRpmEventTime = now;
#if EFI_PROD_CODE || EFI_SIMULATOR
	if (engineConfiguration->analogChartMode == AC_TRIGGER)
		acAddData(getCrankshaftAngle(now), index);
#endif
}
Exemplo n.º 25
0
/**
 * @brief   Waits for a received frame.
 * @details Stops until a frame is received and buffered. If a frame is
 *          not immediately available then the invoking thread is queued
 *          until one is received.
 *
 * @param[in] macp      pointer to the @p MACDriver object
 * @param[out] rdp      pointer to a @p MACReceiveDescriptor structure
 * @param[in] time      the number of ticks before the operation timeouts,
 *                      the following special values are allowed:
 *                      - @a TIME_IMMEDIATE immediate timeout.
 *                      - @a TIME_INFINITE no timeout.
 *                      .
 * @return              The operation status.
 * @retval RDY_OK       the descriptor was obtained.
 * @retval RDY_TIMEOUT  the operation timed out, descriptor not initialized.
 */
msg_t macWaitReceiveDescriptor(MACDriver *macp,
                               MACReceiveDescriptor *rdp,
                               systime_t time) {
  msg_t msg;

  while (((msg = max_lld_get_receive_descriptor(macp, rdp)) != RDY_OK) &&
         (time > 0)) {
    chSysLock();
    systime_t now = chTimeNow();
    if ((msg = chSemWaitTimeoutS(&macp->md_rdsem, time)) == RDY_TIMEOUT)
      break;
    if (time != TIME_INFINITE)
      time -= (chTimeNow() - now);
    chSysUnlock();
  }
  return msg;
}
Exemplo n.º 26
0
static state_t do_state_ignite(instance_data_t *data)
{
    (void)data;
    state_estimation_trust_barometer = true;
    pyro_fire_ignite();
    data->t_last_ignite_attempt = chTimeNow();
    return STATE_WAIT_IGNITION;
}
Exemplo n.º 27
0
void cmd_systime(BaseSequentialStream *chp, int argc, char *argv) {
  (void)argv;
  if (argc > 0) {
    chprintf(chp, "Usage: systime\r\n");
    return;
  }
  chprintf(chp, "%lu\r\n", (unsigned long)chTimeNow());
}
Exemplo n.º 28
0
void inputEncoderFetcher(RadInputConfig* config, RadInputState* state)
{
  (void) config;
  if (state->is_enabled == 1) {
    state->is_enabled = 2;
    state->encoder.last_time = chTimeNow();
  }
}
Exemplo n.º 29
0
void LSM303calibrator::collecting(const float *data, uint32_t still_msk){
  uint32_t i;

  /* check immobility */
  if (! ((still_msk & STILL_MSK) == STILL_MSK)){
    sample = 0;
    red_blink_mb.post((msg_t)fastblink, TIME_IMMEDIATE);
    return;
  }

  /* */
  if (0 == sample){
    /* start from clear states */
    for(i=0; i<3; i++)
      abeta[i].reset(data[i], *zeroflen);
    sample++;
  }
  else{
    if (*zerocnt > sample){
      for(i=0; i<3; i++)
        abeta[i].update(data[i], *zeroflen);
      sample++;
    }
    else{
      /* statistics for single point successfully collected */
      for(uint32_t i=0; i<3; i++)
        P[point][i] = abeta[i].get(*zeroflen);
      point++;
      sample = 0;

      if (4 == point){
        /* we have enough points to calculate compensation sphere */
        SolveSphere(&S, P);
        point = 0;

        /* save data to registry and syng to eeprom */
        *(float *)(param_registry.valueSearch("MAG_xoffset"))   = S.c[0];
        *(float *)(param_registry.valueSearch("MAG_yoffset"))   = S.c[1];
        *(float *)(param_registry.valueSearch("MAG_zoffset"))   = S.c[2];
        *(float *)(param_registry.valueSearch("MAG_vectorlen")) = S.r;
        param_registry.syncParam("MAG_xoffset");
        param_registry.syncParam("MAG_yoffset");
        param_registry.syncParam("MAG_zoffset");
        param_registry.syncParam("MAG_vectorlen");

        /* final stuff */
        mavlink_dbg_print(MAV_SEVERITY_INFO, "MAG: calibration finished");
        mavlink_system_struct.state = MAV_STATE_STANDBY;
        state = LSM303_CAL_SLEEP;
      }
      else{
        mavlink_dbg_print(MAV_SEVERITY_NOTICE, "MAG: waiting for new position");
        last_point_timestamp = chTimeNow();
        state = LSM303_CAL_WAIT_NEXT;
      }
    }
  }
}
Exemplo n.º 30
0
/*
 * @brief   ...
 * @details ...
 */
bool radio_lld_is_timeout_expired(RadioDriver *radio) {
    if ((radio->timeout != 0) && (radio->timeout <= chTimeNow())) {
        consoleDevel("radio_lld_is_timeout_expired() == true\r\n");
        return true;
    } else {
        consoleDevel("radio_lld_is_timeout_expired() == false\r\n");
        return false;
    }
}