common.cpp
Go to the documentation of this file.
1 #include "common.h"
2 
3 double quaternion_error(Eigen::Quaternionf q_eig, turbomath::Quaternion q)
4 {
5  Eigen::Quaternionf est_quat(q.w, q.x, q.y, q.z);
6  Eigen::Quaternionf q_tilde = q_eig * est_quat.inverse();
7  if (q_tilde.vec().norm() < 0.000001)
8  return 0;
9  else
10  {
11  Eigen::Vector3f v_tilde = atan2(q_tilde.vec().norm(), q_tilde.w())*q_tilde.vec()/q_tilde.vec().norm();
12  return v_tilde.norm();
13  }
14 }
15 
17 {
18  Eigen::Quaternionf est_quat(q.w, q.x, q.y, q.z);
19  Eigen::Quaternionf q_eig(q0.w, q0.x, q0.y, q0.z);
20  Eigen::Quaternionf q_tilde = q_eig * est_quat.inverse();
21  if (q_tilde.vec().norm() < 0.000001)
22  return 0;
23  else
24  {
25  Eigen::Vector3f v_tilde = atan2(q_tilde.vec().norm(), q_tilde.w())*q_tilde.vec()/q_tilde.vec().norm();
26  return v_tilde.norm();
27  }
28 }
29 
31 {
32  uint64_t start_time_us = board.clock_micros();
33  float dummy_acc[3] = {0, 0, -9.80665};
34  float dummy_gyro[3] = {0, 0, 0};
35  while (board.clock_micros() < start_time_us + us)
36  {
37  board.set_imu(dummy_acc, dummy_gyro, board.clock_micros() + 1000);
38  rf.run();
39  }
40 }
41 
42 int main(int argc, char **argv)
43 {
44  testing::InitGoogleTest(&argc, argv);
45  return RUN_ALL_TESTS();
46 }
float atan2(float y, float x)
Definition: turbomath.cpp:488
void set_imu(float *acc, float *gyro, uint64_t time_us)
Definition: test_board.cpp:58
double quaternion_error(Eigen::Quaternionf q_eig, turbomath::Quaternion q)
Definition: common.cpp:3
uint64_t clock_micros() override
Definition: test_board.cpp:79
int main(int argc, char **argv)
Definition: common.cpp:42
void run()
Main loop for the ROSflight autopilot flight stack.
Definition: rosflight.cpp:92
void step_firmware(rosflight_firmware::ROSflight &rf, rosflight_firmware::testBoard &board, uint32_t us)
Definition: common.cpp:30


rosflight_firmware
Author(s): Daniel Koch , James Jackson
autogenerated on Wed Jul 3 2019 19:59:24