git.droids-corp.org
/
aversive.git
/ blobdiff
commit
grep
author
committer
pickaxe
?
search:
re
summary
|
shortlog
|
log
|
commit
|
commitdiff
|
tree
raw
|
inline
| side by side
static beacon - offset
[aversive.git]
/
projects
/
microb2010
/
mainboard
/
beacon.c
diff --git
a/projects/microb2010/mainboard/beacon.c
b/projects/microb2010/mainboard/beacon.c
index
e560b6c
..
13cd4cd
100644
(file)
--- 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;
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;
{
uint8_t flags;
int16_t tmpx, tmpy, tmpa;
+ double dtmpx, dtmpy, dtmpa;
IRQ_LOCK(flags);
tmpx = beaconboard.posx;
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;
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;
}
return 0;
}