int main(void) { InitPeripherals(); // if (InitBatteryMonitorHardware()) { // mBlueON; // } EnablePWM(); mRedON; mGreenOFF; mBlueOFF; mYellowOFF; // hardware devices UsbInterface usb; ZigbeeInterface zig; MPU6050 imu(MPU6050_ADDRESS); // initialize IMU imu.setCommDelay(50); // 50usec delay when trying reads imu.setCommRetries(3); // try at most 3 times before bailing // instantiate the main object Quadrotor quad(QUAD_ID, &imu, &usb, &zig); quad.init(); mRedOFF; // enter main run loop, blocks forever quad.run(); return 0; }
int main(int argc, char **argv) { ros::init (argc, argv, "PhidgetsImu"); ros::NodeHandle nh; ros::NodeHandle nh_private("~"); phidgets::ImuRosI imu(nh, nh_private); ros::spin(); return 0; }
int main(int argc, char** argv) { GetPot clArgs(argc, argv); // We need to create a window for Pangolin to handle our program entry point // in Android. pangolin::CreateWindowAndBind("NodeRelay",1280,800); glDisable(GL_LINE_SMOOTH); glDisable(GL_LIGHTING); ///-------------------- PrintMessage("Setting up publisher....\n"); // set up a publisher unsigned int nPort = clArgs.follow( 5001, "-port"); if( Relay.Publish("IMU", nPort) == false ) { PrintMessage("NodeRelay: Error setting publisher.\n"); } PrintMessage("Setting up control subscription....\n"); Relay.Subscribe("CarControl", clArgs.follow("128.164.202.95:6001","-control" ) ); CommandMsg cMsg; FtdiListener& Listener = FtdiListener::GetInstance(); ///-------------------- #if ANDROID PrintMessage("Setting correct permissions....\n"); system("su -c 'chmod 0777 /dev/ttyUSB0'"); #endif PrintMessage("Initializing IMU....\n"); hal::IMU imu( clArgs.follow("ninja:///dev/ttyUSB0", "-imu") ); PrintMessage("Registering callback....\n"); imu.RegisterIMUDataCallback(IMU_Handler); ///-------------------- for( size_t ii = 0; ; ++ii ) { cMsg.Clear(); Relay.ReadBlocking("CarControl", cMsg); const int accel = int(cMsg.accel()); const int phi = int(cMsg.phi()); PrintMessage("Accel: %3d --- Phi: %3d\r", accel, phi ); fflush(stdout); Listener.SendCommandPacket( phi, accel ); } return 0; }
int main(void) { InitPeripherals(); mRedOFF; mGreenOFF; mBlueOFF; // startup blink const float kShortDelay = 0.1; const float kLongDelay = 0.3; mRedON; mGreenON; mBlueON; DelaySeconds(kShortDelay); mRedOFF; mGreenOFF; mBlueOFF; DelaySeconds(kLongDelay); mRedON; mGreenON; mBlueON; DelaySeconds(kShortDelay); mRedOFF; mGreenOFF; mBlueOFF; DelaySeconds(kLongDelay); mRedON; mGreenON; mBlueON; DelaySeconds(kShortDelay); mRedOFF; mGreenOFF; mBlueOFF; DelaySeconds(kLongDelay); UsbInterface usb = UsbInterface(); usb.Init(); PersistentMemory mem; DelaySeconds(1.0f); ZigbeeInterface zig = ZigbeeInterface(); DelaySeconds(0.05f); zig.Init(mem); error_reporting_com1 = &usb; error_reporting_com2 = &zig; Mpu60XXImu imu(mem); GyroAccelDriftUkfIo est; BatteryMonitor battery(mem); battery_ptr = &battery; MotorHal motor_hal(mem); motor_hal_ptr = &motor_hal; // Get_pos addition: InitPositionSensor(); tim1_set_position_callback_2(get_position); QuatPD pd(mem); PulsingCoaxControllerQuat uav(mem, pd, imu, est, motor_hal); CoaxOpenController open_controller(mem); CoaxOpenAttitudeController open_attitude_controller(mem, est); monitor = LoopMonitor(mem); UavReporter reporter(mem, imu, est, motor_hal, battery, uav, open_attitude_controller, pd, monitor); StateMachine state_machine; mem.Freeze(); // freeze memory to make writes possible imu.InitFromMemory(); //imu.DefaultAccelSensitivity(1); // overwrite scale for off-datasheet fix imu.flip_z = 1; // flip z-axis direction for upside-down imu #ifdef STREAM_IMU_RAW imu_raw_msg_logger_init(imu); #endif monitor.InitFromMemory(); PlayTimebase(); imu.StartRead(); DelayMilliseconds(10); tim1_init(); tim1_set_supply_volts(3.7f); ////////////////////////////////////////////////////////////////////////////// // Main Loop while(1) { monitor.Profile(0); //////////////////////////////////////////////////////////////////////////// // Get IMU Data and Start New Measurement while(!imu.FinishRead()) {}; #ifdef STREAM_IMU_RAW imu_raw_msg_logger_push(); #endif DelayMicroseconds(50); // this seems to be critical (?!?) imu.StartRead(); monitor.Profile(1); //////////////////////////////////////////////////////////////////////////// // Update Estimator with Measurement est.Update(imu.time, imu.w, imu.a); //////////////////////////////////////////////////////////////////////////// // Control // update control laws uav.Update(); open_controller.Update(); open_attitude_controller.Update(); // map controllers to outputs based on state enum control_state state = state_machine.get_state(); // in STOP state, send active kill messages to motors if(state == kStop) { mGreenOFF; mAmberON; motor_hal.set_top_cmd_volts(0); motor_hal.set_top_cmd_volts_pulse_amp(0); motor_hal.set_top_cmd_pulse_phase(0); motor_hal.set_bottom_cmd_volts(0); } // in STANDBY state, send no motor commands except on entry else if(state == kStandby) { mGreenOFF; mAmberOFF; if(state_machine.get_standby_needs_init()) { motor_hal.set_top_cmd_volts(0); motor_hal.set_top_cmd_volts_pulse_amp(0); motor_hal.set_top_cmd_pulse_phase(0); motor_hal.set_bottom_cmd_volts(0); state_machine.clear_standby_needs_init(); } } // in QUAT state, send motor commands according to quat control law else if(state == kQuat) { mGreenON; mAmberOFF; motor_hal.set_top_cmd_volts(uav.top_mean); motor_hal.set_top_cmd_volts_pulse_amp(uav.top_pulse_amp); motor_hal.set_top_cmd_pulse_phase(uav.top_pulse_phase); motor_hal.set_bottom_cmd_volts(uav.bottom_mean); } // in OPEN state, send motor command according to open motor control commands else if(state == kOpen) { mGreenON; mAmberOFF; motor_hal.set_top_cmd_volts(open_controller.top_mean); motor_hal.set_top_cmd_volts_pulse_amp(open_controller.top_pulse_amp); motor_hal.set_top_cmd_pulse_phase(open_controller.top_pulse_phase); motor_hal.set_bottom_cmd_volts(open_controller.bottom_mean); } // in OPEN ATTITUDE state, send motor command according to open motor control commands else if(state == kOpenAttitude) { mGreenON; mAmberOFF; motor_hal.set_top_cmd_volts(open_attitude_controller.top_mean); motor_hal.set_top_cmd_volts_pulse_amp(open_attitude_controller.top_pulse_amp); motor_hal.set_top_cmd_pulse_phase(open_attitude_controller.top_pulse_phase); motor_hal.set_bottom_cmd_volts(open_attitude_controller.bottom_mean); } monitor.Profile(2); //////////////////////////////////////////////////////////////////////////// // Packet Communication uint8_t is_data; // 1 iff data received uint8_t *rx_data; // temporary pointer to received type+data bytes uint8_t rx_length; // number of received type+data bytes //////////////////////////////////////////////////////////////////////////// // USB Input is_data = 0; usb.GetBytes(); while(usb.PeekPacket(&rx_data, &rx_length)) { zig.ReadMsg(usb, rx_data, rx_length); imu.ReadMsg(usb, rx_data, rx_length); est.ReadMsg(usb, rx_data, rx_length); mem.ReadMsg(usb, rx_data, rx_length); uav.ReadMsg(usb, rx_data, rx_length); open_controller.ReadMsg( usb, rx_data, rx_length); open_attitude_controller.ReadMsg( usb, rx_data, rx_length); pd.ReadMsg( usb, rx_data, rx_length); monitor.ReadMsg( usb, rx_data, rx_length); motor_hal.ReadMsg(usb, rx_data, rx_length); battery.ReadMsg( usb, rx_data, rx_length); reporter.ReadMsg( usb, rx_data, rx_length); state_machine.ReadMsg(usb, rx_data, rx_length); usb.DropPacket(); is_data = 1; } // while peek... if(is_data) { usb.SendNow(); } //////////////////////////////////////////////////////////////////////////// // Radio Input is_data = 0; zig.GetBytes(); while(zig.PeekPacket(&rx_data, &rx_length)) { #ifdef STREAM_IMU_RAW if(rx_data[0] == kTypeQuatPilot || rx_data[0] == kTypeQuatFullObsPilot || rx_data[0] == kTypeOpenPilot || rx_data[0] == kTypeOpenAttitudePilot) { imu_raw_msg_logger_send(zig); } #endif zig.ReadMsg(zig, rx_data, rx_length); imu.ReadMsg(zig, rx_data, rx_length); est.ReadMsg(zig, rx_data, rx_length); mem.ReadMsg(zig, rx_data, rx_length); uav.ReadMsg(zig, rx_data, rx_length); open_controller.ReadMsg( zig, rx_data, rx_length); open_attitude_controller.ReadMsg( zig, rx_data, rx_length); pd.ReadMsg( zig, rx_data, rx_length); monitor.ReadMsg( zig, rx_data, rx_length); motor_hal.ReadMsg(zig, rx_data, rx_length); battery.ReadMsg( zig, rx_data, rx_length); reporter.ReadMsg( zig, rx_data, rx_length); state_machine.ReadMsg(zig, rx_data, rx_length); zig.DropPacket(); is_data = 1; } // while peek... monitor.Profile(3); if(is_data) { zig.SendNow(); } monitor.Profile(4); monitor.Profile(5); //////////////////////////////////////////////////////////////////////////// // Throttle main loop to main_freq monitor.EndMainLoop(); monitor.Profile(6); //monitor.SendProfile(usb); //usb.SendNow(); } // while(1) return(0); }
int main(int argc, char **argv) { // enableMuxer(0b00000011); if(argc != 5) { printf("Please supply address for lsm, lis, topic, name\n"); exit(0); } int lsmaddr, lisaddr; lsmaddr = (int) strtol(argv[1], NULL, 16); lisaddr = (int) strtol(argv[2], NULL, 16); ros::init(argc, argv, argv[3]); ros::NodeHandle n; ros::Publisher chatter_pub = n.advertise<sensor_msgs::Imu>(argv[4], 1000); std::string frame = "1"; // Initializa IMU MinIMU9 imu("/dev/i2c-1", lsmaddr, lisaddr); imu.loadCalibration(); imu.enable(); imu.measureOffsets(); int count = 0; ros::Rate loop_rate(100); quaternion rotation = quaternion::Identity(); int start = millis(); while (ros::ok()) { int last_start = start; start = millis(); float dt = (start-last_start)/1000.0; if (dt < 0){ throw std::runtime_error("Time went backwards."); } vector angular_velocity = imu.readGyro(); vector acceleration = imu.readAcc(); vector magnetic_field = imu.readMag(); fuse_default(rotation, dt, angular_velocity, acceleration, magnetic_field); sensor_msgs::Imu msg; msg.header.frame_id = frame; msg.header.stamp = ros::Time::now(); output_quaternion(rotation); std::cout << " " << acceleration << " " << magnetic_field << std::endl << std::flush; write_quaternion(msg, rotation); msg.linear_acceleration.x = acceleration[0]; msg.linear_acceleration.y = acceleration[1]; msg.linear_acceleration.z = acceleration[2]; msg.angular_velocity.x = angular_velocity[0]; msg.angular_velocity.y = angular_velocity[1]; msg.angular_velocity.z = angular_velocity[2]; chatter_pub.publish(msg); // Ensure that each iteration of the loop takes at least 20 ms. /* while(millis() - start < 20) { usleep(1000); }*/ } return 0; }
void IMUThread::mainLoop() { UTimer totalTime; UDEBUG(""); if(rate_>0 || captureDelay_) { double delay = rate_>0?1000.0/double(rate_):1000.0f*captureDelay_; int sleepTime = delay - 1000.0f*frameRateTimer_.getElapsedTime(); if(sleepTime > 2) { uSleep(sleepTime-2); } // Add precision at the cost of a small overhead delay/=1000.0; while(frameRateTimer_.getElapsedTime() < delay-0.000001) { // } frameRateTimer_.start(); } captureDelay_ = 0.0; std::string line; if (std::getline(imuFile_, line)) { std::stringstream stream(line); std::string s; std::getline(stream, s, ','); std::string nanoseconds = s.substr(s.size() - 9, 9); std::string seconds = s.substr(0, s.size() - 9); cv::Vec3d gyr; for (int j = 0; j < 3; ++j) { std::getline(stream, s, ','); gyr[j] = uStr2Double(s); } cv::Vec3d acc; for (int j = 0; j < 3; ++j) { std::getline(stream, s, ','); acc[j] = uStr2Double(s); } double stamp = double(uStr2Int(seconds)) + double(uStr2Int(nanoseconds))*1e-9; if(previousStamp_>0 && stamp > previousStamp_) { captureDelay_ = stamp - previousStamp_; } previousStamp_ = stamp; IMU imu(gyr, cv::Mat(), acc, cv::Mat(), localTransform_); this->post(new IMUEvent(imu, stamp)); } else if(!this->isKilled()) { UWARN("no more imu data..."); this->kill(); this->post(new IMUEvent()); } }
int main(int argc, char **argv) { //Name of node ros::init(argc, argv, "kvh_1750_imu"); //Node handle ros::NodeHandle nh("~"); ros::Publisher imu_pub = nh.advertise<sensor_msgs::Imu>("imu", 1); ros::Publisher temp_pub = nh.advertise<sensor_msgs::Temperature>("temp", 1); std::string imu_link_name = DefaultImuLink; nh.getParam("link_name", imu_link_name); std::string plugin_name = ""; nh.getParam("processor_type", plugin_name); if(!plugin_name.empty()) { pluginlib::ClassLoader<kvh::MessageProcessorBase> plugin_loader("kvh1750", "kvh::KVHMessageProcessorBase"); Plugin = plugin_loader.createInstance(plugin_name); Plugin->set_link_name(imu_link_name); } nh.param("rate", Rate, 100); bool use_delta_angles = true; nh.getParam("use_delta_angles", use_delta_angles); sensor_msgs::Imu current_imu; sensor_msgs::Temperature current_temp; int priority = 99; bool use_rt = true; nh.getParam("priority", priority); nh.getParam("use_rt", use_rt); int policy = (use_rt ? SCHED_RR : SCHED_OTHER); priority = std::min(sched_get_priority_max(policy), std::max(sched_get_priority_min(policy), priority)); struct sched_param params; params.sched_priority = (use_rt ? static_cast<int>(priority) : 0); int rc = sched_setscheduler(0, policy, ¶ms); if(rc != 0) { ROS_ERROR("Setting schedule priority produced error: \"%s\"", strerror(errno)); return 1; } std::vector<double> ahrs_cov; std::vector<double> ang_cov; std::vector<double> lin_cov; nh.param<std::vector<double>>("orientation_covariance", ahrs_cov, {1, 0, 0, 0, 1, 0, 0, 0, 1}); std::copy(ahrs_cov.begin(), ahrs_cov.end(), current_imu.orientation_covariance.begin()); if(nh.getParam("angular_covariance", ang_cov)) { std::copy(ang_cov.begin(), ang_cov.end(), current_imu.angular_velocity_covariance.begin()); } else { current_imu.angular_velocity_covariance[0] = 1; current_imu.angular_velocity_covariance[4] = 1; current_imu.angular_velocity_covariance[8] = 1; } if(nh.getParam("linear_covariance", lin_cov)) { std::copy(lin_cov.begin(), lin_cov.end(), current_imu.linear_acceleration_covariance.begin()); } else { current_imu.linear_acceleration_covariance[0] = 1; current_imu.linear_acceleration_covariance[4] = 1; current_imu.linear_acceleration_covariance[8] = 1; } //IMU link locations current_temp.header.frame_id = imu_link_name; current_imu.header.frame_id = imu_link_name; std::string addr = DefaultAddress; nh.getParam("address", addr); std::string tov_addr = ""; nh.getParam("tov_address", tov_addr); uint32_t baud = 921600; int read_baud; //Because rosparam can't provide unsigned ints nh.getParam("baudrate", read_baud); baud = static_cast<uint32_t>(read_baud); int max_temp = kvh::MaxTemp_C; nh.getParam("max_temp", max_temp); uint32_t wait = 100; std::shared_ptr<kvh::IOModule> mod(new kvh::TOVFile(addr, baud, wait, tov_addr)); kvh::IMU1750 imu(mod); imu.set_temp_limit(max_temp); if(!imu.set_angle_units(use_delta_angles)) { ROS_ERROR("Could not set angle units."); } if(Rate > 0) { if(!imu.set_data_rate(Rate)) { ROS_ERROR("Could not set data rate to %d", Rate); } } imu.query_data_rate(Rate); imu.query_angle_units(IsDA); bool keep_reading = true; while(ros::ok() && keep_reading) { kvh::Message msg; switch(imu.read(msg)) { case kvh::IMU1750::VALID: to_ros(msg, current_imu, current_temp); if(Plugin) { Plugin->process_message(msg); } imu_pub.publish(current_imu); temp_pub.publish(current_temp); break; case kvh::IMU1750::BAD_READ: case kvh::IMU1750::BAD_CRC: ROS_ERROR("Bad data from KVH, ignoring."); break; case kvh::IMU1750::FATAL_ERROR: ROS_FATAL("Lost connection to IMU!"); //should reconnect keep_reading = false; break; case kvh::IMU1750::OVER_TEMP: ROS_FATAL("IMU is overheating!"); keep_reading = false; break; case kvh::IMU1750::PARTIAL_READ: default: break; } ros::spinOnce(); } return 0; }