static beacon - offset

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

diff --git a/projects/microb2010/mainboard/beacon.h b/projects/microb2010/mainboard/beacon.h
index 1d08efe..c5dd26a 100644 (file)
--- a/projects/microb2010/mainboard/beacon.h
+++ b/projects/microb2010/mainboard/beacon.h
@@ -20,5 +20,5 @@
  */
 
 void beacon_init(void);
  */
 
 void beacon_init(void);

-int8_t beacon_get_pos(int16_t *x, int16_t *y, double *a);

 void beacon_set_color(uint8_t color);
 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 (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)
 {
 /* 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("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";
 }
 
 prog_char str_beacon_arg0[] = "beacon";