Giter Site home page Giter Site logo

mavlink's Introduction

This project is not actively maintained

At present time and for a long while now, unfortunately, I have not been able to put time and love into dronefleet/mavlink. I may provide quick help within open issues, but it's hard to promise much more than that.

If you maintain a fork of this project that's in reasonable condition for users to depend on, please open an issue and I'll link to your repository from this README.

Overview

A Java SDK for communication using the Mavlink1 and Mavlink2 protocols.

Structure

The project is made up of 3 components:

mavlink-protocol

A low-level API which deals with reading and writing packets. It does not have any knowledge of dialects or about the meaning of messages. It does, however, provide infrastructure for packet CRC validation as well as packet signing. The required implementation or session-specific information (such as message CRC extra and signing parameters) have to be provided, though. If you are after making your own high-level design then this is likely the choice for you. If you would like to use mavlink-protocol without the rest of the components of this repository, then you can read mavlink-protocol's README to learn how.

generator gradle plugin

Used in order to generate dialect-specific sources. The generated sources depend upon the classes of this root project, and therefore the plugin is unlikely to be of any use as a stand-alone -- If you are writing your own dialect XML files, then forking this project is likely what you're after.

mavlink (root project)

A higher level API which provides its users with a complete abstraction from the lower level protocol. This is likely what you want if you're after making an application that communicates with Mavlink devices.

Get it

Maven Central coordinates are io.dronefleet.mavlink:mavlink:1.1.11

Examples

Brief

This is a brief example for the minimum required to communicate using Mavlink1 with the default dialects.

// Reading
MavlinkConnection connection = MavlinkConnection.create(in, out); // InputStream, OutputStream
MavlinkMessage message;
while ((message = connection.next()) != null) {
    // ...
}

// Writing
connection.send1(
        255, /* systemId */
        0, /* componentId */
        Heartbeat.builder()
             .type(MavType.MAV_TYPE_GCS)
             .autopilot(MavAutopilot.MAV_AUTOPILOT_INVALID)
             .systemStatus(MavState.MAV_STATE_UNINIT)
             .mavlinkVersion(3)
             .build()));

Reading & disambiguating messages

This is a detailed example of how to use the API to read and write messages.

// This example uses a TCP socket, however we may also use a UDP socket by injecting
// PipedInputStream/PipedOutputStream to MavlinkConnection, or even USB by using any
// implementation that will eventually yield an InputStream and an OutputStream.
try (Socket socket = new Socket("127.0.0.1", 5760)) {
    // After establishing a connection, we proceed to building a MavlinkConnection instance.
    MavlinkConnection connection = MavlinkConnection.create(
            socket.getInputStream(), 
            socket.getOutputStream());

    // Now we are ready to read and send messages.
    MavlinkMessage message;
    while ((message = connection.next()) != null) {
        // The received message could be either a Mavlink1 message, or a Mavlink2 message.
        // To check if the message is a Mavlink2 message, we could do the following:
        if (message instanceof Mavlink2Message) {
            // This is a Mavlink2 message.
            Mavlink2Message message2 = (Mavlink2Message)message;
            
            if (message2.isSigned()) {
                // This is a signed message. Let's validate its signature.
                if (message2.validateSignature(mySecretKey)) {
                    // Signature is valid.
                } else {
                    // Signature validation failed. This message is suspicious and
                    // should not be handled. Perhaps we should log this incident.
                }
            } else {
                // This is an unsigned message.
            }
        } else {
           // This is a Mavlink1 message.
        }
        
        // When a message is received, its payload type isn't statically available.
        // We can resolve which kind of message it is by its payload, like so:
        if (message.getPayload() instanceof Heartbeat) {
            // This is a heartbeat message
            MavlinkMessage<Heartbeat> heartbeatMessage = (MavlinkMessage<Heartbeat>)message;
        }
        // We are better off by publishing the payload to a pub/sub mechanism such 
        // as RxJava, JMS or any other favorite instead, though.
    }
} catch (EOFException eof) {
    // The stream has ended.
}

Writing Mavlink 2 messages

int systemId = 255;
int componentId = 0;
Heartbeat heartbeat = Heartbeat.builder()
          .type(MavType.MAV_TYPE_GCS)
          .autopilot(MavAutopilot.MAV_AUTOPILOT_INVALID)
          .systemStatus(MavState.MAV_STATE_UNINIT)
          .mavlinkVersion(3)
          .build()

// Write an unsigned heartbeat
connection.send2(systemId, componentId, heartbeat);

// Write a signed heartbeat
int linkId = 1;
long timestamp = /* provide microsecond time */;
byte[] secretKey = MessageDigest.getInstance("SHA-256")
                       .digest("a secret phrase".getBytes(StandardCharsets.UTF_8))
connection.send2(systemId, componentId, heartbeat, linkId, timestamp, secretKey);

mavlink's People

Contributors

benbarkay avatar h4writer avatar lmar avatar thekeveloper avatar

Stargazers

 avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar

Watchers

 avatar  avatar  avatar  avatar  avatar  avatar  avatar  avatar

mavlink's Issues

Any command execution fails

Hi,
I cannot understand the correct set of commands I have to send to PX4 to takeoff.
I'm working on a local windows machine with PX4 and JMAV simulator.
Communication works fine, but I always receive a MAV_RESULT_FAILED.

Here is my simplest example:

	UdpConnector udp = new UdpConnector(InetAddress.getLocalHost(), 14570, 14550);
	final MavlinkConnection connection = MavlinkConnection.create(udp.getInputStream(), udp.getOutputStream());
	new Thread() {
		@Override
		public void run() {
			while (true) {
				try {
					connection.send1(255, 0,
							Heartbeat.builder().type(MavType.MAV_TYPE_GCS).autopilot(MavAutopilot.MAV_AUTOPILOT_PX4)
									.systemStatus(MavState.MAV_STATE_UNINIT).mavlinkVersion(1).build());
					Thread.sleep(1000);
				} catch (Exception e) {
					e.printStackTrace();
				}
			}
		}
	}.start();
	connection.send1(255, 0, CommandInt.builder().command(MavCmd.MAV_CMD_COMPONENT_ARM_DISARM).param1(1).build());

	MavlinkMessage message;
	while ((message = connection.next()) != null) {
		Object p = message.getPayload();
		if (p instanceof Heartbeat || p instanceof CommandAck)
			System.out.println("" + message.getSequence() + " --> " + p);
	}

the output is:

67 --> CommandAck{command=EnumValue{value=400, entry=MAV_CMD_COMPONENT_ARM_DISARM}, result=EnumValue{value=4, entry=MAV_RESULT_FAILED}, progress=0, resultParam2=0, targetSystem=0, targetComponent=0}
176 --> Heartbeat{type=EnumValue{value=2, entry=MAV_TYPE_QUADROTOR}, autopilot=EnumValue{value=12, entry=MAV_AUTOPILOT_PX4}, baseMode=EnumValue{value=81, entry=null}, customMode=65536, systemStatus=EnumValue{value=3, entry=MAV_STATE_STANDBY}, mavlinkVersion=3}

cannot understand where is the problem.. is there any specific handshare or command sequence?

thanks

Mario

Might be good for message ID to be accessable at the MavlinkMessage/MavlinkMessage2 level and to have a list of ID's (static or enum) to compare against

It seems to me that it would be desirable to make the message ID of the payload available without having to cast; possible make the ID an iVAR of MavlinkMessage/MavlinkMessage2. The ID could be the integer value or more likely you would want to make it an enum. Either way, the entire list of ID's should be also made available so that some arbitrary message ID can be determined from that list. The most useful reasons for this is so that a user can use a case statement in their message handler method. Message handling is suggested to be done either by using a bunch of "if" statements comparing the class of the received message to each of the known classes (as described in the readme):

if (message.getPayload() instanceof Heartbeat) {
	// This is a heartbeat message
	MavlinkMessage<Heartbeat> heartbeatMessage = (MavlinkMessage<Heartbeat>)message;
}

But a more interesting way that was suggested was to use some form of pub/sub mechanism, like RxJava2 for example:

Observable<Object> observable = PublishSubject.create();

// this is where you publish an incoming message
while ((message = connection.next()) != null) {
    observable.onNext(message.getPayload()); 
}
observable.onComplete();

// this is where you subscribe to messages
observable.ofType(Heartbeat.class)
	.subscribe((heartbeat) -> {
        // do whatever you want with the heartbeat here.
    });

And it was suggested that even though a case statement would be more efficient that there are far larger bottlenecks involved that make the case statements impact insignificant. However, it would still be easier for some users to simply be able to use a case statement to handle messages like:

MavlinkConnection connection = MavlinkConnection.create(in, out); // InputStream, OutputStream
MavlinkMessage message;
while ((message = connection.next()) != null) {
	switch (message.Id) { //<=== No such IVar so cannot do this
		case MAVLINK_MSG_ID_HEARTBEAT : { //MAVLINK_MSG_ID_HEARTBEAT = 0
			//handle heartbeat
			Heartbeat tempmsg = (Heartbeat) message.getPayload();
			...
			...
			} break;
		case MAVLINK_MSG_ID_SYS_STATUS : { //MAVLINK_MSG_ID_SYS_STATUS = 1
			//handle system status
			} break;
		...
		...
		...
		case MAVLINK_MSG_ID_ATTITUDE_CONTROL : { //MAVLINK_MSG_ID_ATTITUDE_CONTROL = 200
			//handle attitude control
			} break;
		default: {
			System.out.println("MSG Error : unknown MsgId (" + myMsgID + ")");
			}
	}
}

There are other areas of the code that might also be made easier to implement if the message ID and list of message IDs were available. For instance, when creating a hierarchical list box of all incoming messages the message ID could be used as the unique identifier to determine whether a new message is to be added to the hierarchy or if an existing message needs to be updated. All of these uses have work arounds, but if it easy enough to add this extra functionality then maybe it would be useful to do so, if not to simply allow the users to code their projects in whichever way they are most comfortable.

MavlinkFieldInfo annotations effected by Comparator definition using Eclipse only IDE

The MavlinkFieldInfo annotations do not seem to be working when using Eclipse only IDE. The problem actually turns out to be an issue with the Comparator in the MavlinkFieldInfo interface. As it is currently defined the Comparator prevents access to the annotations. I do not know why this is the case because the code looks fine to me. However, if I modify the Comparator declaration within the MavlinkFieldInfo interface then everything works fine (as below):

package io.dronefleet.mavlink.annotations;

import java.lang.annotation.ElementType;
import java.lang.annotation.Retention;
import java.lang.annotation.RetentionPolicy;
import java.lang.annotation.Target;
import java.util.Comparator;

@Retention(RetentionPolicy.RUNTIME)
@Target({ElementType.METHOD})
public @interface MavlinkFieldInfo {	
	
	Comparator<MavlinkFieldInfo> WIRE_COMPARATOR = new Comparator<MavlinkFieldInfo>() {
		@Override
		public int compare(MavlinkFieldInfo a, MavlinkFieldInfo b) {
		    if (a.extension() && !b.extension()) {
		        return 1;
		    }
		    if (!a.extension() && b.extension()) {
		        return -1;
		    }
		
		    if (!a.extension() && a.unitSize() != b.unitSize()) {
		        return b.unitSize() - a.unitSize();
		    }
		    
		    return a.position() - b.position();
		};
	};
	
    int position();
    int unitSize();
    boolean signed() default false;
    int arraySize() default 0;    
    Class<?> enumType() default void.class;    
    boolean extension() default false;    
    String description() default "No description provided";
}

If this above code works in the Maven and Gradle environments then maybe make that change so that it also works without much trouble in the Eclipse only IDE environment. Obviously it is better to distribute the code by Maven, but in some offline development environments Maven or Gradle are difficult to manage, so it would be good for the code to also work within the Eclipse only IDE environment (particularly if it is easy enough to make it so).

Support updating systemDialects directly

Currently the system dialects can only be updated by reading heartbeat during running dynamically. It will be nice to expose a public method to update system dialects directly. This is because some systems may not send heartbeat at all.

Serialization unit tests fail (Mac High Sierra and Java 1.8)

I have been having problems with the serialization system. In an attempt to "start fresh", I did the following:

  1. Downloaded 1.0.10 from the "Releases" tab here on the main project and unzipped
  2. gradle init
  3. gradle build
  4. gradle test

I got the following error message:
io.dronefleet.mavlink.serialization.payload.reflection.ReflectionPayloadSerializationTest > test FAILED
java.lang.AssertionError at ReflectionPayloadSerializationTest.java:26

1 test completed, 1 failed
:test FAILED

I am doing this on a Macbook Pro, Mac OS X High Sierra (64 bit) with Java JRE build 1.8.0_91-b14

I got the same error message when doing a "gradle test" on my Github checkout of 1.0.9. The pull request I submitted about a "null pointer exception" I am sure is related to this issue.

Unable to get any kind of data from UDP connection

Hi,
I have an issue trying to get data with UDP connection to an arducopter >= 3.7.

I'm following these steps:

  • create mavlink connection
  • start to send CGS Heartbeat
  • request Data Stream
  • get messages from mavlinkConnection

I set up some wrapper and functions but these steps can be simplified in this way.

---create mavlink connection

Socket socket = new Socket(host, port, false);
socket.setSoTimeout(10000);

MavlinkConnection connection = MavlinkConnection.builder(socket.getInputStream(), socket.getOutputStream())
          .dialect(MavAutopilot.MAV_AUTOPILOT_GENERIC, new StandardDialect())
          .dialect(MavAutopilot.MAV_AUTOPILOT_ARDUPILOTMEGA, newArdupilotmegaDialect()).build();

  • start to send CGS Heartbeat (sent every second)
      Heartbeat msg = Heartbeat.builder()
          .type(MavType.MAV_TYPE_GCS)
          .autopilot(MavAutopilot.MAV_AUTOPILOT_INVALID)
          .systemStatus(MavState.MAV_STATE_UNINIT)
          .mavlinkVersion(3)
          .build();
      try {
        connection.send2(255, 1, msg);
      } catch (IOException e) {
        Logger.logException(TAG, e);
      }
  • request Data Stream
int heartbeatStreamId = 0;
int batteryStatusStreamId = 147;
CommandLong hbCmd = commandLongBuilder.command(MavCmd.MAV_CMD_SET_MESSAGE_INTERVAL).param1(heartbeatStreamId).param2(0).targetSystem(0).targetComponent(0).build();
CommandLong batteryCmd = commandLongBuilder.command(MavCmd.MAV_CMD_SET_MESSAGE_INTERVAL).param1(heartbeatStreamId).param2(0).targetSystem(0).targetComponent(0).build();
connection.send2(255, 1, hbCmd);
connection.send2(255, 1, batteryCmd);
  • get messages from mavlinkConnection
      while (mavlinkConnection != null) {
        MavlinkMessage message = null;
        try {
          message = mavlinkConnection.next();
          ......
        } catch (Throwable e) {
          message = null;
          ......
        }
      }

I never get any kind of message from this connection, I always get timeout.
What am I doing wrong?

Is there any issue in the create of mavlink connection?
Do I have to do something else to start the mavlink connection in addition to sending the heartbeat and requesting the data stream?

Regarding the connection, I know that this kind of socket constructor is deprecated but I don't know any other way to create a mavlink connection with a UDP socket.
Can you suggest a better one?

Many thanks in advance

Access to message fields name/values at the MavlinkMessage/MavlinkMessage2 level (like toString())

In my code I have a hierarchical list box that I use to allow the user to see the current field values for each message the program receives (whether the program processes the message or not). You can see this same type of structure in QGroundControl and in Mission Planner where they allow you to view the Mavlink messages as they come in (building the structure with each message or updating existing messages). There is a possibility that this heirarchy could be more than just 2 levels, but for my purposes I just list off the message name/ID and then under each of those I list off each field name/value (fitting each into a single string/line). So, for my purpose I just created a ArrayList where the first element of the list is the message name/ID and the subsiquent elements are the field name/values. There are likely better ways of doing this, particularly since some fields may have further sub-fields (like the enum fields).

The problem is that this library only accomodates a call to toString for some arbitrary MavlinkMessage. It would be nice if the MavlinkMessage would also give me the information in a more managable way for displaying it in such a hierarchical list box; maybe as XML or some such. For instance, I might want to add code similar to the following to each of the message classes (in this instance it is for Attribute):

public ArrayList<MAVLinkFieldPair> getMAVLinkFields() {	
	ArrayList<MAVLinkFieldPair> mlf = new ArrayList<MAVLinkFieldPair>();

	mlf.add(new MAVLinkFieldPair("MAVLINK_MSG_ID_ATTITUDE", String.valueOf(MAVLINK_MSG_ID_ATTITUDE)));
	mlf.add(new MAVLinkFieldPair("time_boot_ms", String.valueOf(time_boot_ms)));
	mlf.add(new MAVLinkFieldPair("roll", String.valueOf(roll)));
	mlf.add(new MAVLinkFieldPair("pitch", String.valueOf(pitch)));
	mlf.add(new MAVLinkFieldPair("yaw", String.valueOf(yaw)));
	mlf.add(new MAVLinkFieldPair("rollspeed", String.valueOf(rollspeed)));
	mlf.add(new MAVLinkFieldPair("pitchspeed", String.valueOf(pitchspeed)));
	mlf.add(new MAVLinkFieldPair("yawspeed", String.valueOf(yawspeed)));
	    
	return(mlf);	
}

As it is now there is no such methods, so the only way to get to this information (without casting the message, etc) is to parse the result of the toString call. For instance, I could use a function like this to give me similar results:

public void updateMAVLinkMessage(int pID, String pST) {
	ArrayList<MAVLinkFieldPair> mlf = new ArrayList<MAVLinkFieldPair>();

	int fPos1 = pST.indexOf('{');
	int fPos2 = pST.lastIndexOf('}');

	mlf.add(new MAVLinkFieldPair(pST.substring(0, fPos1), String.valueOf(pID)));
	
	String[] fields = pST.substring(fPos1+1, fPos2).split(",(?![^\\{]*[\\}])");
	
	for (int i=0; i<fields.length; i++) {
		fPos1 = fields[i].indexOf('{');
		if (fPos1<0) {
			fPos2 = fields[i].indexOf('=');
			mlf.add(new MAVLinkFieldPair(fields[i].substring(0, fPos2).trim(), String.valueOf(fields[i].substring(fPos2+1).trim())));								
		} else {
			fPos2 = fields[i].indexOf('=');
			String tempST1 = fields[i].substring(0, fPos2).trim();
			String tempST2 = "[";

			fPos2 = fields[i].lastIndexOf('}');
			String[] enumf = fields[i].substring(fPos1+1, fPos2).split(",");
			for (int j=0; j<enumf.length; j++) {
				if (j>0) tempST2 = tempST2 + ", ";

				fPos2 = enumf[j].indexOf('=');
				tempST2 = tempST2 + String.valueOf(enumf[j].substring(fPos2+1).trim());
			}
			tempST2 = tempST2 + "]";
			mlf.add(new MAVLinkFieldPair(tempST1, tempST2));							
		}
	}

	updateMAVLinkMessage(mlf);
}

Of course this is a bit messy and something that would need to be managed whenever there is any updates to the library (that might effect the toString format). I am not even sure that I have covered the format of all messages (only ones I have run into so far). So, it might be a good idea to have some way to get the message field values without having to cast the message (like is done with toString).

Unable to receive mavlink messages requested through MAV_CMD_SET_MESSAGE_INTERVAL

I followed the instruction given on readme file and successfully able to connect to vehicle(ardupilot sitl) via tcp 127.0.0.1:5760. I am able to receive the heartbeat(mavlink verison3). However when I send,

> commandLong = CommandLong.builder().command(MAV_CMD_SET_MESSAGE_INTERVAL).confirmation(0).param1(147).param2(0).param7(1).targetSystem(1).targetComponent(1).build();
         
> connection.send2(systemId, componentId, commandLong);

I don't receive the corresponding message in reply. I have already set the serial type parameter in vehicle to mavlink2. May I know is there anything I am missing apart from what I am doing?
Thanks!
Sanish

MavlinkSerializationException for supported field type: BigInteger

I'm currently using the Ping protocol to send ping a message to a connected drone. There seems to be a problem with serializing the BigInteger timeUsec in the Ping.java. The stack trace says MavlinkSerializationException: unrecognized field type java.math.BigInteger, yet the source of that exception, ReflectionPayloadSerializer.java, does handle BigInteger types. See stack trace below:

io.dronefleet.mavlink.serialization.MavlinkSerializationException: unrecognized field type java.math.BigInteger
at io.dronefleet.mavlink.serialization.payload.reflection.ReflectionPayloadSerializer.lambda$serialize$6(ReflectionPayloadSerializer.java:73)
at io.dronefleet.mavlink.serialization.payload.reflection.-$$Lambda$ReflectionPayloadSerializer$MYJl8X3lgsDBIeb9xszPJjyTJJk.accept(Unknown Source:10)
at java.util.stream.ForEachOps$ForEachOp$OfRef.accept(ForEachOps.java:184)
at java.util.stream.-$$Lambda$Abl7XfE0Z4AgkViLas9vhsO9mjw.accept(Unknown Source:2)
at java.util.ArrayList.forEach(ArrayList.java:1262)
at java.util.stream.SortedOps$RefSortingSink.end(SortedOps.java:390)
at java.util.stream.Sink$ChainedReference.end(Sink.java:259)
at java.util.stream.AbstractPipeline.copyInto(AbstractPipeline.java:483)
at java.util.stream.AbstractPipeline.wrapAndCopyInto(AbstractPipeline.java:472)
at java.util.stream.ForEachOps$ForEachOp.evaluateSequential(ForEachOps.java:151)
at java.util.stream.ForEachOps$ForEachOp.evaluateSequential(ForEachOps.java:133)
at java.util.stream.AbstractPipeline.evaluate(AbstractPipeline.java:235)
at java.util.stream.ReferencePipeline.forEach(ReferencePipeline.java:419)
at io.dronefleet.mavlink.serialization.payload.reflection.ReflectionPayloadSerializer.serialize(ReflectionPayloadSerializer.java:49)
at io.dronefleet.mavlink.MavlinkConnection.send(MavlinkConnection.java:308)

I build a Mavlink message containing the ping in the code below, but the exception does not occur until I actually send this message over the MavlinkConnection:
new MavlinkMessage<>(255, 0, Ping.builder() .timeUsec(BigInteger.valueOf(System.currentTimeMillis())) .seq(Long.valueOf(seq)) .targetSystem(droneId) .targetComponent(0) .build())

I looked at where the MavlinkSerializationException is thrown in ReflectionPayloadSerializer.java:78 and there is a check for the BigInteger type before throwing that exception.

Payload length is limited to 253

When serializing (ReflectionPayloadSerializer) an FILE_TRANSFER_PROTOCOL message the exception "IllegalStateException: payload length > 253 for messageio.dronefleet.mavlink.common.FileTransferProtocol" is thrown but the FILE_TRANSFER_PROTOCOL has a length of 254. Shouldn't have a payload a maximum length of 255 bytes?

[General guidance] Documentation for using the generator gradle plugin

Hi,
I have the MavLink project(io.dronefleet.mavlink:mavlink:1.1.8) imported into my Android project but I am not seeing classes for parsing or generating messages. Guessing I need to generate these using the generator gradle plugin.

Do you have any documentation or examples on how I should use the plugin?

No such file or directory #include "common/mavlink.h"

Hi,
I tried to add some xmls into directory 'definition-xml' and run task 'generateDialect'.And the error occurred when I build the 'mavlink-1.1.8.jar'.It shows No such file or directory #include "common/mavlink.h".Is there any idea to solve this?
Thanks.
image

Trouble arming my Rover using MAVLink

I am building a GCS application on Android and I am using the dronefleet library to create and parse MAVLink messages.

I am having some trouble arming my rover using the application. I am using the command int message-> MAV_CMD_COMPONENT_ARM_DISARM.

This is the serial message I am sending:

[254, 35, 12, 255, 0, 144, 0, 0, 128, 63, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 144, 1, 0, 0, 0, 0, 0, 18, 3]

The rover arms when I use an RC transmitter to arm it, so I know it is not failing any safety checks.

Questions:

How do I arm ArduRover using MAVLink messages?

Mission Upload

Hi, I am trying to upload a mission to a simulated drone with your great library. While doing this, I followed the instructions under mavlink.io/services/mission. But I continuously getting time out error from the drone side. I think the reason for this initially I send drone mission_count message with value 4 and waiting for mission_request_messages with seq number in a range of 1...4/0...3. But I get a seq number like 123, 196, etc. Thus I do not send responses and get time out error. My work environment build-up from the following components;

  • px4 with gazebo simulator (docker)
    -QGC for visual representation
    -java application based on dronfleet library for mavlink communication
    My question is it wrong waiting mission seq number in seq field in mission_request message if yes how can I know the requested mission item? And also px4 auto_mission mode does not exist in mavlink common messages how can I send mav_cmd_do_set_mode for setting mission mode?
...
if(message.getPayload() instanceof MissionAck){
        MavlinkMessage<MissionAck> missionAck = (MavlinkMessage<MissionAck>)message;
        System.out.println("This is a missionAck message with following values: \n" + missionAck.toString());
        if(missionAck.getPayload().type().entry().equals(MavMissionResult.MAV_MISSION_ACCEPTED)){
                missionUploadFinished = true;
         }
}
...
while(!missionUploadFinished){
            if(requestedMissionItem != transmittedMissionItem){
                 MissionItem missionItem = MissionItem.builder()
                        .targetSystem(1)
                        .targetComponent(1)
                        .seq(requestedMissionItem)
                        .frame(MavFrame.MAV_FRAME_GLOBAL)
                        .command(MavCmd.MAV_CMD_NAV_WAYPOINT)
                        .current(0)
                        .autocontinue(1)
                        .param1(0)
                        .param2(0)
                        .param3(0)
                        .param4(10)
                        .x(waypointList[requestedMissionItem][0])
                        .y(waypointList[requestedMissionItem][1])
                        .z(waypointList[requestedMissionItem][2])
                        .missionType(MavMissionType.MAV_MISSION_TYPE_MISSION)
                        .build();
                transmittedMissionItem = requestedMissionItem;
                // Write an message to drone
                connection.send2(systemId, componentId, missionItem);
                System.out.println("Seq: " + requestedMissionItem + ", Message Send: " + MavCmd.MAV_CMD_NAV_WAYPOINT.name());
            }
}
...

Testing Mavlink 2

Hi all.
I try to use mavlink2.
When i create and send mavlink2 message i receive error with signing package.

     // Now we are ready to read and send messages.
      MavlinkMessage message;
      while ((message = connection.next()) != null) {
	// When a message is received, its payload type isn't statically available.
	// You can resolve which kind of message it is by its payload, like so:
	if (message.getPayload() instanceof Heartbeat) {
		// This is a heartbeat message
				System.out.println("Heartbeat");
				System.out.println(message.toString());
				if ( handshake != true ) {
					
					System.out.println("\n Handshake \n ");

					connection.send(new Mavlink2Message<>(
							1,
							1,
							message.getOriginComponentId(), 
							message.getOriginSystemId(), 
							CommandInt.builder()
							.targetComponent(message.getOriginComponentId())
							.targetSystem(message.getOriginSystemId()).param1(1)
							.frame(MavFrame.MAV_FRAME_MISSION)
							.command(MavCmd.MAV_CMD_REQUEST_PROTOCOL_VERSION)
							.build()
					      ));
					handshake = true;						
				}
                }

}

Update readme to use Gradle Wrapper

Dear all,
It will be great if the Readme file will be updated and some simple instructions about using Gradle wrapper to rebuild the library with an updated protocol description will be provided. I don't know if it is so uncommon if Java developers are not familiar with Gradle nowadays, but I am not and hope that some straightforward instructions will be beneficial for all the users of this great project.

Deserialization failure

I have detected a pair of problems related with deserialization of ReflectionPayloadDeserializer. It does not convert payload to object value param:

  • With the MemoryVect event, the packet received is:

MavlinkPacket{versionMarker=253, incompatibleFlags=0, compatibleFlags=0, sequence=48, systemId=1, componentId=1, messageId=249, payload=[1, 0, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1], checksum=35422, signature=[]}

and the final MemoryVect object is MemoryVect{address=1, ver=0, type=1, value=null}.

  • With the BatteryStatus event is the same behaviour with voltages param:

MavlinkPacket{versionMarker=253, incompatibleFlags=0, compatibleFlags=0, sequence=86, systemId=1, componentId=1, messageId=147, payload=[1, 0, 0, 0, 1, 0, 0, 0, 25, 0, 4, 0, 4, 0, 4, 0, 4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 1, 1, 1, 48, 117, 0, 0, 1], checksum=43340, signature=[]}

BatteryStatus{id=1, batteryFunction=EnumValue{value=1, entry=MAV_BATTERY_FUNCTION_ALL}, type=EnumValue{value=1, entry=MAV_BATTERY_TYPE_LIPO}, temperature=25, voltages=null, currentBattery=1, currentConsumed=1, energyConsumed=1, batteryRemaining=1, timeRemaining=30000, chargeState=EnumValue{value=1, entry=MAV_BATTERY_CHARGE_STATE_OK}

Any idea about what could be happening? Thanks in advance.

I connect via usb serial, I get events (onreceive(byte[] data)) back, no inputstream or outputstream

I connect via usb serial on android, and I don't have a stream to use.

The serial data goes to a receive function:

    private void receive(byte[] data) {
        createMavLinkMessage(data);

...

This feels like a real clunky way of doing it. Is there a better way ?

 void createMavLinkMessage(byte[] data) {
        
        // It's difficult to create a continuous data stream, so I create 1 stream at a time and process the data 
        // all at once. Then I create a new stream and do it again with the new data.
        ByteArrayInputStream serialStream = new ByteArrayInputStream(data);

        OutputStream fakeOutputStream = null; // new OutputStream();
        MavlinkConnection connection = MavlinkConnection.create(serialStream, fakeOutputStream);

        try {
            MavlinkMessage message;
            while ((message = connection.next()) != null) {
                onNewMessage(message);
            }
        }  catch (EOFException eof) {
            Log.e(TAG, "End of Stream", eof);
            // onConnectionClose();
        } catch (IOException ioe) {
            Log.e(TAG, "Failed to establish Stream", ioe);
            // onConnectionClose();
        }
    }

MavlinkMessage to byte array

Hi,
Is there any way to convert MavlinkMessage into byte array (in another words get raw byte representation of single mavlink message)?

data method is null in EncapsulatedData message

Hi,
I try to get the data in the EncapsulatedData message but it's null.
With this line :
encapsulatedData.data().length
I get this error :
java.lang.NullPointerException: Attempt to get length of null array

Is it a issue or I'm noob ? ^^

Intel Aero RTF PX4 1.8, vast majority of messages appear to have CRC errors

I'm trying to talk to an Intel Aero RTF drone, which has been updated to PX4 1.8.0 from;

https://github.com/PX4/Firmware/releases/download/v1.8.0/aerofc-v1_default.px4

When I started a copy of the sample code, I received very very few messages back, something like three or four over the course of minutes. I downloaded the source for mavlink and added some logging in MavlinkConnection.next(), and when I ran that for one minute (over a TCP socket, as was shown in the sample code), MavlinkConnection.next() received 25593 messages, of which 1984 were unsupported by the dialect and 23609 did not pass the CRC test. It received no messages that passed both tests in that minute. I tried configuring the connection as;

					.dialect(MavAutopilot.MAV_AUTOPILOT_GENERIC, new StandardDialect()) //
					.dialect(MavAutopilot.MAV_AUTOPILOT_PX4, new StandardDialect()) //

I also tried with just the second line, i.e. PX4.

I'm kind of at a loss at this point on how to proceed further. Possibly I've not configured something correctly? Pymavlink seems to work much better over a TCP socket talking to this Aero but I would much prefer to stick to Java.

SetMode its not changing the mode

Hi, i was trying to change the mode of an arducopter using SetMode because using CommandLong its not supported for the autopilot , the problem is that every time when i send the command returns a COMMAND_ACK {command : 11, result : 4} i try to change the custom mode param for my autopilot but it didnt work hope you can tell me if this command works or its no longer in use or am i doing something wrong

Here is my code

Socket socket = new Socket("192.168.1.105", 14450);
MavlinkConnection connection = MavlinkConnection.builder(socket.getInputStream(), 
		                                        socket.getOutputStream())
                                                       .dialect(MavAutopilot.MAV_AUTOPILOT_ARDUPILOTMEGA, new ArdupilotmegaDialect())
						       .build();
				
SetMode setMode = SetMode.builder()
		.targetSystem(255)
		.customMode(0)
		.build();
		
connection.send1(255, 1, setMode);

Message serializer always returning empty byte[]

Thanks for the great library!

Having a few issues with serialization of the message before adding it to the mavlink packet to be sent.

Messages are being sent and received successfully by the client, however they have an empty body.

HEARTBEAT {type : 0, autopilot : 0, base_mode : 0, custom_mode : 0, system_status : 0, mavlink_version : 0}
Using the code as given in the example

int systemId = 255;
int componentId = 0;
Heartbeat heartbeat = Heartbeat.builder()
          .type(MavType.MAV_TYPE_GCS)
          .autopilot(MavAutopilot.MAV_AUTOPILOT_INVALID)
          .systemStatus(MavState.MAV_STATE_UNINIT)
          .mavlinkVersion(3)
          .build()

// Write an unsigned heartbeat
connection.send2(systemId, componentId, heartbeat);

Inside of connection.send2() the heartbeat is serialized before it is added to the mavlink packet using:

byte[] serializedPayload = serializer.serialize(heartbeat);

This then implements ReflectionPayloadSerializer to return an array of bytes which is always returning empty. Looking into the problem I looked during debug and found that the payload length was always returning as 0 meaning that the created array was of 0 size (byte[0])

See screenshot

image


public class ReflectionPayloadSerializer implements MavlinkPayloadSerializer {

    private static final WireFieldInfoComparator wireComparator = new WireFieldInfoComparator();

    @Override
    public byte[] serialize(Object message) {
        Class<?> messageClass = message.getClass();
        if (!messageClass.isAnnotationPresent(MavlinkMessageInfo.class)) {
            throw new IllegalArgumentException(messageClass.getName() + " is not annotated with @MavlinkMessageInfo");
        }

        int payloadLength = Arrays.stream(messageClass.getMethods())
                .filter(m -> m.isAnnotationPresent(MavlinkFieldInfo.class))
                .map(m -> m.getAnnotation(MavlinkFieldInfo.class))
                .mapToInt(f -> f.unitSize() * Math.max(1, f.arraySize()))
                .sum();
        if (payloadLength > 253) {
            throw new IllegalStateException("payload length > 253 for message" + messageClass.getName());
        }
        byte[] payload = new byte[payloadLength];  ---->> (payloadLength returning as 0 and hence  byte[] payload = new byte[0]; )


Any insight into the fix of this would be greatly appreciated!

Thanks in advanced,
Zac

Setting Rally Points on Copter 3.6

I implemented a download and upload of missions with this great library and all went well, until I find out that currently released version of ArduCopter 3.6 does not support uploads of Rally Points using the mission protocol.

On the arducopter forum I found out that I have to set a parameter to the rally point size and then "push the rally points using RALLY_POINT" but I did not find a way to do this in dronefleet.io. Can you give me a hint on how can I upload rally points to ArduCopter 3.6 using your library please?

Thanks in advance.

Custom Mode using MAV_CMD_DO_SET_MODE?

I am having trouble getting MAV_CMD_DO_SET_MODE to do anything, where using the deprecated Set_Mode everything works fine.

If I use Set_Mode with a BASE_MODE=89 and a CUSTOM_MODE=4 the UAV will respond in the heartbeat with a BASE_MODE=217 and a CUSTOM_MODE=4. I have read that the BASE_MODE is ignored and only the CUSTOM_MODE matters, but I set the BASE_MODE anyways. In any case, doing that with Set_Mode works fine and I can issue other commands to the UAV (like takeoff and such).

However, when I try the same thing using MAV_CMD_DO_SET_MODE I am not having much luck. I set the param1=89 (BASE_MODE) and I set param2=4 (CUSTOM_MODE) and the responding Heartbeat changes mode to BASE_MODE=209 and CUSTOM_MODE=0. I have tried setting the parameters to numerous other values and it always gives me 209 and 0 back. Although I can arm the UAV from this mode, I am not able to perform many of the other tasks I want to perform (like takeoff). I am not sure if there is any UAV parameters that need to be set to allow MAV_CMD_DO_SET_MODE to function more like Set_Mode or if there is some issue with the library or something I am just doing wrong in general.

Has anyone else had any trouble with setting the CUSTOM_MODE using MAV_CMD_DO_SET_MODE? Can you get its value to set to anything other than 0 (in Heartbeat)? Thanks for the help.

LogData payload has null data array but the bytes are received in the message.

I am trying to pull logs from a pixhawk1 using the library, but I cannot seem to figure out where I am losing the data array. Example below show's what I'm seeing with 2 back to back calls in the reader thread.

LogRequestData was set to match the 00 b4 00 00 that I sniffed as the first request from QGroundControl app which is working. I originally assumed this was a value of 180. It seems to be 46080 in the builder here.

Request

LogRequestData.builder()
                .id(mSource.id())
                .count(46080)
                .ofs(46080)
                .targetSystem(1)
                .targetComponent(1)
                .build();

Response message.getPayload().toString():
RX v1: LogData{id=2, ofs=46080, count=90, data=null}
Response byteArrayToHexString(message.getRawBytes()):
RX v1 Raw bytes: fe 61 5c 01 01 78 00 b4 00 00 02 00 5a af 37 58 92 f4 3d 7d 1c df bd 3b e9 19 c1 a3 95 87 f6 6d 00 00 00 08 f4 36 28 66 4d bb 4f d8 5e 3b 48 60 28 3e e0 ce 6c bd 75 d7 20 c1 a3 95 83 09 6e 00 00 c0 e5 46 3a 00 68 8e b7 dc a4 d0 b9 f8 05 fd 3d 08 48 b9 bd f6 d8 19 c1 a3 95 87 09 6e 00 00 dc 5b 40 bb 70 0a 2d 06 02

From this, I can tell that most of the response seems to be parsed correctly:

  • offset is the same as requested: 00 b4 00 00
  • ID is the same as requested: 02 00
  • byte count is the 90 requested: 5a

The response raw data is the same as what I saw over the wire previously. However, It seems that retrieving the payload as a LogData type does not reference or copy the array. I'm reading up on the deserializer, but haven't found the culprit yet. Any helpful hints are greatly appreciated. Thanks!

Error slicing byte array when parsing V1 packet

I am running into an 'array out of bounds' error using the library with Android over a USB connection. I'm opening the connection as a serial port using https://github.com/felHR85/UsbSerial and then obtaining streams from that. The error I get back is below. Any ideas why I would get 0xFE's back to back? I'm digging further into the iostreams currently, but wondering if you have any ideas from your side. Thanks!

Exception thrown to log:

E/MAVLINk: Caught Array bounds exception reading mavlink messages.
    java.lang.ArrayIndexOutOfBoundsException: src.length=6 srcPos=6 dst.length=254 dstPos=0 length=254
        at java.lang.System.arraycopy(Native Method)
        at io.dronefleet.mavlink.protocol.ByteArray.slice(ByteArray.java:44)
        at io.dronefleet.mavlink.protocol.MavlinkPacket.fromV1Bytes(MavlinkPacket.java:130)
        at io.dronefleet.mavlink.protocol.MavlinkPacketReader.next(MavlinkPacketReader.java:39)
        at io.dronefleet.mavlink.MavlinkConnection.next(MavlinkConnection.java:213)
        at com.company.app.service.commProtocols.Mavlink.Mavlink$ReaderThread.run(Mavlink.java:247)
I/MAVLINk: Closing Mavlink connection

Debugger output:

INCOMPAT_FLAG_SIGNED = 1 (0x1)
 MAGIC_V1 = 254 (0xFE)
 MAGIC_V2 = 253 (0xFD)
 serialVersionUID = 3105533922443436538 (0x2B1912A60D2AA1FA)
rawBytes = {byte[6]@6682} 
 0 = -2 (0xFE)
 1 = -2 (0xFE)
 2 = 9 (0x9)
 3 = 51 (0x33)
 4 = 68 (0x44)
 5 = 0 (0x0)
bytes = {ByteArray@6683} 
 bytes = {byte[6]@6682} 
 shadow$_klass_ = {Class@6605} "class io.dronefleet.mavlink.protocol.ByteArray"
 shadow$_monitor_ = -2076735974 (0x8437861A)
versionMarker = 254 (0xFE)
payloadLength = 254 (0xFE)

Mavlink2Message methods cannot be found

I'm currently importing quite a bit of the dronefleet mavlink library into my java project. It has been working well for Mavlink 1 messages, but when attempting to implement Mavlink2Message, I cannot use any of the methods from Mavlink2Message.

The fact that I can successfully cast to Mavlink2Message, but cannot use any Mavlink2Message method, makes me believe that it is not on my end. Not only can I not use isSigned(), but I can't use any Mavlink2Message method.

It fails to compile with this error:

 error: cannot find symbol
                        if (message2.isSigned()) {
                                    ^
  symbol:   method isSigned()
  location: variable message2 of type Mavlink2Message

I follow the same method shown in the primary readme:

MavlinkMessage message;
    while ((message = connection.next()) != null) {
        if (message instanceof Mavlink2Message) {
            Mavlink2Message message2 = (Mavlink2Message)message;
            
            if (message2.isSigned()) {
                         //dosomething
                } else {
                    // etc
                }
            } else {
                // etc
            }
        } else {
           // etc
        }
    }

And, from dronefleet, I import the following:

import io.dronefleet.mavlink.MavlinkConnection;
import io.dronefleet.mavlink.MavlinkMessage;
import io.dronefleet.mavlink.Mavlink2Message;
import io.dronefleet.mavlink.common.Altitude;
import io.dronefleet.mavlink.common.Attitude;
import io.dronefleet.mavlink.common.AttitudeQuaternion;
import io.dronefleet.mavlink.common.AttitudeTarget;
import io.dronefleet.mavlink.common.BatteryStatus;
import io.dronefleet.mavlink.common.CommandAck;
import io.dronefleet.mavlink.common.CommandLong;
import io.dronefleet.mavlink.common.EstimatorStatus;
import io.dronefleet.mavlink.common.ExtendedSysState;
import io.dronefleet.mavlink.common.GlobalPositionInt;
import io.dronefleet.mavlink.common.GpsRawInt;
import io.dronefleet.mavlink.common.Heartbeat;
import io.dronefleet.mavlink.common.HighresImu;
import io.dronefleet.mavlink.common.HomePosition;
import io.dronefleet.mavlink.common.LocalPositionNed;
import io.dronefleet.mavlink.common.ManualControl;
import io.dronefleet.mavlink.common.MavAutopilot;
import io.dronefleet.mavlink.common.MavCmd;
import io.dronefleet.mavlink.common.MavMissionType;
import io.dronefleet.mavlink.common.MavMode;
import io.dronefleet.mavlink.common.MavState;
import io.dronefleet.mavlink.common.MavType;
import io.dronefleet.mavlink.common.MissionAck;
import io.dronefleet.mavlink.common.MissionClearAll;
import io.dronefleet.mavlink.common.MissionCount;
import io.dronefleet.mavlink.common.MissionSetCurrent;
import io.dronefleet.mavlink.common.MissionCurrent;
import io.dronefleet.mavlink.common.MissionItem;
import io.dronefleet.mavlink.common.MissionRequest;
import io.dronefleet.mavlink.common.MissionRequestInt;
import io.dronefleet.mavlink.common.ParamRequestRead;
import io.dronefleet.mavlink.common.ParamSet;
import io.dronefleet.mavlink.common.ParamValue;
import io.dronefleet.mavlink.common.Ping;
import io.dronefleet.mavlink.common.PositionTargetGlobalInt;
import io.dronefleet.mavlink.common.PositionTargetLocalNed;
import io.dronefleet.mavlink.common.ServoOutputRaw;
import io.dronefleet.mavlink.common.SysStatus;
import io.dronefleet.mavlink.common.VfrHud;
import io.dronefleet.mavlink.common.Vibration;

tips for begginers

First of all, thanks for all the effort and dedication put on this wonderful library.
I am a begginer on programming for mavlink, though I have already made some changes on a program that ran on a RedBear as companion computer for a 3DR iris+ a long time ago, but then the code was already written.

I am trying to develop a simple plan uploader for a ArduPilot drone that I don´t have yet, but I was anxious and wanted to get started already. So I am running a SITL drone and connecting it using the library.

I can get the heartbeat messages fine with this setup and a lot of other messages Im not interested in at the moment, but I was trying to follow the protocol (https://mavlink.io/en/services/mission.html#uploading_mission) for mavlink upload a flight plan but I stoped at the first part because when I send a MissionCount message I get no response ever from the SITL.

So I decided to send the count and ignore the ack and trying to send a MissionItem message with one simple coordinate just to test it and I get a MAV_MISSION_ERROR back, how should I know whats the error?

this is the code for my MissionCount message
MissionCount count = MissionCount.builder() .count(1) .missionType(MavMissionType.MAV_MISSION_TYPE_MISSION) .targetSystem(systemId) .targetComponent(componentId) .build();
and this is the code for my MissionItem message:
MissionItem item = MissionItem.builder() .seq(0) .frame(MavFrame.MAV_FRAME_GLOBAL) .command(MavCmd.MAV_CMD_NAV_WAYPOINT) .missionType(MavMissionType.MAV_MISSION_TYPE_MISSION) .current(1) .autocontinue(1) .param1(0f)//Hold time in decimal seconds .param2(0.2f)//Acceptance radius in meters .param3(0f)//if > 0 radius in meters to pass by WP .param4(0f) .x(48.995098f) .y(9.290475f) .z(215f) .build();

can you point me in the right direction as to what im doing wrong?

Best regards

Unable to parse arrays except uint8_t

The generator converts fields of array types, such as uint16_t[6] to a List (in FieldGenerator.arrayType()), however ReflectionPayloadDeserializer.deserialize doesn't have a case for this.

I would also recommend adding a warning print at the end of ReflectionPayloadDeserializer.deserialize if there is no type match, so at least it's made obvious to the user that something is wrong, instead of just having a null field.

SetMode.java Not Serializing Custom Modes Correctly For PX4

Background:
In attempting to use PX4's Follow Me mode I found that the serialization for the CMD_DO_SET_MODE command does not account for the latest PX4 build and how it handles setting modes through Mavlink, at least when trying to use SetMode.java to do this. I'm assuming that SetMode is a wrapper than contains fields for the CMD_DO_SET_MODE. With a BaseMode field and a CustomMode.

How I found this Issue:

I sent a SetMode with a CustomMode of 12 for the FollowMe custom mode supported by PX4 and BaseMode of MAV_MODE_AUTO_ARMED. Despite setting what I thought were the right params the mode was not being set correctly in my simulation environment (JMAVSIM).

What went wrong:

SetMode.java does not add a custom mode flag even when I specify a CustomMode is set. By not adding this, PX4 just uses the BaseMode I provided in SetMode instance. Secondly, PX4 does not just use one single CustomMode. PX4 uses a main CustomMode, then based off of that main CustomMode PX4 allows a subset of Sub CustomModes to be set.

PX4 Firmware (Reference for handling of MAV_CMD_DO_SET_MODE):
https://github.com/PX4/Firmware/blob/a8ea55d9b63df3ef2a6b688b7dd1ae107916a8a6/src/modules/commander/Commander.cpp

PX4 Custom Mode Enums:
https://github.com/PX4/Firmware/blob/07d656e971a72d1202651dfd3b4642736fb078d7/src/modules/commander/px4_custom_mode.h

How I got around this:

Using a CommandLong containing MAV_CMD_DO_SET_MODE with the parameters: BaseMode of 221 (Doesn't matter what is put here PX4 only checks the lowest bit for a one), 4 (PX4_CUSTOM_MAIN_MODE_AUTO), and 8 (PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET).

Suggestions:
The PX4 documentation wasn't very clear on what I needed to pass to MAV_CMD_DO_SET_MODE. The parameter list suggested 12, but that's not how the firmware works. The firmware is hierarchical: 1st get set the right main CustomMode then the correct sub CustomMode. I'd suggest just making SetMode.java account for this.

MissionAck returning with type=null and missionType=null

Hello, (I edited the whole post, to be more to the point)
I'm using the lib to connect to a SITL configuration via TCP/UDP (tried both)

public static String bytesToHex(byte[] hashInBytes) {
    StringBuilder sb = new StringBuilder();
    for (byte b : hashInBytes)
        sb.append(String.format("%02x ", b));
    return sb.toString();
}

main...
Socket socket = new Socket("127.0.0.1", 58275);
MavlinkFrameReader frameReader = new MavlinkFrameReader(socket.getInputStream());
while (frameReader.next()) {
    byte[] frameBytes = frameReader.frame();
    if (UDPConnection.bytesToHex(frameBytes).contains("01 01 2f")) //show only MISSION_ACK msg's
    	System.out.println(UDPConnection.bytesToHex(frameBytes));

The problem is that i'm getting null fields but they are being correctly sent from the SITL setup (I verified with pymavlink and also qgroundcontrol, they both receive them correctly).

Here is a simple output that can show the problem.
fd 03 00 00 3b 01 01 2f 00 00 ff 00 0d 93 97
fd 03 00 00 3c 01 01 2f 00 00 ff 00 0d 66 53
fd 03 00 00 3d 01 01 2f 00 00 ff 00 0d 41 7f
fd 03 00 00 3f 01 01 2f 00 00 ff 00 0d 0f 27
fd 03 00 00 40 01 01 2f 00 00 ff 00 0d 4f a5
fd 03 00 00 41 01 01 2f 00 00 ff 00 0d 68 89
fd 03 00 00 43 01 01 2f 00 00 ff 00 0d 26 d1
fd 03 00 00 44 01 01 2f 00 00 ff 00 0d d3 15
fd 03 00 00 45 01 01 2f 00 00 ff 00 0d f4 39
fd 01 00 00 46 01 01 2f 00 00 ff e1 ba
fd 03 00 00 48 01 01 2f 00 00 ff 00 01 c6 65
fd 03 00 00 49 01 01 2f 00 00 ff 00 01 e1 49
fd 03 00 00 4a 01 01 2f 00 00 ff 00 01 88 3d
fd 04 00 00 65 01 01 2f 00 00 ff 00 00 02 5d 28
fd 04 00 00 67 01 01 2f 00 00 ff 00 00 02 7f 83

Output from the SITL console:
Got MAVLink msg: MISSION_ACK {target_system : 255, target_component : 0, type : 13, mission_type : 0}
Got MAVLink msg: MISSION_ACK {target_system : 255, target_component : 0, type : 13, mission_type : 0}
Got MAVLink msg: MISSION_ACK {target_system : 255, target_component : 0, type : 13, mission_type : 0}
Got MAVLink msg: MISSION_ACK {target_system : 255, target_component : 0, type : 13, mission_type : 0}
Got MAVLink msg: MISSION_ACK {target_system : 255, target_component : 0, type : 13, mission_type : 0}
Got MAVLink msg: MISSION_ACK {target_system : 255, target_component : 0, type : 13, mission_type : 0}
Got MAVLink msg: MISSION_ACK {target_system : 255, target_component : 0, type : 13, mission_type : 0}
Got MAVLink msg: MISSION_ACK {target_system : 255, target_component : 0, type : 13, mission_type : 0}
Got MAVLink msg: MISSION_ACK {target_system : 255, target_component : 0, type : 13, mission_type : 0}
Got MAVLink msg: MISSION_ACK {target_system : 255, target_component : 0, type : 0, mission_type : 0}
APM: Flight plan received
Got MAVLink msg: MISSION_ACK {target_system : 255, target_component : 0, type : 1, mission_type : 0}
Got MAVLink msg: MISSION_ACK {target_system : 255, target_component : 0, type : 1, mission_type : 0}
Got MAVLink msg: MISSION_ACK {target_system : 255, target_component : 0, type : 1, mission_type : 0}
Got MAVLink msg: MISSION_ACK {target_system : 255, target_component : 0, type : 0, mission_type : 2}
APM: Rally points received
Got MAVLink msg: MISSION_ACK {target_system : 255, target_component : 0, type : 0, mission_type :

As one can see, type and missiontype are null (the bytes aren't even there) in the output from executing main code, but the SITL console shows the type : 0 and mission_type : 0 correctly.

This same behavior also happens with other messages, not only MISSION_ACK, but for instance SYSTEM_TIME's time_unix_usec also is null in EVERY message received.

Any idea why ?

Deserialization performance

Deserialization performance can be drastically improved by caching the list of builder methods. Here's a typical flame chart for reading mavlink messages in a loop:
mavlink-reading

You can see that sorting the builder methods takes almost half the deserialization time. This can easily be solved with:

    private static final Map<Class<?>, List<Method>> builderMethodsCache = Collections.synchronizedMap(new HashMap<>());


    private static List<Method> getBuilderMethods(Class<?> builderClass){
        return builderMethodsCache.computeIfAbsent(builderClass, (c) -> Arrays.stream(c.getMethods())
                .filter(m -> m.isAnnotationPresent(MavlinkFieldInfo.class))
                .sorted((a, b) -> {
                    MavlinkFieldInfo fa = a.getAnnotation(MavlinkFieldInfo.class);
                    MavlinkFieldInfo fb = b.getAnnotation(MavlinkFieldInfo.class);
                    return wireComparator.compare(fa, fb);
                }).collect(Collectors.toList()));
    }

in ReflectionPayloadDeserializer.

Sending/Receiving custom message help

I am trying to write an android app to send custom mavlink1.0 messages to a java server using mostly the same code over UDP

I have a dialect.xml that I am using to test sending custom messages.

<?xml version='1.0'?>
<mavlink>
     <include>common.xml</include>
	 <messages>
          <message id="444" name="WAYPOINT_TESTER">
               <description>WAYPOINT_TESTER</description>
               <field type="int8_t" name="waypoint_id">id</field>
          </message>
	</messages>
</mavlink>

I can build the message just fine

WaypointTester tester = WaypointTester.builder().waypointId(1).build();

on the android side before it goes over the pipe I see the data
in byte[] = [-2, 1, 0, 0, 0, -68, 1, 77, 57]
in hex = FE01000000BC014D39

on the other side I can see that data is reaching just fine, but the mavlink connection never reports that message.

If I send messages from the dialects included in the box it works fine,

   Heartbeat msg = Heartbeat.builder()
               .type(MavType.MAV_TYPE_GCS)
               .autopilot(MavAutopilot.MAV_AUTOPILOT_GENERIC)
               .systemStatus(MavState.MAV_STATE_ACTIVE)
               .mavlinkVersion(1)
               .build();

in byte[] = [-2, 9, 0, 0, 0, 0, 0, 0, 0, 0, 6, 0, 0, 4, 1, -8, -86]
in hex = FE0900000000000000000600000401F8AA

server side

Heartbeat{type=EnumValue{value=6, entry=MAV_TYPE_GCS}, autopilot=EnumValue{value=0, entry=MAV_AUTOPILOT_GENERIC}, baseMode=EnumValue{value=0, entry=null}, customMode=0, systemStatus=EnumValue{value=4, entry=MAV_STATE_ACTIVE}, mavlinkVersion=1}

--steps that I used to build everything--
(I am using windows)

  1. drop the dialect.xml file into definition-xml folder
  2. gradle clean
  3. gradle jar
  4. cd to mavlink-protocol
  5. gradle clean
  6. gradle jar
  7. link the two jar files to the projects

code
So far it is pretty basic, I have two threads that have access to the MavlinkConnection:

sending thread
connection.send1(0, 0, msg);

receiving thread

MavlinkMessage message;
while ((message = connection.next()) != null) {
    System.out.println(message.getPayload());
}

I am not sure what I am doing wrong

Problem with mavlink2 deserialization

I'v met another problem.
I plug my APM. APM start send me message. I see that is mav2 message because i see method From2 of MavlinkPacket work.
But after some portion of message program terminated with error.

Console output

Start
fromV2
MavlinkPacket constructor
MavlinkMessage{originSystemId=1, originComponentId=1, payload=Attitude{timeBootMs=6470765, roll=0.018246904, pitch=0.14143202, yaw=-1.2641025, rollspeed=8.0544874E-4, pitchspeed=2.0475686E-4, yawspeed=-8.0993585E-4}}
fromV1
MavlinkPacket constructor
fromV2
MavlinkPacket constructor
MavlinkMessage{originSystemId=1, originComponentId=1, payload=HighresImu{timeUsec=null, xacc=1.400253, yacc=-0.17800356, zacc=-9.915399, xgyro=8.0544874E-4, ygyro=2.0475686E-4, zgyro=-8.0993585E-4, xmag=0.154, ymag=0.80631995, zmag=0.67248, absPressure=101924.0, diffPressure=0.0, pressureAlt=-49.74959, temperature=41.68, fieldsUpdated=7167}}
fromV2
MavlinkPacket constructor

Exception in thread "main" java.lang.ArrayIndexOutOfBoundsException
	at java.lang.System.arraycopy(Native Method)
	at io.dronefleet.mavlink.serialization.payload.reflection.ReflectionPayloadDeserializer.lambda$5(ReflectionPayloadDeserializer.java:57)
	at java.util.stream.ForEachOps$ForEachOp$OfRef.accept(Unknown Source)
	at java.util.stream.ReferencePipeline$2$1.accept(Unknown Source)
	at java.util.ArrayList.forEach(Unknown Source)
	at java.util.stream.SortedOps$RefSortingSink.end(Unknown Source)
	at java.util.stream.Sink$ChainedReference.end(Unknown Source)
	at java.util.stream.AbstractPipeline.copyInto(Unknown Source)
	at java.util.stream.AbstractPipeline.wrapAndCopyInto(Unknown Source)
	at java.util.stream.ForEachOps$ForEachOp.evaluateSequential(Unknown Source)
	at java.util.stream.ForEachOps$ForEachOp$OfRef.evaluateSequential(Unknown Source)
	at java.util.stream.AbstractPipeline.evaluate(Unknown Source)
	at java.util.stream.ReferencePipeline.forEach(Unknown Source)
	at io.dronefleet.mavlink.serialization.payload.reflection.ReflectionPayloadDeserializer.deserialize(ReflectionPayloadDeserializer.java:48)
	at io.dronefleet.mavlink.MavlinkConnection.next(MavlinkConnection.java:272)
	at simpleMavlink.Main.main(Main.java:110)

This problem occurs on different message types.
Another output stream

Start
fromV2
MavlinkPacket constructor
MavlinkMessage{originSystemId=1, originComponentId=1, payload=Altitude{timeUsec=null, altitudeMonotonic=-49.501213, altitudeAmsl=-49.501213, altitudeLocal=-5.7601304, altitudeRelative=-5.7601304, altitudeTerrain=NaN, bottomClearance=NaN}}
fromV2
MavlinkPacket constructor
MavlinkMessage{originSystemId=1, originComponentId=1, payload=ServoOutputRaw{timeUsec=2199857700, port=1, servo1Raw=1500, servo2Raw=1500, servo3Raw=1500, servo4Raw=1500, servo5Raw=0, servo6Raw=0, servo7Raw=0, servo8Raw=0, servo9Raw=0, servo10Raw=0, servo11Raw=0, servo12Raw=0, servo13Raw=0, servo14Raw=0, servo15Raw=0, servo16Raw=0}}
fromV2
MavlinkPacket constructor
MavlinkMessage{originSystemId=1, originComponentId=1, payload=VfrHud{airspeed=0.0, groundspeed=0.019179275, heading=287, throttle=0, alt=-49.501213, climb=0.6250572}}
fromV2
MavlinkPacket constructor
MavlinkMessage{originSystemId=1, originComponentId=1, payload=AttitudeQuaternion{timeBootMs=6494831, q1=0.8041289, q2=0.048920356, q3=0.05162769, q4=-0.5901849, rollspeed=-1.6618893E-4, pitchspeed=5.9161335E-5, yawspeed=1.5552435E-4}}
fromV2
MavlinkPacket constructor
MavlinkMessage{originSystemId=1, originComponentId=1, payload=ActuatorControlTarget{timeUsec=null, groupMlx=0, controls=null}}
fromV2
MavlinkPacket constructor
MavlinkMessage{originSystemId=1, originComponentId=1, payload=Heartbeat{type=EnumValue{value=13, entry=MAV_TYPE_HEXAROTOR}, autopilot=EnumValue{value=12, entry=MAV_AUTOPILOT_PX4}, baseMode=EnumValue{value=81, entry=null}, customMode=65536, systemStatus=EnumValue{value=0, entry=MAV_STATE_UNINIT}, mavlinkVersion=3}}
Heartbeat
MavlinkMessage{originSystemId=1, originComponentId=1, payload=Heartbeat{type=EnumValue{value=13, entry=MAV_TYPE_HEXAROTOR}, autopilot=EnumValue{value=12, entry=MAV_AUTOPILOT_PX4}, baseMode=EnumValue{value=81, entry=null}, customMode=65536, systemStatus=EnumValue{value=0, entry=MAV_STATE_UNINIT}, mavlinkVersion=3}}

 Handshake 
 
fromV2
MavlinkPacket constructor
MavlinkMessage{originSystemId=1, originComponentId=1, payload=ActuatorControlTarget{timeUsec=null, groupMlx=0, controls=null}}
fromV2
MavlinkPacket constructor
MavlinkMessage{originSystemId=1, originComponentId=1, payload=AttitudeTarget{timeBootMs=6494920, typeMask=0, q=null, bodyRollRate=-0.11579337, bodyPitchRate=-0.91740125, bodyYawRate=0.0035392735, thrust=0.0}}
fromV2
MavlinkPacket constructor


Exception in thread "main" java.lang.ArrayIndexOutOfBoundsException
	at java.lang.System.arraycopy(Native Method)
	at io.dronefleet.mavlink.serialization.payload.reflection.ReflectionPayloadDeserializer.lambda$5(ReflectionPayloadDeserializer.java:56)
	at java.util.stream.ForEachOps$ForEachOp$OfRef.accept(Unknown Source)
	at java.util.stream.ReferencePipeline$2$1.accept(Unknown Source)
	at java.util.ArrayList.forEach(Unknown Source)
	at java.util.stream.SortedOps$RefSortingSink.end(Unknown Source)
	at java.util.stream.Sink$ChainedReference.end(Unknown Source)
	at java.util.stream.AbstractPipeline.copyInto(Unknown Source)
	at java.util.stream.AbstractPipeline.wrapAndCopyInto(Unknown Source)
	at java.util.stream.ForEachOps$ForEachOp.evaluateSequential(Unknown Source)
	at java.util.stream.ForEachOps$ForEachOp$OfRef.evaluateSequential(Unknown Source)
	at java.util.stream.AbstractPipeline.evaluate(Unknown Source)
	at java.util.stream.ReferencePipeline.forEach(Unknown Source)
	at io.dronefleet.mavlink.serialization.payload.reflection.ReflectionPayloadDeserializer.deserialize(ReflectionPayloadDeserializer.java:48)
	at io.dronefleet.mavlink.MavlinkConnection.next(MavlinkConnection.java:272)
	at simpleMavlink.Main.main(Main.java:110)

In this line
System.arraycopy(payload, offset, data, 0, length);

After adding system print

MavlinkMessage{originSystemId=1, originComponentId=1, payload=AttitudeTarget{timeBootMs=7214174, typeMask=0, q=null, bodyRollRate=-0.114282124, bodyPitchRate=-0.912515, bodyYawRate=0.0034739806, thrust=0.0}}
fromV2
MavlinkPacket constructor
payload [B@433c675d
offset 0
data [B@3f91beef
length 8
payload [B@433c675d
offset 8
data [B@1a6c5a9e
length 4
payload [B@433c675d
offset 12
data [B@37bba400
length 4
payload [B@433c675d
offset 16
data [B@179d3b25
length 4
payload [B@433c675d
offset 20
data [B@254989ff
length 4
payload [B@433c675d
offset 24
data [B@5d099f62
length 4
payload [B@433c675d
offset 28
data [B@37f8bb67
length 4
payload [B@433c675d
offset 32
data [B@49c2faae
length 4
payload [B@433c675d
offset 36
data [B@20ad9418
length 4
payload [B@433c675d
offset 40
data [B@31cefde0
length 2
Exception in thread "main" java.lang.ArrayIndexOutOfBoundsException
	at java.lang.System.arraycopy(Native Method)
	at io.dronefleet.mavlink.serialization.payload.reflection.ReflectionPayloadDeserializer.lambda$5(ReflectionPayloadDeserializer.java:62)

[General guidance] Using the ReflectionPayloadDeserializer to deserialise a MAVLink packet

Hi,
I am having some trouble using ReflectionPayloadDeserializer to deserialise a MAVLink packet. Specifically, I don't understand the value I need to provide for the messageType argument in the deserialize method.

For example I am trying to deserialise this heartbeat message: [254, 9, 111, 1, 1, 0, 0, 0, 0, 0, 10, 3, 193, 4, 3, 222, 107]

Heartbeat heartbeat = new ReflectionPayloadDeserializer().deserialize([254, 9, 111, 1, 1, 0, 0, 0, 0, 0, 10, 3, 193, 4, 3, 222, 107] , ??? );

What would I put in place of the ??? to get a Heartbeat?

Connection.next() should timeout or be preemptable

In some instances it might be benificial for connection.next() to timeout or to be able to be forced to return/exit. Even in the case where a user opens up a connection to some COM port they might determine that that port was the wrong COM port and thus they would want to stop connection.next() from continuing. However, in my case I am writting code that performs an auto connect by walking through each COM port looking for a connected MAVLink device (UAV). In my instance, if the COM port is not connected to a MAVLink device the code will lock up waiting for the next message from that port when it will never come. So, it would be nice if there was some way of telling connection.next() to return if it does not recieve a message within some timeout, or to implement the code in a way that allows the user to stop the process themselves (where they can implement thier own timeout timer).

Mavlink enum vs Java enum

There is a problem that Mavlink enums are getting mapped to Java enums. When you try to parse a Mavlink message that contains a value that is not part of the enum XML specification then the parser just throws an exception in ReflectionPayloadDeserializer::enumValue.

This is especially problematic with io.dronefleet.mavlink.common.MavMode as the values in MAV_MODE are basically just example values. In practice you can receive any OR-combination of MAV_MODE_FLAGS in a field of type MAV_MODE.

It is probably necessary to use integer constants instead (or maybe something like java.util.BitSet?).

Overly complicated coding practices leading to difficult usage

I think that a lot of things were done in this effort that are just overly complicated. Such things force limitations on the user, but more importantly since you have scarce documentation it makes it difficult to figure out how to properly use this code.

For example, the code relies far to much on annotations. Why on earth would you annotate FLAGS, MODES, and other constants. Moreover, why would you annotate the message ID's? How is anyone supposed to create a case statement that manages all the messages coming from the UAV (many hundreds)? Your sample suggests that we are to create hundreds of "if" statements? This is a bottleneck routine, so that would be silly. So, the user is forced to create his/her own set of constants for each message ID so that he/she can use them in the message handler case statement. Not a big deal, but that is something that they need to manage separately from your code, when it really should be a part of your code. Still, even with such a set of ID constants, the user still has to get the ID of the received message by writing a long line of code to extract the ID from the annotations when you could have just tossed the ID into the MavLinkMessage class. I know it is in the actual payload, but the user can't get to that until they determine what type of message it is. The ID of the received message should be available to the MavLinkMessage object. Well, it is, but the user has to know how to use annotations to get it. Again, I know this is not a huge deal, but why?!?? Are you trying to win the International Obfuscated code contest or something? I mean, take a look at the example code:

connection.send(new MavlinkMessage<>(
255, // our system id
0, // our component id (0 if we're a ground control system)
Heartbeat.builder()
.type(MavType.MAV_TYPE_GCS)
.autopilot(MavAutopilot.MAV_AUTOPILOT_INVALID)
.systemStatus(MavState.MAV_STATE_UNINIT)
.mavlinkVersion(3)
.build()));

Why do you do it this way? Are you trying to just avoid having to set the attributes in multiple lines of code or something? Is it just because you want to be able to fit everything on one line? I can tell you that most of your users will be expecting to set each attribute up on multiple calls. When they run into this they are surely going to be a bit puzzled, so all you are really doing is making the use of your code difficult. To add insult to injury, this sample code seems to be missing a parameter after the component ID. Can you guess what it is? Yes, it is the message ID, which is annotated, so now I have to go outside of your code and put that in directly myself or I could extract the annotation with a long line of confusing code. I mean, it isn't a big deal, but it just seems like you are being deliberately confusing. Its almost like you want to show people that you know all these tricks, but it isn't the way you should write such code. I know that some of what you are doing is so that you can manage all the messages in a more organized way on the back end, but that doesn't mean you want to force your users to do the same on the front end, it is just confusing. I also understand that some of your usage of annotations is simply to prevent the user from trying to use a value that is outside the scope of such attributes, but I think maybe you went overboard with the annotations is all. I don't claim to be the best JAVA programmer, but odds are a lot of people that are trying to use your code will be less proficient than I am, so why fill your code with complicated trickery? I am really just venting a bit here, so I do apologies, but I thought maybe it will help you to satisfy your users if I say something.

Can you write me some skeleton code that manages say 200 messages coming from the UAV? For instance, most users would expect something like this:

MavlinkConnection connection = MavlinkConnection.create(in, out); // InputStream, OutputStream
MavlinkMessage message;
while ((message = connection.next()) != null) {
switch (message.Id) { //<=== No such IVar so cannot do this
case MAVLINK_MSG_ID_HEARTBEAT : { //MAVLINK_MSG_ID_HEARTBEAT = 0
//handle heartbeat
Heartbeat tempmsg = (Heartbeat) message.getPayload();
...
...
} break;
case MAVLINK_MSG_ID_SYS_STATUS : { //MAVLINK_MSG_ID_SYS_STATUS = 1
//handle system status
} break;
...
...
...
case MAVLINK_MSG_ID_ATTITUDE_CONTROL : { //MAVLINK_MSG_ID_ATTITUDE_CONTROL = 200
//handle attitude control
} break;
default: {
System.out.println("MSG Error : unknown MsgId (" + myMsgID + ")");
}
}
}

There is no list of constants for each message ID, and there is no IVar for message.Id, so the above can't be done. I suppose I could do something to grab the annotation out of the message payload, maybe this?

int myMsgID = ((MavlinkMessageInfo) msg.getPayload().getClass().getAnnotation(MavlinkMessageInfo.class)).id();

Not sure if that is right, but looks OK. Any better ways to get the Id? If not then at least put that type of information in your documentation so people have some clue how to get the message IDs.

In any case, how do you suggest this message handler be done? By putting a bunch of "if" statements with "instanceof" comparisons? Which do you think will be more efficient code, a 200+ "if" statements or a single case statement?

Anyway, that's my troubles, likely I'm just confused, I hope that I am and you can correct my confusion. I hope I'm not coming across angry or mean. I am just trying to express my confusion with the way the code seems to have been written. It really is likely just that there is so little documentation that it is leading me astray. Sorry about that.

Cannot change baudrate

Hi, I am sending events to the drone and they are being received in a 9600 baudrate. I am looking for change that value to 57600 or 115200, but I am not able.

I have found that there exist a class SerialPort that appears to handle it, but I am not able to test it. Can any one help me?

The event is like that:

CommandLong command2 = CommandLong.builder().command(MavCmd.MAV_CMD_NAV_TAKEOFF).build(); connection.send2(1, 1, command2);

Thanks in advance.

Recommend Projects

  • React photo React

    A declarative, efficient, and flexible JavaScript library for building user interfaces.

  • Vue.js photo Vue.js

    🖖 Vue.js is a progressive, incrementally-adoptable JavaScript framework for building UI on the web.

  • Typescript photo Typescript

    TypeScript is a superset of JavaScript that compiles to clean JavaScript output.

  • TensorFlow photo TensorFlow

    An Open Source Machine Learning Framework for Everyone

  • Django photo Django

    The Web framework for perfectionists with deadlines.

  • D3 photo D3

    Bring data to life with SVG, Canvas and HTML. 📊📈🎉

Recommend Topics

  • javascript

    JavaScript (JS) is a lightweight interpreted programming language with first-class functions.

  • web

    Some thing interesting about web. New door for the world.

  • server

    A server is a program made to process requests and deliver data to clients.

  • Machine learning

    Machine learning is a way of modeling and interpreting data that allows a piece of software to respond intelligently.

  • Game

    Some thing interesting about game, make everyone happy.

Recommend Org

  • Facebook photo Facebook

    We are working to build community through open source technology. NB: members must have two-factor auth.

  • Microsoft photo Microsoft

    Open source projects and samples from Microsoft.

  • Google photo Google

    Google ❤️ Open Source for everyone.

  • D3 photo D3

    Data-Driven Documents codes.