size_t debugPut(const uint8_t *block, size_t n)
/*
truncate any block > 255 bytes
returns # of characters actually output (including the trailing newline)
*/
{
if (n) {
chMtxLock(&debugOutLock);
size_t space = chOQGetEmptyI(&debugOutQ);
if (space) {
if (n >= space)
n = space - 1;
if (n) {
if (n > 255)
n = 255;
chOQPutTimeout( &debugOutQ, n, TIME_IMMEDIATE);
chOQWriteTimeout( &debugOutQ, block, n, TIME_IMMEDIATE);
resumeReader();
n++;
}
}else
n = 0;
chMtxUnlock();
}else
if (debugPutc('\n') >= 0)
n = 1;
return n;
}
void state( uint8_t index, uint32_t * val )
{
// Mutex protected.
chMtxLock( &mutex );
*val = ins[index];
chMtxUnlock();
}
bool_t gdispInit(void) {
bool_t res;
unsigned i;
/* Mark all the Messages as free */
for(i=0; i < GDISP_QUEUE_SIZE; i++)
gdispMsgs[i].action = GDISP_LLD_MSG_NOP;
/* Initialise our Mailbox, Mutex's and Counting Semaphore.
* A Mutex is required as well as the Mailbox and Thread because some calls have to be synchronous.
* Synchronous calls get handled by the calling thread, asynchronous by our worker thread.
*/
chMBInit(&gdispMailbox, gdispMailboxQueue, sizeof(gdispMailboxQueue)/sizeof(gdispMailboxQueue[0]));
chMtxInit(&gdispMutex);
chMtxInit(&gdispMsgsMutex);
chSemInit(&gdispMsgsSem, GDISP_QUEUE_SIZE);
lldThread = chThdCreateStatic(waGDISPThread, sizeof(waGDISPThread), NORMALPRIO, GDISPThreadHandler, NULL);
/* Initialise driver - synchronous */
chMtxLock(&gdispMutex);
res = lld_gdisp_init();
chMtxUnlock();
return res;
}
static msg_t ledsThread( void *arg )
{
(void)arg;
chRegSetThreadName( "ld" );
while ( 1 )
{
static uint32_t arg;
chMtxLock( &mutex );
arg = value;
if ( arg & 1 )
palTogglePad( LED_0_PORT, LED_0_PIN );
else
palClearPad( LED_0_PORT, LED_0_PIN );
if ( arg & 2 )
palTogglePad( LED_1_PORT, LED_1_PIN );
else
palClearPad( LED_1_PORT, LED_1_PIN );
chMtxUnlock();
chThdSleepMilliseconds( DURATION_MS );
processDfu( DURATION_MS );
}
return 0;
}
static void mtx5_execute(void) {
bool_t b;
tprio_t prio;
prio = chThdGetPriority();
b = chMtxTryLock(&m1);
test_assert(1, b, "already locked");
b = chMtxTryLock(&m1);
test_assert(2, !b, "not locked");
chSysLock();
chMtxUnlockS();
chSysUnlock();
test_assert(3, isempty(&m1.m_queue), "queue not empty");
test_assert(4, m1.m_owner == NULL, "still owned");
test_assert(5, chThdGetPriority() == prio, "wrong priority level");
chMtxLock(&m1);
chMtxUnlockAll();
test_assert(6, isempty(&m1.m_queue), "queue not empty");
test_assert(7, m1.m_owner == NULL, "still owned");
}
static THD_FUNCTION(thread4b, p) {
(void)p;
chThdSleepMilliseconds(150);
chMtxLock(&m1);
chMtxUnlock(&m1);
}
static THD_FUNCTION(thread10, p) {
chMtxLock(&m1);
chCondWait(&c1);
test_emit_token(*(char *)p);
chMtxUnlock(&m1);
}
/*
* Read and update the current GPS data.
*/
void EM411Update(void)
{
int count=0;
/*
* Get message
*/
while((curMsg[count] = chIOGet(&SD2)) != '\n' && count < EMEA_BUFFER_SIZE) {
//curMsg[count] = chIOGet(&SD2);
count++;
}
/*
* Copy message to output buffer
* Also fill data structure.
*/
chMtxLock(&em411mtx);
strncpy(lastMsg, curMsg, EMEA_BUFFER_SIZE-1);
EM411Decode(lastMsg);
chMtxUnlock();
/*
* Clear buffer.
*/
memset(curMsg, 0, EMEA_BUFFER_SIZE);
}
/*
* Parse blob command (bXXXXX)
*/
static void parse_command_blob(const uint8_t *param, uint8_t param_len)
{
uint16_t bytes;
(void)param_len;
if (sscanf((char*)param, "%hu", &bytes) < 1) {
return;
}
if (bytes == 0) {
// FIXME: Should just print a warning (can't assert on user input)?
chDbgAssert(0, "Failed to parse blob size");
return;
}
if (bytes >= SC_BLOB_MAX_SIZE) {
// FIXME: Should just print a warning (can't assert on user input)?
chDbgAssert(0, "Requested blob size too large");
return;
}
chMtxLock(&blob_mtx);
blob_i = 0;
blob_len = bytes;
chMtxUnlock(&blob_mtx);
}
int rfhelp_rf_status(void) {
chMtxLock(&rf_mutex);
int s = rf_status();
chMtxUnlock();
return s;
}
uint16_t lastTemp( void )
{
chMtxLock( &mutex );
uint16_t t = temperature;
chMtxUnlock();
return t;
}
/**
* Read data from the RX fifo
*
* @param data
* Pointer to the array in which to store the data.
*
* @param len
* Pointer to variable storing the data length.
*
* @param pipe
* Pointer to the pipe on which the data was received. Can be 0.
*
* @return
* 1: Read OK, more data to read.
* 0: Read OK
* -1: No RX data
* -2: Wrong length read. Something is likely wrong.
*/
int rfhelp_read_rx_data(char *data, int *len, int *pipe) {
int retval = -1;
chMtxLock(&rf_mutex);
int s = rf_status();
int pipe_n = NRF_STATUS_GET_RX_P_NO(s);
if (pipe_n != 7) {
*len = rf_get_payload_width();
if (pipe) {
*pipe = pipe_n;
}
if (*len <= 32 && *len >= 0) {
rf_read_rx_payload(data, *len);
rf_clear_rx_irq();
s = rf_status();
if (NRF_STATUS_GET_RX_P_NO(s) == 7) {
retval = 0;
} else {
retval = 1;
}
} else {
*len = 0;
retval = -2;
}
}
chMtxUnlock();
return retval;
}
/**
* Blocking read of the random number generator.
*/
uint32_t rand(void)
{
chMtxLock(&randMtx);
uint32_t r = 0;
int randGood = FALSE;
while (!randGood) {
uint32_t rLast = r;
RNG->CR |= RNG_CR_RNGEN;
while ((RNG->SR & RNG_SR_DRDY) == 0) {
continue;
}
uint32_t status = RNG->SR;
r = RNG->DR;
if (status & RNG_SR_SEIS) {
/* Seed error */
} else if (status & RNG_SR_CEIS) {
/* Clock error */
return FALSE;
} else if ((rLast != 0) && (r != rLast)) {
randGood = TRUE;
}
}
chMtxUnlock();
return r;
}
size_t debugPrint(const char *fmt, ...)
/*
printf style debugging output
outputs a trailing newline
*/
{
va_list ap;
va_start(ap, fmt);
NullStream lenStream = {&nullVmt, 0};
chvprintf((BaseSequentialStream *) &lenStream, fmt, ap);
size_t len = lenStream.len;
if (len) {
chMtxLock(&debugOutLock);
size_t qspace = chOQGetEmptyI(&debugOutQ);
if (qspace) {
if (len >= qspace)
len = qspace - 1; //truncate string if it won't fit in queue
if (len) {
if (len > 255)
len = 255;
qStream dbgStream = {&qVmt, len};
chOQPutTimeout(&debugOutQ, len, TIME_IMMEDIATE);
chvprintf((BaseSequentialStream *) &dbgStream, fmt, ap);
resumeReader();
len++;
}
}else
len=0;
chMtxUnlock();
}else
if (debugPutc('\n') >= 0)
len=1;
va_end(ap);
return len;
}
void sc_extint_clear(ioportid_t port, uint8_t pin)
{
uint8_t need_stop = 1;
uint8_t i;
EXTChannelConfig cfg;
(void)port;
chMtxLock(&cfg_mtx);
chDbgAssert(pin < EXT_MAX_CHANNELS , "EXT pin number outside range");
chDbgAssert(extcfg.channels[pin].cb != NULL,
"EXT pin cb not registered");
// FIXME: should check that port matches as well?
cfg.cb = NULL;
cfg.mode = EXT_CH_MODE_DISABLED;
extSetChannelMode(&EXTD1, pin, &cfg);
for (i = 0; i < EXT_MAX_CHANNELS; ++i) {
if (extcfg.channels[pin].cb != NULL) {
need_stop = 0;
break;
}
}
if (need_stop) {
extStop(&EXTD1);
}
chMtxUnlock(&cfg_mtx);
}
float motor_get_duty_cycle(void)
{
chMtxLock(&_mutex);
float ret = _state.dc_actual;
chMtxUnlock();
return ret;
}
/**
* Uart transmit buffer implementation
*/
void uart_put_buffer(struct uart_periph *p, long fd, const uint8_t *data, uint16_t len)
{
struct SerialInit *init_struct = (struct SerialInit*)(p->init_struct);
if (fd == 0) {
// if fd is zero, assume the driver is not already locked
// and available space should be checked
chMtxLock(init_struct->tx_mtx);
int16_t space = p->tx_extract_idx - p->tx_insert_idx;
if (space <= 0) {
space += UART_TX_BUFFER_SIZE;
}
if ((uint16_t)(space - 1) < len) {
chMtxUnlock(init_struct->tx_mtx);
return; // no room
}
}
// insert data into buffer
int i;
for (i = 0; i < len; i++) {
p->tx_buf[p->tx_insert_idx] = data[i];
p->tx_insert_idx = (p->tx_insert_idx + 1) % UART_TX_BUFFER_SIZE;
}
// unlock if needed
if (fd == 0) {
chMtxUnlock(init_struct->tx_mtx);
// send signal to start transmission
chSemSignal (init_struct->tx_sem);
}
}
enum motor_control_mode motor_get_control_mode(void)
{
chMtxLock(&_mutex);
enum motor_control_mode ret = _state.mode;
chMtxUnlock();
return ret;
}
static void mtx5_execute(void) {
#if !CH_CFG_USE_MUTEXES_RECURSIVE
bool b;
tprio_t prio = chThdGetPriorityX();
b = chMtxTryLock(&m1);
test_assert(1, b, "already locked");
b = chMtxTryLock(&m1);
test_assert(2, !b, "not locked");
chSysLock();
chMtxUnlockS(&m1);
chSysUnlock();
test_assert(3, queue_isempty(&m1.m_queue), "queue not empty");
test_assert(4, m1.m_owner == NULL, "still owned");
test_assert(5, chThdGetPriorityX() == prio, "wrong priority level");
#endif /* !CH_CFG_USE_MUTEXES_RECURSIVE */
chMtxLock(&m1);
chMtxUnlockAll();
test_assert(6, queue_isempty(&m1.m_queue), "queue not empty");
test_assert(7, m1.m_owner == NULL, "still owned");
}
bool motor_is_running(void)
{
chMtxLock(&_mutex);
bool ret = motor_rtctl_get_state() == MOTOR_RTCTL_STATE_RUNNING;
chMtxUnlock();
return ret;
}
static msg_t OscAutosendThread(void *arg)
{
UNUSED(arg);
uint8_t i;
const OscNode* node;
OscChannelData* chd;
while (!chThdShouldTerminate()) {
if (osc.autosendDestination == NONE) {
sleep(250);
}
else {
chd = oscGetChannelByType(osc.autosendDestination);
i = 0;
node = oscRoot.children[i++];
chMtxLock(&chd->lock);
while (node != 0) {
if (node->autosender != 0)
node->autosender(osc.autosendDestination);
node = oscRoot.children[i++];
}
oscSendPendingMessages(osc.autosendDestination);
chMtxUnlock();
sleep(osc.autosendPeriod);
}
}
return 0;
}
bool motor_is_idle(void)
{
chMtxLock(&_mutex);
bool ret = motor_rtctl_get_state() == MOTOR_RTCTL_STATE_IDLE;
chMtxUnlock();
return ret;
}
void toggleLeds( uint32_t arg )
{
chMtxLock( &mutex );
value = ( value & ( ~arg ) ) |
( (value ^ arg ) & (arg & 0x07) );
chMtxUnlock();
}
bool motor_is_blocked(void)
{
chMtxLock(&_mutex);
bool ret = _state.num_unexpected_stops >= _params.num_unexpected_stops_to_latch;
chMtxUnlock();
return ret;
}
static msg_t thread1(void *p) {
chMtxLock(&m1);
test_emit_token(*(char *)p);
chMtxUnlock();
return 0;
}
int motor_get_limit_mask(void)
{
chMtxLock(&_mutex);
int ret = _state.limit_mask;
chMtxUnlock();
return ret;
}
void setOutput( uint8_t index, uint32_t * val )
{
// Mutex protected.
chMtxLock( &mutex );
pendOuts[index] = *val;
chMtxUnlock();
}
int motor_test_motor(void)
{
chMtxLock(&_mutex);
const int res = motor_rtctl_test_motor();
chMtxUnlock();
return res;
}
void clarityCC3000ApiLock(void)
{
if (cc3000Mtx != NULL)
{
chMtxLock(cc3000Mtx);
}
}
void tree_reset(void)
{
chMtxLock(&tree_mtx);
memory_clean();
chMtxUnlock();
tree_clean();
}
