Subversion Repositories shark

Compare Revisions

Ignore whitespace Rev 1420 → Rev 1421

/demos/trunk/gps/initfile.c
0,0 → 1,167
/*
* Project: S.Ha.R.K
*
* Coordinators: Giorgio Buttazzo <giorgio@sssup.it>
*
* ReTiS Lab (Scuola Superiore S.Anna - Pisa - Italy)
*
* http://www.sssup.it
* http://retis.sssup.it
* http://hartik.sssup.it
*/
 
#include "kernel/kern.h"
#include "modules/edf.h"
#include "modules/hardcbs.h"
#include "modules/rr.h"
#include "modules/dummy.h"
#include "modules/intdrive.h"
 
#include "modules/sem.h"
#include "modules/hartport.h"
 
#include <drivers/shark_linuxc26.h>
#include <drivers/shark_pci26.h>
#include <drivers/shark_input26.h>
#include <drivers/shark_keyb26.h>
#include <drivers/shark_fb26.h>
 
#define FRAME_BUFFER_DEVICE 0
 
/*+ sysyem tick in us +*/
#define TICK 0
 
/*+ RR tick in us +*/
#define RRTICK 10000
 
/*+ Interrup Server */
#define INTDRIVE_Q 1000
#define INTDRIVE_T 10000
#define INTDRIVE_FLAG 0
 
void call_shutdown_task(void *arg);
int device_drivers_init();
int device_drivers_close();
void set_shutdown_task();
TASK shutdown_task_body(void *arg);
 
PID shutdown_task_PID = -1;
 
TIME __kernel_register_levels__(void *arg)
{
struct multiboot_info *mb = (struct multiboot_info *)arg;
 
INTDRIVE_register_level(INTDRIVE_Q,INTDRIVE_T,INTDRIVE_FLAG);
EDF_register_level(EDF_ENABLE_ALL);
HCBS_register_level(HCBS_ENABLE_ALL, 1);
RR_register_level(RRTICK, RR_MAIN_YES, mb);
dummy_register_level();
 
SEM_register_module();
 
return TICK;
}
 
TASK __init__(void *arg)
{
struct multiboot_info *mb = (struct multiboot_info *)arg;
 
HARTPORT_init();
 
set_shutdown_task();
 
device_drivers_init();
 
sys_atrunlevel(call_shutdown_task, NULL, RUNLEVEL_SHUTDOWN);
 
__call_main__(mb);
 
return (void *)0;
}
 
void set_shutdown_task() {
 
NRT_TASK_MODEL nrt;
 
nrt_task_default_model(nrt);
nrt_task_def_system(nrt);
 
shutdown_task_PID = task_create("Shutdown Task",shutdown_task_body,&nrt,NULL);
if (shutdown_task_PID == NIL) {
sys_shutdown_message("Error: Cannot create shutdown task\n");
sys_end();
}
 
}
 
int device_drivers_init() {
 
int res;
KEYB_PARMS kparms = BASE_KEYB;
LINUXC26_register_module();
 
PCI26_init();
 
INPUT26_init();
 
KEYB26_init(&kparms);
 
/*
FB26_init();
res = FB26_open(FRAME_BUFFER_DEVICE);
if (res) {
cprintf("Error: Cannot open graphical mode\n");
KEYB26_close();
INPUT26_close();
sys_end();
}
FB26_use_grx(FRAME_BUFFER_DEVICE);
FB26_setmode(FRAME_BUFFER_DEVICE,"640x480-16");
*/
return 0;
 
}
 
int device_drivers_close() {
//FB26_close(FRAME_BUFFER_DEVICE);
KEYB26_close();
INPUT26_close();
return 0;
}
 
#define SHUTDOWN_TIMEOUT_SEC 3
 
void call_shutdown_task(void *arg)
{
struct timespec t;
 
sys_gettime(&t);
t.tv_sec += SHUTDOWN_TIMEOUT_SEC;
 
/* Emergency timeout to exit from RUNLEVEL_SHUTDOWN */
kern_event_post(&t,(void *)((void *)sys_abort_shutdown),(void *)0);
 
task_activate(shutdown_task_PID);
}
 
TASK shutdown_task_body(void *arg) {
 
device_drivers_close();
 
sys_shutdown_message("-- S.Ha.R.K. Closed --\n");
 
sys_abort_shutdown(0);
 
return NULL;
 
}
/demos/trunk/gps/nmea.h
0,0 → 1,97
#define GPVTG "GPVTG"
#define GPGGA "GPGGA"
#define GPGSA "GPGSA"
#define GPGSV "GPGSV"
#define GPRMC "GPRMC"
#define PRWIZCH "PRWIZCH"
 
struct OUTDATA {
int fdin;
int fdout;
 
int last_update; /* When we got last data from GPS receiver */
 
long cmask;
char utc[20]; /* UTC date / time in format "mm/dd/yy hh:mm:ss" */
 
double latitude; /* Latitude and longitude in format "d.ddddd" */
 
double longitude;
 
char grid[7];
 
double altitude; /* Altitude in meters */
 
double speed; /* Speed over ground, knots */
 
double track; /* Track made good, degress True */
 
int satellites; /* Number of satellites used in solution */
 
int status; /* 0 = no fix, 1 = fix, 2 = dgps fix */
 
int mode; /* 1 = no fix, 2 = 2D, 3 = 3D */
 
double pdop; /* Position dilution of precision */
 
double hdop; /* Horizontal dilution of precision */
 
double vdop; /* Vertical dilution of precision */
 
int in_view; /* # of satellites in view */
 
int PRN[12]; /* PRN of satellite */
 
int elevation[12]; /* elevation of satellite */
 
int azimuth[12]; /* azimuth */
 
int ss[12]; /* signal strength */
 
int used[12]; /* used in solution */
 
int ZCHseen; /* flag */
 
int Zs[12]; /* for the rockwell PRWIZCH */
 
int Zv[12]; /* value */
 
int year;
 
int month;
 
int day;
 
int hours;
 
int minutes;
 
int seconds;
 
double separation;
 
double mag_var;
 
double course;
 
int seen[12];
 
int valid[12]; /* signal valid */
};
 
#define C_LATLON 1
#define C_SAT 2
#define C_ZCH 4
 
/* prototypes */
extern void doNMEA(short refNum);
extern void processGPVTG(char *sentence);
extern void processGPRMC(char *sentence);
extern void processGPGGA(char *sentence);
extern void processGPGSV(char *sentence);
extern void processPRWIZCH(char *sentence);
extern void processGPGSA(char *sentence);
extern void process_message(char *sentence);
extern void add_checksum(char *sentence);
extern short checksum(char *sentence);
extern struct OUTDATA gNMEAdata;
/demos/trunk/gps/gps.c
0,0 → 1,155
/*
* Project: S.Ha.R.K.
*
* Coordinators:
* Giorgio Buttazzo <giorgio@sssup.it>
* Paolo Gai <pj@gandalf.sssup.it>
*
* Authors :
* Giacomo Guidi <giacomo@gandalf.sssup.it>
* (see the web pages for full authors list)
*
* ReTiS Lab (Scuola Superiore S.Anna - Pisa - Italy)
*
* http://www.sssup.it
* http://retis.sssup.it
* http://shark.sssup.it
*/
 
#include <kernel/kern.h>
#include <stdlib.h>
#include <math.h>
 
#include "nmea.h"
 
#include <drivers/shark_keyb26.h>
#include <drivers/shark_fb26.h>
 
/* Number of available COM links */
#define COM_LINKS 4
#define COM1 0
#define COM2 1
#define COM3 2
#define COM4 3
/* These values identify interrupt type */
#define RX_FULL 1
#define TX_EMPTY 2
#define LS_CHANGED 4
#define MS_CHANGED 8
 
/* Register displacements */
#define THR 0
#define RBR 0
#define IER 1
#define FCR 2
#define IIR 2
#define LCR 3
#define MCR 4
#define LSR 5
#define MSR 6
#define SPad 7
/* Parity value */
#define NONE 0
#define ODD 1
#define EVEN 3
 
static unsigned int com_base[] = {0x03F8,0x02F8,0x03E8,0x02E8};
 
struct OUTDATA gNMEAdata;
 
unsigned int com_read(unsigned int port,unsigned int reg)
{
unsigned int b;
if (port > 3 || reg > 7) return(0);
b = ll_in(com_base[port]+reg);
return(b);
}
 
void com_write(unsigned int port,unsigned int reg,unsigned int value)
{
if (port > 3 || reg > 7) return;
ll_out(com_base[port]+reg,value);
}
 
void com_send(unsigned int port,BYTE b)
{
while ((com_read(port,LSR) & 32) == 0);
com_write(port,THR,b);
}
unsigned int com_receive(unsigned int port)
{
while ((com_read(port,LSR) & 1) == 0);
return(com_read(port,RBR));
}
 
int open_com(unsigned int port, DWORD speed, BYTE parity, BYTE len, BYTE stop)
{
unsigned long div,b_mask;
 
/* Now set up the serial link */
b_mask = (parity & 3) * 8 + (stop & 1) * 4 + ((len - 5) & 3);
div = 115200L / speed;
/* Clear serial interrupt enable register */
com_write(port,IER,0);
/* Empty input buffer */
com_read(port,RBR);
/* Activate DLAB bit for speed setting */
com_write(port,LCR,0x80);
/* Load baud divisor */
com_write(port,0,div & 0x00FF);
div >>= 8;
com_write(port,1,div & 0x00FF);
/* Load control word (parity,stop bit,bit len) */
com_write(port,LCR,b_mask);
/* Attiva OUT1 & OUT2 */
com_write(port,MCR,0x0C);
 
return 0;
 
}
 
 
int main(int argc, char **argv)
{
 
unsigned int ch,i;
char temp[100];
 
open_com(COM1,4800,NONE,8,0);
i = 0;
 
while(1) {
 
ch = com_receive(COM1);
temp[i] = ch;
if (ch == 13) {
temp[i] = 0;
cprintf("%s\n",temp);
process_message(temp + 1);
i = 0;
 
cprintf("Lat: %f Lon: %f Alt: %f Sat: %d View: %d Mod: %d Time: %s\n",
gNMEAdata.latitude,
gNMEAdata.longitude,
gNMEAdata.altitude,
gNMEAdata.satellites,
gNMEAdata.in_view,
gNMEAdata.mode,
gNMEAdata.utc);
ch = com_receive(COM1);
 
} else {
i++;
}
 
}
 
return 0;
 
}
 
/demos/trunk/gps/nmea_parse.c
0,0 → 1,279
#include <kernel/kern.h>
#include <math.h>
#include <stdio.h>
#include <string.h>
#include "nmea.h"
 
extern struct OUTDATA gNMEAdata;
static void do_lat_lon(char *sentence, int begin);
static void do_grid(void);
static char *field(char *sentence, short n);
static void update_field_i(char *sentence, int fld, int *dest, int mask);
 
void processGPRMC(char *sentence)
{
char s[20], d[10];
int tmp;
 
sscanf(field(sentence, 9), "%s", d); /* Date: ddmmyy */
 
strncpy(s, d + 2, 2); /* copy month */
 
strncpy(s + 3, d, 2); /* copy date */
 
sscanf((d+4), "%2d", &tmp);
 
/* Tf.: Window the year from 1970 to 2069. This buys us some time. */
if (tmp < 70)
strncpy(s + 6, "20", 2); /* 21th century */
else
strncpy(s + 6, "19", 2); /* 20th century */
 
strncpy(s + 8, d + 4, 2); /* copy year */
 
sscanf(field(sentence, 1), "%s", d); /* Time: hhmmss */
 
strncpy(s + 11, d, 2); /* copy hours */
 
strncpy(s + 14, d + 2, 2); /* copy minutes */
 
strncpy(s + 17, d + 4, 2); /* copy seconds */
 
s[2] = s[5] = '/'; /* add the '/'s, ':'s, ' ' and string terminator */
 
s[10] = ' ';
s[13] = s[16] = ':';
s[19] = '\0';
 
strcpy(gNMEAdata.utc, s);
 
/* A = valid, V = invalid */
if (strcmp(field(sentence, 2), "V") == 0)
gNMEAdata.status = 0;
#if 0 /* Let the GGA sentence do the update so we catch diff fixes */
else
gNMEAdata.status = 0;
#endif
 
sscanf(field(sentence, 7), "%lf", &gNMEAdata.speed);
sscanf(field(sentence, 8), "%lf", &gNMEAdata.track);
 
do_lat_lon(sentence, 3);
do_grid();
 
}
 
/* ----------------------------------------------------------------------- */
 
void processGPVTG(char *sentence)
{
sscanf(field(sentence, 3), "%lf", &gNMEAdata.speed);
sscanf(field(sentence, 1), "%lf", &gNMEAdata.track);
}
 
/* ----------------------------------------------------------------------- */
 
void processGPGGA(char *sentence)
{
do_lat_lon(sentence, 2);
do_grid();
/* 0 = none, 1 = normal, 2 = diff */
sscanf(field(sentence, 6), "%d", &gNMEAdata.status);
if ((gNMEAdata.status > 0) && (gNMEAdata.mode < 2))
gNMEAdata.mode = 2;
if ((gNMEAdata.status < 1) && (gNMEAdata.mode > 1))
gNMEAdata.mode = 1;
sscanf(field(sentence, 7), "%d", &gNMEAdata.satellites);
sscanf(field(sentence, 9), "%lf", &gNMEAdata.altitude);
}
 
/* ----------------------------------------------------------------------- */
 
void processGPGSA(char *sentence)
{
 
/* 1 = none, 2 = 2d, 3 = 3d */
sscanf(field(sentence, 2), "%d", &gNMEAdata.mode);
if ((gNMEAdata.mode > 1) && (gNMEAdata.status < 1))
gNMEAdata.status = 1;
if ((gNMEAdata.mode < 2) && (gNMEAdata.status > 0))
gNMEAdata.status = 0;
sscanf(field(sentence, 15), "%lf", &gNMEAdata.pdop);
sscanf(field(sentence, 16), "%lf", &gNMEAdata.hdop);
sscanf(field(sentence, 17), "%lf", &gNMEAdata.vdop);
}
 
/* ----------------------------------------------------------------------- */
 
void processGPGSV(char *sentence)
{
int n, m, f = 4;
 
 
if (sscanf(field(sentence, 2), "%d", &n) < 1)
return;
update_field_i(sentence, 3, &gNMEAdata.in_view, C_SAT);
 
n = (n - 1) * 4;
m = n + 4;
 
while (n < gNMEAdata.in_view && n < m) {
update_field_i(sentence, f++, &gNMEAdata.PRN[n], C_SAT);
update_field_i(sentence, f++, &gNMEAdata.elevation[n], C_SAT);
update_field_i(sentence, f++, &gNMEAdata.azimuth[n], C_SAT);
if (*(field(sentence, f)))
update_field_i(sentence, f, &gNMEAdata.ss[n], C_SAT);
f++;
n++;
}
}
 
/* ----------------------------------------------------------------------- */
 
void processPRWIZCH(char *sentence)
{
int i;
 
for (i = 0; i < 12; i++) {
update_field_i(sentence, 2 * i + 1, &gNMEAdata.Zs[i], C_ZCH);
update_field_i(sentence, 2 * i + 2, &gNMEAdata.Zv[i], C_ZCH);
}
gNMEAdata.ZCHseen = 1;
}
 
/* ----------------------------------------------------------------------- */
 
static void do_lat_lon(char *sentence, int begin)
{
double lat, lon, d, m;
char str[20], *p;
int updated = 0;
 
 
if (*(p = field(sentence, begin + 0)) != '\0') {
strncpy(str, p, 20);
sscanf(p, "%lf", &lat);
m = 100.0 * modf(lat / 100.0, &d);
lat = d + m / 60.0;
p = field(sentence, begin + 1);
if (*p == 'S')
lat = -lat;
if (gNMEAdata.latitude != lat) {
gNMEAdata.latitude = lat;
gNMEAdata.cmask |= C_LATLON;
}
updated++;
}
if (*(p = field(sentence, begin + 2)) != '\0') {
strncpy(str, p, 20);
sscanf(p, "%lf", &lon);
m = 100.0 * modf(lon / 100.0, &d);
lon = d + m / 60.0;
 
p = field(sentence, begin + 3);
if (*p == 'W')
lon = -lon;
if (gNMEAdata.longitude != lon) {
gNMEAdata.longitude = lon;
gNMEAdata.cmask |= C_LATLON;
}
updated++;
}
if (updated == 2)
gNMEAdata.last_update = sys_gettime(NULL);
}
 
/* ----------------------------------------------------------------------- */
 
static void do_grid() {
double lat, lon;
 
lat = gNMEAdata.latitude;
lon = gNMEAdata.longitude;
 
gNMEAdata.grid[0] = 65 + (int)((180.0 + lon) / 20.0);
gNMEAdata.grid[1] = 65 + (int)((90.0 + lat) / 10.0);
gNMEAdata.grid[2] = 48 + (int)((180.0 + lon) / 2) % 10;
gNMEAdata.grid[3] = 48 + (int)(90.0 + lat) % 10;
gNMEAdata.grid[4] = 97 + ((int)(180.0 + lon) % 2) * 12 + (int)(((180.0 + lon) - (int)(180.0 + lon)) * 12);
gNMEAdata.grid[5] = 97 + (int)(((90.0 + lat) - (int)(90.0 + lat)) * 24);
gNMEAdata.grid[6] = '\0';
}
 
/* ----------------------------------------------------------------------- */
 
static void update_field_i(char *sentence, int fld, int *dest, int mask)
{
int tmp;
 
sscanf(field(sentence, fld), "%d", &tmp);
 
if (tmp != *dest) {
*dest = tmp;
gNMEAdata.cmask |= mask;
}
}
 
void process_message(char *sentence)
{
if (checksum(sentence)) {
if (strncmp(GPRMC, sentence, 5) == 0) {
processGPRMC(sentence);
} else if (strncmp(GPGGA, sentence, 5) == 0) {
processGPGGA(sentence);
} else if (strncmp(GPVTG, sentence, 5) == 0) {
processGPVTG(sentence);
} else if (strncmp(GPGSA, sentence, 5) == 0) {
processGPGSA(sentence);
} else if (strncmp(GPGSV, sentence, 5) == 0) {
processGPGSV(sentence);
} else if (strncmp(PRWIZCH, sentence, 7) == 0) {
processPRWIZCH(sentence);
} else {
cprintf("Unknown sentence: \"%s\"\n",sentence);
}
} else {
cprintf("Bad Checksum !\n");
}
}
 
/* ----------------------------------------------------------------------- */
 
short checksum(char *sentence)
{
unsigned char sum = '\0';
char c, *p = sentence, csum[3];
 
while ((c = *p++) != '*' && c != '\0')
sum ^= c;
 
sprintf(csum, "%02x", sum);
return (strncmp(csum, p, 2) == 0);
}
 
void add_checksum(char *sentence)
{
unsigned char sum = '\0';
char c, *p = sentence;
 
while ((c = *p++) != '*')
sum ^= c;
 
sprintf(p, "%02x\r\n", sum);
}
 
static char *field(char *sentence, short n)
{
static char result[100];
char *p = sentence;
 
while (n-- > 0)
while (*p++ != ',');
strcpy(result, p);
p = result;
while (*p && *p != ',' && *p != '*' && *p != '\r')
p++;
 
*p = '\0';
return result;
}
/demos/trunk/gps/makefile
0,0 → 1,16
#
#
#
 
ifndef BASE
BASE=../..
endif
include $(BASE)/config/config.mk
 
PROGS= gps
 
include $(BASE)/config/example.mk
 
gps:
make -f $(SUBMAKE) APP=gps INIT= OTHEROBJS="initfile.o nmea_parse.o" SHARKOPT="__LINUXC26__ __PCI__ __INPUT__ __FB__"