Adding a new serial read driver to ArduPilot

Who this is for: Someone whose programming experience is limited to arduino sketches but wants to make some simple changes to a custum build of the ardupilot code that they are uploading to pixhawk. A lot of what follows should work for other hardware platforms as well but some is specific to pixhawk.

Who this is not for: Someone who wants to have their changes added to the main ardupilot release. The reason for this is that I have bypassed the hardware abstraction layer (hal) mostly because I couldn't totally figure out how to make use of serial manager.

Steps

1. Read: Ardupilot Wiki

2. Follow the instructions on the wiki to get a local copy of the code,

3. Learn how and actually build the code and upload it onto a pixhawk so you know that you can.

4. Learn how to interface with pixhawk over a serial terminal. You will need an ftdi or other serial to usb cable. You may find these links helpful during this step (I sure did!) Sparkfun's Serial Connection Tuturial and Serial Terminal Basics

5. Do the Uart Example (actually build the code and play with the output on a serial terminal). There are many other good examples to play around with and learn on the developers wiki and it is not a bad idea at a bare minimum to read through the code in all of them to learn some new things.

So now you have read through the Ardupilot wiki and played around with the code and you can build and upload that code to a pixhawk and see output on a serial terminal. Now you want to write a serial driver that can read serial information on some serial port and save that data to a variable that you can then do something with (I just wanted to send the data over mavlink to my GCS).

By playing with the Uart Example you should have recognized the similarities with an arduino sketch. There is a setup and loop and the only thing that differs greatly are these things that have to do with AP_HAL. What you should already understand from reading the Ardupilot wiki is that AP_HAL has to do with making the code portable to all kinds of different hardware platforms. Because I don't completely understand exactly how this all works I can't tell you an awful lot more about it but it is helpful to at least understand this much.

At this point it is time to get going writing our simple serial read driver. I will be showing you how I did this with the rover code but the same should work for plane or copter. What it took me a very long time to figure out was where the setup and loop functions were located in the rover code. You will find them in a file called "APMrover2.cpp" starting around line 90 at the time of this post. Notice how little there is in them for such a large codebase. This is where I took a massive shortcut.....

In the void Rover::setup() function add this line hal.uartE->begin(57600); // this sets up uartE with a buadrate of 57600 but you could choose whichever rate you want. You should know this already from playing around with the uart example but uartE == serial 4 on the pixhawk. This is where I deviated from using the HAL this is only going to work on boards like the pixhawk that actually have a uartE.

Now you could make use of uartE by writing some code in the loop function of "APMrover2.cpp" but because you read through the wiki you know why this would be very wrong. Instead we want to write our own function that will be called by the scheduler. It is also useful to put this function in it's own file, the reason for this is that as the main ardupilot code is updated when you use git to retrieve the most up to date code you won't have all of your work undone. I created a file called "tractor.cpp" the code in the file is as follows: notice the lines which are commented out that you can use for debugging on uartE with the uart to usb cable you made earlier.

`// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Rover.h"

void Rover::update_tractor_health()
{
	int buff_len;
	buff_len = hal.uartE->available();
    for(int i = buff_len-1; i >=0; i--){
		if(5 > i){
			tractor_health_info[i] = hal.uartE->read();
			//~ hal.uartE->printf("i = %d tractor_health_info[i] = %c \n", i, tractor_health_info[i]);
		}
		else{
			hal.uartE->read();
		}
	}
	//~ hal.uartE->printf("Read Tractor Health it is.....\n");
	//~ hal.uartE->printf(tractor_health_info);
}`

You should have a few questions at this point...

1. Where did you declare tractor_health_info[]? The answer is in Rover.h around line 108 it says private: following is an indented block of code that declares various variables. I could have declared a variable in my function update_tractor_health() but I wanted to be able to reference the result in mavlink message so I declared the variable in rover.h

2. How do you get this function to run? You need to add the function update_tractor_health() after line 383 in rover.h to in the indented block of private: functions by adding the line void update_tractor_health(); Then in APMrover2.cpp around line 52 is the beginning of the scheduler block add your function to be run pick how often it should run and how long it will take. I totally guessed at how long it will take. There is probably a better way to do this. I wanted to my function to run every 100ms so I added this line SCHED_TASK(update_tractor_health, 5, 1500),

That should do it. Now you have a new serial read driver that is reading serial data on uartE and storing the result in a variable that you can do something with. I am sending the 5 fields of this variable as a mavlink message to my GCS. The documentation on adding a mavlink message was more complete so I wont say too much about it here. One thing that took me a little while to figure out was that you need to modify the mavlink xml file on both ends ie on the pixhawk and on your computer that is receiving the messages.

To install on ubuntu:

1. use git to download mavlink repository and include pymavlink submodule
2. replace common xml files in mavlink repository with file from ardupilot tractor_health fork
3. run setup.py in pymavlink "sudo python setup.py install"

I am sending the serial data from an arduino with a can bus shield that is attached to the can port of my tractor. It looks for can messages dealing with engine rpm, coolant temp, oil pressure, and fuel level, you can find the code here I commented out the line that converts the two rpm fields to rpm. To convert fuel level multiply by 0.4 and the result is the percentage of fuel in the tank. Coolant is 1 degree Celsius per bit with a -40 offset and oil pressure is 4kpa/bit with no offset. You can see more information on this in my gcs here just search for "rpm" to quickly get to the relevant section of code.