Ejemplo n.º 1
0
/*
 * 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;
}
Ejemplo n.º 2
0
/* 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;
}
Ejemplo n.º 3
0
/**
 * 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;
}
Ejemplo n.º 4
0
/*
 * 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;
}
Ejemplo n.º 5
0
/*
 * 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;
}