请选择 进入手机版 | 继续访问电脑版

0

阿木币

0

精华

5 小时

在线时间

应届白菜

Rank: 1

发表于 2021-11-10 18:14:38 182 浏览 5 回复

[飞控嵌入式] offboard无法切入

懸賞50RMB:QQ:1154382929(本人20年扣扣號,昵稱直接是我真名),問題困擾本人許久,可以解決的話,50RMB雙手奉上不是問題!!!

背景:樹莓派安裝MAVROS,創建官方例程節點offboard_node(就是飛到2米的那個),用串口方式和PX4的TELEM2進行連接,並且PX4地面站參數設置如下:



流程:PX4上電連接上地面站----遙控器嘗試解鎖成功,並且切換到定點模式----筆記本上運行roscore,並且ssh遠程登錄到樹莓派----樹莓派運行mavros:
roslaunch mavros px4.launch fcu_url:=/dev/ttyS0:921600------樹莓派運行offboard_node節點(即官方例程)

問題:PX4一直拒絕切換到offboard模式,但是用MAVROS切換到其他模式可以執行,說明通信沒有問題。而且打印rostopic echo /mavros/setpoint_position/local 有設定點消息x=0,y=0,z=2,並且確實是已2HZ的頻率發送,總之官方例程一行也沒改,通信也應該是正常,參數也是按照教程設置,但是飛控一直拒絕切換到offboard模式。


奇怪的事情:實驗室有師兄寫的祖傳代碼,直接寫在PX4固件裏的代碼,在QGC上運行之後,我再此時啓動樹莓派上的offboard_node居然可以順利解鎖,師兄代碼如下:
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <px4_tasks.h>

#include <px4_config.h>
#include <nuttx/sched.h>

#include <systemlib/err.h>

#include <drivers/drv_hrt.h>

#include <uORB/uORB.h>
#include <uORB/topics/rc_channels.h>
#include <uORB/topics/offboard_control_mode.h>
#include <uORB/topics/offboard_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/position_setpoint.h>
#include <uORB/topics/position_setpoint_triplet.h>

static bool thread_should_exit = false;     /**< daemon exit flag */
static bool thread_running = false;     /**< daemon status flag */
static int offboard_pub_task;             /**< Handle of daemon task / thread */
//static int gps_sequence = 0;

static uint8_t SETPOINT_TYPE_POSITION = 0;
static uint8_t SETPOINT_TYPE_LOITER = 2;
static uint8_t SETPOINT_TYPE_TAKEOFF = 3;
static uint8_t SETPOINT_TYPE_LAND = 4;
static uint8_t SETPOINT_TYPE_IDLE = 5;
static uint8_t VELOCITY_FRAME_LOCAL_NED = 1;
static uint8_t VELOCITY_FRAME_BODY_NED = 8;


__EXPORT int offboard_pub_main(int argc, char *argv[]);

int offboard_pub_thread_main(int argc, char *argv[]);

static void usage(const char *reason);


static void usage(const char *reason)
{
    if (reason) {
        warnx("%s\n", reason);
    }

    warnx("usage: offboard_pub {start|stop|numb} [-p <additional params>]\n\n");
}


int offboard_pub_main(int argc, char *argv[])
{
    if (argc < 2) {
        usage("missing command");
        return 1;
    }

    if (!strcmp(argv[1], "start")) {

        if (thread_running) {
            warnx("daemon already running\n");
            /* this is not an error */
            return 0;
        }

        thread_should_exit = false;
        offboard_pub_task = px4_task_spawn_cmd("offboard_pub",
                         SCHED_DEFAULT,
                         SCHED_PRIORITY_DEFAULT,
                         2000,
                         offboard_pub_thread_main,
                         (argv) ? (char *const *)&argv[2] : (char *const *)NULL);
        return 0;
    }

    if (!strcmp(argv[1], "stop")) {
        thread_should_exit = true;
        return 0;
    }

    if (!strcmp(argv[1], "status")) {
        if (thread_running) {
            warnx("\trunning\n");

        } else {
            warnx("\tnot started\n");
        }

        return 0;
    }


    usage("unrecognized command");
    return 1;

}

int offboard_pub_thread_main(int argc, char *argv[])
{

    warnx("[daemon] starting\n");

    thread_running = true;
    int rc_channels_sub = orb_subscribe(ORB_ID(rc_channels));//订阅topic
    int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
    int offboard_setpoint_sub = orb_subscribe(ORB_ID(offboard_setpoint));

    /*定义offboard话题结构体*/
    struct rc_channels_s _rc_channels = {};//定义topic的结构体
    struct offboard_control_mode_s _offboard_control_mode = {};
//    struct vehicle_control_mode_s _control_mode;
    struct position_setpoint_triplet_s _pos_sp_triplet = {};
    struct vehicle_control_mode_s _control_mode;
    struct offboard_setpoint_s _offboard_sp = {};

    /*公告主题*/
    /*offboard_pub_pub为handle指针*/
    orb_advert_t offboard_pub_pub = orb_advertise(ORB_ID(offboard_control_mode), &_offboard_control_mode);
    orb_advert_t pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet), &_pos_sp_triplet);
    while (!thread_should_exit) {

        /*结构体变量赋值*/
        _offboard_control_mode.ignore_acceleration_force = true;
        _offboard_control_mode.ignore_alt_hold = _offboard_sp.ignore_alt_hold;
        _offboard_control_mode.ignore_attitude = _offboard_sp.ignore_attitude;
        _offboard_control_mode.ignore_bodyrate = true;
        _offboard_control_mode.ignore_position = _offboard_sp.ignore_position;
        _offboard_control_mode.ignore_thrust = true;
        _offboard_control_mode.ignore_velocity = _offboard_sp.ignore_velocity;

        _offboard_control_mode.timestamp = hrt_absolute_time();


        /*发布数据*/
        orb_publish(ORB_ID(offboard_control_mode), offboard_pub_pub, &_offboard_control_mode);
    orb_copy(ORB_ID(rc_channels), rc_channels_sub, &_rc_channels);//获取数据
    printf("channel 4 = %8.4f\n", (double)_rc_channels.channels[4]);

        bool updated_mode;
        bool updated_data;

        orb_check(control_mode_sub, &updated_mode);//检查topic是否获取成功
        orb_check(offboard_setpoint_sub, &updated_data);

        if (updated_mode) {
            orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &_control_mode);
        }
        if (updated_data) {
            orb_copy(ORB_ID(offboard_setpoint), offboard_setpoint_sub, &_offboard_sp);//获取数据
        }

        if (_control_mode.flag_control_offboard_enabled) {

            _pos_sp_triplet.timestamp = hrt_absolute_time();
            _pos_sp_triplet.previous.valid = false;
            _pos_sp_triplet.next.valid = false;
            _pos_sp_triplet.current.valid = true;


            bool is_takeoff_sp = _offboard_sp.is_takeoff_sp;
            bool is_land_sp = _offboard_sp.is_land_sp;
            bool is_loiter_sp = _offboard_sp.is_loiter_sp;
            bool is_idle_sp = _offboard_sp.is_idle_sp;

            if (is_loiter_sp) {
                _pos_sp_triplet.current.type = SETPOINT_TYPE_LOITER;

            } else if (is_takeoff_sp) {
                _pos_sp_triplet.current.type = SETPOINT_TYPE_TAKEOFF;

            } else if (is_land_sp) {
                _pos_sp_triplet.current.type = SETPOINT_TYPE_LAND;

            } else if (is_idle_sp) {
                _pos_sp_triplet.current.type = SETPOINT_TYPE_IDLE;

            } else {
                _pos_sp_triplet.current.type = SETPOINT_TYPE_POSITION;
            }

            if (!_offboard_control_mode.ignore_position) {
                _pos_sp_triplet.current.position_valid = true;
                _pos_sp_triplet.current.x = _offboard_sp.x;//设的点赋给系统参数
                _pos_sp_triplet.current.y = _offboard_sp.y;
                _pos_sp_triplet.current.z = _offboard_sp.z;

                warnx("ignore position");

            } else {
                _pos_sp_triplet.current.position_valid = false;
            }

            /* set the local vel values */
            if (!_offboard_control_mode.ignore_velocity) {
                _pos_sp_triplet.current.velocity_valid = true;
                _pos_sp_triplet.current.vx = _offboard_sp.vx;
                _pos_sp_triplet.current.vy = _offboard_sp.vy;
                _pos_sp_triplet.current.vz = _offboard_sp.vz;

                if (_offboard_sp.is_local_frame) {
                    _pos_sp_triplet.current.velocity_frame = VELOCITY_FRAME_LOCAL_NED;
                } else {
                    _pos_sp_triplet.current.velocity_frame = VELOCITY_FRAME_BODY_NED;
                }

//                    set_position_target_local_ned.coordinate_frame;
                warnx("ignore velocity");

            } else {
                _pos_sp_triplet.current.velocity_valid = false;
            }

            if (!_offboard_control_mode.ignore_alt_hold) {
                _pos_sp_triplet.current.alt_valid = true;
                _pos_sp_triplet.current.z = _offboard_sp.z;

            } else {
                _pos_sp_triplet.current.alt_valid = false;
            }

            /* set the local acceleration values if the setpoint type is 'local pos' and none
             * of the accelerations fields is set to 'ignore' */
            if (!_offboard_control_mode.ignore_acceleration_force) {
                _pos_sp_triplet.current.acceleration_valid = true;
                _pos_sp_triplet.current.a_x = 0.0;
                _pos_sp_triplet.current.a_y = 0.0;
                _pos_sp_triplet.current.a_z = 0.0;
//                _pos_sp_triplet.current.acceleration_is_force =
//                    is_force_sp;

            } else {
                _pos_sp_triplet.current.acceleration_valid = false;
            }

            /* set the yaw sp value */
            if (!_offboard_control_mode.ignore_attitude) {
                _pos_sp_triplet.current.yaw_valid = true;
                _pos_sp_triplet.current.yaw = _offboard_sp.yaw;

            } else {
                _pos_sp_triplet.current.yaw_valid = false;
            }

            /* set the yawrate sp value */
            if (!_offboard_control_mode.ignore_bodyrate) {
                _pos_sp_triplet.current.yawspeed_valid = true;
//                _pos_sp_triplet.current.yawspeed = set_position_target_local_ned.yaw_rate;

            } else {
                _pos_sp_triplet.current.yawspeed_valid = false;
            }

            orb_publish(ORB_ID(position_setpoint_triplet), pos_sp_triplet_pub, &_pos_sp_triplet);
        }
        usleep(200000);
    }

    warnx("[daemon] exiting.\n");

    thread_running = false;

    return 0;
}






我知道答案 回答被采纳将会获得3 阿木币 已有5人回答

本帖子中包含更多资源

您需要 登录 才可以下载或查看,没有帐号?立即注册   

x

扫一扫浏览分享
回复

使用道具 举报

126

阿木币

0

精华

28 小时

在线时间

老司机

Rank: 2

发表于 2021-11-11 08:54:31
PX4一直拒絕切換到offboard模式   qgc上有什么提示吗? 然后你师兄的代码 去px4代码看看是不是把一些条件判断语句屏蔽了啊
回复 点赞

使用道具 举报

0

阿木币

0

精华

5 小时

在线时间

应届白菜

Rank: 1

 楼主| 发表于 2021-11-11 17:27:11
goodQ 发表于 2021-11-11 08:54
PX4一直拒絕切換到offboard模式   qgc上有什么提示吗? 然后你师兄的代码 去px4代码看看是不是把一些条件判 ...

最恶心的就是QGC没有任何错误提示
回复 点赞

使用道具 举报

61

阿木币

0

精华

32 小时

在线时间

技术大V

Rank: 4

发表于 2021-11-12 14:34:13
mavros这边连接上后也会有消息提示,你可以在这一块看看有没有什么相关信息提示
回复 点赞

使用道具 举报

87

阿木币

1

精华

158 小时

在线时间

版主

Rank: 7Rank: 7Rank: 7

发表于 2021-11-15 12:18:00
你用是官方的固件,还是已经修改过的固件?还有发送的频率给到20Hz以上,试试
回复 点赞

使用道具 举报

23

阿木币

0

精华

262 小时

在线时间

技术大V

Rank: 4

发表于 2021-11-15 16:16:01
飞控固件是什么版本的呢?
回复 点赞

使用道具 举报

返回列表
您需要登录后才可以回帖 登录 | 立即注册   

本版积分规则

Official store

Official store

 阿木实验室

Number

Number

028-87872048

扫一扫,快速加入

群号652692981

硬件评测

群号652692981

课程学习

快速回复 返回顶部 返回列表