Unable to change to AUTO modes

[gawebb-dstl]

Issue details

I'm unable to switch into the any of the AUTO modes. This is how I'm currently trying:

mavros_msgs::SetMode setMode;
...
setMode.request.custom_mode = "AUTO.LOITER" ;
if( current_state.mode != "AUTO.LOITER"  && (ros::Time::now() - last_request > ros::Duration(TIME_IN_BETWEEN_COMMANDS))){
    if( set_mode_client.call(setMode) && setMode.response.success){
        ROS_INFO_STREAM("Response " << setMode.response);
    }
    last_request = ros::Time::now();
} 
...

However when using any of the AUTO modes it will switch to AUTO.RTL. I can successfully change to OFFBOARD using the same code by replacing "AUTO.LOITER" with "OFFBOARD" and after the if statement sending a position with:

geometry_msgs::PoseStamped pose2send;
local_pos_pub.publish(pose2send);

I feel like this may just be poor implementation rather than a issue as such. If you need any additional information please let me know. I haven't included the full code as it is over 500 lines long (I can launch, land, etc) Thank you in advance for any feedback or advice

MAVROS version and platform

Mavros: 0.18.5 ROS: Kinetic Ubuntu: 16.04

Autopilot type and version

[ ] ArduPilot [ X ] PX4

Version: ?

Node logs

[ WARN] [1499761750.717252861]: TM: Clock skew detected (-0.014167200 s). Hard syncing clocks.
[ INFO] [1499761767.476603621]: FCU: ARMED by arm/disarm component command
[ INFO] [1499761767.484455503]: FCU: [logger] file: rootfs/fs/microsd/log/2017-07-11/0
[ INFO] [1499761767.490913817]: FCU: Taking off
[ERROR] [1499761767.493797606]: FCU: failsafe mode off
[ INFO] [1499761768.166906036]: FCU: Takeoff detected
[ INFO] [1499761769.114373388]: FCU: [lpe] land timeout 
[ WARN] [1499761769.215144471]: TM: Clock skew detected (-0.010285429 s). Hard syncing clocks.
[ WARN] [1499761783.015127143]: TM: Clock skew detected (-0.021271531 s). Hard syncing clocks.
[ WARN] [1499761790.467704894]: CMD: Unexpected command 176, result 0
[ INFO] [1499761790.468432015]: FCU: no datalink
[ERROR] [1499761790.470694533]: FCU: failsafe mode on
[ INFO] [1499761790.481651463]: FCU: RTL: return at 11 m (10 m above home)
[ INFO] [1499761790.496372606]: FCU: RTL: descend to 6 m (5 m above home)
[ INFO] [1499761792.962085623]: FCU: RTL: loiter 5.0s
[ INFO] [1499761798.002369481]: FCU: RTL: land at home
[ INFO] [1499761812.080558908]: FCU: Landing detected
[ INFO] [1499761812.087016952]: FCU: RTL: completed, landed
[ INFO] [1499761812.197009750]: FCU: [lpe] land init
[ INFO] [1499761815.101863741]: FCU: DISARMED by auto disarm on land
[ WARN] [1499762092.713851071]: TM: Clock skew detected (-0.010004356 s). Hard syncing clocks.

Diagnostics

header: 
  seq: 2333
  stamp: 
    secs: 1499762515
    nsecs: 779993466
  frame_id: ''
status: 
  - 
    level: 0
    name: mavros: FCU connection
    message: connected
    hardware_id: udp://:14540@127.0.0.1:14557
    values: 
      - 
        key: Received packets:
        value: 57018
      - 
        key: Dropped packets:
        value: 0
      - 
        key: Buffer overruns:
        value: 0
      - 
        key: Parse errors:
        value: 0
      - 
        key: Rx sequence number:
        value: 210
      - 
        key: Tx sequence number:
        value: 146
      - 
        key: Rx total bytes:
        value: 213701816
      - 
        key: Tx total bytes:
        value: 12767184
      - 
        key: Rx speed:
        value: 132022.000000
      - 
        key: Tx speed:
        value: 13202.000000
  - 
    level: 0
    name: mavros: GPS
    message: 3D fix
    hardware_id: udp://:14540@127.0.0.1:14557
    values: 
      - 
        key: Satellites visible
        value: 10
      - 
        key: Fix type
        value: 3
      - 
        key: EPH (m)
        value: 0.00
      - 
        key: EPV (m)
        value: 0.00
  - 
    level: 0
    name: mavros: Heartbeat
    message: Normal
    hardware_id: udp://:14540@127.0.0.1:14557
    values: 
      - 
        key: Heartbeats since startup
        value: 2947
      - 
        key: Frequency (Hz)
        value: 1.000498
      - 
        key: Vehicle type
        value: Quadrotor
      - 
        key: Autopilot type
        value: PX4 Autopilot
      - 
        key: Mode
        value: AUTO.RTL
      - 
        key: System status
        value: Standby
  - 
    level: 0
    name: mavros: System
    message: Normal
    hardware_id: udp://:14540@127.0.0.1:14557
    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: 0
    name: mavros: Battery
    message: Normal
    hardware_id: udp://:14540@127.0.0.1:14557
    values: 
      - 
        key: Voltage
        value: 12.15
      - 
        key: Current
        value: -1.0
      - 
        key: Remaining
        value: 99.0
  - 
    level: 0
    name: mavros: Time Sync
    message: Normal
    hardware_id: udp://:14540@127.0.0.1:14557
    values: 
      - 
        key: Timesyncs since startup
        value: 8
      - 
        key: Frequency (Hz)
        value: 9.223080
      - 
        key: Last dt (ms)
        value: -0.663441
      - 
        key: Mean dt (ms)
        value: -1.738984
      - 
        key: Last system time (s)
        value: 2948.321254000
      - 
        key: Time offset (s)
        value: 1499759567.391520023

Check ID

OK. I got messages from 1:1.

---
Received 37112 messages, from 1 addresses
sys:comp   list of messages
  1:1     0, 1, 2, 140, 141, 147, 24, 30, 32, 33, 42, 74, 76, 83, 85, 87, 230, 105, 111, 241, 242, 245

[vooon]

It is firmware side. As i remember only AUTO.MISSION is allowed to set from telemetry.

[gawebb-dstl]

The same problem exists with trying to change into AUTO.MISSION mode. I can see that I've sent way-points but I'm unable to get it to move to them. The only way I've managed this, is by using QGroundControl to change the mode to AUTO.MISSION after I've sent the waypoints with mavros. It will then goto to the waypoints.

[feilongfl]

same problem here,i run in gazebo,then i use fallsafe code this

void modeLOITER(ros::NodeHandle& nh)
{
    if (current_state.mode != "AUTO.LOITER") {
        ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>("mavros/set_mode");

        mavros_msgs::SetMode offb_set_mode;
        offb_set_mode.request.custom_mode = "AUTO.LOITER";

        if (set_mode_client.call(offb_set_mode) && offb_set_mode.response.success) {
            ROS_INFO("LOITER enabled");
        }
    }
}

void armAndModeOffboard(ros::NodeHandle& nh)
{
    // turn board into offboard mode
    if (current_state.mode != "OFFBOARD") {
        modeOffBoard(nh);
        // last_request = ros::Time::now();
    } else {
        // check system armd
        if (!current_state.armed) {
            arm(nh);
            // last_request = ros::Time::now();
        }
    }
}

void fallSafe(ros::NodeHandle& nh)
{
    //0.05
    if (ros::Time::now() - msg.Data.t > ros::Duration(1.0)) {
        modeLOITER(nh);
        //msg.sendStatusUseSerial(eular, current_state, pos_status, dis_status);
    } else {
        armAndModeOffboard(nh);
    }
}

all code in https://github.com/feilongfl/Ros-Px4/blob/fallsafebug/src/mavpi/src/offb_node.cpp

but when plan in air,it show me no warning like "no rc ctrl or offboard",but it in auto.rtl mode.

this picture is before change mode,work in offboard

than i change it to AUTO.LOITER,it turn in auto.rtl with no warning!!!and it show me offboard mode,but in gazebo simluator i see it clearly in auto rtl mode

then only landed it turn to auto.loiter

[feilongfl]

system ubuntu 16.04 gazebo is in docker use dockerfile https://github.com/feilongfl/px4-dev-simulation ros Kinetic

[TSC21]

I'm closing this since this is Firmware related.