mavros/setpoint_position/local : Not working #729

b-rohit · 2017-06-14T11:26:46Z

question details

I want to send mocap position data to pixhawk. I wrote a simple program based on https://dev.px4.io/en/ros/mavros_offboard.html. my source code is here https://github.com/b-rohit/offboardnav.
I have tested it in Gazebo simulation environment and It works fine. I want to emulate the same on quacopter based on pixhawk. I followed the tutorial https://dev.px4.io/en/ros/external_position_estimation.html. when I run my program it arms the copter but disarm automatically after 5-6 sec and arm again ... It subscribes to "mavros/local_position/pose" topic to get current position, but the callback for the topic never get called.
I keep getting following error
[ERROR] [1497439390.804899443]: FCU: Failsafe enabled: no RC and no offboard
[ INFO] [1497439391.052993584]: FCU: Failsafe mode enabled
[ WARN] [1497439391.072776041]: PR: request param #79 timeout, retries left 1, and 584 params still missing
[ INFO] [1497439391.091944381]: FCU: Failsafe mode disabled
when I try to change flight mode to offboard using rc controller than I get below error message>
[ERROR] [1497366588.498711872]: FCU: main state transition denied

MAVROS version and platform

Mavros: 0.19.0
ROS: Kinetic
Ubuntu: 16.04

Autopilot type and version

[X ] PX4

Node logs

copy output of mavros_node. Usually console where you run roslaunch

Diagnostics

place here result of:
rostopic echo -n1 /diagnostics

header: 
  seq: 106
  stamp: 
    secs: 1497439460
    nsecs: 791990964
  frame_id: ''
status: 
  - 
    level: 0
    name: mavros: FCU connection
    message: connected
    hardware_id: /dev/ttyUSB0
    values: 
      - 
        key: Received packets:
        value: 3816
      - 
        key: Dropped packets:
        value: 0
      - 
        key: Buffer overruns:
        value: 0
      - 
        key: Parse errors:
        value: 0
      - 
        key: Rx sequence number:
        value: 80
      - 
        key: Tx sequence number:
        value: 35
      - 
        key: Rx total bytes:
        value: 127416
      - 
        key: Tx total bytes:
        value: 111802
      - 
        key: Rx speed:
        value: 1198.000000
      - 
        key: Tx speed:
        value: 3146.000000
  - 
    level: 2
    name: mavros: GPS
    message: No satellites
    hardware_id: /dev/ttyUSB0
    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/ttyUSB0
    values: 
      - 
        key: Heartbeats since startup
        value: 124
      - 
        key: Frequency (Hz)
        value: 1.039986
      - 
        key: Vehicle type
        value: Quadrotor
      - 
        key: Autopilot type
        value: PX4 Autopilot
      - 
        key: Mode
        value: OFFBOARD
      - 
        key: System status
        value: Standby
  - 
    level: 0
    name: mavros: System
    message: Normal
    hardware_id: /dev/ttyUSB0
    values: 
      - 
        key: Sensor present
        value: 0x00000000
      - 
        key: Sensor enabled
        value: 0x00000000
      - 
        key: Sensor helth
        value: 0x00000000
      - 
        key: CPU Load (%)
        value: 33.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/ttyUSB0
    values: 
      - 
        key: Voltage
        value: 15.73
      - 
        key: Current
        value: 0.3
      - 
        key: Remaining
        value: 100.0
  - 
    level: 2
    name: mavros: Time Sync
    message: No events recorded.
    hardware_id: /dev/ttyUSB0
    values: 
      - 
        key: Timesyncs since startup
        value: 0
      - 
        key: Frequency (Hz)
        value: 0.000000
      - 
        key: Last dt (ms)
        value: -6.765325
      - 
        key: Mean dt (ms)
        value: 0.000000
      - 
        key: Last system time (s)
        value: 326.074343000
      - 
        key: Time offset (s)
        value: 1497439134.691754580
  - 
    level: 0
    name: mavros: 3DR Radio
    message: Normal
    hardware_id: /dev/ttyUSB0
    values: 
      - 
        key: RSSI
        value: 193
      - 
        key: RSSI (dBm)
        value: -25.4
      - 
        key: Remote RSSI
        value: 192
      - 
        key: Remote RSSI (dBm)
        value: -25.9
      - 
        key: Tx buffer (%)
        value: 47
      - 
        key: Noice level
        value: 60
      - 
        key: Remote noice level
        value: 64
      - 
        key: Rx errors
        value: 0
      - 
        key: Fixed
        value: 0

Check ID

rosrun mavros checkid

`OK. I got messages from 1:1.

Received 557 messages, from 15 addresses
sys:comp list of messages
62:187 204
1:66 49
152:51 68
69:95 83
0:186 2
254:9 163
1:80 87
51:68 109, 166
1:24 106
1:82 67
0:0 0
1:0 0
134:17 186
1:190 11
1:1 0, 1, 2, 128, 147, 22, 24, 30, 36, 44, 181, 74, 77, 82, 83, 84, 86, 87, 100, 105, 111, 245
`

The text was updated successfully, but these errors were encountered:

TSC21 · 2017-06-15T16:24:00Z

[ERROR] [1497439390.804899443]: FCU: Failsafe enabled: no RC and no offboard

The problem is here. Then

[ERROR] [1497366588.498711872]: FCU: main state transition denied

You have to send a stream of setpoints before and during so the FCU is able to change and maintain the OFFBOARD mode. Check the example you referenced:

//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;

b-rohit · 2017-06-16T11:14:32Z

@TSC21 , yes I am doing the same. I send coordinates before changing flight mode to offboard.
I noticed following difference while running the same code in gazebo simulation and on drone.

I keep getting the error ": FCU: Failsafe enabled: no RC and no offboard" and PR: request param # completely missing. Is it something to do with rate at which I am publishing my coordinates ? What is the rate I should set.
As I start my code in the simulation the callback function to mavros/local_position/pose topic is called and shows current positions. But in real scenario it is not called and there is no response to the command rostopic echo /mavros/local_position/pose.

TSC21 · 2017-06-16T12:32:58Z

run rostopic echo -n1 /diagnostics

TSC21 · 2017-06-16T12:34:10Z

The problem has to be on your code.

b-rohit · 2017-06-16T12:47:30Z

$rostopic echo -n1 /diagnostics
`header: 
  seq: 183
  stamp: 
    secs: 1497617120
    nsecs: 165569446
  frame_id: ''
status: 
  - 
    level: 0
    name: mavros: FCU connection
    message: connected
    hardware_id: /dev/ttyUSB0
    values: 
      - 
        key: Received packets:
        value: 7345
      - 
        key: Dropped packets:
        value: 0
      - 
        key: Buffer overruns:
        value: 0
      - 
        key: Parse errors:
        value: 0
      - 
        key: Rx sequence number:
        value: 42
      - 
        key: Tx sequence number:
        value: 199
      - 
        key: Rx total bytes:
        value: 238807
      - 
        key: Tx total bytes:
        value: 197189
      - 
        key: Rx speed:
        value: 1129.000000
      - 
        key: Tx speed:
        value: 3248.000000
  - 
    level: 2
    name: mavros: GPS
    message: No satellites
    hardware_id: /dev/ttyUSB0
    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/ttyUSB0
    values: 
      - 
        key: Heartbeats since startup
        value: 227
      - 
        key: Frequency (Hz)
        value: 1.034484
      - 
        key: Vehicle type
        value: Quadrotor
      - 
        key: Autopilot type
        value: PX4 Autopilot
      - 
        key: Mode
        value: OFFBOARD
      - 
        key: System status
        value: Active
  - 
    level: 0
    name: mavros: System
    message: Normal
    hardware_id: /dev/ttyUSB0
    values: 
      - 
        key: Sensor present
        value: 0x00000000
      - 
        key: Sensor enabled
        value: 0x00000000
      - 
        key: Sensor helth
        value: 0x00000000
      - 
        key: CPU Load (%)
        value: 41.2
      - 
        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/ttyUSB0
    values: 
      - 
        key: Voltage
        value: 15.61
      - 
        key: Current
        value: 1.4
      - 
        key: Remaining
        value: 100.0
  - 
    level: 2
    name: mavros: Time Sync
    message: No events recorded.
    hardware_id: /dev/ttyUSB0
    values: 
      - 
        key: Timesyncs since startup
        value: 0
      - 
        key: Frequency (Hz)
        value: 0.000000
      - 
        key: Last dt (ms)
        value: -5.613163
      - 
        key: Mean dt (ms)
        value: 0.000000
      - 
        key: Last system time (s)
        value: 285.766994000
      - 
        key: Time offset (s)
        value: 1497616833.544296980
  - 
    level: 0
    name: mavros: 3DR Radio
    message: Normal
    hardware_id: /dev/ttyUSB0
    values: 
      - 
        key: RSSI
        value: 175
      - 
        key: RSSI (dBm)
        value: -34.9
      - 
        key: Remote RSSI
        value: 179
      - 
        key: Remote RSSI (dBm)
        value: -32.8
      - 
        key: Tx buffer (%)
        value: 1
      - 
        key: Noice level
        value: 54
      - 
        key: Remote noice level
        value: 67
      - 
        key: Rx errors
        value: 1
      - 
        key: Fixed
        value: 1
---
`

b-rohit · 2017-06-20T10:31:55Z

I am running https://github.com/b-rohit/offboardnav/blob/master/src/offb_node.cpp. when it arms the drone I ran following commands in another window.
1.
$ rostopic hz /mavros/setpoint_position/local
subscribed to [/mavros/setpoint_position/local]
average rate: 20.002
min: 0.049s max: 0.051s std dev: 0.00037s window: 19
average rate: 19.997
min: 0.049s max: 0.051s std dev: 0.00029s window: 39
average rate: 20.000
min: 0.049s max: 0.051s std dev: 0.00026s window: 59
average rate: 20.001
min: 0.049s max: 0.051s std dev: 0.00027s window: 79
average rate: 20.000
min: 0.049s max: 0.051s std dev: 0.00025s window: 99
average rate: 20.000
min: 0.049s max: 0.051s std dev: 0.00028s window: 119
average rate: 20.000
min: 0.049s max: 0.051s std dev: 0.00028s window: 139
average rate: 20.000
min: 0.049s max: 0.051s std dev: 0.00030s window: 159
average rate: 20.000
min: 0.049s max: 0.051s std dev: 0.00029s window: 166
no new messages
average rate: 16.749
min: 0.000s max: 1.876s std dev: 0.13466s window: 184
average rate: 17.021
min: 0.000s max: 1.876s std dev: 0.12789s window: 204
average rate: 17.252

which explains why does it disarm in between, but I am not able to find why does it stop publishing messages. I put some debug messages and found it sticks while execute following peace of code>
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(); }

$ rostopic hz /mavros/local_position/pose
subscribed to [/mavros/local_position/pose]
no new messages
no new messages
no new messages
no new messages
no new messages
no new messages
no new messages
no new messages
no new messages
no new messages

There is an issue with /mavros/local_position/pose topic in px4fmu-v2_lpe firmware. it works with px4fmu-v2_default.

b-rohit · 2017-06-23T09:38:08Z

I changed lpe mocap timeout from 2s to 5s and rate to 30hz. Now it is working.
PX4/PX4-Autopilot#5174

TSC21 · 2017-06-23T10:15:30Z

Good to know

mandelyoni · 2018-06-23T08:06:12Z

Dear Sir,

We're running in Offboard mode and sending a set_attitude_target message through Mavlink.
The PX4 version is 1.6.5 on Intel Aero.

It seems that the roll and pitch angle/rate commands work fine but the yaw angle/rate doesn't work at all.

Please advise

TSC21 added not a bug question labels Jun 15, 2017

b-rohit closed this as completed Jun 23, 2017

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

mavros/setpoint_position/local : Not working #729

mavros/setpoint_position/local : Not working #729

b-rohit commented Jun 14, 2017 •

edited

Loading

TSC21 commented Jun 15, 2017 •

edited

Loading

b-rohit commented Jun 16, 2017

TSC21 commented Jun 16, 2017

TSC21 commented Jun 16, 2017

b-rohit commented Jun 16, 2017 •

edited

Loading

b-rohit commented Jun 20, 2017 •

edited

Loading

b-rohit commented Jun 23, 2017 •

edited

Loading

TSC21 commented Jun 23, 2017

mandelyoni commented Jun 23, 2018

mavros/setpoint_position/local : Not working #729

mavros/setpoint_position/local : Not working #729

Comments

b-rohit commented Jun 14, 2017 • edited Loading

question details

MAVROS version and platform

Autopilot type and version

Node logs

Diagnostics

Check ID

TSC21 commented Jun 15, 2017 • edited Loading

b-rohit commented Jun 16, 2017

TSC21 commented Jun 16, 2017

TSC21 commented Jun 16, 2017

b-rohit commented Jun 16, 2017 • edited Loading

b-rohit commented Jun 20, 2017 • edited Loading

b-rohit commented Jun 23, 2017 • edited Loading

TSC21 commented Jun 23, 2017

mandelyoni commented Jun 23, 2018

b-rohit commented Jun 14, 2017 •

edited

Loading

TSC21 commented Jun 15, 2017 •

edited

Loading

b-rohit commented Jun 16, 2017 •

edited

Loading

b-rohit commented Jun 20, 2017 •

edited

Loading

b-rohit commented Jun 23, 2017 •

edited

Loading