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