static void thd4_execute(void) {
systime_t time;
test_wait_tick();
/* Timeouts in microseconds.*/
time = chTimeNow();
chThdSleepMicroseconds(100000);
test_assert_time_window(1, time + US2ST(100000), time + US2ST(100000) + 1);
/* Timeouts in milliseconds.*/
time = chTimeNow();
chThdSleepMilliseconds(100);
test_assert_time_window(2, time + MS2ST(100), time + MS2ST(100) + 1);
/* Timeouts in seconds.*/
time = chTimeNow();
chThdSleepSeconds(1);
test_assert_time_window(3, time + S2ST(1), time + S2ST(1) + 1);
/* Absolute timelines.*/
time = chTimeNow() + MS2ST(100);
chThdSleepUntil(time);
test_assert_time_window(4, time, time + 1);
}
static void test_001_002_execute(void) {
systime_t time;
/* The current system time is read then a sleep is performed for 100 system
ticks and on exit the system time is verified again.*/
test_set_step(1);
{
time = chVTGetSystemTimeX();
chThdSleep(100);
test_assert_time_window(time + 100,
time + 100 + 1,
"out of time window");
}
/* The current system time is read then a sleep is performed for 100000
microseconds and on exit the system time is verified again.*/
test_set_step(2);
{
time = chVTGetSystemTimeX();
chThdSleepMicroseconds(100);
test_assert_time_window(time + US2ST(100),
time + US2ST(100) + 1,
"out of time window");
}
/* The current system time is read then a sleep is performed for 100
milliseconds and on exit the system time is verified again.*/
test_set_step(3);
{
time = chVTGetSystemTimeX();
chThdSleepMilliseconds(100);
test_assert_time_window(time + MS2ST(100),
time + MS2ST(100) + 1,
"out of time window");
}
/* The current system time is read then a sleep is performed for 1
second and on exit the system time is verified again.*/
test_set_step(4);
{
time = chVTGetSystemTimeX();
chThdSleepSeconds(1);
test_assert_time_window(time + S2ST(1),
time + S2ST(1) + 1,
"out of time window");
}
test_set_step(5);
{
time = chVTGetSystemTimeX();
chThdSleepUntil(time + 100);
test_assert_time_window(time + 100,
time + 100 + 1,
"out of time window");
}
}
/**
* @brief SSI thread main function
* @param void * arg Thread arguments (NULL)
* @return msg_t Thread messages
*/
msg_t ssi_main(void *arg)
{
ssi_init(); // Initialize SSI Encoder
uint32_t position; // Position buffer
// Thread main loop
systime_t time = chTimeNow();
while(1)
{
time = chTimeNow();
if((ssi_mode == SSI_MODE_STREAM) && (encoder_power_state == ENCODER_ON))
{
time += US2ST(ssi_read_delay);
position = ssi_read(); // Read SSI Encoder
bt_send_position(position); // Send out encoder data via serial
while(!bt_get_tx_state())
{
__asm__("NOP"); // Wait for position send to complete
}
chThdSleepUntil(time);
}
else
{
chThdSleepMilliseconds(500);
}
}
}
void serial_link_update(void) {
if (read_serial_link_connected()) {
serial_link_connected = true;
}
matrix_object_t matrix;
bool changed = false;
for(uint8_t i=0;i<MATRIX_ROWS;i++) {
matrix.rows[i] = matrix_get_row(i);
changed |= matrix.rows[i] != last_matrix.rows[i];
}
systime_t current_time = chVTGetSystemTimeX();
systime_t delta = current_time - last_update;
if (changed || delta > US2ST(1000)) {
last_update = current_time;
last_matrix = matrix;
matrix_object_t* m = begin_write_keyboard_matrix();
for(uint8_t i=0;i<MATRIX_ROWS;i++) {
m->rows[i] = matrix.rows[i];
}
end_write_keyboard_matrix();
*begin_write_serial_link_connected() = true;
end_write_serial_link_connected();
}
matrix_object_t* m = read_keyboard_matrix(0);
if (m) {
matrix_set_remote(m->rows, 0);
}
}
inline
SpinEvent_::Mask SpinEvent_::wait(const Time &timeout) {
systime_t ticks;
if (timeout == Time::IMMEDIATE) ticks = TIME_IMMEDIATE;
else if (timeout == Time::INFINITE) ticks = TIME_INFINITE;
else if (timeout.to_us_raw() > 100000) ticks = MS2ST(timeout.to_ms_raw());
else ticks = US2ST(timeout.to_us_raw());
return chEvtWaitAnyTimeout(ALL_EVENTS, ticks);
}
void vMBMasterPortTimersT35Enable()
{
//chprintf((BaseSequentialStream *)&itm_port, "%s\n", "T35 Enable");
/* Set current timer mode, don't change it.*/
vMBMasterSetCurTimerMode(MB_TMODE_T35);
palSetPad(GPIOC, GPIOC_PIN9);
chSysLockFromISR();
chVTResetI(&vt35);
chVTSetI(&vt35, US2ST((uint32_t)2000), timer_timeout_ind, NULL);
chSysUnlockFromISR();
}
msg_t main() override
{
zubax_chibios::watchdog::Timer wdt;
wdt.startMSec(1000);
setName("mag");
::usleep(500000); // Startup delay
wdt.reset();
node::markComponentInitialized(node::ComponentID::Magnetometer);
while (!tryInit() && !node::hasPendingRestartRequest())
{
setStatus(uavcan::protocol::NodeStatus::HEALTH_ERROR);
lowsyslog("Mag init failed, will retry...\n");
::usleep(500000);
wdt.reset();
}
wdt.reset();
const float variance = param_variance.get();
const uint64_t period_usec = param_period_usec.get();
systime_t sleep_until = chibios_rt::System::getTime();
while (!node::hasPendingRestartRequest())
{
sleep_until += US2ST(period_usec);
float vector[3] = {0, 0, 0};
if (tryRead(vector))
{
transformToNEDFrame(vector);
publish(vector, variance);
setStatus(estimateStatusFromMeasurement(vector));
}
else
{
setStatus(uavcan::protocol::NodeStatus::HEALTH_ERROR);
}
sysSleepUntilChTime(sleep_until);
wdt.reset();
}
lowsyslog("Mag driver terminated\n");
return msg_t();
}
/*
* Note that this packet is stored and sent in another thread after a delay. The delay is done
* in case that this packet is a request from the (Qt) client, which means that more requests
* can come within a short time. We want to give the additional requests a chance to arrive
* before blocking the RF channel by sending the response.
*/
void comm_cc2520_send_packet(uint8_t *data, uint8_t len) {
memcpy(tx_buffer[tx_slot_write], data, len);
tx_slot_len[tx_slot_write] = len;
tx_slot_write++;
if (tx_slot_write >= TX_BUFFER_SLOTS) {
tx_slot_write = 0;
}
chSysLock();
if (!chVTIsArmedI(&vt)) {
chVTSetI(&vt, US2ST(TX_DELAY_US), wakeup_tx, NULL);
}
chSysUnlock();
}
/*! \brief ADIS Newdata Thread
*/
static msg_t Thread_adis_newdata(void *arg) {
(void)arg;
chRegSetThreadName("adis_newdata");
static const evhandler_t evhndl_newdata[] = {
adis_newdata_handler
};
struct EventListener evl_spi_cb2;
chEvtRegister(&adis_spi_cb_newdata, &evl_spi_cb2, 0);
while (TRUE) {
chEvtDispatch(evhndl_newdata, chEvtWaitOneTimeout((eventmask_t)1, US2ST(50)));
}
return -1;
}
static THD_FUNCTION(ThreadINA, arg)
{
(void)arg;
chRegSetThreadName("INA");
systime_t time = chVTGetSystemTimeX(); // T0
while (true) {
time += US2ST(READFREQ);
//palSetPad(GPIOG, GPIOG_LED4_RED);
//palTogglePad(GPIOG, GPIOG_LED4_RED);
busvoltage=ina219GetBusVoltage_V();
current_mA=ina219GetCurrent_mA();
shuntvoltage = ina219GetShuntVoltage_mV();
loadvoltage = busvoltage + (shuntvoltage / 1000);
milliwatthours += busvoltage*current_mA*READFREQ/1e6/3600; // 1 Wh = 3600 joules
milliamphours += current_mA*READFREQ/1e6/3600;
// Update peaks, min and avg during our serial refresh period:
if (current_mA > rpPeakCurrent)
rpPeakCurrent = current_mA;
if (current_mA < rpMinCurrent)
rpMinCurrent = current_mA;
if (loadvoltage > rpPeakLoadVolt)
rpPeakLoadVolt = loadvoltage;
if (loadvoltage < rpMinLoadVolt)
rpMinLoadVolt = loadvoltage;
rpAvgCurrent = (rpAvgCurrent*rpSamples + current_mA)/(rpSamples+1);
rpAvgLoadVolt = (rpAvgLoadVolt*rpSamples + loadvoltage)/(rpSamples+1);
rpSamples++;
// Update absolute peaks and mins
if (current_mA > peakCurrent) {
peakCurrent = current_mA;
voltageAtPeakCurrent = loadvoltage;
}
if (loadvoltage < minVoltage) {
minVoltage = loadvoltage;palSetPadMode(GPIOA, 5, PAL_MODE_OUTPUT_PUSHPULL); palClearPad(GPIOA, 5);
currentAtMinVoltage = current_mA;
}
//palClearPad(GPIOG, GPIOG_LED4_RED);
chThdSleepUntil(time);
}
}
bool mcucom_port_condvar_wait(mcucom_port_cond_t *cond, mcucom_port_mutex_t *mutex, uint32_t timeout_us)
{
(void)mutex;
if (timeout_us == MCUCOM_PORT_TIMEOUT_IMMEDIATE) {
return false;
}
systime_t timeout;
if (timeout_us == MCUCOM_PORT_TIMEOUT_NEVER) {
timeout = TIME_INFINITE;
} else {
timeout = US2ST(timeout_us);
}
msg_t ret = chCondWaitTimeout(cond, timeout);
if (ret == MSG_TIMEOUT) {
chMtxLock(mutex);
return false;
} else {
return true;
}
}
static void bridge(t_hydra_console *con)
{
uint8_t tx_data[UART_BRIDGE_BUFF_SIZE];
uint8_t bytes_read;
//uint8_t bytes_read;
mode_config_proto_t* proto = &con->mode->proto;
cprintf(con, "Interrupt by pressing user button.\r\n");
cprint(con, "\r\n", 2);
thread_t *bthread = chThdCreateFromHeap(NULL, CONSOLE_WA_SIZE, "bridge_thread",
LOWPRIO, bridge_thread, con);
while(!USER_BUTTON) {
bytes_read = chnReadTimeout(con->sdu, tx_data,
UART_BRIDGE_BUFF_SIZE, US2ST(100));
if(bytes_read > 0) {
bsp_uart_write_u8(proto->dev_num, tx_data, bytes_read);
}
}
chThdTerminate(bthread);
chThdWait(bthread);
}
static THD_FUNCTION(bridge_thread, arg)
{
t_hydra_console *con;
con = arg;
chRegSetThreadName("UART reader");
chThdSleepMilliseconds(10);
uint8_t rx_data[UART_BRIDGE_BUFF_SIZE];
uint8_t bytes_read;
mode_config_proto_t* proto = &con->mode->proto;
while (!USER_BUTTON) {
if(bsp_uart_rxne(proto->dev_num)) {
bytes_read = bsp_uart_read_u8_timeout(proto->dev_num,
rx_data,
UART_BRIDGE_BUFF_SIZE,
US2ST(100));
if(bytes_read > 0) {
cprint(con, (char *)rx_data, bytes_read);
}
} else {
chThdYield();
}
}
}
/**
* @brief Sync procedure.
* @details This function implements the synchronization protocol
* between the STM and ESP/RPi
*
* @param[in] driver pointer to the DataLinkLayer driver object
*/
void DLLSyncProcedure(DLLDriver *driver)
{
int FFs = 0;
char c;
DLLSendSyncFrame(driver);
driver->DLLStats.SyncCounter++;
while(FFs != FRAME_SIZE_BYTE)
{
driver->DLLStats.SyncTimeout++;
sdReadTimeout(driver->config->SDriver, &c, 1, US2ST(1000));
if(c == 0xFF)
{
FFs++;
c = 0x00;
}
if(driver->DLLStats.SyncTimeout >= SYNC_TIMEOUT_THRS){
DLLSendSyncFrame(driver);
driver->DLLStats.SyncTimeout = 0;
}
}
driver->DLLStats.SyncTimeout = 0;
return;
}
static THD_FUNCTION(mpu_thread, arg) {
(void)arg;
chRegSetThreadName("MPU Sampling");
static int16_t raw_accel_gyro_mag_tmp[9];
#if USE_MAGNETOMETER
static int mag_cnt = MAG_DIV;
#endif
static systime_t iteration_timer = 0;
static int identical_reads = 0;
iteration_timer = chVTGetSystemTime();
for(;;) {
if (get_raw_accel_gyro(raw_accel_gyro_mag_tmp)) {
int is_identical = 1;
for (int i = 0;i < 6;i++) {
if (raw_accel_gyro_mag_tmp[i] != raw_accel_gyro_mag_no_offset[i]) {
is_identical = 0;
break;
}
}
if (is_identical) {
identical_reads++;
} else {
identical_reads = 0;
}
if (identical_reads >= MAX_IDENTICAL_READS) {
failed_reads++;
chThdSleepMicroseconds(FAIL_DELAY_US);
reset_init_mpu();
iteration_timer = chVTGetSystemTime();
} else {
memcpy((uint16_t*)raw_accel_gyro_mag_no_offset, raw_accel_gyro_mag_tmp, sizeof(raw_accel_gyro_mag));
raw_accel_gyro_mag_tmp[3] -= mpu9150_gyro_offsets[0];
raw_accel_gyro_mag_tmp[4] -= mpu9150_gyro_offsets[1];
raw_accel_gyro_mag_tmp[5] -= mpu9150_gyro_offsets[2];
memcpy((uint16_t*)raw_accel_gyro_mag, raw_accel_gyro_mag_tmp, sizeof(raw_accel_gyro_mag));
update_time_diff = chVTGetSystemTime() - last_update_time;
last_update_time = chVTGetSystemTime();
if (read_callback) {
read_callback();
}
#if USE_MAGNETOMETER
mag_cnt++;
if (mag_cnt >= MAG_DIV) {
mag_cnt = 0;
mag_updated = 1;
int16_t raw_mag_tmp[3];
if (get_raw_mag(raw_mag_tmp)) {
memcpy((uint16_t*)raw_accel_gyro_mag_tmp + 6, raw_mag_tmp, sizeof(raw_mag_tmp));
} else {
failed_mag_reads++;
chThdSleepMicroseconds(FAIL_DELAY_US);
reset_init_mpu();
iteration_timer = chVTGetSystemTime();
}
} else {
mag_updated = 0;
}
#endif
}
} else {
failed_reads++;
chThdSleepMicroseconds(FAIL_DELAY_US);
reset_init_mpu();
iteration_timer = chVTGetSystemTime();
}
iteration_timer += US2ST(ITERATION_TIME_US);
systime_t time_start = chVTGetSystemTime();
if (iteration_timer > time_start) {
chThdSleep(iteration_timer - time_start);
} else {
chThdSleepMicroseconds(MIN_ITERATION_DELAY_US);
iteration_timer = chVTGetSystemTime();
}
}
}
bool rpm_is_engine_running(void)
{
return chVTTimeElapsedSinceX(m_last_update) < US2ST(UPDATE_TIMEOUT_US)
&& m_curr_rpm > gp_rpm_min_idle;
}
static void test_002_001_execute(void) {
systime_t time;
/* [2.1.1] The current system time is read then a sleep is performed
for 100 system ticks and on exit the system time is verified
again.*/
test_set_step(1);
{
time = chVTGetSystemTimeX();
chThdSleep(100);
test_assert_time_window(time + 100,
time + 100 + CH_CFG_ST_TIMEDELTA + 1,
"out of time window");
}
/* [2.1.2] The current system time is read then a sleep is performed
for 100000 microseconds and on exit the system time is verified
again.*/
test_set_step(2);
{
time = chVTGetSystemTimeX();
chThdSleepMicroseconds(100000);
test_assert_time_window(time + US2ST(100000),
time + US2ST(100000) + CH_CFG_ST_TIMEDELTA + 1,
"out of time window");
}
/* [2.1.3] The current system time is read then a sleep is performed
for 100 milliseconds and on exit the system time is verified
again.*/
test_set_step(3);
{
time = chVTGetSystemTimeX();
chThdSleepMilliseconds(100);
test_assert_time_window(time + MS2ST(100),
time + MS2ST(100) + CH_CFG_ST_TIMEDELTA + 1,
"out of time window");
}
/* [2.1.4] The current system time is read then a sleep is performed
for 1 second and on exit the system time is verified again.*/
test_set_step(4);
{
time = chVTGetSystemTimeX();
chThdSleepSeconds(1);
test_assert_time_window(time + S2ST(1),
time + S2ST(1) + CH_CFG_ST_TIMEDELTA + 1,
"out of time window");
}
/* [2.1.5] Function chThdSleepUntil() is tested with a timeline of
"now" + 100 ticks.*/
test_set_step(5);
{
time = chVTGetSystemTimeX();
chThdSleepUntil(time + 100);
test_assert_time_window(time + 100,
time + 100 + CH_CFG_ST_TIMEDELTA + 1,
"out of time window");
}
}
/*! \brief ADIS DIO1 thread
*
* For burst mode transactions t_readrate is 1uS
*
*/
static msg_t Thread_adis_dio1(void *arg) {
(void)arg;
static const evhandler_t evhndl_dio1[] = {
adis_burst_read_handler,
//adis_read_id_handler,
adis_spi_cb_txdone_handler,
adis_release_bus
};
struct EventListener evl_dio;
struct EventListener evl_spi_ev;
struct EventListener evl_spi_release;
chRegSetThreadName("adis_dio");
chEvtRegister(&adis_dio1_event, &evl_dio, 0);
chEvtRegister(&adis_spi_cb_txdone_event, &evl_spi_ev, 1);
chEvtRegister(&adis_spi_cb_releasebus, &evl_spi_release, 2);
while (TRUE) {
chEvtDispatch(evhndl_dio1, chEvtWaitOneTimeout((EVENT_MASK(2)|EVENT_MASK(1)|EVENT_MASK(0)), US2ST(50)));
}
return -1;
}
inline
bool Semaphore_::wait(const Time &timeout) {
return chSemWaitTimeout(&impl, US2ST(timeout.to_us_raw())) == RDY_OK;
}