diff --git a/include/simulation/Serial_Sim.h b/include/simulation/Serial_Sim.h index 949fe27..efad5b5 100644 --- a/include/simulation/Serial_Sim.h +++ b/include/simulation/Serial_Sim.h @@ -160,10 +160,13 @@ class SerialSim { return value; } - // Send ACK (0x06) alongside the current state to the serial port + // Send ACK alongside the current state to the serial port void ack(){ serial->write(stateMachine->getState()); - serial->write(0x06); + // Ack will be a series of 0xaa, 0xbb, 0xcc + serial->write(0xaa); + serial->write(0xbb); + serial->write(0xcc); } private: diff --git a/include/simulation/simulation.py b/include/simulation/simulation.py index ccc20c6..60f9685 100644 --- a/include/simulation/simulation.py +++ b/include/simulation/simulation.py @@ -2,9 +2,18 @@ import time import pandas as pd import matplotlib.pyplot as plt +import sys + +# Serial port is the first arg +if len(sys.argv) <= 1: + raise ValueError("Please provide the serial port as the first argument, e.g. 'COM3' or '/dev/ttyACM0'" + " This is the same port you use to monitor or send commands via the UARTCommandHandler") + + +serial_port = sys.argv[1] # Set up the serial connection (adjust the port to your system) -ser = serial.Serial('/dev/ttyACM0', 115200, timeout=1) # Replace 'COM3' with your port name +ser = serial.Serial(serial_port, 115200, timeout=1) # Replace 'COM3' with your port name time.sleep(2) # Allow time for the connection to be established # Clear the buffer @@ -105,18 +114,24 @@ def stream_csv_data(csv_file): # Wait for acknowledgment before continuing buffer = b'' + wait_for_ack_start_time = time.time() while True: if ser.in_waiting > 0: buffer += ser.read_all() - if b'\x06' in buffer: - # The byte after ack is the current state of the rocket - loc = buffer.index(b'\x06') + # Check for the series of 0xaa, 0xbb, 0xcc in the buffer to confirm acknowledgment + if b'\xaa\xbb\xcc' in buffer: + # The byte right before the ack series is the state + loc = buffer.find(b'\xaa\xbb\xcc') print("Received acknowledgment: ", buffer) state = buffer[loc - 1] break else: # pass print("Waiting for acknowledgment got: ", buffer) + if time.time() - wait_for_ack_start_time > 5: # Timeout after 5 seconds + print("Timeout waiting for acknowledgment, resending data.") + ser.write(interpolated_data.encode()) + wait_for_ack_start_time = time.time() # Reset the timer sent_data.append(interpolated_data.split(',') + [state]) @@ -155,6 +170,9 @@ def stream_csv_data(csv_file): plt.legend() plt.show() + # Save the plot as an image + plt.savefig('serial_simulation_plot.png') + try: # Wait for the start command before streaming wait_for_start_command() diff --git a/include/simulation/simulation_requirements.txt b/include/simulation/simulation_requirements.txt new file mode 100644 index 0000000..aae2ddb --- /dev/null +++ b/include/simulation/simulation_requirements.txt @@ -0,0 +1,3 @@ +matplotlib +pyserial +pandas \ No newline at end of file diff --git a/include/state_estimation/States.h b/include/state_estimation/States.h index 5fe7140..22ac826 100644 --- a/include/state_estimation/States.h +++ b/include/state_estimation/States.h @@ -10,16 +10,16 @@ // apogee is an event that causes a transition from STATE_ASCENT to STATE_DESCENT, but it is not a state itself. // The state machines are in charge of detecting these transitions and updating the states. enum FlightState { - STATE_UNARMED, - STATE_ARMED, - STATE_SOFT_ASCENT, - STATE_ASCENT, // Don't use the ascent state if you are already using powered ascent and coast ascent - STATE_POWERED_ASCENT, - STATE_COAST_ASCENT, - STATE_DESCENT, - STATE_DROGUE_DEPLOYED, - STATE_MAIN_DEPLOYED, - STATE_LANDED, + STATE_UNARMED, // 0x00 + STATE_ARMED, // 0x01 + STATE_SOFT_ASCENT, // 0x02 + STATE_ASCENT, // 0x03 Don't use the ascent state if you are already using powered ascent and coast ascent + STATE_POWERED_ASCENT, // 0x04 + STATE_COAST_ASCENT, // 0x05 + STATE_DESCENT, // 0x06 + STATE_DROGUE_DEPLOYED, // 0x07 + STATE_MAIN_DEPLOYED, // 0x08 + STATE_LANDED, // 0x09 }; #endif \ No newline at end of file diff --git a/src/state_estimation/StateMachine.cpp b/src/state_estimation/StateMachine.cpp index 360a0f8..714b75d 100644 --- a/src/state_estimation/StateMachine.cpp +++ b/src/state_estimation/StateMachine.cpp @@ -46,7 +46,9 @@ int StateMachine::update(const AccelerationTriplet& accel, const DataPoint& alt) // Put the data saver into post-launch mode dataSaver->launchDetected(fastLaunchDetector->getLaunchedTime()); } - else if (launchDetector->isLaunched()) { + + // The FLD should always trigger before the LP, but we check for LP launch just in case + if (launchDetector->isLaunched()) { // Change state to ascent state = STATE_ASCENT;