ISCommunicationsExample.c
Go to the documentation of this file.
1 /*
2 MIT LICENSE
3 
4 Copyright 2014-2018 Inertial Sense, Inc. - http://inertialsense.com
5 
6 Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files(the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions :
7 
8 The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
9 
10 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
11 */
12 
13 #include <stdio.h>
14 
15 // STEP 1: Add Includes
16 // Change these include paths to the correct paths for your project
17 #include "../../src/ISComm.h"
18 #include "../../src/serialPortPlatform.h"
19 
20 static int running = 1;
21 
22 static void handleInsMessage(ins_1_t* ins)
23 {
24  printf("INS TimeOfWeek: %.3fs, LLA: %3.7f,%3.7f,%5.2f, Euler: %5.1f,%5.1f,%5.1f\r\n",
25  ins->timeOfWeek,
26  ins->lla[0], ins->lla[1], ins->lla[2],
27  ins->theta[0] * C_RAD2DEG_F, ins->theta[1] * C_RAD2DEG_F, ins->theta[2] * C_RAD2DEG_F);
28 }
29 
30 static void handleGpsMessage(gps_pos_t* pos)
31 {
32  printf("GPS TimeOfWeek: %dms, LLA: %3.7f,%3.7f,%5.2f\r\n", pos->timeOfWeekMs, pos->lla[0], pos->lla[1], pos->lla[2]);
33 }
34 
35 static void handleImuMessage(dual_imu_t* imu)
36 {
37  printf("IMU Time: %.3fs, PQR: %5.1f,%5.1f,%5.1f, ACC: %5.1f,%5.1f,%5.1f,\r\n",
38  imu->time,
39  imu->I[0].pqr[0], imu->I[0].pqr[1], imu->I[0].pqr[2],
40  imu->I[0].acc[0], imu->I[0].acc[1], imu->I[0].acc[2]);
41 }
42 
43 
45 {
46  // Set INS output Euler rotation in radians to 90 degrees roll for mounting
47  float rotation[3] = { 90.0f*C_DEG2RAD_F, 0.0f, 0.0f };
48  int messageSize = is_comm_set_data(comm, _DID_FLASH_CONFIG, offsetof(nvm_flash_cfg_t, insRotation), sizeof(float) * 3, rotation);
49  if (messageSize != serialPortWrite(serialPort, comm->buf.start, messageSize))
50  {
51  printf("Failed to encode and write set INS rotation\r\n");
52  return -3;
53  }
54 
55  return 0;
56 }
57 
58 
60 {
61  // Stop all broadcasts on the device
62  int messageSize = is_comm_stop_broadcasts_all_ports(comm);
63  if (messageSize != serialPortWrite(serialPort, comm->buf.start, messageSize))
64  {
65  printf("Failed to encode and write stop broadcasts message\r\n");
66  return -3;
67  }
68  return 0;
69 }
70 
71 
73 {
74  system_command_t cfg;
76  cfg.invCommand = ~cfg.command;
77 
78  int messageSize = is_comm_set_data(comm, DID_SYS_CMD, 0, sizeof(system_command_t), &cfg);
79  if (messageSize != serialPortWrite(serialPort, comm->buf.start, messageSize))
80  {
81  printf("Failed to write save persistent message\r\n");
82  return -3;
83  }
84  return 0;
85 }
86 
87 
89 {
90  // Ask for INS message w/ update 40ms period (4ms source period x 10). Set data rate to zero to disable broadcast and pull a single packet.
91  int messageSize;
92  messageSize = is_comm_get_data(comm, _DID_INS_LLA_EULER_NED, 0, 0, 10);
93  if (messageSize != serialPortWrite(serialPort, comm->buf.start, messageSize))
94  {
95  printf("Failed to encode and write get INS message\r\n");
96  return -4;
97  }
98 
99 #if 1
100  // Ask for GPS message at period of 200ms (200ms source period x 1). Offset and size can be left at 0 unless you want to just pull a specific field from a data set.
101  messageSize = is_comm_get_data(comm, _DID_GPS1_POS, 0, 0, 1);
102  if (messageSize != serialPortWrite(serialPort, comm->buf.start, messageSize))
103  {
104  printf("Failed to encode and write get GPS message\r\n");
105  return -5;
106  }
107 #endif
108 
109 #if 0
110  // Ask for IMU message at period of 100ms (1ms source period x 100). This could be as high as 1000 times a second (period multiple of 1)
111  messageSize = is_comm_get_data(comm, _DID_IMU_DUAL, 0, 0, 100);
112  if (messageSize != serialPortWrite(serialPort, comm->buf.start, messageSize))
113  {
114  printf("Failed to encode and write get IMU message\r\n");
115  return -6;
116  }
117 #endif
118  return 0;
119 }
120 
121 
122 int main(int argc, char* argv[])
123 {
124  if (argc < 2)
125  {
126  printf("Please pass the com port as the only argument\r\n");
127  // In Visual Studio IDE, this can be done through "Project Properties -> Debugging -> Command Arguments: COM3"
128  return -1;
129  }
130 
131 
132  // STEP 2: Init comm instance
133  is_comm_instance_t comm;
134  uint8_t buffer[2048];
135 
136  // Initialize the comm instance, sets up state tracking, packet parsing, etc.
137  is_comm_init(&comm, buffer, sizeof(buffer));
138 
139 
140  // STEP 3: Initialize and open serial port
141  serial_port_t serialPort;
142 
143  // Initialize the serial port (Windows, MAC or Linux) - if using an embedded system like Arduino,
144  // you will need to handle the serial port creation, open and reads yourself. In this
145  // case, you do not need to include serialPort.h/.c and serialPortPlatform.h/.c in your project.
146  serialPortPlatformInit(&serialPort);
147 
148  // Open serial, last parameter is a 1 which means a blocking read, you can set as 0 for non-blocking
149  // you can change the baudrate to a supported baud rate (IS_BAUDRATE_*), make sure to reboot the uINS
150  // if you are changing baud rates, you only need to do this when you are changing baud rates.
151  if (!serialPortOpen(&serialPort, argv[1], IS_BAUDRATE_921600, 1))
152  {
153  printf("Failed to open serial port on com port %s\r\n", argv[1]);
154  return -2;
155  }
156 
157 
158  int error;
159 
160  // STEP 4: Stop any message broadcasting
161  if ((error = stop_message_broadcasting(&serialPort, &comm)))
162  {
163  return error;
164  }
165 
166 
167 #if 0 // STEP 5: Set configuration
168  if ((error = set_configuration(&serialPort, &comm)))
169  {
170  return error;
171  }
172 #endif
173 
174 
175  // STEP 6: Enable message broadcasting
176  if ((error = enable_message_broadcasting_get_data(&serialPort, &comm)))
177  {
178  return error;
179  }
180 
181 
182 #if 0 // STEP 7: (Optional) Save currently enabled streams as persistent messages enabled after reboot
183  save_persistent_messages(&serialPort, &comm);
184 #endif
185 
186 
187  // STEP 8: Handle received data
188  int count;
189  uint8_t inByte;
190 
191  // You can set running to false with some other piece of code to break out of the loop and end the program
192  while (running)
193  {
194  // Read one byte with a 20 millisecond timeout
195  while ((count = serialPortReadCharTimeout(&serialPort, &inByte, 20)) > 0)
196  {
197  switch (is_comm_parse_byte(&comm, inByte))
198  {
200  switch (comm.dataHdr.id)
201  {
204  break;
205 
206  case _DID_GPS1_POS:
208  break;
209 
210  case _DID_IMU_DUAL:
212  break;
213 
214  // TODO: add other cases for other data ids that you care about
215  }
216  break;
217 
218  default:
219  break;
220  }
221  }
222  }
223 }
224 
imus_t I
Definition: data_sets.h:617
int serialPortReadCharTimeout(serial_port_t *serialPort, unsigned char *c, int timeoutMilliseconds)
uint32_t id
Definition: ISComm.h:375
int serialPortOpen(serial_port_t *serialPort, const char *port, int baudRate, int blocking)
#define printf(...)
size_t count(InputIterator first, InputIterator last, T const &item)
Definition: catch.hpp:3206
float timeOfWeekMs
Definition: CAN_comm.h:29
protocol_type_t is_comm_parse_byte(is_comm_instance_t *instance, uint8_t byte)
Definition: ISComm.c:499
int is_comm_get_data(is_comm_instance_t *instance, uint32_t dataId, uint32_t offset, uint32_t size, uint32_t periodMultiple)
Definition: ISComm.c:588
int enable_message_broadcasting_get_data(serial_port_t *serialPort, is_comm_instance_t *comm)
int main(int argc, char *argv[])
float pqr[3]
Definition: data_sets.h:603
static int running
int set_configuration(serial_port_t *serialPort, is_comm_instance_t *comm)
int save_persistent_messages(serial_port_t *serialPort, is_comm_instance_t *comm)
static void handleInsMessage(ins_1_t *ins)
double timeOfWeek
Definition: data_sets.h:417
is_comm_buffer_t buf
Definition: ISComm.h:488
int is_comm_set_data(is_comm_instance_t *instance, uint32_t dataId, uint32_t offset, uint32_t size, void *data)
Definition: ISComm.c:634
uint32_t invCommand
Definition: data_sets.h:1112
static void handleImuMessage(dual_imu_t *imu)
#define DID_SYS_CMD
Definition: data_sets.h:41
uint32_t command
Definition: data_sets.h:1109
#define _DID_FLASH_CONFIG
Definition: ISComm.h:71
uint8_t * start
Definition: ISComm.h:447
#define C_RAD2DEG_F
Definition: ISConstants.h:562
#define C_DEG2RAD_F
Definition: ISConstants.h:560
int serialPortWrite(serial_port_t *serialPort, const unsigned char *buffer, int writeCount)
double lla[3]
Definition: data_sets.h:511
int stop_message_broadcasting(serial_port_t *serialPort, is_comm_instance_t *comm)
#define _DID_INS_LLA_EULER_NED
Definition: ISComm.h:47
static void handleGpsMessage(gps_pos_t *pos)
void is_comm_init(is_comm_instance_t *instance, uint8_t *buffer, int bufferSize)
Definition: ISComm.c:185
float acc[3]
Definition: data_sets.h:606
#define _DID_GPS1_POS
Definition: ISComm.h:55
int serialPortPlatformInit(serial_port_t *serialPort)
float theta[3]
Definition: data_sets.h:505
p_data_hdr_t dataHdr
Definition: ISComm.h:506
#define _DID_IMU_DUAL
Definition: ISComm.h:51
is_can_time time
Definition: CAN_comm.h:252
int is_comm_stop_broadcasts_all_ports(is_comm_instance_t *instance)
Definition: ISComm.c:644
uint8_t * dataPtr
Definition: ISComm.h:509


inertial_sense_ros
Author(s):
autogenerated on Sat Sep 19 2020 03:19:04