/*
* Encoder publisher node
*/
msg_t encoder_node(void *arg) {
encoder_node_conf * conf = (encoder_node_conf *) arg;
r2p::Node node(conf->name);
r2p::Publisher<r2p::EncoderMsg> enc_pub;
systime_t time;
qeidelta_t delta;
r2p::EncoderMsg *msgp;
(void) arg;
chRegSetThreadName(conf->name);
/* Enable the QEI driver. */
qeiInit();
qeiStart(&QEI_DRIVER, &qeicfg);
qeiEnable (&QEI_DRIVER);
node.advertise(enc_pub, conf->topic);
for (;;) {
time = chTimeNow();
delta = qeiUpdate(&QEI_DRIVER);
if (enc_pub.alloc(msgp)) {
msgp->delta = delta * conf->t2r;
enc_pub.publish(*msgp);
}
time += MS2ST(20);
chThdSleepUntil(time);
}
return CH_SUCCESS;
}
static msg_t thread1(BaseSequentialStream *chp) {
systime_t time = chTimeNow();
/*
* Thread-specific parameters
*/
chRegSetThreadName("t1");
static int d = 300;
static int w = 75000;
volatile uint32_t i, n;
while(1) {
if(chThdShouldTerminate())
return 0;
palSetPad(GPIOD, GPIOD_LED3);
/*
* Deadline for current execution of this task
*/
time += d;
chprintf(chp, "%s N %d %d %d %d\r\n", chRegGetThreadName(chThdSelf()), chThdGetPriority(),
chTimeNow(), time, chThdGetTicks(chThdSelf()));
/*
* Do some "work"
*/
for(i = 0; i < w; i ++) {
n = i / 3;
}
chprintf(chp, "%s X %d %d %d %d\r\n", chRegGetThreadName(chThdSelf()), chThdGetPriority(),
chTimeNow(), time, chThdGetTicks(chThdSelf()));
palClearPad(GPIOD, GPIOD_LED3);
/*
* Yield control of CPU until the deadline (which is also beginning of next period)
*/
chThdSleepUntil(time);
}
return 0;
}
static void cmd_rms(BaseSequentialStream *chp) {
/*
* Creating dynamic threads using the heap allocator
*/
Thread *tp1 = chThdCreateFromHeap(NULL, WA_SIZE, NORMALPRIO-2, thread1, chp);
Thread *tp2 = chThdCreateFromHeap(NULL, WA_SIZE, NORMALPRIO, thread2, chp);
Thread *tp3 = chThdCreateFromHeap(NULL, WA_SIZE, NORMALPRIO-1, thread3, chp);
Thread *tp4 = chThdCreateFromHeap(NULL, WA_SIZE, NORMALPRIO-3, thread4, chp);
chThdSleepUntil(chTimeNow() + MS2ST(500));
/*
* Try to kill threads
*/
chThdTerminate(tp1);
chThdTerminate(tp2);
chThdTerminate(tp3);
chThdTerminate(tp4);
/*
* Wait for the thread to terminate (if it has not terminated
* already) then get the thread exit message (msg) and returns the
* terminated thread memory to the heap.
*/
msg_t msg = chThdWait(tp1);
msg = chThdWait(tp2);
msg = chThdWait(tp3);
msg = chThdWait(tp4);
}
static void thd4_execute(void) {
systime_t time;
test_wait_tick();
/* Timeouts in microseconds.*/
time = chTimeNow();
chThdSleepMicroseconds(100000);
test_assert_time_window(1, time + US2ST(100000), time + US2ST(100000) + 1);
/* Timeouts in milliseconds.*/
time = chTimeNow();
chThdSleepMilliseconds(100);
test_assert_time_window(2, time + MS2ST(100), time + MS2ST(100) + 1);
/* Timeouts in seconds.*/
time = chTimeNow();
chThdSleepSeconds(1);
test_assert_time_window(3, time + S2ST(1), time + S2ST(1) + 1);
/* Absolute timelines.*/
time = chTimeNow() + MS2ST(100);
chThdSleepUntil(time);
test_assert_time_window(4, time, time + 1);
}
/**
* @brief SSI thread main function
* @param void * arg Thread arguments (NULL)
* @return msg_t Thread messages
*/
msg_t ssi_main(void *arg)
{
ssi_init(); // Initialize SSI Encoder
uint32_t position; // Position buffer
// Thread main loop
systime_t time = chTimeNow();
while(1)
{
time = chTimeNow();
if((ssi_mode == SSI_MODE_STREAM) && (encoder_power_state == ENCODER_ON))
{
time += US2ST(ssi_read_delay);
position = ssi_read(); // Read SSI Encoder
bt_send_position(position); // Send out encoder data via serial
while(!bt_get_tx_state())
{
__asm__("NOP"); // Wait for position send to complete
}
chThdSleepUntil(time);
}
else
{
chThdSleepMilliseconds(500);
}
}
}
msg_t encoder_node(void *arg) {
Node node("encoder");
Publisher<tEncoderMsg> enc_pub;
systime_t time;
tEncoderMsg *msgp;
(void) arg;
chRegSetThreadName("encoder");
qeiStart(&QEI_DRIVER, &qeicfg);
qeiEnable (&QEI_DRIVER);
node.advertise(enc_pub, "encoder"MOTOR_ID_STRING);
for (;;) {
time = chTimeNow();
if (enc_pub.alloc(msgp)) {
msgp->timestamp.sec = chTimeNow();
msgp->timestamp.nsec = chTimeNow();
msgp->delta = T2R(qeiUpdate(&QEI_DRIVER) * 100);
enc_pub.publish(*msgp);
}
time += MS2ST(10);
chThdSleepUntil(time);
}
return CH_SUCCESS;
}
msg_t qeipub_node(void *arg) {
Node node("qeipub");
Publisher<tQEIMsg> qei_pub;
systime_t time;
tQEIMsg *msgp;
(void) arg;
chRegSetThreadName("qeipub");
qeiStart(&QEI_DRIVER, &qeicfg);
qeiEnable (&QEI_DRIVER);
node.advertise(qei_pub, "qei"MOTOR_ID_STRING);
for (;;) {
time = chTimeNow();
int16_t delta = qeiUpdate(&QEI_DRIVER);
if (qei_pub.alloc(msgp)) {
msgp->timestamp.sec = 0;
msgp->timestamp.nsec = 0;
msgp->delta = delta;
qei_pub.publish(*msgp);
}
time += MS2ST(50);
chThdSleepUntil(time);
}
return CH_SUCCESS;
}
static msg_t uart_thread(void *arg) {
(void)arg;
chRegSetThreadName("UART");
uartStart(&HW_UART_DEV, &uart_cfg);
palSetPadMode(HW_UART_TX_PORT, HW_UART_TX_PIN, PAL_MODE_ALTERNATE(HW_UART_GPIO_AF) |
PAL_STM32_OSPEED_HIGHEST |
PAL_STM32_PUDR_PULLUP);
palSetPadMode(HW_UART_RX_PORT, HW_UART_RX_PIN, PAL_MODE_ALTERNATE(HW_UART_GPIO_AF) |
PAL_STM32_OSPEED_HIGHEST |
PAL_STM32_PUDR_PULLUP);
systime_t time = chTimeNow();
for(;;) {
time += MS2ST(1);
if ((systime_t) ((float) chTimeElapsedSince(last_uart_update_time)
/ ((float) CH_FREQUENCY / 1000.0)) > (float)TIMEOUT) {
mcpwm_set_brake_current(-10.0);
} else {
set_output(out_received);
}
chThdSleepUntil(time);
}
return 0;
}
static msg_t
VexSonarTask( void *arg )
{
tVexSonarChannel c;
(void)arg;
chRegSetThreadName("sonar");
gptStart( sonarGpt, &vexSonarGpt );
while(!chThdShouldTerminate())
{
if( vexSonars[nextSonar].flags == (SONAR_INSTALLED | SONAR_ENABLED) )
{
// ping sonar
vexSonarPing(nextSonar);
// wait for next time slot
// the timer is set to timeout in 40mS but we need a 10mS gap before any more
// pings can be sent
chThdSleepUntil(chTimeNow() + 50);
// calculate echo time
vexSonars[nextSonar].time = vexSonars[nextSonar].time_f - vexSonars[nextSonar].time_r;
// was the time too great ?
if( vexSonars[nextSonar].time > 35000 )
vexSonars[nextSonar].time = -1;
// if we have a valid time calculate real distance
if( vexSonars[nextSonar].time != -1 )
{
vexSonars[nextSonar].distance_cm = vexSonars[nextSonar].time / 58;
vexSonars[nextSonar].distance_inch = vexSonars[nextSonar].time / 148;
}
else
{
vexSonars[nextSonar].distance_cm = -1;
vexSonars[nextSonar].distance_inch = -1;
}
// look for next sonar
for(c=kVexSonar_1;c<kVexSonar_Num;c++)
{
if( ++nextSonar == kVexSonar_Num )
nextSonar = kVexSonar_1;
// we need sonar to be installed and enabled
if( vexSonars[nextSonar].flags == (SONAR_INSTALLED | SONAR_ENABLED) )
break;
}
}
else
// Nothing enabled, just wait
chThdSleepMilliseconds(25);
}
return (msg_t)0;
}
static THD_FUNCTION(PulseThread, arg) {
chRegSetThreadName("Pulse");
pulse_t *pulse = arg;
systime_t t = chVTGetSystemTimeX();
while (!chThdShouldTerminateX()) {
t += pulse->high;
palSetPad(GPIOD, pulse->pad);
chThdSleepUntil(t);
palClearPad(GPIOD, pulse->pad);
t+= pulse->low;
chThdSleepUntil(t);
}
chThdExit(MSG_OK);
}
static void test_001_002_execute(void) {
systime_t time;
/* The current system time is read then a sleep is performed for 100 system
ticks and on exit the system time is verified again.*/
test_set_step(1);
{
time = chVTGetSystemTimeX();
chThdSleep(100);
test_assert_time_window(time + 100,
time + 100 + 1,
"out of time window");
}
/* The current system time is read then a sleep is performed for 100000
microseconds and on exit the system time is verified again.*/
test_set_step(2);
{
time = chVTGetSystemTimeX();
chThdSleepMicroseconds(100);
test_assert_time_window(time + US2ST(100),
time + US2ST(100) + 1,
"out of time window");
}
/* The current system time is read then a sleep is performed for 100
milliseconds and on exit the system time is verified again.*/
test_set_step(3);
{
time = chVTGetSystemTimeX();
chThdSleepMilliseconds(100);
test_assert_time_window(time + MS2ST(100),
time + MS2ST(100) + 1,
"out of time window");
}
/* The current system time is read then a sleep is performed for 1
second and on exit the system time is verified again.*/
test_set_step(4);
{
time = chVTGetSystemTimeX();
chThdSleepSeconds(1);
test_assert_time_window(time + S2ST(1),
time + S2ST(1) + 1,
"out of time window");
}
test_set_step(5);
{
time = chVTGetSystemTimeX();
chThdSleepUntil(time + 100);
test_assert_time_window(time + 100,
time + 100 + 1,
"out of time window");
}
}
/*
* Regelungsthread
*/
static msg_t Regelungsthread(void *arg) {
systime_t time = chTimeNow(); // Tnow
while (TRUE) {
time += MS2ST(5); // Next deadline
Regelung();
//chThdSleepMilliseconds(10); /* Fixed interval.*/
chThdSleepUntil(time);
}
}
static THD_FUNCTION(Thread1, arg){
(void)arg;
chRegSetThreadName("blinker");
systime_t time = chVTGetSystemTimeX();
while(true){
time +=MS2ST(1000); // Next deadline
palTogglePad(GPIOA, GPIOA_LED_GREEN);
chThdSleepUntil(time);
}
}
static THD_FUNCTION(thread3, arg){
(void)arg;
chRegSetThreadName("echo-thread");
systime_t time = chVTGetSystemTimeX();
while(1){
time += MS2ST(500);
chprintf(chp, "\n\rHC-SR04: Distance = %d cm", echo/58);
chThdSleepUntil(time);
}
}
/**
* 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);
}
}
static THD_FUNCTION(Thread2, arg){
(void)arg;
chRegSetThreadName("trigger-thread");
systime_t time = chVTGetSystemTimeX();
while(true){
time += MS2ST(500);
palSetPad(TRIGGER_PORT, TRIGGER_PIN);
chThdSleepMicroseconds(10);
palClearPad(TRIGGER_PORT, TRIGGER_PIN);
chThdSleepUntil(time);
}
}
static msg_t stream_gyro_thread(void *arg) {
uint16_t period = *(uint16_t *)arg;
systime_t time = chTimeNow();
chRegSetThreadName("l3g4200d_stream_gyro");
while (TRUE) {
chprintf((BaseSequentialStream*)&SERIAL_DRIVER, "%6d %5d %5d %5d\r\n", (int)time,
gyro_data.x, gyro_data.y, gyro_data.z);
time += MS2ST(period);
chThdSleepUntil(time);
}
return 0;
}
static msg_t stream_mag_thread(void *arg) {
uint16_t period = *(uint16_t *)arg;
systime_t time = chTimeNow();
chRegSetThreadName("lsm303_stream_mag");
while (TRUE) {
chprintf((BaseSequentialStream*)&SERIAL_DRIVER, "%6d %5d %5d %5d %x\r\n", (int)time,
mag_data.x, mag_data.y, mag_data.z, status_m);
time += MS2ST(period);
chThdSleepUntil(time);
}
return 0;
}
static msg_t PollMPU6050Thread(void *arg) {
systime_t time;
(void)arg;
time = chTimeNow();
while (TRUE) {
if (mpu6050GetNewData()) {
chBSemSignal(&bsemNewDataReady);
}
/* Wait until the next 2 milliseconds passes. */
chThdSleepUntil(time += MS2ST(2));
}
/* This point should never be reached. */
return 0;
}
/*
* Stream thread
*/
static msg_t stream_raw_thread(void *arg) {
uint16_t period = *(uint16_t *)arg;
systime_t time = chTimeNow();
while (TRUE) {
chprintf((BaseSequentialStream*)&SERIAL_DRIVER, "%6d %f %f %f %f %f %f %d %d %d\r\n", (int)time,
gyro_data.x / 57.143, gyro_data.y / 57.143, gyro_data.z / 57.143,
acc_data.x / 1000.0, acc_data.y / 1000.0, acc_data.z / 1000.0,
mag_data.x, mag_data.y, mag_data.z);
time += MS2ST(period);
chThdSleepUntil(time);
}
return 0;
}
static THD_FUNCTION(ThreadINA, arg)
{
(void)arg;
chRegSetThreadName("INA");
systime_t time = chVTGetSystemTimeX(); // T0
while (true) {
time += US2ST(READFREQ);
//palSetPad(GPIOG, GPIOG_LED4_RED);
//palTogglePad(GPIOG, GPIOG_LED4_RED);
busvoltage=ina219GetBusVoltage_V();
current_mA=ina219GetCurrent_mA();
shuntvoltage = ina219GetShuntVoltage_mV();
loadvoltage = busvoltage + (shuntvoltage / 1000);
milliwatthours += busvoltage*current_mA*READFREQ/1e6/3600; // 1 Wh = 3600 joules
milliamphours += current_mA*READFREQ/1e6/3600;
// Update peaks, min and avg during our serial refresh period:
if (current_mA > rpPeakCurrent)
rpPeakCurrent = current_mA;
if (current_mA < rpMinCurrent)
rpMinCurrent = current_mA;
if (loadvoltage > rpPeakLoadVolt)
rpPeakLoadVolt = loadvoltage;
if (loadvoltage < rpMinLoadVolt)
rpMinLoadVolt = loadvoltage;
rpAvgCurrent = (rpAvgCurrent*rpSamples + current_mA)/(rpSamples+1);
rpAvgLoadVolt = (rpAvgLoadVolt*rpSamples + loadvoltage)/(rpSamples+1);
rpSamples++;
// Update absolute peaks and mins
if (current_mA > peakCurrent) {
peakCurrent = current_mA;
voltageAtPeakCurrent = loadvoltage;
}
if (loadvoltage < minVoltage) {
minVoltage = loadvoltage;palSetPadMode(GPIOA, 5, PAL_MODE_OUTPUT_PUSHPULL); palClearPad(GPIOA, 5);
currentAtMinVoltage = current_mA;
}
//palClearPad(GPIOG, GPIOG_LED4_RED);
chThdSleepUntil(time);
}
}
/*
* Tilt stream thread
*/
static msg_t stream_tilt_thread(void *arg) {
attitude_t attitude_data;
uint16_t period = *(uint16_t *)arg;
systime_t time = chTimeNow();
while (TRUE) {
MahonyAHRSupdateIMU(0, (gyro_data.y / 57.143) * 3.141592 / 180.0, 0,
-acc_data.x / 1000.0, 0, acc_data.z / 1000.0);
getMahAttitude(&attitude_data);
chprintf((BaseSequentialStream*)&SERIAL_DRIVER, "%6d %f\r\n", (int)time,
attitude_data.pitch * 180.0 / 3.141592);
time += MS2ST(period);
chThdSleepUntil(time);
}
return 0;
}
msg_t qeipub_node(void *arg) {
Node node("qeipub");
Publisher<tQEIMsg> qei_pub;
systime_t time;
tQEIMsg *msgp;
(void) arg;
chRegSetThreadName("qeipub");
qeiStart(&QEI_DRIVER, &qeicfg);
qeiEnable (&QEI_DRIVER);
switch (stm32_id8()) {
case M1:
node.advertise(qei_pub, "qei1");
break;
case M2:
node.advertise(qei_pub, "qei2");
break;
case M3:
node.advertise(qei_pub, "qei3");
break;
default:
node.advertise(qei_pub, "qei");
break;
}
for (;;) {
time = chTimeNow();
int16_t delta = qeiUpdate(&QEI_DRIVER);
if (qei_pub.alloc(msgp)) {
msgp->timestamp.sec = 0;
msgp->timestamp.nsec = 0;
msgp->delta = delta;
qei_pub.publish(*msgp);
}
time += MS2ST(50);
chThdSleepUntil(time);
}
return CH_SUCCESS;
}
msg_t encoder_node(void *arg) {
Node node("encoder");
Publisher<tEncoderMsg> enc_pub;
systime_t time;
tEncoderMsg *msgp;
(void) arg;
chRegSetThreadName("encoder");
qeiStart(&QEI_DRIVER, &qeicfg);
qeiEnable (&QEI_DRIVER);
switch (stm32_id8()) {
case M1:
node.advertise(enc_pub, "encoder1");
break;
case M2:
node.advertise(enc_pub, "encoder2");
break;
case M3:
node.advertise(enc_pub, "encoder3");
break;
default:
node.advertise(enc_pub, "encoder");
break;
}
for (;;) {
time = chTimeNow();
if (enc_pub.alloc(msgp)) {
msgp->timestamp.sec = chTimeNow();
msgp->timestamp.nsec = chTimeNow();
msgp->delta = T2R(qeiUpdate(&QEI_DRIVER) * 100);
enc_pub.publish(*msgp);
}
time += MS2ST(10);
chThdSleepUntil(time);
}
return CH_SUCCESS;
}
static msg_t PowerKeeperThread(void *arg){
chRegSetThreadName("PowerKeeper");
(void)arg;
uint32_t batcap = 0; /* battery capacitance in A*mS */
uint32_t batfill = 0; /* battery filling in A*mS */
int32_t i = -1;
/* get current battery capacitance from parameter structure */
i = _key_index_search("BAT_cap");
if (i == -1)
chDbgPanic("key not found");
else
batcap = 3600 * ((uint32_t)floorf(global_data[i].value));
/* get battery fill in percents and calculate fill in A*mS*/
i = _key_index_search("BAT_fill");
if (i == -1)
chDbgPanic("key not found");
else
batfill = (batcap * (uint32_t)floorf(global_data[i].value)) / 100;
systime_t time = chTimeNow(); // T0
while (TRUE) {
time += MS2ST(PWR_CHECK_PERIOD); // Next deadline
raw_data.main_current = samples[ADC_CURRENT_SENS_OFFSET];
raw_data.main_voltage = samples[ADC_MAIN_SUPPLY_OFFSET];
raw_data.secondary_voltage = samples[ADC_6V_SUPPLY_OFFSET];
comp_data.main_current = get_comp_main_current(raw_data.main_current);
comp_data.secondary_voltage = get_comp_secondary_voltage(raw_data.secondary_voltage);
batfill -= (comp_data.main_current * PWR_CHECK_PERIOD) / 1000;
mavlink_sys_status_struct.battery_remaining = (batfill * 100) / batcap;
mavlink_sys_status_struct.current_battery = (uint16_t)(comp_data.main_current / 10);
mavlink_sys_status_struct.voltage_battery = comp_data.secondary_voltage;
chThdSleepUntil(time);
}
return 0;
}
/*
* Publisher threads.
*/
static msg_t PublisherRawThread(void *arg) {
Middleware & mw = Middleware::instance();
Node n("pubRaw");
Publisher<tIMURaw9> pub("IMURaw");
tIMURaw9 *msg;
systime_t time;
static const int period = 10;
(void) arg;
chRegSetThreadName("tIMURaw9 pub thread");
mw.newNode(&n);
if (! n.advertise(&pub)) {
mw.delNode(&n);
return 0;
}
time = chTimeNow();
while (TRUE) {
msg = pub.alloc();
if (msg != NULL) {
rtcanGetTime(&RTCAND1, (rtcan_time_t *)&(msg->timestamp));
msg->gyro_x = gyro_data.x;
msg->gyro_y = gyro_data.y;
msg->gyro_z = gyro_data.z;
msg->acc_x = acc_data.x;
msg->acc_y = acc_data.y;
msg->acc_z = acc_data.z;
msg->mag_x = mag_data.x;
msg->mag_y = mag_data.y;
msg->mag_z = mag_data.z;
pub.broadcast(msg);
}
time += MS2ST(period);
chThdSleepUntil(time);
}
return 0;
}
static msg_t Thread1(void *arg) {
(void)arg;
// Next deadline.
systime_t time;
chRegSetThreadName("accelreader");
// Reader thread loop.
time = chTimeNow();
while (TRUE) {
readAccel();
chMtxLock(&accelMtx);
// Reprogramming the four PWM channels using the accelerometer data.
if (accel_y[4] < 0) {
pwmEnableChannel(&PWMD4, 0, (pwmcnt_t)-accel_y[4]);
pwmEnableChannel(&PWMD4, 2, (pwmcnt_t)0);
}
else {
pwmEnableChannel(&PWMD4, 2, (pwmcnt_t)accel_y[4]);
pwmEnableChannel(&PWMD4, 0, (pwmcnt_t)0);
}
if (accel_x[4] < 0) {
pwmEnableChannel(&PWMD4, 1, (pwmcnt_t)-accel_x[4]);
pwmEnableChannel(&PWMD4, 3, (pwmcnt_t)0);
}
else {
pwmEnableChannel(&PWMD4, 3, (pwmcnt_t)accel_x[4]);
pwmEnableChannel(&PWMD4, 1, (pwmcnt_t)0);
}
chMtxUnlock();
// Waiting until the next 250 milliseconds time interval.
chThdSleepUntil(time += MS2ST(100));
}
return (msg_t)0;
}
msg_t madgwick_node(void *arg) {
r2p::Node node("madgwick");
r2p::Publisher<r2p::TiltMsg> tilt_pub;
attitude_t attitude_data;
systime_t time;
(void) arg;
chRegSetThreadName("madgwick");
i2cStart(&I2C_DRIVER, &i2c1cfg);
spiStart(&SPI_DRIVER, &spi1cfg);
extStart(&EXTD1, &extcfg);
gyroRun(&SPI_DRIVER, NORMALPRIO);
accRun(&I2C_DRIVER, NORMALPRIO);
// magRun(&I2C_DRIVER, NORMALPRIO);
node.advertise(tilt_pub, "tilt");
time = chTimeNow();
for (;;) {
MadgwickAHRSupdateIMU((gyro_data.x / 57.143) * 3.141592 / 180.0, (gyro_data.y / 57.143) * 3.141592 / 180.0,
(gyro_data.z / 57.143) * 3.141592 / 180.0, acc_data.x / 1000.0, acc_data.y / 1000.0,
acc_data.z / 980.0);
getMadAttitude(&attitude_data);
r2p::TiltMsg *msgp;
if (tilt_pub.alloc(msgp)) {
msgp->angle = (-attitude_data.roll * 57.29578) - 2.35; // basketbot offset
tilt_pub.publish(*msgp);
}
time += MS2ST(20);
chThdSleepUntil(time);
}
return CH_SUCCESS;
}
static msg_t thread2(BaseSequentialStream *chp) {
systime_t time = chTimeNow();
static int d = 170;
static int w = 40000;
chRegSetThreadName("t2");
volatile uint32_t i, n;
while(1) {
if(chThdShouldTerminate())
return 0;
palSetPad(GPIOD, GPIOD_LED4);
time += d;
chprintf(chp, "%s N %d %d %d %d\r\n", chRegGetThreadName(chThdSelf()), chThdGetPriority(),
chTimeNow(), time, chThdGetTicks(chThdSelf()));
for(i = 0; i < w; i ++) {
n = i / 3;
}
chprintf(chp, "%s X %d %d %d %d\r\n", chRegGetThreadName(chThdSelf()), chThdGetPriority(),
chTimeNow(), time, chThdGetTicks(chThdSelf()));
palClearPad(GPIOD, GPIOD_LED4);
chThdSleepUntil(time);
}
return 0;
}
/*
* Mahony stream thread
*/
static msg_t stream_mahony_thread(void *arg) {
attitude_t attitude_data;
uint16_t period = *(uint16_t *)arg;
systime_t time = chTimeNow();
while (TRUE) {
float mx = ((float)mag_data.x - (-440.0)) / (510 - (-440)) * 2 - 1.0;
float my = ((float)mag_data.y - (-740.0)) / (380 - (-740)) * 2 - 1.0;
float mz = ((float)mag_data.z - (-500.0)) / (500 - (-500)) * 2 - 1.0;
MahonyAHRSupdate((-gyro_data.x / 57.143) * 3.141592 / 180.0,
(gyro_data.y / 57.143) * 3.141592 / 180.0,
-(gyro_data.z / 57.143) * 3.141592 / 180.0,
-acc_data.x / 1000.0, acc_data.y / 1000.0,
acc_data.z / 1000.0, mx, -my, -mz);
getMahAttitude(&attitude_data);
chprintf((BaseSequentialStream*)&SERIAL_DRIVER, "%6d %f %f %f\r\n", (int)time, attitude_data.roll,
attitude_data.pitch, attitude_data.yaw);
time += MS2ST(period);
chThdSleepUntil(time);
}
return 0;
}
