(scf)
| 57 | |
| 58 | |
| 59 | def wait_for_position_estimator(scf): |
| 60 | print('Waiting for estimator to find position...') |
| 61 | |
| 62 | log_config = LogConfig(name='Kalman Variance', period_in_ms=500) |
| 63 | log_config.add_variable('kalman.varPX', 'float') |
| 64 | log_config.add_variable('kalman.varPY', 'float') |
| 65 | log_config.add_variable('kalman.varPZ', 'float') |
| 66 | |
| 67 | var_y_history = [1000] * 10 |
| 68 | var_x_history = [1000] * 10 |
| 69 | var_z_history = [1000] * 10 |
| 70 | |
| 71 | threshold = 0.001 |
| 72 | |
| 73 | with SyncLogger(scf, log_config) as logger: |
| 74 | for log_entry in logger: |
| 75 | data = log_entry[1] |
| 76 | |
| 77 | var_x_history.append(data['kalman.varPX']) |
| 78 | var_x_history.pop(0) |
| 79 | var_y_history.append(data['kalman.varPY']) |
| 80 | var_y_history.pop(0) |
| 81 | var_z_history.append(data['kalman.varPZ']) |
| 82 | var_z_history.pop(0) |
| 83 | |
| 84 | min_x = min(var_x_history) |
| 85 | max_x = max(var_x_history) |
| 86 | min_y = min(var_y_history) |
| 87 | max_y = max(var_y_history) |
| 88 | min_z = min(var_z_history) |
| 89 | max_z = max(var_z_history) |
| 90 | |
| 91 | # print("{} {} {}". |
| 92 | # format(max_x - min_x, max_y - min_y, max_z - min_z)) |
| 93 | |
| 94 | if (max_x - min_x) < threshold and ( |
| 95 | max_y - min_y) < threshold and ( |
| 96 | max_z - min_z) < threshold: |
| 97 | break |
| 98 | |
| 99 | |
| 100 | def reset_estimator(scf): |
no test coverage detected