ardupilot 关于设备车Rover的学习《2》------如何解锁

目录

摘要

---

本节主要记录ardupilot中ROVER小车是如何进行通过遥控器进行解锁的，欢迎批评指正！

---

1.序言

---

ardupilot 中ROVER小车通过遥控器解锁有两种方式：

1. 通过遥控器的方向舵实现解锁
2. 通过遥控器上外部开关实现解锁

在进行代码讲解之前需要注意的是小车的遥控器控制通道的映射需要注意：

void Rover::set_control_channels(void)
{
    //检查RCMAP上的更改
    channel_steer    = rc().channel(rcmap.roll()-1);      //横滚通道映射到方向舵
    channel_throttle = rc().channel(rcmap.throttle()-1);  //油门通道映射到油门通道
    channel_lateral  = rc().channel(rcmap.yaw()-1);       //偏航通道映射转向通道

    //设置遥控器的范围
    channel_steer->set_angle(SERVO_MAX); //最大是4500
    channel_throttle->set_angle(100);    //设置0-100
    //横向是空的吗,不是就设置,是的话关闭
    if (channel_lateral != nullptr)
    {
        channel_lateral->set_angle(100); //设置100
    }

    //步行机器人rc输入初始化------ walking robots rc input init
    channel_roll = rc().find_channel_for_option(RC_Channel::AUX_FUNC::ROLL);   //横滚201
    channel_pitch = rc().find_channel_for_option(RC_Channel::AUX_FUNC::PITCH); //俯仰202
    channel_walking_height = rc().find_channel_for_option(RC_Channel::AUX_FUNC::WALKING_HEIGHT);//211
    //设定死区
    if (channel_roll != nullptr) 
    {
        channel_roll->set_angle(SERVO_MAX);
        channel_roll->set_default_dead_zone(30);
    }
    //设定死区
    if (channel_pitch != nullptr) 
    {
        channel_pitch->set_angle(SERVO_MAX);
        channel_pitch->set_default_dead_zone(30);
    }
    //设定死区
    if (channel_walking_height != nullptr) {
        channel_walking_height->set_angle(SERVO_MAX);
        channel_walking_height->set_default_dead_zone(30);
    }    

    //帆船rc输入初始化----- sailboat rc input init
    g2.sailboat.init_rc_in();

    //允许在未启用时重新配置输出------ Allow to reconfigure output when not armed
    if (!arming.is_armed()) 
    {
        g2.motors.setup_servo_output();
        //对于一个漫游者来说，安全是微调油门----- For a rover safety is TRIM throttle
        g2.motors.setup_safety_output();
    }
    // setup correct scaling for ESCs like the UAVCAN ESCs which
    // take a proportion of speed. Default to 1000 to 2000 for systems without
    // a k_throttle output
    //为诸如UAVCAN ESC之类的ESC设置正确的缩放
    //占速度的一部分。对于不带的系统，默认值为1000到2000
    //k_throttle 输出
    hal.rcout->set_esc_scaling(1000, 2000);
    g2.servo_channels.set_esc_scaling_for(SRV_Channel::k_throttle);
}

从这里可以看出，如果我们遥控器是美国手，那么我们之前的横滚通道就是控制小车的方向舵，实现小车左右旋转；我们的油门通道还是控制小车的前进后退。

2.解锁代码实现

---

1.通过遥控器的方向舵实现解锁

SCHED_TASK(read_radio,             50,    200,   3),//读取遥控器数据

void Rover::read_radio()
{

	//判断有没有遥控器输入数据
    if (!rc().read_input())
    {
        //检测假如我们丢失遥控器信号---- check if we lost RC link
        radio_failsafe_check(channel_throttle->get_radio_in());
        return;
    }

    failsafe.last_valid_rc_ms = AP_HAL::millis();
    //检测RC值是否有根据----- check that RC value are valid
    radio_failsafe_check(channel_throttle->get_radio_in());

    //检测我们尝试做遥控器解锁或者上锁----- check if we try to do RC arm/disarm
    if(g.rc_arm_enable==1)
    {
    	//方向舵解锁上锁操作
    	rudder_arm_disarm_check();	 //只有这个功能打开才能开启
    }

}

---

由于小车有时候想不通过方向舵来解锁，以及限制解锁延迟时间，这里新增加一个参数g.rc_arm_enable来实现是否开启这个功能。当然默认代码也可以关闭这个功能，可以通过参数RUDDER来实现。

---

到这里我们重点只需要关注函数：rudder_arm_disarm_check(); 即可。

void Rover::rudder_arm_disarm_check()
{
    // check if arming/disarm using rudder is allowed
	//检查是否允许使用方向舵进行防护/解除防护
    const AP_Arming::RudderArming arming_rudder = arming.get_rudder_arming_type();
    //如果设置没有使能,直接返回
    if (arming_rudder == AP_Arming::RudderArming::IS_DISABLED)
    {
        return;
    }

    // In Rover we need to check that its set to the throttle trim and within the DZ
    // if throttle is not within trim dz,then pilot cannot rudder arm/disarm
    //在ROVER我们需要检查油门是在中立或者死区范围内
    //如果油门不在死区范围内,则飞行员不能操纵舵/解除舵
    if (!channel_throttle->in_trim_dz())
    {
        rudder_arm_timer = 0;
        return;
    }

    //检查此模式是否允许启用/禁用------ check if arming/disarming allowed from this mode
    if (!control_mode->allows_arming_from_transmitter())
    {
        rudder_arm_timer = 0;
        return;
    }
    //判断是解锁
    if (!arming.is_armed())
    {
        // when not armed, full right rudder starts arming counter
    	//未启用时，右满舵开始启用计数器
        if (channel_steer->get_control_in() > 4000) //方向杆往右边解锁
        {
        	//获取现在时间
            const uint32_t now = millis();
            //数据判断,方向舵/转向装置已运行的时间
            if ((rudder_arm_timer == 0 )||(now - rudder_arm_timer < g.rc_arm_time_ms))
            {
            	//方向舵/转向装置已运行的时间
                if (rudder_arm_timer == 0)
                {
                	//记录限制时间
                    rudder_arm_timer = now;
                }
            } else
            {
                //时间到了开始解锁------ time to arm!
                arming.arm(AP_Arming::Method::RUDDER);
                //记得把数据清零
                rudder_arm_timer = 0;
            }
        } else
        {
            //没有在全右操作------ not at full right rudder
            rudder_arm_timer = 0;
        }
    } else if ((arming_rudder == AP_Arming::RudderArming::ARMDISARM) && !g2.motors.active())
    {
        // when armed and motor not active (not moving), full left rudder starts disarming counter
    	//当解锁和电机没有激活（不运动）,全左方向舵开始解除待命计数器
        if (channel_steer->get_control_in() < -4000) //方向杆往左边上锁
        {
        	//获取现在时间
            const uint32_t now = millis();
            //数据判断
            if ((rudder_arm_timer == 0) ||(now - rudder_arm_timer < g.rc_arm_time_ms))
            {
            	//记录下开始记录的时间
                if (rudder_arm_timer == 0)
                {
                	//传递时间,为数据判断做铺垫
                    rudder_arm_timer = now;
                }
            } else
            {
                //时间到了开始上锁----- time to disarm!
                arming.disarm(AP_Arming::Method::RUDDER);
                //数据清零
                rudder_arm_timer = 0;
            }
        } else
        {
            //没有在最左边---- not at full left rudder
            rudder_arm_timer = 0;
        }
    }
}

这段代码相对好懂，重点我增加了一个参数来实现对解锁延迟大小的设定，通过g.rc_arm_time_ms设定这个值就可以来实现解锁延迟了。

2.通过遥控器上外部开关实现解锁

要想实现通过外部开关进行解锁，首先先要把解锁的按键找到，比如我们用遥控器的7通道解锁，那就需要把7通道映射成解锁功能。

做好映射之后，我们只需要看代码实现即可：

    //初始化遥控器通道数据------ initialise rc channels
    rc().convert_options(RC_Channel::AUX_FUNC::ARMDISARM_UNUSED, RC_Channel::AUX_FUNC::ARMDISARM);     //转换信息
    rc().convert_options(RC_Channel::AUX_FUNC::SAVE_TRIM, RC_Channel::AUX_FUNC::TRIM_TO_CURRENT_SERVO_RC);
    //初始化
    rc().init();

void RC_Channels::init(void)
{
    //对每个通道进行初始化 setup ch_in on channels
    for (uint8_t i=0; i<NUM_RC_CHANNELS; i++) 
    {
        channel(i)->ch_in = i;
    }
    //初始化外部开关
    init_aux_all();
}

外部通道初始化

void RC_Channels::init_aux_all()
{
    for (uint8_t i=0; i<NUM_RC_CHANNELS; i++) 
    {
        RC_Channel *c = channel(i);
        if (c == nullptr) 
        {
            continue;
        }
        //初始化
        c->init_aux();
    }
    //复位模式
    reset_mode_switch();
}

void RC_Channel::init_aux()
{
	//外部开关的位置信息
    AuxSwitchPos position;
    //读取三段开关信息
    if (!read_3pos_switch(position)) 
    {
    	//没有读取到,应该设置低电平
        position = AuxSwitchPos::LOW;
    }
    //初始化外部开关
    init_aux_function((aux_func_t)option.get(), position);
}

void RC_Channel::init_aux_function(const aux_func_t ch_option, const AuxSwitchPos ch_flag)
{
    //初始化通道对应的功能-----init channel options
    switch(ch_option) {
    //以下功能不需要初始化：----- the following functions do not need to be initialised:
    case AUX_FUNC::ARMDISARM: //这里就是我们的上锁,解锁映射了
    case AUX_FUNC::CAMERA_TRIGGER:
    case AUX_FUNC::CLEAR_WP:
    case AUX_FUNC::COMPASS_LEARN:
    case AUX_FUNC::DISARM:
    case AUX_FUNC::DO_NOTHING:
    case AUX_FUNC::LANDING_GEAR:
    case AUX_FUNC::LOST_VEHICLE_SOUND:
    case AUX_FUNC::RELAY:
    case AUX_FUNC::RELAY2:
    case AUX_FUNC::RELAY3:
    case AUX_FUNC::RELAY4:
    case AUX_FUNC::RELAY5:
    case AUX_FUNC::RELAY6:
    case AUX_FUNC::VISODOM_ALIGN:
    case AUX_FUNC::EKF_LANE_SWITCH:
    case AUX_FUNC::EKF_YAW_RESET:
    case AUX_FUNC::GENERATOR: // don't turn generator on or off initially
    case AUX_FUNC::EKF_POS_SOURCE:
    case AUX_FUNC::TORQEEDO_CLEAR_ERR:
    case AUX_FUNC::SCRIPTING_1:
    case AUX_FUNC::SCRIPTING_2:
    case AUX_FUNC::SCRIPTING_3:
    case AUX_FUNC::SCRIPTING_4:
    case AUX_FUNC::SCRIPTING_5:
    case AUX_FUNC::SCRIPTING_6:
    case AUX_FUNC::SCRIPTING_7:
    case AUX_FUNC::SCRIPTING_8:
    case AUX_FUNC::VTX_POWER:
    case AUX_FUNC::OPTFLOW_CAL:
    case AUX_FUNC::TURBINE_START:
        break;
    case AUX_FUNC::AVOID_ADSB:
    case AUX_FUNC::AVOID_PROXIMITY:
    case AUX_FUNC::FENCE:
    case AUX_FUNC::GPS_DISABLE:
    case AUX_FUNC::GPS_DISABLE_YAW:
    case AUX_FUNC::GRIPPER:
    case AUX_FUNC::KILL_IMU1:
    case AUX_FUNC::KILL_IMU2:
    case AUX_FUNC::MISSION_RESET:
    case AUX_FUNC::MOTOR_ESTOP:
    case AUX_FUNC::RC_OVERRIDE_ENABLE:
    case AUX_FUNC::RUNCAM_CONTROL:
    case AUX_FUNC::RUNCAM_OSD_CONTROL:
    case AUX_FUNC::SPRAYER:
    case AUX_FUNC::DISABLE_AIRSPEED_USE:
#if HAL_MOUNT_ENABLED
    case AUX_FUNC::RETRACT_MOUNT:
#endif
        run_aux_function(ch_option, ch_flag, AuxFuncTriggerSource::INIT);
        break;
    default:
        gcs().send_text(MAV_SEVERITY_WARNING, "Failed to init: RC%u_OPTION: %u\n",
                           (unsigned)(this->ch_in+1), (unsigned)ch_option);
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
        AP_BoardConfig::config_error("Failed to init: RC%u_OPTION: %u",
                           (unsigned)(this->ch_in+1), (unsigned)ch_option);
#endif
        break;
    }
}

然后会调用

bool RC_Channel::run_aux_function(aux_func_t ch_option, AuxSwitchPos pos, AuxFuncTriggerSource source)
{
	//执行这里
    const bool ret = do_aux_function(ch_option, pos);

    // @LoggerMessage: AUXF
    // @Description: Auxiliary function invocation information
    // @Field: TimeUS: Time since system startup
    // @Field: function: ID of triggered function
    // @Field: pos: switch position when function triggered
    // @Field: source: source of auxillary function invocation
    // @Field: result: true if function was successful
    AP::logger().Write(
        "AUXF",
        "TimeUS,function,pos,source,result",
        "s#---",
        "F----",
        "QHBBB",
        AP_HAL::micros64(),
        uint16_t(ch_option),
        uint8_t(pos),
        uint8_t(source),
        uint8_t(ret)
        );
    return ret;
}

bool RC_Channel::do_aux_function(const aux_func_t ch_option, const AuxSwitchPos ch_flag)
{
    switch(ch_option) {
    case AUX_FUNC::CAMERA_TRIGGER:
        do_aux_function_camera_trigger(ch_flag);
        break;

    case AUX_FUNC::FENCE:
        do_aux_function_fence(ch_flag);
        break;

    case AUX_FUNC::GRIPPER:
        do_aux_function_gripper(ch_flag);
        break;

    case AUX_FUNC::RC_OVERRIDE_ENABLE:
        // Allow or disallow RC_Override
        do_aux_function_rc_override_enable(ch_flag);
        break;

    case AUX_FUNC::AVOID_PROXIMITY:
        do_aux_function_avoid_proximity(ch_flag);
        break;

    case AUX_FUNC::RELAY:
        do_aux_function_relay(0, ch_flag == AuxSwitchPos::HIGH);
        break;
    case AUX_FUNC::RELAY2:
        do_aux_function_relay(1, ch_flag == AuxSwitchPos::HIGH);
        break;
    case AUX_FUNC::RELAY3:
        do_aux_function_relay(2, ch_flag == AuxSwitchPos::HIGH);
        break;
    case AUX_FUNC::RELAY4:
        do_aux_function_relay(3, ch_flag == AuxSwitchPos::HIGH);
        break;
    case AUX_FUNC::RELAY5:
        do_aux_function_relay(4, ch_flag == AuxSwitchPos::HIGH);
        break;
    case AUX_FUNC::RELAY6:
        do_aux_function_relay(5, ch_flag == AuxSwitchPos::HIGH);
        break;

    case AUX_FUNC::RUNCAM_CONTROL:
        do_aux_function_runcam_control(ch_flag);
        break;

    case AUX_FUNC::RUNCAM_OSD_CONTROL:
        do_aux_function_runcam_osd_control(ch_flag);
        break;

    case AUX_FUNC::CLEAR_WP:
        do_aux_function_clear_wp(ch_flag);
        break;
    case AUX_FUNC::MISSION_RESET:
        do_aux_function_mission_reset(ch_flag);
        break;

    case AUX_FUNC::AVOID_ADSB:
        do_aux_function_avoid_adsb(ch_flag);
        break;

#if HAL_GENERATOR_ENABLED
    case AUX_FUNC::GENERATOR:
        do_aux_function_generator(ch_flag);
        break;
#endif

    case AUX_FUNC::SPRAYER:
        do_aux_function_sprayer(ch_flag);
        break;

    case AUX_FUNC::LOST_VEHICLE_SOUND:
        do_aux_function_lost_vehicle_sound(ch_flag);
        break;
    //需要用到的功能
    case AUX_FUNC::ARMDISARM:
        do_aux_function_armdisarm(ch_flag);
        break;

    case AUX_FUNC::DISARM:
        if (ch_flag == AuxSwitchPos::HIGH) {
            AP::arming().disarm(AP_Arming::Method::AUXSWITCH);
        }
        break;

    case AUX_FUNC::COMPASS_LEARN:
        if (ch_flag == AuxSwitchPos::HIGH) {
            Compass &compass = AP::compass();
            compass.set_learn_type(Compass::LEARN_INFLIGHT, false);
        }
        break;

    case AUX_FUNC::LANDING_GEAR: {
        AP_LandingGear *lg = AP_LandingGear::get_singleton();
        if (lg == nullptr) {
            break;
        }
        switch (ch_flag) {
        case AuxSwitchPos::LOW:
            lg->set_position(AP_LandingGear::LandingGear_Deploy);
            break;
        case AuxSwitchPos::MIDDLE:
            // nothing
            break;
        case AuxSwitchPos::HIGH:
            lg->set_position(AP_LandingGear::LandingGear_Retract);
            break;
        }
        break;
    }

    case AUX_FUNC::GPS_DISABLE:
        AP::gps().force_disable(ch_flag == AuxSwitchPos::HIGH);
        break;

    case AUX_FUNC::GPS_DISABLE_YAW:
        AP::gps().set_force_disable_yaw(ch_flag == AuxSwitchPos::HIGH);
        break;

    case AUX_FUNC::DISABLE_AIRSPEED_USE: {
#if AP_AIRSPEED_ENABLED
        AP_Airspeed *airspeed = AP::airspeed();
        if (airspeed == nullptr) {
            break;
        }
        switch (ch_flag) {
        case AuxSwitchPos::HIGH:
            airspeed->force_disable_use(true);
            break;
        case AuxSwitchPos::MIDDLE:
            break;
        case AuxSwitchPos::LOW:
            airspeed->force_disable_use(false);
            break;
        }
#endif
        break;
    }

    case AUX_FUNC::MOTOR_ESTOP:
        switch (ch_flag) {
        case AuxSwitchPos::HIGH: {
            SRV_Channels::set_emergency_stop(true);

            // log E-stop
            AP_Logger *logger = AP_Logger::get_singleton();
            if (logger && logger->logging_enabled()) {
                logger->Write_Event(LogEvent::MOTORS_EMERGENCY_STOPPED);
            }
            break;
        }
        case AuxSwitchPos::MIDDLE:
            // nothing
            break;
        case AuxSwitchPos::LOW: {
            SRV_Channels::set_emergency_stop(false);

            // log E-stop cleared
            AP_Logger *logger = AP_Logger::get_singleton();
            if (logger && logger->logging_enabled()) {
                logger->Write_Event(LogEvent::MOTORS_EMERGENCY_STOP_CLEARED);
            }
            break;
        }
        }
        break;

    case AUX_FUNC::VISODOM_ALIGN:
#if HAL_VISUALODOM_ENABLED
        if (ch_flag == AuxSwitchPos::HIGH) {
            AP_VisualOdom *visual_odom = AP::visualodom();
            if (visual_odom != nullptr) {
                visual_odom->align_sensor_to_vehicle();
            }
        }
#endif
        break;

    case AUX_FUNC::EKF_POS_SOURCE:
        switch (ch_flag) {
        case AuxSwitchPos::LOW:
            // low switches to primary source
            AP::ahrs().set_posvelyaw_source_set(0);
            break;
        case AuxSwitchPos::MIDDLE:
            // middle switches to secondary source
            AP::ahrs().set_posvelyaw_source_set(1);
            break;
        case AuxSwitchPos::HIGH:
            // high switches to tertiary source
            AP::ahrs().set_posvelyaw_source_set(2);
            break;
        }
        break;

    case AUX_FUNC::OPTFLOW_CAL: {
#if AP_OPTICALFLOW_ENABLED
        OpticalFlow *optflow = AP::opticalflow();
        if (optflow == nullptr) {
            gcs().send_text(MAV_SEVERITY_CRITICAL, "OptFlow Cal: failed sensor not enabled");
            break;
        }
        if (ch_flag == AuxSwitchPos::HIGH) {
            optflow->start_calibration();
        } else {
            optflow->stop_calibration();
        }
#endif
        break;
    }

#if !HAL_MINIMIZE_FEATURES
    case AUX_FUNC::KILL_IMU1:
        AP::ins().kill_imu(0, ch_flag == AuxSwitchPos::HIGH);
        break;

    case AUX_FUNC::KILL_IMU2:
        AP::ins().kill_imu(1, ch_flag == AuxSwitchPos::HIGH);
        break;
#endif // HAL_MINIMIZE_FEATURES

    case AUX_FUNC::CAM_MODE_TOGGLE: {
        // Momentary switch to for cycling camera modes
        AP_Camera *camera = AP_Camera::get_singleton();
        if (camera == nullptr) {
            break;
        }
        switch (ch_flag) {
        case AuxSwitchPos::LOW:
            // nothing
            break;
        case AuxSwitchPos::MIDDLE:
            // nothing
            break;
        case AuxSwitchPos::HIGH:
            camera->cam_mode_toggle();
            break;
        }
        break;
    }

    case AUX_FUNC::RETRACT_MOUNT: {
#if HAL_MOUNT_ENABLED
        AP_Mount *mount = AP::mount();
        if (mount == nullptr) {
            break;
        }
        switch (ch_flag) {
            case AuxSwitchPos::HIGH:
                mount->set_mode(MAV_MOUNT_MODE_RETRACT);
                break;
            case AuxSwitchPos::MIDDLE:
                // nothing
                break;
            case AuxSwitchPos::LOW:
                mount->set_mode_to_default();
                break;
        }
#endif
        break;
    }

    case AUX_FUNC::EKF_LANE_SWITCH:
        // used to test emergency lane switch
        AP::ahrs().check_lane_switch();
        break;

    case AUX_FUNC::EKF_YAW_RESET:
        // used to test emergency yaw reset
        AP::ahrs().request_yaw_reset();
        break;

    // clear torqeedo error
    case AUX_FUNC::TORQEEDO_CLEAR_ERR: {
#if HAL_TORQEEDO_ENABLED
        if (ch_flag == AuxSwitchPos::HIGH) {
            AP_Torqeedo *torqeedo = AP_Torqeedo::get_singleton();
            if (torqeedo != nullptr) {
                torqeedo->clear_motor_error();
            }
        }
#endif
        break;
    }

    case AUX_FUNC::SCRIPTING_1:
    case AUX_FUNC::SCRIPTING_2:
    case AUX_FUNC::SCRIPTING_3:
    case AUX_FUNC::SCRIPTING_4:
    case AUX_FUNC::SCRIPTING_5:
    case AUX_FUNC::SCRIPTING_6:
    case AUX_FUNC::SCRIPTING_7:
    case AUX_FUNC::SCRIPTING_8:
        break;

    default:
        gcs().send_text(MAV_SEVERITY_INFO, "Invalid channel option (%u)", (unsigned int)ch_option);
        return false;
    }

    return true;
}

重点关注这个函数：

    case AUX_FUNC::ARMDISARM:
        do_aux_function_armdisarm(ch_flag);
        break;

void RC_Channel::do_aux_function_armdisarm(const AuxSwitchPos ch_flag)
{
    //解锁和上锁这个外设------ arm or disarm the vehicle
    switch (ch_flag) 
    {
    case AuxSwitchPos::HIGH:
        AP::arming().arm(AP_Arming::Method::AUXSWITCH, true); //最高就是解锁
        break;
    case AuxSwitchPos::MIDDLE:
        // nothing
        break;
    case AuxSwitchPos::LOW:
        AP::arming().disarm(AP_Arming::Method::AUXSWITCH); //最低就是上锁
        break;
    }
}

到这里就实现了通过遥控器实现解锁。