bool send_data(vn_100::YPR::Request &req,
vn_100::YPR::Response &res)
{
if(req.ins=="sendYPR")
{
VnYpr ypr;
vn100_getYawPitchRoll(&vn100,&ypr);
res.data[0]=ypr.yaw;
res.data[1]=ypr.pitch;
res.data[2]=ypr.roll;
ROS_INFO("sending response YPR:%f,%f,%f",res.data[0],res.data[1],res.data[2]);
}
return true;
}
void FanControl::controlMotors() {
while(1)
{
m_now = chrono::high_resolution_clock::now();
// Time difference in Sec
//float timeDiff = (chrono::duration_cast<chrono::milliseconds>(m_now-m_lastTime).count())/1000.0f;
if (vn100_getYawPitchRoll(&vn100, &ypr) != 0) {
writeLog("VN100 Error occured");
break;
}
//setSampleRate(timeDiff);
computePID();
setMotorSpeed();
writeLog();
//cout << "Time Diff: " << timeDiff << " s" << endl;
m_lastTime = m_now;
if ( running == false ) break;
}
}
int main(int argc, char ** argv)
{
lcm::LCM lcm("udpm://224.0.0.251:7667?ttl=1");
if(!lcm.good())
return 1;
exlcm::imu1 imu_data;
time_t sta,stp;
time(&sta);
VN_ERROR_CODE errorCode;
Vn100 vn100;
VnYpr ypr;
VnVector3 accel;
VnVector3 angRate;
long i;
errorCode = vn100_connect(
&vn100,
COM_PORT,
BAUD_RATE);
/* Make sure the user has permission to use the COM port. */
if (errorCode == VNERR_PERMISSION_DENIED) {
printf("Current user does not have permission to open the COM port.\n");
printf("Try running again using 'sudo'.\n");
return 0;
}
else if (errorCode != VNERR_NO_ERROR)
{
printf("Error encountered when trying to connect to the sensor.\n");
return 0;
}
for (i = 1; i > 0; i++)
{
/* Query the YawPitchRoll register of the VN-100. Note this method of
retrieving the attitude is blocking since a serial command will be
sent to the physical sensor and this program will wait until a
response is received. */
errorCode = vn100_getYawPitchRoll(&vn100, &ypr);
errorCode = vn100_getAcceleration(&vn100, &accel);
errorCode = vn100_getAngularRate(&vn100, &angRate);
imu_data.timestamp = i;
imu_data.name = "IMU Data";
imu_data.orientation[0] = ypr.yaw;
imu_data.orientation[1] = ypr.pitch;
imu_data.orientation[2] = ypr.roll;
imu_data.linAcc[0] = accel.c0;
imu_data.linAcc[1] = accel.c1;
imu_data.linAcc[2] = accel.c2;
imu_data.angAcc[0] = angRate.c0;
imu_data.angAcc[1] = angRate.c1;
imu_data.angAcc[2] = angRate.c2;
imu_data.enabled = true;
lcm.publish("IMU_DATA", &imu_data);
//printf(" %+#7.2f %+#7.2f %+#7.2f\t\t", ypr.yaw, ypr.pitch, ypr.roll);
//printf(" %+#7.2f %+#7.2f %+#7.2f\t\t", accel.c0, accel.c1, accel.c2);
//printf(" %+#7.2f %+#7.2f %+#7.2f\n", angRate.c0, angRate.c1, angRate.c2);
/* Wait for 1 second before we query the sensor again. */
sleep(0.5);
}
time(&stp);
printf("Duration = %f\n",difftime(stp,sta));
errorCode = vn100_disconnect(&vn100);
if (errorCode != VNERR_NO_ERROR)
{
printf("Error encountered when trying to disconnect from the sensor.\n");
return 0;
}
return 0;
}
void publish_device()
{
static int seq=0;
seq++;
VN_ERROR_CODE vn_error;
std::string vn_err_msg;
ros::Time timestamp=ros::Time::now();
if(pubsens_data.getNumSubscribers()>0)
{
ROS_INFO("subscribed");
VnVector3 Gyro,Mag,Accel;
float Pressure,Temp;
vn_error=vn100_getCalibratedSensorMeasurements(&vn100,&Mag,&Accel,&Gyro,&Temp,&Pressure);
if(vn_error!=VNERR_NO_ERROR)
{
vnerror_msg(vn_error,vn_err_msg);
ROS_INFO("error in reading the sensor data:%s",vn_err_msg.c_str());
}
else
{
vn_100::sensor_data msg_sensor_data;
msg_sensor_data.header.seq=seq;
msg_sensor_data.header.stamp=timestamp;
msg_sensor_data.header.frame_id=imu_frame_id;
msg_sensor_data.Mag.x=Mag.c0;
msg_sensor_data.Mag.y=Mag.c1;
msg_sensor_data.Mag.z=Mag.c2;
msg_sensor_data.Accel.x=Accel.c0;
msg_sensor_data.Accel.y=Accel.c1;
msg_sensor_data.Accel.z=Accel.c2;
msg_sensor_data.Gyro.x=Gyro.c0;
msg_sensor_data.Gyro.y=Gyro.c1;
msg_sensor_data.Gyro.z=Gyro.c2;
msg_sensor_data.Temp=Temp;
msg_sensor_data.Pressure=Pressure;
pubsens_data.publish(msg_sensor_data);
}
}
if(pubins_data.getNumSubscribers()>0)
{
VnYpr YPR;
VnQuaternion quat_data;
vn_100::ins_data msg_ins_data;
vn_error=vn100_getYawPitchRoll(&vn100,&YPR);
if(vn_error!=VNERR_NO_ERROR)
{
vnerror_msg(vn_error,vn_err_msg);
ROS_INFO("error in reading the ins data:%s",vn_err_msg.c_str());
}
else
{
vn_error=vn100_getQuaternion(&vn100,&quat_data);
if(vn_error!=VNERR_NO_ERROR)
{
vnerror_msg(vn_error,vn_err_msg);
ROS_INFO("error in reading the ins data:%s",vn_err_msg.c_str());
}
else
{
msg_ins_data.header.seq=seq;
msg_ins_data.header.stamp=timestamp;
msg_ins_data.header.frame_id=imu_frame_id;
msg_ins_data.YPR.x=YPR.yaw;
msg_ins_data.YPR.y=YPR.pitch;
msg_ins_data.YPR.z=YPR.roll;
msg_ins_data.quat_data[0]=quat_data.x;
msg_ins_data.quat_data[1]=quat_data.y;
msg_ins_data.quat_data[2]=quat_data.z;
msg_ins_data.quat_data[3]=quat_data.w;
pubins_data.publish(msg_ins_data);
}
}
}
}
