Statistics
| Branch: | Tag: | Revision:

amiro-os / test / periphery-lld / A3906_v1 / aos_test_A3906.c @ 3cee92c7

History | View | Annotate | Download (14.232 KB)

1 e545e620 Thomas Schöpping
/*
2
AMiRo-OS is an operating system designed for the Autonomous Mini Robot (AMiRo) platform.
3 84f0ce9e Thomas Schöpping
Copyright (C) 2016..2019  Thomas Schöpping et al.
4 e545e620 Thomas Schöpping

5
This program is free software: you can redistribute it and/or modify
6
it under the terms of the GNU General Public License as published by
7
the Free Software Foundation, either version 3 of the License, or
8
(at your option) any later version.
9

10
This program is distributed in the hope that it will be useful,
11
but WITHOUT ANY WARRANTY; without even the implied warranty of
12
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13
GNU General Public License for more details.
14

15
You should have received a copy of the GNU General Public License
16
along with this program.  If not, see <http://www.gnu.org/licenses/>.
17
*/
18
19 ddf34c3d Thomas Schöpping
#include <amiroos.h>
20 4c72a54c Thomas Schöpping
#include <aos_test_A3906.h>
21 e545e620 Thomas Schöpping
22 4c72a54c Thomas Schöpping
#if (AMIROOS_CFG_TESTS_ENABLE == true) || defined(__DOXYGEN__)
23 e545e620 Thomas Schöpping
24
#include <stdlib.h>
25
#include <math.h>
26
27 f3ac1c96 Thomas Schöpping
/******************************************************************************/
28
/* LOCAL DEFINITIONS                                                          */
29
/******************************************************************************/
30
31 e545e620 Thomas Schöpping
/**
32
 * @brief   Interval to poll QEI in certain tests.
33
 */
34 2e69d671 Thomas Schöpping
#define QEI_POLL_INTERVAL_MS          100
35 e545e620 Thomas Schöpping
36
/**
37
 * @brief   Threshold for QEI differences.
38 b033b3ae Thomas Schöpping
 * @details Differences smaller than or equal to this value are neglected (interpreted as zero).
39
 *          The value can be interpreted as encoder ticks per second (tps).
40
 * @note    The expected value is about 7000 tps and a jitter of up to ±2% is ok.
41 e545e620 Thomas Schöpping
 */
42 b033b3ae Thomas Schöpping
#define QEI_DIFF_THRESHOLD            (apalQEICount_t)(7000 * 0.02f)
43 e545e620 Thomas Schöpping
44 f3ac1c96 Thomas Schöpping
/******************************************************************************/
45
/* EXPORTED VARIABLES                                                         */
46
/******************************************************************************/
47
48
/******************************************************************************/
49
/* LOCAL TYPES                                                                */
50
/******************************************************************************/
51
52 e545e620 Thomas Schöpping
/**
53
 * @brief   Enumerator to distinguish between left and right wheel.
54
 */
55
typedef enum {
56
  WHEEL_LEFT    = 0,  /**< left wheel identifier */
57
  WHEEL_RIGHT   = 1,  /**< right wheel identifier */
58
} wheel_t;
59
60
/**
61
 * @brief   Enumerator to distinguish directions.
62
 */
63
typedef enum {
64
  DIRECTION_FORWARD   = 0,  /**< forward direction identifier */
65
  DIRECTION_BACKWARD  = 1,  /**< backward direction identifier */
66
} direction_t;
67
68 f3ac1c96 Thomas Schöpping
/******************************************************************************/
69
/* LOCAL VARIABLES                                                            */
70
/******************************************************************************/
71
72
/******************************************************************************/
73
/* LOCAL FUNCTIONS                                                            */
74
/******************************************************************************/
75
76 e545e620 Thomas Schöpping
/**
77
 * @brief   helper function to test each wheel and direction separately.
78
 *
79
 * @param[in] stream      Stream for input/output.
80 4c72a54c Thomas Schöpping
 * @param[in] data        Test meta data.
81 e545e620 Thomas Schöpping
 * @param[in] wheel       Wheel to test.
82
 * @param[in] direction   Direction to test.
83
 * @param[in,out] result  Result variable to modify.
84
 */
85 4c72a54c Thomas Schöpping
void _wheelDirectionTest(BaseSequentialStream* stream, aos_test_a3906data_t* data, wheel_t wheel, direction_t direction, aos_testresult_t* result)
86 e545e620 Thomas Schöpping
{
87
  // local variables
88 b23ca7cc Thomas Schöpping
  int32_t status;
89 e545e620 Thomas Schöpping
  bool qei_valid;
90
  apalQEICount_t qei_count[2];
91
  apalQEIDirection_t qei_direction;
92
  uint32_t timeout_counter;
93
94
  chprintf(stream, "%s wheel %s...\n", (wheel == WHEEL_LEFT) ? "left" : "right", (direction == DIRECTION_FORWARD) ? "forward" : "backward");
95
  qei_valid = false;
96
  status = apalQEIGetPosition((wheel == WHEEL_LEFT) ? data->qei.left : data->qei.right, &qei_count[0]);
97
  // increase PWM incrementally and read QEI data
98
  for (apalPWMwidth_t pwm_width = APAL_PWM_WIDTH_MIN; pwm_width < APAL_PWM_WIDTH_MAX; ++pwm_width) {
99 b23ca7cc Thomas Schöpping
    status = a3906_lld_set_pwm(data->pwm.driver, (wheel == WHEEL_LEFT) ?
100 e545e620 Thomas Schöpping
                                  ((direction == DIRECTION_FORWARD) ? data->pwm.channel.left_forward : data->pwm.channel.left_backward) :
101
                                  ((direction == DIRECTION_FORWARD) ? data->pwm.channel.right_forward : data->pwm.channel.right_backward),
102
                                pwm_width);
103
    status |= apalQEIGetPosition((wheel == WHEEL_LEFT) ? data->qei.left : data->qei.right, &qei_count[1]);
104
    qei_valid = qei_valid || (qei_count[0] != qei_count[1]);
105
    aosThdUSleep(5 * MICROSECONDS_PER_SECOND / (APAL_PWM_WIDTH_MAX - APAL_PWM_WIDTH_MIN));
106
    qei_count[0] = qei_count[1];
107
  }
108
  status |= qei_valid ? 0x00 : 0x10;
109
  status |= apalQEIGetDirection((wheel == WHEEL_LEFT) ? data->qei.left : data->qei.right, &qei_direction);
110
  status |= (qei_direction == ((direction == DIRECTION_FORWARD) ? APAL_QEI_DIRECTION_UP : APAL_QEI_DIRECTION_DOWN)) ? 0x00 : 0x20;
111
112
  // let the wheel spin free until it stops
113
  status |= a3906_lld_set_pwm(data->pwm.driver, (wheel == WHEEL_LEFT) ?
114
                                ((direction == DIRECTION_FORWARD) ? data->pwm.channel.left_forward : data->pwm.channel.left_backward) :
115
                                ((direction == DIRECTION_FORWARD) ? data->pwm.channel.right_forward : data->pwm.channel.right_backward),
116
                              0);
117
  qei_count[0] = 0;
118
  qei_count[1] = 0;
119
  timeout_counter = 0;
120
  do {
121
    status |= apalQEIGetPosition((wheel == WHEEL_LEFT) ? data->qei.left : data->qei.right, &qei_count[0]);
122
    aosThdMSleep(1);
123
    status |= apalQEIGetPosition((wheel == WHEEL_LEFT) ? data->qei.left : data->qei.right, &qei_count[1]);
124
    ++timeout_counter;
125
  } while ((qei_count[0] != qei_count[1]) && (timeout_counter * MICROSECONDS_PER_MILLISECOND <= data->timeout));
126
  status |= (timeout_counter * MICROSECONDS_PER_MILLISECOND > data->timeout) ? APAL_STATUS_TIMEOUT : 0x00;
127
128
  // report result
129
  if (status == APAL_STATUS_SUCCESS) {
130 4c72a54c Thomas Schöpping
    aosTestPassed(stream, result);
131 e545e620 Thomas Schöpping
  } else {
132 4c72a54c Thomas Schöpping
    aosTestFailedMsg(stream, result, "0x%08X\n", status);
133 e545e620 Thomas Schöpping
  }
134
135
  return;
136
}
137
138 4c72a54c Thomas Schöpping
void _wheelSpeedTest(BaseSequentialStream* stream, aos_test_a3906data_t* data, wheel_t wheel, direction_t direction, aos_testresult_t* result)
139 47680f67 Thomas Schöpping
{
140
  // local variables
141 b23ca7cc Thomas Schöpping
  int32_t status;
142 47680f67 Thomas Schöpping
  apalQEICount_t qei_range;
143
  apalQEICount_t qei_count[2] = {0};
144
  apalQEICount_t qei_increments[2] = {0};
145
  apalQEICount_t qei_increments_diff = 0;
146
  uint32_t timeout_counter = 0;
147
  uint32_t stable_counter = 0;
148
149
  chprintf(stream, "%s wheel full speed %s...\n", (wheel == WHEEL_LEFT) ? "left" : "right", (direction == DIRECTION_FORWARD) ? "forward" : "backward");
150
  // spin up the wheel with full speed
151
  status = apalQEIGetRange((wheel == WHEEL_LEFT) ? data->qei.left : data->qei.right, &qei_range);
152
  status |= a3906_lld_set_pwm(data->pwm.driver, (wheel == WHEEL_LEFT) ?
153
                               ((direction == DIRECTION_FORWARD) ? data->pwm.channel.left_forward : data->pwm.channel.left_backward) :
154
                               ((direction == DIRECTION_FORWARD) ? data->pwm.channel.right_forward : data->pwm.channel.right_backward),
155
                             APAL_PWM_WIDTH_MAX);
156
  aosThdMSleep(100);
157
  do {
158
    // read QEI data to determine speed
159
    status |= apalQEIGetPosition((wheel == WHEEL_LEFT) ? data->qei.left : data->qei.right, &qei_count[0]);
160
    aosThdMSleep(QEI_POLL_INTERVAL_MS);
161
    status |= apalQEIGetPosition((wheel == WHEEL_LEFT) ? data->qei.left : data->qei.right, &qei_count[1]);
162
    timeout_counter += QEI_POLL_INTERVAL_MS;
163
    qei_increments[0] = qei_increments[1];
164
    qei_increments[1] = (direction == DIRECTION_FORWARD) ?
165
                          ((qei_count[1] > qei_count[0]) ? (qei_count[1] - qei_count[0]) : (qei_count[1] + (qei_range - qei_count[0]))) :
166
                          ((qei_count[0] > qei_count[1]) ? (qei_count[0] - qei_count[1]) : (qei_count[0] + (qei_range - qei_count[1])));
167 b033b3ae Thomas Schöpping
    qei_increments_diff = abs((int32_t)qei_increments[0] - (int32_t)qei_increments[1]) * ((float)MILLISECONDS_PER_SECOND / (float)QEI_POLL_INTERVAL_MS);
168
    stable_counter = ((qei_increments[0] != 0 || qei_increments[1] != 0) && qei_increments_diff <= QEI_DIFF_THRESHOLD) ? stable_counter+1 : 0;
169
    if (qei_increments[0] != 0 && stable_counter == 0) {
170
      chprintf(stream, "\tunstable speed? jitter of %u tps is above threshold (%u tps).\n", qei_increments_diff, QEI_DIFF_THRESHOLD);
171
    }
172 47680f67 Thomas Schöpping
  } while ((stable_counter* QEI_POLL_INTERVAL_MS < MILLISECONDS_PER_SECOND) && (timeout_counter * MICROSECONDS_PER_MILLISECOND <= data->timeout));
173
  status |= a3906_lld_set_pwm(data->pwm.driver, (wheel == WHEEL_LEFT) ?
174
                               ((direction == DIRECTION_FORWARD) ? data->pwm.channel.left_forward : data->pwm.channel.left_backward) :
175
                               ((direction == DIRECTION_FORWARD) ? data->pwm.channel.right_forward : data->pwm.channel.right_backward),
176
                             APAL_PWM_WIDTH_OFF);
177
  status |= (timeout_counter * MICROSECONDS_PER_MILLISECOND > data->timeout) ? APAL_STATUS_TIMEOUT : 0x00;
178
179
  // report results
180
  if (status == APAL_STATUS_SUCCESS) {
181 4c72a54c Thomas Schöpping
    aosTestPassed(stream, result);
182 47680f67 Thomas Schöpping
    const float tps = qei_increments[1] * ((float)MILLISECONDS_PER_SECOND / (float)QEI_POLL_INTERVAL_MS);
183
    const float rpm = tps * SECONDS_PER_MINUTE / (float)data->qei.increments_per_revolution;
184
    const float velocity = tps / (float)data->qei.increments_per_revolution * ((wheel == WHEEL_LEFT) ? data->wheel_diameter.left : data->wheel_diameter.right) * acos(-1);
185
    chprintf(stream, "\t%f tps\n", tps);
186
    chprintf(stream, "\t%f RPM\n", rpm);
187
    chprintf(stream, "\t%f m/s\n", velocity);
188
    chprintf(stream, "\n");
189
  }
190
  else {
191 4c72a54c Thomas Schöpping
    aosTestFailedMsg(stream, result, "0x%08X\n", status);
192 47680f67 Thomas Schöpping
  }
193
}
194
195 f3ac1c96 Thomas Schöpping
/******************************************************************************/
196
/* EXPORTED FUNCTIONS                                                         */
197
/******************************************************************************/
198
199 e545e620 Thomas Schöpping
/**
200 4c72a54c Thomas Schöpping
 * @brief   A3905 test function.
201 e545e620 Thomas Schöpping
 *
202
 * @param[in] stream  Stream for input/output.
203 4c72a54c Thomas Schöpping
 * @param[in] test    Test object.
204 e545e620 Thomas Schöpping
 *
205 4c72a54c Thomas Schöpping
 * @return            Test result value.
206 e545e620 Thomas Schöpping
 */
207 4c72a54c Thomas Schöpping
aos_testresult_t aosTestA3906Func(BaseSequentialStream* stream, const aos_test_t* test)
208 e545e620 Thomas Schöpping
{
209 4c72a54c Thomas Schöpping
  aosDbgCheck((test->data != NULL) &&
210
              (((aos_test_a3906data_t*)test->data)->driver != NULL) &&
211
              (((aos_test_a3906data_t*)test->data)->pwm.driver != NULL) &&
212
              (((aos_test_a3906data_t*)test->data)->qei.left != NULL) &&
213
              (((aos_test_a3906data_t*)test->data)->qei.right != NULL));
214 e545e620 Thomas Schöpping
215
216
217
  // local variables
218 4c72a54c Thomas Schöpping
  aos_testresult_t result = {0, 0};
219 b23ca7cc Thomas Schöpping
  int32_t status;
220 e545e620 Thomas Schöpping
  a3906_lld_power_t power_state;
221
  apalQEICount_t qei_count[2][2];
222
  uint32_t timeout_counter;
223
  uint32_t stable_counter;
224
225
  chprintf(stream, "enable power...\n");
226
  power_state = A3906_LLD_POWER_ON;
227 4c72a54c Thomas Schöpping
  status = a3906_lld_set_power(((aos_test_a3906data_t*)test->data)->driver, power_state);
228
  status |= a3906_lld_get_power(((aos_test_a3906data_t*)test->data)->driver, &power_state);
229 e545e620 Thomas Schöpping
  status |= (power_state != A3906_LLD_POWER_ON) ? 0x10 : 0x00;
230
  if (status == APAL_STATUS_SUCCESS) {
231 4c72a54c Thomas Schöpping
    aosTestPassed(stream, &result);
232 e545e620 Thomas Schöpping
  } else {
233 4c72a54c Thomas Schöpping
    aosTestFailedMsg(stream, &result, "0x%08X\n", status);
234 e545e620 Thomas Schöpping
  }
235
236 4c72a54c Thomas Schöpping
  _wheelDirectionTest(stream, (aos_test_a3906data_t*)test->data, WHEEL_LEFT, DIRECTION_FORWARD, &result);
237 e545e620 Thomas Schöpping
238 4c72a54c Thomas Schöpping
  _wheelDirectionTest(stream, (aos_test_a3906data_t*)test->data, WHEEL_RIGHT, DIRECTION_FORWARD, &result);
239 e545e620 Thomas Schöpping
240 4c72a54c Thomas Schöpping
  _wheelDirectionTest(stream, (aos_test_a3906data_t*)test->data, WHEEL_LEFT, DIRECTION_BACKWARD, &result);
241 47680f67 Thomas Schöpping
242 4c72a54c Thomas Schöpping
  _wheelDirectionTest(stream, (aos_test_a3906data_t*)test->data, WHEEL_RIGHT, DIRECTION_BACKWARD, &result);
243 e545e620 Thomas Schöpping
244 4c72a54c Thomas Schöpping
  _wheelSpeedTest(stream, (aos_test_a3906data_t*)test->data, WHEEL_LEFT, DIRECTION_FORWARD, &result);
245 47680f67 Thomas Schöpping
246 4c72a54c Thomas Schöpping
  _wheelSpeedTest(stream, (aos_test_a3906data_t*)test->data, WHEEL_RIGHT, DIRECTION_FORWARD, &result);
247 47680f67 Thomas Schöpping
248 4c72a54c Thomas Schöpping
  _wheelSpeedTest(stream, (aos_test_a3906data_t*)test->data, WHEEL_LEFT, DIRECTION_BACKWARD, &result);
249 47680f67 Thomas Schöpping
250 4c72a54c Thomas Schöpping
  _wheelSpeedTest(stream, (aos_test_a3906data_t*)test->data, WHEEL_RIGHT, DIRECTION_BACKWARD, &result);
251 e545e620 Thomas Schöpping
252 a193bcf1 Thomas Schöpping
  chprintf(stream, "disable power...\n");
253 e545e620 Thomas Schöpping
  power_state = A3906_LLD_POWER_OFF;
254 4c72a54c Thomas Schöpping
  status = a3906_lld_set_power(((aos_test_a3906data_t*)test->data)->driver, power_state);
255
  status |= a3906_lld_get_power(((aos_test_a3906data_t*)test->data)->driver, &power_state);
256 e545e620 Thomas Schöpping
  status |= (power_state != A3906_LLD_POWER_OFF) ? 0x10 : 0x00;
257
  qei_count[WHEEL_LEFT][0] = 0;
258
  qei_count[WHEEL_LEFT][1] = 0;
259
  qei_count[WHEEL_RIGHT][0] = 0;
260
  qei_count[WHEEL_RIGHT][1] = 0;
261
  timeout_counter = 0;
262
  stable_counter = 0;
263
  do {
264 4c72a54c Thomas Schöpping
    status |= apalQEIGetPosition(((aos_test_a3906data_t*)test->data)->qei.left, &qei_count[WHEEL_LEFT][0]);
265
    status |= apalQEIGetPosition(((aos_test_a3906data_t*)test->data)->qei.right, &qei_count[WHEEL_RIGHT][0]);
266 e545e620 Thomas Schöpping
    aosThdMSleep(1);
267 4c72a54c Thomas Schöpping
    status |= apalQEIGetPosition(((aos_test_a3906data_t*)test->data)->qei.left, &qei_count[WHEEL_LEFT][1]);
268
    status |= apalQEIGetPosition(((aos_test_a3906data_t*)test->data)->qei.right, &qei_count[WHEEL_RIGHT][1]);
269 e545e620 Thomas Schöpping
    ++timeout_counter;
270
    stable_counter = (qei_count[WHEEL_LEFT][0] == qei_count[WHEEL_LEFT][1] && qei_count[WHEEL_RIGHT][0] == qei_count[WHEEL_RIGHT][1]) ? stable_counter+1 : 0;
271 4c72a54c Thomas Schöpping
  } while((stable_counter < 100) && (timeout_counter * MICROSECONDS_PER_MILLISECOND <= ((aos_test_a3906data_t*)test->data)->timeout));
272
  status |= (timeout_counter * MICROSECONDS_PER_MILLISECOND > ((aos_test_a3906data_t*)test->data)->timeout) ? APAL_STATUS_TIMEOUT : 0x00;
273 e545e620 Thomas Schöpping
  if (status == APAL_STATUS_SUCCESS) {
274 4c72a54c Thomas Schöpping
    aosTestPassed(stream, &result);
275 e545e620 Thomas Schöpping
  } else {
276 4c72a54c Thomas Schöpping
    aosTestFailedMsg(stream, &result, "0x%08X\n", status);
277 e545e620 Thomas Schöpping
  }
278
279
  // stop the PWM
280 4c72a54c Thomas Schöpping
  a3906_lld_set_pwm(((aos_test_a3906data_t*)test->data)->pwm.driver, ((aos_test_a3906data_t*)test->data)->pwm.channel.left_forward, APAL_PWM_WIDTH_OFF);
281
  a3906_lld_set_pwm(((aos_test_a3906data_t*)test->data)->pwm.driver, ((aos_test_a3906data_t*)test->data)->pwm.channel.left_backward, APAL_PWM_WIDTH_OFF);
282
  a3906_lld_set_pwm(((aos_test_a3906data_t*)test->data)->pwm.driver, ((aos_test_a3906data_t*)test->data)->pwm.channel.right_forward, APAL_PWM_WIDTH_OFF);
283
  a3906_lld_set_pwm(((aos_test_a3906data_t*)test->data)->pwm.driver, ((aos_test_a3906data_t*)test->data)->pwm.channel.right_backward, APAL_PWM_WIDTH_OFF);
284 e545e620 Thomas Schöpping
285 4c72a54c Thomas Schöpping
  aosTestInfoMsg(stream,"driver object memory footprint: %u bytes\n", sizeof(A3906Driver));
286 e545e620 Thomas Schöpping
287
  return result;
288
}
289
290 4c72a54c Thomas Schöpping
#endif /* (AMIROOS_CFG_TESTS_ENABLE == true) */