jb63
Posts: 122
Joined: Mon May 11, 2015 6:41 pm

Re: catapult project ( raspberry pi ) - physics - need help

Tue Jan 12, 2016 6:31 pm

Interesting discussion. I'll simply add this. If the RPI is attached to the projectile, then you won't be able to measure acceleration, as there won't be any net force between the sensing elements of the accelerometer and the base (PC board). This, assuming the projectile does not turn or change attitude in flight. If it does, and depending how fast, you may be able to see some 'transient' acceleration.

Heater
Posts: 13609
Joined: Tue Jul 17, 2012 3:02 pm

Re: catapult project ( raspberry pi ) - physics - need help

Tue Jan 12, 2016 6:39 pm

Exactly. An object in free fall will not see any acceleration.

Anyway, we kind of moved onto measuring the accelerations and rotations measured during launch. From which you should be able to determine the launch speed and direction and hence predict the trajectory. Think bowler throwing a cricket ball.

A bit of a challenge.
Memory in C++ is a leaky abstraction .

danjperron
Posts: 3404
Joined: Thu Dec 27, 2012 4:05 am
Location: Québec, Canada

Re: catapult project ( raspberry pi ) - physics - need help

Tue Jan 12, 2016 7:10 pm

Exactly. An object in free fall will not see any acceleration.
Exactly so no acceleration means is falling down 9.8m/s*s.

In my point of view if you don't saturate the sensor you should be able to calculate the position , at least having a rought estimate.

The trick will be to separate the G force and the rotation on the three axes to get the parallel vector relative to the gravity force. Then it is just summing the distance between each accelerometer readout.

Adding gyroscope and magnetometer should help to figure out the headings to get that acceleration vector in parallel with the gravity.(perpendicular to the ground level).

Remember d= a*t*t/2

'a' is the acceleration vector from the object to the center of earth - 9.8m/s*s.

Since the acceleration is not constant , you just need to sum all the small readings using the delta in times between readings.

Heater
Posts: 13609
Joined: Tue Jul 17, 2012 3:02 pm

Re: catapult project ( raspberry pi ) - physics - need help

Tue Jan 12, 2016 7:54 pm

danjperron,

Yep. We are assuming we have accelerometer, gyro and magnetometer available.

Figuring out what goes on from all those measurements is the challenge.

Imagine that bowler throwing a cricket ball. His arm is accelerating the ball through an arc. He may be spinning it deliberately. How do we figure out which direction the measured acceleration is actually pointing with respect the ground?

This demands some sensor fusion software. Or you can cheat and use the Bosch BNO055
https://www.adafruit.com/products/2472 which claims to do that all that for you.

But how well does it work?
Memory in C++ is a leaky abstraction .

jb63
Posts: 122
Joined: Mon May 11, 2015 6:41 pm

Re: catapult project ( raspberry pi ) - physics - need help

Wed Jan 13, 2016 7:50 pm

You got it right! Sensor fusion is the answer here. Though, we need to make sure the sensors have sufficient bandwidth and sensitivity to capture all we're after. Not all accelerometers and not all gyros are created equal.

Heater
Posts: 13609
Joined: Tue Jul 17, 2012 3:02 pm

Re: catapult project ( raspberry pi ) - physics - need help

Wed Jan 13, 2016 8:01 pm

jb63,

Yep, that is the challenge of the idea. I have no idea how far we can push this at the moment.
Memory in C++ is a leaky abstraction .

jb63
Posts: 122
Joined: Mon May 11, 2015 6:41 pm

Re: catapult project ( raspberry pi ) - physics - need help

Mon Feb 22, 2016 7:01 am

The math is rather easy, it's about resolving three vectors along three directions, and can be tweaked (debugged) afterwards. What I would do, is capture raw data (9 channels) time histories at whatever rate the RPi can handle, during many trial launches. I would then post-process that data in e.g. Matlab, to figure out how to combine all channels. Once the algorithm is finalized, it should be fairly easy to port to the RPi for a 'live' data conversion.

Heater
Posts: 13609
Joined: Tue Jul 17, 2012 3:02 pm

Re: catapult project ( raspberry pi ) - physics - need help

Mon Feb 22, 2016 10:38 am

jb63,

You idea of "rather easy" maths is perhaps a little different than some for some of us :)

Certainly if we have an acceleration vector in three dimensions we can use that to try and track our position during launch. All relevant maths here: https://en.wikipedia.org/wiki/Equations_of_motion. Although errors will accumulate horribly I think.

Problem is that at any moment the acceleration vector produced by our accelerometer will depend on the current attitude of the device as it is rotated during launch. Think spin bowler in cricket. So we need to track the current orientation of the device and apply that the the accelerometer vector to arrive at the current acceleration with respect to our frame of reference, the ground.

As far as I can make out the easiest and most accurate way to do that is to "fuse" accelerometer, gyro and magnetometer data with quaternion maths. https://en.wikipedia.org/wiki/Quaternion

I have had a go at quaternion sensor fusion here: https://github.com/ZiCog/madgwick.js

I cheat and borrow the code from Sebastian Madgwick. There are his version in C and my version in Javascript in that repository.

Now, getting that IMU to work is pretty straight forward given the code. Actually understanding how it works is a bit tougher :)
Memory in C++ is a leaky abstraction .

jb63
Posts: 122
Joined: Mon May 11, 2015 6:41 pm

Re: catapult project ( raspberry pi ) - physics - need help

Mon Feb 22, 2016 12:20 pm

Heater,

Maybe I spoke too early. It's a given that it's always easy unless you have to do it yourself :). In any event, I was thinking primarily of fusing the data, as you indicated. The idea is to use the magnetometer data to 're-orient' the frame for the accelerometers and gyros data, then resolve those 6 channels onto that new frame. Now, one possible complication I could see is the the new reference frame (from the magnetometers) might not be uniquely defined. That aside, and conceptually, one should be able to write the kinematics in a form suitable for implementation in a Kalman filter algorithm. Here we have to focus primarily on the kinematics and ignore dynamics (air drag/friction ?). The other source of error, and that depends on how long the 'flight' is, is the fact that by nature, gyro data tends to drift, and accelerometers data tends to be noisy ...
So, at least we're starting from few ideas, and the best approach (to me, at least) is to start with some real data, apply the calculation algorithms to it, and see if the results make sense. One can go fancy here and run several data acquisition runs (in a DOE setup), and check the repeatability and sensitivity to the calculation procedure. All I need now is pick up my old dynamics and controls books
...
Is there real 'flight' data available for one to look at?

danjperron
Posts: 3404
Joined: Thu Dec 27, 2012 4:05 am
Location: Québec, Canada

Re: catapult project ( raspberry pi ) - physics - need help

Mon Feb 22, 2016 12:36 pm

If you read the previous post I decide to do my own test using my egg crash prototype!
I prefer to break a small cpu instead of a Raspberry Pi ;-) Also I do not have a trebuchet but a small catapult should do the trick.

I just create a general class to incorporate the mpu6050 accelerometer , the HM5883 magnetometer and of course a black box which is a 24AA1025 flash eprom.

I'm not ready yet to decode the data but my egg system is working!

With a compass you are not aware about the Z axis magnetic amplitude but with a magnetometer you see very well the magnetic force line which is way stronger in the Z axis than the X and Y.

Now my question is about the HM5883 magnetometer. I try to look a magnetic map for the planet earth displaying magnetic signal on the Z axis. I assume that more you are in the center less the Z axis is powerfull and more you are near the pole an stronger it is! Is it correct?

This is my preliminary python library. This is just to read the sensors with a raspberry Pi.

Code: Select all

#!/usr/bin/python3

'''
    <Daniel Perron  BlueGyro  V1.0  Python librarry which encapsulate mpu6050,HM5883
     and 24AA1025 flash eerom using I2C with IO file mode>
    Copyright (C) <2016>  <Daniel Perron>

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.

'''

import io
import fcntl
import math

class i2c:

    def __init__(self, bus):
        self.fh = io.open("/dev/i2c-"+str(bus), "br+", buffering=0)
        self.I2CAddress = -1

    def __del__(self):
        self.fh.close()

    def setAddress(self, I2CAddress):
        fcntl.ioctl(self.fh, 0x703, I2CAddress)
        self.I2CAddress = I2CAddress

    def write(self, values):
        self.fh.write(bytearray(values))

    def read(self, count):
        return list(self.fh.read(count))

    def regWriteByte(self, I2CAddress, Register, ByteValue):
        self.setAddress(I2CAddress)
        self.write([Register, ByteValue & 0xff])

    def regWriteWord(self, I2CAddress, Register, WordValue, BigEndian=True):
        self.setAddress(I2CAddress)
        if BigEndian:
            self.write([Register, (WordValue >> 8) & 0xff, WordValue & 0xff])
        else:
            self.write([Register, WordValue & 0xff, (WordValue >> 8) & 0xff])

    def regWrite(self, I2CAddress, Register, values):
        self.setAddress(I2CAddress)
        self.write([Register] + values)

    def regReadByte(self, I2CAddress, Register):
        self.setAddress(I2CAddress)
        self.write([Register])
        return self.read(1)[0]

    def regReadWord(self, I2CAddress, Register, BigEndian=True):
        self.setAddress(I2CAddress)
        self.write([Register])
        block = self.read(2)
        if BigEndian:
            return (block[0] * 256 + block[1])
        else:
            return (block[1] * 256 + block[0])

    def regRead(self, I2CAddress, Register, Count):
        self.setAddress(I2CAddress)
        self.write([Register])
        return self.read(Count)

    def signedWord(self, value, idx, BigEndian=True):
        wordValue = 0
        if BigEndian:
            wordValue = value[idx] * 256 + value[idx+1]
        else:
            wordValue = value[idx+1] * 256 + value[idx]
        if wordValue > 32767:
            return wordValue - 65536
        else:
            return wordValue


class mpu6050:

    def __init__(self, I2C, I2CAddress=0x68):
        self.I2C = I2C
        self.I2CAddress = I2CAddress
        self.MPU6050_RA_XG_OFFS_TC = 0x00
        self.MPU6050_RA_YG_OFFS_TC = 0x01
        self.MPU6050_RA_ZG_OFFS_TC = 0x02
        self.MPU6050_RA_X_FINE_GAIN = 0x03
        self.MPU6050_RA_Y_FINE_GAIN = 0x04
        self.MPU6050_RA_Z_FINE_GAIN = 0x05
        self.MPU6050_RA_XA_OFFS_H = 0x06
        self.MPU6050_RA_XA_OFFS_L_TC = 0x07
        self.MPU6050_RA_YA_OFFS_H = 0x08
        self.MPU6050_RA_YA_OFFS_L_TC = 0x09
        self.MPU6050_RA_ZA_OFFS_H = 0x0A
        self.MPU6050_RA_ZA_OFFS_L_TC = 0x0B
        self.MPU6050_RA_XG_OFFS_USRH = 0x13
        self.MPU6050_RA_XG_OFFS_USRL = 0x14
        self.MPU6050_RA_YG_OFFS_USRH = 0x15
        self.MPU6050_RA_YG_OFFS_USRL = 0x16
        self.MPU6050_RA_ZG_OFFS_USRH = 0x17
        self.MPU6050_RA_ZG_OFFS_USRL = 0x18
        self.MPU6050_RA_SMPLRT_DIV = 0x19
        self.MPU6050_RA_CONFIG = 0x1A
        self.MPU6050_RA_GYRO_CONFIG = 0x1B
        self.MPU6050_RA_ACCEL_CONFIG = 0x1C
        self.MPU6050_RA_FF_THR = 0x1D
        self.MPU6050_RA_FF_DUR = 0x1E
        self.MPU6050_RA_MOT_THR = 0x1F
        self.MPU6050_RA_MOT_DUR = 0x20
        self.MPU6050_RA_ZRMOT_THR = 0x21
        self.MPU6050_RA_ZRMOT_DUR = 0x22
        self.MPU6050_RA_FIFO_EN = 0x23
        self.MPU6050_RA_I2C_MST_CTRL = 0x24
        self.MPU6050_RA_I2C_SLV0_ADDR = 0x25
        self.MPU6050_RA_I2C_SLV0_REG = 0x26
        self.MPU6050_RA_I2C_SLV0_CTRL = 0x27
        self.MPU6050_RA_I2C_SLV1_ADDR = 0x28
        self.MPU6050_RA_I2C_SLV1_REG = 0x29
        self.MPU6050_RA_I2C_SLV1_CTRL = 0x2A
        self.MPU6050_RA_I2C_SLV2_ADDR = 0x2B
        self.MPU6050_RA_I2C_SLV2_REG = 0x2C
        self.MPU6050_RA_I2C_SLV2_CTRL = 0x2D
        self.MPU6050_RA_I2C_SLV3_ADDR = 0x2E
        self.MPU6050_RA_I2C_SLV3_REG = 0x2F
        self.MPU6050_RA_I2C_SLV3_CTRL = 0x30
        self.MPU6050_RA_I2C_SLV4_ADDR = 0x31
        self.MPU6050_RA_I2C_SLV4_REG = 0x32
        self.MPU6050_RA_I2C_SLV4_DO = 0x33
        self.MPU6050_RA_I2C_SLV4_CTRL = 0x34
        self.MPU6050_RA_I2C_SLV4_DI = 0x35
        self.MPU6050_RA_I2C_MST_STATUS = 0x36
        self.MPU6050_RA_INT_PIN_CFG = 0x37
        self.MPU6050_RA_INT_ENABLE = 0x38
        self.MPU6050_RA_DMP_INT_STATUS = 0x39
        self.MPU6050_RA_INT_STATUS = 0x3A
        self.MPU6050_RA_ACCEL_XOUT_H = 0x3B
        self.MPU6050_RA_ACCEL_XOUT_L = 0x3C
        self.MPU6050_RA_ACCEL_YOUT_H = 0x3D
        self.MPU6050_RA_ACCEL_YOUT_L = 0x3E
        self.MPU6050_RA_ACCEL_ZOUT_H = 0x3F
        self.MPU6050_RA_ACCEL_ZOUT_L = 0x40
        self.MPU6050_RA_TEMP_OUT_H = 0x41
        self.MPU6050_RA_TEMP_OUT_L = 0x42
        self.MPU6050_RA_GYRO_XOUT_H = 0x43
        self.MPU6050_RA_GYRO_XOUT_L = 0x44
        self.MPU6050_RA_GYRO_YOUT_H = 0x45
        self.MPU6050_RA_GYRO_YOUT_L = 0x46
        self.MPU6050_RA_GYRO_ZOUT_H = 0x47
        self.MPU6050_RA_GYRO_ZOUT_L = 0x48
        self.MPU6050_RA_EXT_SENS_DATA_00 = 0x49
        self.MPU6050_RA_EXT_SENS_DATA_01 = 0x4A
        self.MPU6050_RA_EXT_SENS_DATA_02 = 0x4B
        self.MPU6050_RA_EXT_SENS_DATA_03 = 0x4C
        self.MPU6050_RA_EXT_SENS_DATA_04 = 0x4D
        self.MPU6050_RA_EXT_SENS_DATA_05 = 0x4E
        self.MPU6050_RA_EXT_SENS_DATA_06 = 0x4F
        self.MPU6050_RA_EXT_SENS_DATA_07 = 0x50
        self.MPU6050_RA_EXT_SENS_DATA_08 = 0x51
        self.MPU6050_RA_EXT_SENS_DATA_09 = 0x52
        self.MPU6050_RA_EXT_SENS_DATA_10 = 0x53
        self.MPU6050_RA_EXT_SENS_DATA_11 = 0x54
        self.MPU6050_RA_EXT_SENS_DATA_12 = 0x55
        self.MPU6050_RA_EXT_SENS_DATA_13 = 0x56
        self.MPU6050_RA_EXT_SENS_DATA_14 = 0x57
        self.MPU6050_RA_EXT_SENS_DATA_15 = 0x58
        self.MPU6050_RA_EXT_SENS_DATA_16 = 0x59
        self.MPU6050_RA_EXT_SENS_DATA_17 = 0x5A
        self.MPU6050_RA_EXT_SENS_DATA_18 = 0x5B
        self.MPU6050_RA_EXT_SENS_DATA_19 = 0x5C
        self.MPU6050_RA_EXT_SENS_DATA_20 = 0x5D
        self.MPU6050_RA_EXT_SENS_DATA_21 = 0x5E
        self.MPU6050_RA_EXT_SENS_DATA_22 = 0x5F
        self.MPU6050_RA_EXT_SENS_DATA_23 = 0x60
        self.MPU6050_RA_MOT_DETECT_STATUS = 0x61
        self.MPU6050_RA_I2C_SLV0_DO = 0x63
        self.MPU6050_RA_I2C_SLV1_DO = 0x64
        self.MPU6050_RA_I2C_SLV2_DO = 0x65
        self.MPU6050_RA_I2C_SLV3_DO = 0x66
        self. MPU6050_RA_I2C_MST_DELAY_CTRL = 0x67
        self.MPU6050_RA_SIGNAL_PATH_RESET = 0x68
        self.MPU6050_RA_MOT_DETECT_CTRL = 0x69
        self.MPU6050_RA_USER_CTRL = 0x6A
        self.MPU6050_RA_PWR_MGMT_1 = 0x6B
        self.MPU6050_RA_PWR_MGMT_2 = 0x6C
        self.MPU6050_RA_BANK_SEL = 0x6D
        self.MPU6050_RA_MEM_START_ADDR = 0x6E
        self.MPU6050_RA_MEM_R_W = 0x6F
        self.MPU6050_RA_DMP_CFG_1 = 0x70
        self.MPU6050_RA_DMP_CFG_2 = 0x71
        self.MPU6050_RA_FIFO_COUNTH = 0x72
        self.MPU6050_RA_FIFO_COUNTL = 0x73
        self. MPU6050_RA_FIFO_R_W = 0x74
        self.MPU6050_RA_WHO_AM_I = 0x75

        self.initTable = [self.MPU6050_RA_FF_THR,
                          self.MPU6050_RA_FF_DUR,
                          self.MPU6050_RA_MOT_THR,
                          self.MPU6050_RA_MOT_DUR,
                          self.MPU6050_RA_ZRMOT_THR,
                          self.MPU6050_RA_ZRMOT_DUR,
                          self.MPU6050_RA_FIFO_EN,
                          self.MPU6050_RA_I2C_MST_CTRL,
                          self.MPU6050_RA_I2C_SLV0_ADDR,
                          self.MPU6050_RA_I2C_SLV0_REG,
                          self.MPU6050_RA_I2C_SLV0_CTRL,
                          self.MPU6050_RA_I2C_SLV1_ADDR,
                          self.MPU6050_RA_I2C_SLV1_REG,
                          self.MPU6050_RA_I2C_SLV1_CTRL,
                          self.MPU6050_RA_I2C_SLV2_ADDR,
                          self.MPU6050_RA_I2C_SLV2_REG,
                          self.MPU6050_RA_I2C_SLV2_CTRL,
                          self.MPU6050_RA_I2C_SLV3_ADDR,
                          self.MPU6050_RA_I2C_SLV3_REG,
                          self.MPU6050_RA_I2C_SLV3_CTRL,
                          self.MPU6050_RA_I2C_SLV4_ADDR,
                          self.MPU6050_RA_I2C_SLV4_REG,
                          self.MPU6050_RA_I2C_SLV4_DO,
                          self.MPU6050_RA_I2C_SLV4_CTRL,
                          self.MPU6050_RA_I2C_SLV4_DI,
                          self.MPU6050_RA_INT_PIN_CFG,
                          self.MPU6050_RA_INT_ENABLE,
                          self.MPU6050_RA_I2C_SLV0_DO,
                          self.MPU6050_RA_I2C_SLV1_DO,
                          self.MPU6050_RA_I2C_SLV2_DO,
                          self.MPU6050_RA_I2C_SLV3_DO,
                          self.MPU6050_RA_I2C_MST_DELAY_CTRL,
                          self.MPU6050_RA_SIGNAL_PATH_RESET,
                          self.MPU6050_RA_MOT_DETECT_CTRL,
                          self.MPU6050_RA_USER_CTRL,
                          self.MPU6050_RA_CONFIG,
                          self.MPU6050_RA_INT_PIN_CFG,
                          self.MPU6050_RA_INT_ENABLE,
                          self.MPU6050_RA_FIFO_R_W]

        self.GyroXOffset = 0.0
        self.GyroYOffset = 0.0
        self.GyroZOffset = 0.0

        self.reset()

    def reset(self):
        self.setSampleRate(100)
        self.setAccelerometerScale(16)
        self.setGyroScale(2000)
        for i in self.initTable:
            self.I2C.regWriteByte(self.I2CAddress, i, 0)
        self.I2C.regWriteByte(self.I2CAddress,
                              self.MPU6050_RA_PWR_MGMT_1, 0b00000010)
        self.I2C.regWriteByte(self.I2CAddress,
                              self.MPU6050_RA_PWR_MGMT_2, 0x00)

    def setSampleRate(self, rate):
        Srate = int(8000.0 / rate) - 1
        self.I2C.regWriteByte(self.I2CAddress,
                              self.MPU6050_RA_SMPLRT_DIV, Srate)
        self.sampleRate = 8000.0/(Srate+1)

    def setAccelerometerScale(self, scale):
        Scale = 0
        self.AccelerometerScale = 2
        if scale == 4:
            Scale = 1
            self.AccelerometerScale = 4
        elif scale == 8:
            Scale = 2
            self.AccelerometerScale = 8
        elif scale == 16:
            Scale = 3
            self.AccelerometerScale = 16
        self.I2C.regWriteByte(self.I2CAddress,
                              self.MPU6050_RA_ACCEL_CONFIG, Scale << 3)

    def setGyroScale(self, scale):
        Scale = 0
        self.GyroScale = 250
        if scale == 500:
            Scale = 1
            self.GyroScale = 500
        elif scale == 1000:
            Scale = 2
            self.GyroScale = 1000
        elif scale == 2000:
            Scale = 3
            self.GyroScale = 2000
        self.I2C.regWriteByte(self.I2CAddress,
                              self.MPU6050_RA_GYRO_CONFIG, Scale << 3)

    def readAll(self):
        # set address 59, read accelerometer, temperature and gyro
        block = self.I2C.regRead(self.I2CAddress, 59, 14)
        return [self.I2C.signedWord(block, 0) *
                self.AccelerometerScale / 32768.0,
                self.I2C.signedWord(block, 2) *
                self.AccelerometerScale / 32768.0,
                self.I2C.signedWord(block, 4) *
                self.AccelerometerScale / 32768.0,
                self.I2C.signedWord(block, 6) / 340.0 + 36.53,
                self.I2C.signedWord(block, 8) *
                self.GyroScale / 32768.0 + self.GyroXOffset,
                self.I2C.signedWord(block, 10) *
                self.GyroScale / 32768.0 + self.GyroYOffset,
                self.I2C.signedWord(block, 12) *
                self.GyroScale / 32768.0 + self.GyroZOffset]

    def readAccelerometer(self):
        # set address 59, read accelerometer
        block = self.I2C.regRead(self.I2CAddress, 59, 6)
        return [self.I2C.signedWord(block, 0) *
                self.AccelerometerScale / 32768.0,
                self.I2C.signedWord(block, 2) *
                self.AccelerometerScale / 32768.0,
                self.I2C.signedWord(block, 4) *
                self.AccelerometerScale / 32768.0]

    def readGyro(self):
        # set address 59, read accelerometer
        block = self.I2C.regRead(self.I2CAddress, 67, 6)
        return [self.I2C.signedWord(block, 0) *
                self.GyroScale / 32768.0 + self.GyroXOffset,
                self.I2C.signedWord(block, 2) *
                self.GyroScale / 32768.0 + self.GyroYOffset,
                self.I2C.signedWord(block, 4) *
                self.GyroScale / 32768.0 + self.GyroZOffset]

    def readTemperature(self):
        block = self.I2C.regRead(self.I2CAddress, 65, 2)
        return(self.I2C.signedWord(block, 0)/340.0+36.53)

    def zeroGyro(self):
        gX = 0
        gY = 0
        gZ = 0
        for i in range(10):
            v = self.readGyro()
            gX = gX + v[0]
            gY = gY + v[1]
            gZ = gZ + v[2]
            time.sleep(2.0/self.sampleRate)
        self.GyroXOffset = self.GyroXOffset-(gX/10.0)
        self.GyroYOffset = self.GyroYOffset-(gY/10.0)
        self.GyroZOffset = self.GyroZOffset-(gZ/10.0)


class hm5883:

    def __init__(self, I2C, I2CAddress=0x1E):
        self.I2C = I2C
        self.I2CAddress = I2CAddress
        self.HM5883_RA_CONFIG_A = 0
        self.HM5883_RA_CONFIG_B = 1
        self.HM5883_RA_MODE = 2
        self.HM5883_RA_OUT_X_MSB = 3
        self.HM5883_RA_OUT_X_LSB = 4
        self.HM5883_RA_OUT_Y_MSB = 5
        self.HM5883_RA_OUT_Y_LSB = 6
        self.HM5883_RA_OUT_Z_MSB = 7
        self.HM5883_RA_OUT_Z_LSB = 8
        self.HM5883_RA_STATUS = 9
        self.HM5883_RA_ID_A = 10
        self.HM5883_RA_ID_B = 11
        self.HM5883_RA_ID_C = 12

        self.MODE_CONTINOUS = 0
        self.MODE_SINGLE = 1
        self.MODE_IDLE = 3
        self.reset()

    def setMode(self, value):
        self.I2C.regWriteByte(self.I2CAddress,
                              self.HM5883_RA_MODE, value)

    def reset(self):
        self.setSampleRate(3)
        self.setScale(1.88)
        self.setMode(self.MODE_CONTINOUS)

    def setSampleRate(self, rate):
        Srate = 6
        self.sampleRate = 75
        if rate < 1.0:
            Srate = 0
            self.sampleRate = 0.75
        elif rate < 2.0:
            Srate = 1
            self.sampleRate = 1.5
        elif rate < 5.0:
            Srate = 2
            self.sampleRate = 3.0
        elif rate < 10.0:
            Srate = 3
            self.sampleRate = 7.5
        elif rate < 22.0:
            Srate = 4
            self.sampleRate = 15.0
        elif rate < 52:
            Srate = 5
            self.sampleRate = 30.0
        self.I2C.regWriteByte(self.I2CAddress,
                              self.HM5883_RA_CONFIG_A, (Srate << 2) | 0xc0)

    def setScale(self, scale):
        GN = 7
        self.Scale = 8.90445
        if scale < 1.69:
            GN = 0
            self.Scale = 1.49431
        elif scale < 2.19:
            GN = 1
            self.Scale = 1.88324
        elif scale < 2.804:
            GN = 2
            self.Scale = 2.49734
        elif scale < 3.879:
            GN = 3
            self.Scale = 3.11144
        elif scale < 4.944:
            GN = 4
            self.Scale = 4.64669
        elif scale < 5.721:
            GN = 5
            self.Scale = 5.24032
        elif scale < 7.553:
            GN = 6
            self.Scale = 6.20241
        self.I2C.regWriteByte(self.I2CAddress,
                              self.HM5883_RA_CONFIG_B, GN << 5)

    def read(self):
        # read magnetometer
        block = self.I2C.regRead(self.I2CAddress,
                                 self.HM5883_RA_OUT_X_MSB, 6)
        return [self.I2C.signedWord(block, 0) *
                self.Scale / 2047.0,
                self.I2C.signedWord(block, 4) *
                self.Scale / 2047.0,
                self.I2C.signedWord(block, 2) *
                self.Scale / 2047.0]

    def compass(self, Y , X):
        Radian = math.atan2(Y,X)
        return 180.0*Radian/math.pi        


class _24AA1025:

    def __init__(self, I2C, I2CAddress=0x50):
        self.I2C = I2C
        self.size = 131072
        self.writePage = 128
        self.readPage = 8192
        self.block = 0
        self.I2CAddress = I2CAddress

    def checkAddress(self, Address):
        if Address >= self.size:
            return False
        if Address < 0:
            return False
        if Address >= 65536:
            self.block = 4
        else:
            self.block = 0
        return True

    def readByte(self, Address):
        if not self.checkAddress(Address):
            return None
        self.I2C.setAddress(self.I2CAddress + self.block)
        self.I2C.write([(Address >> 8) & 0xff, Address & 0xff])
        return self.I2C.read(1)[0]

    def waitUntilReady(self):
        count = 0
        while count < 10:
            try:
                self.I2C.write([0])
            except:
                count = count + 1
                time.sleep(0.001)
                continue
            break

    def writeByte(self, Address, value):
        if not self.checkAddress(Address):
            return None
        self.I2C.setAddress(self.I2CAddress + self.block)
        self.I2C.write([(Address >> 8) & 0xff, Address & 0xff, value])
        self.waitUntilReady()

    def readBytes(self, Address, Count):
        values = []
        while Count > 0:
            if not self.checkAddress(Address):
                break
            # ok let's do it per page
            # first get start offset
            Start = Address & (self.readPage-1)
            PageByteCount = self.readPage - Start
            if Count < PageByteCount:
                PageByteCount = Count
            # set address
            self.I2C.setAddress(self.I2CAddress + self.block)
            self.I2C.write([(Address >> 8) & 0xff, Address & 0xff])
            values = values+self.I2C.read(PageByteCount)
            Address = Address+PageByteCount
            Count = Count - PageByteCount
        return values

    def writeBytes(self, Address, values):
        totalCount = len(values)
        valuesIdx = 0
        while totalCount > 0:
            if not self.checkAddress(Address):
                break
            # ok let's do it per page
            # first get start offset
            Start = Address & (self.writePage-1)
            PageByteCount = self.writePage - Start
            if totalCount < PageByteCount:
                PageByteCount = totalCount

            # set address
            self.I2C.setAddress(self.I2CAddress + self.block)

            ByteData = [(Address >> 8) & 0xff, Address & 0xff]
            ByteData = ByteData + values[valuesIdx: valuesIdx + PageByteCount]
            self.I2C.write(ByteData)
            self.waitUntilReady()

            Address = Address + PageByteCount
            totalCount = totalCount - PageByteCount
            valuesIdx = valuesIdx + PageByteCount


if __name__ == "__main__":
    import i2c
    import math
    import time
    I2C = i2c.i2c(0)
    mag = hm5883(I2C)
    gyro = mpu6050(I2C)
    flash = _24AA1025(I2C)

    gyro.zeroGyro()

    GData = gyro.readAll()
    MData = mag.read()
    # first reading is always bad
    time.sleep(1)
    MData = mag.read()

    FData = flash.readBytes(0,10)

    print('Accelerometer  X:{:+2.3f} Y:{:+2.3f} Z:{:+2.3f}'.format(
          GData[0],GData[1],GData[2]))
    print('Gyroscope      X:{:+2.3f} Y:{:+2.3f} Z:{:+2.3f}'.format(
          GData[4],GData[5],GData[6]))
    print('Temperature    {:2.1f} Celsius'.format(
          GData[3]))
    print('Magnetometer   X:{:+2.3f} Y:{:+2.3f} Z:{:+2.3f}'.format(
          MData[0],MData[1],MData[2]))
    print('Magnetometer Angle:{:3.1f}'.format(
          mag.compass(MData[1],MData[0])))
    print('First 10 bytes of flash memory: {}'.format(FData))

Code: Select all

pi@FirstPi:~ $ python3 blueGyro.py
Accelerometer  X:-0.067 Y:-0.018 Z:+0.964
Gyroscope      X:-0.049 Y:-0.018 Z:+0.012
Temperature    18.3 Celsius
Magnetometer   X:+0.007 Y:+0.042 Z:-0.461
Magnetometer Angle:80.1
First 10 bytes of flash memory: [11, 12, 13, 3, 4, 1, 6, 7, 8, 9]
I start to do a youtube about it (it is in french).
https://www.youtube.com/watch?v=4Xm-DA170TM

I

Heater
Posts: 13609
Joined: Tue Jul 17, 2012 3:02 pm

Re: catapult project ( raspberry pi ) - physics - need help

Mon Feb 22, 2016 1:19 pm

jb63,
The idea is to use the magnetometer data to 're-orient' the frame for the accelerometers and gyros data,...
That is exactly what an IMU using quaternions does. For example the Madgwick implementation I linked to above. I have yet to test that out with a proper 9 axis sensor though. It works quite well at low speed and for a short time with my lesser sensor here.

I keep hearing about the Kalman filter for this task. So far I have no idea how that works. I have to look into it one day.
The other source of error, and that depends on how long the 'flight' is, is the fact that by nature, gyro data tends to drift, and accelerometers data tends to be noisy
Yep, the gyros will drift. That is why we need the magnetometer to provide a reference to the Earth's magnetic field. The Madgwick algorithm does all this to try and compensate for drift. I won't know how well it does it until I try it.

BUT you have said "depends on how long the 'flight' is". You cannot do this during free flight only during the launch phase. Once the device is in free flight the accelerometers will read zero in all directions. There is no possibility to use the accelerometers to track track the trajectory. This has been discussed at length earlier in this thread.

The launch phase is another matter. The device starts at a known position, call it (0,0,0), a know speed (0,0,0) and a known orientation thanks to
the accelerometer reading the acceleration due to gravity and the magnetometer sensing the Eath's field. During launch the device will be accelerated
and rotated, think cricket spin bowler again. We track all that. When free of the catapult we should be able to determine the start of free flight and the direction and speed we are launched at. From that we can determine our trajectory, max heigh/distance etc, even before we get there.
Memory in C++ is a leaky abstraction .

User avatar
experix
Posts: 204
Joined: Mon Nov 10, 2014 7:39 pm
Location: Coquille OR
Contact: Website

Re: catapult project ( raspberry pi ) - physics - need help

Mon Feb 22, 2016 5:07 pm

Your accelerometer on the projectile will give you the deceleration due to air drag (if it is sensitive enough), and it might tell you about rotation (you might want to use magnetometer readings to help sort that out), and you might get a big reading sometimes from hitting a bird.

jb63
Posts: 122
Joined: Mon May 11, 2015 6:41 pm

Re: catapult project ( raspberry pi ) - physics - need help

Mon Feb 22, 2016 5:27 pm

Heater wrote:jb63,
I keep hearing about the Kalman filter for this task. So far I have no idea how that works. I have to look into it one day.
I'll try to put it in simple words. Think of the Kalman filter as an algorithm that slowly learns the dynamics of the system at hand, and then builds a model to 'predict' the future dynamics (motion) of such system. Then, at any given time, the filter uses the difference between its own prediction and whatever the sensors are telling it, to 'tweak/correct' the model it has built. It is a sophisticated (needless to say) algorithm that does filtering in the time domain, based on some dynamics (physical laws).

Now, there are many mathematical descriptions of the filter that are rather confusing (my guess is that many people did not quite understand how it worked), but in later years, with more real-world applications coming to main-stream, there are more 'understandable' explanations. My favorite is the one below. Try it, it's not mathematically rigorous, but it's a nice introduction.

https://www.youtube.com/watch?v=18TKA-YWhX0

Heater
Posts: 13609
Joined: Tue Jul 17, 2012 3:02 pm

Re: catapult project ( raspberry pi ) - physics - need help

Mon Feb 22, 2016 8:17 pm

jb63,

I appreciate the simple words. For sure I have read such descriptions before.

What worries me here is the "slowly learns the dynamics" part.

Well, a catapult is going to swing the projectile up to it's launch speed and direction in something like 0.1 seconds.

Can a Kalman system determine the resulting trajectory in that time ?

I have to check your video link now....
Memory in C++ is a leaky abstraction .

jb63
Posts: 122
Joined: Mon May 11, 2015 6:41 pm

Re: catapult project ( raspberry pi ) - physics - need help

Mon Feb 22, 2016 8:32 pm

Obviously this might be 'uncharted' territories, though I'm sure someone at NASA has worked on something similar a while back ... Now, the 'slowly learn' part typically means how many iteration cycles before the filter becomes good at predicting the object's trajectory. This is why I wrote earlier that the data from the 9 channels needs to be sampled as fast as the RPI can handle it. Of course, all of this is still 'hypothetical', and the proof is in the pudding as they say. That is also why I said earlier that the best way is to acquire data asap and start experimenting with the calculation algorithm. Granted, subjecting an RPI to repetitive launches might not be too smart, but we can start with trajectories that are not as damaging, first a 2D motion (RPI on a hobby train, going in circles, then in a figure 8), then 3D motion, where the RPI is spun at the end of a rope, with varying 3D motion and speed ... These are just incremental steps yielding more complicated kinematics, to assess the validity/sensitivity of the algorithm.

Heater
Posts: 13609
Joined: Tue Jul 17, 2012 3:02 pm

Re: catapult project ( raspberry pi ) - physics - need help

Mon Feb 22, 2016 9:00 pm

jb63,

I have no idea what NASA does now a days. But I did read some place that at the time of the lunar landing missions the nav software did not use Quaternions. As such it was prone to "gimble lock". And so there was an error message to alert if that happened and the pilot, say Neil Armstrong, had to do something to fix it. As far as I can tell that never happened.

I'm all for the "simple steps" idea.

Now, it occurs to me that it is very important to have your 9 axis IMU chip located at the exact centre of mass of your projectile. I mean, if it is anywhere else the accelerometers are going to be registering centrifugal forces that will confuse everything. I'd rather not have to take that into account in software.
Memory in C++ is a leaky abstraction .

Heater
Posts: 13609
Joined: Tue Jul 17, 2012 3:02 pm

Re: catapult project ( raspberry pi ) - physics - need help

Mon Feb 22, 2016 9:15 pm

Everything you need to know about Quaternions here: https://www.youtube.com/watch?v=3BR8tK-LuB0
Memory in C++ is a leaky abstraction .

User avatar
karrika
Posts: 1070
Joined: Mon Oct 19, 2015 6:21 am
Location: Finland

Re: catapult project ( raspberry pi ) - physics - need help

Tue Feb 23, 2016 8:05 am

Using Kalman filters and putting sensors into Conning positions is everyday stuff for vessel steering today. You don't have to place the sensors to the centre of mass. But when you run your analysis of the movement you have to translate/interpolate all sensor info to a specific point of the craft (called the conning position) in both space and time. Otherwise the Kalman filters don't work correctly.

Usually the conning position is where the helmsman stands steering the ship. It is an exact point documented in the vessels drawings.

Image

I have actually spent the last 10 years working with mariners...

Heater
Posts: 13609
Joined: Tue Jul 17, 2012 3:02 pm

Re: catapult project ( raspberry pi ) - physics - need help

Tue Feb 23, 2016 8:46 am

karrika,

Strangely that diagram you included above only shows up in your post when I start editing this new post. Very odd.
...you have to translate/interpolate all sensor info...
That's exactly what I don't want to do.

For example: Let's say I have nice spherical projectile. A cannon ball say. And that I have my 9 axis sensor mounted on its surface some place. Now I give that cannon ball a good spin and drop it...

It's going to be spinning around it's centre of mass. Those accelerometers are now doing orbits around that centre of mass. They will be reading some acceleration radially to the cannon ball, that depends on the speed of spin. What I actually want is for the accelerometers to read zero because we are in free fall.

One could compensate for this. The gyros are registering speed of rotation so we could use that to compensate the accelerometer readings. Starts to get icky.

The simple way out is to ensure the sensors are at the centre of mass of the projectile.
Memory in C++ is a leaky abstraction .

User avatar
karrika
Posts: 1070
Joined: Mon Oct 19, 2015 6:21 am
Location: Finland

Re: catapult project ( raspberry pi ) - physics - need help

Tue Feb 23, 2016 10:36 am

It is fine to use center of the projectile for the acceleration sensor and define that to be the conning position. For the math the easiest way would be to choose some exact steps in time when to have the measurements taken. Perhaps every 10ms? And interpolate the exact values to the exact time for the calculation. We could maintain velocity, acceleration, angular rotation and angular acceleration. All in 3d as a base for Kalman dead reckoning.

Heater
Posts: 13609
Joined: Tue Jul 17, 2012 3:02 pm

Re: catapult project ( raspberry pi ) - physics - need help

Tue Feb 23, 2016 11:30 am

Yes. One thing I learned along the way is that jitter in your sampling rate and control loop iteration ends up looking like horrible noise.

10ms would be a good. I'm looking forward to using a Bosch BNO055 sensor which works at 100Hz on everything except the magnetic field which runs at 20Hz.

The BN0055 does a lot of data fusion internally so it delivers Absolute Orientation without any external processing required.

That just leaves determining the absolute acceleration...
Last edited by Heater on Wed Feb 24, 2016 9:55 am, edited 1 time in total.
Memory in C++ is a leaky abstraction .

jb63
Posts: 122
Joined: Mon May 11, 2015 6:41 pm

Re: catapult project ( raspberry pi ) - physics - need help

Tue Feb 23, 2016 3:05 pm

Here is a thought, and sorry if this may have been addressed before. If you are after only the TOTAL acceleration (not caring what the vector components are), all you need is then the vector sum (square root of the sum of squares) of all readings from the triax accelerometer. This does not answer in which direction the projectile is being accelerated, but at least it's a first dib as to whether the data makes sense or not. IMHO, going after placing the IMU exactly into the center of the projectile is a lot of work ...
What we can do instead is: Start with a basic design, collect data, rearrange the sensors/components (mass stays the same but rotary inertia might not), re-collect data. With the two runs, I'm assuming the total acceleration vector will be fairly the same from run to run. The total rotation vector (gyros) however might not be the same (because of the rotary inertias). With this exercise, one might be able to assess how sensitive the experiment is to the specifics of mass distribution inside the projectile. We might be starting from a very 'low level' physics here, but it's alright. I too get thrilled by fancy math and 3D motion ... but often the most elegant solutions are the simplest.

Heater
Posts: 13609
Joined: Tue Jul 17, 2012 3:02 pm

Re: catapult project ( raspberry pi ) - physics - need help

Wed Feb 24, 2016 7:20 am

jb63,

Arriving at a scalar value for acceleration would be fine if we were accelerating our projectile linearly. We can estimate the final velocity from that. And if we know the angle of our "cannon" estimate the hight and distance of the resulting trajectory.

This might be a good place to start. But what I have in mind is a catapult. That is to say some kind of arm that swings the projectile through an arc. Not only is the projectile travelling in a curve during launch it is also rotating with respect our ground frame of reference. Think cricket bowler.

To handle all that we need vector directions all the time.

But I was thinking....A reasonable sized catapult can probably launch a projectile in about one tenth of a second or so. That only gives us 10 samples of accelerometer and gyro data during launch. Even less for the magnetometer. At least for the Bosch device. This may not be enough to get any decent accuracy.
Memory in C++ is a leaky abstraction .

jb63
Posts: 122
Joined: Mon May 11, 2015 6:41 pm

Re: catapult project ( raspberry pi ) - physics - need help

Wed Feb 24, 2016 9:47 am

Heater,

I certainly can appreciate the complications/challenges. However, my attempt is not to trivialize the problem, it's more like starting with something easy and build upon it. I'm thinking of the 'old' story regarding William Tell. It is said that, before he perfected shooting an apple on top of his son's head, he had a lot of practice with a pumpkin on his mother-in-law's...

In any event, if the time during launch is very short (as you described below), we can turn this problem into what's called an Initial-Value-Problem (IVP). Let's capture all we can about the data during the entire trajectory, and then try to solve for the initial conditions (displ, velocity, accel) that would yield such trajectory. We thus will have two data points (before the swing arm starts to move, and right after the projectile loses contact with the swing arm). If the whole process works reliably/repeatedly, we can then go back and 'refine' how we can obtain data in between. Of course, there might be sensors with a higher sampling rate, but to my knowledge all/most the small mems for consumer-grade (e.g. in the iPhone) do not go beyond 100 Hz.

Cheers,
JB

Return to “General discussion”