Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Adds a VMUX toggle and updates b2b interface to be nicer #92

Open
wants to merge 2 commits into
base: main
Choose a base branch
from
Open

Adds a VMUX toggle and updates b2b interface to be nicer #92

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
b7902c7
vmux-toggle midas code
mpkarpov-ui Feb 28, 2025
4adeb2c
add comments and add fsm swap
mpkarpov-ui Feb 28, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
51 changes: 47 additions & 4 deletions MIDAS/src/b2b_interface.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,8 +6,9 @@ ErrorCode B2BInterface::init() {
return ErrorCode::NoError;
}



/**
* @brief Transmits the given CameraCommand over I2C / CAN (depending on which interface type is defined)
*/
void CameraB2B::transmit_command(CameraCommand command) {
#ifdef B2B_I2C

Expand All @@ -24,16 +25,25 @@ void CameraB2B::transmit_command(CameraCommand command) {
#endif
}

/**
* @brief Transmits command to toggle on the video transmitter
*/
void CameraB2B::vtx_on() {
transmit_command(CameraCommand::VTX_ON);
vtx_state_ = true;
}

/**
* @brief Transmits command to toggle off the video transmitter
*/
void CameraB2B::vtx_off() {
transmit_command(CameraCommand::VTX_OFF);
vtx_state_ = false;
}

/**
* @brief Transmits command to toggle the video transmitter
*/
void CameraB2B::vtx_toggle() {
if(vtx_state_) {
vtx_off();
Expand All @@ -42,6 +52,31 @@ void CameraB2B::vtx_toggle() {
}
}

/**
* @brief Transmits command to set which camera is currently active on the video multiplexer
*/
void CameraB2B::vmux_set(int cam_select) {
if(cam_select) {
// If cam_select is 1, switch to MUX 2
transmit_command(CameraCommand::MUX_2);
mux_select_ = false;
} else {
// Otherwise switch to MUX 1
transmit_command(CameraCommand::MUX_1);
mux_select_ = true;
}
}

/**
* @brief Transmits command to toggle which camera is currently active on the video multiplexer
*/
void CameraB2B::vmux_toggle() {
vmux_set(!mux_select_);
Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Maybe someone should comment this code

}

/**
* @brief Transmits command to enable power to a camera
*/
void CameraB2B::camera_on(int cam_index) {
switch (cam_index) {
case 0:
Expand All @@ -59,6 +94,11 @@ void CameraB2B::camera_on(int cam_index) {
}
}

/**
* @brief Transmits command to disable power to a camera
*
* If the camera was previously on, it will first stop recording, then power off.
*/
void CameraB2B::camera_off(int cam_index) {
switch (cam_index) {
case 0:
Expand All @@ -70,12 +110,15 @@ void CameraB2B::camera_off(int cam_index) {
cam_state_[1] = false;
break;
default:
Serial.print("B2B camera on -- invalid index ");
Serial.print("B2B camera off -- invalid index ");
Serial.println(cam_index);
break;
}
}

/**
* @brief Transmits command to toggle power to a camera
*/
void CameraB2B::camera_toggle(int cam_index) {
switch (cam_index) {
case 0:
Expand All @@ -93,7 +136,7 @@ void CameraB2B::camera_toggle(int cam_index) {
}
break;
default:
Serial.print("B2B camera on -- invalid index ");
Serial.print("B2B camera toggle -- invalid index ");
Serial.println(cam_index);
break;
}
Expand Down
7 changes: 7 additions & 0 deletions MIDAS/src/b2b_interface.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,9 @@
#error "At least one B2B_I2C or B2B_CAN must be defined"
#endif

#define CAM_1 0
#define CAM_2 1


enum class CameraCommand {
CAMERA1_OFF = 0,
Expand All @@ -36,10 +39,14 @@ struct CameraB2B {
void vtx_off();
void vtx_toggle();

void vmux_set(int cam_select);
void vmux_toggle();

private:
void transmit_command(CameraCommand command);
bool cam_state_[2] = { false, false }; // false: off, true: on
bool vtx_state_ = false;
bool mux_select_ = false; // false: VMUX 1, true: VMUX 2
};

/**
Expand Down
89 changes: 45 additions & 44 deletions MIDAS/src/finite-state-machines/fsm.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -84,49 +84,49 @@ StateEstimate::StateEstimate(RocketData& state) {
*
* @return New FSM State
*/
FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFlags& telem_commands) {
FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFlags& commands) {
//get current time
double current_time = pdTICKS_TO_MS(xTaskGetTickCount());

switch (state) {

case FSMState::STATE_SAFE:
// Deconflict if multip commands are processed
if(telem_commands.should_transition_safe) {
telem_commands.should_transition_pyro_test = false;
telem_commands.should_transition_idle = false;
telem_commands.should_transition_safe = false;
if(commands.should_transition_safe) {
commands.should_transition_pyro_test = false;
commands.should_transition_idle = false;
commands.should_transition_safe = false;
break;
}

// Only switch to STATE_PYRO_TEST if triggered wirelessly
if(telem_commands.should_transition_pyro_test) {
if(commands.should_transition_pyro_test) {
state = FSMState::STATE_PYRO_TEST;
pyro_test_entry_time = current_time;
telem_commands.should_transition_pyro_test = false;
commands.should_transition_pyro_test = false;
}

// Only switch to STATE_IDLE if triggered wirelessly.
if(telem_commands.should_transition_idle) {
if(commands.should_transition_idle) {
state = FSMState::STATE_IDLE;
telem_commands.should_transition_idle = false;
commands.should_transition_idle = false;
}

break;
case FSMState::STATE_PYRO_TEST:

// Force transtion to safe if requested + clear all transition flags.
if(telem_commands.should_transition_safe) {
if(commands.should_transition_safe) {
state = FSMState::STATE_SAFE;
telem_commands.should_transition_pyro_test = false;
telem_commands.should_transition_idle = false;
telem_commands.should_transition_safe = false;
commands.should_transition_pyro_test = false;
commands.should_transition_idle = false;
commands.should_transition_safe = false;
break;
}

// Switch back to STATE_SAFE after a certain amount of time passes
if((current_time - pyro_test_entry_time) > safety_pyro_test_disarm_time) {
telem_commands.should_transition_pyro_test = false;
commands.should_transition_pyro_test = false;
state = FSMState::STATE_SAFE;
}

Expand All @@ -135,11 +135,11 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla
case FSMState::STATE_IDLE:

// Force transtion to safe if requested + clear all transition flags.
if(telem_commands.should_transition_safe) {
if(commands.should_transition_safe) {
state = FSMState::STATE_SAFE;
telem_commands.should_transition_pyro_test = false;
telem_commands.should_transition_idle = false;
telem_commands.should_transition_safe = false;
commands.should_transition_pyro_test = false;
commands.should_transition_idle = false;
commands.should_transition_safe = false;
break;
}

Expand Down Expand Up @@ -221,6 +221,7 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla
if (state_estimate.vertical_speed <= sustainer_coast_to_apogee_vertical_speed_threshold) {
apogee_time = current_time;
state = FSMState::STATE_APOGEE;
commands.FSM_should_swap_camera_feed = true;
}
break;

Expand Down Expand Up @@ -299,11 +300,11 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla

// Check for any telem transitions
// Force transtion to safe if requested + clear all transition flags.
if(telem_commands.should_transition_safe) {
if(commands.should_transition_safe) {
state = FSMState::STATE_SAFE;
telem_commands.should_transition_pyro_test = false;
telem_commands.should_transition_idle = false;
telem_commands.should_transition_safe = false;
commands.should_transition_pyro_test = false;
commands.should_transition_idle = false;
commands.should_transition_safe = false;
}

break;
Expand Down Expand Up @@ -333,59 +334,59 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla
*
* @return New FSM State
*/
FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFlags& telem_commands) {
FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFlags& commands) {
double current_time = pdTICKS_TO_MS(xTaskGetTickCount());

switch (state) {
case FSMState::STATE_SAFE:
// Deconflict if multip commands are processed
if(telem_commands.should_transition_safe) {
telem_commands.should_transition_pyro_test = false;
telem_commands.should_transition_idle = false;
telem_commands.should_transition_safe = false;
if(commands.should_transition_safe) {
commands.should_transition_pyro_test = false;
commands.should_transition_idle = false;
commands.should_transition_safe = false;
break;
}

// Only switch to STATE_PYRO_TEST if triggered wirelessly
if(telem_commands.should_transition_pyro_test) {
if(commands.should_transition_pyro_test) {
state = FSMState::STATE_PYRO_TEST;
pyro_test_entry_time = current_time;
telem_commands.should_transition_pyro_test = false;
commands.should_transition_pyro_test = false;
}

// Only switch to STATE_IDLE if triggered wirelessly.
if(telem_commands.should_transition_idle) {
if(commands.should_transition_idle) {
state = FSMState::STATE_IDLE;
telem_commands.should_transition_idle = false;
commands.should_transition_idle = false;
}

break;
case FSMState::STATE_PYRO_TEST:

// Force transtion to safe if requested + clear all transition flags.
if(telem_commands.should_transition_safe) {
if(commands.should_transition_safe) {
state = FSMState::STATE_SAFE;
telem_commands.should_transition_pyro_test = false;
telem_commands.should_transition_idle = false;
telem_commands.should_transition_safe = false;
commands.should_transition_pyro_test = false;
commands.should_transition_idle = false;
commands.should_transition_safe = false;
break;
}

// Switch back to STATE_SAFE after a certain amount of time passes
if((current_time - pyro_test_entry_time) > safety_pyro_test_disarm_time) {
telem_commands.should_transition_pyro_test = false;
commands.should_transition_pyro_test = false;
state = FSMState::STATE_SAFE;
}

break;

case FSMState::STATE_IDLE:
// Force transtion to safe if requested + clear all transition flags.
if(telem_commands.should_transition_safe) {
if(commands.should_transition_safe) {
state = FSMState::STATE_SAFE;
telem_commands.should_transition_pyro_test = false;
telem_commands.should_transition_idle = false;
telem_commands.should_transition_safe = false;
commands.should_transition_pyro_test = false;
commands.should_transition_idle = false;
commands.should_transition_safe = false;
break;
}

Expand Down Expand Up @@ -508,11 +509,11 @@ FSMState FSM::tick_fsm(FSMState& state, StateEstimate state_estimate, CommandFla

// Check for any telem transitions
// Force transtion to safe if requested + clear all transition flags.
if(telem_commands.should_transition_safe) {
if(commands.should_transition_safe) {
state = FSMState::STATE_SAFE;
telem_commands.should_transition_pyro_test = false;
telem_commands.should_transition_idle = false;
telem_commands.should_transition_safe = false;
commands.should_transition_pyro_test = false;
commands.should_transition_idle = false;
commands.should_transition_safe = false;
}

break;
Expand Down
2 changes: 1 addition & 1 deletion MIDAS/src/finite-state-machines/fsm.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -32,7 +32,7 @@ class FSM {
public:
FSM() = default;

FSMState tick_fsm(FSMState& curr_state, StateEstimate state_estimate, CommandFlags& telem_commands);
FSMState tick_fsm(FSMState& curr_state, StateEstimate state_estimate, CommandFlags& commands);

private:
double launch_time;
Expand Down
3 changes: 3 additions & 0 deletions MIDAS/src/rocket_state.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -157,6 +157,7 @@ class Latency {
* @struct CommandFlags
*
* @brief Stores the status of commands from telemetry as boolean flags, commands are set whenever the corresponding telemetry command comes in.
* Works in both directions, say to toggle states based on FSM transitions
*/
struct CommandFlags {
bool should_reset_kf = false; // CommandType::RESET_KF
Expand All @@ -167,6 +168,8 @@ struct CommandFlags {
bool should_fire_pyro_b = false; // CommandType::FIRE_PYRO_B
bool should_fire_pyro_c = false; // CommandType::FIRE_PYRO_C
bool should_fire_pyro_d = false; // CommandType::FIRE_PYRO_D
// FSM Transition commands
bool FSM_should_swap_camera_feed = false; // Triggered COAST --> APOGEE
};
/**
* @struct RocketData
Expand Down
Loading