static void ephemeris_new(ephemeris_t *e)
{
gps_time_t t = get_current_time();
if (!ephemeris_good(&es[e->sid.sat], t)) {
/* Our currently used ephemeris is bad, so we assume this is better. */
log_info("New untrusted ephemeris for PRN %02d", e->sid.sat+1);
chMtxLock(&es_mutex);
es[e->sid.sat] = es_candidate[e->sid.sat] = *e;
chMtxUnlock();
} else if (ephemeris_equal(&es_candidate[e->sid.sat], e)) {
/* The received ephemeris matches our candidate, so we trust it. */
log_info("New trusted ephemeris for PRN %02d", e->sid.sat+1);
chMtxLock(&es_mutex);
es[e->sid.sat] = *e;
chMtxUnlock();
} else {
/* This is our first reception of this new ephemeris, so treat it with
* suspicion and call it the new candidate. */
log_info("New ephemeris candidate for PRN %02d", e->sid.sat+1);
chMtxLock(&es_mutex);
es_candidate[e->sid.sat] = *e;
chMtxUnlock();
}
}
bool_t gadcLowSpeedStart(uint32_t physdev, adcsample_t *buffer, GADCCallbackFunction fn, void *param) {
struct lsdev *p;
DoInit();
/* Start the Low Speed Timer */
chMtxLock(&gadcmutex);
if (!gtimerIsActive(&LowSpeedGTimer))
gtimerStart(&LowSpeedGTimer, LowSpeedGTimerCallback, NULL, TRUE, TIME_INFINITE);
/* Find a slot */
for(p = ls; p < &ls[GADC_MAX_LOWSPEED_DEVICES]; p++) {
if (!(p->flags & GADC_FLG_ISACTIVE)) {
/* We know we have a slot - this should never wait anyway */
chSemWaitTimeout(&gadcsem, TIME_IMMEDIATE);
p->lld.physdev = physdev;
p->lld.buffer = buffer;
p->fn = fn;
p->param = param;
p->flags = GADC_FLG_ISACTIVE;
chMtxUnlock();
StartADC(FALSE);
return TRUE;
}
}
chMtxUnlock();
return FALSE;
}
static void rawd_new(struct netconn *nc)
{
int i;
int free = -1;
chMtxLock(&rawd_mutex);
/* check for existing connections */
for (i = 0; i < NC_COUNT; i++) {
if (ncs[i] == NULL) {
free = i;
} else if (ncs[i]->pcb.tcp->state != ESTABLISHED) {
/* kill stale connections */
netconn_close(ncs[i]);
netconn_delete(ncs[i]);
ncs[i] = NULL;
free = i;
}
}
if (free < 0) {
netconn_write(nc, RAWD_FULL, sizeof(RAWD_FULL) - 1, NETCONN_COPY);
netconn_close(nc);
netconn_delete(nc);
chMtxUnlock();
return;
}
ncs[free] = nc;
chMtxUnlock();
netconn_write(nc, RAWD_READY, sizeof(RAWD_READY) - 1, NETCONN_COPY);
}
/**
* 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);
}
}
uint8_t cmp_lists(uint16_t leaf, void *buf, uint8_t length)
{
chMtxLock(&tree_mtx);
uint16_t b = memory_get_ids_head(leaf);
if (b == NO_BUCKET) {
chMtxUnlock();
return 0;
}
uint64_t hash = BUCKET_READ_FIELD(b, type.id, 64);
uint8_t nb_to_send = 0;
unsigned int i = 0;
while(b != NO_NEXT && i < length)
if(hash == ((uint64_t *) buf)[i+1]) {
b = BUCKET_READ_FIELD(b, next_id, 16);
i++;
} else if(BUCKET_READ_FIELD(b, type.id, 64) < ((uint64_t *) buf)[i+1]){
i++;
} else {
((uint16_t *) buf)[nb_to_send++] = b;
b = BUCKET_READ_FIELD(b, next_id, 16);
}
while(b != NO_NEXT) {
((uint16_t *) buf)[nb_to_send++] = b;
b = BUCKET_READ_FIELD(b, next_id, 16);
}
chMtxUnlock();
return nb_to_send;
}
uint16_t tree_insert(struct Bucket *b)
{
chMtxLock(&tree_mtx);
uint16_t id = place_message(b);
if(id == MEM_FULL) {
chMtxUnlock();
return MEM_FULL;
}
update_branch(id);
chMtxUnlock();
return id;
}
static THD_FUNCTION(thread11, p) {
chMtxLock(&m2);
chMtxLock(&m1);
#if CH_CFG_USE_CONDVARS_TIMEOUT || defined(__DOXYGEN__)
chCondWaitTimeout(&c1, TIME_INFINITE);
#else
chCondWait(&c1);
#endif
test_emit_token(*(char *)p);
chMtxUnlock(&m1);
chMtxUnlock(&m2);
}
void motor_beep(int frequency, int duration_msec)
{
chMtxLock(&_mutex);
if (motor_rtctl_get_state() == MOTOR_RTCTL_STATE_IDLE) {
_state.beep_frequency = frequency;
_state.beep_duration_msec = duration_msec;
chMtxUnlock();
chEvtBroadcastFlags(&_setpoint_update_event, ALL_EVENTS); // Wake the control thread
} else {
chMtxUnlock();
}
}
/* Low priority thread */
static THD_FUNCTION(thread3L, p) {
(void)p;
chThdSleepMilliseconds(10);
chMtxLock(&m2);
test_cpu_pulse(20);
chMtxLock(&m1);
test_cpu_pulse(10);
chMtxUnlock(&m1);
test_cpu_pulse(10);
chMtxUnlock(&m2);
test_emit_token('D');
}
/* Low priority thread */
static msg_t thread3L(void *p) {
(void)p;
chThdSleepMilliseconds(10);
chMtxLock(&m2);
test_cpu_pulse(20);
chMtxLock(&m1);
test_cpu_pulse(10);
chMtxUnlock();
test_cpu_pulse(10);
chMtxUnlock();
test_emit_token('D');
return 0;
}
void geventSendEvent(GSourceListener *psl) {
chMtxLock(&geventMutex);
if (psl->pListener->callback) { // This test needs to be taken inside the mutex
chMtxUnlock();
// We already know we have the event lock
psl->pListener->callback(psl->pListener->param, &psl->pListener->event);
} else {
// Wake up the listener
if (chSemGetCounterI(&psl->pListener->waitqueue) < 0)
chSemSignal(&psl->pListener->waitqueue);
chMtxUnlock();
}
}
static msg_t thread11(void *p) {
chMtxLock(&m2);
chMtxLock(&m1);
#if CH_USE_CONDVARS_TIMEOUT || defined(__DOXYGEN__)
chCondWaitTimeout(&c1, TIME_INFINITE);
#else
chCondWait(&c1);
#endif
test_emit_token(*(char *)p);
chMtxUnlock();
chMtxUnlock();
return 0;
}
void toggleLeds( uint32_t arg )
{
chMtxLock( &mutex );
value = ( value & ( ~arg ) ) |
( (value ^ arg ) & (arg & 0x07) );
chMtxUnlock();
}
int motor_get_limit_mask(void)
{
chMtxLock(&_mutex);
int ret = _state.limit_mask;
chMtxUnlock();
return ret;
}
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 = gdisp_lld_init();
chMtxUnlock();
return res;
}
static msg_t clarityMgmtResponseMonitoringThd(void *arg)
{
uint32_t attempts = 0;
(void)arg;
#if CH_USE_REGISTRY == TRUE
chRegSetThreadName(__FUNCTION__);
#endif
while (chThdShouldTerminate() == FALSE)
{
if (chMtxTryLock(cc3000Mtx) == TRUE)
{
chMtxUnlock();
attempts = 0;
}
else
{
attempts++;
if (attempts == CC3000_MUTEX_POLL_COUNT)
{
unresponsiveCb();
}
}
chThdSleep(MS2ST(CC3000_MUTEX_POLL_TIME_MS));
}
return CLARITY_SUCCESS;
}
void state( uint8_t index, uint32_t * val )
{
// Mutex protected.
chMtxLock( &mutex );
*val = ins[index];
chMtxUnlock();
}
static THD_FUNCTION(thread4b, p) {
(void)p;
chThdSleepMilliseconds(150);
chMtxLock(&m1);
chMtxUnlock(&m1);
}
bool motor_is_idle(void)
{
chMtxLock(&_mutex);
bool ret = motor_rtctl_get_state() == MOTOR_RTCTL_STATE_IDLE;
chMtxUnlock();
return ret;
}
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);
}
int motor_test_motor(void)
{
chMtxLock(&_mutex);
const int res = motor_rtctl_test_motor();
chMtxUnlock();
return res;
}
bool motor_is_running(void)
{
chMtxLock(&_mutex);
bool ret = motor_rtctl_get_state() == MOTOR_RTCTL_STATE_RUNNING;
chMtxUnlock();
return ret;
}
static msg_t thread1(void *p) {
chMtxLock(&m1);
test_emit_token(*(char *)p);
chMtxUnlock();
return 0;
}
enum motor_control_mode motor_get_control_mode(void)
{
chMtxLock(&_mutex);
enum motor_control_mode ret = _state.mode;
chMtxUnlock();
return ret;
}
void setOutput( uint8_t index, uint32_t * val )
{
// Mutex protected.
chMtxLock( &mutex );
pendOuts[index] = *val;
chMtxUnlock();
}
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;
}
void clarityCC3000ApiUnlock(void)
{
if (cc3000Mtx != NULL)
{
chMtxUnlock();
}
}
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 THD_FUNCTION(thread10, p) {
chMtxLock(&m1);
chCondWait(&c1);
test_emit_token(*(char *)p);
chMtxUnlock(&m1);
}
bool motor_is_blocked(void)
{
chMtxLock(&_mutex);
bool ret = _state.num_unexpected_stops >= _params.num_unexpected_stops_to_latch;
chMtxUnlock();
return ret;
}
