Issue details
I’m developing Human Following Algorithm using Gazebo and RViz, and ArduPilot SITL.
When the algorithm is verified, I will import this system to real uav.
I’m using map - base_link transformation by MAVROS - TF Publisher, by setting /mavros/global_position/tf/send param as “true”.
The tf_tree construction result is here.
The two frames are connected successfully! But there are two main problems.
1. The map Frame and base_link frame orientation don’t match.
The “map” frame is aligned with gazebo world frame.
But there is rotation conversion (yaw 90 deg) even though i’ve never set the rotation conversion…
Lenovo-ThinkBook-16p-Gen-2:~$ rosrun tf tf_echo map base_link
Failure at 3686.455000000
Exception thrown:Could not find a connection between 'map' and 'base_link' because they are not part of the same tree.Tf has two or more unconnected trees.
The current list of frames is:
Frame map_ned exists with parent map.
Frame odom_ned exists with parent odom.
Frame base_link_frd exists with parent base_link.
Failure at 3686.455000000
Exception thrown:Could not find a connection between 'map' and 'base_link' because they are not part of the same tree.Tf has two or more unconnected trees.
The current list of frames is:
Frame map_ned exists with parent map.
Frame odom_ned exists with parent odom.
Frame base_link_frd exists with parent base_link.
At time 3687.393
- Translation: [0.891, -8.522, 4.924]
- Rotation: in Quaternion [0.001, 0.000, 0.710, 0.705]
in RPY (radian) [0.002, -0.002, 1.578]
in RPY (degree) [0.113, -0.092, 90.405]
At time 3688.394
- Translation: [0.891, -8.522, 4.925]
- Rotation: in Quaternion [0.001, 0.000, 0.710, 0.705]
in RPY (radian) [0.002, -0.001, 1.578]
in RPY (degree) [0.116, -0.072, 90.397]
At time 3689.394
- Translation: [0.891, -8.522, 4.926]
- Rotation: in Quaternion [0.001, 0.000, 0.710, 0.705]
in RPY (radian) [0.002, -0.001, 1.578]
in RPY (degree) [0.115, -0.077, 90.395]
At time 3690.395
- Translation: [0.891, -8.522, 4.927]
- Rotation: in Quaternion [0.001, 0.000, 0.710, 0.705]
in RPY (radian) [0.002, -0.002, 1.578]
in RPY (degree) [0.107, -0.096, 90.408]
At time 3691.395
- Translation: [0.891, -8.522, 4.923]
- Rotation: in Quaternion [0.001, 0.000, 0.710, 0.705]
in RPY (radian) [0.002, -0.002, 1.578]
And when i type rostopic echo /mavros/global_position/global, the topic’s frame_id is “base_link”.
When i echo same topic last month. I remember that frame_id was “map” and it makes sense.
Lenovo-ThinkBook-16p-Gen-2:~$ rostopic echo /mavros/global_position/global
header:
seq: 1922
stamp:
secs: 2582
nsecs: 900690912
frame_id: "base_link"
status:
status: 0
service: 1
latitude: -35.3633389
longitude: 149.1652472
altitude: 608.3585570244587
position_covariance: [0.09, 0.0, 0.0, 0.0, 0.09, 0.0, 0.0, 0.0, 0.09]
position_covariance_type: 2
---
header:
seq: 1923
stamp:
secs: 2583
nsecs: 148829285
frame_id: "base_link"
status:
status: 0
service: 1
latitude: -35.3633389
longitude: 149.1652472
altitude: 608.3585570244587
position_covariance: [0.09, 0.0, 0.0, 0.0, 0.09, 0.0, 0.0, 0.0, 0.09]
position_covariance_type: 2
---
2. The Sensor Update Frequency is too slow
As you can see in the picture above, mavros broadcasting rate between map and base_link frame is only 5Hz. It also can be shown by command below.
Lenovo-ThinkBook-16p-Gen-2:~$ rostopic hz /mavros/global_position/global
subscribed to [/mavros/global_position/global]
WARNING: may be using simulated time
average rate: 4.011
min: 0.248s max: 0.250s std dev: 0.00094s window: 4
average rate: 4.009
min: 0.248s max: 0.251s std dev: 0.00105s window: 8
average rate: 4.003
min: 0.247s max: 0.253s std dev: 0.00164s window: 12
So I put this command to set the sensor update rate from mavros.
rosrun mavros mavsys rate --all 50
But It Increased until 50 Hz at initial time, and gradually decreased until 5Hz again…
How can i fix this two errors?
Thanks for your reading.
MAVROS version and platform
Mavros: ?0.18.4?
ROS: Noetic
Ubuntu: 20.04
Autopilot type and version
[ x ] ArduPilot
[ ] PX4
Version: ArduCopter v4.3.0
Node logs
SUMMARY
========
CLEAR PARAMETERS
* /mavros/
PARAMETERS
* /D_angular: 0.01
* /D_linear: 0.01
* /I_angular: 0.3
* /I_linear: 0.2
* /P_angular: 0.1
* /P_linear: 0.1
* /global_position/tf/send: True
* /mavros/cmd/use_comp_id_system_control: False
* /mavros/conn/heartbeat_mav_type: ONBOARD_CONTROLLER
* /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/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_id: 4
* /mavros/fake_gps/gps_rate: 5.0
* /mavros/fake_gps/horiz_accuracy: 0.5
* /mavros/fake_gps/mocap_transform: False
* /mavros/fake_gps/mocap_withcovariance: False
* /mavros/fake_gps/satellites_visible: 6
* /mavros/fake_gps/speed_accuracy: 0.0
* /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_hil_gps: True
* /mavros/fake_gps/use_mocap: True
* /mavros/fake_gps/use_vision: False
* /mavros/fake_gps/vert_accuracy: 0.5
* /mavros/fcu_protocol: v2.0
* /mavros/fcu_url: udp://:14550@
* /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: True
* /mavros/global_position/use_relative_alt: True
* /mavros/guided_target/tf/child_frame_id: target_position
* /mavros/guided_target/tf/frame_id: map
* /mavros/guided_target/tf/listen: False
* /mavros/guided_target/tf/rate_limit: 50.0
* /mavros/image/frame_id: camera_link
* /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/mount/debounce_s: 4.0
* /mavros/mount/err_threshold_deg: 10.0
* /mavros/mount/negate_measured_pitch: False
* /mavros/mount/negate_measured_roll: False
* /mavros/mount/negate_measured_yaw: 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.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: BODY_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: MAVLINK
* /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: 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: map
* /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: map
* /mavros/wheel_odometry/tf/send: True
* /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.14
NODES
/
mavros (mavros/mavros_node)
ROS_MASTER_URI=http://localhost:11311
process[mavros-1]: started with pid [11653]
[ INFO] [1682406603.769868264]: FCU URL: udp://:14550@
[ INFO] [1682406603.772048799]: udp0: Bind address: 0.0.0.0:14550
[ INFO] [1682406603.772101458]: GCS bridge disabled
[ INFO] [1682406603.785326863]: Plugin 3dr_radio loaded
[ INFO] [1682406603.786309368]: Plugin 3dr_radio initialized
[ INFO] [1682406603.786326828]: Plugin actuator_control blacklisted
[ INFO] [1682406603.792892248]: Plugin adsb loaded
[ INFO] [1682406603.794491578]: Plugin adsb initialized
[ INFO] [1682406603.794515393]: Plugin altitude blacklisted
[ INFO] [1682406603.794588166]: Plugin cam_imu_sync loaded
[ INFO] [1682406603.795004690]: Plugin cam_imu_sync initialized
[ INFO] [1682406603.795072015]: Plugin camera loaded
[ INFO] [1682406603.795524716]: Plugin camera initialized
[ INFO] [1682406603.795577585]: Plugin cellular_status loaded
[ INFO] [1682406603.796608349]: Plugin cellular_status initialized
[ INFO] [1682406603.796669389]: Plugin command loaded
[ INFO] [1682406603.799254296]: Plugin command initialized
[ INFO] [1682406603.799320853]: Plugin companion_process_status loaded
[ INFO] [1682406603.800248743]: Plugin companion_process_status initialized
[ INFO] [1682406603.800265435]: Plugin debug_value blacklisted
[ INFO] [1682406603.800317955]: Plugin distance_sensor loaded
[ INFO] [1682406603.805103720]: Plugin distance_sensor initialized
[ INFO] [1682406603.805190531]: Plugin esc_status loaded
[ INFO] [1682406603.805967079]: Plugin esc_status initialized
[ INFO] [1682406603.806048232]: Plugin esc_telemetry loaded
[ INFO] [1682406603.806426065]: Plugin esc_telemetry initialized
[ INFO] [1682406603.806517346]: Plugin fake_gps loaded
[ INFO] [1682406603.818481794]: Plugin fake_gps initialized
[ INFO] [1682406603.818515178]: Plugin ftp blacklisted
[ INFO] [1682406603.818609252]: Plugin geofence loaded
[ INFO] [1682406603.819883617]: Plugin geofence initialized
[ INFO] [1682406603.819963583]: Plugin global_position loaded
[ INFO] [1682406603.828064430]: Plugin global_position initialized
[ INFO] [1682406603.828138879]: Plugin gps_input loaded
[ INFO] [1682406603.829631563]: Plugin gps_input initialized
[ INFO] [1682406603.829739814]: Plugin gps_rtk loaded
[ INFO] [1682406603.831128716]: Plugin gps_rtk initialized
[ INFO] [1682406603.831228936]: Plugin gps_status loaded
[ INFO] [1682406603.832105145]: Plugin gps_status initialized
[ INFO] [1682406603.832206134]: Plugin guided_target loaded
[ INFO] [1682406603.836003458]: Plugin guided_target initialized
[ INFO] [1682406603.836026505]: Plugin hil blacklisted
[ INFO] [1682406603.836115201]: Plugin home_position loaded
[ INFO] [1682406603.838211510]: Plugin home_position initialized
[ INFO] [1682406603.838305514]: Plugin imu loaded
[ INFO] [1682406603.842909138]: Plugin imu initialized
[ INFO] [1682406603.842976393]: Plugin landing_target loaded
[ INFO] [1682406603.849158603]: Plugin landing_target initialized
[ INFO] [1682406603.849265947]: Plugin local_position loaded
[ INFO] [1682406603.851992978]: Plugin local_position initialized
[ INFO] [1682406603.852074551]: Plugin log_transfer loaded
[ INFO] [1682406603.854018748]: Plugin log_transfer initialized
[ INFO] [1682406603.854086912]: Plugin mag_calibration_status loaded
[ INFO] [1682406603.854663159]: Plugin mag_calibration_status initialized
[ INFO] [1682406603.854731742]: Plugin manual_control loaded
[ INFO] [1682406603.856785588]: Plugin manual_control initialized
[ INFO] [1682406603.856884620]: Plugin mocap_pose_estimate loaded
[ INFO] [1682406603.860475708]: Plugin mocap_pose_estimate initialized
[ INFO] [1682406603.860591153]: Plugin mount_control loaded
[ WARN] [1682406603.863285289]: Could not retrive negate_measured_roll parameter value, using default (0)
[ WARN] [1682406603.863474276]: Could not retrive negate_measured_pitch parameter value, using default (0)
[ WARN] [1682406603.863792465]: Could not retrive negate_measured_yaw parameter value, using default (0)
[ WARN] [1682406603.864537864]: Could not retrive debounce_s parameter value, using default (4.000000)
[ WARN] [1682406603.864711975]: Could not retrive err_threshold_deg parameter value, using default (10.000000)
[ INFO] [1682406603.864738304]: Plugin mount_control initialized
[ INFO] [1682406603.864828398]: Plugin nav_controller_output loaded
[ INFO] [1682406603.865085547]: Plugin nav_controller_output initialized
[ INFO] [1682406603.865126124]: Plugin obstacle_distance loaded
[ INFO] [1682406603.866299431]: Plugin obstacle_distance initialized
[ INFO] [1682406603.866369341]: Plugin odom loaded
[ INFO] [1682406603.868344058]: Plugin odom initialized
[ INFO] [1682406603.868414317]: Plugin onboard_computer_status loaded
[ INFO] [1682406603.869547815]: Plugin onboard_computer_status initialized
[ INFO] [1682406603.869605503]: Plugin param loaded
[ INFO] [1682406603.870859056]: Plugin param initialized
[ INFO] [1682406603.870903264]: Plugin play_tune loaded
[ INFO] [1682406603.871947926]: Plugin play_tune initialized
[ INFO] [1682406603.871961196]: Plugin px4flow blacklisted
[ INFO] [1682406603.872012738]: Plugin rallypoint loaded
[ INFO] [1682406603.873162090]: Plugin rallypoint initialized
[ INFO] [1682406603.873206019]: Plugin rangefinder loaded
[ INFO] [1682406603.873429296]: Plugin rangefinder initialized
[ INFO] [1682406603.873475181]: Plugin rc_io loaded
[ INFO] [1682406603.874964932]: Plugin rc_io initialized
[ INFO] [1682406603.874997826]: Plugin safety_area blacklisted
[ INFO] [1682406603.875098605]: Plugin setpoint_accel loaded
[ INFO] [1682406603.876554763]: Plugin setpoint_accel initialized
[ INFO] [1682406603.876682360]: Plugin setpoint_attitude loaded
[ INFO] [1682406603.880971984]: Plugin setpoint_attitude initialized
[ INFO] [1682406603.881073950]: Plugin setpoint_position loaded
[ INFO] [1682406603.890544423]: Plugin setpoint_position initialized
[ INFO] [1682406603.890717416]: Plugin setpoint_raw loaded
[ INFO] [1682406603.897868093]: Plugin setpoint_raw initialized
[ INFO] [1682406603.898045556]: Plugin setpoint_trajectory loaded
[ INFO] [1682406603.900321353]: Plugin setpoint_trajectory initialized
[ INFO] [1682406603.900415566]: Plugin setpoint_velocity loaded
[ INFO] [1682406603.902478003]: Plugin setpoint_velocity initialized
[ INFO] [1682406603.902721464]: Plugin sys_status loaded
[ INFO] [1682406603.907291913]: Plugin sys_status initialized
[ INFO] [1682406603.907375651]: Plugin sys_time loaded
[ INFO] [1682406603.909645791]: TM: Timesync mode: MAVLINK
[ INFO] [1682406603.909803838]: TM: Not publishing sim time
[ INFO] [1682406603.910287827]: Plugin sys_time initialized
[ INFO] [1682406603.910356480]: Plugin terrain loaded
[ INFO] [1682406603.910580735]: Plugin terrain initialized
[ INFO] [1682406603.910661400]: Plugin trajectory loaded
[ INFO] [1682406603.912887051]: Plugin trajectory initialized
[ INFO] [1682406603.912993068]: Plugin tunnel loaded
[ INFO] [1682406603.914126147]: Plugin tunnel initialized
[ INFO] [1682406603.914188793]: Plugin vfr_hud loaded
[ INFO] [1682406603.914408649]: Plugin vfr_hud initialized
[ INFO] [1682406603.914423734]: Plugin vibration blacklisted
[ INFO] [1682406603.914484495]: Plugin vision_pose_estimate loaded
[ INFO] [1682406603.917276616]: Plugin vision_pose_estimate initialized
[ INFO] [1682406603.917295054]: Plugin vision_speed_estimate blacklisted
[ INFO] [1682406603.917368595]: Plugin waypoint loaded
[ INFO] [1682406603.919335071]: Plugin waypoint initialized
[ INFO] [1682406603.919363287]: Plugin wheel_odometry blacklisted
[ INFO] [1682406603.919438644]: Plugin wind_estimation loaded
[ INFO] [1682406603.919731971]: Plugin wind_estimation initialized
[ INFO] [1682406603.919862222]: Built-in SIMD instructions: SSE, SSE2
[ INFO] [1682406603.919879402]: Built-in MAVLink package version: 2022.8.8
[ INFO] [1682406603.919908525]: Known MAVLink dialects: common ardupilotmega ASLUAV AVSSUAS all development icarous matrixpilot paparazzi standard storm32 uAvionix ualberta
[ INFO] [1682406603.919936741]: MAVROS started. MY ID 1.240, TARGET ID 1.1
[ INFO] [1682406603.919970822]: udp0: Remote address: 127.0.0.1:34701
[ INFO] [1682406603.920058331]: FCU: EKF3 IMU1 tilt alignment complete
[ INFO] [1682406603.920096534]: FCU: EKF3 IMU0 tilt alignment complete
[ INFO] [1682406603.920295717]: RC_CHANNELS message detected!
[ INFO] [1682406603.920344255]: IMU: Raw IMU message used.
[ INFO] [1682406603.949509733]: FCU: EKF3 IMU0 MAG0 initial yaw alignment complete
[ INFO] [1682406603.962937813, 982.976000000]: FCU: EKF3 IMU1 MAG0 initial yaw alignment complete
[ WARN] [1682406604.126050466, 983.144000000]: IMU: linear acceleration on RAW_IMU known on APM only.
[ WARN] [1682406604.126158578, 983.144000000]: IMU: ~imu/data_raw stores unscaled raw acceleration report.
[ WARN] [1682406604.126644174, 983.144000000]: GP: No GPS fix
[ WARN] [1682406604.126806900, 983.144000000]: TM: Wrong FCU time.
[ INFO] [1682406604.243089520, 983.258000000]: CON: Got HEARTBEAT, connected. FCU: ArduPilot
[ WARN] [1682406604.337803125, 983.351000000]: CMD: Unexpected command 410, result 4
[ INFO] [1682406604.377814501, 983.395000000]: RC_CHANNELS message detected!
[ INFO] [1682406605.247942300, 984.265000000]: GF: Using MISSION_ITEM_INT
[ INFO] [1682406605.248040495, 984.265000000]: RP: Using MISSION_ITEM_INT
[ INFO] [1682406605.248064450, 984.265000000]: WP: Using MISSION_ITEM_INT
[ INFO] [1682406605.248093992, 984.265000000]: VER: 1.1: Capabilities 0x000000000000fbef
[ INFO] [1682406605.248110474, 984.265000000]: VER: 1.1: Flight software: 04040000 (da0ee9a8)
[ INFO] [1682406605.248126188, 984.265000000]: VER: 1.1: Middleware software: 00000000 ( )
[ INFO] [1682406605.248140785, 984.265000000]: VER: 1.1: OS software: 00000000 ( )
[ INFO] [1682406605.248155032, 984.265000000]: VER: 1.1: Board hardware: 00000000
[ INFO] [1682406605.248170397, 984.265000000]: VER: 1.1: VID/PID: 0000:0000
[ INFO] [1682406605.248182200, 984.265000000]: VER: 1.1: UID: 0000000000000000
[ INFO] [1682406606.654233249, 985.669000000]: FCU: GPS 1: detected as u-blox at 230400 baud
[ INFO] [1682406614.254016694, 993.260000000]: HP: requesting home position
[ INFO] [1682406614.259770581, 993.262000000]: FCU: ArduCopter V4.4.0-dev (da0ee9a8)
[ INFO] [1682406614.260081368, 993.263000000]: FCU: 7680bafd7537468ab0282e358c563feb
[ INFO] [1682406614.260381958, 993.264000000]: FCU: Frame: QUAD/X
[ WARN] [1682406614.357546571, 993.359000000]: CMD: Unexpected command 410, result 4
[ INFO] [1682406614.877816487, 993.879000000]: PR: parameters list received
[ INFO] [1682406616.881127649, 995.884000000]: FCU: EKF3 IMU1 origin set
[ INFO] [1682406617.081009441, 996.083000000]: FCU: EKF3 IMU0 origin set
[ INFO] [1682406619.266370476, 998.263000000]: GF: mission received
[ INFO] [1682406619.267326792, 998.265000000]: RP: mission received
[ INFO] [1682406619.273627875, 998.268000000]: WP: item #0 F:0 C: 16 p: 0 0 0 0 x: -353632621 y: 1491652374 z: 583.99
[ INFO] [1682406619.275233631, 998.273000000]: WP: item #1 F:3 C: 22 p: 0 0 0 0 x: 0 y: 0 z: 8
[ INFO] [1682406619.281530943, 998.276000000]: WP: item #2 F:3 C: 16 p: 0 0 0 0 x: -353630252 y: 1491652326 z: 8
[ INFO] [1682406619.282628335, 998.280000000]: WP: item #3 F:3 C: 16 p: 0 0 0 0 x: -353630240 y: 1491653545 z: 8
[ INFO] [1682406619.288743085, 998.283000000]: WP: item #4 F:3 C: 16 p: 0 0 0 0 x: -353632607 y: 1491653564 z: 8
[ INFO] [1682406619.291423885, 998.288000000]: WP: item #5 F:3 C: 16 p: 0 0 0 0 x: -353632607 y: 1491654316 z: 8
[ INFO] [1682406619.294481749, 998.293000000]: WP: item #6 F:3 C: 16 p: 0 0 0 0 x: -353630345 y: 1491654302 z: 8
[ INFO] [1682406619.300503752, 998.295000000]: WP: item #7 F:0 C: 20 p: 0 0 0 0 x: 0 y: 0 z: 0
[ INFO] [1682406619.300564303, 998.295000000]: WP: mission received
[ INFO] [1682406639.526878933, 1018.501000000]: FCU: EKF3 IMU1 is using GPS
[ INFO] [1682406639.532720476, 1018.501000000]: FCU: EKF3 IMU0 is using GPS
[ WARN] [1682406912.636994513, 1291.265000000]: CMD: Unexpected command 176, result 0
[ INFO] [1682406915.651752434, 1294.273000000]: FCU: Arming motors
[ INFO] [1682406921.832502732, 1300.455000000]: FCU: EKF3 IMU1 MAG0 in-flight yaw alignment complete
[ INFO] [1682406921.845852101, 1300.463000000]: FCU: EKF3 IMU0 MAG0 in-flight yaw alignment complete
[ WARN] [1682406940.529828208, 1319.126000000]: CMD: Unexpected command 176, result 0
Diagnostics
place here result of:
header:
seq: 1381
stamp:
secs: 2672
nsecs: 475000000
frame_id: ''
status:
-
level: 0
name: "mavros: FCU connection"
message: "connected"
hardware_id: "udp://:14550@"
values:
-
key: "Received packets:"
value: "58693"
-
key: "Dropped packets:"
value: "0"
-
key: "Buffer overruns:"
value: "0"
-
key: "Parse errors:"
value: "0"
-
key: "Rx sequence number:"
value: "68"
-
key: "Tx sequence number:"
value: "10"
-
key: "Rx total bytes:"
value: "8634653"
-
key: "Tx total bytes:"
value: "506050"
-
key: "Rx speed:"
value: "3977.000000"
-
key: "Tx speed:"
value: "297.000000"
-
level: 0
name: "mavros: GPS"
message: "3D fix"
hardware_id: "udp://:14550@"
values:
-
key: "Satellites visible"
value: "10"
-
key: "Fix type"
value: "6"
-
key: "EPH (m)"
value: "1.21"
-
key: "EPV (m)"
value: "2.00"
-
level: 1
name: "mavros: Mount"
message: "Can not diagnose in this targeting mode"
hardware_id: "udp://:14550@"
values:
-
key: "Mode"
value: "255"
-
level: 0
name: "mavros: Heartbeat"
message: "Normal"
hardware_id: "udp://:14550@"
values:
-
key: "Heartbeats since startup"
value: "1690"
-
key: "Frequency (Hz)"
value: "0.956439"
-
key: "Vehicle type"
value: "Quadrotor"
-
key: "Autopilot type"
value: "ArduPilot"
-
key: "Mode"
value: "LOITER"
-
key: "System status"
value: "Active"
-
level: 0
name: "mavros: System"
message: "Normal"
hardware_id: "udp://:14550@"
values:
-
key: "Sensor present"
value: "0x5771FDAF"
-
key: "Sensor enabled"
value: "0x5361FDAF"
-
key: "Sensor health"
value: "0x5771FDAF"
-
key: "3D gyro"
value: "Ok"
-
key: "3D accelerometer"
value: "Ok"
-
key: "3D magnetometer"
value: "Ok"
-
key: "absolute pressure"
value: "Ok"
-
key: "GPS"
value: "Ok"
-
key: "computer vision position"
value: "Ok"
-
key: "laser based position"
value: "Ok"
-
key: "3D angular rate control"
value: "Ok"
-
key: "attitude stabilization"
value: "Ok"
-
key: "yaw position"
value: "Ok"
-
key: "z/altitude control"
value: "Ok"
-
key: "x/y position control"
value: "Ok"
-
key: "motor outputs / control"
value: "Ok"
-
key: "rc receiver"
value: "Ok"
-
key: "AHRS subsystem health"
value: "Ok"
-
key: "Terrain subsystem health"
value: "Ok"
-
key: "Logging"
value: "Ok"
-
key: "Battery"
value: "Ok"
-
key: "pre-arm check status. Always healthy when armed"
value: "Ok"
-
key: "propulsion (actuator, esc, motor or propellor)"
value: "Ok"
-
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://:14550@"
values:
-
key: "Voltage"
value: "12.59"
-
key: "Current"
value: "-0.0"
-
key: "Remaining"
value: "100.0"
-
level: 0
name: "mavros: Time Sync"
message: "Normal"
hardware_id: "udp://:14550@"
values:
-
key: "Timesyncs since startup"
value: "16916"
-
key: "Frequency (Hz)"
value: "9.999131"
-
key: "Last RTT (ms)"
value: "0.000000"
-
key: "Mean RTT (ms)"
value: "2.239654"
-
key: "Last remote time (s)"
value: "2671.171311000"
-
key: "Estimated time offset (s)"
value: "1.237766624"
-
level: 0
name: "mavros: APM Memory"
message: "Normal"
hardware_id: "udp://:14550@"
values:
-
key: "Free memory (B)"
value: "131072"
-
key: "Heap top"
value: "0x0000"
-
level: 2
name: "mavros: APM Hardware"
message: "Not initialised"
hardware_id: "udp://:14550@"
values:
-
key: "Core voltage"
value: "-1.000000"
-
key: "I2C errors"
value: "0"
---
Check ID
OK. I got messages from 1:1.
---
Received 1827 messages, from 1 addresses
sys:comp list of messages
1:1 0, 1, 129, 2, 132, 136, 137, 147, 24, 152, 27, 29, 30, 32, 33, 163, 36, 164, 42, 173, 178, 62, 65, 193, 73, 74, 111, 241, 116, 125