GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: tests/filters/test_madgwick_ahrs.cpp Lines: 23 23 100.0 %
Date: 2023-03-13 12:09:37 Branches: 61 122 50.0 %

Line Branch Exec Source
1
/*
2
 * Copyright 2019,
3
 * Olivier Stasse,
4
 *
5
 * CNRS/AIST
6
 *
7
 */
8
9
#include <iostream>
10
#include <sot/core/debug.hh>
11
12
#ifndef WIN32
13
#include <unistd.h>
14
#endif
15
16
using namespace std;
17
18
#include <dynamic-graph/entity.h>
19
#include <dynamic-graph/factory.h>
20
21
#include <sot/core/madgwickahrs.hh>
22
#include <sstream>
23
24
using namespace dynamicgraph;
25
using namespace dynamicgraph::sot;
26
27
#define BOOST_TEST_MODULE test - filter - differentiator
28
29
#include <boost/test/output_test_stream.hpp>
30
#include <boost/test/unit_test.hpp>
31
32
using boost::test_tools::output_test_stream;
33
34
















4
BOOST_AUTO_TEST_CASE(test_filter_differentiator) {
35

2
  sot::MadgwickAHRS *aFilter = new MadgwickAHRS("MadgwickAHRS");
36
37
2
  double timestep = 0.001, beta = 0.01;
38
2
  aFilter->init(timestep);
39
2
  aFilter->set_beta(beta);
40
41
2
  srand(0);
42
4
  dynamicgraph::Vector acc(3);
43
4
  dynamicgraph::Vector angvel(3);
44
2
  acc(0) = 0.3;
45
2
  acc(1) = 0.2;
46
2
  acc(2) = 0.3;
47
2
  aFilter->m_accelerometerSIN = acc;
48
2
  angvel(0) = 0.1;
49
2
  angvel(1) = -0.1;
50
2
  angvel(2) = 0.3;
51
2
  aFilter->m_gyroscopeSIN = angvel;
52
2
  aFilter->m_imu_quatSOUT.recompute(0);
53

4
  output_test_stream output;
54
4
  ostringstream anoss;
55
2
  aFilter->m_imu_quatSOUT.get(output);
56
2
  aFilter->m_imu_quatSOUT.get(anoss);
57
58



2
  BOOST_CHECK(
59
      output.is_equal("1 "
60
                      "5.5547e-05 "
61
                      "-5.83205e-05 "
62
                      "0.00015"));
63
2
}