Self-Aligning Satellite Dish in Rust: Compass

In our project we'll need to know the magnetic heading at any given position to help in precise alignment of the kit. For this we'll use a HMC5883L magnetometer module which communicates with other devices via I2c.

Requirements
Connections
I2C Configuration
HMC5883L Configuration
Final Code

Requirements

1 x Raspberry Pico board
1 x USB Cable type 2.0
1 x HC-05 Bluetooth module
1 x HMC5833L Compass Module
15 x M-M jumper wires
2 x Mini Breadboards

Connections

I2C Configuration

The I2C peripheral of the Pico will first need to be configured to enable communication with the HMC5883L. The Pico board has two similar I2C peripherals. We'll use I2C0 for this project.

Like most peripherals the I2Cs have associated pins which have to be configured. I2C0 can use gp4 as its data pin (SDA) and gp5 as its clock pin (SCL).

Under pins section in our set up we configure the pins as below.

// Configure gp4 as I2C0 SDA pin
pads_bank0.gpio(4).modify(|_, w| w
               .pue().set_bit() //pull up enable
               .od().clear_bit() //output disable
               .ie().set_bit() //input enable
               );

io_bank0.gpio(4).gpio_ctrl().modify(|_, w| w.funcsel().i2c()); 

// Configure gp5 as I2C0 SCL pin
pads_bank0.gpio(5).modify(|_, w| w
               .pue().set_bit() //pull up enable
               .od().clear_bit() //output disable
               .ie().set_bit() //input enable
               );

io_bank0.gpio(5).gpio_ctrl().modify(|_, w| w.funcsel().i2c());

We first have to attain a handle for I2C0.

// Set up I2C
let mut i2c0 = dp.I2C0;
resets.reset().modify(|_,w| w.i2c0().clear_bit());

The Pico will be set as the master and the magnetometer as the slave. The Pico's manual provides important information for this configuration.

i2c0.ic_enable().modify(|_, w| w.enable().clear_bit());// disable i2c
// select controller mode & speed
i2c0.ic_con().modify(|_, w| {
    w.speed().fast();
    w.master_mode().enabled();
    w.ic_slave_disable().slave_disabled();
    w.ic_restart_en().enabled();
    w.tx_empty_ctrl().enabled()
});
// Clear FIFO threshold
i2c0.ic_tx_tl().write(|w| unsafe { w.tx_tl().bits(0) });
i2c0.ic_rx_tl().write(|w| unsafe { w.rx_tl().bits(0) });

We have to set up the COUNT registers before using the I2C. From the RP2040's manual we learn that the minimum SCL high and low values are 600ns and 1300ns respectively.

These are used to find the high and low count periods which are used to program the respective counters.

// IC_xCNT = (ROUNDUP(MIN_SCL_HIGH_LOWtime*OSCFREQ,0))
// IC_HCNT = (600ns * 125MHz) + 1
// IC_LCNT = (1300ns * 125MHz) + 1
i2c0.ic_fs_scl_hcnt().write(|w| unsafe { w.ic_fs_scl_hcnt().bits(76) });
i2c0.ic_fs_scl_lcnt().write(|w| unsafe { w.ic_fs_scl_lcnt().bits(163) });

We finally have to perform spike suppression and program the data hold time during transmission.

// spkln = lcnt/16;
i2c0.ic_fs_spklen().write(|w| unsafe {  w.ic_fs_spklen().bits(163/16) });
// sda_tx_hold_count = freq_in [cycles/s] * 300ns for scl < 1MHz
let sda_tx_hold_count = ((125_000_000 * 3) / 10000000) + 1;
i2c0.ic_sda_hold().modify(|_r, w| unsafe { w.ic_sda_tx_hold().bits(sda_tx_hold_count as u16) });

I2C0 is now set up. We equally have to configure the HMC5883L.

HMC5883L configuration

First we'll write helper functions for reading and writing the I2C lines.

Configuring the magnetometer requires writing to specific registers. The below functions are light tweaks from the rp2040-hal crate. Overall we follow three steps:

Disable the I2C
Write the slave address
Enable the I2C

fn i2c_read(
    i2c0: &mut I2C0,
    addr: u16,
    bytes: &mut [u8],
) -> Result<(), u32> {
    i2c0.ic_enable().modify(|_, w| w.enable().clear_bit());// disable i2c
    i2c0.ic_tar().modify(|_, w| unsafe { w.ic_tar().bits(addr) });// slave address
    i2c0.ic_enable().modify(|_, w| w.enable().set_bit());// enable i2c

    let last_index = bytes.len() - 1;
    for (i, byte) in bytes.iter_mut().enumerate() {
        let first = i == 0;
        let last = i == last_index;

        // wait until there is space in the FIFO to write the next byte
        while TX_FIFO_SIZE - i2c0.ic_txflr().read().txflr().bits() == 0 {}

        i2c0.ic_data_cmd().modify(|_, w| {
            if first {
                w.restart().enable();
            } else {
                w.restart().disable();
            }

            if last {
                w.stop().enable();
            } else {
                w.stop().disable();
            }

            w.cmd().read()
        }); 

        //Wait until address tx'ed
        while i2c0.ic_raw_intr_stat().read().tx_empty().is_inactive() {}
        //Clear ABORT interrupt
        //self.i2c0.ic_clr_tx_abrt.read();

        while i2c0.ic_rxflr().read().bits() == 0 {
            //Wait while receive FIFO empty
            //If attempt aborts : not valid address; return error with
            //abort reason.
            let abort_reason = i2c0.ic_tx_abrt_source().read().bits();
            //Clear ABORT interrupt
            i2c0.ic_clr_tx_abrt().read();
            if abort_reason != 0 {
                return Err(abort_reason)
            } 
        }

        *byte = i2c0.ic_data_cmd().read().dat().bits();
    }

    Ok(())
}

fn i2c_write(
    i2c0: &mut I2C0,
    addr: u16,
    bytes: &[u8],
) -> Result<(), u32> {
    i2c0.ic_enable().modify(|_, w| w.enable().clear_bit());// disable i2c
    i2c0.ic_tar().modify(|_, w| unsafe { w.ic_tar().bits(addr) });// slave address
    i2c0.ic_enable().modify(|_, w| w.enable().set_bit());// enable i2c

    let last_index = bytes.len() - 1;
    for (i, byte) in bytes.iter().enumerate() {
        let last = i == last_index;

        i2c0.ic_data_cmd().modify(|_, w| {
            if last {
                w.stop().enable();
            } else {
                w.stop().disable();
            }
            unsafe { w.dat().bits(*byte)}
        }); 

        // Wait until address and data tx'ed
        while i2c0.ic_raw_intr_stat().read().tx_empty().is_inactive() {}
        // Clear ABORT interrupt
        // self.i2c0.ic_clr_tx_abrt.read();

        // If attempt aborts : not valid address; return error with
        // abort reason.
        let abort_reason = i2c0.ic_tx_abrt_source().read().bits();
        // Clear ABORT interrupt
        i2c0.ic_clr_tx_abrt().read();
        if abort_reason != 0 {
            // Wait until the STOP condition has occured
            while i2c0.ic_raw_intr_stat().read().stop_det().is_inactive() {}
            // Clear STOP interrupt
            i2c0.ic_clr_stop_det().read().clr_stop_det();
            return Err(abort_reason)
        } 

        if last {
            // Wait until the STOP condition has occured
            while i2c0.ic_raw_intr_stat().read().stop_det().is_inactive() {}
            // Clear STOP interrupt
            i2c0.ic_clr_stop_det().read().clr_stop_det();
        }
    }

    Ok(())
}

Import I2C0.

use rp2040_pac::I2C0;

Add the TX_FIFO_SIZE constant.

// Global constants
const TX_FIFO_SIZE: u8 = 16;// I2C FIFO size

Create buffer to hold data to be sent and received via I2C.

// Buffers
let mut writebuf: [u8; 1];// buffer to hold 1 byte
let mut readbuf: [u8; 1] = [0; 1];

Identification

We can begin by confirming the identification of the HMC5883L. The chip has three Identification registers, A, B and C, holding the values 48, 34 and 33 respectively.

To read a register in the compass we first have to send the slave address with the command bit set to read followed by a pointer to the address whose contents we want to read.

Reading ID Reg. A for example we have to send 0x3D 0x10.

Before proceeding further we should create the magnetometer's addresses constants to help write cleanly.

Add the following to our global constant section

// Slave Address
const HMC5883L_ADDR: u16 = 30;

// ID
const IDENTIFICATION_REG_A: u8 = 0xA;// HMC5883L
const IDENTIFICATION_REG_B: u8 = 0xB;// HMC5883L
const IDENTIFICATION_REG_C: u8 = 0xC;// HMC5883L

Continuing the configuration...

// Configure and confirm HMC5833L
// ID the compass
// Read ID REG A
writebuf = [IDENTIFICATION_REG_A];
i2c_write(&mut i2c0, HMC5883L_ADDR, &mut writebuf).unwrap();
i2c_read(&mut i2c0, HMC5883L_ADDR, &mut readbuf).unwrap();
let id_a = readbuf[0];
writeln!(serialbuf, "Id reg a: 0x{:02X}", id_a).unwrap();
transmit_uart_data(&uart_data, serialbuf);

// Read ID REG B 
writebuf = [IDENTIFICATION_REG_B];
i2c_write(&mut i2c0, HMC5883L_ADDR, &mut writebuf).unwrap();
i2c_read(&mut i2c0, HMC5883L_ADDR, &mut readbuf).unwrap();
let id_b = readbuf[0];
writeln!(serialbuf, "Id reg b: 0x{:02X}", id_b).unwrap();
transmit_uart_data(&uart_data, serialbuf);

// Read ID REG C
writebuf = [IDENTIFICATION_REG_C];
i2c_write(&mut i2c0, HMC5883L_ADDR, &mut writebuf).unwrap();
i2c_read(&mut i2c0, HMC5883L_ADDR, &mut readbuf).unwrap();
let id_c = readbuf[0];
writeln!(serialbuf, "Id reg c: 0x{:02X}", id_c).unwrap();
transmit_uart_data(&uart_data, serialbuf);

if id_a == 0x48 && id_b == 0x34 && id_c == 0x33 {
    writeln!(serialbuf, "Magnetometer ID confirmed!").unwrap();
    transmit_uart_data(&uart_data, serialbuf);
}

Flash the program thus far into the Pico. The compass should be positively identified.

Mode

We have to set the compass to be in continuous mode.

To read a register in the compass we first have to send the slave address with command bit set to write followed by a pointer to the address to which we want to write. The data to written then follows.

Setting the compass in continuous mode for example requires that we write 0x0 to the MODE Register. Sending 0x3C 0x02 0x00 writes 0x0 to the MODE register pointed by 0x02.

First we create a buffer capable of holding two bytes.

let mut writebuf2: [u8; 2];// buffer to hold 2 bytes

Introduce the MODE Register pointer in the global constants.

// Mode Register.
const MODE_R: u8 = 0x02; // HMC5883L

Configuring the compass...

// Set compass in continuous mode & confirm
writebuf2 = [MODE_R, 0x0];
i2c_write(&mut i2c0, HMC5883L_ADDR, &mut writebuf2).unwrap();

We can now read the contents of the register to confirm that the magnetometer has indeed been correctly set.

// Set compass in continuous mode & confirm
writebuf2 = [MODE_R, 0x0];
i2c_write(&mut i2c0, HMC5883L_ADDR, &mut writebuf2).unwrap();

//writebuf = [MODE_R];
//i2c_write(&mut i2c0, HMC5883L_ADDR, &mut writebuf).unwrap();
i2c_read(&mut i2c0, HMC5883L_ADDR, &mut readbuf).unwrap();

let mode = readbuf[0];
writeln!(serialbuf, "Mode reg: 0b{:08b}", mode).unwrap();
transmit_uart_data(&uart_data, serialbuf);

//let mode = readbuf[0];
if (mode & 1 << 1) == 0 && (mode & 1 << 0) == 0  { 
    writeln!(serialbuf, "continuous mode set.").unwrap();
    transmit_uart_data(&uart_data, serialbuf);
} else if (mode & 1 << 1) == 0 && (mode & 1 << 0) != 0 {
    writeln!(serialbuf, "single-measurement mode set.").unwrap();
    transmit_uart_data(&uart_data, serialbuf);
} else {
    writeln!(serialbuf, "device in idle mode.").unwrap();
    transmit_uart_data(&uart_data, serialbuf);
}

Output Rate and Gain

The data output rate and gain can be set in a similar manner.

First add the Magnetometer's address pointers.

// Configuration Register A
const CFG_REG_A: u8 = 0x0;// HMC5883L

// Configuration Register B
const CFG_REG_B: u8 = 0x01;// HMC5883L

For a sample average of 8, data output rate of 15Hz and normal measurement configuration we'll have to write 0b01110000 to CFG_REG_A.

// Set data output rate & number of samples & confirm
// sample avg = 8; data output rate = 15Hz; normal measurement cfgn
writebuf2 = [CFG_REG_A, 0b01110000];
i2c_write(&mut i2c0, HMC5883L_ADDR, &mut writebuf2).unwrap();
writebuf = [CFG_REG_A];
i2c_write(&mut i2c0, HMC5883L_ADDR, &mut writebuf).unwrap();
i2c_read(&mut i2c0, HMC5883L_ADDR, &mut readbuf).unwrap();

let cfg_a = readbuf[0];
writeln!(serialbuf, "cfg reg a: 0b{:08b}", cfg_a).unwrap();
transmit_uart_data(&uart_data, serialbuf);

For a gain of 1090 LSB/Gauss we have to write 0b00100000 to CFG_REG_B.

// Set Gain & confirm
// gain = 1090 LSB/Gauss
writebuf2 = [CFG_REG_B, 0b00100000];
i2c_write(&mut i2c0, HMC5883L_ADDR, &mut writebuf2).unwrap();

writebuf = [CFG_REG_B];
i2c_write(&mut i2c0, HMC5883L_ADDR, &mut writebuf).unwrap();
i2c_read(&mut i2c0, HMC5883L_ADDR, &mut readbuf).unwrap();

let cfg_b = readbuf[0];
writeln!(serialbuf, "cfg reg b: 0b{:08b}", cfg_b).unwrap();
transmit_uart_data(&uart_data, serialbuf);

if (cfg_a == 0b01110000) && (cfg_b == 0b00100000) { 
    writeln!(serialbuf, "cfg regs set.").unwrap();
    transmit_uart_data(&uart_data, serialbuf);
}