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