void read_loop(unsigned int sample_rate)
{
unsigned long loop_delay;
mpudata_t mpu; /*
typedef struct {
short rawGyro[3];
short rawAccel[3];
long rawQuat[4];
unsigned long dmpTimestamp;
short rawMag[3];
unsigned long magTimestamp;
short Temp[3];
short calibratedAccel[3];
short calibratedMag[3];
quaternion_t fusedQuat;
vector3d_t fusedEuler;
float lastDMPYaw;
float lastYaw;
} mpudata_t;*/
memset(&mpu, 0, sizeof(mpudata_t));
if (sample_rate == 0)
return;
loop_delay = (1000 / sample_rate) - 2;
printf("\nEntering read loop (ctrl-c to exit)\n\n");
linux_delay_ms(loop_delay);
while (!done) {
while(msg_cnt<MPU_MSG_NUM && !done){
if (mpu9150_read(&mpu) == 0) {
mpu_add_msg(&mpu);
// print_fused_euler_angles(&mpu);
print_fused_quaternions(&mpu);
// print_calibrated_accel(&mpu);
// print_calibrated_mag(&mpu);
}
linux_delay_ms(loop_delay);
}
msg_cnt=0;
publish(mpu_msg);
//publish(msg);
}
printf("\n\n");
}
int read_mpu(float *pitch, float *roll, float *heading)
{
int ret;
static mpudata_t mpu;
memset(&mpu, 0, sizeof(mpudata_t));
ret = mpu9150_read(&mpu);
*pitch = mpu.fusedEuler[0];
*roll = mpu.fusedEuler[1];
*heading = mpu.fusedEuler[2];
return ret;
}
int read_mpu_raw(float *gx, float *gy, float *gz, float *ax, float *ay, float *az, float *mx, float *my, float *mz)
{
int ret;
static mpudata_t mpu;
memset(&mpu, 0, sizeof(mpudata_t));
ret = mpu9150_read(&mpu);
*gx = mpu.rawGyro[0];
*gy = mpu.rawGyro[1];
*gz = mpu.rawGyro[2];
*ax = mpu.rawAccel[0];
*ay = mpu.rawAccel[1];
*az = mpu.rawAccel[2];
*mx = mpu.rawMag[0];
*my = mpu.rawMag[1];
*mz = mpu.rawMag[2];
return ret;
}
/************************************************************************
* flight_core()
* Hardware Interrupt-Driven Flight Control Loop
* - read sensor values
* - estimate system state
* - read setpoint from flight_stack
* - if is position mode, calculate a new attitude setpoint
* - otherwise use user attitude setpoint
* - calculate and send ESC commands
************************************************************************/
int flight_core(){
// remember previous arm mode to detect transition from DISARMED
static arm_state_t previous_arm_state;
int i; // general purpose
static float new_esc[8];
static float u[4]; // normalized roll, pitch, yaw, throttle, components
/************************************************************************
* Begin control loop if there was a valid interrupt with new IMU data
************************************************************************/
if (mpu9150_read(&mpu) != 0){
return -1;
}
/***********************************************************************
* STATE_ESTIMATION
* read sensors and compute the state regardless of if the controller
* is ARMED or DISARMED
************************************************************************/
// collect new IMU roll/pitch data
// positive roll right according to right hand rule
// MPU9150 driver has incorrect minus sign on Y axis, correct for it here
// positive pitch backwards according to right hand rule
cstate.roll = -(mpu.fusedEuler[VEC3_Y] - cstate.imu_roll_err);
cstate.pitch = mpu.fusedEuler[VEC3_X] - cstate.imu_pitch_err;
// current roll/pitch/yaw rates straight from gyro
// converted to rad/s with default FUll scale range
// raw gyro matches sign on MPU9150 coordinate system, unlike Euler angle
cstate.dRoll = mpu.rawGyro[VEC3_Y] * GYRO_FSR * DEGREE_TO_RAD / 32767.0;
cstate.dPitch = mpu.rawGyro[VEC3_X] * GYRO_FSR * DEGREE_TO_RAD / 32767.0;
cstate.dYaw = mpu.rawGyro[VEC3_Z] * GYRO_FSR * DEGREE_TO_RAD / 32767.0;
// if this is the first loop since being armed, reset yaw trim
if(previous_arm_state==DISARMED && setpoint.arm_state!=DISARMED){
cstate.imu_yaw_on_takeoff = mpu.fusedEuler[VEC3_Z];
cstate.num_yaw_spins = 0;
}
float new_yaw = -(mpu.fusedEuler[VEC3_Z] - cstate.imu_yaw_on_takeoff) \
+ (cstate.num_yaw_spins*2*PI);
// detect the crossover point at Z = +-PI
if(new_yaw - cstate.last_yaw > 6){
cstate.num_yaw_spins -= 1;
}
else if(new_yaw - cstate.last_yaw < -6){
cstate.num_yaw_spins += 1;
}
// also reset yaw if the throttle stick is down to prevent drift while on the ground
if (user_interface.throttle_stick < -0.95){
cstate.imu_yaw_on_takeoff = mpu.fusedEuler[VEC3_Z];
cstate.num_yaw_spins=0;
}
// record new yaw compensating for full rotation
cstate.last_yaw = cstate.yaw;
cstate.yaw = -(mpu.fusedEuler[VEC3_Z] - cstate.imu_yaw_on_takeoff) +
(cstate.num_yaw_spins*2*PI);
/***********************************************************************
* Control based on the robotics_library defined state variable
* PAUSED: make sure the controller stays DISARMED
* RUNNING: Normal operation of controller.
* - check for tipover
* - choose mode from setpoint.core_mode
* - evaluate difference equation and check saturation
* - actuate motors
************************************************************************/
switch (get_state()){
// make sure things are off if program is closing
case EXITING:
return 0;
// if the controller is somehow still ARMED, disarm it
case PAUSED:
if(setpoint.arm_state==ARMED){
disarm_controller();
}
break;
// normal operating mode
case RUNNING:
// exit if the controller was not armed properly
if(setpoint.arm_state==DISARMED){
/************************************************************************
* if disarmed, reset controllers and return
************************************************************************/
zero_out_controller();
goto END;
}
// check for a tipover
if (fabs(cstate.roll) >config.tip_angle ||
fabs(cstate.pitch)>config.tip_angle)
{
disarm_controller();
printf("\n TIPOVER DETECTED \n");
goto END;
}
/************************************************************************
* manage the setpoints based on attitude or position mode
************************************************************************/
switch(setpoint.core_mode){
/************************************************************************
* in Position control mode, evaluate an outer loop controller to
* change the attitude setpoint. Discard user attitude setpoints
************************************************************************/
case POSITION:
// TODO: outer loop position controller
break;
/************************************************************************
* in attitude control mode, user has direct control over throttle
* roll, and pitch angles. Absolute yaw setpoint gets updated at
* user-commanded yaw_rate
************************************************************************/
case ATTITUDE:
// only when flying, update the yaw setpoint
if(setpoint.throttle > YAW_CUTOFF_TH){
setpoint.yaw += DT*setpoint.yaw_rate;
}
else{
setpoint.yaw = cstate.yaw;
}
break;
default:
break; //should never get here
}
/************************************************************************
* Finally run the attitude feedback controllers
************************************************************************/
/************************************************************************
* Roll & Pitch Controllers
************************************************************************/
float dRoll_setpoint = (setpoint.roll - cstate.roll) *
config.roll_rate_per_rad;
float dPitch_setpoint = (setpoint.pitch - cstate.pitch) *
config.pitch_rate_per_rad;
cstate.dRoll_err = dRoll_setpoint - cstate.dRoll;
cstate.dPitch_err = dPitch_setpoint - cstate.dPitch;
// // if last state was DISARMED, then errors will all be 0.
// // make the previous error the same
// if(previous_core_mode == DISARMED){
// preFillFilter(&cstate.roll_ctrl, cstate.dRoll_err);
// preFillFilter(&cstate.pitch_ctrl, cstate.dPitch_err);
// }
// only run integrator if airborne
// TODO: proper landing/takeoff detection
if(u[3] > INT_CUTOFF_TH){
cstate.dRoll_err_integrator += cstate.dRoll_err * DT;
cstate.dPitch_err_integrator += cstate.dPitch_err * DT;
}
marchFilter(&cstate.roll_ctrl, cstate.dRoll_err);
marchFilter(&cstate.pitch_ctrl, cstate.dPitch_err);
if(setpoint.throttle<0.1){
saturateFilter(&cstate.roll_ctrl, -LAND_SATURATION,LAND_SATURATION);
saturateFilter(&cstate.pitch_ctrl, -LAND_SATURATION, LAND_SATURATION);
}
else{
saturateFilter(&cstate.roll_ctrl, -MAX_ROLL_COMPONENT, MAX_ROLL_COMPONENT);
saturateFilter(&cstate.pitch_ctrl, -MAX_PITCH_COMPONENT, MAX_PITCH_COMPONENT);
}
u[0] = cstate.roll_ctrl.current_output;
u[1] = cstate.pitch_ctrl.current_output;
/************************************************************************
* Yaw Controller
************************************************************************/
cstate.yaw_err = setpoint.yaw - cstate.yaw;
// only run integrator if airborne
if(u[3] > INT_CUTOFF_TH){
cstate.yaw_err_integrator += cstate.yaw_err * DT;
}
marchFilter(&cstate.yaw_ctrl, cstate.yaw_err);
if (user_interface.throttle_stick < -0.95){
zeroFilter(&cstate.yaw_ctrl);
}
else{
saturateFilter(&cstate.yaw_ctrl, -MAX_YAW_COMPONENT, MAX_YAW_COMPONENT);
}
u[2] = cstate.yaw_ctrl.current_output;
/************************************************************************
* Throttle Controller
************************************************************************/
// compensate for roll/pitch angle to maintain Z thrust
float throttle_compensation;
throttle_compensation = 1 / cos(cstate.roll);
throttle_compensation *= 1 / cos(cstate.pitch);
float thr = setpoint.throttle*(MAX_THRUST_COMPONENT-config.idle_speed)
+ config.idle_speed;
u[3] = throttle_compensation * thr;
// saturate thrust component now, this will help prevent saturation
// later once r,p,y are added
saturate_number(&u[3], 0, MAX_THRUST_COMPONENT);
/************************************************************************
* see mixing_matrixes.h for how the mixer works
************************************************************************/
if(mix_controls(u[0],u[1],u[2],u[3],&new_esc[0],config.rotors)<0){
printf("ERROR, mixing failed\n");
return -1;
}
/************************************************************************
* Prevent saturation under heavy vertical acceleration by reducing all
* outputs evenly such that the largest doesn't exceed 1
************************************************************************/
// find control output limits
float largest_value = 0;
float smallest_value = 1;
for(i=0;i<config.rotors;i++){
if(new_esc[i]>largest_value){
largest_value = new_esc[i];
}
if(new_esc[i]<smallest_value){
smallest_value=new_esc[i];
}
}
// if upper saturation would have occurred, reduce all outputs evenly
if(largest_value>1){
for(i=0;i<config.rotors;i++){
float offset = largest_value - 1;
new_esc[i]-=offset;
}
}
/************************************************************************
* Send a servo pulse immediately at the end of the control loop.
* Intended to update ESCs exactly once per control timestep
* also record this action to cstate.new_esc_out[] for telemetry
************************************************************************/
// if this is the first time armed, make sure to send minimum
// pulse width to prevent ESCs from going into calibration
if(previous_arm_state == DISARMED){
for(i=0;i<config.rotors;i++){
send_servo_pulse_normalized(i+1,0);
}
}
else{
for(i=0;i<config.rotors;i++){
if(new_esc[i]>1.0){
new_esc[i]=1.0;
}
else if(new_esc[i]<0){
new_esc[i]=0;
}
send_servo_pulse_normalized(i+1,new_esc[i]);
cstate.esc_out[i] = new_esc[i];
cstate.control_u[i] = u[i];
}
}
// // pass new information to the log with add_to_buffer
// // this only puts information in memory, doesn't
// // write to disk immediately
// if(config.enable_logging){
// new_log_entry.roll = cstate.roll;
// new_log_entry.pitch = cstate.pitch;
// new_log_entry.yaw = cstate.yaw;
// new_log_entry.u_0 = cstate.u[0];
// new_log_entry.u_1 = cstate.u[1];
// new_log_entry.u_2 = cstate.u[2];
// new_log_entry.u_3 = cstate.u[3];
// new_log_entry.esc_1 = cstate.esc_out[0];
// new_log_entry.esc_2 = cstate.esc_out[1];
// new_log_entry.esc_3 = cstate.esc_out[2];
// new_log_entry.esc_4 = cstate.esc_out[3];
// new_log_entry.v_batt = cstate.v_batt;
// add_to_buffer(new_log_entry);
// }
break;
default:
break;
} // end of switch(get_state())
END: // allow quick jump to here to clean up the several switch statements
//remember the last state to detect transition from DISARMED to ARMED
previous_arm_state = setpoint.arm_state;
return 0;
}
int main(int argc, char **argv)
{
// Initialize ROS
ros::init(argc, argv, "am_mpu9150_node");
ros::NodeHandle n;
ros::Publisher imu_pub = n.advertise<sensor_msgs::Imu>("imu", 1);
ros::Publisher imu_euler_pub = n.advertise<geometry_msgs::Vector3Stamped>("imu_euler", 1);
ros::Rate loop_rate(50);
sensor_msgs::Imu imu;
imu.header.frame_id = "imu";
geometry_msgs::Vector3Stamped euler;
euler.header.frame_id = "imu_euler";
// Init the sensor the values are hardcoded at the local_defaults.h file
int opt, len;
int i2c_bus = DEFAULT_I2C_BUS;
int sample_rate = DEFAULT_SAMPLE_RATE_HZ;
int yaw_mix_factor = DEFAULT_YAW_MIX_FACTOR;
int verbose = 0;
std::string accelFile;
std::string magFile;
// Parameters from ROS
ros::NodeHandle n_private("~");
int def_i2c_bus = 2;
n_private.param("i2cbus", i2c_bus, def_i2c_bus);
if (i2c_bus < MIN_I2C_BUS || i2c_bus > MAX_I2C_BUS)
{
ROS_ERROR("am_mpu9150: Imu Bus problem");
return -1;
}
int def_sample_rate = 50;
n_private.param("samplerate", sample_rate, def_sample_rate);
if (sample_rate < MIN_SAMPLE_RATE || sample_rate > MAX_SAMPLE_RATE)
{
ROS_ERROR("am_mpu9150: Sample rate problem");
return -1;
}
loop_rate = sample_rate;
int def_yaw_mix_factor = 0;
n_private.param("yawmixfactor", yaw_mix_factor, def_yaw_mix_factor);
if (yaw_mix_factor < 0 || yaw_mix_factor > 100)
{
ROS_ERROR("am_mpu9150: yawmixfactor problem");
return -1;
}
std::string defAccelFile = "./accelcal.txt";
n_private.param("accelfile", accelFile, defAccelFile);
std::string defMagFile = "./magcal.txt";
n_private.param("magfile", magFile, defMagFile);
unsigned long loop_delay;
mpudata_t mpu;
// Initialize the MPU-9150
mpu9150_set_debug(verbose);
if (mpu9150_init(i2c_bus, sample_rate, yaw_mix_factor))
{
ROS_ERROR("am_mpu9150::Failed init!");
exit(1);
}
//load_calibration(0, accelFile);
//load_calibration(1, magFile);
memset(&mpu, 0, sizeof(mpudata_t));
vector3d_t e;
quaternion_t q;
while (ros::ok())
{
std::stringstream ss;
if (mpu9150_read(&mpu) == 0)
{
// ROTATE The angles correctly...
quaternionToEuler(mpu.fusedQuat, e);
e[VEC3_Z] = -e[VEC3_Z];
eulerToQuaternion(e, q);
// FUSED
imu.header.stamp = ros::Time::now();
imu.orientation.x = q[QUAT_X];
imu.orientation.y = q[QUAT_Y];
imu.orientation.z = q[QUAT_Z];
imu.orientation.w = q[QUAT_W];
// GYRO
double gx = mpu.rawGyro[VEC3_X];
double gy = mpu.rawGyro[VEC3_Y];
double gz = mpu.rawGyro[VEC3_Z];
gx = gx * gyroScaleX;
gy = gy * gyroScaleY;
gz = gz * gyroScaleZ;
imu.angular_velocity.x = gx;
imu.angular_velocity.y = gy;
imu.angular_velocity.z = gz;
// ACCEL
imu.linear_acceleration.x = mpu.calibratedAccel[VEC3_X];
imu.linear_acceleration.y = mpu.calibratedAccel[VEC3_Y];
imu.linear_acceleration.z = mpu.calibratedAccel[VEC3_Z];
// EULER
euler.header.stamp = imu.header.stamp;
euler.vector.x = mpu.fusedEuler[VEC3_Y];
euler.vector.y = mpu.fusedEuler[VEC3_X];
euler.vector.z = -mpu.fusedEuler[VEC3_Z];
// Publish the messages
imu_pub.publish(imu);
imu_euler_pub.publish(euler);
//ROS_INFO("y=%f, p=%f, r=%f", euler.vector.z, euler.vector.y, euler.vector.x);
}
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
/******************************************************************
* balance_core()
* discrete-time balance controller operated off IMU interrupt
* Called at SAMPLE_RATE_HZ
*******************************************************************/
int balance_core(){
// local variables only in memory scope of balance_core
static int D1_saturation_counter = 0;
float compensated_D1_output = 0;
float dutyL = 0;
float dutyR = 0;
static log_entry_t new_log_entry;
float output_scale; //battery voltage/nominal voltage
// if an IMU packet read failed, ignore and just return
// the mpu9150_read function may print it's own warnings
if (mpu9150_read(&mpu) != 0){
return -1;
}
/**************************************************************
* STATE_ESTIMATION
* read sensors and compute the state regardless of if the
* controller is ARMED or DISARMED
***************************************************************/
// angle theta is positive in the direction of forward tip
// add mounting angle of BBB
cstate.theta[2] = cstate.theta[1]; cstate.theta[1] = cstate.theta[0];
cstate.theta[0] = mpu.fusedEuler[VEC3_X] + config.bb_mount_angle;
cstate.current_theta = cstate.theta[0];
cstate.d_theta = (cstate.theta[0]-cstate.theta[1])/DT;
// collect encoder positions
cstate.wheelAngleR = -(get_encoder_pos(config.encoder_channel_R) * 2*PI) \
/(config.gearbox * config.encoder_res);
cstate.wheelAngleL = (get_encoder_pos(config.encoder_channel_L) * 2*PI) \
/(config.gearbox * config.encoder_res);
// log phi estimate
// wheel angle is relative to body,
// add theta body angle to get absolute wheel position
cstate.phi[2] = cstate.phi[1]; cstate.phi[1] = cstate.phi[0];
cstate.phi[0] = ((cstate.wheelAngleL + cstate.wheelAngleR)/2) +cstate.current_theta;
cstate.current_phi = cstate.phi[0];
cstate.d_phi = (cstate.phi[0]-cstate.phi[1])/DT;
// body turning estimation
cstate.gamma[2] = cstate.gamma[1]; cstate.gamma[1] = cstate.phi[0];
cstate.gamma[0]=(cstate.wheelAngleL-cstate.wheelAngleR) \
* (config.wheel_radius/config.track_width);
cstate.d_gamma = (cstate.gamma[0]-cstate.gamma[1])/DT;
cstate.current_gamma = cstate.gamma[0];
// output scaling
output_scale = cstate.vBatt/config.v_nominal;
/*************************************************************
* Control based on the robotics_library defined state variable
* PAUSED: make sure the controller stays DISARMED
* RUNNING: Normal operation of controller.
* - check for tipover
* - wait for MiP to be within config.start_angle of
* upright
* - choose mode from setpoint.core_mode
* - evaluate difference equation and check saturation
* - actuate motors
***************************************************************/
switch (get_state()){
// make sure things are off if program is closing
case EXITING:
disable_motors();
return 0;
// if the controller is somehow still ARMED, disarm it
case PAUSED:
if(setpoint.arm_state==ARMED){
disarm_controller();
}
break;
// normal operating mode
case RUNNING:
// exit if the controller was not armed properly
if(setpoint.arm_state==DISARMED){
return 0;
}
// check for a tipover before anything else
if(fabs(cstate.current_theta)>config.tip_angle){
disarm_controller();
printf("tip detected \n");
break;
}
/**********************************************************
* POSITION Phi controller
* feedback control of wheel angle Phi by outputting a
* reference theta body angle. This is controller D2 in
* config
***********************************************************/
if(setpoint.core_mode == POSITION){
// move the position set points based on user input
if(setpoint.phi_dot != 0.0){
//setpoint.phi == cstate.current_phi + setpoint.phi_dot*DT;
setpoint.phi += setpoint.phi_dot * DT;
}
// march the different equation terms for the input Phi Error
// and the output theta reference angle
cstate.ePhi[2] = cstate.ePhi[1];
cstate.ePhi[1] = cstate.ePhi[0];
cstate.ePhi[0] = setpoint.phi-cstate.current_phi;
cstate.theta_ref[2] = cstate.theta_ref[1];
cstate.theta_ref[1] = cstate.theta_ref[0];
// evaluate D2 difference equation
cstate.theta_ref[0] = config.K_D2*( \
config.numD2_2 * cstate.ePhi[2] \
+ config.numD2_1 * cstate.ePhi[1] \
+ config.numD2_0 * cstate.ePhi[0]) \
-(config.denD2_2 * cstate.theta_ref[2] \
+ config.denD2_1 * cstate.theta_ref[1]);
//check saturation of outer loop theta reference output signal
saturate_float(&cstate.theta_ref[0],-config.theta_ref_max,config.theta_ref_max);
setpoint.theta = cstate.theta_ref[0];
}
// evaluate inner loop controller D1z
cstate.eTheta[2] = cstate.eTheta[1];
cstate.eTheta[1] = cstate.eTheta[0];
cstate.eTheta[0] = setpoint.theta - cstate.current_theta;
cstate.u[2] = cstate.u[1];
cstate.u[1] = cstate.u[0];
cstate.u[0] = \
config.K_D1 * (config.numD1_0 * cstate.eTheta[0] \
+ config.numD1_1 * cstate.eTheta[1] \
+ config.numD1_2 * cstate.eTheta[2]) \
- (config.denD1_1 * cstate.u[1] \
+ config.denD1_2 * cstate.u[2]);
// check saturation of inner loop knowing that right after
// this control will be scaled by battery voltage
if(saturate_float(&cstate.u[0], -output_scale, output_scale)){
D1_saturation_counter ++;
if(D1_saturation_counter > SAMPLE_RATE_HZ*config.pickup_detection_time){
printf("inner loop controller saturated\n");
disarm_controller();
D1_saturation_counter = 0;
break;
}
}
else{
D1_saturation_counter = 0;
}
cstate.current_u = cstate.u[0];
// scale output to compensate for battery charge level
compensated_D1_output = cstate.u[0] / output_scale;
// // integrate the reference theta to correct for imbalance or sensor
// // only if standing relatively still with zero phi reference
// // to-do, wait for stillness for a time period before integrating
// if(setpoint.phi==0 && fabs(cstate.phi_dot)<2){
// state.thetaTrim += (config.kTrim*cstate.theta_ref[0]) * DT;
// }
//steering controller
// move the controller set points based on user input
setpoint.gamma += setpoint.gamma_dot * DT;
cstate.egamma[1] = cstate.egamma[0];
cstate.egamma[0] = setpoint.gamma - cstate.current_gamma;
cstate.duty_split = config.KP_steer*(cstate.egamma[0] \
+config.KD_steer*(cstate.egamma[0]-cstate.egamma[1]));
// if the steering input would saturate a motor, reduce
// the steering input to prevent compromising balance input
if(fabs(compensated_D1_output)+fabs(cstate.duty_split) > 1){
if(cstate.duty_split > 0){
cstate.duty_split = 1-fabs(compensated_D1_output);
}
else cstate.duty_split = -(1-fabs(compensated_D1_output));
}
// add D1 balance controller and steering control
dutyL = compensated_D1_output - cstate.duty_split;
dutyR = compensated_D1_output + cstate.duty_split;
// send to motors
// one motor is flipped on chassis so reverse duty to L
set_motor(config.motor_channel_L,-dutyL);
set_motor(config.motor_channel_R,dutyR);
cstate.time_us = microsSinceEpoch();
// pass new information to the log with add_to_buffer
// this only puts information in memory, doesn't
// write to disk immediately
if(config.enable_logging){
new_log_entry.time_us = cstate.time_us-core_start_time_us;
new_log_entry.theta = cstate.current_theta;
new_log_entry.theta_ref = setpoint.theta;
new_log_entry.phi = cstate.current_phi;
new_log_entry.u = cstate.current_u;
add_to_buffer(new_log_entry);
}
// end of normal balancing routine
// last_state will be updated beginning of next interrupt
break;
default:
break; // nothing to do if UNINITIALIZED
}
return 0;
}
/************************************************************************
* int read_imu_data()
* only purpose is to update the global mpu struct with new
* data when it is read in. This is called off of an interrupt.
************************************************************************/
int read_imu_data(){
return mpu9150_read(&mpu);
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "mpu9150_node");
ros::NodeHandle n;
//creates publisher of IMU message
ros::Publisher pub = n.advertise<sensor_msgs::Imu>("imu/data_raw", 1000);
//creates publisher of Magnetic FIeld message
ros::Publisher pubM = n.advertise<sensor_msgs::MagneticField>("imu/mag", 1000);
ros::Rate loop_rate(10);
/* Init the sensor the values are hardcoded at the local_defaults.h file */
int opt, len;
int i2c_bus = DEFAULT_I2C_BUS;
int sample_rate = DEFAULT_SAMPLE_RATE_HZ;
int yaw_mix_factor = DEFAULT_YAW_MIX_FACTOR;
int verbose = 0;
char *mag_cal_file = NULL;
char *accel_cal_file = NULL;
unsigned long loop_delay;
//creates object of mpudata_t
mpudata_t mpu;
// Initialize the MPU-9150
register_sig_handler();
mpu9150_set_debug(verbose);
if (mpu9150_init(i2c_bus, sample_rate, yaw_mix_factor))
exit(1);
set_cal(0, accel_cal_file);
set_cal(1, mag_cal_file);
if (accel_cal_file)
free(accel_cal_file);
if (mag_cal_file)
free(mag_cal_file);
if (sample_rate == 0)
return -1;
// ROS loop config
loop_delay = (1000 / sample_rate) - 2;
printf("\nEntering MPU read loop (ctrl-c to exit)\n\n");
linux_delay_ms(loop_delay);
/**
* A count of how many messages we have sent. This is used to create
* a unique string for each message.
*/
int count = 0;
while (ros::ok())
{
//creates objects of each message type
//IMU Message
sensor_msgs::Imu msgIMU;
std_msgs::String header;
geometry_msgs::Quaternion orientation;
geometry_msgs::Vector3 angular_velocity;
geometry_msgs::Vector3 linear_acceleration;
//magnetometer message
sensor_msgs::MagneticField msgMAG;
geometry_msgs::Vector3 magnetic_field;
//modified to output in the format of IMU message
if (mpu9150_read(&mpu) == 0) {
//IMU Message
//sets up header for IMU message
msgIMU.header.seq = count;
msgIMU.header.stamp.sec = ros::Time::now().toSec();
msgIMU.header.frame_id = "/base_link";
//adds data to the sensor message
//orientation
msgIMU.orientation.x = mpu.fusedQuat[QUAT_X];
msgIMU.orientation.y = mpu.fusedQuat[QUAT_Y];
msgIMU.orientation.z = mpu.fusedQuat[QUAT_Z];
msgIMU.orientation.w = mpu.fusedQuat[QUAT_W];
//msgIMU.orientation_covariance[0] =
//angular velocity
msgIMU.angular_velocity.x = mpu.fusedEuler[VEC3_X] * RAD_TO_DEGREE;
msgIMU.angular_velocity.y = mpu.fusedEuler[VEC3_Y] * RAD_TO_DEGREE;
msgIMU.angular_velocity.z = mpu.fusedEuler[VEC3_Z] * RAD_TO_DEGREE;
//msgIMU.angular_velocity_covariance[] =
//linear acceleration
msgIMU.linear_acceleration.x = mpu.calibratedAccel[VEC3_X];
msgIMU.linear_acceleration.y = mpu.calibratedAccel[VEC3_Y];
msgIMU.linear_acceleration.z = mpu.calibratedAccel[VEC3_Z];
//msgIMU.linear_acceleration_covariance[] =
//Magnetometer Message
//sets up header
msgMAG.header.seq = count;
msgMAG.header.stamp.sec = ros::Time::now().toSec();
msgMAG.header.frame_id = "/base_link";
//adds data to magnetic field message
msgMAG.magnetic_field.x = mpu.calibratedMag[VEC3_X];
msgMAG.magnetic_field.y = mpu.calibratedMag[VEC3_Y];
msgMAG.magnetic_field.z = mpu.calibratedMag[VEC3_Z];
//fills the list with zeros as per message spec when no covariance is known
//msgMAG.magnetic_field_covariance[9] = {0, 0, 0, 0, 0, 0, 0, 0, 0};
}
//publish both messages
pub.publish(msgIMU);
pubM.publish(msgMAG);
ros::spinOnce();
loop_rate.sleep();
++count;
}
mpu9150_exit();
return 0;
}