WebSVN - shark - Rev 1650 - /advdemos/trunk/gpsmesa/gpsmesa.c

/*
* Project: S.Ha.R.K.
* G P S D E M O : this program receives and displays data from a gps
* transmitter connected to USB port. This is also a graphic demo that
* uses a software emulation of MESA libraries to display actual latitude
* and longitude on the earth globe. You can also use the program without
* having a gps transmitter because it can run in a demo mode (to do that
* press d key when the program starts).
*
* In the main function graphics and com1 port are initialized, respectively
* with a call to the functions gl_init and open_com. Then all the tasks
* used in the program are created and activated, these are :
*
* TASK refresh : copies the gl buffer to video memory
* TASK draw : draws all the graphics
* TASK gps : receives data from gps transmitter
*
*/

#include <kernel/log.h>
#include <kernel/kern.h>
#include <math.h>
#include <stdlib.h>

#include <drivers/shark_fb26.h>
#include <drivers/shark_keyb26.h>

#include <GL/osmesa.h> //MESA header
#include <GL/glut.h> //MESA header
#include "texture.h" //contains three arrayes that define the R, G, B data of the first texture (phisical earth)
#include "texture2.h" //contains three arrayes that define the R, G, B data of the second texture (political earth)

#ifndef M_PI
#define M_PI 3.14159265
#endif

//Graphic variables and constants

#define WIDTH 800 //Width of the screen in pixel
#define HEIGHT 600 //Height of the screen in pixel
#define BYTES_PP 2 //BytesPerPixel

OSMesaContext ctx;

extern void *video_memory;

unsigned char *rgb_565_buf = NULL; //RGB 16 bpp Buffer
unsigned char *video_buf = NULL; //Video Buffer
unsigned long int VMEMLONG = WIDTH*HEIGHT*BYTES_PP / 4; //Used by copy_videomem_16to16
unsigned long int RGB565MEM = WIDTH*HEIGHT*BYTES_PP; //Total video mem

//Graphic Objects variables and constants

GLdouble radius, twist; //radius and twist of the sphere
float zoom = 0; //zoom factor
int sel_texture = 1; //index of the actually selected texture
GLubyte tex[256][256][3]; //stores the R, G, B data of the first texture
GLubyte tex2[256][256][3]; //stores the R, G, B data of the second texture
GLint sphere; //index of the shpere list (see mesa functions for details)

//Tasks variables and constants

unsigned long int PERIOD_DRAW = 100000; //period of the draw task
unsigned long int PERIOD_GPS = 1000; //period of the gps task
unsigned long int WCET_DRAW, WCET_GPS; //deadlines of the tasks
PID draw_PID, gps_PID; //indexes of the tasks

//Gps receiver task variables and constants

char word[30]; //contains every word received from the gps transmitter
int ind = 0; //contains the index of the next character to be written in the variable word
int ind_gpgga = 0; //index of the next gpgga word to be read from gps
int ind_gpgsv = 0; //index of the next gpgsv word to be read from gps
int ind_gprmc = 0; //index of the next gprmc word to be read from gps
int nlist; //used to index satellites in gps_sat_data structure

//Demo mode variables and functions

int demo_mode = 1; //indicates if demo mode is active(=1) or not (=0)
int ndemo = 0; //a counter used while running in demo mode
int lat, lon; //auxiliary variables used for integer division
int kmov; //used to change the moving direction
float demolat = 0; //stores latitude
float demolon = 0; //stores longitude

//Gps data structures

typedef struct {
float id, elevation, azimuth, signal_level;
} gps_sat_data;

struct {
float latitude, longitude, altitude, nsat, speed, dir_mov, date, var_mov;
char dirlat, dirlon, time[12], dir_var_mov;
gps_sat_data sat_data[4]; //data of the satellites in view (4 maximum)
} gps_data;

TASK refesh(void);
TASK draw(void);
TASK gps(void);

//converts a string to a floating point
float strtof(char *s)
{
int d, n;
float f = 0, pot;

//if the string is null ends returning 0
if(s[0] == '\0')
return 0;

//finds the dot character until the end of the string
for(d = 0; (s[d] != '.') && (s[d] != '\0') ; d++);

//converts non decimal numbers
pot = 1;
for(n = d - 1; n >= 0; n--) {
f = f + pot * (s[n] - 48);
pot = pot * 10;
}

//ends if there are not decimal numbers
if( s[d] == '\0')
return f;

//converts decimal numbers
pot = 0.1;
for(n = d + 1; s[n] != '\0'; n++) {
f = f + pot * (s[n] - 48);
pot = pot / 10;
if(n >= 30)
break;

}
return f;
}

//compares two strings returning 1 if they are equal otherwise 0
int streq(char *s1, char *s2)
{
int n = 0;
while(1)
if( s1[n] == s2[n] )
if( s1[n] == '\0' ) {
return 1;
}
else {
n++;
//ends if the string is larger than the word variable
if(n >= 30)
return 0;
}
else
return 0;
}

//copies s2 string in s1 string
void strcp(char *s1, char *s2)
{
int n = 0;
while( s2[n] != '\0' ) {
s1[n] = s2[n];
n++;
//ends if the string is larger than the word variable
if(n >= 30)
break;
}
s1[n] = '\0';
}

/*rotates the axes of the mesa graphic objects and also of the sphere
using the latitude and longitude data contained in the gps_data structure */
void polarView()
{
float signlat, signlon; //contains directions of rotation

//translates the axes far or near from the observer point to make zoom effect
glTranslated(0.0, 0.0, radius + zoom);
glRotated(-twist, 0.0, 0.0, 1.0);

//direction of rotation in latitude may be Nord 'N' or Suoth 'S'
if(gps_data.dirlat == 'N')
signlat = -1;
else
signlat = 1;

//direction of rotation in longitude may be East 'E' or West 'W'
if(gps_data.dirlon == 'E')
signlon = -1;
else
signlon = 1;

/*there are some graphic errors in the two textures that result in a
wrong placing of latitude and longitude on the image displayed.
To compensate these errors some correction factors that depend
on what texture you are using are added to latitude and longitude.*/

switch(sel_texture) {
case 1 : glRotated(signlat*gps_data.latitude + 3 , 1.0, 0.0, 0.0);
glRotated(signlon*gps_data.longitude, 0.0, 1.0, 0.0);
break;
case 2 : glRotated(signlat*gps_data.latitude + 7 , 1.0, 0.0, 0.0);
glRotated(signlon*gps_data.longitude + 15, 0.0, 1.0, 0.0);
}
}

//initializes the mesa memory and prepares the textures
void gl_init()
{
GLfloat h = (GLfloat) HEIGHT / (GLfloat) WIDTH;
int r, c;

//Create the OSMesa Context
ctx = OSMesaCreateContext(OSMESA_RGB_565, NULL);

//Set the memory pointed by rgb_565_buf to be the mesa buffer
OSMesaMakeCurrent(ctx, rgb_565_buf, GL_UNSIGNED_SHORT_5_6_5, WIDTH, HEIGHT);

glEnable(GL_DEPTH_TEST);
glDisable( GL_DITHER );

//Set up texturing
glEnable( GL_TEXTURE_2D );
glTexEnvi( GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_DECAL );
glHint( GL_PERSPECTIVE_CORRECTION_HINT, GL_FASTEST );

/*puts all data contained in the headers in the tex and tex2 structures
using the right order*/
for(r = 0; r < 256; r++)
for(c = 0; c < 256; c++)
{
tex[r][c][0] = textureR[256*(256-r)+c];
tex[r][c][1] = textureG[256*(256-r)+c];
tex[r][c][2] = textureB[256*(256-r)+c];
tex2[r][c][0] = texture2R[256*(256-r)+c];
tex2[r][c][1] = texture2G[256*(256-r)+c];
tex2[r][c][2] = texture2B[256*(256-r)+c];

}

glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST );
glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST );
glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_REPEAT );
glTexParameteri( GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_REPEAT );

//creates the list that define the sphere object
sphere = 1;
glNewList(sphere, GL_COMPILE);
glEnable(GL_TEXTURE_2D);
GLUquadricObj *quadObj = gluNewQuadric ();
gluQuadricTexture(quadObj, GL_TRUE);
gluQuadricDrawStyle(quadObj, GLU_FILL);
gluQuadricNormals(quadObj, GLU_SMOOTH);
glRotated(-90, 1.0, 0.0, 0.0);
gluSphere (quadObj, 3, 30, 30);
glEndList();

glEnable(GL_NORMALIZE);

//sets mesa(gl) viewport
glViewport(0, 0, (GLint) WIDTH, (GLint) HEIGHT);
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
//sets the 3d visual volume and the observer point
glFrustum(-1.0, 1.0, -h, h, 5.0, 60.0);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
//places the origin of the axes of the 3d view
glTranslated(0, -1, -40);

radius = 0;
twist = 0;

}

/*generates gps-like values to simulate the gps transmitter when it is not connected,
this function is called every time before drawing the image on mesa buffer
if demo mode is on*/
void demo_val()
{
int ns;
int signal_max[4] = {25, 30, 10, 70};
int azimuth_max[4] = {170, 10, 60, 100};
int elevation_max[4] = {10, 60, 30, 79};
ndemo += 1;

//generates speed
gps_data.speed = 25;

//generates next direction increasing or decreasing the last one using kmov
if(gps_data.dir_mov == 135)
kmov = -1;
if(gps_data.dir_mov == 45)
kmov = 1;

gps_data.dir_mov += kmov;

//generates next latitude and longitude data using direction and speed
demolat += cos(gps_data.dir_mov / (360/(2*M_PI)) ) * (gps_data.speed / 20);
demolon += sin(gps_data.dir_mov / (360/(2*M_PI)) ) * (gps_data.speed / 20);

lat = (int) demolat;
lon = (int) demolon;

/*demolat and demonlon count latitude and longitude from -infinite to
+infinite, but latitude is actually displayed from -90 to +90
(respectively nord or south) and longitude from -180 to +180 (respectively
east or west) and so you have to convert demolat and demolon data into real
latitude and longitude data before putting them in gps_data structure*/

if(lon < 0)
lon = 360 - (-lon) % 360;

if(lon % 360 > 180) {
gps_data.longitude = 180 - (lon % 360 - 180);
gps_data.dirlon = 'W';
}
else {
gps_data.longitude = lon % 360;
gps_data.dirlon = 'E';
}

if(lat < 0)
lat = 360 - (-lat) % 360;

if(lat % 360 >= 180) {
gps_data.dirlat = 'S';
if(lat % 360 > 270)
gps_data.latitude = 90 - (lat % 360 - 270);
else {
gps_data.latitude = lat % 360 - 180;

gps_data.longitude = 180 - gps_data.longitude;
if(gps_data.dirlon == 'E')
gps_data.dirlon = 'W';
else
gps_data.dirlon = 'E';
}
}

if(lat % 360 < 180) {
gps_data.dirlat = 'N';
if(lat % 360 > 90) {
gps_data.latitude = 90 - (lat % 360 - 90);

gps_data.longitude = 180 - gps_data.longitude;
if(gps_data.dirlon == 'E')
gps_data.dirlon = 'W';
else
gps_data.dirlon = 'E';
}
else
gps_data.latitude = lat % 360;
}

//generates altitude and time
gps_data.altitude = 79;
strcp(gps_data.time, "113024.00");

//generates the data of the satellites
gps_data.nsat = 4;

for(ns = 0; ns < 4; ns++) {
gps_data.sat_data[ns].id = ns + 20;
gps_data.sat_data[ns].azimuth = ndemo % azimuth_max[ns];
gps_data.sat_data[ns].elevation = ndemo % elevation_max[ns];
gps_data.sat_data[ns].signal_level = ndemo % signal_max[ns];
}

}

//this task draws the whole graphic on video memory and on mesa buffer
TASK draw(void)
{
char text[100];
TIME draw_TIME, gps_TIME;

while(1) {

task_testcancel();

//if demo mode is on it calls demo_val function to generate gps-like values
if(demo_mode == 1)
demo_val();

//gets time of execution of all the tasks
jet_gettable(draw_PID, &draw_TIME, 1);
jet_gettable(gps_PID, &gps_TIME, 1);

//displays text informations and graphics using standard functions (grx functions)

//displays the data of the execution times of the tasks
sprintf(text,"Hard Task Draw PER:%6d us EX:%6d us",(int)PERIOD_DRAW,(int)draw_TIME);

//if there is a risk of system crash warns displaying the text in red color
if(draw_TIME > PERIOD_DRAW * 0.65)
grx_text(text,10,65,rgb16(255,0,0),0);
else
grx_text(text,10,65,rgb16(100,100,100),0);

sprintf(text,"Hard Task Gps PER:%6d us EX:%6d us",(int)PERIOD_GPS,(int)gps_TIME);
grx_text(text,10,75,rgb16(100,100,100),0);

//displays data contained in gps_data structure
grx_rect(0, 0, 799, 89, rgb16(0, 0, 250));
grx_rect(1, 52, 380, 88, rgb16(100, 100, 100));

grx_text("Current position :",390,5,rgb16(0,255,255),0);
sprintf(text,"Latitude %f dir %c ", gps_data.latitude, gps_data.dirlat);
grx_text(text,390,25,rgb16(0,255,255),0);
sprintf(text,"Longitude %f dir %c ", gps_data.longitude, gps_data.dirlon);
grx_text(text,390,35,rgb16(0,255,255),0);
sprintf(text,"Altitude %f ", gps_data.altitude);
grx_text(text,390,45,rgb16(0,255,255),0);
sprintf(text,"Time %c%c:%c%c:%c%c ", gps_data.time[0], gps_data.time[1], gps_data.time[2], gps_data.time[3], gps_data.time[4], gps_data.time[5]);
grx_text(text,390,55,rgb16(0,255,255),0);
sprintf(text,"Speed %f ", gps_data.speed);
grx_text(text,390,65,rgb16(0,0,255),0);
sprintf(text,"Direction %f ", gps_data.dir_mov);
grx_text(text,390,75,rgb16(0,0,255),0);

//displays satellites data contained in the gps_data structure
grx_text("Satellites in view : ",600,5,rgb16(255,0,0),0);
sprintf(text,"Number of satellites %f ", gps_data.nsat);
grx_text(text,600, 25,rgb16(255,0,0),0);
grx_text("Signal level ", 600, 35,rgb16(255,0,0),0);

for(nlist = 0; nlist < 4; nlist++)
if( gps_data.sat_data[nlist].signal_level > 99)
gps_data.sat_data[nlist].signal_level = 99;

grx_box(600, 45, 700, 80, rgb16(0, 0, 0));
grx_box(600, 45, 600+gps_data.sat_data[0].signal_level, 50, rgb16(100, 0, 0));
grx_box(600, 55, 600+gps_data.sat_data[1].signal_level, 60, rgb16(150, 0, 0));
grx_box(600, 65, 600+gps_data.sat_data[2].signal_level, 70, rgb16(200, 0, 0));
grx_box(600, 75, 600+gps_data.sat_data[3].signal_level, 80, rgb16(250, 0, 0));

//displays program title and other informations
grx_text("G P S D E M O", 10, 5, rgb16(0, 255, 0), 0);
grx_text("Esc : end program...",170,5,rgb16(0,255,0),0);
grx_text("t : change texture" ,170,15,rgb16(0,255,0),0);
grx_text("d : demo/gps mode " ,170,25,rgb16(0,255,0),0);
grx_text("a z : zoom near/far " ,170,35,rgb16(0,255,0),0);
if(demo_mode == 1)
grx_text("Demo mode " ,10,25,rgb16(255,255,0),0);
else
grx_text("Gps mode " ,10,25,rgb16(0,255,0),0);

//displays mesa graphic
int nmax;
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

//selects and activates the correct texture
if(sel_texture == 1)
glTexImage2D( GL_TEXTURE_2D, 0, 3, 256, 256, 0, GL_RGB, GL_UNSIGNED_BYTE, tex );
else
glTexImage2D( GL_TEXTURE_2D, 0, 3, 256, 256, 0, GL_RGB, GL_UNSIGNED_BYTE, tex2 );

//draws the sphere calling the appropriate list defined in gl_init function
glPushMatrix();
polarView();
glCallList(sphere);
glPopMatrix();

//draws the arrow and the cross on the sphere
glPushMatrix();
glTranslated (0, 0, zoom);
glDisable(GL_TEXTURE_2D);
glColor3f (0, 1, 1);
glTranslated (0, 0, 6);
glRotated(180 + gps_data.dir_mov, 0.0, 0.0, 1.0);
glLineWidth (2.0);
glBegin(GL_LINES);
glVertex2f(-0.2, 0.2);
glVertex2f(0.2, -0.2);
glVertex2f(0.2, 0.2);
glVertex2f(-0.2, -0.2);
glColor3f (0, 0, 1);
glVertex2f(0, 0);
glVertex2f(0, gps_data.speed/30);
glVertex2f(0, gps_data.speed/30);
glVertex2f(0.2, gps_data.speed/30-0.2);
glVertex2f(0, gps_data.speed/30);
glVertex2f(-0.2, gps_data.speed/30-0.2);
glEnd();
glColor3f (0, 0, 0.3);
glTranslated (0.05, 0.05, 0);
glBegin(GL_LINES);
glVertex2f(-0.2, 0.2);
glVertex2f(0.2, -0.2);
glVertex2f(0.2, 0.2);
glVertex2f(-0.2, -0.2);
glVertex2f(0, 0);
glVertex2f(0, gps_data.speed/30);
glVertex2f(0, gps_data.speed/30);
glVertex2f(0.2, gps_data.speed/30-0.2);
glVertex2f(0, gps_data.speed/30);
glVertex2f(-0.2, gps_data.speed/30-0.2);
glEnd();
glPopMatrix();

//draws sattellites in view using azimuth and elevation data
if(gps_data.nsat < 4) //this function can draw four satellites at maximum
nmax = gps_data.nsat;
else
nmax = 4;

for(nlist = 0; nlist < nmax; nlist++) {
glPushMatrix();
glDisable(GL_TEXTURE_2D);
glColor3f (0.4+0.2*nlist, 0, 0);
glLineWidth (2.0);
glRotated(gps_data.sat_data[nlist].azimuth, 0.0, 0.0, 1.0);
glTranslated (0, 0, 3 + zoom);
glRotated(90 - gps_data.sat_data[nlist].elevation, 1.0, 0.0, 0.0);
glTranslated (0, 0, 4);
glBegin(GL_LINES);
glVertex2f(-0.1, -0.1);
glVertex2f(0.1, 0.1);
glVertex2f(-0.1, 0.1);
glVertex2f(0.1, -0.1);
glEnd();
glPopMatrix();
}

glFinish();

memcpy((video_buf+90*WIDTH*2), rgb_565_buf, RGB565MEM-90*WIDTH*2);

task_endcycle();

}

exit(1);
}

/*This task reads the data from the gps transmitter using the polling system.
Gps sends data with a rate of 4800 bit/s this means 1 byte every 1.6 ms.
If you want to be sure to get all the bytes transmitted by the gps you have
to choose a period <1.6 ms for this gps task, in this program the period is
1000ns or 1ms.*/

TASK gps(void)
{

while(1) {
task_testcancel();
task_endcycle();
}

exit(1);

}

//is called just before program ends
void program_key_end(KEY_EVT *k)
{
exit(1);
}

//is called when t key is pressed and changes the texture to be displayed on the sphere
void program_key_texture(KEY_EVT *k2)
{
if(sel_texture == 1)
sel_texture = 2;
else
sel_texture = 1;
}

//is called when d key is pressed and changes from demo to gps mode and vice versa
void program_key_demo(KEY_EVT *k3)
{
if(demo_mode == 0)
demo_mode = 1;
else
demo_mode = 0;
ndemo = 0;
kmov = -1;
lat = 0;
lon = 0;
demolat = 0;
demolon = 0;
gps_data.dirlat = 'N';
gps_data.dirlon = 'E';
gps_data.latitude = 0;
gps_data.longitude = 0;
gps_data.dir_mov = 90;
}

//is called when a key is pressed and increases the zoom factor
void program_key_near(KEY_EVT *k4)
{
zoom += 0.7;
}

//is called when a key is pressed and decreases the zoom factor
void program_key_far(KEY_EVT *k5)
{
zoom -= 0.7;
}

int main (int argc, char *argv[])
{

HARD_TASK_MODEL ht_draw, ht_gps;

WCET_DRAW =(int)((float) PERIOD_DRAW * (0.90));
WCET_GPS =(int)((float) PERIOD_GPS * (0.05));

//prepares draw task for activation
hard_task_default_model(ht_draw);
hard_task_def_mit(ht_draw,PERIOD_DRAW);
hard_task_def_wcet(ht_draw,WCET_DRAW);
hard_task_def_group(ht_draw,1);
hard_task_def_ctrl_jet(ht_draw);
hard_task_def_usemath(ht_draw);
hard_task_def_stack(ht_draw,35000); //VERY IMPORTANT FOR glCallList !!

draw_PID = task_create("draw", draw, &ht_draw, NULL);
if (draw_PID == -1) {
exit(1);
return 0;
}

//prepares gps task for activation
hard_task_default_model(ht_gps);
hard_task_def_wcet(ht_gps,WCET_GPS);
hard_task_def_mit(ht_gps,PERIOD_GPS);
hard_task_def_usemath(ht_gps);
hard_task_def_group(ht_gps,1);
hard_task_def_ctrl_jet(ht_gps);

gps_PID = task_create("gps", gps, &ht_gps, NULL);
if (gps_PID == -1) {
exit(1);
return 0;
}

//defines and activates all the keyboard events used in the program
{
KEY_EVT k;
k.scan = KEY_ESC;
k.ascii = 27;
k.status = KEY_PRESSED;
keyb_hook(k,program_key_end,FALSE);
}

{
KEY_EVT k2;
k2.scan = KEY_T;
k2.ascii = 't';
k2.status = KEY_PRESSED;
keyb_hook(k2,program_key_texture,FALSE);
}

{
KEY_EVT k3;
k3.scan = KEY_D;
k3.ascii = 'd';
k3.status = KEY_PRESSED;
keyb_hook(k3,program_key_demo,FALSE);
}

{
KEY_EVT k4;
k4.scan = KEY_A;
k4.ascii = 'a';
k4.status = KEY_PRESSED;
keyb_hook(k4,program_key_near,FALSE);
}

{
KEY_EVT k5;
k5.scan = KEY_Z;
k5.ascii = 'z';
k5.status = KEY_PRESSED;
keyb_hook(k5,program_key_far,FALSE);
}

//allocates memory for mesa functions (mesa buffer)
rgb_565_buf = malloc(RGB565MEM);

//puts in a pointer the address of video memory
video_buf = (unsigned char *)video_memory;

//initializes mesa functions and prepares textures
gl_init();

//clears mesa buffer
memset(rgb_565_buf, 0, RGB565MEM);

//clears screen (video memory)
grx_box(0, 0, 1023, 89, rgb16(0, 0, 0));

ndemo = 0;
kmov = -1;
lat = 0;
lon = 0;
demolat = 0;
demolon = 0;
gps_data.dirlat = 'N';
gps_data.dirlon = 'E';
gps_data.latitude = 0;
gps_data.longitude = 0;
gps_data.dir_mov = 90;

//resets the word variable
word[0] = '\0';

//activates the tasks previously created and prepared (draw, refresh, gps)
group_activate(1);

return 0;

}

Subversion Repositories shark

(root)/advdemos/trunk/gpsmesa/gpsmesa.c @ 1660 - Rev 1650