From c987783bc82749f91a8c9c695f50ab2fa8333925 Mon Sep 17 00:00:00 2001 From: zer0 Date: Tue, 11 May 2010 01:31:59 +0200 Subject: [PATCH] static beacon - offset --- projects/microb2010/mainboard/beacon.c | 17 +++++++++++++---- projects/microb2010/mainboard/beacon.h | 2 +- .../microb2010/mainboard/commands_mainboard.c | 7 +++---- 3 files changed, 17 insertions(+), 9 deletions(-) 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"; -- 2.39.5