Code Flight control / interactive map
ArduGeo, from control block to source line.
Explore the PID-only geometric-control path inside ArduPilot. Start with the system architecture, then follow the paper's Guided and Loiter lifecycles into exact source boundaries.
52Interactive mappings
02Lifecycle walkthroughs
23Architecture blocks
81 / 81PID-only unit tests
01 / Interactive architecture
Trace the active control path
Focus a subsystem, then select any block to read its implementation directly in the side inspector—without leaving the architecture.
Tab focus· Enter source· Esc overview
Overview · 23 source-linked blocks
GCS_MAVLINK_Copter::handle_message_set_position_target_local_ned
void GCS_MAVLINK_Copter::handle_message_set_position_target_local_ned(
const mavlink_message_t &msg)
{
mavlink_set_position_target_local_ned_t packet;
mavlink_msg_set_position_target_local_ned_decode(&msg, &packet);
if (!copter.flightmode->in_guided_mode()) {
return;
}
}ModeGuided::run
void ModeGuided::run()
{
if (_geometric_boundary_pending) {
write_geometric_boundary_frame(1);
_geometric_boundary_pending = false;
}
if (!option_is_enabled(Option::GeometricMotorOutput)) {
_geometric_motor_output_rejected = false;
}
}AC_Geometric_GuidedTargetManager
if (should_merge_altitude_for_position_target(
position_ned_m, is_terrain_alt, current_pos_ned_m)) {
_position_target_ned_m.z = position_ned_m.z;
_target_type = TargetType::PositionAltitudeMerge;
return _position_target_ned_m;
}AC_GeometricControl::update
if (_shape_enabled &&
target.build_attitude_from_position &&
target.shape_position_target) {
AC_Geometric_Setpoint_Shaper_Limits limits {};
_setpoint_shaper.set_limits(limits);
_setpoint_shaper.update(state, target, dt, position_target);
_shaper_active = true;
}ModeLoiter::run
update_simple_mode();
get_pilot_desired_lean_angles_rad(
target_roll_rad, target_pitch_rad,
loiter_nav->get_angle_max_rad(),
attitude_control->get_althold_lean_angle_max_rad());
target_yaw_rate_rads = get_pilot_desired_yaw_rate_rads();
target_climb_rate_ms = get_pilot_desired_climb_rate_ms();ModeLoiter::run
void ModeLoiter::run()
{
float target_roll_rad, target_pitch_rad;
float target_yaw_rate_rads = 0.0f;
float target_climb_rate_ms = 0.0f;
bool geometric_takeoff_stopped_this_cycle = false;
bool geometric_ground_safe_prepared = false;
// …
}ModeLoiter::run_geometric_loiter_reference
input.pilot_accel_ne_mss =
pos_control->lean_angles_rad_to_accel_NED_mss(
Vector3f{target_roll_rad, target_pitch_rad,
ahrs.get_yaw_rad()}).xy();
input.pilot_xy_active = rc().has_valid_input() &&
(!is_zero(channel_roll->norm_input_dz()) ||
!is_zero(channel_pitch->norm_input_dz()));AC_Geometric_LoiterReference::update
bool AC_Geometric_LoiterReference::update(
const AC_Geometric_LoiterReference_Input& input,
const AC_Geometric_LoiterReference_Limits& limits,
float dt_s,
AC_Geometric_Target& target,
AC_Geometric_LoiterReference_Status& status)
{
status = {};
// …
}AC_Geometric_State / AC_Geometric_Target
struct AC_Geometric_Target {
Vector3f position_ned_m;
Vector3f velocity_ned_ms;
Vector3f accel_ned_mss;
Quaternion attitude_body_to_ned;
Vector3f omega_body_rads;
Vector3f omega_dot_body_radss;
bool build_attitude_from_position = false;
};Mode::run_geometric_observer
state.position_ned_m = Vector3f{
float(pos_estimate_ned_m.x),
float(pos_estimate_ned_m.y),
float(pos_estimate_ned_m.z)};
state.velocity_ned_ms = pos_control->get_vel_estimate_NED_ms();
ahrs.get_quat_body_to_ned(state.attitude_body_to_ned);
state.omega_body_rads = ahrs.get_gyro_latest();AC_Geometric_Position_PID::update
// Lee/Gao use e_x = x - x_d and e_v = v - v_d in inertial coordinates.
// ArduPilot's inertial frame here is NED, with positive Z pointing down.
const Vector3f position_error_raw_m = state.position_ned_m - target.position_ned_m;
const Vector3f velocity_error_raw_ms = state.velocity_ned_ms - target.velocity_ned_ms;AC_Geometric_Position_PID::update
if (target.build_attitude_from_position) {
output.attitude_body_to_ned =
attitude_from_thrust_vector(
output.thrust_vector_ned, target.yaw_rad);
Vector3f fallback_omega_body_rads = target.omega_body_rads;
fallback_omega_body_rads.z = target.yaw_rate_rads;
}AC_Geometric_Attitude_PID::update
const LeeAttitudeError attitude_diagnostics =
attitude_error_lee(
state.attitude_body_to_ned,
target.attitude_body_to_ned);
output.attitude_error = attitude_diagnostics.vector;
output.attitude_configuration_error =
attitude_diagnostics.configuration_error;
output.attitude_error_angle_rad = attitude_diagnostics.angle_rad;AC_Geometric_OutputMapper::update
output.throttle_norm_raw =
hover_throttle * position.thrust / GRAVITY_MSS;
output.throttle_norm =
constrain_float(output.throttle_norm_raw, 0.0f, 1.0f);
output.rpy_norm.x = constrain_float(
attitude.moment.x / moment_norm.x, -1.0f, 1.0f);ModeGuided::allows_geometric_motor_output
bool ModeGuided::geometric_motor_output_options_requested() const
{
return mode_number() == Number::GUIDED &&
option_is_enabled(Option::GeometricObserver) &&
option_is_enabled(Option::GeometricMotorOutput) &&
!copter.is_tradheli();
}Copter::geometric_motor_output_failure_flags
if (!geometric_control.output_is_fresh(
millis(), GEOMETRIC_OUTPUT_MAX_AGE_MS)) {
failure_flags |= GEO_FAIL_STALE;
}
if (!geometric_motor_output_is_valid()) {
failure_flags |= GEO_FAIL_INVALID;
}Copter::run_rate_controller_main
if (geometric_output_active) {
attitude_control->rate_controller_update_throttle_mix();
geometric_motor_output_to_motors();
geometric_motor_output_frame_count++;
geometric_motor_output_was_active = true;
} else {
attitude_control->rate_controller_run();
native_rate_controller_frame_count++;
}AC_AttitudeControl_Multi::rate_controller_run
void AC_AttitudeControl_Multi::rate_controller_run()
{
Vector3f gyro_latest_rads = _ahrs.get_gyro_latest();
rate_controller_run_dt(gyro_latest_rads, _dt_s);
}Mode::handle_geometric_motor_output_fallback
if (geometric_motor_output_was_active &&
!geometric_output_active) {
// Falling-edge state transfer rebuilds native attitude/rate state for
// fallback. In the normal main-thread case, rate_controller_run()
// below completes the handoff in this frame.
if (flightmode != nullptr) {
flightmode->handle_geometric_motor_output_fallback();
}
geometric_motor_output_was_active = false;
}Copter::geometric_motor_output_to_motors
motors->set_roll(mapped.rpy_norm.x);
motors->set_roll_ff(0.0f);
motors->set_pitch(mapped.rpy_norm.y);
motors->set_pitch_ff(0.0f);
motors->set_yaw(mapped.rpy_norm.z);
motors->set_yaw_ff(0.0f);
attitude_control->set_throttle_out(
mapped.throttle_norm, false, 0.0f);AP_Motors::rc_write
void AP_Motors::rc_write(uint8_t chan, uint16_t pwm)
{
SRV_Channel::Function function = SRV_Channels::get_motor_function(chan);
if ((1U<<chan) & _motor_pwm_scaled.mask) {
SRV_Channels::set_output_scaled(function, float(pwm) - _motor_pwm_scaled.offset);
} else {
SRV_Channels::set_output_pwm(function, pwm);
}
}ModeGuided::publish_geometric_position_reference
const bool position_published = pos_control->publish_external_reference_NED_m(
shaped_target.position_ned_m.topostype(),
shaped_target.velocity_ned_ms,
shaped_target.accel_ned_mss);
if (position_published &&
attitude_control->set_external_attitude_target(
geometric_position.attitude_body_to_ned,
geometric_position.omega_body_rads)) {
return true;
}Copter::Log_Write_Geometric_*
logger.WriteStreaming(
"GEOR", "TimeUS,PsiR,ERn,Ang", "s--r", "F000", "Qfff",
AP_HAL::micros64(),
(double)attitude.attitude_configuration_error,
(double)attitude.attitude_error.length(),
(double)attitude.attitude_error_angle_rad);Source-link status.
The paths and inspector excerpts are pinned to immutable mapped snapshot
0305a73b4ee0,
now published on the public ardugeo-pid branch.
Every selected block can open its exact GitHub line range.02 / Guided lifecycle
Follow Guided from entry to motor ownership
The paper's final Guided lifecycle figure becomes a source-linked walkthrough. Focus a stage, then select a highlighted step to inspect its primary PID-only implementation boundary.
Tab focus· Enter source· Esc overview
Overview · 14 source-linked steps
ModeGuided::init
const bool full_geometric_entry =
geometric_motor_output_options_requested();
if (full_geometric_entry) {
posvelaccel_control_start();
} else {
velaccel_control_start();
}ModeGuided::update_geometric_ground_safe_observer
geometric_target.velocity_ned_ms =
pos_control->get_vel_estimate_NED_ms();
geometric_target.accel_ned_mss =
Vector3f{0.0f, 0.0f, GRAVITY_MSS};
ahrs.get_quat_body_to_ned(
geometric_target.attitude_body_to_ned);ModeGuided::geometric_submode_supported
case SubMode::TakeOff:
return !guided_geometric_takeoff_terrain_alt;
case SubMode::Pos:
return !guided_is_terrain_alt;
case SubMode::PosVelAccel:
case SubMode::Land:
return true;ModeGuided::takeoff_run
if (!motors->armed() || !copter.ap.auto_armed ||
motors->get_spool_state() !=
AP_Motors::SpoolState::THROTTLE_UNLIMITED) {
guided_geometric_takeoff_spool_ready = false;
update_geometric_ground_safe_observer();
return;
}ModeGuided::set_pos_vel_accel_NED_m
guided_pos_target_ned_m =
guided_geometric_target_manager.set_position_target(
pos_ned_m, false,
geometric_position_control_active() ?
¤t_pos_ned_m : nullptr);
guided_vel_target_ned_ms = vel_ned_ms;
guided_accel_target_ned_mss = accel_ned_mss;ModeGuided::geometric_land_run
Vector3p landing_target_ned_m =
guided_geometric_land_hold_ned_m;
landing_target_ned_m.z = current_pos_ned_m.z;
target_velocity_ned_ms.z =
guided_geometric_land_descent_ned_ms;
update_geometric_position_observer(
&landing_target_ned_m, target_velocity_ned_ms,
target_accel_ned_mss, heading, false, false);AC_GeometricControl::update
if (_shape_enabled &&
target.build_attitude_from_position &&
target.shape_position_target) {
_setpoint_shaper.set_limits(limits);
_setpoint_shaper.update(
state, target, dt, position_target);
_shaper_active = true;
}AC_GeometricControl::update
_position_pid.update(
state, position_target, dt, _output.position);
_attitude_pid.update(
state, attitude_target, dt, _output.attitude);
_output_mapper.update(
_output.position, _output.attitude,
hover_throttle_norm(), moment_norm, _output.mapped);ModeGuided::publish_geometric_position_reference
const bool position_published =
pos_control->publish_external_reference_NED_m(
shaped_target.position_ned_m.topostype(),
shaped_target.velocity_ned_ms,
shaped_target.accel_ned_mss);
if (position_published &&
attitude_control->set_external_attitude_target(
geometric_position.attitude_body_to_ned,
geometric_position.omega_body_rads)) {
return true;
}Copter::geometric_motor_output_failure_flags
if (!geometric_control.output_is_fresh(
millis(), GEOMETRIC_OUTPUT_MAX_AGE_MS)) {
failure_flags |= GEO_FAIL_STALE;
}
if (!geometric_motor_output_is_valid()) {
failure_flags |= GEO_FAIL_INVALID;
}
if (flightmode != nullptr &&
!flightmode->geometric_motor_output_is_safe()) {
failure_flags |= GEO_FAIL_MODE_UNSAFE;
}Copter::run_rate_controller_main
if (geometric_output_active) {
attitude_control->rate_controller_update_throttle_mix();
geometric_motor_output_to_motors();
geometric_motor_output_frame_count++;
geometric_motor_output_was_active = true;
} else {
attitude_control->rate_controller_run();
native_rate_controller_frame_count++;
}Copter::run_rate_controller_main
if (geometric_motor_output_was_active &&
!geometric_output_active) {
if (flightmode != nullptr) {
flightmode->
handle_geometric_motor_output_fallback();
}
geometric_motor_output_was_active = false;
}ModeGuided::geometric_land_run
if (copter.ap.land_complete) {
motors->set_desired_spool_state(
AP_Motors::DesiredSpoolState::GROUND_IDLE);
update_geometric_ground_safe_observer();
if (motors->get_spool_state() ==
AP_Motors::SpoolState::GROUND_IDLE) {ModeGuided::geometric_land_run
if (motors->get_spool_state() ==
AP_Motors::SpoolState::GROUND_IDLE) {
// …
if (copter.arming.disarm(
AP_Arming::Method::LANDED)) {
posvelaccel_control_start();
update_geometric_ground_safe_observer();
}
}Mapping boundary.
This interactive map is pinned to public snapshot
0305a73b4ee0 and explains the supported PID-only lifecycle shown in the paper figure. Unsupported mode semantics remain on their native path.03 / Loiter lifecycle
Follow Loiter from pilot intent to ground idle
The paper's final Loiter lifecycle figure becomes a source-linked walkthrough of the dedicated reference, shared cascade, actuator ownership, and fallback boundary.
Tab focus· Enter source· Esc overview
Overview · 15 source-linked steps
ModeLoiter::init
_geometric_motor_output_active = false;
_geometric_motor_output_rejected = false;
reset_geometric_lifecycle();
_geometric_reference.reset();
pos_control->clear_external_reference();
const bool dedicated_prepared =
loiter_nav->geometric_motor_output_enabled() &&
geometric_reference_supported() &&
run_geometric_loiter_reference(initial_state, /* … */);ModeLoiter::run
update_simple_mode();
get_pilot_desired_lean_angles_rad(
target_roll_rad, target_pitch_rad,
loiter_nav->get_angle_max_rad(),
attitude_control->get_althold_lean_angle_max_rad());
target_yaw_rate_rads = get_pilot_desired_yaw_rate_rads();
target_climb_rate_ms = get_pilot_desired_climb_rate_ms();ModeLoiter::run
const bool dedicated_requested =
loiter_nav->geometric_motor_output_enabled();
const bool dedicated_supported =
geometric_reference_supported();
if (dedicated_requested && dedicated_supported &&
!_geometric_motor_output_rejected) {
if (run_geometric_loiter_reference(/* … */)) {
finish_geometric_lifecycle(/* … */);
return;
}
}ModeLoiter::run_geometric_loiter_reference
const bool ground_safe =
loiter_state == AltHoldModeState::MotorStopped ||
loiter_state == AltHoldModeState::Landed_Ground_Idle ||
loiter_state == AltHoldModeState::Landed_Pre_Takeoff;
if (ground_safe) {
build_geometric_ground_safe_target(target);
return update_geometric_observer(loiter_state, &target) &&
_geometric_motor_output_active;
}ModeLoiter::build_geometric_ground_safe_target
target.position_ned_m =
pos_control->get_pos_estimate_NED_m().tofloat();
target.velocity_ned_ms =
pos_control->get_vel_estimate_NED_ms();
target.accel_ned_mss =
Vector3f{0.0f, 0.0f, GRAVITY_MSS};
ahrs.get_quat_body_to_ned(target.attitude_body_to_ned);
target.build_attitude_from_position = false;ModeLoiter::run_geometric_loiter_reference
if (!takeoff.running()) {
takeoff.start_m(constrain_float(
g2.pilot_takeoff_alt_m, 0.0f, 10.0f));
_geometric_takeoff_target_valid = false;
}
if (!_geometric_takeoff_target_valid) {
_geometric_takeoff_target_z_ned_m =
-takeoff.get_complete_alt_U_m();
_geometric_takeoff_target_valid = true;
}ModeLoiter::run_geometric_loiter_reference
if (!_geometric_reference.initialized() &&
!reset_geometric_reference(false)) {
return false;
}
if (!handle_geometric_ekf_resets()) {
return false;
}
input.pilot_accel_ne_mss =
pos_control->lean_angles_rad_to_accel_NED_mss(
Vector3f{target_roll_rad, target_pitch_rad,
ahrs.get_yaw_rad()}).xy();ModeLoiter::geometric_reference_limits
limits.speed_xy_max_ms =
MAX(MIN(MIN(loiter_config.speed_max_ne_ms,
profile.speed_xy_max_ms),
ekf_ground_speed_limit_ms), 0.1f);
limits.accel_xy_max_mss =
MAX(MIN(profile.accel_xy_max_mss * control_scale_xy,
physical_accel_xy_max_mss), 0.1f);
limits.speed_up_max_ms = get_pilot_speed_up_ms();AC_Geometric_LoiterReference::update
const bool vertical_brake_requested =
!input.pilot_z_active || vertical_command_reversing;
// …
const float yaw_rate_target_rads =
input.pilot_yaw_active ? yaw_rate_command_rads : 0.0f;
// …
write_target(target);
_velocity_constraint_pending = true;
return true;AC_GeometricControl::update
_position_pid.update(
state, position_target, dt, _output.position);
_attitude_pid.update(
state, attitude_target, dt, _output.attitude);
_output_mapper.update(
_output.position, _output.attitude,
hover_throttle_norm(), moment_norm, _output.mapped);ModeLoiter::update_geometric_observer
if (!pos_control->publish_external_reference_NED_m(
target.position_ned_m.topostype(),
target.velocity_ned_ms,
target.accel_ned_mss)) {
pos_control->clear_external_reference();
return false;
}
if (!attitude_control->set_external_attitude_target(
geometric_position.attitude_body_to_ned,
geometric_position.omega_body_rads)) {
return false;
}Copter::geometric_motor_output_failure_flags
if (flightmode == nullptr ||
!flightmode->allows_geometric_motor_output()) {
failure_flags |= GEO_FAIL_MODE;
}
if (!motors->armed()) {
failure_flags |= GEO_FAIL_DISARMED;
}
if (!geometric_control.output_is_fresh(
millis(), GEOMETRIC_OUTPUT_MAX_AGE_MS)) {
failure_flags |= GEO_FAIL_STALE;
}Copter::run_rate_controller_main
if (geometric_output_active) {
attitude_control->rate_controller_update_throttle_mix();
geometric_motor_output_to_motors();
geometric_motor_output_frame_count++;
geometric_motor_output_was_active = true;
} else {
attitude_control->rate_controller_run();
native_rate_controller_frame_count++;
}ModeLoiter::deactivate_geometric_motor_output / handle_geometric_motor_output_fallback
_geometric_motor_output_active = false;
pos_control->clear_external_reference();
Mode::handle_geometric_motor_output_fallback();
if (reset_loiter_targets) {
reset_geometric_lifecycle();
_geometric_reference.reset();
loiter_nav->init_target();
pos_control->D_init_controller();
}ModeLoiter::finish_geometric_lifecycle
if (_geometric_lifecycle_in_progress &&
_geometric_touchdown_logged &&
loiter_state == AltHoldModeState::Landed_Ground_Idle &&
motors->get_spool_state() ==
AP_Motors::SpoolState::GROUND_IDLE) {
if ((g.throttle_behavior &
THR_BEHAVE_DISARM_ON_LAND_DETECT) != 0) {
copter.arming.disarm(AP_Arming::Method::LANDED);
}
}Mapping boundary.
This interactive map is pinned to public snapshot
0305a73b4ee0 and explains the supported PID-only lifecycle shown in the paper figure. Unsupported mode semantics remain on their native path.What this page claims
The map describes the verified software architecture for the tested Guided and Loiter PID-only paths. It does not claim HIL or real-flight safety, complete ArduCopter mode coverage, or performance superiority over the native controller.
Source lineage
ArduGeo is derived from ArduPilot. Code excerpts retain the upstream GPLv3 license.