| | |
|---|
| | | linear_acc = dv / dt |
|---|
| | | self.state.linear_velocity += dv |
|---|
| | | |
|---|
| | | dw = target_angular - self.state.angular_velocity |
|---|
| | | # 符号定义： |
|---|
| | | # - MCU已修复方向反转问题（去掉了错误的取反），现在直接使用MCU下发的角速度 |
|---|
| | | # - 数学坐标系：东为0°，逆时针为正（CCW>0） |
|---|
| | | target_angular_math = target_angular # 直接使用，不再取反 |
|---|
| | | |
|---|
| | | dw = target_angular_math - self.state.angular_velocity |
|---|
| | | max_dw = self.max_angular_accel * dt |
|---|
| | | dw = _clamp(dw, -max_dw, max_dw) |
|---|
| | | angular_acc = dw / dt |
|---|
| | |
|---|
| | | self.state.pitch_deg = math.degrees(math.atan2(ax_g, 1.0)) |
|---|
| | | self.state.roll_deg = math.degrees(math.atan2(-ay_g, 1.0)) |
|---|
| | | |
|---|
| | | # 惯导（IMU）符号定义：与MCU方向定义保持一致，不再取反 |
|---|
| | | # MCU已修复方向反转问题，现在直接使用angular_velocity |
|---|
| | | self.state.gyro_deg_s = ( |
|---|
| | | self._rng.gauss(0.0, 0.01), |
|---|
| | | self._rng.gauss(0.0, 0.01), |
|---|
| | | math.degrees(self.state.angular_velocity) + self._rng.gauss(0.0, 0.1), |
|---|
| | | math.degrees(self.state.angular_velocity) + self._rng.gauss(0.0, 0.1), # 直接使用，不再取反 |
|---|
| | | ) |
|---|
| | | |
|---|
| | | temp_drift = self._rng.gauss(0.0, 0.0005) |
|---|
