├── .gitignore
├── IMU_calibrator
    └── imucal
├── LICENSE
├── README.md
├── camera_test
    ├── CMakeLists.txt
    ├── Loitor_VISensor_Setups.txt
    ├── include
    │   ├── loitorimu.h
    │   └── loitorusbcam.h
    ├── loitor-vi-install.sh
    └── src
    │   ├── camtest2.cpp
    │   ├── loitorimu.cpp
    │   └── loitorusbcam.cpp
├── loitor-vi-install.sh
├── loitor_PCcam_cliper.stl
└── loitor_ros_workspace
    └── src
        └── loitor_stereo_visensor
            ├── CMakeLists.txt
            ├── Loitor_VISensor_Setups.txt
            ├── cfg
                ├── Loitor_VISensor_Setups.txt
                └── backup_settings.txt
            ├── include
                └── loitor_stereo_visensor
                │   ├── loitorimu.h
                │   └── loitorusbcam.h
            ├── package.xml
            └── src
                ├── loitor_stereo_visensor.cpp
                ├── loitorimu.cpp
                └── loitorusbcam.cpp


/.gitignore:
--------------------------------------------------------------------------------
 1 | # Prerequisites
 2 | *.d
 3 | 
 4 | # Compiled Object files
 5 | *.slo
 6 | *.lo
 7 | *.o
 8 | *.obj
 9 | 
10 | # Precompiled Headers
11 | *.gch
12 | *.pch
13 | 
14 | # Compiled Dynamic libraries
15 | *.so
16 | *.dylib
17 | *.dll
18 | 
19 | # Fortran module files
20 | *.mod
21 | *.smod
22 | 
23 | # Compiled Static libraries
24 | *.lai
25 | *.la
26 | *.a
27 | *.lib
28 | 
29 | # Executables
30 | *.exe
31 | *.out
32 | *.app
33 | 
34 | # Build folders
35 | build/
36 | 


--------------------------------------------------------------------------------
/IMU_calibrator/imucal:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/loitor-vis/vi_sensor_sdk/eb54545ef5eeb681a5271725e8a9d9a21c97367a/IMU_calibrator/imucal


--------------------------------------------------------------------------------
/LICENSE:
--------------------------------------------------------------------------------
  1 |                     GNU GENERAL PUBLIC LICENSE
  2 |                        Version 2, June 1991
  3 | 
  4 |  Copyright (C) 1989, 1991 Free Software Foundation, Inc., <http://fsf.org/>
  5 |  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
  6 |  Everyone is permitted to copy and distribute verbatim copies
  7 |  of this license document, but changing it is not allowed.
  8 | 
  9 |                             Preamble
 10 | 
 11 |   The licenses for most software are designed to take away your
 12 | freedom to share and change it.  By contrast, the GNU General Public
 13 | License is intended to guarantee your freedom to share and change free
 14 | software--to make sure the software is free for all its users.  This
 15 | General Public License applies to most of the Free Software
 16 | Foundation's software and to any other program whose authors commit to
 17 | using it.  (Some other Free Software Foundation software is covered by
 18 | the GNU Lesser General Public License instead.)  You can apply it to
 19 | your programs, too.
 20 | 
 21 |   When we speak of free software, we are referring to freedom, not
 22 | price.  Our General Public Licenses are designed to make sure that you
 23 | have the freedom to distribute copies of free software (and charge for
 24 | this service if you wish), that you receive source code or can get it
 25 | if you want it, that you can change the software or use pieces of it
 26 | in new free programs; and that you know you can do these things.
 27 | 
 28 |   To protect your rights, we need to make restrictions that forbid
 29 | anyone to deny you these rights or to ask you to surrender the rights.
 30 | These restrictions translate to certain responsibilities for you if you
 31 | distribute copies of the software, or if you modify it.
 32 | 
 33 |   For example, if you distribute copies of such a program, whether
 34 | gratis or for a fee, you must give the recipients all the rights that
 35 | you have.  You must make sure that they, too, receive or can get the
 36 | source code.  And you must show them these terms so they know their
 37 | rights.
 38 | 
 39 |   We protect your rights with two steps: (1) copyright the software, and
 40 | (2) offer you this license which gives you legal permission to copy,
 41 | distribute and/or modify the software.
 42 | 
 43 |   Also, for each author's protection and ours, we want to make certain
 44 | that everyone understands that there is no warranty for this free
 45 | software.  If the software is modified by someone else and passed on, we
 46 | want its recipients to know that what they have is not the original, so
 47 | that any problems introduced by others will not reflect on the original
 48 | authors' reputations.
 49 | 
 50 |   Finally, any free program is threatened constantly by software
 51 | patents.  We wish to avoid the danger that redistributors of a free
 52 | program will individually obtain patent licenses, in effect making the
 53 | program proprietary.  To prevent this, we have made it clear that any
 54 | patent must be licensed for everyone's free use or not licensed at all.
 55 | 
 56 |   The precise terms and conditions for copying, distribution and
 57 | modification follow.
 58 | 
 59 |                     GNU GENERAL PUBLIC LICENSE
 60 |    TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION
 61 | 
 62 |   0. This License applies to any program or other work which contains
 63 | a notice placed by the copyright holder saying it may be distributed
 64 | under the terms of this General Public License.  The "Program", below,
 65 | refers to any such program or work, and a "work based on the Program"
 66 | means either the Program or any derivative work under copyright law:
 67 | that is to say, a work containing the Program or a portion of it,
 68 | either verbatim or with modifications and/or translated into another
 69 | language.  (Hereinafter, translation is included without limitation in
 70 | the term "modification".)  Each licensee is addressed as "you".
 71 | 
 72 | Activities other than copying, distribution and modification are not
 73 | covered by this License; they are outside its scope.  The act of
 74 | running the Program is not restricted, and the output from the Program
 75 | is covered only if its contents constitute a work based on the
 76 | Program (independent of having been made by running the Program).
 77 | Whether that is true depends on what the Program does.
 78 | 
 79 |   1. You may copy and distribute verbatim copies of the Program's
 80 | source code as you receive it, in any medium, provided that you
 81 | conspicuously and appropriately publish on each copy an appropriate
 82 | copyright notice and disclaimer of warranty; keep intact all the
 83 | notices that refer to this License and to the absence of any warranty;
 84 | and give any other recipients of the Program a copy of this License
 85 | along with the Program.
 86 | 
 87 | You may charge a fee for the physical act of transferring a copy, and
 88 | you may at your option offer warranty protection in exchange for a fee.
 89 | 
 90 |   2. You may modify your copy or copies of the Program or any portion
 91 | of it, thus forming a work based on the Program, and copy and
 92 | distribute such modifications or work under the terms of Section 1
 93 | above, provided that you also meet all of these conditions:
 94 | 
 95 |     a) You must cause the modified files to carry prominent notices
 96 |     stating that you changed the files and the date of any change.
 97 | 
 98 |     b) You must cause any work that you distribute or publish, that in
 99 |     whole or in part contains or is derived from the Program or any
100 |     part thereof, to be licensed as a whole at no charge to all third
101 |     parties under the terms of this License.
102 | 
103 |     c) If the modified program normally reads commands interactively
104 |     when run, you must cause it, when started running for such
105 |     interactive use in the most ordinary way, to print or display an
106 |     announcement including an appropriate copyright notice and a
107 |     notice that there is no warranty (or else, saying that you provide
108 |     a warranty) and that users may redistribute the program under
109 |     these conditions, and telling the user how to view a copy of this
110 |     License.  (Exception: if the Program itself is interactive but
111 |     does not normally print such an announcement, your work based on
112 |     the Program is not required to print an announcement.)
113 | 
114 | These requirements apply to the modified work as a whole.  If
115 | identifiable sections of that work are not derived from the Program,
116 | and can be reasonably considered independent and separate works in
117 | themselves, then this License, and its terms, do not apply to those
118 | sections when you distribute them as separate works.  But when you
119 | distribute the same sections as part of a whole which is a work based
120 | on the Program, the distribution of the whole must be on the terms of
121 | this License, whose permissions for other licensees extend to the
122 | entire whole, and thus to each and every part regardless of who wrote it.
123 | 
124 | Thus, it is not the intent of this section to claim rights or contest
125 | your rights to work written entirely by you; rather, the intent is to
126 | exercise the right to control the distribution of derivative or
127 | collective works based on the Program.
128 | 
129 | In addition, mere aggregation of another work not based on the Program
130 | with the Program (or with a work based on the Program) on a volume of
131 | a storage or distribution medium does not bring the other work under
132 | the scope of this License.
133 | 
134 |   3. You may copy and distribute the Program (or a work based on it,
135 | under Section 2) in object code or executable form under the terms of
136 | Sections 1 and 2 above provided that you also do one of the following:
137 | 
138 |     a) Accompany it with the complete corresponding machine-readable
139 |     source code, which must be distributed under the terms of Sections
140 |     1 and 2 above on a medium customarily used for software interchange; or,
141 | 
142 |     b) Accompany it with a written offer, valid for at least three
143 |     years, to give any third party, for a charge no more than your
144 |     cost of physically performing source distribution, a complete
145 |     machine-readable copy of the corresponding source code, to be
146 |     distributed under the terms of Sections 1 and 2 above on a medium
147 |     customarily used for software interchange; or,
148 | 
149 |     c) Accompany it with the information you received as to the offer
150 |     to distribute corresponding source code.  (This alternative is
151 |     allowed only for noncommercial distribution and only if you
152 |     received the program in object code or executable form with such
153 |     an offer, in accord with Subsection b above.)
154 | 
155 | The source code for a work means the preferred form of the work for
156 | making modifications to it.  For an executable work, complete source
157 | code means all the source code for all modules it contains, plus any
158 | associated interface definition files, plus the scripts used to
159 | control compilation and installation of the executable.  However, as a
160 | special exception, the source code distributed need not include
161 | anything that is normally distributed (in either source or binary
162 | form) with the major components (compiler, kernel, and so on) of the
163 | operating system on which the executable runs, unless that component
164 | itself accompanies the executable.
165 | 
166 | If distribution of executable or object code is made by offering
167 | access to copy from a designated place, then offering equivalent
168 | access to copy the source code from the same place counts as
169 | distribution of the source code, even though third parties are not
170 | compelled to copy the source along with the object code.
171 | 
172 |   4. You may not copy, modify, sublicense, or distribute the Program
173 | except as expressly provided under this License.  Any attempt
174 | otherwise to copy, modify, sublicense or distribute the Program is
175 | void, and will automatically terminate your rights under this License.
176 | However, parties who have received copies, or rights, from you under
177 | this License will not have their licenses terminated so long as such
178 | parties remain in full compliance.
179 | 
180 |   5. You are not required to accept this License, since you have not
181 | signed it.  However, nothing else grants you permission to modify or
182 | distribute the Program or its derivative works.  These actions are
183 | prohibited by law if you do not accept this License.  Therefore, by
184 | modifying or distributing the Program (or any work based on the
185 | Program), you indicate your acceptance of this License to do so, and
186 | all its terms and conditions for copying, distributing or modifying
187 | the Program or works based on it.
188 | 
189 |   6. Each time you redistribute the Program (or any work based on the
190 | Program), the recipient automatically receives a license from the
191 | original licensor to copy, distribute or modify the Program subject to
192 | these terms and conditions.  You may not impose any further
193 | restrictions on the recipients' exercise of the rights granted herein.
194 | You are not responsible for enforcing compliance by third parties to
195 | this License.
196 | 
197 |   7. If, as a consequence of a court judgment or allegation of patent
198 | infringement or for any other reason (not limited to patent issues),
199 | conditions are imposed on you (whether by court order, agreement or
200 | otherwise) that contradict the conditions of this License, they do not
201 | excuse you from the conditions of this License.  If you cannot
202 | distribute so as to satisfy simultaneously your obligations under this
203 | License and any other pertinent obligations, then as a consequence you
204 | may not distribute the Program at all.  For example, if a patent
205 | license would not permit royalty-free redistribution of the Program by
206 | all those who receive copies directly or indirectly through you, then
207 | the only way you could satisfy both it and this License would be to
208 | refrain entirely from distribution of the Program.
209 | 
210 | If any portion of this section is held invalid or unenforceable under
211 | any particular circumstance, the balance of the section is intended to
212 | apply and the section as a whole is intended to apply in other
213 | circumstances.
214 | 
215 | It is not the purpose of this section to induce you to infringe any
216 | patents or other property right claims or to contest validity of any
217 | such claims; this section has the sole purpose of protecting the
218 | integrity of the free software distribution system, which is
219 | implemented by public license practices.  Many people have made
220 | generous contributions to the wide range of software distributed
221 | through that system in reliance on consistent application of that
222 | system; it is up to the author/donor to decide if he or she is willing
223 | to distribute software through any other system and a licensee cannot
224 | impose that choice.
225 | 
226 | This section is intended to make thoroughly clear what is believed to
227 | be a consequence of the rest of this License.
228 | 
229 |   8. If the distribution and/or use of the Program is restricted in
230 | certain countries either by patents or by copyrighted interfaces, the
231 | original copyright holder who places the Program under this License
232 | may add an explicit geographical distribution limitation excluding
233 | those countries, so that distribution is permitted only in or among
234 | countries not thus excluded.  In such case, this License incorporates
235 | the limitation as if written in the body of this License.
236 | 
237 |   9. The Free Software Foundation may publish revised and/or new versions
238 | of the General Public License from time to time.  Such new versions will
239 | be similar in spirit to the present version, but may differ in detail to
240 | address new problems or concerns.
241 | 
242 | Each version is given a distinguishing version number.  If the Program
243 | specifies a version number of this License which applies to it and "any
244 | later version", you have the option of following the terms and conditions
245 | either of that version or of any later version published by the Free
246 | Software Foundation.  If the Program does not specify a version number of
247 | this License, you may choose any version ever published by the Free Software
248 | Foundation.
249 | 
250 |   10. If you wish to incorporate parts of the Program into other free
251 | programs whose distribution conditions are different, write to the author
252 | to ask for permission.  For software which is copyrighted by the Free
253 | Software Foundation, write to the Free Software Foundation; we sometimes
254 | make exceptions for this.  Our decision will be guided by the two goals
255 | of preserving the free status of all derivatives of our free software and
256 | of promoting the sharing and reuse of software generally.
257 | 
258 |                             NO WARRANTY
259 | 
260 |   11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY
261 | FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW.  EXCEPT WHEN
262 | OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES
263 | PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED
264 | OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
265 | MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.  THE ENTIRE RISK AS
266 | TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU.  SHOULD THE
267 | PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING,
268 | REPAIR OR CORRECTION.
269 | 
270 |   12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING
271 | WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR
272 | REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES,
273 | INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING
274 | OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED
275 | TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY
276 | YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER
277 | PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE
278 | POSSIBILITY OF SUCH DAMAGES.
279 | 
280 |                      END OF TERMS AND CONDITIONS
281 | 
282 |             How to Apply These Terms to Your New Programs
283 | 
284 |   If you develop a new program, and you want it to be of the greatest
285 | possible use to the public, the best way to achieve this is to make it
286 | free software which everyone can redistribute and change under these terms.
287 | 
288 |   To do so, attach the following notices to the program.  It is safest
289 | to attach them to the start of each source file to most effectively
290 | convey the exclusion of warranty; and each file should have at least
291 | the "copyright" line and a pointer to where the full notice is found.
292 | 
293 |     {description}
294 |     Copyright (C) {year}  {fullname}
295 | 
296 |     This program is free software; you can redistribute it and/or modify
297 |     it under the terms of the GNU General Public License as published by
298 |     the Free Software Foundation; either version 2 of the License, or
299 |     (at your option) any later version.
300 | 
301 |     This program is distributed in the hope that it will be useful,
302 |     but WITHOUT ANY WARRANTY; without even the implied warranty of
303 |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
304 |     GNU General Public License for more details.
305 | 
306 |     You should have received a copy of the GNU General Public License along
307 |     with this program; if not, write to the Free Software Foundation, Inc.,
308 |     51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
309 | 
310 | Also add information on how to contact you by electronic and paper mail.
311 | 
312 | If the program is interactive, make it output a short notice like this
313 | when it starts in an interactive mode:
314 | 
315 |     Gnomovision version 69, Copyright (C) year name of author
316 |     Gnomovision comes with ABSOLUTELY NO WARRANTY; for details type `show w'.
317 |     This is free software, and you are welcome to redistribute it
318 |     under certain conditions; type `show c' for details.
319 | 
320 | The hypothetical commands `show w' and `show c' should show the appropriate
321 | parts of the General Public License.  Of course, the commands you use may
322 | be called something other than `show w' and `show c'; they could even be
323 | mouse-clicks or menu items--whatever suits your program.
324 | 
325 | You should also get your employer (if you work as a programmer) or your
326 | school, if any, to sign a "copyright disclaimer" for the program, if
327 | necessary.  Here is a sample; alter the names:
328 | 
329 |   Yoyodyne, Inc., hereby disclaims all copyright interest in the program
330 |   `Gnomovision' (which makes passes at compilers) written by James Hacker.
331 | 
332 |   {signature of Ty Coon}, 1 April 1989
333 |   Ty Coon, President of Vice
334 | 
335 | This General Public License does not permit incorporating your program into
336 | proprietary programs.  If your program is a subroutine library, you may
337 | consider it more useful to permit linking proprietary applications with the
338 | library.  If this is what you want to do, use the GNU Lesser General
339 | Public License instead of this License.
340 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
1 | # vi_sensor_sdk
2 | Loitor VI Sensor SDK, ROS pack and IMU calibration program.
3 | 


--------------------------------------------------------------------------------
/camera_test/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8)
 2 | 
 3 | project(camtest)
 4 | 
 5 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread")
 6 | set(LIBRARY_OUTPUT_PATH "${PROJECT_SOURCE_DIR}/lib")
 7 | 
 8 | find_package(OpenCV REQUIRED)
 9 | 
10 | include_directories("${PROJECT_SOURCE_DIR}/include")
11 | 
12 | link_directories("${PROJECT_SOURCE_DIR}/lib")
13 | 
14 | add_library(loitorusbcam src/loitorusbcam.cpp)
15 | target_link_libraries(loitorusbcam loitorimu)
16 | 
17 | add_library(loitorimu src/loitorimu.cpp)
18 | 
19 | add_executable(camtest src/camtest2.cpp)
20 | target_link_libraries(camtest loitorusbcam loitorimu usb-1.0 ${OpenCV_LIBS})
21 | 


--------------------------------------------------------------------------------
/camera_test/Loitor_VISensor_Setups.txt:
--------------------------------------------------------------------------------
 1 | #
 2 | Mode
 3 | 12
 4 | #
 5 | HighSpeed Mode
 6 | m1,left,150,VGA,54
 7 | m2,right,150,VGA,54
 8 | m3,left,150,WVGA,54
 9 | m4,right,150,WVGA,54
10 | m5,stereo,250,VGA,54
11 | m6,stereo,270,WVGA,54
12 | #
13 | Normal Mode
14 | m7,left,150,VGA,54
15 | m8,right,150,VGA,54
16 | m9,left,150,WVGA,54
17 | m10,right,150,WVGA,54
18 | m11,stereo,274,VGA,27
19 | m12,stereo,162,WVGA,27
20 | #
21 | m13,Manual Mode
22 | 0
23 | 194
24 | VGA
25 | 54
26 | #
27 | EG_mode
28 | 4
29 | manual,50,200
30 | auto,300,5,58
31 | autoexp_manualgain,300,5,58,200
32 | /dev/ttyUSB0,5
33 | #
34 | IMU-acc-bias
35 | Gx,52.000000
36 | Gy,32.00001
37 | Gz,-243.000000
38 | #
39 | 


--------------------------------------------------------------------------------
/camera_test/include/loitorimu.h:
--------------------------------------------------------------------------------
 1 | #ifndef LOITORIMU_H
 2 | #define LOITORIMU_H
 3 | 
 4 | #include <sys/time.h>
 5 | 
 6 | typedef struct 
 7 | {
 8 | 	float imu_time;		// time-stamp
 9 | 	timeval system_time;
10 | 	unsigned char num;
11 | 	float rx,ry,rz;
12 | 	float ax,ay,az;
13 | 	float qw,qx,qy,qz;
14 | 	//int timestamp;
15 | }visensor_imudata;
16 | 
17 | extern timeval visensor_startTime;
18 | 
19 | bool visensor_query_imu_update();
20 | bool visensor_mark_imu_update();
21 | bool visensor_erase_imu_update();
22 | int visensor_set_opt(int fd,int nSpeed, int nBits, char nEvent, int nStop);
23 | int visensor_open_port(const char* dev_str);
24 | int visensor_get_imu_frame(int fd, unsigned char* imu_frame);
25 | int visensor_get_imu_data(unsigned char* imu_frame,short int* acc_offset,visensor_imudata *imudata_struct,bool show_data);
26 | int visensor_send_imu_frame(int fd, unsigned char* data, int len);
27 | 
28 | int print_imu_datawithtime(int data_count, float* fgyr, float* facc, double timestamp, double camtimes, short int* acc_offset);
29 | int get_imu_datawithtime(int fd, int* data_count, float* gyr, float* acc, double* timestamp, double* camtimes);
30 | 
31 | #endif
32 | 


--------------------------------------------------------------------------------
/camera_test/include/loitorusbcam.h:
--------------------------------------------------------------------------------
 1 | #include <sys/time.h>
 2 | #include "loitorimu.h"
 3 | 
 4 | #ifndef LOITORUSBCAM_H
 5 | #define LOITORUSBCAM_H
 6 | #define IMU_FRAME_LEN 32
 7 | #define IMG_WIDTH_VGA 	640
 8 | #define IMG_HEIGHT_VGA 	480
 9 | #define IMG_SIZE_VGA 	(IMG_WIDTH_VGA*IMG_HEIGHT_VGA)
10 | #define IMG_BUF_SIZE_VGA (IMG_SIZE_VGA+0x200)
11 | #define IMG_WIDTH_WVGA 	752
12 | #define IMG_HEIGHT_WVGA 	480
13 | #define IMG_SIZE_WVGA 	(IMG_WIDTH_WVGA*IMG_HEIGHT_WVGA)
14 | #define IMG_BUF_SIZE_WVGA (IMG_SIZE_WVGA+0x200)
15 | 
16 | /*
17 | *  camera当前分辨率状态
18 | *  0-代表VGA
19 | *  1-代表WVGA
20 | */
21 | extern bool visensor_resolution_status;			// 0-VGA | 1-WVGA
22 | /*
23 | *  camera当前通道选择
24 | *  0-代表开启双目
25 | *  1-代表只开启右眼
26 | *  2-代表只开启左眼
27 | */
28 | extern int visensor_cam_selection;				// 0-stereo | 1-right | 2-left
29 | /*
30 | *  imu 数据
31 | */
32 | extern visensor_imudata visensor_imudata_pack;
33 | 
34 | // setters & getters
35 | void visensor_set_auto_EG(int E_AG);			// 0-ManEG | 1-AutoEG with limits | 2-AutoE&ManG | 3- fully auto
36 | void visensor_set_exposure(int _man_exp);
37 | void visensor_set_gain(int _man_gain);
38 | void visensor_set_max_autoExp(int max_exp);
39 | void visensor_set_min_autoExp(int min_exp);
40 | void visensor_set_resolution(bool set_wvga);
41 | void visensor_set_fps_mode(bool fps_mode);
42 | void visensor_set_current_HB(int HB);
43 | void visensor_set_desired_bin(int db);
44 | void visensor_set_cam_selection_mode(int _visensor_cam_selection);
45 | void visensor_set_imu_bias(float bx,float by,float bz);
46 | void visensor_set_imu_portname(char* input_name);
47 | void visensor_set_current_mode(int _mode);
48 | 
49 | int visensor_get_EG_mode();
50 | int visensor_get_exposure();
51 | int visensor_get_gain();
52 | int visensor_get_max_autoExp();
53 | int visensor_get_min_autoExp();
54 | bool visensor_get_resolution();
55 | int visensor_get_fps();
56 | int visensor_get_current_HB();
57 | int visensor_get_desired_bin();
58 | int visensor_get_cam_selection_mode();
59 | float visensor_get_imu_G_bias_x();
60 | float visensor_get_imu_G_bias_y();
61 | float visensor_get_imu_G_bias_z();
62 | const char* visensor_get_imu_portname();
63 | 
64 | void visensor_save_current_settings();
65 | 
66 | float visensor_get_hardware_fps();
67 | 
68 | void visensor_load_settings(const char* settings_file);
69 | 
70 | bool visensor_is_stereo_good();
71 | bool visensor_is_left_good();
72 | bool visensor_is_right_good();
73 | 
74 | bool visensor_is_left_fresh();
75 | bool visensor_is_right_fresh();
76 | 
77 | /*
78 | * 得到绑定了同步IMU数据之后的图像数据
79 | */
80 | visensor_imudata visensor_get_stereoImg(char* left_img,char* right_img);
81 | visensor_imudata visensor_get_stereoImg(char* left_img,char* right_img,timeval &left_stamp,timeval &right_stamp);
82 | 
83 | visensor_imudata visensor_get_leftImg(char* left_img);
84 | visensor_imudata visensor_get_leftImg(char* left_img,timeval &left_stamp);
85 | 
86 | visensor_imudata visensor_get_rightImg(char* right_img);
87 | visensor_imudata visensor_get_rightImg(char* right_img,timeval &right_stamp);
88 | 
89 | int visensor_Start_Cameras();
90 | void visensor_Close_Cameras();
91 | 
92 | bool visensor_imu_have_fresh_data();
93 | 
94 | int visensor_Start_IMU();
95 | void visensor_Close_IMU();
96 | 
97 | 
98 | #endif
99 | 


--------------------------------------------------------------------------------
/camera_test/loitor-vi-install.sh:
--------------------------------------------------------------------------------
 1 | create_udev_rules() {
 2 | 	echo "" > loitor-vi.rules
 3 | 	echo 'KERNEL=="*", SUBSYSTEM=="usb", ENV{DEVTYPE}=="usb_device", ACTION=="add", ATTR{idVendor}=="04b4", MODE="666" ' >> loitor-vi.rules
 4 | 	echo 'KERNEL=="*", SUBSYSTEM=="usb", ENV{DEVTYPE}=="usb_device", ACTION=="remove" ' >> loitor-vi.rules
 5 | }
 6 | 
 7 | if [ `whoami` != 'root' ]; then
 8 |    echo "You have to be root to run this script"   
 9 |    echo "请使用root权限运行此脚本"
10 |    exit 1;
11 | fi
12 | 
13 | create_udev_rules
14 | cp loitor-vi.rules /etc/udev/rules.d/
15 | rm loitor-vi.rules
16 | 


--------------------------------------------------------------------------------
/camera_test/src/camtest2.cpp:
--------------------------------------------------------------------------------
  1 | #include <stdio.h>
  2 | #include <string.h>
  3 | #include <unistd.h>
  4 | #include <pthread.h>
  5 | #include <sys/time.h>
  6 | 
  7 | #include <iostream>
  8 | 
  9 | #include <opencv2/core/core.hpp>
 10 | #include <opencv2/highgui/highgui.hpp>
 11 | 
 12 | #include "loitorusbcam.h"
 13 | #include "loitorimu.h"
 14 | 
 15 | using namespace std;
 16 | using namespace cv;
 17 | bool close_img_viewer=false;
 18 | bool visensor_Close_IMU_viewer=false;
 19 | 
 20 | // 当前左右图像的时间戳
 21 | timeval left_stamp,right_stamp;
 22 | 
 23 | // image viewer
 24 | void *opencv_showimg(void*)
 25 | {
 26 |     Mat img_left;
 27 |     Mat img_right;
 28 | 
 29 |     if(!visensor_resolution_status)
 30 |     {
 31 |         img_left.create(Size(640,480),CV_8U);
 32 |         img_right.create(Size(640,480),CV_8U);
 33 |         img_left.data=new unsigned char[IMG_WIDTH_VGA*IMG_HEIGHT_VGA];
 34 |         img_right.data=new unsigned char[IMG_WIDTH_VGA*IMG_HEIGHT_VGA];
 35 |     }
 36 |     else
 37 |     {
 38 |         img_left.create(Size(752,480),CV_8U);
 39 |         img_right.create(Size(752,480),CV_8U);
 40 |         img_left.data=new unsigned char[IMG_WIDTH_WVGA*IMG_HEIGHT_WVGA];
 41 |         img_right.data=new unsigned char[IMG_WIDTH_WVGA*IMG_HEIGHT_WVGA];
 42 |     }
 43 |     while(!close_img_viewer)
 44 |     {
 45 |         if(visensor_cam_selection==2)
 46 |         {
 47 |             visensor_imudata paired_imu=visensor_get_leftImg((char *)img_left.data,left_stamp);
 48 | 
 49 |             // 显示同步数据的时间戳（单位微秒）
 50 |             cout<<"left_time : "<<left_stamp.tv_usec<<endl;
 51 |             cout<<"paired_imu time ===== "<<paired_imu.system_time.tv_usec<<endl<<endl;
 52 | 
 53 |             imshow("left",img_left);
 54 |             waitKey(1);
 55 |         }
 56 |         //Cam2
 57 |         else if(visensor_cam_selection==1)
 58 |         {
 59 |             visensor_imudata paired_imu=visensor_get_rightImg((char *)img_right.data,right_stamp);
 60 | 
 61 |             // 显示同步数据的时间戳（单位微秒）
 62 |             cout<<"right_time : "<<right_stamp.tv_usec<<endl;
 63 |             cout<<"paired_imu time ===== "<<paired_imu.system_time.tv_usec<<endl<<endl;
 64 | 
 65 |             imshow("right",img_right);
 66 |             waitKey(1);
 67 |         }
 68 |         // Cam1 && Cam2
 69 |         else if(visensor_cam_selection==0)
 70 |         {
 71 |             visensor_imudata paired_imu=visensor_get_stereoImg((char *)img_left.data,(char *)img_right.data,left_stamp,right_stamp);
 72 | 
 73 |             // 显示同步数据的时间戳（单位微秒）
 74 |             cout<<"left_time : "<<left_stamp.tv_usec<<endl;
 75 |             cout<<"right_time : "<<right_stamp.tv_usec<<endl;
 76 |             cout<<"paired_imu time ===== "<<paired_imu.system_time.tv_usec<<endl<<endl;
 77 | 
 78 |             imshow("left",img_left);
 79 |             imshow("right",img_right);
 80 |             waitKey(1);
 81 |         }
 82 |     }
 83 |     pthread_exit(NULL);
 84 | }
 85 | 
 86 | 
 87 | void* show_imuData(void *)
 88 | {
 89 |     while(!visensor_Close_IMU_viewer)
 90 |     {
 91 |         if(visensor_imu_have_fresh_data())
 92 |         {
 93 |             static int printcount=0;
 94 |             if((printcount++)%20==0)
 95 |                 printf("IMU Gyr: %8.4f,%8.4f,%8.4f, Acc: %8.4f,%8.4f,%8.4f, Quat(WXYZ): %8.4f,%8.4f,%8.4f,%8.4f\n",
 96 |                        visensor_imudata_pack.rx,
 97 |                        visensor_imudata_pack.ry,
 98 |                        visensor_imudata_pack.rz,
 99 |                        visensor_imudata_pack.ax,
100 |                        visensor_imudata_pack.ay,
101 |                        visensor_imudata_pack.az,
102 |                        visensor_imudata_pack.qw,
103 |                        visensor_imudata_pack.qx,
104 |                        visensor_imudata_pack.qy,
105 |                        visensor_imudata_pack.qz);
106 |         }
107 |         usleep(100);
108 |     }
109 |     pthread_exit(NULL);
110 | }
111 | 
112 | int main(int argc, char* argv[])
113 | {
114 | 
115 |     /************************ Start Cameras ************************/
116 |     visensor_load_settings("../Loitor_VISensor_Setups.txt");
117 |     /*
118 |     // 手动设置相机参数
119 |     visensor_set_current_mode(5);
120 |     visensor_set_auto_EG(0);
121 |     visensor_set_exposure(50);
122 |     visensor_set_gain(200);
123 |     visensor_set_cam_selection_mode(2);
124 |     visensor_set_resolution(false);
125 |     visensor_set_fps_mode(true);
126 |     // 保存相机参数到原配置文件
127 |     visensor_save_current_settings();
128 |     */
129 | 
130 |     int r = visensor_Start_Cameras();
131 |     if(r<0)
132 |     {
133 |         printf("Opening cameras failed...\r\n");
134 |         return r;
135 |     }
136 |     /************************** Start IMU **************************/
137 |     int fd=visensor_Start_IMU();
138 |     if(fd<0)
139 |     {
140 |         printf("visensor_open_port error...\r\n");
141 |         return 0;
142 |     }
143 |     printf("visensor_open_port success...\r\n");
144 |     /************************ ************ ************************/
145 | 
146 |     usleep(100000);
147 | 
148 |     //Create img_show thread
149 |     pthread_t showimg_thread;
150 |     int temp;
151 |     if(temp = pthread_create(&showimg_thread, NULL, opencv_showimg, NULL))
152 |         printf("Failed to create thread opencv_showimg\r\n");
153 |     //Create show_imuData thread
154 |     pthread_t showimu_thread;
155 |     if(temp = pthread_create(&showimu_thread, NULL, show_imuData, NULL))
156 |         printf("Failed to create thread show_imuData\r\n");
157 | 
158 |     while(1)
159 |     {
160 |         // Do - Nothing :)
161 |         //cout<<visensor_get_imu_portname()<<endl;
162 |         //cout<<visensor_get_hardware_fps()<<endl;
163 |         usleep(500000);
164 |     }
165 | 
166 |     /* shut-down viewers */
167 |     close_img_viewer=true;
168 |     visensor_Close_IMU_viewer=true;
169 |     if(showimg_thread !=0)
170 |     {
171 |         pthread_join(showimg_thread,NULL);
172 |     }
173 |     if(showimu_thread !=0)
174 |     {
175 |         pthread_join(showimu_thread,NULL);
176 |     }
177 | 
178 |     /* close cameras */
179 |     visensor_Close_Cameras();
180 |     /* close IMU */
181 |     visensor_Close_IMU();
182 | 
183 |     return 0;
184 | }
185 | 


--------------------------------------------------------------------------------
/camera_test/src/loitorimu.cpp:
--------------------------------------------------------------------------------
  1 | #include <stdio.h>
  2 | #include <unistd.h>
  3 | #include <stdlib.h>
  4 | #include <getopt.h>
  5 | #include <string.h>
  6 | #include <pthread.h>
  7 | #include <math.h>
  8 | #include <fcntl.h>
  9 | #include <errno.h>
 10 | #include <termios.h>
 11 | #include "math.h"
 12 | 
 13 | #define IMU_FRAME_LEN 32
 14 | #include "loitorimu.h"
 15 | 
 16 | bool shut_down_imu=false;
 17 | 
 18 | timeval visensor_startTime;		// time-stamp
 19 | struct timeval imu_SysTime;
 20 | 
 21 | float imu_time=0;
 22 | bool visensor_imu_updated=false;
 23 | 
 24 | bool visensor_query_imu_update()
 25 | {
 26 |     return visensor_imu_updated;
 27 | }
 28 | bool visensor_mark_imu_update()
 29 | {
 30 |     visensor_imu_updated=true;
 31 | }
 32 | bool visensor_erase_imu_update()
 33 | {
 34 |     visensor_imu_updated=false;
 35 | }
 36 | 
 37 | int visensor_set_opt(int fd,int nSpeed, int nBits, char nEvent, int nStop)
 38 | {
 39 |     struct termios newtio,oldtio;
 40 |     if ( tcgetattr( fd,&oldtio) != 0)
 41 |     {
 42 |         perror("SetupSerial 1");
 43 |         return -1;
 44 |     }
 45 |     bzero( &newtio, sizeof( newtio ) );
 46 |     newtio.c_cflag |= CLOCAL | CREAD;
 47 |     newtio.c_cflag &= ~CSIZE;
 48 | 
 49 |     switch( nBits )
 50 |     {
 51 |     case 7:
 52 |         newtio.c_cflag |= CS7;
 53 |         break;
 54 |     case 8:
 55 |         newtio.c_cflag |= CS8;
 56 |         break;
 57 |     }
 58 | 
 59 |     switch( nEvent )
 60 |     {
 61 |     case 'O':
 62 |         newtio.c_cflag |= PARENB;
 63 |         newtio.c_cflag |= PARODD;
 64 |         newtio.c_iflag |= (INPCK | ISTRIP);
 65 |         break;
 66 |     case 'E':
 67 |         newtio.c_iflag |= (INPCK | ISTRIP);
 68 |         newtio.c_cflag |= PARENB;
 69 |         newtio.c_cflag &= ~PARODD;
 70 |         break;
 71 |     case 'N':
 72 |         newtio.c_cflag &= ~PARENB;
 73 |         break;
 74 |     }
 75 | 
 76 |     switch( nSpeed )
 77 |     {
 78 |     case 2400:
 79 |         cfsetispeed(&newtio, B2400);
 80 |         cfsetospeed(&newtio, B2400);
 81 |         break;
 82 |     case 4800:
 83 |         cfsetispeed(&newtio, B4800);
 84 |         cfsetospeed(&newtio, B4800);
 85 |         break;
 86 |     case 9600:
 87 |         cfsetispeed(&newtio, B9600);
 88 |         cfsetospeed(&newtio, B9600);
 89 |         break;
 90 |     case 115200:
 91 |         cfsetispeed(&newtio, B115200);
 92 |         cfsetospeed(&newtio, B115200);
 93 |         break;
 94 |     default:
 95 |         cfsetispeed(&newtio, B9600);
 96 |         cfsetospeed(&newtio, B9600);
 97 |         break;
 98 |     }
 99 |     if( nStop == 1 )
100 |         newtio.c_cflag &= ~CSTOPB;
101 |     else if ( nStop == 2 )
102 |         newtio.c_cflag |= CSTOPB;
103 |     newtio.c_cc[VTIME] = 0;
104 |     newtio.c_cc[VMIN] = 32;
105 |     tcflush(fd,TCIFLUSH);
106 |     if((tcsetattr(fd,TCSANOW,&newtio))!=0)
107 |     {
108 |         perror("com set error");
109 |         return -1;
110 |     }
111 |     printf("set done!\n");
112 |     return 0;
113 | }
114 | 
115 | int visensor_open_port(const char* dev_str)
116 | {
117 |     long vdisable;
118 | 
119 |     int fd = open( dev_str, O_RDWR|O_NOCTTY|O_NDELAY);
120 |     if (-1 == fd)
121 |     {
122 |         perror("Can't Open Serial Port");
123 |         return(-1);
124 |     }
125 | 
126 |     if(fcntl(fd, F_SETFL, 0)<0)
127 |         printf("fcntl failed!\n");
128 |     else
129 |         printf("fcntl=%d\n",fcntl(fd, F_SETFL,0));
130 |     if(isatty(STDIN_FILENO)==0)
131 |         printf("standard input is not a terminal device\n");
132 |     else
133 |         printf("isatty success!\n");
134 |     printf("fd-open=%d\n",fd);
135 |     return fd;
136 | }
137 | 
138 | static int find_55aa(unsigned char* buf,int len)
139 | {
140 |     int i;
141 |     for(i=0; i<len-1; i++)
142 |     {
143 |         if(buf[i]==0x55)
144 |             if(buf[i+1]==0xAA)
145 |                 return i;
146 |     }
147 |     if(buf[len-1]==0x55)
148 |         return -1;
149 |     return -2;
150 | }
151 | 
152 | 
153 | int visensor_send_imu_frame(int fd, unsigned char* data, int len)
154 | {
155 |     return write(fd,data,len);
156 | }
157 | 
158 | #include <sys/time.h>
159 | 
160 | int visensor_get_imu_frame(int fd, unsigned char* imu_frame)
161 | {
162 | 
163 |     unsigned char imu_frame_buf[2*IMU_FRAME_LEN];
164 |     memset(imu_frame,0,IMU_FRAME_LEN);
165 |     memset(imu_frame_buf,0,2*IMU_FRAME_LEN);
166 |     int num_get = 0;
167 | 
168 |     static int flush_count = 0;
169 |     flush_count = (flush_count+1)%200;
170 |     if(flush_count==0)
171 |         tcflush(fd,TCIFLUSH);
172 |     static struct timeval getIMUTime;
173 |     struct timeval endTime;
174 |     float fTimeuse;
175 |     while(!shut_down_imu)
176 |     {
177 |         //读到32个以上字节
178 |         while(num_get<IMU_FRAME_LEN)
179 |         {
180 |             int temp_read_get = read(fd,&imu_frame_buf[num_get],IMU_FRAME_LEN);
181 |             if(temp_read_get<=0)
182 |             {
183 |                 continue;//读取错误，不得往num_get上加
184 |             }
185 |             num_get+=temp_read_get;
186 |         }//实际读到了num_get字节数
187 |         gettimeofday(&getIMUTime,NULL);
188 |         gettimeofday(&endTime,NULL);
189 |         gettimeofday(&imu_SysTime,NULL);
190 |         imu_SysTime.tv_usec-=3500;
191 |         if(imu_SysTime.tv_usec<0){imu_SysTime.tv_usec+=1000000;imu_SysTime.tv_sec-=1;}
192 | 
193 |         //查询55AA的位置
194 |         int position_55aa = find_55aa(imu_frame_buf,num_get);
195 |         //如果查不到55AA(-2)，末尾也没有55，那么此次读取失败，清空imu_frame_buf,从头再来
196 |         if(position_55aa == -2)
197 |         {
198 |             memset(imu_frame_buf,0,2*IMU_FRAME_LEN);
199 |             num_get = 0;
200 |             continue;
201 |         }
202 |         //如果末尾是55，那么清空imu_frame_buf,然后将首位置为55
203 |         if(position_55aa == -1)
204 |         {
205 |             memset(imu_frame_buf,0,2*IMU_FRAME_LEN);
206 |             num_get = 1;
207 |             imu_frame_buf[0] = 0x55;
208 |             continue;
209 |         }
210 |         //如果找到了55AA，那么首先检验有效帧长度
211 |         int valid_len = num_get - position_55aa;
212 |         //如果有效帧长度不足32，那么将数据移动到帧头，然后继续读取剩余帧
213 |         if(valid_len<32)
214 |         {
215 |             for(int i=0; i<valid_len; i++)
216 |                 imu_frame_buf[i] = imu_frame_buf[position_55aa+i];
217 |             for(int i=valid_len; i<2*IMU_FRAME_LEN; i++)
218 |                 imu_frame_buf[i] = 0;
219 |             num_get = valid_len;
220 |             continue;
221 |         }
222 |         //如果有效帧长度达到32，那么检查校验和
223 |         unsigned char checksum = 0;
224 |         for(int i=2; i<31; i++)checksum += imu_frame_buf[position_55aa+i];
225 |         //如果校验和不正确，那么清除该帧头和之前的所有数据，然后将剩余数据移动到帧头，再继续读取剩余数据
226 |         if(checksum != imu_frame_buf[position_55aa+31])
227 |         {
228 |             for(int i=2; i<valid_len; i++)
229 |                 imu_frame_buf[i-2] = imu_frame_buf[position_55aa+i];
230 |             for(int i=valid_len-2; i<2*IMU_FRAME_LEN; i++)
231 |                 imu_frame_buf[i] = 0;
232 |             num_get = valid_len-2;
233 |             continue;
234 |         }
235 |         //如果校验和正确，而有效帧长度超过了32，那么说明已经有数据溢出，应立即清除串口缓存,然后重新开始读取
236 |         if(valid_len>32)
237 |         {
238 |             tcflush(fd,TCIFLUSH);
239 |             memset(imu_frame_buf,0,2*IMU_FRAME_LEN);
240 |             num_get = 0;
241 |             continue;
242 |         }
243 |         //如果校验和正确，那么输出该帧数据
244 |         memcpy(imu_frame,&imu_frame_buf[position_55aa],IMU_FRAME_LEN);
245 |         static float time_interval=5000;
246 |         static float last_time,new_time,new_pred,last_pred;
247 |         new_time = getIMUTime.tv_usec;
248 |         int newtime_interval;
249 |         newtime_interval = new_time-last_time;
250 |         if(newtime_interval<4000||newtime_interval>6000)
251 |             newtime_interval = 5000;
252 |         time_interval = time_interval*0.999+0.001*newtime_interval;
253 |         if(new_time-last_pred<time_interval)
254 |             new_pred = new_time;
255 |         else
256 |             new_pred = last_pred + time_interval + 0.001*(new_time - last_pred - time_interval);
257 |         last_time = new_time;
258 |         last_pred = new_pred;
259 | 
260 |         fTimeuse = (endTime.tv_sec - visensor_startTime.tv_sec) + 0.000001*(endTime.tv_usec - visensor_startTime.tv_usec);
261 |         imu_time=fTimeuse;
262 | 
263 |         break;
264 |     }
265 | 
266 |     return 0;
267 | }
268 | 
269 | float data_norm(float * q,int length)
270 | {
271 |     float datasum=0.0;
272 |     for(int i=0; i<length; i++)datasum+=q[i]*q[i];
273 |     return sqrt(datasum);
274 | }
275 | int q_normalize(float* q)
276 | {
277 |     float qnorm = data_norm(q,4);
278 |     for(int i=0; i<4; i++)q[i]/=qnorm;
279 | }
280 | 
281 | int q_mult(float* q1,float* q2,float* qout)
282 | {
283 |     qout[0]=q1[0]*q2[0]-q1[1]*q2[1]-q1[2]*q2[2]-q1[3]*q2[3];
284 |     qout[1]=q1[0]*q2[1]+q1[1]*q2[0]+q1[2]*q2[3]-q1[3]*q2[2];
285 |     qout[2]=q1[0]*q2[2]+q1[2]*q2[0]+q1[3]*q2[1]-q1[1]*q2[3];
286 |     qout[3]=q1[0]*q2[3]+q1[3]*q2[0]+q1[1]*q2[2]-q1[2]*q2[1];
287 | }
288 | 
289 | int q_rotvec(float* q,float* rin,float* rout)
290 | {
291 |     float q_rin[4]= {0,rin[0],rin[1],rin[2]};
292 |     float q_temp[4];
293 |     q_mult(q,q_rin,q_temp);
294 |     float q_inv[4]= {q[0],-q[1],-q[2],-q[3]};
295 |     float q_temp_out[4];
296 |     q_mult(q_temp,q_inv,q_temp_out);
297 |     for(int i=0; i<3; i++)rout[i]=q_temp_out[i+1];
298 | }
299 | 
300 | 
301 | int visensor_get_imu_data(unsigned char* imu_frame,short int* acc_offset,visensor_imudata *imudata_struct,bool show_data=false)
302 | {
303 | 
304 |     imudata_struct->imu_time=imu_time;
305 |     imudata_struct->system_time=imu_SysTime;
306 | 
307 |     static int last_imu_no;
308 |     //for(int i=1; i<(imu_frame[2]+200-last_imu_no)%200; i++)
309 |     //    printf(".................................................................................\r\n");
310 |     last_imu_no = imu_frame[2];
311 |     int imu_data[10];
312 |     imu_data[0] = *(short*)(&imu_frame[3]);
313 |     imu_data[1] = *(short*)(&imu_frame[5]);
314 |     imu_data[2] = *(short*)(&imu_frame[7]);
315 | 
316 |     imu_data[3] = *(short*)(&imu_frame[9]);
317 |     imu_data[4] = *(short*)(&imu_frame[11]);
318 |     imu_data[5] = *(short*)(&imu_frame[13]);
319 | 
320 | 
321 |     imu_data[6] = *(int*)(&imu_frame[15]);
322 |     imu_data[7] = *(int*)(&imu_frame[19]);
323 |     imu_data[8] = *(int*)(&imu_frame[23]);
324 |     imu_data[9] = *(int*)(&imu_frame[27]);
325 | 
326 |     imudata_struct->rx = 1.0*imu_data[0]/32768*2000;
327 |     imudata_struct->ry = 1.0*imu_data[1]/32768*2000;
328 |     imudata_struct->rz = 1.0*imu_data[2]/32768*2000;
329 |     float facc_nobias[3]= {
330 |         (float)(imu_data[3]-acc_offset[0]),
331 |         (float)(imu_data[4]-acc_offset[1]),
332 |         (float)(imu_data[5]-acc_offset[2])};
333 | 
334 |     imudata_struct->ax = 1.0*facc_nobias[0]/16384*9.81;
335 |     imudata_struct->ay = 1.0*facc_nobias[1]/16384*9.81;
336 |     imudata_struct->az = 1.0*facc_nobias[2]/16384*9.81;
337 | 
338 |     float quat[4]= {
339 |         (float)imu_data[6],
340 |         (float)imu_data[7],
341 |         (float)imu_data[8],
342 |         (float)imu_data[9]};
343 |     float facc[3]= {
344 |         (float)imu_data[3],
345 |         (float)imu_data[4],
346 |         (float)imu_data[5]};
347 | 
348 |     static float faccg[3]= {0,0,0};
349 |     q_normalize(quat);
350 |     imudata_struct->qw = quat[0];
351 |     imudata_struct->qx = quat[1];
352 |     imudata_struct->qy = quat[2];
353 |     imudata_struct->qz = quat[3];
354 | 
355 |     imudata_struct->num = imu_frame[2];
356 | 
357 |     q_rotvec(quat,facc_nobias,faccg);
358 | 
359 |     if(show_data)
360 |     {
361 |         printf("Num: %03d",imu_frame[2]);
362 |         printf("  Gyr: %6d,%6d,%6d",imu_data[0],imu_data[1],imu_data[2]);
363 |         printf("  Acc: %6d,%6d,%6d",(int)facc[0],(int)facc[1],(int)facc[2]);
364 |         printf("  Quat: %11f,%11f,%11f,%11f\r\n",quat[0],quat[1],quat[2],quat[3]);
365 |     }
366 | }
367 | 


--------------------------------------------------------------------------------
/camera_test/src/loitorusbcam.cpp:
--------------------------------------------------------------------------------
   1 | #include <stdio.h>
   2 | #include <stdlib.h>
   3 | #include <unistd.h>
   4 | #include <getopt.h>
   5 | #include <fcntl.h>
   6 | #include <string.h>
   7 | #include <math.h>
   8 | #include <pthread.h>
   9 | #include <sys/time.h>
  10 | #include <linux/types.h>
  11 | 
  12 | #include <iostream>
  13 | #include <algorithm>
  14 | #include <fstream>
  15 | #include <string>
  16 | #include <vector>
  17 | #include <iomanip>
  18 | #include <sstream>
  19 | 
  20 | #include <libusb-1.0/libusb.h>
  21 | 
  22 | #include "loitorimu.h"
  23 | 
  24 | typedef struct libusb_device	cyusb_device;
  25 | typedef struct libusb_device_handle	cyusb_handle;
  26 | 
  27 | /* This is the maximum number of 'devices of interest' we are willing to store as default. */
  28 | /* These are the maximum number of devices we will communicate with simultaneously */
  29 | #define MAXDEVICES        10
  30 | 
  31 | /* This is the maximum number of VID/PID pairs that this library will consider. This limits
  32 |    the number of valid VID/PID entries in the configuration file.
  33 |  */
  34 | #define MAX_ID_PAIRS    100
  35 | 
  36 | /* This is the maximum length for the description string for a device in the configuration
  37 |    file. If the actual string in the file is longer, only the first MAX_STR_LEN characters
  38 |    will be considered.
  39 |  */
  40 | #define MAX_STR_LEN     30
  41 | 
  42 | #define LOG			printf
  43 | #define	DEBUGLOG	printf
  44 | #define	CODELOG		printf("File:%s\r\nLine:%d \r\n",__FILE__,__LINE__);
  45 | 
  46 | /*PARAMETER MACRO*/
  47 | #define IMG_WIDTH_VGA 	640
  48 | #define IMG_HEIGHT_VGA 	480
  49 | #define IMG_SIZE_VGA 	(IMG_WIDTH_VGA*IMG_HEIGHT_VGA)
  50 | #define IMG_BUF_SIZE_VGA (IMG_SIZE_VGA+0x200)
  51 | #define IMG_WIDTH_WVGA 	752
  52 | #define IMG_HEIGHT_WVGA 	480
  53 | #define IMG_SIZE_WVGA 	(IMG_WIDTH_WVGA*IMG_HEIGHT_WVGA)
  54 | #define IMG_BUF_SIZE_WVGA (IMG_SIZE_WVGA+0x200)
  55 | #define FRAME_CLUST 216
  56 | 
  57 | /*CAMERA CONTROL INSTRUCTION MACRO*/
  58 | #define CAPTURE_27			0xA1
  59 | #define CAPTURE_54			0xA2
  60 | #define CAPTURE_108			0xA6
  61 | #define STANDBY_SHORT		0xA3
  62 | #define STANDBY_LONG		0xA4
  63 | #define GET_CAM_LR			0xA5
  64 | #define CAM_I2C_R			0xA7
  65 | #define CAM_I2C_W			0xA8
  66 | #define auto_expo			1
  67 | #define EXP_VAL				50
  68 | #define GAI_VAL				100
  69 | 
  70 | #define CAM_I2C_ADDR		0x5C //0x5C
  71 | 
  72 | /* Maximum length of a string read from the Configuration file (/etc/cyusb.conf) for the library. */
  73 | #define MAX_CFG_LINE_LENGTH                     (120)
  74 | 
  75 | #define IMU_FRAME_LEN 32
  76 | pthread_t imu_thread;
  77 | int imu_fd=0;
  78 | bool imu_close=false;
  79 | 
  80 | using namespace std;
  81 | 
  82 | 
  83 | visensor_imudata IMU_FIFO[200];
  84 | int imu_fifo_ct=0;
  85 | 
  86 | bool shut_down_tag=false;
  87 | 
  88 | bool left_fresh=false;
  89 | bool right_fresh=false;
  90 | 
  91 | bool allow_settings_change=true;
  92 | 
  93 | /****************************************************/
  94 | int data_mode;
  95 | int modes_settings[13][4];
  96 | int EG_mode;
  97 | int man_exp;
  98 | int man_gain;
  99 | int auto_EG_top;
 100 | int auto_EG_bottom;
 101 | int auto_EG_des;
 102 | int auto_E_man_G_Etop;
 103 | int auto_E_man_G_Ebottom;
 104 | int auto_E_man_G;
 105 | int agc_aec_skip_frame;
 106 | int VI_FIFO_matcher;
 107 | 
 108 | double imu_acc_bias_X;
 109 | double imu_acc_bias_Y;
 110 | double imu_acc_bias_Z;
 111 | 
 112 | string imu_port_name;
 113 | visensor_imudata visensor_imudata_pack;
 114 | /****************************************************/
 115 | 
 116 | cyusb_handle *pcam1_handle;
 117 | cyusb_handle *pcam2_handle;
 118 | int gFPS = 27;
 119 | int gSelectCam = 0;
 120 | int HORIZ_BLK=150;
 121 | 
 122 | pthread_t cam1_capture_thread;
 123 | pthread_t cam2_capture_thread;
 124 | 
 125 | pthread_t cam1_capture_detect_thread;
 126 | pthread_t cam2_capture_detect_thread;
 127 | 
 128 | int gFrameCam1=0;
 129 | int gFrameCam2=0;
 130 | int gImg1Pass[FRAME_CLUST];
 131 | int gImg2Pass[FRAME_CLUST];
 132 | float gImg1Time[FRAME_CLUST];		// time-stamp
 133 | float gImg2Time[FRAME_CLUST];
 134 | timeval gImg1_SysTime[FRAME_CLUST];
 135 | timeval gImg2_SysTime[FRAME_CLUST];
 136 | 
 137 | unsigned char gImg1_VGA[FRAME_CLUST][IMG_BUF_SIZE_VGA];
 138 | unsigned char gImg2_VGA[FRAME_CLUST][IMG_BUF_SIZE_VGA];
 139 | 
 140 | unsigned char gImg1_WVGA[FRAME_CLUST][IMG_BUF_SIZE_WVGA];
 141 | unsigned char gImg2_WVGA[FRAME_CLUST][IMG_BUF_SIZE_WVGA];
 142 | 
 143 | bool last_is_good=false;
 144 | bool save_img=false;
 145 | bool save_img1=false;
 146 | 
 147 | bool visensor_resolution_status=false;
 148 | int current_HB=194;
 149 | int visensor_cam_selection=0;
 150 | int current_FPS=27;
 151 | 
 152 | string visensor_settings_file_name;
 153 | 
 154 | 
 155 | struct cydev
 156 | {
 157 |     cyusb_device *dev;
 158 |     cyusb_handle *handle;
 159 |     unsigned short vid;
 160 |     unsigned short pid;         /* Product ID */
 161 |     unsigned char is_open;      /* When device is opened, val = 1 */
 162 |     unsigned char busnum;       /* The bus number of this device */
 163 |     unsigned char devaddr;      /* The device address*/
 164 |     unsigned char filler;       /* Padding to make struct = 16 bytes */
 165 | };
 166 | 
 167 | static struct cydev cydev[MAXDEVICES];
 168 | static int number_of_cameras;		/* Number of Interesting Devices	*/
 169 | 
 170 | struct VPD
 171 | {
 172 |     unsigned short vid;
 173 |     unsigned short pid;
 174 |     char desc[MAX_STR_LEN];
 175 | };
 176 | 
 177 | static struct VPD vpd[MAX_ID_PAIRS];
 178 | 
 179 | static libusb_device **libusb_device_list;
 180 | static unsigned int checksum = 0;
 181 | 
 182 | char pidfile[256];
 183 | char logfile[256];
 184 | int logfd;
 185 | int pidfd;
 186 | 
 187 | float visensor_get_hardware_fps();
 188 | 
 189 | 
 190 | void visensor_load_settings(const char* settings_file)
 191 | {
 192 |     /******************************* Load Settings File ***************************************/
 193 | 
 194 |     LOG("\r\n**********************************************\r\n");
 195 |     LOG("Loading Settings File...\r\n");
 196 | 
 197 |     visensor_settings_file_name=settings_file;
 198 | 
 199 |     ifstream ifs_settings(visensor_settings_file_name.c_str());//,ios::in|ios::binary|ios::ate);
 200 |     if(!ifs_settings )
 201 |     {
 202 |         cerr << "Error opening file: "<<visensor_settings_file_name <<endl;
 203 |         exit(EXIT_FAILURE);
 204 |     }
 205 |     else
 206 |     {
 207 |         cout<<"Successfully opened settings file!"<<endl;
 208 |     }
 209 | 
 210 |     long file_size=ifs_settings.tellg();
 211 | 
 212 |     //cout<<"filesize:"<<file_size<<endl;
 213 | 
 214 |     std::string filename;
 215 |     std::string line_settings;
 216 |     std::string timestamp;
 217 |     std::vector<string> settingsList;
 218 |     int linecount=0;
 219 |     while(std::getline(ifs_settings,line_settings))
 220 |     {
 221 |         linecount++;
 222 |         if(line_settings.at(0)!='#')
 223 |             settingsList.push_back(line_settings);
 224 |     }
 225 |     //cout <<"line get end............"<<endl;
 226 |     for (vector<string>::iterator iter = settingsList.begin(); iter != settingsList.end(); ++iter)
 227 |     {
 228 |         cout << *iter << endl;
 229 |     }
 230 |     ifs_settings.close();
 231 | 
 232 | 
 233 |     std::string token;
 234 | 
 235 |     // convert to File_Settings Object
 236 |     istringstream(settingsList.at(1))>>data_mode;
 237 |     //cout << "data_mode: "<<data_mode<<endl;
 238 | 
 239 |     for(int i=3; i<=15; i++)
 240 |     {
 241 |         if(i==9)continue;
 242 |         int visensor_cam_selection=0,_HB=0,res=0,fps=0;
 243 | 
 244 |         istringstream iss_line(settingsList.at(i));
 245 |         //The first word, ignore
 246 |         std::getline(iss_line, token, ',');
 247 | 
 248 |         //The second word, lrs
 249 |         std::getline(iss_line, token, ',');
 250 |         if(token[0]=='l')visensor_cam_selection=2;
 251 |         else if(token[0]=='r')visensor_cam_selection=1;
 252 |         else if(token[0]=='s')visensor_cam_selection=0;
 253 | 
 254 |         //_HB
 255 |         std::getline(iss_line, token, ',');
 256 |         istringstream(token)>>_HB;
 257 | 
 258 |         //WVGA or VGA?
 259 |         std::getline(iss_line, token, ',');
 260 |         res=(token[0]=='V'?0:1);
 261 | 
 262 |         //FPS
 263 |         std::getline(iss_line, token, ',');
 264 |         istringstream(token)>>fps;
 265 | 
 266 |         // fill data
 267 |         if(i<9)// “HighSpeed Pre-Set”
 268 |         {
 269 |             modes_settings[i-3][0]=visensor_cam_selection;
 270 |             modes_settings[i-3][1]=_HB;
 271 |             modes_settings[i-3][2]=res;
 272 |             modes_settings[i-3][3]=fps;
 273 |         }
 274 |         else// “Normal Pre-Set”
 275 |         {
 276 |             modes_settings[i-4][0]=visensor_cam_selection;
 277 |             modes_settings[i-4][1]=_HB;
 278 |             modes_settings[i-4][2]=res;
 279 |             modes_settings[i-4][3]=fps;
 280 |         }
 281 | 
 282 |     }
 283 | 
 284 |     // Manual Mode
 285 |     {
 286 |         int visensor_cam_selection=0,_HB=0,res=0,fps=0;
 287 | 
 288 |         istringstream(settingsList.at(17))>>visensor_cam_selection;
 289 |         istringstream(settingsList.at(18))>>_HB;
 290 |         token = settingsList.at(19);
 291 |         res=(token[0]=='V'?0:1);
 292 |         istringstream(settingsList.at(20))>>fps;
 293 | 
 294 |         modes_settings[12][0]=visensor_cam_selection;
 295 |         modes_settings[12][1]=_HB;
 296 |         modes_settings[12][2]=res;
 297 |         modes_settings[12][3]=fps;
 298 |     }
 299 | 
 300 |     // EXP&GAIN
 301 |     istringstream(settingsList.at(22))>>EG_mode;
 302 | 
 303 |     // manual E&G
 304 |     {
 305 |         int _exp=0,_gain=0;
 306 | 
 307 |         istringstream iss_line(settingsList.at(23));
 308 |         //The first word, ignore
 309 |         std::getline(iss_line, token, ',');
 310 | 
 311 |         std::getline(iss_line, token, ',');
 312 |         istringstream(token)>>_exp;
 313 |         std::getline(iss_line, token, ',');
 314 |         istringstream(token)>>_gain;
 315 | 
 316 |         // fill data
 317 |         man_exp=_exp;
 318 |         man_gain=_gain;
 319 |     }
 320 | 
 321 |     // auto
 322 |     {
 323 |         int _top=0,_bottom=0,_des=0;
 324 | 
 325 |         istringstream iss_line(settingsList.at(24));
 326 |         //The first word, ignore
 327 |         std::getline(iss_line, token, ',');
 328 | 
 329 |         std::getline(iss_line, token, ',');
 330 |         istringstream(token)>>_top;
 331 | 
 332 |         std::getline(iss_line, token, ',');
 333 |         istringstream(token)>>_bottom;
 334 | 
 335 |         std::getline(iss_line, token, ',');
 336 |         istringstream(token)>>_des;
 337 |         // fill data
 338 |         auto_EG_top=_top;
 339 |         auto_EG_bottom=_bottom;
 340 |         auto_EG_des=_des;
 341 |     }
 342 | 
 343 |     // AE_MG
 344 |     {
 345 |         int _top=0,_bottom=0,_des=0,_gain=0;
 346 | 
 347 |         istringstream iss_line(settingsList.at(25));
 348 | 
 349 |         //The first word, ignore
 350 |         std::getline(iss_line, token, ',');
 351 | 
 352 |         std::getline(iss_line, token, ',');
 353 |         istringstream(token)>>_top;
 354 |         std::getline(iss_line, token, ',');
 355 |         istringstream(token)>>_bottom;
 356 |         std::getline(iss_line, token, ',');
 357 |         istringstream(token)>>_des;
 358 |         std::getline(iss_line, token, ',');
 359 |         istringstream(token)>>_gain;
 360 |         // fill data
 361 |         auto_E_man_G_Etop=_top;
 362 |         auto_E_man_G_Ebottom=_bottom;
 363 |         auto_E_man_G=_gain;
 364 |     }
 365 |     // imu_port_name
 366 |     {
 367 |         istringstream iss_line(settingsList.at(26));
 368 |         std::getline(iss_line, token, ',');
 369 | 
 370 |         imu_port_name = token.c_str();
 371 | 
 372 |         std::getline(iss_line, token, ',');
 373 |         istringstream(token)>>VI_FIFO_matcher;
 374 |     }
 375 |     // imu acc bias
 376 |     {
 377 |         istringstream iss_line(settingsList.at(28));
 378 |         //The first word, ignore
 379 |         std::getline(iss_line, token, ',');
 380 |         std::getline(iss_line, token, ',');
 381 |         istringstream(token)>>imu_acc_bias_X;
 382 |     }
 383 |     {
 384 |         istringstream iss_line(settingsList.at(29));
 385 |         //The first word, ignore
 386 |         std::getline(iss_line, token, ',');
 387 |         std::getline(iss_line, token, ',');
 388 |         istringstream(token)>>imu_acc_bias_Y;
 389 |     }
 390 |     {
 391 |         istringstream iss_line(settingsList.at(30));
 392 |         //The first word, ignore
 393 |         std::getline(iss_line, token, ',');
 394 |         std::getline(iss_line, token, ',');
 395 |         istringstream(token)>>imu_acc_bias_Z;
 396 |     }
 397 | 
 398 |     // show settings
 399 |     cout<<endl<<"data_mode : "<<data_mode<<endl;
 400 |     for(int i=0; i<13; i++)
 401 |     {
 402 |         cout<<"mode : "<<(i+1)<<endl;
 403 |         cout<<"fps : "<<modes_settings[i][3]<<endl;
 404 |         cout<<"HB : "<<modes_settings[i][1]<<endl;
 405 |         cout<<"visensor_cam_selection : "<<modes_settings[i][0]<<endl;
 406 |         cout<<"res : "<<modes_settings[i][2]<<endl<<endl;
 407 |     }
 408 |     cout<<"EG_mode : "<<EG_mode<<endl;
 409 |     cout<<"man_exp : "<<man_exp<<endl;
 410 |     cout<<"man_gain : "<<man_gain<<endl;
 411 |     cout<<"auto_EG_top : "<<auto_EG_top<<endl;
 412 |     cout<<"auto_EG_bottom : "<<auto_EG_bottom<<endl;
 413 |     cout<<"auto_EG_des : "<<auto_EG_des<<endl;
 414 |     cout<<"auto_E_man_G_Etop : "<<auto_E_man_G_Etop<<endl;
 415 |     cout<<"auto_E_man_G_Ebottom : "<<auto_E_man_G_Ebottom<<endl;
 416 |     cout<<"auto_E_man_G : "<<auto_E_man_G<<endl;
 417 |     cout<<"VI_FIFO_matcher : "<<VI_FIFO_matcher<<endl;
 418 |     cout<<"imu_acc_bias_X :  "<<imu_acc_bias_X<<endl;
 419 |     cout<<"imu_acc_bias_Y :  "<<imu_acc_bias_Y<<endl;
 420 |     cout<<"imu_acc_bias_Z :  "<<imu_acc_bias_Z<<endl;
 421 |     cout<<"imu_port_name :  "<<imu_port_name<<endl<<endl;
 422 | 
 423 |     // 预设模式+手动模式
 424 |     current_FPS=modes_settings[data_mode-1][3];
 425 |     gFPS = current_FPS;
 426 |     visensor_resolution_status=modes_settings[data_mode-1][2];
 427 |     current_HB=modes_settings[data_mode-1][1];
 428 |     visensor_cam_selection=modes_settings[data_mode-1][0];
 429 | 
 430 |     /****************************************************************************/
 431 | 
 432 | 
 433 |     if(visensor_cam_selection==0)gSelectCam = 3;
 434 |     else if(visensor_cam_selection==2)gSelectCam = 1;
 435 |     else if(visensor_cam_selection==1)gSelectCam = 2;
 436 | 
 437 |     //Add: AGC/AEC Skip frames
 438 |     agc_aec_skip_frame = (int)(visensor_get_hardware_fps()/10);
 439 | }
 440 | 
 441 | // setters
 442 | void visensor_set_auto_EG(int E_AG)
 443 | {
 444 |     if(!allow_settings_change)
 445 |     {
 446 |         cout<<"settings FIXED !"<<endl;
 447 |         return;
 448 |     }
 449 |     EG_mode=E_AG;
 450 | }
 451 | void visensor_set_exposure(int _man_exp)
 452 | {
 453 |     if(!allow_settings_change)
 454 |     {
 455 |         cout<<"settings FIXED !"<<endl;
 456 |         return;
 457 |     }
 458 |     man_exp=_man_exp;
 459 | }
 460 | void visensor_set_gain(int _man_gain)
 461 | {
 462 |     if(!allow_settings_change)
 463 |     {
 464 |         cout<<"settings FIXED !"<<endl;
 465 |         return;
 466 |     }
 467 |     man_gain=_man_gain;
 468 |     auto_E_man_G=_man_gain;
 469 | }
 470 | void visensor_set_max_autoExp(int max_exp)
 471 | {
 472 |     if(!allow_settings_change)
 473 |     {
 474 |         cout<<"settings FIXED !"<<endl;
 475 |         return;
 476 |     }
 477 |     auto_EG_top=max_exp;
 478 | }
 479 | void visensor_set_min_autoExp(int min_exp)
 480 | {
 481 |     if(!allow_settings_change)
 482 |     {
 483 |         cout<<"settings FIXED !"<<endl;
 484 |         return;
 485 |     }
 486 |     auto_EG_bottom=min_exp;
 487 | }
 488 | void visensor_set_resolution(bool set_wvga)
 489 | {
 490 |     if(!allow_settings_change)
 491 |     {
 492 |         cout<<"settings FIXED !"<<endl;
 493 |         return;
 494 |     }
 495 |     visensor_resolution_status=set_wvga;
 496 | }
 497 | void visensor_set_fps_mode(bool fps_mode)
 498 | {
 499 |     if(!allow_settings_change)
 500 |     {
 501 |         cout<<"settings FIXED !"<<endl;
 502 |         return;
 503 |     }
 504 |     if(fps_mode==true)
 505 |     {
 506 |         gFPS=54;
 507 |         current_FPS=54;
 508 |     }
 509 |     else
 510 |     {
 511 |         gFPS=27;
 512 |         current_FPS=27;
 513 |     }
 514 | }
 515 | void visensor_set_current_HB(int HB)
 516 | {
 517 |     if(!allow_settings_change)
 518 |     {
 519 |         cout<<"settings FIXED !"<<endl;
 520 |         return;
 521 |     }
 522 |     current_HB=HB;
 523 | }
 524 | void visensor_set_desired_bin(int db)
 525 | {
 526 |     if(!allow_settings_change)
 527 |     {
 528 |         cout<<"settings FIXED !"<<endl;
 529 |         return;
 530 |     }
 531 |     auto_EG_des=db;
 532 | }
 533 | void visensor_set_cam_selection_mode(int _visensor_cam_selection)
 534 | {
 535 |     if(!allow_settings_change)
 536 |     {
 537 |         cout<<"settings FIXED !"<<endl;
 538 |         return;
 539 |     }
 540 |     visensor_cam_selection=_visensor_cam_selection;
 541 | }
 542 | void visensor_set_imu_bias(float bx,float by,float bz)
 543 | {
 544 |     if(!allow_settings_change)
 545 |     {
 546 |         cout<<"settings FIXED !"<<endl;
 547 |         return;
 548 |     }
 549 |     imu_acc_bias_X=bx;
 550 |     imu_acc_bias_Y=by;
 551 |     imu_acc_bias_Z=bz;
 552 | }
 553 | void visensor_set_imu_portname(char* input_name)
 554 | {
 555 |     if(!allow_settings_change)
 556 |     {
 557 |         cout<<"settings FIXED !"<<endl;
 558 |         return;
 559 |     }
 560 |     imu_port_name=input_name;
 561 | }
 562 | void visensor_set_current_mode(int _mode)
 563 | {
 564 |     if(!allow_settings_change||_mode>13||_mode<=0)
 565 |     {
 566 |         cout<<"settings FIXED !"<<endl;
 567 |         return;
 568 |     }
 569 |     data_mode=_mode;
 570 |     // 预设模式+手动模式
 571 |     current_FPS=modes_settings[data_mode-1][3];
 572 |     gFPS=current_FPS;
 573 |     visensor_resolution_status=modes_settings[data_mode-1][2];
 574 |     current_HB=modes_settings[data_mode-1][1];
 575 |     visensor_cam_selection=modes_settings[data_mode-1][0];
 576 | }
 577 | // getters
 578 | int visensor_get_EG_mode()
 579 | {
 580 |     return EG_mode;
 581 | }
 582 | int visensor_get_exposure()
 583 | {
 584 |     return man_exp;
 585 | }
 586 | int visensor_get_gain()
 587 | {
 588 |     return man_gain;
 589 | }
 590 | int visensor_get_max_autoExp()
 591 | {
 592 |     return auto_EG_top;
 593 | }
 594 | int visensor_get_min_autoExp()
 595 | {
 596 |     return auto_EG_bottom;
 597 | }
 598 | bool visensor_get_resolution()
 599 | {
 600 |     return visensor_resolution_status;
 601 | }
 602 | int visensor_get_fps()
 603 | {
 604 |     return current_FPS;
 605 | }
 606 | int visensor_get_current_HB()
 607 | {
 608 |     return current_HB;
 609 | }
 610 | int visensor_get_desired_bin()
 611 | {
 612 |     return auto_EG_des;
 613 | }
 614 | int visensor_get_cam_selection_mode()
 615 | {
 616 |     return visensor_cam_selection;
 617 | }
 618 | float visensor_get_imu_G_bias_x()
 619 | {
 620 |     return imu_acc_bias_X;
 621 | }
 622 | float visensor_get_imu_G_bias_y()
 623 | {
 624 |     return imu_acc_bias_Y;
 625 | }
 626 | float get_imu_bias_z()
 627 | {
 628 |     return imu_acc_bias_Z;
 629 | }
 630 | const char* visensor_get_imu_portname()
 631 | {
 632 |     return imu_port_name.c_str();
 633 | }
 634 | 
 635 | void visensor_save_current_settings()
 636 | {
 637 |     // 保存设置参数到原配置文件
 638 |     ofstream f;
 639 |     f.open(visensor_settings_file_name.c_str());
 640 |     f.close();
 641 |     f.open(visensor_settings_file_name.c_str());
 642 | 
 643 |     f << fixed;
 644 |     // start writing settings file
 645 |     f<<'#'<<endl<<"Mode"<<endl;
 646 |     f<<data_mode<<endl<<'#'<<endl<<"HighSpeed Pre-Set"<<endl;
 647 |     f<<"m1,left,"<<modes_settings[0][1]<<",VGA,108"<<endl;
 648 |     f<<"m2,right,"<<modes_settings[1][1]<<",VGA,108"<<endl;
 649 |     f<<"m3,left,"<<modes_settings[2][1]<<",WVGA,108"<<endl;
 650 |     f<<"m4,right,"<<modes_settings[3][1]<<",WVGA,108"<<endl;
 651 |     f<<"m5,stereo,"<<modes_settings[4][1]<<",VGA,54"<<endl;
 652 |     f<<"m6,stereo,"<<modes_settings[5][1]<<",WVGA,54"<<endl;
 653 | 
 654 |     f<<"#"<<endl<<"Normal Pre-Set"<<endl;
 655 | 
 656 |     f<<"m7,left,"<<modes_settings[6][1]<<",VGA,54"<<endl;
 657 |     f<<"m8,right,"<<modes_settings[7][1]<<",VGA,54"<<endl;
 658 |     f<<"m9,left,"<<modes_settings[8][1]<<",WVGA,54"<<endl;
 659 |     f<<"m10,right,"<<modes_settings[9][1]<<",WVGA,54"<<endl;
 660 |     f<<"m11,stereo,"<<modes_settings[10][1]<<",VGA,27"<<endl;
 661 |     f<<"m12,stereo,"<<modes_settings[11][1]<<",WVGA,27"<<endl;
 662 | 
 663 |     f<<"#"<<endl<<"m13,Manual Mode"<<endl<<modes_settings[12][0]
 664 |      <<endl<<modes_settings[12][1]<<endl;
 665 |     if(modes_settings[12][2]==0)f<<"VGA"<<endl;
 666 |     else f<<"WVGA"<<endl;
 667 |     f<<modes_settings[12][3]<<endl;
 668 | 
 669 |     f<<"#"<<endl<<"EG_mode"<<endl<<EG_mode<<endl;
 670 |     f<<"manual,"<<man_exp<<","<<man_gain<<endl;
 671 |     f<<"auto,"<<auto_EG_top<<","<<auto_EG_bottom<<","<<auto_EG_des<<endl;
 672 |     f<<"autoexp_manualgain,"<<auto_E_man_G_Etop<<","<<auto_E_man_G_Ebottom<<","<<auto_EG_des<<","<<auto_E_man_G<<endl;
 673 | 
 674 | 
 675 |     f<<imu_port_name<<","<<VI_FIFO_matcher<<endl<<"#"<<endl<<"IMU-acc-bias"<<endl;
 676 |     f<<"Gx,"<<imu_acc_bias_X<<endl;
 677 |     f<<"Gy,"<<imu_acc_bias_Y<<endl;
 678 | 
 679 |     f<<"Gz,"<<imu_acc_bias_Z<<endl<<"#"<<endl;
 680 | 
 681 |     f.close();
 682 | }
 683 | 
 684 | 
 685 | static int device_is_of_interest(cyusb_device *d)
 686 | {
 687 |     int found = 0;
 688 |     struct libusb_device_descriptor desc;
 689 |     libusb_get_device_descriptor(d, &desc);
 690 |     if ( (0x04b4 == desc.idVendor) && (0x1003 == desc.idProduct) )
 691 |         found = 1;
 692 |     return found;
 693 | }
 694 | 
 695 | int cyusb_open(void)
 696 | {
 697 |     int r;
 698 | 
 699 |     r = libusb_init(NULL);
 700 |     if (r)
 701 |     {
 702 |         printf("Error in initializing libusb library...\n");
 703 |         return -2;
 704 |     }
 705 | 
 706 |     int numdev = libusb_get_device_list(NULL, &libusb_device_list);
 707 |     if ( numdev < 0 )
 708 |     {
 709 |         printf("Library: Error in enumerating devices...\n");
 710 |         return -4;
 711 |     }
 712 | 
 713 |     number_of_cameras = 0;
 714 | 
 715 |     for (int i = 0; i < numdev; ++i )
 716 |     {
 717 |         cyusb_device *tdev = libusb_device_list[i];
 718 |         if ( device_is_of_interest(tdev) )
 719 |         {
 720 |             cydev[number_of_cameras].dev = tdev;
 721 |             r = libusb_open(tdev, &cydev[number_of_cameras].handle);
 722 |             if ( r )
 723 |             {
 724 |                 printf("Error in opening device, r=%d\n",r);
 725 |                 return -5;
 726 |             }
 727 |             ++number_of_cameras;
 728 |         }
 729 |         if(number_of_cameras>=MAXDEVICES)
 730 |             break;
 731 |     }
 732 |     return number_of_cameras;
 733 | }
 734 | 
 735 | cyusb_handle * cyusb_gethandle(int index)
 736 | {
 737 |     return cydev[index].handle;
 738 | }
 739 | 
 740 | int cyusb_control_transfer(cyusb_handle *h, unsigned char bmRequestType, unsigned char bRequest,
 741 |                            unsigned short wValue, unsigned short wIndex, unsigned char *data, unsigned short wLength,
 742 |                            unsigned int timeout)
 743 | {
 744 |     return ( libusb_control_transfer(h, bmRequestType, bRequest, wValue, wIndex, data, wLength, timeout) );
 745 | }
 746 | 
 747 | int cyusb_bulk_transfer(cyusb_handle *h, unsigned char endpoint, unsigned char *data, int length,
 748 |                         int *transferred, int timeout)
 749 | {
 750 |     return ( libusb_bulk_transfer(h, endpoint, data, length, transferred, timeout) );
 751 | }
 752 | 
 753 | void cyusb_close(void)
 754 | {
 755 |     for (int i = 0; i < number_of_cameras; ++i )
 756 |     {
 757 |         libusb_close(cydev[i].handle);
 758 |     }
 759 |     libusb_free_device_list(libusb_device_list, 1);
 760 |     libusb_exit(NULL);
 761 | }
 762 | 
 763 | /*****************************************************************************/
 764 | 
 765 | int fps_control()
 766 | {
 767 |     switch(gFPS)
 768 |     {
 769 |     case 27:
 770 |         return CAPTURE_27;
 771 |         break;
 772 |     case 54:
 773 |         return CAPTURE_54;
 774 |         break;
 775 |     case 108:
 776 |         return CAPTURE_108;
 777 |         break;
 778 |     default:
 779 |         return CAPTURE_27;
 780 |         break;
 781 |     }
 782 |     return CAPTURE_27;
 783 | }
 784 | 
 785 | cyusb_handle* get_cam_no_handle(int cam_no)
 786 | {
 787 |     if(cam_no==1)
 788 |         return pcam1_handle;
 789 |     else if(cam_no==2)
 790 |         return pcam2_handle;
 791 |     else
 792 |     {
 793 |         printf("Wrong cam number!\r\n");
 794 |         return NULL;
 795 |     }
 796 | }
 797 | 
 798 | int control_camera(int cam_no, unsigned char instruction)
 799 | {
 800 |     unsigned char buf[1];
 801 |     int r = cyusb_control_transfer(get_cam_no_handle(cam_no),0xC0,instruction,0,0,buf,0,1000);
 802 |     if(r != 0)
 803 |     {
 804 |         //printf("cam%d,i:0x%02X,r:%d\r\n",cam_no,instruction,r);
 805 |     }
 806 |     return r;
 807 | }
 808 | 
 809 | int camera_i2c_read(int cam_no, unsigned char reg,int* value)
 810 | {
 811 |     unsigned char buf[3];
 812 |     int r = cyusb_control_transfer(get_cam_no_handle(cam_no),0xC0,CAM_I2C_R,(CAM_I2C_ADDR<<8)+reg,0,buf,3,1000);
 813 |     if(r != 3)
 814 |     {
 815 |         //printf("cam%d,i:0x%02X,r:%d\r\n",cam_no,CAM_I2C_R,r);
 816 |         return r;
 817 |     }
 818 |     if(buf[0]!=0)
 819 |     {
 820 |         //printf("I2C Read failed...Cam:%d, Addr:0x%02X, Reg:0x%02X, I2C_return:%d\r\n",cam_no,CAM_I2C_ADDR,reg,buf[0]);
 821 |         return r;
 822 |     }
 823 |     *value = buf[1]+buf[2]<<8;
 824 |     //printf("I2C Read Success...Cam:%d, Addr:0x%02X, Reg:0x%02X, Result:0x%04X\r\n",cam_no,CAM_I2C_ADDR,reg,*value);
 825 |     return r;
 826 | }
 827 | 
 828 | int camera_i2c_write(int cam_no, unsigned char reg,int value)
 829 | {
 830 |     unsigned char buf[1];
 831 |     int r = cyusb_control_transfer(get_cam_no_handle(cam_no),0xC0,CAM_I2C_W,(CAM_I2C_ADDR<<8)+reg,value,buf,1,1000);
 832 |     if(r != 1)
 833 |     {
 834 |         //printf("cam%d,i:0x%02X,r:%d\r\n",cam_no,CAM_I2C_W,r);
 835 |         return r;
 836 |     }
 837 |     if(buf[0]!=0)
 838 |     {
 839 |         //printf("I2C Write failed...Cam:%d, Addr:0x%02X, Reg:0x%02X, Write:0x%04X, I2C_return:%d\r\n",cam_no,CAM_I2C_ADDR,reg,value,buf[0]);
 840 |         return r;
 841 |     }
 842 |     //printf("I2C Write Success...Cam:%d, Addr:0x%02X, Reg:0x%02X, Write:0x%04X\r\n",cam_no,CAM_I2C_ADDR,reg,value);
 843 |     return r;
 844 | }
 845 | 
 846 | int check_img(int cam_no,unsigned char *pImg,int *pImgPass)
 847 | {
 848 |     if(!visensor_resolution_status)*pImgPass = (pImg[IMG_SIZE_VGA+0]==0xFF&&pImg[IMG_SIZE_VGA+1]==0x00&&pImg[IMG_SIZE_VGA+2]==0xFE&&pImg[IMG_SIZE_VGA+3]==0x01);
 849 |     else *pImgPass = (pImg[IMG_SIZE_WVGA+0]==0xFF&&pImg[IMG_SIZE_WVGA+1]==0x00&&pImg[IMG_SIZE_WVGA+2]==0xFE&&pImg[IMG_SIZE_WVGA+3]==0x01);
 850 |     if(*pImgPass == 0)
 851 |     {
 852 |         control_camera(cam_no,fps_control());
 853 |         //printf("Img%d check error!\r\n",cam_no);
 854 |     }
 855 | }
 856 | 
 857 | float visensor_get_hardware_fps()
 858 | {
 859 |     if(gFPS==54)
 860 |     {
 861 |         if(!visensor_resolution_status)return 45700.0f/(current_HB+640.0f);
 862 |         else return 45700.0f/(current_HB+752.0f);
 863 |     }
 864 |     else if(gFPS==27)
 865 |     {
 866 |         if(!visensor_resolution_status)return 0.5f*45700.0f/(current_HB+640.0f);
 867 |         else return 0.5f*45700.0f/(current_HB+752.0f);
 868 |     }
 869 | }
 870 | static void set_camreg_default(int cam_no)
 871 | {
 872 |     camera_i2c_write(cam_no,0x00,0x1324);
 873 |     camera_i2c_write(cam_no,0x01,0x0001);
 874 |     camera_i2c_write(cam_no,0x02,0x0004);
 875 |     camera_i2c_write(cam_no,0x03,0x01E0);
 876 | 
 877 |     camera_i2c_write(cam_no,0x04,0x02F0);
 878 |     camera_i2c_write(cam_no,0x05,0x005E);
 879 |     camera_i2c_write(cam_no,0x06,0x002D);
 880 |     camera_i2c_write(cam_no,0x07,0x0388);
 881 | 
 882 |     camera_i2c_write(cam_no,0x0B,0x01E0);
 883 |     camera_i2c_write(cam_no,0x35,0x0010);
 884 |     camera_i2c_write(cam_no,0xA5,0x003A);
 885 |     camera_i2c_write(cam_no,0xA6,0x0002);
 886 | 
 887 |     camera_i2c_write(cam_no,0xA8,0x0000);
 888 |     camera_i2c_write(cam_no,0xA9,0x0002);
 889 |     camera_i2c_write(cam_no,0xAA,0x0002);
 890 |     camera_i2c_write(cam_no,0xAB,0x0040);
 891 | 
 892 |     camera_i2c_write(cam_no,0xAC,0x0001);
 893 |     camera_i2c_write(cam_no,0xAD,0x01E0);
 894 |     camera_i2c_write(cam_no,0xAE,0x0014);
 895 |     camera_i2c_write(cam_no,0xAF,0x0003);
 896 | }
 897 | 
 898 | void *cam1_capture(void*)
 899 | {
 900 |     set_camreg_default(1);
 901 |     int r,transferred = 0;
 902 |     camera_i2c_write(1,0x07,0x0188);//Normal
 903 |     camera_i2c_write(1,0x06,0x002D);//VB
 904 |     // cmos设置
 905 |     camera_i2c_write(1,0x05,current_HB);//HB
 906 |     if(!visensor_resolution_status)camera_i2c_write(1,0x04,IMG_WIDTH_VGA);//HW
 907 |     else camera_i2c_write(1,0x04,IMG_WIDTH_WVGA);//HW
 908 |     switch (EG_mode) {
 909 |     case 0:
 910 |         camera_i2c_write(1,0xAF,0x00);//AEC
 911 |         camera_i2c_write(1,0x0B,man_exp);//Exposure Time
 912 |         camera_i2c_write(1,0x35,man_gain);//Gain
 913 |         break;
 914 |     case 1:
 915 |         camera_i2c_write(1,0xAF,0x03);//AEC
 916 |         camera_i2c_write(1,0xA5,auto_EG_des);//AEC
 917 |         camera_i2c_write(1,0xA6,0x01);//AEC
 918 |         camera_i2c_write(1,0xA8,0x00);//AEC
 919 |         camera_i2c_write(1,0xAC,auto_EG_bottom);//AEC
 920 |         camera_i2c_write(1,0xAD,auto_EG_top);//AEC
 921 |         camera_i2c_write(1,0xAE,2);//AEC
 922 |         break;
 923 |     case 2:
 924 |         camera_i2c_write(1,0xAF,0x01);//AEC
 925 |         camera_i2c_write(1,0xA5,auto_EG_des);//AEC
 926 |         camera_i2c_write(1,0xA6,0x01);//AEC
 927 |         camera_i2c_write(1,0xA8,0x00);//AEC
 928 |         camera_i2c_write(1,0xAC,auto_E_man_G_Ebottom);//AEC
 929 |         camera_i2c_write(1,0xAD,auto_E_man_G_Etop);//AEC
 930 |         camera_i2c_write(1,0xAE,2);//AEC
 931 |         camera_i2c_write(1,0x35,auto_E_man_G);//Gain
 932 |         break;
 933 |     case 3:
 934 |         break;
 935 |     case 4:
 936 |         camera_i2c_write(1,0xA6,agc_aec_skip_frame);
 937 |         camera_i2c_write(1,0xA8,2);
 938 |         camera_i2c_write(1,0xA9,agc_aec_skip_frame);
 939 |         camera_i2c_write(1,0xAA,2);
 940 |         break;
 941 |     default:
 942 |         break;
 943 |     }
 944 | 
 945 |     control_camera(1,fps_control());
 946 |     // usleep(100);
 947 | 
 948 |     static int sync_ct=0;
 949 | 
 950 |     struct timeval endTime;
 951 |     float fTimeuse=0;
 952 | 
 953 |     int ctt=0;
 954 | 
 955 |     float time_offset=1.0f/visensor_get_hardware_fps();
 956 |     control_camera(1,STANDBY_SHORT);
 957 |     while(!shut_down_tag)
 958 |     {
 959 |         ctt++;
 960 |         if(ctt>1)	//该数值越大，则帧同步的周期越长，同步频率越小
 961 |         {
 962 |             control_camera(1,STANDBY_SHORT);
 963 |             ctt = 0;
 964 |         }
 965 |         for(gFrameCam1=0; !shut_down_tag&&gFrameCam1<FRAME_CLUST; gFrameCam1++)
 966 |         {
 967 |             if(!visensor_resolution_status)
 968 |             {
 969 |                 r = cyusb_bulk_transfer(pcam1_handle, 0x82, gImg1_VGA[gFrameCam1], IMG_BUF_SIZE_VGA, &transferred, 1000);
 970 | 
 971 |                 // time-stamp
 972 |                 gettimeofday(&endTime,NULL);
 973 |                 gettimeofday(&gImg1_SysTime[gFrameCam1],NULL);
 974 |                 gImg1_SysTime[gFrameCam1].tv_usec-=1000000*time_offset;
 975 |                 if(gImg1_SysTime[gFrameCam1].tv_usec<0)
 976 |                 {
 977 |                     gImg1_SysTime[gFrameCam1].tv_usec+=1000000;
 978 |                     gImg1_SysTime[gFrameCam1].tv_sec-=1;
 979 |                 }
 980 |                 fTimeuse = (endTime.tv_sec - visensor_startTime.tv_sec) + 0.000001*(endTime.tv_usec - visensor_startTime.tv_usec);
 981 |                 fTimeuse-=time_offset;	// 真正的图像拍摄时间
 982 |                 gImg1Time[gFrameCam1]=fTimeuse;
 983 | 
 984 |                 if(r)printf("cam1 bulk transfer returned: %d\r\n",r);
 985 |                 check_img(1,gImg1_VGA[gFrameCam1],&gImg1Pass[gFrameCam1]);
 986 |                 if(gImg1Pass[gFrameCam1])left_fresh=true;
 987 |             }
 988 |             else
 989 |             {
 990 |                 r = cyusb_bulk_transfer(pcam1_handle, 0x82, gImg1_WVGA[gFrameCam1], IMG_BUF_SIZE_WVGA, &transferred, 1000);
 991 | 
 992 |                 // time-stamp
 993 |                 gettimeofday(&endTime,NULL);
 994 |                 gettimeofday(&gImg1_SysTime[gFrameCam1],NULL);
 995 |                 gImg1_SysTime[gFrameCam1].tv_usec-=1000000*time_offset;
 996 |                 if(gImg1_SysTime[gFrameCam1].tv_usec<0)
 997 |                 {
 998 |                     gImg1_SysTime[gFrameCam1].tv_usec+=1000000;
 999 |                     gImg1_SysTime[gFrameCam1].tv_sec-=1;
1000 |                 }
1001 |                 fTimeuse = (endTime.tv_sec - visensor_startTime.tv_sec) + 0.000001*(endTime.tv_usec - visensor_startTime.tv_usec);
1002 |                 fTimeuse-=time_offset;	// 真正的图像拍摄时间
1003 |                 gImg1Time[gFrameCam1]=fTimeuse;
1004 | 
1005 |                 if(r)printf("cam1 bulk transfer returned: %d\r\n",r);
1006 |                 check_img(1,gImg1_WVGA[gFrameCam1],&gImg1Pass[gFrameCam1]);
1007 |                 if(gImg1Pass[gFrameCam1])left_fresh=true;
1008 |             }
1009 |         }
1010 | 
1011 |         gettimeofday(&endTime,NULL);
1012 |         fTimeuse = (endTime.tv_sec - visensor_startTime.tv_sec) + 0.000001*(endTime.tv_usec - visensor_startTime.tv_usec);
1013 |         //printf("Cam1 frame rate: %5.1f FPS\r\n",FRAME_CLUST/fTimeuse);
1014 |     }
1015 |     pthread_exit(NULL);
1016 | }
1017 | 
1018 | void *cam2_capture(void*)
1019 | {
1020 |     set_camreg_default(2);
1021 |     int r,transferred = 0;
1022 |     camera_i2c_write(2,0x07,0x0188);//Normal
1023 |     camera_i2c_write(2,0x06,0x002D);//VB
1024 |     // cmos设置
1025 |     camera_i2c_write(2,0x05,current_HB);//HB
1026 |     if(!visensor_resolution_status)camera_i2c_write(2,0x04,IMG_WIDTH_VGA);//HW
1027 |     else camera_i2c_write(2,0x04,IMG_WIDTH_WVGA);//HW
1028 |     switch (EG_mode) {
1029 |     case 0:
1030 |         camera_i2c_write(2,0xAF,0x00);//AEC
1031 |         camera_i2c_write(2,0x0B,man_exp);//Exposure Time
1032 |         camera_i2c_write(2,0x35,man_gain);//Gain
1033 |         break;
1034 |     case 1:
1035 |         camera_i2c_write(2,0xAF,0x03);//AEC
1036 |         camera_i2c_write(2,0xA5,auto_EG_des);//AEC
1037 |         camera_i2c_write(2,0xA6,0x01);//AEC
1038 |         camera_i2c_write(2,0xA8,0x00);//AEC
1039 |         camera_i2c_write(2,0xAC,auto_EG_bottom);//AEC
1040 |         camera_i2c_write(2,0xAD,auto_EG_top);//AEC
1041 |         camera_i2c_write(2,0xAE,2);//AEC
1042 |         break;
1043 |     case 2:
1044 |         camera_i2c_write(2,0xAF,0x01);//AEC
1045 |         camera_i2c_write(2,0xA5,auto_EG_des);//AEC
1046 |         camera_i2c_write(2,0xA6,0x01);//AEC
1047 |         camera_i2c_write(2,0xA8,0x00);//AEC
1048 |         camera_i2c_write(2,0xAC,auto_E_man_G_Ebottom);//AEC
1049 |         camera_i2c_write(2,0xAD,auto_E_man_G_Etop);//AEC
1050 |         camera_i2c_write(2,0xAE,2);//AEC
1051 |         camera_i2c_write(2,0x35,auto_E_man_G);//Gain
1052 |         break;
1053 |     case 3:
1054 |         break;
1055 |     case 4:
1056 |         camera_i2c_write(2,0xA6,agc_aec_skip_frame);
1057 |         camera_i2c_write(2,0xA8,2);
1058 |         camera_i2c_write(2,0xA9,agc_aec_skip_frame);
1059 |         camera_i2c_write(2,0xAA,2);
1060 |         break;
1061 |     default:
1062 |         break;
1063 |     }
1064 | 
1065 |     control_camera(2,fps_control());
1066 |     // usleep(100);
1067 |     struct timeval endTime;
1068 |     float fTimeuse;
1069 | 
1070 |     float time_offset=1.0f/visensor_get_hardware_fps();
1071 | 
1072 |     while(!shut_down_tag)
1073 |     {
1074 |         for(gFrameCam2=0; !shut_down_tag&&gFrameCam2<FRAME_CLUST; gFrameCam2++)
1075 |         {
1076 |             if(!visensor_resolution_status)
1077 |             {
1078 |                 r = cyusb_bulk_transfer(pcam2_handle, 0x82, gImg2_VGA[gFrameCam2], IMG_BUF_SIZE_VGA, &transferred, 1000);
1079 | 
1080 |                 // time-stamp
1081 |                 gettimeofday(&endTime,NULL);
1082 |                 gettimeofday(&gImg2_SysTime[gFrameCam2],NULL);
1083 |                 gImg2_SysTime[gFrameCam2].tv_usec-=1000000*time_offset;
1084 |                 if(gImg2_SysTime[gFrameCam2].tv_usec<0)
1085 |                 {
1086 |                     gImg2_SysTime[gFrameCam2].tv_usec+=1000000;
1087 |                     gImg2_SysTime[gFrameCam2].tv_sec-=1;
1088 |                 }
1089 |                 fTimeuse = (endTime.tv_sec - visensor_startTime.tv_sec) + 0.000001*(endTime.tv_usec - visensor_startTime.tv_usec);
1090 |                 fTimeuse-=time_offset;	// 真正的图像拍摄时间
1091 |                 gImg2Time[gFrameCam2]=fTimeuse;
1092 | 
1093 |                 if(r)printf("cam2 bulk transfer returned: %d\r\n",r);
1094 |                 check_img(2,gImg2_VGA[gFrameCam2],&gImg2Pass[gFrameCam2]);
1095 |                 if(gImg2Pass[gFrameCam2])right_fresh=true;
1096 |             }
1097 |             else
1098 |             {
1099 |                 r = cyusb_bulk_transfer(pcam2_handle, 0x82, gImg2_WVGA[gFrameCam2], IMG_BUF_SIZE_WVGA, &transferred, 1000);
1100 | 
1101 |                 // time-stamp
1102 |                 gettimeofday(&endTime,NULL);
1103 |                 gettimeofday(&gImg2_SysTime[gFrameCam2],NULL);
1104 |                 gImg2_SysTime[gFrameCam2].tv_usec-=1000000*time_offset;
1105 |                 if(gImg2_SysTime[gFrameCam2].tv_usec<0)
1106 |                 {
1107 |                     gImg2_SysTime[gFrameCam2].tv_usec+=1000000;
1108 |                     gImg2_SysTime[gFrameCam2].tv_sec-=1;
1109 |                 }
1110 |                 fTimeuse = (endTime.tv_sec - visensor_startTime.tv_sec) + 0.000001*(endTime.tv_usec - visensor_startTime.tv_usec);
1111 |                 fTimeuse-=time_offset;	// 真正的图像拍摄时间
1112 |                 gImg2Time[gFrameCam2]=fTimeuse;
1113 | 
1114 |                 if(r)printf("cam2 bulk transfer returned: %d\r\n",r);
1115 |                 check_img(2,gImg2_WVGA[gFrameCam2],&gImg2Pass[gFrameCam2]);
1116 |                 if(gImg2Pass[gFrameCam2])right_fresh=true;
1117 |             }
1118 |         }
1119 |         gettimeofday(&endTime,NULL);
1120 |         fTimeuse = (endTime.tv_sec - visensor_startTime.tv_sec) + 0.000001*(endTime.tv_usec - visensor_startTime.tv_usec);
1121 |         //printf("Cam2 frame rate: %5.1f FPS\r\n",FRAME_CLUST/fTimeuse);
1122 |     }
1123 |     pthread_exit(NULL);
1124 | }
1125 | 
1126 | 
1127 | bool visensor_is_stereo_good()
1128 | {
1129 |     int img1_pointer = (gFrameCam1 + FRAME_CLUST - 1) % FRAME_CLUST;
1130 |     int img2_pointer = (gFrameCam2 + FRAME_CLUST - 1) % FRAME_CLUST;
1131 |     if(gImg2Pass[img2_pointer]&&gImg1Pass[img1_pointer])return true;
1132 |     else return false;
1133 | }
1134 | bool visensor_is_left_good()
1135 | {
1136 |     int img1_pointer = (gFrameCam1 + FRAME_CLUST - 1) % FRAME_CLUST;
1137 |     if(gImg1Pass[img1_pointer])return true;
1138 |     else return false;
1139 | }
1140 | bool visensor_is_right_good()
1141 | {
1142 |     int img2_pointer = (gFrameCam2 + FRAME_CLUST - 1) % FRAME_CLUST;
1143 |     if(gImg2Pass[img2_pointer])return true;
1144 |     else return false;
1145 | }
1146 | 
1147 | bool visensor_is_left_fresh()
1148 | {
1149 |     if(left_fresh)
1150 |     {
1151 |         left_fresh=false;
1152 |         return true;
1153 |     }
1154 |     else return false;
1155 | }
1156 | bool visensor_is_right_fresh()
1157 | {
1158 |     if(right_fresh)
1159 |     {
1160 |         right_fresh=false;
1161 |         return true;
1162 |     }
1163 |     else return false;
1164 | }
1165 | 
1166 | visensor_imudata visensor_get_stereoImg(char* left_img,char* right_img)
1167 | {
1168 | 
1169 |     while(!visensor_is_stereo_good())usleep(200);
1170 | 
1171 |     int img1_pointer = (gFrameCam1 + FRAME_CLUST - 1) % FRAME_CLUST;
1172 |     int img2_pointer = (gFrameCam2 + FRAME_CLUST - 1) % FRAME_CLUST;
1173 | 
1174 |     if(gImg2Pass[img2_pointer]&&gImg1Pass[img1_pointer]&&(!visensor_resolution_status))
1175 |     {
1176 |         memcpy(left_img,(char *)(gImg1_VGA[img1_pointer]),IMG_BUF_SIZE_VGA);
1177 |         memcpy(right_img,(char *)(gImg2_VGA[img2_pointer]),IMG_BUF_SIZE_VGA);
1178 |         gImg2Pass[img2_pointer]=0;
1179 |         gImg1Pass[img1_pointer]=0;
1180 | 
1181 |         // 绑定最近时间戳的IMU数据
1182 |         timeval left_stamp=gImg1_SysTime[img1_pointer];
1183 |         int min_id=0;
1184 |         int min_dist=10000;
1185 |         for(int i=0; i<200; i++)
1186 |         {
1187 |             int imu_usec=IMU_FIFO[i].system_time.tv_usec;
1188 |             if(IMU_FIFO[i].imu_time!=0&&min_dist>=abs(imu_usec-left_stamp.tv_usec))
1189 |             {
1190 |                 min_dist=abs(imu_usec-left_stamp.tv_usec);
1191 |                 min_id=i;
1192 |             }
1193 |         }
1194 |         visensor_imudata bind_imuData=IMU_FIFO[min_id];
1195 |         return bind_imuData;
1196 | 
1197 |     }
1198 |     else if(gImg2Pass[img2_pointer]&&gImg1Pass[img1_pointer]&&(visensor_resolution_status))
1199 |     {
1200 |         memcpy(left_img,(char *)(gImg1_WVGA[img1_pointer]),IMG_BUF_SIZE_WVGA);
1201 |         memcpy(right_img,(char *)(gImg2_WVGA[img2_pointer]),IMG_BUF_SIZE_WVGA);
1202 |         gImg2Pass[img2_pointer]=0;
1203 |         gImg1Pass[img1_pointer]=0;
1204 | 
1205 |         // 绑定最近时间戳的IMU数据
1206 |         timeval left_stamp=gImg1_SysTime[img1_pointer];
1207 |         int min_id=0;
1208 |         int min_dist=10000;
1209 |         for(int i=0; i<200; i++)
1210 |         {
1211 |             int imu_usec=IMU_FIFO[i].system_time.tv_usec;
1212 |             if(IMU_FIFO[i].imu_time!=0&&min_dist>=abs(imu_usec-left_stamp.tv_usec))
1213 |             {
1214 |                 min_dist=abs(imu_usec-left_stamp.tv_usec);
1215 |                 min_id=i;
1216 |             }
1217 |         }
1218 |         visensor_imudata bind_imuData=IMU_FIFO[min_id];
1219 |         return bind_imuData;
1220 |     }
1221 | 
1222 | 
1223 | }
1224 | visensor_imudata visensor_get_stereoImg(char* left_img,char* right_img,timeval &left_stamp,timeval &right_stamp)
1225 | {
1226 | 
1227 |     while(!visensor_is_stereo_good())usleep(200);
1228 | 
1229 |     int img1_pointer = (gFrameCam1 + FRAME_CLUST - 1) % FRAME_CLUST;
1230 |     int img2_pointer = (gFrameCam2 + FRAME_CLUST - 1) % FRAME_CLUST;
1231 |     if(gImg2Pass[img2_pointer]&&gImg1Pass[img1_pointer]&&(!visensor_resolution_status))
1232 |     {
1233 |         memcpy(left_img,(char *)(gImg1_VGA[img1_pointer]),IMG_BUF_SIZE_VGA);
1234 |         memcpy(right_img,(char *)(gImg2_VGA[img2_pointer]),IMG_BUF_SIZE_VGA);
1235 |         left_stamp=gImg1_SysTime[img1_pointer];
1236 |         right_stamp=gImg2_SysTime[img2_pointer];
1237 |         gImg2Pass[img2_pointer]=0;
1238 |         gImg1Pass[img1_pointer]=0;
1239 | 
1240 |         // 绑定最近时间戳的IMU数据
1241 |         int min_id=0;
1242 |         int min_dist=10000;
1243 |         for(int i=0; i<200; i++)
1244 |         {
1245 |             int imu_usec=IMU_FIFO[i].system_time.tv_usec;
1246 |             if(IMU_FIFO[i].imu_time!=0&&min_dist>=abs(imu_usec-left_stamp.tv_usec))
1247 |             {
1248 |                 min_dist=abs(imu_usec-left_stamp.tv_usec);
1249 |                 min_id=i;
1250 |             }
1251 |         }
1252 |         visensor_imudata bind_imuData=IMU_FIFO[min_id];
1253 |         return bind_imuData;
1254 |     }
1255 |     else if(gImg2Pass[img2_pointer]&&gImg1Pass[img1_pointer]&&(visensor_resolution_status))
1256 |     {
1257 |         memcpy(left_img,(char *)(gImg1_WVGA[img1_pointer]),IMG_BUF_SIZE_WVGA);
1258 |         memcpy(right_img,(char *)(gImg2_WVGA[img2_pointer]),IMG_BUF_SIZE_WVGA);
1259 |         left_stamp=gImg1_SysTime[img1_pointer];
1260 |         right_stamp=gImg2_SysTime[img2_pointer];
1261 |         gImg2Pass[img2_pointer]=0;
1262 |         gImg1Pass[img1_pointer]=0;
1263 | 
1264 |         // 绑定最近时间戳的IMU数据
1265 |         int min_id=0;
1266 |         int min_dist=10000;
1267 |         for(int i=0; i<200; i++)
1268 |         {
1269 |             int imu_usec=IMU_FIFO[i].system_time.tv_usec;
1270 |             if(IMU_FIFO[i].imu_time!=0&&min_dist>=abs(imu_usec-left_stamp.tv_usec))
1271 |             {
1272 |                 min_dist=abs(imu_usec-left_stamp.tv_usec);
1273 |                 min_id=i;
1274 |             }
1275 |         }
1276 |         visensor_imudata bind_imuData=IMU_FIFO[min_id];
1277 |         return bind_imuData;
1278 |     }
1279 | }
1280 | 
1281 | visensor_imudata visensor_get_leftImg(char* left_img)
1282 | {
1283 | 
1284 |     while(!visensor_is_left_good())usleep(200);
1285 | 
1286 |     int img1_pointer = (gFrameCam1 + FRAME_CLUST - 1) % FRAME_CLUST;
1287 |     if(gImg1Pass[img1_pointer]&&(!visensor_resolution_status))
1288 |     {
1289 |         memcpy(left_img,(char *)(gImg1_VGA[img1_pointer]),IMG_BUF_SIZE_VGA);
1290 |         gImg1Pass[img1_pointer]=0;
1291 | 
1292 |         // 绑定最近时间戳的IMU数据
1293 |         timeval left_stamp=gImg1_SysTime[img1_pointer];
1294 |         int min_id=0;
1295 |         int min_dist=10000;
1296 |         for(int i=0; i<200; i++)
1297 |         {
1298 |             int imu_usec=IMU_FIFO[i].system_time.tv_usec;
1299 |             if(IMU_FIFO[i].imu_time!=0&&min_dist>=abs(imu_usec-left_stamp.tv_usec))
1300 |             {
1301 |                 min_dist=abs(imu_usec-left_stamp.tv_usec);
1302 |                 min_id=i;
1303 |             }
1304 |         }
1305 |         visensor_imudata bind_imuData=IMU_FIFO[min_id];
1306 |         return bind_imuData;
1307 |     }
1308 |     else if(gImg1Pass[img1_pointer]&&(visensor_resolution_status))
1309 |     {
1310 |         memcpy(left_img,(char *)(gImg1_WVGA[img1_pointer]),IMG_BUF_SIZE_WVGA);
1311 |         gImg1Pass[img1_pointer]=0;
1312 | 
1313 |         // 绑定最近时间戳的IMU数据
1314 |         timeval left_stamp=gImg1_SysTime[img1_pointer];
1315 |         int min_id=0;
1316 |         int min_dist=10000;
1317 |         for(int i=0; i<200; i++)
1318 |         {
1319 |             int imu_usec=IMU_FIFO[i].system_time.tv_usec;
1320 |             if(IMU_FIFO[i].imu_time!=0&&min_dist>=abs(imu_usec-left_stamp.tv_usec))
1321 |             {
1322 |                 min_dist=abs(imu_usec-left_stamp.tv_usec);
1323 |                 min_id=i;
1324 |             }
1325 |         }
1326 |         visensor_imudata bind_imuData=IMU_FIFO[min_id];
1327 |         return bind_imuData;
1328 |     }
1329 | }
1330 | 
1331 | visensor_imudata visensor_get_leftImg(char* left_img,timeval &left_stamp)
1332 | {
1333 | 
1334 |     while(!visensor_is_left_good())usleep(200);
1335 | 
1336 |     int img1_pointer = (gFrameCam1 + FRAME_CLUST - 1) % FRAME_CLUST;
1337 |     if(gImg1Pass[img1_pointer]&&(!visensor_resolution_status))
1338 |     {
1339 |         memcpy(left_img,(char *)(gImg1_VGA[img1_pointer]),IMG_BUF_SIZE_VGA);
1340 |         left_stamp=gImg1_SysTime[img1_pointer];
1341 |         gImg1Pass[img1_pointer]=0;
1342 | 
1343 |         // 绑定最近时间戳的IMU数据
1344 |         int min_id=0;
1345 |         int min_dist=10000;
1346 |         for(int i=0; i<200; i++)
1347 |         {
1348 |             int imu_usec=IMU_FIFO[i].system_time.tv_usec;
1349 |             if(IMU_FIFO[i].imu_time!=0&&min_dist>=abs(imu_usec-left_stamp.tv_usec))
1350 |             {
1351 |                 min_dist=abs(imu_usec-left_stamp.tv_usec);
1352 |                 min_id=i;
1353 |             }
1354 |         }
1355 |         visensor_imudata bind_imuData=IMU_FIFO[min_id];
1356 |         return bind_imuData;
1357 |     }
1358 |     else if(gImg1Pass[img1_pointer]&&(visensor_resolution_status))
1359 |     {
1360 |         memcpy(left_img,(char *)(gImg1_WVGA[img1_pointer]),IMG_BUF_SIZE_WVGA);
1361 |         left_stamp=gImg1_SysTime[img1_pointer];
1362 |         gImg1Pass[img1_pointer]=0;
1363 | 
1364 |         // 绑定最近时间戳的IMU数据
1365 |         int min_id=0;
1366 |         int min_dist=10000;
1367 |         for(int i=0; i<200; i++)
1368 |         {
1369 |             int imu_usec=IMU_FIFO[i].system_time.tv_usec;
1370 |             if(IMU_FIFO[i].imu_time!=0&&min_dist>=abs(imu_usec-left_stamp.tv_usec))
1371 |             {
1372 |                 min_dist=abs(imu_usec-left_stamp.tv_usec);
1373 |                 min_id=i;
1374 |             }
1375 |         }
1376 |         visensor_imudata bind_imuData=IMU_FIFO[min_id];
1377 |         return bind_imuData;
1378 |     }
1379 | }
1380 | 
1381 | visensor_imudata visensor_get_rightImg(char* right_img)
1382 | {
1383 | 
1384 |     while(!visensor_is_right_good())usleep(200);
1385 | 
1386 |     int img2_pointer = (gFrameCam2 + FRAME_CLUST - 1) % FRAME_CLUST;
1387 | 
1388 |     if(gImg2Pass[img2_pointer]&&(!visensor_resolution_status))
1389 |     {
1390 |         memcpy(right_img,(char *)(gImg2_VGA[img2_pointer]),IMG_BUF_SIZE_VGA);
1391 |         gImg2Pass[img2_pointer]=0;
1392 | 
1393 |         // 绑定最近时间戳的IMU数据
1394 |         timeval right_stamp=gImg2_SysTime[img2_pointer];
1395 |         int min_id=0;
1396 |         int min_dist=10000;
1397 |         for(int i=0; i<200; i++)
1398 |         {
1399 |             int imu_usec=IMU_FIFO[i].system_time.tv_usec;
1400 |             if(IMU_FIFO[i].imu_time!=0&&min_dist>=abs(imu_usec-right_stamp.tv_usec))
1401 |             {
1402 |                 min_dist=abs(imu_usec-right_stamp.tv_usec);
1403 |                 min_id=i;
1404 |             }
1405 |         }
1406 |         visensor_imudata bind_imuData=IMU_FIFO[min_id];
1407 |         return bind_imuData;
1408 |     }
1409 |     else if(gImg2Pass[img2_pointer]&&(visensor_resolution_status))
1410 |     {
1411 |         memcpy(right_img,(char *)(gImg2_WVGA[img2_pointer]),IMG_BUF_SIZE_WVGA);
1412 |         gImg2Pass[img2_pointer]=0;
1413 | 
1414 |         // 绑定最近时间戳的IMU数据
1415 |         timeval right_stamp=gImg2_SysTime[img2_pointer];
1416 |         int min_id=0;
1417 |         int min_dist=10000;
1418 |         for(int i=0; i<200; i++)
1419 |         {
1420 |             int imu_usec=IMU_FIFO[i].system_time.tv_usec;
1421 |             if(IMU_FIFO[i].imu_time!=0&&min_dist>=abs(imu_usec-right_stamp.tv_usec))
1422 |             {
1423 |                 min_dist=abs(imu_usec-right_stamp.tv_usec);
1424 |                 min_id=i;
1425 |             }
1426 |         }
1427 |         visensor_imudata bind_imuData=IMU_FIFO[min_id];
1428 |         return bind_imuData;
1429 |     }
1430 | }
1431 | visensor_imudata visensor_get_rightImg(char* right_img,timeval &right_stamp)
1432 | {
1433 | 
1434 |     while(!visensor_is_right_good())usleep(200);
1435 | 
1436 |     int img2_pointer = (gFrameCam2 + FRAME_CLUST - 1) % FRAME_CLUST;
1437 | 
1438 |     if(gImg2Pass[img2_pointer]&&(!visensor_resolution_status))
1439 |     {
1440 |         memcpy(right_img,(char *)(gImg2_VGA[img2_pointer]),IMG_BUF_SIZE_VGA);
1441 |         right_stamp=gImg2_SysTime[img2_pointer];
1442 |         gImg2Pass[img2_pointer]=0;
1443 | 
1444 |         // 绑定最近时间戳的IMU数据
1445 |         int min_id=0;
1446 |         int min_dist=10000;
1447 |         for(int i=0; i<200; i++)
1448 |         {
1449 |             int imu_usec=IMU_FIFO[i].system_time.tv_usec;
1450 |             if(IMU_FIFO[i].imu_time!=0&&min_dist>=abs(imu_usec-right_stamp.tv_usec))
1451 |             {
1452 |                 min_dist=abs(imu_usec-right_stamp.tv_usec);
1453 |                 min_id=i;
1454 |             }
1455 |         }
1456 |         visensor_imudata bind_imuData=IMU_FIFO[min_id];
1457 |         return bind_imuData;
1458 |     }
1459 |     else if(gImg2Pass[img2_pointer]&&(visensor_resolution_status))
1460 |     {
1461 |         memcpy(right_img,(char *)(gImg2_WVGA[img2_pointer]),IMG_BUF_SIZE_WVGA);
1462 |         right_stamp=gImg2_SysTime[img2_pointer];
1463 |         gImg2Pass[img2_pointer]=0;
1464 | 
1465 |         // 绑定最近时间戳的IMU数据
1466 |         int min_id=0;
1467 |         int min_dist=10000;
1468 |         for(int i=0; i<200; i++)
1469 |         {
1470 |             int imu_usec=IMU_FIFO[i].system_time.tv_usec;
1471 |             if(IMU_FIFO[i].imu_time!=0&&min_dist>=abs(imu_usec-right_stamp.tv_usec))
1472 |             {
1473 |                 min_dist=abs(imu_usec-right_stamp.tv_usec);
1474 |                 min_id=i;
1475 |             }
1476 |         }
1477 |         visensor_imudata bind_imuData=IMU_FIFO[min_id];
1478 |         return bind_imuData;
1479 |     }
1480 | }
1481 | 
1482 | int visensor_Start_Cameras()
1483 | {
1484 |     gSelectCam = 3;
1485 |     allow_settings_change=false;
1486 | 
1487 |     shut_down_tag=false;
1488 | 
1489 |     int ret_val=0;
1490 | 
1491 |     DEBUGLOG("Now opening cameras...\r\n");
1492 |     //Open usb devices
1493 |     int r;
1494 |     r = cyusb_open();
1495 |     if(r<1)
1496 |     {
1497 |         LOG("Error: No cameras found! r = %d\r\n",r);
1498 |         return -2;
1499 |     }
1500 |     else
1501 |     {
1502 |         printf("Number of device of interest found: %d\r\n",r);
1503 |     }
1504 |     //Get camera position
1505 |     cyusb_handle *pusb_handle;
1506 |     for(int i=0; i<r; i++)
1507 |     {
1508 |         unsigned char buf = 0;
1509 |         pusb_handle = cyusb_gethandle(i);
1510 |         int r_num = cyusb_control_transfer(pusb_handle,0xC0,GET_CAM_LR,0,0,&buf,1,1000);
1511 |         if(r_num != 1)
1512 |         {
1513 |             printf("Getting LR: %d error: i:0xA5,r:%d\r\n",i,r_num);
1514 |         }
1515 |         else
1516 |         {
1517 |             if(buf==0xF0)
1518 |             {
1519 |                 pcam1_handle = pusb_handle;
1520 |                 printf("Left camera found!\r\n");
1521 |             }
1522 |             else if(buf==0xF1)
1523 |             {
1524 |                 pcam2_handle = pusb_handle;
1525 |                 printf("Right camera found!\r\n");
1526 |             }
1527 |         }
1528 |     }
1529 |     //Check cameras
1530 |     if(gSelectCam&0x01)
1531 |         if(pcam1_handle==NULL)
1532 |         {
1533 |             LOG("Error: Left camera was not found!\r\n");
1534 |             ret_val= -2;
1535 |         }
1536 |     if(gSelectCam&0x02)
1537 |         if(pcam2_handle==NULL)
1538 |         {
1539 |             LOG("Error: Right camera was not found!\r\n");
1540 |             ret_val= -2;
1541 |         }
1542 | 
1543 |     if(!(ret_val<0))
1544 |     {
1545 |         //Create capture thread
1546 |         gettimeofday(&visensor_startTime,NULL);		// 初始化时间起点
1547 | 
1548 |         int temp=0;
1549 |         if(gSelectCam&0x01)
1550 |             if(temp = pthread_create(&cam1_capture_thread, NULL, cam1_capture, NULL))
1551 |                 printf("Failed to create thread cam1_capture_thread\r\n");
1552 |         if(gSelectCam&0x02)
1553 |             if(temp = pthread_create(&cam2_capture_thread, NULL, cam2_capture, NULL))
1554 |                 printf("Failed to create thread cam2_capture_thread\r\n");
1555 |         usleep(1000);
1556 | 
1557 |         return ret_val;
1558 |     }
1559 |     else return 0;
1560 | }
1561 | void visensor_Close_Cameras()
1562 | {
1563 |     shut_down_tag=true;
1564 |     /* Waiting for Camera threads */
1565 |     if(cam1_capture_thread !=0)
1566 |     {
1567 |         pthread_join(cam1_capture_thread,NULL);
1568 |     }
1569 |     if(cam2_capture_thread !=0)
1570 |     {
1571 |         pthread_join(cam2_capture_thread,NULL);
1572 |     }
1573 |     cyusb_close();
1574 | }
1575 | 
1576 | // 检测有无新数据
1577 | bool visensor_imu_have_fresh_data()
1578 | {
1579 |     if(visensor_query_imu_update())
1580 |     {
1581 |         visensor_erase_imu_update();
1582 |         return true;
1583 |     }
1584 |     else return false;
1585 | }
1586 | 
1587 | void visensor_Close_IMU()
1588 | {
1589 |     imu_close=true;
1590 |     /* Waiting for imu_thread */
1591 |     if(imu_thread !=0)
1592 |     {
1593 |         pthread_join(imu_thread,NULL);
1594 |     }
1595 | }
1596 | 
1597 | void *imu_data_feed(void*)
1598 | {
1599 |     unsigned char imu_frame[IMU_FRAME_LEN];
1600 |     short int biasX=(short int)imu_acc_bias_X;
1601 |     short int biasY=(short int)imu_acc_bias_Y;
1602 |     short int biasZ=(short int)imu_acc_bias_Z;
1603 |     short int setaccoffset[3] = {biasX,  biasY, biasZ};
1604 | 
1605 |     // 在此函数中更新 IMU-FIFO 用于计算同步 VI-pair
1606 | 
1607 |     int counter=0;
1608 | 
1609 |     for(int i=0; i<200; i++)
1610 |     {
1611 |         IMU_FIFO[i].imu_time=0;
1612 |         IMU_FIFO[i].system_time.tv_usec=0;
1613 |         IMU_FIFO[i].system_time.tv_sec=0;
1614 |         IMU_FIFO[i].num=0;
1615 |         IMU_FIFO[i].rx=0;
1616 |         IMU_FIFO[i].ry=0;
1617 |         IMU_FIFO[i].rz=0;
1618 |         IMU_FIFO[i].ax=0;
1619 |         IMU_FIFO[i].ay=0;
1620 |         IMU_FIFO[i].az=0;
1621 |         IMU_FIFO[i].qw=0;
1622 |         IMU_FIFO[i].qx=0;
1623 |         IMU_FIFO[i].qy=0;
1624 |         IMU_FIFO[i].qz=0;
1625 |     }
1626 | 
1627 |     while(!imu_close)
1628 |     {
1629 |         if(visensor_get_imu_frame(imu_fd,imu_frame)==0)
1630 |         {
1631 |             visensor_get_imu_data(imu_frame,setaccoffset,&visensor_imudata_pack,false);
1632 |             // 将imu更新标记打开
1633 |             visensor_mark_imu_update();
1634 | 
1635 |             imu_fifo_ct = (imu_fifo_ct+1)%200;
1636 |             IMU_FIFO[imu_fifo_ct]=visensor_imudata_pack;
1637 | 
1638 |         }
1639 |         usleep(500);
1640 |     }
1641 |     pthread_exit(NULL);
1642 | }
1643 | int visensor_Start_IMU()
1644 | {
1645 | 
1646 |     // serial port
1647 |     int fd=visensor_open_port(imu_port_name.c_str());
1648 |     if(fd<0)
1649 |     {
1650 |         printf("visensor_open_port error...\r\n");
1651 |         return 0;
1652 |     }
1653 |     printf("visensor_open_port success...\r\n");
1654 | 
1655 |     if(visensor_set_opt(fd,115200,8,'N',1)<0)
1656 |     {
1657 |         printf("visensor_set_opt error...\r\n");
1658 |         return 0;
1659 |     }
1660 |     printf("visensor_set_opt(fd,115200,8,'N',1) success...\r\n");
1661 | 
1662 |     static unsigned char sendframe[10]= {0x55,0xAA,0x02};
1663 |     short int setaccoffset[3] = {(short int)imu_acc_bias_X, (short int)imu_acc_bias_Y, (short int)imu_acc_bias_Z};
1664 |     memcpy(&sendframe[3],setaccoffset,6);
1665 |     sendframe[9]=sendframe[3]+sendframe[4]+sendframe[5]+sendframe[6]+sendframe[7]+sendframe[8];
1666 |     visensor_send_imu_frame(fd,sendframe,10);
1667 | 
1668 |     //Create imu_data thread
1669 |     imu_fd=fd;
1670 | 
1671 |     int temp;
1672 |     if(temp = pthread_create(&imu_thread, NULL, imu_data_feed, NULL))
1673 |         printf("Failed to create thread imu_data\r\n");
1674 | 
1675 |     return fd;
1676 | }
1677 | 
1678 | 
1679 | 


--------------------------------------------------------------------------------
/loitor-vi-install.sh:
--------------------------------------------------------------------------------
 1 | create_udev_rules() {
 2 | 	echo "" > loitor-vi.rules
 3 | 	echo 'KERNEL=="*", SUBSYSTEM=="usb", ENV{DEVTYPE}=="usb_device", ACTION=="add", ATTR{idVendor}=="04b4", MODE="666" ' >> loitor-vi.rules
 4 | 	echo 'KERNEL=="*", SUBSYSTEM=="usb", ENV{DEVTYPE}=="usb_device", ACTION=="remove" ' >> loitor-vi.rules
 5 | }
 6 | 
 7 | if [ `whoami` != 'root' ]; then
 8 |    echo "You have to be root to run this script"
 9 |    exit 1;
10 | fi
11 | 
12 | create_udev_rules
13 | cp loitor-vi.rules /etc/udev/rules.d/
14 | 


--------------------------------------------------------------------------------
/loitor_PCcam_cliper.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/loitor-vis/vi_sensor_sdk/eb54545ef5eeb681a5271725e8a9d9a21c97367a/loitor_PCcam_cliper.stl


--------------------------------------------------------------------------------
/loitor_ros_workspace/src/loitor_stereo_visensor/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8.3)
 2 | project(loitor_stereo_visensor)
 3 | 
 4 | find_package(catkin REQUIRED COMPONENTS
 5 |   roscpp
 6 |   rospy
 7 |   std_msgs
 8 |   cv_bridge
 9 |   image_transport
10 |   message_generation
11 |   sensor_msgs
12 | )
13 | 
14 | catkin_package(
15 |   CATKIN_DEPENDS
16 |   cv_bridge
17 |   image_transport
18 |   message_runtime
19 |   sensor_msgs
20 | )
21 | 
22 | include_directories(
23 |   ${catkin_INCLUDE_DIRS}
24 | )
25 | 
26 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread")
27 | 
28 | find_package(OpenCV REQUIRED)
29 | 
30 | include_directories(include 
31 | ${catkin_INCLUDE_DIRS}  
32 | ${OpenCV_INCLUDE_DIRS})
33 | 
34 | include_directories("./include/loitor_stereo_visensor")
35 | 
36 | add_library(loitorusbcam src/loitorusbcam.cpp)
37 | target_link_libraries(loitorusbcam loitorimu)
38 | 
39 | add_library(loitorimu src/loitorimu.cpp)
40 | 
41 | add_executable(loitor_stereo_visensor src/loitor_stereo_visensor.cpp) 
42 | target_link_libraries(loitor_stereo_visensor ${catkin_LIBRARIES})
43 | target_link_libraries(loitor_stereo_visensor loitorusbcam loitorimu usb-1.0 ${OpenCV_LIBS})
44 | add_dependencies(loitor_stereo_visensor loitor_vis_generate_messages_cpp) 
45 | 
46 | 
47 | 
48 | 
49 | 
50 | 
51 | 
52 | 
53 | 
54 | 
55 | 
56 | 
57 | 
58 | 
59 | 
60 | 
61 | 
62 | 
63 | 


--------------------------------------------------------------------------------
/loitor_ros_workspace/src/loitor_stereo_visensor/Loitor_VISensor_Setups.txt:
--------------------------------------------------------------------------------
 1 | #
 2 | Mode
 3 | 12
 4 | #
 5 | HighSpeed Pre-Set
 6 | m1,left,200,VGA,54
 7 | m2,right,250,VGA,54
 8 | m3,left,70,WVGA,54
 9 | m4,right,70,WVGA,54
10 | m5,stereo,280,VGA,54
11 | m6,stereo,281,WVGA,54
12 | #
13 | Normal Pre-Set
14 | m7,left,150,VGA,54
15 | m8,right,150,VGA,54
16 | m9,left,150,WVGA,54
17 | m10,right,150,WVGA,54
18 | m11,stereo,183,VGA,27
19 | m12,stereo,250,WVGA,27
20 | #
21 | m13,Manual Mode
22 | 0
23 | 194
24 | VGA
25 | 54
26 | #
27 | EG_mode
28 | 3
29 | manual,50,200
30 | auto,300,5,58
31 | autoexp_manualgain,300,5,58,200
32 | /dev/ttyUSB0,5
33 | #
34 | IMU-acc-bias
35 | Gx,758
36 | Gy,-2
37 | Gz,-2608
38 | #
39 | 


--------------------------------------------------------------------------------
/loitor_ros_workspace/src/loitor_stereo_visensor/cfg/Loitor_VISensor_Setups.txt:
--------------------------------------------------------------------------------
 1 | #
 2 | Mode
 3 | 6
 4 | #
 5 | HighSpeed Pre-Set
 6 | m1,left,70,VGA,54
 7 | m2,right,70,VGA,54
 8 | m3,left,70,WVGA,54
 9 | m4,right,70,WVGA,54
10 | m5,stereo,280,VGA,54
11 | m6,stereo,281,WVGA,54
12 | #
13 | Normal Pre-Set
14 | m7,left,150,VGA,54
15 | m8,right,150,VGA,54
16 | m9,left,150,WVGA,54
17 | m10,right,150,WVGA,54
18 | m11,stereo,183,VGA,27
19 | m12,stereo,194,WVGA,27
20 | #
21 | m13,Manual Mode
22 | 0
23 | 194
24 | VGA
25 | 54
26 | #
27 | EG_mode
28 | 3
29 | manual,100,100
30 | auto,300,5,58
31 | autoexp_manualgain,300,5,58,100
32 | /dev/ttyUSB0,5
33 | #
34 | IMU-acc-bias
35 | Gx,346
36 | Gy,129
37 | Gz,-1616
38 | #
39 | 


--------------------------------------------------------------------------------
/loitor_ros_workspace/src/loitor_stereo_visensor/cfg/backup_settings.txt:
--------------------------------------------------------------------------------
 1 | #
 2 | Mode
 3 | 6
 4 | #
 5 | HighSpeed Pre-Set
 6 | m1,left,70,VGA,108
 7 | m2,right,70,VGA,108
 8 | m3,left,70,WVGA,108
 9 | m4,right,70,WVGA,108
10 | m5,stereo,280,VGA,54
11 | m6,stereo,281,WVGA,54
12 | #
13 | Normal Pre-Set
14 | m7,left,150,VGA,54
15 | m8,right,150,VGA,54
16 | m9,left,150,WVGA,54
17 | m10,right,150,WVGA,54
18 | m11,stereo,183,VGA,27
19 | m12,stereo,194,WVGA,27
20 | #
21 | m13,Manual Mode
22 | 0
23 | 194
24 | VGA
25 | 54
26 | #
27 | EG_mode
28 | 3
29 | manual,100,100
30 | auto,300,5,58
31 | autoexp_manualgain,300,5,58,100
32 | /dev/ttyUSB0,5
33 | #
34 | IMU-acc-bias
35 | Gx,346.000000
36 | Gy,129.000000
37 | Gz,-1616.000000
38 | #
39 | 


--------------------------------------------------------------------------------
/loitor_ros_workspace/src/loitor_stereo_visensor/include/loitor_stereo_visensor/loitorimu.h:
--------------------------------------------------------------------------------
 1 | #ifndef LOITORIMU_H
 2 | #define LOITORIMU_H
 3 | 
 4 | #include <sys/time.h>
 5 | 
 6 | typedef struct 
 7 | {
 8 | 	float imu_time;		// time-stamp
 9 | 	timeval system_time;
10 | 	unsigned char num;
11 | 	float rx,ry,rz;
12 | 	float ax,ay,az;
13 | 	float qw,qx,qy,qz;
14 | 
15 | }visensor_imudata;
16 | 
17 | extern timeval visensor_startTime;
18 | 
19 | bool visensor_query_imu_update();
20 | bool visensor_mark_imu_update();
21 | bool visensor_erase_imu_update();
22 | int visensor_set_opt(int fd,int nSpeed, int nBits, char nEvent, int nStop);
23 | int visensor_open_port(const char* dev_str);
24 | int visensor_get_imu_frame(int fd, unsigned char* imu_frame);
25 | int visensor_get_imu_data(unsigned char* imu_frame,short int* acc_offset,visensor_imudata *imudata_struct,bool show_data);
26 | int visensor_send_imu_frame(int fd, unsigned char* data, int len);
27 | 
28 | #endif
29 | 


--------------------------------------------------------------------------------
/loitor_ros_workspace/src/loitor_stereo_visensor/include/loitor_stereo_visensor/loitorusbcam.h:
--------------------------------------------------------------------------------
 1 | #include <sys/time.h>
 2 | #include "loitorimu.h"
 3 | 
 4 | #ifndef LOITORUSBCAM_H
 5 | #define LOITORUSBCAM_H
 6 | #define IMU_FRAME_LEN 32
 7 | #define IMG_WIDTH_VGA 	640
 8 | #define IMG_HEIGHT_VGA 	480
 9 | #define IMG_SIZE_VGA 	(IMG_WIDTH_VGA*IMG_HEIGHT_VGA)
10 | #define IMG_BUF_SIZE_VGA (IMG_SIZE_VGA+0x200)
11 | #define IMG_WIDTH_WVGA 	752
12 | #define IMG_HEIGHT_WVGA 	480
13 | #define IMG_SIZE_WVGA 	(IMG_WIDTH_WVGA*IMG_HEIGHT_WVGA)
14 | #define IMG_BUF_SIZE_WVGA (IMG_SIZE_WVGA+0x200)
15 | 
16 | /*
17 | *  camera当前分辨率状态
18 | *  0-代表VGA
19 | *  1-代表WVGA
20 | */
21 | extern bool visensor_resolution_status;			// 0-VGA | 1-WVGA
22 | /*
23 | *  camera当前通道选择
24 | *  0-代表开启双目
25 | *  1-代表只开启右眼
26 | *  2-代表只开启左眼
27 | */
28 | extern int visensor_cam_selection;				// 0-stereo | 1-right | 2-left
29 | /*
30 | *  imu 数据
31 | */
32 | extern visensor_imudata visensor_imudata_pack;
33 | 
34 | // setters & getters
35 | void visensor_set_auto_EG(int E_AG);			// 0-ManEG | 1-AutoEG with limits | 2-AutoE&ManG | 3- fully auto
36 | void visensor_set_exposure(int _man_exp);
37 | void visensor_set_gain(int _man_gain);
38 | void visensor_set_max_autoExp(int max_exp);
39 | void visensor_set_min_autoExp(int min_exp);
40 | void visensor_set_resolution(bool set_wvga);
41 | void visensor_set_fps_mode(bool fps_mode);
42 | void visensor_set_current_HB(int HB);
43 | void visensor_set_desired_bin(int db);
44 | void visensor_set_cam_selection_mode(int _visensor_cam_selection);
45 | void visensor_set_imu_bias(float bx,float by,float bz);
46 | void visensor_set_imu_portname(char* input_name);
47 | void visensor_set_current_mode(int _mode);
48 | 
49 | int visensor_get_EG_mode();
50 | int visensor_get_exposure();
51 | int visensor_get_gain();
52 | int visensor_get_max_autoExp();
53 | int visensor_get_min_autoExp();
54 | bool visensor_get_resolution();
55 | int visensor_get_fps();
56 | int visensor_get_current_HB();
57 | int visensor_get_desired_bin();
58 | int visensor_get_cam_selection_mode();
59 | float visensor_get_imu_G_bias_x();
60 | float visensor_get_imu_G_bias_y();
61 | float visensor_get_imu_G_bias_z();
62 | const char* visensor_get_imu_portname();
63 | 
64 | void visensor_save_current_settings();
65 | 
66 | float visensor_get_hardware_fps();
67 | 
68 | void visensor_load_settings(const char* settings_file);
69 | 
70 | bool visensor_is_stereo_good();
71 | bool visensor_is_left_good();
72 | bool visensor_is_right_good();
73 | 
74 | bool visensor_is_left_fresh();
75 | bool visensor_is_right_fresh();
76 | 
77 | /*
78 | * 得到绑定了同步IMU数据之后的图像数据
79 | */
80 | visensor_imudata visensor_get_stereoImg(char* left_img,char* right_img);
81 | visensor_imudata visensor_get_stereoImg(char* left_img,char* right_img,timeval &left_stamp,timeval &right_stamp);
82 | 
83 | visensor_imudata visensor_get_leftImg(char* left_img);
84 | visensor_imudata visensor_get_leftImg(char* left_img,timeval &left_stamp);
85 | 
86 | visensor_imudata visensor_get_rightImg(char* right_img);
87 | visensor_imudata visensor_get_rightImg(char* right_img,timeval &right_stamp);
88 | 
89 | int visensor_Start_Cameras();
90 | void visensor_Close_Cameras();
91 | 
92 | bool visensor_imu_have_fresh_data();
93 | 
94 | int visensor_Start_IMU();
95 | void visensor_Close_IMU();
96 | 
97 | 
98 | #endif
99 | 


--------------------------------------------------------------------------------
/loitor_ros_workspace/src/loitor_stereo_visensor/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <package>
 3 |   <name>loitor_stereo_visensor</name>
 4 |   <version>0.0.0</version>
 5 |   <description>The loitor_stereo_visensor package</description>
 6 | 
 7 |   <!-- One maintainer tag required, multiple allowed, one person per tag --> 
 8 |   <!-- Example:  -->
 9 |   <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
10 |   <maintainer email="root@todo.todo">root</maintainer>
11 | 
12 | 
13 |   <!-- One license tag required, multiple allowed, one license per tag -->
14 |   <!-- Commonly used license strings: -->
15 |   <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
16 |   <license>TODO</license>
17 | 
18 | 
19 |   <!-- Url tags are optional, but mutiple are allowed, one per tag -->
20 |   <!-- Optional attribute type can be: website, bugtracker, or repository -->
21 |   <!-- Example: -->
22 |   <!-- <url type="website">http://wiki.ros.org/loitor_stereo_visensor</url> -->
23 | 
24 | 
25 |   <!-- Author tags are optional, mutiple are allowed, one per tag -->
26 |   <!-- Authors do not have to be maintianers, but could be -->
27 |   <!-- Example: -->
28 |   <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
29 | 
30 | 
31 |   <!-- The *_depend tags are used to specify dependencies -->
32 |   <!-- Dependencies can be catkin packages or system dependencies -->
33 |   <!-- Examples: -->
34 |   <!-- Use build_depend for packages you need at compile time: -->
35 |   <!--   <build_depend>message_generation</build_depend> -->
36 |   <!-- Use buildtool_depend for build tool packages: -->
37 |   <!--   <buildtool_depend>catkin</buildtool_depend> -->
38 |   <!-- Use run_depend for packages you need at runtime: -->
39 |   <!--   <run_depend>message_runtime</run_depend> -->
40 |   <!-- Use test_depend for packages you need only for testing: -->
41 |   <!--   <test_depend>gtest</test_depend> -->
42 |   <buildtool_depend>catkin</buildtool_depend>
43 |   <build_depend>cv_bridge</build_depend>
44 |   <build_depend>image_transport</build_depend>
45 |   <build_depend>opencv2</build_depend>
46 |   <build_depend>roscpp</build_depend>
47 |   <build_depend>rospy</build_depend>
48 |   <build_depend>sensor_msgs</build_depend>
49 |   <build_depend>std_msgs</build_depend>
50 |   <run_depend>cv_bridge</run_depend>
51 |   <run_depend>image_transport</run_depend>
52 |   <run_depend>message_runtime</run_depend>
53 |   <run_depend>opencv2</run_depend>
54 |   <run_depend>roscpp</run_depend>
55 |   <run_depend>rospy</run_depend>
56 |   <run_depend>sensor_msgs</run_depend>
57 |   <run_depend>std_msgs</run_depend>
58 | 
59 | 
60 |   <!-- The export tag contains other, unspecified, tags -->
61 |   <export>
62 |     <!-- Other tools can request additional information be placed here -->
63 | 
64 |   </export>
65 | </package>


--------------------------------------------------------------------------------
/loitor_ros_workspace/src/loitor_stereo_visensor/src/loitor_stereo_visensor.cpp:
--------------------------------------------------------------------------------
  1 | #include "ros/ros.h" 
  2 | #include "std_msgs/String.h"
  3 | #include <image_transport/image_transport.h>
  4 | #include <cv_bridge/cv_bridge.h>
  5 | #include "sensor_msgs/Imu.h"
  6 | 
  7 | #include <cv.h>
  8 | #include <highgui.h>
  9 | #include "cxcore.hpp"
 10 | #include <pthread.h>
 11 | #include <sys/time.h>
 12 | #include <unistd.h>
 13 | 
 14 | #include <stdio.h>
 15 | #include <stdlib.h>
 16 | #include <string.h>
 17 | 
 18 | #include "loitorusbcam.h"
 19 | #include "loitorimu.h"
 20 | 
 21 | #include <sstream>
 22 | 
 23 | 
 24 | using namespace std;
 25 | using namespace cv;
 26 | 
 27 | 
 28 | ros::Publisher pub_imu;
 29 | 
 30 | /*
 31 | *  用于构造cv::Mat 的左右眼图像
 32 | */
 33 | cv::Mat img_left;
 34 | cv::Mat img_right;
 35 | 
 36 | /*
 37 | *  当前左右图像的时间戳
 38 | */
 39 | timeval left_stamp,right_stamp;
 40 | 
 41 | /*
 42 | *  imu viewer
 43 | */
 44 | bool visensor_Close_IMU_viewer=false;
 45 | bool imu_start_transfer=false;
 46 | void* imu_data_stream(void *)
 47 | {
 48 | 	int counter=0;
 49 | 	imu_start_transfer=false;
 50 | 
 51 | 
 52 | 	while((!visensor_Close_IMU_viewer)&&!imu_start_transfer)usleep(1000);
 53 | 	while(!visensor_Close_IMU_viewer)
 54 | 	{
 55 | 		if(visensor_imu_have_fresh_data())
 56 |            	{
 57 | 			counter++;
 58 | 			// 每隔20帧显示一次imu数据
 59 | 			if(counter>=20) 
 60 | 			{
 61 | 				//cout<<"visensor_imudata_pack->a : "<<visensor_imudata_pack.ax<<" , "<<visensor_imudata_pack.ay<<" , "<<visensor_imudata_pack.az<<endl;
 62 | 				float ax=visensor_imudata_pack.ax;
 63 | 				float ay=visensor_imudata_pack.ay;
 64 | 				float az=visensor_imudata_pack.az;
 65 | 				//cout<<"visensor_imudata_pack->a : "<<sqrt(ax*ax+ay*ay+az*az)<<endl;
 66 | 				//cout<<"imu_time : "<<visensor_imudata_pack.imu_time<<endl;
 67 | 				//cout<<"imu_time : "<<visensor_imudata_pack.system_time.tv_usec<<endl;
 68 | 				counter=0;
 69 | 			}
 70 | 			sensor_msgs::Imu imu_msg;
 71 | 			imu_msg.header.frame_id = "/imu";
 72 | 			ros::Time imu_time;
 73 | 			imu_time.sec=visensor_imudata_pack.system_time.tv_sec;
 74 | 			imu_time.nsec=1000*visensor_imudata_pack.system_time.tv_usec;
 75 | 			imu_msg.header.stamp = imu_time;
 76 | 			imu_msg.header.seq=0;
 77 | 
 78 | 			imu_msg.linear_acceleration.x=visensor_imudata_pack.ax;
 79 | 			imu_msg.linear_acceleration.y=visensor_imudata_pack.ay;
 80 | 			imu_msg.linear_acceleration.z=visensor_imudata_pack.az;
 81 | 			imu_msg.angular_velocity.x=3.1415926f*visensor_imudata_pack.rx/180.0f;
 82 | 			imu_msg.angular_velocity.y=3.1415926f*visensor_imudata_pack.ry/180.0f;
 83 | 			imu_msg.angular_velocity.z=3.1415926f*visensor_imudata_pack.rz/180.0f;
 84 | 			imu_msg.orientation.w=visensor_imudata_pack.qw;
 85 | 			imu_msg.orientation.x=visensor_imudata_pack.qx;
 86 | 			imu_msg.orientation.y=visensor_imudata_pack.qy;
 87 | 			imu_msg.orientation.z=visensor_imudata_pack.qz;
 88 | 
 89 | 			pub_imu.publish(imu_msg);
 90 | 		}
 91 | 		usleep(10);
 92 | 	}
 93 | 	pthread_exit(NULL);
 94 | }
 95 | 
 96 | 
 97 | int main(int argc, char **argv)
 98 | { 
 99 | 	/************************ Start Cameras ************************/
100 | 	if(argv[1])
101 | 		visensor_load_settings(argv[1]);
102 | 	else 
103 | 		visensor_load_settings("src/loitor_stereo_visensor/Loitor_VISensor_Setups.txt");
104 | 
105 | 	// 手动设置相机参数
106 | 	//set_current_mode(5);
107 | 	//set_auto_EG(0);
108 | 	//set_exposure(50);
109 | 	//set_gain(200);
110 | 	//set_visensor_cam_selection_mode(2);
111 | 	//set_resolution(false);
112 | 	//set_fps_mode(true);
113 | 	// 保存相机参数到原配置文件
114 | 	//save_current_settings();
115 | 	
116 | 	int r = visensor_Start_Cameras();
117 | 	if(r<0)
118 | 	{
119 | 		printf("Opening cameras failed...\r\n");
120 | 		return r;
121 | 	}
122 | 	// 创建用来接收camera数据的图像
123 | 	if(!visensor_resolution_status)
124 | 	{
125 | 		img_left.create(cv::Size(640,480),CV_8U);
126 | 		img_right.create(cv::Size(640,480),CV_8U);
127 | 		img_left.data=new unsigned char[IMG_WIDTH_VGA*IMG_HEIGHT_VGA];
128 | 		img_right.data=new unsigned char[IMG_WIDTH_VGA*IMG_HEIGHT_VGA];
129 | 	}
130 | 	else
131 | 	{
132 | 		img_left.create(cv::Size(752,480),CV_8U);
133 | 		img_right.create(cv::Size(752,480),CV_8U);
134 | 		img_left.data=new unsigned char[IMG_WIDTH_WVGA*IMG_HEIGHT_WVGA];
135 | 		img_right.data=new unsigned char[IMG_WIDTH_WVGA*IMG_HEIGHT_WVGA];
136 | 	}
137 | 	float hardware_fps=visensor_get_hardware_fps();
138 | 	/************************** Start IMU **************************/
139 | 	int fd=visensor_Start_IMU();
140 | 	if(fd<0)
141 | 	{
142 | 		printf("open_port error...\r\n");
143 | 		return 0;
144 | 	}
145 | 	printf("open_port success...\r\n");
146 | 	usleep(100000);
147 | 	/************************ ************ ************************/
148 | 
149 | 	//Create imu_data_stream thread
150 | 	pthread_t imu_data_thread;
151 | 	int temp;
152 | 	if(temp = pthread_create(&imu_data_thread, NULL, imu_data_stream, NULL))
153 | 	printf("Failed to create thread imu_data_stream\r\n");
154 | 	
155 | 	ros::init(argc, argv, "loitor_stereo_visensor");
156 | 
157 | 	ros::NodeHandle n;
158 | 
159 | 	// imu publisher
160 | 	pub_imu = n.advertise<sensor_msgs::Imu>("imu0", 200);
161 |  
162 | 	// publish 到这两个 topic
163 | 	image_transport::ImageTransport it(n);
164 | 	image_transport::Publisher pub = it.advertise("/cam0/image_raw", 1);
165 | 	sensor_msgs::ImagePtr msg;
166 | 
167 | 	image_transport::ImageTransport it1(n);
168 | 	image_transport::Publisher pub1 = it1.advertise("/cam1/image_raw", 1);
169 | 	sensor_msgs::ImagePtr msg1;
170 | 
171 | 	// 使用camera硬件帧率设置发布频率
172 | 	ros::Rate loop_rate((int)hardware_fps);
173 | 
174 | 	int static_ct=0;
175 | 
176 | 	timeval img_time_test,img_time_offset;
177 | 	img_time_test.tv_usec=0;
178 | 	img_time_test.tv_sec=0;
179 | 	img_time_offset.tv_usec=50021;
180 | 	img_time_offset.tv_sec=0;
181 | 
182 | 	while (ros::ok())
183 | 	{
184 | 		imu_start_transfer=true;
185 | 
186 | 		//cout<<"visensor_get_hardware_fps() ==== "<<visensor_get_hardware_fps()<<endl;
187 | 
188 | 		if(visensor_cam_selection==0)
189 | 		{
190 | 
191 | 			visensor_imudata paired_imu=visensor_get_stereoImg((char *)img_left.data,(char *)img_right.data,left_stamp,right_stamp);
192 | 
193 | 
194 | 			// 显示同步数据的时间戳（单位微秒）
195 | 			//cout<<"left_time : "<<left_stamp.tv_usec<<endl;
196 | 			//cout<<"right_time : "<<right_stamp.tv_usec<<endl;
197 | 			//cout<<"paired_imu time ===== "<<paired_imu.system_time.tv_usec<<endl<<endl;
198 | 			//cout<<"visensor_get_hardware_fps() ==== "<<1.0f/visensor_get_hardware_fps()<<endl;
199 | 
200 | 			cv_bridge::CvImage t_left=cv_bridge::CvImage(std_msgs::Header(), "mono8", img_left);
201 | 			cv_bridge::CvImage t_right=cv_bridge::CvImage(std_msgs::Header(), "mono8", img_right);
202 | 
203 | 			// 加时间戳(right_time=left_time)
204 | 			ros::Time msg_time;
205 | 			msg_time.sec=left_stamp.tv_sec;
206 | 			msg_time.nsec=1000*left_stamp.tv_usec;
207 | 			t_left.header.stamp = msg_time;
208 | 			
209 | 			ros::Time msg1_time;
210 | 			msg1_time.sec=left_stamp.tv_sec;
211 | 			msg1_time.nsec=1000*left_stamp.tv_usec;
212 | 			t_right.header.stamp = msg1_time;
213 | 			t_right.header.seq=0;
214 | 			t_left.header.seq=0;
215 | 
216 | 			msg = t_left.toImageMsg();
217 | 			msg1 = t_right.toImageMsg();
218 | 
219 | 			static_ct++;
220 | 			{
221 | 				pub.publish(msg);
222 | 				pub1.publish(msg1);
223 | 				static_ct=0;
224 | 			}
225 | 			
226 | 			// 显示时间戳
227 | 			//cout<<"left_time : "<<left_stamp.tv_usec<<endl;
228 | 			//cout<<"right_time : "<<right_stamp.tv_usec<<endl<<endl;
229 | 
230 | 		}
231 | 		else if(visensor_cam_selection==1)
232 | 		{
233 | 			visensor_imudata paired_imu=visensor_get_rightImg((char *)img_right.data,right_stamp);
234 | 
235 | 			// 显示同步数据的时间戳（单位微秒）
236 | 			cout<<"right_time : "<<right_stamp.tv_usec<<endl;
237 | 			cout<<"paired_imu time ===== "<<paired_imu.system_time.tv_usec<<endl<<endl;
238 | 
239 | 			cv_bridge::CvImage t_right=cv_bridge::CvImage(std_msgs::Header(), "mono8", img_right);
240 | 
241 | 			// 加时间戳
242 | 			ros::Time msg1_time;
243 | 			msg1_time.sec=right_stamp.tv_sec;
244 | 			msg1_time.nsec=1000*right_stamp.tv_usec;
245 | 			t_right.header.stamp = msg1_time;
246 | 			t_right.header.seq=0;
247 | 			
248 | 			msg1 = t_right.toImageMsg();
249 | 			
250 | 			pub1.publish(msg1);
251 | 		}
252 | 		else if(visensor_cam_selection==2)
253 | 		{
254 | 			visensor_imudata paired_imu=visensor_get_leftImg((char *)img_left.data,left_stamp);
255 | 
256 | 			// 显示同步数据的时间戳（单位微秒）
257 | 			cout<<"left_time : "<<left_stamp.tv_usec<<endl;
258 | 			cout<<"paired_imu time ===== "<<paired_imu.system_time.tv_usec<<endl<<endl;
259 | 
260 | 			cv_bridge::CvImage t_left=cv_bridge::CvImage(std_msgs::Header(), "mono8", img_left);
261 | 
262 | 			// 加时间戳
263 | 			ros::Time msg_time;
264 | 			msg_time.sec=left_stamp.tv_sec;
265 | 			msg_time.nsec=1000*left_stamp.tv_usec;
266 | 			t_left.header.stamp = msg_time;
267 | 			t_left.header.seq=0;
268 | 
269 | 
270 | 			msg = t_left.toImageMsg();
271 | 
272 | 			
273 | 			static_ct++;
274 | 			if(static_ct>=5)
275 | 			{
276 | 				pub.publish(msg);
277 | 				static_ct=0;
278 | 			}
279 | 		}
280 | 
281 | 		ros::spinOnce(); 
282 | 
283 | 		loop_rate.sleep(); 
284 | 		
285 | 	}
286 | 
287 | 	/* shut-down viewers */
288 | 	visensor_Close_IMU_viewer=true;
289 | 	if(imu_data_thread !=0)
290 | 	{
291 | 		pthread_join(imu_data_thread,NULL);
292 | 	}
293 | 
294 | 	cout<<endl<<"shutting-down Cameras"<<endl;
295 | 
296 | 	/* close cameras */
297 | 	visensor_Close_Cameras();
298 | 	/* close IMU */
299 | 	visensor_Close_IMU();
300 | 	
301 | 	return 0;
302 | }
303 | 
304 | 
305 | 
306 | 
307 | 
308 | 
309 | 
310 | 
311 | 
312 | 
313 | 
314 | 


--------------------------------------------------------------------------------
/loitor_ros_workspace/src/loitor_stereo_visensor/src/loitorimu.cpp:
--------------------------------------------------------------------------------
  1 | #include <stdio.h>
  2 | #include <unistd.h>
  3 | #include <stdlib.h>
  4 | #include <getopt.h>
  5 | #include <string.h>
  6 | #include <pthread.h>
  7 | #include <math.h>
  8 | #include <fcntl.h>
  9 | #include <errno.h>
 10 | #include <termios.h>
 11 | #include "math.h"
 12 | 
 13 | #define IMU_FRAME_LEN 32
 14 | #include "loitorimu.h"
 15 | 
 16 | bool shut_down_imu=false;
 17 | 
 18 | timeval visensor_startTime;		// time-stamp
 19 | struct timeval imu_SysTime;
 20 | 
 21 | float imu_time=0;
 22 | bool visensor_imu_updated=false;
 23 | 
 24 | bool visensor_query_imu_update()
 25 | {
 26 |     return visensor_imu_updated;
 27 | }
 28 | bool visensor_mark_imu_update()
 29 | {
 30 |     visensor_imu_updated=true;
 31 | }
 32 | bool visensor_erase_imu_update()
 33 | {
 34 |     visensor_imu_updated=false;
 35 | }
 36 | 
 37 | int visensor_set_opt(int fd,int nSpeed, int nBits, char nEvent, int nStop)
 38 | {
 39 |     struct termios newtio,oldtio;
 40 |     if ( tcgetattr( fd,&oldtio) != 0)
 41 |     {
 42 |         perror("SetupSerial 1");
 43 |         return -1;
 44 |     }
 45 |     bzero( &newtio, sizeof( newtio ) );
 46 |     newtio.c_cflag |= CLOCAL | CREAD;
 47 |     newtio.c_cflag &= ~CSIZE;
 48 | 
 49 |     switch( nBits )
 50 |     {
 51 |     case 7:
 52 |         newtio.c_cflag |= CS7;
 53 |         break;
 54 |     case 8:
 55 |         newtio.c_cflag |= CS8;
 56 |         break;
 57 |     }
 58 | 
 59 |     switch( nEvent )
 60 |     {
 61 |     case 'O':
 62 |         newtio.c_cflag |= PARENB;
 63 |         newtio.c_cflag |= PARODD;
 64 |         newtio.c_iflag |= (INPCK | ISTRIP);
 65 |         break;
 66 |     case 'E':
 67 |         newtio.c_iflag |= (INPCK | ISTRIP);
 68 |         newtio.c_cflag |= PARENB;
 69 |         newtio.c_cflag &= ~PARODD;
 70 |         break;
 71 |     case 'N':
 72 |         newtio.c_cflag &= ~PARENB;
 73 |         break;
 74 |     }
 75 | 
 76 |     switch( nSpeed )
 77 |     {
 78 |     case 2400:
 79 |         cfsetispeed(&newtio, B2400);
 80 |         cfsetospeed(&newtio, B2400);
 81 |         break;
 82 |     case 4800:
 83 |         cfsetispeed(&newtio, B4800);
 84 |         cfsetospeed(&newtio, B4800);
 85 |         break;
 86 |     case 9600:
 87 |         cfsetispeed(&newtio, B9600);
 88 |         cfsetospeed(&newtio, B9600);
 89 |         break;
 90 |     case 115200:
 91 |         cfsetispeed(&newtio, B115200);
 92 |         cfsetospeed(&newtio, B115200);
 93 |         break;
 94 |     default:
 95 |         cfsetispeed(&newtio, B9600);
 96 |         cfsetospeed(&newtio, B9600);
 97 |         break;
 98 |     }
 99 |     if( nStop == 1 )
100 |         newtio.c_cflag &= ~CSTOPB;
101 |     else if ( nStop == 2 )
102 |         newtio.c_cflag |= CSTOPB;
103 |     newtio.c_cc[VTIME] = 0;
104 |     newtio.c_cc[VMIN] = 32;
105 |     tcflush(fd,TCIFLUSH);
106 |     if((tcsetattr(fd,TCSANOW,&newtio))!=0)
107 |     {
108 |         perror("com set error");
109 |         return -1;
110 |     }
111 |     printf("set done!\n");
112 |     return 0;
113 | }
114 | 
115 | int visensor_open_port(const char* dev_str)
116 | {
117 |     long vdisable;
118 | 
119 |     int fd = open( dev_str, O_RDWR|O_NOCTTY|O_NDELAY);
120 |     if (-1 == fd)
121 |     {
122 |         perror("Can't Open Serial Port");
123 |         return(-1);
124 |     }
125 | 
126 |     if(fcntl(fd, F_SETFL, 0)<0)
127 |         printf("fcntl failed!\n");
128 |     else
129 |         printf("fcntl=%d\n",fcntl(fd, F_SETFL,0));
130 |     if(isatty(STDIN_FILENO)==0)
131 |         printf("standard input is not a terminal device\n");
132 |     else
133 |         printf("isatty success!\n");
134 |     printf("fd-open=%d\n",fd);
135 |     return fd;
136 | }
137 | 
138 | static int find_55aa(unsigned char* buf,int len)
139 | {
140 |     int i;
141 |     for(i=0; i<len-1; i++)
142 |     {
143 |         if(buf[i]==0x55)
144 |             if(buf[i+1]==0xAA)
145 |                 return i;
146 |     }
147 |     if(buf[len-1]==0x55)
148 |         return -1;
149 |     return -2;
150 | }
151 | 
152 | 
153 | int visensor_send_imu_frame(int fd, unsigned char* data, int len)
154 | {
155 |     return write(fd,data,len);
156 | }
157 | 
158 | #include <sys/time.h>
159 | 
160 | int visensor_get_imu_frame(int fd, unsigned char* imu_frame)
161 | {
162 | 
163 |     unsigned char imu_frame_buf[2*IMU_FRAME_LEN];
164 |     memset(imu_frame,0,IMU_FRAME_LEN);
165 |     memset(imu_frame_buf,0,2*IMU_FRAME_LEN);
166 |     int num_get = 0;
167 | 
168 |     static int flush_count = 0;
169 |     flush_count = (flush_count+1)%200;
170 |     if(flush_count==0)
171 |         tcflush(fd,TCIFLUSH);
172 |     static struct timeval getIMUTime;
173 |     struct timeval endTime;
174 |     float fTimeuse;
175 |     while(!shut_down_imu)
176 |     {
177 |         //读到32个以上字节
178 |         while(num_get<IMU_FRAME_LEN)
179 |         {
180 |             int temp_read_get = read(fd,&imu_frame_buf[num_get],IMU_FRAME_LEN);
181 |             if(temp_read_get<=0)
182 |             {
183 |                 continue;//读取错误，不得往num_get上加
184 |             }
185 |             num_get+=temp_read_get;
186 |         }//实际读到了num_get字节数
187 |         gettimeofday(&getIMUTime,NULL);
188 |         gettimeofday(&endTime,NULL);
189 |         gettimeofday(&imu_SysTime,NULL);
190 |         imu_SysTime.tv_usec-=3500;
191 |         if(imu_SysTime.tv_usec<0){imu_SysTime.tv_usec+=1000000;imu_SysTime.tv_sec-=1;}
192 | 
193 |         //查询55AA的位置
194 |         int position_55aa = find_55aa(imu_frame_buf,num_get);
195 |         //如果查不到55AA(-2)，末尾也没有55，那么此次读取失败，清空imu_frame_buf,从头再来
196 |         if(position_55aa == -2)
197 |         {
198 |             memset(imu_frame_buf,0,2*IMU_FRAME_LEN);
199 |             num_get = 0;
200 |             continue;
201 |         }
202 |         //如果末尾是55，那么清空imu_frame_buf,然后将首位置为55
203 |         if(position_55aa == -1)
204 |         {
205 |             memset(imu_frame_buf,0,2*IMU_FRAME_LEN);
206 |             num_get = 1;
207 |             imu_frame_buf[0] = 0x55;
208 |             continue;
209 |         }
210 |         //如果找到了55AA，那么首先检验有效帧长度
211 |         int valid_len = num_get - position_55aa;
212 |         //如果有效帧长度不足32，那么将数据移动到帧头，然后继续读取剩余帧
213 |         if(valid_len<32)
214 |         {
215 |             for(int i=0; i<valid_len; i++)
216 |                 imu_frame_buf[i] = imu_frame_buf[position_55aa+i];
217 |             for(int i=valid_len; i<2*IMU_FRAME_LEN; i++)
218 |                 imu_frame_buf[i] = 0;
219 |             num_get = valid_len;
220 |             continue;
221 |         }
222 |         //如果有效帧长度达到32，那么检查校验和
223 |         unsigned char checksum = 0;
224 |         for(int i=2; i<31; i++)checksum += imu_frame_buf[position_55aa+i];
225 |         //如果校验和不正确，那么清除该帧头和之前的所有数据，然后将剩余数据移动到帧头，再继续读取剩余数据
226 |         if(checksum != imu_frame_buf[position_55aa+31])
227 |         {
228 |             for(int i=2; i<valid_len; i++)
229 |                 imu_frame_buf[i-2] = imu_frame_buf[position_55aa+i];
230 |             for(int i=valid_len-2; i<2*IMU_FRAME_LEN; i++)
231 |                 imu_frame_buf[i] = 0;
232 |             num_get = valid_len-2;
233 |             continue;
234 |         }
235 |         //如果校验和正确，而有效帧长度超过了32，那么说明已经有数据溢出，应立即清除串口缓存,然后重新开始读取
236 |         if(valid_len>32)
237 |         {
238 |             tcflush(fd,TCIFLUSH);
239 |             memset(imu_frame_buf,0,2*IMU_FRAME_LEN);
240 |             num_get = 0;
241 |             continue;
242 |         }
243 |         //如果校验和正确，那么输出该帧数据
244 |         memcpy(imu_frame,&imu_frame_buf[position_55aa],IMU_FRAME_LEN);
245 |         static float time_interval=5000;
246 |         static float last_time,new_time,new_pred,last_pred;
247 |         new_time = getIMUTime.tv_usec;
248 |         int newtime_interval;
249 |         newtime_interval = new_time-last_time;
250 |         if(newtime_interval<4000||newtime_interval>6000)
251 |             newtime_interval = 5000;
252 |         time_interval = time_interval*0.999+0.001*newtime_interval;
253 |         if(new_time-last_pred<time_interval)
254 |             new_pred = new_time;
255 |         else
256 |             new_pred = last_pred + time_interval + 0.001*(new_time - last_pred - time_interval);
257 |         last_time = new_time;
258 |         last_pred = new_pred;
259 | 
260 |         fTimeuse = (endTime.tv_sec - visensor_startTime.tv_sec) + 0.000001*(endTime.tv_usec - visensor_startTime.tv_usec);
261 |         imu_time=fTimeuse;
262 | 
263 |         break;
264 |     }
265 | 
266 |     return 0;
267 | }
268 | 
269 | float data_norm(float * q,int length)
270 | {
271 |     float datasum=0.0;
272 |     for(int i=0; i<length; i++)datasum+=q[i]*q[i];
273 |     return sqrt(datasum);
274 | }
275 | int q_normalize(float* q)
276 | {
277 |     float qnorm = data_norm(q,4);
278 |     for(int i=0; i<4; i++)q[i]/=qnorm;
279 | }
280 | 
281 | int q_mult(float* q1,float* q2,float* qout)
282 | {
283 |     qout[0]=q1[0]*q2[0]-q1[1]*q2[1]-q1[2]*q2[2]-q1[3]*q2[3];
284 |     qout[1]=q1[0]*q2[1]+q1[1]*q2[0]+q1[2]*q2[3]-q1[3]*q2[2];
285 |     qout[2]=q1[0]*q2[2]+q1[2]*q2[0]+q1[3]*q2[1]-q1[1]*q2[3];
286 |     qout[3]=q1[0]*q2[3]+q1[3]*q2[0]+q1[1]*q2[2]-q1[2]*q2[1];
287 | }
288 | 
289 | int q_rotvec(float* q,float* rin,float* rout)
290 | {
291 |     float q_rin[4]= {0,rin[0],rin[1],rin[2]};
292 |     float q_temp[4];
293 |     q_mult(q,q_rin,q_temp);
294 |     float q_inv[4]= {q[0],-q[1],-q[2],-q[3]};
295 |     float q_temp_out[4];
296 |     q_mult(q_temp,q_inv,q_temp_out);
297 |     for(int i=0; i<3; i++)rout[i]=q_temp_out[i+1];
298 | }
299 | 
300 | 
301 | int visensor_get_imu_data(unsigned char* imu_frame,short int* acc_offset,visensor_imudata *imudata_struct,bool show_data=false)
302 | {
303 | 
304 |     imudata_struct->imu_time=imu_time;
305 |     imudata_struct->system_time=imu_SysTime;
306 | 
307 |     static int last_imu_no;
308 |     //for(int i=1; i<(imu_frame[2]+200-last_imu_no)%200; i++)
309 |     //    printf(".................................................................................\r\n");
310 |     last_imu_no = imu_frame[2];
311 |     int imu_data[10];
312 |     imu_data[0] = *(short*)(&imu_frame[3]);
313 |     imu_data[1] = *(short*)(&imu_frame[5]);
314 |     imu_data[2] = *(short*)(&imu_frame[7]);
315 | 
316 |     imu_data[3] = *(short*)(&imu_frame[9]);
317 |     imu_data[4] = *(short*)(&imu_frame[11]);
318 |     imu_data[5] = *(short*)(&imu_frame[13]);
319 | 
320 | 
321 |     imu_data[6] = *(int*)(&imu_frame[15]);
322 |     imu_data[7] = *(int*)(&imu_frame[19]);
323 |     imu_data[8] = *(int*)(&imu_frame[23]);
324 |     imu_data[9] = *(int*)(&imu_frame[27]);
325 | 
326 |     imudata_struct->rx = 1.0*imu_data[0]/32768*2000;
327 |     imudata_struct->ry = 1.0*imu_data[1]/32768*2000;
328 |     imudata_struct->rz = 1.0*imu_data[2]/32768*2000;
329 |     float facc_nobias[3]= {
330 |         (float)(imu_data[3]-acc_offset[0]),
331 |         (float)(imu_data[4]-acc_offset[1]),
332 |         (float)(imu_data[5]-acc_offset[2])};
333 | 
334 |     imudata_struct->ax = 1.0*facc_nobias[0]/16384*9.81;
335 |     imudata_struct->ay = 1.0*facc_nobias[1]/16384*9.81;
336 |     imudata_struct->az = 1.0*facc_nobias[2]/16384*9.81;
337 | 
338 |     float quat[4]= {
339 |         (float)imu_data[6],
340 |         (float)imu_data[7],
341 |         (float)imu_data[8],
342 |         (float)imu_data[9]};
343 |     float facc[3]= {
344 |         (float)imu_data[3],
345 |         (float)imu_data[4],
346 |         (float)imu_data[5]};
347 | 
348 |     static float faccg[3]= {0,0,0};
349 |     q_normalize(quat);
350 |     imudata_struct->qw = quat[0];
351 |     imudata_struct->qx = quat[1];
352 |     imudata_struct->qy = quat[2];
353 |     imudata_struct->qz = quat[3];
354 | 
355 |     imudata_struct->num = imu_frame[2];
356 | 
357 |     q_rotvec(quat,facc_nobias,faccg);
358 | 
359 |     if(show_data)
360 |     {
361 |         printf("Num: %03d",imu_frame[2]);
362 |         printf("  Gyr: %6d,%6d,%6d",imu_data[0],imu_data[1],imu_data[2]);
363 |         printf("  Acc: %6d,%6d,%6d",(int)facc[0],(int)facc[1],(int)facc[2]);
364 |         printf("  Quat: %11f,%11f,%11f,%11f\r\n",quat[0],quat[1],quat[2],quat[3]);
365 |     }
366 | }
367 | 


--------------------------------------------------------------------------------
/loitor_ros_workspace/src/loitor_stereo_visensor/src/loitorusbcam.cpp:
--------------------------------------------------------------------------------
   1 | #include <stdio.h>
   2 | #include <stdlib.h>
   3 | #include <unistd.h>
   4 | #include <getopt.h>
   5 | #include <fcntl.h>
   6 | #include <string.h>
   7 | #include <math.h>
   8 | #include <pthread.h>
   9 | #include <sys/time.h>
  10 | #include <linux/types.h>
  11 | 
  12 | #include <iostream>
  13 | #include <algorithm>
  14 | #include <fstream>
  15 | #include <string>
  16 | #include <vector>
  17 | #include <iomanip>
  18 | #include <sstream>
  19 | 
  20 | #include <libusb-1.0/libusb.h>
  21 | 
  22 | #include "loitorimu.h"
  23 | 
  24 | typedef struct libusb_device	cyusb_device;
  25 | typedef struct libusb_device_handle	cyusb_handle;
  26 | 
  27 | /* This is the maximum number of 'devices of interest' we are willing to store as default. */
  28 | /* These are the maximum number of devices we will communicate with simultaneously */
  29 | #define MAXDEVICES        10
  30 | 
  31 | /* This is the maximum number of VID/PID pairs that this library will consider. This limits
  32 |    the number of valid VID/PID entries in the configuration file.
  33 |  */
  34 | #define MAX_ID_PAIRS    100
  35 | 
  36 | /* This is the maximum length for the description string for a device in the configuration
  37 |    file. If the actual string in the file is longer, only the first MAX_STR_LEN characters
  38 |    will be considered.
  39 |  */
  40 | #define MAX_STR_LEN     30
  41 | 
  42 | #define LOG			printf
  43 | #define	DEBUGLOG	printf
  44 | #define	CODELOG		printf("File:%s\r\nLine:%d \r\n",__FILE__,__LINE__);
  45 | 
  46 | /*PARAMETER MACRO*/
  47 | #define IMG_WIDTH_VGA 	640
  48 | #define IMG_HEIGHT_VGA 	480
  49 | #define IMG_SIZE_VGA 	(IMG_WIDTH_VGA*IMG_HEIGHT_VGA)
  50 | #define IMG_BUF_SIZE_VGA (IMG_SIZE_VGA+0x200)
  51 | #define IMG_WIDTH_WVGA 	752
  52 | #define IMG_HEIGHT_WVGA 	480
  53 | #define IMG_SIZE_WVGA 	(IMG_WIDTH_WVGA*IMG_HEIGHT_WVGA)
  54 | #define IMG_BUF_SIZE_WVGA (IMG_SIZE_WVGA+0x200)
  55 | #define FRAME_CLUST 216
  56 | 
  57 | /*CAMERA CONTROL INSTRUCTION MACRO*/
  58 | #define CAPTURE_27			0xA1
  59 | #define CAPTURE_54			0xA2
  60 | #define CAPTURE_108			0xA6
  61 | #define STANDBY_SHORT		0xA3
  62 | #define STANDBY_LONG		0xA4
  63 | #define GET_CAM_LR			0xA5
  64 | #define CAM_I2C_R			0xA7
  65 | #define CAM_I2C_W			0xA8
  66 | #define auto_expo			1
  67 | #define EXP_VAL				50
  68 | #define GAI_VAL				100
  69 | 
  70 | #define CAM_I2C_ADDR		0x5C //0x5C
  71 | 
  72 | /* Maximum length of a string read from the Configuration file (/etc/cyusb.conf) for the library. */
  73 | #define MAX_CFG_LINE_LENGTH                     (120)
  74 | 
  75 | #define IMU_FRAME_LEN 32
  76 | pthread_t imu_thread;
  77 | int imu_fd=0;
  78 | bool imu_close=false;
  79 | 
  80 | using namespace std;
  81 | 
  82 | 
  83 | visensor_imudata IMU_FIFO[200];
  84 | int imu_fifo_ct=0;
  85 | 
  86 | bool shut_down_tag=false;
  87 | 
  88 | bool left_fresh=false;
  89 | bool right_fresh=false;
  90 | 
  91 | bool allow_settings_change=true;
  92 | 
  93 | /****************************************************/
  94 | int data_mode;
  95 | int modes_settings[13][4];
  96 | int EG_mode;
  97 | int man_exp;
  98 | int man_gain;
  99 | int auto_EG_top;
 100 | int auto_EG_bottom;
 101 | int auto_EG_des;
 102 | int auto_E_man_G_Etop;
 103 | int auto_E_man_G_Ebottom;
 104 | int auto_E_man_G;
 105 | int agc_aec_skip_frame;
 106 | int VI_FIFO_matcher;
 107 | 
 108 | double imu_acc_bias_X;
 109 | double imu_acc_bias_Y;
 110 | double imu_acc_bias_Z;
 111 | 
 112 | string imu_port_name;
 113 | visensor_imudata visensor_imudata_pack;
 114 | /****************************************************/
 115 | 
 116 | cyusb_handle *pcam1_handle;
 117 | cyusb_handle *pcam2_handle;
 118 | int gFPS = 27;
 119 | int gSelectCam = 0;
 120 | int HORIZ_BLK=150;
 121 | 
 122 | pthread_t cam1_capture_thread;
 123 | pthread_t cam2_capture_thread;
 124 | 
 125 | pthread_t cam1_capture_detect_thread;
 126 | pthread_t cam2_capture_detect_thread;
 127 | 
 128 | int gFrameCam1=0;
 129 | int gFrameCam2=0;
 130 | int gImg1Pass[FRAME_CLUST];
 131 | int gImg2Pass[FRAME_CLUST];
 132 | float gImg1Time[FRAME_CLUST];		// time-stamp
 133 | float gImg2Time[FRAME_CLUST];
 134 | timeval gImg1_SysTime[FRAME_CLUST];
 135 | timeval gImg2_SysTime[FRAME_CLUST];
 136 | 
 137 | unsigned char gImg1_VGA[FRAME_CLUST][IMG_BUF_SIZE_VGA];
 138 | unsigned char gImg2_VGA[FRAME_CLUST][IMG_BUF_SIZE_VGA];
 139 | 
 140 | unsigned char gImg1_WVGA[FRAME_CLUST][IMG_BUF_SIZE_WVGA];
 141 | unsigned char gImg2_WVGA[FRAME_CLUST][IMG_BUF_SIZE_WVGA];
 142 | 
 143 | bool last_is_good=false;
 144 | bool save_img=false;
 145 | bool save_img1=false;
 146 | 
 147 | bool visensor_resolution_status=false;
 148 | int current_HB=194;
 149 | int visensor_cam_selection=0;
 150 | int current_FPS=27;
 151 | 
 152 | string visensor_settings_file_name;
 153 | 
 154 | 
 155 | struct cydev
 156 | {
 157 |     cyusb_device *dev;
 158 |     cyusb_handle *handle;
 159 |     unsigned short vid;
 160 |     unsigned short pid;         /* Product ID */
 161 |     unsigned char is_open;      /* When device is opened, val = 1 */
 162 |     unsigned char busnum;       /* The bus number of this device */
 163 |     unsigned char devaddr;      /* The device address*/
 164 |     unsigned char filler;       /* Padding to make struct = 16 bytes */
 165 | };
 166 | 
 167 | static struct cydev cydev[MAXDEVICES];
 168 | static int number_of_cameras;		/* Number of Interesting Devices	*/
 169 | 
 170 | struct VPD
 171 | {
 172 |     unsigned short vid;
 173 |     unsigned short pid;
 174 |     char desc[MAX_STR_LEN];
 175 | };
 176 | 
 177 | static struct VPD vpd[MAX_ID_PAIRS];
 178 | 
 179 | static libusb_device **libusb_device_list;
 180 | static unsigned int checksum = 0;
 181 | 
 182 | char pidfile[256];
 183 | char logfile[256];
 184 | int logfd;
 185 | int pidfd;
 186 | 
 187 | float visensor_get_hardware_fps();
 188 | 
 189 | 
 190 | void visensor_load_settings(const char* settings_file)
 191 | {
 192 |     /******************************* Load Settings File ***************************************/
 193 | 
 194 |     LOG("\r\n**********************************************\r\n");
 195 |     LOG("Loading Settings File...\r\n");
 196 | 
 197 |     visensor_settings_file_name=settings_file;
 198 | 
 199 |     ifstream ifs_settings(visensor_settings_file_name.c_str());//,ios::in|ios::binary|ios::ate);
 200 |     if(!ifs_settings )
 201 |     {
 202 |         cerr << "Error opening file: "<<visensor_settings_file_name <<endl;
 203 |         exit(EXIT_FAILURE);
 204 |     }
 205 |     else
 206 |     {
 207 |         cout<<"Successfully opened settings file!"<<endl;
 208 |     }
 209 | 
 210 |     long file_size=ifs_settings.tellg();
 211 | 
 212 |     //cout<<"filesize:"<<file_size<<endl;
 213 | 
 214 |     std::string filename;
 215 |     std::string line_settings;
 216 |     std::string timestamp;
 217 |     std::vector<string> settingsList;
 218 |     int linecount=0;
 219 |     while(std::getline(ifs_settings,line_settings))
 220 |     {
 221 |         linecount++;
 222 |         if(line_settings.at(0)!='#')
 223 |             settingsList.push_back(line_settings);
 224 |     }
 225 |     //cout <<"line get end............"<<endl;
 226 |     for (vector<string>::iterator iter = settingsList.begin(); iter != settingsList.end(); ++iter)
 227 |     {
 228 |         cout << *iter << endl;
 229 |     }
 230 |     ifs_settings.close();
 231 | 
 232 | 
 233 |     std::string token;
 234 | 
 235 |     // convert to File_Settings Object
 236 |     istringstream(settingsList.at(1))>>data_mode;
 237 |     //cout << "data_mode: "<<data_mode<<endl;
 238 | 
 239 |     for(int i=3; i<=15; i++)
 240 |     {
 241 |         if(i==9)continue;
 242 |         int visensor_cam_selection=0,_HB=0,res=0,fps=0;
 243 | 
 244 |         istringstream iss_line(settingsList.at(i));
 245 |         //The first word, ignore
 246 |         std::getline(iss_line, token, ',');
 247 | 
 248 |         //The second word, lrs
 249 |         std::getline(iss_line, token, ',');
 250 |         if(token[0]=='l')visensor_cam_selection=2;
 251 |         else if(token[0]=='r')visensor_cam_selection=1;
 252 |         else if(token[0]=='s')visensor_cam_selection=0;
 253 | 
 254 |         //_HB
 255 |         std::getline(iss_line, token, ',');
 256 |         istringstream(token)>>_HB;
 257 | 
 258 |         //WVGA or VGA?
 259 |         std::getline(iss_line, token, ',');
 260 |         res=(token[0]=='V'?0:1);
 261 | 
 262 |         //FPS
 263 |         std::getline(iss_line, token, ',');
 264 |         istringstream(token)>>fps;
 265 | 
 266 |         // fill data
 267 |         if(i<9)// “HighSpeed Pre-Set”
 268 |         {
 269 |             modes_settings[i-3][0]=visensor_cam_selection;
 270 |             modes_settings[i-3][1]=_HB;
 271 |             modes_settings[i-3][2]=res;
 272 |             modes_settings[i-3][3]=fps;
 273 |         }
 274 |         else// “Normal Pre-Set”
 275 |         {
 276 |             modes_settings[i-4][0]=visensor_cam_selection;
 277 |             modes_settings[i-4][1]=_HB;
 278 |             modes_settings[i-4][2]=res;
 279 |             modes_settings[i-4][3]=fps;
 280 |         }
 281 | 
 282 |     }
 283 | 
 284 |     // Manual Mode
 285 |     {
 286 |         int visensor_cam_selection=0,_HB=0,res=0,fps=0;
 287 | 
 288 |         istringstream(settingsList.at(17))>>visensor_cam_selection;
 289 |         istringstream(settingsList.at(18))>>_HB;
 290 |         token = settingsList.at(19);
 291 |         res=(token[0]=='V'?0:1);
 292 |         istringstream(settingsList.at(20))>>fps;
 293 | 
 294 |         modes_settings[12][0]=visensor_cam_selection;
 295 |         modes_settings[12][1]=_HB;
 296 |         modes_settings[12][2]=res;
 297 |         modes_settings[12][3]=fps;
 298 |     }
 299 | 
 300 |     // EXP&GAIN
 301 |     istringstream(settingsList.at(22))>>EG_mode;
 302 | 
 303 |     // manual E&G
 304 |     {
 305 |         int _exp=0,_gain=0;
 306 | 
 307 |         istringstream iss_line(settingsList.at(23));
 308 |         //The first word, ignore
 309 |         std::getline(iss_line, token, ',');
 310 | 
 311 |         std::getline(iss_line, token, ',');
 312 |         istringstream(token)>>_exp;
 313 |         std::getline(iss_line, token, ',');
 314 |         istringstream(token)>>_gain;
 315 | 
 316 |         // fill data
 317 |         man_exp=_exp;
 318 |         man_gain=_gain;
 319 |     }
 320 | 
 321 |     // auto
 322 |     {
 323 |         int _top=0,_bottom=0,_des=0;
 324 | 
 325 |         istringstream iss_line(settingsList.at(24));
 326 |         //The first word, ignore
 327 |         std::getline(iss_line, token, ',');
 328 | 
 329 |         std::getline(iss_line, token, ',');
 330 |         istringstream(token)>>_top;
 331 | 
 332 |         std::getline(iss_line, token, ',');
 333 |         istringstream(token)>>_bottom;
 334 | 
 335 |         std::getline(iss_line, token, ',');
 336 |         istringstream(token)>>_des;
 337 |         // fill data
 338 |         auto_EG_top=_top;
 339 |         auto_EG_bottom=_bottom;
 340 |         auto_EG_des=_des;
 341 |     }
 342 | 
 343 |     // AE_MG
 344 |     {
 345 |         int _top=0,_bottom=0,_des=0,_gain=0;
 346 | 
 347 |         istringstream iss_line(settingsList.at(25));
 348 | 
 349 |         //The first word, ignore
 350 |         std::getline(iss_line, token, ',');
 351 | 
 352 |         std::getline(iss_line, token, ',');
 353 |         istringstream(token)>>_top;
 354 |         std::getline(iss_line, token, ',');
 355 |         istringstream(token)>>_bottom;
 356 |         std::getline(iss_line, token, ',');
 357 |         istringstream(token)>>_des;
 358 |         std::getline(iss_line, token, ',');
 359 |         istringstream(token)>>_gain;
 360 |         // fill data
 361 |         auto_E_man_G_Etop=_top;
 362 |         auto_E_man_G_Ebottom=_bottom;
 363 |         auto_E_man_G=_gain;
 364 |     }
 365 |     // imu_port_name
 366 |     {
 367 |         istringstream iss_line(settingsList.at(26));
 368 |         std::getline(iss_line, token, ',');
 369 | 
 370 |         imu_port_name = token.c_str();
 371 | 
 372 |         std::getline(iss_line, token, ',');
 373 |         istringstream(token)>>VI_FIFO_matcher;
 374 |     }
 375 |     // imu acc bias
 376 |     {
 377 |         istringstream iss_line(settingsList.at(28));
 378 |         //The first word, ignore
 379 |         std::getline(iss_line, token, ',');
 380 |         std::getline(iss_line, token, ',');
 381 |         istringstream(token)>>imu_acc_bias_X;
 382 |     }
 383 |     {
 384 |         istringstream iss_line(settingsList.at(29));
 385 |         //The first word, ignore
 386 |         std::getline(iss_line, token, ',');
 387 |         std::getline(iss_line, token, ',');
 388 |         istringstream(token)>>imu_acc_bias_Y;
 389 |     }
 390 |     {
 391 |         istringstream iss_line(settingsList.at(30));
 392 |         //The first word, ignore
 393 |         std::getline(iss_line, token, ',');
 394 |         std::getline(iss_line, token, ',');
 395 |         istringstream(token)>>imu_acc_bias_Z;
 396 |     }
 397 | 
 398 |     // show settings
 399 |     cout<<endl<<"data_mode : "<<data_mode<<endl;
 400 |     for(int i=0; i<13; i++)
 401 |     {
 402 |         cout<<"mode : "<<(i+1)<<endl;
 403 |         cout<<"fps : "<<modes_settings[i][3]<<endl;
 404 |         cout<<"HB : "<<modes_settings[i][1]<<endl;
 405 |         cout<<"visensor_cam_selection : "<<modes_settings[i][0]<<endl;
 406 |         cout<<"res : "<<modes_settings[i][2]<<endl<<endl;
 407 |     }
 408 |     cout<<"EG_mode : "<<EG_mode<<endl;
 409 |     cout<<"man_exp : "<<man_exp<<endl;
 410 |     cout<<"man_gain : "<<man_gain<<endl;
 411 |     cout<<"auto_EG_top : "<<auto_EG_top<<endl;
 412 |     cout<<"auto_EG_bottom : "<<auto_EG_bottom<<endl;
 413 |     cout<<"auto_EG_des : "<<auto_EG_des<<endl;
 414 |     cout<<"auto_E_man_G_Etop : "<<auto_E_man_G_Etop<<endl;
 415 |     cout<<"auto_E_man_G_Ebottom : "<<auto_E_man_G_Ebottom<<endl;
 416 |     cout<<"auto_E_man_G : "<<auto_E_man_G<<endl;
 417 |     cout<<"VI_FIFO_matcher : "<<VI_FIFO_matcher<<endl;
 418 |     cout<<"imu_acc_bias_X :  "<<imu_acc_bias_X<<endl;
 419 |     cout<<"imu_acc_bias_Y :  "<<imu_acc_bias_Y<<endl;
 420 |     cout<<"imu_acc_bias_Z :  "<<imu_acc_bias_Z<<endl;
 421 |     cout<<"imu_port_name :  "<<imu_port_name<<endl<<endl;
 422 | 
 423 |     // 预设模式+手动模式
 424 |     current_FPS=modes_settings[data_mode-1][3];
 425 |     gFPS = current_FPS;
 426 |     visensor_resolution_status=modes_settings[data_mode-1][2];
 427 |     current_HB=modes_settings[data_mode-1][1];
 428 |     visensor_cam_selection=modes_settings[data_mode-1][0];
 429 | 
 430 |     /****************************************************************************/
 431 | 
 432 | 
 433 |     if(visensor_cam_selection==0)gSelectCam = 3;
 434 |     else if(visensor_cam_selection==2)gSelectCam = 1;
 435 |     else if(visensor_cam_selection==1)gSelectCam = 2;
 436 | 
 437 |     //Add: AGC/AEC Skip frames
 438 |     agc_aec_skip_frame = (int)(visensor_get_hardware_fps()/10);
 439 | }
 440 | 
 441 | // setters
 442 | void visensor_set_auto_EG(int E_AG)
 443 | {
 444 |     if(!allow_settings_change)
 445 |     {
 446 |         cout<<"settings FIXED !"<<endl;
 447 |         return;
 448 |     }
 449 |     EG_mode=E_AG;
 450 | }
 451 | void visensor_set_exposure(int _man_exp)
 452 | {
 453 |     if(!allow_settings_change)
 454 |     {
 455 |         cout<<"settings FIXED !"<<endl;
 456 |         return;
 457 |     }
 458 |     man_exp=_man_exp;
 459 | }
 460 | void visensor_set_gain(int _man_gain)
 461 | {
 462 |     if(!allow_settings_change)
 463 |     {
 464 |         cout<<"settings FIXED !"<<endl;
 465 |         return;
 466 |     }
 467 |     man_gain=_man_gain;
 468 |     auto_E_man_G=_man_gain;
 469 | }
 470 | void visensor_set_max_autoExp(int max_exp)
 471 | {
 472 |     if(!allow_settings_change)
 473 |     {
 474 |         cout<<"settings FIXED !"<<endl;
 475 |         return;
 476 |     }
 477 |     auto_EG_top=max_exp;
 478 | }
 479 | void visensor_set_min_autoExp(int min_exp)
 480 | {
 481 |     if(!allow_settings_change)
 482 |     {
 483 |         cout<<"settings FIXED !"<<endl;
 484 |         return;
 485 |     }
 486 |     auto_EG_bottom=min_exp;
 487 | }
 488 | void visensor_set_resolution(bool set_wvga)
 489 | {
 490 |     if(!allow_settings_change)
 491 |     {
 492 |         cout<<"settings FIXED !"<<endl;
 493 |         return;
 494 |     }
 495 |     visensor_resolution_status=set_wvga;
 496 | }
 497 | void visensor_set_fps_mode(bool fps_mode)
 498 | {
 499 |     if(!allow_settings_change)
 500 |     {
 501 |         cout<<"settings FIXED !"<<endl;
 502 |         return;
 503 |     }
 504 |     if(fps_mode==true)
 505 |     {
 506 |         gFPS=54;
 507 |         current_FPS=54;
 508 |     }
 509 |     else
 510 |     {
 511 |         gFPS=27;
 512 |         current_FPS=27;
 513 |     }
 514 | }
 515 | void visensor_set_current_HB(int HB)
 516 | {
 517 |     if(!allow_settings_change)
 518 |     {
 519 |         cout<<"settings FIXED !"<<endl;
 520 |         return;
 521 |     }
 522 |     current_HB=HB;
 523 | }
 524 | void visensor_set_desired_bin(int db)
 525 | {
 526 |     if(!allow_settings_change)
 527 |     {
 528 |         cout<<"settings FIXED !"<<endl;
 529 |         return;
 530 |     }
 531 |     auto_EG_des=db;
 532 | }
 533 | void visensor_set_cam_selection_mode(int _visensor_cam_selection)
 534 | {
 535 |     if(!allow_settings_change)
 536 |     {
 537 |         cout<<"settings FIXED !"<<endl;
 538 |         return;
 539 |     }
 540 |     visensor_cam_selection=_visensor_cam_selection;
 541 | }
 542 | void visensor_set_imu_bias(float bx,float by,float bz)
 543 | {
 544 |     if(!allow_settings_change)
 545 |     {
 546 |         cout<<"settings FIXED !"<<endl;
 547 |         return;
 548 |     }
 549 |     imu_acc_bias_X=bx;
 550 |     imu_acc_bias_Y=by;
 551 |     imu_acc_bias_Z=bz;
 552 | }
 553 | void visensor_set_imu_portname(char* input_name)
 554 | {
 555 |     if(!allow_settings_change)
 556 |     {
 557 |         cout<<"settings FIXED !"<<endl;
 558 |         return;
 559 |     }
 560 |     imu_port_name=input_name;
 561 | }
 562 | void visensor_set_current_mode(int _mode)
 563 | {
 564 |     if(!allow_settings_change||_mode>13||_mode<=0)
 565 |     {
 566 |         cout<<"settings FIXED !"<<endl;
 567 |         return;
 568 |     }
 569 |     data_mode=_mode;
 570 |     // 预设模式+手动模式
 571 |     current_FPS=modes_settings[data_mode-1][3];
 572 |     gFPS=current_FPS;
 573 |     visensor_resolution_status=modes_settings[data_mode-1][2];
 574 |     current_HB=modes_settings[data_mode-1][1];
 575 |     visensor_cam_selection=modes_settings[data_mode-1][0];
 576 | }
 577 | // getters
 578 | int visensor_get_EG_mode()
 579 | {
 580 |     return EG_mode;
 581 | }
 582 | int visensor_get_exposure()
 583 | {
 584 |     return man_exp;
 585 | }
 586 | int visensor_get_gain()
 587 | {
 588 |     return man_gain;
 589 | }
 590 | int visensor_get_max_autoExp()
 591 | {
 592 |     return auto_EG_top;
 593 | }
 594 | int visensor_get_min_autoExp()
 595 | {
 596 |     return auto_EG_bottom;
 597 | }
 598 | bool visensor_get_resolution()
 599 | {
 600 |     return visensor_resolution_status;
 601 | }
 602 | int visensor_get_fps()
 603 | {
 604 |     return current_FPS;
 605 | }
 606 | int visensor_get_current_HB()
 607 | {
 608 |     return current_HB;
 609 | }
 610 | int visensor_get_desired_bin()
 611 | {
 612 |     return auto_EG_des;
 613 | }
 614 | int visensor_get_cam_selection_mode()
 615 | {
 616 |     return visensor_cam_selection;
 617 | }
 618 | float visensor_get_imu_G_bias_x()
 619 | {
 620 |     return imu_acc_bias_X;
 621 | }
 622 | float visensor_get_imu_G_bias_y()
 623 | {
 624 |     return imu_acc_bias_Y;
 625 | }
 626 | float get_imu_bias_z()
 627 | {
 628 |     return imu_acc_bias_Z;
 629 | }
 630 | const char* visensor_get_imu_portname()
 631 | {
 632 |     return imu_port_name.c_str();
 633 | }
 634 | 
 635 | void visensor_save_current_settings()
 636 | {
 637 |     // 保存设置参数到原配置文件
 638 |     ofstream f;
 639 |     f.open(visensor_settings_file_name.c_str());
 640 |     f.close();
 641 |     f.open(visensor_settings_file_name.c_str());
 642 | 
 643 |     f << fixed;
 644 |     // start writing settings file
 645 |     f<<'#'<<endl<<"Mode"<<endl;
 646 |     f<<data_mode<<endl<<'#'<<endl<<"HighSpeed Pre-Set"<<endl;
 647 |     f<<"m1,left,"<<modes_settings[0][1]<<",VGA,108"<<endl;
 648 |     f<<"m2,right,"<<modes_settings[1][1]<<",VGA,108"<<endl;
 649 |     f<<"m3,left,"<<modes_settings[2][1]<<",WVGA,108"<<endl;
 650 |     f<<"m4,right,"<<modes_settings[3][1]<<",WVGA,108"<<endl;
 651 |     f<<"m5,stereo,"<<modes_settings[4][1]<<",VGA,54"<<endl;
 652 |     f<<"m6,stereo,"<<modes_settings[5][1]<<",WVGA,54"<<endl;
 653 | 
 654 |     f<<"#"<<endl<<"Normal Pre-Set"<<endl;
 655 | 
 656 |     f<<"m7,left,"<<modes_settings[6][1]<<",VGA,54"<<endl;
 657 |     f<<"m8,right,"<<modes_settings[7][1]<<",VGA,54"<<endl;
 658 |     f<<"m9,left,"<<modes_settings[8][1]<<",WVGA,54"<<endl;
 659 |     f<<"m10,right,"<<modes_settings[9][1]<<",WVGA,54"<<endl;
 660 |     f<<"m11,stereo,"<<modes_settings[10][1]<<",VGA,27"<<endl;
 661 |     f<<"m12,stereo,"<<modes_settings[11][1]<<",WVGA,27"<<endl;
 662 | 
 663 |     f<<"#"<<endl<<"m13,Manual Mode"<<endl<<modes_settings[12][0]
 664 |      <<endl<<modes_settings[12][1]<<endl;
 665 |     if(modes_settings[12][2]==0)f<<"VGA"<<endl;
 666 |     else f<<"WVGA"<<endl;
 667 |     f<<modes_settings[12][3]<<endl;
 668 | 
 669 |     f<<"#"<<endl<<"EG_mode"<<endl<<EG_mode<<endl;
 670 |     f<<"manual,"<<man_exp<<","<<man_gain<<endl;
 671 |     f<<"auto,"<<auto_EG_top<<","<<auto_EG_bottom<<","<<auto_EG_des<<endl;
 672 |     f<<"autoexp_manualgain,"<<auto_E_man_G_Etop<<","<<auto_E_man_G_Ebottom<<","<<auto_EG_des<<","<<auto_E_man_G<<endl;
 673 | 
 674 | 
 675 |     f<<imu_port_name<<","<<VI_FIFO_matcher<<endl<<"#"<<endl<<"IMU-acc-bias"<<endl;
 676 |     f<<"Gx,"<<imu_acc_bias_X<<endl;
 677 |     f<<"Gy,"<<imu_acc_bias_Y<<endl;
 678 | 
 679 |     f<<"Gz,"<<imu_acc_bias_Z<<endl<<"#"<<endl;
 680 | 
 681 |     f.close();
 682 | }
 683 | 
 684 | 
 685 | static int device_is_of_interest(cyusb_device *d)
 686 | {
 687 |     int found = 0;
 688 |     struct libusb_device_descriptor desc;
 689 |     libusb_get_device_descriptor(d, &desc);
 690 |     if ( (0x04b4 == desc.idVendor) && (0x1003 == desc.idProduct) )
 691 |         found = 1;
 692 |     return found;
 693 | }
 694 | 
 695 | int cyusb_open(void)
 696 | {
 697 |     int r;
 698 | 
 699 |     r = libusb_init(NULL);
 700 |     if (r)
 701 |     {
 702 |         printf("Error in initializing libusb library...\n");
 703 |         return -2;
 704 |     }
 705 | 
 706 |     int numdev = libusb_get_device_list(NULL, &libusb_device_list);
 707 |     if ( numdev < 0 )
 708 |     {
 709 |         printf("Library: Error in enumerating devices...\n");
 710 |         return -4;
 711 |     }
 712 | 
 713 |     number_of_cameras = 0;
 714 | 
 715 |     for (int i = 0; i < numdev; ++i )
 716 |     {
 717 |         cyusb_device *tdev = libusb_device_list[i];
 718 |         if ( device_is_of_interest(tdev) )
 719 |         {
 720 |             cydev[number_of_cameras].dev = tdev;
 721 |             r = libusb_open(tdev, &cydev[number_of_cameras].handle);
 722 |             if ( r )
 723 |             {
 724 |                 printf("Error in opening device, r=%d\n",r);
 725 |                 return -5;
 726 |             }
 727 |             ++number_of_cameras;
 728 |         }
 729 |         if(number_of_cameras>=MAXDEVICES)
 730 |             break;
 731 |     }
 732 |     return number_of_cameras;
 733 | }
 734 | 
 735 | cyusb_handle * cyusb_gethandle(int index)
 736 | {
 737 |     return cydev[index].handle;
 738 | }
 739 | 
 740 | int cyusb_control_transfer(cyusb_handle *h, unsigned char bmRequestType, unsigned char bRequest,
 741 |                            unsigned short wValue, unsigned short wIndex, unsigned char *data, unsigned short wLength,
 742 |                            unsigned int timeout)
 743 | {
 744 |     return ( libusb_control_transfer(h, bmRequestType, bRequest, wValue, wIndex, data, wLength, timeout) );
 745 | }
 746 | 
 747 | int cyusb_bulk_transfer(cyusb_handle *h, unsigned char endpoint, unsigned char *data, int length,
 748 |                         int *transferred, int timeout)
 749 | {
 750 |     return ( libusb_bulk_transfer(h, endpoint, data, length, transferred, timeout) );
 751 | }
 752 | 
 753 | void cyusb_close(void)
 754 | {
 755 |     for (int i = 0; i < number_of_cameras; ++i )
 756 |     {
 757 |         libusb_close(cydev[i].handle);
 758 |     }
 759 |     libusb_free_device_list(libusb_device_list, 1);
 760 |     libusb_exit(NULL);
 761 | }
 762 | 
 763 | /*****************************************************************************/
 764 | 
 765 | int fps_control()
 766 | {
 767 |     switch(gFPS)
 768 |     {
 769 |     case 27:
 770 |         return CAPTURE_27;
 771 |         break;
 772 |     case 54:
 773 |         return CAPTURE_54;
 774 |         break;
 775 |     case 108:
 776 |         return CAPTURE_108;
 777 |         break;
 778 |     default:
 779 |         return CAPTURE_27;
 780 |         break;
 781 |     }
 782 |     return CAPTURE_27;
 783 | }
 784 | 
 785 | cyusb_handle* get_cam_no_handle(int cam_no)
 786 | {
 787 |     if(cam_no==1)
 788 |         return pcam1_handle;
 789 |     else if(cam_no==2)
 790 |         return pcam2_handle;
 791 |     else
 792 |     {
 793 |         printf("Wrong cam number!\r\n");
 794 |         return NULL;
 795 |     }
 796 | }
 797 | 
 798 | int control_camera(int cam_no, unsigned char instruction)
 799 | {
 800 |     unsigned char buf[1];
 801 |     int r = cyusb_control_transfer(get_cam_no_handle(cam_no),0xC0,instruction,0,0,buf,0,1000);
 802 |     if(r != 0)
 803 |     {
 804 |         //printf("cam%d,i:0x%02X,r:%d\r\n",cam_no,instruction,r);
 805 |     }
 806 |     return r;
 807 | }
 808 | 
 809 | int camera_i2c_read(int cam_no, unsigned char reg,int* value)
 810 | {
 811 |     unsigned char buf[3];
 812 |     int r = cyusb_control_transfer(get_cam_no_handle(cam_no),0xC0,CAM_I2C_R,(CAM_I2C_ADDR<<8)+reg,0,buf,3,1000);
 813 |     if(r != 3)
 814 |     {
 815 |         //printf("cam%d,i:0x%02X,r:%d\r\n",cam_no,CAM_I2C_R,r);
 816 |         return r;
 817 |     }
 818 |     if(buf[0]!=0)
 819 |     {
 820 |         //printf("I2C Read failed...Cam:%d, Addr:0x%02X, Reg:0x%02X, I2C_return:%d\r\n",cam_no,CAM_I2C_ADDR,reg,buf[0]);
 821 |         return r;
 822 |     }
 823 |     *value = buf[1]+buf[2]<<8;
 824 |     //printf("I2C Read Success...Cam:%d, Addr:0x%02X, Reg:0x%02X, Result:0x%04X\r\n",cam_no,CAM_I2C_ADDR,reg,*value);
 825 |     return r;
 826 | }
 827 | 
 828 | int camera_i2c_write(int cam_no, unsigned char reg,int value)
 829 | {
 830 |     unsigned char buf[1];
 831 |     int r = cyusb_control_transfer(get_cam_no_handle(cam_no),0xC0,CAM_I2C_W,(CAM_I2C_ADDR<<8)+reg,value,buf,1,1000);
 832 |     if(r != 1)
 833 |     {
 834 |         //printf("cam%d,i:0x%02X,r:%d\r\n",cam_no,CAM_I2C_W,r);
 835 |         return r;
 836 |     }
 837 |     if(buf[0]!=0)
 838 |     {
 839 |         //printf("I2C Write failed...Cam:%d, Addr:0x%02X, Reg:0x%02X, Write:0x%04X, I2C_return:%d\r\n",cam_no,CAM_I2C_ADDR,reg,value,buf[0]);
 840 |         return r;
 841 |     }
 842 |     //printf("I2C Write Success...Cam:%d, Addr:0x%02X, Reg:0x%02X, Write:0x%04X\r\n",cam_no,CAM_I2C_ADDR,reg,value);
 843 |     return r;
 844 | }
 845 | 
 846 | int check_img(int cam_no,unsigned char *pImg,int *pImgPass)
 847 | {
 848 |     if(!visensor_resolution_status)*pImgPass = (pImg[IMG_SIZE_VGA+0]==0xFF&&pImg[IMG_SIZE_VGA+1]==0x00&&pImg[IMG_SIZE_VGA+2]==0xFE&&pImg[IMG_SIZE_VGA+3]==0x01);
 849 |     else *pImgPass = (pImg[IMG_SIZE_WVGA+0]==0xFF&&pImg[IMG_SIZE_WVGA+1]==0x00&&pImg[IMG_SIZE_WVGA+2]==0xFE&&pImg[IMG_SIZE_WVGA+3]==0x01);
 850 |     if(*pImgPass == 0)
 851 |     {
 852 |         control_camera(cam_no,fps_control());
 853 |         //printf("Img%d check error!\r\n",cam_no);
 854 |     }
 855 | }
 856 | 
 857 | float visensor_get_hardware_fps()
 858 | {
 859 |     if(gFPS==54)
 860 |     {
 861 |         if(!visensor_resolution_status)return 45700.0f/(current_HB+640.0f);
 862 |         else return 45700.0f/(current_HB+752.0f);
 863 |     }
 864 |     else if(gFPS==27)
 865 |     {
 866 |         if(!visensor_resolution_status)return 0.5f*45700.0f/(current_HB+640.0f);
 867 |         else return 0.5f*45700.0f/(current_HB+752.0f);
 868 |     }
 869 | }
 870 | static void set_camreg_default(int cam_no)
 871 | {
 872 |     camera_i2c_write(cam_no,0x00,0x1324);
 873 |     camera_i2c_write(cam_no,0x01,0x0001);
 874 |     camera_i2c_write(cam_no,0x02,0x0004);
 875 |     camera_i2c_write(cam_no,0x03,0x01E0);
 876 | 
 877 |     camera_i2c_write(cam_no,0x04,0x02F0);
 878 |     camera_i2c_write(cam_no,0x05,0x005E);
 879 |     camera_i2c_write(cam_no,0x06,0x002D);
 880 |     camera_i2c_write(cam_no,0x07,0x0388);
 881 | 
 882 |     camera_i2c_write(cam_no,0x0B,0x01E0);
 883 |     camera_i2c_write(cam_no,0x35,0x0010);
 884 |     camera_i2c_write(cam_no,0xA5,0x003A);
 885 |     camera_i2c_write(cam_no,0xA6,0x0002);
 886 | 
 887 |     camera_i2c_write(cam_no,0xA8,0x0000);
 888 |     camera_i2c_write(cam_no,0xA9,0x0002);
 889 |     camera_i2c_write(cam_no,0xAA,0x0002);
 890 |     camera_i2c_write(cam_no,0xAB,0x0040);
 891 | 
 892 |     camera_i2c_write(cam_no,0xAC,0x0001);
 893 |     camera_i2c_write(cam_no,0xAD,0x01E0);
 894 |     camera_i2c_write(cam_no,0xAE,0x0014);
 895 |     camera_i2c_write(cam_no,0xAF,0x0003);
 896 | }
 897 | 
 898 | void *cam1_capture(void*)
 899 | {
 900 |     set_camreg_default(1);
 901 |     int r,transferred = 0;
 902 |     camera_i2c_write(1,0x07,0x0188);//Normal
 903 |     camera_i2c_write(1,0x06,0x002D);//VB
 904 |     // cmos设置
 905 |     camera_i2c_write(1,0x05,current_HB);//HB
 906 |     if(!visensor_resolution_status)camera_i2c_write(1,0x04,IMG_WIDTH_VGA);//HW
 907 |     else camera_i2c_write(1,0x04,IMG_WIDTH_WVGA);//HW
 908 |     switch (EG_mode) {
 909 |     case 0:
 910 |         camera_i2c_write(1,0xAF,0x00);//AEC
 911 |         camera_i2c_write(1,0x0B,man_exp);//Exposure Time
 912 |         camera_i2c_write(1,0x35,man_gain);//Gain
 913 |         break;
 914 |     case 1:
 915 |         camera_i2c_write(1,0xAF,0x03);//AEC
 916 |         camera_i2c_write(1,0xA5,auto_EG_des);//AEC
 917 |         camera_i2c_write(1,0xA6,0x01);//AEC
 918 |         camera_i2c_write(1,0xA8,0x00);//AEC
 919 |         camera_i2c_write(1,0xAC,auto_EG_bottom);//AEC
 920 |         camera_i2c_write(1,0xAD,auto_EG_top);//AEC
 921 |         camera_i2c_write(1,0xAE,2);//AEC
 922 |         break;
 923 |     case 2:
 924 |         camera_i2c_write(1,0xAF,0x01);//AEC
 925 |         camera_i2c_write(1,0xA5,auto_EG_des);//AEC
 926 |         camera_i2c_write(1,0xA6,0x01);//AEC
 927 |         camera_i2c_write(1,0xA8,0x00);//AEC
 928 |         camera_i2c_write(1,0xAC,auto_E_man_G_Ebottom);//AEC
 929 |         camera_i2c_write(1,0xAD,auto_E_man_G_Etop);//AEC
 930 |         camera_i2c_write(1,0xAE,2);//AEC
 931 |         camera_i2c_write(1,0x35,auto_E_man_G);//Gain
 932 |         break;
 933 |     case 3:
 934 |         break;
 935 |     case 4:
 936 |         camera_i2c_write(1,0xA6,agc_aec_skip_frame);
 937 |         camera_i2c_write(1,0xA8,2);
 938 |         camera_i2c_write(1,0xA9,agc_aec_skip_frame);
 939 |         camera_i2c_write(1,0xAA,2);
 940 |         break;
 941 |     default:
 942 |         break;
 943 |     }
 944 | 
 945 |     control_camera(1,fps_control());
 946 |     // usleep(100);
 947 | 
 948 |     static int sync_ct=0;
 949 | 
 950 |     struct timeval endTime;
 951 |     float fTimeuse=0;
 952 | 
 953 |     int ctt=0;
 954 | 
 955 |     float time_offset=1.0f/visensor_get_hardware_fps();
 956 |     control_camera(1,STANDBY_SHORT);
 957 |     while(!shut_down_tag)
 958 |     {
 959 |         ctt++;
 960 |         if(ctt>1)	//该数值越大，则帧同步的周期越长，同步频率越小
 961 |         {
 962 |             control_camera(1,STANDBY_SHORT);
 963 |             ctt = 0;
 964 |         }
 965 |         for(gFrameCam1=0; !shut_down_tag&&gFrameCam1<FRAME_CLUST; gFrameCam1++)
 966 |         {
 967 |             if(!visensor_resolution_status)
 968 |             {
 969 |                 r = cyusb_bulk_transfer(pcam1_handle, 0x82, gImg1_VGA[gFrameCam1], IMG_BUF_SIZE_VGA, &transferred, 1000);
 970 | 
 971 |                 // time-stamp
 972 |                 gettimeofday(&endTime,NULL);
 973 |                 gettimeofday(&gImg1_SysTime[gFrameCam1],NULL);
 974 |                 gImg1_SysTime[gFrameCam1].tv_usec-=1000000*time_offset;
 975 |                 if(gImg1_SysTime[gFrameCam1].tv_usec<0)
 976 |                 {
 977 |                     gImg1_SysTime[gFrameCam1].tv_usec+=1000000;
 978 |                     gImg1_SysTime[gFrameCam1].tv_sec-=1;
 979 |                 }
 980 |                 fTimeuse = (endTime.tv_sec - visensor_startTime.tv_sec) + 0.000001*(endTime.tv_usec - visensor_startTime.tv_usec);
 981 |                 fTimeuse-=time_offset;	// 真正的图像拍摄时间
 982 |                 gImg1Time[gFrameCam1]=fTimeuse;
 983 | 
 984 |                 if(r)printf("cam1 bulk transfer returned: %d\r\n",r);
 985 |                 check_img(1,gImg1_VGA[gFrameCam1],&gImg1Pass[gFrameCam1]);
 986 |                 if(gImg1Pass[gFrameCam1])left_fresh=true;
 987 |             }
 988 |             else
 989 |             {
 990 |                 r = cyusb_bulk_transfer(pcam1_handle, 0x82, gImg1_WVGA[gFrameCam1], IMG_BUF_SIZE_WVGA, &transferred, 1000);
 991 | 
 992 |                 // time-stamp
 993 |                 gettimeofday(&endTime,NULL);
 994 |                 gettimeofday(&gImg1_SysTime[gFrameCam1],NULL);
 995 |                 gImg1_SysTime[gFrameCam1].tv_usec-=1000000*time_offset;
 996 |                 if(gImg1_SysTime[gFrameCam1].tv_usec<0)
 997 |                 {
 998 |                     gImg1_SysTime[gFrameCam1].tv_usec+=1000000;
 999 |                     gImg1_SysTime[gFrameCam1].tv_sec-=1;
1000 |                 }
1001 |                 fTimeuse = (endTime.tv_sec - visensor_startTime.tv_sec) + 0.000001*(endTime.tv_usec - visensor_startTime.tv_usec);
1002 |                 fTimeuse-=time_offset;	// 真正的图像拍摄时间
1003 |                 gImg1Time[gFrameCam1]=fTimeuse;
1004 | 
1005 |                 if(r)printf("cam1 bulk transfer returned: %d\r\n",r);
1006 |                 check_img(1,gImg1_WVGA[gFrameCam1],&gImg1Pass[gFrameCam1]);
1007 |                 if(gImg1Pass[gFrameCam1])left_fresh=true;
1008 |             }
1009 |         }
1010 | 
1011 |         gettimeofday(&endTime,NULL);
1012 |         fTimeuse = (endTime.tv_sec - visensor_startTime.tv_sec) + 0.000001*(endTime.tv_usec - visensor_startTime.tv_usec);
1013 |         //printf("Cam1 frame rate: %5.1f FPS\r\n",FRAME_CLUST/fTimeuse);
1014 |     }
1015 |     pthread_exit(NULL);
1016 | }
1017 | 
1018 | void *cam2_capture(void*)
1019 | {
1020 |     set_camreg_default(2);
1021 |     int r,transferred = 0;
1022 |     camera_i2c_write(2,0x07,0x0188);//Normal
1023 |     camera_i2c_write(2,0x06,0x002D);//VB
1024 |     // cmos设置
1025 |     camera_i2c_write(2,0x05,current_HB);//HB
1026 |     if(!visensor_resolution_status)camera_i2c_write(2,0x04,IMG_WIDTH_VGA);//HW
1027 |     else camera_i2c_write(2,0x04,IMG_WIDTH_WVGA);//HW
1028 |     switch (EG_mode) {
1029 |     case 0:
1030 |         camera_i2c_write(2,0xAF,0x00);//AEC
1031 |         camera_i2c_write(2,0x0B,man_exp);//Exposure Time
1032 |         camera_i2c_write(2,0x35,man_gain);//Gain
1033 |         break;
1034 |     case 1:
1035 |         camera_i2c_write(2,0xAF,0x03);//AEC
1036 |         camera_i2c_write(2,0xA5,auto_EG_des);//AEC
1037 |         camera_i2c_write(2,0xA6,0x01);//AEC
1038 |         camera_i2c_write(2,0xA8,0x00);//AEC
1039 |         camera_i2c_write(2,0xAC,auto_EG_bottom);//AEC
1040 |         camera_i2c_write(2,0xAD,auto_EG_top);//AEC
1041 |         camera_i2c_write(2,0xAE,2);//AEC
1042 |         break;
1043 |     case 2:
1044 |         camera_i2c_write(2,0xAF,0x01);//AEC
1045 |         camera_i2c_write(2,0xA5,auto_EG_des);//AEC
1046 |         camera_i2c_write(2,0xA6,0x01);//AEC
1047 |         camera_i2c_write(2,0xA8,0x00);//AEC
1048 |         camera_i2c_write(2,0xAC,auto_E_man_G_Ebottom);//AEC
1049 |         camera_i2c_write(2,0xAD,auto_E_man_G_Etop);//AEC
1050 |         camera_i2c_write(2,0xAE,2);//AEC
1051 |         camera_i2c_write(2,0x35,auto_E_man_G);//Gain
1052 |         break;
1053 |     case 3:
1054 |         break;
1055 |     case 4:
1056 |         camera_i2c_write(2,0xA6,agc_aec_skip_frame);
1057 |         camera_i2c_write(2,0xA8,2);
1058 |         camera_i2c_write(2,0xA9,agc_aec_skip_frame);
1059 |         camera_i2c_write(2,0xAA,2);
1060 |         break;
1061 |     default:
1062 |         break;
1063 |     }
1064 | 
1065 |     control_camera(2,fps_control());
1066 |     // usleep(100);
1067 |     struct timeval endTime;
1068 |     float fTimeuse;
1069 | 
1070 |     float time_offset=1.0f/visensor_get_hardware_fps();
1071 | 
1072 |     while(!shut_down_tag)
1073 |     {
1074 |         for(gFrameCam2=0; !shut_down_tag&&gFrameCam2<FRAME_CLUST; gFrameCam2++)
1075 |         {
1076 |             if(!visensor_resolution_status)
1077 |             {
1078 |                 r = cyusb_bulk_transfer(pcam2_handle, 0x82, gImg2_VGA[gFrameCam2], IMG_BUF_SIZE_VGA, &transferred, 1000);
1079 | 
1080 |                 // time-stamp
1081 |                 gettimeofday(&endTime,NULL);
1082 |                 gettimeofday(&gImg2_SysTime[gFrameCam2],NULL);
1083 |                 gImg2_SysTime[gFrameCam2].tv_usec-=1000000*time_offset;
1084 |                 if(gImg2_SysTime[gFrameCam2].tv_usec<0)
1085 |                 {
1086 |                     gImg2_SysTime[gFrameCam2].tv_usec+=1000000;
1087 |                     gImg2_SysTime[gFrameCam2].tv_sec-=1;
1088 |                 }
1089 |                 fTimeuse = (endTime.tv_sec - visensor_startTime.tv_sec) + 0.000001*(endTime.tv_usec - visensor_startTime.tv_usec);
1090 |                 fTimeuse-=time_offset;	// 真正的图像拍摄时间
1091 |                 gImg2Time[gFrameCam2]=fTimeuse;
1092 | 
1093 |                 if(r)printf("cam2 bulk transfer returned: %d\r\n",r);
1094 |                 check_img(2,gImg2_VGA[gFrameCam2],&gImg2Pass[gFrameCam2]);
1095 |                 if(gImg2Pass[gFrameCam2])right_fresh=true;
1096 |             }
1097 |             else
1098 |             {
1099 |                 r = cyusb_bulk_transfer(pcam2_handle, 0x82, gImg2_WVGA[gFrameCam2], IMG_BUF_SIZE_WVGA, &transferred, 1000);
1100 | 
1101 |                 // time-stamp
1102 |                 gettimeofday(&endTime,NULL);
1103 |                 gettimeofday(&gImg2_SysTime[gFrameCam2],NULL);
1104 |                 gImg2_SysTime[gFrameCam2].tv_usec-=1000000*time_offset;
1105 |                 if(gImg2_SysTime[gFrameCam2].tv_usec<0)
1106 |                 {
1107 |                     gImg2_SysTime[gFrameCam2].tv_usec+=1000000;
1108 |                     gImg2_SysTime[gFrameCam2].tv_sec-=1;
1109 |                 }
1110 |                 fTimeuse = (endTime.tv_sec - visensor_startTime.tv_sec) + 0.000001*(endTime.tv_usec - visensor_startTime.tv_usec);
1111 |                 fTimeuse-=time_offset;	// 真正的图像拍摄时间
1112 |                 gImg2Time[gFrameCam2]=fTimeuse;
1113 | 
1114 |                 if(r)printf("cam2 bulk transfer returned: %d\r\n",r);
1115 |                 check_img(2,gImg2_WVGA[gFrameCam2],&gImg2Pass[gFrameCam2]);
1116 |                 if(gImg2Pass[gFrameCam2])right_fresh=true;
1117 |             }
1118 |         }
1119 |         gettimeofday(&endTime,NULL);
1120 |         fTimeuse = (endTime.tv_sec - visensor_startTime.tv_sec) + 0.000001*(endTime.tv_usec - visensor_startTime.tv_usec);
1121 |         //printf("Cam2 frame rate: %5.1f FPS\r\n",FRAME_CLUST/fTimeuse);
1122 |     }
1123 |     pthread_exit(NULL);
1124 | }
1125 | 
1126 | 
1127 | bool visensor_is_stereo_good()
1128 | {
1129 |     int img1_pointer = (gFrameCam1 + FRAME_CLUST - 1) % FRAME_CLUST;
1130 |     int img2_pointer = (gFrameCam2 + FRAME_CLUST - 1) % FRAME_CLUST;
1131 |     if(gImg2Pass[img2_pointer]&&gImg1Pass[img1_pointer])return true;
1132 |     else return false;
1133 | }
1134 | bool visensor_is_left_good()
1135 | {
1136 |     int img1_pointer = (gFrameCam1 + FRAME_CLUST - 1) % FRAME_CLUST;
1137 |     if(gImg1Pass[img1_pointer])return true;
1138 |     else return false;
1139 | }
1140 | bool visensor_is_right_good()
1141 | {
1142 |     int img2_pointer = (gFrameCam2 + FRAME_CLUST - 1) % FRAME_CLUST;
1143 |     if(gImg2Pass[img2_pointer])return true;
1144 |     else return false;
1145 | }
1146 | 
1147 | bool visensor_is_left_fresh()
1148 | {
1149 |     if(left_fresh)
1150 |     {
1151 |         left_fresh=false;
1152 |         return true;
1153 |     }
1154 |     else return false;
1155 | }
1156 | bool visensor_is_right_fresh()
1157 | {
1158 |     if(right_fresh)
1159 |     {
1160 |         right_fresh=false;
1161 |         return true;
1162 |     }
1163 |     else return false;
1164 | }
1165 | 
1166 | visensor_imudata visensor_get_stereoImg(char* left_img,char* right_img)
1167 | {
1168 | 
1169 |     while(!visensor_is_stereo_good())usleep(200);
1170 | 
1171 |     int img1_pointer = (gFrameCam1 + FRAME_CLUST - 1) % FRAME_CLUST;
1172 |     int img2_pointer = (gFrameCam2 + FRAME_CLUST - 1) % FRAME_CLUST;
1173 | 
1174 |     if(gImg2Pass[img2_pointer]&&gImg1Pass[img1_pointer]&&(!visensor_resolution_status))
1175 |     {
1176 |         memcpy(left_img,(char *)(gImg1_VGA[img1_pointer]),IMG_BUF_SIZE_VGA);
1177 |         memcpy(right_img,(char *)(gImg2_VGA[img2_pointer]),IMG_BUF_SIZE_VGA);
1178 |         gImg2Pass[img2_pointer]=0;
1179 |         gImg1Pass[img1_pointer]=0;
1180 | 
1181 |         // 绑定最近时间戳的IMU数据
1182 |         timeval left_stamp=gImg1_SysTime[img1_pointer];
1183 |         int min_id=0;
1184 |         int min_dist=10000;
1185 |         for(int i=0; i<200; i++)
1186 |         {
1187 |             int imu_usec=IMU_FIFO[i].system_time.tv_usec;
1188 |             if(IMU_FIFO[i].imu_time!=0&&min_dist>=abs(imu_usec-left_stamp.tv_usec))
1189 |             {
1190 |                 min_dist=abs(imu_usec-left_stamp.tv_usec);
1191 |                 min_id=i;
1192 |             }
1193 |         }
1194 |         visensor_imudata bind_imuData=IMU_FIFO[min_id];
1195 |         return bind_imuData;
1196 | 
1197 |     }
1198 |     else if(gImg2Pass[img2_pointer]&&gImg1Pass[img1_pointer]&&(visensor_resolution_status))
1199 |     {
1200 |         memcpy(left_img,(char *)(gImg1_WVGA[img1_pointer]),IMG_BUF_SIZE_WVGA);
1201 |         memcpy(right_img,(char *)(gImg2_WVGA[img2_pointer]),IMG_BUF_SIZE_WVGA);
1202 |         gImg2Pass[img2_pointer]=0;
1203 |         gImg1Pass[img1_pointer]=0;
1204 | 
1205 |         // 绑定最近时间戳的IMU数据
1206 |         timeval left_stamp=gImg1_SysTime[img1_pointer];
1207 |         int min_id=0;
1208 |         int min_dist=10000;
1209 |         for(int i=0; i<200; i++)
1210 |         {
1211 |             int imu_usec=IMU_FIFO[i].system_time.tv_usec;
1212 |             if(IMU_FIFO[i].imu_time!=0&&min_dist>=abs(imu_usec-left_stamp.tv_usec))
1213 |             {
1214 |                 min_dist=abs(imu_usec-left_stamp.tv_usec);
1215 |                 min_id=i;
1216 |             }
1217 |         }
1218 |         visensor_imudata bind_imuData=IMU_FIFO[min_id];
1219 |         return bind_imuData;
1220 |     }
1221 | 
1222 | 
1223 | }
1224 | visensor_imudata visensor_get_stereoImg(char* left_img,char* right_img,timeval &left_stamp,timeval &right_stamp)
1225 | {
1226 | 
1227 |     while(!visensor_is_stereo_good())usleep(200);
1228 | 
1229 |     int img1_pointer = (gFrameCam1 + FRAME_CLUST - 1) % FRAME_CLUST;
1230 |     int img2_pointer = (gFrameCam2 + FRAME_CLUST - 1) % FRAME_CLUST;
1231 |     if(gImg2Pass[img2_pointer]&&gImg1Pass[img1_pointer]&&(!visensor_resolution_status))
1232 |     {
1233 |         memcpy(left_img,(char *)(gImg1_VGA[img1_pointer]),IMG_BUF_SIZE_VGA);
1234 |         memcpy(right_img,(char *)(gImg2_VGA[img2_pointer]),IMG_BUF_SIZE_VGA);
1235 |         left_stamp=gImg1_SysTime[img1_pointer];
1236 |         right_stamp=gImg2_SysTime[img2_pointer];
1237 |         gImg2Pass[img2_pointer]=0;
1238 |         gImg1Pass[img1_pointer]=0;
1239 | 
1240 |         // 绑定最近时间戳的IMU数据
1241 |         int min_id=0;
1242 |         int min_dist=10000;
1243 |         for(int i=0; i<200; i++)
1244 |         {
1245 |             int imu_usec=IMU_FIFO[i].system_time.tv_usec;
1246 |             if(IMU_FIFO[i].imu_time!=0&&min_dist>=abs(imu_usec-left_stamp.tv_usec))
1247 |             {
1248 |                 min_dist=abs(imu_usec-left_stamp.tv_usec);
1249 |                 min_id=i;
1250 |             }
1251 |         }
1252 |         visensor_imudata bind_imuData=IMU_FIFO[min_id];
1253 |         return bind_imuData;
1254 |     }
1255 |     else if(gImg2Pass[img2_pointer]&&gImg1Pass[img1_pointer]&&(visensor_resolution_status))
1256 |     {
1257 |         memcpy(left_img,(char *)(gImg1_WVGA[img1_pointer]),IMG_BUF_SIZE_WVGA);
1258 |         memcpy(right_img,(char *)(gImg2_WVGA[img2_pointer]),IMG_BUF_SIZE_WVGA);
1259 |         left_stamp=gImg1_SysTime[img1_pointer];
1260 |         right_stamp=gImg2_SysTime[img2_pointer];
1261 |         gImg2Pass[img2_pointer]=0;
1262 |         gImg1Pass[img1_pointer]=0;
1263 | 
1264 |         // 绑定最近时间戳的IMU数据
1265 |         int min_id=0;
1266 |         int min_dist=10000;
1267 |         for(int i=0; i<200; i++)
1268 |         {
1269 |             int imu_usec=IMU_FIFO[i].system_time.tv_usec;
1270 |             if(IMU_FIFO[i].imu_time!=0&&min_dist>=abs(imu_usec-left_stamp.tv_usec))
1271 |             {
1272 |                 min_dist=abs(imu_usec-left_stamp.tv_usec);
1273 |                 min_id=i;
1274 |             }
1275 |         }
1276 |         visensor_imudata bind_imuData=IMU_FIFO[min_id];
1277 |         return bind_imuData;
1278 |     }
1279 | }
1280 | 
1281 | visensor_imudata visensor_get_leftImg(char* left_img)
1282 | {
1283 | 
1284 |     while(!visensor_is_left_good())usleep(200);
1285 | 
1286 |     int img1_pointer = (gFrameCam1 + FRAME_CLUST - 1) % FRAME_CLUST;
1287 |     if(gImg1Pass[img1_pointer]&&(!visensor_resolution_status))
1288 |     {
1289 |         memcpy(left_img,(char *)(gImg1_VGA[img1_pointer]),IMG_BUF_SIZE_VGA);
1290 |         gImg1Pass[img1_pointer]=0;
1291 | 
1292 |         // 绑定最近时间戳的IMU数据
1293 |         timeval left_stamp=gImg1_SysTime[img1_pointer];
1294 |         int min_id=0;
1295 |         int min_dist=10000;
1296 |         for(int i=0; i<200; i++)
1297 |         {
1298 |             int imu_usec=IMU_FIFO[i].system_time.tv_usec;
1299 |             if(IMU_FIFO[i].imu_time!=0&&min_dist>=abs(imu_usec-left_stamp.tv_usec))
1300 |             {
1301 |                 min_dist=abs(imu_usec-left_stamp.tv_usec);
1302 |                 min_id=i;
1303 |             }
1304 |         }
1305 |         visensor_imudata bind_imuData=IMU_FIFO[min_id];
1306 |         return bind_imuData;
1307 |     }
1308 |     else if(gImg1Pass[img1_pointer]&&(visensor_resolution_status))
1309 |     {
1310 |         memcpy(left_img,(char *)(gImg1_WVGA[img1_pointer]),IMG_BUF_SIZE_WVGA);
1311 |         gImg1Pass[img1_pointer]=0;
1312 | 
1313 |         // 绑定最近时间戳的IMU数据
1314 |         timeval left_stamp=gImg1_SysTime[img1_pointer];
1315 |         int min_id=0;
1316 |         int min_dist=10000;
1317 |         for(int i=0; i<200; i++)
1318 |         {
1319 |             int imu_usec=IMU_FIFO[i].system_time.tv_usec;
1320 |             if(IMU_FIFO[i].imu_time!=0&&min_dist>=abs(imu_usec-left_stamp.tv_usec))
1321 |             {
1322 |                 min_dist=abs(imu_usec-left_stamp.tv_usec);
1323 |                 min_id=i;
1324 |             }
1325 |         }
1326 |         visensor_imudata bind_imuData=IMU_FIFO[min_id];
1327 |         return bind_imuData;
1328 |     }
1329 | }
1330 | 
1331 | visensor_imudata visensor_get_leftImg(char* left_img,timeval &left_stamp)
1332 | {
1333 | 
1334 |     while(!visensor_is_left_good())usleep(200);
1335 | 
1336 |     int img1_pointer = (gFrameCam1 + FRAME_CLUST - 1) % FRAME_CLUST;
1337 |     if(gImg1Pass[img1_pointer]&&(!visensor_resolution_status))
1338 |     {
1339 |         memcpy(left_img,(char *)(gImg1_VGA[img1_pointer]),IMG_BUF_SIZE_VGA);
1340 |         left_stamp=gImg1_SysTime[img1_pointer];
1341 |         gImg1Pass[img1_pointer]=0;
1342 | 
1343 |         // 绑定最近时间戳的IMU数据
1344 |         int min_id=0;
1345 |         int min_dist=10000;
1346 |         for(int i=0; i<200; i++)
1347 |         {
1348 |             int imu_usec=IMU_FIFO[i].system_time.tv_usec;
1349 |             if(IMU_FIFO[i].imu_time!=0&&min_dist>=abs(imu_usec-left_stamp.tv_usec))
1350 |             {
1351 |                 min_dist=abs(imu_usec-left_stamp.tv_usec);
1352 |                 min_id=i;
1353 |             }
1354 |         }
1355 |         visensor_imudata bind_imuData=IMU_FIFO[min_id];
1356 |         return bind_imuData;
1357 |     }
1358 |     else if(gImg1Pass[img1_pointer]&&(visensor_resolution_status))
1359 |     {
1360 |         memcpy(left_img,(char *)(gImg1_WVGA[img1_pointer]),IMG_BUF_SIZE_WVGA);
1361 |         left_stamp=gImg1_SysTime[img1_pointer];
1362 |         gImg1Pass[img1_pointer]=0;
1363 | 
1364 |         // 绑定最近时间戳的IMU数据
1365 |         int min_id=0;
1366 |         int min_dist=10000;
1367 |         for(int i=0; i<200; i++)
1368 |         {
1369 |             int imu_usec=IMU_FIFO[i].system_time.tv_usec;
1370 |             if(IMU_FIFO[i].imu_time!=0&&min_dist>=abs(imu_usec-left_stamp.tv_usec))
1371 |             {
1372 |                 min_dist=abs(imu_usec-left_stamp.tv_usec);
1373 |                 min_id=i;
1374 |             }
1375 |         }
1376 |         visensor_imudata bind_imuData=IMU_FIFO[min_id];
1377 |         return bind_imuData;
1378 |     }
1379 | }
1380 | 
1381 | visensor_imudata visensor_get_rightImg(char* right_img)
1382 | {
1383 | 
1384 |     while(!visensor_is_right_good())usleep(200);
1385 | 
1386 |     int img2_pointer = (gFrameCam2 + FRAME_CLUST - 1) % FRAME_CLUST;
1387 | 
1388 |     if(gImg2Pass[img2_pointer]&&(!visensor_resolution_status))
1389 |     {
1390 |         memcpy(right_img,(char *)(gImg2_VGA[img2_pointer]),IMG_BUF_SIZE_VGA);
1391 |         gImg2Pass[img2_pointer]=0;
1392 | 
1393 |         // 绑定最近时间戳的IMU数据
1394 |         timeval right_stamp=gImg2_SysTime[img2_pointer];
1395 |         int min_id=0;
1396 |         int min_dist=10000;
1397 |         for(int i=0; i<200; i++)
1398 |         {
1399 |             int imu_usec=IMU_FIFO[i].system_time.tv_usec;
1400 |             if(IMU_FIFO[i].imu_time!=0&&min_dist>=abs(imu_usec-right_stamp.tv_usec))
1401 |             {
1402 |                 min_dist=abs(imu_usec-right_stamp.tv_usec);
1403 |                 min_id=i;
1404 |             }
1405 |         }
1406 |         visensor_imudata bind_imuData=IMU_FIFO[min_id];
1407 |         return bind_imuData;
1408 |     }
1409 |     else if(gImg2Pass[img2_pointer]&&(visensor_resolution_status))
1410 |     {
1411 |         memcpy(right_img,(char *)(gImg2_WVGA[img2_pointer]),IMG_BUF_SIZE_WVGA);
1412 |         gImg2Pass[img2_pointer]=0;
1413 | 
1414 |         // 绑定最近时间戳的IMU数据
1415 |         timeval right_stamp=gImg2_SysTime[img2_pointer];
1416 |         int min_id=0;
1417 |         int min_dist=10000;
1418 |         for(int i=0; i<200; i++)
1419 |         {
1420 |             int imu_usec=IMU_FIFO[i].system_time.tv_usec;
1421 |             if(IMU_FIFO[i].imu_time!=0&&min_dist>=abs(imu_usec-right_stamp.tv_usec))
1422 |             {
1423 |                 min_dist=abs(imu_usec-right_stamp.tv_usec);
1424 |                 min_id=i;
1425 |             }
1426 |         }
1427 |         visensor_imudata bind_imuData=IMU_FIFO[min_id];
1428 |         return bind_imuData;
1429 |     }
1430 | }
1431 | visensor_imudata visensor_get_rightImg(char* right_img,timeval &right_stamp)
1432 | {
1433 | 
1434 |     while(!visensor_is_right_good())usleep(200);
1435 | 
1436 |     int img2_pointer = (gFrameCam2 + FRAME_CLUST - 1) % FRAME_CLUST;
1437 | 
1438 |     if(gImg2Pass[img2_pointer]&&(!visensor_resolution_status))
1439 |     {
1440 |         memcpy(right_img,(char *)(gImg2_VGA[img2_pointer]),IMG_BUF_SIZE_VGA);
1441 |         right_stamp=gImg2_SysTime[img2_pointer];
1442 |         gImg2Pass[img2_pointer]=0;
1443 | 
1444 |         // 绑定最近时间戳的IMU数据
1445 |         int min_id=0;
1446 |         int min_dist=10000;
1447 |         for(int i=0; i<200; i++)
1448 |         {
1449 |             int imu_usec=IMU_FIFO[i].system_time.tv_usec;
1450 |             if(IMU_FIFO[i].imu_time!=0&&min_dist>=abs(imu_usec-right_stamp.tv_usec))
1451 |             {
1452 |                 min_dist=abs(imu_usec-right_stamp.tv_usec);
1453 |                 min_id=i;
1454 |             }
1455 |         }
1456 |         visensor_imudata bind_imuData=IMU_FIFO[min_id];
1457 |         return bind_imuData;
1458 |     }
1459 |     else if(gImg2Pass[img2_pointer]&&(visensor_resolution_status))
1460 |     {
1461 |         memcpy(right_img,(char *)(gImg2_WVGA[img2_pointer]),IMG_BUF_SIZE_WVGA);
1462 |         right_stamp=gImg2_SysTime[img2_pointer];
1463 |         gImg2Pass[img2_pointer]=0;
1464 | 
1465 |         // 绑定最近时间戳的IMU数据
1466 |         int min_id=0;
1467 |         int min_dist=10000;
1468 |         for(int i=0; i<200; i++)
1469 |         {
1470 |             int imu_usec=IMU_FIFO[i].system_time.tv_usec;
1471 |             if(IMU_FIFO[i].imu_time!=0&&min_dist>=abs(imu_usec-right_stamp.tv_usec))
1472 |             {
1473 |                 min_dist=abs(imu_usec-right_stamp.tv_usec);
1474 |                 min_id=i;
1475 |             }
1476 |         }
1477 |         visensor_imudata bind_imuData=IMU_FIFO[min_id];
1478 |         return bind_imuData;
1479 |     }
1480 | }
1481 | 
1482 | int visensor_Start_Cameras()
1483 | {
1484 |     gSelectCam = 3;
1485 |     allow_settings_change=false;
1486 | 
1487 |     shut_down_tag=false;
1488 | 
1489 |     int ret_val=0;
1490 | 
1491 |     DEBUGLOG("Now opening cameras...\r\n");
1492 |     //Open usb devices
1493 |     int r;
1494 |     r = cyusb_open();
1495 |     if(r<1)
1496 |     {
1497 |         LOG("Error: No cameras found! r = %d\r\n",r);
1498 |         return -2;
1499 |     }
1500 |     else
1501 |     {
1502 |         printf("Number of device of interest found: %d\r\n",r);
1503 |     }
1504 |     //Get camera position
1505 |     cyusb_handle *pusb_handle;
1506 |     for(int i=0; i<r; i++)
1507 |     {
1508 |         unsigned char buf = 0;
1509 |         pusb_handle = cyusb_gethandle(i);
1510 |         int r_num = cyusb_control_transfer(pusb_handle,0xC0,GET_CAM_LR,0,0,&buf,1,1000);
1511 |         if(r_num != 1)
1512 |         {
1513 |             printf("Getting LR: %d error: i:0xA5,r:%d\r\n",i,r_num);
1514 |         }
1515 |         else
1516 |         {
1517 |             if(buf==0xF0)
1518 |             {
1519 |                 pcam1_handle = pusb_handle;
1520 |                 printf("Left camera found!\r\n");
1521 |             }
1522 |             else if(buf==0xF1)
1523 |             {
1524 |                 pcam2_handle = pusb_handle;
1525 |                 printf("Right camera found!\r\n");
1526 |             }
1527 |         }
1528 |     }
1529 |     //Check cameras
1530 |     if(gSelectCam&0x01)
1531 |         if(pcam1_handle==NULL)
1532 |         {
1533 |             LOG("Error: Left camera was not found!\r\n");
1534 |             ret_val= -2;
1535 |         }
1536 |     if(gSelectCam&0x02)
1537 |         if(pcam2_handle==NULL)
1538 |         {
1539 |             LOG("Error: Right camera was not found!\r\n");
1540 |             ret_val= -2;
1541 |         }
1542 | 
1543 |     if(!(ret_val<0))
1544 |     {
1545 |         //Create capture thread
1546 |         gettimeofday(&visensor_startTime,NULL);		// 初始化时间起点
1547 | 
1548 |         int temp=0;
1549 |         if(gSelectCam&0x01)
1550 |             if(temp = pthread_create(&cam1_capture_thread, NULL, cam1_capture, NULL))
1551 |                 printf("Failed to create thread cam1_capture_thread\r\n");
1552 |         if(gSelectCam&0x02)
1553 |             if(temp = pthread_create(&cam2_capture_thread, NULL, cam2_capture, NULL))
1554 |                 printf("Failed to create thread cam2_capture_thread\r\n");
1555 |         usleep(1000);
1556 | 
1557 |         return ret_val;
1558 |     }
1559 |     else return 0;
1560 | }
1561 | void visensor_Close_Cameras()
1562 | {
1563 |     shut_down_tag=true;
1564 |     /* Waiting for Camera threads */
1565 |     if(cam1_capture_thread !=0)
1566 |     {
1567 |         pthread_join(cam1_capture_thread,NULL);
1568 |     }
1569 |     if(cam2_capture_thread !=0)
1570 |     {
1571 |         pthread_join(cam2_capture_thread,NULL);
1572 |     }
1573 |     cyusb_close();
1574 | }
1575 | 
1576 | // 检测有无新数据
1577 | bool visensor_imu_have_fresh_data()
1578 | {
1579 |     if(visensor_query_imu_update())
1580 |     {
1581 |         visensor_erase_imu_update();
1582 |         return true;
1583 |     }
1584 |     else return false;
1585 | }
1586 | 
1587 | void visensor_Close_IMU()
1588 | {
1589 |     imu_close=true;
1590 |     /* Waiting for imu_thread */
1591 |     if(imu_thread !=0)
1592 |     {
1593 |         pthread_join(imu_thread,NULL);
1594 |     }
1595 | }
1596 | 
1597 | void *imu_data_feed(void*)
1598 | {
1599 |     unsigned char imu_frame[IMU_FRAME_LEN];
1600 |     short int biasX=(short int)imu_acc_bias_X;
1601 |     short int biasY=(short int)imu_acc_bias_Y;
1602 |     short int biasZ=(short int)imu_acc_bias_Z;
1603 |     short int setaccoffset[3] = {biasX,  biasY, biasZ};
1604 | 
1605 |     // 在此函数中更新 IMU-FIFO 用于计算同步 VI-pair
1606 | 
1607 |     int counter=0;
1608 | 
1609 |     for(int i=0; i<200; i++)
1610 |     {
1611 |         IMU_FIFO[i].imu_time=0;
1612 |         IMU_FIFO[i].system_time.tv_usec=0;
1613 |         IMU_FIFO[i].system_time.tv_sec=0;
1614 |         IMU_FIFO[i].num=0;
1615 |         IMU_FIFO[i].rx=0;
1616 |         IMU_FIFO[i].ry=0;
1617 |         IMU_FIFO[i].rz=0;
1618 |         IMU_FIFO[i].ax=0;
1619 |         IMU_FIFO[i].ay=0;
1620 |         IMU_FIFO[i].az=0;
1621 |         IMU_FIFO[i].qw=0;
1622 |         IMU_FIFO[i].qx=0;
1623 |         IMU_FIFO[i].qy=0;
1624 |         IMU_FIFO[i].qz=0;
1625 |     }
1626 | 
1627 |     while(!imu_close)
1628 |     {
1629 |         if(visensor_get_imu_frame(imu_fd,imu_frame)==0)
1630 |         {
1631 |             visensor_get_imu_data(imu_frame,setaccoffset,&visensor_imudata_pack,false);
1632 |             // 将imu更新标记打开
1633 |             visensor_mark_imu_update();
1634 | 
1635 |             imu_fifo_ct = (imu_fifo_ct+1)%200;
1636 |             IMU_FIFO[imu_fifo_ct]=visensor_imudata_pack;
1637 | 
1638 |         }
1639 |         usleep(500);
1640 |     }
1641 |     pthread_exit(NULL);
1642 | }
1643 | int visensor_Start_IMU()
1644 | {
1645 | 
1646 |     // serial port
1647 |     int fd=visensor_open_port(imu_port_name.c_str());
1648 |     if(fd<0)
1649 |     {
1650 |         printf("visensor_open_port error...\r\n");
1651 |         return 0;
1652 |     }
1653 |     printf("visensor_open_port success...\r\n");
1654 | 
1655 |     if(visensor_set_opt(fd,115200,8,'N',1)<0)
1656 |     {
1657 |         printf("visensor_set_opt error...\r\n");
1658 |         return 0;
1659 |     }
1660 |     printf("visensor_set_opt(fd,115200,8,'N',1) success...\r\n");
1661 | 
1662 |     static unsigned char sendframe[10]= {0x55,0xAA,0x02};
1663 |     short int setaccoffset[3] = {(short int)imu_acc_bias_X, (short int)imu_acc_bias_Y, (short int)imu_acc_bias_Z};
1664 |     memcpy(&sendframe[3],setaccoffset,6);
1665 |     sendframe[9]=sendframe[3]+sendframe[4]+sendframe[5]+sendframe[6]+sendframe[7]+sendframe[8];
1666 |     visensor_send_imu_frame(fd,sendframe,10);
1667 | 
1668 |     //Create imu_data thread
1669 |     imu_fd=fd;
1670 | 
1671 |     int temp;
1672 |     if(temp = pthread_create(&imu_thread, NULL, imu_data_feed, NULL))
1673 |         printf("Failed to create thread imu_data\r\n");
1674 | 
1675 |     return fd;
1676 | }
1677 | 
1678 | 
1679 | 


--------------------------------------------------------------------------------