This module provides useful basic facilities to use the KoreBot with the Stargazer sensor. More...
Go to the source code of this file.
Defines | |
#define | PORT_NAME "/dev/ttyS2" |
#define | BATTERY_GPIO 16 |
#define | CMD_CALC_STOP "~#CalcStop`" |
#define | CMD_CALC_STOP_ACK "~!CalcStop`" |
#define | CMD_CALC_START "~#CalcStart`" |
#define | CMD_CALC_START_ACK "~!CalcStart`" |
#define | CMD_VERSION "~@Version`" |
#define | CMD_VERSION_ACK "~$Version|" |
#define | CMD_DEADZONE_ACK "DeadZone" |
#define | CMD_MAPID_ACK "MAPID" |
#define | CMD_LANDMARK_NB "IDNum" |
#define | CMD_REFERENCE_ID "RefID" |
#define | CMD_HEIGHT_FIX "HeightFix" |
#define | CMD_MARK_TYPE "MarkType" |
#define | CMD_MARK_MODE "MarkMode" |
#define | CMD_SET_END "SetEnd" |
#define | CMD_PARAM_UPDATE "~!ParameterUpdate`" |
#define | CMD_MAP_MODE_START "~#MapMode|Start`" |
#define | ACK_CHAR '!' |
#define | MAX_STOP 10 |
#define | MAX_BUF 2048 |
#define | BOOKSTEIN 0 |
#define | TAUBIN 1 |
#define | FPF 2 |
#define | CONFIGURE_TIMEOUT 60 |
#define | STOP_THRESHOLD_ANGLE 5 |
#define | CALIBRATION_SPEED 4500 |
#define | MAX_CALIB_DATA 2048 |
Functions | |
void | write_serial (int fd, char *text) |
int | openserial (int *fd, const char *name) |
void | rotate (double *A, int i, int j, int k, int l, double tau, double s, int n) |
int | jacobi (double *a, int n, double *d, double *v, int *nrot) |
int | choldc (double *a, int n, double *l) |
int | inverse (double *TB, double *InvB, int N) |
void | AperB (double *_A, double *_B, double *_res, int _righA, int _colA, int _righB, int _colB) |
void | A_TperB (double *_A, double *_B, double *_res, int _righA, int _colA, int _righB, int _colB) |
void | AperB_T (double *_A, double *_B, double *_res, int _righA, int _colA, int _righB, int _colB) |
int | compute_ellipse (double x[], double y[], int np, double *_x0, double *_y0, double *_angle, double *_a, double *_b) |
int | kb_gazer_get_version (char *version) |
int | kb_gazer_stop_computation () |
int | kb_gazer_wait_stop_computation () |
int | kb_gazer_start_computation () |
int | kb_stargazer_Init (char *DeviceName) |
int | kb_gazer_set_landmark_number (int number) |
int | kb_gazer_get_landmark_number (int *number) |
int | kb_gazer_set_ref_id (int refid) |
int | kb_gazer_get_ref_id (int *refid) |
int | kb_gazer_set_landmark_type (int type) |
int | kb_gazer_get_landmark_type (int *type) |
int | kb_gazer_set_landmark_mode (int mode) |
int | kb_gazer_get_landmark_mode (int *mode) |
int | kb_gazer_set_height_fix_mode (int mode) |
int | kb_gazer_get_height_fix_mode (int *mode) |
int | kb_gazer_start_map_mode () |
int | kb_gazer_set_end_command () |
int | kb_stargazer_read_data (double *x, double *y, double *z, double *angle, int *idnum, char *cmode, int corr) |
void | kb_stargazer_Close () |
int | kb_gazer_calibration (knet_dev_t *mot1, knet_dev_t *mot2, double *_center_x0, double *_center_y0, double *_angle_rot, double *_a_axis, double *_b_axis, double *_stddev_x, double *_stddev_y) |
Variables | |
static int | SerialDeviceHandle = -1 |
const char * | kb_gazer_landmark_types [] = {"HLD1S","HLD1L","HLD2S","HLD2L","HLD3S","HLD3L"} |
const char * | kb_gazer_landmark_modes [] = {"Alone","Map"} |
const char * | kb_gazer_height_fix_modes [] = {"No","Yes"} |
double | a_axis |
double | b_axis |
double | center_x0 |
double | center_y0 |
double | angle_rot |
This module provides useful basic facilities to use the KoreBot with the Stargazer sensor.
Support for KoreBot Stargazer
Definition in file kb_stargazer.c.
#define ACK_CHAR '!' |
Definition at line 47 of file kb_stargazer.c.
Referenced by kb_gazer_set_height_fix_mode(), and kb_gazer_start_map_mode().
#define BATTERY_GPIO 16 |
Definition at line 25 of file kb_stargazer.c.
Referenced by kb_stargazer_Close(), and kb_stargazer_Init().
#define BOOKSTEIN 0 |
Definition at line 75 of file kb_stargazer.c.
Referenced by compute_ellipse().
#define CALIBRATION_SPEED 4500 |
Definition at line 83 of file kb_stargazer.c.
Referenced by kb_gazer_calibration().
#define CMD_CALC_START "~#CalcStart`" |
Definition at line 30 of file kb_stargazer.c.
Referenced by kb_gazer_start_computation().
#define CMD_CALC_START_ACK "~!CalcStart`" |
Definition at line 31 of file kb_stargazer.c.
Referenced by kb_gazer_start_computation().
#define CMD_CALC_STOP "~#CalcStop`" |
Definition at line 28 of file kb_stargazer.c.
Referenced by kb_gazer_stop_computation().
#define CMD_CALC_STOP_ACK "~!CalcStop`" |
Definition at line 29 of file kb_stargazer.c.
Referenced by kb_gazer_stop_computation().
#define CMD_DEADZONE_ACK "DeadZone" |
Definition at line 34 of file kb_stargazer.c.
Referenced by kb_stargazer_read_data().
#define CMD_HEIGHT_FIX "HeightFix" |
Definition at line 40 of file kb_stargazer.c.
Referenced by kb_gazer_get_height_fix_mode(), and kb_gazer_set_height_fix_mode().
#define CMD_LANDMARK_NB "IDNum" |
Definition at line 38 of file kb_stargazer.c.
Referenced by kb_gazer_get_landmark_number(), and kb_gazer_set_landmark_number().
#define CMD_MAP_MODE_START "~#MapMode|Start`" |
Definition at line 45 of file kb_stargazer.c.
Referenced by kb_gazer_start_map_mode().
#define CMD_MAPID_ACK "MAPID" |
Definition at line 35 of file kb_stargazer.c.
Referenced by kb_stargazer_read_data().
#define CMD_MARK_MODE "MarkMode" |
Definition at line 42 of file kb_stargazer.c.
Referenced by kb_gazer_get_landmark_mode(), and kb_gazer_set_landmark_mode().
#define CMD_MARK_TYPE "MarkType" |
Definition at line 41 of file kb_stargazer.c.
Referenced by kb_gazer_get_landmark_type(), and kb_gazer_set_landmark_type().
#define CMD_PARAM_UPDATE "~!ParameterUpdate`" |
Definition at line 44 of file kb_stargazer.c.
Referenced by kb_gazer_set_end_command(), and kb_stargazer_read_data().
#define CMD_REFERENCE_ID "RefID" |
Definition at line 39 of file kb_stargazer.c.
Referenced by kb_gazer_get_ref_id(), and kb_gazer_set_ref_id().
#define CMD_SET_END "SetEnd" |
Definition at line 43 of file kb_stargazer.c.
Referenced by kb_gazer_set_end_command().
#define CMD_VERSION "~@Version`" |
Definition at line 32 of file kb_stargazer.c.
Referenced by kb_gazer_get_version().
#define CMD_VERSION_ACK "~$Version|" |
Definition at line 33 of file kb_stargazer.c.
Referenced by kb_gazer_get_version().
#define CONFIGURE_TIMEOUT 60 |
Definition at line 81 of file kb_stargazer.c.
Referenced by kb_gazer_calibration().
#define FPF 2 |
Definition at line 77 of file kb_stargazer.c.
Referenced by compute_ellipse().
#define MAX_BUF 2048 |
Definition at line 60 of file kb_stargazer.c.
Referenced by kb_gazer_get_height_fix_mode(), kb_gazer_get_landmark_mode(), kb_gazer_get_landmark_number(), kb_gazer_get_landmark_type(), kb_gazer_get_ref_id(), kb_gazer_get_version(), kb_gazer_set_end_command(), kb_gazer_set_height_fix_mode(), kb_gazer_set_landmark_mode(), kb_gazer_set_landmark_number(), kb_gazer_set_landmark_type(), kb_gazer_set_ref_id(), kb_gazer_start_computation(), kb_gazer_start_map_mode(), kb_gazer_stop_computation(), and kb_stargazer_read_data().
#define MAX_CALIB_DATA 2048 |
Definition at line 84 of file kb_stargazer.c.
Referenced by compute_ellipse(), and kb_gazer_calibration().
#define MAX_STOP 10 |
Definition at line 58 of file kb_stargazer.c.
Referenced by kb_gazer_wait_stop_computation().
#define PORT_NAME "/dev/ttyS2" |
Definition at line 22 of file kb_stargazer.c.
Referenced by kb_stargazer_Init().
#define STOP_THRESHOLD_ANGLE 5 |
Definition at line 82 of file kb_stargazer.c.
Referenced by kb_gazer_calibration().
#define TAUBIN 1 |
Definition at line 76 of file kb_stargazer.c.
Referenced by compute_ellipse().
void A_TperB | ( | double * | _A, | |
double * | _B, | |||
double * | _res, | |||
int | _righA, | |||
int | _colA, | |||
int | _righB, | |||
int | _colB | |||
) |
Definition at line 444 of file kb_stargazer.c.
Referenced by compute_ellipse().
void AperB | ( | double * | _A, | |
double * | _B, | |||
double * | _res, | |||
int | _righA, | |||
int | _colA, | |||
int | _righB, | |||
int | _colB | |||
) |
Definition at line 427 of file kb_stargazer.c.
Referenced by compute_ellipse().
void AperB_T | ( | double * | _A, | |
double * | _B, | |||
double * | _res, | |||
int | _righA, | |||
int | _colA, | |||
int | _righB, | |||
int | _colB | |||
) |
Definition at line 460 of file kb_stargazer.c.
Referenced by compute_ellipse().
int choldc | ( | double * | a, | |
int | n, | |||
double * | l | |||
) |
Perform the Cholesky decomposition the lower triangular L such that L*L'=A
a | input matrix pointer | |
n | size of the matrix | |
l | output matrix pointer |
Definition at line 279 of file kb_stargazer.c.
Referenced by compute_ellipse().
int compute_ellipse | ( | double | x[], | |
double | y[], | |||
int | np, | |||
double * | _x0, | |||
double * | _y0, | |||
double * | _angle, | |||
double * | _a, | |||
double * | _b | |||
) |
compute the best ellipse fitting data (x,y)
x | x coordinates of data | |
y | y coordinates of data | |
np | number of data | |
*_x0 | x of ellipse center | |
*_y0 | y of ellipse center | |
*_angle | angle between x axis and ellipse biggest axis | |
*_a | half major axis of the ellipse | |
*_b | half minor axis of the ellipse |
Scan columns
Definition at line 490 of file kb_stargazer.c.
References A_TperB(), AperB(), AperB_T(), BOOKSTEIN, choldc(), FPF, inverse(), jacobi(), MAX_CALIB_DATA, and TAUBIN.
Referenced by kb_gazer_calibration().
int inverse | ( | double * | TB, | |
double * | InvB, | |||
int | N | |||
) |
Compute the inverse of the matrice TB in InvB It uses Gauss-Jordan.
TB | input matrix pointer | |
InvB | ouput matrix pointer | |
N | size of the matrix |
Definition at line 329 of file kb_stargazer.c.
Referenced by compute_ellipse().
int jacobi | ( | double * | a, | |
int | n, | |||
double * | d, | |||
double * | v, | |||
int * | nrot | |||
) |
Perform Jacobi eigenvalue algorithm
a | input matrix pointer | |
n | size of the matrix | |
d | eigenvalue matrix pointer | |
v | output eigenvalue vector pointer | |
nrot | number of rotation |
Definition at line 181 of file kb_stargazer.c.
References rotate().
Referenced by compute_ellipse().
int kb_gazer_calibration | ( | knet_dev_t * | mot1, | |
knet_dev_t * | mot2, | |||
double * | _center_x0, | |||
double * | _center_y0, | |||
double * | _angle_rot, | |||
double * | _a_axis, | |||
double * | _b_axis, | |||
double * | _stddev_x, | |||
double * | _stddev_y | |||
) |
configure the Stargazer rotation compensation
mot1 | left motor pointer | |
mot2 | right motor pointer | |
_center_x0 | x center of the fitted ellipse | |
_center_y0 | y center of the fitted ellipse | |
_angle_rot | angle of rotation of the fitted ellipse | |
_a_axis | half major axis of the fitted ellipse | |
_b_axis | half minor axis of the fitted ellipse | |
_stddev_x | standard deviation error of x | |
_stddev_y | standard deviation error of y |
-1 timeout while computing calibration -2 data buffer too short -3 error computing ellipse parameters -4 error: data are too scattered < -4+ (return kb_stargazer_read_data) error getting Stargazer data
Definition at line 1645 of file kb_stargazer.c.
References a_axis, ANGLE_CORRECTION, angle_rot, b_axis, CALIB_STDEV_MAX, CALIBRATION_SPEED, center_x0, center_y0, compute_ellipse(), CONFIGURE_TIMEOUT, kb_stargazer_read_data(), kmot_SetPoint(), kMotRegSpeed, MAX_CALIB_DATA, stop(), and STOP_THRESHOLD_ANGLE.
Referenced by main().
int kb_gazer_get_height_fix_mode | ( | int * | mode | ) |
Get the height fix mode
mode | height fix mode: 0 = NO, 1 = YES |
Definition at line 1300 of file kb_stargazer.c.
References buf, CMD_HEIGHT_FIX, kb_gazer_height_fix_modes, MAX_BUF, NB_HEIGHT_FIX_MODES, SerialDeviceHandle, and write_serial().
Referenced by main(), and set_Stargazer_parameters().
int kb_gazer_get_landmark_mode | ( | int * | mode | ) |
Get the landmark mode
mode | landmark mode |
Definition at line 1200 of file kb_stargazer.c.
References buf, CMD_MARK_MODE, kb_gazer_landmark_modes, MAX_BUF, NB_MARK_MODES, SerialDeviceHandle, and write_serial().
Referenced by main(), and set_Stargazer_parameters().
int kb_gazer_get_landmark_number | ( | int * | number | ) |
Get the number of landmarks
number | number of landmarks |
Definition at line 928 of file kb_stargazer.c.
References buf, CMD_LANDMARK_NB, MAX_BUF, SerialDeviceHandle, and write_serial().
Referenced by main(), and set_Stargazer_parameters().
int kb_gazer_get_landmark_type | ( | int * | type | ) |
Get the landmark type
type | landmark type |
Definition at line 1101 of file kb_stargazer.c.
References buf, CMD_MARK_TYPE, kb_gazer_landmark_types, MAX_BUF, NB_MARK_TYPES, SerialDeviceHandle, and write_serial().
Referenced by main(), and set_Stargazer_parameters().
int kb_gazer_get_ref_id | ( | int * | refid | ) |
Get the landmark id reference
refid | landmark id number reference |
Definition at line 1012 of file kb_stargazer.c.
References buf, CMD_REFERENCE_ID, MAX_BUF, SerialDeviceHandle, and write_serial().
Referenced by main(), and set_Stargazer_parameters().
int kb_gazer_get_version | ( | char * | version | ) |
Get Stargazer firmware version
version | version |
Definition at line 681 of file kb_stargazer.c.
References CMD_VERSION, CMD_VERSION_ACK, MAX_BUF, SerialDeviceHandle, and write_serial().
Referenced by main().
int kb_gazer_set_end_command | ( | ) |
Set end of commands for update
Definition at line 1401 of file kb_stargazer.c.
References buf, CMD_PARAM_UPDATE, CMD_SET_END, MAX_BUF, SerialDeviceHandle, and write_serial().
Referenced by set_Stargazer_parameters().
int kb_gazer_set_height_fix_mode | ( | int | mode | ) |
Set the height fix mode
mode | height fix: 0 = NO, 1 = YES |
Definition at line 1254 of file kb_stargazer.c.
References ACK_CHAR, buf, CMD_HEIGHT_FIX, kb_gazer_height_fix_modes, MAX_BUF, SerialDeviceHandle, and write_serial().
Referenced by set_Stargazer_parameters().
int kb_gazer_set_landmark_mode | ( | int | mode | ) |
Set the landmark mode
mode | landmark mode |
Definition at line 1155 of file kb_stargazer.c.
References buf, CMD_MARK_MODE, kb_gazer_landmark_modes, MAX_BUF, SerialDeviceHandle, and write_serial().
Referenced by set_Stargazer_parameters().
int kb_gazer_set_landmark_number | ( | int | number | ) |
Set the number of landmarks
number | number of landmarks |
Definition at line 887 of file kb_stargazer.c.
References buf, CMD_LANDMARK_NB, MAX_BUF, SerialDeviceHandle, and write_serial().
Referenced by set_Stargazer_parameters().
int kb_gazer_set_landmark_type | ( | int | type | ) |
Set the landmark type
type | landmark type |
Definition at line 1056 of file kb_stargazer.c.
References buf, CMD_MARK_TYPE, kb_gazer_landmark_types, MAX_BUF, SerialDeviceHandle, and write_serial().
Referenced by set_Stargazer_parameters().
int kb_gazer_set_ref_id | ( | int | refid | ) |
Set the landmark id as reference
refid | landmark id number to set as reference |
Definition at line 971 of file kb_stargazer.c.
References buf, CMD_REFERENCE_ID, MAX_BUF, SerialDeviceHandle, and write_serial().
Referenced by set_Stargazer_parameters().
int kb_gazer_start_computation | ( | ) |
Start computation of position
Definition at line 805 of file kb_stargazer.c.
References CMD_CALC_START, CMD_CALC_START_ACK, MAX_BUF, SerialDeviceHandle, and write_serial().
Referenced by main().
int kb_gazer_start_map_mode | ( | ) |
Start the map building mode
Definition at line 1354 of file kb_stargazer.c.
References ACK_CHAR, buf, CMD_MAP_MODE_START, kb_gazer_wait_stop_computation(), MAX_BUF, SerialDeviceHandle, and write_serial().
Referenced by main().
int kb_gazer_stop_computation | ( | ) |
Stop computation of position
Definition at line 736 of file kb_stargazer.c.
References CMD_CALC_STOP, CMD_CALC_STOP_ACK, MAX_BUF, SerialDeviceHandle, and write_serial().
Referenced by kb_gazer_wait_stop_computation().
int kb_gazer_wait_stop_computation | ( | ) |
Stop computation of position and wait until it stops (retry MAX_STOP)
Definition at line 774 of file kb_stargazer.c.
References kb_gazer_stop_computation(), and MAX_STOP.
Referenced by kb_gazer_start_map_mode(), kb_stargazer_Init(), and main().
void kb_stargazer_Close | ( | ) |
kb_stargazer_Close release the Stargazer
Definition at line 1605 of file kb_stargazer.c.
References BATTERY_GPIO, kb_gpio_cleanup(), kb_gpio_clear(), and SerialDeviceHandle.
Referenced by main().
int kb_stargazer_Init | ( | char * | DeviceName | ) |
kb_gazer_Init does openning port, then certify the link
DeviceName | String name of the serial port device where the Stargazer is connected |
Definition at line 846 of file kb_stargazer.c.
References BATTERY_GPIO, kb_gazer_wait_stop_computation(), kb_gpio_dir(), kb_gpio_function(), kb_gpio_init(), kb_gpio_set(), openserial(), PORT_NAME, and SerialDeviceHandle.
Referenced by main().
int kb_stargazer_read_data | ( | double * | x, | |
double * | y, | |||
double * | z, | |||
double * | angle, | |||
int * | idnum, | |||
char * | cmode, | |||
int | corr | |||
) |
Read data and interpret data from the Stargazer serial port kb_stargazer_read_data must be called periodically (up to 10 times/s) kb_gazer_start_computation must be called once before
x | x position relative to the reference landmark in [cm], right direction | |
y | y position relative to the reference landmark in [cm], forward direction | |
z | height to the landmark in [cm] | |
angle | angle relative to the reference landmark orientation in [degree] (counterclockwise) in 0..360 range | |
idnum | id number of the currently used landmark | |
cmode | current mode: 'F' = map building mode, 'I' map mode, 'Z' height calculation mode | |
corr | 1 = apply position correction; 0 do not apply it |
Definition at line 1477 of file kb_stargazer.c.
References a_axis, ANGLE_CORRECTION, angle_rot, b_axis, CMD_DEADZONE_ACK, CMD_MAPID_ACK, CMD_PARAM_UPDATE, id, kb_erase_line(), MAX_BUF, and SerialDeviceHandle.
Referenced by kb_gazer_calibration(), and main().
int openserial | ( | int * | fd, | |
const char * | name | |||
) |
Open serial port
fd | file descriptor | |
name | serial device name |
Definition at line 117 of file kb_stargazer.c.
Referenced by kb_stargazer_Init().
void rotate | ( | double * | A, | |
int | i, | |||
int | j, | |||
int | k, | |||
int | l, | |||
double | tau, | |||
double | s, | |||
int | n | |||
) |
compute rotation in matrix A of Aij and Akl:
A | input/ouput matrix pointer to be rotated | |
i | index | |
j | index | |
k | index | |
l | index | |
tau | parameter | |
s | parameter | |
n | matrix size |
Definition at line 159 of file kb_stargazer.c.
Referenced by jacobi().
void write_serial | ( | int | fd, | |
char * | text | |||
) |
write text to file fd
fd | file descriptor | |
text | char array to write |
Definition at line 97 of file kb_stargazer.c.
Referenced by kb_gazer_get_height_fix_mode(), kb_gazer_get_landmark_mode(), kb_gazer_get_landmark_number(), kb_gazer_get_landmark_type(), kb_gazer_get_ref_id(), kb_gazer_get_version(), kb_gazer_set_end_command(), kb_gazer_set_height_fix_mode(), kb_gazer_set_landmark_mode(), kb_gazer_set_landmark_number(), kb_gazer_set_landmark_type(), kb_gazer_set_ref_id(), kb_gazer_start_computation(), kb_gazer_start_map_mode(), and kb_gazer_stop_computation().
double a_axis |
Definition at line 63 of file kb_stargazer.c.
Referenced by kb_gazer_calibration(), kb_stargazer_read_data(), and main().
double angle_rot |
Definition at line 63 of file kb_stargazer.c.
Referenced by kb_gazer_calibration(), kb_stargazer_read_data(), and main().
double b_axis |
Definition at line 63 of file kb_stargazer.c.
Referenced by kb_gazer_calibration(), kb_stargazer_read_data(), and main().
double center_x0 |
Definition at line 63 of file kb_stargazer.c.
Referenced by kb_gazer_calibration().
double center_y0 |
Definition at line 63 of file kb_stargazer.c.
Referenced by kb_gazer_calibration().
const char* kb_gazer_height_fix_modes[] = {"No","Yes"} |
Definition at line 55 of file kb_stargazer.c.
Referenced by kb_gazer_get_height_fix_mode(), kb_gazer_set_height_fix_mode(), main(), and set_Stargazer_parameters().
const char* kb_gazer_landmark_modes[] = {"Alone","Map"} |
Definition at line 54 of file kb_stargazer.c.
Referenced by kb_gazer_get_landmark_mode(), kb_gazer_set_landmark_mode(), main(), and set_Stargazer_parameters().
const char* kb_gazer_landmark_types[] = {"HLD1S","HLD1L","HLD2S","HLD2L","HLD3S","HLD3L"} |
Definition at line 53 of file kb_stargazer.c.
Referenced by kb_gazer_get_landmark_type(), kb_gazer_set_landmark_type(), main(), and set_Stargazer_parameters().
int SerialDeviceHandle = -1 [static] |
Definition at line 49 of file kb_stargazer.c.
Referenced by kb_gazer_get_height_fix_mode(), kb_gazer_get_landmark_mode(), kb_gazer_get_landmark_number(), kb_gazer_get_landmark_type(), kb_gazer_get_ref_id(), kb_gazer_get_version(), kb_gazer_set_end_command(), kb_gazer_set_height_fix_mode(), kb_gazer_set_landmark_mode(), kb_gazer_set_landmark_number(), kb_gazer_set_landmark_type(), kb_gazer_set_ref_id(), kb_gazer_start_computation(), kb_gazer_start_map_mode(), kb_gazer_stop_computation(), kb_stargazer_Close(), kb_stargazer_Init(), and kb_stargazer_read_data().