search

PX4 / PX4-Autopilot

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

RealFlight/FlightAxis SITL/HITL #17642

Open podhrmic opened 3 years ago

podhrmic commented 3 years ago

Describe problem solved by the proposed feature My goal is to add RealFlight simulator support into PX4, and be able to use for SITL/HITL. That way the user can simulate lots of small RC planes (and some copters/multirotors). I know the simulator is not free, but neither was X-plane, and the physics simulation seems to be pretty realistic.

Describe your preferred solution RealFligth uses Flight Axis protocol (details here) - which is essentially a SOAP XML wrapper around the simulation data (sample packet here).

I have a python script that gets data from Real Flight, passes the HIL messages to PX4, gets the actuator states from PX4 and sends them back to Real Flight. All working at ~150Hz in SITL mode with a standard plane. Not tested in HITL mode.

Issues I am running into Real Flight does not provide simulation at the sensor level, so I had to copy some parts from the flightgear bridge to try to simulate the sensor data, but I probably have something wrong, because when I run the simulation, PX4 constantly resets the EKF, claiming inconsistent sensor readings.

Questions for the developers 1) is there an easy way to run PX4 with external INS - so I can provide the attitude data as is, without the need to inject/simulate the sensor noise etc? 2) do you have any guidance about how to debug the EKF errors?

dagar commented 3 years ago

Are you using the default build with lockstep enabled? https://github.com/PX4/PX4-Autopilot/blob/e18b1c0355597d344186f8485014e835005085ba/boards/px4/sitl/default.cmake#L118-L120

You could bypass the estimator and publish attitude, local & global position, etc directly, but I think you might as well do it at the sensor level if 150 Hz is viable.

Can you share a flight log? https://logs.px4.io/

podhrmic commented 3 years ago

That was with the default build (lockstep enabled). When I changed the line 120 to set(ENABLE_LOCKSTEP_SCHEDULER no) I get a build error:

$ make px4_sitl none_iris
[0/808] git submodule src/drivers/gps/devices
[11/808] git submodule src/lib/ecl
[237/808] git submodule mavlink/include/mavlink/v2.0
[250/808] Generating romfs_extract.stamp
FAILED: ROMFS/romfs_extract.stamp
cd /cygdrive/c/Users/galoisuser/Documents/GitHub/PX4-Autopilot/build/px4_sitl_default/etc && /usr/bin/cmake.exe -E remove_directory /cygdrive/c/Users/galoisuser/Documents/GitHub/PX4-Autopilot/build/px4_sitl_default/etc/* && /usr/bin/cmake.exe -E tar xf /cygdrive/c/Users/galoisuser/Documents/GitHub/PX4-Autopilot/build/px4_sitl_default/romfs_files.tar && /usr/bin/cmake.exe -E touch /cygdrive/c/Users/galoisuser/Documents/GitHub/PX4-Autopilot/build/px4_sitl_default/ROMFS/romfs_extract.stamp
CMake Error: cmake version 3.14.5
Usage: /usr/bin/cmake -E <command> [arguments...]
Available commands:
  capabilities              - Report capabilities built into cmake in JSON format
  chdir dir cmd [args...]   - run command in a given directory
  compare_files [--ignore-eol] file1 file2
                              - check if file1 is same as file2
  copy <file>... destination  - copy files to destination (either file or directory)
  copy_directory <dir>... destination   - copy content of <dir>... directories to 'destination' directory
  copy_if_different <file>... destination  - copy files if it has changed
  echo [<string>...]        - displays arguments as text
  echo_append [<string>...] - displays arguments as text but no new line
  env [--unset=NAME]... [NAME=VALUE]... COMMAND [ARG]...
                            - run command in a modified environment
  environment               - display the current environment
  make_directory <dir>...   - create parent and <dir> directories
  md5sum <file>...          - create MD5 checksum of files
  sha1sum <file>...         - create SHA1 checksum of files
  sha224sum <file>...       - create SHA224 checksum of files
  sha256sum <file>...       - create SHA256 checksum of files
  sha384sum <file>...       - create SHA384 checksum of files
  sha512sum <file>...       - create SHA512 checksum of files
  remove [-f] <file>...     - remove the file(s), use -f to force it
  remove_directory dir      - remove a directory and its contents
  rename oldname newname    - rename a file or directory (on one volume)
  server                    - start cmake in server mode
  sleep <number>...         - sleep for given number of seconds
  tar [cxt][vf][zjJ] file.tar [file/dir1 file/dir2 ...]
                            - create or extract a tar or zip archive
  time command [args...]    - run command and display elapsed time
  touch file                - touch a file.
  touch_nocreate file       - touch a file but do not create it.
  create_symlink old new    - create a symbolic link new -> old

[251/808] Building CXX object src/lib/parameters/tinybson/CMakeFiles/tinybson.dir/tinybson.cpp.o
ninja: build stopped: subcommand failed.
make: *** [Makefile:228: px4_sitl] Error 1

Lockstep enabled

Here if the flight log: https://review.px4.io/plot_app?log=ce0ccbb6-747a-4c68-98a5-d2716d7525d1

Here is the SITL output (running this in Windows):

$ make px4_sitl none_iris
...
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 406 bytes, decoded 406 bytes
[param] Loaded: eeprom/parameters_10016
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  [simulator] Simulator connected on TCP port 4560.
ERROR [px4_work_queue] setting sched params for wq:lp_default failed (134)
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  [ekf2] starting instance 2, IMU:2 (1311004), MAG:0 (197388)
ERROR [ekf2] 2 - vehicle_magnetometer lost, generation 23 -> 25
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: Normal, data rate: 4000000 B/s on udp port 18570 remote port 14550
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-05-25/03_35_22.ulg
INFO  [logger] Opened full log file: ./log/2021-05-25/03_35_22.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  [mavlink] partner IP: 127.0.0.1
INFO  [px4] Startup script returned successfully
pxh> INFO  [tone_alarm] notify negative
ERROR [ekf2] 2 - vehicle_magnetometer lost, generation 2105 -> 2107
ERROR [ekf2] 5 - vehicle_magnetometer lost, generation 2105 -> 2107
ERROR [ekf2] 5 - vehicle_magnetometer lost, generation 2786 -> 2788
ERROR ERROR [ekf2] [ekf2] ERROR 1 - vehicle_magnetometer lost, generation 6335 -> 63370 - vehicle_magnetometer lost, generation 6335 -> 6337[ekf2]

2 - vehicle_magnetometer lost, generation 6335 -> 6337
ERROR ERROR [ekf2] ERROR [ekf2] 4 - vehicle_magnetometer lost, generation 6334 -> 6336[ekf2] 3 - vehicle_magnetometer lost, generation 6334 -> 6336
5 - vehicle_magnetometer lost, generation 6334 -> 6336

ninja: build stopped: interrupted by user.
make: *** [Makefile:228: px4_sitl] Interrupt

Does that help?

Jaeyoung-Lim commented 3 years ago

Are you already adding noise to the sensor data? Your data might being invalidatednif you are sending the same values.

Could you maybe share how you are sending the data already so we can have a closer look?

podhrmic commented 3 years ago

@Jaeyoung-Lim certainly! I am not adding any noise right now, so that might be a problem - does PX4 detect "stale" values like that?

I am sending HIL_SENSOR message here and HIL_GPS message here. Note the #TODO comments about the particular unit conversion - they might not be right, perhaps you can spot an obvious mistake?

I am also sending HIL_STATE_QUATERNION here but I don't seem to be able to see the actual MAVLINK message being logged - do I need to enable anything to capture it?

julianoes commented 3 years ago

@podhrmic I don't see that cmake error that you describe. What is your cmake version?

podhrmic commented 3 years ago 
$ cmake --version
cmake version 3.14.5

But it looks like the problem is in my Windows setup, investigating...

podhrmic commented 3 years ago

Update: it was an issue with my Windows setup! After getting latest master and running it on a Linux machine, I can connect to FlightAxis and fly in manual mode using Joystick.

There are still some unresolved issues: 1) baro and airspeed is not simulated. That seems to confuse the estimator, so I need to add the proper simulated data. 2) The way I have it setup now I am averaging ~70Hz update rate - both with and without the lockstep. Given RealFlight doesn't care about the lockstep, disabling it on PX4 makes sense. But as a result, the EKF timeouts are more common (see below).

Question for the developers: what is the desired update rate for the estimator on a standard plane so I know what is "good enough"?

Here is the current PX4 output:

px4 starting.

INFO  [px4] Calling startup script: /bin/sh etc/init.d-posix/rcS 0
Info: found model autostart file as SYS_AUTOSTART=1030
INFO  [param] selected parameter default file eeprom/parameters_1030
INFO  [parameters] BSON document size 462 bytes, decoded 462 bytes
[param] Loaded: eeprom/parameters_1030
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  [commander] LED: open /dev/led0 failed (22)
INFO  [init] Mixer: etc/mixers-sitl/plane_sitl.main.mix on /dev/pwm_output0
INFO  [mavlink] mode: Normal, data rate: 4000000 B/s on udp port 18570 remote port 14550
INFO  [airspeed_selector] No airspeed sensor detected. Switch to non-airspeed mode.
INFO  [airspeed_selector] Airspeed estimation invalid
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-05-28/16_07_20.ulg
INFO  [logger] Opened full log file: ./log/2021-05-28/16_07_20.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  [mavlink] partner IP: 127.0.0.1
INFO  [commander] Failsafe mode activated
INFO  [tone_alarm] battery warning (fast)
INFO  [simulator] Simulator connected on TCP port 4560.
INFO  [ekf2] starting instance 0, IMU:2 (1311004), MAG:0 (197388)
INFO  [airspeed_selector] Airspeed sensor healthy, start using again (-1, 1)
INFO  [ekf2] starting instance 1, IMU:0 (1310988), MAG:1 (197644)
INFO  [ekf2] starting instance 2, IMU:1 (1310996), MAG:1 (197644)
INFO  [ekf2] starting instance 3, IMU:2 (1311004), MAG:1 (197644)
INFO  [ekf2] starting instance 4, IMU:0 (1310988), MAG:0 (197388)
INFO  [ekf2] starting instance 5, IMU:1 (1310996), MAG:0 (197388)
ERROR [vehicle_air_data] BARO #0 failed:  STALE!
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  [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] GPS checks passed
INFO  [ecl/EKF] GPS checks passed
INFO  [ecl/EKF] GPS checks passed
INFO  [ecl/EKF] GPS checks passed
WARN  [ecl/EKF] baro hgt timeout - reset to gps
WARN  [ecl/EKF] baro hgt timeout - reset to gps
WARN  [ecl/EKF] baro hgt timeout - reset to gps
WARN  [ecl/EKF] baro hgt timeout - reset to gps
INFO  [ecl/EKF] 109217730225: EKF aligned, (gps hgt, IMU buf: 12, OBS buf: 9)
INFO  [ecl/EKF] 109217730225: EKF aligned, (gps hgt, IMU buf: 12, OBS buf: 9)
INFO  [ecl/EKF] 109217730225: EKF aligned, (gps hgt, IMU buf: 12, OBS buf: 9)
INFO  [ecl/EKF] 109217730225: EKF aligned, (gps hgt, IMU buf: 12, OBS buf: 9)
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  [ecl/EKF] reset position to GPS
INFO  [ecl/EKF] reset velocity to GPS
INFO  [ecl/EKF] starting GPS fusion
WARN  [ekf2] primary EKF changed 0 (timeout) -> 1
WARN  [ekf2] primary EKF changed 1 (timeout) -> 2
WARN  [ekf2] primary EKF changed 2 (timeout) -> 0
...(many more timeout warnings)
Jaeyoung-Lim commented 3 years ago

@podhrmic As a reference, gazebo / jsbsim / jmavsim is working with ~250Hz with lockstep.

If you disable lockstep, as you mentioned you will see inconsistencies depending on the system performance.

Few questions regarding realflight.

what are your plans regarding the implementation? Do you plan to merge it into the firmware?

bduva002 commented 1 year ago

@podhrmic I am trying to do the same thing you are. Have you gotten any further with this? Do you have any examples of how to talk to real flight over SOAP XML. Also where are you putting your python script in the px4 directory's. I have been using the autopilot setup for a while now. This would be a really nice addition to the PX4 stack.

podhrmic commented 1 year ago

@bduva002 here is the script for talking to RealFlight, it was a student project so the code is not particularly polished, but should be a good starting point. We ran out of time so IIRC it connects PX4 with RealFlight, but it doesn't take off or control the plane.

bduva002 commented 1 year ago

Thanks!

rete0304 commented 8 months ago

I also have the same need for HITL/SITL on VTOL . I will also try it simultaneously. If you have more news, please let me know. thank you