search

mavlink / mavros

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

Unable to connect to the FCU with my APM #958

Closed onyezem96 closed 6 years ago

onyezem96 commented 6 years ago

I have been unable to run the offboard code found here for my APM. I know that APMs and PX4 have different modes so I have changed some of the code to account for that but still no success. I think the apm still cannot connect to the FCU connection. Also pardon my formatting, this is my first time creating an issue and i am a novice programmer.

MAVROS version and platform

Mavros: i'm not sure ROS: Kinetic Ubuntu: 16.04

Autopilot type and version

[ X ] ArduPilot [ ] PX4

Version: ?3.7.1?i'm not sure

Node logs

started roslaunch server http://michael-HP-ENVY-x360-Convertible:40749/

SUMMARY
========

CLEAR PARAMETERS
 * /mavros/

PARAMETERS
 * /mavros/cmd/use_comp_id_system_control: False
 * /mavros/conn/heartbeat_rate: 1.0
 * /mavros/conn/system_time_rate: 0.0
 * /mavros/conn/timeout: 10.0
 * /mavros/conn/timesync_rate: 0.0
 * /mavros/distance_sensor/rangefinder_pub/field_of_view: 0.0
 * /mavros/distance_sensor/rangefinder_pub/frame_id: lidar
 * /mavros/distance_sensor/rangefinder_pub/id: 0
 * /mavros/distance_sensor/rangefinder_pub/send_tf: False
 * /mavros/distance_sensor/rangefinder_pub/sensor_position/x: 0.0
 * /mavros/distance_sensor/rangefinder_pub/sensor_position/y: 0.0
 * /mavros/distance_sensor/rangefinder_pub/sensor_position/z: -0.1
 * /mavros/distance_sensor/rangefinder_sub/id: 1
 * /mavros/distance_sensor/rangefinder_sub/orientation: PITCH_270
 * /mavros/distance_sensor/rangefinder_sub/subscriber: True
 * /mavros/fake_gps/eph: 2.0
 * /mavros/fake_gps/epv: 2.0
 * /mavros/fake_gps/fix_type: 3
 * /mavros/fake_gps/geo_origin/alt: 408.0
 * /mavros/fake_gps/geo_origin/lat: 47.3667
 * /mavros/fake_gps/geo_origin/lon: 8.55
 * /mavros/fake_gps/gps_rate: 5.0
 * /mavros/fake_gps/mocap_transform: True
 * /mavros/fake_gps/satellites_visible: 5
 * /mavros/fake_gps/tf/child_frame_id: fix
 * /mavros/fake_gps/tf/frame_id: map
 * /mavros/fake_gps/tf/listen: False
 * /mavros/fake_gps/tf/rate_limit: 10.0
 * /mavros/fake_gps/tf/send: False
 * /mavros/fake_gps/use_mocap: True
 * /mavros/fake_gps/use_vision: False
 * /mavros/fcu_protocol: v2.0
 * /mavros/fcu_url: /dev/ttyACM0:57600
 * /mavros/gcs_url: udp://:14560@127....
 * /mavros/global_position/child_frame_id: base_link
 * /mavros/global_position/frame_id: map
 * /mavros/global_position/rot_covariance: 99999.0
 * /mavros/global_position/tf/child_frame_id: base_link
 * /mavros/global_position/tf/frame_id: map
 * /mavros/global_position/tf/global_frame_id: earth
 * /mavros/global_position/tf/send: False
 * /mavros/global_position/use_relative_alt: True
 * /mavros/image/frame_id: px4flow
 * /mavros/imu/angular_velocity_stdev: 0.000349065850399
 * /mavros/imu/frame_id: base_link
 * /mavros/imu/linear_acceleration_stdev: 0.0003
 * /mavros/imu/magnetic_stdev: 0.0
 * /mavros/imu/orientation_stdev: 1.0
 * /mavros/local_position/frame_id: map
 * /mavros/local_position/tf/child_frame_id: base_link
 * /mavros/local_position/tf/frame_id: map
 * /mavros/local_position/tf/send: False
 * /mavros/local_position/tf/send_fcu: False
 * /mavros/mission/pull_after_gcs: True
 * /mavros/mocap/use_pose: True
 * /mavros/mocap/use_tf: False
 * /mavros/odometry/estimator_type: 3
 * /mavros/odometry/frame_tf/desired_frame: ned
 * /mavros/plugin_blacklist: ['actuator_contro...
 * /mavros/plugin_whitelist: []
 * /mavros/px4flow/frame_id: px4flow
 * /mavros/px4flow/ranger_fov: 0.118682389136
 * /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_thrust: False
 * /mavros/setpoint_attitude/tf/child_frame_id: target_attitude
 * /mavros/setpoint_attitude/tf/frame_id: map
 * /mavros/setpoint_attitude/tf/listen: False
 * /mavros/setpoint_attitude/tf/rate_limit: 50.0
 * /mavros/setpoint_attitude/use_quaternion: False
 * /mavros/setpoint_position/mav_frame: LOCAL_NED
 * /mavros/setpoint_position/tf/child_frame_id: target_position
 * /mavros/setpoint_position/tf/frame_id: map
 * /mavros/setpoint_position/tf/listen: False
 * /mavros/setpoint_position/tf/rate_limit: 50.0
 * /mavros/setpoint_velocity/mav_frame: LOCAL_NED
 * /mavros/startup_px4_usb_quirk: False
 * /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/time/timesync_mode: NONE
 * /mavros/vibration/frame_id: base_link
 * /mavros/vision_pose/tf/child_frame_id: vision_estimate
 * /mavros/vision_pose/tf/frame_id: map
 * /mavros/vision_pose/tf/listen: False
 * /mavros/vision_pose/tf/rate_limit: 10.0
 * /mavros/vision_speed/listen_twist: False
 * /rosdistro: kinetic
 * /rosversion: 1.12.7

NODES
  /
    mavros (mavros/mavros_node)
    offb_node (hector_path_follower/offb_node)

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

setting /run_id to d3a9bc7a-1b0a-11e8-91a5-d0577b40b3a2
process[rosout-1]: started with pid [13475]
started core service [/rosout]
process[mavros-2]: started with pid [13493]
process[offb_node-3]: started with pid [13494]
[ INFO] [1519659395.574538137]: Offboard node started
[ INFO] [1519659395.626322232]: FCU URL: /dev/ttyACM0:57600
[ INFO] [1519659395.627845723]: serial0: device: /dev/ttyACM0 @ 57600 bps
[ INFO] [1519659395.628265643]: GCS URL: udp://:14560@127.0.0.1:14550
[ INFO] [1519659395.628522609]: udp1: Bind address: 0.0.0.0:14560
[ INFO] [1519659395.628572858]: udp1: Remote address: 127.0.0.1:14550
[ INFO] [1519659395.660364703]: Plugin 3dr_radio loaded
[ INFO] [1519659395.662356555]: Plugin 3dr_radio initialized
[ INFO] [1519659395.662381865]: Plugin actuator_control blacklisted
[ INFO] [1519659395.685542610]: Plugin adsb loaded
[ INFO] [1519659395.690326675]: Plugin adsb initialized
[ INFO] [1519659395.690356726]: Plugin altitude blacklisted
[ INFO] [1519659395.690473529]: Plugin cam_imu_sync loaded
[ INFO] [1519659395.691113403]: Plugin cam_imu_sync initialized
[ INFO] [1519659395.691270697]: Plugin command loaded
[ INFO] [1519659395.695774790]: Plugin command initialized
[ INFO] [1519659395.695799831]: Plugin debug_value blacklisted
[ INFO] [1519659395.695952394]: Plugin distance_sensor loaded
[ INFO] [1519659395.703683187]: Plugin distance_sensor initialized
[ INFO] [1519659395.703850965]: Plugin fake_gps loaded
[ INFO] [1519659395.717677123]: Plugin fake_gps initialized
[ INFO] [1519659395.717722776]: Plugin ftp blacklisted
[ INFO] [1519659395.717920664]: Plugin global_position loaded
[ INFO] [1519659395.732861604]: Plugin global_position initialized
[ INFO] [1519659395.732893464]: Plugin hil blacklisted
[ INFO] [1519659395.733050155]: Plugin home_position loaded
[ INFO] [1519659395.736556629]: Plugin home_position initialized
[ INFO] [1519659395.736710337]: Plugin imu loaded
[ INFO] [1519659395.743296749]: Plugin imu initialized
[ INFO] [1519659395.743463262]: Plugin local_position loaded
[ INFO] [1519659395.750604327]: Plugin local_position initialized
[ INFO] [1519659395.750821971]: Plugin manual_control loaded
[ INFO] [1519659395.754151769]: Plugin manual_control initialized
[ INFO] [1519659395.754183193]: Plugin mocap_pose_estimate blacklisted
[ INFO] [1519659395.754311262]: Plugin obstacle_distance loaded
[ INFO] [1519659395.756695786]: Plugin obstacle_distance initialized
[ INFO] [1519659395.756827570]: Plugin odom loaded
[ INFO] [1519659395.760329254]: Plugin odom initialized
[ INFO] [1519659395.760562865]: Plugin param loaded
[ INFO] [1519659395.762981712]: Plugin param initialized
[ INFO] [1519659395.763014596]: Plugin px4flow blacklisted
[ INFO] [1519659395.763140122]: Plugin rangefinder loaded
[ INFO] [1519659395.763787494]: Plugin rangefinder initialized
[ INFO] [1519659395.763947344]: Plugin rc_io loaded
[ INFO] [1519659395.767259573]: Plugin rc_io initialized
[ INFO] [1519659395.767287524]: Plugin safety_area blacklisted
[ INFO] [1519659395.767439388]: Plugin setpoint_accel loaded
[ INFO] [1519659395.770234993]: Plugin setpoint_accel initialized
[ INFO] [1519659395.770468245]: Plugin setpoint_attitude loaded
[ INFO] [1519659395.779708837]: Plugin setpoint_attitude initialized
[ INFO] [1519659395.779910778]: Plugin setpoint_position loaded
[ INFO] [1519659395.794532004]: Plugin setpoint_position initialized
[ INFO] [1519659395.794696747]: Plugin setpoint_raw loaded
[ INFO] [1519659395.803327505]: Plugin setpoint_raw initialized
[ INFO] [1519659395.803484689]: Plugin setpoint_velocity loaded
[ INFO] [1519659395.809167968]: Plugin setpoint_velocity initialized
[ INFO] [1519659395.809454780]: Plugin sys_status loaded
[ INFO] [1519659395.817709484]: Plugin sys_status initialized
[ INFO] [1519659395.817905328]: Plugin sys_time loaded
[ INFO] [1519659395.821290073]: TM: Timesync mode: NONE
[ INFO] [1519659395.821866840]: Plugin sys_time initialized
[ INFO] [1519659395.822029988]: Plugin vfr_hud loaded
[ INFO] [1519659395.823229187]: Plugin vfr_hud initialized
[ INFO] [1519659395.823257325]: Plugin vibration blacklisted
[ INFO] [1519659395.823280711]: Plugin vision_pose_estimate blacklisted
[ INFO] [1519659395.823301393]: Plugin vision_speed_estimate blacklisted
[ INFO] [1519659395.823470399]: Plugin waypoint loaded
[ INFO] [1519659395.827876038]: Plugin waypoint initialized
[ INFO] [1519659395.827971508]: Built-in SIMD instructions: SSE, SSE2
[ INFO] [1519659395.827994542]: Built-in MAVLink package version: 2018.2.2
[ INFO] [1519659395.828027264]: Known MAVLink dialects: common ardupilotmega ASLUAV autoquad matrixpilot paparazzi slugs standard uAvionix ualberta
[ INFO] [1519659395.828050199]: MAVROS started. MY ID 1.240, TARGET ID 1.1

Diagnostics

header: 
  seq: 124
  stamp: 
    secs: 1519658446
    nsecs: 974573769
  frame_id: ''
status: 
  - 
    level: 1
    name: mavros: FCU connection
    message: not connected
    hardware_id: /dev/ttyACM0:57600
    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: 79
      - 
        key: Rx total bytes:
        value: 833
      - 
        key: Tx total bytes:
        value: 1659
      - 
        key: Rx speed:
        value: 8.000000
      - 
        key: Tx speed:
        value: 21.000000
  - 
    level: 2
    name: mavros: GPS
    message: No satellites
    hardware_id: /dev/ttyACM0:57600
    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/ttyACM0:57600
    values: 
      - 
        key: Heartbeats since startup
        value: 0
      - 
        key: Frequency (Hz)
        value: 0.000000
      - 
        key: Vehicle type
        value: Generic micro air vehicle
      - 
        key: Autopilot type
        value: Generic autopilot
      - 
        key: Mode
        value: ''
      - 
        key: System status
        value: Uninit
  - 
    level: 0
    name: mavros: System
    message: Normal
    hardware_id: /dev/ttyACM0:57600
    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/ttyACM0:57600
    values: 
      - 
        key: Voltage
        value: -1.00
      - 
        key: Current
        value: 0.0
      - 
        key: Remaining
        value: 0.0
---

Check ID

ERROR. I got 2 addresses, but not your target 1:1

---
Received 3 messages, from 2 addresses
sys:comp   list of messages
  0:8     8
 132:8     40, 8

My Code

/**
 * @file offb_node.cpp
 * @brief Offboard control example node, written with MAVROS version 0.19.x, PX4 Pro Flight
 * Stack and tested in Gazebo SITL
 */

#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_INFO("Offboard node started");
    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_INFO("waiting for connection");
        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 = "GUIDED";

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

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

    while(ros::ok()){
        if( current_state.mode != "GUIDED" &&
            (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("GUIDED 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;
}
vooon commented 6 years ago

Did you able to connect with QGC/APM Planner via /dev/ttyACM0? Maybe it need some setup.

onyezem96 commented 6 years ago

Yes I did. When I try to run both however I get a new error saying

[FATAL] [1519661387.212448533]: FCU: DeviceError:serial:open: Device or resource busy REQUIRED process [mavros-2] has died! process has finished cleanly log file: /home/michael/.ros/log/76baae2a-1b0f-11e8-91a5-d0577b40b3a2/mavros-2*.log Initiating shutdown!

TSC21 commented 6 years ago

You cannot connect them at the same time. If the serial is open to a port, cannot be reserved accessed by another,

onyezem96 commented 6 years ago

Ok I did not know that. I do not need to use apm planner to use this code initially however right?

mellonUAV commented 6 years ago

@onyezem96,

I connect to both FCU and GCS(ground control station) by launching mavros apm.launch like this:

$ roslaunch mavros apm.launch fcu_url:=udp://10.0.0.2:6001@ gcs_url:=tcp-l://

I first launch mavros, then FCU and after that I open the APM Planner. You can also add these to corresponding ros launch file. Hope this helps.

onyezem96 commented 6 years ago

It helped a bit. It made me realize that my baud rate was set too low. It worked with 115200 and not 57600 for some reason.

guduzhe123 commented 6 years ago

You cannot have QGC to automatically connect to the FCU. Go to QGC settings and disable the option to connect to the Pixhawk and the SiK Radio. And launch: rosrun mavros mavros_node _fcu_url:=/dev/ttyACM0:921600 _gcs_url:=udp-b://127.0.0.1:14555@14550 This works for me.