OFFBOARD mode not accessible with RC or ROS

[KochC]

Bug Report

I try to switch to the OFFBOARD mode, but it's still not possible. I tried to switch with my RC, with a ROS-node and with the QGC.

I set the RC_MAP_OFFB_SWto my RC channel 5.

It's possible to switch for example from MANUALto STABILIZEDmode. Even with my ROS-node.

Do I have to use a specific airframe?

[TSC21]

Are you publishing a stream of setpoints before changing to OFFBOARD mode?

[KochC]

@TSC21 yes, I just use the example code here:

 cat offboard.cpp 
#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>

mavros_msgs::State current_state;
void state_cb(const mavros_msgs::State::ConstPtr& msg){
    current_state = *msg;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "offb_node");
    ros::NodeHandle nh;

    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
            ("mavros/state", 10, state_cb);
    ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
            ("mavros/setpoint_position/local", 10);
    ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
            ("mavros/cmd/arming");
    ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
            ("mavros/set_mode");

    //the setpoint publishing rate MUST be faster than 2Hz
    ros::Rate rate(20.0);

    // wait for FCU connection
    while(ros::ok() && current_state.connected){
        ros::spinOnce();
        rate.sleep();
    }

    geometry_msgs::PoseStamped pose;
    pose.pose.position.x = 0;
    pose.pose.position.y = 0;
    pose.pose.position.z = 2;

    //send a few setpoints before starting
    for(int i = 100; ros::ok() && i > 0; --i){
        local_pos_pub.publish(pose);
        ros::spinOnce();
        rate.sleep();
    }

    mavros_msgs::SetMode offb_set_mode;
    offb_set_mode.request.custom_mode = "OFFBOARD";

    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = true;

    ros::Time last_request = ros::Time::now();

    while(ros::ok()){
        if( current_state.mode != "OFFBOARD" &&
            (ros::Time::now() - last_request > ros::Duration(5.0))){
            if( set_mode_client.call(offb_set_mode) &&
                offb_set_mode.response.mode_sent){
                ROS_INFO("Offboard enabled");
            }
            last_request = ros::Time::now();
        } else {
            if( !current_state.armed &&
                (ros::Time::now() - last_request > ros::Duration(5.0))){
                if( arming_client.call(arm_cmd) &&
                    arm_cmd.response.success){
                    ROS_INFO("Vehicle armed");
                }
                last_request = ros::Time::now();
            }
        }

        local_pos_pub.publish(pose);

        ros::spinOnce();
        rate.sleep();
    }

    return 0;
}

[TSC21]

What msgs do you get when you try to set to OFFBOARD mode? Does it give you mode rejection?

[KochC]

I'm not sure if thats what you wanted. The Flightcontroller just gives that Error-Beeg (Bibo-Bibo)

[ INFO] [1511959049.508794366]: VER: 1.1: Board hardware:      00000011
[ INFO] [1511959049.509000302]: VER: 1.1: VID/PID:             26ac:0011
[ INFO] [1511959049.509205301]: VER: 1.1: UID:                 3334510b32363238
[ INFO] [1511959050.520981809]: VER: 1.1: Capabilities         0x000000000000e4ef
[ INFO] [1511959050.521294098]: VER: 1.1: Flight software:     010700c0 (000000006E696C5F)
[ INFO] [1511959050.521543575]: VER: 1.1: Middleware software: 010700c0 (000000006E696C5F)
[ INFO] [1511959050.521773313]: VER: 1.1: OS software:         071600ff (000000006E696C5F)
[ INFO] [1511959050.521987791]: VER: 1.1: Board hardware:      00000011
[ INFO] [1511959050.522191904]: VER: 1.1: VID/PID:             26ac:0011
[ INFO] [1511959050.522426277]: VER: 1.1: UID:                 3334510b32363238
[ WARN] [1511959050.527712589]: VER: unicast request timeout, retries left 2
[ INFO] [1511959051.514597105]: VER: 1.1: Capabilities         0x000000000000e4ef
[ INFO] [1511959051.514905541]: VER: 1.1: Flight software:     010700c0 (0000000000000001)
[ INFO] [1511959051.515164341]: VER: 1.1: Middleware software: 010700c0 (0000000000000001)
[ INFO] [1511959051.515388558]: VER: 1.1: OS software:         071600ff (0000000000000001)
[ INFO] [1511959051.515607566]: VER: 1.1: Board hardware:      00000011
[ INFO] [1511959051.515818190]: VER: 1.1: VID/PID:             26ac:0011
[ INFO] [1511959051.516036834]: VER: 1.1: UID:                 3334510b32363238
[ WARN] [1511959051.517839632]: VER: unicast request timeout, retries left 1
[ INFO] [1511959052.518497268]: VER: 1.1: Capabilities         0x000000000000e4ef
[ INFO] [1511959052.518804141]: VER: 1.1: Flight software:     010700c0 (000000006E696C5F)
[ INFO] [1511959052.519049868]: VER: 1.1: Middleware software: 010700c0 (000000006E696C5F)
[ INFO] [1511959052.519276168]: VER: 1.1: OS software:         071600ff (000000006E696C5F)
[ INFO] [1511959052.519490906]: VER: 1.1: Board hardware:      00000011
[ INFO] [1511959052.519703300]: VER: 1.1: VID/PID:             26ac:0011
[ INFO] [1511959052.519925382]: VER: 1.1: UID:                 3334510b32363238
[ WARN] [1511959052.521350214]: VER: unicast request timeout, retries left 0
[ INFO] [1511959053.520088698]: VER: 1.1: Capabilities         0x000000000000e4ef
[ INFO] [1511959053.520582028]: VER: 1.1: Flight software:     010700c0 (000000006E696C5F)
[ INFO] [1511959053.520969421]: VER: 1.1: Middleware software: 010700c0 (000000006E696C5F)
[ INFO] [1511959053.521876028]: VER: 1.1: OS software:         071600ff (000000006E696C5F)
[ INFO] [1511959053.522583627]: VER: 1.1: Board hardware:      00000011
[ INFO] [1511959053.523433360]: VER: 1.1: VID/PID:             26ac:0011
[ INFO] [1511959053.524207364]: VER: 1.1: UID:                 3334510b32363238
[ WARN] [1511959053.527570619]: VER: your FCU don't support AUTOPILOT_VERSION, switched to default capabilities
[ WARN] [1511959053.645056932]: WP: timeout, retries left 2
[ WARN] [1511959054.645953732]: WP: timeout, retries left 1
[ WARN] [1511959055.646757042]: WP: timeout, retries left 0
[ INFO] [1511959056.469804044]: HP: requesting home position
[ERROR] [1511959056.648342272]: WP: timed out.
[ WARN] [1511959057.470608082]: PR: request list timeout, retries left 2
[ WARN] [1511959057.638264208]: CMD: Unexpected command 176, result 1
[ WARN] [1511959058.471280870]: PR: request list timeout, retries left 1
[ WARN] [1511959059.472493810]: PR: request list timeout, retries left 0

[TSC21]

I mean with QGC on (so you require to set the gcs_url also on the launch file). What do you get on the msg console?

[TSC21]

Also, what's the output of rostopic echo -n1 /diagnostics and rosrun mavros checkid?

[KochC]

In my launch file I set this to the IP of my notebook so I get a connection with QGC via UDP:

<arg name="gcs_url" default="udp://:14556@192.168.178.28:14550" />

rostopic echo -n1 /diagnostics
header: 
  seq: 63
  stamp: 
    secs: 1511959482
    nsecs: 996512451
  frame_id: ''
status: 
  - 
    level: 0
    name: mavros: FCU connection
    message: connected
    hardware_id: /dev/ttyS0:57600
    values: 
      - 
        key: Received packets:
        value: 1298
      - 
        key: Dropped packets:
        value: 0
      - 
        key: Buffer overruns:
        value: 0
      - 
        key: Parse errors:
        value: 0
      - 
        key: Rx sequence number:
        value: 6
      - 
        key: Tx sequence number:
        value: 139
      - 
        key: Rx total bytes:
        value: 41770
      - 
        key: Tx total bytes:
        value: 53179
      - 
        key: Rx speed:
        value: 1675.000000
      - 
        key: Tx speed:
        value: 2258.000000
  - 
    level: 2
    name: mavros: GPS
    message: No satellites
    hardware_id: /dev/ttyS0:57600
    values: 
      - 
        key: Satellites visible
        value: 0
      - 
        key: Fix type
        value: 0
      - 
        key: EPH (m)
        value: Unknown
      - 
        key: EPV (m)
        value: Unknown
  - 
    level: 0
    name: mavros: Heartbeat
    message: Normal
    hardware_id: /dev/ttyS0:57600
    values: 
      - 
        key: Heartbeats since startup
        value: 38
      - 
        key: Frequency (Hz)
        value: 0.999986
      - 
        key: Vehicle type
        value: Quadrotor
      - 
        key: Autopilot type
        value: PX4 Autopilot
      - 
        key: Mode
        value: MANUAL
      - 
        key: System status
        value: Standby
  - 
    level: 0
    name: mavros: System
    message: Normal
    hardware_id: /dev/ttyS0:57600
    values: 
      - 
        key: Sensor present
        value: 0x00000000
      - 
        key: Sensor enabled
        value: 0x00000000
      - 
        key: Sensor helth
        value: 0x00000000
      - 
        key: CPU Load (%)
        value: 46.3
      - 
        key: Drop rate (%)
        value: 0.0
      - 
        key: Errors comm
        value: 0
      - 
        key: Errors count #1
        value: 0
      - 
        key: Errors count #2
        value: 0
      - 
        key: Errors count #3
        value: 0
      - 
        key: Errors count #4
        value: 0
  - 
    level: 0
    name: mavros: Battery
    message: Normal
    hardware_id: /dev/ttyS0:57600
    values: 
      - 
        key: Voltage
        value: 65.54
      - 
        key: Current
        value: -0.0
      - 
        key: Remaining
        value: -1.0
  - 
    level: 0
    name: mavros: Time Sync
    message: Normal
    hardware_id: /dev/ttyS0:57600
    values: 
      - 
        key: Timesyncs since startup
        value: 385
      - 
        key: Frequency (Hz)
        value: 9.999864
      - 
        key: Last dt (ms)
        value: 5.377713
      - 
        key: Mean dt (ms)
        value: 0.016951
      - 
        key: Last system time (s)
        value: 4137.750638000
      - 
        key: Time offset (s)
        value: 1511955345.149517775
---

rosrun mavros checkid
Unable to register with master node [http://localhost:11311]: master may not be running yet. Will keep trying.
ERROR. I got 1 addresses, but not your target 1:50

---
Received 373 messages, from 1 addresses
sys:comp   list of messages
  1:1     0, 1, 36, 230, 32, 105, 74, 231, 141, 77, 111, 241, 147, 148, 245, 253, 30

[KochC]

If I just switch the OFFBOARD to STABILIZED in the example code, it works perfectly.

[TSC21]

If I just switch the OFFBOARD to STABILIZED in the example code, it works perfectly.

That's normal cause OFFBOARD have specific switching conditions.

[TSC21]

Also, you don't have a position lock right? How do you expect to do OFFBOARD control of position without the vehicle knowing its position?

[TSC21]

ERROR. I got 1 addresses, but not your target 1:50. Suspicious. Did you change the comp_id on the launch file? Reason: https://github.com/PX4/Firmware/blob/master/src/modules/mavlink/mavlink_receiver.cpp#L851-L854

[KochC]

Wenn no I did not change the id. Where can I find the id I have to set to? So I need do attach a GPS to get a proper position lock correct? Let me try this.

[KochC]

This is the example launch file for PX4 based FCUs which I am using. There is no comp_id to change :/

<launch>
        <!-- vim: set ft=xml noet : -->
        <!-- example launch script for PX4 based FCU's -->

        <arg name="fcu_url" default="/dev/ttyS0:57600" />
        <arg name="gcs_url" default="udp://:14556@192.168.178.28:14550" />
        <arg name="tgt_system" default="1" />
        <arg name="tgt_component" default="50" />
        <arg name="log_output" default="screen" />

        <include file="$(find mavros)/launch/node.launch">
                <arg name="pluginlists_yaml" value="$(find mavros)/launch/px4_p$
                <arg name="config_yaml" value="$(find mavros)/launch/px4_config$

                <arg name="fcu_url" value="$(arg fcu_url)" />
                <arg name="gcs_url" value="$(arg gcs_url)" />
                <arg name="tgt_system" value="$(arg tgt_system)" />
                <arg name="tgt_component" value="$(arg tgt_component)" />
                <arg name="log_output" value="$(arg log_output)" />
        </include>
</launch>

[KochC]

OK and I do not understand why it got no position lock in the simulation?

[TSC21]

There is no comp_id to change

That's not what I'm seeing here: <arg name="tgt_component" default="50" /> Set it to 1.

[KochC]

Well that was the problem. I Still get the requesting home positionbut the example works now.

[DiegoHerrera1890]

Hello guys. I am facing the same problem. I cannot change to OFFBOARD mode using the code @KochC posted In my case, I am using T265 real-sense for VIO. I am not using GPS so until now the communication between TX2 -->Mavros-->Pixhawk4 is working well because I can see the Mavlink inspector in QGC. I can arm the vehicle using the CPP code posted above but I cannot take-off and I have seen that the mode is not changing to offboard. <arg name="tgt_component" default="1" />

[DiegoHerrera1890]

`

<!-- example launch script for PX4 based FCU's -->

<arg name="fcu_url" default="/dev/ttyACM0:57600" />
<arg name="gcs_url" default="" />
<arg name="tgt_system" default="1" />
<arg name="tgt_component" default="1" />
<arg name="log_output" default="screen" />
<arg name="fcu_protocol" default="v2.0" />
<arg name="respawn_mavros" default="false" />

<include file="$(find mavros)/launch/node.launch">
    <arg name="pluginlists_yaml" value="$(find mavros)/launch/px4_pluginlists.yaml" />
    <arg name="config_yaml" value="$(find mavros)/launch/px4_config.yaml" />

    <arg name="fcu_url" value="$(arg fcu_url)" />
    <arg name="gcs_url" value="$(arg gcs_url)" />
    <arg name="tgt_system" value="$(arg tgt_system)" />
    <arg name="tgt_component" value="$(arg tgt_component)" />
    <arg name="log_output" value="$(arg log_output)" />
    <arg name="fcu_protocol" value="$(arg fcu_protocol)" />
    <arg name="respawn_mavros" default="$(arg respawn_mavros)" />
</include>

`

[DiegoHerrera1890]

`

header: seq: 2542 stamp: secs: 1517165870 nsecs: 192120416 frame_id: '' status:

level: 0
name: "mavros: FCU connection"
message: "connected"
hardware_id: "/dev/ttyTHS2:921600"
values: 

    key: "Received packets:"
    value: "27135"

    key: "Dropped packets:"
    value: "0"

    key: "Buffer overruns:"
    value: "0"
    key: "Parse errors:"
    value: "0"

    key: "Rx sequence number:"
    value: "23"

    key: "Tx sequence number:"
    value: "0"

    key: "Rx total bytes:"
    value: "27876254"

    key: "Tx total bytes:"
    value: "11870899"

    key: "Rx speed:"
    value: "23356.000000"

    key: "Tx speed:"
    value: "9825.000000"

level: 2
name: "mavros: GPS"
message: "No satellites"
hardware_id: "/dev/ttyTHS2:921600"
values: 

    key: "Satellites visible"
    value: "0"

    key: "Fix type"
    value: "0"

    key: "EPH (m)"
    value: "99.99"

    key: "EPV (m)"
    value: "99.99"

level: 0
name: "mavros: Heartbeat"
message: "Normal"
hardware_id: "/dev/ttyTHS2:921600"
values: 

    key: "Heartbeats since startup"
    value: "1787"

    key: "Frequency (Hz)"
    value: "0.965535"

    key: "Vehicle type"
    value: "Quadrotor"

    key: "Autopilot type"
    value: "PX4 Autopilot"

    key: "Mode"
    value: "STABILIZED"

    key: "System status"
    value: "Standby"

level: 2
name: "mavros: System"
message: "Sensor health"
hardware_id: "/dev/ttyTHS2:921600"
values: 

    key: "Sensor present"
    value: "0x122F002F"

    key: "Sensor enabled"
    value: "0x1221002F"

    key: "Sensor health"
    value: "0x122E000F"

    key: "3D gyro"
    value: "Ok"

    key: "3D accelerometer"
    value: "Ok"

    key: "3D magnetometer"
    value: "Ok"

    key: "absolute pressure"
    value: "Ok"

    key: "GPS"
    value: "Fail"

    key: "rc receiver"
    value: "Fail"

    key: "AHRS subsystem health"
    value: "Ok"

    key: "Battery"
    value: "Ok"

    key: "CPU Load (%)"
    value: "50.6"

    key: "Drop rate (%)"
    value: "0.0"

    key: "Errors comm"
    value: "0"

    key: "Errors count #1"
    value: "0"

    key: "Errors count #2"
    value: "0"

    key: "Errors count #3"
    value: "0"

    key: "Errors count #4"
    value: "0"

level: 0
name: "mavros: Battery"
message: "Normal"
hardware_id: "/dev/ttyTHS2:921600"
values: 

    key: "Voltage"
    value: "15.30"

    key: "Current"
    value: "0.0"

    key: "Remaining"
    value: "68.0"

level: 0
name: "mavros: Time Sync"
message: "Normal"
hardware_id: "/dev/ttyTHS2:921600"
values: 

    key: "Timesyncs since startup"
    value: "17875"

    key: "Frequency (Hz)"
    value: "10.000194"

    key: "Last RTT (ms)"
    value: "4.166176"

    key: "Mean RTT (ms)"
    value: "2.783167"

    key: "Last remote time (s)"
    value: "1880.071636000"

    key: "Estimated time offset (s)"
    value: "1517163990.052936554"

level: 0
name: "mavros: 3DR Radio"
message: "Normal"
hardware_id: "/dev/ttyTHS2:921600"
values: 

    key: "RSSI"
    value: "172"

    key: "RSSI (dBm)"
    value: "-36.5"

    key: "Remote RSSI"
    value: "171"

    key: "Remote RSSI (dBm)"
    value: "-37.0"

    key: "Tx buffer (%)"
    value: "0"

    key: "Noice level"
    value: "65"
    key: "Remote noice level"
    value: "74"

    key: "Rx errors"
    value: "16"

    key: "Fixed"
    value: "0"

`

[DiegoHerrera1890]

OK. I got messages from 1:1. Received 5163 messages, from 3 addresses sys:comp list of messages 255:190 0 51:68 109 1:1 0, 1, 2, 4, 140, 141, 147, 24, 30, 31, 32, 36, 65, 74, 76, 77, 83, 230, 105, 111, 241, 245