void testbench::capture_output( ac_int<8, true > *output)
{
if (_capture_output)
{
int cur_iter=output_iteration_count;
++output_iteration_count;
mc_golden_info< ac_int<8, true >, ac_int<8, true > > output_tmp((*output), output_ignore, ~0, false, output_iteration_count);
// BEGIN: testbench output_mask control for field_name output
if ( output_use_mask ) {
output_tmp._use_mask = true;
output_tmp._mask = output_output_mask ;
}
// END: testbench output_mask control for field_name output
if (!output_skip) {
output_golden.put(output_tmp);
++output_capture_count;
} else {
std::ostringstream msg; msg.str("");
msg << "output_skip=true for iteration=" << output_iteration_count << " @ " << sc_time_stamp();
SC_REPORT_WARNING("User testbench", msg.str().c_str());
}
mc_testbench_process_wait_ctrl("output",output_wait_cycles,output_wait_ctrl,ccs_wait_ctrl_output.operator->(),cur_iter,output_capture_count);
output_ignore = false;
output_use_mask = false;
}
output_skip = false;
}
void linear_regression(NeuralNetwork& nn, train_set& t)
{
if(nn.net.size() != 1) return;
if(t.input.size() != t.output.size())
throw "Training set misaligned!";
values_t output_tmp(nn.num_output,0);
for(int k=0; k<t.iterations; k++){
for(int set=0; set<t.input.size(); set++){
nn.process(t.input[set], output_tmp);
//NP Buch S.266
for(int i=0; i<nn.num_output; i++){
nn.net[0][i].connections += (t.learn_rate/abs(t.input[set])) * (t.output[set][i] - output_tmp[i])*t.input[set];
}
}
}
}
// filter loop
void OdomEstimationNode::spin(const ros::TimerEvent& e)
{
ROS_DEBUG("Spin function at time %f", ros::Time::now().toSec());
// check for timing problems
if ( (vo_active_ || vo_initializing_ ) && (imu_active_ || imu_initializing_ ) )
{
//ROS_INFO("vo time: %f \n imu time: %f", vo_stamp_.toSec(), imu_stamp_.toSec());
double diff = fabs( (vo_stamp_ - imu_stamp_).toSec() );
if (diff > 1.0) ROS_ERROR("Timestamps of vo and imu are %f seconds apart.", diff);
}
// initial value for filter stamp; keep this stamp when no sensors are active
filter_stamp_ = Time::now();
// check which sensors are still active
//ROS_INFO("active time: %f ",Duration(Time::now() - imu_stamp_).toSec());
if (imu_initializing_ && Duration(Time::now() - imu_stamp_).toSec() > timeout_)
{
imu_active_ = false; imu_initializing_ = false;
ROS_ERROR("Imu sensor not active any more");
}
if (vo_initializing_ && Duration(Time::now() - vo_stamp_).toSec() > timeout_)
{
vo_active_ = false; vo_initializing_ = false;
ROS_ERROR("VO sensor not active any more");
}
// only update filter when one of the sensors is active
if (imu_active_ || vo_active_ )
{
// update filter at time where all sensor measurements are available
if (imu_active_) filter_stamp_ = min(filter_stamp_, imu_stamp_);
if (vo_active_) filter_stamp_ = min(filter_stamp_, vo_stamp_);
//ROS_INFO("filter_stamp_ time: %f ", filter_stamp_.toSec());
}
// initialize filer with odometry frame
if (imu_active_ && vo_active_ && !my_filter_.isInitialized())
{
my_filter_.initialize(filter_stamp_);
if (my_filter_.isInitialized())
{
ROS_INFO("Kalman filter initialized! \n Frequency is : %f Hz\n", freq);
}
imu_active_ = false;
vo_active_ = false;
}
// update filter
if( !my_filter_.isInitialized() || !imu_initializing_ || !vo_initializing_ )
{
output_.header.stamp = ros::Time::now();
output_.isctrl = 0;
pose_pub_.publish(output_);
}
else if ( my_filter_.isInitialized() )
{
if (my_filter_.update(imu_active_, vo_active_, filter_stamp_ ))
{
imu_active_ = false;
vo_active_ = false;
// output most recent estimate and relative covariance
if(my_filter_.getEstimate(output_tmp))
{
// tmp -> msg
output_.header.stamp = ros::Time::now();
output_.xyz.x = output_tmp(0);
output_.xyz.y = output_tmp(1);
output_.xyz.z = output_tmp(2);
//output_.xyz.z = imu_height;
output_.q.x = output_tmp(4);
output_.q.y = output_tmp(5);
output_.q.z = output_tmp(6);
output_.q.w = output_tmp(3);
output_.v.x = output_tmp(7);
output_.v.y = output_tmp(8);
output_.v.z = output_tmp(9);
output_.w.x = output_tmp(10);
output_.w.y = output_tmp(11);
output_.w.z = output_tmp(12);
output_.a.x = output_tmp(13);
output_.a.y = output_tmp(14);
output_.a.z = output_tmp(15);
output_.dt = output_tmp(16);
output_.isctrl = 1;
//std::cout<<"ukf_sent_counter_:"<<ukf_sent_counter_<<std::endl;
/*
cout << "w_b_x: "<< output_.w.x/M_PI*180.0f<<endl
<< "w_b_y: "<< output_.w.y/M_PI*180.0f<<endl
<< "w_b_z: "<< output_.w.z/M_PI*180.0f<<endl<<endl;
*/
//debug yaw roll pitch
/*
vector4f q_debug;
q_debug[0] = output_.q.w;
q_debug[1] = output_.q.x;
q_debug[2] = output_.q.y;
q_debug[3] = output_.q.z;
float yaw,pitch,roll;
quat_to_eular(&yaw, &pitch, &roll, q_debug);
cout<< "yaw : "<<yaw<<endl
<< "pitch: "<<pitch<<endl
<< "roll : "<<roll<<endl<<endl;
*/
pose_pub_.publish(output_);
ukf_sent_counter_++;
}
}
}
};
