kb_stargazer.c File Reference

This module provides useful basic facilities to use the KoreBot with the Stargazer sensor. More...

Include dependency graph for kb_stargazer.c:

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

Detailed Description

This module provides useful basic facilities to use the KoreBot with the Stargazer sensor.

Support for KoreBot Stargazer

Author:
J. Tharin (K-Team SA)
Note:
Copyright (C) 2012 K-TEAM SA
Bug:
none discovered.
Todo:

Definition in file kb_stargazer.c.


Define Documentation

#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
#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().


Function Documentation

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

Parameters:
a input matrix pointer
n size of the matrix
l output matrix pointer
Returns:
0 Ok, else not positive definite

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)

Parameters:
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
Returns:
0 for OK, -1 error (scalar matrix while inverting)

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().

Here is the call graph for this function:

int inverse ( double *  TB,
double *  InvB,
int  N 
)

Compute the inverse of the matrice TB in InvB It uses Gauss-Jordan.

Parameters:
TB input matrix pointer
InvB ouput matrix pointer
N size of the matrix
Returns:
0 Ok, else singular

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

Parameters:
a input matrix pointer
n size of the matrix
d eigenvalue matrix pointer
v output eigenvalue vector pointer
nrot number of rotation
Returns:
0 Ok, -1 : Too many iterations

Definition at line 181 of file kb_stargazer.c.

References rotate().

Referenced by compute_ellipse().

Here is the call graph for this function:

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

Parameters:
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
Returns:
0 no error

-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().

Here is the call graph for this function:

int kb_gazer_get_height_fix_mode ( int *  mode  ) 

Get the height fix mode

Parameters:
mode height fix mode: 0 = NO, 1 = YES
Returns:
0 : no error -1 : serial port not open -2 : command not acknowledged -3 : unknown height mode

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().

Here is the call graph for this function:

int kb_gazer_get_landmark_mode ( int *  mode  ) 

Get the landmark mode

Parameters:
mode landmark mode
Returns:
0 : no error -1 : serial port not open -2 : command not acknowledged -3 : unknown 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().

Here is the call graph for this function:

int kb_gazer_get_landmark_number ( int *  number  ) 

Get the number of landmarks

Parameters:
number number of landmarks
Returns:
0 : no error -1 : serial port not open -2 : command not acknowledged

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().

Here is the call graph for this function:

int kb_gazer_get_landmark_type ( int *  type  ) 

Get the landmark type

Parameters:
type landmark type
Returns:
0 : no error -1 : serial port not open -2 : command not acknowledged -3 : unknown 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().

Here is the call graph for this function:

int kb_gazer_get_ref_id ( int *  refid  ) 

Get the landmark id reference

Parameters:
refid landmark id number reference
Returns:
0 : no error -1 : serial port not open -2 : command not acknowledged

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().

Here is the call graph for this function:

int kb_gazer_get_version ( char *  version  ) 

Get Stargazer firmware version

Parameters:
version version
Returns:
0 : no error -1 : serial port not open -2 : command not acknowledged -3 : command not acknowledged

Definition at line 681 of file kb_stargazer.c.

References CMD_VERSION, CMD_VERSION_ACK, MAX_BUF, SerialDeviceHandle, and write_serial().

Referenced by main().

Here is the call graph for this function:

int kb_gazer_set_end_command (  ) 

Set end of commands for update

Returns:
0 : no error -1 : serial port not open -2 : command not acknowledged -3 : data not upated

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().

Here is the call graph for this function:

int kb_gazer_set_height_fix_mode ( int  mode  ) 

Set the height fix mode

Parameters:
mode height fix: 0 = NO, 1 = YES
Returns:
0 : no error -1 : serial port not open -2 : command not acknowledged -3 : mode not valid

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().

Here is the call graph for this function:

int kb_gazer_set_landmark_mode ( int  mode  ) 

Set the landmark mode

Parameters:
mode landmark mode
Returns:
0 : no error -1 : serial port not open -2 : command not acknowledged -3 : mode not valid

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().

Here is the call graph for this function:

int kb_gazer_set_landmark_number ( int  number  ) 

Set the number of landmarks

Parameters:
number number of landmarks
Returns:
0 : no error -1 : serial port not open -2 : command not acknowledged

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().

Here is the call graph for this function:

int kb_gazer_set_landmark_type ( int  type  ) 

Set the landmark type

Parameters:
type landmark type
Returns:
0 : no error -1 : serial port not open -2 : command not acknowledged -3 : type not valid

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().

Here is the call graph for this function:

int kb_gazer_set_ref_id ( int  refid  ) 

Set the landmark id as reference

Parameters:
refid landmark id number to set as reference
Returns:
0 : no error -1 : serial port not open -2 : command not acknowledged

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().

Here is the call graph for this function:

int kb_gazer_start_computation (  ) 

Start computation of position

Returns:
0 : no error -1 : serial port not open -2 : command not acknowledged

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().

Here is the call graph for this function:

int kb_gazer_start_map_mode (  ) 

Start the map building mode

Returns:
0 : no error -1 : serial port not open -2 : could not stop receiving position -3 : command not acknowledged

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().

Here is the call graph for this function:

int kb_gazer_stop_computation (  ) 

Stop computation of position

Returns:
0 : no error -1 : serial port not open -2 : command not acknowledged

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().

Here is the call graph for this function:

int kb_gazer_wait_stop_computation (  ) 

Stop computation of position and wait until it stops (retry MAX_STOP)

Returns:
0 : no error -1 : serial port not open -2 : command not acknowledged

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().

Here is the call graph for this function:

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().

Here is the call graph for this function:

int kb_stargazer_Init ( char *  DeviceName  ) 

kb_gazer_Init does openning port, then certify the link

Parameters:
DeviceName String name of the serial port device where the Stargazer is connected
Returns:
0 : no error -1 : error initialising gpio -2 : cannot open serial port -3 : cannot communicate with the Stargazer

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().

Here is the call graph for this function:

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

Parameters:
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
Returns:
0 : no error -1 : cannot serial port not open -2 : cannot communicate with the Stargazer -3 : buffer overrun (try to call more often this function!) -4 : read command not acknowledged -5 : data error -6 : no landmark found -7 : data error -8 : no received data -9 : mapid error 1 : update parameters after map building mode > 1: MAPID in map building mode

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().

Here is the call graph for this function:

int openserial ( int *  fd,
const char *  name 
)

Open serial port

Parameters:
fd file descriptor
name serial device name
Returns:
0 OK, <0 ERROR

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:

Parameters:
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 
)

Variable Documentation

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"}
const char* kb_gazer_landmark_modes[] = {"Alone","Map"}
const char* kb_gazer_landmark_types[] = {"HLD1S","HLD1L","HLD2S","HLD2L","HLD3S","HLD3L"}
int SerialDeviceHandle = -1 [static]
Generated on Mon Dec 3 09:43:48 2012 for KoreBot Library by  doxygen 1.6.3