Ardusubソースコード解析学習(五)--manual modelから

Ardusubソースコード解析学習(五)--manual modelから

manual_init()

manual_run()

本編からは、Ardusubの様々なパターンを続々と紹介していきますが、stabilize modelはシリーズ初のブログで紹介しました(時間があればもう1編書き直してもいいかな?)こちらはそのまま手動モードからご紹介しましょう(一番簡単なので…).
manual model:手動制御モード.PixhawkはArdusubファームウェアに書き込んだ後、デフォルトではこのモードから起動します.その名の通り、このモードではROVの各種制御はハンドルで直接実現され、ロボットの姿勢を自動的に安定させることはできません.
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;
}

一般的に、モード制御のプログラムはinit()とrun()の2つの部分に分けられ、manualモードでのinit()関数は比較的簡単で、stabilizeと同様に、まず期待高さをゼロにする.次に、姿勢保持入力を推力入力に変換し、不要な振動や揺動を防止するために空室に設定する.
次のようにset_neutral_コントロール()内部プログラム.

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());
}

前の部分はstabilize modelと一致しています(具体的にはここを参照)、後のset_xxx()部分ができました.注意この形式関数は入力チャネルを設定し、それぞれ横転、ピッチ、偏航、浮遊、前後平行移動、左右平行移動の6チャネルであり、各チャネルが複数のモータ(具体的にはここを参照)に対応できることに注意する.括弧内に入力するパラメータの要求範囲は-1~1の間である.
では、内部のnorm_を重点的に見てみましょう.input().この関数はライブラリにあるRC_Channel.cpp(RC_Channel.cppは単一チャネルを制御するために使用され、RC_Channels.cppは複数チャネルを一括制御するために使用されることに留意されたい).
簡単に言えば、このプログラムはまずチャネル入力を逆方向にするかどうかを判断し、結果に基づいてreverse_に値を付与する.mulという変数.次に、チャネル入力の値を制限して計算し、入力値radio_をin-1~1間の出力量に変換し、出力量が0値からずれる大きさは、なるべく早くある方向に動きたいことを示す.

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);
}

同じファイルの変数テーブルで対応する値を見つけることができます.

    // @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),