API リファレンス

Warning

本リポジトリで提供されているソフトウェアおよび API リファレンスは、講師・開発者向けの参考実装およびデモ用のものです。 ゼミ参加者(受講生)に対してオープンに提供・公開されているものではありません。

C++ (Pico SDK) および Python の API リファレンスです。

C++ 衛星フライトソフトウェア (satelite)

衛星側で動作する C++ プログラムの API リファレンスです。Doxygen によりソースコードから自動生成されています。

struct client_state

Public Members

char buffer[64]
int buf_len
class ICM42688
#include <icm42688.h>

Subclassed by ICM42688_FIFO

Public Types

enum GyroFS

Values:

enumerator dps2000
enumerator dps1000
enumerator dps500
enumerator dps250
enumerator dps125
enumerator dps62_5
enumerator dps31_25
enumerator dps15_625
enum AccelFS

Values:

enumerator gpm16
enumerator gpm8
enumerator gpm4
enumerator gpm2
enum ODR

Values:

enumerator odr32k
enumerator odr16k
enumerator odr8k
enumerator odr4k
enumerator odr2k
enumerator odr1k
enumerator odr200
enumerator odr100
enumerator odr50
enumerator odr25
enumerator odr12_5
enumerator odr6a25
enumerator odr3a125
enumerator odr1a5625
enumerator odr500
enum GyroNFBWsel

Values:

enumerator nfBW1449Hz
enumerator nfBW680z
enumerator nfBW329Hz
enumerator nfBW162Hz
enumerator nfBW80Hz
enumerator nfBW40Hz
enumerator nfBW20Hz
enumerator nfBW10Hz
enum UIFiltOrd

Values:

enumerator first_order
enumerator second_order
enumerator third_order

Public Functions

ICM42688(i2c_inst_t *bus, uint8_t address)

Constructor for I2C communication.

Parameters:
  • bus – I2C bus

  • address[in] Address of ICM 42688-p device

ICM42688(i2c_inst_t *bus, uint8_t address, uint8_t sda_pin, uint8_t scl_pin)

Constructor for I2C communication using SDA, SCL pins.

Parameters:
  • bus – I2C bus

  • address[in] Address of ICM 42688-p device

  • sda_pin[in] GPIO pin to use for I2C SDA signal

  • scl_pin[in] GPIO pin to use for I2C SCL signal

int begin()

Initialize the device.

Returns:

ret < 0 if error

int setAccelFS(AccelFS fssel)

Sets the full scale range for the accelerometer.

Parameters:

fssel[in] Full scale selection

Returns:

ret < 0 if error

int getAccelFS()
int setGyroFS(GyroFS fssel)

Sets the full scale range for the gyro.

Parameters:

fssel[in] Full scale selection

Returns:

ret < 0 if error

int getGyroFS()
int setAccelODR(ODR odr)

Set the ODR for accelerometer.

Parameters:

odr[in] Output data rate

Returns:

ret < 0 if error

int getAccelODR()
int setGyroODR(ODR odr)

Set the ODR for gyro.

Parameters:

odr[in] Output data rate

Returns:

ret < 0 if error

int getGyroODR()
int setFilters(bool gyroFilters, bool accFilters)
int enableDataReadyInterrupt()

Enables the data ready interrupt.

        - routes UI data ready interrupt to INT1
        - push-pull, pulsed, active HIGH interrupts
Returns:

ret < 0 if error

int disableDataReadyInterrupt()

Masks the data ready interrupt.

Returns:

ret < 0 if error

int getAGT()

Transfers data from ICM 42688-p to mcu. Must be called to access new measurements.

Returns:

ret < 0 if error

int getRawAGT()
inline float accX() const

Get accelerometer data, per axis.

Returns:

Acceleration in g’s

inline float accY() const
inline float accZ() const
inline float gyrX() const

Get gyro data, per axis.

Returns:

Angular velocity in dps

inline float gyrY() const
inline float gyrZ() const
inline float temp() const

Get temperature of gyro die.

Returns:

Temperature in Celsius

inline int16_t rawAccX() const

Get accelerometer Raw data, per axis.

Returns:

Acceleration in bytes

inline int16_t rawAccY() const
inline int16_t rawAccZ() const
inline int16_t rawGyrX() const

Get gyro raw data, per axis.

Returns:

Angular velocity in bytes

inline int16_t rawGyrY() const
inline int16_t rawGyrZ() const
inline int16_t rawTemp() const

Get raw temperature of gyro die.

Returns:

Temperature in bytes

inline int32_t rawBiasAccX() const

Get raw temperature of gyro die.

Returns:

Temperature in bytes

inline int32_t rawBiasAccY() const
inline int32_t rawBiasAccZ() const
inline int32_t rawBiasGyrX() const
inline int32_t rawBiasGyrY() const
inline int32_t rawBiasGyrZ() const
int computeOffsets()
int setAllOffsets()
int setGyrXOffset(int16_t gyrXoffset)
int setGyrYOffset(int16_t gyrYoffset)
int setGyrZOffset(int16_t gyrZoffset)
int setAccXOffset(int16_t accXoffset)
int setAccYOffset(int16_t accYoffset)
int setAccZOffset(int16_t accZoffset)
float getAccelRes()
float getGyroRes()
int setUIFilterBlock(UIFiltOrd gyroUIFiltOrder, UIFiltOrd accelUIFiltOrder)
int setGyroNotchFilter(float gyroNFfreq_x, float gyroNFfreq_y, float gyroNFfreq_z, GyroNFBWsel gyro_nf_bw)
int selfTest()
int testingFunction()
int calibrateGyro()
float getGyroBiasX()
float getGyroBiasY()
float getGyroBiasZ()
void setGyroBiasX(float bias)
void setGyroBiasY(float bias)
void setGyroBiasZ(float bias)
int calibrateAccel()
float getAccelBiasX_mss()
float getAccelScaleFactorX()
float getAccelBiasY_mss()
float getAccelScaleFactorY()
float getAccelBiasZ_mss()
float getAccelScaleFactorZ()
void setAccelCalX(float bias, float scaleFactor)
void setAccelCalY(float bias, float scaleFactor)
void setAccelCalZ(float bias, float scaleFactor)

Protected Functions

int writeRegister(uint8_t subAddress, uint8_t data)
int readRegisters(uint8_t subAddress, uint8_t count, uint8_t *dest)
int setBank(uint8_t bank)
void reset()

Software reset of the device.

uint8_t whoAmI()

Read the WHO_AM_I register.

Returns:

Value of WHO_AM_I register

void selfTest(int16_t *accelDiff, int16_t *gyroDiff, float *ratio)

Protected Attributes

uint8_t _address = 0

I2C Communication.

i2c_inst_t *_i2c = {}
size_t _numBytes = 0
uint8_t _sda_pin = 0
uint8_t _scl_pin = 0
uint8_t _buffer[15] = {}
float _t = 0.0f
float _acc[3] = {}
float _gyr[3] = {}
int16_t _rawT = 0
int16_t _rawAcc[3] = {}
int16_t _rawGyr[3] = {}
int32_t _rawAccBias[3] = {0, 0, 0}

Raw Gyro and Accelerometer Bias.

int32_t _rawGyrBias[3] = {0, 0, 0}
int16_t _AccOffset[3] = {0, 0, 0}

Raw Gyro and Accelerometer Offsets.

int16_t _GyrOffset[3] = {0, 0, 0}
float _accelScale = 0.0f

Full scale resolution factors.

float _gyroScale = 0.0f
AccelFS _accelFS = gpm16

Full scale selections.

GyroFS _gyroFS = dps2000
float _accBD[3] = {}

Accel calibration.

float _accB[3] = {}
float _accS[3] = {1.0f, 1.0f, 1.0f}
float _accMax[3] = {}
float _accMin[3] = {}
float _gyroBD[3] = {}

Gyro calibration.

float _gyrB[3] = {}
uint8_t _bank = 0

current user bank

const uint8_t FIFO_EN = 0x5F
const uint8_t FIFO_TEMP_EN = 0x04
const uint8_t FIFO_GYRO = 0x02
const uint8_t FIFO_ACCEL = 0x01
const uint8_t GYRO_NF_ENABLE = 0x00
const uint8_t GYRO_NF_DISABLE = 0x01
const uint8_t GYRO_AAF_ENABLE = 0x00
const uint8_t GYRO_AAF_DISABLE = 0x02
const uint8_t ACCEL_AAF_ENABLE = 0x00
const uint8_t ACCEL_AAF_DISABLE = 0x01

Protected Static Attributes

static uint32_t I2C_CLK = 100'000
static uint8_t WHO_AM_I = 0x47

Constants.

expected value in UB0_REG_WHO_AM_I reg

static int NUM_CALIB_SAMPLES = 1000

for gyro/accel bias calib

static float TEMP_DATA_REG_SCALE = 132.48f

Conversion formula to get temperature in Celsius (Sec 4.13).

static float TEMP_OFFSET = 25.0f
class ICM42688_FIFO : public ICM42688
#include <icm42688.h>

Public Functions

int enableFifo(bool accel, bool gyro, bool temp)
int streamToFifo()
int readFifo()
void getFifoAccelX_mss(size_t *size, float *data)
void getFifoAccelY_mss(size_t *size, float *data)
void getFifoAccelZ_mss(size_t *size, float *data)
void getFifoGyroX(size_t *size, float *data)
void getFifoGyroY(size_t *size, float *data)
void getFifoGyroZ(size_t *size, float *data)
void getFifoTemperature_C(size_t *size, float *data)
ICM42688(i2c_inst_t *bus, uint8_t address)

Constructor for I2C communication.

Parameters:
  • bus – I2C bus

  • address[in] Address of ICM 42688-p device

ICM42688(i2c_inst_t *bus, uint8_t address, uint8_t sda_pin, uint8_t scl_pin)

Constructor for I2C communication using SDA, SCL pins.

Parameters:
  • bus – I2C bus

  • address[in] Address of ICM 42688-p device

  • sda_pin[in] GPIO pin to use for I2C SDA signal

  • scl_pin[in] GPIO pin to use for I2C SCL signal

Protected Attributes

bool _enFifoAccel = false
bool _enFifoGyro = false
bool _enFifoTemp = false
bool _enFifoTimestamp = false
bool _enFifoHeader = false
size_t _fifoSize = 0
size_t _fifoFrameSize = 0
float _axFifo[85] = {}
float _ayFifo[85] = {}
float _azFifo[85] = {}
size_t _aSize = 0
float _gxFifo[85] = {}
float _gyFifo[85] = {}
float _gzFifo[85] = {}
size_t _gSize = 0
float _tFifo[256] = {}
size_t _tSize = 0
class MCP3008
#include <mcp3008.h>

Public Functions

MCP3008(const MCP3008_Config &config)
~MCP3008()
int adc_read_channel(uint8_t channel)

Read single ended ADC channel.

Parameters:

channel

Returns:

-1 if channel < 0 or channel > 7, otherwise ADC (int)

int adc_read_channel_diff(AdcDifferentialMode diffMode)

Read differential ADC channel.

Parameters:

diffMode

Returns:

-1 if channel < 0 or channel > 7, otherwise ADC difference (int)

Public Static Attributes

static const uint8_t NUM_CHANNELS = 8
static const uint8_t ADC_RESOLUTION = 10

Private Functions

int spiRead(uint8_t channel, bool differential)

Private Members

spi_inst_t *spi_port
uint8_t spi_cs

Private Static Functions

static inline void cs_select(uint pin_cs)
static inline void cs_deselect(uint pin_cs)
struct MCP3008_Config
#include <mcp3008.h>

Public Members

spi_inst_t *spi_port = PICO_DEFAULT_SPI
uint spi_baud = 1000000
uint spi_sck_gpio = PICO_DEFAULT_SPI_SCK_PIN
uint spi_tx_gpio = PICO_DEFAULT_SPI_TX_PIN
uint spi_rx_gpio = PICO_DEFAULT_SPI_RX_PIN
uint spi_cs_gpio = PICO_DEFAULT_SPI_CSN_PIN
struct rcv_data_t

Public Members

uint8_t data[128]
int len
struct regval_list

Public Members

uint8_t reg_num
uint16_t value
namespace ICM42688reg

Variables

static uint8_t REG_BANK_SEL = 0x76
static uint8_t UB0_REG_DEVICE_CONFIG = 0x11
static uint8_t UB0_REG_DRIVE_CONFIG = 0x13
static uint8_t UB0_REG_INT_CONFIG = 0x14
static uint8_t UB0_REG_FIFO_CONFIG = 0x16
static uint8_t UB0_REG_TEMP_DATA1 = 0x1D
static uint8_t UB0_REG_TEMP_DATA0 = 0x1E
static uint8_t UB0_REG_ACCEL_DATA_X1 = 0x1F
static uint8_t UB0_REG_ACCEL_DATA_X0 = 0x20
static uint8_t UB0_REG_ACCEL_DATA_Y1 = 0x21
static uint8_t UB0_REG_ACCEL_DATA_Y0 = 0x22
static uint8_t UB0_REG_ACCEL_DATA_Z1 = 0x23
static uint8_t UB0_REG_ACCEL_DATA_Z0 = 0x24
static uint8_t UB0_REG_GYRO_DATA_X1 = 0x25
static uint8_t UB0_REG_GYRO_DATA_X0 = 0x26
static uint8_t UB0_REG_GYRO_DATA_Y1 = 0x27
static uint8_t UB0_REG_GYRO_DATA_Y0 = 0x28
static uint8_t UB0_REG_GYRO_DATA_Z1 = 0x29
static uint8_t UB0_REG_GYRO_DATA_Z0 = 0x2A
static uint8_t UB0_REG_TMST_FSYNCH = 0x2B
static uint8_t UB0_REG_TMST_FSYNCL = 0x2C
static uint8_t UB0_REG_INT_STATUS = 0x2D
static uint8_t UB0_REG_FIFO_COUNTH = 0x2E
static uint8_t UB0_REG_FIFO_COUNTL = 0x2F
static uint8_t UB0_REG_FIFO_DATA = 0x30
static uint8_t UB0_REG_APEX_DATA0 = 0x31
static uint8_t UB0_REG_APEX_DATA1 = 0x32
static uint8_t UB0_REG_APEX_DATA2 = 0x33
static uint8_t UB0_REG_APEX_DATA3 = 0x34
static uint8_t UB0_REG_APEX_DATA4 = 0x35
static uint8_t UB0_REG_APEX_DATA5 = 0x36
static uint8_t UB0_REG_INT_STATUS2 = 0x37
static uint8_t UB0_REG_INT_STATUS3 = 0x38
static uint8_t UB0_REG_SIGNAL_PATH_RESET = 0x4B
static uint8_t UB0_REG_INTF_CONFIG0 = 0x4C
static uint8_t UB0_REG_INTF_CONFIG1 = 0x4D
static uint8_t UB0_REG_PWR_MGMT0 = 0x4E
static uint8_t UB0_REG_GYRO_CONFIG0 = 0x4F
static uint8_t UB0_REG_ACCEL_CONFIG0 = 0x50
static uint8_t UB0_REG_GYRO_CONFIG1 = 0x51
static uint8_t UB0_REG_GYRO_ACCEL_CONFIG0 = 0x52
static uint8_t UB0_REG_ACCEFL_CONFIG1 = 0x53
static uint8_t UB0_REG_TMST_CONFIG = 0x54
static uint8_t UB0_REG_APEX_CONFIG0 = 0x56
static uint8_t UB0_REG_SMD_CONFIG = 0x57
static uint8_t UB0_REG_FIFO_CONFIG1 = 0x5F
static uint8_t UB0_REG_FIFO_CONFIG2 = 0x60
static uint8_t UB0_REG_FIFO_CONFIG3 = 0x61
static uint8_t UB0_REG_FSYNC_CONFIG = 0x62
static uint8_t UB0_REG_INT_CONFIG0 = 0x63
static uint8_t UB0_REG_INT_CONFIG1 = 0x64
static uint8_t UB0_REG_INT_SOURCE0 = 0x65
static uint8_t UB0_REG_INT_SOURCE1 = 0x66
static uint8_t UB0_REG_INT_SOURCE3 = 0x68
static uint8_t UB0_REG_INT_SOURCE4 = 0x69
static uint8_t UB0_REG_FIFO_LOST_PKT0 = 0x6C
static uint8_t UB0_REG_FIFO_LOST_PKT1 = 0x6D
static uint8_t UB0_REG_SELF_TEST_CONFIG = 0x70
static uint8_t UB0_REG_WHO_AM_I = 0x75
static uint8_t UB1_REG_SENSOR_CONFIG0 = 0x03
static uint8_t UB1_REG_GYRO_CONFIG_STATIC2 = 0x0B
static uint8_t UB1_REG_GYRO_CONFIG_STATIC3 = 0x0C
static uint8_t UB1_REG_GYRO_CONFIG_STATIC4 = 0x0D
static uint8_t UB1_REG_GYRO_CONFIG_STATIC5 = 0x0E
static uint8_t UB1_REG_GYRO_CONFIG_STATIC6 = 0x0F
static uint8_t UB1_REG_GYRO_CONFIG_STATIC7 = 0x10
static uint8_t UB1_REG_GYRO_CONFIG_STATIC8 = 0x11
static uint8_t UB1_REG_GYRO_CONFIG_STATIC9 = 0x12
static uint8_t UB1_REG_GYRO_CONFIG_STATIC10 = 0x13
static uint8_t UB1_REG_XG_ST_DATA = 0x5F
static uint8_t UB1_REG_YG_ST_DATA = 0x60
static uint8_t UB1_REG_ZG_ST_DATA = 0x61
static uint8_t UB1_REG_TMSTVAL0 = 0x62
static uint8_t UB1_REG_TMSTVAL1 = 0x63
static uint8_t UB1_REG_TMSTVAL2 = 0x64
static uint8_t UB1_REG_INTF_CONFIG4 = 0x7A
static uint8_t UB1_REG_INTF_CONFIG5 = 0x7B
static uint8_t UB1_REG_INTF_CONFIG6 = 0x7C
static uint8_t UB2_REG_ACCEL_CONFIG_STATIC2 = 0x03
static uint8_t UB2_REG_ACCEL_CONFIG_STATIC3 = 0x04
static uint8_t UB2_REG_ACCEL_CONFIG_STATIC4 = 0x05
static uint8_t UB2_REG_XA_ST_DATA = 0x3B
static uint8_t UB2_REG_YA_ST_DATA = 0x3C
static uint8_t UB2_REG_ZA_ST_DATA = 0x3D
static uint8_t UB4_REG_APEX_CONFIG1 = 0x40
static uint8_t UB4_REG_APEX_CONFIG2 = 0x41
static uint8_t UB4_REG_APEX_CONFIG3 = 0x42
static uint8_t UB4_REG_APEX_CONFIG4 = 0x43
static uint8_t UB4_REG_APEX_CONFIG5 = 0x44
static uint8_t UB4_REG_APEX_CONFIG6 = 0x45
static uint8_t UB4_REG_APEX_CONFIG7 = 0x46
static uint8_t UB4_REG_APEX_CONFIG8 = 0x47
static uint8_t UB4_REG_APEX_CONFIG9 = 0x48
static uint8_t UB4_REG_ACCEL_WOM_X_THR = 0x4A
static uint8_t UB4_REG_ACCEL_WOM_Y_THR = 0x4B
static uint8_t UB4_REG_ACCEL_WOM_Z_THR = 0x4C
static uint8_t UB4_REG_INT_SOURCE6 = 0x4D
static uint8_t UB4_REG_INT_SOURCE7 = 0x4E
static uint8_t UB4_REG_INT_SOURCE8 = 0x4F
static uint8_t UB4_REG_INT_SOURCE9 = 0x50
static uint8_t UB4_REG_INT_SOURCE10 = 0x51
static uint8_t UB4_REG_OFFSET_USER0 = 0x77
static uint8_t UB4_REG_OFFSET_USER1 = 0x78
static uint8_t UB4_REG_OFFSET_USER2 = 0x79
static uint8_t UB4_REG_OFFSET_USER3 = 0x7A
static uint8_t UB4_REG_OFFSET_USER4 = 0x7B
static uint8_t UB4_REG_OFFSET_USER5 = 0x7C
static uint8_t UB4_REG_OFFSET_USER6 = 0x7D
static uint8_t UB4_REG_OFFSET_USER7 = 0x7E
static uint8_t UB4_REG_OFFSET_USER8 = 0x7F
file camera_unit.cpp
#include <stdio.h>
#include “math.h”
#include “pico/stdlib.h”
#include “hardware/i2c.h”
#include “hardware/clocks.h”
#include “hardware/uart.h”
#include “hal/ov7675.h

Defines

PIN_CAM_DATA_START
PIN_SERVO
PIN_UART_TX
PIN_UART_RX
PIN_CAM_SDA
PIN_CAM_SCL
PIN_SPI_MISO
PIN_SPI_CS
PIN_SPI_CLK
PIN_SPI_MOSI
PIN_IMU_SDA
PIN_IMU_SCL
PIN_CAM_PCLK
PIN_CAM_HS
PIN_CAM_VS
PIN_CAM_XCLK

Functions

void send_bytes_over_usb(const uint8_t *data, size_t len)
int main()
file control.cpp
#include “control.h
#include <cstdint>
#include <cmath>
#include “pico/stdlib.h”

Functions

double angle_diff_rad(double input_rad, double base_rad)
void state_transition(ControlState state)
void set_control_enabled(bool enabled)
bool is_control_enabled()
uint16_t update_servo_pulse(double sun_angle_rad, double angular_velocity_rad_per_s, double target_angle_rad)
ControlState get_control_state()
bool is_control_finished()

Variables

static ControlState control_state = UNLOAD_TIME
static uint32_t state_start_time_us = 0
static uint16_t current_servo_pulse_us = 0
static bool control_enabled = true
static uint16_t target_torque_delta_us = 0
static const uint16_t servo_pulse_center_us = 1520
static const uint16_t pulse_delta_us = 1000
static const uint32_t unload_min_duration_us = 300000
static const uint32_t rotate_duration_us = 200000
static const double angle_tolerance = 10 / 180.0 * M_PI
static const double angle_velocity_tolerance_rad_per_s = (0.5 / 180.0 * M_PI)
bool is_target_in_angle = false
file control.h
#include <cstdint>

Enums

enum ControlState

Values:

enumerator UNLOAD_TIME
enumerator UNLOAD_ROTATE
enumerator STABLE
enumerator LOTATE

Functions

uint16_t update_servo_pulse(double sun_angle_rad, double angular_velocity_rad_per_s, double target_angle_rad)
void set_control_enabled(bool enabled)
bool is_control_enabled()
ControlState get_control_state()
bool is_control_finished()
file icm42688.cpp
#include “icm42688.h
#include “icm42688_registers.h
#include “pico/stdlib.h”
#include “hardware/i2c.h”
#include <string.h>
#include <math.h>
#include <stdio.h>
file icm42688.h
#include “hardware/i2c.h”
file icm42688_registers.h
#include <stdint.h>
file mcp3008.cpp
#include “mcp3008.h
#include “pico/binary_info.h”
file mcp3008.h
#include “hardware/spi.h”
#include “hardware/gpio.h”

Enums

enum AdcDifferentialMode

Values:

enumerator CH_0_MINUS_1
enumerator CH_1_MINUS_0
enumerator CH_2_MINUS_3
enumerator CH_3_MINUS_2
enumerator CH_4_MINUS_5
enumerator CH_5_MINUS_4
enumerator CH_6_MINUS_7
enumerator CH_7_MINUS_6
file ov7675.cpp
#include <stdio.h>
#include “pico/stdlib.h”
#include “hardware/i2c.h”
#include “hardware/pwm.h”
#include “hardware/gpio.h”
#include “hardware/clocks.h”

Defines

vga
qvga
qqvga
yuv422
rgb565
bayerRGB
camAddr_WR
camAddr_RD
REG_GAIN
REG_BLUE
REG_RED
REG_VREF
REG_COM1
COM1_CCIR656
REG_BAVE
REG_GbAVE
REG_AECHH
REG_RAVE
REG_COM2
COM2_SSLEEP
REG_PID
REG_VER
REG_COM3
COM3_SWAP
COM3_SCALEEN
COM3_DCWEN
REG_COM4
REG_COM5
REG_COM6
REG_AECH
REG_CLKRC
CLK_EXT
CLK_SCALE
REG_COM7
COM7_RESET
COM7_FMT_MASK
COM7_FMT_VGA
COM7_FMT_CIF
COM7_FMT_QVGA
COM7_FMT_QCIF
COM7_RGB
COM7_YUV
COM7_BAYER
COM7_PBAYER
REG_COM8
COM8_FASTAEC
COM8_AECSTEP
COM8_BFILT
COM8_AGC
COM8_AWB
COM8_AEC
REG_COM9
REG_COM10
COM10_HSYNC
COM10_PCLK_HB
COM10_HREF_REV
COM10_VS_LEAD
COM10_VS_NEG
COM10_HS_NEG
REG_HSTART
REG_HSTOP
REG_VSTART
REG_VSTOP
REG_PSHFT
REG_MIDH
REG_MIDL
REG_MVFP
MVFP_MIRROR
MVFP_FLIP
REG_AEW
REG_AEB
REG_VPT
REG_HSYST
REG_HSYEN
REG_HREF
REG_TSLB
TSLB_YLAST
REG_COM11
COM11_NIGHT
COM11_NMFR
COM11_HZAUTO
COM11_50HZ
COM11_EXP
REG_COM12
COM12_HREF
REG_COM13
COM13_GAMMA
COM13_UVSAT
COM13_UVSWAP
REG_COM14
COM14_DCWEN
REG_EDGE
REG_COM15
COM15_R10F0
COM15_R01FE
COM15_R00FF
COM15_RGB565
COM15_RGB555
REG_COM16
COM16_AWBGAIN
REG_COM17
COM17_AECWIN
COM17_CBAR
REG_CMATRIX_BASE
CMATRIX_LEN
REG_CMATRIX_SIGN
REG_BRIGHT
REG_CONTRAS
REG_GFIX
REG_REG76
R76_BLKPCOR
R76_WHTPCOR
REG_RGB444
R444_ENABLE
R444_RGBX
REG_HAECC1
REG_HAECC2
REG_BD50MAX
REG_HAECC3
REG_HAECC4
REG_HAECC5
REG_HAECC6
REG_HAECC7
REG_BD60MAX
REG_GAIN
REG_BLUE
REG_RED
REG_VREF
REG_COM1
COM1_CCIR656
REG_BAVE
REG_GbAVE
REG_AECHH
REG_RAVE
REG_COM2
COM2_SSLEEP
REG_PID
REG_VER
REG_COM3
COM3_SWAP
COM3_SCALEEN
COM3_DCWEN
REG_COM4
REG_COM5
REG_COM6
REG_AECH
REG_CLKRC
CLK_EXT
CLK_SCALE
REG_COM7
COM7_RESET
COM7_FMT_MASK
COM7_FMT_VGA
COM7_FMT_CIF
COM7_FMT_QVGA
COM7_FMT_QCIF
COM7_RGB
COM7_YUV
COM7_BAYER
COM7_PBAYER
REG_COM8
COM8_FASTAEC
COM8_AECSTEP
COM8_BFILT
COM8_AGC
COM8_AWB
COM8_AEC
REG_COM9
REG_COM10
COM10_HSYNC
COM10_PCLK_HB
COM10_HREF_REV
COM10_VS_LEAD
COM10_VS_NEG
COM10_HS_NEG
REG_HSTART
REG_HSTOP
REG_VSTART
REG_VSTOP
REG_PSHFT
REG_MIDH
REG_MIDL
REG_MVFP
MVFP_MIRROR
MVFP_FLIP
REG_AEW
REG_AEB
REG_VPT
REG_HSYST
REG_HSYEN
REG_HREF
REG_TSLB
TSLB_YLAST
REG_COM11
COM11_NIGHT
COM11_NMFR
COM11_HZAUTO
COM11_50HZ
COM11_EXP
REG_COM12
COM12_HREF
REG_COM13
COM13_GAMMA
COM13_UVSAT
COM13_UVSWAP
REG_COM14
COM14_DCWEN
REG_EDGE
REG_COM15
COM15_R10F0
COM15_R01FE
COM15_R00FF
COM15_RGB565
COM15_RGB555
REG_COM16
COM16_AWBGAIN
REG_COM17
COM17_AECWIN
COM17_CBAR
CMATRIX_LEN
REG_BRIGHT
REG_REG76
R76_BLKPCOR
R76_WHTPCOR
REG_RGB444
R444_ENABLE
R444_RGBX
REG_HAECC1
REG_HAECC2
REG_BD50MAX
REG_HAECC3
REG_HAECC4
REG_HAECC5
REG_HAECC6
REG_HAECC7
REG_BD60MAX
MTX1
MTX2
MTX3
MTX4
MTX5
MTX6
REG_CONTRAS
MTXS
AWBC7
AWBC8
AWBC9
AWBC10
AWBC11
AWBC12
REG_GFI
GGAIN
DBLV
AWBCTR3
AWBCTR2
AWBCTR1
AWBCTR0
OV7675_I2C_ADDR

Functions

void xclk_init()
int camera_write_reg(uint8_t reg, uint8_t value)
uint8_t camera_read_reg(uint8_t reg)
void wrReg(uint8_t reg, uint8_t dat)
void wrSensorRegs8_8(const struct regval_list reglist[])
void setColor(void)
void setColorRGB565(void)
void camera_set_yuv422(void)
void camera_set_rgb565(void)
void setRes(void)
void ov7675_setup(void)
void camera_init(i2c_inst_t *i2c_port, uint8_t i2c_sda_pin, uint8_t i2c_scl_pin, uint8_t pin_xclk, uint8_t pin_vsync, uint8_t pin_href, uint8_t pin_pclk, uint8_t data_pin_start)
void capture_frame(uint8_t *buffer, int width, int height)
void capture_frame_rgb(uint8_t *buffer, int width, int height)

Variables

const struct regval_list qvga_ov7670[] = {{REG_COM7, COM7_FMT_QVGA}, {REG_COM3, COM3_SCALEEN | COM3_DCWEN}, {REG_COM14, 0x19}, {0x70, 0x3a}, {0x71, 0x3a}, {0x72, 0x11}, {0x73, 0xf1}, {REG_HSTART, 0x16}, {REG_HSTOP, 0x04}, {REG_HREF, 0xa4}, {REG_VSTART, 0x02}, {REG_VSTOP, 0x7a}, {REG_VREF, 0x0a}, {0xff, 0xff},}
const struct regval_list yuv422_ov7670[] = {{REG_COM7, COM7_FMT_QVGA | COM7_YUV}, {REG_RGB444, 0}, {REG_COM1, 0}, {REG_COM15, COM15_R00FF}, {REG_COM9, 0x6A}, {0x4f, 0x80}, {0x50, 0x80}, {0x51, 0}, {0x52, 0x22}, {0x53, 0x5e}, {0x54, 0x80}, {REG_COM13, COM13_UVSAT}, {0xff, 0xff},}
const struct regval_list rgb565_ov7670[] = {{REG_COM7, COM7_FMT_QVGA | COM7_RGB}, {REG_RGB444, 0}, {REG_COM1, 0}, {REG_COM15, COM15_R00FF | COM15_RGB565}, {REG_COM9, 0x6A}, {0x4f, 0xb3}, {0x50, 0xb3}, {0x51, 0x00}, {0x52, 0x3d}, {0x53, 0xa7}, {0x54, 0xe4}, {REG_COM13, COM13_GAMMA | COM13_UVSAT}, {0xff, 0xff},}
const struct regval_list ov7670_default_regs[]
static i2c_inst_t *I2C_PORT
uint DATA_PIN_START = 0
uint NUM_DATA_PINS = 8
uint PIN_HREF = 0
uint PIN_VSYNC = 0
uint PIN_XCLK = 0
uint PIN_PCLK = 0
const int IMG_WIDTH = 320
const int IMG_HEIGHT = 240
file ov7675.h

Functions

void camera_init(i2c_inst_t *i2c_port, uint8_t i2c_sda_pin, uint8_t i2c_scl_pin, uint8_t pin_xclk, uint8_t pin_vsync, uint8_t pin_href, uint8_t pin_pclk, uint8_t data_pin_start)
void capture_frame(uint8_t *buffer, int width, int height)
void capture_frame_rgb(uint8_t *buffer, int width, int height)
void camera_set_yuv422(void)
void camera_set_rgb565(void)
file servo.cpp
#include “servo.h
#include “pico/stdlib.h”
#include “hardware/pwm.h”
#include “hardware/clocks.h”
#include “stdio.h”

Defines

TARGET_FREQ
SERVO_PERIOD_US

Functions

void servo_init(uint8_t servo_pin)
void servo_set_pulse(uint8_t servo_pin, uint16_t pulse_us)

Variables

static uint slice_num
file servo.h
#include <stdint.h>

Functions

void servo_init(uint8_t pin)
void servo_set_pulse(uint8_t servo_pin, uint16_t pulse_us)
file wifi.cpp
#include “wifi.h
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include “pico/stdlib.h”
#include “pico/cyw43_arch.h”
#include “lwip/tcp.h”
#include “lwip/ip_addr.h”
#include “lwip/dhcp.h”
#include “lwip/netif.h”

Defines

TCP_PORT
TCP_PORT_TLM

Functions

static err_t recv_callback(void *arg, struct tcp_pcb *tpcb, struct pbuf *p, err_t err)
static err_t accept_callback(void *arg, struct tcp_pcb *newpcb, err_t err)
static void tlm_error_callback(void *arg, err_t err)
static err_t tlm_accept_callback(void *arg, struct tcp_pcb *newpcb, err_t err)
int wifi_init(const char *ssid, const char *password)
int wifi_deinit()
int wifi_read(uint8_t *data, int len)
int wifi_write(const uint8_t *data, int len)

Variables

struct rcv_data_t rcv_data
static struct tcp_pcb *tlm_pcb = NULL
file wifi.h
#include <stdint.h>
#include <stdbool.h>

Functions

int wifi_init(const char *ssid, const char *password)
int wifi_deinit()
int wifi_read(uint8_t *data, int len)
int wifi_write(const uint8_t *data, int len)
file lwipopts.h

Defines

NO_SYS
LWIP_SOCKET
MEM_LIBC_MALLOC
MEM_ALIGNMENT
MEM_SIZE
MEMP_NUM_TCP_SEG
MEMP_NUM_ARP_QUEUE
PBUF_POOL_SIZE
LWIP_ARP
LWIP_ETHERNET
LWIP_ICMP
LWIP_RAW
TCP_WND
TCP_MSS
TCP_SND_BUF
TCP_SND_QUEUELEN
LWIP_NETIF_STATUS_CALLBACK
LWIP_NETIF_HOSTNAME
LWIP_NETCONN
MEM_STATS
SYS_STATS
MEMP_STATS
LWIP_CHKSUM_ALGORITHM
LWIP_DHCP
LWIP_IPV4
LWIP_TCP
LWIP_UDP
LWIP_DNS
LWIP_TCP_KEEPALIVE
LWIP_NETIF_TX_SINGLE_PBUF
DHCP_DOES_ARP_CHECK
LWIP_DHCP_DOES_ACD_CHECK
LWIP_DEBUG
LWIP_STATS
LWIP_STATS_DISPLAY
ETHARP_DEBUG
NETIF_DEBUG
PBUF_DEBUG
API_LIB_DEBUG
API_MSG_DEBUG
SOCKETS_DEBUG
ICMP_DEBUG
INET_DEBUG
IP_DEBUG
IP_REASS_DEBUG
RAW_DEBUG
MEM_DEBUG
MEMP_DEBUG
SYS_DEBUG
TCP_DEBUG
TCP_INPUT_DEBUG
TCP_OUTPUT_DEBUG
TCP_RTO_DEBUG
TCP_CWND_DEBUG
TCP_WND_DEBUG
TCP_FR_DEBUG
TCP_QLEN_DEBUG
TCP_RST_DEBUG
UDP_DEBUG
TCPIP_DEBUG
PPP_DEBUG
SLIP_DEBUG
DHCP_DEBUG
file main.cpp
#include <stdio.h>
#include “math.h”
#include “pico/stdlib.h”
#include “hardware/spi.h”
#include “hardware/i2c.h”
#include “hardware/clocks.h”
#include “pico/cyw43_arch.h”
#include “hardware/uart.h”
#include “hardware/watchdog.h”
#include “hardware/vreg.h”
#include “control.h
#include “hal/icm42688.h
#include “hal/mcp3008.h
#include “hal/ov7675.h
#include “hal/servo.h
#include “hal/wifi.h

Defines

PIN_CAM_DATA_START
PIN_SERVO
PIN_UART_TX
PIN_UART_RX
PIN_CAM_SDA
PIN_CAM_SCL
PIN_SPI_MISO
PIN_SPI_CS
PIN_SPI_CLK
PIN_SPI_MOSI
PIN_IMU_SDA
PIN_IMU_SCL
PIN_CAM_PCLK
PIN_CAM_HS
PIN_CAM_VS
PIN_CAM_XCLK

Functions

int main()
dir hal

Python 地上局ソフトウェア (ground)

地上局側で動作する Python モジュールの API リファレンスです。

Backend

地上局のバックエンドサーバーおよび通信ブリッジです。

FastAPI 地上局バックエンド

MQTT からテレメトリ/画像を受信し、WebSocket でフロントエンドへリアルタイム配信。 InfluxDB クエリ API と コマンド送信 API を提供。

class server.CommandRequest(*, command: str)[source]

Bases: BaseModel

command: str
model_config = {}

Configuration for the model, should be a dictionary conforming to [ConfigDict][pydantic.config.ConfigDict].

class server.ConnectionManager[source]

Bases: object

active_connections: list[WebSocket]
async broadcast(message: str)[source]
async connect(websocket: WebSocket)[source]
disconnect(websocket: WebSocket)[source]
server.broadcast_threadsafe(message: dict) None[source]

MQTT スレッド等イベントループ外からも安全に WebSocket ブロードキャストする。

async server.get_commands(limit: int = 100, pass_id: str = '')[source]

送信済みコマンドの履歴を取得(古い順)

async server.get_history(field: str = 'battery_volt', duration: str = '-5m', session: str = '')[source]

InfluxDB から過去のテレメトリを取得

async server.get_passes()[source]

InfluxDB に記録されたパス一覧を取得(新しい順)

async server.get_sessions()[source]

InfluxDB に保存されたセッション一覧を取得

async server.get_status()[source]

バックエンド各レイヤーの接続状態と現在のパスを返す

async server.get_telemetry_history(limit: int = 0, pass_id: str = '', max_points: int = 0)[source]

InfluxDB からテレメトリ全フィールドを取得。

用途別の使い分け:
  • ライブ初期ロード: pass_id + limit(直近 limit 点のみ)/ もしくは pass 未指定で直近 limit 点

  • リプレイ: pass_id のみ(パス全体)+ max_points(多すぎる場合はサーバ側で間引き)

max_points を指定すると、返却点数がそれを超える場合に等間隔で間引いて軽量化する。

server.lifespan(app: FastAPI)[source]
async server.list_images()[source]

保存済み画像一覧を返す(新しい順)

async server.new_pass()[source]

手動で新しいパスを開始する(運用者操作)

server.on_connect(client, userdata, flags, rc, properties)[source]
server.on_disconnect(client, userdata, flags, rc, properties)[source]
server.on_image_msg(client, userdata, msg)[source]

画像受信 → JPG 保存 → WebSocket 通知

server.on_telemetry_msg(client, userdata, msg)[source]

テレメトリ受信 → InfluxDB 保存 & WebSocket ブロードキャスト

async server.send_command(req: CommandRequest)[source]

コマンドを MQTT トピックに publish し、履歴を InfluxDB に記録する

server.start_mqtt()[source]

MQTT クライアントを別スレッドで起動

server.start_new_pass(reason: str, ts: float | None = None) str[source]

新しいパスを開始してフロントへ通知する。

Parameters:
  • reason – 区切りの理由 (“init”=初回 / “gap”=通信途絶検出 / “manual”=手動)

  • ts – パス開始時刻 (Unix 秒)。省略時は現在時刻。

Returns:

新しい pass_id(開始時刻ベースの文字列)

async server.websocket_endpoint(websocket: WebSocket)[source]
server.write_command(command: str, status: str, ts: float) None[source]

送信コマンドを InfluxDB に記録する(後から履歴として参照可能)。

Simulator (SILS)

模擬衛星プログラムです。

連続時間 物理環境モデル (Plant)

衛星の力学系・センサ応答をシミュレートする。 制御ロジックとは独立し、外部から与えられたサーボパルス指令に応じて状態を更新する。

class simulator.plant.SatellitePlant[source]

Bases: object

衛星の物理モデル

FRICTION = 0.008
MOMENT_OF_INERTIA = 0.002
RW_INERTIA = 0.0002
SERVO_CENTER = 1520
SERVO_TORQUE_GAIN = 5e-06
get_sensor_readings() dict[source]

現在の状態からセンサ値を生成(ADCノイズ含む)

reset()[source]

状態を初期化する

step(dt: float, servo_pulse_us: int)[source]

物理モデルを dt [s] だけ進める

Parameters:
  • dt – タイムステップ [s]

  • servo_pulse_us – サーボパルス指令 [μs]

property sun_angle: float

太陽方向に対する衛星の角度 [rad] (Environmentから計算)

離散時間 制御ロジック (Controller)

satelite/control.cpp の Python 移植。 衛星のメインループ(~200Hz)ごとに呼ばれ、サーボパルス指令を返す。

class sils.controller.ControlState[source]

Bases: object

LOTATE = 3
STABLE = 2
UNLOAD_ROTATE = 1
UNLOAD_TIME = 0
class sils.controller.SatelliteController[source]

Bases: object

衛星の姿勢制御ステートマシン (control.cpp の移植)

ANGLE_TOLERANCE_RAD = 0.17453292519943295
ANGULAR_VELOCITY_TOLERANCE_RAD_PER_S = 0.008726646259971648
PULSE_DELTA_US = 1000
ROTATE_DURATION_US = 200000
SERVO_CENTER_US = 1520
UNLOAD_MIN_DURATION_US = 300000
control_enabled: bool
current_servo_pulse_us: int
get_control_state() int[source]
is_control_enabled() bool[source]
is_control_finished() bool[source]
is_target_in_angle: bool
reset()[source]

状態を初期化する

set_control_enabled(enabled: bool)[source]
set_target(angle_rad: float)[source]

目標角度を設定

state_start_time_us: int
target_angle_rad: float
update(dt_us: int, sun_angle_rad: float, angular_velocity_rad_per_s: float) int[source]

制御ループ1ステップ(衛星メインループ相当)

Parameters:
  • dt_us – 経過時間 [μs]

  • sun_angle_rad – 太陽センサから推定した角度 [rad]

  • angular_velocity_rad_per_s – IMU角速度 [rad/s]

Returns:

サーボパルス指令 [μs]