/*
* Read the next 3-D grid from the file.
*/
static int read_grid( int f, int id, float data[] )
{
int compsize = ((Nr*Nc*Nl+3)/4)*4;
float ga[MAXLEVELS], gb[MAXLEVELS];
int_1 compdata[MAXROWS*MAXCOLUMNS*MAXLEVELS];
int p, lev, i;
float min, max;
int missing_count;
if (id==0x80808080 || id==0x80808081) {
read_float4( f, &ga[0] );
read_float4( f, &gb[0] );
for (i=1;i<Nl;i++) {
ga[i] = ga[0];
gb[i] = gb[0];
}
}
else if (id==0x80808082 || id==0x80808083) {
int mcfile, mcgrid;
if (id==0x80808083) {
read_int4( f, &mcfile );
read_int4( f, &mcgrid );
}
read_float4_array( f, ga, Nl );
read_float4_array( f, gb, Nl );
}
if (read_bytes( f, compdata, compsize )!=compsize) {
printf("read_bytes failed\n");
}
min = 10000.0;
max = -min;
missing_count = 0;
/* decompress */
p = 0;
for (lev=0;lev<Nl;lev++) {
float ga_inv = 1.0 / ga[lev];
float gb_val = gb[lev];
for (i=0;i<Nr*Nc;i++) {
if (compdata[p] == 127) {
data[p] = MISSING;
missing_count++;
}
else {
data[p] = ( (float) (int) compdata[p] - gb_val ) * ga_inv;
if (data[p]>max)
max = data[p];
if (data[p]<min)
min = data[p];
}
p++;
}
}
printf("min=%g max=%g missing=%d\n", min, max, missing_count );
return 1;
}
/* test if ip matches current idx.
* return values: -1, 0, 1 standing for ip smaller, in range, or greater.
*/
static inline int ip_match(FILE *fp, unsigned int ip, int idx)
{
fseek(fp, idx, SEEK_SET);
unsigned int n = read_int4(fp); /* ip range start */
if (ip < n)
return -1;
fseek(fp, read_int3(fp), SEEK_SET);
n = read_int4(fp); /* ip range end */
return (ip <= n) ? 0:1;
}
/**
* base_proc_update - process new information received via ethercat.
* update stats, reset EC_TIMEOUT
*
* @param base
*
* @return
*/
static int base_proc_update(struct youbot_base_info* base)
{
int i, tmp, ret=-1;
int32_t cm;
in_motor_t *motor_in;
out_motor_t *motor_out;
int32_t cmd_cur_vel[YOUBOT_NR_OF_WHEELS];
/* copy data */
for(i=0; i<YOUBOT_NR_OF_WHEELS; i++) {
motor_in = (in_motor_t*) ec_slave[ base->wheel_inf[i].slave_idx ].inputs;
base->wheel_inf[i].msr_pos = motor_in->position;
base->wheel_inf[i].msr_vel = motor_in->velocity;
base->wheel_inf[i].msr_cur = motor_in->current;
base->wheel_inf[i].error_flags = motor_in->error_flags;
#ifdef FIRMWARE_V2
base->wheel_inf[i].msr_pwm = motor_in->pwm;
#endif
}
base_proc_errflg(base);
/* Force control_mode to INITIALIZE if unconfigured */
if(!base_is_configured(base)) {
base->control_mode = YOUBOT_CMODE_INITIALIZE;
goto handle_cm;
} else if (base_is_configured(base) && base->control_mode == YOUBOT_CMODE_INITIALIZE) {
DBG("init completed, switching to MOTORSTOP");
base->control_mode = YOUBOT_CMODE_MOTORSTOP;
tmp=YOUBOT_CMODE_MOTORSTOP;
write_int(base->p_control_mode, &tmp);
}
/* new control mode? */
if(read_int(base->p_control_mode, &cm) > 0) {
if(cm<0 || cm>7) {
ERR("invalid control_mode %d", cm);
base->control_mode=YOUBOT_CMODE_MOTORSTOP;
tmp=YOUBOT_CMODE_MOTORSTOP;
write_int(base->p_control_mode, &tmp);
} else {
DBG("setting control_mode to %d", cm);
base->control_mode=cm;
tmp=cm;
write_int(base->p_control_mode, &tmp);
}
/* reset output quantity upon control_mode switch */
for(i=0; i<YOUBOT_NR_OF_WHEELS; i++)
base->wheel_inf[i].cmd_val=0;
base_check_watchdog(base, 1);
}
base_output_msr_data(base);
handle_cm:
switch(base->control_mode) {
case YOUBOT_CMODE_MOTORSTOP:
break;
case YOUBOT_CMODE_VELOCITY:
if(read_int4(base->p_cmd_vel, &cmd_cur_vel) == 4) { /* raw velocity */
for(i=0; i<YOUBOT_NR_OF_WHEELS; i++)
base->wheel_inf[i].cmd_val=cmd_cur_vel[i];
base_check_watchdog(base, 1);
} else if (read_kdl_twist(base->p_cmd_twist, &base->cmd_twist) > 0) { /* twist */
twist_to_wheel_val(base);
base_check_watchdog(base, 1);
} else { /* no new data */
/* if cmd_vel is zero, trigger watchdog */
if(base->wheel_inf[0].cmd_val==0 && base->wheel_inf[1].cmd_val==0 &&
base->wheel_inf[2].cmd_val==0 && base->wheel_inf[3].cmd_val==0) {
base_check_watchdog(base, 1);
} else {
base_check_watchdog(base, 0);
}
}
break;
case YOUBOT_CMODE_CURRENT:
if(read_int4(base->p_cmd_cur, &cmd_cur_vel) == 4) { /* raw velocity */
for(i=0; i<YOUBOT_NR_OF_WHEELS; i++)
base->wheel_inf[i].cmd_val=cmd_cur_vel[i];
base_check_watchdog(base, 1);
} else {
if(base->wheel_inf[0].cmd_val==0 && base->wheel_inf[1].cmd_val==0 &&
base->wheel_inf[2].cmd_val==0 && base->wheel_inf[3].cmd_val==0) {
base_check_watchdog(base, 1);
} else {
base_check_watchdog(base, 0);
}
}
break;
case YOUBOT_CMODE_INITIALIZE:
break;
default:
ERR("unexpected control_mode: %d, switching to MOTORSTOP", base->control_mode);
base->control_mode = YOUBOT_CMODE_MOTORSTOP;
break;
}
/* copy cmd_val to output buffer */
for(i=0; i<YOUBOT_NR_OF_WHEELS; i++) {
motor_out = (out_motor_t*) ec_slave[ base->wheel_inf[i].slave_idx ].outputs;
motor_out->value = base->wheel_inf[i].cmd_val;
motor_out->controller_mode = base->control_mode;
}
ret=0;
return ret;
}
/*
* Read the header info in the COMP* file and put into global vars.
*/
static int read_comp_header( int f )
{
int id, gridtimes, gridvars, i, j, ip;
/* read id */
read_int4( f, &id );
if (id==0x80808080 || id==0x80808081) {
float top;
if (id==0x80808080) {
gridtimes = 300;
gridvars = 20;
}
else {
gridtimes = 400;
gridvars = 30;
}
read_int4( f, &NumTimes );
read_int4( f, &NumVars );
read_int4( f, &Nr );
read_int4( f, &Nc );
read_int4( f, &Nl );
read_float4( f, &NorthLat[0] );
read_float4( f, &WestLon[0] );
read_float4( f, &top );
read_float4( f, &LatInc );
read_float4( f, &LonInc );
read_float4( f, &HgtInc );
Bottom = top - HgtInc * (Nl-1);
for (i=1;i<NumTimes;i++) {
NorthLat[i] = NorthLat[0];
WestLon[i] = WestLon[0];
}
/* read dates and times */
read_int4_array( f, DateStamp, gridtimes );
read_int4_array( f, TimeStamp, gridtimes );
/* read parm names */
for (i=0;i<gridvars;i++) {
if ( read_bytes(f, VarName[i], 4)!=4 ) {
printf("Error: Unable to read parameter names.\n");
return 0;
}
/* remove trailing spaces, if any */
for (j=7;j>0;j--) {
if (VarName[i][j]==' ' || VarName[i][j]==0)
VarName[i][j] = 0;
else
break;
}
}
}
else if (id==0x80808082 || id==0x80808083) {
float height[MAXLEVELS];
read_int4( f, &gridtimes );
read_int4( f, &NumVars );
read_int4( f, &NumTimes );
read_int4( f, &Nr );
read_int4( f, &Nc );
read_int4( f, &Nl );
read_float4( f, &LatInc );
read_float4( f, &LonInc );
read_float4_array( f, height, Nl );
Bottom = height[0];
HgtInc = (height[Nl-1] - height[0]) / (Nl-1);
for (ip=0;ip<NumVars;ip++) {
if ( read_bytes(f, VarName[ip], 8)!=8 ) {
printf("Error: Unable to read parameter names.\n");
return 0;
}
/* remove trailing spaces, if any */
for (j=7;j>0;j--) {
if (VarName[ip][j]==' ' || VarName[ip][j]==0)
VarName[ip][j] = 0;
else
break;
}
}
read_float4_array( f, MinVal, NumVars );
read_float4_array( f, MaxVal, NumVars );
read_int4_array( f, TimeStamp, gridtimes );
read_int4_array( f, DateStamp, gridtimes );
read_float4_array( f, NorthLat, gridtimes );
read_float4_array( f, WestLon, gridtimes );
}
for (i=0;i<NumTimes;i++) {
TimeStamp[i] = v5dSecondsToHHMMSS( TimeStamp[i] );
DateStamp[i] = v5dDaysToYYDDD( DateStamp[i] );
}
return id;
}
/*
* Read a topography file and initialize Topo and TopoData.
* Input: filename - name of topo file.
* Return: 0 if error, otherwise non-zero for success.
*/
int read_topo( struct Topo *topo, char *filename )
{
int f;
int n;
char id[40];
f = open( filename, O_RDONLY );
if (f<0) {
printf("Topo file %s not found\n", filename );
return 0;
}
/* Read topo file header */
read_bytes( f, id, 40 );
read_float4( f, &(topo->Topo_westlon) );
read_float4( f, &(topo->Topo_eastlon) );
read_float4( f, &(topo->Topo_northlat) );
read_float4( f, &(topo->Topo_southlat) );
read_int4( f, &(topo->Topo_rows) );
read_int4( f, &(topo->Topo_cols) );
if (strncmp(id,"TOPO2",5)==0) {
/* OK */
}
else if (strncmp(id,"TOPO",4)==0) {
/* OLD STYLE: bounds given as ints, convert to floats */
int *p;
p = (int *) &(topo->Topo_westlon); topo->Topo_westlon = (float) *p / 100.0;
p = (int *) &(topo->Topo_eastlon); topo->Topo_eastlon = (float) *p / 100.0;
p = (int *) &(topo->Topo_northlat); topo->Topo_northlat = (float) *p / 100.0;
p = (int *) &(topo->Topo_southlat); topo->Topo_southlat = (float) *p / 100.0;
}
else {
printf("%s is not a TOPO file >%s<\n", filename,id);
close(f);
return 0;
}
if(topo->TopoData)
free(topo->TopoData);
topo->TopoData = (short *) malloc(topo->Topo_rows * topo->Topo_cols * sizeof(short));
/* dtx->TopoData = (short *) allocate( dtx, dtx->Topo_rows * dtx->Topo_cols
* sizeof(short) ); */
if (!topo->TopoData) {
printf("ERROR: Failed to allocate space for topo data\n");
close(f);
return 0;
}
n = topo->Topo_rows * topo->Topo_cols;
if (read_int2_array( f, topo->TopoData, n) < n) {
printf("ERROR: could not read data file or premature end of file\n");
free( topo->TopoData);
topo->TopoData = NULL;
close(f);
return 0;
}
close(f);
return 1;
}