Heading Hold Mode with Gyro Only

zlantz

So I am not sure if this method is even possible. In theroy it is (i think), I am just not quite sure how to code it.

What I have come up with is this:

    ' // Convert Gyro Value to Degrees per Second
    ' Gyro = Gyro[x] / 16.4       (FS-3)
    Gyro[0] := f.fDiv(A, GyroFS)
    Gyro[1] := f.fDiv(B, GyroFS)
    Gyro[2] := f.fDiv(C, GyroFS)    

    gA := f.fAdd(gA, f.fMul(cdt, Gyro[0]))
    gB := f.fAdd(gB, f.fMul(cdt, Gyro[1]))
    gC := f.fAdd(gC, f.fMul(cdt, Gyro[2]))

    '// g# * 28.64788975 to convert values to degrees  (1/2 of 57.2957795)
    g1 := f.fMul(gA, 28.64788975)  
    g2 := f.fMul(gB, 28.64788975) 
    g3 := f.fMul(gC, 28.64788975)
    
    if Gyro[2] & $8000_0000   ' if Gyro Val is Negative
      dirRot := 0             ' Rotation Direction = 1 (Negative)
    else
      dirRot := 1             ' Rotation Direction = 0 (Positive)
     
    ' // If center, store Current rotation Degrees 
    if f.fRound(Gyro[2]) < 10 and f.fRound(Gyro[2]) > -10       ' if not rotating... store current position
      if isRot == 1
        'repeat until g3 ~= cGyroPos1
        ' Turn(-dirRot)
        isRot := 0
      elseif isRot == 0
        cGyroPos1 := g3

    ' // Detect Rotation
    if f.fRound(Gyro[2]) > 10 or f.fRound(Gyro[2]) < -10        ' if rotating... calc differnece & set tail to correct
      if Gyro[2] & $8000_0000
        if dirRot == 1          ' New <> Old
          RotateBack            ' Direction Changed 
          'repeat until g3 ~= cGyroPos1
          ' Turn(-dirRot)
          isRot := 0
        elseif dirRot == 0      ' New = Old
          cGyroPos2 := g3
          isRot := 1
      else
        if dirRot == 0          ' New <> Old
          RotateBack            ' Direction Changed 
          'repeat until g3 ~= cGyroPos1
          ' Turn(-dirRot)
          isRot := 0
        elseif dirRot == 1      ' New = Old
          cGyroPos2 := g3
          isRot := 1
                                             
    tVal1 := f.fMul(f.fFloat(isRot), 500.0)        
    tVal2 := f.fSub(cGyroPos2, cGyroPos1)
    tVal3 := g3

tVal1 is either 0 or 500. 0 if not rotating, 500 while rotating.
tVal2 shows the direction & amount of rotation turned (difference of NewVal - OldVal)
tVal3 is the Current position of rotation

I am displaying the data in CSV to Serial Chart. Below is a picture of my output from the above code.
You can see the flat spots where Red is 0, which is where it is not rotating. Blue shows amount & direction of rotation, becoming 0 at each moment of no rotation (flat spots).

Basically, I should be able to track the amount of rotation (difference), then calculate a rudder throw to make up for the amount of rotation. While it turns back, it again tracks the amount of rotation until it matches the amount initially turned, where it stops & starts over.

Much easier said than done. Easy with a compass. That would be pretty much no different than Rate Mode (4 lines of code that could really be just one). Tho, I do plan on upgrading to the MPU-9150, which has an on board Magnetometer, I would still like to be able to mix that data with the gryo via a complementary filter to obtain a much more accurate position update than would be from gps & mag, and a potentially faster update rate as well.

Gyro Drift is not a concern as I am only after the High-Pass data (during rotation) then it resets, Plus I found you can filter the drift from seconds to hours (time it takes to drift), but it makes the signal noisy:

    ' // Convert Gyro Value to Degrees per Second
    ' Gyro = Gyro[x] / 16.4       (FS-3)
    Gyro[0] := f.fDiv(A, GyroFS)
    Gyro[1] := f.fDiv(B, GyroFS)
    Gyro[2] := f.fDiv(C, GyroFS)    

  gX := f.fAdd(gX, f.fMul(cdt, Gyro[0]))
  gY := f.fAdd(gY, f.fMul(cdt, Gyro[1]))
  gZ := f.fAdd(gZ, f.fMul(cdt, Gyro[2]))

  tVal1 := f.fMul(gX, 28.64788975) ' * 28.64788975 to convert values to degrees  (1/2 of 57.2957795) 
  tVal2 := f.fMul(gY, 28.64788975) ' Degrees Rotated *** Has Constant Drift from adding time to each equation
  tVal3 := f.fMul(gZ, 28.64788975)    

  ' // Drift Filter, Drifts over Hours, Dirty Signal
  ' tVal = gX - (GyroNew - GyroOld)
  tVal1 := f.fSub(gX, f.fSub(Gyro[0], GyroOld[0]))  
  tVal2 := f.fSub(gY, f.fSub(Gyro[1], GyroOld[1]))
  tVal3 := f.fSub(gZ, f.fSub(Gyro[2], GyroOld[2]))

Although a Low-Pass, Complementary, or maybe an Offset Moving Average Filter might be more proper for the job. I have yet to have success with a Kalman Filter, with the exception of a web example I ported to VB6 (works 100%), but didnt work when I reported to spin.

One potential problem I have noticed is the number count has no limit. it can rotate in a circle 1000 times & it will add up well beyond 360 (to be more like 360,000). So I could do a few tests to find what 360 deg rotation is equal to then divide the total by that to get a value between 0 & 360. But this would require the heli to stop rotating before it would attempt to return itself to position. Which means it wont work like it is suppose to. Instead, it needs to kick in immeaditely and track the amout Drifted via wind, etc & at the same time, track the amount of degrees returned by rudder correction so that it will keep it self center (set rudder to turn to % equal to that of % of rotation, and only rotate that amount), and will return to original position even if drifting ceases.

That is pretty much how Rate Mode works, in that case, you set the rudder to the inverse of the Gyro Deg/s (converted to %). That will make the rudder increase in opposite direction as long as the nose is being turned (no rudder input). But will stop as soon as it stops drifting. I have included a rudder Mix so Rate Mode works with rudder input as well.

Any thoughts / ideas would be greatly appreaciated.