mavlink / mavros

MAVLink to ROS gateway with proxy for Ground Control Station
Other
879 stars 989 forks source link

OFFBOARD REJECTED #493

Closed damanfb closed 8 years ago

damanfb commented 8 years ago

I am trying to control my pixhawk with PX4 stack through a USB connected Odroid XU4. I have mavros running on the odroid and I can connect to the Pixhawk and view the topic and services as I expect. I'm trying to control the roll and pitch angles, yaw rate, and throttle. I am using the setpoint_attitude plugin and for my test I have the following code in a node publishing the setpoint:

int main(int argc, char** argv) {
  ros::init(argc, argv, "darc_custom_quad_node");
  ros::NodeHandle node;
  ros::Rate loop_rate(50);

  ros::Publisher att_pub, cmd_pub, thrust_pub;
  att_pub = node.advertise<geometry_msgs::PoseStamped>("/mavros/setpoint_attitude/attitude",1);
  cmd_pub = node.advertise<geometry_msgs::TwistStamped>("/mavros/setpoint_attitude/cmd_vel",1);
  thrust_pub = node.advertise<std_msgs::Float64>("/mavros/setpoint_attitude/att_throttle",1);

  geometry_msgs::PoseStamped att_out;
  geometry_msgs::TwistStamped cmd_out;
  std_msgs::Float64 thrust_out;

  geometry_msgs::Quaternion orientation;
  orientation.x = 0.0;
  orientation.y = 0.0;
  orientation.z = 0.0;
  orientation.w = 1.0;

  geometry_msgs::Vector3 body_rate;
  body_rate.x = 0.0;
  body_rate.y = 0.0;
  body_rate.z = 0.0;

  float thrust = 1.0;

  att_out.pose.orientation = orientation;
  cmd_out.twist.angular = body_rate;
  thrust_out.data = thrust;

  while (ros::ok()) {
    ros::spinOnce();
    att_out.header.stamp = ros::Time::now();
    att_pub.publish(att_out);
    cmd_out.header.stamp = ros::Time::now();
    cmd_pub.publish(cmd_out);
    thrust_pub.publish(thrust_out);
    loop_rate.sleep();
  }
  return 0;
}

and I perform the following steps: 1) Launch mavros on my odroid using px4.launch and my setpoint publisher 2) Arm the pixhawk with my RC transmitter 3) Switch to offboard mode

Here is the output of Mavros during this process:

[ INFO] [1454511320.804161977]: FCU URL: /dev/ttyACM0:115200
[ INFO] [1454511320.805861879]: serial0: device: /dev/ttyACM0 @ 115200 bps
[ INFO] [1454511320.808560021]: GCS bridge disabled
[ INFO] [1454511320.829721084]: Plugin 3dr_radio blacklisted
[ INFO] [1454511320.829970582]: Plugin actuator_control blacklisted
[ INFO] [1454511321.013820120]: Plugin altitude loaded and initialized
[ INFO] [1454511321.014633071]: Plugin cam_imu_sync blacklisted
[ INFO] [1454511321.042562323]: Plugin command loaded and initialized
[ INFO] [1454511321.042983278]: Plugin distance_sensor blacklisted
[ INFO] [1454511321.082019182]: Plugin ftp loaded and initialized
[ INFO] [1454511321.082486095]: Plugin global_position blacklisted
[ INFO] [1454511321.082694760]: Plugin image_pub blacklisted
[ INFO] [1454511321.120715173]: Plugin imu_pub loaded and initialized
[ INFO] [1454511321.121266210]: Plugin local_position blacklisted
[ INFO] [1454511321.124942761]: Plugin manual_control loaded and initialized
[ INFO] [1454511321.125368132]: Plugin mocap_pose_estimate blacklisted
[ INFO] [1454511321.140404290]: Plugin param loaded and initialized
[ INFO] [1454511321.140804828]: Plugin px4flow blacklisted
[ INFO] [1454511321.162699384]: Plugin rc_io loaded and initialized
[ INFO] [1454511321.163125922]: Plugin safety_area blacklisted
[ INFO] [1454511321.163339337]: Plugin setpoint_accel blacklisted
[ INFO] [1454511321.233174969]: Plugin setpoint_attitude loaded and initialized
[ INFO] [1454511321.233481799]: Plugin setpoint_position blacklisted
[ INFO] [1454511321.233704172]: Plugin setpoint_raw blacklisted
[ INFO] [1454511321.233924962]: Plugin setpoint_velocity blacklisted
[ INFO] [1454511321.239550829]: RC_CHANNELS message detected!
[ INFO] [1454511321.242985215]: IMU: High resolution IMU detected!
[ INFO] [1454511321.268425656]: Plugin sys_status loaded and initialized
[ INFO] [1454511321.287254615]: Plugin sys_time loaded and initialized
[ INFO] [1454511321.287698694]: Plugin vfr_hud blacklisted
[ INFO] [1454511321.287940234]: Plugin vibration blacklisted
[ INFO] [1454511321.288147982]: Plugin vision_pose_estimate blacklisted
[ INFO] [1454511321.288369021]: Plugin vision_speed_estimate blacklisted
[ INFO] [1454511321.310588825]: Plugin waypoint loaded and initialized
[ INFO] [1454511321.311097237]: Autostarting mavlink via USB on PX4
[ INFO] [1454511321.311652357]: Built-in SIMD instructions: None
[ INFO] [1454511321.311882438]: Built-in MAVLink package version: 2016.1.8
[ INFO] [1454511321.312083061]: Built-in MAVLink dialect: ardupilotmega
[ INFO] [1454511321.312274601]: MAVROS started. MY ID [1, 240], TARGET ID [1, 240]
[ WARN] [1454511321.390234453]: TM: Clock skew detected (-1454510506.190109491 s). Hard syncing clocks.
[ INFO] [1454511322.192728346]: CON: Got HEARTBEAT, connected.
[ INFO] [1454511322.207717713]: IMU: High resolution IMU detected!
[ INFO] [1454511322.250467043]: RC_CHANNELS message detected!
[ INFO] [1454511323.207210988]: VER: 1.1: Capabilities 0x00000000000004eb
[ INFO] [1454511323.207944482]: VER: 1.1: Flight software:     00000000 (c7929937505c34d0)
[ INFO] [1454511323.208516352]: VER: 1.1: Middleware software: 00000000 (c7929937505c34d0)
[ INFO] [1454511323.209085013]: VER: 1.1: OS software:         00000000 (0000000000000000)
[ INFO] [1454511323.209543092]: VER: 1.1: Board hardware:      00000000
[ INFO] [1454511323.210052255]: VER: 1.1: VID/PID: 26ac:0011
[ INFO] [1454511323.210554542]: VER: 1.1: UID: 3532471736343032
[ WARN] [1454511324.205015280]: VER: broadcast request timeout, retries left 4
[ INFO] [1454511324.205722274]: VER: 1.1: Capabilities 0x00000000000004eb
[ INFO] [1454511324.206371560]: VER: 1.1: Flight software:     00000000 (c7929937505c34d0)
[ INFO] [1454511324.206996221]: VER: 1.1: Middleware software: 00000000 (c7929937505c34d0)
[ INFO] [1454511324.207621674]: VER: 1.1: OS software:         00000000 (0000000000000000)
[ INFO] [1454511324.208191919]: VER: 1.1: Board hardware:      00000000
[ INFO] [1454511324.208674290]: VER: 1.1: VID/PID: 26ac:0011
[ INFO] [1454511324.209258409]: VER: 1.1: UID: 3532471736343032
[ WARN] [1454511325.204286062]: VER: broadcast request timeout, retries left 3
[ INFO] [1454511325.205261928]: VER: 1.1: Capabilities 0x00000000000004eb
[ INFO] [1454511325.205832090]: VER: 1.1: Flight software:     00000000 (c7929937505c34d0)
[ INFO] [1454511325.206554333]: VER: 1.1: Middleware software: 00000000 (c7929937505c34d0)
[ INFO] [1454511325.207183661]: VER: 1.1: OS software:         00000000 (0000000000000000)
[ INFO] [1454511325.207696240]: VER: 1.1: Board hardware:      00000000
[ INFO] [1454511325.208223443]: VER: 1.1: VID/PID: 26ac:0011
[ INFO] [1454511325.208708106]: VER: 1.1: UID: 3532471736343032
[ WARN] [1454511326.204307505]: VER: unicast request timeout, retries left 2
[ WARN] [1454511327.204510157]: VER: unicast request timeout, retries left 1
[ WARN] [1454511328.204611562]: VER: unicast request timeout, retries left 0
[ WARN] [1454511329.205883959]: VER: your FCU don't support AUTOPILOT_VERSION, switched to default capabilities
[ WARN] [1454511333.195617286]: PR: request list timeout, retries left 2
[ WARN] [1454511334.196810610]: PR: request list timeout, retries left 1
[ WARN] [1454511335.198058186]: PR: request list timeout, retries left 0
[ WARN] [1454511338.196445889]: WP: timeout, retries left 2
[ WARN] [1454511339.197554016]: WP: timeout, retries left 1
[ WARN] [1454511340.199444346]: WP: timeout, retries left 0
[ERROR] [1454511341.200972307]: WP: timed out.
[ INFO] [1454511344.288413637]: FCU: ARMED by RC
[ INFO] [1454511344.339599061]: FCU: [log] log dir: /fs/microsd/log/2016-02-03
[ INFO] [1454511344.390398113]: FCU: TAKEOFF DETECTED
[ INFO] [1454511344.440257549]: FCU: [log] start: 14_41_07.px4log
[ERROR] [1454511349.715118603]: FCU: REJECT OFFBOARD
[ INFO] [1454511353.463019677]: FCU: DISARMED by RC
[ INFO] [1454511353.513487274]: FCU: LANDING DETECTED
[ INFO] [1454511354.785315260]: FCU: [log] logging stopped
^C[mavros-4] killing on exit
damanfb commented 8 years ago

In case it's useful, I am using trying to control the attitude without a GPS of PXflow sensor indoors and here's my diagnostics:

odroid@odroid:~/ros_catkin_ws$ rostopic echo /diagnostics -n1
header: 
  seq: 737
  stamp: 
    secs: 1454512513
    nsecs: 993846509
  frame_id: ''
status: 
  - 
    level: 0
    name: mavros: FCU connection
    message: connected
    hardware_id: /dev/ttyACM0:115200
    values: 
      - 
        key: Received packets:
        value: 53132
      - 
        key: Dropped packets:
        value: 0
      - 
        key: Buffer overruns:
        value: 0
      - 
        key: Parse errors:
        value: 0
      - 
        key: Rx sequence number:
        value: 217
      - 
        key: Tx sequence number:
        value: 86
      - 
        key: Rx total bytes:
        value: 12833251
      - 
        key: Tx total bytes:
        value: 9394043
      - 
        key: Rx speed:
        value: 19430.000000
      - 
        key: Tx speed:
        value: 10972.000000
  - 
    level: 0
    name: mavros: Heartbeat
    message: Normal
    hardware_id: /dev/ttyACM0:115200
    values: 
      - 
        key: Heartbeats since startup
        value: 993
      - 
        key: Frequency (Hz)
        value: 0.962987
      - 
        key: Vehicle type
        value: Quadrotor
      - 
        key: Autopilot type
        value: PX4
      - 
        key: Mode
        value: MANUAL
      - 
        key: System status
        value: Standby
  - 
    level: 0
    name: mavros: System
    message: Normal
    hardware_id: /dev/ttyACM0:115200
    values: 
      - 
        key: Sensor present
        value: 0x00000000
      - 
        key: Sensor enabled
        value: 0x00000000
      - 
        key: Sensor helth
        value: 0x00000000
      - 
        key: CPU Load (%)
        value: 30.0
      - 
        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/ttyACM0:115200
    values: 
      - 
        key: Voltage
        value: 11.81
      - 
        key: Current
        value: 0.4
      - 
        key: Remaining
        value: 78.0
  - 
    level: 0
    name: mavros: Time Sync
    message: Normal
    hardware_id: /dev/ttyACM0:115200
    values: 
      - 
        key: Timesyncs since startup
        value: 9934
      - 
        key: Frequency (Hz)
        value: 10.000248
      - 
        key: Last dt (ms)
        value: -0.431917
      - 
        key: Mean dt (ms)
        value: 0.007517
      - 
        key: Last system time (s)
        value: 2007.832714000
      - 
        key: Time offset (s)
        value: 1454510506.135898113
---
damanfb commented 8 years ago

Another symptom possibly related is when I try to arm the quad using the service /mavros/cmd/arming value: true, I get a response that says it was successful but the motors only spin up when I arm from the RC. The reverse is true where if I armed it from the RC, I call the service /mavros/cmd/arming value: false and it returns that it was successful but it doesn't disarm.

andre-nguyen commented 8 years ago

Try this example first http://dev.px4.io/ros-mavros-offboard.html

damanfb commented 8 years ago

I don't have the gui tools on my Odroid to run Gazebo, do you think I can run this on my full hardware with the props off? My problem I foresee is not having GPS so that I can't control position.

damanfb commented 8 years ago

I took the code from that example and copied it directly into my publisher while commenting out my code provided originally. Now I get the following output of mavros:

[ INFO] [1454518865.634191252]: FCU URL: /dev/ttyACM0:115200
[ INFO] [1454518865.635959621]: serial0: device: /dev/ttyACM0 @ 115200 bps
[ INFO] [1454518865.638340260]: GCS bridge disabled
[ INFO] [1454518865.656745944]: Plugin 3dr_radio blacklisted
[ INFO] [1454518865.657026085]: Plugin actuator_control blacklisted
[ INFO] [1454518865.833272830]: Plugin altitude loaded and initialized
[ INFO] [1454518865.834100674]: Plugin cam_imu_sync blacklisted
[ INFO] [1454518865.859435326]: Plugin command loaded and initialized
[ INFO] [1454518865.859918877]: Plugin distance_sensor blacklisted
[ INFO] [1454518865.895712821]: Plugin ftp loaded and initialized
[ INFO] [1454518865.896175919]: Plugin global_position blacklisted
[ INFO] [1454518865.896381161]: Plugin image_pub blacklisted
[ INFO] [1454518865.931457777]: Plugin imu_pub loaded and initialized
[ INFO] [1454518865.932011478]: Plugin local_position blacklisted
[ INFO] [1454518865.935748751]: Plugin manual_control loaded and initialized
[ INFO] [1454518865.936170484]: Plugin mocap_pose_estimate blacklisted
[ INFO] [1454518865.950263157]: Plugin param loaded and initialized
[ INFO] [1454518865.950658145]: Plugin px4flow blacklisted
[ INFO] [1454518865.970710977]: Plugin rc_io loaded and initialized
[ INFO] [1454518865.971175824]: Plugin safety_area blacklisted
[ INFO] [1454518865.989734805]: Plugin setpoint_accel loaded and initialized
[ INFO] [1454518866.051344683]: Plugin setpoint_attitude loaded and initialized
[ INFO] [1454518866.081761728]: Plugin setpoint_position loaded and initialized
[ INFO] [1454518866.130205085]: Plugin setpoint_raw loaded and initialized
[ INFO] [1454518866.144065634]: Plugin setpoint_velocity loaded and initialized
[ INFO] [1454518866.174854381]: Plugin sys_status loaded and initialized
[ INFO] [1454518866.178959201]: IMU: High resolution IMU detected!
[ INFO] [1454518866.192397683]: Plugin sys_time loaded and initialized
[ INFO] [1454518866.192830539]: Plugin vfr_hud blacklisted
[ INFO] [1454518866.193043905]: Plugin vibration blacklisted
[ INFO] [1454518866.193210241]: Plugin vision_pose_estimate blacklisted
[ INFO] [1454518866.193432022]: Plugin vision_speed_estimate blacklisted
[ INFO] [1454518866.213959950]: Plugin waypoint loaded and initialized
[ INFO] [1454518866.214603589]: Autostarting mavlink via USB on PX4
[ INFO] [1454518866.215008701]: Built-in SIMD instructions: None
[ INFO] [1454518866.215206613]: Built-in MAVLink package version: 2016.1.8
[ INFO] [1454518866.215369241]: Built-in MAVLink dialect: ardupilotmega
[ INFO] [1454518866.215549615]: MAVROS started. MY ID [1, 240], TARGET ID [1, 240]
[ WARN] [1454518866.294614550]: TM: Clock skew detected (-1454518256.020970345 s). Hard syncing clocks.
[ INFO] [1454518867.021950652]: CON: Got HEARTBEAT, connected.
[ INFO] [1454518867.025209053]: IMU: High resolution IMU detected!
[ INFO] [1454518868.035977419]: VER: 1.1: Capabilities 0x00000000000004eb
[ INFO] [1454518868.036844925]: VER: 1.1: Flight software:     00000000 (c7929937505c34d0)
[ INFO] [1454518868.037349807]: VER: 1.1: Middleware software: 00000000 (c7929937505c34d0)
[ INFO] [1454518868.037848191]: VER: 1.1: OS software:         00000000 (0000000000000000)
[ INFO] [1454518868.038274425]: VER: 1.1: Board hardware:      00000000
[ INFO] [1454518868.038653795]: VER: 1.1: VID/PID: 26ac:0011
[ INFO] [1454518868.039067740]: VER: 1.1: UID: 3532471736343032
[ WARN] [1454518869.033167830]: VER: broadcast request timeout, retries left 4
[ INFO] [1454518869.034099324]: VER: 1.1: Capabilities 0x00000000000004eb
[ INFO] [1454518869.035464342]: VER: 1.1: Flight software:     00000000 (c7929937505c34d0)
[ INFO] [1454518869.036441033]: VER: 1.1: Middleware software: 00000000 (c7929937505c34d0)
[ INFO] [1454518869.036886180]: VER: 1.1: OS software:         00000000 (0000000000000000)
[ INFO] [1454518869.037344616]: VER: 1.1: Board hardware:      00000000
[ INFO] [1454518869.038979488]: VER: 1.1: VID/PID: 26ac:0011
[ INFO] [1454518869.039404098]: VER: 1.1: UID: 3532471736343032
[ WARN] [1454518870.033431229]: VER: broadcast request timeout, retries left 3
[ INFO] [1454518870.034681068]: VER: 1.1: Capabilities 0x00000000000004eb
[ INFO] [1454518870.035205615]: VER: 1.1: Flight software:     00000000 (c7929937505c34d0)
[ INFO] [1454518870.036815704]: VER: 1.1: Middleware software: 00000000 (c7929937505c34d0)
[ INFO] [1454518870.037267600]: VER: 1.1: OS software:         00000000 (0000000000000000)
[ INFO] [1454518870.038861277]: VER: 1.1: Board hardware:      00000000
[ INFO] [1454518870.039355663]: VER: 1.1: VID/PID: 26ac:0011
[ INFO] [1454518870.039843302]: VER: 1.1: UID: 3532471736343032
[ WARN] [1454518871.033417538]: VER: unicast request timeout, retries left 2
[ WARN] [1454518872.033034414]: VER: unicast request timeout, retries left 1
[ WARN] [1454518873.033034314]: VER: unicast request timeout, retries left 0
[ WARN] [1454518874.033630300]: VER: your FCU don't support AUTOPILOT_VERSION, switched to default capabilities
[ INFO] [1454518874.921988680]: Offboard enabled
[ERROR] [1454518874.958163911]: FCU: Rejecting mode switch cmd
[ WARN] [1454518874.960512851]: CMD: Unexpected command 176, result 1
[ERROR] [1454518875.008518687]: FCU: command temporarily rejected: 176
[ WARN] [1454518878.025217000]: PR: request list timeout, retries left 2
[ WARN] [1454518879.026290018]: PR: request list timeout, retries left 1
[ INFO] [1454518879.970673979]: Offboard enabled
[ WARN] [1454518880.027756703]: PR: request list timeout, retries left 0
[ERROR] [1454518880.031690548]: FCU: Rejecting mode switch cmd
[ERROR] [1454518880.081645528]: FCU: command temporarily rejected: 176
[ WARN] [1454518883.025546075]: WP: timeout, retries left 2
[ WARN] [1454518884.026862483]: WP: timeout, retries left 1
[ INFO] [1454518885.021111274]: Offboard enabled
[ WARN] [1454518885.028362994]: WP: timeout, retries left 0
[ WARN] [1454518885.040450244]: CMD: Unexpected command 176, result 1
[ERROR] [1454518885.046816680]: FCU: Rejecting mode switch cmd
[ERROR] [1454518885.098019127]: FCU: command temporarily rejected: 176
[ERROR] [1454518886.030331330]: WP: timed out.
[ INFO] [1454518890.070927942]: Offboard enabled
[ERROR] [1454518890.120162804]: FCU: Rejecting mode switch cmd
[ERROR] [1454518890.171216818]: FCU: command temporarily rejected: 176
[ INFO] [1454518895.120905630]: Offboard enabled
[ WARN] [1454518895.169745694]: CMD: Unexpected command 176, result 1
[ERROR] [1454518895.183826796]: FCU: Rejecting mode switch cmd
[ERROR] [1454518895.235296340]: FCU: command temporarily rejected: 176
vooon commented 8 years ago

Did you changed target id to incorrect values?

MAVROS started. MY ID [1, 240], TARGET ID [1, 240]

Yes, you did. FCU that respond on version request has id 1, 1

vooon commented 8 years ago

And for testing issue checker please run:

rosrun mavros checkid
damanfb commented 8 years ago

I did not change the system is or target id in my files but I will check then as [1, 1] when I'm back by the quad rotor. I'm away from it until Monday. I'll update then if this worked, thanks for the help so far

On Feb 3, 2016, at 1:28 PM, Vladimir Ermakov notifications@github.com<mailto:notifications@github.com> wrote:

Did you changed target id to incorrect values?

MAVROS started. MY ID [1, 240], TARGET ID [1, 240]

Yes, you did. FCU that respond on version request has id 1, 1

— Reply to this email directly or view it on GitHubhttps://github.com/mavlink/mavros/issues/493#issuecomment-179444966.

damanfb commented 8 years ago

@vooon The target id was the problem. Thanks!