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; }