int init () {
printf("Opening Voice/Analog Resource: %s ",dxDev);
if ((dx = dx_open(dxDev,0)) == -1) {
printf("failed\n");
return -1;
}
printf("handle: %d\n",dx);
printf("Opening MSI Resource: %s ",msiDev);
if ((msi = ms_open(msiDev,0)) == -1) {
printf("failed\n");
return -1;
}
printf("handle: %d\n",msi);
dxTs.sc_numts = 1;
dxTs.sc_tsarrayp = &dxTsNum;
msiTs.sc_numts = 1;
msiTs.sc_tsarrayp = &msiTsNum;
if (dx_getxmitslot(dx,&dxTs) == -1) {
printf("dx_getxmitslot() failed: %s\n",ATDV_ERRMSGP(dx));
return -1;
}
if (ms_getxmitslot(msi,&msiTs) == -1) {
printf("msi_getxmitslot() failed: %s\n",ATDV_ERRMSGP(msi));
return -1;
}
if (dx_listen(dx,&msiTs) == -1) {
printf("dx_listen() failed: %s\n",ATDV_ERRMSGP(dx));
return -1;
}
if (ms_listen(msi,&dxTs) == -1) {
printf("ms_listen() failed: %s\n",ATDV_ERRMSGP(msi));
return -1;
}
return 0;
}
int main() {
ms_open();
do{
ms_update();
printf("yaw = %2.1f\tpitch = %2.1f\troll = %2.1f\ttemperature = %2.1f\tcompass = %2.1f, %2.1f, %2.1f\n",
ypr[YAW], ypr[PITCH],
ypr[ROLL],temp,compass[0],compass[1],compass[2]);
delay_ms(5);
}while(1);
return 0;
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "mpu9150_driver");
ros::NodeHandle n;
ros::Publisher pub_data = n.advertise<imu_msgs::MPU9150>("mpu9150_data", 10);
imu_msgs::MPU9150 imu;
ros::Rate r(40);
ms_open();
while(ros::ok()){
ms_update();
imu.euler.x = ypr[ROLL];
imu.euler.y = ypr[PITCH];
imu.euler.z = ypr[YAW];
imu.gyro.x = gyro[0];
imu.gyro.y = gyro[1];
imu.gyro.z = gyro[2];
imu.accel.x = accel[2];
imu.accel.y = accel[1];
imu.accel.z = accel[0];
imu.compass.x = compass[0];
imu.compass.y = compass[1];
imu.compass.z = compass[2];
imu.temprature = temp;
//printf("yaw = %2.1f\tpitch = %2.1f\troll = %2.1f\ttemperature = %2.1f\tcompass = %2.1f, %2.1f, %2.1f\n",
// ypr[YAW], ypr[PITCH],
// ypr[ROLL],temp,compass[0],compass[1],compass[2]);
//delay_ms(5);
pub_data.publish(imu);
//r.sleep();
}
return 0;
}
int main() {
struct sched_param param;
signal(SIGTERM, catch_signal);
signal(SIGINT, catch_signal);
param.sched_priority = 49;
if(sched_setscheduler(0, SCHED_FIFO, ¶m) == -1) {
perror("sched_setscheduler failed");
exit(-1);
}
if(mlockall(MCL_CURRENT|MCL_FUTURE) == -1) {
perror("mlockall failed");
exit(-2);
}
stack_prefault();
err=config_open("/var/local/rpicopter.config");
if (err<0) {
printf("Failed to initiate config! [%s]\n", strerror(err));
return -1;
}
err=flog_open("/var/local/");
if (err<0) {
printf("Failed to initiate log! [%s]\n", strerror(err));
return -1;
}
err = sc_open();
if (err != 0) {
printf("Failed to initiate speedcontroller! [%s]\n", strerror(err));
return -1;
}
err=ms_open(GYRO_RATE);
if (err != 0) {
printf("Failed to initiate gyro! [%s]\n", strerror(err));
return -1;
}
err=rec_open();
if (err<0) {
printf("Failed to initiate receiver! [%s]\n", strerror(err));
return -1;
}
/*
err=bs_open();
if (err<0) {
printf("Failed to initiate pressure sensor! [%s]\n", strerror(err));
return -1;
}
*/
printf("Waiting for MPU calibration... \n");
delay_ms(1000);
controller_stable(NULL);
return 0;
}
static void openMouse(void)
{
ms= ms_new();
ms_open(ms, msDev, msProto);
ms_setCallback(ms, enqueueMouseEvent);
}
