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;
}
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);
}
/**
* 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;
}
}
// 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;
}
}
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;
}
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;
}
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;
}
/*
* 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;
}
/**
* @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();
}
/**
* @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
}
/**
* @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;
}
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;
}
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());
}
void inputEncoderFetcher(RadInputConfig* config, RadInputState* state)
{
(void) config;
if (state->is_enabled == 1) {
state->is_enabled = 2;
state->encoder.last_time = chTimeNow();
}
}
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;
}
}
}
}
/*
* @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;
}
}
