PX4 1.14.0中Offboard模式下为什么无人船不启动

QUT_HXY 2024-02-21 19:01:55

PX4版本：1.14.0

目标：前进10m

问题：无人船不动，QGC界面中显示为offboard模式

源码：控制逻辑部分

void Boat_pos_control::run()
{
    float begin_x;
    float begin_y;
    float begin_z;


//进入offboard模式并解锁
    while(!should_exit())
    {
        _vehicle_status_sub.copy(&_status);
        if((_status.nav_state==vehicle_status_s::NAVIGATION_STATE_OFFBOARD)&&(_status.arming_state==vehicle_status_s::ARMING_STATE_ARMED))
        {
            PX4_INFO("in offboard and armed");
            break;
        }
        _command.target_system = _status.system_id;
        _command.target_component = _status.component_id;
        ocm.timestamp = hrt_absolute_time();
        _vehicle_local_position_sub.copy(&_vehicle_local_position);
        sp_local.x=_vehicle_local_position.x;
        sp_local.y=_vehicle_local_position.y;
        sp_local.z=_vehicle_local_position.z-5;
        sp_local.timestamp = hrt_absolute_time();
        _trajectory_setpoint_pub.publish(sp_local);
        ocm.position=true;
        ocm.timestamp = hrt_absolute_time();
        _offboard_control_mode_pub.publish(ocm);
        _command.command =vehicle_command_s::VEHICLE_CMD_DO_SET_MODE;
        _command.param1=1.0f;
        _command.param2=PX4_CUSTOM_MAIN_MODE_OFFBOARD;
        _command.timestamp = hrt_absolute_time();
        _vehicle_command_pub.publish(_command);
        usleep(10000);
        _command.command = vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM;
        _command.param1 = 1.0f;
        _command.timestamp = hrt_absolute_time();
        _vehicle_command_pub.publish(_command);
    }
//记录当前点的位置
    _vehicle_local_position_sub.copy(&_vehicle_local_position);
    begin_x=_vehicle_local_position.x;
    begin_y=_vehicle_local_position.y;
    begin_z=_vehicle_local_position.z;
    time_tick=hrt_absolute_time();

    double target = begin_x + 10;
    double distance;
    //业务逻辑
    while (!should_exit())
    {
        _vehicle_local_position_sub.copy(&_vehicle_local_position);
        distance = target - (double)_vehicle_local_position.x;
        if(distance <= 0){}
        else
        {
            ocm.position=true;
            ocm.velocity=false;
            ocm.acceleration=true;
            ocm.timestamp = hrt_absolute_time();
            _offboard_control_mode_pub.publish(ocm);
            sp_local.x=target;
            sp_local.y=begin_y;
            sp_local.z=begin_z;
            sp_local.timestamp = hrt_absolute_time();
            _trajectory_setpoint_pub.publish(sp_local);
        }

        usleep(100000);

        parameters_update();
    }

}

控制台中显示如下：