help building complementary filter

Log in or register to post comments
Go To Last Post
7 posts / 0 new
Author
Message
jeet55
jeet55's picture
Level: New Member
Joined: Fri. Dec 20, 2019
Posts: 13 View posts
#1
Posted by jeet55: Tue. Dec 31, 2019 - 02:15 PM
  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

i'm using a 8.8 fixed format for building a complementary filter for self balancing robot, i get correct angle for positive values but it doesn't work for negative values of the angle, i get 0 on the serial monitor.

 

here is the code,

#include "complementary_filter.h"

 uint8_t MPU6050_data[12];
volatile short int acx,gyy,acx_lut_index,gyy_lut_index,acx_deg,gyy_deg,anglex = 0;



void COMPFILT_Initialize()
{
    TCCR0 = (1 <<  WGM01) | (1 << CS00) | (1 << CS02);
    OCR0 = 156;
    //100.16hz sample rate
    TIMSK = (1 << OCIE0);
}

ISR(TIMER0_COMP_vect)
{
    MPU6050_ReadByteS(ACXH,&MPU6050_data[0],6);
    MPU6050_ReadByteS(GYXH,&MPU6050_data[6],6);
    
    acx = MPU6050_data[0] << 8 | MPU6050_data[1];
    gyy = MPU6050_data[8] << 8 | MPU6050_data[9];
    
    
    if (acx < 0)
    {
        acx_lut_index = -acx;
    }
    else
    {
        acx_lut_index = acx;
    }
    
    acx_lut_index = acx_lut_index >> 6;
    acx_lut_index = acx_lut_index << 1;
    acx_deg = (pgm_read_byte(&arcsinelut[acx_lut_index]) << 8 | pgm_read_byte(&arcsinelut[acx_lut_index + 1]));
    
    if (acx < 0)
    {
        //UART_WriteInteger(-(ac_deg >> 8),5);
        acx_deg = -acx_deg;
    }
    
    if (gyy < 0)
    {
        gyy_lut_index = -gyy;
    }
    else
    {
        gyy_lut_index = gyy;
    }
    
    gyy_lut_index = gyy_lut_index >> 7;
    gyy_lut_index = gyy_lut_index << 1;
    gyy_deg = (pgm_read_byte(&gyrolut[gyy_lut_index]) << 8 | pgm_read_byte(&gyrolut[gyy_lut_index + 1]));
    
    if (gyy < 0)
    {
        gyy_deg = -gyy_deg;
    }
    
    anglex =  multfix(0x00fb,anglex) + gyy_deg + multfix(0x0005,acx_deg); //00fb,0005
    if (anglex < 0)
    {
        anglex = -anglex;
        UART_WriteInteger(-(anglex >> 8),5);
        
    }else
    {
        UART_WriteInteger(anglex >> 8,5);
    }
    UART_Write('\n');    
}

 

Tags:

AVR Microcontrollers, megaAVR and tinyAVR
Last Edited: Tue. Dec 31, 2019 - 02:41 PM
Top
Brian Fairchild
Brian Fairchild's picture
Level: Raving Lunatic
Joined: Thu. Oct 18, 2001
Posts: 6379 View posts
Location: Eastern England.
Posted by Brian Fairchild: Tue. Dec 31, 2019 - 02:21 PM
  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

Your 'cut-and paste' seems to have stripped out all your comments.

#1 Hardware Problem? https://www.avrfreaks.net/forum/...

#2 Hardware Problem? Read AVR042.

#3 All grounds are not created equal

#4 Have you proved your chip is running at xxMHz?

#5 "If you think you need floating point to solve the problem then you don't understand the problem. If you really do need floating point then you have a problem you do not understand."

Top
Bronze Markawneil
awneil's picture
Level: 10k+ Postman
Joined: Fri. Jul 1, 2005
Posts: 25563 View posts
Location: Basingstoke, Hampshire, UK
Posted by awneil: Tue. Dec 31, 2019 - 02:24 PM
  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

Please see Tip #1 (in my signature, below; may not be visible on mobile) for how to properly post source code.

Top Tips:

  1. How to properly post source code - see: https://www.avrfreaks.net/comment... - also how to properly include images/pictures
  2. "Garbage" characters on a serial terminal are (almost?) invariably due to wrong baud rate - see: https://learn.sparkfun.com/tutorials/serial-communication
  3. Wrong baud rate is usually due to not running at the speed you thought; check by blinking a LED to see if you get the speed you expected
  4. Difference between a crystal, and a crystal oscillator: https://www.avrfreaks.net/comment...
  5. When your question is resolved, mark the solution: https://www.avrfreaks.net/comment...
  6. Beginner's "Getting Started" tips: https://www.avrfreaks.net/comment...
Top
ki0bk
ki0bk's picture
Level: 10k+ Postman
Joined: Mon. Sep 8, 2014
Posts: 10632 View posts
Location: Over the rainbow
Posted by ki0bk: Tue. Dec 31, 2019 - 02:25 PM
  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

jeet55 wrote:
i get 0 on the serial monitor.

Have you verified that the UART_WriteInteger( ) function works with any neg number?

Jim

 

 

FF = PI > S.E.T

 

Top
Bronze Markawneil
awneil's picture
Level: 10k+ Postman
Joined: Fri. Jul 1, 2005
Posts: 25563 View posts
Location: Basingstoke, Hampshire, UK
Posted by awneil: Tue. Dec 31, 2019 - 02:28 PM
  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

The actual filtering part shouldn't depend on any AVR specifics; so have you tried it on a PC or similar - where debugging will be much easier ...

 

EDIT:  N.Winterbottom has provided an example of doing exactly this in #7, below

 

You do seem to be doing rather a lot in your ISR ...

Top Tips:

  1. How to properly post source code - see: https://www.avrfreaks.net/comment... - also how to properly include images/pictures
  2. "Garbage" characters on a serial terminal are (almost?) invariably due to wrong baud rate - see: https://learn.sparkfun.com/tutorials/serial-communication
  3. Wrong baud rate is usually due to not running at the speed you thought; check by blinking a LED to see if you get the speed you expected
  4. Difference between a crystal, and a crystal oscillator: https://www.avrfreaks.net/comment...
  5. When your question is resolved, mark the solution: https://www.avrfreaks.net/comment...
  6. Beginner's "Getting Started" tips: https://www.avrfreaks.net/comment...
Last Edited: Wed. Jan 1, 2020 - 07:09 PM
Top
jeet55
jeet55's picture
Level: New Member
Joined: Fri. Dec 20, 2019
Posts: 13 View posts
Posted by jeet55: Tue. Dec 31, 2019 - 02:28 PM (Reply to #4)
  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

yes i have

Top
N.Winterbottom
N.Winterbottom's picture
Level: Raving Lunatic
Joined: Tue. Oct 1, 2002
Posts: 2968 View posts
Location: Redbourn, Herts, UK
Posted by N.Winterbottom: Wed. Jan 1, 2020 - 06:30 PM
  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 1

I wondered whether all that negation and shifting would have a bad effect, so I wrote this simple test.

#include <stdio.h>

void UART_WriteInteger(short val, int width)
{
    printf("%*d", width, val);
}

void test_Print (short anglex)
{
    printf("Test Print anglex = %5hd (0x%04hx): ", anglex, anglex);
    if (anglex < 0)
    {
        anglex = -anglex;
        UART_WriteInteger(-(anglex >> 8), 5);
        
    }else
    {
        UART_WriteInteger(anglex >> 8, 5);
    }
    printf("\n");
}	

int main ()
{
    test_Print(+0x00ff);
    test_Print(-0x00ff);
    test_Print(+0x0100);
    test_Print(-0x0100);
}

Compiled using MSVC and ran it.

C:\Users\Nigel\Desktop>jeet55.exe
Test Print anglex =   255 (0x00ff):     0
Test Print anglex =  -255 (0xff01):     0
Test Print anglex =   256 (0x0100):     1
Test Print anglex =  -256 (0xff00):    -1

Perhaps all your trial -VE angles are less than 1.

 

Top