int main(int argc, char *argv[]) {
SensorA sens_data;
SSM_sid sensA_sid;
double measured_time;
int tid;
double now_time;
initSSM();
sensA_sid = openSSM("sensor_A", 0, 0);
while (1) {
now_time = gettimeSSM(); //現在時刻を取得
//現在時刻の1秒前のデータを取得
tid = readSSM_time(sensA_sid, (char*) &sens_data, now_time - 1,
&measured_time);
printf("nowtime=%f\n time=%f tid=%d a=%f b=%f c=%d\n", gettimeSSM()
- 1, measured_time, tid, sens_data.a, sens_data.b, sens_data.c);
//その一つ後のデータを取得
//tid++;
tid = readSSM(sensA_sid, (char*) &sens_data, &measured_time, tid + 1);
printf(" time=%f tid=%d a=%f b=%f c=%d\n\n", measured_time, tid,
sens_data.a, sens_data.b, sens_data.c);
sleep(1);
}
}
/*-----------------------------------------------------------
*/
void initSMAQ() {
int rc;
//int vsp=-1;
//rc= vmxopen(&vsp,"0x820000", "0xd000");
rc= vmeopen("0x820000", "0xd000");
if(rc!=0) {
printf("vmeopen CTP vme:%d\n", rc); exit(8);
};
ctpshmbase= (Tctpshm *)mallocShared(CTPSHMKEY, sizeof(Tctpshm), &ctpsegid);
//ctpshmbase= (Tctpshm *)malloc( sizeof(Tctpshm));
validCTPINPUTs= &ctpshmbase->validCTPINPUTs[0];
validLTUs= &ctpshmbase->validLTUs[0];
checkCTP();
ddr3_reset();
initSSM();
}
int main(int argc, char *argv[]) {
SensorB sens_data;
double measured_time;
SSM_sid sensB_sid;
initSSM();
//名前をsensor_Bで、IDを0で、0.05秒おきに来るデータを5秒間保持するよう領域を確保。
sensB_sid = createSSM_time("sensor_B", 0, sizeof(SensorB), 5, 0.05);
while (1) {
//SensorBのデータを何かしらの方法（例えばシリアル通信）で取得し、頑張って観測時刻を推定する関数。
receive_sensorB(&sens_data, &measured_time);
//SSMへのデータの書き込み。
writeSSM(sensB_sid, (char*) &sens_data, measured_time);
}
}
int main(int argc, char *argv[]) {
SSMLog<Spur_Odometry> ssmlog_odometry; // gyro_odm
SSMLog<Spur_Odometry> ssmlog_adjust; // spur_adjust
SSMLog<PWSMotor> ssmlog_mtr; // moter ssm
SSMLog<YP_ad> ssmlog_ad; // ad
SSMLog_OdometryErr ssmlog_odmerr; // odometry error ratio
struct {
double radius_r;
double radius_l;
double tread;
double gear;
double count_rev;
} odm_prop;
Odometer::proc_configuration pconf; // process configuration
Odometer::proc_option opt(&pconf); // process option
FILE* fp = 0;
gnd::cui_reader cuireader;
{ // ---> initialize
uint32_t phase = 1;
// get option
if( opt.get_option(argc, argv) ) {
return 0;
}
::fprintf(stderr, "========== Initialize ==========\n");
::fprintf(stderr, " %d. allocate signal \"\x1b[4mSIGINT\x1b[0m\" to shut-off\n", phase++);
::fprintf(stderr, " %d. get kinematics parameter\n", phase++);
::fprintf(stderr, " %d. initialize \x1b[4mssm\x1b[0m\n", phase++);
::fprintf(stderr, " %d. create ssm-data \"\x1b[4m%s\x1b[0m\"\n", phase++, pconf.output_ssmname.value );
if( pconf.spur_adjust.value && ::strcmp(pconf.output_ssmname.value, SNAME_ADJUST) ) {
::fprintf(stderr, " %d. create ssm-data \"\x1b[4m%s\x1b[0m\"\n", phase++, SNAME_ADJUST );
}
if( pconf.gyrodometry.value ) {
::fprintf(stderr, " %d. open ssm-data \"\x1b[4m%s\x1b[0m\"\n", phase++, pconf.ad_ssmname.value );
::fprintf(stderr, " %d. open ssm-data \"\x1b[4m%s\x1b[0m\"\n", phase++, pconf.motor_ssmname.value );
}
if(pconf.debug.value)
::fprintf(stderr, " %d. open debug log\n", phase++);
::fprintf(stderr, "\n");
{ // ---> allocate SIGINT to shut-off
::proc_shutoff_clear();
::proc_shutoff_alloc_signal(SIGINT);
} // <--- allocate SIGINT to shut-off
if( !::is_proc_shutoff() ) { // ---> set initial kinematics
::fprintf(stderr, "\n");
::fprintf(stderr, " => get initial kinematics parameter\n");
{ // ---> set-zero kinematics property
odm_prop.radius_l = 0;
odm_prop.radius_r = 0;
odm_prop.tread = 0;
odm_prop.gear = 0;
odm_prop.count_rev = 0;
} // <--- set-zero kinematics property
// read kinematics configure file
if(*pconf.kfile.value != '\0') {
::fprintf(stderr, " load kinematics parameter file \"\x1b[4m%s\x1b[0m\"\n", pconf.kfile.value);
read_kinematics_config(&odm_prop.radius_r, &odm_prop.radius_l, &odm_prop.tread,
&odm_prop.gear, &odm_prop.count_rev, pconf.kfile.value);
}
// check kinematic parameter
if( odm_prop.radius_r > 0 && odm_prop.radius_l > 0 && odm_prop.tread > 0 && odm_prop.gear > 0 && odm_prop.count_rev > 0) {
::fprintf(stderr, " load lacking parameter from \x1b[4mypsypur-coordinator\x1b[0m\n");
}
else {
// initialize for using ypspur-coordinator
if( Spur_init() < 0) {
::fprintf(stderr, " ... \x1b[1m\x1b[31mERROR\x1b[39m\x1b[0m: fail to connect ypspur-coordinator.\n");
::proc_shutoff();
}
// get kinmeatics from ypspur-coordinator
if( !::is_proc_shutoff() && YP_get_parameter( YP_PARAM_RADIUS_R, &odm_prop.radius_r ) < 0 ) {
::proc_shutoff();
}
if( !::is_proc_shutoff() && YP_get_parameter( YP_PARAM_RADIUS_L, &odm_prop.radius_l ) < 0 ) {
::proc_shutoff();
}
if( !::is_proc_shutoff() && YP_get_parameter( YP_PARAM_TREAD, &odm_prop.tread ) < 0 ) {
::proc_shutoff();
}
if( !::is_proc_shutoff() && YP_get_parameter(YP_PARAM_COUNT_REV, &odm_prop.count_rev) < 0 ) {
::proc_shutoff();
}
if( !::is_proc_shutoff() && YP_get_parameter(YP_PARAM_GEAR, &odm_prop.gear) < 0 ) {
::proc_shutoff();
}
}
// <--- get parameter from ypspur-coordinater
// check kinematic parameter
if( odm_prop.radius_r <= 0 || odm_prop.radius_l <= 0 || odm_prop.tread <= 0 || odm_prop.gear <= 0 || odm_prop.count_rev <= 0) {
::fprintf(stderr, " => \x1b[1m\x1b[31mERROR\x1b[39m\x1b[0m : Incomplete loading kinematics parameter.\n");
::proc_shutoff();
}
} // <--- set initial kinematics
// ---> initialize ssm
if( !::is_proc_shutoff() ) {
::fprintf(stderr, "\n");
::fprintf(stderr, " => initialize SSM\n");
if( !initSSM() ) {
::fprintf(stderr, " \x1b[1m\x1b[31mERROR\x1b[39m\x1b[0m: SSM is not available.\n");
::proc_shutoff();
}
else {
::fprintf(stderr, " ... \x1b[1mOK\x1b[0m\n");
}
} // <--- initialize ssm
// ---> ssm odometry open
if( !::is_proc_shutoff() ) {
::fprintf(stderr, "\n");
::fprintf(stderr, " => create ssm-data \"\x1b[4m%s\x1b[0m\"\n", pconf.output_ssmname.value );
ssmk_odometry.data.x = 0;
ssmk_odometry.data.y = 0;
ssmk_odometry.data.theta = 0;
ssmk_odometry.data.v = 0;
ssmk_odometry.data.w = 0;
if( !ssmk_odometry.create( pconf.output_ssmname.value, pconf.output_ssmid.value, 5, 0.005 ) ) {
::fprintf(stderr, " \x1b[1m\x1b[31mERROR\x1b[39m\x1b[0m: fail to create \"\x1b[4m%s\x1b[0m\"\n", pconf.output_ssmname.value );
::proc_shutoff();
}
else {
// ssmlog_odometry.write();
::fprintf(stderr, " ... \x1b[1mOK\x1b[0m\n");
}
} // <--- ssm gyro-odometry open
if( !::is_proc_shutoff() && pconf.spur_adjust.value && ::strcmp(pconf.output_ssmname.value, SNAME_ADJUST) ) {
::fprintf(stderr, "\n");
::fprintf(stderr, " => create ssm-data \"\x1b[4m%s\x1b[0m\"\n", SNAME_ADJUST );
ssmlog_adjust.data.x = 0;
ssmlog_adjust.data.y = 0;
ssmlog_adjust.data.theta = 0;
ssmlog_adjust.data.v = 0;
ssmlog_adjust.data.w = 0;
if( !ssmlog_adjust.create( SNAME_ADJUST, 0, 5, 0.005 ) ) {
::fprintf(stderr, " \x1b[1m\x1b[31mERROR\x1b[39m\x1b[0m: fail to create \"\x1b[4m%s\x1b[0m\"\n", pconf.output_ssmname.value );
::proc_shutoff();
}
else {
ssmlog_adjust.write();
::fprintf(stderr, " ... \x1b[1mOK\x1b[0m\n");
}
}
// ---> ssm ad conversion open
if( !::is_proc_shutoff() && pconf.gyrodometry.value ) {
::fprintf(stderr, "\n");
::fprintf(stderr, " => open ssm-data \"\x1b[4m%s\x1b[0m\"\n", pconf.ad_ssmname.value );
if( !ssmlog_ad.openWait(pconf.ad_ssmname.value, pconf.ad_ssmid.value, 0.0, SSM_READ) ) {
::fprintf(stderr, " \x1b[1m\x1b[31mERROR\x1b[39m\x1b[0m: fail to open \"\x1b[4m%s\x1b[0m\"\n", pconf.ad_ssmname.value );
::proc_shutoff();
}
else {
::fprintf(stderr, " ... \x1b[1mOK\x1b[0m\n");
}
} // <--- ssm ad conversion open
// ---> ssm ad conversion open
if( !::is_proc_shutoff() && pconf.odmerr_id.value >= 0 ) {
::fprintf(stderr, "\n");
::fprintf(stderr, " => open ssm-data \"\x1b[4m%s\x1b[0m\"\n", pconf.odmerr_name.value );
if( !ssmlog_odmerr.open(pconf.odmerr_name.value) ) {
::fprintf(stderr, " \x1b[1m\x1b[31mERROR\x1b[39m\x1b[0m: fail to open \"\x1b[4m%s\x1b[0m\"\n", pconf.odmerr_name.value );
::proc_shutoff();
}
else {
::fprintf(stderr, " ... \x1b[1mOK\x1b[0m\n");
}
} // <--- ssm ad conversion open
// ---> ssm pws-motor open
if( !::is_proc_shutoff() ) {
::fprintf(stderr, "\n");
::fprintf(stderr, " => open ssm-log \"\x1b[4m%s\x1b[0m\"\n", pconf.motor_ssmname.value );
if( !ssmlog_mtr.open( pconf.motor_ssmname.value ) ) {
::fprintf(stderr, " \x1b[1m\x1b[31mERROR\x1b[39m\x1b[0m: fail to open \"\x1b[4m%s\x1b[0m\"\n", pconf.motor_ssmname.value );
::proc_shutoff();
}
else {
::fprintf(stderr, " ... \x1b[1mOK\x1b[0m\n");
}
} // <--- ssm pws-motor open
// initialize cui
cuireader.set_command( Odometer::cui_cmd, sizeof( Odometer::cui_cmd ) / sizeof( Odometer::cui_cmd[0] ));
if(pconf.debug.value) {
fp = ::fopen(__Debug_Log__, "w");
::fprintf(fp, "#[1.time] [2.x] [3.y] [4.theta] [5.v] [6.w] [7.ad] [8.bias] [9.sf]\n");
}
} // <--- initialize
{ // ---> operation
bool show_st = true;
ssmTimeT init_time = 0;
ssmTimeT prev_time = -1;
double cuito = 0;
double bias = pconf.bias.value;
double bias_update_ratio = 0.01;
bool error_generate = false;
init_time = ssmlog_mtr.time;
if(pconf.error_simulation.value > 0.0) { // ---> error simulation initialize
gnd::random_set_seed();
error_generate = pconf.error_distributuion.value[0] || pconf.error_distributuion.value[1] || pconf.error_distributuion.value[2];
} // <--- error simulation initialize
// ---> while
while( !::is_proc_shutoff() ) {
// ---> read encoder counter
if( ssmlog_mtr.readNext() ) {
double v, w;
double dt;
double vol;
if( prev_time < 0 || ssmlog_mtr.time <= prev_time) {
prev_time = ssmlog_mtr.time;
continue;
}
dt = ssmlog_mtr.time - prev_time;
{ // ---> compute odometry
double wr = ( 2.0 * odm_prop.radius_r * M_PI * ( (double) ssmlog_mtr.data.counter2 ) ) / ( odm_prop.count_rev * odm_prop.gear );
double wl = ( 2.0 * odm_prop.radius_l * M_PI * ( (double) ssmlog_mtr.data.counter1 ) ) / ( odm_prop.count_rev * odm_prop.gear );
v = (wr + wl) / 2 / dt;
w = (wr - wl) / odm_prop.tread / dt;
} // <--- compute odometry
// ---> compute angular velocity with gyro-sensor
if( pconf.gyrodometry.value ) {
// read A/D
ssmlog_ad.readTime( ssmlog_mtr.time );
vol = ssmlog_ad.data.ad[pconf.ratio_port.value] * pconf.voltage.value / (1 << pconf.ad_bits.value);
// in the case of just about stationary
// compute bias
if( ::abs(ssmlog_mtr.data.counter1) ==0 && ::abs(ssmlog_mtr.data.counter2) ==0 ) {
bias += ( vol - bias ) * bias_update_ratio;
}
else {
w = -gnd_deg2ang( 1.0 / pconf.scale_factor.value ) * (vol - bias);
}
} // <--- compute angular velocity with gyro-sensor
{ // ---> transration
ssmk_odometry.data.v = v;
ssmk_odometry.data.w = w;
ssmk_odometry.data.x += v * dt * ::cos(ssmk_odometry.data.theta);
ssmk_odometry.data.y += v * dt * ::sin(ssmk_odometry.data.theta);
ssmk_odometry.data.theta += w * dt;
// ---> error simulation
if( error_generate && pconf.error_simulation.value * v * dt > gnd::random_uniform() ) {
gnd::matrix::fixed<3, 1> ws, error;
gnd::matrix::fixed<3, 3> error_distribution;
gnd::matrix::set_zero(&error_distribution);
error_distribution[0][0] = pconf.error_distributuion.value[0] * pconf.error_distributuion.value[0];
error_distribution[1][1] = pconf.error_distributuion.value[1] * pconf.error_distributuion.value[1];
error_distribution[2][2] = gnd_deg2ang(pconf.error_distributuion.value[2]) * gnd_deg2ang(pconf.error_distributuion.value[2]);
gnd::random_gaussian_mult( &error_distribution, 3, &ws, &error );
ssmk_odometry.data.x += v * error[0][0];
ssmk_odometry.data.y += v * error[1][0];
ssmk_odometry.data.theta += error[2][0];
fprintf(fp, "#error %lf %lf %lf %lf\n", ssmk_odometry.time - init_time,
error[0][0], error[1][0], error[2][0]);
} // <--- error simulation
ssmk_odometry.write( ssmlog_mtr.time );
prev_time = ssmlog_mtr.time;
if( ssmlog_adjust.isOpen() ) {
ssmlog_adjust.data = ssmk_odometry.data;
ssmlog_adjust.write( ssmlog_mtr.time );
}
// debug log
if(pconf.debug.value && fp) {
fprintf(fp, "%lf %lf %lf %lf %lf %lf %d %lf\n", ssmk_odometry.time - init_time,
ssmk_odometry.data.x, ssmk_odometry.data.y, ssmk_odometry.data.theta,
ssmk_odometry.data.v, ssmk_odometry.data.w,
ssmlog_ad.data.ad[pconf.ratio_port.value], bias);
}
} // <--- transration
} // <--- read encoder counter
while( !::is_proc_shutoff() && ssmlog_odmerr.readNext() ) { // ---> marge odm error
ssmk_odometry.data.x -= ssmlog_odmerr.data.dx;
ssmk_odometry.data.y -= ssmlog_odmerr.data.dy;
if( !pconf.gyrodometry.value ) {
ssmk_odometry.data.theta -= ssmlog_odmerr.data.dtheta;
}
ssmk_odometry.write( ssmlog_mtr.time );
if( ssmlog_adjust.isOpen() ) {
ssmlog_adjust.data = ssmk_odometry.data;
ssmlog_adjust.write( ssmlog_mtr.time );
}
// debug log
if(pconf.debug.value && fp) {
fprintf(fp, "%lf %lf %lf %lf %lf %lf %d %lf # fix\n", ssmk_odometry.time - init_time,
ssmlog_odometry.data.x, ssmlog_odometry.data.y, ssmlog_odometry.data.theta,
ssmlog_odometry.data.v, ssmlog_odometry.data.w,
ssmlog_ad.data.ad[pconf.ratio_port.value],
bias);
}
} // ---> marge odm error
{ // ---> cui
int cuival = 0;
char cuiarg[512];
::memset(cuiarg, 0, sizeof(cuiarg));
if( cuireader.poll(&cuival, cuiarg, sizeof(cuiarg), cuito) > 0 ) {
if( show_st ) {
// if show status mode, quit show status mode
show_st = false;
::fprintf(stderr, "-------------------- cui mode --------------------\n");
}
else {
switch(cuival) {
// exit
case 'Q': ::proc_shutoff();
break;
// help
case 'h':
cuireader.show(stderr, " ");
break;
// show status
case 's':
show_st = true;
break;
// stand-by mode
case 'B':
cuito = -1;
break;
// start
case 'o':
cuito = 0;
break;
// debug log-mode
case 'D': {
::fprintf(stderr, " => debug-log mode\n");
if( ::strncmp("on", cuiarg, 2) == 0) {
bool flg = fp ? true : false;
if( !fp && !(fp = fopen(__Debug_Log__, "w")) ) {
::fprintf(stderr, " ... \x1b[1m\x1b[31mError\x1b[39m\x1b[0m: fail to open \"\x1b[4m%s\x1b[0m\"", __Debug_Log__);
pconf.debug.value = false;
}
else if( flg ) {
fprintf(fp, "# 1.[time] 2.[x] 3.[y] 4.[theta] 5.[v] 6.[w] 7.[wh mean] 8.[wh ratio] 9.[trd ratio] 10.[pwm1] 11.[pwm2] 12.[counter1] 13.[counter2]\n");
pconf.debug.value = true;
::fprintf(stderr, " ... \x1b[1mon\x1b[0m\n");
}
else {
pconf.debug.value = true;
::fprintf(stderr, " ... \x1b[1mon\x1b[0m\n");
}
}
else if( ::strncmp("off", cuiarg, 3) == 0) {
pconf.debug.value = false;
::fprintf(stderr, " ... \x1b[1moff\x1b[0m\n");
}
else {
::fprintf(stderr, " ... %s\n", pconf.debug.value ? "on": "off");
::fprintf(stderr, " if you want to change mode, input \"on/off\"\n");
}
}
break;
case '\0':
break;
default:
::fprintf(stderr, " ... \x1b[31m\x1b[1mError\x1b[0m\x1b[39m: invalid command\n");
::fprintf(stderr, " Please input \x1b[4mhelp\x1b[0m/\x1b[4mh\x1b[0m to show command\n");
break;
}
}
::fprintf(stderr, " \x1b[33m\x1b[1m%s\x1b[0m\x1b[39m > ", Odometer::proc_name);
cuireader.poll(&cuival, cuiarg, sizeof( cuiarg ), 0);
}
}// ---> cui
if( show_st ) { // ---> show status
struct timespec cur;
static struct timespec next;
clock_gettime(CLOCK_REALTIME, &cur);
if( cur.tv_sec > next.tv_sec ||
( cur.tv_sec == next.tv_sec && cur.tv_nsec > next.tv_nsec )) {
::fprintf(stderr, "\x1b[0;0H\x1b[2J"); // display clear
::fprintf(stderr, "-------------------- \x1b[33m\x1b[1m%s\x1b[0m\x1b[39m --------------------\n", Odometer::proc_name);
::fprintf(stderr, " position : %.02lf[m] %.02lf[m] %.01lf[deg]\n", ssmlog_odometry.data.x, ssmlog_odometry.data.y, gnd_ang2deg(ssmlog_odometry.data.theta) );
::fprintf(stderr, " velocity : v %.02lf[m/s] w %.03lf[deg/s]\n", ssmlog_odometry.data.v, gnd_ang2deg(ssmlog_odometry.data.w) );
::fprintf(stderr, " mode : %s\n", pconf.gyrodometry.value ? "gyrodometry" : "odometry" );
::fprintf(stderr, "\n");
::fprintf(stderr, " Push \x1b[1mEnter\x1b[0m to change CUI Mode\n");
next = cur;
next.tv_sec++;
}
} // <--- show status
// stand-by mode
if( cuito < 0) continue;
} // <--- while
} // <--- operation
{ // ---> finalize
::endSSM();
::fprintf(stderr, "\n");
::fprintf(stderr, "Finish.\x1b[49m\n");
} // <--- finalize
return 0;
}
int main ()
{
struct sockaddr_in dstAddr;
int dstSock;
char c;
char raw_start_buf[26];
char raw_line_buf[20][1113];
char raw_end_buf[33];
char *line_buf[22];
char str_buf[5];
char det;
long dist[20][137];
int n;
int numrcv;
char *toSendText1 = "#TD2GET\n";
char *toSendText2 = "#TD2STOP\n";
FILE *fp; //ファイルポインタ
int i,j,k;
//-角度用-//
long U_x_beta[20][136];
long U_y_beta[20][136];
long D_x_beta[20][136];
long D_y_beta[20][136];
double U_angle_x[20][136];
double U_angle_y[20][136];
double D_angle_x[20][136];
double D_angle_y[20][136];
//-時間計測用-//
double time_1; //時間計測用
double time_2; //時間計測用
double time_3; //時間計測用
//-座標値-//
double x;
double angle_convert(long angle); //プロトタイプ宣言
void calc_xyz(double angle_x, double angle_y, long dist, double *x, double *y, double *z); //プロトタイプ宣言
double get_time(void); //プロトタイプ宣言
//-SSM-//
LS3D SCAN_DATA;
initSSM();
SSM_sid LS3D_sid;
LS3D_sid = createSSM_time("LS3D", 0, sizeof(LS3D), 1.0, 0.025);
//-SCAN_DATAの初期化-//
// for(i=0; i<2880; i++)
for(i=0; i<STEP_NUM; i++)
{
SCAN_DATA.det = 'U';
SCAN_DATA.dist[i] = 0.0;
SCAN_DATA.x[i] = 0.0;
SCAN_DATA.y[i] = 0.0;
SCAN_DATA.z[i] = 0.0;
}
//-----------角度データ読み込み-----------//
//-ファイルオープン-//
fp = fopen("beta_data", "r");
for(j=0; j<20; j++)
{
for(i=0; i<136; i++)
{
fscanf(fp, "%ld", &U_x_beta[j][i]);
U_angle_x[j][i] = angle_convert(U_x_beta[j][i]);
}
}
for(j=0; j<20; j++)
{
for(i=0; i<136; i++)
{
fscanf(fp, "%ld", &U_y_beta[j][i]);
U_angle_y[j][i] = angle_convert(U_y_beta[j][i]);
}
}
for(j=0; j<20; j++)
{
for(i=0; i<136; i++)
{
fscanf(fp, "%ld", &D_x_beta[j][i]);
D_angle_x[j][i] = angle_convert(D_x_beta[j][i]);
}
}
for(j=0; j<20; j++)
{
for(i=0; i<136; i++)
{
fscanf(fp, "%ld", &D_y_beta[j][i]);
D_angle_y[j][i] = angle_convert(D_y_beta[j][i]);
}
}
//-----------距離データ取得ループ-----------//
//-ソケットの作成-//
dstSock = socket(AF_INET, SOCK_STREAM, 0);
//-接続先指定用構造体の準備-//
dstAddr.sin_family = AF_INET;
dstAddr.sin_port = htons(10940);
dstAddr.sin_addr.s_addr = inet_addr("192.168.0.10");
//-コネクト-//
connect(dstSock, (struct sockaddr *)&dstAddr, sizeof(dstAddr));
//-サーバにメッセージを送信-//
memset(&dstAddr, 0, sizeof(dstAddr));
send(dstSock, toSendText1, strlen(toSendText1)+1, 0);
sleep(1);
printf("sending...\n");
//-サーバからデータを受信-//
memset(raw_start_buf, 0, sizeof(raw_start_buf));
memset(raw_line_buf, 0, sizeof(raw_line_buf));
memset(raw_end_buf, 0, sizeof(raw_end_buf));
while(1){ //距離データ取得ループ
//time_1 = get_time();
for(i=0; i<26; i++)
{
numrcv = recv(dstSock, &c, 1, 0);
if(c=='\n')
{
raw_start_buf[i] = '\0';
break;
}
raw_start_buf[i] = c;
det = raw_start_buf[19]; //ループの外だとなぜか文字が消える
}
for(j=0; j<20; j++)
{
for(i=0; i<1177; i++)
{
numrcv = recv(dstSock, &c, 1, 0);
if(c=='\n'){
raw_line_buf[j][i] = '\0';
break;
}
raw_line_buf[j][i] = c;
}
}
for(i=0; i<33; i++)
{
numrcv = recv(dstSock, &c, 1, 0);
if(c=='\n')
{
raw_end_buf[i] = '\0';
break;
}
raw_end_buf[i] = c;
}
//-距離データを抽出-//
for(i=0; i<20; i++)
{
char *tp;
tp = strtok(&raw_line_buf[i][0], "#:");
for(j=0; j<3; j++)
{
tp = strtok(NULL, "#:");
}
line_buf[i] = tp;
}
for(i=0; i<20; i++)
{
if(line_buf[i] != NULL)
{
for(j=0; j<136; j++)
{
str_buf[0] = *(line_buf[i] + 8*j);
str_buf[1] = *(line_buf[i] + 8*j + 1);
str_buf[2] = *(line_buf[i] + 8*j + 2);
str_buf[3] = *(line_buf[i] + 8*j + 3);
str_buf[4] = '\0';
dist[i][j] = strtol(str_buf, NULL, 16);
}
}
}
//-xyz座標値に変換-//
if(det == 'U')
{
SCAN_DATA.det = 'U';
for(i=0; i<20; i++)
{
for(j=0; j<136; j++)
{
calc_xyz(U_angle_x[i][j], U_angle_y[i][j], dist[i][j], &SCAN_DATA.x[i*136 + j], &SCAN_DATA.y[i*136 + j], &SCAN_DATA.z[i * 136 + j]);
}
}
}else if(det == 'D'){
SCAN_DATA.det = 'D';
for(i=0; i<20; i++)
{
for(j=0; j<136; j++)
{
calc_xyz(D_angle_x[i][j], D_angle_y[i][j], dist[i][j], &SCAN_DATA.x[i*136 + j], &SCAN_DATA.y[i*136 + j], &SCAN_DATA.z[i * 136 + j]);
}
}
}
//-SSM書き込み-//
for(i=0; i<20; i++)
{
for(j=0; j<136; j++)
{
SCAN_DATA.dist[136*i + j] = dist[i][j];
}
}
printf("%c\n", SCAN_DATA.det);
writeSSM(LS3D_sid, (char*)&SCAN_DATA, gettimeSSM());
usleep(1000);
//time_2 = get_time();
//usleep(freq*1000000 - (time_2 - time_1)*1000000);
//time_3 = get_time();
} //距離データ取得ループ
//-計測停止要求-//
send(dstSock, toSendText2, strlen(toSendText2)+1, 0);
//-socketの終了-//
close(dstSock);
return 0;
}
