[jawood]

hi thanks for your reply, the screen is competly blank.

I set to CLI only on boot and still same no display.

[jawood]

This is what i had on my origonal build that works

 

bottom of /boot/config.txt file

dtoverlay=piscreen,speed=24000000
dtparm=i2c_arm=on
dtparm=i2c1=on

end of line in /boot/cmdline.txt

fbcon=map:10 fbcon=rotate:2 fbcon=font:ProFont6x11

[jawood]

For those that are interested I did work out how to make it work full screen and without a title bar.

if you just want no title bar add |SDL_NOFRAME to the end of
screen = SDL_SetVideoMode(480, 320, 0, SDL_SWSURFACE); (note I've change the first bit of this to hard define the size of the window the same size as the back ground image. Should look like this now
screen = SDL_SetVideoMode(480, 320, 0, SDL_SWSURFACE|SDL_NOFRAME); if you want to run full screen you only need to add |SDL_FULLSCREEN instead of SDL_NOFRAME the full screen command removes the boarders automatically.

Note:
If you remove the title bar you wont be able to move the window around the screen.
If you cant get to the terminal window you used to start the inclinometer you wont be able to kill it easily. (Need to work out how to enable the close button on the title bar, and how to move window when no frame.)
Also would be nice to be able to get it to run from a shortcut on the desktop....

[jawood]

yeah another happy customer...

Got to support our Aussie Companies.

Mark's help here was awesome...

[jawood]

Yeah no problem that code I posted before has the Kalman Filter applied. I just copy and paste the Kalman Filter bits from the code in gyro_accelerometer_tutorial03_Kalman_filter and added it to the code in gyro_accelerometer_tutorial02_inclinometer.

The full screen thing still didn't seem to work thou, still has the blue title bar at the top of the window, and can only see half the ROLL and PITCH laybels.

Other then that all is working great now PiScreen and BerryIMU.

[jawood]

Im assuming I add

SDL_WM_ToggleFullScreen(SDL_Surface *surface);

I added it to function:-

//Initialise SDL and Disable Mouse
SDL_Init(SDL_INIT_VIDEO);
SDL_ShowCursor(SDL_DIABLE);
SDL_WM_ToggleFullScreen(SDL_Surface *surface);

Compile:-

gcc -o KalmanFilterLevelMeter1 -lm LevelMeterKalmanFiltered.c sdl-config --cflags sdl-config --libs -lSDL_image -lSDL_gfx

And get error:-

LevelMeterKalmanFiltered.c: In function ‘startSDL’:
LevelMeterKalmanFiltered.c:233:26: error: expected expression before ‘SDL_Surface’
LevelMeterKalmanFiltered.c:236:19: warning: assignment discards ‘const’ qualifier from pointer target type [enabled by default]
LevelMeterKalmanFiltered.c: In function ‘graphics’:
LevelMeterKalmanFiltered.c:299:38: warning: assignment makes pointer from integer without a cast [enabled by default]
LevelMeterKalmanFiltered.c:300:39: warning: assignment makes pointer from integer without a cast [enabled by default]

[jawood]

Ow one more thing I just did the screen blanking thing and its still blanking.

sudo apt-get install x11-xserver-utils
sudo nano /etc/xdg/lxsession/LXDE/autostart

added

@xset s off # don't activate screensaver
@xset -dpms # disable DPMS (Energy Star) features.
@xset s noblank # don't blank the video device

Reboot. Screen still blanking after 10mins

[jawood]

Yeah it works great now with the Kalman Filter applied.
I would like to be able to get it to run without the blue bar at the top of the window and how to get it to open full screen. Any help with that would be much appreciated. Thanks

[jawood]

Ow yeah that's better very stable now and accurate. Cant believe the difference just using the kalman filter makes.

Now all I need to know is how to make it run as a full screen app without taskbar

[jawood]

I think this should do it. Whats in bold I added.

/*

This program is used to create an Inclinometer using a
Raspberry Pi and an IMU

https://ozzmaker.com/2014/12/12/inclinometer-using-raspberry-pi-imu/

Copyright (C) 2014 Mark Williams

This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Library General Public
License as published by the Free Software Foundation; either
version 2 of the License, or (at your option) any later version.
This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
Library General Public License for more details.
You should have received a copy of the GNU Library General Public
License along with this library; if not, write to the Free
Software Foundation, Inc., 59 Temple Place - Suite 330, Boston,
MA 02111-1307, USA
*/

#include "SDL.h"
#include "SDL/SDL_image.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 "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

//Used by Kalman Filters
float Q_angle = 0.01;
float Q_gyro = 0.0003;
float R_angle = 0.01;
float x_bias = 0;
float y_bias = 0;
float XP_00 = 0, XP_01 = 0, XP_10 = 0, XP_11 = 0;
float YP_00 = 0, YP_01 = 0, YP_10 = 0, YP_11 = 0;
float KFangleX = 0.0;
float KFangleY = 0.0;

float kalmanFilterX(float accAngle, float gyroRate);
float kalmanFilterY(float accAngle, float gyroRate);

int graphics(float carRoll, float carPitch);
int startSDL();
SDL_Surface* screen = NULL;
SDL_Surface* inclinometerJeepFront = NULL;
SDL_Surface* inclinometerJeepSide = NULL;
SDL_Surface* inclinometerOverlay = NULL;
SDL_Surface* compatibleInclinometerJeepFront = NULL;
SDL_Surface* compatibleInclinometerJeepSide = NULL;
SDL_Surface* compatibleInclinometerOverlay = NULL;
SDL_Surface* rotationInclinometerJeepFront = NULL;
SDL_Surface* rotationInclinometerJeepSide = NULL;
SDL_Rect inclinometerJeepOverlayPosition;
SDL_Rect inclinometerJeepFrontPosition;
SDL_Rect inclinometerJeepSidePosition;

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;

signed int acc_y = 0;
signed int acc_x = 0;
signed int acc_z = 0;
signed int gyr_x = 0;
signed int gyr_y = 0;
signed int gyr_z = 0;

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;

//Kalman Filter
float kalmanX = kalmanFilterX(AccXangle, rate_gyr_x);
float kalmanY = kalmanFilterY(AccYangle, rate_gyr_y);
printf ("33[22;31mkalmanX %7.3f 33[22;36mkalmanY %7.3f\t\e[m",kalmanX,kalmanY);

//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(kalmanX,kalmanY);
else if (strcmp(argv[1],"nosdl")!= 0)
graphics(kalmanX,kalmanY);

//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.
screen = SDL_SetVideoMode(videoInfo->current_w, videoInfo->current_h, videoInfo->vfmt->BitsPerPixel, SDL_SWSURFACE );
if ( screen == NULL ) {
fprintf(stderr, "Unable to setvideo: %s\n", SDL_GetError());
exit(1);
}

//Load image.
inclinometerJeepFront = IMG_Load("inclinometerJeepFront.png");
if (inclinometerJeepFront == NULL){
printf("error loading JeepFront image\n");
SDL_Quit();
exit(1);
}
//Convert the image to a format we can use.
compatibleInclinometerJeepFront = SDL_DisplayFormatAlpha(inclinometerJeepFront);

//Load image.
inclinometerJeepSide = IMG_Load("inclinometerJeepSide.png");
if (inclinometerJeepSide == NULL){
printf("error loading JeepSide image\n");
SDL_Quit();
exit(1);
}
//Convert the image to a format we can use.
compatibleInclinometerJeepSide = SDL_DisplayFormatAlpha(inclinometerJeepSide);

//Load image.
inclinometerOverlay = IMG_Load("inclinometerOverlay.png");
if (inclinometerOverlay == NULL){
printf("error loading Overlay image\n");
SDL_Quit();
exit(1);
}
//Convert the image to a format we can use.
compatibleInclinometerOverlay = SDL_DisplayFormatAlpha(inclinometerOverlay);

//Position the overlay in the middle of the screen. screen heigth minus the height of the overlay image.
inclinometerJeepOverlayPosition.y = (videoInfo->current_h/2)-(compatibleInclinometerOverlay->h/2);

}

int graphics(float carRoll, float carPitch)
{

//Set all pixals to black to clear the last image.
SDL_FillRect(screen,NULL,0x000000);

//Position of both jeep images.
inclinometerJeepFrontPosition.x = 30;
inclinometerJeepFrontPosition.y = (videoInfo->current_h/2)-(compatibleInclinometerJeepFront->h/2);
inclinometerJeepSidePosition.x = 262;
inclinometerJeepSidePosition.y = (videoInfo->current_h/2)-(compatibleInclinometerJeepFront->h/2);

//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 -= rotationInclinometerJeepFront->w/2-compatibleInclinometerJeepFront->w/2;
inclinometerJeepFrontPosition.y -= rotationInclinometerJeepFront->h/2-compatibleInclinometerJeepFront->h/2;
inclinometerJeepSidePosition.x -= rotationInclinometerJeepSide->w/2-compatibleInclinometerJeepSide->w/2;
inclinometerJeepSidePosition.y -= rotationInclinometerJeepSide->h/2-compatibleInclinometerJeepSide->h/2;

//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);

//Send the surface to the display.
SDL_Flip(screen);

//Free surfaces.
SDL_FreeSurface(screen);
SDL_FreeSurface(rotationInclinometerJeepFront);
SDL_FreeSurface(rotationInclinometerJeepSide);
return 0;

}

float kalmanFilterX(float accAngle, float gyroRate)
{
float y, S;
float K_0, K_1;

KFangleX += DT * (gyroRate - x_bias);

XP_00 += - DT * (XP_10 + XP_01) + Q_angle * DT;
XP_01 += - DT * XP_11;
XP_10 += - DT * XP_11;
XP_11 += + Q_gyro * DT;

y = accAngle - KFangleX;
S = XP_00 + R_angle;
K_0 = XP_00 / S;
K_1 = XP_10 / S;

KFangleX += K_0 * y;
x_bias += K_1 * y;
XP_00 -= K_0 * XP_00;
XP_01 -= K_0 * XP_01;
XP_10 -= K_1 * XP_00;
XP_11 -= K_1 * XP_01;

return KFangleX;
}

float kalmanFilterY(float accAngle, float gyroRate)
{
float y, S;
float K_0, K_1;

KFangleY += DT * (gyroRate - y_bias);

YP_00 += - DT * (YP_10 + YP_01) + Q_angle * DT;
YP_01 += - DT * YP_11;
YP_10 += - DT * YP_11;
YP_11 += + Q_gyro * DT;

y = accAngle - KFangleY;
S = YP_00 + R_angle;
K_0 = YP_00 / S;
K_1 = YP_10 / S;

KFangleY += K_0 * y;
y_bias += K_1 * y;
YP_00 -= K_0 * YP_00;
YP_01 -= K_0 * YP_01;
YP_10 -= K_1 * YP_00;
YP_11 -= K_1 * YP_01;

return KFangleY;
}

[jawood]

thanks for the sugestions I was wondering if adding the kalman filter would make it a bit better. I had a look and it seems pretty straight forward to add it.

I think change this graphics(CFangleX,CFangleY); to graphics(kalmanX,kalmanY); and add the functions variables etc..

[jawood]

Ow one other thing I would love to be able to run it as a full screen app, so you cant see the task bar or anything else. Would appreciate some advise on how to do that!

And thanks for all your help getting it going!

[jawood]

gcc -o gyro_accelerometer_tutorial02 -lm gyro_accelerometer_tutorial02.c sdl-config --cflags sdl-config --libs -lSDL_image -lSDL_gfx
gyro_accelerometer_tutorial02.c: In function ‘startSDL’:
gyro_accelerometer_tutorial02.c:217:19: warning: assignment discards ‘const’ qualifier from pointer target type [enabled by default]
gyro_accelerometer_tutorial02.c: In function ‘graphics’:
gyro_accelerometer_tutorial02.c:280:38: warning: assignment makes pointer from integer without a cast [enabled by default]
gyro_accelerometer_tutorial02.c:281:39: warning: assignment makes pointer from integer without a cast [enabled by default]

sudo ./gyro_accelerometer_tutorial02 It Works!!!! So cool!!!!

Want to use this as a leveller for my Caravan connected to 4 worm shafts to auto level the Van. (I think the update rate is going to be to slow thou.)

Thou it is working now it is quite slow to update and appears to be slightly out of calibration when on a flat level surface after a second or so when it stabilize I have to hold the unit at about 10 degrees off flat on the left/right axis to show flat on the PiScreen, front to back seems to be ok.

[jawood]

sudo apt-get install libi2c-dev
Reading package lists... Done
Building dependency tree
Reading state information... Done
libi2c-dev is already the newest version.
0 upgraded, 0 newly installed, 0 to remove and 4 not upgraded.

[jawood]

yeah tried that no change same error

[Author]

Posts