Project files

Control Frame for the FinsROV Lower-Level Controller

I. Project Overview

This project implements the lower-level control program for FinsROV. Its main functions include:

1. Attitude sensing: Sensor data from the BMI088 (gyroscope + accelerometer) and the optional IST8310 magnetometer are acquired via SPI + DMA, and the MahonyAHRS algorithm is used to estimate attitude (quaternion / Euler angles).

2. Depth and attitude-related estimation: Four MS5837-30BA pressure sensors are read via I2C with temperature compensation, providing estimates of depth, pitch, and roll.

3. Thruster control: A PCA9685 (PWM expansion board) drives 8 thrusters. The system supports depth closed-loop PID control and roll/pitch double-loop PID control, and also supports open-loop control via serial communication.

4. Servo control: A PCA9685 expansion board drives 4 servos.

5. Serial communication: Commands are received via UART interrupts and then dispatched to each device module for parsing.

II. Overall System Architecture

Unified Device Management (Device Framework)

In Usermain.h, the system abstracts each hardware functional module as a Device class (interfaces: Init / Receive / Handle) to uniformly manage all devices:

• Init(): Initializes all devices. It is executed in the main program Usermain.cpp, after the initialization of on-board (Robomaster-C board) devices.

• Receive(): Invoked in the UART interrupt service routine HAL_UARTEx_RxEventCallback. When serial data are received, they are stored in RxBuffer.

• Handle(): Periodically processes each device.

In Usermain.cpp, devices are registered through a pointer array:

• IMU::imu
• PressureSensor::pressure_sensor
• Propeller_I2C
• Servo_I2C
• LED

Main Loop Scheduling

The system uses the TIM1 periodic interrupt as the primary scheduler:

1. In HAL_TIM_PeriodElapsedCallback(), when htim == &htim1, the flag tick_XXXhz is set.

2. In the main loop while(1), this tick flag is checked:

   • Flag set → clear the flag → iterate over device[] and execute Handle() for each device.

   • Other .h/.cpp files inherit from the Device class to implement different device types (e.g., imu, pressure_sensor, propeller_i2c, servo_i2c, led) and provide concrete implementations of the above three pure virtual functions.

UART Reception

UART6 uses HAL_UARTEx_ReceiveToIdle_IT() for reception, and an idle event triggers HAL_UARTEx_RxEventCallback().

In the UART6 Rx callback, Receive() is invoked for all devices in the device[] array.

III. Module Descriptions

IMU.cpp

1. Initialize BMI088.

2. SPI1 DMA acquire in segment:

   • gyroscope (Gyro)
   • accelerometer (Accel)
   • temperature (Temp)
   • magnetometer (Mag)

3. Apply bias correction and scaling:

   • offset_correction()

4. Use MahonyAHRS to compute quaternions and convert to Euler angles:

   • MahonyAHRSupdate(...)
   • get_angle(...)

5. Update global attitude (yaw/pitch/roll):

   • attitude_update()

Sensor.cpp

Pressure Sensor

The program uses four distributed MS5837-30BA pressure sensors and accesses them in a time-multiplexed manner via an I2C expansion board (before reading each channel, TCA_SetChannel(i) is called to switch the channel).

The overall acquisition logic in Handle_all() is as follows:

1. Initiate temperature conversion (GET_TEMPERATURE): Switch sequentially to the four channels and send a “start temperature conversion” command to each sensor.

2. Read temperature results and initiate pressure conversion (GET_PRESSURE): Read the temperature conversion result for each channel, and then sequentially send a “start pressure conversion” command for each channel.

3. Read pressure results and compute depth (CALCULATE): Read the pressure conversion result for each channel, then perform compensation and calibration to obtain the depth estimate for that channel.

Propeller.cpp

Thruster Control Method

PWM output is generated by the PCA9685; channels 0–7 correspond to the 8 thrusters.

Thruster Index Definition

• InID[4]: Mapping of the inner ring (vertical thrusters) to PCA channels
• OutID[4]: Mapping of the outer ring (horizontal thrusters) to PCA channels
• Sign[8]: Rotation direction (CW/CCW) for each thruster (+1/−1)

Hover Control Mode (flag_float)

When enabled:

• Inner-ring thrusters perform closed-loop control for depth, roll, and pitch.
• Outer-ring thrusters perform open-loop translation/rotation (WASD/QE/Z).

When disabled:

• All thrusters output InitPWM (stop).

PID Structure

• Depth: DepthPID
• Roll/Pitch: double-loop
  • Outer loop uses pressure-sensor differential signals:
    • RollOutPID(Target_roll, pressure_roll_diff)
    • PitchOutPID(Target_pitch, pressure_pitch_diff)
  • Inner loop uses IMU angular rates:
    • Roll inner loop uses IMU::imu.attitude.neg_roll_v (note the sign inversion)
    • Pitch inner loop uses IMU::imu.attitude.pitch_v

Thruster Control Method

PCA9685 is used to output PWM, with channels 0-7 corresponding to the 8 thrusters respectively.

Thruster Index Definition

• InID[4]: Inner ring (vertical thrusters) mapped to PCA channels
• OutID[4]: Outer ring (horizontal thrusters) mapped to PCA channels
• Sign[8]: Forward/reverse direction of each thruster (+1/-1)

Hover Control Mode (flag_float)

When enabled:

• Inner ring thrusters perform closed-loop control for depth, roll, and pitch
• Outer ring thrusters perform open-loop translation/rotation (WASD/QE/Z)

When disabled:

• All thrusters output InitPWM (stop)

PID Structure

• Depth: DepthPID
• Roll/Pitch: Dual-loop PID
  • Outer loop uses differential pressure sensor:
    • RollOutPID(Target_roll, pressure_roll_diff)
    • PitchOutPID(Target_pitch, pressure_pitch_diff)
  • Inner loop uses IMU angular velocity:
    • Inner loop for Roll uses IMU::imu.attitude.neg_roll_v (note the sign inversion)
    • Inner loop for Pitch uses IMU::imu.attitude.pitch_v

Servo.cpp

Servos are controlled via the PWM expansion board:

• The Servo_I2C::Handle() function first calls TCA_SetChannel to select the I2C expansion channel.
• Then, for each servo, PCA_Setpwm(uint8_t num, uint32_t on, uint32_t off) is called to control the angle through the PWM duty cycle.

Extension.cpp

This file defines no class type and only contains function implementations for handling the I2C expansion board and the PWM expansion board.

IV. Serial Communication Protocol

V. Internal Debugging Issue Records (for external reference only)

1. The initial PWM values for the ESCs of V33-0 and V33-2 are different. If you do not hear the ESC initialization sound after power-on, you can try modifying the initial PWM value in the propeller.cpp file of the lower-level controller code.

2. The wiring sequences of the RS485/422-to-TTL converters inside V33-0 and V33-2 are temporarily different (discovered during debugging on April 5, 2026). If you use the cable reel with the camera and the receiver end works normally but the transmitter end does not respond when sending information, you may need to adjust the wiring sequence of the RS485/422-to-USB converter on the transmitter end (swap the T/R+ and T/R- wires).