Statistics
| Branch: | Tag: | Revision:

amiro-os / unittests / periphery-lld / src / ut_alld_a3906.c @ f3ac1c96

History | View | Annotate | Download (14.1 KB)

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

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
#include <ut_alld_a3906.h>
20

    
21
#if ((AMIROOS_CFG_TESTS_ENABLE == true) && defined(AMIROLLD_CFG_USE_A3906)) || defined(__DOXYGEN__)
22

    
23
#include <aos_debug.h>
24
#include <chprintf.h>
25
#include <alld_a3906.h>
26
#include <aos_thread.h>
27
#include <stdlib.h>
28
#include <math.h>
29

    
30
/******************************************************************************/
31
/* LOCAL DEFINITIONS                                                          */
32
/******************************************************************************/
33

    
34
/**
35
 * @brief   Interval to poll QEI in certain tests.
36
 */
37
#define QEI_POLL_INTERVAL_MS          100
38

    
39
/**
40
 * @brief   Threshold for QEI differences.
41
 * @details Differences smaller than or equal to this value are neglected (interpreted as zero).
42
 *          The value can be interpreted as encoder ticks per second (tps).
43
 * @note    The expected value is about 7000 tps and a jitter of up to ±2% is ok.
44
 */
45
#define QEI_DIFF_THRESHOLD            (apalQEICount_t)(7000 * 0.02f)
46

    
47
/******************************************************************************/
48
/* EXPORTED VARIABLES                                                         */
49
/******************************************************************************/
50

    
51
/******************************************************************************/
52
/* LOCAL TYPES                                                                */
53
/******************************************************************************/
54

    
55
/**
56
 * @brief   Enumerator to distinguish between left and right wheel.
57
 */
58
typedef enum {
59
  WHEEL_LEFT    = 0,  /**< left wheel identifier */
60
  WHEEL_RIGHT   = 1,  /**< right wheel identifier */
61
} wheel_t;
62

    
63
/**
64
 * @brief   Enumerator to distinguish directions.
65
 */
66
typedef enum {
67
  DIRECTION_FORWARD   = 0,  /**< forward direction identifier */
68
  DIRECTION_BACKWARD  = 1,  /**< backward direction identifier */
69
} direction_t;
70

    
71
/******************************************************************************/
72
/* LOCAL VARIABLES                                                            */
73
/******************************************************************************/
74

    
75
/******************************************************************************/
76
/* LOCAL FUNCTIONS                                                            */
77
/******************************************************************************/
78

    
79
/**
80
 * @brief   helper function to test each wheel and direction separately.
81
 *
82
 * @param[in] stream      Stream for input/output.
83
 * @param[in] data        Unit test meta data.
84
 * @param[in] wheel       Wheel to test.
85
 * @param[in] direction   Direction to test.
86
 * @param[in,out] result  Result variable to modify.
87
 */
88
void _wheelDirectionTest(BaseSequentialStream* stream, ut_a3906data_t* data, wheel_t wheel, direction_t direction, aos_utresult_t* result)
89
{
90
  // local variables
91
  uint32_t status;
92
  bool qei_valid;
93
  apalQEICount_t qei_count[2];
94
  apalQEIDirection_t qei_direction;
95
  uint32_t timeout_counter;
96

    
97
  chprintf(stream, "%s wheel %s...\n", (wheel == WHEEL_LEFT) ? "left" : "right", (direction == DIRECTION_FORWARD) ? "forward" : "backward");
98
  qei_valid = false;
99
  status = apalQEIGetPosition((wheel == WHEEL_LEFT) ? data->qei.left : data->qei.right, &qei_count[0]);
100
  // increase PWM incrementally and read QEI data
101
  for (apalPWMwidth_t pwm_width = APAL_PWM_WIDTH_MIN; pwm_width < APAL_PWM_WIDTH_MAX; ++pwm_width) {
102
    status |= a3906_lld_set_pwm(data->pwm.driver, (wheel == WHEEL_LEFT) ?
103
                                  ((direction == DIRECTION_FORWARD) ? data->pwm.channel.left_forward : data->pwm.channel.left_backward) :
104
                                  ((direction == DIRECTION_FORWARD) ? data->pwm.channel.right_forward : data->pwm.channel.right_backward),
105
                                pwm_width);
106
    status |= apalQEIGetPosition((wheel == WHEEL_LEFT) ? data->qei.left : data->qei.right, &qei_count[1]);
107
    qei_valid = qei_valid || (qei_count[0] != qei_count[1]);
108
    aosThdUSleep(5 * MICROSECONDS_PER_SECOND / (APAL_PWM_WIDTH_MAX - APAL_PWM_WIDTH_MIN));
109
    qei_count[0] = qei_count[1];
110
  }
111
  status |= qei_valid ? 0x00 : 0x10;
112
  status |= apalQEIGetDirection((wheel == WHEEL_LEFT) ? data->qei.left : data->qei.right, &qei_direction);
113
  status |= (qei_direction == ((direction == DIRECTION_FORWARD) ? APAL_QEI_DIRECTION_UP : APAL_QEI_DIRECTION_DOWN)) ? 0x00 : 0x20;
114

    
115
  // let the wheel spin free until it stops
116
  status |= a3906_lld_set_pwm(data->pwm.driver, (wheel == WHEEL_LEFT) ?
117
                                ((direction == DIRECTION_FORWARD) ? data->pwm.channel.left_forward : data->pwm.channel.left_backward) :
118
                                ((direction == DIRECTION_FORWARD) ? data->pwm.channel.right_forward : data->pwm.channel.right_backward),
119
                              0);
120
  qei_count[0] = 0;
121
  qei_count[1] = 0;
122
  timeout_counter = 0;
123
  do {
124
    status |= apalQEIGetPosition((wheel == WHEEL_LEFT) ? data->qei.left : data->qei.right, &qei_count[0]);
125
    aosThdMSleep(1);
126
    status |= apalQEIGetPosition((wheel == WHEEL_LEFT) ? data->qei.left : data->qei.right, &qei_count[1]);
127
    ++timeout_counter;
128
  } while ((qei_count[0] != qei_count[1]) && (timeout_counter * MICROSECONDS_PER_MILLISECOND <= data->timeout));
129
  status |= (timeout_counter * MICROSECONDS_PER_MILLISECOND > data->timeout) ? APAL_STATUS_TIMEOUT : 0x00;
130

    
131
  // report result
132
  if (status == APAL_STATUS_SUCCESS) {
133
    aosUtPassed(stream, result);
134
  } else {
135
    aosUtFailedMsg(stream, result, "0x%08X\n", status);
136
  }
137

    
138
  return;
139
}
140

    
141
void _wheelSpeedTest(BaseSequentialStream* stream, ut_a3906data_t* data, wheel_t wheel, direction_t direction, aos_utresult_t* result)
142
{
143
  // local variables
144
  uint32_t status;
145
  apalQEICount_t qei_range;
146
  apalQEICount_t qei_count[2] = {0};
147
  apalQEICount_t qei_increments[2] = {0};
148
  apalQEICount_t qei_increments_diff = 0;
149
  uint32_t timeout_counter = 0;
150
  uint32_t stable_counter = 0;
151

    
152
  chprintf(stream, "%s wheel full speed %s...\n", (wheel == WHEEL_LEFT) ? "left" : "right", (direction == DIRECTION_FORWARD) ? "forward" : "backward");
153
  // spin up the wheel with full speed
154
  status = apalQEIGetRange((wheel == WHEEL_LEFT) ? data->qei.left : data->qei.right, &qei_range);
155
  status |= a3906_lld_set_pwm(data->pwm.driver, (wheel == WHEEL_LEFT) ?
156
                               ((direction == DIRECTION_FORWARD) ? data->pwm.channel.left_forward : data->pwm.channel.left_backward) :
157
                               ((direction == DIRECTION_FORWARD) ? data->pwm.channel.right_forward : data->pwm.channel.right_backward),
158
                             APAL_PWM_WIDTH_MAX);
159
  aosThdMSleep(100);
160
  do {
161
    // read QEI data to determine speed
162
    status |= apalQEIGetPosition((wheel == WHEEL_LEFT) ? data->qei.left : data->qei.right, &qei_count[0]);
163
    aosThdMSleep(QEI_POLL_INTERVAL_MS);
164
    status |= apalQEIGetPosition((wheel == WHEEL_LEFT) ? data->qei.left : data->qei.right, &qei_count[1]);
165
    timeout_counter += QEI_POLL_INTERVAL_MS;
166
    qei_increments[0] = qei_increments[1];
167
    qei_increments[1] = (direction == DIRECTION_FORWARD) ?
168
                          ((qei_count[1] > qei_count[0]) ? (qei_count[1] - qei_count[0]) : (qei_count[1] + (qei_range - qei_count[0]))) :
169
                          ((qei_count[0] > qei_count[1]) ? (qei_count[0] - qei_count[1]) : (qei_count[0] + (qei_range - qei_count[1])));
170
    qei_increments_diff = abs((int32_t)qei_increments[0] - (int32_t)qei_increments[1]) * ((float)MILLISECONDS_PER_SECOND / (float)QEI_POLL_INTERVAL_MS);
171
    stable_counter = ((qei_increments[0] != 0 || qei_increments[1] != 0) && qei_increments_diff <= QEI_DIFF_THRESHOLD) ? stable_counter+1 : 0;
172
    if (qei_increments[0] != 0 && stable_counter == 0) {
173
      chprintf(stream, "\tunstable speed? jitter of %u tps is above threshold (%u tps).\n", qei_increments_diff, QEI_DIFF_THRESHOLD);
174
    }
175
  } while ((stable_counter* QEI_POLL_INTERVAL_MS < MILLISECONDS_PER_SECOND) && (timeout_counter * MICROSECONDS_PER_MILLISECOND <= data->timeout));
176
  status |= a3906_lld_set_pwm(data->pwm.driver, (wheel == WHEEL_LEFT) ?
177
                               ((direction == DIRECTION_FORWARD) ? data->pwm.channel.left_forward : data->pwm.channel.left_backward) :
178
                               ((direction == DIRECTION_FORWARD) ? data->pwm.channel.right_forward : data->pwm.channel.right_backward),
179
                             APAL_PWM_WIDTH_OFF);
180
  status |= (timeout_counter * MICROSECONDS_PER_MILLISECOND > data->timeout) ? APAL_STATUS_TIMEOUT : 0x00;
181

    
182
  // report results
183
  if (status == APAL_STATUS_SUCCESS) {
184
    aosUtPassed(stream, result);
185
    const float tps = qei_increments[1] * ((float)MILLISECONDS_PER_SECOND / (float)QEI_POLL_INTERVAL_MS);
186
    const float rpm = tps * SECONDS_PER_MINUTE / (float)data->qei.increments_per_revolution;
187
    const float velocity = tps / (float)data->qei.increments_per_revolution * ((wheel == WHEEL_LEFT) ? data->wheel_diameter.left : data->wheel_diameter.right) * acos(-1);
188
    chprintf(stream, "\t%f tps\n", tps);
189
    chprintf(stream, "\t%f RPM\n", rpm);
190
    chprintf(stream, "\t%f m/s\n", velocity);
191
    chprintf(stream, "\n");
192
  }
193
  else {
194
    aosUtFailedMsg(stream, result, "0x%08X\n", status);
195
  }
196
}
197

    
198
/******************************************************************************/
199
/* EXPORTED FUNCTIONS                                                         */
200
/******************************************************************************/
201

    
202
/**
203
 * @brief   A3905 unit test function.
204
 *
205
 * @param[in] stream  Stream for input/output.
206
 * @param[in] ut      Unit test object.
207
 *
208
 * @return            Unit test result value.
209
 */
210
aos_utresult_t utAlldA3906Func(BaseSequentialStream* stream, aos_unittest_t* ut)
211
{
212
  aosDbgCheck((ut->data != NULL) &&
213
              (((ut_a3906data_t*)ut->data)->driver != NULL) &&
214
              (((ut_a3906data_t*)ut->data)->pwm.driver != NULL) &&
215
              (((ut_a3906data_t*)ut->data)->qei.left != NULL) &&
216
              (((ut_a3906data_t*)ut->data)->qei.right != NULL));
217

    
218

    
219

    
220
  // local variables
221
  aos_utresult_t result = {0, 0};
222
  uint32_t status = 0;
223
  a3906_lld_power_t power_state;
224
  apalQEICount_t qei_count[2][2];
225
  uint32_t timeout_counter;
226
  uint32_t stable_counter;
227

    
228
  chprintf(stream, "enable power...\n");
229
  power_state = A3906_LLD_POWER_ON;
230
  status = a3906_lld_set_power(((ut_a3906data_t*)ut->data)->driver, power_state);
231
  status |= a3906_lld_get_power(((ut_a3906data_t*)ut->data)->driver, &power_state);
232
  status |= (power_state != A3906_LLD_POWER_ON) ? 0x10 : 0x00;
233
  if (status == APAL_STATUS_SUCCESS) {
234
    aosUtPassed(stream, &result);
235
  } else {
236
    aosUtFailedMsg(stream, &result, "0x%08X\n", status);
237
  }
238

    
239
  _wheelDirectionTest(stream, (ut_a3906data_t*)ut->data, WHEEL_LEFT, DIRECTION_FORWARD, &result);
240

    
241
  _wheelDirectionTest(stream, (ut_a3906data_t*)ut->data, WHEEL_RIGHT, DIRECTION_FORWARD, &result);
242

    
243
  _wheelDirectionTest(stream, (ut_a3906data_t*)ut->data, WHEEL_LEFT, DIRECTION_BACKWARD, &result);
244

    
245
  _wheelDirectionTest(stream, (ut_a3906data_t*)ut->data, WHEEL_RIGHT, DIRECTION_BACKWARD, &result);
246

    
247
  _wheelSpeedTest(stream, (ut_a3906data_t*)ut->data, WHEEL_LEFT, DIRECTION_FORWARD, &result);
248

    
249
  _wheelSpeedTest(stream, (ut_a3906data_t*)ut->data, WHEEL_RIGHT, DIRECTION_FORWARD, &result);
250

    
251
  _wheelSpeedTest(stream, (ut_a3906data_t*)ut->data, WHEEL_LEFT, DIRECTION_BACKWARD, &result);
252

    
253
  _wheelSpeedTest(stream, (ut_a3906data_t*)ut->data, WHEEL_RIGHT, DIRECTION_BACKWARD, &result);
254

    
255
  chprintf(stream, "disable power... \n");
256
  power_state = A3906_LLD_POWER_OFF;
257
  status = a3906_lld_set_power(((ut_a3906data_t*)ut->data)->driver, power_state);
258
  status |= a3906_lld_get_power(((ut_a3906data_t*)ut->data)->driver, &power_state);
259
  status |= (power_state != A3906_LLD_POWER_OFF) ? 0x10 : 0x00;
260
  qei_count[WHEEL_LEFT][0] = 0;
261
  qei_count[WHEEL_LEFT][1] = 0;
262
  qei_count[WHEEL_RIGHT][0] = 0;
263
  qei_count[WHEEL_RIGHT][1] = 0;
264
  timeout_counter = 0;
265
  stable_counter = 0;
266
  do {
267
    status |= apalQEIGetPosition(((ut_a3906data_t*)ut->data)->qei.left, &qei_count[WHEEL_LEFT][0]);
268
    status |= apalQEIGetPosition(((ut_a3906data_t*)ut->data)->qei.right, &qei_count[WHEEL_RIGHT][0]);
269
    aosThdMSleep(1);
270
    status |= apalQEIGetPosition(((ut_a3906data_t*)ut->data)->qei.left, &qei_count[WHEEL_LEFT][1]);
271
    status |= apalQEIGetPosition(((ut_a3906data_t*)ut->data)->qei.right, &qei_count[WHEEL_RIGHT][1]);
272
    ++timeout_counter;
273
    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;
274
  } while((stable_counter < 100) && (timeout_counter * MICROSECONDS_PER_MILLISECOND <= ((ut_a3906data_t*)ut->data)->timeout));
275
  status |= (timeout_counter * MICROSECONDS_PER_MILLISECOND > ((ut_a3906data_t*)ut->data)->timeout) ? APAL_STATUS_TIMEOUT : 0x00;
276
  if (status == APAL_STATUS_SUCCESS) {
277
    aosUtPassed(stream, &result);
278
  } else {
279
    aosUtFailedMsg(stream, &result, "0x%08X\n", status);
280
  }
281

    
282
  // stop the PWM
283
  a3906_lld_set_pwm(((ut_a3906data_t*)ut->data)->pwm.driver, ((ut_a3906data_t*)ut->data)->pwm.channel.left_forward, APAL_PWM_WIDTH_OFF);
284
  a3906_lld_set_pwm(((ut_a3906data_t*)ut->data)->pwm.driver, ((ut_a3906data_t*)ut->data)->pwm.channel.left_backward, APAL_PWM_WIDTH_OFF);
285
  a3906_lld_set_pwm(((ut_a3906data_t*)ut->data)->pwm.driver, ((ut_a3906data_t*)ut->data)->pwm.channel.right_forward, APAL_PWM_WIDTH_OFF);
286
  a3906_lld_set_pwm(((ut_a3906data_t*)ut->data)->pwm.driver, ((ut_a3906data_t*)ut->data)->pwm.channel.right_backward, APAL_PWM_WIDTH_OFF);
287

    
288
  aosUtInfoMsg(stream,"driver object memory footprint: %u bytes\n", sizeof(A3906Driver));
289

    
290
  return result;
291
}
292

    
293
#endif /* (AMIROOS_CFG_TESTS_ENABLE == true) && defined(AMIROLLD_CFG_USE_A3906) */