Home › Forums › Forums › Technical Support for BerryIMU › BerryIMU SDL2 Code Example › Reply To: BerryIMU SDL2 Code Example
September 7, 2015 at 2:08 pm #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, ¢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;
}