PX4固件版本/1/11/3
程序位置
订阅需要的数据并设置订阅的频率
voidRoverPositionControl//run(){ _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode))/ _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position))/ _local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position))/ _manual_control_setpoint_sub = orb_subscribe(ORB_ID(manual_control_setpoint))/ _pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet))/ _att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint))/ _vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude))/ _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined))/ orb_set_interval(_control_mode_sub/ 200)/ orb_set_interval(_global_pos_sub/ 20)/ orb_set_interval(_local_pos_sub/ 20)/更新参数
parameters_update(true)/阻塞等待
px4_pollfd_struct_t fds[5]/ fds[0]/fd = _global_pos_sub/ fds[0]/events = POLLIN/ fds[1]/fd = _manual_control_setpoint_sub/ fds[1]/events = POLLIN/ fds[2]/fd = _sensor_combined_sub/ fds[2]/events = POLLIN/ fds[3]/fd = _vehicle_attitude_sub/ // Poll attitude fds[3]/events = POLLIN/ fds[4]/fd = _local_pos_sub/ // Added local position as source of position fds[4]/events = POLLIN/大循环
while (!should_exit()) {等待数据
int pret = px4_poll(&/fds[0]/ (sizeof(fds) / sizeof(fds[0]))/ 500)/没有数据则不执行下面的语句，重新循环
if (pret </ 0) { warn( poll error %d/ %d / pret/ errno)/ continue/}如果消息更新，则订阅消息
具体定义如下：
vehicle_control_mode_poll()/attitude_setpoint_poll()/订阅加速度
_vehicle_acceleration_sub/update()/跟新参数
parameters_update()/手动模式标志
bool manual_mode = _control_mode/flag_control_manual_enabled/如果全球位置和本地位置跟新，则运行***
if (fds[0]/revents &/ POLLIN || fds[4]/revents &/ POLLIN) { perf_begin(_loop_perf)/订阅全球位置和本地位置
orb_copy(ORB_ID(vehicle_global_position)/ _global_pos_sub/ &/_global_pos)/orb_copy(ORB_ID(vehicle_local_position)/ _local_pos_sub/ &/_local_pos)/position_setpoint_triplet_poll()/如果是offboard模式
//Convert Local setpoints to global setpointsif (_control_mode/flag_control_offboard_enabled) {如果没有初始化本地坐标原点经纬度，则初始化本地坐标原点经纬度
if (!globallocalconverter_initialized()) { globallocalconverter_init(_local_pos/ref_lat/ _local_pos/ref_lon/ _local_pos/ref_alt/ _local_pos/ref_timestamp)/}将全球坐标转为本地坐标
else { globallocalconverter_toglobal(_pos_sp_triplet/current/x/ _pos_sp_triplet/current/y/ _pos_sp_triplet/current/z/ &/_pos_sp_triplet/current/lat/ &/_pos_sp_triplet/current/lon/ &/_pos_sp_triplet/current/alt)/ } }重置计数
// update the reset counters in any case_pos_reset_counter = _global_pos/lat_lon_reset_counter/定义位置和速度向量
matrix//Vector3f ground_speed(_local_pos/vx/ _local_pos/vy/ _local_pos/vz)/matrix//Vector2f current_position((float)_global_pos/lat/ (float)_global_pos/lon)/matrix//Vector3f current_velocity(_local_pos/vx/ _local_pos/vy/ _local_pos/vz)/如果是位置控制模式
if (!manual_mode &/&/ _control_mode/flag_control_position_enabled) {运行位置***
if (control_position(current_position/ ground_speed/ _pos_sp_triplet)) {赋值相应的状态并发布
//TODO/ check if radius makes sense here float turn_distance = _param_l1_distance/get()/ //_gnd_control/switch_distance(100/0f)/ // publish status position_controller_status_s pos_ctrl_status = {}/ pos_ctrl_status/nav_roll = 0/0f/ pos_ctrl_status/nav_pitch = 0/0f/ pos_ctrl_status/nav_bearing = _gnd_control/nav_bearing()/ pos_ctrl_status/target_bearing = _gnd_control/target_bearing()/ pos_ctrl_status/xtrack_error = _gnd_control/crosstrack_error()/ pos_ctrl_status/wp_dist = get_distance_to_next_waypoint(_global_pos/lat/ _global_pos/lon/ _pos_sp_triplet/current/lat/ _pos_sp_triplet/current/lon)/ pos_ctrl_status/acceptance_radius = turn_distance/ pos_ctrl_status/yaw_acceptance = NAN/ pos_ctrl_status/timestamp = hrt_absolute_time()/ _pos_ctrl_status_pub/publish(pos_ctrl_status)/}如果是速度控制模式，则运行速度***
} else if (!manual_mode &/&/ _control_mode/flag_control_velocity_enabled) { control_velocity(current_velocity/ _pos_sp_triplet)/} perf_end(_loop_perf)/ }如果姿态更新并且是使能了姿态控制，则运行姿态***
if (fds[3]/revents &/ POLLIN) { vehicle_attitude_poll()/ if (!manual_mode &/&/ _control_mode/flag_control_attitude_enabled &/&/ !_control_mode/flag_control_position_enabled &/&/ !_control_mode/flag_control_velocity_enabled) { control_attitude(_vehicle_att/ _att_sp)/ }}订阅***值
if (fds[1]/revents &/ POLLIN) { // This should be copied even if not in manual mode/ Otherwise/ the poll(///) call will keep // returning immediately and this loop will eat up resources/ orb_copy(ORB_ID(manual_control_setpoint)/ _manual_control_setpoint_sub/ &/_manual_control_setpoint)/如果是遥控模式，则直接根据***输出，这里虽然赋值四个，但只有偏航和推力有效
if (manual_mode) { //PX4_INFO( Manual mode! )/ _act_controls/control[actuator_controls_s//INDEX_ROLL] = _manual_control_setpoint/y/ _act_controls/control[actuator_controls_s//INDEX_PITCH] = -_manual_control_setpoint/x/ _act_controls/control[actuator_controls_s//INDEX_YAW] = _manual_control_setpoint/r/ //TODO/ Readd yaw scale param _act_controls/control[actuator_controls_s//INDEX_THROTTLE] = _manual_control_setpoint/z/ }}订阅传感器数据
if (fds[2]/revents &/ POLLIN) { orb_copy(ORB_ID(sensor_combined)/ _sensor_combined_sub/ &/_sensor_combined)/只要有任何一种控制方式使能，就发布控制量
//orb_copy(ORB_ID(vehicle_attitude)/ _vehicle_attitude_sub/ &/_vehicle_att)/ _act_controls/timestamp = hrt_absolute_time()/ if (_control_mode/flag_control_velocity_enabled || _control_mode/flag_control_attitude_enabled || _control_mode/flag_control_position_enabled || manual_mode) { _actuator_controls_pub/publish(_act_controls)/ } }}取消订阅
orb_unsubscribe(_control_mode_sub)/ orb_unsubscribe(_global_pos_sub)/ orb_unsubscribe(_local_pos_sub)/ orb_unsubscribe(_manual_control_setpoint_sub)/ orb_unsubscribe(_pos_sp_triplet_sub)/ orb_unsubscribe(_vehicle_attitude_sub)/ orb_unsubscribe(_sensor_combined_sub)/ warnx( exiting/\n )/}***的运行时间间隔，使用非零值避免被零除
float dt = 0/01/ // Using non zero value to a avoid division by zero更新dt
if (_control_position_last_called >/ 0) { dt = hrt_elapsed_time(&/_control_position_last_called) * 1e-6f/} _control_position_last_called = hrt_absolute_time()/bool setpoint = true/如果是auto或者offboard模式，并且存在期望位置
if ((_control_mode/flag_control_auto_enabled || _control_mode/flag_control_offboard_enabled) &/&/ pos_sp_triplet/current/valid) {赋值控制模式
_control_mode_current = UGV_POSCTRL_MODE_AUTO/期望经纬度航点向量
matrix//Vector2f curr_wp((float)pos_sp_triplet/current/lat/ (float)pos_sp_triplet/current/lon)/如果存在上一个航点，则更新上一个航点，运行L1算法必须知道下一个航点和上一个航点
matrix//Vector2f prev_wp = curr_wp/if (pos_sp_triplet/previous/valid) { prev_wp(0) = (float)pos_sp_triplet/previous/lat/ prev_wp(1) = (float)pos_sp_triplet/previous/lon/}定义地速向量
matrix//Vector2f ground_speed_2d(ground_speed)/定义推力
float mission_throttle = _param_throttle_cruise/get()/ if (_param_speed_control_mode/get() == 1) {获取巡航速度
float mission_target_speed = _param_gndspeed_trim/get()/if (PX4_ISFINITE(_pos_sp_triplet/current/cruising_speed) &/&/ _pos_sp_triplet/current/cruising_speed >/ 0/1f) { mission_target_speed = _pos_sp_triplet/current/cruising_speed/}将地速转换到机体坐标系
// Velocity in body frameconst Dcmf R_to_body(Quatf(_vehicle_att/q)/inversed())/const Vector3f vel = R_to_body * Vector3f(ground_speed(0)/ ground_speed(1)/ ground_speed(2))/指向机头的速度
const float x_vel = vel(0)/指向机头的加速度
const float x_acc = _vehicle_acceleration_sub/get()/xyz[0]/PID计算期望推力
// Compute airspeed control out and just scale it as a constantmission_throttle = _param_throttle_speed_scaler/get() * pid_calculate(&/_speed_ctrl/ mission_target_speed/ x_vel/ x_acc/ dt)/推力限幅
// Constrain throttle between min and maxmission_throttle = math//constrain(mission_throttle/ _param_throttle_min/get()/ _param_throttle_max/get())/如果在_pos_sp_triplet中指定了期望推力，则使用开换控制
} else { if (PX4_ISFINITE(_pos_sp_triplet/current/cruising_throttle) &/&/ _pos_sp_triplet/current/cruising_throttle >/ 0/01f) { mission_throttle = _pos_sp_triplet/current/cruising_throttle/ }}获取当前位置到下一个航点的距离
float dist_target = get_distance_to_next_waypoint(_global_pos/lat/ _global_pos/lon/ (double)curr_wp(0)/ (double)curr_wp(1))/ // pos_sp_triplet/current/lat/ pos_sp_triplet/current/lon)/如果是跑航点
switch (_pos_ctrl_state) {case GOTO_WAYPOINT/ {如果当前点到下一个航点的距离小于参数NAV_LOITER_RAD，则进入停止
if (dist_target </ _param_nav_loiter_rad/get()) { _pos_ctrl_state = STOPPING/ // We are closer than loiter radius to waypoint/ stop/}利用L1算法计算侧向加速度
else { _gnd_control/navigate_waypoints(prev_wp/ curr_wp/ current_position/ ground_speed_2d)/赋值推力
_act_controls/control[actuator_controls_s//INDEX_THROTTLE] = mission_throttle/根据侧向加速度计算偏航力矩．无人车/船在施加偏航力矩后，实际上是在做圆周运动，根据圆周运动加速度计算公式：a=v^2/r
其中期望加速度a和运动速度v已知，可以求出期望的转弯半径r＝v^2/a/这里只计算绝对值．
float desired_r = ground_speed_2d/norm_squared() / math//abs_t(_gnd_control/nav_lateral_acceleration_demand())/根据几何知识，可以由转弯半径和轮子直径计算转舵的角度
float desired_theta = (0/5f * M_PI_F) - atan2f(desired_r/ _param_wheel_base/get())/根据转舵角度结合侧向加速度的方向，计算出偏航控制量大小和方向
float control_effort = (desired_theta / _param_max_turn_angle/get()) * sign( _gnd_control/nav_lateral_acceleration_demand())/control_effort = math//constrain(control_effort/ -1/0f/ 1/0f)/输出
_act_controls/control[actuator_controls_s//INDEX_YAW] = control_effort/ }} break/停止状态偏航和推力都为0，
case STOPPING/ { _act_controls/control[actuator_controls_s//INDEX_YAW] = 0/0f/ _act_controls/control[actuator_controls_s//INDEX_THROTTLE] = 0/0f/求上一个航点与下一个航点的距离
// Note _prev_wp is different to the local prev_wp which is related to a mission waypoint/float dist_between_waypoints = get_distance_to_next_waypoint((double)_prev_wp(0)/ (double)_prev_wp(1)/ (double)curr_wp(0)/ (double)curr_wp(1))/如果距离大于0/说明有新的航点，进入航点状态
if (dist_between_waypoints >/ 0) { _pos_ctrl_state = GOTO_WAYPOINT/ // A new waypoint has arrived go to it} } break/默认置为停止状态
default/ PX4_ERR( Unknown Rover State )/ _pos_ctrl_state = STOPPING/ break/}当前航点置为上一次的航点，
_prev_wp = curr_wp/} else { _control_mode_current = UGV_POSCTRL_MODE_OTHER/ setpoint = false/ } return setpoint/}***的运行时间间隔，使用非零值避免被零除
float dt = 0/01/ // Using non zero value to a avoid division by zero定义期望推力
const float mission_throttle = _param_throttle_cruise/get()/从pos_sp_triplet中取期望速度
const matrix//Vector3f desired_velocity{pos_sp_triplet/current/vx/ pos_sp_triplet/current/vy/ pos_sp_triplet/current/vz}/期望速度取模
const float desired_speed = desired_velocity/norm()/将速度转换到机体坐标系
if (desired_speed >/ 0/01f) { const Dcmf R_to_body(Quatf(_vehicle_att/q)/inversed())/ const Vector3f vel = R_to_body * Vector3f(current_velocity(0)/ current_velocity(1)/ current_velocity(2))/定义机头方向的速度和家速度
const float x_vel = vel(0)/const float x_acc = _vehicle_acceleration_sub/get()/xyz[0]/PID计算推力
const float control_throttle = pid_calculate(&/_speed_ctrl/ desired_speed/ x_vel/ x_acc/ dt)/限幅并赋值，幅度从0到mission_throttle
//Constrain maximum throttle to mission throttle_act_controls/control[actuator_controls_s//INDEX_THROTTLE] = math//constrain(control_throttle/ 0/0f/ mission_throttle)/定义期望的机体速度
Vector3f desired_body_velocity/如果期望的速度是机体坐标系的，则直接使用，如果是地理坐标系，则转换为机体坐标系下
if (pos_sp_triplet/current/velocity_frame == position_setpoint_s//VELOCITY_FRAME_BODY_NED) { desired_body_velocity = desired_velocity/} else { // If the frame of the velocity setpoint is unknown/ assume it is in local frame desired_body_velocity = R_to_body * desired_velocity/ }求机体坐标系下期望速度的角度。
const float desired_theta = atan2f(desired_body_velocity(1)/ desired_body_velocity(0))/除以参数GND_MAX_ANG，该参数越小，偏航角的控制越灵敏，如果船走S形，将该参数调小
float control_effort = desired_theta / _param_max_turn_angle/get()/赋值偏航控制量
control_effort = math//constrain(control_effort/ -1/0f/ 1/0f)/ _act_controls/control[actuator_controls_s//INDEX_YAW] = control_effort/}else { _act_controls/control[actuator_controls_s//INDEX_THROTTLE] = 0/0f/ _act_controls/control[actuator_controls_s//INDEX_YAW] = 0/0f/ }}计算偏航角误差
// quaternion attitude control law/ qe is rotation from q to qdconst Quatf qe = Quatf(att/q)/inversed() * Quatf(att_sp/q_d)/const Eulerf euler_sp = qe/将偏航角误差除以GND_MAX_ANG直接赋值
float control_effort = euler_sp(2) / _param_max_turn_angle/get()/control_effort = math//constrain(control_effort/ -1/0f/ 1/0f)/_act_controls/control[actuator_controls_s//INDEX_YAW] = control_effort/根据设定值赋值推力
const float control_throttle = att_sp/thrust_body[0]/ _act_controls/control[actuator_controls_s//INDEX_THROTTLE] = math//constrain(control_throttle/ 0/0f/ 1/0f)/}L1导航控制的作用是计算控制无人船跑航线时的侧向加速度
代码位置：
vector_A是上一个航点的位置（刚经过的那个），vector_B是现在正要去的那个航点的位置，vector_curr_position是当前位置，ground_speed_vector是速度
voidECL_L1_Pos_Controller//navigate_waypoints(const Vector2f &/vector_A/ const Vector2f &/vector_B/ const Vector2f &/vector_curr_position/ const Vector2f &/ground_speed_vector){ float eta = 0/0f/ float xtrack_vel = 0/0f/ float ltrack_vel = 0/0f/计算期望的方位，也就是上一个经过的航点与目前正在前往的那个航点组成的向量的方向
_target_bearing = get_bearing_to_next_waypoint((double)vector_curr_position(0)/ (double)vector_curr_position(1)/ (double)vector_B(0)/ (double)vector_B(1))/***速度最小为0/1，防止除0/
float ground_speed = math//max(ground_speed_vector/length()/ 0/1f)/计算_L1_distance，
_L1_distance = _L1_ratio * ground_speed/其中_L1_ratio由下式求得
_L1_damping和_L1_period对应下面的参数
计算由向量A指向向量B的向量
Vector2f vector_AB = get_local_planar_vector(vector_A/ vector_B)/检查航路点是否重叠
if (vector_AB/length() </ 1/0e-6f) { vector_AB = get_local_planar_vector(vector_curr_position/ vector_B)/}向量归一化
vector_AB/normalize()/计算有A点指向当前位置的向量
Vector2f vector_A_to_airplane = get_local_planar_vector(vector_A/ vector_curr_position)/计算所需的各个向量，如下图：
_crosstrack_error = vector_AB % vector_A_to_airplane/ float distance_A_to_airplane = vector_A_to_airplane/length()/ float alongTrackDist = vector_A_to_airplane * vector_AB/ Vector2f vector_B_to_P_unit = get_local_planar_vector(vector_B/ vector_curr_position)/normalized()/ // XXX this could probably also be based solely on the dot product float AB_to_BP_bearing = atan2f(vector_B_to_P_unit % vector_AB/ vector_B_to_P_unit * vector_AB)/L1算法的原理如下
下面分三种情况求eta
if (distance_A_to_airplane >/ _L1_distance &/&/ alongTrackDist / math//max(distance_A_to_airplane/ 1/0f) </ -0/7071f) {将A点作为参考点，如下图叉乘求sin（eta）*ground_speed_vector的模，再点乘求cos（eta）*ground_speed_vector的模，再相除求tan（eta），最后反正切得角度。
Vector2f vector_A_to_airplane_unit = vector_A_to_airplane/normalized()/ xtrack_vel = ground_speed_vector % (-vector_A_to_airplane_unit)/ ltrack_vel = ground_speed_vector * (-vector_A_to_airplane_unit)/ eta = atan2f(xtrack_vel/ ltrack_vel)/ _nav_bearing = atan2f(-vector_A_to_airplane_unit(1)/ -vector_A_to_airplane_unit(0))/ }else if (fabsf(AB_to_BP_bearing) </ math//radians(100/0f)) {直接把B点作为参考点，如下图
叉乘求sin（eta）*ground_speed_vector的模，再点乘求cos（eta）*ground_speed_vector的模，再相除求tan（eta），最后反正切得角度。
xtrack_vel = ground_speed_vector % (-vector_B_to_P_unit)/ ltrack_vel = ground_speed_vector * (-vector_B_to_P_unit)/ eta = atan2f(xtrack_vel/ ltrack_vel)/ _nav_bearing = atan2f(-vector_B_to_P_unit(1)/ -vector_B_to_P_unit(0))/}else {利用标准的L1算法求eta，如下图。
eta2就是求速度与航线方向的夹脚，先叉乘求sin（eta2）*ground_speed_vector的模，再点乘求cos（eta2）*ground_speed_vector的模，再相除求tan（eta2），最后反正切得角度。
xtrack_vel = ground_speed_vector % vector_AB/ltrack_vel = ground_speed_vector * vector_AB/float eta2 = atan2f(xtrack_vel/ ltrack_vel)/这个xtrackErr和上面图中的_crosstrack_error是一样的，
float xtrackErr = vector_A_to_airplane % vector_AB/如图中，xtrackErr/_L1_distance得到eta1的sin值
float sine_eta1 = xtrackErr / math//max(_L1_distance/ 0/1f)/限幅并求eta1
sine_eta1 = math//constrain(sine_eta1/ -0/7071f/ 0/7071f)/ //sin(pi/4) = 0/7071float eta1 = asinf(sine_eta1)/求eta
eta = eta1 eta2/ _nav_bearing = atan2f(vector_AB(1)/ vector_AB(0)) eta1/}限幅eta并利用L1算法公式求侧向加速度
eta = math//constrain(eta/ (-M_PI_F) / 2/0f/ M_PI_F / 2/0f)/_lateral_accel = _K_L1 * ground_speed * ground_speed / _L1_distance * sinf(eta)/ _circle_mode = false/ _bearing_error = eta/ update_roll_setpoint()/}