static void cmd_read(BaseSequentialStream *chp, int argc, char *argv[]) { (void)argv; if (argc != 1) { chprintf(chp, "Usage: read [raw|cooked]\r\n"); return; } while (chnGetTimeout((BaseChannel *)chp, 150) == Q_TIMEOUT) { if (!strcmp (argv[0], "raw")) { #if CHPRINTF_USE_ANSI_CODE chprintf(chp, "\033[2J\033[1;1H"); #endif gyroscopeReadRaw(&L3GD20D1, rawdata); chprintf(chp, "L3GD20 Gyroscope raw data...\r\n"); for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++) { chprintf(chp, "%c-axis: %d\r\n", axisID[i], rawdata[i]); } } else if (!strcmp (argv[0], "cooked")) { #if CHPRINTF_USE_ANSI_CODE chprintf(chp, "\033[2J\033[1;1H"); #endif gyroscopeReadCooked(&L3GD20D1, cookeddata); chprintf(chp, "L3GD20 Gyroscope cooked data...\r\n"); for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++) { chprintf(chp, "%c-axis: %.4f DPS\r\n", axisID[i], cookeddata[i]); } } else { chprintf(chp, "Usage: read [raw|cooked]\r\n"); return; } } chprintf(chp, "Stopped\r\n"); }
/* * 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(); /* * Initializes a serial-over-USB CDC driver. */ sduObjectInit(&SDU1); sduStart(&SDU1, &serusbcfg); /* * Activates the USB driver and then the USB bus pull-up on D+. * Note, a delay is inserted in order to not have to disconnect the cable * after a reset. */ usbDisconnectBus(serusbcfg.usbp); chThdSleepMilliseconds(1500); usbStart(serusbcfg.usbp, &usbcfg); usbConnectBus(serusbcfg.usbp); /* * Creates the blinker thread. */ chThdCreateStatic(waThread1, sizeof(waThread1), NORMALPRIO + 1, Thread1, NULL); /* * L3GD20 Object Initialization */ l3gd20ObjectInit(&L3GD20D1); /* * Activates the L3GD20 driver. */ l3gd20Start(&L3GD20D1, &l3gd20cfg); while(!palReadLine(LINE_BUTTON)){ chprintf(chp, "Press BTN to calibrate gyroscope...\r\n"); chThdSleepMilliseconds(150); #if CHPRINTF_USE_ANSI_CODE chprintf(chp, "\033[2J\033[1;1H"); #endif } chprintf(chp, "Calibrating Gyroscope sampling bias...\r\n"); chprintf(chp, "Keep it in the rest position while red LED is on\r\n"); chThdSleepMilliseconds(3000); palSetLine(LINE_LED10_RED); chThdSleepMilliseconds(1000); gyroscopeSampleBias(&L3GD20D1); palClearLine(LINE_LED10_RED); #if CHPRINTF_USE_ANSI_CODE chprintf(chp, "\033[2J\033[1;1H"); #endif while (TRUE) { palToggleLine(LINE_LED10_RED); gyroscopeReadRaw(&L3GD20D1, rawdata); for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++) chprintf(chp, "RAW-%c:%d\r\n", axesID[i], rawdata[i]); gyroscopeReadCooked(&L3GD20D1, cookeddata); for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++) chprintf(chp, "COOKED-%c:%.3f\r\n", axesID[i], cookeddata[i]); gyroscopeGetTemp(&L3GD20D1, &temperature); chprintf(chp, "TEMP:%.1f C°\r\n", temperature); chThdSleepMilliseconds(150); #if CHPRINTF_USE_ANSI_CODE chprintf(chp, "\033[2J\033[1;1H"); #endif } l3gd20Stop(&L3GD20D1); }
static void cmd_read(BaseSequentialStream *chp, int argc, char *argv[]) { (void)argv; if (argc != 2) { chprintf(chp, "Usage: read [acc|gyro|both] [raw|cooked]\r\n"); return; } while (chnGetTimeout((BaseChannel *)chp, 150) == Q_TIMEOUT) { if (!strcmp (argv[0], "acc")) { if (!strcmp (argv[1], "raw")) { #if CHPRINTF_USE_ANSI_CODE chprintf(chp, "\033[2J\033[1;1H"); #endif accelerometerReadRaw(&LSM6DS0D1, rawdata); chprintf(chp, "LSM6DS0 Accelerometer raw data...\r\n"); for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++) { chprintf(chp, "%c-axis: %d\r\n", axisID[i], rawdata[i]); } } else if (!strcmp (argv[1], "cooked")) { #if CHPRINTF_USE_ANSI_CODE chprintf(chp, "\033[2J\033[1;1H"); #endif accelerometerReadCooked(&LSM6DS0D1, cookeddata); chprintf(chp, "LSM6DS0 Accelerometer cooked data...\r\n"); for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++) { chprintf(chp, "%c-axis: %.4f mG\r\n", axisID[i], cookeddata[i]); } } else { chprintf(chp, "Usage: read [acc|gyro|both] [raw|cooked]\r\n"); return; } } else if (!strcmp (argv[0], "gyro")) { if (!strcmp (argv[1], "raw")) { #if CHPRINTF_USE_ANSI_CODE chprintf(chp, "\033[2J\033[1;1H"); #endif gyroscopeReadRaw(&LSM6DS0D1, rawdata); chprintf(chp, "LSM6DS0 Gyroscope raw data...\r\n"); for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++) { chprintf(chp, "%c-axis: %d\r\n", axisID[i], rawdata[i]); } } else if (!strcmp (argv[1], "cooked")) { #if CHPRINTF_USE_ANSI_CODE chprintf(chp, "\033[2J\033[1;1H"); #endif gyroscopeReadCooked(&LSM6DS0D1, cookeddata); chprintf(chp, "LSM6DS0 Gyroscope cooked data...\r\n"); for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++) { chprintf(chp, "%c-axis: %.4f DPS\r\n", axisID[i], cookeddata[i]); } } else { chprintf(chp, "Usage: read [acc|gyro|both] [raw|cooked]\r\n"); return; } } else if (!strcmp (argv[0], "both")) { if (!strcmp (argv[1], "raw")) { #if CHPRINTF_USE_ANSI_CODE chprintf(chp, "\033[2J\033[1;1H"); #endif sensorReadRaw(&LSM6DS0D1, rawdata); chprintf(chp, "LSM6DS0 Accelerometer raw data...\r\n"); for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++) { chprintf(chp, "%c-axis: %d\r\n", axisID[i], rawdata[i]); } chprintf(chp, "LSM6DS0 Gyroscope raw data...\r\n"); for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++) { chprintf(chp, "%c-axis: %d\r\n", axisID[i], rawdata[i + LSM6DS0_ACC_NUMBER_OF_AXES]); } } else if (!strcmp (argv[1], "cooked")) { #if CHPRINTF_USE_ANSI_CODE chprintf(chp, "\033[2J\033[1;1H"); #endif sensorReadCooked(&LSM6DS0D1, cookeddata); chprintf(chp, "LSM6DS0 Accelerometer cooked data...\r\n"); for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++) { chprintf(chp, "%c-axis: %.4f mG\r\n", axisID[i], cookeddata[i]); } chprintf(chp, "LSM6DS0 Gyroscope cooked data...\r\n"); for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++) { chprintf(chp, "%c-axis: %.4f DPS\r\n", axisID[i], cookeddata[i + LSM6DS0_ACC_NUMBER_OF_AXES]); } } else { chprintf(chp, "Usage: read [acc|gyro|both] [raw|cooked]\r\n"); return; } } else { chprintf(chp, "Usage: read [acc|gyro|both] [raw|cooked]\r\n"); return; } } chprintf(chp, "Stopped\r\n"); }