int main(void) { lcd_init(); ds1307_init(); adc_init(); timer_init(); asm("sei"); /* //test lcd_str("Hello, World"); lcd_gotoxy(1,0); lcd_str("by shtomik"); _delay_ms(1000); lcd_clr(); lcd_str("after clr");*/ start_t(); //test ds1307_setdate(12, 12, 31, 23, 01, 35); //смонтировать microSD res = pf_mount(&fs); if(res == FR_OK) { //диск смонтирован и мы продолжаем работу if(pf_open("test.txt") == FR_OK) { //открыли файл в корне test.txt //... pf_lseek(0); //заполняем буффер, надо набрать 512 байт инфы и писать //иначе будет много лишних пробелов //sprintf(buff, "%s\n%s", "Hello,World!", "by_sht"); //записываем его на карту pf_write(buff, 512, &br); //финализируем запись pf_write(NULL, 0, &br); pf_lseek(512); pf_write(buff, 512, &br); //финализируем запись pf_write(NULL, 0, &br); lcd_clr(); lcd_str("ok"); } else { //file not found or ... lcd_str("file not found"); } //демонтируем диск передав функции нулевой указатель pf_mount(NULL); lcd_clr(); lcd_str("file write"); } else { //не удалось смонтировать диск lcd_clr(); lcd_str("not flash"); } while( 1 ) { ; } return 0; }
bool DynamicTransformationElement::getTransformation(const base::Time& atTime, bool doInterpolation, transformer::TransformationType& result) { if(!gotTransform) { //no sample available, return return false; } if(doInterpolation) { double timeForward = (atTime - lastTransformTime).toSeconds(); if(timeForward < 0) { throw std::runtime_error("Error, time of sample is lower than transformation time"); } if(timeForward == 0) { //transform time is equal to sample time, no interpolation needed result = lastTransform; return true; } std::pair<base::Time, TransformationType> next_sample; if(!aggregator.getNextSample(streamIdx, next_sample)) { //std::cout << "Transformer: could not get next sample" << std::endl; //not enought samples for itnerpolation available return false; } TransformationType interpolated; interpolated.initSane(); double timeBetweenTransforms = (next_sample.first - lastTransformTime).toSeconds(); assert(timeBetweenTransforms > timeForward); double factor = timeForward / timeBetweenTransforms; Eigen::Quaterniond start_r(lastTransform.orientation); Eigen::Quaterniond end_r(next_sample.second.orientation); interpolated.orientation = (start_r.slerp(factor, end_r)); Eigen::Vector3d start_t(lastTransform.position); Eigen::Vector3d end_t(next_sample.second.position); interpolated.position = factor * start_t + (1.0-factor) * end_t; // perform linear interpolation of uncertainties interpolated.cov_position = factor * lastTransform.cov_position + (1.0-factor) * next_sample.second.cov_position; interpolated.cov_orientation = factor * lastTransform.cov_orientation + (1.0-factor) * next_sample.second.cov_orientation; result = interpolated; } else { result = lastTransform; } return true; };