X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=src%2Fdrivers%2Fao_gps_skytraq.c;h=3b4a62ec1a521507da3f2b95e72fd4c8033599ae;hp=d789974d1c8bd3adf722307bdfe517c942d4792b;hb=HEAD;hpb=db6003d34595fbd103d5b131912b6a797254f1c5

diff --git a/src/drivers/ao_gps_skytraq.c b/src/drivers/ao_gps_skytraq.c
index d789974d..00c6932f 100644
--- a/src/drivers/ao_gps_skytraq.c
+++ b/src/drivers/ao_gps_skytraq.c
@@ -3,7 +3,8 @@
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; version 2 of the License.
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
  *
  * This program is distributed in the hope that it will be useful, but
  * WITHOUT ANY WARRANTY; without even the implied warranty of
@@ -32,20 +33,21 @@
 #define ao_gps_set_speed	ao_serial1_set_speed
 #endif
 
-__xdata uint8_t ao_gps_new;
-__xdata uint8_t ao_gps_mutex;
-static __data char ao_gps_char;
-static __data uint8_t ao_gps_cksum;
-static __data uint8_t ao_gps_error;
+uint8_t ao_gps_new;
+uint8_t ao_gps_mutex;
+static char ao_gps_char;
+static uint8_t ao_gps_cksum;
+static uint8_t ao_gps_error;
 
-__pdata uint16_t ao_gps_tick;
-__xdata struct ao_telemetry_location	ao_gps_data;
-__xdata struct ao_telemetry_satellite	ao_gps_tracking_data;
+AO_TICK_TYPE ao_gps_tick;
+AO_TICK_TYPE ao_gps_utc_tick;
+struct ao_telemetry_location	ao_gps_data;
+struct ao_telemetry_satellite	ao_gps_tracking_data;
 
-static __pdata uint16_t				ao_gps_next_tick;
-static __pdata struct ao_telemetry_location	ao_gps_next;
-static __pdata uint8_t				ao_gps_date_flags;
-static __pdata struct ao_telemetry_satellite	ao_gps_tracking_next;
+static AO_TICK_TYPE			ao_gps_next_tick;
+static struct ao_telemetry_location	ao_gps_next;
+static uint8_t				ao_gps_date_flags;
+static struct ao_telemetry_satellite	ao_gps_tracking_next;
 
 #define STQ_S 0xa0, 0xa1
 #define STQ_E 0x0d, 0x0a
@@ -59,7 +61,7 @@ static __pdata struct ao_telemetry_satellite	ao_gps_tracking_next;
     STQ_S, 0,15, id, a,b,c,d,e,f,g,h,i,j,k,l,m,n, \
     (id^a^b^c^d^e^f^g^h^i^j^k^l^m^n), STQ_E
 
-static __code uint8_t ao_gps_config[] = {
+static const uint8_t ao_gps_config[] = {
 	SKYTRAQ_MSG_8(0x08, 1, 0, 1, 0, 1, 0, 0, 0), /* configure nmea */
 	/* gga interval */
 	/* gsa interval */
@@ -87,7 +89,7 @@ ao_gps_lexchar(void)
 	ao_gps_char = c;
 }
 
-void
+static void
 ao_gps_skip_field(void)
 {
 	for (;;) {
@@ -98,7 +100,7 @@ ao_gps_skip_field(void)
 	}
 }
 
-void
+static void
 ao_gps_skip_sep(void)
 {
 	char c = ao_gps_char;
@@ -106,7 +108,7 @@ ao_gps_skip_sep(void)
 		ao_gps_lexchar();
 }
 
-__data static uint8_t ao_gps_num_width;
+static uint8_t ao_gps_num_width;
 
 static int16_t
 ao_gps_decimal(uint8_t max_width)
@@ -125,7 +127,7 @@ ao_gps_decimal(uint8_t max_width)
 		uint8_t c = ao_gps_char;
 		if (c < (uint8_t) '0' || (uint8_t) '9' < c)
 			break;
-		v = v * 10 + (uint8_t) (c - (uint8_t) '0');
+		v = (int16_t) (v * 10 + (uint8_t) (c - (uint8_t) '0'));
 		ao_gps_num_width++;
 		ao_gps_lexchar();
 	}
@@ -144,16 +146,15 @@ ao_gps_hex(void)
 	ao_gps_num_width = 0;
 	while (ao_gps_num_width < 2) {
 		uint8_t c = ao_gps_char;
-		uint8_t	d;
 		if ((uint8_t) '0' <= c && c <= (uint8_t) '9')
-			d = - '0';
+			c -= '0';
 		else if ((uint8_t) 'A' <= c && c <= (uint8_t) 'F')
-			d = - 'A' + 10;
+			c -= 'A' - 10;
 		else if ((uint8_t) 'a' <= c && c <= (uint8_t) 'f')
-			d = - 'a' + 10;
+			c -= 'a' - 10;
 		else
 			break;
-		v = (v << 4) | (c + d);
+		v = (uint8_t) ((v << 4) | c);
 		ao_gps_num_width++;
 		ao_gps_lexchar();
 	}
@@ -161,11 +162,11 @@ ao_gps_hex(void)
 }
 
 static int32_t
-ao_gps_parse_pos(uint8_t deg_width) __reentrant
+ao_gps_parse_pos(uint8_t deg_width) 
 {
-	static __pdata uint16_t	d;
-	static __pdata uint8_t	m;
-	static __pdata uint16_t	f;
+	int16_t		d;
+	int16_t		m;
+	int16_t		f;
 	char c;
 
 	d = ao_gps_decimal(deg_width);
@@ -255,13 +256,10 @@ ao_nmea_gga(void)
 
 	ao_gps_next_tick = ao_time();
 	ao_gps_next.flags = AO_GPS_RUNNING | ao_gps_date_flags;
-	ao_gps_next.hour = ao_gps_decimal(2);
-	ao_gps_next.minute = ao_gps_decimal(2);
-	ao_gps_next.second = ao_gps_decimal(2);
+	ao_gps_next.hour = (uint8_t) ao_gps_decimal(2);
+	ao_gps_next.minute = (uint8_t) ao_gps_decimal(2);
+	ao_gps_next.second = (uint8_t) ao_gps_decimal(2);
 	ao_gps_skip_field();	/* skip seconds fraction */
-#if HAS_FLIGHT || HAS_TRACKER
-				ao_gps_data.state = ao_flight_state;
-#endif
 
 	ao_gps_next.latitude = ao_gps_parse_pos(2);
 	if (ao_gps_parse_flag('N', 'S'))
@@ -270,27 +268,28 @@ ao_nmea_gga(void)
 	if (ao_gps_parse_flag('E', 'W'))
 		ao_gps_next.longitude = -ao_gps_next.longitude;
 
-	i = ao_gps_decimal(0xff);
+	i = (uint8_t) ao_gps_decimal(0xff);
 	if (i == 1)
 		ao_gps_next.flags |= AO_GPS_VALID;
 
-	i = ao_gps_decimal(0xff) << AO_GPS_NUM_SAT_SHIFT;
+	i = (uint8_t) (ao_gps_decimal(0xff) << AO_GPS_NUM_SAT_SHIFT);
 	if (i > AO_GPS_NUM_SAT_MASK)
 		i = AO_GPS_NUM_SAT_MASK;
 	ao_gps_next.flags |= i;
 
 	ao_gps_lexchar();
-	i = ao_gps_decimal(0xff);
-	if (i <= 50) {
-		i = (uint8_t) 5 * i;
+	i = (uint8_t) ao_gps_decimal(0xff);
+	if (i <= 25) {
+		i = (uint8_t) 10 * i;
 		if (ao_gps_char == '.')
-			i = (i + ((uint8_t) ao_gps_decimal(1) >> 1));
+			i = (i + ((uint8_t) ao_gps_decimal(1)));
 	} else
 		i = 255;
 	ao_gps_next.hdop = i;
 	ao_gps_skip_field();
 
-	ao_gps_next.altitude = ao_gps_decimal(0xff);
+	AO_TELEMETRY_LOCATION_SET_ALTITUDE(&ao_gps_next, ao_gps_decimal(0xff));
+
 	ao_gps_skip_field();	/* skip any fractional portion */
 
 	ao_nmea_finish();
@@ -298,8 +297,8 @@ ao_nmea_gga(void)
 	if (!ao_gps_error) {
 		ao_mutex_get(&ao_gps_mutex);
 		ao_gps_new |= AO_GPS_NEW_DATA;
-		ao_gps_tick = ao_gps_next_tick;
-		ao_xmemcpy(&ao_gps_data, PDATA_TO_XDATA(&ao_gps_next), sizeof (ao_gps_data));
+		ao_gps_tick = ao_gps_utc_tick = ao_gps_next_tick;
+		memcpy(&ao_gps_data, &ao_gps_next, sizeof (ao_gps_data));
 		ao_mutex_put(&ao_gps_mutex);
 		ao_wakeup(&ao_gps_new);
 	}
@@ -308,7 +307,7 @@ ao_nmea_gga(void)
 static void
 ao_nmea_gsv(void)
 {
-	char	c;
+	uint8_t	c;
 	uint8_t	i;
 	uint8_t	done;
 	/* Now read the data into the GPS tracking data record
@@ -327,8 +326,8 @@ ao_nmea_gsv(void)
 	 *	...		other SVIDs
 	 *	72		checksum
 	 */
-	c = ao_gps_decimal(1);	/* total messages */
-	i = ao_gps_decimal(1);	/* message sequence */
+	c = (uint8_t) ao_gps_decimal(1);	/* total messages */
+	i = (uint8_t) ao_gps_decimal(1);	/* message sequence */
 	if (i == 1) {
 		ao_gps_tracking_next.channels = 0;
 	}
@@ -337,14 +336,14 @@ ao_nmea_gsv(void)
 	ao_gps_skip_field();	/* sats in view */
 	while (ao_gps_char != '*' && ao_gps_char != '\n' && ao_gps_char != '\r') {
 		i = ao_gps_tracking_next.channels;
-		c = ao_gps_decimal(2);	/* SVID */
+		c = (uint8_t) ao_gps_decimal(2);	/* SVID */
 		if (i < AO_MAX_GPS_TRACKING)
 			ao_gps_tracking_next.sats[i].svid = c;
 		ao_gps_lexchar();
 		ao_gps_skip_field();	/* elevation */
 		ao_gps_lexchar();
 		ao_gps_skip_field();	/* azimuth */
-		c = ao_gps_decimal(2);	/* C/N0 */
+		c = (uint8_t) ao_gps_decimal(2);	/* C/N0 */
 		if (i < AO_MAX_GPS_TRACKING) {
 			if ((ao_gps_tracking_next.sats[i].c_n_1 = c) != 0)
 				ao_gps_tracking_next.channels = i + 1;
@@ -358,7 +357,7 @@ ao_nmea_gsv(void)
 	else if (done) {
 		ao_mutex_get(&ao_gps_mutex);
 		ao_gps_new |= AO_GPS_NEW_TRACKING;
-		ao_xmemcpy(&ao_gps_tracking_data, PDATA_TO_XDATA(&ao_gps_tracking_next), sizeof(ao_gps_tracking_data));
+		memcpy(&ao_gps_tracking_data, &ao_gps_tracking_next, sizeof(ao_gps_tracking_data));
 		ao_mutex_put(&ao_gps_mutex);
 		ao_wakeup(&ao_gps_new);
 	}
@@ -367,7 +366,7 @@ ao_nmea_gsv(void)
 static void
 ao_nmea_rmc(void)
 {
-	char	a, c;
+	uint8_t	a, c;
 	uint8_t	i;
 	/* Parse the RMC record to read out the current date */
 
@@ -398,9 +397,9 @@ ao_nmea_rmc(void)
 		ao_gps_lexchar();
 		ao_gps_skip_field();
 	}
-	a = ao_gps_decimal(2);
-	c = ao_gps_decimal(2);
-	i = ao_gps_decimal(2);
+	a = (uint8_t) ao_gps_decimal(2);
+	c = (uint8_t) ao_gps_decimal(2);
+	i = (uint8_t) ao_gps_decimal(2);
 
 	ao_nmea_finish();
 
@@ -415,7 +414,7 @@ ao_nmea_rmc(void)
 #define ao_skytraq_sendstruct(s) ao_skytraq_sendbytes((s), sizeof(s))
 
 static void
-ao_skytraq_sendbytes(__code uint8_t *b, uint8_t l)
+ao_skytraq_sendbytes(const uint8_t *b, uint8_t l)
 {
 	while (l--) {
 		uint8_t	c = *b++;
@@ -463,7 +462,7 @@ ao_gps_nmea_parse(void)
 static uint8_t	ao_gps_updating;
 
 void
-ao_gps(void) __reentrant
+ao_gps(void) 
 {
 	ao_gps_set_speed(AO_SERIAL_SPEED_9600);
 
@@ -487,9 +486,9 @@ ao_gps(void) __reentrant
 	}
 }
 
-__xdata struct ao_task ao_gps_task;
+struct ao_task ao_gps_task;
 
-static __code uint8_t ao_gps_115200[] = {
+static const uint8_t ao_gps_115200[] = {
 	SKYTRAQ_MSG_3(5,0,5,0)	/* Set to 115200 baud */
 };
 
@@ -501,7 +500,7 @@ ao_gps_set_speed_delay(uint8_t speed) {
 }
 
 static void
-gps_update(void) __reentrant
+gps_update(void) 
 {
 	ao_gps_updating = 1;
 	ao_task_minimize_latency = 1;
@@ -518,7 +517,7 @@ gps_update(void) __reentrant
 		ao_gps_putchar(ao_usb_getchar());
 }
 
-__code struct ao_cmds ao_gps_cmds[] = {
+const struct ao_cmds ao_gps_cmds[] = {
 	{ ao_gps_show, 	"g\0Display GPS" },
 	{ gps_update,	"U\0Update GPS firmware" },
 	{ 0, NULL },