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