自主避障无人机FastDrone的控制代码笔记

PX4Ctrl进程介绍主进程主要对象LinearControl类的对象controller与PX4CtrlFSM类的对象fsm。订阅mavros广播的消息遥控通道在回调更新fsm中的rc_data。ros::Subscriber rc_sub;if(!param.takeoff_land.no_RC)// mavros will still publish wrong rc messages although no RC is connected{rc_subnh.subscribemavros_msgs::RCIn(/mavros/rc/in,10,boost::bind(RC_Data_t::feed,fsm.rc_data,_1));}在src/realflight_modules/px4ctrl/src/input.cpp实现原通道输出一般在1000 - 2000模式相关的通道归一化为0-1。reboot_cmd是RC通道8与教程中紧急停止开关RC的通道7不是同一个。for(inti0;i4;i){ch[i]((double)msg.channels[i]-1500.0)/500.0;if(ch[i]DEAD_ZONE)ch[i](ch[i]-DEAD_ZONE)/(1-DEAD_ZONE);elseif(ch[i]-DEAD_ZONE)ch[i](ch[i]DEAD_ZONE)/(1-DEAD_ZONE);elsech[i]0.0;}mode((double)msg.channels[4]-1000.0)/1000.0;gear((double)msg.channels[5]-1000.0)/1000.0;reboot_cmd((double)msg.channels[7]-1000.0)/1000.0;使用ROS服务设置飞控模式、解锁与操控fsm.set_FCU_mode_srvnh.serviceClientmavros_msgs::SetMode(/mavros/set_mode);fsm.arming_client_srvnh.serviceClientmavros_msgs::CommandBool(/mavros/cmd/arming);fsm.reboot_FCU_srvnh.serviceClientmavros_msgs::CommandLong(/mavros/cmd/command);# 在src/realflight_modules/px4ctrl/src/PX4CtrlFSM.h中PX4CtrlFSM类中 ros::ServiceClient set_FCU_mode_srv;状态机逻辑根据state值、遥控通道、定位信息等控制state切换。核心思路MANUAL_CTRL状态遥控器控制↓[多重检查条件]├─ 收到定位信息├─ 没有命令冲突├─ 速度合理↓state AUTO_HOVERcontroller.resetThrustMapping()set_hov_with_odom()toggle_offboard_mode(true) ← ★ 切换控制模式↓AUTO_HOVER 状态程序自动控制↓px4ctrl可以向PX4发送控制指令voidPX4CtrlFSM::process(){ros::Time now_timeros::Time::now();Controller_Output_t u;Desired_State_tdes(odom_data);boolrotor_low_speed_during_landfalse;// STEP1: state machine runsswitch(state){caseMANUAL_CTRL:if(rc_data.enter_hover_mode)// Try to jump to AUTO_HOVER{if(!odom_is_received(now_time)){ROS_ERROR([px4ctrl] Reject AUTO_HOVER(L2). No odom!);break;}//...}}}Fast-Drone-250/src/realflight_modules/px4ctrl/src/input.cpp中函数void RC_Data_t::feed(mavros_msgs::RCInConstPtr pMsg)切换与state相关的变量enter_hover_mode、第5通道 mode 0.75设置enter_hover_mode注释1下面的if-else被后面的覆盖了第6通道 gear 0.75is_command_mode trueif(!have_init_last_mode){have_init_last_modetrue;last_modemode;}if(!have_init_last_gear){have_init_last_geartrue;last_geargear;}if(!have_init_last_reboot_cmd){have_init_last_reboot_cmdtrue;last_reboot_cmdreboot_cmd;}// 1// 注意遥控第5通道档位控制模式只能从小PWM值到高PWM值切换才能使得enter_hover_mode为true。// 这个条件的含义在mode、last_mode均大于阈值即通道5在拨下后想要使得enter_hover_mode true得上下拨通道5if(last_modeAPI_MODE_THRESHOLD_VALUEmodeAPI_MODE_THRESHOLD_VALUE)enter_hover_modetrue;elseenter_hover_modefalse;if(modeAPI_MODE_THRESHOLD_VALUE)is_hover_modetrue;elseis_hover_modefalse;if(is_hover_mode){if(last_gearGEAR_SHIFT_VALUEgearGEAR_SHIFT_VALUE)enter_command_modetrue;elseif(gearGEAR_SHIFT_VALUE)enter_command_modefalse;if(gearGEAR_SHIFT_VALUE)is_command_modetrue;elseis_command_modefalse;}src/realflight_modules/px4ctrl/config/ctrl_param_fpv.yaml中thrust_model的hover_percentage设置等于无人机重力的推力对应油门百分比thr2acc_则是单位百分比产生的加速度不包含重力加速度。voidLinearControl::resetThrustMapping(void){thr2acc_param_.gra/param_.thr_map.hover_percentage;P_1e6;}参数符号单位物理意义param_.gragggm/s²重力加速度。无人机所在环境的重力加速度通常为9.8 m/s²param_.thr_map.hover_percentageThoverT_{hover}Thover​百分比 (0~1)悬停油门比例。指无人机保持静止悬停时所需的油门信号占总油门范围的百分比thr2acc_kthr2acck_{thr2acc}kthr2acc​m/s²/%推力-加速度映射系数。表示单位油门信号能产生的加速度是后续控制中用来从期望加速度计算所需油门的关键参数P_P0P_0P0​无量纲递推最小二乘算法的初始协方差。用于后续的在线推力模型估计初值较大表示对初始估计的不确定性很高ROS消息控制起飞降落的实现自定义控制起降的ROS消息FAST-DRONE-250/src/utils/quadrotor_msgs/msg/TakeoffLand.msg前两项是枚举第三项是用到的。uint8 TAKEOFF 1 uint8 LAND 2 uint8 takeoff_land_cmd使用方式rostopic pub -1 /px4ctrl/takeoff_land quadrotor_msgs/TakeoffLand takeoff_land_cmd: 2或rostopic pub -1 /px4ctrl/takeoff_land quadrotor_msgs/TakeoffLand takeoff_land_cmd: 1PX4Ctrl接收该消息的回调函数voidTakeoff_Land_Data_t::feed(quadrotor_msgs::TakeoffLandConstPtr pMsg){msg*pMsg;rcv_stampros::Time::now();triggeredtrue;takeoff_land_cmdpMsg-takeoff_land_cmd;}主进程循环调用void PX4CtrlFSM::process()先判断rc_data.enter_hover_mode由于为flase进入else前面执行takeoff.sh使得takeoff_land_data.takeoff_land_cmd quadrotor_msgs::TakeoffLand::TAKEOFF成立获得cmd信息// 从HOVER状态切换到CMD_CTRL状态elseif(rc_data.is_command_modecmd_is_received(now_time)){if(state_data.current_state.modeOFFBOARD){stateCMD_CTRL;desget_cmd_des();// 获取命令期望值}}// 或者处于CMD_CTRL状态{desget_cmd_des();}在switch语句后按步骤执行if(stateAUTO_HOVER||stateCMD_CTRL){// controller.estimateThrustModel(imu_data.a, bat_data.volt, param);controller.estimateThrustModel(imu_data.a,param);}// STEP3: solve and update new control commandsif(rotor_low_speed_during_land)// used at the start of auto takeoff{motors_idling(imu_data,u);}else{debug_msgcontroller.calculateControl(des,odom_data,imu_data,u);debug_msg.header.stampnow_time;debug_pub.publish(debug_msg);}// STEP4: publish control commands to mavrosif(param.use_bodyrate_ctrl){publish_bodyrate_ctrl(u,now_time);}else{publish_attitude_ctrl(u,now_time);}综合条件成立执行state AUTO_TAKEOFF; controller.resetThrustMapping(); set_start_pose_for_takeoff_land(odom_data); toggle_offboard_mode(true); // toggle on offboard before arm for (int i 0; i 10 ros::ok(); i) // wait for 0.1 seconds to allow mode change by FMU // mark { ros::Duration(0.01).sleep(); ros::spinOnce(); } if (param.takeoff_land.enable_auto_arm) { toggle_arm_disarm(true); } takeoff_land.toggle_takeoff_land_time now_time;函数toggle_offboard_mode在px4ctrl/src/PX4CtrlFSM.cpp参数true意为进入offboard模式反之退出并回到进入前的模式。bool PX4CtrlFSM::toggle_offboard_mode(bool on_off) { mavros_msgs::SetMode offb_set_mode; if (on_off) { state_data.state_before_offboard state_data.current_state; if (state_data.state_before_offboard.mode OFFBOARD) // Not allowed state_data.state_before_offboard.mode MANUAL; offb_set_mode.request.custom_mode OFFBOARD; if (!(set_FCU_mode_srv.call(offb_set_mode) offb_set_mode.response.mode_sent)) { ROS_ERROR(Enter OFFBOARD rejected by PX4!); return false; } } else { offb_set_mode.request.custom_mode state_data.state_before_offboard.mode; if (!(set_FCU_mode_srv.call(offb_set_mode) offb_set_mode.response.mode_sent)) { ROS_ERROR(Exit OFFBOARD rejected by PX4!); return false; } } return true; // if (param.print_dbg) // printf(offb_set_mode mode_sent%d(uint8_t)\n, offb_set_mode.response.mode_sent); }逻辑背景上述takeoff.sh执行前的操作及其顺序遥控器5通道拨到内侧六通道拨到下侧油门打到中位再执行roslaunch px4ctrl run_ctrl.launchsrc/realflight_modules/px4ctrl/src/input.cppif (last_mode API_MODE_THRESHOLD_VALUE mode API_MODE_THRESHOLD_VALUE) enter_hover_mode true; else enter_hover_mode false;因此enter_hover_mode false;double des_a_z exp((delta_t - AutoTakeoffLand_t::MOTORS_SPEEDUP_TIME) * 6.0) * 7.0 - 7.0;实现控制配置文件px4ctrl/config/ctrl_param_fpv.yaml中use_bodyrate_ctrl: false因此使用的是publish_attitude_ctrl(u, now_time);细节起飞分三个状态准备、上升、完成。准备3秒时长估计推力与加速度系数计算控制输出if(rotor_low_speed_during_land)// used at the start of auto takeoff{motors_idling(imu_data,u);}else{debug_msgcontroller.calculateControl(des,odom_data,imu_data,u);debug_msg.header.stampnow_time;debug_pub.publish(debug_msg);}有趣细节检查IMU消息频率的写法很妙小于100Hz就警告// check the frequencystaticintone_min_count9999;staticros::Time last_clear_count_timeros::Time(0.0);if((now-last_clear_count_time).toSec()1.0){if(one_min_count100){ROS_WARN(IMU frequency seems lower than 100Hz, which is too low!);}one_min_count0;last_clear_count_timenow;}one_min_count;判断降落的逻辑voidPX4CtrlFSM::land_detector(constState_t state,constDesired_State_tdes,constOdom_Data_todom){staticState_t last_stateState_t::MANUAL_CTRL;if(last_stateState_t::MANUAL_CTRL(stateState_t::AUTO_HOVER||stateState_t::AUTO_TAKEOFF)){takeoff_land.landedfalse;// Always holds}last_statestate;if(stateState_t::MANUAL_CTRL!state_data.current_state.armed){takeoff_land.landedtrue;return;// No need of other decisions}// land_detector parametersconstexprdoublePOSITION_DEVIATION_C-0.5;// Constraint 1: target position below real position for POSITION_DEVIATION_C meters.constexprdoubleVELOCITY_THR_C0.1;// Constraint 2: velocity below VELOCITY_MIN_C m/s.constexprdoubleTIME_KEEP_C3.0;// Constraint 3: Time(s) the Constraint 12 need to keep.staticros::Time time_C12_reached;// time_Constraints12_reachedstaticboolis_last_C12_satisfy;if(takeoff_land.landed){time_C12_reachedros::Time::now();is_last_C12_satisfyfalse;}else{boolC12_satisfy(des.p(2)-odom.p(2))POSITION_DEVIATION_Codom.v.norm()VELOCITY_THR_C;if(C12_satisfy!is_last_C12_satisfy){time_C12_reachedros::Time::now();}elseif(C12_satisfyis_last_C12_satisfy){if((ros::Time::now()-time_C12_reached).toSec()TIME_KEEP_C)// Constraint 3 reached{takeoff_land.landedtrue;}}is_last_C12_satisfyC12_satisfy;}}