Failed to setpoint in offboard mode in SITL

[shrit]

Hello,

I am using the MAVSDK for gazebo SITL with IRIS quadrotor. I have created automated tests to fly IRIS quadcopter in a specific trajectories. The quadrotor functions normally in most cases of test. However, during some tests IRIS quadrotors fails to set point, I receive this error rarely:

Offboard::start() failed: No setpoint set

I do not understand why this error is occurring from time to time (about once each 30 flights). You can find the log of one of the flights in the following link. However, I feel it is more related to the SDK rather than PX4.

https://logs.px4.io/plot_app?log=e85cdc2d-f3f1-4907-bddb-08440c3684d4

Do you have any idea??

[julianoes]

Could you share the C++ code that you use with MAVSDK which has that behavior?

[shrit]

This is the function call, where I ask several quadcopters at the same time to switch to offboard mode. Only one quadcopter gives rarely the above error while the other switch the mode without any problem.

   bool takeoff;                                                                                                                                                                                                 
    for (auto it : quads_){                                                                                                                                                                                       
      takeoff = it->takeoff();                                                                                                                                                                                    
      if(!takeoff)                                                                                                                                                                                                
        stop_episode = true;                                                                                                                                                                                      
    }                                                                                                                                                                                                             

    std::this_thread::sleep_for(std::chrono::seconds(3));                                                                                                                                                         
    /*  Setting up speed_ is important to switch the mode */                                                                                                                                                      
    for (auto it : quads_){                                                                                                                                                                                       
      it->init_speed();                                                                                                                                                                                           
    } 

    /*  Switch to offboard mode, Allow the control */                                                                                                                                                             
    for(auto it : quads_){                                                                                                                                                                                        
      it->start_offboard_mode();                                                                                                                                                                                  
    }                                                           

Here is the implementation of the above functions.

bool Px4Device::takeoff()                                                                                                                                                                                         
{                                                                                                                                                                                                                 

  LogInfo() << "taking off..." ;                                                                                                                                                                                  
  const Action::Result takeoff_result = action_->takeoff();                                                                                                                                                       
  if(takeoff_result != Action::Result::SUCCESS){                                                                                                                                                                  
    LogInfo() << ERROR_CONSOLE_TEXT                                                                                                                                                                               
              << "take off failed: "                                                                                                                                                                              
              << Action::result_str(takeoff_result);                                                                                                                                                              
    return false;                                                                                                                                                                                                 
  }                                                                                                                                                                                                               
  return true;                                                                                                                                                                                                    
}  

void Px4Device::init_speed()                                                                                                                                                                                      
{                                                                                                                                                                                                                 
  offboard_->set_velocity_body({0.0f, 0.0f, 0.0f, 0.0f});                                                                                                                                                         
} 

Offboard::Result Px4Device::start_offboard_mode()                                                                                                                                                                 
{                                                                                                                                                                                                                 

  Offboard::Result offboard_result = offboard_->start();                                                                                                                                                          

  if (offboard_result != Offboard::Result::SUCCESS) {                                                                                                                                                             
    std::cerr << "Offboard::start() failed: "                                                                                                                                                                     
              << Offboard::result_str(offboard_result) ;                                                                                                                                                          

    return offboard_result;                                                                                                                                                                                       
  }                                                                                                                                                                                                               
  return offboard_result;                                                                                                                                                                                         
} 

[julianoes]

Interesting, I think this is the same problem as I've found in CI every now and then.

I think this should get fixed with https://github.com/mavlink/MAVSDK/pull/808/commits/491236e3408417502095984b1105936c38b42f0e.