C++ Flight Software

1. Design Philosophy

This flight software was built as a learning exercise in aerospace GNC engineering. Every module is implemented from scratch with minimal external dependencies (Eigen3 for linear algebra, Google Test for validation). The goal was not to build a flight-ready autopilot, but to understand the engineering tradeoffs behind each architectural decision.

Why hand-rolled?

The state machine uses explicit switch/case rather than a statechart library (e.g., Boost.SML). This was deliberate: a library would hide the transition logic behind template metaprogramming, making it harder to audit every state change. In production autopilots (PX4, ArduPilot), the state machine is similarly hand-rolled for the same reason—flight software must be fully auditable.

Why from-scratch dynamics?

Physics engines (Bullet, MuJoCo) provide ready-made rigid body solvers, but using one would skip the hardest part: correctly handling quaternion kinematics, gyroscopic coupling, and the interaction between aerodynamic forces and body-frame rotation. Writing the 6-DOF kernel from scratch forced explicit treatment of every term in the equations of motion.

Dependencies

2. Build System

The build uses CMake targeting C++17 with a static library for the core flight software and separate executables for demos, SIL, and tests.

fw/
  CMakeLists.txt
  include/fsw/        # 14 header files
  src/                # 14 source files + 3 executables
  tests/              # 8 test files
  build/              # CMake output (gitignored)

Build Targets

Build Commands

# Configure (C++17, Eigen3, export compile_commands.json)
cd fw && cmake -B build -DCMAKE_BUILD_TYPE=Debug

# Build all targets
cmake --build build -j$(sysctl -n hw.ncpu)

# Run tests
./build/tests/SwarmGNC_FW_Tests

# Run demos
./build/src/SwarmGNC_FW_Demo     # state machine + FDIR demo
./build/src/SwarmGNC_FW_SIL      # SIL runner (UDP MAVLink)
./build/src/SwarmGNC_6Dof_Demo   # hover stability test

3. Type System

All shared enumerations and data structures live in Types.hpp. Every module includes this header, creating a single source of truth for the flight software's vocabulary.

Enums

All enums use enum class with explicit uint8_t underlying type for size efficiency on embedded targets. Inline name-lookup functions (state_name(), event_name(), fault_name()) avoid virtual dispatch overhead.

Telemetry Message

struct TelemetryMessage {
  uint64_t timestamp_us;       // microsecond timestamp
  State    state;              // current flight phase
  float    pos_ned[3];         // position NED (m)
  float    vel_ned[3];         // velocity NED (m/s)
  float    quat[4];            // orientation (w,x,y,z)
  float    angular_rate[3];    // body-frame omega (rad/s)
  float    battery_voltage;    // V
  float    battery_current;    // A
  uint8_t  gps_satellites;     // count
  float    temperature;        // board temp (C)
};  // packed, 57 bytes

The packed struct matches the wire format for direct memcpy into MAVLink payloads. All fields are fixed-size for deterministic serialization.

4. Flight State Machine

The state machine manages 10 flight phases with guarded transitions. It is the central coordinator: every other module (FDIR, telemetry, navigation) interacts with it through events or state queries.

States

Transition Table

Complete transition logic with guard conditions. Guards are only checked on the most critical transitions.

Guard Conditions

Observer Pattern

The state machine registers callbacks that fire on every transition, allowing the telemetry logger, event logger, and FDIR module to react without direct coupling:

sm.on_transition([](State from, State to, Event event) {
  log_transition(state_name(from), state_name(to), event_name(event));
});

sm.set_fdir(&fdir);  // FDIR monitors state for recovery actions

5. FDIR Architecture

FDIR (Fault Detection, Isolation, and Recovery) is the safety backbone. The architecture uses a facade pattern: FdirManager aggregates four independent detectors behind a single interface. Each detector uses a different mechanism to cover different failure modes.

                      FdirManager (facade)
                     /        |        \        \
               Watchdog  Heartbeat  LimitChecker  KalmanDetector
               (push)    (pull)     (event)      (statistical)

Latching Fault Semantics

FdirManager uses latching semantics: the first fault detected becomes the primary_fault_ and is never overwritten. Subsequent faults are logged but do not replace the primary. This preserves the root cause for post-flight analysis.

Recovery Hierarchy

  Sensor Failover → Subsystem Isolation → EMERGENCY → SAFE_MODE

Fault Event Log

Every fault is logged with a timestamp and fault code:

struct FaultEvent {
  FaultCode code;
  uint64_t  timestamp_us;
};

// Query the fault log after a mission
for (const auto& event : fdir.fault_log()) {
  printf("[%lu] %s\n", event.timestamp_us, fault_name(event.code));
}

6. Watchdog Timer

The watchdog monitors whether the flight software is executing on schedule. It uses a push model: the software must actively "kick" the timer before each deadline. If the deadline expires without a kick, the missed counter increments. After max_missed consecutive misses, the watchdog declares a fault.

Configuration

Algorithm

bool Watchdog::check() {
  if (faulted_) return false;

  auto now = current_time_us();
  if (now - last_kick_us_ > timeout_ms * 1000) {
    missed_count_++;
    last_kick_us_ = now;
    if (missed_count_ >= max_missed_) {
      faulted_ = true;
    }
  }
  return !faulted_;
}

void Watchdog::kick() {
  missed_count_ = 0;
  faulted_ = false;
  last_kick_us_ = current_time_us();
}

7. Heartbeat Monitor

The heartbeat monitor watches for periodic signals from an external source (e.g., a companion computer, ground station, or another subsystem). Unlike the watchdog (push model), this is a pull model: the monitor expects heartbeats to arrive on schedule.

Configuration

Algorithm

The deadline is expected_period_ms + tolerance_ms. If no heartbeat arrives within this window, the missed counter increments. This two-parameter design (period + tolerance) is more robust than a single timeout: it explicitly models expected jitter rather than lumping it into a generous timeout.

bool HeartbeatMonitor::check() {
  if (faulted_) return false;

  auto now = current_time_us();
  uint32_t deadline_ms = expected_period_ms_ + tolerance_ms_;

  if (now - last_received_us_ > deadline_ms * 1000) {
    missed_count_++;
    last_received_us_ = now;
    if (missed_count_ >= max_missed_) {
      faulted_ = true;
    }
  }
  return !faulted_;
}

8. Limit Checker

The limit checker validates sensor values against configurable bounds. It uses a three-tier result system (OK, WARNING, FAULT) rather than a binary pass/fail, enabling progressive degradation.

Result Levels

API

LimitChecker checker;
checker.add_limit("voltage", 9.0f, 12.8f);  // 3S LiPo range
checker.add_limit("temperature", -20.0f, 60.0f);

auto result = checker.check("voltage", 9.2f);
switch (result) {
  case LimitResult::OK:      /* normal */ break;
  case LimitResult::WARNING: /* log warning */ break;
  case LimitResult::FAULT:   /* trigger FDIR */ break;
}

Named sensor lookup (string-keyed map) allows flexible registration without a fixed enum. Unknown sensor names return OK (graceful degradation).

9. Kalman Detector

The Kalman detector monitors the innovation (residual) of an Extended Kalman Filter (EKF) to detect estimator divergence. Under normal operation, innovations should be zero-mean white noise. A sustained bias indicates sensor failure, model mismatch, or filter divergence.

Statistical Test

The detector uses a chi-squared test on the innovation magnitude. The threshold sigma_threshold = 5.991 corresponds to the 95th percentile of the chi-squared distribution with 2 degrees of freedom (typical for 2D position estimation).

Configuration

Algorithm

void KalmanDetector::update(float innovation) {
  history_.push_back(innovation);
  if (history_.size() > window_size_) history_.pop_front();

  if (std::abs(innovation) > sigma_threshold_) {
    consecutive_exceeded_++;
    if (consecutive_exceeded_ >= fault_threshold_) {
      faulted_ = true;
    }
  } else {
    consecutive_exceeded_ = 0;
  }
}

The consecutive threshold (5 samples) prevents false alarms from single-sample spikes. A single large innovation could be noise; five in a row indicates a systematic problem.

10. 6-DOF Dynamics Kernel

From-scratch implementation of 6-degree-of-freedom rigid body dynamics with quaternion orientation representation. The kernel integrates position, orientation, velocity, and angular velocity under gravity, aerodynamic drag, and motor thrust.

State Vector

struct SixDofState {
  std::array<float, 3> position = {0,0,0};        // NED position (m)
  Quaternion orientation;                                  // body-to-NED
  std::array<float, 3> velocity = {0,0,0};        // NED velocity (m/s)
  std::array<float, 3> angular_velocity = {0,0,0}; // body-frame omega (rad/s)
  float mass = 1.2f;                                     // kg
  InertiaTensor inertia;                                  // kg*m^2
};  // 13 elements total

Inertia Tensor

struct InertiaTensor {
  float Ixx = 0.01f, Iyy = 0.01f, Izz = 0.02f;  // principal moments
  float Ixy = 0.0f,  Ixz = 0.0f,  Iyz = 0.0f;   // products of inertia

  std::array<float, 9> to_matrix() const {
    return { Ixx, -Ixy, -Ixz,
            -Ixy,  Iyy, -Iyz,
            -Ixz, -Iyz,  Izz };
  }
};

RK4 Integration

The integrator runs at 100 Hz (dt = 0.01s). Quaternion integration averages the derivatives from all four RK4 stages and renormalizes to prevent drift:

SixDofState SixDof::step_rk4(float dt, const SixDofForces& forces) {
  auto k1 = derivatives(state_, forces);
  auto k2 = derivatives(euler_step(state_, k1, dt*0.5f), forces);
  auto k3 = derivatives(euler_step(state_, k2, dt*0.5f), forces);
  auto k4 = derivatives(euler_step(state_, k3, dt), forces);

  // Position and velocity: standard RK4 combination
  state_.position = state_.position + (dt/6.0f) * (k1.pos + 2*k2.pos + 2*k3.pos + k4.pos);
  state_.velocity = state_.velocity + (dt/6.0f) * (k1.vel + 2*k2.vel + 2*k3.vel + k4.vel);

  // Quaternion: average derivative, then renormalize
  auto q_dot_avg = (k1.quat + 2*k2.quat + 2*k3.quat + k4.quat) * (1.0f/6.0f);
  state_.orientation = (state_.orientation + q_dot_avg * dt).normalized();
  return state_;
}

Physical Parameters

11. Motor Mixing

Motor mixing maps high-level commands (total thrust, roll/pitch/yaw torques) to individual motor thrusts for an X-configuration quadcopter. The mixing matrix is derived from the geometry: each motor contributes to thrust, roll, pitch, and yaw based on its position relative to the center of gravity.

Forward Mix (motor thrusts → F, tau)

  Motor layout (X-config, top view):

       M1 (front-left)     M0 (front-right)
              \               /
               \    FWD     /
                +----------+
                |    CG    |
                +----------+
               /             \
              /               \
       M2 (rear-left)      M3 (rear-right)
  

// Forward mix: motor thrusts -> [F_total, tau_roll, tau_pitch, tau_yaw]
MixOutput MotorMixing::mix(const MotorOutput& m) const {
  MixOutput out;
  out.total_thrust = m.thrust[0] + m.thrust[1] + m.thrust[2] + m.thrust[3];
  out.roll_torque  = arm_length_ * (-m.thrust[0] + m.thrust[1]
                                    + m.thrust[2] - m.thrust[3]);
  out.pitch_torque = arm_length_ * (-m.thrust[0] - m.thrust[1]
                                    + m.thrust[2] + m.thrust[3]);
  out.yaw_torque   = yaw_coeff_ * (-m.thrust[0] + m.thrust[1]
                                    - m.thrust[2] + m.thrust[3]);
  return out;
}

Inverse Mix (F, tau → motor thrusts)

The inverse mix solves for individual motor thrusts given desired total thrust and torques. With 4 motors and 4 outputs (F, roll, pitch, yaw), the system is exactly determined:

// Inverse mix: [F, tau_roll, tau_pitch, tau_yaw] -> motor thrusts
MotorOutput MotorMixing::inverse_mix(float F, float tau_r,
                                     float tau_p, float tau_y) const {
  float f_avg  = F / 4.0f;
  float f_roll  = tau_r / (4.0f * arm_length_);
  float f_pitch = tau_p / (4.0f * arm_length_);
  float f_yaw   = tau_y / (4.0f * yaw_coeff_);

  MotorOutput out;
  out.thrust[0] = f_avg - f_roll - f_pitch - f_yaw;  // front-right
  out.thrust[1] = f_avg + f_roll - f_pitch + f_yaw;  // front-left
  out.thrust[2] = f_avg + f_roll + f_pitch - f_yaw;  // rear-left
  out.thrust[3] = f_avg - f_roll + f_pitch + f_yaw;  // rear-right
  return out;
}

Constants

12. Wind Model

Atmospheric turbulence is modeled as a first-order Gauss-Markov process matching the Dryden power spectral density (MIL-STD-1797A). Each axis is independently sampled using a Mersenne Twister RNG with deterministic seeding for reproducibility.

Discrete Update

std::array<float, 3> WindModel::sample(float dt) {
  float decay = std::exp(-dt / tau_);
  float sigma_eff = sigma_ * std::sqrt(1.0f - std::exp(-2.0f*dt/tau_));

  for (int i = 0; i < 3; i++) {
    std::normal_distribution<float> dist(0.0f, 1.0f);
    wind_[i] = decay * wind_[i] + sigma_eff * dist(rng_);
  }
  wind_[1] *= 0.5f;  // attenuate vertical gusts (ground effect)
  return wind_;
}

Configuration

13. MAVLink Protocol

MAVLink v2 is the de facto standard for UAV communication. The C++ implementation uses a minimal, self-contained encoder with packed structs for zero-copy serialization. No pymavlink dependency—the encoder is ~200 lines of straightforward byte manipulation.

Wire Format

  [STX=0xFD] [LEN] [INC] [CMP] [SEQ] [SYS] [CMP] [MSG_ID:3B] [PAYLOAD:N B] [CRC8]
  ——————————————————————————————————————————————————
              10-byte header                              N bytes        1B
  

Message Types

Encoder API

MavlinkEncoder encoder(sys_id=1, cmp_id=1);

// Encode HEARTBEAT into buffer
uint8_t buf[300];
uint32_t len = encoder.encode_heartbeat(buf, sizeof(buf));

// Encode ATTITUDE
len = encoder.encode_attitude(buf, sizeof(buf),
    timestamp_ms, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed);

// Send over UDP
sendto(sock, buf, len, 0, &dest_addr, sizeof(dest_addr));

CRC-8

Per-packet integrity checking with polynomial 0x07, initial value 0xFF. The CRC covers the entire packet (header + payload). The encoder auto-increments a sequence counter for ordering verification.

14. SIL Runner

The SIL (Software-in-the-Loop) runner is the integration point that ties all modules together. It runs the full flight software stack in a standalone executable and communicates with the Python GCS via MAVLink over UDP.

Architecture

  Python GCS (port 14551) ←→ UDP ←→ SilRunner (port 14550)
      receives telemetry                    receives commands
      sends commands                        sends telemetry

Module Composition

SilRunner owns all subsystems via composition:

class SilRunner {
  StateMachine    sm_;        // flight phase management
  FdirManager     fdir_;      // fault detection
  SixDof          dynamics_;  // 6-DOF rigid body
  MotorMixing     mixing_;    // motor mixing
  WindModel       wind_;      // Dryden turbulence
  MavlinkEncoder  encoder_;   // MAVLink encoder
  int             sock_;      // UDP socket
};

Main Loop

The SIL runner uses rate-separated execution: dynamics at 100 Hz, telemetry at 50 Hz, heartbeat at 1 Hz, console output at 0.5 Hz.

// main_sil.cpp main loop
while (g_running) {
  runner.step();                          // 100 Hz dynamics + FDIR

  if (now - last_telem >= 20ms) {       // 50 Hz telemetry
    runner.send_telemetry();              // 4 MAVLink packets
    last_telem = now;
  }

  if (now - last_hb >= 1000ms) {        // 1 Hz heartbeat
    fdir.heartbeat_received();
    last_hb = now;
  }

  fdir.update(timestamp);                // check watchdog + heartbeat
  runner.receive_command();               // non-blocking UDP poll

  if (now - last_print >= 2000ms) {     // 0.5 Hz console
    print_state(sm.current_state(), dynamics_.get_state());
    last_print = now;
  }

  sleep_for(dt);                         // 10ms tick
}

Step Function

Each step: sample wind, compute thrust, integrate dynamics, update state machine, kick watchdog, run FDIR:

bool SilRunner::step() {
  auto wind = wind_.sample(config_.dt);

  // Hardcoded hover thrust + slight climb
  std::array<float,3> thrust = {0, 0, -1.2f * 9.81f};
  std::array<float,3> torque = {0, 0, 0};

  dynamics_.step_rk4(config_.dt, thrust, torque, wind);

  // Process state machine events
  sm_.process_event(Event::ARM_CMD);
  sm_.process_event(Event::TAKEOFF_CMD);

  // FDIR checks
  fdir_.watchdog_kick();
  fdir_.heartbeat_received();
  fdir_.update(timestamp_us());

  step_count_++;
  return true;
}

15. Testing Strategy

Unit Tests (GTest)

85 unit tests across 8 test suites, covering every module in isolation:

SIL Tests (pytest)

10 automated test scenarios driving the C++ flight software via MAVLink over UDP:

Validation Criteria

• 6-DOF dynamics validated against analytical solutions (free-fall, hover equilibrium)
• FDIR detecting injected faults within 100 ms (3x heartbeat period)
• Quaternion norm stays within 1.0 +/- 1e-6 after 5000 RK4 steps
• Motor mix round-trip error < 0.01 N (inverse → forward → inverse)

Running Tests

# C++ unit tests
./build/tests/SwarmGNC_FW_Tests

# Python SIL tests
uv run pytest tests/sil/ -v

# All tests
uv run pytest tests/sil/ -v && ./build/tests/SwarmGNC_FW_Tests

16. Build & Run

# Build C++ flight software
cd fw && cmake -B build -DCMAKE_BUILD_TYPE=Debug
cmake --build build -j$(sysctl -n hw.ncpu)

# Run all tests
./build/tests/SwarmGNC_FW_Tests

# Run SIL (start GCS first)
./build/src/SwarmGNC_FW_SIL

# Python GCS (separate terminal)
uv run python ground/gcs.py

# Demo mission (no GCS needed)
./build/src/SwarmGNC_FW_Demo

# Hover validation
./build/src/SwarmGNC_6Dof_Demo

Expected Console Output (SIL)

[ARMED]    pos=(0.00, 0.00, 0.00) vel=(0.00, 0.00, 0.00) rpy=(0.0, 0.0, 0.0)
[TAKEOFF]  pos=(0.01, 0.00,-0.12) vel=(0.01, 0.00,-1.18) rpy=(0.1, 0.0, 0.0)
[CRUISE]   pos=(0.04, 0.01,-1.22) vel=(0.02, 0.01,-1.18) rpy=(0.2, 0.1, 0.0)
[CRUISE]   pos=(0.08, 0.02,-2.44) vel=(0.03, 0.01,-1.18) rpy=(0.3, 0.1, 0.0)
...

17. Module Reference

Quick reference for all 14 flight software modules.

Module Interaction Diagram

                    SilRunner (integration facade)
                   /    |     \      \       \
            StateMachine |   SixDof   WindModel  MavlinkEncoder
                 |       |      |         |          |
            FdirManager  |  AerodynamicModel    UDP Socket
                 |       |      |              ←→ Python GCS
            [Watchdog]   |  MotorMixing
            [Heartbeat]  |
            [LimitChecker] |
            [KalmanDetector] |

18. Other Components

The C++ flight software is the core deliverable. Two additional components complete the system:

Python Simulation Engine

A decentralized multi-agent swarm simulation (7 drones, LQR + APF + graph Laplacian consensus, Dryden wind). The simulation demonstrates guidance and control theory; the C++ FSW demonstrates how to implement those concepts in production-grade software.

src/core/
  models.py      # QuadcopterAgent (6-state, RK4, 50 Hz)
  dynamics.py    # LQR gain synthesis (CARE solver)
  consensus.py   # Graph Laplacian, APF, 3D DCM rotation
  wind.py        # DrydenGustModel (Gauss-Markov)
  obstacles.py   # SphereObstacle, Environment

Ground Control Station

GPU-accelerated desktop GCS built with Dear PyGui and ImPlot (120 FPS). NASA MOCR-inspired panel layout with SpaceX-inspired dark theme. Connects to the C++ FSW via MAVLink over UDP or runs a standalone simulator for demo.

• Primary Flight Display: Canvas-drawn attitude indicator with sky/ground, pitch ladder, roll pointer
• GPS Map: Leaflet moving map with waypoint route and drone trail
• Fleet Status: 7-drone wedge formation with color-coded state badges
• State Machine Viz: Animated node diagram showing current state
• Command Panel: Arm/disarm, takeoff, RTL, land buttons

See Theory & Methodology for the mathematical foundations of the Python simulation.