Ardusub source code analysis learning (two)-motor library learning
1. RC input and output
1.1 RC Input
Before starting to learn Ardusub's motor library, let's take a look at its RC input and output. The most basic channels for motion control are declared in Sub.h. There are 6 input channels, which are:
// primary input control channels
RC_Channel *channel_roll;
RC_Channel *channel_pitch;
RC_Channel *channel_throttle;
RC_Channel *channel_yaw;
RC_Channel *channel_forward;
RC_Channel *channel_lateral;
Let me first explain that the most basic vehicle type is defined in Sub.h, namely the Sub class, which contains the most basic variables and functions for controlling underwater unmanned vehicles.
The meaning of these channels is to control the movement of the underwater unmanned submarine in a certain direction through the channel input. It does not refer to a specific motor, usually a channel affects many related to its control motion Motor. Everyone should be familiar with the first four. Aerial drones basically contain these four input control channels. The last two I mentioned briefly in my previous blog post. These are the forward and backward translation of underwater drones. And RC input channel for left and right pan control.
The official manual states that the RC input is a set of control channels, representing the operator's input, such as forward and yaw commands. Although the underwater unmanned vehicle system Ardusub may not accept RC receivers, the input control logic using joysticks (similar to game consoles, etc.) is similar. In addition to the most basic motion control channels, the official also lists other default input channel mappings, as shown in the following table.
| Channel | Meaning |
|---|---|
| 1 | Pitch |
| 2 | Roll |
| 3 | Throttle |
| 4 | Yaw |
| 5 | Forward |
| 6 | Lateral |
| 7 | Camera Pan |
| 8 | Camera Tilt |
| 9 | Lights 1 Level |
| 10 | Lights 2 Level |
| 11 | Video Switch |
1.2 RC Output
Pixhawk 2.4.8 should belong to the first generation of products. It has 8 main output channels and 6 auxiliary output channels. Among them, 8 MAIN OUTs are connected to underwater propellers and output PWM waves for control. The update frequency is 200Hz. The 6 AUX OUTs can also generate PWM control signals to control some auxiliary devices such as gimbal servos, light brightness, etc. (Of course you can also use MAIN OUT), but it should be noted that channels 5 and 6 are reserved by default. Ordinary GPIO use.
The default configuration of the number of PWM outputs for AUX OUT is determined by the BRD_PWM_COUNT parameter , and the default is 4. Optional configuration as shown
2. Motor library learning
Ardusub's motor library configuration is located in the AP_Motors6DOF.cpp/AP_Motors6DOF.h file under the libraries\AP_Motors path. The internal inheritance relationship is as follows.
AP_Motors
| ---- AP_MotorsMulticopter
| ---- AP_MotorsMatrix
| ---- AP_Motors6DOF
The AP_Motors6DOF class is defined in AP_Motors6DOF.h, which inherits from the AP_MotorsMatrix class upwards, and adds some ROV control exclusive variables and functions to the inside. Here is mainly to learn about the code content in the AP_Motors6DOF.cpp file.
2.1 setup_motors()
Since the internal code is still a bit small and long, I won't put it all in here, and the excerpt will be explained.
The main content of this function is to configure the motors, including the degree of influence of each motor on different movements.
At the beginning of the function, the first thing is to remove all the original motor configurations to facilitate subsequent changes. Here AP_MOTORS_MAX_NUM_MOTORS is the maximum number of motors, which is defined as 12 in AP_Motors_Class.h.
// remove existing motors
for (int8_t i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
remove_motor(i);
}
Then enter a switch() function for specific configuration content. First, determine which ROV architecture belongs to. Ardusub gives 8 configuration structures, which are defined in the enumeration type in AP_Motors6DOF.h, as shown below Show
// Supported frame types
typedef enum {
SUB_FRAME_BLUEROV1,
SUB_FRAME_VECTORED,
SUB_FRAME_VECTORED_6DOF,
SUB_FRAME_VECTORED_6DOF_90DEG,
SUB_FRAME_SIMPLEROV_3,
SUB_FRAME_SIMPLEROV_4,
SUB_FRAME_SIMPLEROV_5,
SUB_FRAME_CUSTOM
} sub_frame_t;
The specific corresponding types are given as follows. The CUSTOM at the end of the above is the supported user-defined architecture.
Take vectored6dof for example, its configuration content is as follows:
// Motor # Roll Factor Pitch Factor Yaw Factor Throttle Factor Forward Factor Lateral Factor Testing Order
case SUB_FRAME_VECTORED_6DOF:
add_motor_raw_6dof(AP_MOTORS_MOT_1, 0, 0, 1.0f, 0, -1.0f, 1.0f, 1);
add_motor_raw_6dof(AP_MOTORS_MOT_2, 0, 0, -1.0f, 0, -1.0f, -1.0f, 2);
add_motor_raw_6dof(AP_MOTORS_MOT_3, 0, 0, -1.0f, 0, 1.0f, 1.0f, 3);
add_motor_raw_6dof(AP_MOTORS_MOT_4, 0, 0, 1.0f, 0, 1.0f, -1.0f, 4);
add_motor_raw_6dof(AP_MOTORS_MOT_5, 1.0f, -1.0f, 0, -1.0f, 0, 0, 5);
add_motor_raw_6dof(AP_MOTORS_MOT_6, -1.0f, -1.0f, 0, -1.0f, 0, 0, 6);
add_motor_raw_6dof(AP_MOTORS_MOT_7, 1.0f, 1.0f, 0, -1.0f, 0, 0, 7);
add_motor_raw_6dof(AP_MOTORS_MOT_8, -1.0f, 1.0f, 0, -1.0f, 0, 0, 8);
break;
The function of add_motor_raw_6dof() is to configure the influence factor of each motor on a certain motion function, generally -1~1. For motor5, its influence on the ROLL direction movement during ROV movement is 1, while for the PITCH direction movement, it is counteracting -1, and its influence factor on the throttle Throttle control ups and downs is -1. The positive and negative of the edge is related to the installation direction of the underwater propeller and the positive and negative propellers.
2.2 add_motor_raw_6dof()
void AP_Motors6DOF::add_motor_raw_6dof(int8_t motor_num, float roll_fac, float pitch_fac, float yaw_fac, float throttle_fac, float forward_fac, float lat_fac, uint8_t testing_order)
{
//Parent takes care of enabling output and setting up masks
add_motor_raw(motor_num, roll_fac, pitch_fac, yaw_fac, testing_order);
//These are additional parameters for an ROV
_throttle_factor[motor_num] = throttle_fac;
_forward_factor[motor_num] = forward_fac;
_lateral_factor[motor_num] = lat_fac;
}
The code content is as above. This part of the program directly calls the add_motor_raw() function in AP_MotorsMatrix.cpp. The original function of this function is to configure the influence factor of each motor in the RPY direction of the aerial drone on each direction, but the ROV is In underwater movement, the functions of ups and downs, forward and backward translation and left and right translation are added, so 3 related influencing factor configuration arrays are added at the back.
2.3 output_min()
// output_min - sends minimum values out to the motors
void AP_Motors6DOF::output_min()
{
int8_t i;
// set limits flags
limit.roll = true;
limit.pitch = true;
limit.yaw = true;
limit.throttle_lower = false;
limit.throttle_upper = false;
// fill the motor_out[] array for HIL use and send minimum value to each motor
// ToDo find a field to store the minimum pwm instead of hard coding 1500
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
rc_write(i, 1500);
}
}
}
The function of this function is to set the minimum output to the motor. Because ROV has the function of sinking and floating underwater, it also depends on the forward and reverse rotation of the underwater propeller. So, for example, the propeller with 20ms as the PWM control period , Different from the propeller on the air drone which only has one direction of rotation function, the underwater propeller needs to reach the maximum number of revolutions in different steering, so its minimum speed is in the middle position, that is, when pwm = 1500 (not considering safety restrictions 1000 is the maximum reverse direction, 2000 is the maximum forward direction). The previous limit is whether the configuration has reached the limit of a certain movement, for example, the thruster has reached the maximum movement capacity when it reaches pwm=1000 or 2000.
For details on the control of underwater propeller, please refer to my blog: STM32 controls ESC30C ESC through PWM
2.4 calc_thrust_to_pwm()
int16_t AP_Motors6DOF::calc_thrust_to_pwm(float thrust_in) const
{
return constrain_int16(1500 + thrust_in * 400, _throttle_radio_min, _throttle_radio_max);
}
As the name implies, the function of this function is to convert -1~1 thrust_in input to pwm output. Due to the increased safety restrictions, the pwm control range here is from 1100 to 1900. The latter two parameters are used for limiting.
2.5 output_to_motors()
void AP_Motors6DOF::output_to_motors()
{
int8_t i;
int16_t motor_out[AP_MOTORS_MAX_NUM_MOTORS]; // final pwm values sent to the motor
switch (_spool_state) {
case SpoolState::SHUT_DOWN:
// sends minimum values out to the motors
// set motor output based on thrust requests
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
motor_out[i] = 1500;
}
}
break;
case SpoolState::GROUND_IDLE:
// sends output to motors when armed but not flying
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
motor_out[i] = 1500;
}
}
break;
case SpoolState::SPOOLING_UP:
case SpoolState::THROTTLE_UNLIMITED:
case SpoolState::SPOOLING_DOWN:
// set motor output based on thrust requests
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
motor_out[i] = calc_thrust_to_pwm(_thrust_rpyt_out[i]);
}
}
break;
}
// send output to each motor
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
rc_write(i, motor_out[i]);
}
}
}
This function is also very simple. The first motor_out array is used to store the last pwm value of each motor. The switch function judges the axis status. For details about the axis status, see my previous blog post. Here you only need to know that the thruster is not working in SHUT_DOWN and GROUND_IDLE states. In SPOOLING_UP, THROTTLE_UNLIMITED and SPOOLING_DOWN states, call the calc_thrust_to_pwm() function for each motor to calculate the final pwm value and save it in the motor_out array. Finally, output to each channel through the rc_write() function.
2.6 get_current_limit_max_throttle()
float AP_Motors6DOF::get_current_limit_max_throttle()
{
return 1.0f;
}
It's too simple to explain, just attach the code.
2.7 output_armed_stabilizing()
This function is the most important, which is divided into motor part and current part. Let’s talk about the control part of the motor here, and it’s easy for everyone to study the current part.
At the beginning of this code, first determine what type of frame structure is. If it is vectored, call output_armed_stabilizing_vectored() function. If it is vectored_6odf, then call output_armed_stabilizing_vectored_6dof() for a long time. Otherwise, directly enter the else part of the program (specifically See section 2.1 for the framework). The internal procedures of these functions are roughly similar, so here only the code inside the else is used as an explanation.
if ((sub_frame_t)_last_frame_class == SUB_FRAME_VECTORED) {
output_armed_stabilizing_vectored();
} else if ((sub_frame_t)_last_frame_class == SUB_FRAME_VECTORED_6DOF) {
output_armed_stabilizing_vectored_6dof();
} else {
uint8_t i; // general purpose counter
float roll_thrust; // roll thrust input value, +/- 1.0
float pitch_thrust; // pitch thrust input value, +/- 1.0
float yaw_thrust; // yaw thrust input value, +/- 1.0
float throttle_thrust; // throttle thrust input value, +/- 1.0
float forward_thrust; // forward thrust input value, +/- 1.0
float lateral_thrust; // lateral thrust input value, +/- 1.0
roll_thrust = (_roll_in + _roll_in_ff);
pitch_thrust = (_pitch_in + _pitch_in_ff);
yaw_thrust = (_yaw_in + _yaw_in_ff);
throttle_thrust = get_throttle_bidirectional();
forward_thrust = _forward_in;
lateral_thrust = _lateral_in;
float rpy_out[AP_MOTORS_MAX_NUM_MOTORS]; // buffer so we don't have to multiply coefficients multiple times.
float linear_out[AP_MOTORS_MAX_NUM_MOTORS]; // 3 linear DOF mix for each motor
// initialize limits flags
limit.roll = false;
limit.pitch = false;
limit.yaw = false;
limit.throttle_lower = false;
limit.throttle_upper = false;
// sanity check throttle is above zero and below current limited throttle
if (throttle_thrust <= -_throttle_thrust_max) {
throttle_thrust = -_throttle_thrust_max;
limit.throttle_lower = true;
}
if (throttle_thrust >= _throttle_thrust_max) {
throttle_thrust = _throttle_thrust_max;
limit.throttle_upper = true;
}
// calculate roll, pitch and yaw for each motor
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
rpy_out[i] = roll_thrust * _roll_factor[i] +
pitch_thrust * _pitch_factor[i] +
yaw_thrust * _yaw_factor[i];
}
}
// calculate linear command for each motor
// linear factors should be 0.0 or 1.0 for now
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
linear_out[i] = throttle_thrust * _throttle_factor[i] +
forward_thrust * _forward_factor[i] +
lateral_thrust * _lateral_factor[i];
}
}
// Calculate final output for each motor
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
_thrust_rpyt_out[i] = constrain_float(_motor_reverse[i]*(rpy_out[i] + linear_out[i]),-1.0f,1.0f);
}
}
}
- When the function comes in, we first define the thrusters acting in each direction of motion (this does not refer to a single physical thruster, but an abstract concept of thrusters that can act in a certain direction of motion).
- Then, the thruster in the RPY direction obtains the desired attitude value and the expected attitude value obtained by the feedforward through the attitude controller and adds them; the thruster in the sink and float direction acquires the filtered input control value through the get_throttle_bidirectional() function; The thruster in the translation direction obtains the latest input instruction in the translation direction from the set_xxx method. The above control range is -1~1.
- Create two arrays to save the control variables of each motor in the RPY direction and the translation direction. And setting each movement direction through limit has not reached the limit. Then limit the control amount of the thruster in the ups and downs direction, if the limit value is reached, the corresponding limit value is changed to true.
- For each motor, synthesize the control quantity signals in each direction and the pre-set motion influence factors to obtain the final control signal (including all motors in the RPY direction and the translation direction).
- Finally, judge whether each motor has been activated. If it is activated, the rotation and translation amount will be integrated and multiplied by the direction coefficient (_motor_reverse[i] has been defined at the beginning of the file, and it is not released in this article. Generally, it is 1. ), and then stored in the _thrust_rpyt_out array (this array is defined in AP_MotorsMatrix.h, used to aggregate the final motor output control amount).
Other functions
get_motor_angular_factors() : Get the influence factors of each motor's attitude control.
Vector3f AP_Motors6DOF::get_motor_angular_factors(int motor_number) {
if (motor_number < 0 || motor_number >= AP_MOTORS_MAX_NUM_MOTORS) {
return Vector3f(0,0,0);
}
return Vector3f(_roll_factor[motor_number], _pitch_factor[motor_number], _yaw_factor[motor_number]);
}
motor_is_enabled() : Determine whether the motor has been started and whether the number of motors is correct.
bool AP_Motors6DOF::motor_is_enabled(int motor_number) {
if (motor_number < 0 || motor_number >= AP_MOTORS_MAX_NUM_MOTORS) {
return false;
}
return motor_enabled[motor_number];
}
set_reversed() : Change the direction of the motor.
bool AP_Motors6DOF::set_reversed(int motor_number, bool reversed) {
if (motor_number < 0 || motor_number >= AP_MOTORS_MAX_NUM_MOTORS) {
return false;
}
if (reversed) {
_motor_reverse[motor_number].set_and_save(-1);
} else {
_motor_reverse[motor_number].set_and_save(1);
}
return true;
}
Three, reference materials
Ardusub official manual
Ardupilot source code
If you need to reprint, please indicate the source