Revision 47680f67 unittests/periphery-lld/src/ut_alld_a3906.c
unittests/periphery-lld/src/ut_alld_a3906.c | ||
---|---|---|
30 | 30 |
/** |
31 | 31 |
* @brief Interval to poll QEI in certain tests. |
32 | 32 |
*/ |
33 |
#define QEI_POLL_INTERVAL_MS 100
|
|
33 |
#define QEI_POLL_INTERVAL_MS 10 |
|
34 | 34 |
|
35 | 35 |
/** |
36 | 36 |
* @brief Threshold for QEI differences. |
... | ... | |
116 | 116 |
return; |
117 | 117 |
} |
118 | 118 |
|
119 |
void _wheelSpeedTest(BaseSequentialStream* stream, ut_a3906data_t* data, wheel_t wheel, direction_t direction, aos_utresult_t* result) |
|
120 |
{ |
|
121 |
// local variables |
|
122 |
uint32_t status; |
|
123 |
apalQEICount_t qei_range; |
|
124 |
apalQEICount_t qei_count[2] = {0}; |
|
125 |
apalQEICount_t qei_increments[2] = {0}; |
|
126 |
apalQEICount_t qei_increments_diff = 0; |
|
127 |
uint32_t timeout_counter = 0; |
|
128 |
uint32_t stable_counter = 0; |
|
129 |
|
|
130 |
chprintf(stream, "%s wheel full speed %s...\n", (wheel == WHEEL_LEFT) ? "left" : "right", (direction == DIRECTION_FORWARD) ? "forward" : "backward"); |
|
131 |
// spin up the wheel with full speed |
|
132 |
status = apalQEIGetRange((wheel == WHEEL_LEFT) ? data->qei.left : data->qei.right, &qei_range); |
|
133 |
status |= a3906_lld_set_pwm(data->pwm.driver, (wheel == WHEEL_LEFT) ? |
|
134 |
((direction == DIRECTION_FORWARD) ? data->pwm.channel.left_forward : data->pwm.channel.left_backward) : |
|
135 |
((direction == DIRECTION_FORWARD) ? data->pwm.channel.right_forward : data->pwm.channel.right_backward), |
|
136 |
APAL_PWM_WIDTH_MAX); |
|
137 |
aosThdMSleep(100); |
|
138 |
do { |
|
139 |
// read QEI data to determine speed |
|
140 |
status |= apalQEIGetPosition((wheel == WHEEL_LEFT) ? data->qei.left : data->qei.right, &qei_count[0]); |
|
141 |
aosThdMSleep(QEI_POLL_INTERVAL_MS); |
|
142 |
status |= apalQEIGetPosition((wheel == WHEEL_LEFT) ? data->qei.left : data->qei.right, &qei_count[1]); |
|
143 |
timeout_counter += QEI_POLL_INTERVAL_MS; |
|
144 |
qei_increments[0] = qei_increments[1]; |
|
145 |
qei_increments[1] = (direction == DIRECTION_FORWARD) ? |
|
146 |
((qei_count[1] > qei_count[0]) ? (qei_count[1] - qei_count[0]) : (qei_count[1] + (qei_range - qei_count[0]))) : |
|
147 |
((qei_count[0] > qei_count[1]) ? (qei_count[0] - qei_count[1]) : (qei_count[0] + (qei_range - qei_count[1]))); |
|
148 |
qei_increments_diff = abs((int32_t)qei_increments[0] - (int32_t)qei_increments[1]); |
|
149 |
stable_counter = (qei_increments[1] != 0 && qei_increments_diff <= QEI_DIFF_THRESHOLD) ? stable_counter+1 : 0; |
|
150 |
} while ((stable_counter* QEI_POLL_INTERVAL_MS < MILLISECONDS_PER_SECOND) && (timeout_counter * MICROSECONDS_PER_MILLISECOND <= data->timeout)); |
|
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_OFF); |
|
155 |
status |= (timeout_counter * MICROSECONDS_PER_MILLISECOND > data->timeout) ? APAL_STATUS_TIMEOUT : 0x00; |
|
156 |
|
|
157 |
// report results |
|
158 |
if (status == APAL_STATUS_SUCCESS) { |
|
159 |
aosUtPassed(stream, result); |
|
160 |
const float tps = qei_increments[1] * ((float)MILLISECONDS_PER_SECOND / (float)QEI_POLL_INTERVAL_MS); |
|
161 |
const float rpm = tps * SECONDS_PER_MINUTE / (float)data->qei.increments_per_revolution; |
|
162 |
const float velocity = tps / (float)data->qei.increments_per_revolution * ((wheel == WHEEL_LEFT) ? data->wheel_diameter.left : data->wheel_diameter.right) * acos(-1); |
|
163 |
chprintf(stream, "\t%f tps\n", tps); |
|
164 |
chprintf(stream, "\t%f RPM\n", rpm); |
|
165 |
chprintf(stream, "\t%f m/s\n", velocity); |
|
166 |
chprintf(stream, "\n"); |
|
167 |
} |
|
168 |
else { |
|
169 |
aosUtFailedMsg(stream, result, "0x%08X\n", status); |
|
170 |
} |
|
171 |
} |
|
172 |
|
|
119 | 173 |
/** |
120 | 174 |
* @brief A3905 unit test function. |
121 | 175 |
* |
... | ... | |
139 | 193 |
uint32_t status = 0; |
140 | 194 |
a3906_lld_power_t power_state; |
141 | 195 |
apalQEICount_t qei_count[2][2]; |
142 |
apalQEICount_t qei_range[2]; |
|
143 | 196 |
uint32_t timeout_counter; |
144 |
apalQEICount_t qei_increments[2][2]; |
|
145 | 197 |
uint32_t stable_counter; |
146 |
apalQEICount_t qei_increments_diff[2]; |
|
147 | 198 |
|
148 | 199 |
chprintf(stream, "enable power...\n"); |
149 | 200 |
power_state = A3906_LLD_POWER_ON; |
... | ... | |
158 | 209 |
|
159 | 210 |
_wheelDirectionTest(stream, (ut_a3906data_t*)ut->data, WHEEL_LEFT, DIRECTION_FORWARD, &result); |
160 | 211 |
|
161 |
_wheelDirectionTest(stream, (ut_a3906data_t*)ut->data, WHEEL_LEFT, DIRECTION_BACKWARD, &result); |
|
162 |
|
|
163 | 212 |
_wheelDirectionTest(stream, (ut_a3906data_t*)ut->data, WHEEL_RIGHT, DIRECTION_FORWARD, &result); |
164 | 213 |
|
214 |
_wheelDirectionTest(stream, (ut_a3906data_t*)ut->data, WHEEL_LEFT, DIRECTION_BACKWARD, &result); |
|
215 |
|
|
165 | 216 |
_wheelDirectionTest(stream, (ut_a3906data_t*)ut->data, WHEEL_RIGHT, DIRECTION_BACKWARD, &result); |
166 | 217 |
|
167 |
chprintf(stream, "both wheels full speed forward...\n"); |
|
168 |
status = APAL_STATUS_SUCCESS; |
|
169 |
status |= a3906_lld_set_pwm(((ut_a3906data_t*)ut->data)->pwm.driver, ((ut_a3906data_t*)ut->data)->pwm.channel.left_forward, APAL_PWM_WIDTH_MAX); |
|
170 |
status |= a3906_lld_set_pwm(((ut_a3906data_t*)ut->data)->pwm.driver, ((ut_a3906data_t*)ut->data)->pwm.channel.right_forward, APAL_PWM_WIDTH_MAX); |
|
171 |
aosThdMSleep(100); |
|
172 |
qei_count[WHEEL_LEFT][0] = 0; |
|
173 |
qei_count[WHEEL_LEFT][1] = 0; |
|
174 |
qei_count[WHEEL_RIGHT][0] = 0; |
|
175 |
qei_count[WHEEL_RIGHT][1] = 0; |
|
176 |
status |= apalQEIGetRange(((ut_a3906data_t*)ut->data)->qei.left, &qei_range[WHEEL_LEFT]); |
|
177 |
status |= apalQEIGetRange(((ut_a3906data_t*)ut->data)->qei.right, &qei_range[WHEEL_RIGHT]); |
|
178 |
timeout_counter = 0; |
|
179 |
qei_increments[WHEEL_LEFT][0] = 0; |
|
180 |
qei_increments[WHEEL_LEFT][1] = 0; |
|
181 |
qei_increments[WHEEL_RIGHT][0] = 0; |
|
182 |
qei_increments[WHEEL_RIGHT][1] = 0; |
|
183 |
stable_counter = 0; |
|
184 |
do { |
|
185 |
status |= apalQEIGetPosition(((ut_a3906data_t*)ut->data)->qei.left, &qei_count[WHEEL_LEFT][0]); |
|
186 |
status |= apalQEIGetPosition(((ut_a3906data_t*)ut->data)->qei.right, &qei_count[WHEEL_RIGHT][0]); |
|
187 |
aosThdMSleep(QEI_POLL_INTERVAL_MS); |
|
188 |
status |= apalQEIGetPosition(((ut_a3906data_t*)ut->data)->qei.left, &qei_count[WHEEL_LEFT][1]); |
|
189 |
status |= apalQEIGetPosition(((ut_a3906data_t*)ut->data)->qei.right, &qei_count[WHEEL_RIGHT][1]); |
|
190 |
timeout_counter += QEI_POLL_INTERVAL_MS; |
|
191 |
qei_increments[WHEEL_LEFT][0] = qei_increments[WHEEL_LEFT][1]; |
|
192 |
qei_increments[WHEEL_LEFT][1] = (qei_count[WHEEL_LEFT][1] > qei_count[WHEEL_LEFT][0]) ? (qei_count[WHEEL_LEFT][1] - qei_count[WHEEL_LEFT][0]) : (qei_count[WHEEL_LEFT][1] + (qei_range[WHEEL_LEFT] - qei_count[WHEEL_LEFT][0])); |
|
193 |
qei_increments[WHEEL_RIGHT][0] = qei_increments[WHEEL_RIGHT][1]; |
|
194 |
qei_increments[WHEEL_RIGHT][1] = (qei_count[WHEEL_RIGHT][1] > qei_count[WHEEL_RIGHT][0]) ? (qei_count[WHEEL_RIGHT][1] - qei_count[WHEEL_RIGHT][0]) : (qei_count[WHEEL_RIGHT][1] + (qei_range[WHEEL_RIGHT] - qei_count[WHEEL_RIGHT][0])); |
|
195 |
qei_increments_diff[WHEEL_LEFT] = abs((int32_t)(qei_increments[WHEEL_LEFT][0]) - (int32_t)qei_increments[WHEEL_LEFT][1]); |
|
196 |
qei_increments_diff[WHEEL_RIGHT] = abs((int32_t)(qei_increments[WHEEL_RIGHT][0]) - (int32_t)qei_increments[WHEEL_RIGHT][1]); |
|
197 |
stable_counter = (qei_increments_diff[WHEEL_LEFT] <= QEI_DIFF_THRESHOLD && qei_increments_diff[WHEEL_RIGHT] < QEI_DIFF_THRESHOLD) ? stable_counter+1 : 0; |
|
198 |
} while((stable_counter * QEI_POLL_INTERVAL_MS < MILLISECONDS_PER_SECOND) && (timeout_counter * MICROSECONDS_PER_MILLISECOND <= ((ut_a3906data_t*)ut->data)->timeout)); |
|
199 |
status |= (timeout_counter * MICROSECONDS_PER_MILLISECOND > ((ut_a3906data_t*)ut->data)->timeout) ? APAL_STATUS_TIMEOUT : 0x00; |
|
200 |
if (status == APAL_STATUS_SUCCESS) { |
|
201 |
aosUtPassed(stream, &result); |
|
202 |
const float tps[2] = {qei_increments[WHEEL_LEFT][1] * ((float)MILLISECONDS_PER_SECOND / (float)QEI_POLL_INTERVAL_MS), |
|
203 |
qei_increments[WHEEL_RIGHT][1] * ((float)MILLISECONDS_PER_SECOND / (float)QEI_POLL_INTERVAL_MS)}; |
|
204 |
const float rpm[2] = {tps[WHEEL_LEFT] * SECONDS_PER_MINUTE / (float)(((ut_a3906data_t*)ut->data)->qei.increments_per_revolution), |
|
205 |
tps[WHEEL_RIGHT] * SECONDS_PER_MINUTE / (float)(((ut_a3906data_t*)ut->data)->qei.increments_per_revolution)}; |
|
206 |
const float velocity[2] = {tps[WHEEL_LEFT] / (float)(((ut_a3906data_t*)ut->data)->qei.increments_per_revolution) * ((ut_a3906data_t*)ut->data)->wheel_diameter * acos(-1), |
|
207 |
tps[WHEEL_RIGHT] / (float)(((ut_a3906data_t*)ut->data)->qei.increments_per_revolution) * ((ut_a3906data_t*)ut->data)->wheel_diameter * acos(-1)}; |
|
208 |
chprintf(stream, "left wheel:\n"); |
|
209 |
chprintf(stream, "\t%f tps\n", tps[WHEEL_LEFT]); |
|
210 |
chprintf(stream, "\t%f RPM\n", rpm[WHEEL_LEFT]); |
|
211 |
chprintf(stream, "\t%f m/s\n", velocity[WHEEL_LEFT]); |
|
212 |
chprintf(stream, "right wheel:\n"); |
|
213 |
chprintf(stream, "\t%f tps\n", tps[WHEEL_RIGHT]); |
|
214 |
chprintf(stream, "\t%f RPM\n", rpm[WHEEL_RIGHT]); |
|
215 |
chprintf(stream, "\t%f m/s\n", velocity[WHEEL_RIGHT]); |
|
216 |
chprintf(stream, "\n"); |
|
217 |
} else { |
|
218 |
aosUtFailedMsg(stream, &result, "0x%08X\n", status); |
|
219 |
} |
|
218 |
_wheelSpeedTest(stream, (ut_a3906data_t*)ut->data, WHEEL_LEFT, DIRECTION_FORWARD, &result); |
|
219 |
|
|
220 |
_wheelSpeedTest(stream, (ut_a3906data_t*)ut->data, WHEEL_RIGHT, DIRECTION_FORWARD, &result); |
|
221 |
|
|
222 |
_wheelSpeedTest(stream, (ut_a3906data_t*)ut->data, WHEEL_LEFT, DIRECTION_BACKWARD, &result); |
|
223 |
|
|
224 |
_wheelSpeedTest(stream, (ut_a3906data_t*)ut->data, WHEEL_RIGHT, DIRECTION_BACKWARD, &result); |
|
220 | 225 |
|
221 | 226 |
chprintf(stream, "disable power... \n"); |
222 | 227 |
power_state = A3906_LLD_POWER_OFF; |
Also available in: Unified diff