static void sp100_wait_measurement(int n) {
/* TODO Signal the error over serial too */
systime_t start_time = chVTGetSystemTime();
while(sp100_status(n) & SP100_STATUS_MEASURE) {
/* If we've been waiting more than 50ms, signal an error and
* continue anyway.
*/
if(ST2MS(chVTGetSystemTime() - start_time) > 50) {
palSetLine(LINE_LED_RED);
return;
}
}
}
uint8_t EE_t::Write(uint32_t Addr, void *ptr, uint32_t Sz) {
uint8_t DevAddr = EE_BLOCK1_DEV_ADDR;
uint8_t ByteAddr;
while(Sz != 0) {
if(Addr >= EE_BLOCK_SZ) {
ByteAddr = (uint8_t)(Addr - EE_BLOCK_SZ);
DevAddr = EE_BLOCK2_DEV_ADDR;
}
else ByteAddr = (uint8_t)Addr;
uint32_t SzToW;
if(Sz < 8) SzToW = Sz;
else SzToW = 8;
if(Addr + SzToW >= EE_BLOCK_SZ) SzToW = EE_BLOCK_SZ - Addr;
Uart.Printf("DA=%X; BA=%u; SzToW=%u\r", DevAddr, ByteAddr, SzToW);
uint8_t Rslt = pi2c->WriteWrite(DevAddr, &ByteAddr, 1, (uint8_t*)ptr, SzToW);
if(Rslt != OK) return Rslt;
// Wait write cycle completion
systime_t t = chVTGetSystemTime();
do {
chThdSleepMilliseconds(2);
Rslt = pi2c->Write(DevAddr, &ByteAddr, 1);
if(chVTTimeElapsedSinceX(t) > 999) return TIMEOUT;
} while(Rslt != OK);
Addr += SzToW;
Sz -= SzToW;
}
Uart.Printf("Done\r");
return OK;
}
static void cmd_systime(BaseSequentialStream *chp, int argc, char *argv[]) {
(void)argv;
if (argc > 0) {
usage(chp, "systime");
return;
}
chprintf(chp, "%lu\r\n", (unsigned long)chVTGetSystemTime());
}
static void system_monitor_thread(void *arg)
{
(void)arg;
chRegSetThreadName("system monitor");
systime_t time = chVTGetSystemTime();
bool ant_status = 0;
while (TRUE) {
if (ant_status != frontend_ant_status()) {
ant_status = frontend_ant_status();
if (ant_status && frontend_ant_setting() == AUTO) {
log_info("Now using external antenna.");
}
else if (frontend_ant_setting() == AUTO) {
log_info("Now using patch antenna.");
}
}
u32 status_flags = ant_status << 31 | SBP_MAJOR_VERSION << 16 | SBP_MINOR_VERSION << 8;
sbp_send_msg(SBP_MSG_HEARTBEAT, sizeof(status_flags), (u8 *)&status_flags);
/* If we are in base station mode then broadcast our known location. */
if (broadcast_surveyed_position && position_quality == POSITION_FIX) {
double tmp[3];
double base_ecef[3];
double base_distance;
llhdeg2rad(base_llh, tmp);
wgsllh2ecef(tmp, base_ecef);
vector_subtract(3, base_ecef, position_solution.pos_ecef, tmp);
base_distance = vector_norm(3, tmp);
if (base_distance > BASE_STATION_DISTANCE_THRESHOLD) {
log_warn("Invalid surveyed position coordinates\n");
} else {
sbp_send_msg(SBP_MSG_BASE_POS_ECEF, sizeof(msg_base_pos_ecef_t), (u8 *)&base_ecef);
}
}
msg_iar_state_t iar_state;
if (simulation_enabled_for(SIMULATION_MODE_RTK)) {
iar_state.num_hyps = 1;
} else {
iar_state.num_hyps = dgnss_iar_num_hyps();
}
sbp_send_msg(SBP_MSG_IAR_STATE, sizeof(msg_iar_state_t), (u8 *)&iar_state);
DO_EVERY(2,
send_thread_states();
);
sleep_until(&time, MS2ST(heartbeat_period_milliseconds));
}
static void cmd_systime( int argc, char * argv[] )
{
(void)argv;
if( argc > 0 )
{
usage( "systime" );
return;
}
chprint( "%lu\r\n", (unsigned long)chVTGetSystemTime() );
}
/**
* Returns the total bytes divided by the total elapsed seconds since the
* previous call of this function.
*
* \param s The USART stats structure
*/
float usart_throughput(struct usart_stats* s)
{
float elapsed = ((float)chVTTimeElapsedSinceX(s->last_byte_ticks) /
(double)CH_CFG_ST_FREQUENCY*1000.0);
float kbps = s->byte_counter / elapsed;
s->byte_counter = 0;
s->last_byte_ticks = chVTGetSystemTime();
return kbps;
}
static void mtx2_execute(void) {
systime_t time;
test_wait_tick();
time = chVTGetSystemTime();
threads[0] = chThdCreateStatic(wa[0], WA_SIZE, chThdGetPriorityX()-1, thread2H, 0);
threads[1] = chThdCreateStatic(wa[1], WA_SIZE, chThdGetPriorityX()-2, thread2M, 0);
threads[2] = chThdCreateStatic(wa[2], WA_SIZE, chThdGetPriorityX()-3, thread2L, 0);
test_wait_threads();
test_assert_sequence(1, "ABC");
test_assert_time_window(2, time + MS2ST(100), time + MS2ST(100) + ALLOWED_DELAY);
}
/**
* Test Thread
*
* Replaces main_periodic_05()
*
*/
static __attribute__((noreturn)) void thd_main_periodic_05(void *arg)
{
chRegSetThreadName("thd_main_periodic_05");
(void) arg;
systime_t time = chVTGetSystemTime();
while (TRUE)
{
time += TIME_MS2I(500);
main_periodic_05();
chThdSleepUntil(time);
}
}
/** Clear unhealthy flags after some time, so we eventually retry
those sats in case they recover from their sickness. Call this
function regularly, and once per day it will reset the flags. */
static void check_clear_unhealthy(void)
{
static systime_t ticks;
if (chVTTimeElapsedSinceX(ticks) < S2ST(24*60*60))
return;
ticks = chVTGetSystemTime();
for (u32 i=0; i<PLATFORM_SIGNAL_COUNT; i++) {
if (acq_status[i].state == ACQ_PRN_UNHEALTHY)
acq_status[i].state = ACQ_PRN_ACQUIRING;
}
}
static void Draw(void)
{
int i;
systime_t current = chVTGetSystemTime();
/* clear buffer */
memset(display.pixels, 0, sizeof(struct Color) * LEDCOUNT);
if (effects.p_next != NULL)
{
EffectUpdate(effects.p_next, 0, 0, current, &display);
}
/* render volume for a 5s */
if (showVolume == true)
{
startVolumeEffect = current;
showVolume = false;
effVolume_cfg.volume = newVolume;
showVolumeEffect = true;
if (chVTTimeElapsedSinceX(startVolumeEffect) > MS2ST(15000))
{
EffectReset(&effVolume, 0, 0, current);
}
}
if (showVolumeEffect == true)
{
if (chVTTimeElapsedSinceX(startVolumeEffect) <= MS2ST(5000))
{
EffectUpdate(&effVolume, 0, 0, current, &display);
}
else
{
showVolumeEffect = false;
}
}
//draw buttons
if (showControls == true)
{
EffectUpdate(&effButtons, 0, 0, current, &display);
}
for (i = 0; i < LEDCOUNT; i++)
{
SetLedColor(i, &display.pixels[i]);
}
SetUpdateLed();
}
void packet_handler_int_process_packet(unsigned char *buffer, unsigned char len) {
if (!len) {
return;
}
// TODO: Currently, this function is run from an interrupt. It might be
// better to do the work from a thread.
PACKET_INT_CMD cmd = buffer[0];
buffer++;
len--;
int32_t index = 0;
int16_t int16;
uint8_t anchor_index = 0;
float alt_rec;
switch (cmd) {
case PACKET_INT_CMD_ALTITUDE:
int16 = buffer_get_int16(buffer, &index);
alt_rec = (float)int16 / 1000.0;
if (alt_rec < MIN_ALTUTUDE) {
alt_rec = MAX_ALTUTUDE;
}
last_received_altutude = alt_rec;
last_update_time_altitude = chVTGetSystemTime();
pos_correct_altitude(last_received_altutude);
break;
case PACKET_INT_CMD_RANGE:
anchor_index = buffer[index++];
pos_correct_anchor(anchor_index, (float)buffer_get_int32(buffer, &index) / 1000.0);
break;
case PACKET_INT_CMD_CLOCK:
TIM5->CNT = buffer_get_uint32(buffer, &index);
break;
case PACKET_INT_CMD_LOS:
safety_los_int_received(buffer[0]);
break;
default:
break;
}
}
void ResetWithColor(struct Color* color)
{
if (activeEffect == 0xFF)
{
activeEffect = 0xFF - 1;
}
else
{
activeEffect = 0xFF;
}
ColorCopy(color, &resetColor);
lastPatternSelect = chVTGetSystemTime();
}
void log_writer_thread(void *p)
{
int write_next = EVENT_BUF1;
int writeptr = 0;
chRegSetThreadName("logwriter");
chThdSleepMilliseconds(2000);
for (;;)
{
chThdSleepMilliseconds(50);
int x, y, z, gx, gy, gz;
sensors_get_accel(&x, &y, &z);
sensors_get_gyro(&gx, &gy, &gz);
static char buf[512];
chsnprintf(buf, sizeof(buf),
"%8d %8d %8d %8d %8d %8d %8d %8d %8d %8d %8d %8d %8d %8d %8d\r\n",
chVTGetSystemTime(),
get_battery_voltage_mV(), get_battery_current_mA(),
get_motor_temperature_mC(), get_mosfet_temperature_mC(),
motor_orientation_get_rpm(), x, y, z, gx,
bike_control_get_acceleration_level(), bike_control_get_motor_current(), bike_control_get_I_accumulator(),
!palReadPad(GPIOB, GPIOB_BRAKE), motor_limits_get_max_duty()
);
char *p = buf;
while (*p)
{
uint8_t *dest = (write_next == EVENT_BUF1) ? g_logbuffer1 : g_logbuffer2;
while (*p && writeptr < sizeof(g_logbuffer1))
{
dest[writeptr++] = *p++;
}
if (writeptr == sizeof(g_logbuffer1))
{
chEvtSignal(g_logsaver, write_next);
write_next = (write_next == EVENT_BUF1) ? EVENT_BUF2 : EVENT_BUF1;
writeptr = 0;
}
}
}
}
static void icuwidthcb(ICUDriver *icup) {
last_len_received[0] = ((float)icuGetWidthX(icup) / ((float)TIMER_FREQ / 1000.0));
float len = last_len_received[0] - pulse_start;
const float len_set = (pulse_end - pulse_start);
if (len > len_set) {
if (len < (len_set * 1.2)) {
len = len_set;
} else {
// Too long pulse. Most likely something is wrong.
len = -1.0;
}
} else if (len < 0.0) {
if ((len + pulse_start) > (pulse_start * 0.8)) {
len = 0.0;
} else {
// Too short pulse. Most likely something is wrong.
len = -1.0;
}
}
if (len >= 0.0) {
if (use_median_filter) {
float c = (len * 2.0 - len_set) / len_set;
static float c1 = 0.5;
static float c2 = 0.5;
float med = utils_middle_of_3(c, c1, c2);
c2 = c1;
c1 = c;
servo_pos[0] = med;
} else {
servo_pos[0] = (len * 2.0 - len_set) / len_set;
}
last_update_time = chVTGetSystemTime();
if (done_func) {
done_func();
}
}
}
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] = 0x00;
while (true) {
txmsg.data32[1] = chVTGetSystemTime();
msg_t ret = canTransmit(&CAND1, CAN_ANY_MAILBOX, &txmsg, MS2ST(100));
if(ret == MSG_TIMEOUT || ret == MSG_RESET)
oca_led_toggle(oca_led_err);
chThdSleepMilliseconds(100);
}
}
static THD_FUNCTION(AcceleroThd, arg)
{
systime_t time;
(void)arg;
chRegSetThreadName("accelerometer");
/* Reader thread loop.*/
while (TRUE) {
unsigned i;
time = chVTGetSystemTime();
/* Reading MEMS accelerometer X, Y and Z registers.*/
acc_sample.acceleration[0] = (int8_t)lis302dlReadRegister(&SPID1, LIS302DL_OUTX);
acc_sample.acceleration[1] = (int8_t)lis302dlReadRegister(&SPID1, LIS302DL_OUTY);
acc_sample.acceleration[2] = (int8_t)lis302dlReadRegister(&SPID1, LIS302DL_OUTZ);
acc_callback();
/* Waiting until the next 100 milliseconds time interval.*/
chThdSleepUntilWindowed(time, time + MS2ST(100));
}
}
void transmitter_receiver_isr(uint8_t pin) {
uint16_t cnt = TIM3->CNT;
uint16_t diff = 0;
float v = 0.0;
for (int i = 0; i < RECEIVER_CHANNELS; i++) {
if (pin == transmitter_receiver_channel[i].pin) {
if (palReadPad(transmitter_receiver_channel[i].port, transmitter_receiver_channel[i].pin)) {
transmitter_receiver_channel[i].t_start = cnt;
} else {
diff = (cnt > transmitter_receiver_channel[i].t_start) ?
cnt - transmitter_receiver_channel[i].t_start : 0xffff - (transmitter_receiver_channel[i].t_start - cnt);
v = (float)diff * 1000.0 / (float)RECEIVER_TIMER_FREQUENCY - PPM_LENGTH_MIN;
if (v < 0.0) {
v = 0.0;
} else if (v > (PPM_LENGTH_MAX - PPM_LENGTH_MIN)) {
v = PPM_LENGTH_MAX - PPM_LENGTH_MIN;
}
transmitter_receiver_channel[i].value = v;
last_update_time = chVTGetSystemTime();
}
}
}
}
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);
}
}
void cmd_sdc(BaseSequentialStream *chp, int argc, char *argv[]) {
static const char *mode[] = {"SDV11", "SDV20", "MMC", NULL};
systime_t start, end;
uint32_t n, startblk;
if (argc != 1) {
chprintf(chp, "Usage: sdiotest read|write|erase|all\r\n");
return;
}
/* Card presence check.*/
if (!blkIsInserted(&SDCD1)) {
chprintf(chp, "Card not inserted, aborting.\r\n");
return;
}
/* Connection to the card.*/
chprintf(chp, "Connecting... ");
if (sdcConnect(&SDCD1)) {
chprintf(chp, "failed\r\n");
return;
}
chprintf(chp, "OK\r\n\r\nCard Info\r\n");
chprintf(chp, "CSD : %08X %8X %08X %08X \r\n",
SDCD1.csd[3], SDCD1.csd[2], SDCD1.csd[1], SDCD1.csd[0]);
chprintf(chp, "CID : %08X %8X %08X %08X \r\n",
SDCD1.cid[3], SDCD1.cid[2], SDCD1.cid[1], SDCD1.cid[0]);
chprintf(chp, "Mode : %s\r\n", mode[SDCD1.cardmode & 3U]);
chprintf(chp, "Capacity : %DMB\r\n", SDCD1.capacity / 2048);
/* The test is performed in the middle of the flash area.*/
startblk = (SDCD1.capacity / MMCSD_BLOCK_SIZE) / 2;
if ((strcmp(argv[0], "read") == 0) ||
(strcmp(argv[0], "all") == 0)) {
/* Single block read performance, aligned.*/
chprintf(chp, "Single block aligned read performance: ");
start = chVTGetSystemTime();
end = start + MS2ST(1000);
n = 0;
do {
if (blkRead(&SDCD1, startblk, buf, 1)) {
chprintf(chp, "failed\r\n");
goto exittest;
}
n++;
} while (chVTIsSystemTimeWithin(start, end));
chprintf(chp, "%D blocks/S, %D bytes/S\r\n", n, n * MMCSD_BLOCK_SIZE);
/* Multiple sequential blocks read performance, aligned.*/
chprintf(chp, "16 sequential blocks aligned read performance: ");
start = chVTGetSystemTime();
end = start + MS2ST(1000);
n = 0;
do {
if (blkRead(&SDCD1, startblk, buf, SDC_BURST_SIZE)) {
chprintf(chp, "failed\r\n");
goto exittest;
}
n += SDC_BURST_SIZE;
} while (chVTIsSystemTimeWithin(start, end));
chprintf(chp, "%D blocks/S, %D bytes/S\r\n", n, n * MMCSD_BLOCK_SIZE);
#if STM32_SDC_SDIO_UNALIGNED_SUPPORT
/* Single block read performance, unaligned.*/
chprintf(chp, "Single block unaligned read performance: ");
start = chVTGetSystemTime();
end = start + MS2ST(1000);
n = 0;
do {
if (blkRead(&SDCD1, startblk, buf + 1, 1)) {
chprintf(chp, "failed\r\n");
goto exittest;
}
n++;
} while (chVTIsSystemTimeWithin(start, end));
chprintf(chp, "%D blocks/S, %D bytes/S\r\n", n, n * MMCSD_BLOCK_SIZE);
/* Multiple sequential blocks read performance, unaligned.*/
chprintf(chp, "16 sequential blocks unaligned read performance: ");
start = chVTGetSystemTime();
end = start + MS2ST(1000);
n = 0;
do {
if (blkRead(&SDCD1, startblk, buf + 1, SDC_BURST_SIZE)) {
chprintf(chp, "failed\r\n");
goto exittest;
}
n += SDC_BURST_SIZE;
} while (chVTIsSystemTimeWithin(start, end));
chprintf(chp, "%D blocks/S, %D bytes/S\r\n", n, n * MMCSD_BLOCK_SIZE);
#endif /* STM32_SDC_SDIO_UNALIGNED_SUPPORT */
}
if ((strcmp(argv[0], "write") == 0) ||
(strcmp(argv[0], "all") == 0)) {
unsigned i;
memset(buf, 0xAA, MMCSD_BLOCK_SIZE * 2);
chprintf(chp, "Writing...");
if(sdcWrite(&SDCD1, startblk, buf, 2)) {
chprintf(chp, "failed\r\n");
goto exittest;
}
chprintf(chp, "OK\r\n");
memset(buf, 0x55, MMCSD_BLOCK_SIZE * 2);
chprintf(chp, "Reading...");
if (blkRead(&SDCD1, startblk, buf, 1)) {
chprintf(chp, "failed\r\n");
goto exittest;
}
chprintf(chp, "OK\r\n");
for (i = 0; i < MMCSD_BLOCK_SIZE; i++)
buf[i] = i + 8;
chprintf(chp, "Writing...");
if(sdcWrite(&SDCD1, startblk, buf, 2)) {
chprintf(chp, "failed\r\n");
goto exittest;
}
chprintf(chp, "OK\r\n");
memset(buf, 0, MMCSD_BLOCK_SIZE * 2);
chprintf(chp, "Reading...");
if (blkRead(&SDCD1, startblk, buf, 1)) {
chprintf(chp, "failed\r\n");
goto exittest;
}
chprintf(chp, "OK\r\n");
}
if ((strcmp(argv[0], "erase") == 0) ||
(strcmp(argv[0], "all") == 0)) {
/**
* Test sdcErase()
* Strategy:
* 1. Fill two blocks with non-constant data
* 2. Write two blocks starting at startblk
* 3. Erase the second of the two blocks
* 3.1. First block should be equal to the data written
* 3.2. Second block should NOT be equal too the data written (i.e. erased).
* 4. Erase both first and second block
* 4.1 Both blocks should not be equal to the data initially written
* Precondition: SDC_BURST_SIZE >= 2
*/
memset(buf, 0, MMCSD_BLOCK_SIZE * 2);
memset(buf2, 0, MMCSD_BLOCK_SIZE * 2);
/* 1. */
unsigned int i = 0;
for (; i < MMCSD_BLOCK_SIZE * 2; ++i) {
buf[i] = (i + 7) % 'T'; //Ensure block 1/2 are not equal
}
/* 2. */
if(sdcWrite(&SDCD1, startblk, buf, 2)) {
chprintf(chp, "sdcErase() test write failed\r\n");
goto exittest;
}
/* 3. (erase) */
if(sdcErase(&SDCD1, startblk + 1, startblk + 2)) {
chprintf(chp, "sdcErase() failed\r\n");
goto exittest;
}
sdcflags_t errflags = sdcGetAndClearErrors(&SDCD1);
if(errflags) {
chprintf(chp, "sdcErase() yielded error flags: %d\r\n", errflags);
goto exittest;
}
if(sdcRead(&SDCD1, startblk, buf2, 2)) {
chprintf(chp, "single-block sdcErase() failed\r\n");
goto exittest;
}
/* 3.1. */
if(memcmp(buf, buf2, MMCSD_BLOCK_SIZE) != 0) {
chprintf(chp, "sdcErase() non-erased block compare failed\r\n");
goto exittest;
}
/* 3.2. */
if(memcmp(buf + MMCSD_BLOCK_SIZE,
buf2 + MMCSD_BLOCK_SIZE, MMCSD_BLOCK_SIZE) == 0) {
chprintf(chp, "sdcErase() erased block compare failed\r\n");
goto exittest;
}
/* 4. */
if(sdcErase(&SDCD1, startblk, startblk + 2)) {
chprintf(chp, "multi-block sdcErase() failed\r\n");
goto exittest;
}
if(sdcRead(&SDCD1, startblk, buf2, 2)) {
chprintf(chp, "single-block sdcErase() failed\r\n");
goto exittest;
}
/* 4.1 */
if(memcmp(buf, buf2, MMCSD_BLOCK_SIZE) == 0) {
chprintf(chp, "multi-block sdcErase() erased block compare failed\r\n");
goto exittest;
}
if(memcmp(buf + MMCSD_BLOCK_SIZE,
buf2 + MMCSD_BLOCK_SIZE, MMCSD_BLOCK_SIZE) == 0) {
chprintf(chp, "multi-block sdcErase() erased block compare failed\r\n");
goto exittest;
}
/* END of sdcErase() test */
}
/* Card disconnect and command end.*/
exittest:
sdcDisconnect(&SDCD1);
}
static void ResetEffects(void)
{
systime_t current = chVTGetSystemTime();
struct Effect* effect = effects.p_next;
EffectReset(effect, 0, 0, current);
}
static THD_FUNCTION(Thread1, arg) {
static int8_t xbuf[4], ybuf[4]; /* Last accelerometer data.*/
systime_t time; /* Next deadline.*/
(void)arg;
chRegSetThreadName("reader");
/* LIS302DL initialization.*/
lis302dlWriteRegister(&SPID1, LIS302DL_CTRL_REG1, 0x43);
lis302dlWriteRegister(&SPID1, LIS302DL_CTRL_REG2, 0x00);
lis302dlWriteRegister(&SPID1, LIS302DL_CTRL_REG3, 0x00);
/* Reader thread loop.*/
time = chVTGetSystemTime();
while (TRUE) {
int32_t x, y;
unsigned i;
/* Keeping an history of the latest four accelerometer readings.*/
for (i = 3; i > 0; i--) {
xbuf[i] = xbuf[i - 1];
ybuf[i] = ybuf[i - 1];
}
/* Reading MEMS accelerometer X and Y registers.*/
xbuf[0] = (int8_t)lis302dlReadRegister(&SPID1, LIS302DL_OUTX);
ybuf[0] = (int8_t)lis302dlReadRegister(&SPID1, LIS302DL_OUTY);
/* Transmitting accelerometer the data over SPI2.*/
spiSelect(&SPID2);
spiSend(&SPID2, 4, xbuf);
spiSend(&SPID2, 4, ybuf);
spiUnselect(&SPID2);
/* Calculating average of the latest four accelerometer readings.*/
x = ((int32_t)xbuf[0] + (int32_t)xbuf[1] +
(int32_t)xbuf[2] + (int32_t)xbuf[3]) / 4;
y = ((int32_t)ybuf[0] + (int32_t)ybuf[1] +
(int32_t)ybuf[2] + (int32_t)ybuf[3]) / 4;
/* Reprogramming the four PWM channels using the accelerometer data.*/
if (y < 0) {
pwmEnableChannel(&PWMD4, 0, (pwmcnt_t)-y);
pwmEnableChannel(&PWMD4, 2, (pwmcnt_t)0);
}
else {
pwmEnableChannel(&PWMD4, 2, (pwmcnt_t)y);
pwmEnableChannel(&PWMD4, 0, (pwmcnt_t)0);
}
if (x < 0) {
pwmEnableChannel(&PWMD4, 1, (pwmcnt_t)-x);
pwmEnableChannel(&PWMD4, 3, (pwmcnt_t)0);
}
else {
pwmEnableChannel(&PWMD4, 3, (pwmcnt_t)x);
pwmEnableChannel(&PWMD4, 1, (pwmcnt_t)0);
}
/* Waiting until the next 250 milliseconds time interval.*/
chThdSleepUntil(time += MS2ST(100));
}
}
static THD_FUNCTION(EffectControlThread, arg)
{
(void) arg;
chRegSetThreadName("effectcontrol");
effects.p_next = NULL;
int16_t effectNumber = -1;
systime_t time = chVTGetSystemTime();
while (TRUE)
{
if (noEffect == true)
{
effects.p_next = NULL;
}
if (nextEffect == true)
{
noEffect = false;
nextEffect = false;
activeEffect++;
if (activeEffect > 3)
{
activeEffect = 1;
}
time = chVTGetSystemTime();
(void)time;
}
if (activeEffect != effectNumber)
{
if(effectNumber == 1)
{
}
effectNumber = activeEffect;
if(effectNumber == 1)
{
effects.p_next = &effRandomPixels;
}
else if (effectNumber == 2)
{
effects.p_next = &effFallingPixels;
ColorRandom(&effFallingPixels_cfg.color);
effFallingPixels_cfg.randomRed = (rand() % 2) > 0;
effFallingPixels_cfg.randomGreen = (rand() % 2) > 0;
effFallingPixels_cfg.randomBlue = (rand() % 2) > 0;
}
else if (effectNumber == 3)
{
effFadingPixels_cfg.number = 1 + (rand() % 3);
ColorRandom(&effFadingPixels_cfg.color);
effFadingPixels_cfg.randomColor = (rand() % 2) > 0;
effects.p_next = &effFadingPixels;
}
ResetEffects();
}
Draw();
chThdSleepMilliseconds(10);
}
}
/**
* @brief Delays execution until next system time tick.
*
* @return The system time.
*/
systime_t test_wait_tick(void) {
chThdSleep(1);
return chVTGetSystemTime();
}
static void evt2_execute(void) {
eventmask_t m;
event_listener_t el1, el2;
systime_t target_time;
/*
* Test on chEvtWaitOne() without wait.
*/
chEvtAddEvents(5);
m = chEvtWaitOne(ALL_EVENTS);
test_assert(1, m == 1, "single event error");
m = chEvtWaitOne(ALL_EVENTS);
test_assert(2, m == 4, "single event error");
m = chEvtGetAndClearEvents(ALL_EVENTS);
test_assert(3, m == 0, "stuck event");
/*
* Test on chEvtWaitOne() with wait.
*/
test_wait_tick();
target_time = chVTGetSystemTime() + MS2ST(50);
threads[0] = chThdCreateStatic(wa[0], WA_SIZE, chThdGetPriorityX() - 1,
thread1, chThdGetSelfX());
m = chEvtWaitOne(ALL_EVENTS);
test_assert_time_window(4, target_time, target_time + ALLOWED_DELAY);
test_assert(5, m == 1, "single event error");
m = chEvtGetAndClearEvents(ALL_EVENTS);
test_assert(6, m == 0, "stuck event");
test_wait_threads();
/*
* Test on chEvtWaitAny() without wait.
*/
chEvtAddEvents(5);
m = chEvtWaitAny(ALL_EVENTS);
test_assert(7, m == 5, "unexpected pending bit");
m = chEvtGetAndClearEvents(ALL_EVENTS);
test_assert(8, m == 0, "stuck event");
/*
* Test on chEvtWaitAny() with wait.
*/
test_wait_tick();
target_time = chVTGetSystemTime() + MS2ST(50);
threads[0] = chThdCreateStatic(wa[0], WA_SIZE, chThdGetPriorityX() - 1,
thread1, chThdGetSelfX());
m = chEvtWaitAny(ALL_EVENTS);
test_assert_time_window(9, target_time, target_time + ALLOWED_DELAY);
test_assert(10, m == 1, "single event error");
m = chEvtGetAndClearEvents(ALL_EVENTS);
test_assert(11, m == 0, "stuck event");
test_wait_threads();
/*
* Test on chEvtWaitAll().
*/
chEvtObjectInit(&es1);
chEvtObjectInit(&es2);
chEvtRegisterMask(&es1, &el1, 1);
chEvtRegisterMask(&es2, &el2, 4);
test_wait_tick();
target_time = chVTGetSystemTime() + MS2ST(50);
threads[0] = chThdCreateStatic(wa[0], WA_SIZE, chThdGetPriorityX() - 1,
thread2, "A");
m = chEvtWaitAll(5);
test_assert_time_window(12, target_time, target_time + ALLOWED_DELAY);
m = chEvtGetAndClearEvents(ALL_EVENTS);
test_assert(13, m == 0, "stuck event");
test_wait_threads();
chEvtUnregister(&es1, &el1);
chEvtUnregister(&es2, &el2);
test_assert(14, !chEvtIsListeningI(&es1), "stuck listener");
test_assert(15, !chEvtIsListeningI(&es2), "stuck listener");
}
static THD_FUNCTION(mpu_thread, arg) {
(void)arg;
chRegSetThreadName("MPU Sampling");
static int16_t raw_accel_gyro_mag_tmp[9];
#if USE_MAGNETOMETER
static int mag_cnt = MAG_DIV;
#endif
static systime_t iteration_timer = 0;
static int identical_reads = 0;
iteration_timer = chVTGetSystemTime();
for(;;) {
if (get_raw_accel_gyro(raw_accel_gyro_mag_tmp)) {
int is_identical = 1;
for (int i = 0;i < 6;i++) {
if (raw_accel_gyro_mag_tmp[i] != raw_accel_gyro_mag_no_offset[i]) {
is_identical = 0;
break;
}
}
if (is_identical) {
identical_reads++;
} else {
identical_reads = 0;
}
if (identical_reads >= MAX_IDENTICAL_READS) {
failed_reads++;
chThdSleepMicroseconds(FAIL_DELAY_US);
reset_init_mpu();
iteration_timer = chVTGetSystemTime();
} else {
memcpy((uint16_t*)raw_accel_gyro_mag_no_offset, raw_accel_gyro_mag_tmp, sizeof(raw_accel_gyro_mag));
raw_accel_gyro_mag_tmp[3] -= mpu9150_gyro_offsets[0];
raw_accel_gyro_mag_tmp[4] -= mpu9150_gyro_offsets[1];
raw_accel_gyro_mag_tmp[5] -= mpu9150_gyro_offsets[2];
memcpy((uint16_t*)raw_accel_gyro_mag, raw_accel_gyro_mag_tmp, sizeof(raw_accel_gyro_mag));
update_time_diff = chVTGetSystemTime() - last_update_time;
last_update_time = chVTGetSystemTime();
if (read_callback) {
read_callback();
}
#if USE_MAGNETOMETER
mag_cnt++;
if (mag_cnt >= MAG_DIV) {
mag_cnt = 0;
mag_updated = 1;
int16_t raw_mag_tmp[3];
if (get_raw_mag(raw_mag_tmp)) {
memcpy((uint16_t*)raw_accel_gyro_mag_tmp + 6, raw_mag_tmp, sizeof(raw_mag_tmp));
} else {
failed_mag_reads++;
chThdSleepMicroseconds(FAIL_DELAY_US);
reset_init_mpu();
iteration_timer = chVTGetSystemTime();
}
} else {
mag_updated = 0;
}
#endif
}
} else {
failed_reads++;
chThdSleepMicroseconds(FAIL_DELAY_US);
reset_init_mpu();
iteration_timer = chVTGetSystemTime();
}
iteration_timer += US2ST(ITERATION_TIME_US);
systime_t time_start = chVTGetSystemTime();
if (iteration_timer > time_start) {
chThdSleep(iteration_timer - time_start);
} else {
chThdSleepMicroseconds(MIN_ITERATION_DELAY_US);
iteration_timer = chVTGetSystemTime();
}
}
}
