Intel Aero RTF HITL -SYS_HITL mode jMavSim - WARN [commander] rejecting takeoff, no position lock yet.

[trgiman]

Hello. I am about to run HITL (Hardware In The Loop simulation). I am about to use jMavSim. But it seems, that QGround Control and jMavsim are not talking to each other. Or JMAVSim is not sending position lock info to Aero FC. Below are the steps I have done recently

1.Trough Nuttx Shell I set param set SYS_HITL 1 and SYS_AUTOSTART to 1001

2.But after reboot SYS_AUTOSTART keeps changing to 4070 (Aero RTF Frame) as I can see from Nuttx SyS console during boot + SYS_AUTOSTART: curr: 1001 -> new: 4070

3. Running JMAVSim in UDP mode connecting to wifi of Aero Board ./jmavsim_run.sh -i 192.168.8.1 -r 250 on Ubuntu. As per sh script default port is 14560 and it writes Init MAVlink to terminal

4.Than I run QGround connected trough UDP - default configuration. Port 14550

5. I can arm and disarm trough QGC. But no MAV messages are visible in JMAVSim terminal. When I try take off in QGC. Nuttx console writes WARN [commander] rejecting takeoff, no position lock yet.

6. When I try the same by running commander takeoff in Nuttx console. Result is the same WARN [commander] rejecting takeoff, no position lock yet.

I guess WARN [commander] rejecting takeoff, no position lock yet. Please retry.. is because JMavsim is not sending message to Aero FC.....because SYS_AUTOSTART can not be set permanently to 1001?And also when I run gps status on NSH console. lat,lon values are not provided from jMAVSIM

INFO [gps] Main GPS INFO [gps] protocol: UBX INFO [gps] port: /dev/ttyS5, baudrate: 38400, status: OK INFO [gps] sat info: disabled, noise: 95, jamming detected: NO INFO [gps] position lock: 0, satellites: 0, last update: 78.8240ms ago INFO [gps] lat: 0, lon: 0, alt: -17000 INFO [gps] vel: 0.00m/s, 0.00m/s, 0.00m/s INFO [gps] hdop: 99.99, vdop: 99.99 INFO [gps] eph: 4294967.50m, epv: 3750725.00m INFO [gps] rate position: 0.00 Hz INFO [gps] rate velocity: 0.00 Hz INFO [gps] rate publication: 0.00 Hz INFO [gps] rate RTCM injection: 0.00 Hz

Any suggestions please?

[bkueng]

Hi There are multiple issues on AeroFC to get HITL working:

• The fact that the gps driver is running, indicates that SYS_HITL is not 1. See https://github.com/PX4/Firmware/blob/master/ROMFS/px4fmu_common/init.d/rcS#L400-L414. This is why you don't get a position lock.
• the SYS_AUTOSTART is hardcoded on AeroFC, which is why it changes back. But this should not be a problem.
• the tap_esc ESC driver is started unconditionally (https://github.com/PX4/Firmware/blob/master/ROMFS/px4fmu_common/init.d/4070_aerofc#L71), but it should not run in HITL mode
• WiFi has variable latency and which negatively affects HITL performance. It might or might not work.

[trgiman]

Hi @bkueng. Thanks for hints. I went trough whole process again.

1. Last time the nsh> gps status command. gps driver was manually started by me using gps start. By default gps driver was off.

2. This time I renabled TXRX pins on Intel Aero RTF TELEM port. And reenabled MAVLINK on these pins. Validated that MAVLINK runs on these ports by using ./mavlink_shell.py --baudrate 57600 /dev/ttyUSB0...closed the connection

3. In nsh console by param show SYS_HITL validated, that value is 1

4.Connected JMavlink trough FTDI cable to connector mentioned in point 2 by command ./jmavsim_run.sh /dev/ttyUSB0 57600

5. Tried commander arm in nsh shell ok

6. Tried commander takeoff in nsh shell - rejecting takeoff, no position lock yet.

7. Connected QGround (tried via USB and via WIFI) - Commands to take off send from QGround Controll were rejected with same output - rejecting takeoff, no position lock yet (when monitored trough nsh boot long console). Commands to ARM were accepted but not visible in terminal of jMAVSim

8. When run in nsh> commander status and gps status output is:

nsh> commander status WARN [commander] type: symmetric motion WARN [commander] safety: USB enabled: [NO], power state valid: [OK] WARN [commander] avionics rail: -1.00 V WARN [commander] home: lat = 0.0000000, lon = 0.0000000, alt = 0.00, yaw: 0.00 WARN [commander] home: x = 0.0000000, y = 0.0000000, z = 0.00 WARN [commander] datalink: OK WARN [commander] main state: 0 WARN [commander] nav state: 0 WARN [commander] arming: ARMING_STATE_STANDBY nsh> gps status INFO [gps] not running

Any suggestions please? I do not know how exactly HITL simulates the GPS? Running fake gps from nsh also does not help. QGround obviously reports that it has enough satellites, 3dfix but commander takeoff result is the same - rejecting takeoff, no position lock yet.

Thank you.

[bkueng]

4.Connected JMavlink trough FTDI cable to connector mentioned in point 2 by command ./jmavsim_run.sh /dev/ttyUSB0 57600

The FTDI is probably too slow to run HIL, I suggest to try via USB. The vehicle should receive GPS position data from jMAVSim via HIL_GPS MAVLink messages. It seems to me there's a problem with that communication. On the nsh shell, you can check if there are gps messages being published via uorb top gps - the #MSG should be >0. You can look at the actual gps messages via listener vehicle_gps_position 10. The same holds for the IMU data, for example the sensor_combined message should be published.

[trgiman]

Hi @bkueng I tried commands above. There is no activity for sensor_combined topic,vehical_gps_positions. System is showing 0 for all variables. I want to be sure. Am I really connected with jMavSim to AeroFC flight controller?

1.Can you please advice me where in the code of jMavSim I can verify, that I do have got connectivity to AeroFC?

or

2.Or verify, that PX4 on Intel Board - AeroFC is listening on po 14560 via nsh Nuttx?

or

3./jmavsim_run.sh 192.168.7.2:14560 should connect me to AeroFC PX4 trough UDP (USB connector on AeroBoard). Correct?But I never get following message after JMavSim is started:

Init MAVLink MSG: Switched to ON hil state MSG: [cmd] DISARMED by set mode command

But for instance I have no issues running ./mavlink_shell.py 0.0.0.0:14550 connection to AeroFc Nuttx shell.

[trgiman]

@bkueng I am also adding following system output:

My config of jmavsim_run.sh is:

! /bin/bash

SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" cd "$SCRIPT_DIR/jMAVSim"

udp_port=14560 extra_args= baudrate=921600 device="" ip="192.168.7.2"

ss -au command shows:

State Recv-Q Send-Q Local Address:Port Peer Address:Port UNCONN 0 0 :ipp : UNCONN 0 0 :mdns : UNCONN 0 0 :47184 : UNCONN 0 0 %enp1s0f0:47736 : UNCONN 0 0 127.0.1.1:domain : UNCONN 0 0 :bootpc : UNCONN 0 0 :bootpc : UNCONN 0 0 :::45589 ::: UNCONN 0 0 :::mdns ::: ESTAB 0 0 ::ffff:192.168.7.17:54564 ::ffff:192.168.7.2:14560 ESTAB 0 0 ::ffff:127.0.0.1:55161 ::ffff:127.0.0.1:14550

ESTAB is jMavsim connection. Is it correct? 192.168.7.2 is address of Intel RTF Aero Board. mavlink- router is running there ( I enabled port 14560 in the mavlink-router)

[bkueng]

So the GPS messages don't arrive at the flight controller. This can be a port/connectivity issue or some issue in mavlink router (not forwarding these messages for some reason). To further test, you can do:

• Make sure QGC is not running (or anything else that could use the mavlink ports and interfere)
• is ./mavlink_shell.py 192.168.7.2:14560 working? If so, then start jmavsim with ./Tools/jmavsim_run.sh -q -i 192.168.7.2 -p 14560 -r 250
• make sure the 14550 port is not enabled in mavlink router, because jMAVSim also opens that port with a mavlink stream, that is used for QGC to connect.

The HIL mavlink connection is created here: https://github.com/PX4/jMAVSim/blob/master/src/me/drton/jmavsim/Simulator.java#L175

[trgiman]

@bkueng mavlink_shell.py 0.0.0.0:14560 works (or IP that has been assigned to by AeroBoard to my Ubuntu USB device works as well). Unfortunately command you recommended does not work. But I did following steps - befor jMavsimSim throws exception some mavlink data are send to AeroFC. Please take a look below:

1. My current mavlink-router conf file on Yocto Linux on Aero Board is:

[General] TcpServerPort=5760 ReportStats=false Log=/var/lib/mavlink-router/

[UartEndpoint uart] Device = /dev/ttyS1 Baud = 921600,460800

[UdpEndpoint wifi] Mode = Normal Address = 192.168.8.255

[UdpEndpoint ethernet-over-usb] Mode = Normal Address = 192.168.7.255 Port = 14560 <-----this means that i can use only 14560 port when using ./mavlink_shell

[UdpEndpoint csd] Address = 0.0.0.0 Port = 80550 Mode = eavesdropping

2. ssh root@192.168.7.2 ---> AeroBoard Yocto Linux

3. Aero Board Yocto Local ports of mavlink-routerd: netstat -aulp | grep "mavlink-routerd"

udp 0 0 :50506 : 341/mavlink-routerd udp 0 0 :41355 : 341/mavlink-routerd udp 0 0 :15014 :* 341/mavlink-routerd

4. When I try run ./jmavsim_run.sh -q -i 192.168.7.2 -p 41355 -r 250 (on 41355 port or other 2 ports)

a)jMavsim writes Init MAVLink. Than it throws exception:

BUILD SUCCESSFUL Total time: 0 seconds Options parsed, starting Sim. Starting GUI... 3D [dev] 1.6.0-pre12-daily-experimental daily

Init MAVLink Exception in Simulator.world.update() : java.lang.IllegalArgumentException at java.nio.Buffer.position(Buffer.java:244) at me.drton.jmavlib.mavlink.MAVLinkMessage.(MAVLinkMessage.java:121) at me.drton.jmavlib.mavlink.MAVLinkStream.read(MAVLinkStream.java:48) at me.drton.jmavsim.UDPMavLinkPort.update(UDPMavLinkPort.java:139) at me.drton.jmavsim.MAVLinkConnection.update(MAVLinkConnection.java:44) at me.drton.jmavsim.World.update(World.java:33) at me.drton.jmavsim.Simulator.run(Simulator.java:404) at java.util.concurrent.Executors$RunnableAdapter.call(Executors.java:511) at java.util.concurrent.FutureTask.runAndReset(FutureTask.java:308) at java.util.concurrent.ScheduledThreadPoolExecutor$ScheduledFutureTask.access$301(ScheduledThreadPoolExecutor.java:180) at java.util.concurrent.ScheduledThreadPoolExecutor$ScheduledFutureTask.run(ScheduledThreadPoolExecutor.java:294) at java.util.concurrent.ThreadPoolExecutor.runWorker(ThreadPoolExecutor.java:1149) at java.util.concurrent.ThreadPoolExecutor$Worker.run(ThreadPoolExecutor.java:624) at java.lang.Thread.run(Thread.java:748)

a) NuttX PX4 nsh shell writes:

ERROR [sensors] Mag #0 fail: TIMEOUT! ERROR [sensors] Accel #0 fail: TIMEOUT! ERROR [sensors] Gyro #0 fail: TIMEOUT!

b) In nsh shell I run listener sensor_combined.....I can see that some data has been sent before Mavsim crashed

nsh> listener sensor_combined

TOPIC: sensor_combined instance 0 #1 timestamp: 1875614246 gyro_rad: 0.0175 0.0021 0.0208 gyro_integral_dt: 3981 accelerometer_m_s2: -0.0629 -0.0191 -9.8326 accelerometer_integral_dt: 3981 magnetometer_ga: 0.9034 0.0367 1.7795 baro_alt_meter: 487.9796 baro_temp_celcius: 0.0000

Can you please suggest if this is wrong configuration of mavlink-router or is this related to jMavSim?

[bkueng]

mavlink_shell.py 0.0.0.0:14560 works (or IP that has been assigned to by AeroBoard to my Ubuntu USB device works as well). Unfortunately command you recommended does not work

My mistake, you're right, it should be 0.0.0.0:14560. You're setup looks correct now, and the crash looks like a MAVLink 2 parsing bug in jMAVSim in https://github.com/PX4/jMAVlib/blob/master/src/me/drton/jmavlib/mavlink/MAVLinkMessage.java#L121. I think it should be

buffer.position(buffer.position() + payloadLen + CRC_LENGTH + getSignatureLength());

instead, since at this point we already read the msg ID & extension bytes. @julianoes fyi

[trgiman]

@bkueng @julianoes I made the fix in the source code as recommended. Helped but not copletely. Please read below.

1. I can connect with JMAvsim to Aero FC

2. QGC connects to 14550 port advertised by JMavsim

3. I can use RC controller to send MAVLINK messages trough QGC -> JMavSim -> to AeroFC and JMAVsim reports take_off and land messages etc.

4. But Model in the JMavsim is not moving at all, no matter if I take-off from QGC or TakeOff via RC....but mavlink messages are reported

I have notice few reports from NSH Boot console:

a) ERROR[sensors] FAILED APPLYING mag CAL 0 b) VDOP and HDOP values are reported as 0 when run from nsh> listener vehicle_gps_position

Should I set different values for eph and epv values in MAVLINK message. Can you suggest where?Probably SimpleSensors sensors = new SimpleSensors(); and than sensors.SomeFunction all this in Simulator.java function buildMulticopter? Or the reason why model is not moving is different?

Thanks for advices.

[bkueng]

Good, we're making progress :) Can you send a PR for the jMAVSim fix? This line needs the same update: https://github.com/PX4/jMAVlib/blob/master/src/me/drton/jmavlib/mavlink/MAVLinkMessage.java#L130

Next you need to check if the simulator receives the HIL_ACTUATOR_CONTROLS and that they're not zero (these are the motor controls). You can add printf's in here: https://github.com/PX4/jMAVSim/blob/master/src/me/drton/jmavsim/MAVLinkHILSystem.java#L50. On the vehicle, make sure that tap_esc is not running and pwm_out_sim is running (can be seen with top). uorb top should show actuator_outputs messages being published.

The VDOP & HDOP being 0 is not a problem.

[trgiman]

@bkueng yes we have some progress :-).I will send PR for jMAVSim fix.

1. Fixed the line 130 in MAVLinkMessage.java

2. Added System.out.printf("HIL_ACTUATOR_CONTROLS=true") and

Number d = ((Number)((Object[])msg.get("controls"))[i]).doubleValue(); System.out.println(d);

3. Push Takeoff trough QGC and terminal is full of

HIL_ACTUATOR_CONTROLS = trueHIL_ACTUATOR_CONTROLS = trueHIL_ACTUATOR_CONTROLS = trueHIL_ACTUATOR_CONTROLS = trueHIL_ACTUATOR_CONTROLS = trueHIL_ACTUATOR_CONTROLS = trueHIL_ACTUATOR_CONTROLS = true

0.1900000125169754 0.1900000125169754 0.1900000125169754 0.1900000125169754 0.0 0.0 0.0 0.0

4.uorb top tap_esc shows

update: 1s, num topics: 77 TOPIC NAME INST #SUB #MSG #LOST #QSIZE

5.uorb top pwm_out_sim

update: 1s, num topics: 77 TOPIC NAME INST #SUB #MSG #LOST #QSIZE

Seems it is not running.

6. uorb top actuator_outputs

update: 1s, num topics: 78 TOPIC NAME INST #SUB #MSG #LOST #QSIZE actuator_outputs 0 2 166 165 1

Seems almost all messages are lost?

7. listener actuator_outputs

timestamp: 129270006 noutputs: 4 output: 1190.0000 1190.0000 1190.0000 1190.0000 NaN NaN NaN NaN NaN NaN NaN NaN NaN NaN NaN NaN

1190 is propably puls length right? In this case each one for 1 motor. To give motor more power value should be rather higher than only 1190?

[bkueng]

uorb top pwm_out_sim

No, these are separate commands: use top to see which processes are running, and then uorb top to display the uORB message publications.

1190 is propably puls length right?

Correct. The range is from 1000 to 2000 and will be rescaled to [0, 1] for HIL_ACTUATOR_CONTROLS. After takeoff the values should go up to ~0.5, 0.19 is probably not enough to move the vehicle. Can you test with a joystick and give full throttle? (there's a virtual joystick in QGC)

[julianoes]

@bkueng

instead, since at this point we already read the msg ID & extension bytes.

Where have we already read them?

[bkueng]

https://github.com/PX4/jMAVlib/blob/master/src/me/drton/jmavlib/mavlink/MAVLinkMessage.java#L111 and https://github.com/PX4/jMAVlib/blob/master/src/me/drton/jmavlib/mavlink/MAVLinkMessage.java#L99. These already advance the buffer position.

[trgiman]

@bkueng after using top command. I see tap_esc is running.So I did

1. tap_esc stop
2. Pushed button on QGC UI to TakeOff. Used Virtual Joysticks and Tried it also with RC (Takeoff, more Throttle). But message for PWM for motors still prints

0.1900000125169754 0.1900000125169754 0.1900000125169754 0.1900000125169754

[julianoes]

@bkueng you're right, I wonder why I added that.

[trgiman]

@bkueng Ok. Great. It works

1. Modified /Firmware/cmake/configs for Intel Aero and added /drivers/pwm_out_sim

2. tap_esc was still running. So I did tap_esc stop. Now what is the best place to stop tap_esc during boot. https://github.com/PX4/Firmware/blob/master/ROMFS/px4fmu_common/init.d/rcS or https://github.com/PX4/Firmware/blob/master/ROMFS/px4fmu_common/init.d/4070_aerofc

3. Just works

4. I did implement "indoor GPS...device" sending NMEA to PX4. It works with standard PX4 gps driver." Any suggestion if I can switch of simulated GPS and use real indoor GPS during jMavsim simulation. Also suggestions, where I could insert real Intel AeroFC physical, mechanical values? Or should I open the new issue for this? Should I still do PR as I noticed @julianoes comments.

[bkueng]

Cool.

Modified /Firmware/cmake/configs for Intel Aero and added /drivers/pwm_out_sim

Can you make a PR for that? And modify 4070_aerofc like this:

if param compare SYS_HITL 0
then
  tap_esc start -d /dev/ttyS0 -n 4
  usleep 300000

  set OUTPUT_MODE tap_esc
fi

Should I still do PR as I noticed @julianoes comments.

Yes please.

I did implement "indoor GPS...device" sending NMEA to PX4. It works with standard PX4 gps driver." Any suggestion if I can switch of simulated GPS and use real indoor GPS during jMavsim simulation. Also suggestions, where I could insert real Intel AeroFC physical, mechanical values? Or should I open the new issue for this?

This needs a bigger discussion and thus a new issue would be better, yes (I suggest in Firmware). I don't understand how you want to integrate that? If you have a real position that is not moving, and your simulated vehicle moves, how do you want to use the real position?

[trgiman]

@bkueng I made PR for both. In one case. In this case http://ci.px4.io:8080/blue/organizations/jenkins/Firmware/detail/PR-9132/1/pipeline there are some problems.

you have a real position that is not moving, and your simulated vehicle moves, how do you want to use the real position?

:-) yes that is correct. Basically I was just thinking to test that hedgehog is providing continuously accurate position.In position mode MAV should keep the position, in this case by using "indoor GPS". If hedgehog will provide some sudden inaccurate GPS coordinates(while HH is not moving) vehicle will start to drift? That was the main purpose.

Should I create new Issue for setting up correct model/frame Aero RTF physical variables here in jMAVSim rep? I may need some suggestions to set it correctly, also add PX4Flow sensor, surfaces for sensor and suggestions how to add simulation of other sensors (Garmin v3 lite, switch off barometer etc. if possible). Or Gazebo is better for that kind of modifications?

Anyway thanks a lot for guidance.

[bkueng]

If you want to add flow and other sensors then I suggest you use gazebo. It also has HIL support.

[trgiman]

Ok.Thanks a lot. I think we can close this issue.