ssm_spur_handler.c
Go to the documentation of this file.
1 // Copyright (c) 2010-2016 The YP-Spur Authors, except where otherwise indicated.
2 //
3 // Permission is hereby granted, free of charge, to any person obtaining a copy
4 // of this software and associated documentation files (the "Software"), to
5 // deal in the Software without restriction, including without limitation the
6 // rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
7 // sell copies of the Software, and to permit persons to whom the Software is
8 // furnished to do so, subject to the following conditions:
9 //
10 // The above copyright notice and this permission notice shall be included in
11 // all copies or substantial portions of the Software.
12 //
13 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
14 // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
15 // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
16 // THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
17 // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
18 // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
19 // SOFTWARE.
20 
21 #include <stdio.h>
22 #include <string.h>
23 
24 #ifdef HAVE_CONFIG_H
25 #include <config.h>
26 #endif // HAVE_CONFIG_H
27 
28 /* SSM 用 */
29 #ifdef HAVE_SSM
30 #include <ssm.h>
31 #include "ssmtype/spur-odometry.h"
32 #include "ssmtype/pws-motor.h"
33 #include "ssmtype/ypspur-ad.h"
34 #endif // HAVE_SSM
35 
36 /* yp-spur用 */
37 #include "odometry.h"
38 #include "utility.h"
39 #include "ypspur.h"
40 #include "param.h"
41 #include "shvel-param.h"
42 #include "ssm_spur_handler.h"
43 
44 #ifdef HAVE_SSM
45 SSM_sid g_odm_bs_sid = 0, g_odm_sid = 0, g_motor_sid = 0, g_odm_adj_sid = 0, g_ad_sid = 0;
46 int g_ssm_enable;
47 int g_ssm_adj_enable = 0;
48 int g_ssm_id = 0;
49 #endif // HAVE_SSM
50 
51 /* SSM の初期化 */
52 void init_ypspurSSM(int ssm_id)
53 {
54 #ifdef HAVE_SSM
55  g_ssm_id = ssm_id;
56 
57  yprintf(OUTPUT_LV_INFO, " with SSM\n");
58  if (!initSSM())
59  {
60  /* SSMが起動していない */
61  yprintf(OUTPUT_LV_ERROR, "\n SSM is not available.\n");
62  g_ssm_enable = 0;
63  }
64  else
65  {
66  g_ssm_enable = 1;
67  g_odm_bs_sid = createSSM_time(SNAME_ODOMETRY, ssm_id, sizeof(Spur_Odometry), 5, 0.005);
68  g_odm_sid = createSSM_time(SNAME_GLOBAL, ssm_id, sizeof(Spur_Odometry), 5, 0.005);
69  g_motor_sid = createSSM_time(SNAME_PWS_MOTOR, ssm_id, sizeof(PWSMotor), 1, 0.005);
70  g_ad_sid = createSSM_time(SNAME_YPSPUR_AD, ssm_id, sizeof(YP_ad), 1, 0.005);
71  if (!(g_odm_bs_sid && g_odm_sid && g_motor_sid && g_ad_sid))
72  {
73  g_ssm_enable = 0;
74  yprintf(OUTPUT_LV_WARNING, "\nWarning : cannot create ssm.\n");
75  }
76  }
77  g_ssm_adj_enable = 0;
78 #endif // HAVE_SSM
79 }
80 
82 {
83 #ifdef HAVE_SSM
84  if (g_ssm_enable)
85  {
86  releaseSSM(&g_odm_bs_sid);
87  releaseSSM(&g_odm_sid);
88  releaseSSM(&g_motor_sid);
89  releaseSSM(&g_ad_sid);
90  releaseSSM(&g_odm_adj_sid);
91  endSSM();
92  }
93 #endif // HAVE_SSM
94 }
95 
96 void write_ypspurSSM(int odometry_updated, int receive_count,
97  Odometry *odm_log, int readdata_num, Short_2Char *cnt1_log, Short_2Char *cnt2_log,
98  Short_2Char *pwm1_log, Short_2Char *pwm2_log, int ad_log[][8])
99 {
100 #ifdef HAVE_SSM
101  if (g_ssm_enable)
102  {
103  int i;
104  PWSMotor motor;
105  Spur_Odometry odm;
106  YP_ad ad;
107  ssmTimeT time;
108  int time_offset;
109  /* SSMへの出力 */
110  time_offset = receive_count - odometry_updated + 1;
111  for (i = 0; i < odometry_updated; i++)
112  {
113  time = time_estimate(time_offset + i);
114  /* BSの出力 */
115  odm.x = odm_log[i].x;
116  odm.y = odm_log[i].y;
117  odm.theta = odm_log[i].theta;
118  odm.v = odm_log[i].v;
119  odm.w = odm_log[i].w;
120  writeSSM(g_odm_bs_sid, &odm, time);
121  /* GLの出力 */
123  writeSSM(g_odm_sid, &odm, time);
124  }
125  /* PWM,カウンタ値,AD値の出力 */
126  time_offset = receive_count - readdata_num + 1;
127  for (i = 0; i < readdata_num; i++)
128  {
129  time = time_estimate(time_offset + i);
130  motor.counter1 = cnt1_log[i].integer;
131  motor.counter2 = cnt2_log[i].integer;
132  motor.pwm1 = pwm1_log[i].integer;
133  motor.pwm2 = pwm2_log[i].integer;
134  memcpy(ad.ad, ad_log[i], sizeof(int) * 8);
135  writeSSM(g_motor_sid, &motor, time);
136  writeSSM(g_ad_sid, &ad, time);
137  }
138  }
139 #endif // HAVE_SSM
140 }
141 
142 /* オドメトリ修正情報との融合 */
144 {
145 #ifdef HAVE_SSM
146  static double before_time;
147  double now_time, time;
148  Odometry bs_odometry, adj_odometry, target_pos;
149  CoordinateSystem bs_cs, adj_cs;
150  int tid;
151  if (g_ssm_enable)
152  { /* 20ms毎? */
153  if (!g_ssm_adj_enable)
154  { /* SSMcheck */
155  now_time = get_time();
156  if (now_time > before_time + 1)
157  {
158  g_odm_adj_sid = openSSM(SNAME_ADJUST, 0, 0);
159  if (g_odm_adj_sid > 0)
160  { // openできた
161  g_ssm_adj_enable = 1;
162  yprintf(OUTPUT_LV_INFO, "SSMInfo: Find adjust information.\n");
163  }
164  else
165  {
166  g_ssm_adj_enable = 0;
167  }
168  before_time = now_time;
169  }
170  }
171  else
172  {
173  /* パラメータの変更がおこらないようにブロック */
174  pthread_mutex_lock(&spur->mutex);
175  // 最新の修正位置
176  if ((tid = readSSM(g_odm_adj_sid, (char *)&adj_odometry, &now_time, -1)) >= 0)
177  {
178  // 同時刻のGL座標
179  if (now_time > get_time() - 1)
180  {
181  if ((tid = readSSM_time(g_odm_bs_sid, (char *)&bs_odometry, now_time, &time)) >= 0)
182  {
183  // 時間が1秒以内(止まっていない)で、データがあるなら実行
184  /* 座標系作成 */
185  bs_cs.x = bs_odometry.x;
186  bs_cs.y = bs_odometry.y;
187  bs_cs.theta = bs_odometry.theta;
188  adj_cs.x = adj_odometry.x;
189  adj_cs.y = adj_odometry.y;
190  adj_cs.theta = adj_odometry.theta;
191  /* 修正後の位置を計算 */
192  target_pos = *odm; /* 最新の位置 */
193  /* 現在位置との微妙な差を計算 */
194  trans_cs(&bs_cs, &target_pos.x, &target_pos.y, &target_pos.theta);
195  /* 微妙な差を付け加える */
196  inv_trans_cs(&adj_cs, &target_pos.x, &target_pos.y, &target_pos.theta);
197 
198  double data[3] = { target_pos.x, target_pos.y, target_pos.theta };
199  set_adjust_com(CS_GL, data, spur);
200  }
201  }
202  }
203  /* 保護終わり */
204  pthread_mutex_unlock(&spur->mutex);
205  }
206  }
207 #endif // HAVE_SSM
208 }
void init_ypspurSSM(int ssm_id)
double theta
Definition: cartesian2d.h:38
void coordinate_synchronize(Odometry *odm, SpurUserParamsPtr spur)
void inv_trans_cs(CSptr target_cs, double *x, double *y, double *theta)
Definition: libcarte2d.c:173
double y
Definition: odometry.h:34
void trans_cs(CSptr target_cs, double *x, double *y, double *theta)
Definition: libcarte2d.c:187
void CS_recursive_trans(CSptr target_cs, CSptr now_cs, double *x, double *y, double *theta)
Definition: libcarte2d.c:137
double get_time(void)
Definition: utility.c:45
int counter2
Definition: pws-motor.h:30
Definition: ypparam.h:492
void set_adjust_com(int cs, double *data, SpurUserParamsPtr spur)
Definition: command_set.c:91
short integer
Definition: shvel-param.h:34
#define SNAME_GLOBAL
Definition: spur-odometry.h:25
static YPSpur spur
Definition: libypspur.c:42
double y
Definition: cartesian2d.h:37
int ad[16]
Definition: adinput.c:48
CSptr get_cs_pointer(YPSpur_cs cs)
Definition: odometry.c:103
double w
Definition: odometry.h:37
int ad[8]
Definition: ypspur-ad.h:28
double v
Definition: odometry.h:36
#define SNAME_ADJUST
Definition: spur-odometry.h:26
#define SNAME_ODOMETRY
Definition: spur-odometry.h:24
double x
Definition: cartesian2d.h:36
int pwm2
Definition: pws-motor.h:32
void end_ypspurSSM()
double theta
Definition: odometry.h:35
void write_ypspurSSM(int odometry_updated, int receive_count, Odometry *odm_log, int readdata_num, Short_2Char *cnt1_log, Short_2Char *cnt2_log, Short_2Char *pwm1_log, Short_2Char *pwm2_log, int ad_log[][8])
void yprintf(ParamOutputLv level, const char *format,...)
Definition: yprintf.c:32
pthread_mutex_t mutex
Definition: command.h:88
int counter1
Definition: pws-motor.h:29
#define SNAME_PWS_MOTOR
Definition: pws-motor.h:24
int pwm1
Definition: pws-motor.h:31
double time_estimate(int readnum)
時刻の推定 (n回目の計測結果の時刻を計算する)
Definition: odometry.c:375
Definition: ypparam.h:490
double x
Definition: odometry.h:33
#define SNAME_YPSPUR_AD
Definition: ypspur-ad.h:24


yp-spur
Author(s):
autogenerated on Fri May 7 2021 02:12:17