search

PX4 / PX4-Autopilot

PX4 Autopilot Software
https://px4.io
BSD 3-Clause "New" or "Revised" License
8.56k stars 13.55k forks source link

PX4 1.12.3 Autoland Sets Yaw setpoint to +/- 90 degrees instead of current heading #18766

Closed gssammy closed 1 month ago

gssammy commented 2 years ago

Describe the bug A clear and concise description of the bug. Setting mode to Autoland from Offboard causes the drone to yaw to 180 and then yaw back to the current setting while landing. This happens both in sitl simulation and real flying.

To Reproduce Steps to reproduce the behavior:

  1. Start sending waypoints to drone
  2. Arm drone and changed mode to Offboard
  3. While drone is following commanded waypoints switch mode to AutoLand
  4. See drone do weird yaws while descending

Expected behavior The drone should go straight down and land

Log Files and Screenshots Always provide a link to the flight log file: Ulog from real flight: https://logs.px4.io/plot_app?log=90c95b8f-47be-41b4-a508-4609fe3b3edf#Nav-Yaw-Angle

Ulog from sitl with custom gazebo setup: https://logs.px4.io/plot_app?log=5bf30ece-9637-4a9a-b847-230927c8159f#Nav-Yaw-Angle

Add screenshots to help explain your problem.

Drone (please complete the following information): This is a PX4 quadcopter Real drone uses a pixracer

Additional context

It seems that there is something in land.cpp that is setting the yaw of the waypoint before landing to 180. I edited land.cpp with some print outs just to see what was happening:

Land::on_activation()
{
    /* set current mission item to Land */
    set_land_item(&_mission_item, true);
    _navigator->get_mission_result()->finished = false;
    _navigator->set_mission_result_updated();
    reset_mission_item_reached();

    /* convert mission item to current setpoint */
    struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
    printf("Current Yaw: %9.6f" ,pos_sp_triplet->current.yaw);
    pos_sp_triplet->previous.valid = false;
    mission_apply_limitation(_mission_item);
    mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
    pos_sp_triplet->next.valid = false;

    _navigator->set_can_loiter_at_sp(false);

    _navigator->set_position_setpoint_triplet_updated();
    printf("New Yaw: %9.6f",pos_sp_triplet->current.yaw);
    mavlink_log_info(&_mavlink_log_pub, "i changed waypoints");

}

and in the sitl terminal this is what i see:

INFO  [commander] Armed by external command
INFO  [commander] Takeoff detected
Current Yaw:  0.000000New Yaw:  1.570682INFO  [navigator] i changed waypoints
[ WARN] [1638329903.252801509, 47.616000000]: CMD: Unexpected command 176, result 0
INFO  [commander] Landing at current position
gssammy commented 2 years ago

using the stock px4 sitl with gazebo (roslaunch px4 mavros_posix_sitl.launch) and sending the waypoints

rostopic pub -r 10 /mavros/setpoint_position/local geometry_msgs/PoseStamped "header:
  seq: 0
  stamp:
    secs: 0
    nsecs: 0
  frame_id: ''
pose:
  position:
    x: 0.0
    y: 0.0
    z: 3.0
  orientation:
    x: 0.0
    y: 0.0
    z: 1.0
    w: 0.0"

i see the effect but not as bad: https://logs.px4.io/plot_app?log=52625e87-0565-4381-b2ee-bead9eb4c2d5#Nav-Yaw-Angle.

Full terminal output:

... logging to /home/george/.ros/log/011dc2b6-52b9-11ec-80fc-5b7f4fcc37e0/roslaunch-george-pensa-23686.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://george-pensa:36293/

SUMMARY
========

CLEAR PARAMETERS
 * /mavros/

PARAMETERS
 * /gazebo/enable_ros_network: True
 * /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: PITCH_270
 * /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: PITCH_270
 * /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: PITCH_270
 * /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/horizontal_fov_ratio: 1.0
 * /mavros/distance_sensor/sonar_1_sub/id: 2
 * /mavros/distance_sensor/sonar_1_sub/orientation: PITCH_270
 * /mavros/distance_sensor/sonar_1_sub/subscriber: True
 * /mavros/distance_sensor/sonar_1_sub/vertical_fov_ratio: 1.0
 * /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: udp://:14540@loca...
 * /mavros/gcs_url: 
 * /mavros/global_position/child_frame_id: base_link
 * /mavros/global_position/frame_id: map
 * /mavros/global_position/gps_uere: 1.0
 * /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.0003490659 // 0...
 * /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/landing_target/camera/fov_x: 2.0071286398
 * /mavros/landing_target/camera/fov_y: 2.0071286398
 * /mavros/landing_target/image/height: 480
 * /mavros/landing_target/image/width: 640
 * /mavros/landing_target/land_target_type: VISION_FIDUCIAL
 * /mavros/landing_target/listen_lt: False
 * /mavros/landing_target/mav_frame: LOCAL_NED
 * /mavros/landing_target/target_size/x: 0.3
 * /mavros/landing_target/target_size/y: 0.3
 * /mavros/landing_target/tf/child_frame_id: camera_center
 * /mavros/landing_target/tf/frame_id: landing_target
 * /mavros/landing_target/tf/listen: False
 * /mavros/landing_target/tf/rate_limit: 10.0
 * /mavros/landing_target/tf/send: True
 * /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/mission/use_mission_item_int: True
 * /mavros/mocap/use_pose: True
 * /mavros/mocap/use_tf: False
 * /mavros/odometry/fcu/odom_child_id_des: base_link
 * /mavros/odometry/fcu/odom_parent_id_des: map
 * /mavros/plugin_blacklist: ['safety_area', '...
 * /mavros/plugin_whitelist: []
 * /mavros/px4flow/frame_id: px4flow
 * /mavros/px4flow/ranger_fov: 0.118682
 * /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_raw/thrust_scaling: 1.0
 * /mavros/setpoint_velocity/mav_frame: LOCAL_NED
 * /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/publish_sim_time: False
 * /mavros/time/time_ref_source: fcu
 * /mavros/time/timesync_avg_alpha: 0.6
 * /mavros/time/timesync_mode: MAVLINK
 * /mavros/vibration/frame_id: base_link
 * /mavros/vision_pose/tf/child_frame_id: vision_estimate
 * /mavros/vision_pose/tf/frame_id: odom
 * /mavros/vision_pose/tf/listen: False
 * /mavros/vision_pose/tf/rate_limit: 10.0
 * /mavros/vision_speed/listen_twist: True
 * /mavros/vision_speed/twist_cov: True
 * /mavros/wheel_odometry/child_frame_id: base_link
 * /mavros/wheel_odometry/count: 2
 * /mavros/wheel_odometry/frame_id: odom
 * /mavros/wheel_odometry/send_raw: True
 * /mavros/wheel_odometry/send_twist: False
 * /mavros/wheel_odometry/tf/child_frame_id: base_link
 * /mavros/wheel_odometry/tf/frame_id: odom
 * /mavros/wheel_odometry/tf/send: False
 * /mavros/wheel_odometry/use_rpm: False
 * /mavros/wheel_odometry/vel_error: 0.1
 * /mavros/wheel_odometry/wheel0/radius: 0.05
 * /mavros/wheel_odometry/wheel0/x: 0.0
 * /mavros/wheel_odometry/wheel0/y: -0.15
 * /mavros/wheel_odometry/wheel1/radius: 0.05
 * /mavros/wheel_odometry/wheel1/x: 0.0
 * /mavros/wheel_odometry/wheel1/y: 0.15
 * /rosdistro: noetic
 * /rosversion: 1.15.13
 * /use_sim_time: True

NODES
  /
    gazebo (gazebo_ros/gzserver)
    gazebo_gui (gazebo_ros/gzclient)
    mavros (mavros/mavros_node)
    sitl (px4/px4)
    vehicle_spawn_george_pensa_23686_5066516929890246095 (gazebo_ros/spawn_model)

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

setting /run_id to 011dc2b6-52b9-11ec-80fc-5b7f4fcc37e0
process[rosout-1]: started with pid [23708]
started core service [/rosout]
process[sitl-2]: started with pid [23717]
INFO  [px4] Creating symlink /home/george/px4/PX4-Autopilot/build/px4_sitl_default/etc -> /home/george/.ros/etc

______  __   __    ___ 
| ___ \ \ \ / /   /   |
| |_/ /  \ V /   / /| |
|  __/   /   \  / /_| |
| |     / /^\ \ \___  |
\_|     \/   \/     |_/

px4 starting.

INFO  [px4] Calling startup script: /bin/sh etc/init.d-posix/rcS 0
Info: found model autostart file as SYS_AUTOSTART=10016
INFO  [param] selected parameter default file eeprom/parameters_10016
INFO  [parameters] BSON document size 465 bytes, decoded 465 bytes
[param] Loaded: eeprom/parameters_10016
process[gazebo-3]: started with pid [23737]
process[gazebo_gui-4]: started with pid [23749]
process[vehicle_spawn_george_pensa_23686_5066516929890246095-5]: started with pid [23760]
process[mavros-6]: started with pid [23771]
INFO  [dataman] Unknown restart, data manager file './dataman' size is 11798680 bytes
PX4 SIM HOST: localhost
INFO  [simulator] Waiting for simulator to accept connection on TCP port 4560
[ INFO] [1638371509.483340662]: FCU URL: udp://:14540@localhost:14557
[ INFO] [1638371509.485030914]: udp0: Bind address: 0.0.0.0:14540
[ INFO] [1638371509.485117426]: udp0: Remote address: 127.0.0.1:14557
[ INFO] [1638371509.485218885]: GCS bridge disabled
[ INFO] [1638371509.494030244]: Plugin 3dr_radio loaded
[ INFO] [1638371509.495698450]: Plugin 3dr_radio initialized
[ INFO] [1638371509.495783152]: Plugin actuator_control loaded
[ INFO] [1638371509.499294706]: Plugin actuator_control initialized
[ INFO] [1638371509.502395843]: Plugin adsb loaded
[ INFO] [1638371509.505650274]: Plugin adsb initialized
[ INFO] [1638371509.505750054]: Plugin altitude loaded
[ INFO] [1638371509.506682497]: Plugin altitude initialized
[ INFO] [1638371509.506773420]: Plugin cam_imu_sync loaded
[ INFO] [1638371509.507399654]: Plugin cam_imu_sync initialized
[ INFO] [1638371509.507488381]: Plugin command loaded
[ INFO] [1638371509.514023764]: Plugin command initialized
[ INFO] [1638371509.514141430]: Plugin companion_process_status loaded
[ INFO] [1638371509.516303963]: Plugin companion_process_status initialized
[ INFO] [1638371509.516422339]: Plugin debug_value loaded
[ INFO] [1638371509.520590926]: Plugin debug_value initialized
[ INFO] [1638371509.520636436]: Plugin distance_sensor blacklisted
[ INFO] [1638371509.520736822]: Plugin esc_status loaded
[ INFO] [1638371509.521821610]: Plugin esc_status initialized
[ INFO] [1638371509.521951064]: Plugin esc_telemetry loaded
[ INFO] [1638371509.522591922]: Plugin esc_telemetry initialized
[ INFO] [1638371509.522715897]: Plugin fake_gps loaded
[ INFO] [1638371509.536219195]: Plugin fake_gps initialized
[ INFO] [1638371509.536361646]: Plugin ftp loaded
[ INFO] [1638371509.541374584]: Plugin ftp initialized
[ INFO] [1638371509.541466800]: Plugin geofence loaded
[ INFO] [1638371509.543501118]: Plugin geofence initialized
[ INFO] [1638371509.543595402]: Plugin global_position loaded
[ INFO] [1638371509.555759441]: Plugin global_position initialized
[ INFO] [1638371509.555890438]: Plugin gps_input loaded
[ INFO] [1638371509.559691405]: Plugin gps_input initialized
[ INFO] [1638371509.559822375]: Plugin gps_rtk loaded
[ INFO] [1638371509.562181442]: Plugin gps_rtk initialized
[ INFO] [1638371509.562303119]: Plugin gps_status loaded
[ INFO] [1638371509.564346611]: Plugin gps_status initialized
[ INFO] [1638371509.564455885]: Plugin hil loaded
[ INFO] [1638371509.575668991]: Plugin hil initialized
[ INFO] [1638371509.575887246]: Plugin home_position loaded
[ INFO] [1638371509.579111263]: Plugin home_position initialized
[ INFO] [1638371509.579318921]: Plugin imu loaded
[ INFO] [1638371509.586163297]: Plugin imu initialized
[ INFO] [1638371509.586370926]: Plugin landing_target loaded
[ INFO] [1638371509.599676841]: Plugin landing_target initialized
[ INFO] [1638371509.599787487]: Plugin local_position loaded
[ INFO] [1638371509.603951422]: Plugin local_position initialized
[ INFO] [1638371509.604057993]: Plugin log_transfer loaded
[ INFO] [1638371509.606080470]: Plugin log_transfer initialized
[ INFO] [1638371509.606189865]: Plugin mag_calibration_status loaded
[ INFO] [1638371509.606952038]: Plugin mag_calibration_status initialized
[ INFO] [1638371509.607057836]: Plugin manual_control loaded
[ INFO] [1638371509.609770132]: Plugin manual_control initialized
[ INFO] [1638371509.609907613]: Plugin mocap_pose_estimate loaded
[ INFO] [1638371509.613528711]: Plugin mocap_pose_estimate initialized
[ INFO] [1638371509.613667699]: Plugin mount_control loaded
[ INFO] [1638371509.618668721]: Plugin mount_control initialized
[ INFO] [1638371509.618849244]: Plugin nav_controller_output loaded
[ INFO] [1638371509.619754228]: Plugin nav_controller_output initialized
[ INFO] [1638371509.619941826]: Plugin obstacle_distance loaded
[ INFO] [1638371509.622546339]: Plugin obstacle_distance initialized
[ INFO] [1638371509.622689679]: Plugin odom loaded
[ INFO] [1638371509.627001734]: Plugin odom initialized
[ INFO] [1638371509.627142091]: Plugin onboard_computer_status loaded
[ INFO] [1638371509.629118483]: Plugin onboard_computer_status initialized
[ INFO] [1638371509.629302734]: Plugin param loaded
[ INFO] [1638371509.632021183]: Plugin param initialized
[ INFO] [1638371509.632235117]: Plugin play_tune loaded
[ INFO] [1638371509.634550556]: Plugin play_tune initialized
[ INFO] [1638371509.634645398]: Plugin px4flow loaded
[ INFO] [1638371509.640565850]: Plugin px4flow initialized
[ INFO] [1638371509.640688101]: Plugin rallypoint loaded
[ INFO] [1638371509.643505484]: Plugin rallypoint initialized
[ INFO] [1638371509.643545072]: Plugin rangefinder blacklisted
[ INFO] [1638371509.643650199]: Plugin rc_io loaded
[ INFO] [1638371509.646804923]: Plugin rc_io initialized
[ INFO] [1638371509.646873223]: Plugin safety_area blacklisted
[ INFO] [1638371509.646996529]: Plugin setpoint_accel loaded
[ INFO] [1638371509.650552536]: Plugin setpoint_accel initialized
[ INFO] [1638371509.650743128]: Plugin setpoint_attitude loaded
[ INFO] [1638371509.658981726]: Plugin setpoint_attitude initialized
[ INFO] [1638371509.659133518]: Plugin setpoint_position loaded
[ INFO] [1638371509.670733648]: Plugin setpoint_position initialized
[ INFO] [1638371509.670882990]: Plugin setpoint_raw loaded
[ INFO] [1638371509.676877718]: Plugin setpoint_raw initialized
[ INFO] [1638371509.677014252]: Plugin setpoint_trajectory loaded
[ INFO] [1638371509.681227230]: Plugin setpoint_trajectory initialized
[ INFO] [1638371509.681463069]: Plugin setpoint_velocity loaded
[ INFO] [1638371509.686484955]: Plugin setpoint_velocity initialized
[ INFO] [1638371509.686656110]: Plugin sys_status loaded
[ INFO] [1638371509.697032511]: Plugin sys_status initialized
[ INFO] [1638371509.697172152]: Plugin sys_time loaded
[ INFO] [1638371509.702657866]: TM: Timesync mode: MAVLINK
[ INFO] [1638371509.703062959]: TM: Not publishing sim time
[ INFO] [1638371509.704105894]: Plugin sys_time initialized
[ INFO] [1638371509.704248895]: Plugin trajectory loaded
[ INFO] [1638371509.708001889]: Plugin trajectory initialized
[ INFO] [1638371509.708208054]: Plugin tunnel loaded
[ INFO] [1638371509.710700782]: Plugin tunnel initialized
[ INFO] [1638371509.710970380]: Plugin vfr_hud loaded
[ INFO] [1638371509.711663195]: Plugin vfr_hud initialized
[ INFO] [1638371509.711701407]: Plugin vibration blacklisted
[ INFO] [1638371509.711805028]: Plugin vision_pose_estimate loaded
[ INFO] [1638371509.719508647]: Plugin vision_pose_estimate initialized
[ INFO] [1638371509.719624806]: Plugin vision_speed_estimate loaded
[ INFO] [1638371509.722323703]: Plugin vision_speed_estimate initialized
[ INFO] [1638371509.722454989]: Plugin waypoint loaded
[ INFO] [1638371509.725470687]: Plugin waypoint initialized
[ INFO] [1638371509.725511315]: Plugin wheel_odometry blacklisted
[ INFO] [1638371509.725585109]: Plugin wind_estimation loaded
[ INFO] [1638371509.726154751]: Plugin wind_estimation initialized
[ INFO] [1638371509.726188041]: Autostarting mavlink via USB on PX4
[ INFO] [1638371509.726226002]: Built-in SIMD instructions: SSE, SSE2
[ INFO] [1638371509.726249816]: Built-in MAVLink package version: 2021.11.11
[ INFO] [1638371509.726284511]: Known MAVLink dialects: common ardupilotmega ASLUAV all development icarous matrixpilot paparazzi standard uAvionix ualberta
[ INFO] [1638371509.726313605]: MAVROS started. MY ID 1.240, TARGET ID 1.1
[INFO] [1638371509.815980, 0.000000]: Loading model XML from file /home/george/px4/PX4-Autopilot/Tools/sitl_gazebo/models/iris/iris.sdf
[INFO] [1638371509.820797, 0.000000]: Waiting for service /gazebo/spawn_sdf_model
[ INFO] [1638371510.038824646]: Finished loading Gazebo ROS API Plugin.
[ INFO] [1638371510.039868177]: waitForService: Service [/gazebo/set_physics_properties] has not been advertised, waiting...
[ INFO] [1638371510.166923282]: Finished loading Gazebo ROS API Plugin.
[ INFO] [1638371510.168110232]: waitForService: Service [/gazebo_gui/set_physics_properties] has not been advertised, waiting...
[ INFO] [1638371510.492138001]: waitForService: Service [/gazebo/set_physics_properties] is now available.
[ INFO] [1638371510.511310806, 0.012000000]: Physics dynamic reconfigure ready.
[INFO] [1638371510.725429, 0.224000]: Calling service /gazebo/spawn_sdf_model
[INFO] [1638371510.974575, 0.372000]: Spawn status: SpawnModel: Successfully spawned entity
INFO  [simulator] Simulator connected on TCP port 4560.
INFO  [commander] LED: open /dev/led0 failed (22)
INFO  [init] Mixer: etc/mixers/quad_w.main.mix on /dev/pwm_output0
INFO  [ekf2] starting instance 0, IMU:0 (1310988), MAG:0 (197388)
INFO  [ekf2] starting instance 1, IMU:1 (1310996), MAG:0 (197388)
INFO  [mavlink] mode: Normal, data rate: 4000000 B/s on udp port 18570 remote port 14550
INFO  [ekf2] starting instance 2, IMU:2 (1311004), MAG:0 (197388)
INFO  [ekf2] starting instance 3, IMU:0 (1310988), MAG:1 (197644)
INFO  [ekf2] starting instance 4, IMU:1 (1310996), MAG:1 (197644)
INFO  [ekf2] starting instance 5, IMU:2 (1311004), MAG:1 (197644)
INFO  [mavlink] mode: Onboard, data rate: 4000000 B/s on udp port 14580 remote port 14540
INFO  [mavlink] mode: Onboard, data rate: 4000 B/s on udp port 14280 remote port 14030
INFO  [mavlink] mode: Gimbal, data rate: 400000 B/s on udp port 13030 remote port 13280
INFO  [logger] logger started (mode=all)
INFO  [logger] Start file log (type: full)
INFO  [logger] [logger] ./log/2021-12-01/15_11_51.ulg
INFO  [logger] Opened full log file: ./log/2021-12-01/15_11_51.ulg
INFO  [mavlink] MAVLink only on localhost (set param MAV_{i}_BROADCAST = 1 to enable network)
INFO  [mavlink] MAVLink only on localhost (set param MAV_{i}_BROADCAST = 1 to enable network)
INFO  [px4] Startup script returned successfully
pxh> [ INFO] [1638371511.363159017, 0.572000000]: udp0: Remote address: 127.0.0.1:14580
[ INFO] [1638371511.363689965, 0.572000000]: IMU: High resolution IMU detected!
INFO  [mavlink] partner IP: 127.0.0.1
[vehicle_spawn_george_pensa_23686_5066516929890246095-5] process has finished cleanly
log file: /home/george/.ros/log/011dc2b6-52b9-11ec-80fc-5b7f4fcc37e0/vehicle_spawn_george_pensa_23686_5066516929890246095-5*.log
INFO  [ecl/EKF] reset position to last known position
INFO  [ecl/EKF] reset velocity to zero
INFO  [ecl/EKF] INFO  reset position to last known position
[ecl/EKF] reset position to last known position
INFO  [ecl/EKF] reset velocity to zero
INFO  [ecl/EKF] reset position to last known position
INFO  [ecl/EKF] reset velocity to zero
INFO  [ecl/EKF] reset velocity to zero
INFO  [ecl/EKF] reset position to last known position
INFO  [ecl/EKF] reset velocity to zero
INFO  [ecl/EKF] reset position to last known position
INFO  [ecl/EKF] reset velocity to zero
INFO  [mavlink] partner IP: 127.0.0.1
[ INFO] [1638371512.339282923, 1.544000000]: CON: Got HEARTBEAT, connected. FCU: PX4 Autopilot
[ INFO] [1638371512.343223529, 1.548000000]: IMU: High resolution IMU detected!
INFO  [ecl/EKF] GPS checks passed
INFO  [ecl/EKF] GPS checks passed
INFO  [ecl/EKF] GPS checks passed
INFO  [ecl/EKF] GPS checks passed
INFO  [ecl/EKF] GPS checks passed
INFO  [ecl/EKF] GPS checks passed
[ INFO] [1638371513.347530686, 2.548000000]: GF: Using MISSION_ITEM_INT
[ INFO] [1638371513.347710569, 2.548000000]: RP: Using MISSION_ITEM_INT
[ INFO] [1638371513.347835973, 2.552000000]: WP: Using MISSION_ITEM_INT
[ INFO] [1638371513.347990325, 2.552000000]: VER: 1.1: Capabilities         0x000000000000e4ef
[ INFO] [1638371513.348106757, 2.552000000]: VER: 1.1: Flight software:     010c0300 (2e8918da66000000)
[ INFO] [1638371513.348197118, 2.552000000]: VER: 1.1: Middleware software: 010c0300 (2e8918da66000000)
[ INFO] [1638371513.348289590, 2.552000000]: VER: 1.1: OS software:         050b00ff (bf660cba2af81f05)
[ INFO] [1638371513.348386023, 2.552000000]: VER: 1.1: Board hardware:      00000001
[ INFO] [1638371513.348510686, 2.552000000]: VER: 1.1: VID/PID:             0000:0000
[ INFO] [1638371513.348599542, 2.552000000]: VER: 1.1: UID:                 4954414c44494e4f
INFO  [ecl/EKF] 4444000: EKF aligned, (baro hgt, IMU buf: 12, OBS buf: 9)
INFO  [ecl/EKF] 4440000: EKF aligned, (baro hgt, IMU buf: 12, OBS buf: 9)
INFO  [ecl/EKF] 4448000: EKF aligned, (baro hgt, IMU buf: 12, OBS buf: 9)
INFO  [ecl/EKF] 4444000: EKF aligned, (baro hgt, IMU buf: 12, OBS buf: 9)
INFO  [ecl/EKF] 4448000: EKF aligned, (baro hgt, IMU buf: 12, OBS buf: 9)
INFO  [ecl/EKF] 4580000: EKF aligned, (baro hgt, IMU buf: 12, OBS buf: 9)
[ INFO] [1638371515.890743825, 5.080000000]: IMU: Attitude quaternion IMU detected!
INFO  [ecl/EKF] reset position to GPS
INFO  [ecl/EKF] reset velocity to GPS
INFO  [ecl/EKF] starting GPS fusion
INFO  [ecl/EKF] INFO  [ecl/EKF] reset position to GPS
INFO  [ecl/EKF] reset velocity to GPS
INFO  [ecl/EKF] starting GPS fusion
reset position to GPS
INFO  [ecl/EKF] reset velocity to GPS
INFO  [ecl/EKF] starting GPS fusion
INFO  [ecl/EKF] reset position to GPS
INFO  [ecl/EKF] reset velocity to GPS
INFO  [ecl/EKF] starting GPS fusion
INFO  [ecl/EKF] reset position to GPS
INFO  [ecl/EKF] reset velocity to GPS
INFO  [ecl/EKF] starting GPS fusion
INFO  [ecl/EKF] reset position to GPS
INFO  [ecl/EKF] reset velocity to GPS
INFO  [ecl/EKF] starting GPS fusion
INFO  [tone_alarm] home set
INFO  [tone_alarm] notify negative
[ INFO] [1638371527.380049541, 16.544000000]: RP: mission received
[ INFO] [1638371527.380477882, 16.544000000]: WP: mission received
[ INFO] [1638371527.380737406, 16.544000000]: GF: mission received
pxh> commander mode offboard
pxh> [ WARN] [1638371533.052255230, 22.208000000]: CMD: Unexpected command 176, result 0
pxh> commander arm
pxh> INFO  [commander] Armed by internal command
INFO  [commander] Takeoff detected
pxh> commander mode auto:land
pxh> Current Yaw:  0.000000New Yaw: -1.570360INFO  [navigator] i changed waypoints
[ WARN] [1638371554.463229513, 43.484000000]: CMD: Unexpected command 176, result 0
INFO  [commander] Landing detected
INFO  [commander] Disarmed by landing
INFO  [logger] closed logfile, bytes written: 3438881
pxh> shutdown
AlexisTM commented 1 month ago

You are sending a setpoint with an orientation. Furthermore, this is not the unary quaternion (which should be w = 1, not z = 1)