BLOG

NAVIO: Visualizing Mahony AHRS

3DIMU

One of examples for Navio demonstrates the work of Mahony AHRS with data from the MPU9250. We’ve also made a simple but cool visualizer for it that you can run on your PC\Mac. Here’s the instruction how to run AHRS and visualizer:

On your PC\Mac

Download the archive with Navio utilities.

Extract the archive, enter the directory with 3DIMU utility and run it:

cd Navio/Utilities/3DIMU
python 3Dimu.py
On your Raspberry Pi

Clone Navio repository, navigate to the folder with AHRS example, compile and run it:

git clone https://github.com/emlid/Navio.git
cd Navio/C++/AHRS
make
./AHRS X.X.X.X 7000

Where X.X.X.X is an ip address of your PC. 7000 is the port number used in 3DIMU.

 

Now try to move your Navio and check how the “brick” on screen moves along with it.

NAVIO: Using RTKLIB for RTK GPS processing

Navio-RTKLIB

RTKLIB is a set of open source programs for GNSS data processing written by Tomoji Takasu. It’s main function is software processing of raw GNSS data in differential mode allowing to achieve centimeter or decimeter accuracy depending on the hardware used. It’s an amazing software as it provides the possibility to do RTK without spending a few thousand dollars on equipment. Navio Raw is equipped with U-blox NEO-6T GPS receiver that provides raw data necessary for RTK processing in RTKLIB.

More

Navio boards are in stock!

professor-farnsworth

Good news, everyone!

 

As we have just shipped all 200 boards to our Indiegogo supporters it is time to say thank you and make announcements about the project. It was a long journey for us to get from designing a product to presenting it on a crowdfunding and finally to delivering the perks. All this was only possible because of your support and suggestions along the way. Trust me, that means a lot to us. We are confident that this is just the beginning for Navio project and a lot more is still to come.

 

Today we present the Navio page. It includes a detailed description of the board, tutorials, downloads and much more. We will keep adding information there. Tutorials still require some polish, however they should be good to get you started when you receive your own Navio. If you already feel confident with Raspberry Pi and programming you can head straight to our GitHub Navio repository and check out drivers and examples for every function of Navio.

 

As soon as you receive the board, please read carefully the Getting Started page, as it has some valuable information about acceptable voltages and different ways to power your Navio and Raspberry Pi.

 

Another important leap forward, and something we have been working on is Navio support in the APM (ArduPilot). It is not in the mainline yet, but you can check out status and instruction how to build it here. There are several things that are still left to do, but we are now as close to the first autonomous flight on Raspberry Pi with Navio as never before.

 

Get a Navio while they last! We also offer accessories: GPS/GNSS antennas and DF13 wire packs – very handy for prototyping.

NAVIO: Getting accelerometer, gyroscope and magnetometer data from MPU9250 [C++]

MPU9250 is one of the best in class inertial sensors, which combines a gyroscope, an accelerometer and a magnetometer in one device. The MPU sensor family is not only popular as a part of drone autopilot projects, but is also widely used in devices like cellphones, tablets, etc.

MPU9250 example

If you haven’t already done that, download NAVIO drivers and examples code like this:

git clone www.github.com/emlid/navio

Move to folder Navio/Examples/AccelGyroMag, compile and run the example

cd Navio/Examples/AccelGyroMag
make 
./AccelGyroMag

You should immediately see 9 values, updated in real time. Try to move the device around and see them change. They include Accelerometer, Gyroscope and Magnetometer data, three axis each.

MPU9250 driver

Let’s see the example code:

#include "Navio/MPU9250.h"

int main()
{
   MPU9250 imu;
   imu.initialize();

   float ax, ay, az, gx, gy, gz, mx, my, mz;

   while(1) {
      imu.getMotion9(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz);

      printf("Acc: %+05.3f %+05.3f %+05.3f ", ax, ay, az);
      printf("Gyr: %+05.3f %+05.3f %+05.3f ", gx, gy, gz);
      printf("Mag: %+05.3f %+05.3f %+05.3fn", mx, my, mz);

      sleep(0.5);
   }
}

The first thing we should pay attention to is the line imu.initialize(), as it’s does an important job of setting internal device parameters:

bool MPU9250::initialize(int sample_rate_div, int low_pass_filter)
{
    uint8_t i = 0;
    uint8_t MPU_Init_Data[MPU_InitRegNum][2] = {
        {0x80, MPUREG_PWR_MGMT_1},     // Reset Device
        {0x01, MPUREG_PWR_MGMT_1},     // Clock Source
        {0x00, MPUREG_PWR_MGMT_2},     // Enable Acc & Gyro
        {low_pass_filter, MPUREG_CONFIG},  // Use DLPF set Gyroscope bandwidth 184Hz, temperature bandwidth     188Hz
        {0x18, MPUREG_GYRO_CONFIG},    // +-2000dps
        {0x08, MPUREG_ACCEL_CONFIG},   // +-4G
        {0x09, MPUREG_ACCEL_CONFIG_2}, // Set Acc Data Rates, Enable Acc LPF , Bandwidth 184Hz
        {0x30, MPUREG_INT_PIN_CFG},    //
        //{0x40, MPUREG_I2C_MST_CTRL},   // I2C Speed 348 kHz
        //{0x20, MPUREG_USER_CTRL},      // Enable AUX
        {0x20, MPUREG_USER_CTRL},       // I2C Master mode
        {0x0D, MPUREG_I2C_MST_CTRL}, //  I2C configuration multi-master  IIC 400KHz

        {AK8963_I2C_ADDR, MPUREG_I2C_SLV0_ADDR},  //Set the I2C slave address of AK8963 and set for write.
        //{0x09, MPUREG_I2C_SLV4_CTRL},
        //{0x81, MPUREG_I2C_MST_DELAY_CTRL}, //Enable I2C delay

        {AK8963_CNTL2, MPUREG_I2C_SLV0_REG}, //I2C slave 0 register address from where to begin data transfer
        {0x01, MPUREG_I2C_SLV0_DO}, // Reset AK8963
        {0x81, MPUREG_I2C_SLV0_CTRL},  //Enable I2C and set 1 byte

        {AK8963_CNTL1, MPUREG_I2C_SLV0_REG}, //I2C slave 0 register address from where to begin data transfer
        {0x12, MPUREG_I2C_SLV0_DO}, // Register value to continuous measurement in 16bit
        {0x81, MPUREG_I2C_SLV0_CTRL}  //Enable I2C and set 1 byte

    };

for(i=0; i < MPU_InitRegNum; i++) {
WriteReg(MPU_Init_Data[i][1], MPU_Init_Data[i][0]);
usleep(10000); //I2C must slow down the write speed, otherwise it won't work
}

set_acc_scale(BITS_FS_16G);
set_gyro_scale(BITS_FS_2000DPS);

calib_mag();
return 0;
}

Note that this function also sets scales for both Accelerometer and Gyroscope:

{0x18, MPUREG_GYRO_CONFIG},    // +-2000dps
{0x08, MPUREG_ACCEL_CONFIG},   // +-4G

The main function loop is pretty straightforward: read the data, print the data.

You can find all the useful information about MPU9250, including the datasheet and the register map here.

NAVIO: Reading and storing data in MB85R FRAM [C++]

MB85R

Ferroelectric RAM has the same functionality as flash memory, but increased overall write speed, lower power consumption and significantly bigger maximum number of write-erase cycles. All this makes FRAM-type memory devices a popular choice in embedded systems. The model we used on NAVIO is Fujitsu’s MB85RC04 with 4096 bytes of storage. It is connected over I2C. MB85RC04 uses a special I2C addressing system, shown in the image below:

FRAMaddressing

S is a start condition, part of theI2C protocol. It is followed by bits 1010, which are a device address prefix. Then, come three upper bits of the register address. On our device, we use 9-bit addresses, which means A2 and A1 always remain zeroes. After this comes a Read/Write bit(on this picture it is a 0, representing a write operation). Then, the device has to send back an Acknowledge statement(A on the white background). After the ‘ACK’ is received, I2C master sends the lower 8 bits of the register address. After another ‘ACK’, it sends the write data to the FRAM device.

FRAM example:

Our FRAM example is in the Navio/Examples/Fram folder. Fram.cpp file contains a sequence of write/read tests, which write and then read specific values under the specific address of the memory. If the values read are the same as the values written, then the test is passed.

Change the directory to Navio/Examples/Fram, then make and run the example:

cd Navio/Examples/Fram
make
sudo ./Fram

After that, you will see program’s output, showing what it wrote, what it read afterwards and if the memory test is passed(pay attention to the last line of the output).

FRAM driver:

To interact with I2C devices, we use the I2Cdev library, and the FRAM driver is a simple extension to it. It takes care of the 9-bit registeraddressing and also keeps the device address. Let’s take a look at the function, which writes one byte:

uint8_t MB85RC04::writeByte(uint16_t register_address, uint8_t data)
{
   bool ninth_bit = register_address & 0x100;
   uint8_t dev_address = device_address | ninth_bit;
   return I2Cdev::writeByte(dev_address, register_address, data);
}

It has two input parameters: a 16-bit register_address, which is a memory write location, and a data byte, which is to be written to the device. Our memory has the capacity of 512 bytes, so it uses a 9-bit address for inside memory mapping. It takes the 9 least significant bits from the register_address parameter and ignores the rest. MB85RC04 user manual requires to use the 9th bit as a part of the device address in the I2C write sequence, so it’s all complied to one variable dev_address. After that, we use the standard I2Cdev function to write a single byte over I2C.

NAVIO: Measuring voltage with ADS1115 [C++]

ads1115

The ADS1115 is a precision analog-to-digital converter (ADC) with 16 bits of resolution. The ADS1115 features an onboard reference and oscillator. Data is transferred via an I2C serial interface. The ADS1115 can perform conversions at rates up to 860 samples per second (SPS). Both large and small signals to be measured with high resolution ranging from 156mV to 3.3V. The ADS1115 also features an input multiplexer (MUX) that provides two differential or four single-ended inputs. The ADS1115 operates either in continuous conversion mode or a single-shot mode that automatically powers down after a conversion and greatly reduces current consumption during idle periods.

ADS1115 example

Move to the folder with the source code, compile and run the example:

cd Navio/Examples/ADC
make
sudo ./ADC

In this example ADC continuously measures the voltage on P1 available on the DF13 connector marked “ADC”. Result in mV is printed to console. You can measure other channels in the same manner.

ADS1115 driver

#include "Navio/ADS1115.h"

int main() {

	ADS1115 adc(RASPBERRY_PI_MODEL_B_I2C, ADS1115_DEFAULT_ADDRESS);
	//ADS1115 adc(RASPBERRY_PI_MODEL_A_I2C, ADS1115_DEFAULT_ADDRESS);

	adc.setMultiplexer(ADS1115_MUX_P1_NG);
	adc.setMode(ADS1115_MODE_CONTINUOUS);

	while (true) {
		float mV = adc.getMilliVolts();
		printf("mV: %fn", mV );
	}

	return 0;
}

For more detailed information please refer to the ADS1115 datasheet.

NAVIO: Controlling RGB LED and servos with PCA9685 [C++]

pca9685

PCA9685 is a PWM generator chip that can be used to control servos and LEDs. It features:

  • 16 channels with separate control
  • 12-bit resolution
  • Configurable frequency
  • I2C operation up to 1MHz
  • Output enable pin

PCA9685 is clocked by the 24.576MHz TCXO oscillator and allows to adjust frequency using the PRE_SCALE register. Rising and falling edges for each channel can be set from 0 to 4095 allowing to control duty cycle as well as phase.

Controlling RGB LED

Channels 0, 1, 2 are connected to Navio’s onboard RGB LED. Since that is a LED with a common anode the logic is inverted – lowest value (0) will result in max brightness for the channel and vice versa.

You can try controlling RGB LED by running the provided example.

Move to the folder with the source code, compile and run the example

cd Navio/Examples/LED
make
sudo ./LED

In this example channels values for RGB LED are slowly decreased and increased creating the mix of different colors.

#include "Navio/PCA9685.h"

int main()
{
    PCA9685 pwm(RASPBERRY_PI_MODEL_B_I2C, PCA9685_DEFAULT_ADDRESS);
    // PCA9685 pwm(RASPBERRY_PI_MODEL_A_I2C, PCA9685_DEFAULT_ADDRESS);
    
    pwm.initialize();

    uint16_t R = 0, G = 0, B = 4095;

    pwm.setPWM(0, R);
    pwm.setPWM(1, G);
    pwm.setPWM(2, B);

    while (true) {
        for (R = 0; R < 4095; R++)             
        pwm.setPWM(0, R);         
        for (B = 4095; B > 0; B--)
        pwm.setPWM(2, B);

        for (G = 0; G < 4095; G++)             
        pwm.setPWM(1, G);         
        for (R = 4095; R > 0; R--)
        pwm.setPWM(0, R);

        for (B = 0; B < 4095; B++)             
        pwm.setPWM(2, B);         
        for (G = 4095; G > 0; G--)
        pwm.setPWM(1, G);
    }

    return 0;
}

Controlling servos

Channels 3-15 are available on 2.54mm header pins numbered 1-13 accordingly. Servos can be controlled by setting the correct frequency (50hz is a common frequency) and duty cycle corresponding to the length of a pulse (usually between 1 and 2 milliseconds).

To try to control servo connect the servo to the Navio’s output channel number 1 and run the provided example.
Move to the folder with the source code, compile and run the example

cd Navio/Examples/Servo
make
sudo ./Servo

In this example we perform the following:

  1. Initialize the PCA9685
  2. Set the frequency
  3. Set the channel’s value thus controlling the pulse length.

To set the pulse range appropriate for your servo you can change the SERVO_MIN and SERVO_MAX values.

#define NAVIO_RCOUTPUT_1 3
#define SERVO_MIN 1.250 /*mS*/
#define SERVO_MAX 1.750 /*mS*/

#include "Navio/PCA9685.h"

int main()
{
    PCA9685 pwm(RASPBERRY_PI_MODEL_B_I2C, PCA9685_DEFAULT_ADDRESS);
    // PCA9685 pwm(RASPBERRY_PI_MODEL_A_I2C, PCA9685_DEFAULT_ADDRESS);


    pwm.initialize();
    pwm.setFrequency(100);

    while (true) {
        pwm.setPWMmS(NAVIO_RCOUTPUT_1, SERVO_MIN);
        sleep(1);
        pwm.setPWMmS(NAVIO_RCOUTPUT_1, SERVO_MAX);
        sleep(1);
    }

    return 0;
}

PCA9685 registers

Here’s a short description of the registers:

0x00 – MODE1 – controls sleep, restart, auto-increment, external clocking, addressing

0x01 – MODE2 – controls inversion, output condition, drive type

 

Settings for additional addresses

0x02 – SUBADDR1

0x03 – SUBADDR2

0x04 – SUBADDR3

0x05 – ALLCALLADDR

 

Channel control registers: 

0x06 – LED0_ON_L – low byte of 12-bit word for rising edge of channel 0

0x07 – LED0_ON_H – high byte of 12-bit word for rising edge of channel 0

0x08 – LED0_OFF_L – low byte of 12-bit word for falling edge of channel 0

0x09 – LED0_OFF_H – high byte of 12-bit word for falling edge of channel 0

 

Registers for other channels are similar and their addresses are shifted by 4 per channel, for example, channels for LED1 are: 

0x0A – LED1_ON_L

0x0B – LED1_ON_H

0x0C – LED1_OFF_L

0x0D – LED1_OFF_H

 

To control all channels at once it is possible to use ALL_LED registers: 

0xFA – ALL_LED_ON_L

0xFB – ALL_LED_ON_H

0xFC – ALL_LED_OFF_L

0xFD – ALL_LED_OFF_H

 

To set the frequency use the following register:

0xFE 0 PRE_SCALE – minimal value is 3

How does the PCA9685 driver work?

At first, we call for initialize() method which restarts the device by first setting the SLEEP_BIT to ‘0’ in MODE1 register and then setting the RESTART_BIT to ‘1’ in the same register. Also, it sets the AI_BIT to ‘1’ which allows to perform multi byte I2C operations for faster data transfer.

 

setFrequency() method is used to control the output frequency. It calculates the prescale value based on the required frequency and writes it to the PRE_SCALE register.

 

setPWM() method allows to set the duty cycle from 0 to 4095 for the output channel. It can be used to control LED’s brightness. It writes four bytes of data representing the rising edge (LED_ON) and falling edge cycles (LED_OFF) to the following registers: LEDx_ON_L, LEDx_ON_H, LEDx_OFF_L, LEDx_OFF_H where x is the output channel number.

 

setPWMmS() or setPWMuS() methods allow to set the pulse length of the output channel. It can be used to control servos or other PWM-controlled devices. Internally, they use setPWM() method to set the duty cycle which is calculated based on the required length and the clock frequency.

 

To get more information about PCA9685 please refer to the PCA9685 data sheet.

NAVIO: building and running APM (ArduPilot)

APM

It is possible to run APM (ArduPilot) on Raspberry Pi with Navio. The autopilot’s code works directly on Raspberry Pi using the APM’s Linux HAL. Even though it is possible to run APM on standard Raspbian distribution it won’t work properly as it requires lower latency. Please use Raspbian with real time kernel for running APM, you can get it here.

State of the ArduPilot port to Raspberry Pi with Navio

Please keep in mind that the code for Navio is in the experimental state.

What has been added:

  • Raspberry Pi build configuration
  • Navio board configuration
  • GPIO driver for Raspberry Pi
  • RCOutput based on PCA9685
  • RCInput that uses pigpio daemon to sample GPIOs with 1MHz rate (to be rewritten to work without pigpio)

These things were already supported in APM’s library and worked with minimal configuration:

  • MPU9250 inertial sensor
  • MS5611 barometer
  • Serial port

Left to do:

  • MPU9250 built-in compass driver – the code has been written and works fine, just requires some polish
  • U-blox GPS SPI driver – in progress, we are trying to integrate GPS SPI support in APM without too much hacking
  • ADS1115 ADC support – haven’t started yet
  • RGB LED – not started, but should be easy

Where to get the code

Working repository for Navio APM port is on our GitHub.

Changes are being pushed in the main ArduPilot repository after some time.

Building APM

Download the APM’s port for Navio to your Raspberry Pi:

git clone https://github.com/emlid/ardupilot.git
git checkout navio

 

Navigate to the autopilot’s directory, for example rover, run configuration and build it:

cd ardupilot/APMrover2
make configure
make navio

Running APM

Executable file will be placed in /tmp/APMrover2.build directory, navigate there and run it:

sudo ./APMrover2.elf -A tcp:*:7000:wait

Arguments specify serial ports: -A is for telemetry, -B is for GPS, -C is for secondary telemetry. TCP can be used instead of serial ports.

APM Planner

APM Planner is a ground station software for APM. It can be downloaded from the ardupilot.com

Run APM Planner and enable Advanced Mode in the File menu.

Navigate to Communication – Add Link – TCP and enter the IP address of your Raspberry Pi and specify TCP port number then press connect.

MAVProxy

MAVProxy is a console-oriented ground station software written in Python that can be used standalone or together with APM Planner. It’s well suited for advanced users and developers. MAVProxy can be installed with pip:

pip install mavlink mavproxy console wp

 

To run it specify the –master port, which can be serial or TCP. It also can perform data passthrough using –out option (14550 is the default TCP port for APM Planner).

mavproxy.py --master tcp:192.168.1.40:7000 --out 127.0.0.1:14550 --console

Load More