MMA 8452 filtering

Hardware Autosteer
21

In the software there is a line:
//Place the MPU-6050 spirit level and note the values in the following line for calibration.
Read the roll value when the tractor is level and write the offset in the code once.
There is a “set roll to zero” button in AgOpen as well.
This was the result I got out of the MPU while shaking the unit a little:

MPU-6050 Acc roll_Accplusgyro

The blue line is the raw accelerometer roll, the red one the combination of gyro and accelerometer (angle_roll = angle_roll * 0.9996 + angle_roll_acc * 0.0004) to prevent the
gyro from drifting, dampened with: angle_roll_output = angle_roll_output * 0.90 + angle_roll * 0.1.
Have to get this into the PCBv2.ino and test it on a real tractor…

2 Likes
22

The roll isn’t really changing much at all. That version is pretty old btw.

10 to 12 degrees off, seems to be no problem.

23

Hello Brian, I have it with another version from August too. So first idea, it is due to the ino of Coffeetrac.
Roll values are behaving like they are supposed to. Still the spikes in position are caused by MMA because it does not happen when it is turned off. Everything behaves OK at the range of -1 to 1 degrees. In the video you see what happens on a slope of 1-2 degrees without any bumps. More than 3 degrees roll and the steering gets unusable because of these position spikes, no matter how smooth and bumpless the surface.

The only difference I find in the arduino code is also this part:

const float varProcess = 0.0001; //smaller is more filtering

In the Autosteer_Switch by Coffeetrac, this value is 0.00005. Would this matter?

@Tooki57, @Simon_1 which version of AgOpenGPS do you use? I should update to newest version but I don’t have a slope available to try anyway.

24

I always use the last version. I didn’t do test of mma or ino since the first post here. I have to put steer switch to use Brian’s ino.

25

That (0.00005)is one of the differences, another between switch.ino of dec. 2018 and 08-11-2019 autosteerpcbv2.ino, is mapping, and I am not sure(edit:if the numbers in switch.ino or) the numbers for Dogs2 are correct in the analog part.
From autosteerpcbv2:
//inclinometer goes from -25 to 25 from 0.5 volts to 4.5 volts
rollK = map(roll, 102, 921, -400, 400); //16 counts per degree
Kalman looks the same in the two I compare.

image

26

what i think is the real problem is the ino is far too complicated, the combinations of all the different possibilities is like Linux - most of the people who try to understand what is going on walk away from it. Caution: Gonna vent a little bit here oneveryone lol.

There are 3 different kinds of MMA devices ranging from 12bit to 14 bit med grade (the 8452). The original code is written for the 8451 12 bit code, if using the 8452 i have no idea what the effect of that is.

Here is the setup code for the 8 million possibilities of hardware combinations out there:

//##########################################################################################################
//### Setup Zone ###########################################################################################
//##########################################################################################################

#define GPS_Refresh 10 // Enter the Hz refresh rate, example 5 or 10 or 8 with ublox
// Best is leave it at 10

#define Motor_Valve_Driver_Board 1 // 1 = Steering Motor/valves + Cytron MD30C, MD13A Driver
// 2 = Steering Motor/valves + IBT 2 Driver

#define A2D_Convertor_Mode 1 // 0 = No ADS, connect Wheel Angle Sensor (WAS) to Arduino A0
// Really try to use the ADS, it is much much better.
// 1 = ADS1115 Single Input Mode - Connect Signal to A0
// These sensors are DIY installed ones.
// 2 = ADS1115 Differential Mode - Connect Sensor GND to A1, Signal to A0
// These sensors are factory installed and powered by tractor oem wiring.

#define SteerPosZero 512 //adjust linkage as much as possible to read 0 degrees when wheels staight ahead
// Set to 1660 if using the ADS
// Set to 512 if using the Arduino A0

#define WAS_Invert 0 // set to 1 to Change Direction of Wheel Angle Sensor, must be positive turning right

#define Motor_Direction_Invert 0 // 1 = reverse output direction (Valve & Motor) 0 = Normal

#define SwitchOrButton 1 // set to 0 to use steer switch as switch
// set to 1 to use steer switch as button
// Button/switch pulls pin low to activate

#define BNO_Installed 0 // set to 1 to enable BNO055 IMU, otherwise set to 0 for none

#define Inclinometer_Installed 0 // set to 0 for none
// set to 1 if DOGS2 Inclinometer is installed and connected to ADS pin A2
// set to 2 if MMA8452 installed GY-45 (1C)
// set to 3 if MMA8452 installed Sparkfun, Adafruit MMA8451 (1D)
// set to 4 if DOGS2 installed and connected to Arduino pin A1

                                    // Depending on board orientation, choose the right Axis for MMA, 
                                    // arrow shaft on MMA points in same direction as axle

#define UseMMA_X_Axis 1 // Set to 0 to use X axis of MMA
// Set to 1 to use Y axis of MMA

#define Roll_Invert 0 // Roll to the right must be positive
// Set to 1 if roll to right shows negative, otherwise set to 0

#define Relay_Type 0 // set to 0 for No Relays
// set to 1 for Section Relays
// set to 2 for uTurn Relays

#define EtherNet 0 // 0 = Serial/USB communcation with AOG
// 1 = Ethernet comunication with AOG (using a ENC28J60 chip)

#define CS_Pin 10 // Arduino Nano = 10 depending how CS of Ethernet Controller ENC28J60 is Connected

#define MaxSpeed 20 // km/h above → steering off
#define MinSpeed 1 // km/h below → sterring off (minimum = 0.25)

//##########################################################################################################
//### End of Setup Zone ####################################################################################
//##########################################################################################################

There are 5 different combinations for roll alone. I would like to make the setup for the arduino originate in AOG where it is just pulldowns and checkboxes - but the current format to create the firmware is compiler logic based creation. You can’t “set options” since the options are used to compile and then uploaded. Far too easy for anyone setting up to mess up big time.

For 2 years I’ve watched the conversations of how it doesn’t work on combine forum, so i designed a pcb so everyone could build the same basic system, 1 basic usb autosteer board. I thought people would just build it since all the information was there, a simple ino was working for it, and it worked. But that did not work out that way, people wanted to do their own thing - which i totally understand - not use the pcb, use the bno inside the cab, use the arduino a/d (terrible), use many different combinations of parts. Way too complicated and the problems will happen. Found out only a few actually wanted to make the pcb, back to square one.

Reading this thread - it really shouldn’t be happening. Andreas has assembled pcb boards that work perfectly with existing ino. If you build the pcb on github with the right parts, load the ino, set all the options correctly, it works perfectly - with UDP or USB. Works with the DOGS2 or the MMA equally well - we have 15 degree slopes - it works.

Yes, the ino needs the encoder code added. Easy enough. But i really really think we need an “approved setup” with a known functioning parts list, ino that doesn’t have a million settings, etc. If you choose to build your own thing or don’t want to build it that way - is there any reason you kind of shouldn’t be on your own? And if you don’t want to build the pcb - just buy it from Andreas. It’s still 5000x cheaper then commercial - and it works. And perhaps i need to start another thread called “Deciding on a boilerplate setup”. We know what works and what doesn’t, stop writing code for setups that are barely functional and lets use that experience and save a ton or tonne of grief to provide a successful experience to build autosteer.

Try setting the Kalman filter values by adding zeros or taking away zeros from varProcess - i can’t remember which way does what, the comment is wrong maybe. Recompile, go for a drive. The MMA is well tested, works perfectly. Look carefully at the MMA code of coffeetrac, it is sensitive for either 12 or 14 bits.

8 Likes
27

Brian, I agree with you. The standard PCB is the way to go and we really should let go of self-wired systems. I think others and I built a self-wired system when we were somewhere between the two (I did in March 2019). Except for this one thing, it worked really well so far and I haven’t found the courage yet to throw everything out and rebuild, although I have the PCB in the house. Will do this winter.
Happy holidays to all!

2 Likes
28

Okay so I have a question about the Kalman filter in the AutosteerPCBv2.ino sketch. I think something is missing. I took just the filtering code and did some tests on it in Python, and I don’t see any prediction step, nor can I see any feedback getting pumped back into the algorithm. Shouldn’t the various variables like Pc, G,P, etc, all change over time as the roll changes?

Right now they don’t, once they converge on certain values, irrespective of the roll. In other words they converge on some particular values and stay there. I did a test where I fed a roll of 3 degrees into the filter for 100,000 iterations, and then I switched the roll to 4 degrees and ran another 100,000 iterations and none of these core variables changed at all. Essentially the filter ends up taking a constant portion of the difference between the current roll and the last roll angles, and adds that to the last roll, regardless of how the roll is chaning. Basically you’d get the same effect if you threw out all the variables except G and Kalroll, and set G to 0.03112672920173689. By the way it takes nearly 100 iterations to move the KalRoll from 3 to 4.

I’m not sure that’s quite right. Doesn’t quite seem like a kalman filter to me. Any thoughts? Furthermore, what’s the prediction formula one would use with roll? It would depend on the roll rate, right? But normally a tractor’s roll rate would average to nearly zero nearly all the time, so maybe a kalman filter isn’t appropriate anyway.

Seems like a simple complementary filter would work just as well, for example taking 98% of the new roll and adding it to 2% of the old roll kind of thing. That’s essentially the effect of the code now (albeit with a much smaller amount of new roll).

I can post my little python test script if anyone wants. I could have missed something vital.

29

Not sure, but it seems to filter really nice and by setting varprocess, you can set to as slow or fast as you want. I know Andreas even uses it for dual antenna filtering.
Been using it for 3 years now, but in no way means it works. Code below for reference.

float rollK = 0, Pc = 0.0, G = 0.0, P = 1.0, Xp = 0.0, Zp = 0.0;
float XeRoll = 0;
const float varRoll = 0.1; // variance,
const float varProcess = 0.0001; //smaller is more filtering

//Kalman filter
Pc = P + varProcess;
G = Pc / (Pc + varRoll);
P = (1 - G) * Pc;
Xp = XeRoll;
Zp = Xp;
XeRoll = G * (rollK - Zp) + Xp;
30

Yeah I’m just not too up on how a kalman filter’s terms work. Whatever the case, G ends up being a constant, and a small one at that.

I just double checked my test code just now in case I missed something. With 0.1 and 0.0001, the filter responds extremely slowly to changes in roll. at 10 Hz updates, it would take 6 or more seconds to move from a roll of say 3 degrees to 4 degrees. So if you want the MMA to account for small bumps, it’s not doing that at all at present. It is doing a decent job at representing the longer-term average roll.

I wonder if that’s been part of the issue that @Tooki57 and @Simon_1 are having with their MMA and steering.

Perhaps for testing purposes they might try replacing this filter code in the ino file with something simple like:

//slight complementary filter
XeRoll = 0.98 * rollK + 0.02 * XeRoll

And see what the behavior is. If they need to compensate for smaller lurches, then they’ll need them to not be completely filtered out. But they still need filtering of some kind to cover up the oscillations from vibrations. The coefficients of a complementary filter can be adjusted to see what works provided they add up to 1. A smaller term with rollK produces more filtering.

Anyway just a thought. I don’t have an MMA hooked up to test just yet but plan to.

31

At the moment, all my tractors are hibernating. But I will make sure to set up and test this at spring sowing in early February.

32

I have all the boards ripped out of AgraBot. Gonna make up a test station with it and hopefully can answer some of these questions. I have not seen any issue at all, so this will be fun!

I tried finding again where i found that filter on the web, but as usual can’t seem to. There are hundreds of single dimension Kalman formats and equations.

1 Like
33

EDIT: Had P wrongly initialized to 0. Changed it to 1.

I am curious about it for sure. Here’s the python script I was using to do some simplistic testing, which you could easily adapt to C#.

Pc = 0
P = 1
varProcess = 0.001
varRoll = 0.1
G = 0
Xp = 0
Zp = 0
KalRoll = 0

print ("Roll from 0 to 3 degrees and hold.")
roll = 3
for x in range(1000): #nothing seems to change after 1000 iterations
    Pc = P + varProcess
    G = Pc / (Pc + varRoll)
    P = (1-G) * Pc
    Xp = KalRoll
    Zp = Xp
    KalRoll = (G * (roll-Zp)) + Xp
    print (x, roll, KalRoll, Pc, G, P)

print ("Roll to 4 degrees and hold.")
roll = 4
for x in range(1000): #nothing seems to change after 1000 iterations
    Pc = P + varProcess
    G = Pc / (Pc + varRoll)
    P = (1-G) * Pc
    Xp = KalRoll
    Zp = Xp
    KalRoll = (G * (roll-Zp)) + Xp
    print (x, roll, KalRoll, Pc, G, P)

Seems like it takes about 40-50 iterations to get KalRoll as close to the “measured” roll as it’s ever going to get, and never does quite reach it. That’s about 4 seconds if running the loop at 10 hz. Much faster (fast enough?) if running it at say 200 hz. I guess I didn’t look too closely at how fast the arduino sketch was running this filter.

I started looking at this when I was trying to use the code to filter some roll information was I getting from the Starfire, but it’s only at 5hz. So it definitely didn’t work for my situation.

34

Set P to 1 initially instead of zero.

35

Yes that did make it move a little faster. It gets most of the way there quickly now. Still takes about 40 iterations for it to move from 3 to close to 4 though. And G is still settling into a constant, which means that none of the other calculations are really necessary once you know what G is for the parameters one chooses. Making VarProcess 0.01 makes it move more quickly of course. I haven’t tried generating noisy roll values to see what happens just yet. so it looks to me like it’s using part of the Kalman algorithm to filter, but it’s not predicting (Xp is set simply to the last observation with no change), and it’s not adjusting to the observations, which is fine if it’s doing the filtering that’s needed. So maybe nothing further to do if it’s working.

36

That filter has a lot of capability we are not using - mostly because we don’t know the noise variance, covariance etc. Maybe this will help to see it in real time. Little program on Telegram, can’t post it here, no zips allowed. Maybe should change that.

https://youtu.be/u23P-mLuyxo

1 Like
37

Good explanation Brian. Thank you. Looks like everything is working as intended. Very interesting.

Here’s a question for you, though. You mention that it’s better to filter out the bumps the tractor goes over. In other words don’t compensate for every little bump that will throw the tractor momentarily off the average roll. However since GPS is coming in at a fast rate, say 10 hz, the GPS position will be changed by that bump, enough to move it by a foot sometimes if it was, say a ditch. Does this cause some wavy driving?

38

Well that’s part of the problem - delays in the system. By the time you receive a gps fix, then do all the frame calcs, then send to arduino, wait till arduino sends back roll, do the roll calcs, the bump is long gone.

I still feel the best solution would be gps to an arduino which has the imu in it, it corrects the position immediately and then sends to aog. However there is still delay in the sent out nmea. There is real advantage by commercial units to use the stream directly and have it all in one box.

39

Interesting development. I built a test station with the motor fairly close to the mma - about 15cm away - and the mma behaves quite erratically when the motor turns. It moves the line position all over the place just as you said. So, are you getting interference when using the MMA in the tractor? I used all the same parts from my tractor which worked flawless before removing. Same code, same computer.

40

The only test i have done of mma was : all electronics in a box ( pcbv2, 12/24V, cytron md30, ardusimple) and tablet on the top of the box .