Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-22 20:36:06 +08:00
Code Issues Actions 26 Packages Projects Releases Wiki Activity

[ground] fix natnet typos

Browse Source
This commit is contained in:
kirkscheper
2016-01-29 11:24:15 +01:00
committed by Gautier Hattenberger
parent 818e949c6f
commit 80857d9edd
1 changed files with 8 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/ground_segment/misc/natnet2ivy.c
+8 -8
View File
@@ -512,25 +512,25 @@ gboolean timeout_transmit_callback(gpointer data)
* used.
*/
uint32_t pos_xyz;
uint32_t pos_xyz = 0;
// check if position within limits
if (fabs(pos.x * 100) < pow(2, 10)) {
if (fabs(pos.x * 100.) < pow(2, 10)) {
pos_xyz = (((uint32_t)(pos.x * 100.0)) & 0x7FF) << 21; // bits 31-21 x position in cm
} else {
fprintf(stderr, "Warning!! X position out of maximum range of small message (±%.2fm): %.2f", pow(2, 10) / 100, pos.x);
pos_xyz = (((uint32_t)(pow(2, 10) * pos.x / fabs(pos.x))) & 0x7FF) << 21; // bits 31-21 x position in cm
}
if (fabs(pos.y * 100) < pow(2, 10)) {
if (fabs(pos.y * 100.) < pow(2, 10)) {
pos_xyz |= (((uint32_t)(pos.y * 100.0)) & 0x7FF) << 10; // bits 20-10 y position in cm
} else {
fprintf(stderr, "Warning!! Y position out of maximum range of small message (±%.2fm): %.2f", pow(2, 10) / 100, pos.y);
pos_xyz |= (((uint32_t)(pow(2, 10) * pos.y / fabs(pos.y))) & 0x7FF) << 10; // bits 20-10 y position in cm
}
if (pos.z * 100 < pow(2, 10)) {
pos_xyz |= (((uint32_t)(pos.z * 100.0)) & 0x3FF); // bits 9-0 z position in cm
} else {
if (pos.z * 100. < pow(2, 10) && pos.z > 0.) {
pos_xyz |= (((uint32_t)(fabs(pos.z) * 100.0)) & 0x3FF); // bits 9-0 z position in cm
} else if (pos.z > 0.) {
fprintf(stderr, "Warning!! Z position out of maximum range of small message (%.2fm): %.2f", pow(2, 10) / 100, pos.z);
pos_xyz |= (((uint32_t)(pow(2, 10))) & 0x3FF); // bits 9-0 z position in cm
}
@@ -539,7 +539,7 @@ gboolean timeout_transmit_callback(gpointer data)
/* The speed is an int32 and the 11 LSBs of the x and y axis and 10 LSBs of z (all signed) are compressed into
* a single integer.
*/
uint32_t speed_xyz;
uint32_t speed_xyz = 0;
// check if speed within limits
if (fabs(speed.x * 100) < pow(2, 10)) {
speed_xyz = (((uint32_t)(speed.x * 100.0)) & 0x7FF) << 21; // bits 31-21 speed x in cm/s
@@ -555,7 +555,7 @@ gboolean timeout_transmit_callback(gpointer data)
speed_xyz |= (((uint32_t)(pow(2, 10) * speed.y / fabs(speed.y))) & 0x7FF) << 10; // bits 20-10 speed y in cm/s
}
if (fabs(pos.z * 100) < pow(2, 9)) {
if (fabs(speed.z * 100) < pow(2, 9)) {
speed_xyz |= (((uint32_t)(speed.z * 100.0)) & 0x3FF); // bits 9-0 speed z in cm/s
} else {
fprintf(stderr, "Warning!! Z Speed out of maximum range of small message (±%.2fm/s): %.2f", pow(2, 9) / 100, speed.z);