Can't Send External Position Estimation with Mavlink C library

rijesha · February 15, 2018, 12:13am

Hi All,

I am attempting to send external position estimation information to a pixhawk running px4 v1.7.3.
I am trying to send local position information from an off-board computer that includes x,y,z and yaw.

I would like to do this without using ros and only the mavlink c library. I am building my code based off of the c_uart_interface_example.

In the mavlink c library I am using att_pos_mocap message as follows:

    mavlink_att_pos_mocap_t cp;

    cp.x = 10;
    cp.y = 10;
    cp.z = 10;
    cp.q[0] = 1;
    cp.q[1] = 0;
    cp.q[2] = 0;
    cp.q[3] = 0;
    cp.time_usec =  get_time_usec() - time_sync_offset;

    mavlink_message_t message;
    mavlink_msg_att_pos_mocap_encode(1, 1, &message, &cp);
    
    int len = autopilot_interface_quit->write_message(message);

I syncronize the time by reading the time_sync message when it arrives as follows:

uint64_t time_sync_offset = get_time_usec() - timesyncmsg.ts1/1000;

I have tried sending the time_sync message to update the tc1 field, but that field has not updated and remains as 0.
```
mavlink_timesync_t ts;

ts.tc1 =(uint32_t)  (get_time_usec()/1000);;
ts.ts1 =(uint32_t)  (get_time_usec()/1000);; //this field will be overwritten by the pixhawk
mavlink_message_t message;
mavlink_msg_timesync_encode(1, 1, &message, &ts);

int len = autopilot_interface_quit->write_message(message);


I am monitoring the pixhawk using the direct USB connection and am using Mavlink Inspector to see if the fields are updating.
Local position is not updating but the z value does change a bit. (due to barometer).

I am sending the att_pos_mocap and timesync message over telem2 at a baudrate of 921600.
I have attempted to change SYS_MC_EST_GROUP from ekf2 to lpe but on reboot, the setting doesn't hold and defaults to ekf2.
One other thing that I have tried is changing the ekf2_aid_mask to include vision position fusion and vision yaw fusion. Doing so prevents the local position message appearing in mavlink inspector.


I have two thoughts on what the problem could be but I could be way off:

1. I am incorrectly sending messages through the companion interface. Evidence of this is that I am not able to update the timesync field. Although, I am able to enable/disable offboard flight mode. Again I am using the c_uart_interface_example as a framework and am able to read messages from this link.

2. I need to set the pixhawk into lpe mode and something is preventing it.


Any help regarding this would be much appreciated.

regards
Rijesh

rijesha · February 15, 2018, 9:39pm

Here are a few more things that I have tried:

building and uploading the px4fmu-v2_lpe firmware.
I still did not see the local position message updating.
I thought I would increase the timeout and minimum number of messages in mocap.cpp to see if it was a timing issue

static const uint32_t		REQ_MOCAP_INIT_COUNT = 2;
static const uint32_t		MOCAP_TIMEOUT = 20000000;	// 20 s

I built and uploaded, but this didn’t help either. The local_position_ned message has an x,y value of 0 and a fluctuating z value.

rijesha · February 16, 2018, 11:23pm

So I have successfully sent the local position estimation message using drone kit.

from dronekit import connect, VehicleMode
import time
from time import sleep
# Connect to UDP endpoint.
vehicle = connect('/dev/ttyACM0', wait_ready=False, baud=57600)
# Use returned Vehicle object to query device state - e.g. to get the mode:
sleep(3)
for i in range(0,100):
    time_usec = int(round(time.time() * 1000000))
    vehicle._master.mav.att_pos_mocap_send(time_usec, [1,0,0,0], 5, 5, 19)
    sleep(.1)

print " Mode: %s" % vehicle.mode.name
print vehicle.location.local_frame

I am going to investigate further as to what I am doing wrong with mavlink.

rijesha · February 20, 2018, 8:24pm

I managed to send the information using the c library.
I was writing to the serial port from two different threads. I’m not sure why this was causing a problem since there is a mutex controlling write access on the serial port.

So for v1.7.3 the steps to send position data.
Build the lpe release of px4. (https://dev.px4.io/en/setup/building_px4.html) and upload it.

make px4fmu-v2_lpe
make px4fmu-v2_lpe upload

Then select a multirotor airframe.
Change SYS_MC_EST_GROUP to LPE.
Change LPE_FUSION to accept vision information.
Change SYS_COMPANION to 921600.

And that is it.
The usec timestamp has absolutely no effect on the message being successfully sent/received. You can set it to 0 and forget about it. There is no need to do anything with the timesync message either.

Also sending the message at 5Hz is too slow. 10 Hz works great for me.

OmarMorando · March 6, 2018, 11:09am

Hello Rijesh,
thanks a lot for your information. Could you share your example code?
Thanks, regards

rijesha · March 6, 2018, 3:24pm

I don’t have a lot of documentation but check out this github.com repository and main.cpp.

Remeber to do a git submodule init and git submodule update.

OmarMorando · March 6, 2018, 4:33pm

Thanks a lot, great job!

rijesha · March 12, 2018, 8:04pm

I’ve updated the documentation and included a better example for reading messages.

OmarMorando · March 13, 2018, 8:08am

Thanks again for your update, Augustine. I’m trying to implement the same project using UWB and a PX4 1.7.3 on a F450 frame and a PixHawk 2.1.
I have some doubts that hope that you can clarify.

My goal is to realize a completely autonomous system without a radio controller, do I have to set some specific parameters using QGroundControl? I mean, do I have to set flight mode in “offboard” as default or it can be set by the onboard companion computer?
It’s the first time that I use LPE mode. I have 6 UWB anchors around a room with a high level of accuracy (3-5 cm), and is not clear for me how LPE works. For the first flight do I set the origin of my local position (x, y) to the flight control? Or is it sufficient to update the current position sending the mavlink message and than to send the desired position?

Thank you for your precious help!

rijesha · March 13, 2018, 4:59pm

I don’t think you can set it to offboard by default. You can change it to offboard via the onboard companion computer. (Just like I do in main.cpp). If you plan on never using an RC controller you can still arm the vehicle if you change the COM_RC_IN_MODE parameter.
By default if you power on the vehicle it will centre to 0,0,0. Afterwards, these values may change based on what you have set in LPE_fusion.

Here is what your program should do.

Start periodic current_position message stream (and update whenever new data is available)
Start periodic desired_position message stream at current position.
Wait for ~100 desired_position_messages
Enable offboard mode.
Update desired position message to new desired position.

OmarMorando · March 15, 2018, 7:19am

Ok, thanks again, it’s more clear.
I tried your example with a real flight control and it works.
Only a question, I hope the last one: current_position is used to tell to the FC where is it reading information from UWB, is it right? Like a fake GPS?

In LPE_FUSION I disabled the GPS flag working in indoor environment.

Thanks

rijesha · March 15, 2018, 3:43pm

Yes that is correct.

OmarMorando · April 13, 2018, 4:05pm

Thanks for yor help.

We are able to do a “mission” passing desired position to reach. The idea is to make a path corresponding to the drawing subdividing it in segment of minimum size of 10 cm (UWB accuracy is 7 cm) when the image is not a smooth trajectory (ex. circle or spline) in order to follow it as accurate as possibile.

The problem is that the drone moves from A to B based on MPC_XY_VEL_MAX (= 0.3 m/s), then when it is in proximity of the desired position it starts to decrease speed, it stops, then it moves very very slow to reach position and, when it is done, it goes to the next desired pos, but this approach it takes some seconds and mouvement is discontinuous.

We would like to have a fluent mouvement during the path because we need to sprying, but I’m not able to find the right params to do that.

rijesha · April 13, 2018, 5:20pm

So if you are sending desired positions the UAV will just try to get to that position no questions asked. There is no settings/parameters for having a fluent movement.

You will need to develop your own scheduler/trajectory controller to do that.

What you could do is continue to stream external position estimation messages, but NOT use offboard mode. Rather use mission mode and send mission commands.
see http://mavlink.org/messages/common and look at all the messages in MAV_CMD and look at the MISSION_ITEM message.

I think mission_item is probably the message you should be sending.