crtp.cpp
Go to the documentation of this file.
1 #include <stdint.h>
2 #include <math.h>
3 #include <cstring>
4 #include "crtp.h"
5 
6 // Note: the quaternion compression code is copied from
7 // github.com/jpreiss/quatcompress
8 
9 // compress a unit quaternion into 32 bits.
10 // assumes input quaternion is normalized. will fail if not.
11 static inline uint32_t quatcompress(float const q[4])
12 {
13  // we send the values of the quaternion's smallest 3 elements.
14  unsigned i_largest = 0;
15  for (unsigned i = 1; i < 4; ++i) {
16  if (fabsf(q[i]) > fabsf(q[i_largest])) {
17  i_largest = i;
18  }
19  }
20 
21  // since -q represents the same rotation as q,
22  // transform the quaternion so the largest element is positive.
23  // this avoids having to send its sign bit.
24  unsigned negate = q[i_largest] < 0;
25 
26  // 1/sqrt(2) is the largest possible value
27  // of the second-largest element in a unit quaternion.
28  float const SMALL_MAX = 1.0 / sqrt(2);
29 
30  // do compression using sign bit and 9-bit precision per element.
31  uint32_t comp = i_largest;
32  for (unsigned i = 0; i < 4; ++i) {
33  if (i != i_largest) {
34  unsigned negbit = (q[i] < 0) ^ negate;
35  unsigned mag = ((1 << 9) - 1) * (fabsf(q[i]) / SMALL_MAX) + 0.5f;
36  comp = (comp << 10) | (negbit << 9) | mag;
37  }
38  }
39 
40  return comp;
41 }
42 
43 // This is the matching function to decompress
44 // decompress a quaternion from 32 bit compressed representation.
45 void quatdecompress(uint32_t comp, float q[4])
46 {
47  float const SMALL_MAX = 1.0 / sqrt(2);
48  unsigned const mask = (1 << 9) - 1;
49 
50  int const i_largest = comp >> 30;
51  float sum_squares = 0;
52  for (int i = 3; i >= 0; --i) {
53  if (i != i_largest) {
54  unsigned mag = comp & mask;
55  unsigned negbit = (comp >> 9) & 0x1;
56  comp = comp >> 10;
57  q[i] = SMALL_MAX * ((float)mag) / mask;
58  if (negbit == 1) {
59  q[i] = -q[i];
60  }
61  sum_squares += q[i] * q[i];
62  }
63  }
64  q[i_largest] = sqrtf(1.0f - sum_squares);
65 }
66 
68  float x, float y, float z,
69  float vx, float vy, float vz,
70  float ax, float ay, float az,
71  float qx, float qy, float qz, float qw,
72  float rollRate, float pitchRate, float yawRate)
73  : header(0x07, 0), type(6)
74 {
75  float s = 1000.0;
76  this->x = s * x;
77  this->y = s * y;
78  this->z = s * z;
79  this->vx = s * vx;
80  this->vy = s * vy;
81  this->vz = s * vz;
82  this->ax = s * ax;
83  this->ay = s * ay;
84  this->az = s * az;
85 
86  float q[4] = { qx, qy, qz, qw };
87  this->quat = quatcompress(q);
88  this->omegax = s * rollRate;
89  this->omegay = s * pitchRate;
90  this->omegaz = s * yawRate;
91 }
92 
94  : header(0X07, 0), type(0)
95 {}
96 
97 // m/s for velocity, deg/s for yawrate, and m for zDistance
99  float vx,
100  float vy,
101  float yawrate,
102  float zDistance)
103  : header(0X07, 0), type(5)
104 {
105  this->vx = vx;
106  this->vy = vy;
107  this->yawrate = yawrate;
108  this->zDistance = zDistance;
109 }
110 
111 // m in position, degree in yaw
113  float x,
114  float y,
115  float z,
116  float yaw)
117  : header(0X07, 0), type(7)
118 {
119  this->x = x;
120  this->y = y;
121  this->z = z;
122  this->yaw = yaw;
123 }
124 
125 template<class T>
127  const char* group,
128  const char* name,
129  uint8_t paramType,
130  const void* value,
131  uint8_t valueSize)
132  : header(2,3)
133 {
134  size_t groupLen = strlen(group);
135  size_t nameLen = strlen(name);
136  // TODO: do size checking here...
137 
138  int idx = 0;
139  // first insert group (followed by \0)
140  memcpy(&data[idx], group, groupLen + 1);
141  idx += groupLen + 1;
142  // insert name (followed by \0)
143  memcpy(&data[idx], name, nameLen + 1);
144  idx += nameLen + 1;
145  // insert type
146  data[idx] = paramType;
147  idx++;
148  // insert value
149  memcpy(&data[idx], value, valueSize);
150  idx += valueSize;
151 
152  size_ = idx + 2;
153  responseSize_ = 1+groupLen+1+nameLen+1+1;
154 }
155 
156 template <>
158  const char* group,
159  const char* name,
160  const uint8_t& value)
161  : crtpParamSetByNameRequest(group, name, ParamTypeUint8, &value, sizeof(uint8_t))
162 {
163 }
164 
165 template <>
167  const char* group,
168  const char* name,
169  const int8_t& value)
170  : crtpParamSetByNameRequest(group, name, ParamTypeInt8, &value, sizeof(int8_t))
171 {
172 }
173 
174 template <>
176  const char* group,
177  const char* name,
178  const uint16_t& value)
179  : crtpParamSetByNameRequest(group, name, ParamTypeUint16, &value, sizeof(uint16_t))
180 {
181 }
182 
183 template <>
185  const char* group,
186  const char* name,
187  const int16_t& value)
188  : crtpParamSetByNameRequest(group, name, ParamTypeInt16, &value, sizeof(int16_t))
189 {
190 }
191 
192 template <>
194  const char* group,
195  const char* name,
196  const uint32_t& value)
197  : crtpParamSetByNameRequest(group, name, ParamTypeUint32, &value, sizeof(uint32_t))
198 {
199 }
200 
201 template <>
203  const char* group,
204  const char* name,
205  const int32_t& value)
206  : crtpParamSetByNameRequest(group, name, ParamTypeInt32, &value, sizeof(int32_t))
207 {
208 }
209 
210 template <>
212  const char* group,
213  const char* name,
214  const float& value)
215  : crtpParamSetByNameRequest(group, name, ParamTypeFloat, &value, sizeof(float))
216 {
217 }
crtpPositionSetpointRequest(float x, float y, float z, float yaw)
Definition: crtp.cpp:112
float qy
Definition: crtp.h:456
float yawrate
Definition: crtp.h:441
float vy
Definition: crtp.h:440
void quatdecompress(uint32_t comp, float q[4])
Definition: crtp.cpp:45
int16_t vz
Definition: crtp.h:445
const T value
Definition: crtp.h:26
float vx
Definition: crtp.h:439
float z
Definition: crtp.h:445
float zDistance
Definition: crtp.h:442
crtpHoverSetpointRequest(float vx, float vy, float yawrate, float zDistance)
Definition: crtp.cpp:98
uint8_t type
Definition: crtp.h:23
float qw
Definition: crtp.h:458
crtpFullStateSetpointRequest(float x, float y, float z, float vx, float vy, float vz, float ax, float ay, float az, float qx, float qy, float qz, float qw, float rollRate, float pitchRate, float yawRate)
Definition: crtp.cpp:67
uint8_t group
Definition: crtp.h:27
float yaw
Definition: crtp.h:442
char name[30]
Definition: crtp.h:433
crtpParamSetByNameRequest(const char *group, const char *name, const T &value)
static uint32_t quatcompress(float const q[4])
Definition: crtp.cpp:11
int16_t ay
Definition: crtp.h:447
uint8_t data[29]
Definition: crtp.h:368
const crtp header
Definition: crtp.h:1000
crtpStopRequest()
Definition: crtp.cpp:93
int16_t az
Definition: crtp.h:448
float x
Definition: crtp.h:443
uint8_t type
Definition: crtp.h:1001
float qz
Definition: crtp.h:457
int16_t ax
Definition: crtp.h:446
const crtp header
Definition: crtp.h:29
const crtp header
Definition: crtp.h:1012
float y
Definition: crtp.h:444
float qx
Definition: crtp.h:455


crazyflie_cpp
Author(s): Wolfgang Hoenig
autogenerated on Fri Nov 1 2019 03:39:56