Commit ce5f945f authored by Philippe Gaussier's avatar Philippe Gaussier
Browse files

Correction d'un bug. Boucle while qui bloquait le programme dans la lecture du port serie.

PG.
parent d1b8e1ce
Pipeline #1267 failed with stage
in 3 minutes and 28 seconds
......@@ -21,6 +21,7 @@ Thread_SerialPort::Thread_SerialPort(int task_period, int task_deadline, std::sh
{
Thread_SerialPort::serial_Port_1 = serial_Port_1;
cout<<"init shared memory\n";
//Channels use to control manually the drone
blc_control_motors = std::shared_ptr<blc_channel>(new blc_channel("/pixhawk.control.motors", BLC_CHANNEL_READ, 'FL32', 'NDEF', 1, 4));
......@@ -49,45 +50,53 @@ Thread_SerialPort::~Thread_SerialPort()
}
void Thread_SerialPort::run(){
void Thread_SerialPort::run()
{
cout<<"start Thread_SErialPort run()\n";
long long currentThreadDelay;
bool success=false;
gettimeofday(&begin, 0);
gettimeofday(&front_checkpoint, 0);
while(isRunFlag()){
while(isRunFlag())
{
state = THREAD_STATE_WORK_SLEEP;
usleep(task_period);
gettimeofday(&end_checkpoint, 0);
currentThreadDelay=(end_checkpoint.tv_sec-front_checkpoint.tv_sec) * 1000000L + (end_checkpoint.tv_usec-front_checkpoint.tv_usec);
if (currentThreadDelay > task_period ){
if (currentThreadDelay > task_period )
{
gettimeofday(&front_checkpoint, 0);
if (currentThreadDelay > task_period + task_deadline){
if (currentThreadDelay > task_period + task_deadline)
{
state = THREAD_STATE_WORK_DEADLINE;
}
else {
else
{
state = THREAD_STATE_WORK_TASK;
// cout<<"R/W drone \n ";
write_messages();
// cout<<"end W \n";
//Task1: read message
if(mavlink1.get()->read_message_serial(message, serial_Port_1)){
if(mavlink1.get()->read_message_serial(message, serial_Port_1))
{
read_messages();
}
// cout<<"end R/W \n";
}
}
}
}
void Thread_SerialPort::write_messages(){
void Thread_SerialPort::write_messages()
{
if(blc_control_commands.get()->floats[0]>0.5) //arm command
{
//printf("mémoire partagée : demande armement \n");
......@@ -96,7 +105,7 @@ void Thread_SerialPort::write_messages(){
serial_Port_1.get()->write_message(Bus::getInstanceOf().getMavlinkTool().get()->command_arm(1));
}
if(blc_control_commands.get()->floats[1]>0.5) // arm unarm command
{
//printf("mémoire partagée : demande desarmement \n");
......@@ -104,57 +113,49 @@ void Thread_SerialPort::write_messages(){
blc_control_commands.get()->floats[1] = 0.0;
serial_Port_1.get()->write_message(Bus::getInstanceOf().getMavlinkTool().get()->command_arm(1));
}
if(blc_control_motors.get()->floats[0]>0.5) // right command
{
//printf("mémoire partagée : demande motor 0 \n");
//blc_control_commands.get()->floats[1] = 0.0;S
serial_Port_1.get()->write_message(Bus::getInstanceOf().getMavlinkTool().get()->command_right(1));
{
//printf("mémoire partagée : demande motor 0 \n");
//blc_control_commands.get()->floats[1] = 0.0;S
serial_Port_1.get()->write_message(Bus::getInstanceOf().getMavlinkTool().get()->command_right(1));
}
}
else // left command
{
//printf("mémoire partagée : demande motor < 0,5 \n");
//blc_control_commands.get()->floats[1] = 0.0;S
serial_Port_1.get()->write_message(Bus::getInstanceOf().getMavlinkTool().get()->command_left(1));
}
else // left command
{
//printf("mémoire partagée : demande motor < 0,5 \n");
//blc_control_commands.get()->floats[1] = 0.0;S
serial_Port_1.get()->write_message(Bus::getInstanceOf().getMavlinkTool().get()->command_left(1));
}
while (blc_control_motors.get()->floats[1]<0.5) {
// while (blc_control_motors.get()->floats[1]<0.5) //PB si on a un while ici PG !
// {
if(blc_control_motors.get()->floats[1]>0.5) // up/down axis command
{
{
//printf("mémoire partagée : demande motors 1 > 0,5 \n");
//blc_control_commands.get()->floats[1] = 0.0;S
serial_Port_1.get()->write_message(Bus::getInstanceOf().getMavlinkTool().get()->command_up(1));
serial_Port_1.get()->write_message(Bus::getInstanceOf().getMavlinkTool().get()->command_up(1));
// action.set_takeoff_altitude(3.0);
}
else
{
}
else // PG : ????
{
//printf("mémoire partagée : demande motor 1 < 0,5 \n");
//blc_control_commands.get()->floats[1] = 0.0;S
serial_Port_1.get()->write_message(Bus::getInstanceOf().getMavlinkTool().get()->command_down(1));
}
serial_Port_1.get()->write_message(Bus::getInstanceOf().getMavlinkTool().get()->command_down(1));
}
}
// }
if(blc_control_motors.get()->floats[3]>0.5) //pow command
{
//printf("mémoire partagée : demande d'augmentation de la puissance \n");
//blc_control_commands.get()->floats[0] = 0.0;
serial_Port_1.get()->write_message(Bus::getInstanceOf().getMavlinkTool().get()->command_pow(1));
{
//printf("mémoire partagée : demande d'augmentation de la puissance \n");
//blc_control_commands.get()->floats[0] = 0.0;
serial_Port_1.get()->write_message(Bus::getInstanceOf().getMavlinkTool().get()->command_pow(1));
}
}
// COMMAND FOR CONTROL THE DRONE. THIS PART OF THE THREAD NEED TO BE CHANGE BEFORE ALL APPLICATION
// if(1){
......@@ -163,14 +164,13 @@ void Thread_SerialPort::write_messages(){
}
void Thread_SerialPort::read_messages(){
void Thread_SerialPort::read_messages()
{
Bus::getInstanceOf().event_newMessage(message);
cout<< "LOG "<<message.msgid<<"\n";
// cout<< "LOG "<<message.msgid<<"\n";
switch (message.msgid)
{
//SENSORS HANG
case MAVLINK_MSG_ID_HEARTBEAT:
{
......@@ -178,7 +178,6 @@ void Thread_SerialPort::read_messages(){
Bus::getInstanceOf().update(heartbeat);
break;
}
case MAVLINK_MSG_ID_SYS_STATUS:
{
mavlink_msg_sys_status_decode(&message, &sys_status);
......@@ -277,7 +276,8 @@ void Thread_SerialPort::read_messages(){
break;
}*/
case MAVLINK_MSG_ID_GPS_RAW_INT:{
case MAVLINK_MSG_ID_GPS_RAW_INT:
{
mavlink_msg_gps_raw_int_decode(&message, &gps_raw_int);
Bus::getInstanceOf().update(gps_raw_int);
break;
......@@ -338,21 +338,22 @@ void Thread_SerialPort::read_messages(){
//printf("Warning, did not handle message id %i\n",message.msgid);
break;
}
}
}
int Thread_SerialPort::testAck(mavlink_command_ack_t *ack, std::string message){
int Thread_SerialPort::testAck(mavlink_command_ack_t *ack, std::string message)
{
std::string buffer1="-> Ack-";
buffer1+=message;
if (ack->result == MAV_RESULT_ACCEPTED) {
if (ack->result == MAV_RESULT_ACCEPTED)
{
buffer1+="-succes";
//Display_Interface::Instance_of().print_Log(buffer1);
return 0;
} else {
} else
{
buffer1+="-fail";
//Display_Interface::Instance_of().print_Log(buffer1);
return 1;
......@@ -361,9 +362,11 @@ int Thread_SerialPort::testAck(mavlink_command_ack_t *ack, std::string message){
void Thread_SerialPort::handle_command_ack(mavlink_command_ack_t *ack)
{
switch (ack->command) {
switch (ack->command)
{
case MAV_CMD_COMPONENT_ARM_DISARM:
if(testAck(ack, "Arm_desarm") == 1){ //if command is succeed
if(testAck(ack, "Arm_desarm") == 1) //if command is succeed
{
//if(drone1.get()->motors == ARM){
// drone1.get()->motors = UNARM;
//}else if(drone1.get()->motors == UNARM){
......@@ -387,6 +390,5 @@ void Thread_SerialPort::handle_command_ack(mavlink_command_ack_t *ack)
default:
testAck(ack, "Unknow");
break;
}
}
}
Supports Markdown
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