local_position_estimator
mavlink
mc_pos_control
mc_att_control
mixer
一般的offboard模式需要给飞控发送两条mavlink消息：
//飞控在接收到mavlink设置点消息的时候执行下面函数
void MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t *msg)
{
//这个函数把消息解析后发布了两个消息：
//offboard_control_mode和position_setpoint_triplet
}//飞控在接收到mavlink当前点消息用来进行外部位置估计的时候执行下面函数
void MavlinkReceiver::handle_message_att_pos_mocap(mavlink_message_t *msg)
{
//这个函数把消息解析后发布消息：
//att_pos_mocap
}首先分析handle_message_set_position_target_local_ned这个函数发出的两条消息各是什么内容
发出的offboard_control_mode_s结构体:
offboard_control_mode.timestamp = hrt_absolute_time();
offboard_control_mode.ignore_position = (bool)(set_position_target_local_ned.type_mask & 0x7);
offboard_control_mode.ignore_alt_hold = (bool)(set_position_target_local_ned.type_mask & 0x4);
offboard_control_mode.ignore_velocity = (bool)(set_position_target_local_ned.type_mask & 0x38);
offboard_control_mode.ignore_acceleration_force = (bool)(set_position_target_local_ned.type_mask & 0x1C0);
bool is_force_sp = (bool)(set_position_target_local_ned.type_mask & (1 << 9));
offboard_control_mode.ignore_attitude = (bool)(set_position_target_local_ned.type_mask & 0x400);
offboard_control_mode.ignore_bodyrate = (bool)(set_position_target_local_ned.type_mask & 0x800);读取的vehicle_control_mode_s结构体判断当前是否是offboard模式，如果是则发送position_setpoint_triplet消息
发出的position_setpoint_triplet_s结构体:
pos_sp_triplet.current.type
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;
pos_sp_triplet.current.position_valid = true;
pos_sp_triplet.current.x = set_position_target_local_ned.x;
pos_sp_triplet.current.y = set_position_target_local_ned.y;
pos_sp_triplet.current.z = set_position_target_local_ned.z;
pos_sp_triplet.current.velocity_valid = true;
pos_sp_triplet.current.vx = set_position_target_local_ned.vx;
pos_sp_triplet.current.vy = set_position_target_local_ned.vy;
pos_sp_triplet.current.vz = set_position_target_local_ned.vz;
pos_sp_triplet.current.acceleration_valid = true;
pos_sp_triplet.current.a_x = set_position_target_local_ned.afx;
pos_sp_triplet.current.a_y = set_position_target_local_ned.afy;
pos_sp_triplet.current.a_z = set_position_target_local_ned.afz;
pos_sp_triplet.current.acceleration_is_force = is_force_sp;
pos_sp_triplet.current.yaw_valid = true;
pos_sp_triplet.current.yaw = set_position_target_local_ned.yaw;
pos_sp_triplet.current.yawspeed_valid = true;
pos_sp_triplet.current.yawspeed = set_position_target_local_ned.yaw_rate;在Firmware\src\modules\local_position_estimator\sensors下的mocap.cpp中有这么一行
static const uint32_t MOCAP_TIMEOUT = 200000; // 0.2 s这代表att_pos_mocap消息的发送频率必须大于5Hz，不然就会timeout，但是在实际测试过程中，发送的频率在30Hz以上才能稳定不timeout，这是因为它的程序写的太苛刻，只要每两帧有效数据的时间戳间隔大于0.2s就会timeout。
在Firmware\src\modules\local_position_estimator下的BlockLocalPositionEstimator.cpp中有这么一段
if (mocapUpdated) {
if (_sensorTimeout & SENSOR_MOCAP)
{
mocapInit();
{
mocapCorrect();
}
}只要超时就会重新初始化。
如果外部估计的位置足够精确，可以选择不用飞控上的其他传感器，可以在参数LPE_FUSION中设置。
然后转到旋翼的位置控制模块：
在目录E:\github\Firmware\src\modules\mc_pos_control下的mc_pos_control_main.cpp文件
打开task_main函数，发现
_pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));它订阅了position_setpoint_triplet和vehicle_control_mode这两个消息。
然后是
update_velocity_derivative();
//这个函数用来更新速度偏差，如果目标位置丢失，则速度偏差为0。do_control(dt);
//这里面的一些分支判断用的是vehicle_control_mode_s这个结构体。do_control调用了
control_non_manual(dt);control_non_manual调用了
control_offboard(dt);
control_position();control_offboard用来判断是位置控制还是姿态控制并把控制量获取并发布。
我这里用的是位置和偏航角控制，所以在这个函数中执行的内容有
_pos_sp(0) = _pos_sp_triplet.current.x;
_pos_sp(1) = _pos_sp_triplet.current.y;
_run_pos_control = true;
_hold_offboard_xy = false;
_att_sp.yaw_body = _pos_sp_triplet.current.yaw;最后发布vehicle_local_position_setpoint这个消息
_local_pos_sp.timestamp = hrt_absolute_time();
_local_pos_sp.x = _pos_sp(0);
_local_pos_sp.y = _pos_sp(1);
_local_pos_sp.z = _pos_sp(2);
_local_pos_sp.yaw = _att_sp.yaw_body;
_local_pos_sp.vx = _vel_sp(0);
_local_pos_sp.vy = _vel_sp(1);
_local_pos_sp.vz = _vel_sp(2);
orb_advertise(ORB_ID(vehicle_local_position_setpoint), &_local_pos_sp);control_position调用calculate_velocity_setpoint函数和calculate_thrust_setpoint函数，calculate_velocity_setpoint根据位置偏差计算速度偏差，里面代码不长，直接贴出
void MulticopterPositionControl::control_position(float dt)
{
calculate_velocity_setpoint(dt);
if (_control_mode.flag_control_climb_rate_enabled || _control_mode.flag_control_velocity_enabled ||
_control_mode.flag_control_acceleration_enabled)
{
calculate_thrust_setpoint(dt);
} else {
_reset_int_z = true;
}
}在计算速度设置点函数calculate_velocity_setpoint中：
/* run position & altitude controllers, if enabled (otherwise use already computed velocity setpoints) */
if (_run_pos_control)
{
// If for any reason, we get a NaN position setpoint, we better just stay where we are.
if (PX4_ISFINITE(_pos_sp(0)) && PX4_ISFINITE(_pos_sp(1)))
{
_vel_sp(0) = (_pos_sp(0) - _pos(0)) * _params.pos_p(0);
_vel_sp(1) = (_pos_sp(1) - _pos(1)) * _params.pos_p(1);
} else
{
_vel_sp(0) = 0.0f;
_vel_sp(1) = 0.0f;
warn_rate_limited("Caught invalid pos_sp in x and y");
}
}
limit_altitude();
if (_run_alt_control)
{
if (PX4_ISFINITE(_pos_sp(2)))
{
_vel_sp(2) = (_pos_sp(2) - _pos(2)) * _params.pos_p(2);
} else
{
_vel_sp(2) = 0.0f;
warn_rate_limited("Caught invalid pos_sp in z");
}
}
//后面是对起飞速度的设置和各个方向速度的一个限制。calculate_thrust_setpoint函数根据速度设置点计算姿态设置点然后发布姿态设置点vehicle_attitude_setpoint。
再转到姿态控制模块
它订阅了vehicle_control_mode和vehicle_attitude_setpoint这两个消息。
_v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));姿态控制模块把姿态设置点转换为需要的角速度。
从位置设置点到最后的三个方向的设置角速度，中间代码仔细分析比较复杂，具体的数学公式整理起来是一个浩大的工程，一个大概的思路是：
这个模块把姿态设置点转化为角速率设置点，然后在mixer中控制电机。
