Statistics
| Branch: | Tag: | Revision:

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

History | View | Annotate | Download (12.7 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
 * @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
 * @brief   Enumerator to distinguish between left and right wheel.
45
 */
46
typedef enum {
47
  WHEEL_LEFT    = 0,  /**< left wheel identifier */
48
  WHEEL_RIGHT   = 1,  /**< right wheel identifier */
49
} wheel_t;
50

    
51
/**
52
 * @brief   Enumerator to distinguish directions.
53
 */
54
typedef enum {
55
  DIRECTION_FORWARD   = 0,  /**< forward direction identifier */
56
  DIRECTION_BACKWARD  = 1,  /**< backward direction identifier */
57
} direction_t;
58

    
59
/**
60
 * @brief   helper function to test each wheel and direction separately.
61
 *
62
 * @param[in] stream      Stream for input/output.
63
 * @param[in] data        Unit test meta data.
64
 * @param[in] wheel       Wheel to test.
65
 * @param[in] direction   Direction to test.
66
 * @param[in,out] result  Result variable to modify.
67
 */
68
void _wheelDirectionTest(BaseSequentialStream* stream, ut_a3906data_t* data, wheel_t wheel, direction_t direction, aos_utresult_t* result)
69
{
70
  // local variables
71
  uint32_t status;
72
  bool qei_valid;
73
  apalQEICount_t qei_count[2];
74
  apalQEIDirection_t qei_direction;
75
  uint32_t timeout_counter;
76

    
77
  chprintf(stream, "%s wheel %s...\n", (wheel == WHEEL_LEFT) ? "left" : "right", (direction == DIRECTION_FORWARD) ? "forward" : "backward");
78
  qei_valid = false;
79
  status = apalQEIGetPosition((wheel == WHEEL_LEFT) ? data->qei.left : data->qei.right, &qei_count[0]);
80
  // increase PWM incrementally and read QEI data
81
  for (apalPWMwidth_t pwm_width = APAL_PWM_WIDTH_MIN; pwm_width < APAL_PWM_WIDTH_MAX; ++pwm_width) {
82
    status |= a3906_lld_set_pwm(data->pwm.driver, (wheel == WHEEL_LEFT) ?
83
                                  ((direction == DIRECTION_FORWARD) ? data->pwm.channel.left_forward : data->pwm.channel.left_backward) :
84
                                  ((direction == DIRECTION_FORWARD) ? data->pwm.channel.right_forward : data->pwm.channel.right_backward),
85
                                pwm_width);
86
    status |= apalQEIGetPosition((wheel == WHEEL_LEFT) ? data->qei.left : data->qei.right, &qei_count[1]);
87
    qei_valid = qei_valid || (qei_count[0] != qei_count[1]);
88
    aosThdUSleep(5 * MICROSECONDS_PER_SECOND / (APAL_PWM_WIDTH_MAX - APAL_PWM_WIDTH_MIN));
89
    qei_count[0] = qei_count[1];
90
  }
91
  status |= qei_valid ? 0x00 : 0x10;
92
  status |= apalQEIGetDirection((wheel == WHEEL_LEFT) ? data->qei.left : data->qei.right, &qei_direction);
93
  status |= (qei_direction == ((direction == DIRECTION_FORWARD) ? APAL_QEI_DIRECTION_UP : APAL_QEI_DIRECTION_DOWN)) ? 0x00 : 0x20;
94

    
95
  // let the wheel spin free until it stops
96
  status |= a3906_lld_set_pwm(data->pwm.driver, (wheel == WHEEL_LEFT) ?
97
                                ((direction == DIRECTION_FORWARD) ? data->pwm.channel.left_forward : data->pwm.channel.left_backward) :
98
                                ((direction == DIRECTION_FORWARD) ? data->pwm.channel.right_forward : data->pwm.channel.right_backward),
99
                              0);
100
  qei_count[0] = 0;
101
  qei_count[1] = 0;
102
  timeout_counter = 0;
103
  do {
104
    status |= apalQEIGetPosition((wheel == WHEEL_LEFT) ? data->qei.left : data->qei.right, &qei_count[0]);
105
    aosThdMSleep(1);
106
    status |= apalQEIGetPosition((wheel == WHEEL_LEFT) ? data->qei.left : data->qei.right, &qei_count[1]);
107
    ++timeout_counter;
108
  } while ((qei_count[0] != qei_count[1]) && (timeout_counter * MICROSECONDS_PER_MILLISECOND <= data->timeout));
109
  status |= (timeout_counter * MICROSECONDS_PER_MILLISECOND > data->timeout) ? APAL_STATUS_TIMEOUT : 0x00;
110

    
111
  // report result
112
  if (status == APAL_STATUS_SUCCESS) {
113
    aosUtPassed(stream, result);
114
  } else {
115
    aosUtFailedMsg(stream, result, "0x%08X\n", status);
116
  }
117

    
118
  return;
119
}
120

    
121
void _wheelSpeedTest(BaseSequentialStream* stream, ut_a3906data_t* data, wheel_t wheel, direction_t direction, aos_utresult_t* result)
122
{
123
  // local variables
124
  uint32_t status;
125
  apalQEICount_t qei_range;
126
  apalQEICount_t qei_count[2] = {0};
127
  apalQEICount_t qei_increments[2] = {0};
128
  apalQEICount_t qei_increments_diff = 0;
129
  uint32_t timeout_counter = 0;
130
  uint32_t stable_counter = 0;
131

    
132
  chprintf(stream, "%s wheel full speed %s...\n", (wheel == WHEEL_LEFT) ? "left" : "right", (direction == DIRECTION_FORWARD) ? "forward" : "backward");
133
  // spin up the wheel with full speed
134
  status = apalQEIGetRange((wheel == WHEEL_LEFT) ? data->qei.left : data->qei.right, &qei_range);
135
  status |= a3906_lld_set_pwm(data->pwm.driver, (wheel == WHEEL_LEFT) ?
136
                               ((direction == DIRECTION_FORWARD) ? data->pwm.channel.left_forward : data->pwm.channel.left_backward) :
137
                               ((direction == DIRECTION_FORWARD) ? data->pwm.channel.right_forward : data->pwm.channel.right_backward),
138
                             APAL_PWM_WIDTH_MAX);
139
  aosThdMSleep(100);
140
  do {
141
    // read QEI data to determine speed
142
    status |= apalQEIGetPosition((wheel == WHEEL_LEFT) ? data->qei.left : data->qei.right, &qei_count[0]);
143
    aosThdMSleep(QEI_POLL_INTERVAL_MS);
144
    status |= apalQEIGetPosition((wheel == WHEEL_LEFT) ? data->qei.left : data->qei.right, &qei_count[1]);
145
    timeout_counter += QEI_POLL_INTERVAL_MS;
146
    qei_increments[0] = qei_increments[1];
147
    qei_increments[1] = (direction == DIRECTION_FORWARD) ?
148
                          ((qei_count[1] > qei_count[0]) ? (qei_count[1] - qei_count[0]) : (qei_count[1] + (qei_range - qei_count[0]))) :
149
                          ((qei_count[0] > qei_count[1]) ? (qei_count[0] - qei_count[1]) : (qei_count[0] + (qei_range - qei_count[1])));
150
    qei_increments_diff = abs((int32_t)qei_increments[0] - (int32_t)qei_increments[1]) * ((float)MILLISECONDS_PER_SECOND / (float)QEI_POLL_INTERVAL_MS);
151
    stable_counter = ((qei_increments[0] != 0 || qei_increments[1] != 0) && qei_increments_diff <= QEI_DIFF_THRESHOLD) ? stable_counter+1 : 0;
152
    if (qei_increments[0] != 0 && stable_counter == 0) {
153
      chprintf(stream, "\tunstable speed? jitter of %u tps is above threshold (%u tps).\n", qei_increments_diff, QEI_DIFF_THRESHOLD);
154
    }
155
  } while ((stable_counter* QEI_POLL_INTERVAL_MS < MILLISECONDS_PER_SECOND) && (timeout_counter * MICROSECONDS_PER_MILLISECOND <= data->timeout));
156
  status |= a3906_lld_set_pwm(data->pwm.driver, (wheel == WHEEL_LEFT) ?
157
                               ((direction == DIRECTION_FORWARD) ? data->pwm.channel.left_forward : data->pwm.channel.left_backward) :
158
                               ((direction == DIRECTION_FORWARD) ? data->pwm.channel.right_forward : data->pwm.channel.right_backward),
159
                             APAL_PWM_WIDTH_OFF);
160
  status |= (timeout_counter * MICROSECONDS_PER_MILLISECOND > data->timeout) ? APAL_STATUS_TIMEOUT : 0x00;
161

    
162
  // report results
163
  if (status == APAL_STATUS_SUCCESS) {
164
    aosUtPassed(stream, result);
165
    const float tps = qei_increments[1] * ((float)MILLISECONDS_PER_SECOND / (float)QEI_POLL_INTERVAL_MS);
166
    const float rpm = tps * SECONDS_PER_MINUTE / (float)data->qei.increments_per_revolution;
167
    const float velocity = tps / (float)data->qei.increments_per_revolution * ((wheel == WHEEL_LEFT) ? data->wheel_diameter.left : data->wheel_diameter.right) * acos(-1);
168
    chprintf(stream, "\t%f tps\n", tps);
169
    chprintf(stream, "\t%f RPM\n", rpm);
170
    chprintf(stream, "\t%f m/s\n", velocity);
171
    chprintf(stream, "\n");
172
  }
173
  else {
174
    aosUtFailedMsg(stream, result, "0x%08X\n", status);
175
  }
176
}
177

    
178
/**
179
 * @brief   A3905 unit test function.
180
 *
181
 * @param[in] stream  Stream for input/output.
182
 * @param[in] ut      Unit test object.
183
 *
184
 * @return            Unit test result value.
185
 */
186
aos_utresult_t utAlldA3906Func(BaseSequentialStream* stream, aos_unittest_t* ut)
187
{
188
  aosDbgCheck((ut->data != NULL) &&
189
              (((ut_a3906data_t*)ut->data)->driver != NULL) &&
190
              (((ut_a3906data_t*)ut->data)->pwm.driver != NULL) &&
191
              (((ut_a3906data_t*)ut->data)->qei.left != NULL) &&
192
              (((ut_a3906data_t*)ut->data)->qei.right != NULL));
193

    
194

    
195

    
196
  // local variables
197
  aos_utresult_t result = {0, 0};
198
  uint32_t status = 0;
199
  a3906_lld_power_t power_state;
200
  apalQEICount_t qei_count[2][2];
201
  uint32_t timeout_counter;
202
  uint32_t stable_counter;
203

    
204
  chprintf(stream, "enable power...\n");
205
  power_state = A3906_LLD_POWER_ON;
206
  status = a3906_lld_set_power(((ut_a3906data_t*)ut->data)->driver, power_state);
207
  status |= a3906_lld_get_power(((ut_a3906data_t*)ut->data)->driver, &power_state);
208
  status |= (power_state != A3906_LLD_POWER_ON) ? 0x10 : 0x00;
209
  if (status == APAL_STATUS_SUCCESS) {
210
    aosUtPassed(stream, &result);
211
  } else {
212
    aosUtFailedMsg(stream, &result, "0x%08X\n", status);
213
  }
214

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

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

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

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

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

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

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

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

    
231
  chprintf(stream, "disable power... \n");
232
  power_state = A3906_LLD_POWER_OFF;
233
  status = a3906_lld_set_power(((ut_a3906data_t*)ut->data)->driver, power_state);
234
  status |= a3906_lld_get_power(((ut_a3906data_t*)ut->data)->driver, &power_state);
235
  status |= (power_state != A3906_LLD_POWER_OFF) ? 0x10 : 0x00;
236
  qei_count[WHEEL_LEFT][0] = 0;
237
  qei_count[WHEEL_LEFT][1] = 0;
238
  qei_count[WHEEL_RIGHT][0] = 0;
239
  qei_count[WHEEL_RIGHT][1] = 0;
240
  timeout_counter = 0;
241
  stable_counter = 0;
242
  do {
243
    status |= apalQEIGetPosition(((ut_a3906data_t*)ut->data)->qei.left, &qei_count[WHEEL_LEFT][0]);
244
    status |= apalQEIGetPosition(((ut_a3906data_t*)ut->data)->qei.right, &qei_count[WHEEL_RIGHT][0]);
245
    aosThdMSleep(1);
246
    status |= apalQEIGetPosition(((ut_a3906data_t*)ut->data)->qei.left, &qei_count[WHEEL_LEFT][1]);
247
    status |= apalQEIGetPosition(((ut_a3906data_t*)ut->data)->qei.right, &qei_count[WHEEL_RIGHT][1]);
248
    ++timeout_counter;
249
    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;
250
  } while((stable_counter < 100) && (timeout_counter * MICROSECONDS_PER_MILLISECOND <= ((ut_a3906data_t*)ut->data)->timeout));
251
  status |= (timeout_counter * MICROSECONDS_PER_MILLISECOND > ((ut_a3906data_t*)ut->data)->timeout) ? APAL_STATUS_TIMEOUT : 0x00;
252
  if (status == APAL_STATUS_SUCCESS) {
253
    aosUtPassed(stream, &result);
254
  } else {
255
    aosUtFailedMsg(stream, &result, "0x%08X\n", status);
256
  }
257

    
258
  // stop the PWM
259
  a3906_lld_set_pwm(((ut_a3906data_t*)ut->data)->pwm.driver, ((ut_a3906data_t*)ut->data)->pwm.channel.left_forward, APAL_PWM_WIDTH_OFF);
260
  a3906_lld_set_pwm(((ut_a3906data_t*)ut->data)->pwm.driver, ((ut_a3906data_t*)ut->data)->pwm.channel.left_backward, APAL_PWM_WIDTH_OFF);
261
  a3906_lld_set_pwm(((ut_a3906data_t*)ut->data)->pwm.driver, ((ut_a3906data_t*)ut->data)->pwm.channel.right_forward, APAL_PWM_WIDTH_OFF);
262
  a3906_lld_set_pwm(((ut_a3906data_t*)ut->data)->pwm.driver, ((ut_a3906data_t*)ut->data)->pwm.channel.right_backward, APAL_PWM_WIDTH_OFF);
263

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

    
266
  return result;
267
}
268

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