mc_pos_control

pixhawk

mc_pos_control
ALTCTL

POSCTL



\/ALTCTL

AUTO
OFFBOARD
\/\/
MAVLink
MAVROSDronekit

ALTCTL

POSCTL
AUTO

OFFBOARD

(AzimuthalEquidistantProjection)

do_control(dt)

control_manual(dt)
ALTCTLPOSCTL
ALTCTL

_manual.zman_vel_sp(2)vel_sp(2)
_pos_sp(2)=_pos(2);
_run_alt_control=false;

_pos_sp(2)
_pos_hold_engaged
_pos_sp(0)=_pos(0);
_pos_sp(1)=_pos(1);
_run_pos_control=false;
_vel_sp(0)=man_vel_sp(0);
_vel_sp(1)=man_vel_sp(1);

POSCTL

_manual.zman_vel_sp(2)vel_sp(2)
_manual.xman_vel_sp(0)vel_sp(0)
_manual.yman_vel_sp(1)vel_sp(1)
_pos_sp(2)=_pos(2);
_run_alt_control=false;
_pos_sp(0)=_pos(0);
_pos_sp(1)=_pos(1);
_run_pos_control=false;
_vel_sp(0)=man_vel_sp(0);
_vel_sp(1)=man_vel_sp(1);

_pos_sp(2)
_pos_sp(0)
_pos_sp(1)

control_non_manual(dt)
OFFBOARDAUTO
control_offboard(dt)OFFBOARD

control_auto(dt)_pos_sp_triplet_pos_sp

pos_ff


cross_sphere_line

_pos_sp

control_position(dt)
calculate_velocity_setpoint(dt)
_run_pos_control_pos_sp(0)-_pos(0)_pos_sp(1)-_pos(1)
_vel_sp(0)_vel_sp(1)P
_run_alt_control_pos_sp(2)-_pos(2)_vel_sp(2)P
_control_mode.flag_control_velocity_enabledALTCTL_vel_sp(0)=
0.0f_vel_sp(1)=0.0f;

calculate_thrust_setpoint(dt)
POSCTL
vel_sp-_velPD_thrust_intNED
thrust_sp
thrust_spzthrust_body_z
_att_sp.thrust
thrust_sp_att_sp.yaw_body_att_sp.q_d

ALTCTL:
vel_sp-_velPD_thrust_intNED
thrust_sp
ALTCTLthrust_sp(0)=0.0f;thrust_sp(1)=0.0f;
thrust_spzthrust_body_z
_att_sp.thrust

AUTO
vel_sp-_velPD_thrust_intNED
thrust_sp
thrust_spzthrust_body_z
_att_sp.thrust
thrust_sp_att_sp.yaw_body_att_sp.q_d;

POSCTLALTCTLAUTOvel_sp-_velPD
_thrust_intNEDthrust_sp
ALTCTLthrust_sp(0)=0.0f;thrust_sp(1)=0.0f;
POSCTLALTCTLAUTOthrust_spz
thrust_body_z_att_sp.thrust
POSCTLAUTOOFFBOARDthrust_sp
_att_sp.yaw_body_att_sp.q_d

generate_attitude_setpoint(dt)
POSCTL:

_att_sp.yaw_sp_move_rate_att_sp.yaw_body
ALTCTL:

_att_sp.yaw_sp_move_rate_att_sp.yaw_body

_att_sp.roll_body_att_sp.pitch_body

:
POSCTLALTCTL
_att_sp.yaw_sp_move_rate_att_sp.yaw_body
ALTCTL
_att_sp.roll_body_att_sp.pitch_body