From: zer0 <zer0@carbon.local>
Date: Mon, 10 May 2010 23:31:59 +0000 (+0200)
Subject: static beacon - offset
X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=commitdiff_plain;h=c987783bc82749f91a8c9c695f50ab2fa8333925

static beacon - offset
---

diff --git a/projects/microb2010/mainboard/beacon.c b/projects/microb2010/mainboard/beacon.c
index e560b6c..13cd4cd 100644
--- a/projects/microb2010/mainboard/beacon.c
+++ b/projects/microb2010/mainboard/beacon.c
@@ -78,10 +78,12 @@ static volatile int16_t pos_x = I2C_BEACON_NOT_FOUND;
 static volatile int16_t pos_y = I2C_BEACON_NOT_FOUND;
 static volatile int16_t pos_a = I2C_BEACON_NOT_FOUND;
 
-int8_t beacon_get_pos(int16_t *x, int16_t *y, double *a)
+#define BEACON_OFFSET (-50.)
+int8_t beacon_get_pos_double(double *x, double *y, double *a_rad)
 {
 	uint8_t flags;
 	int16_t tmpx, tmpy, tmpa;
+	double dtmpx, dtmpy, dtmpa;
 
 	IRQ_LOCK(flags);
 	tmpx = beaconboard.posx;
@@ -92,9 +94,16 @@ int8_t beacon_get_pos(int16_t *x, int16_t *y, double *a)
 	if (tmpx == I2C_BEACON_NOT_FOUND)
 		return -1;
 
-	*x = tmpx;
-	*y = tmpy;
-	*a = ((double)tmpa / 10.);
+	dtmpx = tmpx;
+	dtmpy = tmpy;
+	dtmpa = RAD((double)tmpa / 10.);
+
+	dtmpx += cos(dtmpa) * BEACON_OFFSET;
+	dtmpx += sin(dtmpa) * BEACON_OFFSET;
+
+	*x = dtmpx;
+	*y = dtmpy;
+	*a_rad = dtmpa;
 	return 0;
 }
 
diff --git a/projects/microb2010/mainboard/beacon.h b/projects/microb2010/mainboard/beacon.h
index 1d08efe..c5dd26a 100644
--- a/projects/microb2010/mainboard/beacon.h
+++ b/projects/microb2010/mainboard/beacon.h
@@ -20,5 +20,5 @@
  */
 
 void beacon_init(void);
-int8_t beacon_get_pos(int16_t *x, int16_t *y, double *a);
 void beacon_set_color(uint8_t color);
+int8_t beacon_get_pos_double(double *x, double *y, double *a_rad);
diff --git a/projects/microb2010/mainboard/commands_mainboard.c b/projects/microb2010/mainboard/commands_mainboard.c
index aae4b61..fcc47e6 100644
--- a/projects/microb2010/mainboard/commands_mainboard.c
+++ b/projects/microb2010/mainboard/commands_mainboard.c
@@ -296,13 +296,12 @@ struct cmd_beacon_result {
 /* function called when cmd_beacon is parsed successfully */
 static void cmd_beacon_parsed(void *parsed_result, void *data)
 {
-	int16_t x, y;
-	double a;
+	double x, y, a;
 
-	if (beacon_get_pos(&x, &y, &a) < 0)
+	if (beacon_get_pos_double(&x, &y, &a) < 0)
 		printf_P(PSTR("No position from beacon\r\n"));
 	else
-		printf_P(PSTR("x=%d y=%d a=%2.2f\r\n"), x, y, a);
+		printf_P(PSTR("x=%2.2f y=%2.2f a=%2.2f\r\n"), x, y, DEG(a));
 }
 
 prog_char str_beacon_arg0[] = "beacon";