Home › Forums › Forums › Technical Support for BerryIMU › BerryIMU SDL2 Code Example
- This topic has 4 replies, 2 voices, and was last updated 8 years, 2 months ago by Micard.
- AuthorPosts
- September 7, 2015 at 8:13 am #4253MicardParticipant
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!
September 7, 2015 at 11:32 am #4254MicardParticipantMy main problem is the rotation of surfaces. Since Im using textures and rectangles in SDL2, I dont know how to rotate them…
September 7, 2015 at 2:08 pm #4255MicardParticipantHere’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, ¢er, flip); SDL_RenderCopyEx(renderer, compatibleInclinometerJeepSide, NULL, &inclinometerJeepFrontPosition, carPitch, ¢er, flip); //Free surfaces. SDL_RenderPresent(renderer); /*SDL_FreeSurface(screen); SDL_FreeSurface(rotationInclinometerJeepFront); SDL_FreeSurface(rotationInclinometerJeepSide);*/ return 0; }September 7, 2015 at 2:36 pm #4256Mark WilliamsKeymasterwhere are you trying to display the output? out the HDMI port?
Mark --OzzMaker.com --
September 8, 2015 at 5:34 am #4257MicardParticipantYes, 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.
- AuthorPosts
- You must be logged in to reply to this topic.