APM:Libraries
INS_generic.cpp
Go to the documentation of this file.
1 //
2 // Simple test for the AP_InertialSensor driver.
3 //
4 
5 #include <AP_HAL/AP_HAL.h>
8 
10 
12 
13 static void display_offsets_and_scaling();
14 static void run_test();
15 
16 // board specific config
18 
19 void setup(void);
20 void loop(void);
21 
22 void setup(void)
23 {
24  // setup any board specific drivers
25  BoardConfig.init();
26 
27  hal.console->printf("AP_InertialSensor startup...\n");
28 
29  ins.init(100);
30 
31  // display initial values
33 
34  // display number of detected accels/gyros
35  hal.console->printf("\n");
36  hal.console->printf("Number of detected accels : %u\n", ins.get_accel_count());
37  hal.console->printf("Number of detected gyros : %u\n\n", ins.get_gyro_count());
38 
39  hal.console->printf("Complete. Reading:\n");
40 }
41 
42 void loop(void)
43 {
44  int16_t user_input;
45 
46  hal.console->printf("\n");
47  hal.console->printf("%s\n",
48  "Menu:\n"
49  " d) display offsets and scaling\n"
50  " l) level (capture offsets from level)\n"
51  " t) test\n"
52  " r) reboot");
53 
54  // wait for user input
55  while (!hal.console->available()) {
56  hal.scheduler->delay(20);
57  }
58 
59  // read in user input
60  while (hal.console->available()) {
61  user_input = hal.console->read();
62 
63  if (user_input == 'd' || user_input == 'D') {
65  }
66 
67  if (user_input == 't' || user_input == 'T') {
68  run_test();
69  }
70 
71  if (user_input == 'r' || user_input == 'R') {
72  hal.scheduler->reboot(false);
73  }
74  }
75 }
76 
78 {
79  const Vector3f &accel_offsets = ins.get_accel_offsets();
80  const Vector3f &accel_scale = ins.get_accel_scale();
81  const Vector3f &gyro_offsets = ins.get_gyro_offsets();
82 
83  // display results
84  hal.console->printf("\nAccel Offsets X:%10.8f \t Y:%10.8f \t Z:%10.8f\n",
85  (double)accel_offsets.x,
86  (double)accel_offsets.y,
87  (double)accel_offsets.z);
88  hal.console->printf("Accel Scale X:%10.8f \t Y:%10.8f \t Z:%10.8f\n",
89  (double)accel_scale.x,
90  (double)accel_scale.y,
91  (double)accel_scale.z);
92  hal.console->printf("Gyro Offsets X:%10.8f \t Y:%10.8f \t Z:%10.8f\n",
93  (double)gyro_offsets.x,
94  (double)gyro_offsets.y,
95  (double)gyro_offsets.z);
96 }
97 
98 static void run_test()
99 {
100  Vector3f accel;
101  Vector3f gyro;
102  uint8_t counter = 0;
103  static uint8_t accel_count = ins.get_accel_count();
104  static uint8_t gyro_count = ins.get_gyro_count();
105  static uint8_t ins_count = MAX(accel_count, gyro_count);
106 
107  // flush any user input
108  while (hal.console->available()) {
109  hal.console->read();
110  }
111 
112  // clear out any existing samples from ins
113  ins.update();
114 
115  // loop as long as user does not press a key
116  while (!hal.console->available()) {
117  // wait until we have a sample
118  ins.wait_for_sample();
119 
120  // read samples from ins
121  ins.update();
122 
123  // print each accel/gyro result every 50 cycles
124  if (counter++ % 50 != 0) {
125  continue;
126  }
127 
128  // loop and print each sensor
129  for (uint8_t ii = 0; ii < ins_count; ii++) {
130  char state;
131 
132  if (ii > accel_count - 1) {
133  // No accel present
134  state = '-';
135  } else if (ins.get_accel_health(ii)) {
136  // Healthy accel
137  state = 'h';
138  } else {
139  // Accel present but not healthy
140  state = 'u';
141  }
142 
143  accel = ins.get_accel(ii);
144 
145  hal.console->printf("%u - Accel (%c) : X:%6.2f Y:%6.2f Z:%6.2f norm:%5.2f",
146  ii, state, (double)accel.x, (double)accel.y, (double)accel.z,
147  (double)accel.length());
148 
149  gyro = ins.get_gyro(ii);
150 
151  if (ii > gyro_count - 1) {
152  // No gyro present
153  state = '-';
154  } else if (ins.get_gyro_health(ii)) {
155  // Healthy gyro
156  state = 'h';
157  } else {
158  // Gyro present but not healthy
159  state = 'u';
160  }
161 
162  hal.console->printf(" Gyro (%c) : X:%6.2f Y:%6.2f Z:%6.2f\n",
163  state, (double)gyro.x, (double)gyro.y, (double)gyro.z);
164  auto temp = ins.get_temperature(ii);
165  hal.console->printf(" t:%6.2f\n", (double)temp);
166  }
167  }
168 
169  // clear user input
170  while (hal.console->available()) {
171  hal.console->read();
172  }
173 }
174 
175 AP_HAL_MAIN();
static uint8_t counter
const AP_HAL::HAL & hal
-*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
Definition: INS_generic.cpp:9
AP_HAL::UARTDriver * console
Definition: HAL.h:110
const Vector3f & get_gyro(uint8_t i) const
float get_temperature(uint8_t instance) const
void setup(void)
Definition: INS_generic.cpp:22
static auto MAX(const A &one, const B &two) -> decltype(one > two ? one :two)
Definition: AP_Math.h:202
const Vector3f & get_accel(uint8_t i) const
virtual void delay(uint16_t ms)=0
static AP_InertialSensor ins
Definition: INS_generic.cpp:11
virtual void printf(const char *,...) FMT_PRINTF(2
Definition: BetterStream.cpp:5
static AP_BoardConfig BoardConfig
Definition: INS_generic.cpp:17
void loop(void)
Definition: INS_generic.cpp:42
T y
Definition: vector3.h:67
const Vector3f & get_gyro_offsets(uint8_t i) const
T z
Definition: vector3.h:67
static int state
Definition: Util.cpp:20
virtual int16_t read()=0
const HAL & get_HAL()
float length(void) const
Definition: vector3.cpp:288
static void display_offsets_and_scaling()
Definition: INS_generic.cpp:77
virtual uint32_t available()=0
virtual void reboot(bool hold_in_bootloader)=0
void init(uint16_t sample_rate_hz)
const Vector3f & get_accel_offsets(uint8_t i) const
uint8_t get_gyro_count(void) const
const Vector3f & get_accel_scale(uint8_t i) const
bool get_gyro_health(uint8_t instance) const
AP_HAL::Scheduler * scheduler
Definition: HAL.h:114
static void run_test()
Definition: INS_generic.cpp:98
T x
Definition: vector3.h:67
uint8_t get_accel_count(void) const
bool get_accel_health(uint8_t instance) const
AP_HAL_MAIN()