static beacon - offset

diff --git a/projects/microb2010/mainboard/beacon.c b/projects/microb2010/mainboard/beacon.c
index e560b6c7c833e138120906c991dcb0088b2eead9..13cd4cd5537293d2fce2e5827458e6b7629e4129 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;
 
-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 1d08efe87de20ddaee618c4ffee3a3938ee31c31..c5dd26ad1ec6f95d6b5ab35bbb44f448186cd4cf 100644 (file)
--- 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 aae4b61921cefc68be3998a5c4a7b8916a1aeb78..fcc47e6fe2b09e5a34b1cd6a203a69a749084763 100644 (file)
--- 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";