FCU estimation

[SyluxDX]

I have a Pixhawk connected to a companion computer with ros and when I launch mavros with the quad on the ground the estimation of the FCU fluctuates. Is there anyway to correct the estimator?

[jgoppert]

@SyluxDX fluctuates how? It is normal for the altitude to fluctuate +/- 1 meter if you are just using the baro. If you have a px4flow/ sonar/ lidar plugged in, you can expect better results, +/- 5 cm.

[SyluxDX]

I have the PX4Flow connected to the Pixhawk on the I2C port and the fluctuation is +/- 1meter. And is it normal for the FCU estimate the quad being 6 meters of the ground?

[vooon]

More information, what exactly changes, where you see that, what firmware used, mavros version?

[SyluxDX]

@vooon The firmware of the pixhawk and px4flow are the lasted stable version, (updated two days ago) and mavros is also on the latest version, 0.16.6

[vooon]

Check that px4flow is actually used (check params).

Latest mavros 0.17.1, but stable repos perhaps 0.16.6. But never mind main difference is changes in frame rotations (and that affect mocap and vision estimators).

What diagnostics show? But I'm not sure that flow is shown in system status...

[jgoppert]

@SyluxDX I have been working on flow estimation on this branch (PX4/PX4-Autopilot#4036), it will improve the performance significantly, note that in this branch, the global altitude will still drift as you are observing, but the distance above ground is regulated separately and is known very accurately, you can plot it in dist_bottom:

Here is a video of the flight performance in position hold: https://www.youtube.com/watch?v=CccoyyX-xtE

Install

git clone -b lpe_default https://github.com/jgoppert/Firmware/

It should be merged with master soon, but the process is a bit tedious right now.

Steps to install after cloning:

make px4fmu-v2_lpe upload

In QGC, set

SYS_MC_EST_GROUP = 1
MPC_ALT_MODE = 1

reboot

You should now see the LPE estimator parameters.
If you are flying indoors you should make sure that you don't get a weak GPS signal and start using it:

LPE_GPS_ON = 0

[SyluxDX]

On qgroundcontrol and on mavros I have access to the data from the px4flow, so i assumed that he is used.
As for what the diagnostics shows:

  seq: 92
  stamp: 
    secs: 1460128610
    nsecs: 516284038
  frame_id: ''
status: 
  - 
    level: 0
    name: mavros: FCU connection
    message: connected
    hardware_id: /dev/ttyPixhawk:57600
    values: 
      - 
        key: Received packets:
        value: 32845
      - 
        key: Dropped packets:
        value: 0
      - 
        key: Buffer overruns:
        value: 0
      - 
        key: Parse errors:
        value: 0
      - 
        key: Rx sequence number:
        value: 114
      - 
        key: Tx sequence number:
        value: 161
      - 
        key: Rx total bytes:
        value: 1389837
      - 
        key: Tx total bytes:
        value: 151267
      - 
        key: Rx speed:
        value: 17975.000000
      - 
        key: Tx speed:
        value: 2006.000000
  - 
    level: 0
    name: mavros: Heartbeat
    message: Normal
    hardware_id: /dev/ttyPixhawk:57600
    values: 
      - 
        key: Heartbeats since startup
        value: 114
      - 
        key: Frequency (Hz)
        value: 1.000018
      - 
        key: Vehicle type
        value: Quadrotor
      - 
        key: Autopilot type
        value: PX4
      - 
        key: Mode
        value: MANUAL
      - 
        key: System status
        value: Standby
  - 
    level: 0
    name: mavros: System
    message: Normal
    hardware_id: /dev/ttyPixhawk:57600
    values: 
      - 
        key: Sensor present
        value: 0x00000040
      - 
        key: Sensor enabled
        value: 0x00000040
      - 
        key: Sensor helth
        value: 0x00000040
      - 
        key: Sensor Optical Flow
        value: Ok
      - 
        key: CPU Load (%)
        value: 27.5
      - 
        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: 1
    name: mavros: Battery
    message: Low voltage
    hardware_id: /dev/ttyPixhawk:57600
    values: 
      - 
        key: Voltage
        value: 0.00
      - 
        key: Current
        value: 0.0
      - 
        key: Remaining
        value: -1.0
  - 
    level: 0
    name: mavros: Time Sync
    message: Normal
    hardware_id: /dev/ttyPixhawk:57600
    values: 
      - 
        key: Timesyncs since startup
        value: 10
      - 
        key: Frequency (Hz)
        value: 0.076924
      - 
        key: Last dt (ms)
        value: 0.597284
      - 
        key: Mean dt (ms)
        value: 0.562132
      - 
        key: Last system time (s)
        value: 166.186766000
      - 
        key: Time offset (s)
        value: 1460128440.286227465

[SyluxDX]

I'm trying to control the quad with a Nvidia Jetson and sending to the Pixhawk setpoint, to be at 0.5 meters of the ground, to the topic /mavros/setpoint_position/local and the quad won't takeoff. I assume that the problem is with the FCU estimation since it already puts the quad off the ground by 6 meters.

[vooon]

Did you set OFFBOARD mode? Stream setpoint >2 Hz?

[vooon]

Jetson's UART can operate on 921600?

[SyluxDX]

Yes i set it to OFFBOARD and the pixhawk is connected through USB

[vooon]

USB or USB-UART? Plain USB is not recommended for companion link.

Try 6.5 meters.

[SyluxDX]

I tried to put the altitude in the setpoint to 0.5 meters plus the FCU initial offset, but still the quad doesn't takeoff.

[vooon]

What printed in mavros log (console where roslaunch started)? Use triple ` for code block.

[SyluxDX]

SUMMARY
========

CLEAR PARAMETERS
 * /mavros/

PARAMETERS
 * /mavros/cmd/use_comp_id_system_control: False
 * /mavros/conn/heartbeat_rate: 5.0
 * /mavros/conn/system_time_rate: 1.0
 * /mavros/conn/timeout: 10.0
 * /mavros/conn/timesync_rate: 0.1
 * /mavros/fcu_url: /dev/ttyPixhawk:5...
 * /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/gps/frame_id: gps
 * /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: map
 * /mavros/local_position/tf/send: False
 * /mavros/local_position/tf/send_fcu: True
 * /mavros/mission/pull_after_gcs: True
 * /mavros/mocap/use_pose: True
 * /mavros/mocap/use_tf: False
 * /mavros/plugin_blacklist: ['image_pub', 'gl...
 * /mavros/plugin_whitelist: []
 * /mavros/px4flow/frame_id: sonar_link
 * /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/vision_pose/child_frame_id: vision
 * /mavros/vision_pose/frame_id: local_origin
 * /mavros/vision_pose/listen_tf: False
 * /mavros/vision_pose/pose_with_covariance: False
 * /mavros/vision_pose/tf_rate_limit: 10.0
 * /mavros/vision_speed/listen_twist: False
 * /rosdistro: indigo
 * /rosversion: 1.11.16

[ INFO] [1460137217.141509540]: FCU URL: /dev/ttyPixhawk:57600
[ INFO] [1460137217.142665194]: serial0: device: /dev/ttyPixhawk @ 57600 bps
[ INFO] [1460137222.144331322]: GCS bridge disabled
[ INFO] [1460137222.355170690]: Plugin 3dr_radio loaded and initialized
[ INFO] [1460137222.363733434]: Plugin actuator_control loaded and initialized
[ INFO] [1460137222.366784902]: Plugin altitude loaded and initialized
[ INFO] [1460137222.715733153]: Plugin cam_imu_sync loaded and initialized
[ INFO] [1460137222.731755234]: Plugin command loaded and initialized
[ INFO] [1460137222.731989065]: Plugin distance_sensor blacklisted
[ INFO] [1460137222.753850586]: Plugin ftp loaded and initialized
[ INFO] [1460137222.754075333]: Plugin global_position blacklisted
[ INFO] [1460137222.754146166]: Plugin image_pub blacklisted
[ INFO] [1460137222.773953374]: Plugin imu_pub loaded and initialized
[ INFO] [1460137222.778331828]: IMU: High resolution IMU detected!
[ INFO] [1460137222.792209349]: Plugin local_position loaded and initialized
[ INFO] [1460137222.795775062]: Plugin manual_control loaded and initialized
[ INFO] [1460137222.808813675]: Plugin mocap_pose_estimate loaded and initialized
[ INFO] [1460137222.817774497]: Plugin param loaded and initialized
[ INFO] [1460137222.833563748]: Plugin px4flow loaded and initialized
[ INFO] [1460137222.861483122]: Plugin rc_io loaded and initialized
[ INFO] [1460137222.868874294]: SA: Set safty area: P1(1.000000 1.000000 1.000000) P2(-1.000000 -1.000000 -1.000000)
[ INFO] [1460137222.877443621]: Plugin safety_area loaded and initialized
[ INFO] [1460137222.895009520]: Plugin setpoint_accel loaded and initialized
[ INFO] [1460137222.930362315]: Plugin setpoint_attitude loaded and initialized
[ INFO] [1460137222.948216961]: Plugin setpoint_position loaded and initialized
[ INFO] [1460137222.976221333]: Plugin setpoint_raw loaded and initialized
[ INFO] [1460137222.984955491]: Plugin setpoint_velocity loaded and initialized
[ INFO] [1460137223.003983541]: Plugin sys_status loaded and initialized
[ INFO] [1460137223.013076946]: Plugin sys_time loaded and initialized
[ INFO] [1460137223.016923739]: Plugin vfr_hud loaded and initialized
[ INFO] [1460137223.020627450]: Plugin vibration loaded and initialized
[ INFO] [1460137223.041289983]: Plugin vision_pose_estimate loaded and initialized
[ INFO] [1460137223.051216795]: Plugin vision_speed_estimate loaded and initialized
[ INFO] [1460137223.063521833]: Plugin waypoint loaded and initialized
[ INFO] [1460137223.063774913]: Autostarting mavlink via USB on PX4
[ INFO] [1460137223.063852163]: Built-in SIMD instructions: None
[ INFO] [1460137223.063895995]: Built-in MAVLink package version: 2016.3.3
[ INFO] [1460137223.063935912]: Built-in MAVLink dialect: ardupilotmega
[ INFO] [1460137223.063977828]: MAVROS started. MY ID 1.240, TARGET ID 1.1
[ INFO] [1460137223.466635932]: CON: Got HEARTBEAT, connected. FCU: PX4
[ INFO] [1460137223.468217165]: IMU: High resolution IMU detected!
[ INFO] [1460137224.471986371]: VER: 1.1: Capabilities 0x00000000000004eb
[ INFO] [1460137224.472115536]: VER: 1.1: Flight software:     00000000 (5efe0afb013e9e28)
[ INFO] [1460137224.472167869]: VER: 1.1: Middleware software: 00000000 (5efe0afb013e9e28)
[ INFO] [1460137224.472217869]: VER: 1.1: OS software:         00000000 (0000000000000000)
[ INFO] [1460137224.472259368]: VER: 1.1: Board hardware:      00000000
[ INFO] [1460137224.472315201]: VER: 1.1: VID/PID: 26ac:0011
[ INFO] [1460137224.472365367]: VER: 1.1: UID: 3034510636353832

[vooon]

No PR:, no WP: and any FCU:. Little strange, but ids is correct.

[vooon]

Try to blacklist safety_area plugin.

[SyluxDX]

I blacklistted the safety_area but the quad still don't take off.