vsspdefs.h
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2014, ATR, Atsushi Watanabe
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the copyright holder nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 #ifndef VSSPDEFS_H
31 #define VSSPDEFS_H
32 
33 #include <cstdint>
34 #include <cmath>
35 
36 namespace vssp
37 {
38 static const uint32_t VSSP_MARK = ('V' << 0) | ('S' << 8) | ('S' << 16) | ('P' << 24);
39 static const uint32_t STATUS_OK = ('0' << 0) | ('0' << 8) | ('0' << 16) | (0xA << 24);
40 static const uint32_t STATUS_COMMAND_UNKNOWN = ('1' << 0) | ('0' << 8) | ('1' << 16) | (0xA << 24);
41 static const uint32_t STATUS_COMMAND_INVALID = ('1' << 0) | ('0' << 8) | ('2' << 16) | (0xA << 24);
42 static const uint32_t STATUS_COMMAND_INVALUD_METHOD = ('1' << 0) | ('0' << 8) | ('3' << 16) | (0xA << 24);
43 static const uint32_t STATUS_COMMAND_OUT_OF_RANGE = ('1' << 0) | ('0' << 8) | ('4' << 16) | (0xA << 24);
44 static const uint32_t STATUS_COMMUNICATION_FAILURE = ('2' << 0) | ('0' << 8) | ('1' << 16) | (0xA << 24);
45 static const uint32_t STATUS_UNKNOWN_ERROR = ('3' << 0) | ('0' << 8) | ('1' << 16) | (0xA << 24);
46 
47 static const uint32_t TYPE_GET = ('G' << 0) | ('E' << 8) | ('T' << 16) | (':' << 24);
48 static const uint32_t TYPE_SET = ('S' << 0) | ('E' << 8) | ('T' << 16) | (':' << 24);
49 static const uint32_t TYPE_DAT = ('D' << 0) | ('A' << 8) | ('T' << 16) | (':' << 24);
50 static const uint32_t TYPE_VER = ('V' << 0) | ('E' << 8) | ('R' << 16) | (':' << 24);
51 static const uint32_t TYPE_PNG = ('P' << 0) | ('N' << 8) | ('G' << 16) | (':' << 24);
52 static const uint32_t TYPE_ERR = ('E' << 0) | ('R' << 8) | ('R' << 16) | (':' << 24);
53 static const uint32_t TYPE_RI = ('_' << 0) | ('r' << 8) | ('i' << 16) | (':' << 24);
54 static const uint32_t TYPE_RO = ('_' << 0) | ('r' << 8) | ('o' << 16) | (':' << 24);
55 static const uint32_t TYPE_AX = ('_' << 0) | ('a' << 8) | ('x' << 16) | (':' << 24);
56 static const uint32_t TYPE_ER = ('_' << 0) | ('e' << 8) | ('r' << 16) | (':' << 24);
57 
58 enum AuxId
59 {
72 };
73 static const uint32_t AX_MASK_ANGVEL = (1 << AX_MASK_ANGVEL_X) | (1 << AX_MASK_ANGVEL_Y) | (1 << AX_MASK_ANGVEL_Z);
74 static const uint32_t AX_MASK_LINACC = (1 << AX_MASK_LINACC_X) | (1 << AX_MASK_LINACC_Y) | (1 << AX_MASK_LINACC_Z);
75 static const uint32_t AX_MASK_MAG = (1 << AX_MASK_MAG_X) | (1 << AX_MASK_MAG_Y) | (1 << AX_MASK_MAG_Z);
76 
77 #pragma pack(push, 1)
78 struct Header
79 {
80  uint32_t mark;
81  uint32_t type;
82  uint32_t status;
83  uint16_t header_length;
84  uint16_t length;
85  uint32_t received_time_ms;
86  uint32_t send_time_ms;
87 };
89 {
90  uint16_t header_length;
95  uint8_t frame;
96  uint8_t field;
97  uint16_t line;
98  uint16_t spot;
99 };
101 {
102  uint8_t vertical_field;
104 };
106  {
107  0, 1
108  };
110 {
111  uint16_t index_length;
112  uint16_t nspots;
113 };
115 {
116  uint16_t range_mm;
117  uint16_t intensity;
118 };
120 {
121  uint16_t range_mm;
122 };
123 struct AuxHeader
124 {
125  uint16_t header_length;
126  uint32_t timestamp_ms;
127  uint32_t data_bitfield;
128  uint8_t data_count;
129  uint8_t data_ms;
130 };
131 struct AuxData
132 {
133  int32_t val;
134 };
135 #pragma pack(pop)
136 
138 {
139 public:
140  double operator[](AuxId id) const
141  {
142  return k_[static_cast<int>(id)];
143  }
144  double k_[AX_MASK_LAST + 1];
145 };
147 {
148  double s;
149  double c;
150 };
151 struct XYZ
152 {
153  double x;
154  double y;
155  double z;
156 };
157 class XYZI
158 {
159 public:
160  double x;
161  double y;
162  double z;
163  double i;
164  double r;
165 
167  {
168  }
169  XYZI(const double& v_sin, const double& v_cos, const double& h_sin, const double& h_cos)
170  {
171  i = 0;
172  r = 0;
173  x = v_cos * h_cos;
174  y = v_cos * h_sin;
175  z = v_sin;
176  }
177  XYZI operator*(const DataRangeIntensity& data) const
178  {
179  XYZI ret = *this;
180  double r = data.range_mm * 0.001;
181  ret.i = data.intensity;
182  ret.x *= r;
183  ret.y *= r;
184  ret.z *= r;
185  ret.r = r;
186  return ret;
187  }
188  XYZI operator*(const DataRangeOnly& data) const
189  {
190  XYZI ret = *this;
191  double r = data.range_mm * 0.001;
192  ret.i = 0;
193  ret.x *= r;
194  ret.y *= r;
195  ret.z *= r;
196  ret.r = r;
197  return *this;
198  }
199 };
200 class Aux
201 {
202 public:
206  double temp;
207 
208  Aux()
209  {
210  ang_vel.x = ang_vel.y = ang_vel.z = 0.0;
211  lin_acc.x = lin_acc.y = lin_acc.z = 0.0;
212  mag.x = mag.y = mag.z = 0.0;
213  temp = 0.0;
214  }
215  double operator[](AuxId id) const
216  {
217  return operator[](id);
218  }
219  double& operator[](AuxId id)
220  {
221  switch (id)
222  {
224  return ang_vel.x;
226  return ang_vel.y;
228  return ang_vel.z;
230  return lin_acc.x;
232  return lin_acc.y;
234  return lin_acc.z;
235  case vssp::AX_MASK_MAG_X:
236  return mag.x;
237  case vssp::AX_MASK_MAG_Y:
238  return mag.y;
239  case vssp::AX_MASK_MAG_Z:
240  return mag.z;
241  case vssp::AX_MASK_TEMP:
242  return temp;
243  }
244  throw "Invalid AUX data id";
245  }
246 };
247 
248 static const double G = 9.807;
249 static const double DEG = (M_PI / 180.0);
250 
251 // Default value of Aux data scaling factor
252 // note: there is no way to get these values in communication
253 // with prototype edition of the HOKUYO 3D sensor
255  {
256  {
257  1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
258  1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
259  1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
260  1.0, // Temperature [?]
261  0.6e-6, 0.6e-6, 0.6e-6, // Magnetometer [T]
262  G / 8192.0, G / 8192.0, G / 8192.0, // Accelerometer [m/ss]
263  DEG / 65.5, DEG / 65.5, DEG / 65.5 // Gyrometer [rad/s]
264  }
265  };
266 
267 } // namespace vssp
268 
269 #endif // VSSPDEFS_H
static const uint32_t TYPE_PNG
Definition: vsspdefs.h:51
static const uint32_t STATUS_COMMAND_INVALID
Definition: vsspdefs.h:41
static const uint32_t TYPE_SET
Definition: vsspdefs.h:48
Definition: vssp.h:48
uint16_t header_length
Definition: vsspdefs.h:90
static const uint32_t AX_MASK_MAG
Definition: vsspdefs.h:75
uint8_t data_count
Definition: vsspdefs.h:128
double x
Definition: vsspdefs.h:153
static const uint32_t STATUS_COMMUNICATION_FAILURE
Definition: vsspdefs.h:44
uint16_t header_length
Definition: vsspdefs.h:83
double z
Definition: vsspdefs.h:155
double z
Definition: vsspdefs.h:162
static const uint32_t STATUS_COMMAND_OUT_OF_RANGE
Definition: vsspdefs.h:43
static const uint32_t TYPE_AX
Definition: vsspdefs.h:55
uint32_t send_time_ms
Definition: vsspdefs.h:86
static const uint32_t AX_MASK_ANGVEL
Definition: vsspdefs.h:73
static const double G
Definition: vsspdefs.h:248
uint16_t line
Definition: vsspdefs.h:97
double i
Definition: vsspdefs.h:163
XYZI operator*(const DataRangeIntensity &data) const
Definition: vsspdefs.h:177
uint32_t line_head_timestamp_ms
Definition: vsspdefs.h:91
uint32_t mark
Definition: vsspdefs.h:80
uint8_t field
Definition: vsspdefs.h:96
double operator[](AuxId id) const
Definition: vsspdefs.h:140
XYZ lin_acc
Definition: vsspdefs.h:204
XYZ mag
Definition: vsspdefs.h:205
static const double DEG
Definition: vsspdefs.h:249
static const AuxFactorArray AUX_FACTOR_DEFAULT
Definition: vsspdefs.h:254
XYZI(const double &v_sin, const double &v_cos, const double &h_sin, const double &h_cos)
Definition: vsspdefs.h:169
uint8_t data_ms
Definition: vsspdefs.h:129
double operator[](AuxId id) const
Definition: vsspdefs.h:215
int16_t line_tail_h_angle_ratio
Definition: vsspdefs.h:94
static const uint32_t STATUS_COMMAND_UNKNOWN
Definition: vsspdefs.h:40
uint16_t spot
Definition: vsspdefs.h:98
uint32_t data_bitfield
Definition: vsspdefs.h:127
static const uint32_t TYPE_RI
Definition: vsspdefs.h:53
uint8_t frame
Definition: vsspdefs.h:95
uint16_t nspots
Definition: vsspdefs.h:112
uint8_t vertical_field
Definition: vsspdefs.h:102
static const uint32_t STATUS_UNKNOWN_ERROR
Definition: vsspdefs.h:45
static const uint32_t AX_MASK_LINACC
Definition: vsspdefs.h:74
static const uint32_t TYPE_ERR
Definition: vsspdefs.h:52
uint32_t timestamp_ms
Definition: vsspdefs.h:126
uint16_t length
Definition: vsspdefs.h:84
XYZ ang_vel
Definition: vsspdefs.h:203
static const uint32_t TYPE_DAT
Definition: vsspdefs.h:49
int16_t line_head_h_angle_ratio
Definition: vsspdefs.h:93
static const uint32_t TYPE_RO
Definition: vsspdefs.h:54
static const uint32_t TYPE_ER
Definition: vsspdefs.h:56
double y
Definition: vsspdefs.h:161
double x
Definition: vsspdefs.h:160
const RangeHeaderV2R1 RANGE_HEADER_V2R1_DEFAULT
Definition: vsspdefs.h:105
AuxId
Definition: vsspdefs.h:58
uint32_t line_tail_timestamp_ms
Definition: vsspdefs.h:92
int32_t val
Definition: vsspdefs.h:133
uint16_t range_mm
Definition: vsspdefs.h:121
uint32_t received_time_ms
Definition: vsspdefs.h:85
static const uint32_t STATUS_OK
Definition: vsspdefs.h:39
double y
Definition: vsspdefs.h:154
static const uint32_t STATUS_COMMAND_INVALUD_METHOD
Definition: vsspdefs.h:42
static const uint32_t VSSP_MARK
Definition: vsspdefs.h:38
uint8_t vertical_interlace
Definition: vsspdefs.h:103
static const uint32_t TYPE_VER
Definition: vsspdefs.h:50
uint32_t status
Definition: vsspdefs.h:82
double temp
Definition: vsspdefs.h:206
double & operator[](AuxId id)
Definition: vsspdefs.h:219
uint32_t type
Definition: vsspdefs.h:81
double r
Definition: vsspdefs.h:164
XYZI operator*(const DataRangeOnly &data) const
Definition: vsspdefs.h:188
uint16_t header_length
Definition: vsspdefs.h:125
uint16_t index_length
Definition: vsspdefs.h:111
static const uint32_t TYPE_GET
Definition: vsspdefs.h:47


hokuyo3d
Author(s): Atsushi Watanabe
autogenerated on Thu May 13 2021 02:27:44