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)