int main(int argc, char **argv) { ros::init(argc, argv, "imu"); ros::NodeHandle nh; ros::Publisher chatter_pub = nh.advertise<std_msgs::Float64>("yaw", 1000); std_msgs::Float64 msg; ros::Rate loopRate(10); int temp = 0; removegyrooff(); while (ros::ok()) { read_sensors(); msg.data = TO_DEG(-atan2(magnetom[1], magnetom[0])); chatter_pub.publish(msg); ROS_INFO("%s %f", "send an imu message", TO_DEG(-atan2(magnetom[1], magnetom[0]))); // ROS_INFO("%d \n",temp); ros::spinOnce(); loopRate.sleep(); temp++; } }
int bot_gps_linearize_to_lat_lon(BotGPSLinearize *gl, const double xy[2], double ll_deg[2]) { double dlat = asin(xy[1] / gl->radius_ns); ll_deg[0] = TO_DEG(dlat) + gl->lat0_deg; double dlon = asin(xy[0] / gl->radius_ew / cos(TO_RAD(gl->lat0_deg))); ll_deg[1] = TO_DEG(dlon) + gl->lon0_deg; return 0; }
void SensorsProcessor:: processSample(const IMUEvent *se) { // process incoming sensory sample // gyroscope data (raw data in deg/s) // sampling @ 125Hz const double dt = 0.008f; _rpyEstimate->roll[IMUEvent::GYROSCOPE] += se->g[IMUEvent::XAXIS]*dt; _rpyEstimate->pitch[IMUEvent::GYROSCOPE] += se->g[IMUEvent::YAXIS]*dt; _rpyEstimate->yaw[IMUEvent::GYROSCOPE] += se->g[IMUEvent::ZAXIS]*dt; // accelerometer data (raw data in g) // LPF fa[IMUEvent::XAXIS] = se->a[IMUEvent::XAXIS]*IN_WEIGHT + fa[IMUEvent::XAXIS]*(1.0f - IN_WEIGHT); fa[IMUEvent::YAXIS] = se->a[IMUEvent::YAXIS]*IN_WEIGHT + fa[IMUEvent::YAXIS]*(1.0f - IN_WEIGHT); fa[IMUEvent::ZAXIS] = se->a[IMUEvent::ZAXIS]*IN_WEIGHT + fa[IMUEvent::ZAXIS]*(1.0f - IN_WEIGHT); // fill in the RPY struct // [-90...90] deg range _rpyEstimate->pitch[IMUEvent::ACCELEROMETER] = TO_DEG(atan2(sqrt(fa[IMUEvent::XAXIS]*fa[IMUEvent::XAXIS] + fa[IMUEvent::ZAXIS]*fa[IMUEvent::ZAXIS]),fa[IMUEvent::YAXIS])) - 90.0; _rpyEstimate->roll[IMUEvent::ACCELEROMETER] = TO_DEG(atan2(sqrt(fa[IMUEvent::YAXIS]*fa[IMUEvent::YAXIS] + fa[IMUEvent::ZAXIS]*fa[IMUEvent::ZAXIS]),fa[IMUEvent::XAXIS])) - 90.0; _rpyEstimate->yaw[IMUEvent::ACCELEROMETER] = 0.0f; // for yaw motion we are perpedicular to the gravity vector, so no contribution // magneto data (raw data in uT) // LPF fm[IMUEvent::XAXIS] = se->m[IMUEvent::XAXIS]*IN_WEIGHT + fm[IMUEvent::XAXIS]*(1.0f - IN_WEIGHT); fm[IMUEvent::YAXIS] = se->m[IMUEvent::YAXIS]*IN_WEIGHT + fm[IMUEvent::YAXIS]*(1.0f - IN_WEIGHT); fm[IMUEvent::ZAXIS] = se->m[IMUEvent::ZAXIS]*IN_WEIGHT + fm[IMUEvent::ZAXIS]*(1.0f - IN_WEIGHT); double magNorm = sqrt((fm[IMUEvent::XAXIS]*fm[IMUEvent::XAXIS]) + (fm[IMUEvent::YAXIS]*fm[IMUEvent::YAXIS]) + (fm[IMUEvent::ZAXIS]*fm[IMUEvent::ZAXIS])); fm[IMUEvent::XAXIS] /= magNorm; fm[IMUEvent::YAXIS] /= magNorm; fm[IMUEvent::ZAXIS] /= magNorm; _rpyEstimate->roll[IMUEvent::MAGNETOMETER] = 0.0f; // no contribution for roll _rpyEstimate->pitch[IMUEvent::MAGNETOMETER] = 0.0f; // no contribution for pitch // TODO check this equations for compensated / uncompensated yaw _rpyEstimate->yaw[IMUEvent::MAGNETOMETER] = TO_DEG(atan2((fm[IMUEvent::YAXIS]*cos(_rpyEstimate->roll[IMUEvent::ACCELEROMETER]))- (fm[IMUEvent::ZAXIS]*sin(_rpyEstimate->roll[IMUEvent::ACCELEROMETER])), (fm[IMUEvent::XAXIS]*cos(_rpyEstimate->pitch[IMUEvent::ACCELEROMETER]))+ (fm[IMUEvent::YAXIS]*sin(_rpyEstimate->roll[IMUEvent::ACCELEROMETER])*sin(_rpyEstimate->pitch[IMUEvent::ACCELEROMETER]))- (fm[IMUEvent::ZAXIS]*cos(_rpyEstimate->roll[IMUEvent::ACCELEROMETER])*sin(_rpyEstimate->pitch[IMUEvent::ACCELEROMETER])))) - 90.0; //printf("RPYg = [%f %f %f]\n", _rpyEstimate->roll[IMUEvent::GYROSCOPE], _rpyEstimate->pitch[IMUEvent::GYROSCOPE], _rpyEstimate->yaw[IMUEvent::GYROSCOPE]); //printf("RPYa = [%f %f %f]\n", _rpyEstimate->roll[IMUEvent::ACCELEROMETER], _rpyEstimate->pitch[IMUEvent::ACCELEROMETER], _rpyEstimate->yaw[IMUEvent::ACCELEROMETER]); //printf("RPYm = [%f %f %f]\n", _rpyEstimate->roll[IMUEvent::MAGNETOMETER], _rpyEstimate->pitch[IMUEvent::MAGNETOMETER] , _rpyEstimate->yaw[IMUEvent::MAGNETOMETER]); auto ev = std::make_shared<SensorEvent>(); ev->imu = *se; ev->rpy = *_rpyEstimate; emit sensorEvent(ev); }
void output_angles(){ if (output_format == OUTPUT__FORMAT_BINARY) { float ypr[3]; ypr[0] = TO_DEG(yaw); ypr[1] = TO_DEG(pitch); ypr[2] = TO_DEG(roll); // Serial.write((byte*) ypr, 12); // No new-line } else if (output_format == OUTPUT__FORMAT_TEXT) { // printf(" YPR="); // printf("%f\t",TO_DEG(yaw)); // printf("%f\t",TO_DEG(pitch)); // printf("%f\n",TO_DEG(roll)); } }
void Satellite::render(sf::RenderTarget &target) { satBody.setPosition(position); satBody.setRotation(TO_DEG(rotation)); target.draw(satBody); if(satBody.getColor() == sf::Color::Red) satBody.setColor(sf::Color::White); }
void SpaceStation::render(sf::RenderTarget& target) { if(position != station.getPosition()) { station.setPosition(position); station.setRotation(TO_DEG(rotation)); } target.draw(trajectory); target.draw(station); }
int main(int argc, char**argv) { ros::init(argc,argv,"imu"); ros::NodeHandle n; ros::Publisher out_pub = n.advertise<sensor_msgs::Imu >("imuyrp", 1000); ros::NodeHandle nh; ros::Publisher chatter_pub = nh.advertise<std_msgs::Float64>("yaw", 1000); sensor_msgs::Imu imu_msg; std_msgs::Float64 msg; // Read sensors, init DCM algorithm reset_sensor_fusion(); removegyrooff(); float Y=0.0; int i=0; while(ros::ok()) { // Time to read the sensors again? if((clock() - timestamp) >= OUTPUT__DATA_INTERVAL) { timestamp_old = timestamp; timestamp = clock(); if (timestamp > timestamp_old) G_Dt = (float) (timestamp - timestamp_old) / 1000.0f; // Real time of loop run. We use this on the DCM algorithm (gyro integration time) else G_Dt = 0; //cout << G_Dt << endl; // Update sensor readings read_sensors(); // Run DCM algorithm Compass_Heading(); // Calculate magnetic heading Matrix_update(); Normalize(); Drift_correction(); Euler_angles(); output_angles(); imu_msg = sensor_msgs::Imu(); imu_msg.header.stamp = ros::Time::now(); imu_msg.header.frame_id = "imu"; imu_msg.orientation.x = TO_DEG(pitch); imu_msg.orientation.y = TO_DEG(roll); imu_msg.orientation.z = TO_DEG(yaw); imu_msg.orientation.w = 0.0; imu_msg.orientation_covariance[0] = -1; imu_msg.angular_velocity.x = 0.0; imu_msg.angular_velocity.y = 0.0; imu_msg.angular_velocity.z = 0.0; imu_msg.angular_velocity_covariance[0] = -1; imu_msg.linear_acceleration.x = 0.0; imu_msg.linear_acceleration.y = 0.0; imu_msg.linear_acceleration.z = 0.0; imu_msg.linear_acceleration_covariance[0] = -1; msg.data = TO_DEG(-atan2(magnetom[1],magnetom[0])); chatter_pub.publish(msg); ROS_INFO("%s %f", "send an imu message",TO_DEG(-atan2(magnetom[1],magnetom[0]))); //ros::spinOnce(); } } }
void holonomic_trajectory_manager_event(void * param) { ///@todo : probablement des fonctions de la lib math qui font ça struct h_trajectory *traj = (struct h_trajectory *) param; double x = holonomic_position_get_x_double(traj->position); double y = holonomic_position_get_y_double(traj->position); vect2_cart vector_pos; int32_t s_consign = 0; /**< The speed consign */ int32_t a_consign = 0; /**< The angle consign */ int32_t o_consign = 0; /**< The angular speed (omega) consign */ float target_norm = sqrtf(pow(traj->xy_target.x, 2) + pow(traj->xy_target.y, 2)); // TODO : variable never used float position_norm = sqrtf(pow(x, 2) + pow(y, 2)); // TODO : variable never used int32_t distance2target = sqrtf(pow(x - traj->xy_target.x, 2) + pow(y - traj->xy_target.y, 2)); vector_pos.x = x; vector_pos.y = y; /** @todo : move the following in the right siwtch-case */ vect2_cart vec_target = {.x = traj->xy_target.x - x, .y = traj->xy_target.y - y}; vect2_cart vec_to_center = {.x = traj->circle_center.x - x, // TODO : variable never used .y = traj->circle_center.y - y}; static vect2_cart keyframe = {.x = -1, .y = -1}; /* step 1 : process new commands to quadramps */ switch (traj->moving_state) { case MOVING_STRAIGHT: /* Calcul de la consigne d'angle */ a_consign = TO_DEG(vect2_angle_vec_x_rad_cart(&vec_target)); /* Calcul de la consigne de vitesse */ s_consign = SPEED_ROBOT; if (distance2target < 250) s_consign = 2*distance2target; break; case MOVING_CIRCLE: if (keyframe.x < 0) { keyframe.x = traj->circle_center.x + cos(atan2f(y - traj->circle_center.y, x - traj->circle_center.x) - ANGLE_INC)*traj->radius; keyframe.y = traj->circle_center.y + sin(atan2f(y - traj->circle_center.y, x - traj->circle_center.x)-ANGLE_INC)*traj->radius; } traj->xy_target.x = keyframe.x; traj->xy_target.y = keyframe.y; /* Calcul de la consigne d'angle */ a_consign = TO_DEG(vect2_angle_vec_x_rad_cart(&vec_target)); printf("%d\n", a_consign); s_consign = SPEED_ROBOT/5; //if (distance2target < 250) ///@todo : faire un truc avec holonomic_length_arc_of_circle_pnt(traj, RAD)) plutôt. //s_consign = 2*distance2target; break; case MOVING_IDLE: break; } switch (traj->turning_state) { case TURNING_CAP: if(traj->a_target - holonomic_position_get_a_rad_double(traj->position) < 0 || traj->a_target - holonomic_position_get_a_rad_double(traj->position) > M_PI) o_consign = 50; else o_consign = -50;//holonomic_angle_2_x_rad(traj, traj->a_target);//cs_do_process(traj->csm_omega, holonomic_angle_2_x_rad(traj, ANG)); break; case TURNING_SPEEDOFFSET: o_consign = 1;//cs_do_process(traj->csm_omega, holonomic_angle_2_speed_rad(traj, ANG)); break; case TURNING_FACEPOINT: o_consign = 1;//cs_do_process(traj->csm_omega, holonomic_angle_facepoint_rad(traj, &FP)); break; case TURNING_IDLE: break; } /* step 3 : check the end of the move */ if (traj->turning_state == TURNING_IDLE && holonomic_robot_in_xy_window(traj, traj->d_win)) //@todo : not only distance, angle { if (traj->moving_state == MOVING_CIRCLE) { keyframe.x = traj->circle_center.x + cos(atan2f(y - traj->circle_center.y, x - traj->circle_center.x) - ANGLE_INC) * traj->radius; keyframe.y = traj->circle_center.y + sin(atan2f(y - traj->circle_center.y, x - traj->circle_center.x) - ANGLE_INC) * traj->radius; printf("x: %f y: %f\n", keyframe.x, keyframe.y); // If we have finished the circular trajectory /**@todo do not work beaucause the pos has changed -> take the depaeture of the pos of restart for scratch */ vect2_cart arrival = { .x = traj->circle_center.x + cos(atan2f(y - traj->circle_center.y, x - traj->circle_center.x) - traj->arc_angle) * traj->radius, .y = traj->circle_center.y + sin(atan2f(y - traj->circle_center.y, x - traj->circle_center.x) - traj->arc_angle) * traj->radius}; printf("Arrival : x: %f y: %f\n", arrival.x, arrival.y); if (vect2_dist_cart(&arrival, &vector_pos) < traj->d_win) { holonomic_delete_event(traj); } return; } if (prev_speed < 20) { traj->moving_state = MOVING_IDLE; holonomic_delete_event(traj); return; } else s_consign = 0; } if (traj->moving_state == MOVING_IDLE && holonomic_robot_in_angle_window(traj, traj->a_win)) { traj->turning_state = TURNING_IDLE; holonomic_delete_event(traj); return; } /* step 2 : pass the consign to rsh */ set_consigns_to_rsh(traj, s_consign, a_consign, o_consign); /** @todo : re-init le keyframe !!! */ } /** near the target (dist in x,y) ? */ uint8_t holonomic_robot_in_xy_window(struct h_trajectory *traj, double d_win) { vect2_cart vcp = {.x = holonomic_position_get_x_double(traj->position), .y = holonomic_position_get_y_double(traj->position)}; return (vect2_dist_cart(&vcp, &traj->xy_target) < d_win); } /** returns true if the robot is in an area enclosed by a certain angle */ uint8_t holonomic_robot_in_angle_window(struct h_trajectory *traj, double a_win_rad) { double d_a = traj->a_target - holonomic_position_get_a_rad_double(traj->position); if (ABS(d_a) < M_PI) { return (ABS(d_a) < (a_win_rad/2)); } else { return ((2 * M_PI-ABS(d_a)) < (a_win_rad/2)); } } /** remove event if any @todo */ void holonomic_delete_event(struct h_trajectory *traj) { prev_speed = 0; traj->end_of_traj = 1; set_consigns_to_rsh(traj, 0, holonomic_position_get_theta_v_int(traj->position), 0); /** do not work without this : */ rsh_set_speed(traj->robot,0); if ( traj->scheduler_task != -1) { DEBUG(E_TRAJECTORY, "Delete event"); scheduler_del_event(traj->scheduler_task); traj->scheduler_task = -1; } } /** schedule the trajectory event */ void holonomic_schedule_event(struct h_trajectory *traj) { if ( traj->scheduler_task != -1) { DEBUG(E_TRAJECTORY, "Schedule event, already scheduled"); } else { traj->scheduler_task = scheduler_add_periodical_event_priority(&holonomic_trajectory_manager_event, (void*)traj, TRAJ_EVT_PERIOD, 30); } } /** do a modulo 2.pi -> [-Pi,+Pi], knowing that 'a' is in [-3Pi,+3Pi] */ double holonomic_simple_modulo_2pi(double a) { if (a < -M_PI) { a += M_PI_2; } else if (a > M_PI) { a -= M_PI_2; } return a; } /** do a modulo 2.pi -> [-Pi,+Pi] */ double holonomic_modulo_2pi(double a) { double res = a - (((int32_t) (a/M_PI_2)) * M_PI_2); return holonomic_simple_modulo_2pi(res); }