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;
}
/*
* Madgwick stream thread
*/
static msg_t stream_madgwick_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;
MadgwickAHRSupdate((-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);
getMadAttitude(&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;
}
