[SOLVED] PX4 SITL position hold not working

jake · November 30, 2016, 3:31am

Hi,

I am attempting to run the example off board control code ( http://dev.px4.io/ros-mavros-offboard.html ).

I am able to get the drone to arm and take off, but it only seems to only hold altitude (Z), but not X and Y position.

Here is my terminal output:
drone@drone:~$ roslaunch UAV_Offboard px4Local.launch
WARNING: Package name “UAV_Offboard” does not follow the naming conventions. It should start with a lower case letter and only contain lower case letters, digits and underscores.
… logging to /home/drone/.ros/log/dfc152a4-b6aa-11e6-9f14-080027fec8b7/roslaunch-drone-11877.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.

WARNING: Package name "UAV_Offboard" does not follow the naming conventions. It should start with a lower case letter and only contain lower case letters, digits and underscores.
WARNING: Package name "UAV_Offboard" does not follow the naming conventions. It should start with a lower case letter and only contain lower case letters, digits and underscores.
WARNING: Package name "UAV_Offboard" does not follow the naming conventions. It should start with a lower case letter and only contain lower case letters, digits and underscores.
started roslaunch server http://drone:34477/

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/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: udp://:14550@loca...
 * /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: True
 * /mavros/mocap/use_tf: False
 * /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: kinetic
 * /rosversion: 1.12.6

NODES
  /
    mavros (mavros/mavros_node)

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

setting /run_id to dfc152a4-b6aa-11e6-9f14-080027fec8b7
WARNING: Package name "UAV_Offboard" does not follow the naming conventions. It should start with a lower case letter and only contain lower case letters, digits and underscores.
process[rosout-1]: started with pid [11901]
started core service [/rosout]
process[mavros-2]: started with pid [11909]
[ INFO] [1480475570.715751081]: FCU URL: udp://:14550@localhost
[ WARN] [1480475570.716326623]: init: message from ASLUAV, MSG-ID 201 ignored! Table has different entry.
[ WARN] [1480475570.716505943]: init: message from autoquad, MSG-ID 150 ignored! Table has different entry.
[ WARN] [1480475570.716558728]: init: message from autoquad, MSG-ID 152 ignored! Table has different entry.
[ WARN] [1480475570.716695910]: init: message from matrixpilot, MSG-ID 150 ignored! Table has different entry.
[ WARN] [1480475570.716721511]: init: message from matrixpilot, MSG-ID 151 ignored! Table has different entry.
[ WARN] [1480475570.716739688]: init: message from matrixpilot, MSG-ID 152 ignored! Table has different entry.
[ WARN] [1480475570.716776984]: init: message from matrixpilot, MSG-ID 153 ignored! Table has different entry.
[ WARN] [1480475570.716801737]: init: message from matrixpilot, MSG-ID 155 ignored! Table has different entry.
[ WARN] [1480475570.716823396]: init: message from matrixpilot, MSG-ID 156 ignored! Table has different entry.
[ WARN] [1480475570.716841641]: init: message from matrixpilot, MSG-ID 157 ignored! Table has different entry.
[ WARN] [1480475570.716859262]: init: message from matrixpilot, MSG-ID 158 ignored! Table has different entry.
[ WARN] [1480475570.716892050]: init: message from matrixpilot, MSG-ID 170 ignored! Table has different entry.
[ WARN] [1480475570.716917463]: init: message from matrixpilot, MSG-ID 171 ignored! Table has different entry.
[ WARN] [1480475570.716937967]: init: message from matrixpilot, MSG-ID 172 ignored! Table has different entry.
[ WARN] [1480475570.716958592]: init: message from matrixpilot, MSG-ID 173 ignored! Table has different entry.
[ WARN] [1480475570.716978828]: init: message from matrixpilot, MSG-ID 174 ignored! Table has different entry.
[ WARN] [1480475570.716998930]: init: message from matrixpilot, MSG-ID 175 ignored! Table has different entry.
[ WARN] [1480475570.717015598]: init: message from matrixpilot, MSG-ID 176 ignored! Table has different entry.
[ WARN] [1480475570.717049296]: init: message from matrixpilot, MSG-ID 177 ignored! Table has different entry.
[ WARN] [1480475570.717072921]: init: message from matrixpilot, MSG-ID 178 ignored! Table has different entry.
[ WARN] [1480475570.717103410]: init: message from matrixpilot, MSG-ID 179 ignored! Table has different entry.
[ WARN] [1480475570.717125815]: init: message from matrixpilot, MSG-ID 180 ignored! Table has different entry.
[ WARN] [1480475570.717145798]: init: message from matrixpilot, MSG-ID 181 ignored! Table has different entry.
[ WARN] [1480475570.717165025]: init: message from matrixpilot, MSG-ID 182 ignored! Table has different entry.
[ WARN] [1480475570.717187618]: init: message from matrixpilot, MSG-ID 183 ignored! Table has different entry.
[ WARN] [1480475570.717199828]: init: message from matrixpilot, MSG-ID 184 ignored! Table has different entry.
[ WARN] [1480475570.717252487]: init: message from matrixpilot, MSG-ID 185 ignored! Table has different entry.
[ WARN] [1480475570.717280007]: init: message from matrixpilot, MSG-ID 186 ignored! Table has different entry.
[ WARN] [1480475570.717408674]: init: message from paparazzi, MSG-ID 180 ignored! Table has different entry.
[ WARN] [1480475570.717434310]: init: message from paparazzi, MSG-ID 181 ignored! Table has different entry.
[ WARN] [1480475570.717555633]: init: message from paparazzi, MSG-ID 182 ignored! Table has different entry.
[ WARN] [1480475570.717586497]: init: message from paparazzi, MSG-ID 183 ignored! Table has different entry.
[ WARN] [1480475570.717607773]: init: message from paparazzi, MSG-ID 184 ignored! Table has different entry.
[ WARN] [1480475570.717735762]: init: message from slugs, MSG-ID 170 ignored! Table has different entry.
[ WARN] [1480475570.717759582]: init: message from slugs, MSG-ID 172 ignored! Table has different entry.
[ WARN] [1480475570.717779035]: init: message from slugs, MSG-ID 173 ignored! Table has different entry.
[ WARN] [1480475570.717820721]: init: message from slugs, MSG-ID 176 ignored! Table has different entry.
[ WARN] [1480475570.717851702]: init: message from slugs, MSG-ID 177 ignored! Table has different entry.
[ WARN] [1480475570.717875623]: init: message from slugs, MSG-ID 179 ignored! Table has different entry.
[ WARN] [1480475570.717895589]: init: message from slugs, MSG-ID 180 ignored! Table has different entry.
[ WARN] [1480475570.717919412]: init: message from slugs, MSG-ID 181 ignored! Table has different entry.
[ WARN] [1480475570.717955650]: init: message from slugs, MSG-ID 184 ignored! Table has different entry.
[ WARN] [1480475570.717987088]: init: message from slugs, MSG-ID 185 ignored! Table has different entry.
[ WARN] [1480475570.718011739]: init: message from slugs, MSG-ID 186 ignored! Table has different entry.
[ WARN] [1480475570.718035943]: init: message from slugs, MSG-ID 188 ignored! Table has different entry.
[ WARN] [1480475570.718061061]: init: message from slugs, MSG-ID 191 ignored! Table has different entry.
[ WARN] [1480475570.718084868]: init: message from slugs, MSG-ID 192 ignored! Table has different entry.
[ WARN] [1480475570.718111204]: init: message from slugs, MSG-ID 193 ignored! Table has different entry.
[ WARN] [1480475570.718147481]: init: message from slugs, MSG-ID 194 ignored! Table has different entry.
[ INFO] [1480475570.718575841]: udp0: Bind address: 0.0.0.0:14550
[ INFO] [1480475570.718728862]: udp0: Remote address: 127.0.0.1:14550
[ INFO] [1480475570.718894441]: GCS bridge disabled
[ INFO] [1480475570.722335775]: udp0: Remote address: 127.0.0.1:14556
[ INFO] [1480475570.816816813]: Plugin 3dr_radio loaded
[ INFO] [1480475570.820193856]: Plugin 3dr_radio initialized
[ INFO] [1480475570.820368547]: Plugin actuator_control loaded
[ INFO] [1480475570.837178014]: Plugin actuator_control initialized
[ INFO] [1480475570.837374964]: Plugin altitude loaded
[ INFO] [1480475570.844188322]: Plugin altitude initialized
[ INFO] [1480475570.927789088]: Plugin cam_imu_sync loaded
[ INFO] [1480475570.931899753]: Plugin cam_imu_sync initialized
[ INFO] [1480475570.932241803]: Plugin command loaded
[ INFO] [1480475570.948657941]: Plugin command initialized
[ INFO] [1480475570.948785942]: Plugin distance_sensor blacklisted
[ INFO] [1480475570.948930959]: Plugin ftp loaded
[ INFO] [1480475570.982602313]: Plugin ftp initialized
[ INFO] [1480475570.982845977]: Plugin global_position loaded
[ INFO] [1480475571.007636801]: Plugin global_position initialized
[ INFO] [1480475571.007934937]: Plugin hil_actuator_controls loaded
[ INFO] [1480475571.009354793]: Plugin hil_actuator_controls initialized
[ INFO] [1480475571.009615285]: Plugin hil_controls loaded
[ INFO] [1480475571.012392486]: Plugin hil_controls initialized
[ INFO] [1480475571.012670241]: Plugin imu_pub loaded
[ INFO] [1480475571.063698529]: Plugin imu_pub initialized
[ INFO] [1480475571.063872647]: Plugin local_position loaded
[ INFO] [1480475571.080477304]: Plugin local_position initialized
[ INFO] [1480475571.080655165]: Plugin manual_control loaded
[ INFO] [1480475571.081854992]: Plugin manual_control initialized
[ INFO] [1480475571.082013526]: Plugin mocap_fake_gps loaded
[ INFO] [1480475571.096113244]: Plugin mocap_fake_gps initialized
[ INFO] [1480475571.096402167]: Plugin mocap_pose_estimate loaded
[ INFO] [1480475571.112966403]: Plugin mocap_pose_estimate initialized
[ INFO] [1480475571.113209818]: Plugin param loaded
[ INFO] [1480475571.117810928]: Plugin param initialized
[ INFO] [1480475571.118016898]: Plugin px4flow loaded
[ INFO] [1480475571.145336598]: Plugin px4flow initialized
[ INFO] [1480475571.145544488]: Plugin rc_io loaded
[ INFO] [1480475571.151988834]: Plugin rc_io initialized
[ INFO] [1480475571.152052905]: Plugin safety_area blacklisted
[ INFO] [1480475571.152183843]: Plugin setpoint_accel loaded
[ INFO] [1480475571.166690168]: Plugin setpoint_accel initialized
[ INFO] [1480475571.166929553]: Plugin setpoint_attitude loaded
[ INFO] [1480475571.251640242]: Plugin setpoint_attitude initialized
[ INFO] [1480475571.252042558]: Plugin setpoint_position loaded
[ INFO] [1480475571.303299835]: Plugin setpoint_position initialized
[ INFO] [1480475571.303499056]: Plugin setpoint_raw loaded
[ INFO] [1480475571.325236283]: Plugin setpoint_raw initialized
[ INFO] [1480475571.325648899]: Plugin setpoint_velocity loaded
[ INFO] [1480475571.331732014]: Plugin setpoint_velocity initialized
[ INFO] [1480475571.331921715]: Plugin sys_status loaded
[ INFO] [1480475571.360280469]: Plugin sys_status initialized
[ INFO] [1480475571.361958414]: Plugin sys_time loaded
[ INFO] [1480475571.377467357]: Plugin sys_time initialized
[ INFO] [1480475571.377724508]: Plugin vfr_hud loaded
[ INFO] [1480475571.380911387]: Plugin vfr_hud initialized
[ INFO] [1480475571.380991407]: Plugin vibration blacklisted
[ INFO] [1480475571.381137056]: Plugin vision_pose_estimate loaded
[ INFO] [1480475571.425272987]: Plugin vision_pose_estimate initialized
[ INFO] [1480475571.425480026]: Plugin vision_speed_estimate loaded
[ INFO] [1480475571.441654162]: Plugin vision_speed_estimate initialized
[ INFO] [1480475571.441917676]: Plugin waypoint loaded
[ INFO] [1480475571.455217484]: Plugin waypoint initialized
[ INFO] [1480475571.455304484]: Autostarting mavlink via USB on PX4
[ INFO] [1480475571.455421473]: Built-in SIMD instructions: SSE, SSE2
[ INFO] [1480475571.455457895]: Built-in MAVLink package version: 2016.11.11
[ INFO] [1480475571.455500616]: Known MAVLink dialects: common ardupilotmega ASLUAV autoquad matrixpilot paparazzi slugs standard uAvionix ualberta
[ INFO] [1480475571.455535813]: MAVROS started. MY ID 1.240, TARGET ID 1.1
[ INFO] [1480475571.467886350]: IMU: Attitude quaternion IMU detected!
[ INFO] [1480475571.471285622]: IMU: High resolution IMU detected!
[ WARN] [1480475571.478517844]: TM: Clock skew detected (-1480475563.003579140 s). Hard syncing clocks.
[ INFO] [1480475572.064478260]: CON: Got HEARTBEAT, connected. FCU: PX4 Autopilot
[ INFO] [1480475572.065106379]: IMU: High resolution IMU detected!
[ INFO] [1480475572.065674621]: IMU: Attitude quaternion IMU detected!
[ INFO] [1480475573.067213625]: VER: 1.1: Capabilities         0x00000000000024ef
[ INFO] [1480475573.067275482]: VER: 1.1: Flight software:     01050000 (000000005E91B390)
[ INFO] [1480475573.067320106]: VER: 1.1: Middleware software: 01050000 (000000005E91B390)
[ INFO] [1480475573.067359279]: VER: 1.1: OS software:         000000ff (0000000000000000)
[ INFO] [1480475573.067380315]: VER: 1.1: Board hardware:      00000001
[ INFO] [1480475573.067567473]: VER: 1.1: VID/PID:             0000:0000
[ INFO] [1480475573.067606748]: VER: 1.1: UID:                 0000000100000002
[ WARN] [1480475581.778425763]: TM: Clock skew detected (-0.011956273 s). Hard syncing clocks.
[ WARN] [1480475584.361216434]: CMD: Unexpected command 176, result 0
[ INFO] [1480475587.066047456]: WP: mission received
[ INFO] [1480475589.369496431]: FCU: ARMED by arm/disarm component command
[ INFO] [1480475589.432546936]: FCU: Takeoff detected
[ WARN] [1480475592.279338885]: TM: Clock skew detected (-0.011860683 s). Hard syncing clocks.
[ WARN] [1480475602.777673085]: TM: Clock skew detected (-0.010517425 s). Hard syncing clocks.

I suspect this line might reveal the relevant problem:
[ WARN] [1480475584.361216434]: CMD: Unexpected command 176, result 0

I can confirm that the correct mavlink messages are being sent:
drone@drone:~$ rostopic echo /mavros/setpoint_position/local
header:
seq: 55
stamp:
secs: 0
nsecs: 0
frame_id: ‘’
pose:
position:
x: 0.0
y: 0.0
z: 2.0
orientation:
x: 0.0
y: 0.0
z: 0.0
w: 0.0
—
header:
seq: 56
stamp:
secs: 0
nsecs: 0
frame_id: ‘’
pose:
position:
x: 0.0
y: 0.0
z: 2.0
orientation:
x: 0.0
y: 0.0
z: 0.0
w: 0.0
—

Does anyone have any idea how this may be resolved?
Thanks!

zyq1993 · December 1, 2016, 9:00am

@jake
I don’t know whether I have understood your meanings.
The example code in the link( http://dev.px4.io/ros-mavros-offboard.html ) sets the coordinates of the target location is (0,0,2), so the iris only moves in the height direction(Z axis).

jake · December 2, 2016, 9:01pm

@zyq1993 here is a video of what happens:

The drone only holds z altidude, but drifts out in X and Y.

eric1221bday · December 2, 2016, 9:58pm

@jake Looking at your video and logs, it looks like your local position estimator is not getting a gps lock. I’ve actually had this issue before. If you look closely you’ll notice in your flight log and although your lpe emits z resumed, it does not emit xy resumed, which is what it would show if there is a position lock. If you have qgroundcontrol installed, would you mind opening qgroundcontrol while the simulation is running and check if the parameter LPE_GPS_ON is enabled? For some bizarre reason sometimes that parameter gets disabled, resulting in the lpe not being able to grab the simulated GPS, if you enable it thing should work

jake · December 2, 2016, 11:51pm

@eric1221bday thanks, that was the problem!!

It works now