Injecting fake GPS Data into APM 2.6

I recently got a new version of APM2.6 controller which would be used for the catamaran project or other projects later on. Since we have a Vicon camera system in the lab which can provide us position and orientation data, we try to replace the raw GPS data from the GPS sensor on the board with the data we get from the camera. The protocol the APM controller using is called MAVLink that could also be integrated in the DUNE system - the system we are currently using to communicate with Vicon Cameras. If you are curious about how to send/get data between Dune and APM, see this.

Now, Let's start the topic.
First of all, we need to download the source code. Since APM using github to organize their code, we need to clone their git. The branch "ArduCopter-3.2.1" is the one we are using, later versions can not be fitted to our board.
make a new folder and do this

$ git clone -b ArduCopter-3.2.1 https://github.com/diydrones/ardupilot.git --single-branch ArduCopter-3.2.1

the rest of the settings could be found here. 

Now we can start to modify the code.
First in the "APM_Config.h" header file:
Since we are using Vicon Camera System as our source of GPS data,  We can add this line:
#define GPS_PROTOCOL    GPS_VICON
This is pretty much for "APM_Config.h" file.

Next is the "GCS_Mavlink.pde" file, which handles all the MAVLink messages between vehicle and ground station. In the line 1435, we can add the following:
#if GPS_PROTOCOL == GPS_VICON
    case MAVLINK_MSG_ID_GPS_RAW_INT:

        mavlink_gps_raw_int_t packet;

        mavlink_msg_gps_raw_int_decode(msg, &packet);

        // set gps hil sensor
        Location loc;
        loc.lat = packet.lat;
        loc.lng = packet.lon;
        loc.alt = packet.alt;
        // loc.alt = packet.alt/10;

        // hardcode gps data to current location
        current_loc = loc;
       
        // current velocity ???
        Vector3f vel(0, 0, 0);
        vel *= 0.01f;

        gps.setHIL(0, AP_GPS::GPS_OK_FIX_3D,
                   packet.time_usec/1000,
                   loc, vel, 10, 0, true);

    break;
#endif

The basic thing these lines do is to tell the APM if we are set as GPS_VICON mode and we get a MAVLINK_MSG_ID_GPS_RAW_INT message, we will call this gps.setHIL() function to treat the incoming gps as real gps data. One more thing to point out is that the variable "current_loc" stores the current location information of the vehicle.

 Next is the "ArduCopter.pde" file, which is the main loop of all the code. Comment out the line 1235 like this:
// gps.update();
Since this update function is calling the hardware sensor on the board to update the gps data at a frequency of 50Hz, but we don't want to use this on board sensor, so just comment this line out. If you don't do that, you might have to plug in a GPS module, even though it does nothing.
One more thing to mention is that if we have some user defined function want to run at some particular frequency in every loop, we can add our function in the "AP_Scheduler::Task scheduler_tasks[] PROGMEM" line 777(fast mode: e.g. Pixhawk) or line 845(slow mode: e.g. APM2)
#ifdef USERHOOK_FASTLOOP
    { userhook_FastLoop,     1,    100  },
#endif
#ifdef USERHOOK_50HZLOOP
    { userhook_50Hz,         2,    100  },
#endif
#ifdef USERHOOK_MEDIUMLOOP
    { userhook_MediumLoop,   10,    100 },
#endif
#ifdef USERHOOK_SLOWLOOP
    { userhook_SlowLoop,     30,    100 },
#endif
#ifdef USERHOOK_SUPERSLOWLOOP
    { userhook_SuperSlowLoop,100,   100 },
#endif

Make sure to define these macros in the "APM_Config.h" before calling them.

The last file we might need to modify is the "system.pde". If you never solder your board after you get it, you don't need any modification here. By default, the APM2.6 is always using UART0 for telemetry But for some reason, my board got soldered and now it is using UART2 for telemetry. However the ArduCopter-3.2.1 code never use UART2 on APM2.6, so we have to change a little bit. Comment out line 166 and line 177 like this:
//#if CONFIG_HAL_BOARD != HAL_BOARD_APM2
    // we have a 2nd serial port for telemetry on all boards except
    // APM2. We actually do have one on APM2 but it isn't necessary as
    // a MUX is used
    gcs[1].setup_uart(hal.uartC, map_baudrate(g.serial1_baud), 128, 128);
//#endif

make sure to leave the gcs[1].setup_uart() line uncommented.

I have already upload the modified code to my google drive so that you can download here if you would like to.

Now all the modifications are done, we can send MAVLink message to the APM either from USB cable(115200 baud-rate) or Xbee(57600 baud-rate)

How to compile and upload code to an APM1.4(mega1280) board under Ubuntu

The reason why I post this blog is that recently I am working on a project of catamaran in our Lab, in which we are using ArduPilot controller. However, the chip we have is pretty old, an Atmega1280, which is used probably 5 or 6 years ago. (The current version is Atmega2560 which has a double memory size). Since I don't have a newer version one, I have to google all the possible websites and try to figure out a way to make this old one work. Fortunately, I find some useful instructions and helpful resources eventually so that I could compile and upload a modified code to this obsolete chip.

Alright, Let's get started.

1. First of all, ArduPilot has its own new website here. Apparently, this website is for their latest boards. If you have a new board, follow the instruction on that website and you will have a basic idea how it works. Since I am using Ubuntu, I need some core packages to build the code under Linux. The required packages are listed on this page, simply run this line in the terminal:

$ sudo apt-get install gcc-avr avrdude avr-libc binutils-avr

2. The next step is to install Arduino IDE in the Ubuntu. The currently version of Arduino in the Ubuntu repository is 1.0.5. However the code of the ArduPilot is not compilable in this version. As far as I know, one reason is that in the 1.0.5 Arduino IDE, "PROGMEM" variables are supposed to be defined as const variables, but in the ardupilot code, they are not.

Anyway, we first install the arduino from the repository. Run this line in the terminal:

$ sudo apt-get install arduino

You can download this file(arduino-1.0.3-linux32.tgz). I share this file from my google drive. You can also find it online. After you download the file, unzip it using this line:

$ tar xvzf arduino-1.0.3-linux32.tgz

In this way, you will have a 1.0.3 arduino working in the ubuntu.

3. Next, get an old version code. As I mentioned at the beginning, the current code from their developer github or the firmware from their website can not be fitted into an Atmega1280 sadly:(. So we have to find their previous deprecated ardupilot-mega project website here. Go to the "Downloads" tag and choose "All downloads" in the search drop down menu. They also have their old ardurover and arducopter download websites.
I also shared a copy of ArduPlane2.40.zip from my google drive here just in case those links are broken.

4. After you download the code, unzip the file and copy the libraries folder to arduino-1.0.3 and merge with the libraries there.

5. Go to the arduino folder and run this line to open the arduino-1.0.3

$ ./arduino

Change the sketchbook location in the preference to the source code folder of the Arducopter or ArduPlane.

Open the ArduCopter.pde. Based on the instruction here, add these lines at the bottom of the APM_Config.h header file:

# define GPS_PROTOCOL GPS_PROTOCOL_MTK16
// # define TELEMETRY_UART2         DISABLED
# define CONFIG_RELAY            DISABLED
# define CAMERA                 DISABLED
# define MOUNT                  DISABLED
// # define LOGGING_ENABLED        DISABLED //for APM1 1280 logging is disabled by default
# define CLI_ENABLED            DISABLED

which can make the size of the code small enough to fit a 1280 board.

Choose Atmega1280 in the board selection and also select the correct serial port as well.
Finally, click the verify button to compile the code and then upload button to upload the code to the board.

6. We can Install the APMPlaner, which is a simulation software for ArduPilot. They have an ubuntu trusty version. I install the latest one on my laptop and it still can connect to my old mega1280 chip and show the roll, pitch and yaw data  neatly.

Setting USB serial port on VirutalBox [Ubuntu host with WindowsXP guest]

This blog is an instruction showing how to setup a VirtualBox on Ubuntu host for the USB serial port communication to a Windows guest.

The first step is to install the VirtualBox 4.3.26 Oracle VM VirtualBox Extension Pack. Follow the instruction on that website.
The latest version is 4.3.26 currently. Make sure install the VirtualBox from this official website as well since the one in the Ubuntu repository is kinda out of date.

The second step is to Enable USB 2.0 (EHCI) Controller as shown below



The third step is to setup the serial ports for the virtual machine.
Go to the "Serial Ports" tag and check the "Enable Serial Port"
Choose "COM1" in the Port Number drop down menu, This is the port showing in the WindowsXP.
Choose "Host Device" in the Port Mode section
Type the ubuntu port in the Port/File Path. To figure out what is the Port Path of your current device, open your terminal (Ctrl+Alt+T), type

$ dmesg | grep tty

Usually the port path in Ubuntu might be "/dev/ttyUSB0" or "/dev/ttyACM0" etc..
The total configuration of step 3 is shown as follows



Now all the configuration is done, once you turn on the guest side windows, you will have the COM1 as a serial port. If you do not have the usb device connected to your Ubuntu, make sure you disable the serial port selection in the configuration, otherwise virtualbox will have errors saying "Failed to open host device '/dev/ttyUSB0'"

This is the how my windows guest looks like and now I can upload the code through this COM1 port to the Arduino chip just like what it does in the real windows environment.