00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021 #include <stdio.h>
00022 #include <string.h>
00023
00024 #ifdef HAVE_CONFIG_H
00025 #include <config.h>
00026 #endif // HAVE_CONFIG_H
00027
00028
00029 #ifdef HAVE_SSM
00030 #include <ssm.h>
00031 #include "ssmtype/spur-odometry.h"
00032 #include "ssmtype/pws-motor.h"
00033 #include "ssmtype/ypspur-ad.h"
00034 #endif // HAVE_SSM
00035
00036
00037 #include "odometry.h"
00038 #include "utility.h"
00039 #include "ypspur.h"
00040 #include "param.h"
00041 #include "shvel-param.h"
00042 #include "ssm_spur_handler.h"
00043
00044 #ifdef HAVE_SSM
00045 SSM_sid g_odm_bs_sid = 0, g_odm_sid = 0, g_motor_sid = 0, g_odm_adj_sid = 0, g_ad_sid = 0;
00046 int g_ssm_enable;
00047 int g_ssm_adj_enable = 0;
00048 int g_ssm_id = 0;
00049 #endif // HAVE_SSM
00050
00051
00052 void init_ypspurSSM(int ssm_id)
00053 {
00054 #ifdef HAVE_SSM
00055 g_ssm_id = ssm_id;
00056
00057 yprintf(OUTPUT_LV_INFO, " with SSM\n");
00058 if (!initSSM())
00059 {
00060
00061 yprintf(OUTPUT_LV_ERROR, "\n SSM is not available.\n");
00062 g_ssm_enable = 0;
00063 }
00064 else
00065 {
00066 g_ssm_enable = 1;
00067 g_odm_bs_sid = createSSM_time(SNAME_ODOMETRY, ssm_id, sizeof(Spur_Odometry), 5, 0.005);
00068 g_odm_sid = createSSM_time(SNAME_GLOBAL, ssm_id, sizeof(Spur_Odometry), 5, 0.005);
00069 g_motor_sid = createSSM_time(SNAME_PWS_MOTOR, ssm_id, sizeof(PWSMotor), 1, 0.005);
00070 g_ad_sid = createSSM_time(SNAME_YPSPUR_AD, ssm_id, sizeof(YP_ad), 1, 0.005);
00071 if (!(g_odm_bs_sid && g_odm_sid && g_motor_sid && g_ad_sid))
00072 {
00073 g_ssm_enable = 0;
00074 yprintf(OUTPUT_LV_WARNING, "\nWarning : cannot create ssm.\n");
00075 }
00076 }
00077 g_ssm_adj_enable = 0;
00078 #endif // HAVE_SSM
00079 }
00080
00081 void end_ypspurSSM()
00082 {
00083 #ifdef HAVE_SSM
00084 if (g_ssm_enable)
00085 {
00086 releaseSSM(&g_odm_bs_sid);
00087 releaseSSM(&g_odm_sid);
00088 releaseSSM(&g_motor_sid);
00089 releaseSSM(&g_ad_sid);
00090 releaseSSM(&g_odm_adj_sid);
00091 endSSM();
00092 }
00093 #endif // HAVE_SSM
00094 }
00095
00096 void write_ypspurSSM(int odometry_updated, int receive_count,
00097 Odometry *odm_log, int readdata_num, Short_2Char *cnt1_log, Short_2Char *cnt2_log,
00098 Short_2Char *pwm1_log, Short_2Char *pwm2_log, int ad_log[][8])
00099 {
00100 #ifdef HAVE_SSM
00101 if (g_ssm_enable)
00102 {
00103 int i;
00104 PWSMotor motor;
00105 Spur_Odometry odm;
00106 YP_ad ad;
00107 ssmTimeT time;
00108 int time_offset;
00109
00110 time_offset = receive_count - odometry_updated + 1;
00111 for (i = 0; i < odometry_updated; i++)
00112 {
00113 time = time_estimate(time_offset + i);
00114
00115 odm.x = odm_log[i].x;
00116 odm.y = odm_log[i].y;
00117 odm.theta = odm_log[i].theta;
00118 odm.v = odm_log[i].v;
00119 odm.w = odm_log[i].w;
00120 writeSSM(g_odm_bs_sid, &odm, time);
00121
00122 CS_recursive_trans(get_cs_pointer(CS_GL), get_cs_pointer(CS_BS), &odm.x, &odm.y, &odm.theta);
00123 writeSSM(g_odm_sid, &odm, time);
00124 }
00125
00126 time_offset = receive_count - readdata_num + 1;
00127 for (i = 0; i < readdata_num; i++)
00128 {
00129 time = time_estimate(time_offset + i);
00130 motor.counter1 = cnt1_log[i].integer;
00131 motor.counter2 = cnt2_log[i].integer;
00132 motor.pwm1 = pwm1_log[i].integer;
00133 motor.pwm2 = pwm2_log[i].integer;
00134 memcpy(ad.ad, ad_log[i], sizeof(int) * 8);
00135 writeSSM(g_motor_sid, &motor, time);
00136 writeSSM(g_ad_sid, &ad, time);
00137 }
00138 }
00139 #endif // HAVE_SSM
00140 }
00141
00142
00143 void coordinate_synchronize(Odometry *odm, SpurUserParamsPtr spur)
00144 {
00145 #ifdef HAVE_SSM
00146 static double before_time;
00147 double now_time, time;
00148 Odometry bs_odometry, adj_odometry, target_pos;
00149 CoordinateSystem bs_cs, adj_cs;
00150 int tid;
00151 if (g_ssm_enable)
00152 {
00153 if (!g_ssm_adj_enable)
00154 {
00155 now_time = get_time();
00156 if (now_time > before_time + 1)
00157 {
00158 g_odm_adj_sid = openSSM(SNAME_ADJUST, 0, 0);
00159 if (g_odm_adj_sid > 0)
00160 {
00161 g_ssm_adj_enable = 1;
00162 yprintf(OUTPUT_LV_INFO, "SSMInfo: Find adjust information.\n");
00163 }
00164 else
00165 {
00166 g_ssm_adj_enable = 0;
00167 }
00168 before_time = now_time;
00169 }
00170 }
00171 else
00172 {
00173
00174 pthread_mutex_lock(&spur->mutex);
00175
00176 if ((tid = readSSM(g_odm_adj_sid, (char *)&adj_odometry, &now_time, -1)) >= 0)
00177 {
00178
00179 if (now_time > get_time() - 1)
00180 {
00181 if ((tid = readSSM_time(g_odm_bs_sid, (char *)&bs_odometry, now_time, &time)) >= 0)
00182 {
00183
00184
00185 bs_cs.x = bs_odometry.x;
00186 bs_cs.y = bs_odometry.y;
00187 bs_cs.theta = bs_odometry.theta;
00188 adj_cs.x = adj_odometry.x;
00189 adj_cs.y = adj_odometry.y;
00190 adj_cs.theta = adj_odometry.theta;
00191
00192 target_pos = *odm;
00193
00194 trans_cs(&bs_cs, &target_pos.x, &target_pos.y, &target_pos.theta);
00195
00196 inv_trans_cs(&adj_cs, &target_pos.x, &target_pos.y, &target_pos.theta);
00197
00198 double data[3] = { target_pos.x, target_pos.y, target_pos.theta };
00199 set_adjust_com(CS_GL, data, spur);
00200 }
00201 }
00202 }
00203
00204 pthread_mutex_unlock(&spur->mutex);
00205 }
00206 }
00207 #endif // HAVE_SSM
00208 }