static THD_FUNCTION(mp45dt02ProcessingThd, arg)
{
(void)arg;
chRegSetThreadName(__FUNCTION__);
while (chThdShouldTerminateX() == false)
{
chSemWait(&mp45dt02ProcessingSem);
if (chThdShouldTerminateX() == true)
{
break;
}
if (mp45dt02I2sData.number != MP45DT02_I2S_SAMPLE_SIZE_2B)
{
PRINT_CRITICAL("Unexpected number of samples provided. %d not %d.",
mp45dt02I2sData.number,
MP45DT02_I2S_SAMPLE_SIZE_2B);
}
/**********************************************************************/
/* Convert I2S data to a useful format */
/**********************************************************************/
expand(mp45dt02ExpandedBuffer,
&mp45dt02I2sData.buffer[mp45dt02I2sData.offset]);
/**********************************************************************/
/* Filtering */
/**********************************************************************/
arm_fir_decimate_f32(&cmsisDsp.decimateInstance,
mp45dt02ExpandedBuffer,
mp45dt02DecimatedBuffer,
MP45DT02_EXPANDED_BUFFER_SIZE);
/**********************************************************************/
/* Notify of new data */
/**********************************************************************/
initConfig.fullbufferCb(mp45dt02DecimatedBuffer,
MP45DT02_DECIMATED_BUFFER_SIZE);
if (mp45dt02I2sData.guard != MEMORY_GUARD)
{
PRINT_CRITICAL("Overflow detected.",0);
}
if (cmsisDsp.guard != MEMORY_GUARD)
{
PRINT_CRITICAL("Overflow detected.",0);
}
}
}
/*
* Application entry point.
*/
int main(void) {
/*
* System initializations.
* - HAL initialization, this also initializes the configured device drivers
* and performs the board-specific initializations.
* - Kernel initialization, the main() function becomes a thread and the
* RTOS is active.
*/
halInit();
chSysInit();
/*
* Activates serial 1 (UART0) using the driver default configuration.
*/
sdStart(&SD1, &s0cfg);
sdPut(&SD1,'B');
chThdCreateStatic(waSerEcho, sizeof(waSerEcho), NORMALPRIO, thSerEcho, NULL);
while (!chThdShouldTerminateX()) {
chThdSleepMilliseconds(1000);
palTogglePad(TEENSY_PIN13_IOPORT, TEENSY_PIN13);
sdPut(&SD1,'B');
}
return 0;
}
static THD_FUNCTION(thSerEcho, arg)
{
(void)arg;
chRegSetThreadName("SerEcho");
event_listener_t elSerData;
eventflags_t flags;
chEvtRegisterMask((event_source_t *)chnGetEventSource(&SD1), &elSerData, EVENT_MASK(1));
while (!chThdShouldTerminateX())
{
chEvtWaitOneTimeout(EVENT_MASK(1), MS2ST(10));
flags = chEvtGetAndClearFlags(&elSerData);
if (flags & CHN_INPUT_AVAILABLE)
{
msg_t charbuf;
do
{
charbuf = chnGetTimeout(&SD1, TIME_IMMEDIATE);
if ( charbuf != Q_TIMEOUT )
{
chSequentialStreamPut(&SD1, charbuf);
}
}
while (charbuf != Q_TIMEOUT);
}
}
}
/*
* Application entry point.
*/
int main(void) {
/*
* System initializations.
* - HAL initialization, this also initializes the configured device drivers
* and performs the board-specific initializations.
* - Kernel initialization, the main() function becomes a thread and the
* RTOS is active.
*/
halInit();
chSysInit();
/*
* Activates the ADC1 driver.
*/
adcStart(&ADCD1, &adccfg1);
while (!chThdShouldTerminateX()) {
/*
* ADC linear conversion.
*/
adcConvert(&ADCD1, &adcgrpcfg1, samples1, ADC_GRP1_BUF_DEPTH);
chThdSleepMilliseconds(1000);
}
return 0;
}
static THD_FUNCTION(cancom_thread, arg) {
(void)arg;
chRegSetThreadName("CAN");
event_listener_t el;
CANRxFrame rxmsg;
uint8_t buffer[9];
chEvtRegister(&CANDx.rxfull_event, &el, 0);
while(!chThdShouldTerminateX()) {
if (chEvtWaitAnyTimeout(ALL_EVENTS, MS2ST(100)) == 0) {
continue;
}
while (canReceive(&CANDx, CAN_ANY_MAILBOX, &rxmsg, TIME_IMMEDIATE) == MSG_OK) {
buffer[0] = rxmsg.SID;
for (int i = 0;i < rxmsg.DLC;i++) {
buffer[i + 1] = rxmsg.data8[i];
}
packet_handler_int_process_packet(buffer, rxmsg.DLC + 1);
}
}
chEvtUnregister(&CAND1.rxfull_event, &el);
}
/*
* Test worker threads.
*/
static THD_FUNCTION(irq_storm_thread, arg) {
static volatile unsigned x = 0;
static unsigned cnt = 0;
unsigned me = (unsigned)arg;
unsigned target;
unsigned r;
msg_t msg;
chRegSetThreadName("irq_storm");
/* Thread loop, until terminated.*/
while (chThdShouldTerminateX() == false) {
/* Waiting for a message.*/
chMBFetch(&mb[me], &msg, TIME_INFINITE);
#if IRQ_STORM_CFG_RANDOMIZE != FALSE
/* Pseudo-random delay.*/
{
chSysLock();
r = rand() & 15;
chSysUnlock();
while (r--)
x++;
}
#else /* IRQ_STORM_CFG_RANDOMIZE == FALSE */
/* Fixed delay.*/
{
r = me >> 4;
while (r--)
x++;
}
#endif /* IRQ_STORM_CFG_RANDOMIZE == FALSE */
/* Deciding in which direction to re-send the message.*/
if (msg == MSG_SEND_LEFT)
target = me - 1;
else
target = me + 1;
if (target < IRQ_STORM_CFG_NUM_THREADS) {
/* If this thread is not at the end of a chain re-sending the message,
note this check works because the variable target is unsigned.*/
msg = chMBPost(&mb[target], msg, TIME_IMMEDIATE);
if (msg != MSG_OK)
saturated = TRUE;
}
else {
/* Provides a visual feedback about the system.*/
if (++cnt >= 500) {
cnt = 0;
palTogglePad(config->port, config->pad);
}
}
}
}
/*
* Application entry point.
*/
int main(void) {
thread_t *shelltp = NULL;
/*
* System initializations.
* - HAL initialization, this also initializes the configured device drivers
* and performs the board-specific initializations.
* - Kernel initialization, the main() function becomes a thread and the
* RTOS is active.
*/
halInit();
chSysInit();
#if defined(DEBUG_USB)
palSetPadMode(GPIOA, 1, PAL_MODE_ALTERNATIVE_2);
palSetPadMode(GPIOA, 2, PAL_MODE_ALTERNATIVE_2);
sdStart(&SD1, &s0cfg);
#endif /* DEBUG_USB */
/*
* Initializes a serial-over-USB CDC driver.
*/
sduObjectInit(&SDU1);
sduStart(&SDU1, &serusbcfg);
/*
* Activates the USB driver and then the USB bus pull-up on D+.
* Note, a delay is inserted in order to not have to disconnect the cable
* after a reset.
*/
usbDisconnectBus(serusbcfg.usbp);
#if defined(DEBUG_USB)
usb_debug_init();
#endif /* DEBUG_USB */
chThdSleepMilliseconds(1000);
usbStart(serusbcfg.usbp, &usbcfg);
usbConnectBus(serusbcfg.usbp);
/*
* Shell manager initialization.
*/
shellInit();
while (!chThdShouldTerminateX()) {
if (!shelltp && (serusbcfg.usbp->state == USB_ACTIVE))
shelltp = shellCreate(&shell_cfg1, SHELL_WA_SIZE, NORMALPRIO);
else if (chThdTerminatedX(shelltp)) {
chThdRelease(shelltp); /* Recovers memory of the previous shell. */
shelltp = NULL; /* Triggers spawning of a new shell. */
}
chThdSleepMilliseconds(1000);
palTogglePad(GPIOB, GPIOB_LED);
}
return 0;
}
/*
* Console print server done using synchronous messages. This makes the access
* to the C printf() thread safe and the print operation atomic among threads.
* In this example the message is the zero terminated string itself.
*/
static THD_FUNCTION(console_thread, arg) {
(void)arg;
while (!chThdShouldTerminateX()) {
thread_t *tp = chMsgWait();
puts((char *)chMsgGet(tp));
fflush(stdout);
chMsgRelease(tp, MSG_OK);
}
}
static THD_FUNCTION(thEDisplay, arg)
{
(void)arg;
chRegSetThreadName("eDisplay");
kbb_display.header.preamble = KBB_PREAMBLE;
kbb_display.header.msg_class = KBB_CLASS_EXTERNAL;
kbb_display.header.msg_subclass = KBB_SUBCLASS_EXTERNAL_01;
kbb_display.header.msg_size = KBB_DISPLAY_SIZE;
while( !chThdShouldTerminateX() )
{
chSysLock();
eDisplayThreadForSleep = chThdGetSelfX();
chSchGoSleepS(CH_STATE_SUSPENDED);
chSysUnlock();
if ( !ed_serialPort )
continue;
if ( ed_serialPort == &Serial1 )
if ( kbse_getBaudSerial1() < 115200 )
continue;
if ( ed_serialPort == &Serial2 )
if ( kbse_getBaudSerial2() < 115200 )
continue;
const kbb_current_msg_t * msg = kbw_getCurrentMsg();
memcpy(&kbb_display.ltc_frame, &msg->ltc_frame, sizeof(kbb_display.ltc_frame));
memcpy(&kbb_display.smpte_time, &msg->smpte_time, sizeof(kbb_display.smpte_time));
kbb_display.ecef[0] = msg->nav_sol.ecefX;
kbb_display.ecef[1] = msg->nav_sol.ecefY;
kbb_display.ecef[2] = msg->nav_sol.ecefZ;
memcpy(&kbb_display.vnav, &msg->vnav, sizeof(kbb_display.vnav));
kbb_display.temperature = msg->temperature;
int32_t * i32buf = (int32_t*)kbb_display.__pad;
i32buf[0] = AA;
i32buf[1] = BB;
i32buf[2] = CC;
uint8_t * buf = (uint8_t*) &kbb_display;
kbb_display.header.checksum = calc_checksum_16(buf+KBB_CHECKSUM_START,
KBB_DISPLAY_SIZE-KBB_CHECKSUM_START);
// display!!!
int size_written = sdWrite(ed_serialPort, buf, KBB_DISPLAY_SIZE);
if ( size_written != KBB_DISPLAY_SIZE )
{
kbg_toggleLED3();
ASSERT(size_written==KBB_DISPLAY_SIZE,"thEDisplay", "Wrong sent data!!");
}
}
return;
}
/*
* Console print server done using synchronous messages. This makes the access
* to the C printf() thread safe and the print operation atomic among threads.
* In this example the message is the zero terminated string itself.
*/
static msg_t console_thread(void *arg) {
(void)arg;
while (!chThdShouldTerminateX()) {
thread_t *tp = chMsgWait();
puts((char *)chMsgGet(tp));
fflush(stdout);
chMsgRelease(tp, MSG_OK);
}
return 0;
}
/*------------------------------------------------------------------------*
* Simulator main. *
*------------------------------------------------------------------------*/
int main(void) {
event_listener_t tel;
/*
* System initializations.
* - HAL initialization, this also initializes the configured device drivers
* and performs the board-specific initializations.
* - Kernel initialization, the main() function becomes a thread and the
* RTOS is active.
*/
halInit();
chSysInit();
/*
* Serial ports (simulated) initialization.
*/
sdStart(&SD1, NULL);
sdStart(&SD2, NULL);
/*
* Shell manager initialization.
*/
shellInit();
chEvtRegister(&shell_terminated, &tel, 0);
/*
* Console thread started.
*/
cdtp = chThdCreateFromHeap(NULL, CONSOLE_WA_SIZE, NORMALPRIO + 1,
console_thread, NULL);
/*
* Initializing connection/disconnection events.
*/
cputs("Shell service started on SD1, SD2");
cputs(" - Listening for connections on SD1");
chEvtRegister(chnGetEventSource(&SD1), &sd1fel, 1);
cputs(" - Listening for connections on SD2");
chEvtRegister(chnGetEventSource(&SD2), &sd2fel, 2);
/*
* Events servicing loop.
*/
while (!chThdShouldTerminateX())
chEvtDispatch(fhandlers, chEvtWaitOne(ALL_EVENTS));
/*
* Clean simulator exit.
*/
chEvtUnregister(chnGetEventSource(&SD1), &sd1fel);
chEvtUnregister(chnGetEventSource(&SD2), &sd2fel);
return 0;
}
THD_FUNCTION(key_sniff, arg)
{
int i;
(void)arg;
chRegSetThreadName("HydraNFC key-sniff");
while (TRUE) {
if (K1_BUTTON)
D4_ON;
else
D4_OFF;
if (K2_BUTTON)
D3_ON;
else
D3_OFF;
/* If K3_BUTTON is pressed */
if (K3_BUTTON) {
/* Wait Until K3_BUTTON is released */
while(K3_BUTTON) {
D2_ON;
chThdSleepMilliseconds(100);
}
/* Blink Fast */
for(i = 0; i < 4; i++) {
D2_ON;
chThdSleepMilliseconds(25);
D2_OFF;
chThdSleepMilliseconds(25);
}
D2_ON;
hydranfc_sniff_14443A(NULL);
D2_OFF;
}
if (K4_BUTTON)
D5_ON;
else
D5_OFF;
if (chThdShouldTerminateX())
return 0;
chThdSleepMilliseconds(100);
}
return 0;
}
/*
* PPRZ/AP thread
*
* Call PPRZ AP periodic and event functions
*/
static void thd_ap(void *arg)
{
(void) arg;
chRegSetThreadName("AP");
while (!chThdShouldTerminateX()) {
Ap(handle_periodic_tasks);
Ap(event_task);
chThdSleepMicroseconds(500);
}
chThdExit(0);
}
static THD_FUNCTION(BlueBlinkThread, arg) {
chRegSetThreadName("NormalBlink");
(void)arg;
const int16_t *rx;
msg_t status = MSG_RESET;
while(!chThdShouldTerminateX()){
status = blue_blink_mb.fetch(&rx, MS2ST(100));
if (MSG_OK == status){
blink_cycle(rx, BLINKER_NORMAL);
}
}
chThdExit(MSG_OK);
}
static THD_FUNCTION(RedBlinkThread, arg) {
chRegSetThreadName("WarningBlink");
(void)arg;
const int16_t *rx;
msg_t status = MSG_RESET;
while(!chThdShouldTerminateX()){
status = red_blink_mb.fetch(&rx, MS2ST(100));
if (MSG_OK == status){
blink_cycle(rx, BLINKER_WARNING);
}
}
chThdExit(MSG_OK);
}
static THD_FUNCTION(PulseThread, arg) {
chRegSetThreadName("Pulse");
pulse_t *pulse = arg;
systime_t t = chVTGetSystemTimeX();
while (!chThdShouldTerminateX()) {
t += pulse->high;
palSetPad(GPIOD, pulse->pad);
chThdSleepUntil(t);
palClearPad(GPIOD, pulse->pad);
t+= pulse->low;
chThdSleepUntil(t);
}
chThdExit(MSG_OK);
}
static THD_FUNCTION(can_tx, p) {
CANTxFrame txmsg;
(void)p;
chRegSetThreadName("transmitter");
txmsg.IDE = CAN_IDE_EXT;
txmsg.EID = 0x01234567;
txmsg.RTR = CAN_RTR_DATA;
txmsg.DLC = 8;
txmsg.data32[0] = 0x55AA55AA;
txmsg.data32[1] = 0x00FF00FF;
while (!chThdShouldTerminateX()) {
canTransmit(&CAND1, CAN_ANY_MAILBOX, &txmsg, MS2ST(100));
chThdSleepMilliseconds(500);
}
}
static THD_FUNCTION(can_rx, p) {
event_listener_t el;
CANRxFrame rxmsg;
(void)p;
chRegSetThreadName("receiver");
chEvtRegister(&CAND1.rxfull_event, &el, 0);
while(!chThdShouldTerminateX()) {
if (chEvtWaitAnyTimeout(ALL_EVENTS, MS2ST(100)) == 0)
continue;
while (canReceive(&CAND1, CAN_ANY_MAILBOX, &rxmsg, TIME_IMMEDIATE) == MSG_OK) {
/* Process message.*/
palTogglePad(GPIOE, GPIOE_LED3_RED);
}
}
chEvtUnregister(&CAND1.rxfull_event, &el);
}
static THD_FUNCTION(can_rx, p) {
struct can_instance *cip = p;
event_listener_t el;
CANRxFrame rxmsg;
(void)p;
chRegSetThreadName("receiver");
chEvtRegister(&cip->canp->rxfull_event, &el, 0);
while(!chThdShouldTerminateX()) {
if (chEvtWaitAnyTimeout(ALL_EVENTS, TIME_MS2I(100)) == 0)
continue;
while (canReceive(cip->canp, CAN_ANY_MAILBOX,
&rxmsg, TIME_IMMEDIATE) == MSG_OK) {
/* Process message.*/
palToggleLine(cip->led);
}
}
chEvtUnregister(&CAND1.rxfull_event, &el);
}
static THD_FUNCTION(can_reader_thread, arg)
{
t_hydra_console *con;
con = arg;
chRegSetThreadName("CAN reader");
chThdSleepMilliseconds(10);
can_rx_frame rx_msg;
mode_config_proto_t* proto = &con->mode->proto;
while (!chThdShouldTerminateX()) {
if(bsp_can_rxne(proto->dev_num)) {
chSysLock();
bsp_can_read(proto->dev_num, &rx_msg);
can_slcan_out(con, &rx_msg);
chSysUnlock();
} else {
chThdYield();
}
}
chThdExit((msg_t)1);
}
static THD_FUNCTION(BootBlinkThread, arg) {
chRegSetThreadName("BootBlink");
(void)arg;
while (!chThdShouldTerminateX()) {
red_led_on();
warning_led_off();
PAUSE();
red_led_off();
warning_led_on();
PAUSE();
warning_led_off();
red_led_off();
PAUSE();
}
warning_led_off();
red_led_off();
chThdExit(MSG_OK);
}
static THD_FUNCTION(thWriter,arg)
{
ASSERT(LS_INIT == logger_state, "thWriter, 1", "logger_state is not LS_INIT");
(void)arg;
chRegSetThreadName("Writer");
// this data never changes, until we start added in new message types
header_msg.header.preamble = KBB_PREAMBLE;
header_msg.header.msg_class = KBB_CLASS_HEADER;
header_msg.header.msg_subclass = KBB_SUBCLASS_HEADER_01;
header_msg.header.msg_size = KBB_MSG_SIZE;
current_msg.header.preamble = KBB_PREAMBLE;
current_msg.header.msg_class = KBB_CLASS_DATA;
current_msg.header.msg_subclass = KBB_SUBCLASS_DATA_CURRENT;
current_msg.header.msg_size = KBB_MSG_SIZE;
logger_state = LS_WAIT_FOR_SD;
while( !chThdShouldTerminateX() && LS_EXITING != logger_state)
{
switch ( logger_state )
{
case LS_INIT:
chDbgAssert(0, "thLogger, 2" "logger_state is should not be LS_INIT");
break;
case LS_WAIT_FOR_SD:
wait_for_sd();
break;
case LS_RUNNING:
writing_run();
break;
case LS_EXITING:
break;
}
}
return;
}
THD_FUNCTION(TimeKeeper::TimekeeperThread, arg) {
chRegSetThreadName("Timekeeper");
TimeKeeper *self = static_cast<TimeKeeper *>(arg);
self->GNSS.subscribe(&gps);
while (!chThdShouldTerminateX()) {
if ((gps.fresh) && (gps.fix > 0) && (0 == gps.msec)) {
int64_t tmp = 1000000;
tmp *= mktime(&gps.time);
osalSysLock();
time_gps_us = tmp;
if (! time_verified) {
time_verified = true;
unix_usec = time_gps_us;
}
osalSysUnlock();
/* now correct time in internal RTC (if needed) */
int32_t t1 = time_gps_us / 1000000;
int32_t t2 = rtc_get_time_unix(nullptr);
int32_t dt = t1 - t2;
if (abs(dt) > TIME_CORRECTION_THRESHOLD)
rtc_set_time(&gps.time);
}
if (gps.fresh) {
gps.fresh = false;
}
osalThreadSleepMilliseconds(20);
}
self->GNSS.unsubscribe(&gps);
chThdExit(MSG_OK);
}
//-----------------------------------------------------------------------------
// This is called by the writer thread, and doesn't return until the SD
// is in a known state, the inital header is written out, and all is ready to
// go for normal writing. It will exit if we have requested the thread should
// terminate (for shutdown)
static uint8_t
wait_for_sd(void)
{
uint8_t sd_det_count = POLLING_COUNT;
while(1)
{
if (chThdShouldTerminateX())
{
logger_state = LS_EXITING;
break;
}
if (sdc_lld_is_card_inserted(&SDCD1))
{
if (--sd_det_count == 0)
{
if ( on_insert() == KB_OK )
{
break;
}
else
{
sd_det_count = POLLING_COUNT;
continue;
}
}
}
else
{
kbs_setFName(KUROBOX_BLANK_FNAME);
sd_det_count = POLLING_COUNT;
}
chThdSleepMilliseconds(POLLING_DELAY);
}
return KB_OK;
}
//-----------------------------------------------------------------------------
// main writer thread function - is called when SD card is ready and we're just
// going to keep writing until SD card is ejected.
// @TODO: when file exceeds 1GB, start a new file
static uint8_t
writing_run(void)
{
uint8_t ret = 1;
uint32_t writer_buffers_written = 0;
while(LS_RUNNING == logger_state)
{
if (chThdShouldTerminateX())
{
logger_state = LS_EXITING;
f_sync(&kbfile);
f_close(&kbfile);
sdcDisconnect(&SDCD1);
break;
}
int8_t idx = new_write_buffer_idx_to_write();
if ( idx == -1 )
{
// no more buffers to write, go to sleep, wait to be notified
chSysLock();
writerThreadForSleep = chThdGetSelfX();
chSchGoSleepS(CH_STATE_SUSPENDED);
chSysUnlock();
// we can get woken up by either a buffer ready to be written
// or a request to terminate, either way, back to the start
// of the loop to check.
continue;
}
//----------------------------------------------------------------------
// start of write
kbg_setLED1(1);
// here we have a full buffer ready to write
write_buffer_t * buf = &write_buffers[idx];
UINT bytes_written = 0;
FRESULT stat = FR_OK;
DEBG_WRITER_TIME_WRITES_TIMER(0);
stat = f_write(&kbfile, buf->buffer, sizeof(buf->buffer), &bytes_written);
DEBG_WRITER_TIME_WRITES_TIMER(1);
kbg_setLED1(0);
// end of write
//----------------------------------------------------------------------
if (bytes_written != sizeof(buf->buffer) || stat != FR_OK)
current_msg.write_errors++;
//----------------------------------------------------------------------
// start of flush
//kbg_setLED2(1);
DEBG_WRITER_TIME_WRITES_TIMER(2);
stat = f_sync(&kbfile);
DEBG_WRITER_TIME_WRITES_TIMER(3);
if (stat != FR_OK)
current_msg.write_errors++;
// we're done, return it
return_write_buffer_idx_after_writing(idx);
//kbg_setLED2(0);
// end of flush
//----------------------------------------------------------------------
#ifdef DEBG_WRITER_TIME_WRITES
chprintf(DEBG, "%12f, %12f, %12f, %d, %6d, 0x%.8d\n",
(t1-t0) / 168000.0f, (t3-t2) / 168000.0f, (t3-t0) / 168000.0f,
idx, bytes_written, buf->buffer);
#endif // DEBG_WRITER_TIME_WRITES
kbs_setWriteCount(current_msg.msg_num);
kbs_setWriteErrors(current_msg.write_errors);
// after X buffers, start a new file. this has to be in sync with
// the logger thread, so that you both do roll over at the same time
writer_buffers_written++;
if ( writer_buffers_written > MAX_BUFFERS_PER_FILE )
{
writer_buffers_written = 0;
// we've hit the file size limit, close the old, open a new file
if ( new_file() != FR_OK )
{
// no new file? that's bad!!
logger_state = LS_WAIT_FOR_SD;
break;
}
}
sd_card_status();
if (LS_RUNNING != logger_state)
break;
}
return ret;
}
static THD_FUNCTION(thLogger, arg)
{
(void)arg;
chRegSetThreadName("Logger");
// These nested while() loops require some explanation.
// If the current state is running, we should be in the inner loop
// always - just storing, etc. But if the SD is not ready yet,
// we will be in the outer loop, flushing buffers, and signalling
//
uint32_t logger_buffers_written = 0;
current_msg.global_count = 0;
while( !chThdShouldTerminateX() && LS_EXITING != logger_state)
{
flush_write_buffers();
int8_t current_idx = -1;
systime_t sleep_until = chVTGetSystemTimeX() + MS2ST(5);
while( !chThdShouldTerminateX() && LS_RUNNING == logger_state )
{
if ( current_idx == -1 )
{
current_idx = new_write_buffer_idx_to_fill();
if ( current_idx == -1 )
{
// no buffers? sleep and try again
chThdSleepMilliseconds(1);
continue;
}
}
// here we'll have a good buffer to fill up until it's all full!
write_buffer_t * wb = &write_buffers[current_idx];
{
kbb_current_msg_t * lm = &wb->buffer[wb->current_idx];
chSysLock();
// make sure that we complete a write uninterrupted
memcpy(lm, ¤t_msg, sizeof(current_msg));
chSysUnlock();
kbt_getRTC(&lm->rtc);
uint8_t * buf = (uint8_t*) lm;
// NOTHING must get written after this, ok?
lm->header.checksum = calc_checksum_16(buf+KBB_CHECKSUM_START,
KBB_MSG_SIZE-KBB_CHECKSUM_START);
}
// chTimeNow() will roll over every ~49 days
// @TODO: make this code handle that (or not, 49 days is a lot!!)
while ( sleep_until < chVTGetSystemTimeX() )
{
// this code handles for when we skipped a writing-slot
// i'm counting a skipped msg as a write error
current_msg.write_errors++;
current_msg.msg_num++;
sleep_until += MS2ST(5);
}
chThdSleepUntil(sleep_until);
sleep_until += MS2ST(5); // Next deadline
current_msg.msg_num++;
current_msg.global_count++;
// @TODO: fix this!
if ( current_msg.msg_num%2048 == 0 ) // we write 1MB every 2048 msgs
{
kbs_setSDCFree(--cardsize_MB);
}
wb->current_idx++;
if ( wb->current_idx == BUFFER_SIZE )
{
// we're full up, return it for a new one
return_write_buffer_idx_after_filling(current_idx);
// now set it to -1, it will get a new one on next loop
current_idx = -1;
// now we check to see if we need to roll over into a new
// file, this has to be in sync with the writer thread!!
logger_buffers_written++;
if ( logger_buffers_written > MAX_BUFFERS_PER_FILE )
{
logger_buffers_written = 0;
// we've hit the file size limit, restart counters
flush_counters();
}
}
}
chThdSleepMilliseconds(1);
}
return;
}
bool BaseThread::shouldTerminate(void) {
return chThdShouldTerminateX();
}
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);
}
}
