The LiteWing Drone Flight Positioning Module enables autonomous position hold capabilities using optical flow sensors combined with dead reckoning algorithms. This guide explores how to implement a fully autonomous position hold system where the drone takes off, hovers at a target height, and actively maintains its position in space without manual intervention. This creates a hands-free flying experience where the drone stabilizes itself against environmental disturbances and holds its position with minimal drift. This tutorial is demonstrated using the Litewing ESP32 Drone and Litewing Drone Positioning Module.
System Overview
The optical position hold system represents the foundation of autonomous flight control. Unlike manual control where you constantly adjust the drone's position, or joystick-assisted control where you command movements, this system operates completely autonomously. Once you initiate the flight sequence, the drone takes off to the target height and immediately begins working to maintain that exact position. The system continuously measures velocity using the optical flow sensor, integrates these measurements to estimate position through dead reckoning, and applies PID control corrections to counteract any detected drift or motion. This approach is ideal for applications requiring stable hover, aerial photography, or as a learning platform for understanding autonomous flight control algorithms.
Hardware Setup
Before working with the optical position hold system, ensure your LiteWing drone is properly equipped with the Flight Positioning Module and running compatible firmware. If you have not yet installed the Module or updated the firmware, refer to the Flight Positioning Module User Guide. That document provides complete hardware installation instructions and firmware flashing procedures.
Software Setup
Setting up your development environment is critical for working with the position hold system. You need Python installed with the cflib library properly configured, and you must be able to establish CRTP communication with the LiteWing drone over UDP. The cflib library has specific prerequisites that must be met. We recommend you follow the LiteWing Python SDK Programming Guide for detailed setup instructions. That guide covers installing Python dependencies, setting up cflib, connecting to the LiteWing Wi-Fi interface, and validating your communication setup with test scripts. Completing that setup is a prerequisite for this guide.
How Dead Reckoning Position Hold Works
The position hold system relies on dead reckoning, a technique that estimates position by integrating velocity measurements over time rather than receiving absolute position information from an external source like GPS. Dead reckoning builds up a relative position estimate starting from an arbitrary origin point that we define at a particular moment in the flight.
The core principle is straightforward. If you know how fast you're moving and in which direction, you can estimate how far you've traveled by multiplying velocity by time. The optical flow sensor provides velocity measurements in the X and Y directions (relative to the drone's body frame). By integrating these velocities at each sensor update, we estimate how far the drone has moved from its starting position
.The mathematical expression for this integration is:
In these equations, Δt is the time interval between measurements, vx and vy represent velocities in meters per second To compute these values accurately, a height reference relative to the ground is required. This is obtained by combining data from an optical flow sensor (PMW3901MB) and a Time-of-Flight (ToF) sensor (VL53L1X). The detailed calculations for this process are described in our Optical Flow Tracking Guide. During the position-hold phase, this integration runs continuously, gradually building an estimate of the system’s position relative to the point at which integration started.
Once we have this position estimate, implementing position hold becomes conceptually simple. The system captures the current estimated position and treats it as the target position. From that point on, calculations run at approximately 50 Hz. First, we compute the position error, which is the difference between the target position and the current estimated position. Second, we compute the velocity error, which is how fast the drone is currently moving (we want zero velocity when holding position). These two error signals feed into PID controllers that compute velocity correction commands. The flight controller receives these corrections and adjusts thrust and tilt to counteract the detected errors.
The control law combines both position error and velocity error. Position error indicates how far we've drifted from the target:
Velocity error indicates how fast we're moving when we should be stationary:
The correction command combines contributions from both error signals:
This dual-loop approach, called cascaded control, provides both position restoration and velocity damping. The position controller generates a restoring force that pulls the drone back toward the target position. The velocity controller provides damping that prevents oscillation and overshoot. Without velocity damping, the position controller might cause the drone to overshoot repeatedly, creating a bouncing behavior. The velocity term smooths this response.
One fundamental challenge of all dead reckoning systems is drift accumulation. Small inaccuracies in velocity measurements, when integrated over time, result in position errors that grow continuously. A velocity measurement error of just 1 cm/s causes a position error of 60 cm after one minute. To mitigate this without relying on absolute position references, we implement complementary mechanisms.
| Mechanism | Description | Purpose / Effect | Key Parameters |
| Velocity smoothing and thresholding | Raw optical flow velocity is filtered using an exponential moving average to reduce sample-to-sample noise. Very small velocity values are clamped to zero. | Suppresses random noise while remaining responsive to real motion; prevents integrating noise when the drone is stationary. | EMA smoothing factor; velocity threshold ≈ 0.005 m/s |
| Integral decay | When velocity magnitude is near zero, a small decay term pulls the estimated position back toward the origin, acting like a weak spring. | Gradually corrects drift during stationary periods without affecting motion tracking. | Decay rate (tunable); applied only under low-velocity conditions |
| Periodic position reset | The integrated position estimate is reset to zero at fixed time intervals. The drone does not physically move; the origin is redefined. | Prevents unbounded drift accumulation over long durations. | Reset interval: 90 seconds |
PID Control System
The position hold implementation uses cascaded PID controllers for both position and velocity. Although the default configuration uses proportional-only control (P-control), the full PID structure is available for tuning to optimize performance in your specific environment.
Understanding how these controllers work helps with effective tuning. A PID controller computes its output as the sum of three terms: proportional, integral, and derivative. The proportional term responds to current error magnitude. The integral term accumulates error over time and responds to persistent offsets. The derivative term responds to the rate of change of error and provides predictive damping.
Our position PID controller attempts to drive position error to zero. Its output is:
The default gains for the position controller are:
| Gain | Default Value | Effect |
| POSITION_KP | 1.1 | Main restoring force toward target position |
| POSITION_KI | 0.03 | Eliminates steady-state offset |
| POSITION_KD | 0.0 | Provides additional damping (disabled by default) |
The proportional term provides the main restoring force. Higher proportional gain creates stronger position correction, pulling the drone back toward the target more aggressively. However, excessive gain causes instability and oscillation. Insufficient gain allows drift without adequate correction. A good starting range is 1.0 to 1.5, with the default of 1.2 typically working well.
Our velocity PID controller provides active damping by opposing residual velocity. Its output is:
The default gains for the velocity controller are:
| Gain | Default Value | Effect |
| VELOCITY_KP | 1.2 | Active damping to prevent overshoot |
| VELOCITY_KI | 0.01 | Typically not needed |
| VELOCITY_KD | 0.0 | Typically not needed |
The velocity controller acts as active damping. When the position controller commands a correction, it creates motion. The velocity controller opposes that motion, preventing overshoot and oscillation. This is why we can use relatively high position gains without instability, the velocity loop provides necessary damping.
The final velocity command sent to the drone combines both PID outputs:
total_correction_vx = u_pos_x + u_vel_x
total_correction_vy = u_pos_y + u_vel_yThese corrections are clamped to safe limits to prevent aggressive maneuvers that could destabilize the drone. The default limit is ±0.1 m/s, providing smooth, controlled corrections without sudden motions.
CRTP Communication for Position Hold
The optical position hold script communicates with the drone using CRTP log variables that stream sensor data at high frequency. We request three primary variables at 100 Hz (every 10 milliseconds) to provide sufficient update frequency for the control loop.
| Variable | Type | Purpose |
| motion.deltaX | int16_t | Pixel shift in X direction from optical flow sensor |
| motion.deltaY | int16_t | Pixel shift in Y direction from optical flow sensor |
| stateEstimate.z | float | Fused altitude estimate in meters |
The motion deltas come directly from the PMW3901MB optical flow sensor and represent how many pixels the ground pattern has moved between consecutive sensor frames. The height value comes from the drone's state estimator, which fuses data from the VL53L1X Time-of-Flight sensor with IMU measurements to provide stable height readings.
To convert raw pixel deltas into velocities expressed in meters per second, we apply a geometry-based scaling calculation that accounts for the sensor's field of view, optical resolution, and sample rate. The conversion follows this formula:
velocity_constant = (4.4 * DEG_TO_RAD) / (30.0 * DT)
velocity = delta_value * altitude * velocity_constantThe constant 4.4 represents the effective field of view in degrees. The value 30 corresponds to the optical resolution (how many pixels span that field of view). DT is the sample period in seconds, typically 0.01 for our 10ms logging interval. This scaling produces velocities in meters per second for position integration and control calculations.
Code Walkthrough
The complete script implementation is available on the LiteWing GitHub repository under the Python-Scripts/Flight_Positioning_Shield/ directory. You can access it at:
GitHub Link
The file we're examining is named dead-reckoning-optical-position-hold.py. In the following sections, we'll walk through the key components to understand how everything fits together.
Configuration Parameters
The script begins with configuration parameters controlling the drone's connection, flight behavior, control timing, and PID tuning. These parameters are defined as constants at the top of the file, making them easy to locate and modify.
# Connection Configuration
DRONE_URI = "udp://192.168.43.42"
TARGET_HEIGHT = 0.3 # Target hover height in meters
TAKEOFF_TIME = 0.5 # Time allocated for takeoff sequence
HOVER_DURATION = 60.0 # How long to hover with position hold
LANDING_TIME = 0.5 # Time allocated for landing sequence
DEBUG_MODE = False # Set to True to disable motors for testing
# Control Loop Timing
VELOCITY_SMOOTHING_ALPHA = 0.9 # Filtering strength for velocity
VELOCITY_THRESHOLD = 0.005 # Consider drone "stationary" below this
CONTROL_UPDATE_RATE = 0.02 # 50Hz control loop (20ms period)
SENSOR_PERIOD_MS = 10 # 100Hz sensor data streaming
DT = SENSOR_PERIOD_MS / 1000.0 # Convert to seconds
# Trim Corrections (manual bias adjustments)
TRIM_VX = 0.0
TRIM_VY = 0.0
# Position PID Controller Gains
POSITION_KP = 1.2
POSITION_KI = 0.0
POSITION_KD = 0.0
# Velocity PID Controller Gains
VELOCITY_KP = 1.2
VELOCITY_KI = 0.0
VELOCITY_KD = 0.0
# Control Limits and Safety
MAX_CORRECTION = 0.1 # Maximum velocity correction (m/s)
DRIFT_COMPENSATION_RATE = 0.004 # Position decay rate when stationary
MAX_POSITION_ERROR = 2.0 # Position estimate clamp limit
PERIODIC_RESET_INTERVAL = 90.0 # Reset integrated position periodicallySeveral parameters deserve special attention.
| Parameter | Purpose | Notes |
| CONTROL_UPDATE_RATE | Sets how often position-hold calculations run | At 50 Hz (20 ms), provides responsive corrections without overloading the drone’s command interface |
| TARGET_HEIGHT | Defines the desired hover height after takeoff | Used as the steady-state height for position hold |
| TRIM_VX | Compensates for drift along the X axis | |
| TRIM_VY | Compensates for drift along the Y axis | Apply a small bias if the drone drifts even when commanded to hold position |
| PID Gains | Control aggressiveness of the position-hold controller | Default values suit typical indoor environments with reliable optical flow |
| MAX_CORRECTION | Limits the maximum velocity command | Acts as a safety clamp to prevent overly aggressive maneuvers during large position errors |
| PERIODIC_RESET_INTERVAL | Resets the position estimate periodically | Prevents unbounded drift accumulation over time |
Global State Variables
The script maintains several categories of state variables tracking the complete control system status from raw sensor readings through computed position to PID controller internal state. Understanding what these variables represent helps you follow the control flow.
# Raw sensor data from the drone
current_height = 0.0
motion_delta_x = 0
motion_delta_y = 0
sensor_data_ready = False
current_battery_voltage = 0.0
battery_data_ready = False
# Computed velocities (m/s)
current_vx = 0.
current_vy = 0.0
velocity_x_history = [0.0, 0.0] # For smoothing filter
velocity_y_history = [0.0, 0.0]
# Integrated position estimate (dead reckoning)
integrated_position_x = 0.0
integrated_position_y = 0.0
last_integration_time = time.time()
last_reset_time = time.time()
position_integration_enabled = False
# PID controller internal state
position_integral_x = 0.0
position_integral_y = 0.0
last_position_error_x = 0.0
last_position_error_y = 0.0
velocity_integral_x = 0.0
velocity_integral_y = 0.0
last_velocity_error_x = 0.0
last_velocity_error_y = 0.0
# Target position for hold mode
target_position_x = 0.0
target_position_y = 0.0
# Control outputs sent to drone
current_correction_vx = 0.0
current_correction_vy = 0.0
# Flight state
flight_phase = "IDLE"
flight_active = FalseThe sensor data variables hold the most recent values received from the drone. Velocity variables contain both raw and smoothed velocity estimates computed from optical flow deltas. Integrated position variables represent our dead reckoning estimate of where the drone is relative to the origin. PID state variables store integral accumulation and previous error values needed for I and D terms. Target position variables define where we want to hold. Finally, flight state variables track the current phase of operation.
Velocity Calculation and Smoothing
Raw optical flow deltas must be converted to velocities and smoothed before use. Two functions handle this processing.
def calculate_velocity(delta_value, altitude):
"""Convert optical flow delta to linear velocity"""
if altitude <= 0:
return 0.0
if USE_HEIGHT_SCALING:
velocity_constant = (4.4 * DEG_TO_RAD) / (30.0 * DT)
velocity = delta_value * altitude * velocity_constant
else:
velocity = delta_value * OPTICAL_FLOW_SCALE * DT
return velocityThe calculate_velocity function takes a raw pixel delta from the optical flow sensor and current altitude, then returns the corresponding linear velocity in meters per second. The calculation accounts for the sensor's 4.4-degree field of view, 30-pixel resolution, and logging sample period. If altitude is zero or negative, the function returns zero velocity because we cannot reliably compute velocity without valid altitude information.
def smooth_velocity(new_velocity, history):
"""Simple 2-point smoothing filter with adjustable alpha"""
history[1] = history[0]
history[0] = new_velocity
alpha = VELOCITY_SMOOTHING_ALPHA
smoothed = (history[0] * alpha) + (history[1] * (1 - alpha))
if abs(smoothed) < VELOCITY_THRESHOLD:
smoothed = 0.0
return smoothedThe smooth_velocity function implements a simple exponential moving average filter. We maintain a two-point history for each axis, and the smoothed output is a weighted combination of current and previous values. The alpha parameter (typically 0.9) controls the balance between new and old data. Higher alpha produces smoother output with slightly more lag. After smoothing, we apply a threshold test: velocities smaller than 0.005 m/s are clamped to exactly zero. This prevents integrating noise when the drone is actually stationary.
Position Integration
Velocities are integrated over time to estimate position through dead reckoning. The integrate_position function handles this calculation along with drift compensation and bounds checking.
def integrate_position(vx, vy, dt):
"""Dead reckoning: integrate velocity to position"""
global integrated_position_x, integrated_position_y
# Sanity check on time step
if dt <= 0 or dt > 0.1:
return
# Simple integration
integrated_position_x += vx * dt
integrated_position_y += vy * dt
# Apply drift compensation when moving slowly
velocity_magnitude = (vx * vx + vy * vy) ** 0.5
if velocity_magnitude < VELOCITY_THRESHOLD * 2:
integrated_position_x -= integrated_position_x * DRIFT_COMPENSATION_RATE * dt
integrated_position_y -= integrated_position_y * DRIFT_COMPENSATION_RATE * dt
# Clamp position error
integrated_position_x = max(-MAX_POSITION_ERROR,
min(MAX_POSITION_ERROR, integrated_position_x))
integrated_position_y = max(-MAX_POSITION_ERROR,
min(MAX_POSITION_ERROR, integrated_position_y))The function begins by validating the time step. If dt is zero, negative, or unreasonably large (which could happen if logging temporarily paused), we skip integration to avoid creating a large jump in the position estimate. The core integration is straightforward: we add the product of velocity and time to the current position.
The drift compensation mechanism activates when velocity magnitude is very small, indicating the drone is nearly stationary. In this condition, we apply a small decay that gradually pulls the position estimate back toward zero. The decay rate is proportional to current position error, so larger errors decay faster. This acts like a weak centering spring that helps counteract long-term drift without interfering with tracking real motion.
Finally, we clamp the position estimate to reasonable bounds. If sensor errors or other problems cause integration to run away, the clamp prevents absurdly large position values. The default limit of ±2 meters is sufficient for typical indoor flight spaces.
Periodic Position Reset
To prevent unbounded drift accumulation in the dead reckoning system, we implement periodic position resets that completely zero the integrated position estimate at regular intervals.
def periodic_position_reset():
"""Reset integrated position periodically to prevent drift accumulation"""
global integrated_position_x, integrated_position_y, last_reset_time
current_time = time.time()
if current_time - last_reset_time >= PERIODIC_RESET_INTERVAL:
integrated_position_x = 0.0
integrated_position_y = 0.0
last_reset_time = current_time
return True
return FalseThis function checks if the configured reset interval has elapsed since the last reset. By default, this interval is 90 seconds. When the interval expires, we reset both position estimates to zero and update the last reset timestamp. The function returns True when a reset occurs, allowing the calling code to log this event. The reset is transparent to the physical drone, it doesn't move, we simply redefine the current location as the new origin and continue position hold from there.
Position Hold Reset
When starting position hold or transitioning between flight phases, we need to reset the control system state to ensure clean initialization.
def reset_position_tracking():
"""Reset integrated position tracking"""
global integrated_position_x, integrated_position_y
global last_integration_time, last_reset_time
global position_integration_enabled
global position_integral_x, position_integral_y
global velocity_integral_x, velocity_integral_y
global last_position_error_x, last_position_error_y
global last_velocity_error_x, last_velocity_error_y
global target_position_x, target_position_y
integrated_position_x = 0.0
integrated_position_y = 0.0
target_position_x = 0.0
target_position_y = 0.0
last_integration_time = time.time()
last_reset_time = time.time()
position_integration_enabled = True
# Reset PID state
position_integral_x = 0.0
position_integral_y = 0.0
velocity_integral_x = 0.0
velocity_integral_y = 0.0
last_position_error_x = 0.0
last_position_error_y = 0.0
last_velocity_error_x = 0.0
last_velocity_error_y = 0.0This function performs a complete state reset. We set both current position estimate and target position to zero, effectively defining the origin at the drone's current location. All PID controller internal state gets cleared, including integral accumulators and stored previous errors. If we didn't clear these, the controller would "remember" errors from previous flights and incorrectly apply them to the new position hold attempt. We also enable position integration so the system begins tracking position immediately.
PID Control Calculation
The calculate_position_hold_corrections function implements the core position hold logic by combining position and velocity PID controllers to compute correction commands.
def calculate_position_hold_corrections():
"""Calculate control corrections using PID controllers"""
global current_correction_vx, current_correction_vy
global position_integral_x, position_integral_y
global last_position_error_x, last_position_error_y
global velocity_integral_x, velocity_integral_y
global last_velocity_error_x, last_velocity_error_y
if not sensor_data_ready or current_height <= 0:
current_correction_vx = 0.0
current_correction_vy = 0.0
return 0.0, 0.0
# Calculate position errors (negative because we want to correct toward target)
position_error_x = -(integrated_position_x - target_position_x)
position_error_y = -(integrated_position_y - target_position_y)
# Calculate velocity errors (negative because we want to dampen velocity)
velocity_error_x = -current_vx
velocity_error_y = -current_vy
# Position PID Controller
# Proportional
position_p_x = position_error_x * POSITION_KP
position_p_y = position_error_y * POSITION_KP
# Integral (with anti-windup)
position_integral_x += position_error_x * CONTROL_UPDATE_RATE
position_integral_y += position_error_y * CONTROL_UPDATE_RATE
position_integral_x = max(-0.1, min(0.1, position_integral_x))
position_integral_y = max(-0.1, min(0.1, position_integral_y))
position_i_x = position_integral_x * POSITION_KI
position_i_y = position_integral_y * POSITION_KI
# Derivative
position_derivative_x = (position_error_x - last_position_error_x) / CONTROL_UPDATE_RATE
position_derivative_y = (position_error_y - last_position_error_y) / CONTROL_UPDATE_RATE
position_d_x = position_derivative_x * POSITION_KD
position_d_y = position_derivative_y * POSITION_KD
last_position_error_x = position_error_x
last_position_error_y = position_error_y
# Velocity PID Controller
# Proportional
velocity_p_x = velocity_error_x * VELOCITY_KP
velocity_p_y = velocity_error_y * VELOCITY_KP
# Integral (with anti-windup)
velocity_integral_x += velocity_error_x * CONTROL_UPDATE_RATE
velocity_integral_y += velocity_error_y * CONTROL_UPDATE_RATE
velocity_integral_x = max(-0.05, min(0.05, velocity_integral_x))
velocity_integral_y = max(-0.05, min(0.05, velocity_integral_y))
velocity_i_x = velocity_integral_x * VELOCITY_KI
velocity_i_y = velocity_integral_y * VELOCITY_KI
# Derivative
velocity_derivative_x = (velocity_error_x - last_velocity_error_x) / CONTROL_UPDATE_RATE
velocity_derivative_y = (velocity_error_y - last_velocity_error_y) / CONTROL_UPDATE_RATE
velocity_d_x = velocity_derivative_x * VELOCITY_KD
velocity_d_y = velocity_derivative_y * VELOCITY_KD
last_velocity_error_x = velocity_error_x
last_velocity_error_y = velocity_error_y
# Combine PID outputs
position_correction_vx = position_p_x + position_i_x + position_d_x
position_correction_vy = position_p_y + position_i_y + position_d_y
velocity_correction_vx = velocity_p_x + velocity_i_x + velocity_d_x
velocity_correction_vy = velocity_p_y + velocity_i_y + velocity_d_y
# Total corrections
total_vx = position_correction_vx + velocity_correction_vx
total_vy = position_correction_vy + velocity_correction_vy
# Apply limits
total_vx = max(-MAX_CORRECTION, min(MAX_CORRECTION, total_vx))
total_vy = max(-MAX_CORRECTION, min(MAX_CORRECTION, total_vy))
# Store for GUI display
current_correction_vx = total_vx
current_correction_vy = total_vy
return total_vx, total_vy
This function implements the full cascaded PID architecture. We compute position errors by comparing where we are to where we want to be, and velocity errors by comparing current velocity to the desired velocity of zero. For each axis, we calculate the P, I, and D terms of both position and velocity controllers. Integral terms are accumulated over time and clamped to prevent windup. Derivative terms are computed as the rate of change of error. Finally, we sum all contributions from both controllers and apply a safety clamp to ensure commanded corrections stay within reasonable bounds.
Motion Sensor Callback
The motion_callback function receives optical flow data from the drone and processes it into velocity and position estimates.
def motion_callback(timestamp, data, logconf):
"""Motion sensor data callback"""
global current_height, motion_delta_x, motion_delta_y, sensor_data_ready
global current_vx, current_vy, last_integration_time
# Get sensor data
current_height = data.get("stateEstimate.z", 0)
motion_delta_x = data.get("motion.deltaX", 0)
motion_delta_y = data.get("motion.deltaY", 0)
sensor_data_ready = True
# Calculate velocities
raw_velocity_x = calculate_velocity(motion_delta_x, current_height)
raw_velocity_y = calculate_velocity(motion_delta_y, current_height)
# Apply smoothing
current_vx = smooth_velocity(raw_velocity_x, velocity_x_history)
current_vy = smooth_velocity(raw_velocity_y, velocity_y_history)
# Dead reckoning position integration
current_time = time.time()
dt = current_time - last_integration_time
if 0.001 <= dt <= 0.1 and position_integration_enabled:
integrate_position(current_vx, current_vy, dt)
last_integration_time = current_time
# Update history for GUI
update_history()This callback runs every time we receive new sensor data from the drone, which happens at 100 Hz. We extract altitude and optical flow deltas, convert deltas to velocities, apply smoothing, and integrate position. The time step validation ensures we only integrate when dt is reasonable, neither too small (which could indicate a duplicate packet) nor too large (which could indicate a communication gap). Position integration only occurs when position_integration_enabled is True, allowing us to disable integration during certain flight phases like takeoff and landing.
Flight Control Loop
The main flight control loop executes the complete position hold sequence from takeoff through landing.
def flight_thread_func(self):
"""Position hold flight thread"""
global flight_active, flight_phase
global position_integration_enabled
# Initialize
cflib.crtp.init_drivers()
cf = Crazyflie(rw_cache="./cache")
try:
with SyncCrazyflie(DRONE_URI, cf=cf) as scf:
flight_active = True
# Setup logging
log_motion, log_battery = setup_logging(cf, logger=self.log_to_output)
time.sleep(1.0)
# Reset position tracking
reset_position_tracking()
# Enable high-level commander
if not DEBUG_MODE:
cf.commander.send_setpoint(0, 0, 0, 0)
time.sleep(0.1)
cf.param.set_value("commander.enHighLevel", "1")
time.sleep(0.5)
# Takeoff
flight_phase = "TAKEOFF"
init_csv_logging(logger=self.log_to_output)
start_time_local = time.time()
while time.time() - start_time_local < TAKEOFF_TIME and flight_active:
if not DEBUG_MODE:
cf.commander.send_hover_setpoint(TRIM_VX, TRIM_VY, 0, TARGET_HEIGHT)
log_to_csv()
time.sleep(0.01)
# Stabilization
flight_phase = "STABILIZING"
stabilization_start = time.time()
while time.time() - stabilization_start < 3.0 and flight_active:
motion_vx, motion_vy = calculate_position_hold_corrections()
log_to_csv()
total_vx = TRIM_VX + motion_vy
total_vy = TRIM_VY + motion_vx
if not DEBUG_MODE:
cf.commander.send_hover_setpoint(total_vx, total_vy, 0, TARGET_HEIGHT)
time.sleep(CONTROL_UPDATE_RATE)
# Position Hold
flight_phase = "POSITION_HOLD"
hover_start = time.time()
self.log_to_output(f"Position Hold active for {HOVER_DURATION:.0f}s")
while time.time() - hover_start < HOVER_DURATION and flight_active:
motion_vx, motion_vy = calculate_position_hold_corrections()
if periodic_position_reset():
flight_phase = "POSITION_HOLD (RESET)"
self.log_to_output("Position reset to origin")
else:
flight_phase = "POSITION_HOLD"
log_to_csv()
total_vx = TRIM_VX + motion_vy
total_vy = TRIM_VY + motion_vx
if not DEBUG_MODE:
cf.commander.send_hover_setpoint(total_vx, total_vy, 0, TARGET_HEIGHT)
time.sleep(CONTROL_UPDATE_RATE)
# Landing
flight_phase = "LANDING"
landing_start = time.time()
while time.time() - landing_start < LANDING_TIME and flight_active:
if not DEBUG_MODE:
cf.commander.send_hover_setpoint(TRIM_VX, TRIM_VY, 0, 0)
log_to_csv()
time.sleep(0.01)
if not DEBUG_MODE:
cf.commander.send_setpoint(0, 0, 0, 0)
flight_phase = "COMPLETE"
except Exception as e:
flight_phase = "ERROR"
self.log_to_output(f"Error: {str(e)}")
finally:
close_csv_logging(logger=self.log_to_output)
flight_active = False
This function executes the complete flight sequence. It starts by initializing CRTP drivers and establishing a connection to the drone. After setting up sensor logging and enabling the high-level commander, it executes four distinct flight phases. During takeoff, the drone ascends to the target height using simple hover commands without position hold corrections. During stabilization, position hold calculations begin but the drone is still settling from the ascent. During the main position hold phase, the full control loop runs continuously, calculating corrections at 50 Hz and periodically resetting the position estimate. During landing, position hold is disabled and the drone descends using simple hover commands. Finally, motors are stopped and the flight completes.
GUI and Visualization
The script includes a comprehensive Tkinter-based graphical user interface that provides both control and monitoring capabilities. The interface is divided into several functional areas for parameter adjustment, system state observation, and flight path visualization.
The parameter adjustment panel allows modifying critical system parameters without editing code and restarting. You can change the target height, which determines how high the drone hovers after takeoff. The hover duration controls how long the position hold phase lasts. TRIM VX and TRIM VY parameters let you add small bias corrections if the drone consistently drifts in a particular direction. The PID gain controls allow adjusting both position and velocity controller gains. The "Apply All Values" button commits your parameter changes to the running system.
The real-time status display section shows current values of important system variables. You can see current height as measured by sensors, battery voltage (helps you know when to land), estimated velocities in X and Y, estimated position from dead reckoning, computed correction values being sent to the drone, and current flight phase. The flight phase indicator tells you whether the system is idle, in sensor test, taking off, stabilizing, holding position, or landing.
The trajectory visualization uses four matplotlib plots embedded in the Tkinter window. The first plot shows estimated velocities in X and Y over time, giving you a sense of how the drone is moving.
The second plot displays the integrated 2D position estimate, this is your dead reckoning trajectory with current position marked by a red dot and the target position (origin) marked in yellow.
The third plot shows control corrections being applied by the PID controller, helping you understand how hard the system is working to maintain position.
The fourth plot tracks height over time with a reference line indicating your target height.
The sensor test functionality provides a way to verify all sensors are working correctly before flight. When you click "Sensor Test", the system connects to the drone, sets up sensor logging, and begins streaming data without starting the motors.
This allows you to confirm the optical flow sensor is detecting motion (move the drone manually and watch velocity values change), the ToF sensor provides valid readings, and battery voltage is above the safe threshold. Once sensors are verified, you can proceed with the flight.
Tuning PID Parameters
The default P-only control configuration works well for basic position hold in most environments, but you may want to tune PID gains to optimize performance for your specific conditions. Understanding what each gain does and how to adjust it helps you achieve better position hold quality.
| Gain | What It Does | If Too Low | If Too High | Typical / Default Values |
| POSITION_KP | Determines how aggressively the drone moves back toward the target position | Drone drifts away with weak correction | Oscillation and instability around the target | Start 1.0–1.5, default 1.2 |
| POSITION_KI | Accumulates error over time to remove steady-state offset | Persistent small offset from target | Integral windup, instability | Start 0.0, typical 0.01–0.05 |
| POSITION_KD | Responds to rate of position error change (predictive damping) | Usually unnecessary in cascaded control | Can add noise and instability | Typically 0.0 |
| VELOCITY_KP | Provides damping; resists motion to prevent overshoot | Overshoot, oscillation | Sluggish, overdamped response | Default 1.2 |
| VELOCITY_KI | Corrects steady-state velocity error | Rarely needed | Can destabilize system | 0.0 |
| VELOCITY_KD | Reacts to velocity error rate | Not required in most setups | Noise amplification | 0.0 |
Recommended PID Tuning Procedure:
| Step | Action | What to Observe | Adjustment |
| 1 | Start with P-only control | Stable hover baseline | POSITION_KP = 1.2, VELOCITY_KP = 1.2, all others 0.0 |
| 2 | Check for oscillation | Drone bounces back and forth | Decrease POSITION_KP in 0.1 steps |
| 3 | Check for drift | Drone slowly wanders away | Increase POSITION_KP in 0.1 steps |
| 4 | Look for steady-state offset | Drone sits consistently off-target | Add POSITION_KI (0.01-0.05) |
| 5 | Evaluate overshoot | Drone passes target before settling | Increase VELOCITY_KP |
| 6 | Validate changes | Stable hover without oscillation or drift | Test thoroughly before further tuning |
Testing the System
Testing the optical position hold system should follow a structured progression, starting with sensor validation, moving to basic flight testing, and finally evaluating position hold quality. This staged approach helps isolate problems early and avoids unnecessary risk during flight.
Sensor Validation
Begin by launching the script and clicking the Sensor Test (ARM) button. This connects to the drone and starts streaming sensor data without spinning the motors. The GUI will report connection status and sensor availability while displaying live values that should respond to manual movement.
| Parameter | Expected Behavior (On Ground) |
| Height | Close to 0 m |
| Battery Voltage | Above LOW_BATTERY_THRESHOLD (≈ 2.9 V) |
| Velocity (X/Y) | Changes when sliding the drone on a textured surface |
| Sensor Status | All sensors report valid data |
If the optical flow sensor is working correctly, velocity values will change when the drone is moved across a patterned surface. Once all readings look reasonable, click Stop Sensor Test to prepare for flight.
Basic Flight Testing
Before flight, adjust the target height if needed. For initial tests, keep this conservative, around 0.2 to 0.3 meters, until you’re confident in the system’s behavior. Ensure the flight area is clear, with at least 2 meters of open space and a textured surface below for reliable optical flow tracking.
| Test Step | What to Observe |
| Start Position Hold | Smooth autonomous takeoff |
| Climb Phase | Drone reaches target height without oscillation |
| Hold Phase | Drone stabilizes before entering position hold |
| Trajectory Plot | Red position marker stays near the yellow origin marker |
When you click Start Position Hold, the drone will take off, stabilize at the target height, and enter the main hold loop. The position estimate should remain clustered near the origin in the GUI.
Evaluating Position Hold Quality
Evaluating performance means knowing what “good” looks like and recognizing signs of poor tuning or environmental issues. A properly tuned system will hold position with minimal correction and no consistent drift.
| Metric | Good Performance Indicator |
| Position Error | Within ±0.15 m of target |
| Velocity Corrections | Typically below 0.05 m/s |
| Trajectory Drift | No consistent bias in one direction |
| Motion Pattern | Minor wandering, not continuous movement |
Some slow wandering is normal due to dead-reckoning error accumulation, but it should not steadily pull the drone in a single direction. Consistent drift usually points to tuning issues, poor surface texture, or uneven lighting.
Troubleshooting Common Issues
Several common issues can affect position hold performance. Understanding how to diagnose and resolve these problems will help you achieve reliable operation.
| Issue | Observed Behavior | Likely Cause | Recommended Action |
| Position Hold Drift | The drone drifts steadily in one direction even though position hold mode is enabled. | This behavior is usually caused by an unsuitable flight surface that lacks visible texture, operation outside the optical flow sensor’s effective height range, external air disturbances such as fans, or incorrect trim values that introduce directional bias(TRIM_VX, TRIM_VY). | Use a textured and non-reflective surface, keep the flight height between 0.08 and 3 meters, remove sources of airflow, and verify that TRIM_VX and TRIM_VY are correctly adjusted. |
| Oscillation | The drone repeatedly overshoots and oscillates around the target position. | This typically occurs when the position controller proportional gain is set too high, resulting in an overly aggressive response with insufficient damping. | Gradually reduce POSITION_KP in steps of 0.1 until oscillation stops, and if necessary increase VELOCITY_KP to add damping and stabilize the response. |
| Sensor Data Not Ready | The sensor test or flight initialization fails with a “Sensor data not ready” error message. | The error indicates that the CRTP logging configuration failed due to incompatible firmware, missing log variables, improper shield installation, or network communication issues. | Ensure the drone is running shield-compatible firmware with the required log variables, confirm the shield is properly connected, verify network connectivity by pinging the drone’s IP address, and restart both the drone and the PC before retrying. |
| Battery Warnings | The system refuses to arm or start flight and reports a low battery condition. | The measured battery voltage is below the defined safety threshold required for reliable operation. | Fully charge the battery before attempting flight, as operating below the LOW_BATTERY_THRESHOLD can lead to unexpected power loss and crashes. |
| Large Position Errors | The 2D trajectory plot shows the estimated position rapidly diverging from the origin. | This behavior is usually caused by poor optical flow tracking due to an inappropriate surface, incorrect height, insufficient lighting, or a malfunctioning sensor. | Move to a flight area with better surface texture and lighting, verify the height is within the supported range, and inspect the optical flow sensor if the issue persists. |
| Debug Mode | Control logic and sensor data appear active while the motors remain inactive. | This occurs when debug mode is intentionally enabled to prevent motor output during testing. | Enable DEBUG_MODE in the GUI to safely test sensor logging, position estimation, and control algorithms without spinning the motors. |
Document Index
Complete Project Code
"""
Dead Reckoning Optical Position Hold - Standalone Script
=========================================================
A simplified standalone script for optical flow-based position hold.
Uses dead reckoning with PID controllers to maintain the drone's position
after takeoff.
This script is designed for learners to understand the core concepts
of position hold using optical flow sensors.
Features:
- Sensor Test to arm/prepare the drone
- Optical flow-based position hold
- Adjustable TRIM values, Height, and PID parameters
- Real-time position, velocity, and correction feedback
- Safety checks for battery and sensors
Usage:
1. Connect to the drone via UDP
2. Click "Sensor Test" to arm and verify sensors
3. Adjust parameters if needed
4. Click "Start Position Hold" to takeoff and hover
5. The drone will maintain its position automatically
6. Click "Stop" or press Enter for emergency stop
Author: Dharageswaran S
Version: 1.0
"""
import time
import threading
import cflib.crtp
from cflib.crazyflie import Crazyflie
from cflib.crazyflie.log import LogConfig
from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
import tkinter as tk
from tkinter import ttk
from matplotlib.backends.backend_tkagg import FigureCanvasTkAgg
from matplotlib.figure import Figure
import matplotlib.animation as animation
import csv
from datetime import datetime
# === CONFIGURATION PARAMETERS ===
DRONE_URI = "udp://192.168.43.42"
TARGET_HEIGHT = 0.3 # Target hover height in meters
TAKEOFF_TIME = 0.5 # Time to takeoff and stabilize
HOVER_DURATION = 60.0 # How long to hover with position hold (seconds)
LANDING_TIME = 0.5 # Time to land
DEBUG_MODE = False # Set to True to disable motors (sensors and logging still work)
# Velocity and control parameters
VELOCITY_SMOOTHING_ALPHA = 0.9 # Filtering strength for velocity smoothing
VELOCITY_THRESHOLD = 0.005 # Consider drone "stationary" below this velocity
CONTROL_UPDATE_RATE = 0.02 # 50Hz control loop
SENSOR_PERIOD_MS = 10 # Motion sensor update rate
DT = SENSOR_PERIOD_MS / 1000.0
# Basic trim corrections (adjustable via UI)
TRIM_VX = 0.0 # Forward/backward trim correction
TRIM_VY = 0.0 # Left/right trim correction
# Battery monitoring
LOW_BATTERY_THRESHOLD = 2.9 # Low battery warning threshold in volts
# Height sensor safety
HEIGHT_SENSOR_MIN_CHANGE = 0.015 # Minimum height change expected during takeoff
# === DEAD RECKONING POSITION CONTROL PARAMETERS ===
# PID Controller Parameters (adjustable via UI)
POSITION_KP = 1.2
POSITION_KI = 0.0
POSITION_KD = 0.0
VELOCITY_KP = 1.2
VELOCITY_KI = 0.0
VELOCITY_KD = 0.0
# Control limits
MAX_CORRECTION = 0.1 # Maximum control correction allowed
DRIFT_COMPENSATION_RATE = 0.004 # Gentle pull toward zero when moving slowly
MAX_POSITION_ERROR = 2.0 # Clamp position error to prevent runaway
PERIODIC_RESET_INTERVAL = 90.0 # Reset integrated position periodically
# Velocity calculation constants
DEG_TO_RAD = 3.1415926535 / 180.0
OPTICAL_FLOW_SCALE = 4.4 # Empirical scaling factor
USE_HEIGHT_SCALING = True
# === GLOBAL VARIABLES ===
# Sensor data
current_height = 0.0
motion_delta_x = 0
motion_delta_y = 0
sensor_data_ready = False
# Battery voltage data
current_battery_voltage = 0.0
battery_data_ready = False
# Velocity tracking
current_vx = 0.0
current_vy = 0.0
velocity_x_history = [0.0, 0.0]
velocity_y_history = [0.0, 0.0]
# Dead reckoning position integration
integrated_position_x = 0.0
integrated_position_y = 0.0
last_integration_time = time.time()
last_reset_time = time.time()
position_integration_enabled = False
# Control corrections
current_correction_vx = 0.0
current_correction_vy = 0.0
# PID Controller state variables
position_integral_x = 0.0
position_integral_y = 0.0
last_position_error_x = 0.0
last_position_error_y = 0.0
velocity_integral_x = 0.0
velocity_integral_y = 0.0
last_velocity_error_x = 0.0
last_velocity_error_y = 0.0
# Target position for position hold
target_position_x = 0.0
target_position_y = 0.0
# Flight state
flight_phase = "IDLE"
flight_active = False
sensor_test_active = False
scf_instance = None
# Data history for plotting
max_history_points = 200
time_history = []
velocity_x_history_plot = []
velocity_y_history_plot = []
position_x_history = []
position_y_history = []
correction_vx_history = []
correction_vy_history = []
height_history = []
complete_trajectory_x = []
complete_trajectory_y = []
start_time = None
# CSV logging
log_file = None
log_writer = None
# === HELPER FUNCTIONS ===
def calculate_velocity(delta_value, altitude):
"""Convert optical flow delta to linear velocity"""
if altitude <= 0:
return 0.0
if USE_HEIGHT_SCALING:
velocity_constant = (4.4 * DEG_TO_RAD) / (30.0 * DT)
velocity = delta_value * altitude * velocity_constant
else:
velocity = delta_value * OPTICAL_FLOW_SCALE * DT
return velocity
def smooth_velocity(new_velocity, history):
"""Simple 2-point smoothing filter with adjustable alpha"""
history[1] = history[0]
history[0] = new_velocity
alpha = VELOCITY_SMOOTHING_ALPHA
smoothed = (history[0] * alpha) + (history[1] * (1 - alpha))
if abs(smoothed) < VELOCITY_THRESHOLD:
smoothed = 0.0
return smoothed
def integrate_position(vx, vy, dt):
"""Dead reckoning: integrate velocity to position"""
global integrated_position_x, integrated_position_y
if dt <= 0 or dt > 0.1:
return
# Simple integration
integrated_position_x += vx * dt
integrated_position_y += vy * dt
# Apply drift compensation when moving slowly
velocity_magnitude = (vx * vx + vy * vy) ** 0.5
if velocity_magnitude < VELOCITY_THRESHOLD * 2:
integrated_position_x -= integrated_position_x * DRIFT_COMPENSATION_RATE * dt
integrated_position_y -= integrated_position_y * DRIFT_COMPENSATION_RATE * dt
# Clamp position error
integrated_position_x = max(-MAX_POSITION_ERROR, min(MAX_POSITION_ERROR, integrated_position_x))
integrated_position_y = max(-MAX_POSITION_ERROR, min(MAX_POSITION_ERROR, integrated_position_y))
def periodic_position_reset():
"""Reset integrated position periodically to prevent drift accumulation"""
global integrated_position_x, integrated_position_y, last_reset_time
current_time = time.time()
if current_time - last_reset_time >= PERIODIC_RESET_INTERVAL:
integrated_position_x = 0.0
integrated_position_y = 0.0
last_reset_time = current_time
return True
return False
def reset_position_tracking():
"""Reset integrated position tracking"""
global integrated_position_x, integrated_position_y, last_integration_time, last_reset_time
global position_integration_enabled
global position_integral_x, position_integral_y, velocity_integral_x, velocity_integral_y
global last_position_error_x, last_position_error_y, last_velocity_error_x, last_velocity_error_y
global target_position_x, target_position_y
integrated_position_x = 0.0
integrated_position_y = 0.0
target_position_x = 0.0
target_position_y = 0.0
last_integration_time = time.time()
last_reset_time = time.time()
position_integration_enabled = True
# Reset PID state
position_integral_x = 0.0
position_integral_y = 0.0
velocity_integral_x = 0.0
velocity_integral_y = 0.0
last_position_error_x = 0.0
last_position_error_y = 0.0
last_velocity_error_x = 0.0
last_velocity_error_y = 0.0
def calculate_position_hold_corrections():
"""Calculate control corrections using PID controllers"""
global current_correction_vx, current_correction_vy
global position_integral_x, position_integral_y
global last_position_error_x, last_position_error_y
global velocity_integral_x, velocity_integral_y
global last_velocity_error_x, last_velocity_error_y
if not sensor_data_ready or current_height <= 0:
current_correction_vx = 0.0
current_correction_vy = 0.0
return 0.0, 0.0
# Calculate position errors (negative because we want to correct toward target)
position_error_x = -(integrated_position_x - target_position_x)
position_error_y = -(integrated_position_y - target_position_y)
# Calculate velocity errors (negative because we want to dampen velocity)
velocity_error_x = -current_vx
velocity_error_y = -current_vy
# Position PID Controller
# Proportional
position_p_x = position_error_x * POSITION_KP
position_p_y = position_error_y * POSITION_KP
# Integral (with anti-windup)
position_integral_x += position_error_x * CONTROL_UPDATE_RATE
position_integral_y += position_error_y * CONTROL_UPDATE_RATE
position_integral_x = max(-0.1, min(0.1, position_integral_x))
position_integral_y = max(-0.1, min(0.1, position_integral_y))
position_i_x = position_integral_x * POSITION_KI
position_i_y = position_integral_y * POSITION_KI
# Derivative
position_derivative_x = (position_error_x - last_position_error_x) / CONTROL_UPDATE_RATE
position_derivative_y = (position_error_y - last_position_error_y) / CONTROL_UPDATE_RATE
position_d_x = position_derivative_x * POSITION_KD
position_d_y = position_derivative_y * POSITION_KD
last_position_error_x = position_error_x
last_position_error_y = position_error_y
# Velocity PID Controller
# Proportional
velocity_p_x = velocity_error_x * VELOCITY_KP
velocity_p_y = velocity_error_y * VELOCITY_KP
# Integral (with anti-windup)
velocity_integral_x += velocity_error_x * CONTROL_UPDATE_RATE
velocity_integral_y += velocity_error_y * CONTROL_UPDATE_RATE
velocity_integral_x = max(-0.05, min(0.05, velocity_integral_x))
velocity_integral_y = max(-0.05, min(0.05, velocity_integral_y))
velocity_i_x = velocity_integral_x * VELOCITY_KI
velocity_i_y = velocity_integral_y * VELOCITY_KI
# Derivative
velocity_derivative_x = (velocity_error_x - last_velocity_error_x) / CONTROL_UPDATE_RATE
velocity_derivative_y = (velocity_error_y - last_velocity_error_y) / CONTROL_UPDATE_RATE
velocity_d_x = velocity_derivative_x * VELOCITY_KD
velocity_d_y = velocity_derivative_y * VELOCITY_KD
last_velocity_error_x = velocity_error_x
last_velocity_error_y = velocity_error_y
# Combine PID outputs
position_correction_vx = position_p_x + position_i_x + position_d_x
position_correction_vy = position_p_y + position_i_y + position_d_y
velocity_correction_vx = velocity_p_x + velocity_i_x + velocity_d_x
velocity_correction_vy = velocity_p_y + velocity_i_y + velocity_d_y
# Total corrections
total_vx = position_correction_vx + velocity_correction_vx
total_vy = position_correction_vy + velocity_correction_vy
# Apply limits
total_vx = max(-MAX_CORRECTION, min(MAX_CORRECTION, total_vx))
total_vy = max(-MAX_CORRECTION, min(MAX_CORRECTION, total_vy))
# Store for GUI display
current_correction_vx = total_vx
current_correction_vy = total_vy
return total_vx, total_vy
def update_history():
"""Update data history for plotting"""
global start_time
if start_time is None:
start_time = time.time()
current_time = time.time() - start_time
# Add new data points
time_history.append(current_time)
velocity_x_history_plot.append(current_vx)
velocity_y_history_plot.append(current_vy)
position_x_history.append(integrated_position_x)
position_y_history.append(integrated_position_y)
correction_vx_history.append(current_correction_vx)
correction_vy_history.append(current_correction_vy)
height_history.append(current_height)
complete_trajectory_x.append(integrated_position_x)
complete_trajectory_y.append(integrated_position_y)
# Trim history to max points
if len(time_history) > max_history_points:
time_history.pop(0)
velocity_x_history_plot.pop(0)
velocity_y_history_plot.pop(0)
position_x_history.pop(0)
position_y_history.pop(0)
correction_vx_history.pop(0)
correction_vy_history.pop(0)
height_history.pop(0)
def motion_callback(timestamp, data, logconf):
"""Motion sensor data callback"""
global current_height, motion_delta_x, motion_delta_y, sensor_data_ready
global current_vx, current_vy, last_integration_time
# Get sensor data
current_height = data.get("stateEstimate.z", 0)
motion_delta_x = data.get("motion.deltaX", 0)
motion_delta_y = data.get("motion.deltaY", 0)
sensor_data_ready = True
# Calculate velocities
raw_velocity_x = calculate_velocity(motion_delta_x, current_height)
raw_velocity_y = calculate_velocity(motion_delta_y, current_height)
# Apply smoothing
current_vx = smooth_velocity(raw_velocity_x, velocity_x_history)
current_vy = smooth_velocity(raw_velocity_y, velocity_y_history)
# Dead reckoning position integration
current_time = time.time()
dt = current_time - last_integration_time
if 0.001 <= dt <= 0.1 and position_integration_enabled:
integrate_position(current_vx, current_vy, dt)
last_integration_time = current_time
# Update history for GUI
update_history()
def battery_callback(timestamp, data, logconf):
"""Battery voltage data callback"""
global current_battery_voltage, battery_data_ready
current_battery_voltage = data.get("pm.vbat", 0.0)
battery_data_ready = True
def setup_logging(cf, logger=None):
"""Setup motion sensor and battery voltage logging"""
log_motion = LogConfig(name="Motion", period_in_ms=SENSOR_PERIOD_MS)
log_battery = LogConfig(name="Battery", period_in_ms=500)
try:
toc = cf.log.toc.toc
# Setup motion logging
motion_variables = [
("motion.deltaX", "int16_t"),
("motion.deltaY", "int16_t"),
("stateEstimate.z", "float"),
]
added_motion_vars = []
for var_name, var_type in motion_variables:
group, name = var_name.split(".")
if group in toc and name in toc[group]:
try:
log_motion.add_variable(var_name, var_type)
added_motion_vars.append(var_name)
except Exception as e:
if logger:
logger(f"Failed to add motion variable {var_name}: {e}")
else:
if logger:
logger(f"Motion variable not found: {var_name}")
if len(added_motion_vars) < 2:
if logger:
logger("ERROR: Not enough motion variables found!")
return None, None
# Setup battery logging
battery_variables = [("pm.vbat", "float")]
added_battery_vars = []
for var_name, var_type in battery_variables:
group, name = var_name.split(".")
if group in toc and name in toc[group]:
try:
log_battery.add_variable(var_name, var_type)
added_battery_vars.append(var_name)
except Exception as e:
if logger:
logger(f"Failed to add battery variable {var_name}: {e}")
else:
if logger:
logger(f"Battery variable not found: {var_name}")
# Setup callbacks
log_motion.data_received_cb.add_callback(motion_callback)
if len(added_battery_vars) > 0:
log_battery.data_received_cb.add_callback(battery_callback)
# Add configurations
cf.log.add_config(log_motion)
if len(added_battery_vars) > 0:
cf.log.add_config(log_battery)
time.sleep(0.5)
# Validate configurations
if not log_motion.valid:
if logger:
logger("ERROR: Motion log configuration invalid!")
return None, None
if len(added_battery_vars) > 0 and not log_battery.valid:
if logger:
logger("WARNING: Battery log configuration invalid!")
log_battery = None
# Start logging
log_motion.start()
if log_battery:
log_battery.start()
time.sleep(0.5)
if logger:
logger(f"Logging started - Motion: {len(added_motion_vars)} vars, Battery: {len(added_battery_vars)} vars")
return log_motion, log_battery
except Exception as e:
error_msg = f"Logging setup failed: {str(e)}"
if logger:
logger(error_msg)
raise Exception(error_msg)
def init_csv_logging(logger=None):
"""Initialize CSV logging"""
global log_file, log_writer
timestamp = datetime.now().strftime("%Y%m%d_%H%M%S")
log_filename = f"position_hold_log_{timestamp}.csv"
log_file = open(log_filename, mode="w", newline="")
log_writer = csv.writer(log_file)
log_writer.writerow([
"Timestamp (s)", "Position X (m)", "Position Y (m)",
"Height (m)", "Velocity X (m/s)", "Velocity Y (m/s)",
"Correction VX", "Correction VY"
])
if logger:
logger(f"Logging to CSV: {log_filename}")
def log_to_csv():
"""Log current state to CSV"""
global log_writer, start_time
if log_writer is None or start_time is None:
return
elapsed = time.time() - start_time
log_writer.writerow([
f"{elapsed:.3f}", f"{integrated_position_x:.6f}", f"{integrated_position_y:.6f}",
f"{current_height:.6f}", f"{current_vx:.6f}", f"{current_vy:.6f}",
f"{current_correction_vx:.6f}", f"{current_correction_vy:.6f}"
])
def close_csv_logging(logger=None):
"""Close CSV log file"""
global log_file
if log_file:
log_file.close()
log_file = None
if logger:
logger("CSV log closed.")
class PositionHoldGUI:
"""
GUI for Optical Flow Position Hold
This class provides a user interface for testing optical flow-based
position hold with adjustable PID parameters.
"""
def __init__(self, root):
self.root = root
self.root.title("Dead Reckoning Optical Position Hold")
self.root.geometry("1200x800")
# Control variables
self.flight_thread = None
self.flight_running = False
self.sensor_test_thread = None
self.sensor_test_running = False
self.create_ui()
self.setup_plots()
# Start animation
self.anim = animation.FuncAnimation(
self.fig, self.update_plots, interval=100, cache_frame_data=False
)
# Bind keyboard events
self.root.bind("<KeyPress>", self.on_key_press)
self.root.focus_set()
def create_ui(self):
"""Create the user interface"""
# Control panel
control_frame = tk.Frame(self.root)
control_frame.pack(fill=tk.X, padx=10, pady=5)
# Sensor Test button
self.sensor_test_button = tk.Button(
control_frame, text="Sensor Test (ARM)",
command=self.start_sensor_test,
bg="lightblue", fg="black", font=("Arial", 11, "bold"), width=16
)
self.sensor_test_button.pack(side=tk.LEFT, padx=5)
# Start Position Hold button
self.start_button = tk.Button(
control_frame, text="Start Position Hold",
command=self.start_flight,
bg="green", fg="white", font=("Arial", 11, "bold"), width=16
)
self.start_button.pack(side=tk.LEFT, padx=5)
# Debug mode checkbox
self.debug_mode_var = tk.BooleanVar(value=DEBUG_MODE)
tk.Checkbutton(
control_frame, text="Debug Mode", variable=self.debug_mode_var,
command=self.toggle_debug_mode, font=("Arial", 9)
).pack(side=tk.LEFT, padx=10)
# Status display
self.status_var = tk.StringVar(value="Status: Ready - Run Sensor Test to ARM")
tk.Label(
control_frame, textvariable=self.status_var,
font=("Arial", 11, "bold"), fg="blue"
).pack(side=tk.LEFT, padx=20)
# Main frame
main_frame = tk.Frame(self.root)
main_frame.pack(fill=tk.BOTH, expand=True, padx=10, pady=5)
# Left side - Controls
left_frame = tk.Frame(main_frame)
left_frame.pack(side=tk.LEFT, fill=tk.Y, padx=(0, 5))
# === Flight Parameters Frame ===
params_frame = tk.LabelFrame(left_frame, text="Flight Parameters", padx=10, pady=10)
params_frame.pack(fill=tk.X, pady=5)
# Target Height
height_frame = tk.Frame(params_frame)
height_frame.pack(fill=tk.X, pady=2)
tk.Label(height_frame, text="Target Height (m):", width=16, anchor=tk.W).pack(side=tk.LEFT)
self.height_entry_var = tk.StringVar(value=str(TARGET_HEIGHT))
tk.Entry(height_frame, textvariable=self.height_entry_var, width=8).pack(side=tk.LEFT, padx=5)
# Hover Duration
duration_frame = tk.Frame(params_frame)
duration_frame.pack(fill=tk.X, pady=2)
tk.Label(duration_frame, text="Hover Duration (s):", width=16, anchor=tk.W).pack(side=tk.LEFT)
self.duration_var = tk.StringVar(value=str(HOVER_DURATION))
tk.Entry(duration_frame, textvariable=self.duration_var, width=8).pack(side=tk.LEFT, padx=5)
# TRIM VX
trim_vx_frame = tk.Frame(params_frame)
trim_vx_frame.pack(fill=tk.X, pady=2)
tk.Label(trim_vx_frame, text="TRIM VX:", width=16, anchor=tk.W).pack(side=tk.LEFT)
self.trim_vx_var = tk.StringVar(value=str(TRIM_VX))
tk.Entry(trim_vx_frame, textvariable=self.trim_vx_var, width=8).pack(side=tk.LEFT, padx=5)
# TRIM VY
trim_vy_frame = tk.Frame(params_frame)
trim_vy_frame.pack(fill=tk.X, pady=2)
tk.Label(trim_vy_frame, text="TRIM VY:", width=16, anchor=tk.W).pack(side=tk.LEFT)
self.trim_vy_var = tk.StringVar(value=str(TRIM_VY))
tk.Entry(trim_vy_frame, textvariable=self.trim_vy_var, width=8).pack(side=tk.LEFT, padx=5)
# === PID Parameters Frame ===
pid_frame = tk.LabelFrame(left_frame, text="PID Parameters", padx=10, pady=10)
pid_frame.pack(fill=tk.X, pady=5)
# Container for two columns
pid_columns = tk.Frame(pid_frame)
pid_columns.pack(fill=tk.X)
# Left column - Position PID
pos_column = tk.Frame(pid_columns)
pos_column.pack(side=tk.LEFT, fill=tk.Y, padx=(0, 10))
tk.Label(pos_column, text="Position PID:", font=("Arial", 9, "bold")).pack(anchor=tk.W)
pos_kp_frame = tk.Frame(pos_column)
pos_kp_frame.pack(fill=tk.X, pady=1)
tk.Label(pos_kp_frame, text="Kp:", width=4, anchor=tk.W).pack(side=tk.LEFT)
self.pos_kp_var = tk.StringVar(value=str(POSITION_KP))
tk.Entry(pos_kp_frame, textvariable=self.pos_kp_var, width=6).pack(side=tk.LEFT)
pos_ki_frame = tk.Frame(pos_column)
pos_ki_frame.pack(fill=tk.X, pady=1)
tk.Label(pos_ki_frame, text="Ki:", width=4, anchor=tk.W).pack(side=tk.LEFT)
self.pos_ki_var = tk.StringVar(value=str(POSITION_KI))
tk.Entry(pos_ki_frame, textvariable=self.pos_ki_var, width=6).pack(side=tk.LEFT)
pos_kd_frame = tk.Frame(pos_column)
pos_kd_frame.pack(fill=tk.X, pady=1)
tk.Label(pos_kd_frame, text="Kd:", width=4, anchor=tk.W).pack(side=tk.LEFT)
self.pos_kd_var = tk.StringVar(value=str(POSITION_KD))
tk.Entry(pos_kd_frame, textvariable=self.pos_kd_var, width=6).pack(side=tk.LEFT)
# Right column - Velocity PID
vel_column = tk.Frame(pid_columns)
vel_column.pack(side=tk.LEFT, fill=tk.Y)
tk.Label(vel_column, text="Velocity PID:", font=("Arial", 9, "bold")).pack(anchor=tk.W)
vel_kp_frame = tk.Frame(vel_column)
vel_kp_frame.pack(fill=tk.X, pady=1)
tk.Label(vel_kp_frame, text="Kp:", width=4, anchor=tk.W).pack(side=tk.LEFT)
self.vel_kp_var = tk.StringVar(value=str(VELOCITY_KP))
tk.Entry(vel_kp_frame, textvariable=self.vel_kp_var, width=6).pack(side=tk.LEFT)
vel_ki_frame = tk.Frame(vel_column)
vel_ki_frame.pack(fill=tk.X, pady=1)
tk.Label(vel_ki_frame, text="Ki:", width=4, anchor=tk.W).pack(side=tk.LEFT)
self.vel_ki_var = tk.StringVar(value=str(VELOCITY_KI))
tk.Entry(vel_ki_frame, textvariable=self.vel_ki_var, width=6).pack(side=tk.LEFT)
vel_kd_frame = tk.Frame(vel_column)
vel_kd_frame.pack(fill=tk.X, pady=1)
tk.Label(vel_kd_frame, text="Kd:", width=4, anchor=tk.W).pack(side=tk.LEFT)
self.vel_kd_var = tk.StringVar(value=str(VELOCITY_KD))
tk.Entry(vel_kd_frame, textvariable=self.vel_kd_var, width=6).pack(side=tk.LEFT)
# Apply button (full width below columns)
tk.Button(
pid_frame, text="Apply All Values", command=self.apply_values,
bg="green", fg="white", font=("Arial", 10)
).pack(pady=5)
# === Real-time values display ===
values_frame = tk.LabelFrame(left_frame, text="Real-Time Values", padx=10, pady=10)
values_frame.pack(fill=tk.X, pady=5)
self.height_var = tk.StringVar(value="Height: 0.000m")
self.battery_var = tk.StringVar(value="Battery: N/A")
self.vx_var = tk.StringVar(value="VX: 0.000 m/s")
self.vy_var = tk.StringVar(value="VY: 0.000 m/s")
self.pos_x_var = tk.StringVar(value="Pos X: 0.000m")
self.pos_y_var = tk.StringVar(value="Pos Y: 0.000m")
self.corr_vx_var = tk.StringVar(value="Corr VX: 0.000")
self.corr_vy_var = tk.StringVar(value="Corr VY: 0.000")
self.phase_var = tk.StringVar(value="Phase: IDLE")
tk.Label(values_frame, textvariable=self.height_var, font=("Arial", 10), fg="blue").pack(anchor=tk.W)
tk.Label(values_frame, textvariable=self.battery_var, font=("Arial", 10), fg="orange").pack(anchor=tk.W)
tk.Label(values_frame, textvariable=self.vx_var, font=("Arial", 10)).pack(anchor=tk.W)
tk.Label(values_frame, textvariable=self.vy_var, font=("Arial", 10)).pack(anchor=tk.W)
tk.Label(values_frame, textvariable=self.pos_x_var, font=("Arial", 10), fg="darkgreen").pack(anchor=tk.W)
tk.Label(values_frame, textvariable=self.pos_y_var, font=("Arial", 10), fg="darkgreen").pack(anchor=tk.W)
tk.Label(values_frame, textvariable=self.corr_vx_var, font=("Arial", 10), fg="red").pack(anchor=tk.W)
tk.Label(values_frame, textvariable=self.corr_vy_var, font=("Arial", 10), fg="red").pack(anchor=tk.W)
tk.Label(values_frame, textvariable=self.phase_var, font=("Arial", 10, "bold"), fg="purple").pack(anchor=tk.W)
# === Output log ===
output_frame = tk.LabelFrame(left_frame, text="Output Log", padx=5, pady=5)
output_frame.pack(fill=tk.BOTH, expand=True, pady=5)
self.output_text = tk.Text(output_frame, height=8, width=40, font=("Consolas", 9))
self.output_text.pack(fill=tk.BOTH, expand=True)
# Right side - Plots
right_frame = tk.Frame(main_frame)
right_frame.pack(side=tk.RIGHT, fill=tk.BOTH, expand=True, padx=(5, 0))
self.fig = Figure(figsize=(9, 7))
self.canvas = FigureCanvasTkAgg(self.fig, master=right_frame)
self.canvas.get_tk_widget().pack(fill=tk.BOTH, expand=True)
def setup_plots(self):
"""Setup matplotlib plots"""
self.ax1 = self.fig.add_subplot(2, 2, 1) # Velocities
self.ax2 = self.fig.add_subplot(2, 2, 2) # Position (2D)
self.ax3 = self.fig.add_subplot(2, 2, 3) # Corrections
self.ax4 = self.fig.add_subplot(2, 2, 4) # Height
# Velocities
self.ax1.set_title("Velocities")
self.ax1.set_xlabel("Time (s)")
self.ax1.set_ylabel("Velocity (m/s)")
self.ax1.grid(True, alpha=0.3)
(self.line_vx,) = self.ax1.plot([], [], "b-", label="VX")
(self.line_vy,) = self.ax1.plot([], [], "r-", label="VY")
self.ax1.legend()
# 2D Position
self.ax2.set_title("Position (Dead Reckoning)")
self.ax2.set_xlabel("X (m)")
self.ax2.set_ylabel("Y (m)")
self.ax2.set_aspect("equal")
self.ax2.grid(True, alpha=0.3)
(self.line_pos,) = self.ax2.plot([], [], "purple", alpha=0.7, label="Trajectory")
(self.current_pos,) = self.ax2.plot([], [], "ro", markersize=8, label="Current")
self.ax2.plot(0, 0, "ko", markersize=10, markerfacecolor="yellow", label="Target")
self.ax2.legend()
# Corrections
self.ax3.set_title("Control Corrections")
self.ax3.set_xlabel("Time (s)")
self.ax3.set_ylabel("Correction")
self.ax3.grid(True, alpha=0.3)
(self.line_corr_vx,) = self.ax3.plot([], [], "g-", label="Corr VX")
(self.line_corr_vy,) = self.ax3.plot([], [], "m-", label="Corr VY")
self.ax3.legend()
# Height
self.ax4.set_title("Height")
self.ax4.set_xlabel("Time (s)")
self.ax4.set_ylabel("Height (m)")
self.ax4.grid(True, alpha=0.3)
(self.line_height,) = self.ax4.plot([], [], "orange", label="Height")
self.target_height_line = self.ax4.axhline(y=TARGET_HEIGHT, color="red", linestyle="--", alpha=0.7, label="Target")
self.ax4.legend()
self.fig.tight_layout()
def update_plots(self, frame):
"""Update all plots with new data"""
if not time_history:
return []
# Update value displays
self.height_var.set(f"Height: {current_height:.3f}m")
self.phase_var.set(f"Phase: {flight_phase}")
if current_battery_voltage > 0:
self.battery_var.set(f"Battery: {current_battery_voltage:.2f}V")
self.vx_var.set(f"VX: {current_vx:.3f} m/s")
self.vy_var.set(f"VY: {current_vy:.3f} m/s")
self.pos_x_var.set(f"Pos X: {integrated_position_x:.3f}m")
self.pos_y_var.set(f"Pos Y: {integrated_position_y:.3f}m")
self.corr_vx_var.set(f"Corr VX: {current_correction_vx:.3f}")
self.corr_vy_var.set(f"Corr VY: {current_correction_vy:.3f}")
# Update plots
try:
self.line_vx.set_data(time_history, velocity_x_history_plot)
self.line_vy.set_data(time_history, velocity_y_history_plot)
if complete_trajectory_x and complete_trajectory_y:
plot_x = [-x for x in complete_trajectory_x]
self.line_pos.set_data(plot_x, complete_trajectory_y)
self.current_pos.set_data([-integrated_position_x], [integrated_position_y])
self.line_corr_vx.set_data(time_history, correction_vx_history)
self.line_corr_vy.set_data(time_history, correction_vy_history)
self.line_height.set_data(time_history, height_history)
# Adjust axis limits
if len(time_history) > 1:
for ax in [self.ax1, self.ax3, self.ax4]:
ax.set_xlim(min(time_history), max(time_history))
if velocity_x_history_plot and velocity_y_history_plot:
all_vel = velocity_x_history_plot + velocity_y_history_plot
if any(v != 0 for v in all_vel):
self.ax1.set_ylim(min(all_vel) - 0.01, max(all_vel) + 0.01)
if complete_trajectory_x and complete_trajectory_y:
plot_x = [-x for x in complete_trajectory_x]
margin = max(max(plot_x) - min(plot_x), max(complete_trajectory_y) - min(complete_trajectory_y), 0.02) * 0.6
center_x = (max(plot_x) + min(plot_x)) / 2
center_y = (max(complete_trajectory_y) + min(complete_trajectory_y)) / 2
self.ax2.set_xlim(center_x - margin, center_x + margin)
self.ax2.set_ylim(center_y - margin, center_y + margin)
if correction_vx_history and correction_vy_history:
all_corr = correction_vx_history + correction_vy_history
if any(c != 0 for c in all_corr):
self.ax3.set_ylim(min(all_corr) - 0.01, max(all_corr) + 0.01)
if height_history:
self.ax4.set_ylim(min(height_history) - 0.05, max(height_history) + 0.05)
except Exception:
pass
return [self.line_vx, self.line_vy, self.line_pos, self.current_pos,
self.line_corr_vx, self.line_corr_vy, self.line_height]
def log_to_output(self, message):
"""Log a message to the output window"""
timestamp = time.strftime("%H:%M:%S")
self.output_text.insert(tk.END, f"[{timestamp}] {message}\n")
self.output_text.see(tk.END)
def clear_output(self):
"""Clear the output log"""
self.output_text.delete(1.0, tk.END)
def clear_graphs(self):
"""Clear all graph data"""
global time_history, velocity_x_history_plot, velocity_y_history_plot
global position_x_history, position_y_history, height_history
global correction_vx_history, correction_vy_history
global complete_trajectory_x, complete_trajectory_y, start_time
time_history.clear()
velocity_x_history_plot.clear()
velocity_y_history_plot.clear()
position_x_history.clear()
position_y_history.clear()
correction_vx_history.clear()
correction_vy_history.clear()
height_history.clear()
complete_trajectory_x.clear()
complete_trajectory_y.clear()
start_time = None
def apply_values(self):
"""Apply all parameter values from UI"""
global TARGET_HEIGHT, HOVER_DURATION, TRIM_VX, TRIM_VY
global POSITION_KP, POSITION_KI, POSITION_KD
global VELOCITY_KP, VELOCITY_KI, VELOCITY_KD
try:
TARGET_HEIGHT = float(self.height_entry_var.get())
HOVER_DURATION = float(self.duration_var.get())
TRIM_VX = float(self.trim_vx_var.get())
TRIM_VY = float(self.trim_vy_var.get())
POSITION_KP = float(self.pos_kp_var.get())
POSITION_KI = float(self.pos_ki_var.get())
POSITION_KD = float(self.pos_kd_var.get())
VELOCITY_KP = float(self.vel_kp_var.get())
VELOCITY_KI = float(self.vel_ki_var.get())
VELOCITY_KD = float(self.vel_kd_var.get())
# Update height line in plot
self.target_height_line.set_ydata([TARGET_HEIGHT, TARGET_HEIGHT])
self.log_to_output(f"Applied: Height={TARGET_HEIGHT:.2f}m, Duration={HOVER_DURATION:.0f}s")
self.log_to_output(f"TRIM: VX={TRIM_VX:.2f}, VY={TRIM_VY:.2f}")
self.log_to_output(f"Pos PID: Kp={POSITION_KP}, Ki={POSITION_KI}, Kd={POSITION_KD}")
self.log_to_output(f"Vel PID: Kp={VELOCITY_KP}, Ki={VELOCITY_KI}, Kd={VELOCITY_KD}")
self.status_var.set("Status: Values applied")
except ValueError as e:
self.status_var.set(f"Status: Invalid value - {str(e)}")
self.log_to_output(f"Error: {str(e)}")
def toggle_debug_mode(self):
"""Toggle debug mode"""
global DEBUG_MODE
DEBUG_MODE = self.debug_mode_var.get()
mode_text = "ON (motors disabled)" if DEBUG_MODE else "OFF"
self.log_to_output(f"Debug mode: {mode_text}")
# ==================== SENSOR TEST ====================
def start_sensor_test(self):
"""Start sensor test to ARM the drone"""
if not self.sensor_test_running and not self.flight_running:
self.sensor_test_running = True
self.sensor_test_button.config(text="Stop Sensor Test", command=self.stop_sensor_test, bg="red")
self.status_var.set("Status: Sensor Test Running - ARMING...")
self.sensor_test_thread = threading.Thread(target=self.sensor_test_thread_func)
self.sensor_test_thread.daemon = True
self.sensor_test_thread.start()
elif self.flight_running:
self.status_var.set("Status: Cannot run Sensor Test during flight")
def stop_sensor_test(self):
"""Stop sensor test"""
global sensor_test_active
if self.sensor_test_running:
sensor_test_active = False
self.sensor_test_running = False
if self.sensor_test_thread and self.sensor_test_thread.is_alive():
self.sensor_test_thread.join(timeout=2.0)
self.status_var.set("Status: Sensor Test Stopped - ARMED ✓")
self.sensor_test_button.config(text="Sensor Test (ARM)", command=self.start_sensor_test, bg="lightblue")
def sensor_test_thread_func(self):
"""Sensor test thread"""
global flight_phase, sensor_test_active, scf_instance
global current_battery_voltage, battery_data_ready
self.root.after(0, self.clear_output)
self.root.after(0, self.clear_graphs)
sensor_test_active = True
flight_phase = "SENSOR_TEST"
current_battery_voltage = 0.0
battery_data_ready = False
cflib.crtp.init_drivers()
cf = Crazyflie(rw_cache="./cache")
log_motion = None
log_battery = None
try:
with SyncCrazyflie(DRONE_URI, cf=cf) as scf:
scf_instance = scf
log_motion, log_battery = setup_logging(cf, logger=self.log_to_output)
if log_motion is not None:
time.sleep(1.0)
if not DEBUG_MODE:
cf.commander.send_setpoint(0, 0, 0, 0)
time.sleep(0.1)
cf.param.set_value("commander.enHighLevel", "1")
time.sleep(0.5)
reset_position_tracking()
self.log_to_output("Sensor test running - reading sensors...")
while sensor_test_active:
if sensor_data_ready:
calculate_position_hold_corrections()
time.sleep(CONTROL_UPDATE_RATE)
except Exception as e:
flight_phase = "ERROR"
self.log_to_output(f"Sensor Test Error: {str(e)}")
finally:
if log_motion:
try:
log_motion.stop()
except:
pass
if log_battery:
try:
log_battery.stop()
except:
pass
sensor_test_active = False
flight_phase = "IDLE"
self.sensor_test_running = False
self.root.after(0, lambda: self.sensor_test_button.config(
text="Sensor Test (ARM)", command=self.start_sensor_test, bg="lightblue"
))
self.root.after(0, lambda: self.status_var.set("Status: Sensor Test Stopped - ARMED ✓"))
# ==================== POSITION HOLD FLIGHT ====================
def start_flight(self):
"""Start position hold flight"""
if self.flight_running or self.sensor_test_running:
if self.sensor_test_running:
self.status_var.set("Status: Stop Sensor Test first!")
return
# Apply current values
self.apply_values()
# Safety checks
if current_battery_voltage > 0 and current_battery_voltage < LOW_BATTERY_THRESHOLD:
self.status_var.set(f"Status: Battery too low ({current_battery_voltage:.2f}V)!")
return
elif current_battery_voltage == 0.0:
self.log_to_output("WARNING: Battery unknown - run Sensor Test first!")
self.status_var.set("Status: Run Sensor Test first!")
return
if not sensor_data_ready:
self.status_var.set("Status: Sensor data not ready - Run Sensor Test first!")
return
if current_height <= 0.0:
self.status_var.set("Status: Invalid height reading!")
return
# Start flight
self.flight_running = True
self.start_button.config(text="Stop Flight", command=self.emergency_stop, bg="red")
self.status_var.set("Status: Starting Position Hold Flight...")
self.log_to_output("Position Hold flight started")
self.flight_thread = threading.Thread(target=self.flight_thread_func)
self.flight_thread.daemon = True
self.flight_thread.start()
def emergency_stop(self):
"""Emergency stop"""
global flight_active, sensor_test_active
flight_active = False
sensor_test_active = False
self.flight_running = False
self.sensor_test_running = False
self.start_button.config(text="Start Position Hold", command=self.start_flight, bg="green")
self.sensor_test_button.config(text="Sensor Test (ARM)", command=self.start_sensor_test, bg="lightblue")
self.status_var.set("Status: EMERGENCY STOPPED")
self.log_to_output("EMERGENCY STOP!")
def flight_thread_func(self):
"""Position hold flight thread"""
global flight_active, flight_phase
global current_battery_voltage, battery_data_ready
global position_integration_enabled
self.root.after(0, self.clear_output)
self.root.after(0, self.clear_graphs)
cflib.crtp.init_drivers()
cf = Crazyflie(rw_cache="./cache")
log_motion = None
log_battery = None
current_battery_voltage = 0.0
battery_data_ready = False
try:
with SyncCrazyflie(DRONE_URI, cf=cf) as scf:
scf_instance = scf
flight_active = True
log_motion, log_battery = setup_logging(cf, logger=self.log_to_output)
use_position_hold = log_motion is not None
if use_position_hold:
time.sleep(1.0)
reset_position_tracking()
if not DEBUG_MODE:
cf.commander.send_setpoint(0, 0, 0, 0)
time.sleep(0.1)
cf.param.set_value("commander.enHighLevel", "1")
time.sleep(0.5)
else:
self.log_to_output("DEBUG MODE: Motors disabled")
# Takeoff
flight_phase = "TAKEOFF"
start_time_local = time.time()
init_csv_logging(logger=self.log_to_output)
while time.time() - start_time_local < TAKEOFF_TIME and flight_active:
if not DEBUG_MODE:
cf.commander.send_hover_setpoint(TRIM_VX, TRIM_VY, 0, TARGET_HEIGHT)
log_to_csv()
time.sleep(0.01)
# Stabilization
flight_phase = "STABILIZING"
stabilization_start = time.time()
while time.time() - stabilization_start < 3.0 and flight_active:
if use_position_hold and sensor_data_ready:
motion_vx, motion_vy = calculate_position_hold_corrections()
else:
motion_vx, motion_vy = 0.0, 0.0
log_to_csv()
total_vx = TRIM_VX + motion_vy
total_vy = TRIM_VY + motion_vx
if not DEBUG_MODE:
cf.commander.send_hover_setpoint(total_vx, total_vy, 0, TARGET_HEIGHT)
time.sleep(CONTROL_UPDATE_RATE)
# Position Hold
flight_phase = "POSITION_HOLD"
hover_start = time.time()
self.log_to_output(f"Position Hold active for {HOVER_DURATION:.0f}s")
while time.time() - hover_start < HOVER_DURATION and flight_active:
if use_position_hold and sensor_data_ready:
motion_vx, motion_vy = calculate_position_hold_corrections()
if periodic_position_reset():
flight_phase = "POSITION_HOLD (RESET)"
self.log_to_output("Position reset to origin")
else:
flight_phase = "POSITION_HOLD"
else:
motion_vx, motion_vy = 0.0, 0.0
log_to_csv()
total_vx = TRIM_VX + motion_vy
total_vy = TRIM_VY + motion_vx
if not DEBUG_MODE:
cf.commander.send_hover_setpoint(total_vx, total_vy, 0, TARGET_HEIGHT)
time.sleep(CONTROL_UPDATE_RATE)
# Landing
flight_phase = "LANDING"
landing_start = time.time()
while time.time() - landing_start < LANDING_TIME and flight_active:
if not DEBUG_MODE:
cf.commander.send_hover_setpoint(TRIM_VX, TRIM_VY, 0, 0)
log_to_csv()
time.sleep(0.01)
if not DEBUG_MODE:
cf.commander.send_setpoint(0, 0, 0, 0)
flight_phase = "COMPLETE"
self.log_to_output("Flight complete")
except Exception as e:
flight_phase = "ERROR"
self.log_to_output(f"Error: {str(e)}")
finally:
close_csv_logging(logger=self.log_to_output)
if log_motion:
try:
log_motion.stop()
except:
pass
if log_battery:
try:
log_battery.stop()
except:
pass
flight_active = False
self.flight_running = False
self.root.after(0, lambda: self.start_button.config(
text="Start Position Hold", command=self.start_flight, bg="green"
))
self.root.after(0, lambda: self.status_var.set("Status: Flight Complete"))
def on_key_press(self, event):
"""Handle key press events"""
if event.keysym in ("Return", "KP_Enter"):
self.emergency_stop()
self.log_to_output("EMERGENCY STOP: Enter key pressed")
def main():
"""Main entry point"""
try:
cflib.crtp.init_drivers()
print("Crazyflie CRTP drivers initialized")
except Exception as e:
print(f"Warning: cflib.crtp.init_drivers() failed: {e}")
root = tk.Tk()
app = PositionHoldGUI(root)
def on_closing():
global flight_active, sensor_test_active
flight_active = False
sensor_test_active = False
root.quit()
root.destroy()
root.protocol("WM_DELETE_WINDOW", on_closing)
root.mainloop()
if __name__ == "__main__":
main()
