void adjustByIMUData(sensor_msgs::PointCloud& cloud) { for(int i = 0; i < cloud.points.size(); ++i) { geometry_msgs::Point32 curPoint = cloud.points[i]; Eigen::Vector3f curVec(curPoint.x, curPoint.y, curPoint.z); curVec = set_pitch(imu_pitch) * set_roll(imu_roll) * curVec; cloud.points[i].x = curVec[0]; cloud.points[i].y = curVec[1]; cloud.points[i].z = curVec[2]; } }
void SWITCH_DRONE_COMMANDE(int _order) { char message [64]; switch(_order){ case 0 : reset_com(message); break; case 1 : landing(message); inC.flag_land = 0; break; case 2 : //printf("SWITCH COMMANDE take off\n"); take_off(message); inC.flag_takeoff = 0; break; case 3 : //printf("case 3 : set_roll, roll_power=%f\n", roll_power); set_roll(message, roll_move, roll_power); inC.flag_rollcalled = 0; break; case 4 : //printf("case 4 : set_pitch, pitch_power=%f, yaw_power=%f\n", pitch_power, yaw_power); set_pitch(message, pitch_move, pitch_power); inC.flag_pitchcalled = 0; break; case 5 : set_yaw(message, yaw_move, yaw_power); inC.flag_yawcalled = 0; break; case 6 : set_roll(message, roll_move, roll_power); set_pitch(message, pitch_move, pitch_power); inC.flag_rollpitchcalled = 0; break; case 7 : set_trim(message); inC.flag_calibH = 0; break; case 8 : calibrate_magneto(message); sleep(3); while(1) { set_yaw(message, RIGHT, 0.1); while(isControllerReady()==0); Main_Nav = getNavdata(); if (Main_Nav.magneto.heading_fusion_unwrapped > 0.0) nav_suiv = Main_Nav.magneto.heading_fusion_unwrapped - 360.0; else nav_suiv = Main_Nav.magneto.heading_fusion_unwrapped + 360.0; printf("angle_trouve et angle +/- 360 = %.2f, %.2f \r", Main_Nav.magneto.heading_fusion_unwrapped, nav_suiv); if(nav_suiv < 0.0 && nav_prec > 0.0 || nav_suiv > 0.0 && nav_prec < 0.0) { inC.flag_calibM = 0; printf("\nnav_suiv = %.2f | nav_prec = %.2f\n", nav_suiv, nav_prec); break; } nav_prec = nav_suiv; } set_yaw(message, LEFT, 0.0); inC.flag_calibM = 0; break; case 9 : //set_emergency(message); inC.flag_emergency = 0; set_gaz(message, UP, 0.2);// this is an ugly hack (don't commit this) break; case 10 : //anti_emergency(message); inC.falg_antiemergency = 0; set_gaz(message, UP, 0.0);// this is an ugly hack (don't commit this) break; case 20 : calcul_mission(); break; default : printf("Scade ne marche pas si bien que ça finalement\n"); break; } }