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