发表于 2021-11-10 18:14:38
6354 浏览 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 ( " \t running \n " );
} else {
warnx ( " \t not 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 ;
}
17798546041已获得悬赏 3 阿木币 最佳答案
最恶心的就是QGC没有任何错误提示
本帖子中包含更多资源
您需要 登录 才可以下载或查看,没有帐号?立即注册
x
扫一扫浏览分享
发表于 2021-11-11 08:54:31
PX4一直拒絕切換到offboard模式 qgc上有什么提示吗? 然后你师兄的代码 去px4代码看看是不是把一些条件判断语句屏蔽了啊
楼主|
发表于 2021-11-11 17:27:11
发表于 2021-11-12 14:34:13
mavros这边连接上后也会有消息提示,你可以在这一块看看有没有什么相关信息提示
发表于 2021-11-15 12:18:00
你用是官方的固件,还是已经修改过的固件?还有发送的频率给到20Hz以上,试试
发表于 2021-11-15 16:16:01