Commit 6c0fff99 authored by Amine AIT-GANI's avatar Amine AIT-GANI
Browse files

Mise a jour des fonctions de test

parent 4eeebb0a
Pipeline #1454 failed with stage
in 1 minute and 51 seconds
......@@ -136,6 +136,7 @@ int Drone::command_arm(int param1)
command.command = MAV_CMD_COMPONENT_ARM_DISARM;
command.confirmation = false;
command.param1 = param1;
command.param2 = 21196;
mavlink_msg_command_long_encode(255, 1, &message, &command);
......@@ -226,6 +227,40 @@ int Drone::landing()
return write_message(message);
}
//============================================TEST=============================================================
int Drone::move_to(float param1, float param2, float param3)
{
int16_t xx,yy,zz;
xx=(int16_t)param1; yy=(int16_t)param2; zz=(int16_t)param3;
//Message buffer
mavlink_message_t message;
mavlink_command_long_t command;
// printf("Demande au drone de tourner vers la droite \n");
command = mavlink_newCommand();
command.target_system = 1;
command.target_component = 1;
command.command = MAV_FRAME_LOCAL_NED;
command.confirmation = false;
command.param1 = param1;
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::command_right(float param1)
{
//Message buffer
......@@ -289,7 +324,7 @@ int Drone::command_setModeGuided(float param1)
command.target_system = 1;
command.target_component = 1;
command.command = MAV_CMD_NAV_GUIDED_ENABLE;
command.confirmation = false;
command.confirmation = true; //false;
command.param1 = param1; // flag >0.5 => start, <0.5 => stop
mavlink_msg_command_long_encode(255, 1, &message, &command);
......@@ -330,7 +365,7 @@ 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, 0, xx, yy, zz, rr,0);
mavlink_msg_manual_control_pack(255, 1, &message, 1, xx, yy, zz, rr,1);
cout<<"command_directControl : "<<xx<<" "<<yy<<" "<<zz<<" "<<rr<<" "<<endl;
return write_message(message);
......@@ -448,6 +483,57 @@ mavlink_command_long_t Drone::mavlink_newCommand()
return com;
}
/*
PARAM_REQUEST_LIST
Request all parameters of this component. After this request, all parameters are emitted. The parameter microservice is documented at https://mavlink.io/en/services/parameter.html
mavlink_param_union_t param;
int32_t integer = 20000;
param.param_int32 = integer;
param.type = MAV_PARAM_TYPE_INT32;
// Then send the param by providing the float bytes to the send function
mavlink_msg_param_set_send(xxx, xxx, param.param_float, param.type, xxx);
?????? mavlink_msg_param_set_send ( 255, 1, 20000, MAV_PARAM_TYPE_INT32, ????)
static inline void mavlink_msg_param_set_send( mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const char *param_id, float param_value, uint8_t param_type)
*/
/**
* @brief Send a param_set message
* @param chan MAVLink channel to send the message : 255
*
* @param target_system System ID : 1
* @param target_component Component ID
* @param param_id Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string
* @param param_value Onboard parameter value
* @param param_type Onboard parameter type.
*/
/*
You need to assign base_mode, main_mode, and sub_mode.
For instance, in order to trigger ‘position’ flight mode, you can use the following pymavlink code. I think that it will be same in dronekit code.
master.mav.command_long_send ( master.target_system, master.target_component, mavutil.mavlink.MAV_CMD_DO_SET_MODE, 0, base_mode, main_mode, sub_mode, 0, 0, 0, 0)
sub_mode is only defined for the AUTO_MOD
master.mav.command_long_send( master.target_system, master.target_component, mavutil.mavlink.MAV_CMD_DO_SET_MODE, 0, 217, 3, 0, 0, 0, 0, 0)
Manual: base_mode:217, main_mode:1, sub_mode:0
Stabilized: base_mode:209, main_mode:7, sub_mode:0
Acro: base_mode:209, main_mode:5, sub_mode:0
Rattitude: base_mode:193, main_mode:8, sub_mode:0
Altitude: base_mode:193, main_mode:2, sub_mode:0
Offboard: base_mode:209, main_mode:6, sub_mode:0
Position: base_mode:209, main_mode:3, sub_mode:0
Hold: base_mode:217, main_mode:4, sub_mode:3
Missition: base_mode:157, main_mode:4, sub_mode:4
Return: base_mode:157, main_mode:4, sub_mode:5
Follow me: base_mode:157, main_mode:4, sub_mode:8
*/
int Drone::command_setMode(Drone_mode mode)
{
switch(mode)
......
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