diff --git a/aerpaw/MESSAGE_TYPE.m b/aerpaw/MESSAGE_TYPE.m index 9637edc..2ef2f04 100644 --- a/aerpaw/MESSAGE_TYPE.m +++ b/aerpaw/MESSAGE_TYPE.m @@ -8,5 +8,6 @@ classdef MESSAGE_TYPE < uint8 GUIDANCE_TOGGLE (6) % Server->Client: toggle guidance mode on/off REQUEST_POSITION (7) % Server->Client: respond with current ENU position POSITION (8) % Client->Server: current ENU position (3 doubles) + TAKEOFF (9) % Server->Client: take off to 25 m, ACK then READY on arrival end end diff --git a/aerpaw/client/uav_runner.py b/aerpaw/client/uav_runner.py index 2581102..99f8ec4 100644 --- a/aerpaw/client/uav_runner.py +++ b/aerpaw/client/uav_runner.py @@ -44,6 +44,7 @@ class MessageType(IntEnum): GUIDANCE_TOGGLE = 6 REQUEST_POSITION = 7 POSITION = 8 + TAKEOFF = 9 AERPAW_DIR = Path('/root/miSim/aerpaw') @@ -184,13 +185,11 @@ class UAVRunner(BasicRunner): log_task = None nav_task = None try: - # Takeoff to above AERPAW minimum altitude - print("[UAV] Taking off...") - await drone.takeoff(25) - print("[UAV] Takeoff complete, waiting for commands...") - - # Start GPS logging in background + # Takeoff is now driven by a TAKEOFF command from the controller + # (staggered in time between UAVs), not performed at startup. Start + # GPS logging immediately so the ground/idle period is captured too. log_task = asyncio.create_task(_gps_log_loop(drone)) + print("[UAV] Connected; waiting for commands...") # Command loop - handle all messages from controller waypoint_num = 0 @@ -213,6 +212,27 @@ class UAVRunner(BasicRunner): await send_message_type(writer, MessageType.ACK) print("[UAV] Sent ACK (guidance mode exited, ready for sequential commands)") + elif msg_type == MessageType.TAKEOFF: + # aerpawlib only switches the vehicle to GUIDED inside + # _initialize_postarm(), which await_ready_to_move() runs only + # when the vehicle is DISARMED at the first movement command. + # The safety pilot arms all UAVs at experiment start, so a UAV + # whose takeoff is deliberately delayed (for time separation) + # is already armed by now and would skip that path -> no GUIDED + # switch -> simple_takeoff is ignored in ALT_HOLD and it never + # leaves the ground. Run the post-arm init explicitly to + # guarantee GUIDED mode (and home capture) before taking off; + # takeoff()'s own gate then sees the vehicle armed and does not + # re-initialize. + await send_message_type(writer, MessageType.ACK) + print("[UAV] Sent ACK (takeoff)") + print("[UAV] Initializing (GUIDED) and taking off to 25 m...") + await drone._initialize_postarm() + await drone.takeoff(25) + print("[UAV] Takeoff complete") + await send_message_type(writer, MessageType.READY) + print("[UAV] Sent READY") + elif msg_type == MessageType.REQUEST_POSITION: # Respond immediately with current ENU position relative to origin pos = drone.position @@ -288,6 +308,11 @@ class UAVRunner(BasicRunner): except (ValueError, asyncio.IncompleteReadError, ConnectionError) as e: print(f"[UAV] Error: {e}") + except Exception as e: + import traceback + print(f"[UAV] Unhandled exception: {type(e).__name__}: {e}") + traceback.print_exc() + finally: if nav_task is not None and not nav_task.done(): nav_task.cancel() diff --git a/aerpaw/controller.coderprj b/aerpaw/controller.coderprj index ee5af41..56b3121 100644 --- a/aerpaw/controller.coderprj +++ b/aerpaw/controller.coderprj @@ -1,33 +1,33 @@ - + - + - + - + + + controller.m + + controller false numClients + true - true - - controller.m - - controller @@ -38,151 +38,16 @@ - - int32 - - - - - int32 - - - - - int32 - - - - - int32 - - - - - int32 - - - - - int32 - - - - - int32 - - - - - int32 - - - - - int32 - - - - - int32 - - - - - int32 - - - - - int32 - - - - - int32 - - - - - int32 - - - - - int32 - - - - - int32 - - - - - int32 - - - - - int32 - - - - - int32 - - - - - int32 - - - - - int32 - - - - - int32 - - - - - int32 - - - - - int32 - - - - - int32 - - - - - int32 - - - - - int32 - - - - + - + EXE @@ -198,989 +63,917 @@ /home/kdee/Desktop/miSim/aerpaw/codegen - /home/kdee/Desktop/miSim/aerpaw/codegen/RemoveDependentEq_.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/RemoveDependentIneq_.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/RemoveDependentEq_.h + /home/kdee/Desktop/miSim/aerpaw/codegen/RemoveDependentIneq_.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/RemoveDependentIneq_.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/abs.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/RemoveDependentIneq_.h + /home/kdee/Desktop/miSim/aerpaw/codegen/abs.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/abs.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/addBoundToActiveSetMatrix_.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/abs.h + /home/kdee/Desktop/miSim/aerpaw/codegen/addBoundToActiveSetMatrix_.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/addBoundToActiveSetMatrix_.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/agent.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/addBoundToActiveSetMatrix_.h + /home/kdee/Desktop/miSim/aerpaw/codegen/agent.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/agent.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/all.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/agent.h + /home/kdee/Desktop/miSim/aerpaw/codegen/all.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/all.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/any1.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/all.h + /home/kdee/Desktop/miSim/aerpaw/codegen/any1.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/any1.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/atan2d.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/any1.h + /home/kdee/Desktop/miSim/aerpaw/codegen/atan2d.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/applyToMultipleDims.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/coder_array.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/applyToMultipleDims.h + /home/kdee/Desktop/miSim/aerpaw/codegen/coder_bounded_array.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/atan2d.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/colon.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/atan2d.h + /home/kdee/Desktop/miSim/aerpaw/codegen/colon.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/coder_array.h + /home/kdee/Desktop/miSim/aerpaw/codegen/computeFirstOrderOpt.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/coder_bounded_array.h + /home/kdee/Desktop/miSim/aerpaw/codegen/computeFirstOrderOpt.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/colon.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/computeFval.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/colon.h + /home/kdee/Desktop/miSim/aerpaw/codegen/computeFval.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/computeFirstOrderOpt.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/computeFval_ReuseHx.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/computeFirstOrderOpt.h + /home/kdee/Desktop/miSim/aerpaw/codegen/computeFval_ReuseHx.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/computeFval.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/computeGrad_StoreHx.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/computeFval.h + /home/kdee/Desktop/miSim/aerpaw/codegen/computeGrad_StoreHx.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/computeFval_ReuseHx.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/computeQ_.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/computeFval_ReuseHx.h + /home/kdee/Desktop/miSim/aerpaw/codegen/computeQ_.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/computeGrad_StoreHx.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/compute_deltax.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/computeGrad_StoreHx.h + /home/kdee/Desktop/miSim/aerpaw/codegen/compute_deltax.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/computeQ_.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/compute_lambda.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/computeQ_.h + /home/kdee/Desktop/miSim/aerpaw/codegen/compute_lambda.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/compute_deltax.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/cone.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/compute_deltax.h + /home/kdee/Desktop/miSim/aerpaw/codegen/cone.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/compute_lambda.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/constrainMotion.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/compute_lambda.h + /home/kdee/Desktop/miSim/aerpaw/codegen/constrainMotion.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/cone.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/controller.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/cone.h + /home/kdee/Desktop/miSim/aerpaw/codegen/controller.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/constrainMotion.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/controller_data.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/constrainMotion.h + /home/kdee/Desktop/miSim/aerpaw/codegen/controller_data.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/controller.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/controller_initialize.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/controller.h + /home/kdee/Desktop/miSim/aerpaw/codegen/controller_initialize.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/controller_data.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/controller_internal_types.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/controller_data.h + /home/kdee/Desktop/miSim/aerpaw/codegen/controller_terminate.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/controller_initialize.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/controller_terminate.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/controller_initialize.h + /home/kdee/Desktop/miSim/aerpaw/codegen/controller_types.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/controller_internal_types.h + /home/kdee/Desktop/miSim/aerpaw/codegen/countsort.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/controller_terminate.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/countsort.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/controller_terminate.h + /home/kdee/Desktop/miSim/aerpaw/codegen/deleteColMoveEnd.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/controller_types.h + /home/kdee/Desktop/miSim/aerpaw/codegen/deleteColMoveEnd.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/countsort.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/dot.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/countsort.h + /home/kdee/Desktop/miSim/aerpaw/codegen/dot.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/deleteColMoveEnd.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/driver.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/deleteColMoveEnd.h + /home/kdee/Desktop/miSim/aerpaw/codegen/driver.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/dot.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/exp.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/dot.h + /home/kdee/Desktop/miSim/aerpaw/codegen/exp.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/driver.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/eye.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/driver.h + /home/kdee/Desktop/miSim/aerpaw/codegen/eye.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/exp.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/factorQR.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/exp.h + /home/kdee/Desktop/miSim/aerpaw/codegen/factorQR.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/eye.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/feasibleX0ForWorkingSet.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/eye.h + /home/kdee/Desktop/miSim/aerpaw/codegen/feasibleX0ForWorkingSet.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/factorQR.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/feasibleratiotest.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/factorQR.h + /home/kdee/Desktop/miSim/aerpaw/codegen/feasibleratiotest.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/feasibleX0ForWorkingSet.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/findHelper.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/feasibleX0ForWorkingSet.h + /home/kdee/Desktop/miSim/aerpaw/codegen/findHelper.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/feasibleratiotest.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/fullColLDL2_.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/feasibleratiotest.h + /home/kdee/Desktop/miSim/aerpaw/codegen/fullColLDL2_.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/find.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/guidance_step.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/find.h + /home/kdee/Desktop/miSim/aerpaw/codegen/guidance_step.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/fullColLDL2_.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/iterate.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/fullColLDL2_.h + /home/kdee/Desktop/miSim/aerpaw/codegen/iterate.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/guidance_step.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/ixfun.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/guidance_step.h + /home/kdee/Desktop/miSim/aerpaw/codegen/ixfun.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/iterate.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/lesserNeighbor.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/iterate.h + /home/kdee/Desktop/miSim/aerpaw/codegen/lesserNeighbor.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/ixamax.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/maxConstraintViolation.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/ixamax.h + /home/kdee/Desktop/miSim/aerpaw/codegen/maxConstraintViolation.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/ixfun.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/meshgrid.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/ixfun.h + /home/kdee/Desktop/miSim/aerpaw/codegen/meshgrid.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/lesserNeighbor.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/miSim.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/lesserNeighbor.h + /home/kdee/Desktop/miSim/aerpaw/codegen/miSim.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/linearForm_.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/minOrMax.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/linearForm_.h + /home/kdee/Desktop/miSim/aerpaw/codegen/minOrMax.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/maxConstraintViolation.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/mpower.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/maxConstraintViolation.h + /home/kdee/Desktop/miSim/aerpaw/codegen/mpower.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/meshgrid.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/norm.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/meshgrid.h + /home/kdee/Desktop/miSim/aerpaw/codegen/norm.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/miSim.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/partition.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/miSim.h + /home/kdee/Desktop/miSim/aerpaw/codegen/partition.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/minOrMax.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/phaseone.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/minOrMax.h + /home/kdee/Desktop/miSim/aerpaw/codegen/phaseone.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/mpower.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/quadprog.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/mpower.h + /home/kdee/Desktop/miSim/aerpaw/codegen/quadprog.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/nchoosek.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/rectangularPrism.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/nchoosek.h + /home/kdee/Desktop/miSim/aerpaw/codegen/rectangularPrism.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/norm.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/removeConstr.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/norm.h + /home/kdee/Desktop/miSim/aerpaw/codegen/removeConstr.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/partition.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/repmat.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/partition.h + /home/kdee/Desktop/miSim/aerpaw/codegen/repmat.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/phaseone.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/rtGetInf.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/phaseone.h + /home/kdee/Desktop/miSim/aerpaw/codegen/rtGetInf.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/quadprog.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/rtGetNaN.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/quadprog.h + /home/kdee/Desktop/miSim/aerpaw/codegen/rtGetNaN.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/ratiotest.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/rt_defines.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/ratiotest.h + /home/kdee/Desktop/miSim/aerpaw/codegen/rt_nonfinite.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/rectangularPrism.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/rt_nonfinite.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/rectangularPrism.h + /home/kdee/Desktop/miSim/aerpaw/codegen/rtwtypes.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/removeConstr.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/run.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/removeConstr.h + /home/kdee/Desktop/miSim/aerpaw/codegen/run.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/repmat.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/sensingObjective.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/repmat.h + /home/kdee/Desktop/miSim/aerpaw/codegen/sensingObjective.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/rtGetInf.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/sensorPerformance.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/rtGetInf.h + /home/kdee/Desktop/miSim/aerpaw/codegen/sensorPerformance.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/rtGetNaN.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/setProblemType.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/rtGetNaN.h + /home/kdee/Desktop/miSim/aerpaw/codegen/setProblemType.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/rt_defines.h + /home/kdee/Desktop/miSim/aerpaw/codegen/sigmoidSensor.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/rt_nonfinite.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/sigmoidSensor.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/rt_nonfinite.h + /home/kdee/Desktop/miSim/aerpaw/codegen/sort.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/rtwtypes.h + /home/kdee/Desktop/miSim/aerpaw/codegen/sort.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/run.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/spherical.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/run.h + /home/kdee/Desktop/miSim/aerpaw/codegen/spherical.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/sensingObjective.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/sprintf.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/sensingObjective.h + /home/kdee/Desktop/miSim/aerpaw/codegen/sprintf.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/sensorPerformance.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/squareQ_appendCol.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/sensorPerformance.h + /home/kdee/Desktop/miSim/aerpaw/codegen/squareQ_appendCol.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/setProblemType.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/string1.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/setProblemType.h + /home/kdee/Desktop/miSim/aerpaw/codegen/string1.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/sigmoidSensor.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/sum.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/sigmoidSensor.h + /home/kdee/Desktop/miSim/aerpaw/codegen/sum.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/sort.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/triu.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/sort.h + /home/kdee/Desktop/miSim/aerpaw/codegen/triu.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/spherical.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/unique.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/spherical.h + /home/kdee/Desktop/miSim/aerpaw/codegen/unique.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/sprintf.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/vecnorm.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/sprintf.h + /home/kdee/Desktop/miSim/aerpaw/codegen/vecnorm.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/squareQ_appendCol.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/xgemv.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/squareQ_appendCol.h + /home/kdee/Desktop/miSim/aerpaw/codegen/xgemv.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/string1.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/xgeqp3.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/string1.h + /home/kdee/Desktop/miSim/aerpaw/codegen/xgeqp3.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/sum.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/xnrm2.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/sum.h + /home/kdee/Desktop/miSim/aerpaw/codegen/xnrm2.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/triu.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/xrotg.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/triu.h + /home/kdee/Desktop/miSim/aerpaw/codegen/xrotg.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/unique.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/xzgeqp3.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/unique.h + /home/kdee/Desktop/miSim/aerpaw/codegen/xzgeqp3.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/vecnorm.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/xzlarf.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/vecnorm.h + /home/kdee/Desktop/miSim/aerpaw/codegen/xzlarf.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/xgemv.cpp + /home/kdee/Desktop/miSim/aerpaw/codegen/xzlarfg.cpp GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/xgemv.h + /home/kdee/Desktop/miSim/aerpaw/codegen/xzlarfg.h GENERATED_SOURCE - /home/kdee/Desktop/miSim/aerpaw/codegen/xgeqp3.cpp + /home/kdee/matlab/R2026a/extern/include/tmwtypes.h GENERATED_SOURCE - - /home/kdee/Desktop/miSim/aerpaw/codegen/xgeqp3.h - - GENERATED_SOURCE - - - - /home/kdee/Desktop/miSim/aerpaw/codegen/xnrm2.cpp - - GENERATED_SOURCE - - - - /home/kdee/Desktop/miSim/aerpaw/codegen/xnrm2.h - - GENERATED_SOURCE - - - - /home/kdee/Desktop/miSim/aerpaw/codegen/xrotg.cpp - - GENERATED_SOURCE - - - - /home/kdee/Desktop/miSim/aerpaw/codegen/xrotg.h - - GENERATED_SOURCE - - - - /home/kdee/Desktop/miSim/aerpaw/codegen/xzgeqp3.cpp - - GENERATED_SOURCE - - - - /home/kdee/Desktop/miSim/aerpaw/codegen/xzgeqp3.h - - GENERATED_SOURCE - - - - /home/kdee/Desktop/miSim/aerpaw/codegen/xzlarf.cpp - - GENERATED_SOURCE - - - - /home/kdee/Desktop/miSim/aerpaw/codegen/xzlarf.h - - GENERATED_SOURCE - - - - /home/kdee/Desktop/miSim/aerpaw/codegen/xzlarfg.cpp - - GENERATED_SOURCE - - - - /home/kdee/Desktop/miSim/aerpaw/codegen/xzlarfg.h - - GENERATED_SOURCE - - - - /home/kdee/matlab/R2025a/extern/include/tmwtypes.h - - GENERATED_SOURCE - - /home/kdee/Desktop/miSim/aerpaw/codegen/examples/main.cpp GENERATED_SOURCE - + /home/kdee/Desktop/miSim/aerpaw/codegen/examples/main.h @@ -1188,13 +981,13 @@ true - 2026-04-07T22:43:01 + 2026-06-02T20:38:33.631381 - + buildFolderPath @@ -1211,21 +1004,14 @@ - genCodeOnly + generateReport - true - generateReport - - - - - targetLang - + C++ @@ -1234,7 +1020,7 @@ - + diff --git a/aerpaw/controller.m b/aerpaw/controller.m index 4d088a6..da202a4 100644 --- a/aerpaw/controller.m +++ b/aerpaw/controller.m @@ -127,37 +127,72 @@ if ~coder.target('MATLAB') end % ------------------------------------------------------------------------------- -% Waypoint loop: send each waypoint to all clients, wait for all to arrive -for w = 1:numWaypoints - % Send TARGET for waypoint w to each client - for i = 1:numClients - % Targets are grouped by client: client i's waypoints are at rows - % (i-1)*numWaypoints+1 through i*numWaypoints - targetIdx = (i - 1) * numWaypoints + w; - target = targets(targetIdx, :); - - if coder.target('MATLAB') +% Waypoint loop +if coder.target('MATLAB') + % Simulation: simple lockstep (collisions are not modelled here). + for w = 1:numWaypoints + for i = 1:numClients + targetIdx = (i - 1) * numWaypoints + w; + target = targets(targetIdx, :); disp(['Sending TARGET to client ', num2str(i), ' (waypoint ', num2str(w), '): ', ... num2str(target(1)), ',', num2str(target(2)), ',', num2str(target(3))]); - else - coder.ceval('sendTarget', int32(i), coder.ref(target)); end - end - - % Wait for ACK from all clients - if coder.target('MATLAB') disp('Waiting for ACK from all clients...'); - else - coder.ceval('waitForAllMessageType', int32(numClients), ... - int32(MESSAGE_TYPE.ACK)); - end - - % Wait for READY from all clients (all arrived at waypoint w) - if coder.target('MATLAB') disp(['All UAVs arrived at waypoint ', num2str(w)]); - else - coder.ceval('waitForAllMessageType', int32(numClients), ... - int32(MESSAGE_TYPE.READY)); + end +else + % ---- Staggered flyout ----------------------------------------------------- + % Separate the two UAVs in TIME so they are never doing the same vertical + % climb at the same moment over their ~10 m-separated pads (which can trip + % the proximity guard on a spurious GPS sample). The UAV that transits + % highest leads by exactly one event. + % + % Events per UAV: 1 = TAKEOFF (25 m), 2 = WP1 (climb at pad), 3 = WP2 + % (traverse), 4 = WP3 (descend). At each step the higher UAV does event + % `step` while the lower UAV does event `step-1`; both are dispatched, then + % we collect ACK and then READY (arrival) from each active UAV. + numEvents = double(numWaypoints) + 1; % 3 waypoints + 1 takeoff + for step = 1:(numEvents + 1) + hEvent = step; + lEvent = step - 1; + hActive = (hEvent >= 1) && (hEvent <= numEvents); + lActive = (lEvent >= 1) && (lEvent <= numEvents); + + % --- Dispatch this step's event to each active UAV --- + if hActive + if hEvent == 1 + coder.ceval('sendMessageType', higherIdx, int32(MESSAGE_TYPE.TAKEOFF)); + else + hRow = (double(higherIdx) - 1) * double(numWaypoints) + (hEvent - 1); + hTarget = targets(hRow, :); + coder.ceval('sendTarget', higherIdx, coder.ref(hTarget)); + end + end + if lActive + if lEvent == 1 + coder.ceval('sendMessageType', lowerIdx, int32(MESSAGE_TYPE.TAKEOFF)); + else + lRow = (double(lowerIdx) - 1) * double(numWaypoints) + (lEvent - 1); + lTarget = targets(lRow, :); + coder.ceval('sendTarget', lowerIdx, coder.ref(lTarget)); + end + end + + % --- Collect ACK (command received) from each active UAV --- + if hActive + coder.ceval('waitForClientMessageType', higherIdx, int32(MESSAGE_TYPE.ACK)); + end + if lActive + coder.ceval('waitForClientMessageType', lowerIdx, int32(MESSAGE_TYPE.ACK)); + end + + % --- Collect READY (arrived at event) from each active UAV --- + if hActive + coder.ceval('waitForClientMessageType', higherIdx, int32(MESSAGE_TYPE.READY)); + end + if lActive + coder.ceval('waitForClientMessageType', lowerIdx, int32(MESSAGE_TYPE.READY)); + end end end @@ -262,17 +297,48 @@ if ~coder.target('MATLAB') retBase = double(iUAV - 1) * double(NUM_RETURN_WP); returnTargets(retBase + 1, :) = [positions(iUAV,1), positions(iUAV,2), returnTransitAlt(iUAV)]; returnTargets(retBase + 2, :) = [initialPositions(iUAV,1), initialPositions(iUAV,2), returnTransitAlt(iUAV)]; - returnTargets(retBase + 3, :) = initialPositions(iUAV, :); + % Final return waypoint: pad XY from the pre-takeoff GPS query, but a fixed + % 25 m up. That query ran while the UAV was on the ground, so its ENU + % up-component is ~0 (often slightly negative) — using it directly commands + % a negative altitude. RTL/LAND perform the actual touchdown from here. + returnTargets(retBase + 3, :) = [initialPositions(iUAV,1), initialPositions(iUAV,2), 25.0]; end - for w = 1:NUM_RETURN_WP - for i = 1:numClients - retIdx = double(i - 1) * double(NUM_RETURN_WP) + w; - retTarget = returnTargets(retIdx, :); - coder.ceval('sendTarget', int32(i), coder.ref(retTarget)); + % Staggered flyback: same higher-leads-by-one pattern as the flyout, but + % over the 3 return waypoints only (the UAVs are already airborne, so there + % is no TAKEOFF event). The higher UAV climbs/traverses/descends one event + % ahead of the lower, so they never share the same descent over their pads. + numReturnEvents = double(NUM_RETURN_WP); % 3 + for step = 1:(numReturnEvents + 1) + hEvent = step; + lEvent = step - 1; + hActive = (hEvent >= 1) && (hEvent <= numReturnEvents); + lActive = (lEvent >= 1) && (lEvent <= numReturnEvents); + + if hActive + hRow = (double(higherRetIdx) - 1) * double(NUM_RETURN_WP) + hEvent; + hTarget = returnTargets(hRow, :); + coder.ceval('sendTarget', higherRetIdx, coder.ref(hTarget)); + end + if lActive + lRow = (double(lowerRetIdx) - 1) * double(NUM_RETURN_WP) + lEvent; + lTarget = returnTargets(lRow, :); + coder.ceval('sendTarget', lowerRetIdx, coder.ref(lTarget)); + end + + if hActive + coder.ceval('waitForClientMessageType', higherRetIdx, int32(MESSAGE_TYPE.ACK)); + end + if lActive + coder.ceval('waitForClientMessageType', lowerRetIdx, int32(MESSAGE_TYPE.ACK)); + end + + if hActive + coder.ceval('waitForClientMessageType', higherRetIdx, int32(MESSAGE_TYPE.READY)); + end + if lActive + coder.ceval('waitForClientMessageType', lowerRetIdx, int32(MESSAGE_TYPE.READY)); end - coder.ceval('waitForAllMessageType', int32(numClients), int32(MESSAGE_TYPE.ACK)); - coder.ceval('waitForAllMessageType', int32(numClients), int32(MESSAGE_TYPE.READY)); end else disp('Altitude-staggered return (simulation): UAVs commanded back to takeoff positions.'); diff --git a/aerpaw/impl/controller_impl.cpp b/aerpaw/impl/controller_impl.cpp index effe0fe..2cf0124 100644 --- a/aerpaw/impl/controller_impl.cpp +++ b/aerpaw/impl/controller_impl.cpp @@ -512,6 +512,7 @@ static const char* messageTypeName(uint8_t msgType) { case 6: return "GUIDANCE_TOGGLE"; case 7: return "REQUEST_POSITION"; case 8: return "POSITION"; + case 9: return "TAKEOFF"; default: return "UNKNOWN"; } } @@ -614,6 +615,33 @@ int waitForAllMessageType(int numClients, int expectedType) { return 1; } +// Wait for a specific message type from a SINGLE client (blocking). +// Used by the staggered flyout/flyback, where the controller waits on one UAV +// while the other is still flying its own (offset) event. Skips past any +// unexpected byte and returns 0 on disconnect/error, 1 on success. +int waitForClientMessageType(int clientId, int expectedType) { + if (clientId <= 0 || clientId > (int)clientSockets.size()) return 0; + int sock = clientSockets[clientId - 1]; + uint8_t expected = (uint8_t)expectedType; + + while (true) { + uint8_t msg; + int len = recv(sock, &msg, 1, MSG_WAITALL); + if (len <= 0) { + std::cerr << logPrefix() << "waitForClientMessageType: client " << clientId + << " disconnected while waiting for " << messageTypeName(expected) << "\n"; + return 0; + } + if (msg == expected) { + std::cout << logPrefix() << "Received " << messageTypeName(expected) + << " from client " << clientId << "\n"; + return 1; + } + std::cerr << logPrefix() << "Unexpected " << messageTypeName(msg) << " from client " + << clientId << " (expected " << messageTypeName(expected) << ")\n"; + } +} + // Broadcast GUIDANCE_TOGGLE to all clients void sendGuidanceToggle(int numClients) { for (int i = 1; i <= numClients; i++) { diff --git a/aerpaw/impl/controller_impl.h b/aerpaw/impl/controller_impl.h index 28b81c3..b569e98 100644 --- a/aerpaw/impl/controller_impl.h +++ b/aerpaw/impl/controller_impl.h @@ -61,6 +61,9 @@ int loadObstacles(const char* filename, int sendMessageType(int clientId, int msgType); int sendTarget(int clientId, const double* coords); int waitForAllMessageType(int numClients, int expectedType); +// Block until a SINGLE client sends expectedType (used by the staggered +// flyout/flyback to wait on one UAV while the other is mid-flight). +int waitForClientMessageType(int clientId, int expectedType); // Guidance loop operations void setGuidanceStep(int step, int totalSteps); // call at the top of each guidance iteration