Commit 39a808cc authored by Amine AIT-GANI's avatar Amine AIT-GANI
Browse files

Program to communicate with the drone

parent 893cdf2e
Pipeline #1532 failed with stage
in 1 minute and 30 seconds
......@@ -3,7 +3,7 @@
#define MAVLINK_MSG_ID_COMMAND_LONG 76
MAVPACKED(
MAVPACKED(
typedef struct __mavlink_command_long_t {
float param1; /*< Parameter 1 (for the specific command).*/
float param2; /*< Parameter 2 (for the specific command).*/
......@@ -118,6 +118,21 @@ static inline uint16_t mavlink_msg_command_long_pack(uint8_t system_id, uint8_t
memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_COMMAND_LONG_LEN);
#endif
printf("COMMAND %u\n", command);
printf("target_system %u\n", target_system);
printf("target_component %u\n", target_component);
printf("confirmation %u\n", confirmation);
printf("param1 %f\n", param1);
printf("param2 %f\n", param2);
printf("param3 %f\n", param3);
printf("param4 %f\n", param4);
printf("param5 %f\n", param5);
printf("param6 %f\n", param6);
printf("param7 %f\n", param7);
msg->msgid = MAVLINK_MSG_ID_COMMAND_LONG;
return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_COMMAND_LONG_MIN_LEN, MAVLINK_MSG_ID_COMMAND_LONG_LEN, MAVLINK_MSG_ID_COMMAND_LONG_CRC);
......
......@@ -13,9 +13,16 @@
#include <iostream>
#include <string>
#include <iostream>
#include <fstream>
#include <fstream>
#include <thread>
//include "Com_server/action.h"
using namespace std;
using std::chrono::seconds;
using std::this_thread::sleep_for;
char device_path[255] = "/dev/ttyUSB0";
int device_baudrate = 57600;
......@@ -29,6 +36,8 @@ Drone drone1;
ofstream MyFile;
ofstream MyFilew;
ofstream compassx;
ofstream compassy;
blc_channel blc_control_arm("/pixhawk.control.arm", BLC_CHANNEL_READ, 'IN16', 'NDEF', 1, 1); // 1=on, 0=off;
blc_channel blc_control_remote_vector("/pixhawk.control.remoteVectors", BLC_CHANNEL_WRITE, 'FL32', 'NDEF', 1, 4);
......@@ -36,7 +45,7 @@ blc_channel blc_control_remote_vector("/pixhawk.control.remoteVectors", BLC_CHAN
blc_channel blc_control_motors("/pixhawk.control.motors", BLC_CHANNEL_READ, 'FL32', 'NDEF', 1, 4); //x, y, z, r
//Channels use to command the drone
blc_channel blc_control_commands("/pixhawk.control.commands", BLC_CHANNEL_READ, 'FL32', 'NDEF', 1, 4); // 1=on, 0=off
blc_channel blc_control_commands("/pixhawk.control.commands", BLC_CHANNEL_READ, 'FL32', 'NDEF', 1, 5); // 1=on, 0=off
blc_channel blc_highres_imu("/pixhawk.sensors.imu", BLC_CHANNEL_WRITE, 'FL32', 'NDEF', 1, 9);
//Channel use to know altitude of drone
blc_channel blc_attitude("/pixhawk.sensors.attitude", BLC_CHANNEL_WRITE, 'FL32', 'NDEF', 1, 7);
......@@ -58,26 +67,57 @@ void command_loop()
cout<<"Enter command : \n";
cin >> buffer_command;
/*_____________SET_MODE____________*/
//drone1.make_command_msg_rate(1);
drone1.command_setMode(DRONE_OFF);
//drone1.make_mode(1);
/*___________________________________*/
if(buffer_command=="a")
{
cout<<"arm \n";
cout<<"arm \n";
drone1.command_arm(1);
//drone1.make_mode(193);
}
else if(buffer_command=="!a")
{
drone1.command_arm(0);
}
else if(buffer_command=="m")
{
drone1.followhold();
}
else if(buffer_command=="p")
{
drone1.go_to();
}
else if(buffer_command=="f")
{
drone1.take_off();
cout<<"fly \n";
ret = drone1.take_off();
ret = drone1.take_off();
cout<<"fly ret = "<<ret<<endl;
sleep_for(seconds(10));
}
else if(buffer_command=="!f")
{
//drone1.take(0);
}
else if(buffer_command=="b")
{
drone1.take_off_vtol();
}
else if(buffer_command=="i")
{
cout<<"return \n";
......@@ -132,7 +172,10 @@ void command_loop()
{
drone1.command_kill(0);
}
/* else if(buffer_command=="n")
{
drone1.waypoint(0.1,0.1,0.1);
} */
else if(buffer_command=="z")
{
cout<<"motor 1 \n";
......@@ -178,7 +221,7 @@ void command_loop()
mavlink_msg_play_tune_pack(255, drone1.component_id, &message, 1, 1 , "AAAA", "");
drone1.write_message(message);
}
else if (buffer_command=="g")
/*else if (buffer_command=="g")
{
drone1.command_setModeGuided(1);
}
......@@ -186,6 +229,7 @@ void command_loop()
{
drone1.command_setModeGuided(0);
}
*/
else
{
cout<<"command not recognized\n";
......@@ -249,6 +293,8 @@ int main(int argc, char *argv[])
// ofstream MyFile("mesures.txt");
MyFile.open("mesures.txt");
MyFilew.open("tcdrain.txt");
compassx.open("compassx.txt");
compassy.open("compassy.txt");
writingThread = shared_ptr<Serial_Port_WritingThread>(new Serial_Port_WritingThread(1000000, 200)); //beug ne marche pas
......@@ -312,5 +358,7 @@ int main(int argc, char *argv[])
}
MyFile.close();
MyFilew.close();
compassx.close();
compassy.close();
exit(0);
}
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment