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

trgiman · 2018-03-12T21:04:42Z

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

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

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.
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 · 2018-03-15T06:53:01Z

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 · 2018-03-15T13:19:49Z

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

Last time the nsh> gps status command. gps driver was manually started by me using gps start. By default gps driver was off.
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
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

Tried commander arm in nsh shell ok
Tried commander takeoff in nsh shell - rejecting takeoff, no position lock yet.
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
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 · 2018-03-16T13:37:53Z

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 · 2018-03-17T15:04:10Z

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 · 2018-03-18T20:53:15Z

@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 · 2018-03-19T10:17:49Z

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 · 2018-03-19T14:35:36Z

@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:

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

ssh root@192.168.7.2 ---> AeroBoard Yocto Linux
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 · 2018-03-20T08:41:08Z

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 · 2018-03-20T14:37:53Z

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

I can connect with JMAvsim to Aero FC
QGC connects to 14550 port advertised by JMavsim
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 · 2018-03-20T15:08:47Z

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 · 2018-03-20T21:36:18Z

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

Fixed the line 130 in MAVLinkMessage.java
Added System.out.printf("HIL_ACTUATOR_CONTROLS=true") and

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

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.

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?

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 · 2018-03-21T09:43:44Z

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 · 2018-03-21T10:24:06Z

@bkueng

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

Where have we already read them?

bkueng · 2018-03-21T10:29:47Z

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 · 2018-03-21T10:30:35Z

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

tap_esc stop
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 · 2018-03-21T10:38:32Z

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

trgiman · 2018-03-21T11:50:16Z

@bkueng Ok. Great. It works

Modified /Firmware/cmake/configs for Intel Aero and added /drivers/pwm_out_sim
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
Just works
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 · 2018-03-21T12:08:27Z

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 · 2018-03-21T21:25:09Z

@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 · 2018-03-22T08:54:56Z

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

trgiman · 2018-03-22T15:40:47Z

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

bkueng closed this as completed Mar 22, 2018

trgiman mentioned this issue Mar 23, 2018

Can not connect to Intel AERO FC in HITL mode via UDP PX4/PX4-SITL_gazebo-classic#192

Closed

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

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

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

trgiman commented Mar 12, 2018

bkueng commented Mar 15, 2018

trgiman commented Mar 15, 2018 •

edited

Loading

bkueng commented Mar 16, 2018

trgiman commented Mar 17, 2018 •

edited

Loading

trgiman commented Mar 18, 2018

bkueng commented Mar 19, 2018

trgiman commented Mar 19, 2018

bkueng commented Mar 20, 2018

trgiman commented Mar 20, 2018

bkueng commented Mar 20, 2018

trgiman commented Mar 20, 2018 •

edited

Loading

bkueng commented Mar 21, 2018

julianoes commented Mar 21, 2018

bkueng commented Mar 21, 2018

trgiman commented Mar 21, 2018

julianoes commented Mar 21, 2018

trgiman commented Mar 21, 2018

bkueng commented Mar 21, 2018

trgiman commented Mar 21, 2018

bkueng commented Mar 22, 2018

trgiman commented Mar 22, 2018

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

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

Comments

trgiman commented Mar 12, 2018

bkueng commented Mar 15, 2018

trgiman commented Mar 15, 2018 • edited Loading

bkueng commented Mar 16, 2018

trgiman commented Mar 17, 2018 • edited Loading

trgiman commented Mar 18, 2018

bkueng commented Mar 19, 2018

trgiman commented Mar 19, 2018

bkueng commented Mar 20, 2018

trgiman commented Mar 20, 2018

bkueng commented Mar 20, 2018

trgiman commented Mar 20, 2018 • edited Loading

bkueng commented Mar 21, 2018

julianoes commented Mar 21, 2018

bkueng commented Mar 21, 2018

trgiman commented Mar 21, 2018

julianoes commented Mar 21, 2018

trgiman commented Mar 21, 2018

bkueng commented Mar 21, 2018

trgiman commented Mar 21, 2018

bkueng commented Mar 22, 2018

trgiman commented Mar 22, 2018

trgiman commented Mar 15, 2018 •

edited

Loading

trgiman commented Mar 17, 2018 •

edited

Loading

trgiman commented Mar 20, 2018 •

edited

Loading