/*
* Parse ping command
*/
static void parse_command_ping(const uint8_t *param, uint8_t param_len)
{
(void)param;
(void)param_len;
msg_t pingmsg;
pingmsg = sc_event_msg_create_type(SC_EVENT_TYPE_PING);
sc_event_msg_post(pingmsg, SC_EVENT_MSG_POST_FROM_NORMAL);
// Application is expected to reply with pong
}
/*
* Stop a event loop thread
*/
void sc_event_loop_stop(void)
{
msg_t nop;
chDbgAssert(event_thread != NULL, "Event thread not running", "#1");
chThdTerminate(event_thread);
// Send an event to wake up the thread
nop = sc_event_msg_create_type(SC_EVENT_TYPE_NOP);
sc_event_msg_post(nop, SC_EVENT_MSG_POST_FROM_NORMAL);
chThdWait(event_thread);
event_thread = NULL;
// Clear all handlers
#if HAL_USE_UART
cb_handle_byte = NULL;
#endif
#if HAL_USE_EXT
{
uint8_t i;
for (i = 0; i < EXT_MAX_CHANNELS; ++i) {
cb_extint[i] = NULL;
}
}
#endif
cb_gsm_state_changed = NULL;
cb_gsm_cmd_done = NULL;
#if HAL_USE_ADC
cb_adc_available = NULL;
#endif
cb_temp_available = NULL;
cb_9dof_available = NULL;
cb_blob_available = NULL;
cb_ahrs_available = NULL;
}
/*
* Receive byte. This can be called only from the main thread.
* FIXME: per uart buffers. Now we happily mix all uarts together.
*/
void sc_cmd_push_byte(uint8_t byte)
{
// If in a middle of blob transfer, store raw data without interpretation
chMtxLock(&blob_mtx);
if (blob_i < blob_len) {
// FIXME: HACK: Assume \n is from the previous command and not
// part of the blob
if (blob_i == 0 && byte == '\n') {
chMtxUnlock(&blob_mtx);
return;
}
if (blob_i+1 >= SC_BLOB_MAX_SIZE) {
chDbgAssert(0, "blob_i out of bounds");
chMtxUnlock(&blob_mtx);
return;
}
blob_buf[blob_i++] = byte;
if (blob_i % 1024 == 0) {
SC_DBG("1K received\r\n");
}
if (blob_i == blob_len) {
msg_t drdy;
// Create and send blob ready notification
drdy = sc_event_msg_create_type(SC_EVENT_TYPE_BLOB_AVAILABLE);
sc_event_msg_post(drdy, SC_EVENT_MSG_POST_FROM_NORMAL);
}
}
chMtxUnlock(&blob_mtx);
// Convert all different types of newlines to single \n
if (byte == '\r') {
byte = '\n';
}
// Do nothing on \n if there's no previous command in the buffer
if ((byte == '\n') && (recv_i == 0 || receive_buffer[recv_i - 1] == '\n')) {
return;
}
if (echo) {
if (byte == '\n') {
SC_LOG_PRINTF("\r\n");
} else {
SC_LOG_PRINTF("%c", byte);
}
}
// Store the received character
receive_buffer[recv_i] = byte;
// Check for full command in the buffer
if (byte == '\n') {
// Null terminate
receive_buffer[recv_i] = '\0';
// Parse the received command
parse_command();
// Mark buffer empty
recv_i = 0;
return;
}
// Discard all data in buffer if buffer is full.
if (++recv_i == SC_CMD_MAX_RECV_BUF_LEN) {
// FIXME: increase some overflow stat?
recv_i = 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);
}
}
