| | |
|---|
| | | frame, gps_time_s = self._build_nav_frame() |
|---|
| | | if frame: |
|---|
| | | self.uart2.write(frame) |
|---|
| | | self._log_binary("PY->STM32 UART2 IM23A_NAV", frame, gps_time_s=gps_time_s, source_rank=0) |
|---|
| | | self._sleep_remaining(start, period) |
|---|
| | | |
|---|
| | | def _loop_imu(self): |
|---|
| | |
|---|
| | | frame, gps_time_s = self._build_imu_frame() |
|---|
| | | if frame: |
|---|
| | | self.uart2.write(frame) |
|---|
| | | self._log_binary("PY->STM32 UART2 IM23A_IMU", frame, gps_time_s=gps_time_s, source_rank=0) |
|---|
| | | self._sleep_remaining(start, period) |
|---|
| | | |
|---|
| | | def _loop_control(self): |
|---|
| | |
|---|
| | | def _build_nav_frame(self) -> tuple[bytes | None, float]: |
|---|
| | | state, timestamp = self._snapshot() |
|---|
| | | lat, lon, alt = geo.enu_to_lla(state.east, state.north, state.up, self.origin) |
|---|
| | | heading_nav = geo.heading_math_to_nav(state.heading) |
|---|
| | | heading_nav_deg = geo.heading_math_to_nav(state.heading) |
|---|
| | | # 将导航坐标系角度（0-360度）转换为弧度（-π到π），0度正北对应0弧度 |
|---|
| | | # 0°(正北)→0rad, 90°(正东)→π/2rad, 180°(正南)→πrad, 270°(正西)→-π/2rad |
|---|
| | | heading_nav_rad = math.radians(heading_nav_deg) if heading_nav_deg <= 180.0 else math.radians(heading_nav_deg - 360.0) |
|---|
| | | gps_time_s = _seconds_of_day(timestamp) |
|---|
| | | status_flags = int(self.config.position_quality) & 0xFF |
|---|
| | | frame = build_im23a_nav_frame( |
|---|
| | |
|---|
| | | east_vel=state.east_velocity, |
|---|
| | | north_vel=state.north_velocity, |
|---|
| | | up_vel=state.up_velocity, |
|---|
| | | heading_deg=heading_nav, |
|---|
| | | heading_rad=heading_nav_rad, |
|---|
| | | pitch_deg=state.pitch_deg, |
|---|
| | | roll_deg=state.roll_deg, |
|---|
| | | accel_bias=(0.0, 0.0, 0.0), |
|---|
