Skip to content

GitLab

Commit 99e0cd04 authored by Amine AIT-GANI's avatar Amine AIT-GANI
Browse files

Modification des fichiers de test

parent e1b2198d
Expand all Hide whitespace changes
Inline Side-by-side
Com_server/test/TestDroneRemoteControl.cpp
View file @ 99e0cd04
......@@ -42,10 +42,10 @@ 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);
//Channels use to control manually the drone
blc_channel blc_control_motors("/pixhawk.control.motors", BLC_CHANNEL_READ, 'FL32', 'NDEF', 1, 4); //x, y, z, r
blc_channel blc_control_motors("/pixhawk.control.motors", BLC_CHANNEL_READ, 'FL32', 'NDEF', 1, 6); //x, y, z, r
//Channels use to command the drone
blc_channel blc_control_commands("/pixhawk.control.commands", BLC_CHANNEL_READ, 'FL32', 'NDEF', 1, 9); // 1=on, 0=off
blc_channel blc_control_commands("/pixhawk.control.commands", BLC_CHANNEL_READ, 'FL32', 'NDEF', 1, 7); // 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);
......@@ -56,6 +56,7 @@ blc_channel blc_global_position("/pixhawk.sensors.global_position", BLC_CHANNEL_
//Different blc channel use to publish data
blc_channel blc_heartbeat; //Hearbeat message PG: PB ????? pas d'init pour l'instant ????
blc_channel blc_test_detection("/pixhawk.test.detection", BLC_CHANNEL_READ, 'IN16', 'NDEF', 1, 7);
int j = 0;
void command_loop()
{
......@@ -72,13 +73,10 @@ void command_loop()
//drone1.make_command_msg_rate(1);
drone1.command_setMode(DRONE_OFF);
//drone1.make_mode(1);
/*___________________________________*/
if(buffer_command=="a")
{
cout<<"arm \n";
......@@ -91,7 +89,37 @@ drone1.command_setMode(DRONE_OFF);
drone1.command_arm(0);
}
else if(buffer_command=="h"){
cout<<"HOME_POSITION \n";
drone1.request_home_position(1,1,1);
}
else if(buffer_command=="e"){
cout<<"X_POSITION \n";
drone1.get_home_x(1);
}
else if(buffer_command=="o")
{
cout<<"current_consumed :"<<drone1.battery_status.current_consumed<<endl;
cout<<"=========================================="<<endl;
cout<<"energy_consumed :"<<drone1.battery_status.energy_consumed<<endl;
cout<<"=========================================="<<endl;
cout<<"temperature :"<<drone1.battery_status.temperature<<endl;
cout<<"=========================================="<<endl;
cout<<"voltages :"<<drone1.battery_status.voltages[10]<<endl;
cout<<"=========================================="<<endl;
cout<<"current_battery :"<<drone1.battery_status.current_battery<<endl;
cout<<"=========================================="<<endl;
cout<<"battery_function :"<<drone1.battery_status.battery_function<<endl;
cout<<"=========================================="<<endl;
cout<<"type :"<<drone1.battery_status.type<<endl;
cout<<"=========================================="<<endl;
cout<<"battery_remaining :"<<drone1.battery_status.battery_remaining<<endl;
cout<<"=========================================="<<endl;
cout<<"time_remaining :"<<drone1.battery_status.time_remaining<<endl;
cout<<"=========================================="<<endl;
cout<<"charge_state :"<<drone1.battery_status.charge_state<<endl;
}
else if(buffer_command=="m")
{
......@@ -100,6 +128,7 @@ drone1.command_setMode(DRONE_OFF);
else if(buffer_command=="p")
{
drone1.go_to();
}
else if(buffer_command=="f")
{
......@@ -107,17 +136,32 @@ drone1.command_setMode(DRONE_OFF);
cout<<"fly \n";
ret = drone1.take_off();
cout<<"fly ret = "<<ret<<endl;
sleep_for(seconds(10));
}
cout<<"test"<<drone1.altitude.altitude_local<<endl;
//cout<<"pos"<<altitude.altitude_local<<endl;
}
else if(buffer_command=="g")
{
cout<<"===================CORRDONNEES===================="<<endl;
cout<<"HOME_LATITUDE_DEG"<<drone1.home_position.latitude<<endl;
cout<<"_______________________"<<endl;
cout<<"HOME_LONGITUDE_DEG"<<drone1.home_position.longitude<<endl;
cout<<"_______________________"<<endl;
cout<<"HOME_ALTITUDE_DEG"<<drone1.home_position.altitude<<endl;
cout<<"_______________________"<<endl;
cout<<"HOME_X_metres"<<drone1.home_position.x<<endl;
cout<<"_______________________"<<endl;
cout<<"HOME_Y_metres"<<drone1.home_position.y<<endl;
cout<<"_______________________"<<endl;
cout<<"HOME_Z_metres"<<drone1.home_position.z<<endl;
cout<<"_______________________"<<endl;
cout<<"BOUSSOLE"<<drone1.highres_imu.xmag<<endl;
}
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";
......@@ -146,12 +190,7 @@ drone1.command_setMode(DRONE_OFF);
}
*/
else if(buffer_command=="o")
{
cout<<"right \n";
drone1.move_to(1,0,0);
}
else if(buffer_command=="r")
{
cout<<"right \n";
......@@ -176,35 +215,36 @@ drone1.command_setMode(DRONE_OFF);
{
drone1.move_drone_to(1,0,0);
}
/* else if(buffer_command=="n")
{
drone1.waypoint(0.1,0.1,0.1);
} */
else if(buffer_command=="y"){
drone1.move_to(1,1,1);
}
else if(buffer_command=="z")
{
cout<<"motor 1 \n";
drone1.command_directControl(1000,0,0,0);
drone1.move_drone_to(0,0.01,0); //move forward 0.01m/s
}
else if(buffer_command=="s")
{
cout<<"motor 1 \n";
drone1.command_directControl(-1000,0,0,0);
drone1.move_drone_to(0,-0.01,0); //move backward 0.01m/s
}
else if(buffer_command=="d")
{
drone1.command_directControl(0,1000,0,0);
drone1.move_drone_to(0.01,0,0); //move right 0.01m/s
}
else if(buffer_command=="q")
{
drone1.command_directControl(0,-1000,0,0);
drone1.move_drone_to(-0.01,0,0); //move left 0.01m/s
}
else if(buffer_command=="w")
{
drone1.command_directControl(0,0,1000,0);
drone1.move_drone_to(0,0,1); //UP for 0.01m/s
}
else if(buffer_command=="x")
{
drone1.command_directControl(0,0,-1000,0);
drone1.move_drone_to(0,0,-0.01); //DOWN for 0.01m/s
}
else if(buffer_command=="c")
{
......@@ -271,7 +311,39 @@ drone1.command_setMode(DRONE_OFF);
// drone1.get()->write_message(message);
// }
/**
* @fn void signalHandler(int number)
* @brief Handles received signals. Triggered when a signal is received.
*
* @param number The signal code.
**/
void signalHandler(int number)
{
switch(number)
{
case SIGINT :
printf("SIGINT caught\n"); //CTRL+C
exit(0);
//TODO handle SIGINT reception situation
break;
case SIGTERM :
printf("SIGTERM caught\n"); //kill (term command)
//TODO handle SIGTERM reception situation
exit(0);
break;
case SIGFPE :
printf("SIGFPE caught\n"); //Floating-Point arithmetic Exception
//TODO handle SIGFPE reception situation
exit(0); //to prevent infinite loop
break;
default :
printf("Signal number : %d caught\n", number);
}
}
/**
* Only the main exe of the programm
......@@ -282,8 +354,18 @@ int main(int argc, char *argv[])
// General var
// bool flag_run = true; //Flag use for the main loop of the system
struct sigaction action;
if(argc>1) {strcpy(device_path ,argv[1]); printf("port = %s \n",device_path);}
//signal handler initialisation
sigfillset(&action.sa_mask);
action.sa_handler = signalHandler;
action.sa_flags = 0;
//Make the list of all SIG to catch (will not be caught if not listed here, the list is not definitive and all are not necessarily usefull)
if(sigaction(SIGINT, &action, NULL) != 0) printf("SIGINT couldn't be attached\n"); //ctrl+C
if(sigaction(SIGTERM, &action, NULL) != 0) printf("SIGTERM couldn't be attached\n"); //kill [PID]
if(sigaction(SIGFPE, &action, NULL) != 0) printf("SIGFPE couldn't be attached\n"); //integer divided by 0
drone1.open(device_path, device_baudrate);
string buffer1="";
......
Com_server/test/include/Data_Drone.h
View file @ 99e0cd04
......@@ -161,6 +161,7 @@ public:
mavlink_highres_imu_t highres_imu; //Accelerometers of the system
mavlink_altitude_t altitude; //Altitude data
mavlink_home_position_t home_position;
mavlink_attitude_t attitude; //Attitude of the drone use to manual control and target attitude
mavlink_attitude_target_t attitude_target; //Attitude of the drone use to manual control and target attitude
......@@ -238,6 +239,7 @@ public:
// int command_setModeGuided(float param1);
int make_command_msg_rate(int param1);
int command_directControl(float x, float y, float z, float r);
int move_drone_to(float x, float y, float z);
int return_to_launch(int param1);
int take(int param1) ;
int take_off();
......@@ -245,14 +247,20 @@ public:
int command_right(float param1) ;
int command_left(float param1) ;
int go_to();
int waypointrl(float param5);
int waypoint(float param5, float param6, float param7);
int battery_check(float x);
int waypointforback(float param6);
int waypointupdown(float param7);
int take_off_vtol();
int followhold();
int make_mode(int param1);
int get_home_x(float x);
int request_home_position(float x, float y, float z);
void send_setpoint_velocity(float vx, float vy, float vz);
int move_to(float param1, float param2, float param3);
......
Com_server/test/src/Com_SerialReadingThread.cpp
View file @ 99e0cd04
......@@ -358,7 +358,7 @@ void Serial_Port_ReadingThread::read_messages(mavlink_message_t message)
printf("Type: %d ", hb.type);
printf("Autopilot type: %d ", hb.autopilot);
printf("System mode: %d ", hb.base_mode);
printf("Custom mode: %d ", hb.custom_mode);
// printf("Custom mode: %ald ", hb.custom_mode);
printf("System status: %d ", hb.system_status);
printf("Mavlink version: %d \n", hb.mavlink_version);
......@@ -625,7 +625,16 @@ void Serial_Port_ReadingThread::read_messages(mavlink_message_t message)
printf("altitude_local %f altitude_relative %f altitude_terrain %f bottom_clearance %f\n",altitude->altitude_local , altitude->altitude_relative, altitude->altitude_terrain,altitude->bottom_clearance );
break;
}
}
/* case MAVLINK_MSG_ID_HOME_POSITION:
{
printf("MAVLINK_MSG_ID_HOME_POSITION \n");
//mavlink_home_position_t* home_position;
mavlink_msg_home_position_decode(&message, &drone1.home_position);
//home_position= &drone1.home_position;
printf("latitude %d, longitude %d, altitude %d \n", drone1.home_position.latitude, drone1.home_position.longitude, drone1.home_position.altitude);
break;
} */
case MAVLINK_MSG_ID_VIBRATION:
{
// printf("MAVLINK_MSG_ID_VIBRATION ???\n");
......
Com_server/test/src/Com_SerialWritingThread.cpp
View file @ 99e0cd04
......@@ -74,6 +74,7 @@ void Serial_Port_WritingThread::run()
}
}
int i = 0;
int Serial_Port_WritingThread::main_loop()
{
......@@ -81,10 +82,9 @@ int Serial_Port_WritingThread::main_loop()
//cout<<"__FUNCTION__"<<endl;
if(blc_control_commands.floats[0]>0.5 && drone1.motors == UNARM)
{
drone1.command_setMode(DRONE_OFF);
//drone1.command_setMode(DRONE_OFF);
cout<<" command_arm 1\n";
drone1.command_arm(1);
drone1.motors = ARM;
......@@ -100,6 +100,8 @@ int Serial_Port_WritingThread::main_loop()
{
cout<<" takeoff 0 \n";
drone1.take_off();
}
if(blc_control_commands.floats[3]>0.5 && drone1.motors == ARM)
......@@ -107,51 +109,71 @@ int Serial_Port_WritingThread::main_loop()
cout<<" landing 0 \n";
drone1.landing();
}
if(blc_control_commands.floats[4]>0.5 && drone1.motors == ARM)
if(blc_control_commands.floats[4]>0.5 && drone1.motors == ARM)
{
cout<<" Return to llaunch0 \n";
drone1.return_to_launch(1);
}
if(blc_control_commands.floats[5]>0.5 && drone1.motors == ARM)
{
cout <<"UPPPPP 1\n";
drone1.waypoint(0,0,5);
}
if(blc_control_commands.floats[6]>0.5 && drone1.motors == ARM)
{
cout <<"DOWNNN 1\n";
drone1.waypoint(0,0,3);
}
if(blc_control_motors.floats[0]>0.5 && drone1.motors == ARM)
{
cout <<"move_to _right 1\n";
drone1.command_directControl(0,1000,0,0);
drone1.move_drone_to(0.5,0,0); //move right 0.5m/s
}
if(blc_control_motors.floats[1]>0.5 && drone1.motors == ARM)
{
cout <<"move_to left 1\n";
drone1.command_directControl(0,-1000,0,0);
drone1.move_drone_to(-0.5,0,0); //move left 0.5m/s
}
/*
if(blc_control_motors.floats[2]>0.5 && drone1.motors == ARM)
{
cout <<"move_forward 1\n";
drone1.command_directControl(1000,0,0,0);
drone1.move_drone_to(0,0.5,0);
}
if(blc_control_motors.floats[3]>0.5 && drone1.motors == ARM)
{
cout <<"move_backward 1\n";
drone1.command_directControl(-1000,0,0,0);
} */
/*
if(blc_control_motors.floats[1]>0.5 && drone1.motors == ARM)
drone1.move_drone_to(0,-0.5,0);
}
if(blc_control_motors.floats[4]>0.5 && drone1.motors == ARM)
{
cout <<"TURN RIGHT 1\n";
drone1.command_directControl(0,0,0,1000);
}
if(blc_control_motors.floats[5]<0.5 && drone1.motors == ARM)
{
cout <<"TURN LEFT 1\n";
drone1.command_directControl(0,0,0,-1000);
}
/* if(blc_control_commands.floats[7]>0.5 && drone1.motors == ARM)
{
cout <<"forward 1\n";
float f =0.01;
f = f + 0.01;
drone1.waypointforback(f);
cout <<"UP 1\n";
drone1.command_directControl(0,0,0,1000);
}
if(blc_control_motors.floats[3]<0.5 && drone1.motors == ARM)
if(blc_control_commands.floats[8]<0.5 && drone1.motors == ARM)
{
cout <<"backward 1\n";
float b = -0.01;
b = b - 0.01;
drone1.waypointforback(b);
cout <<"DOWN 1\n";
drone1.command_directControl(0,0,0,-1000);
} */
/*
if(blc_control_commands.floats[7]>0.5 && drone1.motors == ARM)
......
Com_server/test/src/Data_Drone.cpp
View file @ 99e0cd04
......@@ -6,6 +6,8 @@
* @version 1.0
*/
#include <chrono>
#include <ctime>
#include <iostream>
#include <string>
......@@ -82,8 +84,24 @@ int Drone::init_communication()
return 0;
}
/*
void Drone::send_setpoint_velocity(float vx, float vy, float vz) {
/* Documentation start from bit 1 instead 0,
* but implementation PX4 Firmware #1151 starts from 0
*/
// uint16_t ignore_all_except_v_xyz = (7<<6)|(7<<0);
// ENU->NED. Issue #49.
/* set_position_target_local_ned(std::chrono::system_clock::now(),
MAV_FRAME_LOCAL_NED,
ignore_all_except_v_xyz,
0.0, 0.0, 0.0,
vy, vx, -vz,
0.0, 0.0, 0.0);
}
*/
/**
* Initialisation of a drone
*/
......@@ -214,7 +232,7 @@ int Drone::make_command_msg_rate(int param1)
int Drone::waypointrl(float param5)
int Drone::waypoint(float param5, float param6, float param7)
{
//Message buffer
mavlink_message_t message;
......@@ -223,14 +241,14 @@ int Drone::waypointrl(float param5)
command.target_system = 1;
command.target_component = 1;
command.command = MAV_CMD_NAV_WAYPOINT;
command.confirmation = false;
command.param1 = 0; //Hold time. (ignored by fixed wing, time to stay at waypoint for rotary wing) min : 0 (seconds)
command.confirmation = true;
command.param1 = 100; //Hold time. (ignored by fixed wing, time to stay at waypoint for rotary wing) min : 0 (seconds)
command.param2 = 0; //Acceptance radius (if the sphere with this radius is hit, the waypoint counts as reached) min 0 (m)
command.param3 = 0; //0 to pass through the WP, if > 0 radius to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control. (m)
command.param4 = 0; //Desired yaw angle at waypoint (rotary wing). NaN to use the current system yaw heading mode (e.g. yaw towards next waypoint, yaw to home, etc.). (deg)
command.param5 = 0; //Latitude (m)
// command.param6 = 0; //Longitude
// command.param7 = 0; //Altitude
command.param5 = param5; //Latitude (m)
command.param6 = param6; //Longitude
command.param7 = param7; //Altitude
mavlink_msg_command_long_encode(255, 1, &message, &command);
return write_message(message);
}
......@@ -337,14 +355,14 @@ int Drone::take_off()
command.target_component = 1;
command.command = MAV_CMD_NAV_TAKEOFF;
command.confirmation = true ;
command.param1 = 245; //Minimum pitch (if airspeed sensor present), desired pitch without sensor
// command.param1 = 245; //Minimum pitch (if airspeed sensor present), desired pitch without sensor
command.param2 = 1e+06;
command.param3 = NAN;
command.param4 = NAN; //Yaw angle (if magnetometer present), ignored without magnetometer. NaN to use the current system yaw heading mode (e.g. yaw towards next waypoint, yaw to home, etc.).
command.param5 = NAN; //global_position_int.lat; // local_position_ned.x; //Latitude
command.param6 = NAN; //global_position_int.lon; //local_position_ned.y; //longitude
// printf("departure alt = %d \n",departure_alt);
command.param7 = NAN; // 10m altitude
command.param5 =NAN ; //global_position_int.lat; // local_position_ned.x; //Latitude
command.param6 =NAN; //global_position_int.lon; //local_position_ned.y; //longitude
printf("departure alt = %d \n",departure_alt);
command.param7 = 1 ; // 10m altitude
mavlink_msg_command_long_encode(255, 1, &message, &command);
......@@ -422,10 +440,10 @@ int Drone::landing()
command.param1 = 1; //Minimum target altitude if landing is aborted (0 = undefined/use system default).
command.param2 = 0; //Precision land mode. //PRECISION_LAND_MODE
command.param3 = 0; //Empty
command.param4 = NAN; //Desired yaw angle. NaN to use the current system yaw heading mode (e.g. yaw towards next waypoint, yaw to home, etc.).
command.param5 = gps_raw_int.lat; //Latitude
command.param6 = gps_raw_int.lat; //Longitude
command.param7 = departure_alt; //Landing altitude (ground level in current frame).
command.param4 = 1.5; //Desired yaw angle. NaN to use the current system yaw heading mode (e.g. yaw towards next waypoint, yaw to home, etc.).
command.param5 = 1; //Latitude
command.param6 = 1; //Longitude
command.param7 = 10; //Landing altitude (ground level in current frame).
mavlink_msg_command_long_encode(255, 1, &message, &command);
......@@ -434,6 +452,8 @@ int Drone::landing()
//============================================TEST=============================================================
int Drone::move_to(float param1, float param2, float param3)
{
int16_t xx,yy,zz;
......@@ -445,9 +465,9 @@ int Drone::move_to(float param1, float param2, float param3)
command = mavlink_newCommand();
command.target_system = 1;
command.target_component = 1;
command.command = MAV_FRAME_LOCAL_NED;
command.command = MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED;
command.confirmation = false;
command.param1 = param1;
command.param5 = 1;
mavlink_msg_command_long_encode(255, 1, &message, &command);
......@@ -458,7 +478,6 @@ int Drone::move_to(float param1, float param2, float param3)
{
//Display_IHM::getInstanceOf().printLog("STOP ARMING");
}
return write_message(message);
}
......@@ -504,7 +523,7 @@ int Drone::command_kill(int param1)
command.target_system = 1;
command.target_component = 1;
command.command = MAV_CMD_DO_FLIGHTTERMINATION;
command.confirmation = false;
command.confirmation = true;
command.param1 = param1;
mavlink_msg_command_long_encode(255, 1, &message, &command);
......@@ -578,6 +597,86 @@ int Drone::command_directControl(float x, float y, float z, float r)
// return 1;
}
int Drone::battery_check(float x)
{
cout<<"current_consumed :"<<battery_status.current_consumed<<endl;
cout<<"=========================================="<<endl;
cout<<"energy_consumed :"<<battery_status.energy_consumed<<endl;
cout<<"=========================================="<<endl;
cout<<"temperature :"<<battery_status.temperature<<endl;
cout<<"=========================================="<<endl;
cout<<"voltages :"<<battery_status.voltages[10]<<endl;
cout<<"=========================================="<<endl;
cout<<"current_battery :"<<battery_status.current_battery<<endl;
cout<<"=========================================="<<endl;
cout<<"battery_function :"<<battery_status.battery_function<<endl;
cout<<"=========================================="<<endl;
cout<<"type :"<<battery_status.type<<endl;
cout<<"=========================================="<<endl;
cout<<"battery_remaining :"<<battery_status.battery_remaining<<endl;
cout<<"=========================================="<<endl;
cout<<"time_remaining :"<<battery_status.time_remaining<<endl;
cout<<"=========================================="<<endl;
cout<<"charge_state :"<<battery_status.charge_state<<endl;
}
int Drone::move_drone_to(float vx, float vy, float vz)
{
mavlink_message_t message;
uint8_t system_id = 255;
uint8_t component_id = 1;
uint32_t time_boot_ms = 1000;
uint8_t target_system = 1;
uint8_t target_component = 1;
uint8_t coordinate_frame = MAV_FRAME_LOCAL_NED; //Valid options are: MAV_FRAME_LOCAL_NED = 1, MAV_FRAME_LOCAL_OFFSET_NED = 7, MAV_FRAME_BODY_NED = 8, MAV_FRAME_BODY_OFFSET_NED = 9
uint16_t mask = 0b0000111111000111; // only speed enabled //itmask to indicate which dimensions should be ignored by the vehicle: a value of 0b0000000000000000 or 0b0000001000000000 indicates that none of the setpoint dimensions should be ignored.
float x =0 ; // X Position in NED frame in meters
float y = 0; // Y Position in NED frame in meters
float z =0; // Z Position in NED frame in meters (note, altitude is negative in NED)
float vvx = vx; // X velocity in NED frame in meter / s
float vvy = vy; //Y velocity in NED frame in meter / s
float vvz =