X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Fbeacon.c;h=13cd4cd5537293d2fce2e5827458e6b7629e4129;hp=e560b6c7c833e138120906c991dcb0088b2eead9;hb=c987783bc82749f91a8c9c695f50ab2fa8333925;hpb=1aa73d20096ad6be733b2ddf1c890434bd82db75

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;
 }