Teensy 3.5 Rover with Mavlink and APM Planner 2.0

[mjs513]

Hi all,
One of the things that I always wanted to give a try to is using Mavlink messaging to communicate to a base station as opposed to just dumping data to the serial monitor and later processing. I finally finished a demo project using Teensy 3.5 with Mavlink v1 messaging sending data to the APM Planner Ground Station. Took a while to get done but it works.

Here is a brief description of the setup:

The basic rover setup is a Axel Scropion RC Rock Crawler, with a Ublox M8N and a Adafruit BNO055 attitude sensor. A Teensy 3.5 is used as the controller to process Mavlink messages to and from APM Planner. A joystick is using to control the steering servo and ESC. Two batteries are used, one for the motor and one for the electronics. A set of 3dr Radios are used for communication between the base station and the rover.



A couple of things to note. The best way to get connected is to first connect APM Planner to the radio attached to the com port and then power up the Rover. You will see the connection established almost immediately. The next thing is to enable the joystick from the APM Planner menu. Then you will need to arm the system for the joystick messages to be sent.

Source Code: https://github.com/mjs513/Mavlink-Rover-Teensy3.x
You Tube Demo: https://www.youtube.com/watch?v=9sa0XhpODVo

Hope this helps someone else interested in using mavlink. There are a couple of good references that I will post here once I get them together.

Mike

[mjs513]

@klimbo13. Based on your question it sounds like you have a APM or Pixhawk flight controller in you aircraft? It also sounds like you are using Ardupilot. If this is the case you might be better off posting on the ardupilot forum. You really didn't provide any info on what your hw or sw you are using.

[mar]

Hi mjs513!

I was wondering if you had any resources you could share that helped you get started with packets and Mavlink in general. In the past, I would send the raw sensor data from my drone to my ground station. However, I would like to send formal data packets like Mavlink that can be validated once received and tossed out if corrupt.

I've watched videos and I've read a lot of articles, but I'm not really sure how to create telemetry data packets.

Any help would be appreciated.

Thanks.

[brtaylor]

It's a very small piece of code, but I wonder if this would help:
https://gitlab.com/bolderflight/software/mavlink

I wrapped a class around a few of the mavlink packets I use for our flight software to make it easier to use. I haven't documented it, but the code is commented. It should be an okay example of how to use mavlink.

Little sidetrack: I'm kind of frustrated with how inefficient mavlink is - I don't use half the fields in the packets and everything is sent at full resolution when I could send compressed data just for a display (i.e. why send a full 4 bytes for pitch angle when I probably only need 0.1 degree resolution for driving the display? I could fit that in two bytes). So the display lags a little from when the vehicle is moved. Long term I want to understand how to define my own packets so I can make some that are a lot more efficient.

[mar]

We meet again Brian!

Thank you for the example. At what frequency are you able to transmit down to your ground station? How fast do you think you could transmit with your own data packet structure?

I found out about Mavlink through the drone community but really I'm trying to develop a sound telemetry for model rockets. Either way, I haven't found any super helpful documentation for dummies yet.

I know there are some libraries for arduino like Serial Transfer but I haven't experimented with it yet.

[mjs513]

First nice job Brian on the Mavlink wrapper - have to look at it more if I get back to mavlink

@mar to your question - not really sure what happened to the references I used but I do remember it was quite a bit of searching and following a basic example that some one else put together. But did go back to see if I could find some for you:
Mavlink for Absolute Dummies
Discussion DIYDrones
Micro Air Vehicle Link (MAVLink) in a Nutshell: A Survey
MAVLink Erle Robotics Gitbook
Arduino Controlled Wifi Rover using MavLink Protocol
- check the link to the code by clicking on show more.

And of course alot of reading from the Mavlink site.

[brtaylor]

I send the heartbeat at 1 Hz, the GNSS data as I get new data from the receiver at 5 Hz, and the attitude, speed and heading, and battery voltage at 25 Hz.

The packet sizes are:
* Heartbeat: 9 bytes, includes information about the vehicle and autopilot
* GPS: 52 bytes, includes position, fix type, number of satellites, and uncertainties (why?)
* Attitude: 28 bytes, includes the 3 euler angles and 3 angular rates (Why? Who is looking at angular rate?)
* HUD (airspeed and heading): 20 bytes, includes airspeed, ground speed, altitude, climb rate, heading, and throttle
* Battery: 49 bytes, includes charge state and voltage

So, I’m sending 21,552 bits/second and I’m using a baudrate of 57,600 on those cheap SiK 900 MHz radios.

My approach was to look through the MAVLink message set (http://mavlink.io/en/messages/common.html) and find the messages that I was interested in sending. The MAVLink c library (https://github.com/mavlink/c_library_v2) will have a corresponding header file for each message. For example, mavlink_msg_attitude.h (https://github.com/mavlink/c_library...msg_attitude.h). Then I use the pack function provided in the header to pack the info into a mavlink_message_t and get the message length. I then use mavlink_msg_to_send_buffer to convert mavlink_message_t to a uint8_t buffer and send that over the serial port. An example of that process with the mavlink_msg_attitude is here: https://gitlab.com/bolderflight/soft...mavlink.cc#L32

I’m interested in getting attitude (euler angles), speed, battery voltage, GPS position, GPS fix, and number of satellites. If I’m compressing data optimally, I can store pitch and roll in 11 bits (+/- 90 deg with 0.1 deg resolution) yaw in 12 bits (+/- 180 deg, 0.1 deg resolution), speed and battery voltage in 9 bits (0 – 45 m/s or volt range, 0.1 resolution), GPS latitude and longitude in 32 bits each, GPS altitude in 16 bits (altitude up to 6,000 m), GPS fix in 2 bits, and number of satellites in 6 bits (up to 64 satellites tracked). So, something around 18 bytes total to send all of the data.

Packing data into bits can be annoying, so if I use a less optimal approach and pack each to the next higher byte boundary, then I’m storing attitude, speed, and battery voltage as uint16. GPS position as uint32, and a uint8 for fix and number of satellites. That’s easier to write code for and brings the message size up to 22 or 24 bytes depending on whether I encode altitude as a uint16 or uint32.

I have some code to frame messages (https://github.com/bolderflight/framing). It uses 4 bytes of overhead to mark the start and end of the message and provide a checksum. My biggest hurdle is figuring out how to modify Q Ground Control or Mission Planner to use new packets.

[brtaylor]

I just thought I would add that looking at the bits/second isn't the best measure because they aren't spread evenly over the time interval. I'm running this on a control system running at 50 Hz. If we consider that all messages are sent on the first frame, then I'm sending 158 bytes (1264 bits), which will take .02194 seconds to send. My frame is 0.02 seconds long, so I'm trying to send my second set of telemetry data (this time it's 97 bytes, 776 bits, .0134 seconds) while the first frame is still being sent. So I could be a few frames delayed because of the way the packets stack, and that's before accounting for how the buffering is working on the ground station side and whether any re-sending is going on with the radios.

[mjs513]

For comparison in the sketch I was using I have slightly different timing for messages: heartbeat = 1Hz (think this interval is fixed), AHRS (roll, pitch, yaw, rotation rates) every 100ms, GPS (lat, long, vel, alt, other data) every second.

My approach was to look through the MAVLink message set (http://mavlink.io/en/messages/common.html) and find the messages that I was interested in sending. The MAVLink c library (https://github.com/mavlink/c_library_v2) will have a corresponding header file for each message. For example, mavlink_msg_attitude.h (https://github.com/mavlink/c_library...msg_attitude.h). Then I use the pack function provided in the header to pack the info into a mavlink_message_t and get the message length

Pretty much did the same thing when I determing what messages to send back. I also looked at the source code QGroundControl and Mission Planner as well as search on DIYDrones - good resource.

I have some code to frame messages (https://github.com/bolderflight/framing). It uses 4 bytes of overhead to mark the start and end of the message and provide a checksum. My biggest hurdle is figuring out how to modify Q Ground Control or Mission Planner to use new packets.

Good luck on that one. When I was rumaging around both there are alot of moving parts that you have to look at. Although I think there is only one function on receiving packets - way too long ago to remember at this point.

[brtaylor]

Good luck on that one. When I was rumaging around both there are alot of moving parts that you have to look at. Although I think there is only one function on receiving packets - way too long ago to remember at this point.

Thanks! It seems pretty daunting and I'm not sure if it'll be successful. Probably faster to just make another GCS from scratch.

[brtaylor]

FYI, I did a pretty major update to the MAVLink library:
https://github.com/bolderflight/mavlink

It now supports:
1. The heartbeat protocol
2. Sending status messages to the ground station
3. Sending system, battery, IMU, GNSS, navigation filter, actuator, and inceptor telemetry to the ground station
4. The parameter microservice
5. Mission / flight plan, fence, and rally points

It's written around our CMake build tooling. Hope to get an Arduino compatible version released, but in the mean time I hope this helps anyone implementing a MAVLink protocol.

[mjs513]

FYI, I did a pretty major update to the MAVLink library:
https://github.com/bolderflight/mavlink

It now supports:
1. The heartbeat protocol
2. Sending status messages to the ground station
3. Sending system, battery, IMU, GNSS, navigation filter, actuator, and inceptor telemetry to the ground station
4. The parameter microservice
5. Mission / flight plan, fence, and rally points

It's written around our CMake build tooling. Hope to get an Arduino compatible version released, but in the mean time I hope this helps anyone implementing a MAVLink protocol.

Very cool Brian. Eventually may get back to this one when other distractions settle down. Think you know how that goes

[MGeo]

Hi mjs513,

Thanks for sharing and documenting this example. I'm trying to climb the Mavlink learning curve, simple examples help for sure.

I'm trying to retrace your steps, I'm working through the library dependencies. Here is what I have so far:

• I found ublox2.h here, not sure if is the correct version https://forum.pjrc.com/threads/36103...l=1#post193345
• Streaming https://github.com/janelia-arduino/Streaming
• PWMServo https://github.com/PaulStoffregen/PWMServo
• Adafruit_Sensor https://github.com/adafruit/Adafruit_Sensor
• Adafruit_BNO055 https://github.com/adafruit/Adafruit...fruit_BNO055.h
• Adafruit_BusIO https://github.com/adafruit/Adafruit_BusIO

I searched the forum here and found I needed to comment out "utils/debug.h" from TeensyThreads.cpp (https://forum.pjrc.com/threads/68192...l=1#post288745).

Currently stuck on a macro error (below). Any help would be much appreciated. Ubuntu 18.04/Teensyduino 1.55.

Thanks,
George

Error from IDE

In file included from /opt/arduino-1.8.16/hardware/tools/arm/arm-none-eabi/include/c++/5.4.1/bits/char_traits.h:39:0,
                 from /opt/arduino-1.8.16/hardware/tools/arm/arm-none-eabi/include/c++/5.4.1/string:40,
                 from /home/geo/Arduino/MavlinkRoverMav3drv3/MavlinkRoverMav3drv3.ino:15:
/opt/arduino-1.8.16/hardware/tools/arm/arm-none-eabi/include/c++/5.4.1/bits/stl_algobase.h:243:56: error: macro "min" passed 3 arguments, but takes just 2
     min(const _Tp& __a, const _Tp& __b, _Compare __comp)

Copy of includes from top of MavlinkRoverMav3drv3.ino (for reference)

#include <Wire.h>
#include "./MavLink/common/mavlink.h"
#include "mavGlobals.h" 
#include <inttypes.h>
#include <ublox2.h>
#include "Streaming.h"
#include <string>

#include "TeensyThreads.h"
#include "EEPROM.h"
#include <PWMServo.h>

#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>

[mjs513]

@MGeo
Wow - haven't touched this in a long while. But will see what I can do. Not sure if its going to a Linux issue or something else. I am running on Windows10 pc with the latest Teensyduino release 1.56beta2 but should still be compatible with 1.55 for what is going on with the sketch. I did just compile it again on my machine but can not seem to duplicate the error:

Opening Teensy Loader...
Sketch uses 63372 bytes (6%) of program storage space. Maximum is 1048576 bytes.
Global variables use 12028 bytes (4%) of dynamic memory, leaving 250116 bytes for local variables. Maximum is 262144 bytes.

Your error seems to be pointing to a conflict with #incluse <string> you could try using #include <string.h> to see itf that helps.

[MGeo]

Thanks mjs513,

I moved over to a Win 10 machine to remove a variable. Same error but changing to #include <string.h> as you mentioned fixed that error. I'm using IDE 1.8.13 if that matters, along with V1.55.

You must have a different ublox2 lib than the one I linked, as GPS.SetRATE takes 3 parameters instead of 2, so I got an error from line 61 of main INO file.

I changed that line to the following and things now appear to have compiled correctly.

  GPS.SetRATE(200, 200, false);                 // Navigation/Measurement Rate Settings, e.g. 100ms => 10Hz, 200 => 5.00Hz, 1000ms => 1Hz, 10000ms => 0.1Hz

Thanks for the help, I'll start scrounging for hardware.

George

// UBX-CFG-RATE (0x06 0x08)  Set Navigation/Measurement Rate Settings (100ms=10.00Hz, 200ms=5.00Hz, 1000ms=1.00Hz, Etc)
// Generate the configuration command's syntax
void UBLOX::SetRATE(uint16_t measRate, uint16_t navRate, bool printACK) {
  bool Supported = false;
  if ( printACK == true ) {
    Serial.print("Setting CFG-RATE (0x06 0x08) Navigation/Measurement Rate ");
    _pACK_RATE = true; // print ACK to USB
  }
  ...

[PaulStoffregen]

This was posted on the PJRC website. Here's the link.

https://www.pjrc.com/rover-with-mavlink-messaging/

[brtaylor]

Finally got around to updating my MavLink library so that it should now work with Arduino build systems:
https://github.com/bolderflight/mavlink

Supports the heartbeat, utility, telemetry, parameter, mission, and RTK microservices. Plan on adding the UTM (UAS Traffic Management) microservice in the near future.