Bench testing MAVLink commanding

[Philip Giacalone]

Hi all,

Question about mavlink. 

I'm writing some java code that sends MAVLink commands to the UDB from a remote PC via the serial port. In order to test this, I need a simple way to verify that the commands are being correctly setup by my code and then executed once received by the UDB. 

What mavlink command(s) are available in MatrixPilot and which ones would be best to use for a simple bench test? As a first test, I'd like to just command a UDB reboot or change the flight mode (to see the LEDs change). Have these commands been implemented and tested? 

I've tried sending an RTL command and a reboot command as follows but they have no apparent effect on the UDB. 

BTW, mavlink serial communication has been verified, since I am successfully receiving and displaying mavlink messages via the same serial port during this test. Command communication appears to be working as well, since I can see the receive light reacting on the 3DR radio as the commands arrive. 

        //send RTL command

        msg_command_long cmd1 = new msg_command_long();

        cmd1.target_system = 0;

        cmd1.target_component = 0;

        cmd1.confirmation = 0;

        cmd1.command = MAV_CMD.MAV_CMD_NAV_RETURN_TO_LAUNCH;

        System.out.println("msg_command_long=" + cmd1);

        result = this.send(cmd1);

        //send autopilot reboot command

        msg_command_long cmd2 = new msg_command_long(); //COMMAND_LONG ( #76 )

        cmd2.target_system = 0; //target all systems

        cmd2.target_component = 0;  //0 for all components

        cmd2.confirmation = 0;

        cmd2.command = MAV_CMD.MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN;   //Command ID, as defined by MAV_CMD enum

        //parameters of the specific command (per the MAV_CMD enum)

        cmd2.param1 = 1; //0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot.

        cmd2.param2 = 1; //0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown

        System.out.println("msg_command_long=" + cmd2);

        result = this.send(cmd2);

        assertTrue(result);

Thanks in advance for any suggestions or advice...

Best regards,

PhilG

[Peter Hollands]

Hi Phil,

I suppose your questions is, "What commands can I send to MatrixPIlot with MAVLink that are actually implemented ?

The ones that I had involvement with are:-

1) Change the rates at which various data streams run.

2) Read and Set Parameters inside of MatrixPilot (e.g. the PID gains, or read the offsets).

Reading the code it looks as if some work has also been done on setting and reading waypoints. I have not tried that code out.

[In the past, I have used LOGO to read the status of a switch on my transmitter (read a channel),

and then the LOGO routines perform different Flight Plans, depending on the swtich on my transmitter.

Oher autopilots do not have a dynamic flight control language so they cannot do that]

MAVlink Commands such as "Reboot" and "RTL" are not implemented yet in MatrixPilot.

In terms of debugging your java code, the best approach, is to print an ascii string when a command should be activated.

You can see that Robert has enabled this in many places with a macro called DPRINT(....).  I think that if you are using the AUV3, you can arrange for the DPRINT,

to come out on it's own serial channel, which would be ideal. 

I'm not sure which version of the code your are using. Trunk, or WJP Helical Turns branch, or some earlier version.

Best wishes, Pete

--
You received this message because you are subscribed to the Google Groups "uavdevboard-dev" group.
To unsubscribe from this group and stop receiving emails from it, send an email to uavdevboard-d...@googlegroups.com.
For more options, visit https://groups.google.com/d/optout.

[Philip Giacalone]

Hi Pete,

I'm running the latest wjp helical turns branch from the Google repository on the UDB4.

I'll take a look at the code areas you mentioned later today.

Changing data stream rates is something I should be able to remotely see/verify, since I'm receiving telemetry during these test runs. This may be the easiest first test.

Yes, it would be helpful to print the commands to the console in real time for debugging. Can Robert's DPRINT code be used on the UDB?

Once I've got basic commanding working, dynamic logo will be of great interest.

Thanks very much for your help!

PhilG

[Robert Dickenson]

"Have these commands been implemented and tested? "

No. but i have started..

"Can Robert's DPRINT code be used on the UDB? "

Yes. but you have to choose which port to give up for a console connection. ie. gps or telemetry. with an AUAV3 i use the third uart and have all three communications channels at once, albeit a tad tricky at times on the PC end with so many virtual serial ports..

"dynamic logo will be of great interest. "

have you looked at my logo work as featured in the beta branch?

and could you please switch from using the _wjp_helicalTurns and try working with _helicalTurns instead?

you may very well find a few problems along the way, but the feedback will be very much appreciated.

Kind regards, Robert.

[Philip Giacalone]

Hi Robert,

Thanks for those suggestions and pointers.

Right now, I've got a bunch of 'balls in the air'. At the moment, I'm trying to keep things simple and prove my java/MAVLink commanding code works -- as a first step. I'd prefer to use MAVLink commands that are already proven to work on the UDB/MatrixPilot. It sounds like the telemetry rate commands Pete mentioned have been proven (please correct me if I'm wrong), so I'll want to start with those. 

I like the idea of moving to the AUAV3, since it can better support multiple serial paths. Phil K was kind enough to provide an AUAV3 during the NASA Challenge (thanks, Phil!), so I've started setting that up. But once again, I'd prefer to go with my existing setup and just prove the code first. Hoping that can be done reasonably quickly. 

Then I can move ahead with the AUAV3, multiple serial channels, other branches, dynamic LOGO, etc. I definitely understand your desire for feedback, but not quite ready to introduce other variables right now. Sorry...

Best regards,

Phil

[Philip Giacalone]

Pete,

So far my attempts to change the stream rates via MAVLink are not working. I'm using message type "msg_request_data_stream" (message ID #66). Is this the correct message type to use? Also, has this method been proven to change the rates when called remotely? 

The java code I'm using is shown below. I'm hoping that the problem is simply a missing or incorrect message attribute. Is there a working code example that sets up this same remote message? 

The resulting message rates are shown below. The msgId shown before the "=" sign and the message count after. As you can see, the rates are about the same both before and after the STOP command is sent. 

As the code shows, I'm trying to simply stop the messages by sending 0 for the start_stop flag, but the command has no effect. I tried targeting MAV_DATA_STREAM_POSITION message (which should effect msgId #33). I also tried targeting MAV_DATA_STREAM_ALL. Still no effect. 

Before STOP CMD: 0=19, 1=21, 24=10, 27=10, 30=39, 33=198, 35=10, 74=40, 

After STOP CMD : 0=20, 1=20, 24=10, 27=10, 30=40, 33=199, 35=10, 74=40

Here are some code snippets that show the key lines. See the send() method below to see how the bytes are created. This method relies on generated mavlink code to pack() and encode() the bytes prior to sending them out the serial port. 

Thanks!

Phil

        //turn ON stream rates

        msg_request_data_stream ds1 = new msg_request_data_stream();

        ds1.start_stop = 1;             //1 to start sending, 0 to stop sending.

        ds1.req_message_rate = 10;      //interval between two messages of this type

        ds1.req_stream_id = MAV_DATA_STREAM.MAV_DATA_STREAM_POSITION;

        ds1.target_component= MAV_COMPONENT.MAV_COMP_ID_ALL;

        ds1.target_system = 0;

        System.out.println("1) msg_request_data_stream [START]=" + ds1);

        result = this.send(ds1);

        System.out.println("1) SENT [START] MESSAGE, result=" + result);

        //turn OFF stream rates

        msg_request_data_stream ds2 = new msg_request_data_stream();

        ds2.start_stop = 0;             //1 to start sending, 0 to stop sending.

        ds2.req_message_rate = 0;

        ds2.req_stream_id = MAV_DATA_STREAM.MAV_DATA_STREAM_POSITION;

        ds1.target_component= MAV_COMPONENT.MAV_COMP_ID_ALL;

        ds2.target_system = 0;

        System.out.println("2) msg_request_data_stream [STOP]=" + ds2);

        result = this.send(ds2);

        System.out.println("2) SENT [STOP] MESSAGE, result=" + result);

    public boolean send(MAVLinkMessage messageToUav) throws SerialPortException {

        boolean result = false;

        if (messageToUav != null){

            MAVLinkPacket packet = messageToUav.pack();

            byte[] bytes = packet.encodePacket();

            result = this.serialPort.writeBytes(bytes);

        }

        return result;

    }

[Peter Hollands]

Here is a message that I meant to post here a few minutes back, before Phil's latest post.

Phil,

I have just done some testing of MAVLink with a UDB4, QGroundControl to and the latest version of branch matrixPilot_wjp_helicalTurns.

I have proved that the ability to set the rate of the raw_imu data stream from QgroundControl is working. On starting up,

the IMU_RAW sensor rate is set by mavlink_options.h at 

#define MAVLINK_RATE_RAW_SENSORS            4

i.e. 4 Hz.

At the same time, using the Analyze Perspective, I have M1:RAW_IMU.zacc displaying the noise on the accelerometer.

Then in the On board Parameters window (widget) of QGroundControl, I am able to set the Raw Sensor value to 40.

It is then easy to see the graph of the noise for the Z accelerometer moving from 4hz up to 40Hz, which proves that

the UDB is receiving the command to change the data rate on that stream.

So I hope that can give you a firm foundation on which to run an initial test.

Setting and receiving parameters (e.g. PID parameters) worked most of the time. But a couple of times I had to re-request the parameters from QGroundControl.

As for using DPRINT and #define USE_DEBUG_IO in optons.h ; That did not work out so well for me.

I set the system to try and send text debug messages back in the mavlink stream, and it confused QGroundControl.

So I cannot recommend that route myself for now. 

Best wishes, Pete

[Philip Giacalone]

Ok, that tells me which message to target as a first test (MAVLINK_RATE_RAW_SENSORS). Thanks for taking the time to verify the functionality. 

What I could _really_ use now is the underlying code from QGroundControl so I can see exactly how it is setting up the MAVLink command message prior to sending it. If anyone can point me to it, I'd be very grateful. 

I recall that Tom and I struggled mightily with changing the message rates on a prior project. We never were able to get it working. It seems simple enough but there is some ingredient missing. The question is what? 

For now, I'll try again by targeting this message...

Best regards,

Phil

[Peter Hollands]

Phil,

Please can you check that your sysid (mavlink system id of the UDB / plane) is set correctly and that the same ID is used in your java code as the sys id that you have set in options.h.

We originally had a sysid of 055 in options.h. But it turns out that for most Ardupilot GCS systems, a sysid of 001 in options.h is better.

Looking at your code above, you seem to be using a sysid of 0.

ds1.target_system = 0;

In option.h the relevant field is 

#define MAVLINK_SYSID                       1

Best wishes, Pete

[Philip Giacalone]

Will do, thanks. I'll let you know...

[Robert Dickenson]

You may want to search your codebase for MAVLINK_SYSID, as not so long ago i started making a move towards having it live in mavlink_options.h instead. If it is apparent more than once, just ensure they are defined the same.

May I also suggest connecting a serial cable to your GPS port and defining CONSOLE_UART to 1 (making your GPS port the console). Then you can sprinkle printf's throughout the MAVLink packet handling to get some visibility on what's going on. Of course you'll never get a GPS lock, but you should be able to exercise much of the MAVLink code regardless.

[Philip Giacalone]

Great advice, Robert. I'll likely do both. 

Thanks,

Phil

[Philip Giacalone]

Pete,

I'm happy to report that MAVLink commanding is working. Hooray! The relevant java client code is posted below. 

Here is the output, which reflects the correct change in message rates for the raw sensor messages (#24, 27 & 35). 

Initial message counts: 0=19, 1=20, 24=10, 27=10, 30=49, 33=198, 35=9, 74=50, 

Final message counts  : 0=20, 1=20, 24=19, 27=20, 30=50, 33=200, 35=20, 74=50

Other tests showed that the start_stop flag is also working as expected. Nice.

Note that message field "target_component" was not required/populated. I suppose the mavlink code in MatrixPilot is not checking for that field, is that correct? 

Thank you very much for your help with this! 

Best regards,

Phil

    public void testSendCommands() throws Exception {

        //============ set the stream rate ============

        msg_request_data_stream cmd = new msg_request_data_stream();

        cmd.target_system = 1;          //match to value of MAVLINK_SYSID in options.h file

//        cmd.target_component = MAV_COMPONENT.MAV_COMP_ID_ALL;   //not required by MatrixPilot

        cmd.req_stream_id = MAV_DATA_STREAM.MAV_DATA_STREAM_RAW_SENSORS;     //ID of requested data stream

        cmd.start_stop = 1;             //1 to start sending, 0 to stop sending.

        cmd.req_message_rate = 2;       //the requested message rate in messages/second

        System.out.println("1) SENDING: msg_request_data_stream=" + cmd);

        boolean result = this.send(cmd);

        System.out.println("1) SENT, result=" + result);

        //sleep while we receive and count messages arriving via method onMAVLinkMessage()

        int seconds = 5;

        this.sleep(seconds);

        //============ now change stream rate ============

        cmd.start_stop = 1;             //1 to start sending, 0 to stop sending.

        cmd.req_message_rate = 4;       //the requested message rate in messages/second

        System.out.println("2) SENDING: msg_request_data_stream=" + cmd);

        result = this.send(cmd);

        this.after = true;

        System.out.println("2) SENT, result=" + result);

        //sleep while we receive and count messages arriving via method onMAVLinkMessage()

        this.sleep(seconds);

        //print out the resulting list of message counts, listed by MAVLink msgId

        System.out.println("Initial message counts: " + this.toString(this.beforeCountMap, true));

        System.out.println("Final message counts  : " + this.toString(this.afterCountMap, true));

    }

    /**

     * Sends the given MAVLinkMessage to the UAV via this serial port

     */

[Peter Hollands]

Hi Phil,

Glad you made progress.

Yes, the code for datastream changes is only checking for the sysid not the component id.

// QgroundControl sends data stream request to component ID 1, which is not our component for UDB.

if (packet.target_system != mavlink_system.sysid) return;

The component ID check was failing, as QGroundControl was not taking note of the component ID of the UDB. 

As we only have one component currently in our planes, (The UDB or AUAV3), I simply opted to ignore the component ID and our code will respond to any component ID within the plane.

That change was made a couple of years ago. I expect QGroundControl has been fixed since then.

Best wishes, Pete

[Philip Giacalone]

Thanks for that info. Sounds good. 

I have to say that slight improvements to MAVLink documentation would make it easier to use (https://pixhawk.ethz.ch/mavlink/). Note, for example, how the target_system and target_component definitions are identical below. Also, the definition for req_message_rate  is incorrect. If the MatrixPilot implementation is correct, then it should be simply defined as "the desired message rate in messages/second". The documentation page says it is a generated document, so it would probably be pretty easy to edit the source file containing these definitions. 

But enough venting about things outside our control. Time to move-on happily. 

Thanks again,

Phil

REQUEST_DATA_STREAM ( #66 )

Field Name	Type	Description
target_system	uint8_t	The target requested to send the message stream.
target_component	uint8_t	The target requested to send the message stream.
req_stream_id	uint8_t	The ID of the requested data stream
req_message_rate	uint16_t	The requested interval between two messages of this type
start_stop	uint8_t

1 to start sending, 0 to stop sending.

[Mark Whitehorn]

Phil,

not completely outside your control :)
https://github.com/mavlink/mavlink/issues

--Mark

Mark Whitehorn
kd0...@gmail.com

[Philip Giacalone]

Thanks for the link, Mark. I'll have to collect suggestions and submit them.

Note that I see from this post that the rate units for the request_data_stream message is confusing to others, too. This post implies it's a period rather than a frequency!

https://github.com/mavlink/mavlink/issues/347

A popular protocol really needs an unambiguous specification. My unit test will pass for MatrixPilot and fail on other autopilots if behaviors are inconsistent. We end up with client code that's dependent on the specific autopilot. Ugly.

Best regards,
Phil

[Tom Pittenger]

It's a good thing it's an open source protocol where you have the power to fix the problem yourself.

[Philip Giacalone]

Good point, Tom.

[Philip Giacalone]

FYI. Tests of mavlink commanding with the Pixhawk today show that the APM implementation also defines field req_message_rate as the message rate as a frequency, in messages/second. 

Best regards,

PhilG