search

mavlink / mavros

MAVLink to ROS gateway with proxy for Ground Control Station
Other
902 stars 993 forks source link

MODE: Unsupported FCU when launched...!! #743

Closed anudeepsekhar closed 7 years ago

anudeepsekhar commented 7 years ago

I have recently updated my pixhawk firmware with the latest stable release PX4 v.1.6.3 and upgraded it using QGC. This was the file i used for upgrade px4fmu-v2_lpe.px4

I am trying to launch the iforoffb.launch and it gives me the following error:

[ERROR] [1498730824.057626067]: MODE: Unsupported FCU

I am really confused whats causing this issue please can somebody help me. I have been at it since a month with no success . Please help me out....!! @TSC21

Here is my launch file:

<launch>

<arg name="fcu_url" default= "/dev/ttyUSB0:921600"/>
<arg name="gcs_url" default="" />
<arg name="tgt_system" default="1" />
<arg name="tgt_component" default="1" />

<node name="mavros" pkg="mavros" type="mavros_node" output="screen" required="true" clear_params="true">
    <param name="fcu_url" value="$(arg fcu_url)" />
    <param name="gcs_url" value="$(arg gcs_url)" />
    <param name="target_system_id" value="$(arg tgt_system)" />
    <param name="target_component_id" value="$(arg tgt_component)" />
    <param name="conn_heartbeat" value="5.0" />
    <param name="conn_timeout" value="5.0" />
    <param name="startup_px4_usb_quirk" value="true" />
    <param name="mocap/use_tf" value="true"/>
    <param name="mocap/use_pose" value="false"/>

    <rosparam command="load" file="$(find mavros)/launch/px4_pluginlists.yaml" />
    <rosparam command="load" file="$(find mavros)/launch/px4_config.yaml" />
</node>

<node name="iforoffb_node" pkg="iforoffb" type="iforoffb_node" output="screen">
</node> 

</launch>

cpp file:

#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.success){
                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;
}

output:

odroid@odroid:~/ifor_ws/src/ifor_offb2/launch$ roslaunch ifor_offb2.launch
... logging to /home/odroid/.ros/log/a96cfc54-5cb2-11e7-944d-c83a35c0110c/roslaunch-odroid-29551.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://odroid:44219/

SUMMARY
========

CLEAR PARAMETERS
 * /mavros/

PARAMETERS
 * /mavros/cmd/use_comp_id_system_control: False
 * /mavros/conn/heartbeat_rate: 1.0
 * /mavros/conn/system_time_rate: 1.0
 * /mavros/conn/timeout: 10.0
 * /mavros/conn/timesync_rate: 10.0
 * /mavros/conn_heartbeat: 5.0
 * /mavros/conn_timeout: 5.0
 * /mavros/distance_sensor/hrlv_ez4_pub/field_of_view: 0.0
 * /mavros/distance_sensor/hrlv_ez4_pub/frame_id: hrlv_ez4_sonar
 * /mavros/distance_sensor/hrlv_ez4_pub/id: 0
 * /mavros/distance_sensor/hrlv_ez4_pub/orientation: ROLL_180
 * /mavros/distance_sensor/hrlv_ez4_pub/send_tf: True
 * /mavros/distance_sensor/hrlv_ez4_pub/sensor_position/x: 0.0
 * /mavros/distance_sensor/hrlv_ez4_pub/sensor_position/y: 0.0
 * /mavros/distance_sensor/hrlv_ez4_pub/sensor_position/z: -0.1
 * /mavros/distance_sensor/laser_1_sub/id: 3
 * /mavros/distance_sensor/laser_1_sub/orientation: ROLL_180
 * /mavros/distance_sensor/laser_1_sub/subscriber: True
 * /mavros/distance_sensor/lidarlite_pub/field_of_view: 0.0
 * /mavros/distance_sensor/lidarlite_pub/frame_id: lidarlite_laser
 * /mavros/distance_sensor/lidarlite_pub/id: 1
 * /mavros/distance_sensor/lidarlite_pub/orientation: ROLL_180
 * /mavros/distance_sensor/lidarlite_pub/send_tf: True
 * /mavros/distance_sensor/lidarlite_pub/sensor_position/x: 0.0
 * /mavros/distance_sensor/lidarlite_pub/sensor_position/y: 0.0
 * /mavros/distance_sensor/lidarlite_pub/sensor_position/z: -0.1
 * /mavros/distance_sensor/sonar_1_sub/id: 2
 * /mavros/distance_sensor/sonar_1_sub/orientation: ROLL_180
 * /mavros/distance_sensor/sonar_1_sub/subscriber: True
 * /mavros/fcu_url: /dev/ttyUSB0:921600
 * /mavros/gcs_url:
 * /mavros/global_position/frame_id: fcu
 * /mavros/global_position/rot_covariance: 99999.0
 * /mavros/global_position/tf/child_frame_id: fcu_utm
 * /mavros/global_position/tf/frame_id: local_origin
 * /mavros/global_position/tf/send: False
 * /mavros/image/frame_id: px4flow
 * /mavros/imu/angular_velocity_stdev: 0.000349065850399
 * /mavros/imu/frame_id: fcu
 * /mavros/imu/linear_acceleration_stdev: 0.0003
 * /mavros/imu/magnetic_stdev: 0.0
 * /mavros/imu/orientation_stdev: 1.0
 * /mavros/local_position/frame_id: fcu
 * /mavros/local_position/tf/child_frame_id: fcu
 * /mavros/local_position/tf/frame_id: local_origin
 * /mavros/local_position/tf/send: False
 * /mavros/local_position/tf/send_fcu: False
 * /mavros/mission/pull_after_gcs: True
 * /mavros/mocap/use_pose: False
 * /mavros/mocap/use_tf: True
 * /mavros/plugin_blacklist: ['safety_area', '...
 * /mavros/plugin_whitelist: []
 * /mavros/px4flow/frame_id: px4flow
 * /mavros/px4flow/ranger_fov: 0.0
 * /mavros/px4flow/ranger_max_range: 5.0
 * /mavros/px4flow/ranger_min_range: 0.3
 * /mavros/safety_area/p1/x: 1.0
 * /mavros/safety_area/p1/y: 1.0
 * /mavros/safety_area/p1/z: 1.0
 * /mavros/safety_area/p2/x: -1.0
 * /mavros/safety_area/p2/y: -1.0
 * /mavros/safety_area/p2/z: -1.0
 * /mavros/setpoint_accel/send_force: False
 * /mavros/setpoint_attitude/reverse_throttle: False
 * /mavros/setpoint_attitude/tf/child_frame_id: attitude
 * /mavros/setpoint_attitude/tf/frame_id: local_origin
 * /mavros/setpoint_attitude/tf/listen: False
 * /mavros/setpoint_attitude/tf/rate_limit: 10.0
 * /mavros/setpoint_position/tf/child_frame_id: setpoint
 * /mavros/setpoint_position/tf/frame_id: local_origin
 * /mavros/setpoint_position/tf/listen: False
 * /mavros/setpoint_position/tf/rate_limit: 50.0
 * /mavros/startup_px4_usb_quirk: True
 * /mavros/sys/disable_diag: False
 * /mavros/sys/min_voltage: 10.0
 * /mavros/target_component_id: 1
 * /mavros/target_system_id: 1
 * /mavros/tdr_radio/low_rssi: 40
 * /mavros/time/time_ref_source: fcu
 * /mavros/time/timesync_avg_alpha: 0.6
 * /mavros/vibration/frame_id: vibration
 * /mavros/vision_pose/tf/child_frame_id: vision
 * /mavros/vision_pose/tf/frame_id: local_origin
 * /mavros/vision_pose/tf/listen: False
 * /mavros/vision_pose/tf/rate_limit: 10.0
 * /mavros/vision_speed/listen_twist: False
 * /rosdistro: indigo
 * /rosversion: 1.11.21

NODES
  /
    ifor_offb2_node (ifor_offb2/ifor_offb2_node)
    mavros (mavros/mavros_node)

auto-starting new master
process[master]: started with pid [29562]
ROS_MASTER_URI=http://localhost:11311

setting /run_id to a96cfc54-5cb2-11e7-944d-c83a35c0110c
process[rosout-1]: started with pid [29575]
started core service [/rosout]
process[mavros-2]: started with pid [29593]
process[ifor_offb2_node-3]: started with pid [29594]

[ifor] test set_mission!
[ INFO] [1498730808.997422409]: arming now!
[ INFO] [1498730809.133934038]: FCU URL: /dev/ttyUSB0:921600
[ INFO] [1498730809.134572199]: serial0: device: /dev/ttyUSB0 @ 921600 bps
[ INFO] [1498730809.137104386]: GCS bridge disabled
[ INFO] [1498730809.301420735]: Plugin 3dr_radio loaded and initialized
[ INFO] [1498730809.324360498]: Plugin actuator_control loaded and initialized
[ INFO] [1498730809.332590428]: Plugin altitude loaded and initialized
[ INFO] [1498730809.577176552]: Plugin cam_imu_sync loaded and initialized
[ INFO] [1498730809.598188123]: Plugin command loaded and initialized
[ INFO] [1498730809.598362705]: Plugin distance_sensor blacklisted
[ INFO] [1498730809.632378457]: Plugin ftp loaded and initialized
[ INFO] [1498730809.668769688]: Plugin global_position loaded and initialized
[ INFO] [1498730809.671801246]: Plugin hil_controls loaded and initialized
[ INFO] [1498730809.671955911]: Plugin image_pub blacklisted
[ INFO] [1498730809.710434958]: Plugin imu_pub loaded and initialized
[ INFO] [1498730809.739807083]: Plugin local_position loaded and initialized
[ INFO] [1498730809.744271295]: Plugin manual_control loaded and initialized
[ INFO] [1498730809.766431773]: Plugin mocap_pose_estimate loaded and initialized
[ INFO] [1498730809.783613584]: Plugin param loaded and initialized
[ INFO] [1498730809.819943733]: Plugin px4flow loaded and initialized
[ INFO] [1498730809.838455325]: Plugin rc_io loaded and initialized
[ INFO] [1498730809.838658782]: Plugin safety_area blacklisted
[ INFO] [1498730809.858498030]: Plugin setpoint_accel loaded and initialized
[ INFO] [1498730809.918916598]: Plugin setpoint_attitude loaded and initialized
[ INFO] [1498730809.952434187]: Plugin setpoint_position loaded and initialized
[ INFO] [1498730810.000345238]: Plugin setpoint_raw loaded and initialized
[ INFO] [1498730810.013812331]: Plugin setpoint_velocity loaded and initialized
[ INFO] [1498730810.046417845]: Plugin sys_status loaded and initialized
[ INFO] [1498730810.066766505]: Plugin sys_time loaded and initialized
[ INFO] [1498730810.072953910]: Plugin vfr_hud loaded and initialized
[ INFO] [1498730810.073137951]: Plugin vibration blacklisted
[ INFO] [1498730810.126972033]: Plugin vision_pose_estimate loaded and initialized
[ INFO] [1498730810.146840947]: Plugin vision_speed_estimate loaded and initialized
[ INFO] [1498730810.163806678]: Plugin waypoint loaded and initialized
[ INFO] [1498730810.164006718]: Autostarting mavlink via USB on PX4
[ INFO] [1498730810.164194091]: Built-in SIMD instructions: None
[ INFO] [1498730810.164305507]: Built-in MAVLink package version: 2016.5.20
[ INFO] [1498730810.164401839]: Built-in MAVLink dialect: ardupilotmega
[ INFO] [1498730810.164493047]: MAVROS started. MY ID 1.240, TARGET ID 1.1
[ERROR] [1498730814.016906800]: MODE: Unsupported FCU
[ERROR] [1498730819.037340725]: MODE: Unsupported FCU
[ERROR] [1498730824.057626067]: MODE: Unsupported FCU
^C[ifor_offb2_node-3] killing on exit
[mavros-2] killing on exit
terminate called without an active exception
[rosout-1] killing on exit
[master] killing on exit
shutting down processing monitor...
... shutting down processing monitor complete
done

diagnostics:

odroid@odroid:~$ rostopic echo -n1 /diagnostics
header:
  seq: 11
  stamp:
    secs: 1498734114
    nsecs: 601373564
  frame_id: ''
status:
  -
    level: 1
    name: mavros: FCU connection
    message: not connected
    hardware_id: /dev/ttyUSB0:921600
    values:
      -
        key: Received packets:
        value: 0
      -
        key: Dropped packets:
        value: 0
      -
        key: Buffer overruns:
        value: 0
      -
        key: Parse errors:
        value: 0
      -
        key: Rx sequence number:
        value: 0
      -
        key: Tx sequence number:
        value: 175
      -
        key: Rx total bytes:
        value: 0
      -
        key: Tx total bytes:
        value: 7064
      -
        key: Rx speed:
        value: 0.000000
      -
        key: Tx speed:
        value: 1349.000000
  -
    level: 2
    name: mavros: GPS
    message: No satellites
    hardware_id: /dev/ttyUSB0:921600
    values:
      -
        key: Satellites visible
        value: 0
      -
        key: Fix type
        value: 0
      -
        key: EPH (m)
        value: Unknown
      -
        key: EPV (m)
        value: Unknown
  -
    level: 2
    name: mavros: Heartbeat
    message: No events recorded.
    hardware_id: /dev/ttyUSB0:921600
    values:
      -
        key: Heartbeats since startup
        value: 0
      -
        key: Frequency (Hz)
        value: 0.000000
      -
        key: Vehicle type
        value: Generic
      -
        key: Autopilot type
        value: Generic
      -
        key: Mode
        value: ''
      -
        key: System status
        value: Uninit
  -
    level: 0
    name: mavros: System
    message: Normal
    hardware_id: /dev/ttyUSB0:921600
    values:
      -
        key: Sensor present
        value: 0x00000000
      -
        key: Sensor enabled
        value: 0x00000000
      -
        key: Sensor helth
        value: 0x00000000
      -
        key: CPU Load (%)
        value: 0.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: 2
    name: mavros: Battery
    message: No data
    hardware_id: /dev/ttyUSB0:921600
    values:
      -
        key: Voltage
        value: -1.00
      -
        key: Current
        value: 0.0
      -
        key: Remaining
        value: 0.0
  -
    level: 2
    name: mavros: Time Sync
    message: No events recorded.
    hardware_id: /dev/ttyUSB0:921600
    values:
      -
        key: Timesyncs since startup
        value: 0
      -
        key: Frequency (Hz)
        value: 0.000000
      -
        key: Last dt (ms)
        value: 0.000000
      -
        key: Mean dt (ms)
        value: 0.000000
      -
        key: Last system time (s)
        value: 0.000000000
      -
        key: Time offset (s)
        value: 0.000000000
---
vooon commented 7 years ago

You do not have a connection with flight controller. Check wiring and param config, e.g. SYS_COMPANION=921600.

anudeepsekhar commented 7 years ago

Yes the problem seems to be with the connector as when we re-soldered the wires of the connectors it worked. We were able to launch it successfully with Odroid as our companion computer and the quad armed as well, but when i used putty to serial into pixhawk it is still simply spitting out garbage.

TSC21 commented 7 years ago

@anudeepsekhar the nsh shell will only work for 57600 baud rate.

Since this is working now, I'm closing this.