Self-Aligning Satellite Dish in Rust: Compass Application

In the previous part we were able to receive raw magnetometer data from the HMC5833L module and process it to acquire magnetic heading and strength. We will now apply those functions to our self aligning dish project.

The magnetic heading is important for the project as it allows our mock dish to ascertain its orientation in its attempts to align to the look angles.

Requirements
Implementation
- Connections Diagram
- Functions
Results

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
1 x Neo-6M GPS Module
Serial Bluetooth App

Implementation

Connections Diagram

Functions

Let's organize the compass configuration code we developed earlier under a function: configure_compass.

fn configure_compass(i2c0: &mut I2C0, uart: &UART0) { // Configure and confirm HMC5833L
    let mut writebuf: [u8; 1];// buffer to hold 1 byte
    let mut writebuf2: [u8; 2];// buffer to hold 2 bytes
    let mut readbuf: [u8; 1] = [0; 1];
    let serialbuf = &mut String::<164>::new();

    // ID the compass
    // Read ID REG A
    writebuf = [IDENTIFICATION_REG_A];
    i2c_write(i2c0, HMC5883L_ADDR, &mut writebuf).unwrap();
    i2c_read(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, serialbuf);

    // Read ID REG B 
    writebuf = [IDENTIFICATION_REG_B];
    i2c_write(i2c0, HMC5883L_ADDR, &mut writebuf).unwrap();
    i2c_read(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, serialbuf);

    // Read ID REG C
    writebuf = [IDENTIFICATION_REG_C];
    i2c_write(i2c0, HMC5883L_ADDR, &mut writebuf).unwrap();
    i2c_read(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, serialbuf);

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

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

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

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

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

    // 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(i2c0, HMC5883L_ADDR, &mut writebuf2).unwrap();
    writebuf = [CFG_REG_A];
    i2c_write(i2c0, HMC5883L_ADDR, &mut writebuf).unwrap();
    i2c_read(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, serialbuf);

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

    writebuf = [CFG_REG_B];
    i2c_write(i2c0, HMC5883L_ADDR, &mut writebuf).unwrap();
    i2c_read(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, serialbuf);

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

Create another function get_magnetic_heading which returns the heading. We shall sample a number of readings to get a more accurate reading.

fn get_magnetic_heading(i2c0: &mut I2C0) -> f32 { 
    // Read raw data from compass.
    // Capture 50 heading samples and return the average
    let mut count = 0.;
    let samples = 50.;
    let mut headings = 0.;

    while count != samples {
        // Point to the address of DATA_OUTPUT_X_MSB_R
        let mut writebuf = [DATA_OUTPUT_X_MSB_R];
        i2c_write(i2c0, HMC5883L_ADDR, &mut writebuf).unwrap();
        // Read the output of the HMC5883L
        // All six registers are read into the
        // rawbuf buffer
        let mut rawbuf: [u8; 6] = [0; 6];
        i2c_read(i2c0, HMC5883L_ADDR, &mut rawbuf).unwrap();

        let x_h = rawbuf[0] as u16;
        let x_l = rawbuf[1] as u16;
        let z_h = rawbuf[2] as u16;
        let z_l = rawbuf[3] as u16;
        let y_h = rawbuf[4] as u16;
        let y_l = rawbuf[5] as u16;

        let x = ((x_h << 8) + x_l) as i16;
        let y = ((y_h << 8) + y_l) as i16;
        let _z = ((z_h << 8) + z_l) as i16;

        // North-Clockwise convention for getting the heading
        let mut heading = (y as f32 + 185.).atan2(x as f32 + 75.);

        heading += MAGNETIC_DECLINATION*(PI/180.);// add declination in radians

        // Check for sign
        if heading < 0. {
            heading += 2.*PI;
        }

        // Check for value wrap
        if heading > 2.*PI {
            heading -= 2.*PI;
        }

        heading *= 180./PI;

        headings += heading;

        count += 1.;
    }

    headings/samples
}

Under the auto arm in the application code we can add the code below to print the magnetic heading.

heading = get_magnetic_heading(i2c.as_mut().unwrap());
writeln!(&mut serialbuf, "heading: {}", heading.round()).unwrap();
transmit_uart_data(uart_data.as_mut().unwrap(), serialbuf);