static void cmd_stream_tilt(BaseSequentialStream*chp, int argc, char *argv[]) {
Thread *tp;
if (argc != 1) {
chprintf(chp, "Usage: stilt <Hz>\r\n");
return;
}
period = (1000 / atoi(argv[0]));
if (gyrotp == NULL)
gyrotp = gyroRun(&SPI_DRIVER, NORMALPRIO);
if (acctp == NULL)
acctp = accRun(&I2C_DRIVER, NORMALPRIO);
tp = chThdCreateFromHeap(NULL, WA_SIZE_1K, NORMALPRIO, stream_tilt_thread,
(void *)&period);
if (tp == NULL) {
chprintf(chp, "out of memory\r\n");
return;
}
return;
}
static void cmd_gyro(BaseSequentialStream*chp, int argc, char *argv[]) {
(void)argv;
if (argc > 0) {
chprintf(chp, "Usage: gyro\r\n");
return;
}
if (gyrotp == NULL)
gyrotp = gyroRun(&SPI_DRIVER, NORMALPRIO);
}
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 void cmd_stream_gyro(BaseSequentialStream*chp, int argc, char *argv[]) {
Thread *tp;
if (argc != 1) {
chprintf(chp, "Usage: sg <Hz>\r\n");
return;
}
period = (1000 / atoi(argv[0]));
if (gyrotp == NULL)
gyrotp = gyroRun(&SPI_DRIVER, NORMALPRIO);
tp = chThdCreateFromHeap(NULL, WA_SIZE_256B, chThdGetPriority(),
stream_gyro_thread, (void *)&period);
if (tp == NULL) {
chprintf(chp, "out of memory\r\n");
return;
}
return;
}
/*
* Application entry point.
*/
int main(void) {
/*
* System initializations.
* - HAL initialization, this also initializes the configured device drivers
* and performs the board-specific initializations.
* - Kernel initialization, the main() function becomes a thread and the
* RTOS is active.
*/
halInit();
chSysInit();
/*
* Activates the serial driver 1 using the driver default configuration.
*/
sdStart(&SERIAL_DRIVER, NULL);
/*
* Activates the EXT driver.
*/
extStart(&EXTD1, &extcfg);
/*
* Activates the I2C driver.
*/
i2cStart(&I2C_DRIVER, &i2c1cfg);
/*
* Activates the SPI driver.
*/
spiStart(&SPI_DRIVER, &spi1cfg);
/*
* Activates the RTCAN driver.
*/
rtcanInit();
rtcanStart(&RTCAND1, &rtcan_config);
/*
* Creates the blinker thread.
*/
chThdCreateStatic(waThread1, sizeof(waThread1), NORMALPRIO, Thread1, NULL);
chThdSleepMilliseconds(100);
/*
* Creates the sensor threads.
*/
gyrotp = gyroRun(&SPI_DRIVER, NORMALPRIO);
acctp = accRun(&I2C_DRIVER, NORMALPRIO);
magtp = magRun(&I2C_DRIVER, NORMALPRIO);
/*
* Creates the publisher threads.
*/
chThdCreateFromHeap(NULL, WA_SIZE_2K, NORMALPRIO + 1, PublisherRawThread,
NULL);
chThdSleepMilliseconds(100);
remote_sub("IMURaw");
/*
* Normal main() thread activity, in this demo it does nothing except
* sleeping in a loop and check the button state.
*/
while (TRUE) {
chThdSleepMilliseconds(200);
}
}
/*
* Application entry point.
*/
int main(void) {
Thread *shelltp = NULL;
/*
* System initializations.
* - HAL initialization, this also initializes the configured device drivers
* and performs the board-specific initializations.
* - Kernel initialization, the main() function becomes a thread and the
* RTOS is active.
*/
halInit();
chSysInit();
/*
* Activates the serial driver 1 using the driver default configuration.
*/
sdStart(&SERIAL_DRIVER, NULL);
/*
* Shell manager initialization.
*/
shellInit();
/*
* Activates the EXT driver.
*/
extStart(&EXTD1, &extcfg);
/*
* Activates the I2C driver.
*/
i2cStart(&I2C_DRIVER, &i2c1cfg);
/*
* Activates the SPI driver.
*/
spiStart(&SPI_DRIVER, &spi1cfg);
/*
* Creates the blinker thread.
*/
chThdCreateStatic(waThread1, sizeof(waThread1), NORMALPRIO, Thread1, NULL);
chThdSleepMilliseconds(200);
if (gyrotp == NULL)
gyrotp = gyroRun(&SPI_DRIVER, NORMALPRIO);
if (acctp == NULL)
acctp = accRun(&I2C_DRIVER, NORMALPRIO);
if (magtp == NULL)
magtp = magRun(&I2C_DRIVER, NORMALPRIO);
/*
* Normal main() thread activity, in this demo it does nothing except
* sleeping in a loop and check the button state.
*/
while (TRUE) {
if (!shelltp)
shelltp = shellCreate(&shell_cfg1, SHELL_WA_SIZE, NORMALPRIO - 1);
else if (chThdTerminated(shelltp)) {
chThdRelease(shelltp);
shelltp = NULL;
}
chThdSleepMilliseconds(200);
}
}
