ssm_spur_handler.c
Go to the documentation of this file.
00001 // Copyright (c) 2010-2016 The YP-Spur Authors, except where otherwise indicated.
00002 //
00003 // Permission is hereby granted, free of charge, to any person obtaining a copy
00004 // of this software and associated documentation files (the "Software"), to
00005 // deal in the Software without restriction, including without limitation the
00006 // rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
00007 // sell copies of the Software, and to permit persons to whom the Software is
00008 // furnished to do so, subject to the following conditions:
00009 //
00010 // The above copyright notice and this permission notice shall be included in
00011 // all copies or substantial portions of the Software.
00012 //
00013 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
00014 // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
00015 // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
00016 // THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
00017 // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00018 // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
00019 // SOFTWARE.
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 /* SSM 用 */
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 /* yp-spur用 */
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 /* SSM の初期化 */
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     /* SSMが起動していない */
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     /* SSMへの出力 */
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       /* BSの出力 */
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       /* GLの出力 */
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     /* PWM,カウンタ値,AD値の出力 */
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   { /* 20ms毎? */
00153     if (!g_ssm_adj_enable)
00154     { /* SSMcheck */
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         {  // openできた
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         // 同時刻のGL座標
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             // 時間が1秒以内(止まっていない)で、データがあるなら実行
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 }


yp-spur
Author(s):
autogenerated on Fri May 10 2019 02:52:19