APM 2.5 Hello World and Serial Communication

Hello World

When using the Arduino-based APM, you’ll need to download the Ardupilot version of the Arduino IDE to use their Hardware Abstraction Layer (HAL) libraries. Instructions on installation and set up of the Ardupilot version of IDE as well as how to compile and upload the complete open source ArduCopter code can be found on the Ardupilot website. I recommend downloading the full ardupilot source code from Github as it contains libraries you may want to use, as well as example scripts on how to access some of the board’s capabilities. Note that the APM hardware is no longer supported, so you’ll need to download an older version (branch in Github); Version 3.2.1 is the last version which supported the APM boards.

Once you have the Arduino IDE setup for the APM, the following code is a useful first test to see if you can successfully upload your compiled code to the board. This code also tests if you are able to receive data from the APM over serial.

USB: As a first step try this code with the APM connected to the computer over USB, which you’ll need to upload the code to begin with anyways. After the code has been successfully uploaded, open the Serial Monitor in the Arduino IDE and set the bottom-right options of the GUI to “No line ending” and “57600 Baud.” Normally USB is transmitted at 115200 Baud, however I set this lower rate so the same code can transmit over USB or SiK Radio. If everything worked correctly, you should see the Hello World loop printed to the serial monitor.

Telemetry Radio: The USB and Telemetry output are both accessed through “hal.console,” however the USB has priority. So you will not be able to transmit over telemetry if the USB is connected. With the SiK radio connected to the telemetry port of the APM, the serial monitor in the Arduino IDE should be able to read the serial over the COM port the same as if it were connected through USB.

 

Notes:

If you have not used the SiK Radio on the computer before, you may need to install a Virtual COM FTDI driver from here: http://www.ftdichip.com/Drivers/VCP.htm

If you only see nonsense characters in the serial port, make sure you have the correct Baud Rate selected and it matches what is defined in the code which was uploaded to the APM.

The APM resets every time you open the Serial Monitor, so if you close and reopen the Serial Monitor the Arduino sketch will start from the beginning.

 

#include <AP_HAL.h>
#include <AP_HAL_AVR.h>
#include <AP_HAL_AVR_SITL.h>
#include <AP_Common.h>
#include <StorageManager.h>
#include <AP_Param.h>
#include <AP_Progmem.h>


// Identify which board type for Hardware Abstraction Layer (HAL)
const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;

uint16_t counter = 0;

void setup(void) {
  
    //Set baud rate to 57600 in order to transmit over SIK Radio
    hal.uartA->begin(map_baudrate(57600));
    hal.console->println_P(PSTR("Starting Printf test"));
}


void loop(void) 
{	
    
    // Prints statement to Arduino IDE serial at 57600 baud over USB or Radio
    hal.console->printf_P(PSTR("Hello World, loop %d\n"),counter);
    
    // Increment a counter up to 100 and repeat to show unique messages
    counter++;
    if (counter>100){
      counter = 0;
    }
    
    // Pause for 100 msec between print statements
    hal.scheduler->delay(100);
}

AP_HAL_MAIN();   //Required function call for APM

 

 

 

Two Way Communication

The next step is to check that not only can you receive data from the APM but can you also send it commands. The following script gives a simple, single character instruction to the flight controller and it prints a response to show it is processing everything correctly.

 

#include <AP_HAL.h>
#include <AP_HAL_AVR.h>
#include <AP_HAL_AVR_SITL.h>
#include <AP_Common.h>
#include <StorageManager.h>
#include <AP_Param.h>
#include <AP_Progmem.h>
#include <AP_Math.h>


// Identify which board type for Hardware Abstraction Layer (HAL)
const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;


void setup(void) {
    //Set baud rate to 57600 in order to transmit over SIK Radio
    hal.uartA->begin(map_baudrate(57600));
    hal.console->println_P(PSTR("Starting hal.console printf and read test"));
}


void loop(void) 
{	
    int16_t user_input;
    int menuMode = 0;
    
    hal.console->println();
    hal.console->println_P(PSTR(
    "Menu:\r\n"
    "    a) Echo response\r\n"
    "    b) Calculate a number\r\n"
    "\n"
    "    Enter selection: "));

    // wait for user input
    while( !hal.console->available() ) {
        hal.scheduler->delay(20);
    }

    // read in user input
    while( hal.console->available() ) {
        user_input = hal.console->read();
    
        if( user_input == 'a' || user_input == 'A' ) {
            menuMode = 1;
        }  
    
        if( user_input == 'b' || user_input == 'B' ) {
            menuMode = 2;
        }
    }
    
    if (menuMode == 1){
      echo();
    }
    
    if (menuMode == 2){
      calc();
    }
        
}
    
void echo(){
  
    // Echo back what user inputs
    hal.console->println_P(PSTR("Watch me repeat back any letter you say.\n"
                         "Give me a letter."));
    hal.scheduler->delay(100);
    
    // wait for user input
    while( !hal.console->available() ) {
        hal.scheduler->delay(20);
    }
    
    
    // read in user input
    while( hal.console->available() ) {
        char char_input = hal.console->read();
    
        // Echo back to console
        hal.console->printf_P(PSTR("Echo: %c\n"),char_input);
    }
    
}
    
void calc(){
        
    // Echo back what user inputs
    hal.console->println("I can multiply by two.\n"
                         "Give me an integer: ");
    
    // wait for user input
    while( !hal.console->available() ) {
        hal.scheduler->delay(20);
    }
    
    int valueInput = 0;
    // read in user input
    while( hal.console->available() ) {
        int int_input = hal.console->read();
        
        // ASCII for 0 is 48
        if (int_input >= 48 && int_input <=57){ // is a key 0-9
            valueInput = valueInput*10 + int_input - 48;
        }
    }
    
    // Echo back to console
    hal.console->printf_P(PSTR("Your provided number is: %d\n"),valueInput);
    hal.console->printf_P(PSTR("Your number doubled = %d\n"),2*valueInput);
        
}


AP_HAL_MAIN();   //Required function call for APM

 

Leave a comment or ask a question: