amiro-os / unittests / periphery-lld / src / ut_alld_A3906_v1.c @ 55a84503
History | View | Annotate | Download (14.104 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 | 2a4dbf93 | Thomas Schöpping | #include <ut_alld_A3906_v1.h> |
| 21 | e545e620 | Thomas Schöpping | |
| 22 | ddf34c3d | Thomas Schöpping | #if ((AMIROOS_CFG_TESTS_ENABLE == true) && defined(AMIROLLD_CFG_A3906) && (AMIROLLD_CFG_A3906 == 1)) || 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 | * @param[in] data Unit test meta data.
|
||
| 81 | * @param[in] wheel Wheel to test.
|
||
| 82 | * @param[in] direction Direction to test.
|
||
| 83 | * @param[in,out] result Result variable to modify.
|
||
| 84 | */
|
||
| 85 | void _wheelDirectionTest(BaseSequentialStream* stream, ut_a3906data_t* data, wheel_t wheel, direction_t direction, aos_utresult_t* result)
|
||
| 86 | {
|
||
| 87 | // local variables
|
||
| 88 | uint32_t status; |
||
| 89 | 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 | status |= a3906_lld_set_pwm(data->pwm.driver, (wheel == WHEEL_LEFT) ? |
||
| 100 | ((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 | aosUtPassed(stream, result); |
||
| 131 | } else {
|
||
| 132 | aosUtFailedMsg(stream, result, "0x%08X\n", status);
|
||
| 133 | } |
||
| 134 | |||
| 135 | return;
|
||
| 136 | } |
||
| 137 | |||
| 138 | 47680f67 | Thomas Schöpping | void _wheelSpeedTest(BaseSequentialStream* stream, ut_a3906data_t* data, wheel_t wheel, direction_t direction, aos_utresult_t* result)
|
| 139 | {
|
||
| 140 | // local variables
|
||
| 141 | uint32_t status; |
||
| 142 | 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 | aosUtPassed(stream, result); |
||
| 182 | 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 | aosUtFailedMsg(stream, result, "0x%08X\n", status);
|
||
| 192 | } |
||
| 193 | } |
||
| 194 | |||
| 195 | f3ac1c96 | Thomas Schöpping | /******************************************************************************/
|
| 196 | /* EXPORTED FUNCTIONS */
|
||
| 197 | /******************************************************************************/
|
||
| 198 | |||
| 199 | e545e620 | Thomas Schöpping | /**
|
| 200 | * @brief A3905 unit test function.
|
||
| 201 | *
|
||
| 202 | * @param[in] stream Stream for input/output.
|
||
| 203 | * @param[in] ut Unit test object.
|
||
| 204 | *
|
||
| 205 | * @return Unit test result value.
|
||
| 206 | */
|
||
| 207 | aos_utresult_t utAlldA3906Func(BaseSequentialStream* stream, aos_unittest_t* ut) |
||
| 208 | {
|
||
| 209 | aosDbgCheck((ut->data != NULL) &&
|
||
| 210 | (((ut_a3906data_t*)ut->data)->driver != NULL) &&
|
||
| 211 | (((ut_a3906data_t*)ut->data)->pwm.driver != NULL) &&
|
||
| 212 | (((ut_a3906data_t*)ut->data)->qei.left != NULL) &&
|
||
| 213 | (((ut_a3906data_t*)ut->data)->qei.right != NULL));
|
||
| 214 | |||
| 215 | |||
| 216 | |||
| 217 | // local variables
|
||
| 218 | aos_utresult_t result = {0, 0};
|
||
| 219 | uint32_t status = 0;
|
||
| 220 | 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 | status = a3906_lld_set_power(((ut_a3906data_t*)ut->data)->driver, power_state); |
||
| 228 | status |= a3906_lld_get_power(((ut_a3906data_t*)ut->data)->driver, &power_state); |
||
| 229 | status |= (power_state != A3906_LLD_POWER_ON) ? 0x10 : 0x00; |
||
| 230 | if (status == APAL_STATUS_SUCCESS) {
|
||
| 231 | aosUtPassed(stream, &result); |
||
| 232 | } else {
|
||
| 233 | aosUtFailedMsg(stream, &result, "0x%08X\n", status);
|
||
| 234 | } |
||
| 235 | |||
| 236 | _wheelDirectionTest(stream, (ut_a3906data_t*)ut->data, WHEEL_LEFT, DIRECTION_FORWARD, &result); |
||
| 237 | |||
| 238 | _wheelDirectionTest(stream, (ut_a3906data_t*)ut->data, WHEEL_RIGHT, DIRECTION_FORWARD, &result); |
||
| 239 | |||
| 240 | 47680f67 | Thomas Schöpping | _wheelDirectionTest(stream, (ut_a3906data_t*)ut->data, WHEEL_LEFT, DIRECTION_BACKWARD, &result); |
| 241 | |||
| 242 | e545e620 | Thomas Schöpping | _wheelDirectionTest(stream, (ut_a3906data_t*)ut->data, WHEEL_RIGHT, DIRECTION_BACKWARD, &result); |
| 243 | |||
| 244 | 47680f67 | Thomas Schöpping | _wheelSpeedTest(stream, (ut_a3906data_t*)ut->data, WHEEL_LEFT, DIRECTION_FORWARD, &result); |
| 245 | |||
| 246 | _wheelSpeedTest(stream, (ut_a3906data_t*)ut->data, WHEEL_RIGHT, DIRECTION_FORWARD, &result); |
||
| 247 | |||
| 248 | _wheelSpeedTest(stream, (ut_a3906data_t*)ut->data, WHEEL_LEFT, DIRECTION_BACKWARD, &result); |
||
| 249 | |||
| 250 | _wheelSpeedTest(stream, (ut_a3906data_t*)ut->data, WHEEL_RIGHT, DIRECTION_BACKWARD, &result); |
||
| 251 | e545e620 | Thomas Schöpping | |
| 252 | chprintf(stream, "disable power... \n");
|
||
| 253 | power_state = A3906_LLD_POWER_OFF; |
||
| 254 | status = a3906_lld_set_power(((ut_a3906data_t*)ut->data)->driver, power_state); |
||
| 255 | status |= a3906_lld_get_power(((ut_a3906data_t*)ut->data)->driver, &power_state); |
||
| 256 | 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 | status |= apalQEIGetPosition(((ut_a3906data_t*)ut->data)->qei.left, &qei_count[WHEEL_LEFT][0]);
|
||
| 265 | status |= apalQEIGetPosition(((ut_a3906data_t*)ut->data)->qei.right, &qei_count[WHEEL_RIGHT][0]);
|
||
| 266 | aosThdMSleep(1);
|
||
| 267 | status |= apalQEIGetPosition(((ut_a3906data_t*)ut->data)->qei.left, &qei_count[WHEEL_LEFT][1]);
|
||
| 268 | status |= apalQEIGetPosition(((ut_a3906data_t*)ut->data)->qei.right, &qei_count[WHEEL_RIGHT][1]);
|
||
| 269 | ++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 | } while((stable_counter < 100) && (timeout_counter * MICROSECONDS_PER_MILLISECOND <= ((ut_a3906data_t*)ut->data)->timeout)); |
||
| 272 | status |= (timeout_counter * MICROSECONDS_PER_MILLISECOND > ((ut_a3906data_t*)ut->data)->timeout) ? APAL_STATUS_TIMEOUT : 0x00;
|
||
| 273 | if (status == APAL_STATUS_SUCCESS) {
|
||
| 274 | aosUtPassed(stream, &result); |
||
| 275 | } else {
|
||
| 276 | aosUtFailedMsg(stream, &result, "0x%08X\n", status);
|
||
| 277 | } |
||
| 278 | |||
| 279 | // stop the PWM
|
||
| 280 | a3906_lld_set_pwm(((ut_a3906data_t*)ut->data)->pwm.driver, ((ut_a3906data_t*)ut->data)->pwm.channel.left_forward, APAL_PWM_WIDTH_OFF); |
||
| 281 | a3906_lld_set_pwm(((ut_a3906data_t*)ut->data)->pwm.driver, ((ut_a3906data_t*)ut->data)->pwm.channel.left_backward, APAL_PWM_WIDTH_OFF); |
||
| 282 | a3906_lld_set_pwm(((ut_a3906data_t*)ut->data)->pwm.driver, ((ut_a3906data_t*)ut->data)->pwm.channel.right_forward, APAL_PWM_WIDTH_OFF); |
||
| 283 | a3906_lld_set_pwm(((ut_a3906data_t*)ut->data)->pwm.driver, ((ut_a3906data_t*)ut->data)->pwm.channel.right_backward, APAL_PWM_WIDTH_OFF); |
||
| 284 | |||
| 285 | aosUtInfoMsg(stream,"driver object memory footprint: %u bytes\n", sizeof(A3906Driver)); |
||
| 286 | |||
| 287 | return result;
|
||
| 288 | } |
||
| 289 | |||
| 290 | ddf34c3d | Thomas Schöpping | #endif /* (AMIROOS_CFG_TESTS_ENABLE == true) && defined(AMIROLLD_CFG_A3906) && (AMIROLLD_CFG_A3906 == 1) */ |