void Navi6dWrapper::stop_time_measurement(float dT) { chTMStopMeasurementX(&tmeas); time_meas_decimator += dT; if (tmeas.last / float(STM32_SYSCLK) > dT) { time_overrun_cnt++; if (time_meas_decimator > 0.25) { time_meas_decimator = 0; mavlink_dbg_print(MAV_SEVERITY_CRITICAL, "SINS time overrun!", MAV_COMP_ID_ALL); } } }
/** * @brief Initializes the time measurement unit. * * @init */ void _tm_init(void) { time_measurement_t tm; /* Time Measurement subsystem calibration, it does a null measurement and calculates the call overhead which is subtracted to real measurements.*/ ch.tm.offset = 0; chTMObjectInit(&tm); chTMStartMeasurementX(&tm); chTMStopMeasurementX(&tm); ch.tm.offset = tm.last; }
void membench_run(membench_t *dest, const membench_t *src, membench_result_t *result) { time_measurement_t mem_tmu; size_t len; if (src->size < dest->size) len = src->size; else len = dest->size; /* memset */ chTMObjectInit(&mem_tmu); chTMStartMeasurementX(&mem_tmu); memset(dest->start, 0x55, dest->size); chTMStopMeasurementX(&mem_tmu); result->memset = speed_bps(&mem_tmu, dest->size); /* memcpy */ chTMObjectInit(&mem_tmu); chTMStartMeasurementX(&mem_tmu); memcpy(dest->start, src->start, len); chTMStopMeasurementX(&mem_tmu); result->memcpy = speed_bps(&mem_tmu, len); /* memcmp */ chTMObjectInit(&mem_tmu); chTMStartMeasurementX(&mem_tmu); warning_suppressor = memcmp(dest->start, src->start, len); chTMStopMeasurementX(&mem_tmu); result->memcmp = speed_bps(&mem_tmu, len); /* memcpy DMA */ memcpy_dma_start(); chTMObjectInit(&mem_tmu); chTMStartMeasurementX(&mem_tmu); memcpy_dma(dest->start, src->start, len); chTMStopMeasurementX(&mem_tmu); result->memcpy_dma = speed_bps(&mem_tmu, len); memcpy_dma_stop(); }
/* * Application entry point. */ int main(void) { /* performance counters */ int32_t adc_ints = 0; int32_t spi_ints = 0; int32_t uart_ints = 0; int32_t adc_idle_ints = 0; int32_t spi_idle_ints = 0; int32_t uart_idle_ints = 0; uint32_t background_cnt = 0; systime_t T = 0; /* * 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(); #if STM32_NAND_USE_EXT_INT extStart(&EXTD1, &extcfg); #endif chTMObjectInit(&tmu_driver_start); chTMStartMeasurementX(&tmu_driver_start); #if USE_BAD_MAP nandStart(&NAND, &nandcfg, &badblock_map); #else nandStart(&NAND, &nandcfg, NULL); #endif chTMStopMeasurementX(&tmu_driver_start); chThdSleepMilliseconds(4000); chThdCreateStatic(BackgroundThreadWA, sizeof(BackgroundThreadWA), NORMALPRIO - 20, BackgroundThread, NULL); nand_wp_release(); /* * run NAND test in parallel with DMA load and background thread */ dma_storm_adc_start(); dma_storm_uart_start(); dma_storm_spi_start(); T = chVTGetSystemTimeX(); general_test(&NAND, NAND_TEST_START_BLOCK, NAND_TEST_END_BLOCK, 1); T = chVTGetSystemTimeX() - T; adc_ints = dma_storm_adc_stop(); uart_ints = dma_storm_uart_stop(); spi_ints = dma_storm_spi_stop(); chSysLock(); background_cnt = BackgroundThdCnt; BackgroundThdCnt = 0; chSysUnlock(); /* * run DMA load and background thread _without_ NAND test */ dma_storm_adc_start(); dma_storm_uart_start(); dma_storm_spi_start(); chThdSleep(T); adc_idle_ints = dma_storm_adc_stop(); uart_idle_ints = dma_storm_uart_stop(); spi_idle_ints = dma_storm_spi_stop(); /* * ensure that NAND code have negligible impact on other subsystems */ osalDbgCheck(background_cnt > (BackgroundThdCnt / 4)); osalDbgCheck(abs(adc_ints - adc_idle_ints) < (adc_idle_ints / 20)); osalDbgCheck(abs(uart_ints - uart_idle_ints) < (uart_idle_ints / 20)); osalDbgCheck(abs(spi_ints - spi_idle_ints) < (spi_idle_ints / 10)); /* * perform ECC calculation test */ ecc_test(&NAND, NAND_TEST_END_BLOCK); #if USE_KILL_BLOCK_TEST kill_block(&NAND, NAND_TEST_KILL_BLOCK); #endif nand_wp_assert(); /* * Normal main() thread activity, in this demo it does nothing. */ while (true) { chThdSleepMilliseconds(500); } }
static void general_test (NANDDriver *nandp, size_t first, size_t last, size_t read_rounds){ size_t block, page, round; bool status; uint8_t op_status; uint32_t recc, wecc; red_led_on(); /* initialize time measurement units */ chTMObjectInit(&tmu_erase); chTMObjectInit(&tmu_write_data); chTMObjectInit(&tmu_write_spare); chTMObjectInit(&tmu_read_data); chTMObjectInit(&tmu_read_spare); /* perform basic checks */ for (block=first; block<last; block++){ if (!nandIsBad(nandp, block)){ if (!is_erased(nandp, block)){ op_status = nandErase(nandp, block); osalDbgCheck(0 == (op_status & 1)); /* operation failed */ } } } /* write block with pattern, read it back and compare */ for (block=first; block<last; block++){ if (!nandIsBad(nandp, block)){ for (page=0; page<nandp->config->pages_per_block; page++){ pattern_fill(); chTMStartMeasurementX(&tmu_write_data); op_status = nandWritePageData(nandp, block, page, nand_buf, nandp->config->page_data_size, &wecc); chTMStopMeasurementX(&tmu_write_data); osalDbgCheck(0 == (op_status & 1)); /* operation failed */ chTMStartMeasurementX(&tmu_write_spare); op_status = nandWritePageSpare(nandp, block, page, nand_buf + nandp->config->page_data_size, nandp->config->page_spare_size); chTMStopMeasurementX(&tmu_write_spare); osalDbgCheck(0 == (op_status & 1)); /* operation failed */ /* read back and compare */ for (round=0; round<read_rounds; round++){ memset(nand_buf, 0, NAND_PAGE_SIZE); chTMStartMeasurementX(&tmu_read_data); nandReadPageData(nandp, block, page, nand_buf, nandp->config->page_data_size, &recc); chTMStopMeasurementX(&tmu_read_data); osalDbgCheck(0 == (recc ^ wecc)); /* ECC error detected */ chTMStartMeasurementX(&tmu_read_spare); nandReadPageSpare(nandp, block, page, nand_buf + nandp->config->page_data_size, nandp->config->page_spare_size); chTMStopMeasurementX(&tmu_read_spare); osalDbgCheck(0 == memcmp(ref_buf, nand_buf, NAND_PAGE_SIZE)); /* Read back failed */ } } /* make clean */ chTMStartMeasurementX(&tmu_erase); op_status = nandErase(nandp, block); chTMStopMeasurementX(&tmu_erase); osalDbgCheck(0 == (op_status & 1)); /* operation failed */ status = is_erased(nandp, block); osalDbgCheck(true == status); /* blocks was not erased successfully */ }/* if (!nandIsBad(nandp, block)){ */ } red_led_off(); }
/** * @brief Stops the measurement of an ISR critical zone. */ void _stats_stop_measure_crit_isr(void) { chTMStopMeasurementX(&ch.kernel_stats.m_crit_isr); }
bool GetMPUData(void) { #if TIME_METER { extern time_measurement_t FilterTime; chTMStartMeasurementX(&FilterTime); } #endif ReadGyroData(); #if TIME_METER { extern time_measurement_t FilterTime; chTMStopMeasurementX(&FilterTime); } #endif #if TIME_METER { extern time_measurement_t IntTime; chSysLock(); chTMStopMeasurementX(&IntTime); chSysUnlock(); } #endif RawGyroData.roll=(int16_t) (GyroReadDataBuffer[IDX_GYRO_XOUT_H]<<8 |GyroReadDataBuffer[IDX_GYRO_XOUT_L]); RawGyroData.roll/= GYRO_SCALE_INT; RawGyroData.pitch=(int16_t) (GyroReadDataBuffer[IDX_GYRO_YOUT_H]<<8 |GyroReadDataBuffer[IDX_GYRO_YOUT_L]); RawGyroData.pitch/= (-1*GYRO_SCALE_INT); RawGyroData.yaw=(int16_t) (GyroReadDataBuffer[IDX_GYRO_ZOUT_H]<<8 |GyroReadDataBuffer[IDX_GYRO_ZOUT_L]); RawGyroData.yaw/= GYRO_SCALE_INT; #if 0//TIME_METER { extern time_measurement_t FilterTime; chTMStartMeasurementX(&FilterTime); } #endif GyroData[ROLL]=Filter2ndOrder(RawGyroData.roll, &(GyroFilterIns[0]), &(GyroFilterOuts[0]), &(Butter2_16_ACoeffs[0]), &(Butter2_16_BCoeffs[0])); GyroData[PITCH]=Filter2ndOrder(RawGyroData.pitch, &(GyroFilterIns[2]), &(GyroFilterOuts[2]), &(Butter2_16_ACoeffs[0]), &(Butter2_16_BCoeffs[0])); GyroData[YAW]=Filter2ndOrder(RawGyroData.yaw, &(GyroFilterIns[4]), &(GyroFilterOuts[4]), &(Butter2_16_ACoeffs[0]), &(Butter2_16_BCoeffs[0])); #if 0//TIME_METER { extern time_measurement_t FilterTime; chTMStopMeasurementX(&FilterTime); } #endif if (CalibrationCycles==0) { GyroData[ROLL]-=GyroBias[ROLL]; GyroData[PITCH]-=GyroBias[PITCH]; GyroData[YAW]-=GyroBias[YAW]; } else if (CalibrationCycles==CALIB_CYCLES) { TURN_LED_ON(); GyroBias[ROLL]=0; GyroBias[PITCH]=0; GyroBias[YAW]=0; CalibrationCycles--; } else // (CalibrationCycles > 0) { GyroBias[ROLL]+=GyroData[ROLL]; GyroBias[PITCH]+=GyroData[PITCH]; GyroBias[YAW]+=GyroData[YAW]; if (CalibrationCycles==1) { TURN_LED_OFF(); GyroBias[ROLL]/= CALIB_CYCLES; GyroBias[PITCH]/= CALIB_CYCLES; GyroBias[YAW]/= CALIB_CYCLES; } GyroData[ROLL]=0; GyroData[PITCH]=0; GyroData[YAW]=0; CalibrationCycles--; } SampleCounter++; #if 0 //Used to log gyro data only { extern void AddGyroEntry(void); extern volatile bool Armed; if (Armed) AddGyroEntry(); } #endif if ((SampleCounter%SAMPLES_PER_LOOP)==0) return TRUE; else return FALSE; }