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