Ardusub source code analysis and learning (5)-starting from the manual model

---

Starting from this article, various modes in Ardusub will be introduced one after another. Stabilize model has already been introduced in the first blog post in the series (if you have time to consider writing another one?), let’s start with manual mode directly. Introduce it (because it is the easiest...).

---

manual model: manual control mode. After Pixhawk is programmed into the Ardusub firmware, it will boot from this mode by default. As the name implies, the various controls of the ROV in this mode are directly realized through the handle, and the system will not automatically help you stabilize the robot posture.

manual_init()

// manual_init - initialise manual controller
bool Sub::manual_init()
{
    
    
    // set target altitude to zero for reporting
    pos_control.set_alt_target(0);

    // attitude hold inputs become thrust inputs in manual mode
    // set to neutral to prevent chaotic behavior (esp. roll/pitch)
    set_neutral_controls();

    return true;
}

Generally speaking, the mode control program is divided into two parts, init() and run(). The init() function in manual mode is relatively simple, just like stabilize, the first is to reset the expected height to zero. Then change the attitude holding input to thrust input and set it to neutral to prevent unnecessary shock and swing.

The following shows the internal program of set_neutral_controls().

void Sub::set_neutral_controls()
{
    
    
    uint32_t tnow = AP_HAL::millis();

    for (uint8_t i = 0; i < 6; i++) {
    
    
        RC_Channels::set_override(i, 1500, tnow);
    }

    // Clear pitch/roll trim settings
    pitchTrim = 0;
    rollTrim  = 0;
}

manual_run()

// manual_run - runs the manual (passthrough) controller
// should be called at 100hz or more
void Sub::manual_run()
{
    
    
    // if not armed set throttle to zero and exit immediately
    if (!motors.armed()) {
    
    
        motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
        attitude_control.set_throttle_out(0,true,g.throttle_filt);
        attitude_control.relax_attitude_controllers();
        return;
    }

    motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);

    motors.set_roll(channel_roll->norm_input());
    motors.set_pitch(channel_pitch->norm_input());
    motors.set_yaw(channel_yaw->norm_input() * g.acro_yaw_p / ACRO_YAW_P);
    motors.set_throttle(channel_throttle->norm_input());
    motors.set_forward(channel_forward->norm_input());
    motors.set_lateral(channel_lateral->norm_input());
}

The previous part is consistent with the stabilize model ( see here for details ), let’s talk about the later set_xxx() part. Note that this formal function is to set the input channels, which are roll, pitch, yaw, sink and float, forward and backward translation, and left and right translation. Note that each channel can correspond to multiple motors ( see here for details ). The required range of parameters entered in brackets is between -1 and 1.

Then focus on the internal norm_input(). This function is located in RC_Channel.cpp in the library (note that RC_Channel.cpp is used to control a single channel, and RC_Channels.cpp is used to control multiple channels in batch).

In simple terms, this program first determines whether to reverse the channel input, and assigns the value to the variable reverse_mul based on the result. Then the channel input value is limited and calculated, and the input value radio_in is converted into an output value between -1 and 1. The deviation of the output value from 0 indicates the desire to move in a certain direction as soon as possible.

float RC_Channel::norm_input() const
{
    
    
    float ret;
    int16_t reverse_mul = (reversed?-1:1);
    if (radio_in < radio_trim) {
    
    
        if (radio_min >= radio_trim) {
    
    
            return 0.0f;
        }
        ret = reverse_mul * (float)(radio_in - radio_trim) / (float)(radio_trim - radio_min);
    } else {
    
    
        if (radio_max <= radio_trim) {
    
    
            return 0.0f;
        }
        ret = reverse_mul * (float)(radio_in - radio_trim) / (float)(radio_max  - radio_trim);
    }
    return constrain_float(ret, -1.0f, 1.0f);
}

The corresponding value can be found in the variable table in the same file

    // @Param: MIN
    // @DisplayName: RC min PWM
    // @Description: RC minimum PWM pulse width in microseconds. Typically 1000 is lower limit, 1500 is neutral and 2000 is upper limit.
    // @Units: PWM
    // @Range: 800 2200
    // @Increment: 1
    // @User: Advanced
    AP_GROUPINFO("MIN",  1, RC_Channel, radio_min, 1100),

    // @Param: TRIM
    // @DisplayName: RC trim PWM
    // @Description: RC trim (neutral) PWM pulse width in microseconds. Typically 1000 is lower limit, 1500 is neutral and 2000 is upper limit.
    // @Units: PWM
    // @Range: 800 2200
    // @Increment: 1
    // @User: Advanced
    AP_GROUPINFO("TRIM", 2, RC_Channel, radio_trim, 1500),

    // @Param: MAX
    // @DisplayName: RC max PWM
    // @Description: RC maximum PWM pulse width in microseconds. Typically 1000 is lower limit, 1500 is neutral and 2000 is upper limit.
    // @Units: PWM
    // @Range: 800 2200
    // @Increment: 1
    // @User: Advanced
    AP_GROUPINFO("MAX",  3, RC_Channel, radio_max, 1900),

    // @Param: REVERSED
    // @DisplayName: RC reversed
    // @Description: Reverse channel input. Set to 0 for normal operation. Set to 1 to reverse this input channel.
    // @Values: 0:Normal,1:Reversed
    // @User: Advanced
    AP_GROUPINFO("REVERSED",  4, RC_Channel, reversed, 0),