bool_t osWaitForEvent(OsEvent *event, systime_t timeout)
{
msg_t msg;
//Wait until the specified event is in the signaled
//state or the timeout interval elapses
if(timeout == 0)
{
//Non-blocking call
msg = chBSemWaitTimeout(event, TIME_IMMEDIATE);
}
else if(timeout == INFINITE_DELAY)
{
//Infinite timeout period
msg = chBSemWaitTimeout(event, TIME_INFINITE);
}
else
{
//Wait until the specified event becomes set
msg = chBSemWaitTimeout(event, OS_MS_TO_SYSTICKS(timeout));
}
//Check whether the specified event is set
if(msg == RDY_OK)
return TRUE;
else
return FALSE;
}
/** Read bytes from the USART RX DMA buffer.
*
* \param s The USART DMA state structure.
* \param data Pointer to a buffer where the received data will be stored.
* \param len The number of bytes to attempt to read.
* \param timeout Return if this time passes with no reception.
* \return The number of bytes successfully read from the DMA receive buffer.
*/
u32 usart_read_dma_timeout(usart_rx_dma_state* s, u8 data[], u32 len, u32 timeout)
{
u32 n_available;
do {
n_available = usart_n_read_dma(s);
} while ((n_available < len) &&
(chBSemWaitTimeout(&s->ready_sem, timeout) == RDY_OK));
n_available = usart_n_read_dma(s);
u16 n = (len > n_available) ? n_available : len;
if (s->rd + n < USART_RX_BUFFER_LEN) {
memcpy(data, &(s->buff[s->rd]), n);
s->rd += n;
} else {
s->rd_wraps++;
memcpy(&data[0], &(s->buff[s->rd]), USART_RX_BUFFER_LEN - s->rd);
memcpy(&data[USART_RX_BUFFER_LEN - s->rd],
&(s->buff[0]), n - USART_RX_BUFFER_LEN + s->rd);
s->rd = n - USART_RX_BUFFER_LEN + s->rd;
}
s->byte_counter += n;
return n;
}
u32 usart_support_read_timeout(void *sd, u8 data[], u32 len, u32 timeout)
{
struct usart_rx_dma_state *s = &((struct usart_support_s *)sd)->rx;
u32 n_available;
do {
n_available = usart_support_n_read(sd);
} while ((n_available < len) &&
(chBSemWaitTimeout(&s->ready, timeout) == MSG_OK));
n_available = usart_support_n_read(sd);
u16 n = (len > n_available) ? n_available : len;
if (s->rd + n < USART_RX_BUFFER_LEN) {
memcpy(data, &(s->buff[s->rd]), n);
s->rd += n;
} else {
s->rd_wraps++;
memcpy(&data[0], &(s->buff[s->rd]), USART_RX_BUFFER_LEN - s->rd);
memcpy(&data[USART_RX_BUFFER_LEN - s->rd],
&(s->buff[0]), n - USART_RX_BUFFER_LEN + s->rd);
s->rd = n - USART_RX_BUFFER_LEN + s->rd;
}
return n;
}
/*
* Application entry point.
*/
int main(void) {
msg_t status = RDY_TIMEOUT;
halInit();
chSysInit();
chBSemInit(&alarm_sem, TRUE);
rtcGetTime(&RTCD1, ×pec);
alarmspec.tv_sec = timespec.tv_sec + RTC_ALARMPERIOD;
rtcSetAlarm(&RTCD1, 0, &alarmspec);
rtcSetCallback(&RTCD1, rtc_cb);
while (TRUE) {
/* Wait until alarm callback signaled semaphore.*/
status = chBSemWaitTimeout(&alarm_sem, S2ST(RTC_ALARMPERIOD + 10));
if (status == RDY_TIMEOUT) {
chSysHalt();
}
else {
rtcGetTime(&RTCD1, ×pec);
alarmspec.tv_sec = timespec.tv_sec + RTC_ALARMPERIOD;
rtcSetAlarm(&RTCD1, 0, &alarmspec);
}
}
return 0;
}
uint8_t Pill_t::CheckIfConnected() {
if(chBSemWaitTimeout(&ISem, TIME_INFINITE) != RDY_OK) return FAILURE;
uint8_t Rslt = i2c.CmdWriteWrite(EEADDR+IAddr, NULL, 0, NULL, 0);
Connected = (Rslt == OK);
if(i2c.Error == true) PillReset();
chBSemSignal(&ISem);
return Rslt;
}
uint8_t Pill_t::Read(uint8_t *Ptr, uint8_t Length) {
if(chBSemWaitTimeout(&ISem, TIME_INFINITE) != RDY_OK) return FAILURE;
uint8_t WordAddress = PILL_START_ADDR;
uint8_t Rslt = i2c.CmdWriteRead(EEADDR+IAddr, &WordAddress, 1, Ptr, Length);
Connected = (Rslt == OK);
if(i2c.Error == true) PillReset();
chBSemSignal(&ISem);
return Rslt;
}
/**
* @brief Wait on a mutex.
*/
osStatus osMutexWait(osMutexId mutex_id, uint32_t millisec) {
msg_t msg = chBSemWaitTimeout((binary_semaphore_t *)mutex_id,
(systime_t)millisec);
switch (msg) {
case MSG_OK:
return osOK;
case MSG_TIMEOUT:
return osErrorTimeoutResource;
}
return osErrorResource;
}
/**
* @brief Wait on a mutex.
*/
osStatus osMutexWait(osMutexId mutex_id, uint32_t millisec) {
systime_t timeout = ((millisec == 0) || (millisec == osWaitForever)) ?
TIME_INFINITE : MS2ST(millisec);
msg_t msg = chBSemWaitTimeout((binary_semaphore_t *)mutex_id, timeout);
switch (msg) {
case MSG_OK:
return osOK;
case MSG_TIMEOUT:
return osErrorTimeoutResource;
}
return osErrorResource;
}
static void nap_exti_thread(void *arg)
{
(void)arg;
chRegSetThreadName("NAP ISR");
while (TRUE) {
/* Waiting for the IRQ to happen.*/
chBSemWaitTimeout(&nap_exti_sem, MS2ST(PROCESS_PERIOD_ms));
handle_nap_exti();
tracking_channels_process();
}
}
static msg_t PollAccelThread(void *semp){
chRegSetThreadName("PollAccel");
msg_t sem_status = RDY_OK;
struct EventListener self_el;
chEvtRegister(&init_event, &self_el, INIT_FAKE_EVID);
while (TRUE) {
sem_status = chBSemWaitTimeout((BinarySemaphore*)semp, MS2ST(20));
txbuf[0] = ACCEL_STATUS;
if ((i2c_transmit(mma8451addr, txbuf, 1, rxbuf, 7) == RDY_OK) && (sem_status == RDY_OK)){
raw_data.xacc = complement2signed(rxbuf[1], rxbuf[2]);
raw_data.yacc = complement2signed(rxbuf[3], rxbuf[4]);
raw_data.zacc = complement2signed(rxbuf[5], rxbuf[6]);
/* there is no need of correcting of placement. Just get milli g */
mavlink_raw_imu_struct.xacc = raw_data.xacc * *xpol;
mavlink_raw_imu_struct.yacc = raw_data.yacc * *ypol;
mavlink_raw_imu_struct.zacc = raw_data.zacc * *zpol;
comp_data.xacc = 1000 * (((int32_t)raw_data.xacc) * *xpol + *xoffset) / *xsens;
comp_data.yacc = 1000 * (((int32_t)raw_data.yacc) * *ypol + *yoffset) / *ysens;
comp_data.zacc = 1000 * (((int32_t)raw_data.zacc) * *zpol + *zoffset) / *zsens;
/* fill scaled debug struct */
mavlink_scaled_imu_struct.xacc = comp_data.xacc;
mavlink_scaled_imu_struct.yacc = comp_data.yacc;
mavlink_scaled_imu_struct.zacc = comp_data.zacc;
}
else{
raw_data.xacc = -32768;
raw_data.yacc = -32768;
raw_data.zacc = -32768;
mavlink_raw_imu_struct.xacc = -32768;
mavlink_raw_imu_struct.yacc = -32768;
mavlink_raw_imu_struct.zacc = -32768;
mavlink_scaled_imu_struct.xacc = -32768;
mavlink_scaled_imu_struct.yacc = -32768;
mavlink_scaled_imu_struct.zacc = -32768;
}
if (chThdSelf()->p_epending & EVENT_MASK(SIGHALT_EVID))
chThdExit(RDY_OK);
}
return 0;
}
uint8_t Pill_t::Write(uint8_t *Ptr, uint8_t Length) {
if(chBSemWaitTimeout(&ISem, TIME_INFINITE) != RDY_OK) return FAILURE;
uint8_t WordAddress = PILL_START_ADDR;
uint8_t Rslt = OK;
// Write page by page
while(Length) {
uint8_t ToWriteCnt = (Length > PILL_PAGE_SZ)? PILL_PAGE_SZ : Length;
Rslt = i2c.CmdWriteWrite(EEADDR+IAddr, &WordAddress, 1, Ptr, ToWriteCnt);
if(Rslt == OK) {
chThdSleepMilliseconds(5); // Allow memory to complete writing
Length -= ToWriteCnt;
Ptr += ToWriteCnt;
WordAddress += ToWriteCnt;
}
else break;
}
Connected = (Rslt == OK);
if(i2c.Error == true) PillReset();
chBSemSignal(&ISem);
return Rslt;
}
static void nap_exti_thread(void *arg)
{
(void)arg;
chRegSetThreadName("NAP ISR");
while (TRUE) {
/* Waiting for the IRQ to happen.*/
chBSemWaitTimeout(&nap_exti_sem, MS2ST(PROCESS_PERIOD_ms));
/* We need a level (not edge) sensitive interrupt -
* if there is another interrupt pending on the Swift
* NAP then the IRQ line will stay high. Therefore if
* the line is still high, don't suspend the thread.
*/
spi_lock(SPI_SLAVE_FPGA);
while (palReadLine(LINE_NAP_IRQ)) {
handle_nap_exti();
}
tracking_channels_process();
spi_unlock(SPI_SLAVE_FPGA);
}
}
msg_t BinarySemaphore::waitTimeout(systime_t time) {
return chBSemWaitTimeout(&bsem, time);
}
static msg_t GTimerThreadHandler(void *arg) {
(void)arg;
GTimer *pt;
systime_t tm;
systime_t nxtTimeout;
systime_t lastTime;
GTimerFunction fn;
void *param;
#if CH_USE_REGISTRY
chRegSetThreadName("GTimer");
#endif
nxtTimeout = TIME_INFINITE;
lastTime = 0;
while(1) {
/* Wait for work to do. */
chThdYield(); // Give someone else a go no matter how busy we are
chBSemWaitTimeout(&waitsem, nxtTimeout);
restartTimerChecks:
// Our reference time
tm = chTimeNow();
nxtTimeout = TIME_INFINITE;
/* We need to obtain the mutex */
chMtxLock(&mutex);
if (pTimerHead) {
pt = pTimerHead;
do {
// Do we have something to do for this timer?
if ((pt->flags & GTIMER_FLG_JABBED) || (!(pt->flags & GTIMER_FLG_INFINITE) && TimeIsWithin(pt->when, lastTime, tm))) {
// Is this timer periodic?
if ((pt->flags & GTIMER_FLG_PERIODIC) && pt->period != TIME_IMMEDIATE) {
// Yes - Update ready for the next period
if (!(pt->flags & GTIMER_FLG_INFINITE)) {
// We may have skipped a period.
// We use this complicated formulae rather than a loop
// because the gcc compiler stuffs up the loop so that it
// either loops forever or doesn't get executed at all.
pt->when += ((tm + pt->period - pt->when) / pt->period) * pt->period;
}
// We are definitely no longer jabbed
pt->flags &= ~GTIMER_FLG_JABBED;
} else {
// No - get us off the timers list
if (pt->next == pt->prev)
pTimerHead = 0;
else {
pt->next->prev = pt->prev;
pt->prev->next = pt->next;
if (pTimerHead == pt)
pTimerHead = pt->next;
}
pt->flags = 0;
}
// Call the callback function
fn = pt->fn;
param = pt->param;
chMtxUnlock();
fn(param);
// We no longer hold the mutex, the callback function may have taken a while
// and our list may have been altered so start again!
goto restartTimerChecks;
}
// Find when we next need to wake up
if (!(pt->flags & GTIMER_FLG_INFINITE) && pt->when - tm < nxtTimeout)
nxtTimeout = pt->when - tm;
pt = pt->next;
} while(pt != pTimerHead);
}
// Ready for the next loop
lastTime = tm;
chMtxUnlock();
}
return 0;
}
THD_FUNCTION(scMS5611Thread, arg)
{
systime_t sleep_until;
systime_t interval_st = MS2ST(ms5611_interval_ms);
(void)arg;
chRegSetThreadName(__func__);
SC_LOG_PRINTF("d: ms5611 init\r\n");
// We need to give a bit of time to the chip until we can ask it to shut down
// i2c not ready initially? (This is inherited from the LSM9DS0 driver)
chThdSleepMilliseconds(20);
ms5611_reset();
chThdSleepMilliseconds(100);
ms5611_reset();
chThdSleepMilliseconds(100);
ms5611_reset();
chThdSleepMilliseconds(100);
ms5611_read_prom();
uint8_t ref_crc = ms5611_prom[MS5611_PROM_SIZE - 1] & 0x0F;
uint8_t calc_crc = ms5611_calc_crc();
if (ref_crc != calc_crc) {
SC_LOG_PRINTF("e: ms5611 crc failure: 0x%x vs 0x%x\r\n", ref_crc, calc_crc);
}
sleep_until = chVTGetSystemTime() + interval_st;
// Loop initiating measurements and waiting for results
while (!chThdShouldTerminateX()) {
systime_t now = chVTGetSystemTime();
systime_t wait_time = sleep_until - now;
uint32_t temp;
uint32_t pres;
msg_t msg;
// Using a semaphore allows to cancel the sleep outside the thread
if (chBSemWaitTimeout(&ms5611_wait_sem, wait_time) != MSG_TIMEOUT) {
continue;
}
sleep_until += interval_st;
now = chVTGetSystemTime();
temp = ms5611_read(true);
if (chThdShouldTerminateX()) {
break;
}
pres = ms5611_read(false);
chMtxLock(&data_mtx);
ms5611_temp = temp;
ms5611_pres = pres;
ms5611_time_st = now;
chMtxUnlock(&data_mtx);
// Notify main app about new measurements being available
msg = sc_event_msg_create_type(SC_EVENT_TYPE_MS5611_AVAILABLE);
sc_event_msg_post(msg, SC_EVENT_MSG_POST_FROM_NORMAL);
}
}
