void imu_startblink(void) {
imu_leds_on(true);
Pin pins[] = {PINS_IMU_LED};
for(uint8_t i = 0; i < 6; i++) {
pins[i].type = PIO_OUTPUT_1;
}
PIO_Configure(pins, PIO_LISTSIZE(pins));
for(uint8_t i = 0; i < 41; i++) {
TC0->TC_CHANNEL[0].TC_RA = blink_lookup[i];
TC0->TC_CHANNEL[1].TC_RA = blink_lookup[i];
TC0->TC_CHANNEL[2].TC_RB = blink_lookup[i];
PWMC_SetDutyCycle(PWM, 3, blink_lookup[40-i]);
SLEEP_MS(6);
}
for(uint8_t i = 0; i < 41; i++) {
TC0->TC_CHANNEL[0].TC_RA = blink_lookup[40-i];
TC0->TC_CHANNEL[1].TC_RA = blink_lookup[40-i];
TC0->TC_CHANNEL[2].TC_RB = blink_lookup[40-i];
PWMC_SetDutyCycle(PWM, 3, blink_lookup[i]);
SLEEP_MS(6);
}
for(uint8_t i = 0; i < 41; i++) {
TC0->TC_CHANNEL[0].TC_RA = blink_lookup[i];
TC0->TC_CHANNEL[1].TC_RA = blink_lookup[i];
TC0->TC_CHANNEL[2].TC_RB = blink_lookup[i];
PWMC_SetDutyCycle(PWM, 3, blink_lookup[40-i]);
SLEEP_MS(6);
}
imu_leds_on(false);
}
bool read_calibration_from_bno055_and_save_to_flash(void) {
if(sensor_data.calibration_status != 0xFF) {
return false;
}
bmo_write_register(REG_OPR_MODE, 0b00000000); // Configuration Mode
SLEEP_MS(19);
IMUCalibration imu_calibration = {{0}};
bmo_read_registers(REG_ACC_OFFSET_X_LSB, (uint8_t *)&imu_calibration, IMU_CALIBRATION_LENGTH);
imu_calibration.password = IMU_CALIBRATION_PASSWORD;
bmo_write_register(REG_OPR_MODE, 0b00001100); // Enable NDOF, see Table 3-5
SLEEP_MS(7);
logimui("Read calibration from BNO055 and save to flash:\n\r");
logimui(" Mag Offset: %d %d %d\n\r", imu_calibration.mag_offset[0], imu_calibration.mag_offset[1], imu_calibration.mag_offset[2]);
logimui(" Acc Offset: %d %d %d\n\r", imu_calibration.acc_offset[0], imu_calibration.acc_offset[1], imu_calibration.acc_offset[2]);
logimui(" Gyr Offset: %d %d %d\n\r", imu_calibration.gyr_offset[0], imu_calibration.gyr_offset[1], imu_calibration.gyr_offset[2]);
logimui(" Acc Radius: %d\n\r", imu_calibration.acc_radius);
logimui(" Mag Radius: %d\n\r", imu_calibration.mag_radius);
DISABLE_RESET_BUTTON();
__disable_irq();
// Unlock flash region
if(FLASHD_Unlock(IMU_CALIBRATION_ADDRESS,
END_OF_BRICKLET_MEMORY,
NULL,
NULL) != 0) {
return false;
}
if(FLASHD_Write(IMU_CALIBRATION_ADDRESS,
&imu_calibration,
sizeof(IMUCalibration)) != 0) {
return false;
}
if(FLASHD_Lock(IMU_CALIBRATION_ADDRESS,
END_OF_BRICKLET_MEMORY,
NULL,
NULL) != 0) {
return false;
}
__enable_irq();
ENABLE_RESET_BUTTON();
return true;
}
// Function: wdShutdown
// Access: Public
// Description: This function allows the abstracted component functionality contained in this file to be stoped from an external source.
// If the component is in the running state, this function will terminate all threads running in this file
// This function will also close the Jms connection to the Node Manager and check out the component from the Node Manager
int wdShutdown(void)
{
double timeOutSec;
if(wd && wd->state != JAUS_SHUTDOWN_STATE) // Execute the shutdown routines only if the component is running
{
wdRun = FALSE;
timeOutSec = getTimeSeconds() + WD_THREAD_TIMEOUT_SEC;
while(wdThreadRunning)
{
SLEEP_MS(1000);
if(getTimeSeconds() >= timeOutSec)
{
pthread_cancel(wdThreadId);
wdThreadRunning = FALSE;
//cError("wd: wdThread Shutdown Improperly\n");
break;
}
}
nodeManagerClose(wdNmi); // Close Node Manager Connection
jausSubsystemDestroy(wd->node->subsystem);
jausNodeDestroy(wd->node);
jausServicesDestroy(wd->services);
wd->state = JAUS_SHUTDOWN_STATE;
propertiesDestroy(wdProperties);
}
return 0;
}
void constructor(void) {
_Static_assert(sizeof(BrickContext) <= BRICKLET_CONTEXT_MAX_SIZE, "BrickContext too big");
PIN_PWR.type = PIO_OUTPUT_0;
PIN_PWR.attribute = PIO_DEFAULT;
BA->PIO_Configure(&PIN_PWR, 1);
PIN_SCL.type = PIO_INPUT;
PIN_SCL.attribute = PIO_PULLUP;
BA->PIO_Configure(&PIN_SCL, 1);
PIN_SDA.type = PIO_INPUT;
PIN_SDA.attribute = PIO_PULLUP;
BA->PIO_Configure(&PIN_SCL, 1);
PIN_PWM.type = PIO_INPUT;
PIN_PWM.attribute = PIO_PULLUP;
BA->PIO_Configure(&PIN_PWM, 1);
SLEEP_MS(100);
PIN_PWR.type = PIO_OUTPUT_1;
BA->PIO_Configure(&PIN_PWR, 1);
simple_constructor();
BC->laser_enabled = false;
BC->new_mode = 0;
BC->moving_average_upto[SIMPLE_UNIT_DISTANCE] = DEFAULT_MOVING_AVERAGE_DISTANCE;
BC->moving_average_upto[SIMPLE_UNIT_VELOCITY] = DEFAULT_MOVING_AVERAGE_VELOCITY;
reinitialize_moving_average_distance();
reinitialize_moving_average_velocity();
BC->measurement_state = MS_START_ACQUISITION;
BC->next_measurement_state_counter = 100;
}
static THREAD_FN
register_events_subthread(void *arg)
{
struct timeval tv = {0,0};
SLEEP_MS(100);
event_active(&time_events[0], EV_TIMEOUT, 1);
SLEEP_MS(100);
event_active(&time_events[1], EV_TIMEOUT, 1);
SLEEP_MS(100);
tv.tv_usec = 100*1000;
event_add(&time_events[2], &tv);
tv.tv_usec = 150*1000;
event_add(&time_events[3], &tv);
SLEEP_MS(200);
event_active(&time_events[4], EV_TIMEOUT, 1);
THREAD_RETURN();
}
void calibrate(const ComType com, const Calibrate *data) {
__disable_irq();
PIN_ENABLE.pio->PIO_SODR = PIN_ENABLE.mask;
uint32_t tick_sum = 0;
uint16_t tick_last = 0;
for(uint16_t freq = 0; freq <= FREQUENCY_VALUE_SUM_MAX; freq+=8) {
set_frequency(freq);
SLEEP_MS(1);
while(!(PIN_FEEDBACK.pio->PIO_PDSR & PIN_FEEDBACK.mask)) {
__NOP();
}
while(PIN_FEEDBACK.pio->PIO_PDSR & PIN_FEEDBACK.mask) {
__NOP();
}
tick_last = SysTick->VAL;
SLEEP_US(1);
while(!(PIN_FEEDBACK.pio->PIO_PDSR & PIN_FEEDBACK.mask)) {
uint16_t tick_next = SysTick->VAL;
if(tick_last < tick_next) {
tick_sum += FEEDBACK_TICK_MAX - tick_next + tick_last;
} else {
tick_sum += tick_last - tick_next;
}
tick_last = tick_next;
}
SLEEP_US(1);
do {
uint16_t tick_next = SysTick->VAL;
if(tick_last < tick_next) {
tick_sum += FEEDBACK_TICK_MAX - tick_next + tick_last;
} else {
tick_sum += tick_last - tick_next;
}
tick_last = tick_next;
} while(PIN_FEEDBACK.pio->PIO_PDSR & PIN_FEEDBACK.mask);
uint16_t real_freq = (BOARD_MCK+tick_sum/2)/tick_sum;
BC->frequency_match[freq/8] = real_freq;
tick_sum = 0;
}
PIN_ENABLE.pio->PIO_CODR = PIN_ENABLE.mask;
save_calibration();
__enable_irq();
CalibrateReturn cr;
cr.header = data->header;
cr.header.length = sizeof(CalibrateReturn);
cr.calibration = true;
BA->send_blocking_with_timeout(&cr, sizeof(CalibrateReturn), com);
}
static THREAD_FN
load_deferred_queue(void *arg)
{
struct deferred_test_data *data = arg;
size_t i;
for (i = 0; i < CB_COUNT; ++i) {
event_deferred_cb_init(&data->cbs[i], deferred_callback, NULL);
event_deferred_cb_schedule(data->queue, &data->cbs[i]);
SLEEP_MS(1);
}
THREAD_RETURN();
}
void LocalClient::KeystateTaskThread::task() {
#define KEYSTATE_GRANULARITY_MS 46.66 // aiming for as low a rate as possible
static TIMER keystate_timer;
keystate_timer.begin();
while (LocalClient::KeystateTaskThread::is_running() && LocalClient::is_connected()) {
if (keystate_timer.get_ms() > KEYSTATE_GRANULARITY_MS) {
update_keystate(keys);
post_keystate();
keystate_timer.begin();
long wait_ms = KEYSTATE_GRANULARITY_MS - keystate_timer.get_ms();
if (wait_ms > 1) { SLEEP_MS(wait_ms); }
}
}
}
bool i2c_eeprom_master_write(Twi *twi,
const uint16_t internal_address,
const char *data,
const uint16_t length) {
uint32_t timeout;
mutex_take(mutex_twi_bricklet, MUTEX_BLOCKING);
// Start write
TWI_StartWrite(twi,
bricklet_eeprom_address,
internal_address,
I2C_EEPROM_INTERNAL_ADDRESS_BYTES,
data[0]);
for(uint16_t i = 1; i < length; i++) {
timeout = 0;
// Wait until byte is sent, otherwise return false
while(!TWI_ByteSent(twi) && (++timeout < I2C_EEPROM_TIMEOUT)) {}
if(timeout == I2C_EEPROM_TIMEOUT) {
logieew("write timeout (nothing sent)\n\r");
mutex_give(mutex_twi_bricklet);
return false;
}
TWI_WriteByte(twi, data[i]);
}
// Send STOP
TWI_SendSTOPCondition(twi);
timeout = 0;
// Wait for transfer to be complete
while(!TWI_TransferComplete(twi) && (++timeout < I2C_EEPROM_TIMEOUT)) {}
if (timeout == I2C_EEPROM_TIMEOUT) {
logieew("write timeout (transfer incomplete)\n\r");
mutex_give(mutex_twi_bricklet);
return false;
}
// Wait at least 5ms between writes (see m24128-bw.pdf)
SLEEP_MS(5);
mutex_give(mutex_twi_bricklet);
return true;
}
int main() {
brick_init();
led_init();
Pin ch_st = PIN_CH_ST;
PIO_Configure(&ch_st, 1);
while(1) {
if(PIO_Get(&ch_st)) {
led_toggle(LED_STD_BLUE);
}
else {
led_on(LED_STD_BLUE);
}
SLEEP_MS(100);
wdt_restart();
}
}
void LocalClient::PingmanagerTaskThread::task() {
#define PING_GRANULARITY_MS 2000
#define TIMEOUT_MS 5000
timer.begin();
while(LocalClient::PingmanagerTaskThread::is_running() && LocalClient::is_connected()) {
if (time_since_last_ping_ms() > TIMEOUT_MS) {
// gtfo?
}
ping();
long wait_ms = PING_GRANULARITY_MS - timer.get_ms();
SLEEP_MS(wait_ms); // this needn't be precise
timer.begin();
}
}
int usart_tgetc(int timeout)
{
int c;
int i;
if (!timeout) /* timeout specified? */
return usart_getc(); /* no, just return it */
for (i=0; i < timeout; i++)
{
if ((c = usart_getc()) != -1) /* any character found? */
return c; /* yes, return it */
SLEEP_MS(10);
}
return -1;
}
void imu_init(void) {
logimui("IMU init start\n\r");
Pin pins_bno[] = {PINS_BNO};
PIO_Configure(pins_bno, PIO_LISTSIZE(pins_bno));
imu_startblink();
bmo_write_register(REG_OPR_MODE, 0b00000000); // Configuration Mode
SLEEP_MS(19);
bmo_write_register(REG_SYS_TRIGGER, 1 << 7); // Use external clock
read_calibration_from_flash_and_save_to_bno055();
#ifndef PROFILING
imu_leds_on(true);
#endif
logimui("IMU init done\n\r");
}
bool read_calibration_from_flash_and_save_to_bno055(void) {
bool ret = false;
if(imu_calibration_in_flash->password == IMU_CALIBRATION_PASSWORD) {
logimui("Read calibration from flash and save to BNO055:\n\r");
logimui(" Mag Offset: %d %d %d\n\r", imu_calibration_in_flash->mag_offset[0], imu_calibration_in_flash->mag_offset[1], imu_calibration_in_flash->mag_offset[2]);
logimui(" Acc Offset: %d %d %d\n\r", imu_calibration_in_flash->acc_offset[0], imu_calibration_in_flash->acc_offset[1], imu_calibration_in_flash->acc_offset[2]);
logimui(" Gyr Offset: %d %d %d\n\r", imu_calibration_in_flash->gyr_offset[0], imu_calibration_in_flash->gyr_offset[1], imu_calibration_in_flash->gyr_offset[2]);
logimui(" Acc Radius: %d\n\r", imu_calibration_in_flash->acc_radius);
logimui(" Mag Radius: %d\n\r", imu_calibration_in_flash->mag_radius);
ret = true;
bmo_write_registers(REG_ACC_OFFSET_X_LSB, (uint8_t *)IMU_CALIBRATION_ADDRESS, IMU_CALIBRATION_LENGTH);
} else {
logimui("No calibration found\n\r");
}
bmo_write_register(REG_OPR_MODE, 0b00001100); // Enable NDOF, see Table 3-5
SLEEP_MS(7);
return ret;
}
static void xmit_ax25_frames (int c, int p, packet_t pp)
{
unsigned char fbuf[AX25_MAX_PACKET_LEN+2];
int flen;
char stemp[1024]; /* max size needed? */
int info_len;
unsigned char *pinfo;
int pre_flags, post_flags;
int num_bits; /* Total number of bits in transmission */
/* including all flags and bit stuffing. */
int duration; /* Transmission time in milliseconds. */
int already;
int wait_more;
int maxframe; /* Maximum number of frames for one transmission. */
int numframe; /* Number of frames sent during this transmission. */
/*
* These are for timing of a transmission.
* All are in usual unix time (seconds since 1/1/1970) but higher resolution
*/
double time_ptt; /* Time when PTT is turned on. */
double time_now; /* Current time. */
int nb;
maxframe = (p == TQ_PRIO_0_HI) ? 1 : 7;
/*
* Print trasmitted packet. Prefix by channel and priority.
* Do this before we get into the time critical part.
*/
ax25_format_addrs (pp, stemp);
info_len = ax25_get_info (pp, &pinfo);
text_color_set(DW_COLOR_XMIT);
dw_printf ("[%d%c] ", c, p==TQ_PRIO_0_HI ? 'H' : 'L');
dw_printf ("%s", stemp); /* stations followed by : */
ax25_safe_print ((char *)pinfo, info_len, ! ax25_is_aprs(pp));
dw_printf ("\n");
(void)ax25_check_addresses (pp);
/* Optional hex dump of packet. */
if (g_debug_xmit_packet) {
text_color_set(DW_COLOR_DEBUG);
dw_printf ("------\n");
ax25_hex_dump (pp);
dw_printf ("------\n");
}
/*
* Turn on transmitter.
* Start sending leading flag bytes.
*/
time_ptt = dtime_now ();
#if DEBUG
text_color_set(DW_COLOR_DEBUG);
dw_printf ("xmit_thread: Turn on PTT now for channel %d. speed = %d\n", c, xmit_bits_per_sec[c]);
#endif
ptt_set (OCTYPE_PTT, c, 1);
pre_flags = MS_TO_BITS(xmit_txdelay[c] * 10, c) / 8;
num_bits = hdlc_send_flags (c, pre_flags, 0);
#if DEBUG
text_color_set(DW_COLOR_DEBUG);
dw_printf ("xmit_thread: txdelay=%d [*10], pre_flags=%d, num_bits=%d\n", xmit_txdelay[c], pre_flags, num_bits);
#endif
/*
* Transmit the frame.
*/
flen = ax25_pack (pp, fbuf);
assert (flen >= 1 && flen <= sizeof(fbuf));
nb = hdlc_send_frame (c, fbuf, flen);
num_bits += nb;
numframe = 1;
#if DEBUG
text_color_set(DW_COLOR_DEBUG);
dw_printf ("xmit_thread: flen=%d, nb=%d, num_bits=%d, numframe=%d\n", flen, nb, num_bits, numframe);
#endif
ax25_delete (pp);
/*
* Additional packets if available and not exceeding max.
*/
while (numframe < maxframe && tq_count (c,p) > 0) {
pp = tq_remove (c, p);
#if DEBUG
text_color_set(DW_COLOR_DEBUG);
dw_printf ("xmit_thread: tq_remove(chan=%d, prio=%d) returned %p\n", c, p, pp);
#endif
ax25_format_addrs (pp, stemp);
info_len = ax25_get_info (pp, &pinfo);
text_color_set(DW_COLOR_XMIT);
dw_printf ("[%d%c] ", c, p==TQ_PRIO_0_HI ? 'H' : 'L');
dw_printf ("%s", stemp); /* stations followed by : */
ax25_safe_print ((char *)pinfo, info_len, ! ax25_is_aprs(pp));
dw_printf ("\n");
(void)ax25_check_addresses (pp);
if (g_debug_xmit_packet) {
text_color_set(DW_COLOR_DEBUG);
dw_printf ("------\n");
ax25_hex_dump (pp);
dw_printf ("------\n");
}
/*
* Transmit the frame.
*/
flen = ax25_pack (pp, fbuf);
assert (flen >= 1 && flen <= sizeof(fbuf));
nb = hdlc_send_frame (c, fbuf, flen);
num_bits += nb;
numframe++;
#if DEBUG
text_color_set(DW_COLOR_DEBUG);
dw_printf ("xmit_thread: flen=%d, nb=%d, num_bits=%d, numframe=%d\n", flen, nb, num_bits, numframe);
#endif
ax25_delete (pp);
}
/*
* Need TXTAIL because we don't know exactly when the sound is done.
*/
post_flags = MS_TO_BITS(xmit_txtail[c] * 10, c) / 8;
nb = hdlc_send_flags (c, post_flags, 1);
num_bits += nb;
#if DEBUG
text_color_set(DW_COLOR_DEBUG);
dw_printf ("xmit_thread: txtail=%d [*10], post_flags=%d, nb=%d, num_bits=%d\n", xmit_txtail[c], post_flags, nb, num_bits);
#endif
/*
* While demodulating is CPU intensive, generating the tones is not.
* Example: on the RPi, with 50% of the CPU taken with two receive
* channels, a transmission of more than a second is generated in
* about 40 mS of elapsed real time.
*/
audio_wait(ACHAN2ADEV(c));
/*
* Ideally we should be here just about the time when the audio is ending.
* However, the innards of "audio_wait" are not satisfactory in all cases.
*
* Calculate how long the frame(s) should take in milliseconds.
*/
duration = BITS_TO_MS(num_bits, c);
/*
* See how long it has been since PTT was turned on.
* Wait additional time if necessary.
*/
time_now = dtime_now();
already = (int) ((time_now - time_ptt) * 1000.);
wait_more = duration - already;
#if DEBUG
text_color_set(DW_COLOR_DEBUG);
dw_printf ("xmit_thread: xmit duration=%d, %d already elapsed since PTT, wait %d more\n", duration, already, wait_more );
#endif
if (wait_more > 0) {
SLEEP_MS(wait_more);
}
else if (wait_more < -100) {
/* If we run over by 10 mSec or so, it's nothing to worry about. */
/* However, if PTT is still on about 1/10 sec after audio */
/* should be done, something is wrong. */
/* Looks like a bug with the RPi audio system. Never an issue with Ubuntu. */
/* This runs over randomly sometimes. TODO: investigate more fully sometime. */
#ifndef __arm__
text_color_set(DW_COLOR_ERROR);
dw_printf ("Transmit timing error: PTT is on %d mSec too long.\n", -wait_more);
#endif
}
/*
* Turn off transmitter.
*/
#if DEBUG
text_color_set(DW_COLOR_DEBUG);
time_now = dtime_now();
dw_printf ("xmit_thread: Turn off PTT now. Actual time on was %d mS, vs. %d desired\n", (int) ((time_now - time_ptt) * 1000.), duration);
#endif
ptt_set (OCTYPE_PTT, c, 0);
} /* end xmit_ax25_frames */
static void
deferred_callback(struct deferred_cb *cb, void *arg)
{
SLEEP_MS(1);
callback_count += 1;
}
void routing_table_create_stack(void) {
uint8_t stack_address = 0;
uint8_t tries = 0;
while(stack_address < SPI_ADDRESS_MAX) {
StackEnumerate se;
com_make_default_header(&se, 0, sizeof(StackEnumerate), FID_STACK_ENUMERATE);
uint32_t options = stack_address + 1;
if(spi_stack_send(&se, sizeof(StackEnumerate), &options) != 0) {
spi_stack_select(stack_address + 1);
tries = 0;
while(!spi_stack_master_transceive() && tries < 10) {
SLEEP_MS(50);
tries++;
}
if(tries == 10) {
break;
}
spi_stack_deselect();
spi_stack_buffer_size_recv = 0;
}
StackEnumerateReturn ser;
tries = 0;
while(tries < 10) {
SLEEP_MS(50);
spi_stack_select(stack_address + 1);
spi_stack_master_transceive();
spi_stack_deselect();
if(spi_stack_recv(&ser, sizeof(StackEnumerateReturn), NULL)) {
break;
}
tries++;
}
if(tries == 10) {
logspise("Did not receive answer for Stack Enumerate\n\r");
break;
}
stack_address++;
com_info.last_stack_address = stack_address;
for(uint8_t i = 0; i < STACK_ENUMERATE_MAX_UIDS; i++) {
if(ser.uids[i] != 0) {
if(routing_table_size >= ROUTING_TABLE_MAX_SIZE) {
break;
}
routing_table[routing_table_size].uid = ser.uids[i];
routing_table[routing_table_size].route_to.to = ROUTING_STACK;
routing_table[routing_table_size].route_to.option = stack_address;
routing_table_last_stack = routing_table_size;
routing_table_size++;
logspisi("New Stack participant (sa, uid): %d, %lu\n\r", stack_address, ser.uids[i]);
}
}
}
spi_stack_buffer_size_send = 0;
}
// Function: wdThread
// Access: Private
// Description: All core component functionality is contained in this thread.
// All of the JAUS component state machine code can be found here.
void *wdThread(void *threadData)
{
JausMessage rxMessage;
double time, prevTime, nextExcecuteTime = 0.0;
struct timespec sleepTime;
wdThreadRunning = TRUE;
sleepTime.tv_sec = 0;
sleepTime.tv_nsec = 1000;
time = getTimeSeconds();
wd->state = JAUS_INITIALIZE_STATE; // Set JAUS state to INITIALIZE
wdStartupState();
while(wdRun) // Execute state machine code while not in the SHUTDOWN state
{
do
{
if(nodeManagerReceive(wdNmi, &rxMessage))
{
//cDebug(4, "WD: Got message: %s from %d.%d.%d.%d\n", jausMessageCommandCodeString(rxMessage), rxMessage->source->subsystem, rxMessage->source->node, rxMessage->source->component, rxMessage->source->instance);
wdProcessMessage(rxMessage);
}
else
{
if(getTimeSeconds() > nextExcecuteTime)
{
break;
}
else
{
//nanosleep(&sleepTime, NULL);
SLEEP_MS(1);
}
}
}while(getTimeSeconds() < nextExcecuteTime);
prevTime = time;
time = getTimeSeconds();
wdThreadHz = 1.0/(time-prevTime); // Compute the update rate of this thread
switch(wd->state) // Switch component behavior based on which state the machine is in
{
case JAUS_INITIALIZE_STATE:
wdInitState();
break;
case JAUS_STANDBY_STATE:
wdStandbyState();
break;
case JAUS_READY_STATE:
wdReadyState();
break;
case JAUS_EMERGENCY_STATE:
wdEmergencyState();
break;
case JAUS_FAILURE_STATE:
wdFailureState();
break;
case JAUS_SHUTDOWN_STATE:
wdRun = FALSE;
break;
default:
wd->state = JAUS_FAILURE_STATE; // The default case JAUS_is undefined, therefore go into Failure State
break;
}
wdAllState();
nodeManagerSendCoreServiceConnections(wdNmi);
nextExcecuteTime = 2.0 * time + 1.0/WD_THREAD_DESIRED_RATE_HZ - getTimeSeconds();
}
wdShutdownState();
//usleep(50000); // Sleep for 50 milliseconds and then exit
SLEEP_MS(50);
wdThreadRunning = FALSE;
return NULL;
}
static int wait_for_clear_channel (int channel, int nowait, int slottime, int persist)
{
int r;
int n;
n = 0;
start_over_again:
while (hdlc_rec_data_detect_any(channel)) {
SLEEP_MS(WAIT_CHECK_EVERY_MS);
n++;
if (n > (WAIT_TIMEOUT_MS / WAIT_CHECK_EVERY_MS)) {
return 0;
}
}
//TODO1.2: rethink dwait.
/*
* Added in version 1.2 - for transceivers that can't
* turn around fast enough when using squelch and VOX.
*/
if (save_audio_config_p->achan[channel].dwait > 0) {
SLEEP_MS (save_audio_config_p->achan[channel].dwait * 10);
}
if (hdlc_rec_data_detect_any(channel)) {
goto start_over_again;
}
if ( ! nowait) {
while (1) {
SLEEP_MS (slottime * 10);
if (hdlc_rec_data_detect_any(channel)) {
goto start_over_again;
}
r = rand() & 0xff;
if (r <= persist) {
break;
}
}
}
// TODO1.2
while ( ! dw_mutex_try_lock(&(audio_out_dev_mutex[ACHAN2ADEV(channel)]))) {
SLEEP_MS(WAIT_CHECK_EVERY_MS);
n++;
if (n > (WAIT_TIMEOUT_MS / WAIT_CHECK_EVERY_MS)) {
return 0;
}
}
return 1;
} /* end wait_for_clear_channel */
static void * xmit_thread (void *arg)
#endif
{
int c = (int)(long)arg; // channel number.
packet_t pp;
int p;
int ok;
/*
* These are for timing of a transmission.
* All are in usual unix time (seconds since 1/1/1970) but higher resolution
*/
while (1) {
tq_wait_while_empty (c);
#if DEBUG
text_color_set(DW_COLOR_DEBUG);
dw_printf ("xmit_thread, channel %d: woke up\n", c);
#endif
for (p=0; p<TQ_NUM_PRIO; p++) {
pp = tq_remove (c, p);
#if DEBUG
text_color_set(DW_COLOR_DEBUG);
dw_printf ("xmit_thread: tq_remove(chan=%d, prio=%d) returned %p\n", c, p, pp);
#endif
if (pp != NULL) {
/*
* Wait for the channel to be clear.
* For the high priority queue, begin transmitting immediately.
* For the low priority queue, wait a random amount of time, in hopes
* of minimizing collisions.
*/
ok = wait_for_clear_channel (c, (p==TQ_PRIO_0_HI), xmit_slottime[c], xmit_persist[c]);
if (ok) {
/*
* Channel is clear and we have lock on output device.
*
* If destination is "SPEECH" send info part to speech synthesizer.
* If destination is "MORSE" send as morse code.
*/
char dest[AX25_MAX_ADDR_LEN];
int ssid = 0;
if (ax25_is_aprs (pp)) {
ax25_get_addr_no_ssid(pp, AX25_DESTINATION, dest);
ssid = ax25_get_ssid(pp, AX25_DESTINATION);
}
else {
strlcpy (dest, "", sizeof(dest));
}
if (strcmp(dest, "SPEECH") == 0) {
xmit_speech (c, pp);
}
else if (strcmp(dest, "MORSE") == 0) {
int wpm = ssid * 2;
if (wpm == 0) wpm = MORSE_DEFAULT_WPM;
// This is a bit of a hack so we don't respond too quickly for APRStt.
// It will be sent in high priority queue while a beacon wouldn't.
// Add a little delay so user has time release PTT after sending #.
// This and default txdelay would give us a second.
if (p == TQ_PRIO_0_HI) {
//text_color_set(DW_COLOR_DEBUG);
//dw_printf ("APRStt morse xmit delay hack...\n");
SLEEP_MS (700);
}
xmit_morse (c, pp, wpm);
}
else {
xmit_ax25_frames (c, p, pp);
}
dw_mutex_unlock (&(audio_out_dev_mutex[ACHAN2ADEV(c)]));
}
else {
/*
* Timeout waiting for clear channel.
* Discard the packet.
* Display with ERROR color rather than XMIT color.
*/
char stemp[1024]; /* max size needed? */
int info_len;
unsigned char *pinfo;
text_color_set(DW_COLOR_ERROR);
dw_printf ("Waited too long for clear channel. Discarding packet below.\n");
ax25_format_addrs (pp, stemp);
info_len = ax25_get_info (pp, &pinfo);
text_color_set(DW_COLOR_INFO);
dw_printf ("[%d%c] ", c, p==TQ_PRIO_0_HI ? 'H' : 'L');
dw_printf ("%s", stemp); /* stations followed by : */
ax25_safe_print ((char *)pinfo, info_len, ! ax25_is_aprs(pp));
dw_printf ("\n");
ax25_delete (pp);
} /* wait for clear channel. */
} /* for high priority then low priority */
}
}
return 0; /* unreachable but quiet the warning. */
} /* end xmit_thread */
void brick_init(void) {
// Wait 5ms so everything can power up
SLEEP_MS(5);
logging_init();
logsi("Booting %d\n\r", BRICK_DEVICE_IDENTIFIER);
logsi("Compiled on %s %s\n\r", __DATE__, __TIME__);
logsi("Processor family %s\n\r", IS_SAM3() ? "SAM3S" : "SAM4S");
led_init();
led_on(LED_STD_BLUE);
#ifdef LED_STD_RED
#if LOGGING_LEVEL == LOGGING_NONE
led_off(LED_STD_RED);
#else
led_on(LED_STD_RED);
#endif
#endif
logsi("LEDs initialized\n\r");
com_info.uid = uid_get_uid32();
// Add 0 at end for printing
char sn[MAX_BASE58_STR_SIZE] = {'\0'};
uid_to_serial_number(com_info.uid, sn);
set_serial_number_descriptor(sn, MAX_BASE58_STR_SIZE);
logsi("Unique ID %s (%lu)\n\r\n\r", sn, com_info.uid);
wdt_start();
logsi("Watchdog disabled\n\r");
mutex_init();
logsi("Mutexes initialized\n\r");
// Disable JTAG (Pins are needed for i2c)
#ifdef DISABLE_JTAG_ON_STARTUP
MATRIX->CCFG_SYSIO |= (CCFG_SYSIO_SYSIO12 |
CCFG_SYSIO_SYSIO4 |
CCFG_SYSIO_SYSIO5 |
CCFG_SYSIO_SYSIO6 |
CCFG_SYSIO_SYSIO7);
logsi("JTAG disabled\n\r");
#endif
com_info.current = COM_NONE;
PIO_InitializeInterrupts(15);
bricklet_clear_eeproms();
i2c_eeprom_master_init(TWI_BRICKLET);
logsi("I2C for Bricklets initialized\n\r");
usb_detect_configure();
adc_init();
adc_enable_temperature_sensor();
#ifndef NO_PERIODIC_ADC_CONVERISION
adc_start_periodic_conversion();
#endif
logsi("A/D converter initialized\n\r");
bricklet_init();
}
