Witmotion IMU Library  1.3.1~dev_4684448
Open source UART communication library for sensor devices manufactured by WitMotion Shenzhen Co.,Ltd
util.h
Go to the documentation of this file.
1 /*!
2  \file util.h
3  \brief Helper and utility functions to decode and represent the data
4  \author Andrey Vukolov andrey.vukolov@elettra.eu
5 */
6 
7 #ifndef WITMOTION_UTIL
8 #define WITMOTION_UTIL
9 #include "witmotion/types.h"
10 
11 namespace witmotion
12 {
13 
14 class witmotion_typed_packets
15 {
16 private:
17  witmotion_datapacket array[32];
18 public:
19  witmotion_datapacket& operator[](const witmotion_packet_id id);
20 };
21 
22 class witmotion_typed_bytecounts
23 {
24 private:
25  size_t array[32];
26 public:
27  witmotion_typed_bytecounts();
28  size_t& operator[](const witmotion_packet_id id);
29 };
30 
31 /*!
32  \brief Converts the frequency value in Hertz to subsequent Witmotion opcode.
33 
34  Special values for the `hertz` argument are:
35  |Value|Description|
36  |-----|-----------|
37  |`0` | Shuts down the measurements but does not turn the device into [dormant mode](\ref ridStandbyMode) |
38  |`-1` | Orders the single-shot measurement, then shutdown |
39  |`-2` | 1 measurement in 2 seconds |
40  |`-10`| 1 measurement in 10 seconds |
41  \param hertz - frequency in Hertz, or a special value as it is described above
42  \return Witmotion opcode value as a byte, `0x06` (10 Hz) by default is the argument is inacceptable
43  */
44 uint8_t witmotion_output_frequency(const int hertz);
45 
46 uint8_t witmotion_baud_rate(const QSerialPort::BaudRate rate);
47 
48 bool id_registered(const size_t id);
49 
50 /* COMPONENT DECODERS */
51 float decode_acceleration(const int16_t* value);
52 float decode_angular_velocity(const int16_t* value);
53 float decode_angle(const int16_t* value);
54 float decode_temperature(const int16_t* value);
55 float decode_orientation(const int16_t* value);
56 void decode_gps_coord(const int32_t* value,
57  double& deg,
58  double& min);
59 
60 /* PACKET DECODERS */
61 void decode_realtime_clock(const witmotion_datapacket& packet,
62  uint8_t& year,
63  uint8_t& month,
64  uint8_t& day,
65  uint8_t& hour,
66  uint8_t& minute,
67  uint8_t& second,
68  uint16_t& millisecond);
69 void decode_accelerations(const witmotion_datapacket& packet,
70  float& x,
71  float& y,
72  float& z,
73  float& t);
74 void decode_angular_velocities(const witmotion_datapacket& packet,
75  float& x,
76  float& y,
77  float& z,
78  float& t);
79 void decode_angles(const witmotion_datapacket& packet,
80  float& roll,
81  float& pitch,
82  float& yaw,
83  float& t);
84 void decode_magnetometer(const witmotion_datapacket& packet,
85  float& x,
86  float& y,
87  float& z,
88  float& t);
89 void decode_altimeter(const witmotion_datapacket& packet,
90  double& pressure,
91  double& height);
92 void decode_gps(const witmotion_datapacket& packet,
93  double& longitude_deg,
94  double& longitude_min,
95  double& latitude_deg,
96  double& latitude_min);
97 void decode_gps_ground_speed(const witmotion_datapacket& packet,
98  float& altitude,
99  float& angular_velocity,
100  double& ground_speed);
101 void decode_orientation(const witmotion_datapacket& packet,
102  float& x,
103  float& y,
104  float& z,
105  float& w);
106 void decode_gps_accuracy(const witmotion_datapacket& packet,
107  size_t& satellites,
108  float& local_accuracy,
109  float& horizontal_accuracy,
110  float& vertical_accuracy);
111 
112 /* MISCELLANEOUS UTILITIES */
113 template<typename T> T variance(const std::vector<T>& array)
114 {
115  T sum = std::accumulate(array.begin(), array.end(), 0.f);
116  T mean = sum / static_cast<T>(array.size());
117  T sq_dif = 0.f;
118  for(auto i = array.begin(); i != array.end(); i++)
119  sq_dif += std::pow((*i) - mean, 2);
120  sq_dif /= (array.size() > 1) ? static_cast<T>(array.size() - 1) : 1.f;
121  return std::sqrt(sq_dif);
122 }
123 
124 }
125 #endif