int main(int argc, char **argv) { time_t begin = time(NULL); autopilot_initialize(); serial_start("/dev/ttyUSB0"); printf("Opened serial connection.\n"); read_messages(); // first read to make sure that there is a connection printf("Acquired initial read\n"); autopilot_start(); printf("locked data from read_messages()\n"); send_pre_arm_void_commands(); // arm_sequence(); usleep(200000); // 200ms = 5Hz offboard_control_sequence(); usleep(200000); mavlink_set_position_target_local_ned_t set_point; for(int i = 0; i < 30; i++){ // printf("Current Initial position : x = %f , y = %f , z = %f\n", ip.x, ip.y, ip.z); printf("%d: Current set point : x = %f, y = %f z=%f\n", i, ip.x, ip.y, ip.z - 0.25); set_yaw (ip.yaw, &set_point); set__(- 1.0 + ip.x , ip.y, ip.z - 2, &set_point); usleep(100000); } // disarm_sequence(); usleep(200000); disable_offboard_control_sequence(); usleep(200000); return 0; }
int main () { char message [512]; int n = 1; int tps = 1; if (initialize_socket(DEST_IP_AT, DEST_PORT_AT) != 0) { printf("[FAILED] Socket initialization failed\n"); } else { sleep(1); set_trim(message, n++); usleep(500000); ////////////// take off ////////////////////////// while(tps < 167) //100 avec 50ms { take_off(message, n++); usleep(30000); tps++; } tps = 0; ////////////// WAIT //////////////////// while(tps < 133) //80 { reset_com(message); usleep(30000); tps++; } tps = 0; ////////////// GO //////////////////// while(tps < 58) //35 { set_pitch(message, n++, FRONT, 0.25); usleep(30000); tps++; } tps = 0; while(tps < 17) //10 { set_pitch(message, n++, BACK, 0.25); usleep(30000); tps++; } tps = 0; while(tps < 17) { set_pitch(message, n++, FRONT, 0.0); usleep(30000); tps++; } tps = 0; ////////////// WAIT //////////////////// while(tps < 25) { reset_com(message); usleep(30000); tps++; } tps = 0; ////////////// ROTATE //////////////////// while(tps < 110) //140 { set_yaw(message, n++, LEFT, 0.4); usleep(30000); tps++; } tps = 0; set_yaw(message, n++, RIGHT, 0.0); usleep(30000); ////////////// WAIT //////////////////// while(tps < 80) { reset_com(message); usleep(30000); tps++; } tps = 0; ////////////// RETURN //////////////////// while(tps < 58) { set_pitch(message, n++, FRONT, 0.25); usleep(30000); tps++; } tps = 0; while(tps < 17) { set_pitch(message, n++, BACK, 0.25); usleep(30000); tps++; } tps = 0; while(tps < 17) { set_pitch(message, n++, FRONT, 0.0); usleep(30000); tps++; } tps = 0; ////////////// WAIT //////////////////// while(tps < 167) { reset_com(message); usleep(30000); tps++; } tps = 0; ////////////// LAND //////////////////// landing(message, n++); sleep(1); } close_socket(); return 0; }
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; } }
void commands(Autopilot_Interface &api) { // -------------------------------------------------------------------------- // START OFFBOARD MODE // -------------------------------------------------------------------------- api.enable_offboard_control(); usleep(100); // give some time to let it sink in // now the autopilot is accepting setpoint commands // -------------------------------------------------------------------------- // SEND OFFBOARD COMMANDS // -------------------------------------------------------------------------- printf("SEND OFFBOARD COMMANDS\n"); // initialize command data strtuctures mavlink_set_position_target_local_ned_t sp; mavlink_set_position_target_local_ned_t ip = api.initial_position; // autopilot_interface.h provides some helper functions to build the command // Example 1 - Set Velocity // set_velocity( -1.0 , // [m/s] // -1.0 , // [m/s] // 0.0 , // [m/s] // sp ); // Example 2 - Set Position set_position( ip.x - 5.0 , // [m] ip.y - 5.0 , // [m] ip.z , // [m] sp ); // Example 1.2 - Append Yaw Command set_yaw( ip.yaw , // [rad] sp ); // SEND THE COMMAND api.update_setpoint(sp); // NOW pixhawk will try to move // Wait for 8 seconds, check position for (int i=0; i < 8; i++) { mavlink_local_position_ned_t pos = api.current_messages.local_position_ned; printf("%i CURRENT POSITION XYZ = [ % .4f , % .4f , % .4f ] \n", i, pos.x, pos.y, pos.z); sleep(1); } printf("\n"); // -------------------------------------------------------------------------- // STOP OFFBOARD MODE // -------------------------------------------------------------------------- api.disable_offboard_control(); // now pixhawk isn't listening to setpoint commands // -------------------------------------------------------------------------- // GET A MESSAGE // -------------------------------------------------------------------------- printf("READ SOME MESSAGES \n"); // copy current messages Mavlink_Messages messages = api.current_messages; // local position in ned frame mavlink_local_position_ned_t pos = messages.local_position_ned; printf("Got message LOCAL_POSITION_NED (spec: https://pixhawk.ethz.ch/mavlink/#LOCAL_POSITION_NED)\n"); printf(" pos (NED): %f %f %f (m)\n", pos.x, pos.y, pos.z ); // hires imu mavlink_highres_imu_t imu = messages.highres_imu; printf("Got message HIGHRES_IMU (spec: https://pixhawk.ethz.ch/mavlink/#HIGHRES_IMU)\n"); printf(" ap time: %lu \n", imu.time_usec); printf(" acc (NED): % f % f % f (m/s^2)\n", imu.xacc , imu.yacc , imu.zacc ); printf(" gyro (NED): % f % f % f (rad/s)\n", imu.xgyro, imu.ygyro, imu.zgyro); printf(" mag (NED): % f % f % f (Ga)\n" , imu.xmag , imu.ymag , imu.zmag ); printf(" baro: %f (mBar) \n" , imu.abs_pressure); printf(" altitude: %f (m) \n" , imu.pressure_alt); printf(" temperature: %f C \n" , imu.temperature ); printf("\n"); // -------------------------------------------------------------------------- // END OF COMMANDS // -------------------------------------------------------------------------- return; }
int main () { char message [512]; int n = 1; int tps = 1; if (initialize_socket(DEST_IP_AT, DEST_PORT_AT) != 0) { printf("[FAILED] Socket initialization failed\n"); } else { sleep(1); set_trim(message, n++); usleep(500000); while(tps < 167) { take_off(message, n++); usleep(30000); tps++; } tps = 0; while(tps < 133) { reset_com(message); usleep(30000); tps++; } tps = 0; /* while(tps < 30) { set_pitch(message, n++, BACK, 0.25); usleep(50000); tps++; } tps = 0; while(tps < 10) { set_pitch(message, n++, FRONT, 0.1); usleep(50000); tps++; } tps = 0; while(tps < 30) { set_pitch(message, n++, BACK, 0.0); usleep(50000); tps++; } tps = 0;*/ /* while(tps < 60) { take_off(message, n++); usleep(50000); tps++; } tps = 0;*/ while(tps < 150) { set_yaw(message, n++, LEFT, 0.4); usleep(30000); tps++; } tps = 0; set_yaw(message, n++, RIGHT, 0.0); usleep(30000); while(tps < 133) { reset_com(message); usleep(30000); tps++; } tps = 0; landing(message, n++); sleep(1); } close_socket(); return 0; }