Compass and ViDAR

First, ViDAR: I’ve got fed up of calling it video macro-block something or the other, so now it’s Video Detection and Ranging.  I think it’s now working as well as it can without a height detector; it’s a long way from perfect, but until I can rely on stable height, I can’t identify further bugs.  I’m almost certainly going to wait for the Garmin LiDAR-Lite (GLL) to provide the height information.

There’s one bug I’ve fixed which explains why the PX4FLOW only uses a gyro – the angles involved are increments not absolutes.  The picture below tries to show that it’s not the absolute tilt of the quadcopter that needs to be accounted for, but the increment:

ViDAR angles

ViDAR angles

I’ve also got the compass working – I’ve just pulled out the critical bits.  Most of the samples I found use the MPU9250 master I2C to attach to the compass; compass data is then picked from MPU9250 registers.  But I found this version which exposed the compass registers for direct access which is much cleaner and clearer.

####################################################################################################
#
#  Gyroscope / Accelerometer class for reading position / movement.  Works with the Invensense IMUs:
#
#  - MPU-6050
#  - MPU-9150
#  - MPU-9250
#
#  The compass / magnetometer of the MPU-9250 is not used
#
####################################################################################################
class MPU6050:
    .
    .
    .
    .
    .
    .
    .
    def initCompass(self):    
        #-------------------------------------------------------------------------------------------
        # Set up the I2C master pass through.
        #-------------------------------------------------------------------------------------------
        int_bypass = self.i2c.readU8(self.__MPU6050_RA_INT_PIN_CFG)
        self.i2c.write8(self.__MPU6050_RA_INT_PIN_CFG , int_bypass | 0x02)

        #-------------------------------------------------------------------------------------------
        # Connect directly to the bypassed magnetometer, and configured it for 16 bit continuous data
        #-------------------------------------------------------------------------------------------
        self.i2c_compass = I2C(0x0C)
        self.i2c_compass.write8(self.__AK893_RA_CNTL1, 0x16);
   
    def readCompass(self):
        compass_bytes = self.i2c_compass.readList(self.__AK893_RA_X_LO, 7)

        #-------------------------------------------------------------------------------------------
        # Convert the array of 6 bytes to 3 shorts - 7th byte kicks off another read
        #-------------------------------------------------------------------------------------------
        compass_data = []
        for ii in range(0, 6, 2):
            lobyte = compass_bytes[ii]
            hibyte = compass_bytes[ii + 1]
            if (hibyte > 127):
                hibyte -= 256

            compass_data.append((hibyte << 8) + lobyte)

        [mgx, mgy, mgz] = compass_data
        return mgx, mgy, mgz


mpu6050 = MPU6050()
mpu6050.initCompass()

try:
    while True:
        mx, my, mz = mpu6050.readCompass()
        print "%d, %d, %d" % (mx, my, mz)
        time.sleep(0.5)


except KeyboardInterrupt, e:
    pass
finally:
    pass


I probably won’t include the compass data for yaw and orientation yet, but at least the main hurdle has been overcome.

Leave a Reply

Your email address will not be published. Required fields are marked *