| | |
|---|
| | | linear_acc = dv / dt |
|---|
| | | self.state.linear_velocity += dv |
|---|
| | | |
|---|
| | | dw = target_angular - self.state.angular_velocity |
|---|
| | | # 符号定义： |
|---|
| | | # - 罗盘方向：负的控制信号 → 逆时针旋转，正的控制信号 → 顺时针旋转 |
|---|
| | | # - 数学坐标系：东为0°，逆时针为正（CCW>0） |
|---|
| | | # - 所以：负的turn_rate（逆时针）→ angular_velocity为正，正的turn_rate（顺时针）→ angular_velocity为负 |
|---|
| | | # - 需要取反：angular_velocity = -target_angular |
|---|
| | | 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）符号定义：顺时针正，逆时针负（从上往下看Z轴，右手螺旋定则） |
|---|
| | | # 在数学坐标系中：顺时针 → angular_velocity为负，逆时针 → angular_velocity为正 |
|---|
| | | # 所以：GyroZ = -angular_velocity（取反以匹配惯导定义） |
|---|
| | | # 例如：angular_velocity = -0.8 rad/s（顺时针）→ GyroZ = 0.8 deg/s（正值，顺时针） |
|---|
| | | 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) |
|---|
