BerryIMU SDL2 Code Example

Home Forums Forums Technical Support for BerryIMU BerryIMU SDL2 Code Example

Viewing 5 posts - 1 through 5 (of 5 total)
  • Author
    Posts
  • #4253
    Micard
    Participant

    Hi,

    First of all, thank you for your piece of amazingness! So my BerriIMU is working and is being recognized by my RasPi but I can't really compile the code in the tutorial since I'm using SDL2 and "downgrading" to SDL 1.2 on RPi is just not an option... Could you please re-write some examples for SDL2. I tried myself but I'm not a pro at this so I can't figure somethings out by myself.

    Thank you!

    #4254
    Micard
    Participant

    My main problem is the rotation of surfaces. Since Im using textures and rectangles in SDL2, I dont know how to rotate them...

    #4255
    Micard
    Participant

    Here's what I got so far. THE CONSOLE IS WORKING. I get the readings from BerryIMU but the window itself doesn't even initialize. Not errors during compilation:

    #include <SDL2/SDL.h>
    #include <SDL2/SDL_image.h>
    #include <SDL2/SDL_render.h>
    #include <unistd.h>
    #include <math.h>
    #include <signal.h>
    #include <stdio.h>
    #include <stdlib.h>
    #include <fcntl.h>
    #include <string.h>
    #include <time.h>
    #include <sys/time.h>
    #include "sensor.c"
    #define DT 0.2         	//  loop period.  0.2  = 200ms
    #define AA 0.97         // complementary filter constant
    #define A_GAIN 0.0573   // [deg/LSB]
    #define G_GAIN 0.070    // [deg/s/LSB]
    #define RAD_TO_DEG 57.29578
    #define M_PI 3.14159265358979323846
    #define DISPLAY_WIDTH 1024
    #define DISPLAY_HEIGHT 768
    int graphics(float carRoll, float carPitch);
    int startSDL();
            //SDL_Surface* screen = NULL;
            SDL_Texture* inclinometerJeepFront;
            SDL_Texture* inclinometerJeepSide;
            SDL_Texture* inclinometerOverlay;
            SDL_Texture* compatibleInclinometerJeepFront;
            SDL_Texture* compatibleInclinometerJeepSide;
            SDL_Texture* compatibleInclinometerOverlay;
            SDL_Texture* rotationInclinometerJeepFront;
            SDL_Texture* rotationInclinometerJeepSide;
    		SDL_Rect inclinometerJeepOverlayPosition;
            SDL_Rect inclinometerJeepFrontPosition;
            SDL_Rect inclinometerJeepSidePosition;
    		SDL_Point center = {1, 1};
    		SDL_RendererFlip flip = SDL_FLIP_NONE;
    	SDL_Window *screen = SDL_CreateWindow("Window", SDL_WINDOWPOS_UNDEFINED, SDL_WINDOWPOS_UNDEFINED, 1024, 768, SDL_WINDOW_OPENGL);
    		SDL_Renderer *renderer = SDL_CreateRenderer(screen, -1, 0);
    	//SDL_VideoInfo* videoInfo;
    void  INThandler(int sig)
    {
            //SDL_FreeSurface(screen);
            SDL_Quit();
            signal(sig, SIG_IGN);
            exit(0);
    }
    int mymillis()
    {
    	struct timeval tv;
    	gettimeofday(&tv, NULL);
    	return (tv.tv_sec) * 1000 + (tv.tv_usec)/1000;
    }
    int timeval_subtract(struct timeval *result, struct timeval *t2, struct timeval *t1)
    {
        long int diff = (t2->tv_usec + 1000000 * t2->tv_sec) - (t1->tv_usec + 1000000 * t1->tv_sec);
        result->tv_sec = diff / 1000000;
        result->tv_usec = diff % 1000000;
        return (diff<0);
    }
    int main(int argc, char *argv[])
    {
    	float rate_gyr_y = 0.0;   // [deg/s]
    	float rate_gyr_x = 0.0;    // [deg/s]
    	float rate_gyr_z = 0.0;     // [deg/s]
    	int  acc_raw[3];
    	int  mag_raw[3];
    	int  gyr_raw[3];
    	float gyroXangle = 0.0;
    	float gyroYangle = 0.0;
    	float gyroZangle = 0.0;
    	float AccYangle = 0.0;
    	float AccXangle = 0.0;
    	float CFangleX = 0.0;
    	float CFangleY = 0.0;
    	int startInt  = mymillis();
    	struct  timeval tvBegin, tvEnd,tvDiff;
            signal(SIGINT, INThandler);
    	enableIMU();
    	gettimeofday(&tvBegin, NULL);
            if(argc <=1)
                    startSDL();
            else if (strcmp(argv[1],"nosdl")!= 0)
                    startSDL();
    	while(1)
    	{
    	startInt = mymillis();
    	//read ACC and GYR data
    	readACC(acc_raw);
    	readGYR(gyr_raw);
    	//Convert Gyro raw to degrees per second
    	rate_gyr_x = (float) gyr_raw[0] * G_GAIN;
    	rate_gyr_y = (float) gyr_raw[1]  * G_GAIN;
    	rate_gyr_z = (float) gyr_raw[2]  * G_GAIN;
    	//Calculate the angles from the gyro
    	gyroXangle+=rate_gyr_x*DT;
    	gyroYangle+=rate_gyr_y*DT;
    	gyroZangle+=rate_gyr_z*DT;
    	//Convert Accelerometer values to degrees
    	AccXangle = (float) (atan2(acc_raw[1],acc_raw[2])+M_PI)*RAD_TO_DEG;
    	AccYangle = (float) (atan2(acc_raw[2],acc_raw[0])+M_PI)*RAD_TO_DEG;
            //Change the rotation value of the accelerometer to -/+ 180 and move the Y axis '0' point to up.
            //Two different pieces of code are used depending on how your IMU is mounted.
            //If IMU is upside down
            /*
            if (AccXangle >180)
                    AccXangle -= (float)360.0;
            AccYangle-=90;
            if (AccYangle >180)
                    AccYangle -= (float)360.0;
            */
            //If IMU is up the correct way, use these lines
            AccXangle -= (float)180.0;
            if (AccYangle > 90)
                    AccYangle -= (float)270;
            else
                    AccYangle += (float)90;
    	//Complementary filter used to combine the accelerometer and gyro values.
    	CFangleX=AA*(CFangleX+rate_gyr_x*DT) +(1 - AA) * AccXangle;
    	CFangleY=AA*(CFangleY+rate_gyr_y*DT) +(1 - AA) * AccYangle;
    	printf ("   GyroX  %7.3f \t AccXangle \e[m %7.3f \t 33[22;31mCFangleX %7.3f33[0m\t GyroY  %7.3f \t AccYangle %7.3f \t 33[22;36mCFangleY %7.3f\t33[0m\n",gyroXangle,AccXangle,CFangleX,gyroYangle,AccYangle,CFangleY);
            if(argc <=1)
                    graphics(CFangleX,CFangleY);
            else if (strcmp(argv[1],"nosdl")!= 0)
                    graphics(CFangleX,CFangleY);
    	//Each loop should be at least 20ms.
            while(mymillis() - startInt < (DT*1000))
            {
                usleep(100);
            }
    	printf("Loop Time %d\t", mymillis()- startInt);
        }
    }
    int startSDL()
    {
    	//fb1 = small TFT.   fb0 = HDMI/RCA output
            putenv("SDL_FBDEV=/dev/fb0");
    	//Initialize  SDL and disable mouse
            SDL_Init(SDL_INIT_VIDEO);
            SDL_ShowCursor(SDL_DISABLE);
    	//Get information about the current video device.  E.g. resolution and bits per pixal
           // videoInfo = SDL_GetVideoInfo ();
    	//Setup a Video mode.
            
    	//Load image.
    	compatibleInclinometerJeepFront =  IMG_LoadTexture(renderer, "inclinometerJeepFront.png");
            if (compatibleInclinometerJeepFront == NULL){
            printf("error loading JeepFront image\n");
                            SDL_Quit();
                            exit(1);
                    }
    	//Load image.
    	compatibleInclinometerJeepSide =  IMG_LoadTexture(renderer, "inclinometerJeepSide.png");
                    if (compatibleInclinometerJeepSide == NULL){
                             printf("error loading JeepSide image\n");
                            SDL_Quit();
                            exit(1);
                    }
    	//Load image.
            compatibleInclinometerOverlay =  IMG_LoadTexture(renderer, "inclinometerOverlay.png");
                    if (compatibleInclinometerOverlay == NULL){
                             printf("error loading Overlay image\n");
                            SDL_Quit();
                            exit(1);
                    }
    	//Position the overlay in the middle of the screen.   screen heigth minus the height of the overlay image.
            inclinometerJeepOverlayPosition.y = (DISPLAY_HEIGHT/2)-160;
    }
    int graphics(float carRoll, float carPitch)
    {
    	//Set all pixals to black to clear the last image.
            SDL_SetRenderDrawColor(renderer, 0, 0, 0, 255);
    		SDL_RenderClear(renderer);
    		
    	//Position of both jeep images.
            inclinometerJeepFrontPosition.x = 30;
            inclinometerJeepFrontPosition.y = (DISPLAY_HEIGHT/2)-65;
            inclinometerJeepSidePosition.x = 262;
            inclinometerJeepSidePosition.y = (DISPLAY_HEIGHT/2)-65;
    	//Rotate the images based on pitch and roll.
           /* rotationInclinometerJeepSide = rotozoomSurface(compatibleInclinometerJeepSide, carPitch, 1.0, 0.0);
            rotationInclinometerJeepFront = rotozoomSurface(compatibleInclinometerJeepFront, carRoll, 1.0, 0.0);*/
    		
    	//Recenter pivot point.
    		inclinometerJeepFrontPosition.x -= 90;
            inclinometerJeepFrontPosition.y -= 65;
            inclinometerJeepSidePosition.x -= 95;
            inclinometerJeepSidePosition.y -= 65;
    	//Blit the three iamges to the surface.
    	/*SDL_BlitSurface( compatibleInclinometerOverlay, NULL, screen, &inclinometerJeepOverlayPosition);
            SDL_BlitSurface( rotationInclinometerJeepFront, NULL, screen, &inclinometerJeepFrontPosition);
            SDL_BlitSurface( rotationInclinometerJeepSide, NULL, screen, &inclinometerJeepSidePosition);*/
    		
    	SDL_RenderCopy(renderer, compatibleInclinometerOverlay, NULL, &inclinometerJeepOverlayPosition);
    	SDL_RenderCopyEx(renderer, compatibleInclinometerJeepFront, NULL, &inclinometerJeepFrontPosition, carRoll, &center, flip);
    	SDL_RenderCopyEx(renderer, compatibleInclinometerJeepSide, NULL, &inclinometerJeepFrontPosition, carPitch, &center, flip);
    	//Free surfaces.
    	
    		SDL_RenderPresent(renderer);
    		
            /*SDL_FreeSurface(screen);
            SDL_FreeSurface(rotationInclinometerJeepFront);
            SDL_FreeSurface(rotationInclinometerJeepSide);*/
            return 0;
    }
    
    #4256
    Mark Williams
    Keymaster

    where are you trying to display the output? out the HDMI port?

    Mark --OzzMaker.com --

    #4257
    Micard
    Participant

    Yes, HDMI port. The putenv function I believe is set to display video through HDMI. I'm pretty sure it's in the code... I've run code using SDL2 before on my Pi. I just don't want to completely rewrite it since I'm not quite sure how the interaction works yet.

Viewing 5 posts - 1 through 5 (of 5 total)
  • You must be logged in to reply to this topic.

Blip, blop, bloop...