Skip to content

PX4 Info and Documentation#

Full list of Config#

http://docs.px4.io/v1.9.0/en/advanced_config/parameter_reference.html

Flight Modes#

  • Manual/Stabilized - same for multi rotors, transmit pitch and roll commands to output mixer, output mixer also returns drone to an angle of zero when joysticks are centered.
  • Acro - transmit angular rates(how fast to go to an angle)
  • Position - Same as manual/stabilized but tries to correct for wind. May cause crash on larger drones if PID is not tuned properly as will start oscillating. Exiting Mission mode through max throttle will automatically change the mode to position mode

Reference: https://dev.px4.io/v1.9.0/en/concept/flight_modes.html

GPS and EKF#

The Estimation and Control Library (ECL) uses an Extended Kalman Filter (EKF) algorithm to process sensor measurements and provide an estimate of the following states:

  • Quaternion defining the rotation from North, East, Down local earth frame to X,Y,Z body frame
  • Velocity at the IMU - North,East,Down (m/s)
  • Position at the IMU - North,East,Down (m)
  • IMU delta angle bias estimates - X,Y,Z (rad)
  • IMU delta velocity bias estimates - X,Y,Z(m/s)
  • Earth Magnetic field components - North,East,Down (gauss)
  • Vehicle body frame magnetic field bias - X,Y,Z (gauss)
  • Wind velocity - North,East (m/s)

Basically, the filters that process the sensor data. EKF2 processes and corrects for errors generated by aerodynamic disturbances caused by vehicle wind relative velocity and orientation. ECL library can be used with better performance but requires knowledge in tuning kalman filters.

EKF Tuning

GPS EKF Params#

Metric Minimum required Average Value Units Notes
eph 3 (EKF2_REQ_EPH) 0.8 m Standard deviation of horizontal position error
epv 5 (EKF2_REQ_EPV) 1.5 m Standard deviation of vertical position error
Number of satellites 6 (EKF2_REQ_NSATS) 14 -
Speed variance 0.5 0.3 m/s
Fix type 3 4 -
hpos_drift_rate 0.1 (EKF2_REQ_HDRIFT) 0.01 m/s Drift rate calculated from reported GPS position (when stationary).
vpos_drift_rate 0.2 (EKF2_REQ_VDRIFT) 0.02 m/s Drift rate calculated from reported GPS altitude (when stationary).
hspd 0.1 (EKF2_REQ_SACC) 0.01 m/s Filtered magnitude of reported GPS horizontal velocity.

Arming Checks#

EKF Preflight Checks/Errors The following errors (with associated checks and parameters) are reported by the EKF (and propagate to QGroundControl):

PREFLIGHT FAIL: EKF HGT ERROR:

  • This error is produced when the IMU and height measurement data are inconsistent.
  • Perform an accel and gyro calibration and restart the vehicle. If the error persists, check the height sensor data for problems.
  • The check is controlled by the COM_ARM_EKF_HGT parameter.

PREFLIGHT FAIL: EKF VEL ERROR:

  • This error is produced when the IMU and GPS velocity measurement data are inconsistent.
  • Check the GPS velocity data for un-realistic data jumps. If GPS quality looks OK, perform an accel and gyro calibration and restart the vehicle.
  • The check is controlled by the COM_ARM_EKF_VEL parameter.

PREFLIGHT FAIL: EKF HORIZ POS ERROR:

  • This error is produced when the IMU and position measurement data (either GPS or external vision) are inconsistent.
  • Check the position sensor data for un-realistic data jumps. If data quality looks OK, perform an accel and gyro calibration and restart the vehicle.
  • The check is controlled by the COM_ARM_EKF_POS parameter.

PREFLIGHT FAIL: EKF YAW ERROR:

  • This error is produced when the yaw angle estimated using gyro data and the yaw angle from the magnetometer or external vision system are inconsistent.
  • Check the IMU data for large yaw rate offsets and check the magnetometer alignment and calibration.
  • The check is controlled by the COM_ARM_EKF_POS parameter

PREFLIGHT FAIL: EKF HIGH IMU ACCEL BIAS:

  • This error is produced when the IMU accelerometer bias estimated by the EKF is excessive.
  • The check is controlled by the COM_ARM_EKF_AB parameter.

PREFLIGHT FAIL: EKF HIGH IMU GYRO BIAS:

  • This error is produced when the IMU gyro bias estimated by the EKF is excessive.
  • The check is controlled by the COM_ARM_EKF_GB parameter.

PREFLIGHT FAIL: ACCEL SENSORS INCONSISTENT - CHECK CALIBRATION:

  • This error message is produced when the acceleration measurements from different IMU units are inconsistent.
  • This check only applies to boards with more than one IMU.
  • The check is controlled by the COM_ARM_IMU_ACC parameter.

PREFLIGHT FAIL: GYRO SENSORS INCONSISTENT - CHECK CALIBRATION:

  • This error message is produced when the angular rate measurements from different IMU units are inconsistent.
  • This check only applies to boards with more than one IMU.
  • The check is controlled by the COM_ARM_IMU_GYR parameter.

PREFLIGHT FAIL: COMPASS SENSORS INCONSISTENT - CHECK CALIBRATION:

  • This error message is produced when the difference in measurements from different compass sensors is too great.
  • It indicates bad calibration, orientation or magnetic interference.
  • This check only applies to when more than one compass/magnetometer is connected.
  • The check is controlled by the COM_ARM_MAG parameter.

PREFLIGHT FAIL: EKF INTERNAL CHECKS:

  • This error message is generated if the innovation magnitudes of either the horizontal GPS velocity, magnetic yaw, vertical GPS velocity or vertical position sensor (Baro by default but could be range finder or GPS if non-standard parameters are being used) are excessive. Innovations are the difference between the value predicted by the inertial navigation calculation and measured by the sensor.
  • Users should check the innovation levels in the log file to determine the cause. These can be found under the ekf2_innovations message. Common problems/solutions include:
    • IMU drift on warmup. May be resolved by restarting the autopilot. May require an IMU accel and gyro calibration.
    • Adjacent magnetic interference combined with vehicle movement. Resolve my moving vehicle and waiting or re-powering.
    • Bad magnetometer calibration combined with vehicle movement. Resolve by recalibrating.
    • Initial shock or rapid movement on startup that caused a bad inertial nav solution. Resolve by restarting the vehicle and minimizing movement for the first 5 seconds.

Other Parameters#

The following parameters also affect preflight checks.

COM_ARM_WO_GPS
The COM_ARM_WO_GPS parameter controls whether or not arming is allowed without a global position estimate.

  • 1 (default): Arming is allowed without a position estimate for flight modes that do not require position information (only).
  • 0: Arming is allowed only if EKF is providing a global position estimate and EFK GPS quality checks are passing

COM_ARM_EKF_YAW
The COM_ARM_EKF_YAW parameter determines the maximum difference (in radians) between the navigation yaw angle and magnetic yaw angle (magnetometer or external vision) allowed before preflight checks fail. The default value of 0.5 allows the differences to be no more than 50% of the maximum tolerated by the EKF and provides some margin for error increase when flight commences. It can fail if the yaw gyro has a large offset or if the vehicle is moved or rotated in the presence of a bad magnetic interference or magnetometer calibration.

PX4 Sensors#

Certain important advice.

Ensure that the EKF2_AID_MASKS is changed to allow for visual or gps fusion

The 1.10 firmware allow for configuration on sensors on multiple port instead of only running on the default one. For example, the SENS_TFMINI_CFG parameter. However, do note that this selection is based on the SERIAL PORTS ON THE ORIGINAL PIXHAWK 1 (https://www.mathworks.com/help/supportpkg/px4/ref/port-mapping-for-serial.html). So, if you were using the GPS2 port on the pixhawk 2, which is the /dev/ttyS6 port, you would need to choose the relevant S6 port on the pixhawk 1 which is serial 4. (The UART port on the pixhawk 4 is serial 4 too).

Take note that you can still manually start the sensor with but if the correct serial port is chosen it should automatically start everytime.

TF Mini#

By default on 1.8.2, PX4 reads the rangefinder port at SERIAL4/GPS2 which corresponds to the/dev/tty/S6. the port can be rebound with SENS_TFMINI_CFG on later versions.

Ensure that SENS_EN_TFMINI is activated depending on firmware version. RNG AID should be enabled too. Use the mavlink console to probe if the driver is running.

tfmini start -d /dev/tty/S6
tfmini status

Related params

EKF2_HGT_MODE  - changes the primary height data source, still uses other for fusing
EKF2_RNG_AID -can be enabled to reduce the barometer inaccuracies caused by ground effect if the range sensor is not the primary sensor.
MPC_ALT_MODE - follow altitude or terrain

Use the listener distance_sensor command to test if it still works. A distance_sensor tab should pop up too.

Heres a useful link for pixhawk fc serial port mapping

Optical Flow#

Before attempting to implement optical flow for the first time, read through this entire page. Aside from the obvious stuff like ensuring that the orientation and the offsets of the optical flow sensor is correct, ensure that a separate downward facing rangefinder is used(barometer is not allowed) and that integrated_xgyro and integrated_x,.. are the same. For sensors without an integrated imu, integrated_xgyro will show nan and integrated_x will not correspond with HIGHRES_IMU/xgyro.

If connected to PX4 via (USB) MAVLink the Optical Flow device must publish to the OPTICAL_FLOW_RAD topic, and the distance sensor must publish to the DISANCE_SENSOR topic. Only then can fusing occur. To view this topic over telem, the param MAV_1_MODE can be changed from normal to onboard to force all the messages to be sent. This can be checked with the command mavlink status. Do note that this causes other issues such as inability to pick up impulse inputs on the graph during PID Tuning, so any mavlink driver is set to this during tuning.

Aside from that, diagnosing flow issues are abit harder due to the lack of feedback in PX4. Do check your 1)orientation 2)altitude(needs to be high enough) and 3)mpc_xy_vel pids 4) flow sensor offset with EKF2_OF_POS. If tuning is still required, try reducing the mpc_xy_vel_p to 0.08. If that does not help, try changing mpc_xy_vel_d before touching the other mpc params but do note that it should not need to be changed much as the defaults should work okay with the .

Hereflow#

Take note that on pre 2019 pixhawk 2.1 boards the can 1 and can 2 boards are swapped. The hereflow should be plugged in on the can 1 port by default.

The following links contain the required perimeters for it.

Do note that as of 1.10 the downward rangefinder does not seem to be supported. Hence, a seperate downward facing rangefinder might be needed.

The status of the hereflow can be checked with the mavlink commands UAVCAN status and listener optical_flow. Do note that the optical flow mavlink messages are not broadcasted over telem(in PX4) by default to save bandwidth.

Do note that the x and y axis in the hereflow manual seems to be swapped in px4

Personally, I had issues getting the hereflow to work in the horizontal position so I placed it in the vertical position with the circle on the pmw3901 facing the back.

Here+/2 GPS#

Considerations These errors show up as generic errors in qgc and is not diagnosable via logs

  • Ideally the base and rover antenna to have clear view of the sky that is 30 degrees above horizon.
  • To attain GPS lock look for open spaces with view of sky instead of indoor areas
  • Place GPS away from other electronic devices which generate noise. Eg, FC, other antennas, wifi antennas, power transformers and anything that emits EM/radio waves. https://ardupilot.org/copter/docs/common-magnetic-interference.html#common-magnetic-interference
  • Likewise with the Pixhawk FCU, dampening might be required

Time-To-First-Fix

123 GPS & GLONASS GPS & BeiDou GPS
Cold Start 26S 28S 29S
Hot Start 1S 1S 1S
Aided Starts 2S 3S 2S

Here+ /Here+ v2 LED Meaning#

[PROOFREADING NEEDED]There are 3 LEDs on the Here2, two at the side which are the UI LED and one in the center which should flash red with an emergency stop button in the center. If the two UI LEDs are not flashing, chances are the mode switch within the here2 (casing) is in I2C Mode(default) instead of CAN Mode and swap to I2C cables. This I2C and CAN modes are for the compass only, GPS is still connected via Serial.

On PX4 this is information is vital as the errors are not very clear or displayed. However Ardupilot does display more GPS errors which makes it easier to diagnose even without LEDs.

  • Flashing red and blue: Initializing sensors. Place the vehicle still and level while it initializes the sensors.
  • Flashing blue: Disarmed, no GPS lock. Auto-mission, loiter and return-to-launch flight modes require GPS lock
  • Solid blue: Armed with no GPS lock
  • Flashing green: Disarmed (ready to arm), GPS lock acquired. Quick double tone when disarming from the armed state.
  • Fast Flashing green: Same as above but GPS is using SBAS (so should have better position estimate)
  • Solid green: with single long tone at time of arming: Armed, GPS lock acquired. Ready to fly!
  • Double flashing yellow: Failing pre-arm checks (system refuses to arm)
  • Single Flashing yellow: Radio failsafe activated
  • Flashing yellow - with quick beeping tone: Battery failsafe activated
  • Flashing yellow and blue- with high-high-high-low tone sequence (dah-dah-dah-doh): GPS glitch or GPS failsafe activated
  • Flashing red and yellow: EKF or Inertial Nav failure
  • Flashing purple and yellow: Barometer glitch Solid Red: Error,usually due to the SD card(re-plug or place the SD card to solve),MTD or IMU,you may check the SD card and have a look at BOOT.txt for boot message analysis
  • Solid red with SOS tone sequence: SD card missing or SD card bad format
  • No light when power on: No firmware,firmware lost,SD card missing or bad format(ac3.4 or higher version)

https://docs.cubepilot.org/user-guides/here+/here+v2-user-manual

LED Status#

UI LED - On I2C device (GPS LED, same as here2 LED)#

Indicates Readiness of Flight - [Solid Blue] Armed, No GPS Lock: Indicates vehicle has been armed and has no position lock from a GPS unit. When vehicle is armed, PX4 will unlock control of the motors, allowing you to fly your drone. As always, exercise caution when arming, as large propellers can be dangerous at high revolutions. Vehicle cannot perform guided missions in this mode. - [Pulsing Blue] Disarmed, No GPS Lock: Similar to above, but your vehicle is disarmed. This means you will not be able to control motors, but all other subsystems are working. - [Solid Green] Armed, GPS Lock: Indicates vehicle has been armed and has a valid position lock from a GPS unit. When vehicle is armed, PX4 will unlock control of the motors, allowing you to fly your drone. As always, exercise caution when arming, as large propellers can be dangerous at high revolutions. In this mode, vehicle can perform guided missions. - [Pulsing Green] Disarmed, GPS Lock: Similar to above, but your vehicle is disarmed. This means you will not be able to control motors, but all other subsystems including GPS position lock are working. - [Solid Purple] Failsafe Mode: This mode will activate whenever vehicle encounters an issue during flight, such as losing manual control, a critically low battery, or an internal error. During failsafe mode, vehicle will attempt to return to its takeoff location, or may simply descend where it currently is. - [Solid Amber] Low Battery Warning: Indicates your vehicle's battery is running dangerously low. After a certain point, vehicle will go into failsafe mode. However, this mode should signal caution that it's time to end this flight. - [Blinking Red] Error / Setup Required: Indicates that your autopilot needs to be configured or calibrated before flying. Attach your autopilot to a Ground Control Station to verify what the problem is. If you have completed the setup process and autopilot still appears as red and flashing, there may be another error.

Status LED (on FC)#

Color Label Bootloader usage APP usage
Blue ACT (Activity) Flutters when the bootloader is receiving data Indication of ARM state
Red/Amber B/E (In Bootloader / Error) Flutters when in the bootloader Indication of an ERROR
Green PWR (Power) Not used by bootloader Indication of ARM state

https://docs.px4.io/v1.9.0/en/getting_started/led_meanings.html

Other Params#

UAVCAN_ENABLE - For GPS via CAN (PX4)

Misc Tips#

In PX4 v1.8.2, calibrating esc does not always change the motor pwm_min or set one that is low enough. Do change it manually if the drone still spins too fast when arming.

In particular, ESCs like the ones on the DJI Snail seems to have issues calibrating with ardupilot and px4. If the other methods in the snail manual do not work, try calibrating each esc manually with a PWM/Servo tester

For reference, at min pwm, motors should not stop spinning when drone is tilted by 60 deg to each side. https://docs.px4.io/v1.9.0/en/config_mc/pid_tuning_guide_multicopter.html

On px4 reflashing to different software versions does not necessarily reset all parameters, ensure reset to factory default is also used

On pixhawk 2.1, battery voltage/current is only read from the first power port, second port will power the device without any readings.

Back to top