BerryIMU SDL2 Code Example

[Micard]

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!

[Micard]

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

[Micard]

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;
}

[Mark Williams]

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

Mark --OzzMaker.com --

[Micard]

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.

[Author]

Posts