Ardusub源码解析学习(五)——从manual model开始

Ardusub源码解析学习(五)—— 从manual model开始

  • manual_init()
  • manual_run()


从本篇开始,将会陆续对Ardusub中各种模式进行介绍,stabilize model在系列第一篇博文已经介绍过了(如果有时间考虑再重新写一篇?),这边就直接从手动模式开始介绍吧(因为算是最简单的了...)。
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()两部分,manual模式下的init()函数比较简单,跟stabilize 一样,首先是将期望高度归零。然后将姿态保持输入转变为推力输入,并设置为空挡以防止不必要的震荡和摆动。

如下所示为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());
}

前面部分和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),

你可能感兴趣的:(Ardupilot,c++,apm,rov)