// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- // // test harness for vibration testing // #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 #include #include #include #include #include #include #include const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER; static int accel_fd[INS_MAX_INSTANCES]; static int gyro_fd[INS_MAX_INSTANCES]; static uint32_t total_samples[INS_MAX_INSTANCES]; static uint64_t last_accel_timestamp[INS_MAX_INSTANCES]; static uint64_t last_gyro_timestamp[INS_MAX_INSTANCES]; static uint32_t accel_deltat_min[INS_MAX_INSTANCES]; static uint32_t accel_deltat_max[INS_MAX_INSTANCES]; static uint32_t gyro_deltat_min[INS_MAX_INSTANCES]; static uint32_t gyro_deltat_max[INS_MAX_INSTANCES]; static DataFlash_File DataFlash("/fs/microsd/VIBTEST"); static const struct LogStructure log_structure[] PROGMEM = { LOG_COMMON_STRUCTURES, LOG_EXTRA_STRUCTURES }; void setup(void) { for (uint8_t i=0; ipanic("Failed to open accel/gyro 0"); } ioctl(gyro_fd[0], SENSORIOCSPOLLRATE, 1000); ioctl(gyro_fd[0], GYROIOCSLOWPASS, 0); ioctl(gyro_fd[0], GYROIOCSHWLOWPASS, 256); ioctl(gyro_fd[0], GYROIOCSSAMPLERATE, 1000); ioctl(gyro_fd[0], SENSORIOCSQUEUEDEPTH, 100); ioctl(gyro_fd[1], SENSORIOCSPOLLRATE, 800); ioctl(gyro_fd[1], GYROIOCSLOWPASS, 0); ioctl(gyro_fd[1], GYROIOCSHWLOWPASS, 100); ioctl(gyro_fd[1], GYROIOCSSAMPLERATE, 800); ioctl(gyro_fd[1], SENSORIOCSQUEUEDEPTH, 100); ioctl(accel_fd[0], SENSORIOCSPOLLRATE, 1000); ioctl(accel_fd[0], ACCELIOCSLOWPASS, 0); ioctl(accel_fd[0], ACCELIOCSRANGE, 16); ioctl(accel_fd[0], ACCELIOCSHWLOWPASS, 256); ioctl(accel_fd[0], ACCELIOCSSAMPLERATE, 1000); ioctl(accel_fd[0], SENSORIOCSQUEUEDEPTH, 100); ioctl(accel_fd[1], SENSORIOCSPOLLRATE, 1600); ioctl(accel_fd[1], ACCELIOCSLOWPASS, 0); ioctl(accel_fd[1], ACCELIOCSRANGE, 16); ioctl(accel_fd[1], ACCELIOCSHWLOWPASS, 194); ioctl(accel_fd[1], ACCELIOCSSAMPLERATE, 1600); ioctl(accel_fd[1], SENSORIOCSQUEUEDEPTH, 100); DataFlash.Init(log_structure, sizeof(log_structure)/sizeof(log_structure[0])); DataFlash.StartNewLog(); } void loop(void) { bool got_sample = false; static uint32_t last_print; do { got_sample = false; for (uint8_t i=0; i accel_deltat_max[i]) { accel_deltat_max[i] = deltat; } if (accel_deltat_min[i] == 0 || deltat < accel_deltat_max[i]) { accel_deltat_min[i] = deltat; } last_accel_timestamp[i] = accel_report.timestamp; struct log_ACCEL pkt = { LOG_PACKET_HEADER_INIT((uint8_t)(LOG_ACC1_MSG+i)), time_us : accel_report.timestamp, AccX : accel_report.x, AccY : accel_report.y, AccZ : accel_report.z }; DataFlash.WriteBlock(&pkt, sizeof(pkt)); got_sample = true; total_samples[i]++; } if (gyro_fd[i] != -1 && ::read(gyro_fd[i], &gyro_report, sizeof(gyro_report)) == sizeof(gyro_report) && gyro_report.timestamp != last_gyro_timestamp[i]) { uint32_t deltat = gyro_report.timestamp - last_gyro_timestamp[i]; if (deltat > gyro_deltat_max[i]) { gyro_deltat_max[i] = deltat; } if (gyro_deltat_min[i] == 0 || deltat < gyro_deltat_max[i]) { gyro_deltat_min[i] = deltat; } last_gyro_timestamp[i] = gyro_report.timestamp; struct log_GYRO pkt = { LOG_PACKET_HEADER_INIT((uint8_t)(LOG_GYR1_MSG+i)), time_us : gyro_report.timestamp, GyrX : gyro_report.x, GyrY : gyro_report.y, GyrZ : gyro_report.z }; DataFlash.WriteBlock(&pkt, sizeof(pkt)); got_sample = true; total_samples[i]++; } } if (got_sample) { if (total_samples[0] % 2000 == 0 && last_print != total_samples[0]) { last_print = total_samples[0]; hal.console->printf("t=%lu total_samples=%lu/%lu/%lu adt=%u:%u/%u:%u/%u:%u gdt=%u:%u/%u:%u/%u:%u\n", (unsigned long)hal.scheduler->millis(), (unsigned long)total_samples[0], (unsigned long)total_samples[1], (unsigned long)total_samples[2], accel_deltat_min[0], accel_deltat_max[0], accel_deltat_min[1], accel_deltat_max[1], accel_deltat_min[2], accel_deltat_max[2], gyro_deltat_min[0], gyro_deltat_max[0], gyro_deltat_min[1], gyro_deltat_max[1], gyro_deltat_min[2], gyro_deltat_max[2]); #if 0 ::printf("t=%lu total_samples=%lu/%lu/%lu adt=%u:%u/%u:%u/%u:%u gdt=%u:%u/%u:%u/%u:%u\n", hal.scheduler->millis(), total_samples[0], total_samples[1],total_samples[2], accel_deltat_min[0], accel_deltat_max[0], accel_deltat_min[1], accel_deltat_max[1], accel_deltat_min[2], accel_deltat_max[2], gyro_deltat_min[0], gyro_deltat_max[0], gyro_deltat_min[1], gyro_deltat_max[1], gyro_deltat_min[2], gyro_deltat_max[2]); #endif memset(accel_deltat_min, 0, sizeof(accel_deltat_min)); memset(accel_deltat_max, 0, sizeof(accel_deltat_max)); memset(gyro_deltat_min, 0, sizeof(gyro_deltat_min)); memset(gyro_deltat_max, 0, sizeof(gyro_deltat_max)); } } } while (got_sample); hal.scheduler->delay_microseconds(100); } #else const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER; void setup() {} void loop() {} #endif // CONFIG_HAL_BOARD AP_HAL_MAIN();