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.
Table of contents
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);
This will print out the current heading of our mock dish during operation.
Results
Organizing all the code thus far we'll have this final copy.
Loading and running the program in the Pico, we can now be able to see the magnetic heading of our system.
In the next part we look at how we can actuate the PTZ kit so that it aligns with a chosen satellite.
Top comments (0)