/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__" |