Commit 58ed71c4 authored by Amine AIT-GANI's avatar Amine AIT-GANI
Browse files

Mise a jour des fonctions de test

parent ac713d82
Pipeline #1535 failed with stage
in 1 minute and 32 seconds
......@@ -10,7 +10,7 @@
#include <chrono>
#include <iostream>
#include <fstream>
#include <cmath>
#include <sys/time.h>
......@@ -22,6 +22,9 @@ using namespace std;
#include "../include/global_variables.h"
extern ofstream MyFile;
extern ofstream compassx;
extern ofstream compassy;
//#define DISPLAY
......@@ -433,10 +436,44 @@ void Serial_Port_ReadingThread::read_messages(mavlink_message_t message)
blc_highres_imu.floats[7] = drone1.highres_imu.ymag;
blc_highres_imu.floats[8] = drone1.highres_imu.zmag;
// cout<<"Com_SerialReadingThread "<<buffer1<<endl;
double xmag = drone1.highres_imu.xmag;
double ymag = drone1.highres_imu.ymag;
double angle;
if ( xmag>=-0.7 && xmag<=0.7 ) {
angle = acos(xmag);
cout << "acos(xmag) = " << angle << " radians" << endl;
cout << "acos(x) = " << angle*180/3.1415 << " degrees" << endl;
}
else if ( ymag>=-0.7 && ymag<=0.7 )
{
angle = acos(ymag);
cout << "acos(ymag) = " << angle << " radians" << endl;
cout << "acos(x) = " << angle*180/3.1415 << " degrees" << endl;
}
cout << "xmag : " << drone1.highres_imu.xmag << "ymag :" << drone1.highres_imu.ymag << "zmag :" << drone1.highres_imu.zmag << endl;
compassx << drone1.highres_imu.xmag << endl;
compassy << drone1.highres_imu.ymag << endl;
cout << "buffer 1 mag" << buffer1 << endl;
cout<<"Com_SerialReadingThread "<<buffer1<<endl;
break;
}
/* case MAVLINK_MSG_ID_COMPASSMOT_STATUS:
mavlink_compassmot_status_decode(&message, &drone1.compassmot_status);
buffer1 = buffer1+" CompensationX: "+std::to_string(drone1.compassmot_status.CompensationX)+" CompensationY:"+std::to_string(drone1.compassmot_status.CompensationY)+" CompensationZ :"+std::to_string(drone1.compassmot_status.CompensationZ)+"\n";
blc_compassmot_status.floats[0] = drone1.compassmot_status.CompensationX;
blc_compassmot_status.floats[1] = drone1.compassmot_status.CompensationY;
blc_compassmot_status.floats[2] = drone1.compassmot_status.CompensationZ;
// cout<<"Com_SerialReadingThread "<<buffer1<<endl;
break;
*/
case MAVLINK_MSG_ID_GPS_RAW_INT:
{
// cout << "MAVLINK_MSG_ID_GPS_RAW_INT:" << endl;
......
......@@ -81,9 +81,11 @@ int Serial_Port_WritingThread::main_loop()
//cout<<"__FUNCTION__"<<endl;
if(blc_control_commands.floats[0]>0.5 && drone1.motors == UNARM)
{
cout<<" command_arm 1\n";
{
drone1.command_setMode(DRONE_OFF);
cout<<" command_arm 1\n";
drone1.command_arm(1);
drone1.motors = ARM;
}
......@@ -100,8 +102,76 @@ int Serial_Port_WritingThread::main_loop()
drone1.take_off();
}
if(blc_control_commands.floats[3]>0.5 && drone1.motors == ARM)
{
cout<<" landing 0 \n";
drone1.landing();
}
if(blc_control_commands.floats[4]>0.5 && drone1.motors == ARM)
{
cout<<" Return to llaunch0 \n";
drone1.return_to_launch(1);
}
if(blc_control_motors.floats[0]>0.5 && drone1.motors == ARM)
{
cout <<"move_to _right 1\n";
drone1.command_directControl(0,1000,0,0);
}
if(blc_control_motors.floats[1]>0.5 && drone1.motors == ARM)
{
cout <<"move_to left 1\n";
drone1.command_directControl(0,-1000,0,0);
}
/*
if(blc_control_motors.floats[2]>0.5 && drone1.motors == ARM)
{
cout <<"move_forward 1\n";
drone1.command_directControl(1000,0,0,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)
{
cout <<"forward 1\n";
float f =0.01;
f = f + 0.01;
drone1.waypointforback(f);
}
if(blc_control_motors.floats[3]<0.5 && drone1.motors == ARM)
{
cout <<"backward 1\n";
float b = -0.01;
b = b - 0.01;
drone1.waypointforback(b);
} */
/*
if(blc_control_commands.floats[7]>0.5 && drone1.motors == ARM)
{
cout <<"up 1\n";
float u = 0.01;
u = u + 0.01;
drone1.waypointupdown(u);
}
if(blc_control_commands.floats[8]>0.5 && drone1.motors == ARM)
{
cout <<"down 1\n";
float d =0.01;
d = d -0.01;
drone1.waypointupdown(d);
}
*/
//drone1.return_to_launch(1); // retour au position initiale
/* float offset=0.05;
......
......@@ -136,7 +136,7 @@ int Drone::command_arm(int param1)
command.command = MAV_CMD_COMPONENT_ARM_DISARM;
command.confirmation = false;
command.param1 = param1;
command.param2 = 21196;
command.param2 = 21196; //21196; Force Arm/disarm 0 // ????
mavlink_msg_command_long_encode(255, 1, &message, &command);
......@@ -152,6 +152,155 @@ int Drone::command_arm(int param1)
}
int Drone::make_mode(int param1)
{
//Message buffer
mavlink_message_t message;
mavlink_command_long_t command;
command=mavlink_newCommand();
command.target_system = 1;
command.target_component = 1;
command.command = MAV_CMD_DO_SET_MODE;
// command.confirmation = false;
command.param1 = 193;
command.param2 = 1; //21196; // ????
command.param3 = 0;
mavlink_msg_command_long_encode(255, 1, &message, &command);
if(param1 >0)
{
//Display_IHM::getInstanceOf().printLog("ARMING");
}else
{
//Display_IHM::getInstanceOf().printLog("STOP ARMING");
}
return write_message(message);
}
int Drone::make_command_msg_rate(int param1)
{
float interval_us = 0.0f;
//Message buffer
mavlink_message_t message;
mavlink_command_long_t command;
command=mavlink_newCommand();
command.target_system = 1;
command.target_component = 1;
command.command = MAV_CMD_SET_MESSAGE_INTERVAL;
command.param1 = 1; //rate
command.param2 = 1; //intervall en us
mavlink_msg_command_long_encode(255, 1, &message, &command);
if(param1 >0)
{
//Display_IHM::getInstanceOf().printLog("ARMING");
}else
{
//Display_IHM::getInstanceOf().printLog("STOP ARMING");
}
return write_message(message);
}
int Drone::waypointrl(float param5)
{
//Message buffer
mavlink_message_t message;
mavlink_command_long_t command;
command=mavlink_newCommand();
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.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
mavlink_msg_command_long_encode(255, 1, &message, &command);
return write_message(message);
}
int Drone::waypointforback(float param6)
{
//Message buffer
mavlink_message_t message;
mavlink_command_long_t command;
command=mavlink_newCommand();
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.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
mavlink_msg_command_long_encode(255, 1, &message, &command);
return write_message(message);
}
int Drone::waypointupdown(float param7)
{
//Message buffer
mavlink_message_t message;
mavlink_command_long_t command;
command=mavlink_newCommand();
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.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
mavlink_msg_command_long_encode(255, 1, &message, &command);
return write_message(message);
}
int Drone::go_to()
{
//Message buffer
mavlink_message_t message;
mavlink_command_long_t command;
command=mavlink_newCommand();
command.target_system = 1;
command.target_component = 1;
command.command = MAV_CMD_DO_REPOSITION;
command.confirmation = false;
command.param1 = -1;
command.param4 = NAN;
command.param5 = 0;
command.param6 = 0;
command.param7 = 1;
mavlink_msg_command_long_encode(255, 1, &message, &command);
return write_message(message);
}
int Drone::return_to_launch(int param1)
{
......@@ -162,8 +311,7 @@ int Drone::return_to_launch(int param1)
command.target_system = 1;
command.target_component = 1;
command.command = MAV_CMD_NAV_RETURN_TO_LAUNCH;
command.confirmation = false;
command.param1 = param1;
// command.param1 = param1;
mavlink_msg_command_long_encode(255, 1, &message, &command);
......@@ -187,14 +335,43 @@ int Drone::take_off()
command = mavlink_newCommand();
command.target_system = 1;
command.target_component = 1;
command.command = MAV_CMD_NAV_TAKEOFF;
command.command = MAV_CMD_NAV_TAKEOFF;
command.confirmation = true ;
command.param1 = 0; //Pitch
command.param2 = 0;
command.param3 = 0;
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
mavlink_msg_command_long_encode(255, 1, &message, &command);
cout <<"param1"<<command.param1<<"param2"<<command.param2<<"param3"<<command.param3<<"param4 "<<command.param4<<"param5 "<<command.param5<<"param6 "<<command.param6<<"param7 "<<command.param7<<endl;
return write_message(message);
}
int Drone::take_off_vtol()
{
//Message buffer
mavlink_message_t message;
mavlink_command_long_t command;
printf("=========================== take_off Demande au drone de monter en altitude \n");
command = mavlink_newCommand();
command.target_system = 1;
command.target_component = 1;
command.command = MAV_CMD_NAV_VTOL_TAKEOFF;
command.confirmation = true ;
// command.param1 = 0; //Pitch
//command.param2 = VTOL_TRANSITION_HEADING;
//command.param3 = 0;
command.param4 = NAN; //YAW
command.param5 = gps_raw_int.lat; //global_position_int.lat; // local_position_ned.x; //Latitude
command.param6 = gps_raw_int.lon; //global_position_int.lon; //local_position_ned.y; //longitude
command.param6 = gps_raw_int.lon; //global_position_int.lon; //local_position_ned.y; //longitude
printf("departure alt = %d \n",departure_alt);
command.param7 = departure_alt+1000; // 10m altitude
......@@ -203,6 +380,33 @@ int Drone::take_off()
return write_message(message);
}
int Drone::followhold()
{
//Message buffer
mavlink_message_t message;
mavlink_command_long_t command;
printf("=========================== take_off Demande au drone de monter en altitude \n");
command = mavlink_newCommand();
command.target_system = 1;
command.target_component = 1;
command.command = MAV_CMD_DO_FOLLOW;
command.confirmation = true ;
command.param1 = 0; //Hold mode
//command.param2 = VTOL_TRANSITION_HEADING;
//command.param3 = 0;
command.param4 = 2; //Altitude mode: 0: Keep current altitude, 1: keep altitude difference to target, 2: go to a fixed altitude above home.
command.param5 = departure_alt+1000; // 10m altitudeAltitude above home. (used if mode=2)
//command.param6 = gps_raw_int.lon; //global_position_int.lon; //local_position_ned.y; //longitude
printf("departure alt = %d \n",departure_alt);
command.param7 = 1; //Time to land in which the MAV should go to the default position hold mode after a message RX timeout.
mavlink_msg_command_long_encode(255, 1, &message, &command);
return write_message(message);
}
int Drone::landing()
{
......@@ -215,13 +419,13 @@ int Drone::landing()
command.target_component = 1;
command.command = MAV_CMD_NAV_LAND;
command.confirmation = true ;
command.param1 = 1;
command.param2 = 0;
command.param3 = 0;
command.param4 = NAN;
command.param5 = gps_raw_int.lat;
command.param6 = gps_raw_int.lat;
command.param7 = departure_alt;
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).
mavlink_msg_command_long_encode(255, 1, &message, &command);
......@@ -314,12 +518,11 @@ int Drone::command_kill(int param1)
return write_message(message);
}
int Drone::command_setModeGuided(float param1)
/*int Drone::command_setModeGuided(float param1)
{
//Message buffer
mavlink_message_t message;
mavlink_command_long_t command;
command=mavlink_newCommand();
command.target_system = 1;
command.target_component = 1;
......@@ -365,7 +568,10 @@ int Drone::command_directControl(float x, float y, float z, float r)
int16_t xx,yy,zz,rr;
xx=(int16_t)x; yy=(int16_t)y; zz=(int16_t)z; rr=(int16_t)r;
// mavlink_msg_manual_control_pack(255, 1, &message, 1, 0, xx, yy, zz, rr);
mavlink_msg_manual_control_pack(255, 1, &message, 1, xx, yy, zz, rr,1);
mavlink_msg_manual_control_pack(255, 1, &message, 1, xx, yy, zz, rr,1); //xx : Generally corresponds to forward(1000)-backward(-1000) movement on a joystick and the pitch of a vehicle.
//yy : Generally corresponds to left(-1000)-right(1000) movement on a joystick and the roll of a vehicle.
//zz : Generally corresponds to a separate slider movement with maximum being 1000 and minimum being -1000 on a joystick and the thrust of a vehicle.
//rr : Generally corresponds to a twisting of the joystick, with counter-clockwise being 1000 and clockwise being -1000, and the yaw of a vehicle.
cout<<"command_directControl : "<<xx<<" "<<yy<<" "<<zz<<" "<<rr<<" "<<endl;
return write_message(message);
......@@ -534,13 +740,99 @@ master.mav.command_long_send( master.target_system, master.target_component, mav
Follow me: base_mode:157, main_mode:4, sub_mode:8
*/
int Drone::command_setMode(Drone_mode mode)
/*
std::pair<MavlinkCommandSender::Result, MavlinkCommandSender::CommandLong>
SystemImpl::make_command_flight_mode(FlightMode flight_mode, uint8_t component_id)
{
const uint8_t flag_safety_armed = is_armed() ? MAV_MODE_FLAG_SAFETY_ARMED : 0;
const uint8_t flag_hitl_enabled = _hitl_enabled ? MAV_MODE_FLAG_HIL_ENABLED : 0;
const uint8_t mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED | flag_safety_armed | flag_hitl_enabled;
// Note: the safety flag is not needed in future versions of the PX4 Firmware
// but want to be rather safe than sorry.
uint8_t custom_mode = px4::PX4_CUSTOM_MAIN_MODE_AUTO;
uint8_t custom_sub_mode = 0;
switch (flight_mode) {
case FlightMode::Hold:
custom_sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
break;
case FlightMode::ReturnToLaunch:
custom_sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_RTL;
break;
case FlightMode::Takeoff:
custom_sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF;
break;
case FlightMode::Land:
custom_sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_LAND;
break;
case FlightMode::Mission:
custom_sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_MISSION;
break;
case FlightMode::FollowMe:
custom_sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET;
break;
case FlightMode::Offboard:
custom_mode = px4::PX4_CUSTOM_MAIN_MODE_OFFBOARD;
break;
case FlightMode::Manual:
custom_mode = px4::PX4_CUSTOM_MAIN_MODE_MANUAL;
break;
case FlightMode::Posctl:
custom_mode = px4::PX4_CUSTOM_MAIN_MODE_POSCTL;
break;
case FlightMode::Altctl:
custom_mode = px4::PX4_CUSTOM_MAIN_MODE_ALTCTL;
break;
case FlightMode::Rattitude:
custom_mode = px4::PX4_CUSTOM_MAIN_MODE_RATTITUDE;
break;
case FlightMode::Acro:
custom_mode = px4::PX4_CUSTOM_MAIN_MODE_ACRO;
break;
case FlightMode::Stabilized:
custom_mode = px4::PX4_CUSTOM_MAIN_MODE_STABILIZED;
break;
default:
LogErr() << "Unknown Flight mode.";
MavlinkCommandSender::CommandLong empty_command{};
return std::make_pair<>(MavlinkCommandSender::Result::UnknownError, empty_command);
}
MavlinkCommandSender::CommandLong command{};
command.command = MAV_CMD_DO_SET_MODE;
command.params.param1 = float(mode);
command.params.param2 = float(custom_mode);
command.params.param3 = float(custom_sub_mode);
command.target_component_id = component_id;
return std::make_pair<>(MavlinkCommandSender::Result::Success, command);
}
*/
//int Drone::command_setMode(Drone_mode my_mode)
int Drone::command_setMode(Drone_mode my_mode)
{
switch(mode)
const uint8_t flag_safety_armed = MAV_MODE_FLAG_SAFETY_ARMED; // = is_armed() ? MAV_MODE_FLAG_SAFETY_ARMED : 0;
const uint8_t flag_hitl_enabled = MAV_MODE_FLAG_HIL_ENABLED; // = _hitl_enabled ? MAV_MODE_FLAG_HIL_ENABLED : 0;
const uint8_t mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED | flag_safety_armed | flag_hitl_enabled;
// Drone_mode my_mode;
// Note: the safety flag is not needed in future versions of the PX4 Firmware
// but want to be rather safe than sorry.
uint8_t custom_mode = px4::PX4_CUSTOM_MAIN_MODE_AUTO;
uint8_t custom_sub_mode = 0;
FlightMode flight_mode = FlightMode::Hold;
custom_sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
/* switch(my_mode)
{
case DRONE_OFF:
//We must reset all value to 0
Drone::mode=mode;
//We must reset all value to 0
Drone::mode=flight_mode;
break;
case DRONE_MANUAL_DIRECT:
......@@ -549,17 +841,94 @@ int Drone::command_setMode(Drone_mode mode)
//beep();
}else{
//Display_IHM::getInstanceOf().printLog("Server: mode manual_direct");
Drone::mode=mode;
Drone::mode=flight_mode;
}
break;
default:
break;
}
return 0;
return 0;
*/
// FlightMode flight_mode = FlightMode::Hold;
switch (flight_mode) {
case FlightMode::Hold:
custom_sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
break;
case FlightMode::ReturnToLaunch:
custom_sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_RTL;
break;
case FlightMode::Takeoff:
custom_sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF;
break;
case FlightMode::Land:
custom_sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_LAND;
break;
case FlightMode::Mission:
custom_sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_MISSION;
break;
case FlightMode::FollowMe:
custom_sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET;
break;
case FlightMode::Offboard:
custom_mode = px4::PX4_CUSTOM_MAIN_MODE_OFFBOARD;
break;
case FlightMode::Manual:
custom_mode = px4::PX4_CUSTOM_MAIN_MODE_MANUAL;
break;
case FlightMode::Posctl:
custom_mode = px4::PX4_CUSTOM_MAIN_MODE_POSCTL;
break;
case FlightMode::Altctl:
custom_mode = px4::PX4_CUSTOM_MAIN_MODE_ALTCTL;
break;
case FlightMode::Rattitude:
custom_mode = px4::PX4_CUSTOM_MAIN_MODE_RATTITUDE;
break;
case FlightMode::Acro: