Statistics
| Branch: | Tag: | Revision:

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

History | View | Annotate | Download (14 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 <stdlib.h>
24
#include <math.h>
25

    
26
/******************************************************************************/
27
/* LOCAL DEFINITIONS                                                          */
28
/******************************************************************************/
29

    
30
/**
31
 * @brief   Interval to poll QEI in certain tests.
32
 */
33
#define QEI_POLL_INTERVAL_MS          100
34

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

    
43
/******************************************************************************/
44
/* EXPORTED VARIABLES                                                         */
45
/******************************************************************************/
46

    
47
/******************************************************************************/
48
/* LOCAL TYPES                                                                */
49
/******************************************************************************/
50

    
51
/**
52
 * @brief   Enumerator to distinguish between left and right wheel.
53
 */
54
typedef enum {
55
  WHEEL_LEFT    = 0,  /**< left wheel identifier */
56
  WHEEL_RIGHT   = 1,  /**< right wheel identifier */
57
} wheel_t;
58

    
59
/**
60
 * @brief   Enumerator to distinguish directions.
61
 */
62
typedef enum {
63
  DIRECTION_FORWARD   = 0,  /**< forward direction identifier */
64
  DIRECTION_BACKWARD  = 1,  /**< backward direction identifier */
65
} direction_t;
66

    
67
/******************************************************************************/
68
/* LOCAL VARIABLES                                                            */
69
/******************************************************************************/
70

    
71
/******************************************************************************/
72
/* LOCAL FUNCTIONS                                                            */
73
/******************************************************************************/
74

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

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

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

    
127
  // report result
128
  if (status == APAL_STATUS_SUCCESS) {
129
    aosUtPassed(stream, result);
130
  } else {
131
    aosUtFailedMsg(stream, result, "0x%08X\n", status);
132
  }
133

    
134
  return;
135
}
136

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

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

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

    
194
/******************************************************************************/
195
/* EXPORTED FUNCTIONS                                                         */
196
/******************************************************************************/
197

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

    
214

    
215

    
216
  // local variables
217
  aos_utresult_t result = {0, 0};
218
  uint32_t status = 0;
219
  a3906_lld_power_t power_state;
220
  apalQEICount_t qei_count[2][2];
221
  uint32_t timeout_counter;
222
  uint32_t stable_counter;
223

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

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

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

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

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

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

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

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

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

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

    
278
  // stop the PWM
279
  a3906_lld_set_pwm(((ut_a3906data_t*)ut->data)->pwm.driver, ((ut_a3906data_t*)ut->data)->pwm.channel.left_forward, APAL_PWM_WIDTH_OFF);
280
  a3906_lld_set_pwm(((ut_a3906data_t*)ut->data)->pwm.driver, ((ut_a3906data_t*)ut->data)->pwm.channel.left_backward, APAL_PWM_WIDTH_OFF);
281
  a3906_lld_set_pwm(((ut_a3906data_t*)ut->data)->pwm.driver, ((ut_a3906data_t*)ut->data)->pwm.channel.right_forward, APAL_PWM_WIDTH_OFF);
282
  a3906_lld_set_pwm(((ut_a3906data_t*)ut->data)->pwm.driver, ((ut_a3906data_t*)ut->data)->pwm.channel.right_backward, APAL_PWM_WIDTH_OFF);
283

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

    
286
  return result;
287
}
288

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