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

Allow for better input control #188

Merged
merged 3 commits into from
Nov 26, 2024
Merged

Allow for better input control #188

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
ce6ed09
Add better input control selection on vrb.
bperseghetti Nov 25, 2024
2a3c3e7
fix enet topic source guard.
bperseghetti Nov 26, 2024
7d0f11f
Update velocity guards in FSM.
bperseghetti Nov 26, 2024
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
40 changes: 39 additions & 1 deletion app/b3rb/src/fsm.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -95,6 +95,7 @@ struct status_input {
bool input_source_ethernet;
bool input_source_radio_control;
bool topic_timeout;
bool cmd_vel_timeout;
bool topic_status_loss;

// derived booleans
Expand Down Expand Up @@ -126,6 +127,8 @@ struct context {
struct k_thread thread_data;
int64_t now_ticks;
int64_t input_last_ticks;
int64_t cmd_vel_last_ticks;
const int64_t cmd_vel_loss_ticks;
const int64_t input_loss_ticks;
int64_t topic_last_ticks;
const int64_t topic_loss_ticks;
Expand Down Expand Up @@ -167,6 +170,8 @@ static struct context g_ctx = {
.now_ticks = 0,
.input_last_ticks = 0,
.topic_last_ticks = 0,
.cmd_vel_last_ticks = 0,
.cmd_vel_loss_ticks = 1.0 * CONFIG_SYS_CLOCK_TICKS_PER_SEC,
.input_loss_ticks = 1.0 * CONFIG_SYS_CLOCK_TICKS_PER_SEC,
.topic_loss_ticks = 1.0 * CONFIG_SYS_CLOCK_TICKS_PER_SEC,
};
Expand Down Expand Up @@ -244,6 +249,8 @@ static void fsm_compute_input(struct status_input *input, struct context *ctx)
input->topic_source_input =
ctx->status.topic_source == synapse_pb_Status_TopicSource_TOPIC_SOURCE_INPUT;
input->input_timeout = (ctx->now_ticks - ctx->input_last_ticks) > ctx->input_loss_ticks;
input->cmd_vel_timeout =
(ctx->now_ticks - ctx->cmd_vel_last_ticks) > ctx->cmd_vel_loss_ticks;
input->input_status_loss =
ctx->status.input_status == synapse_pb_Status_LinkStatus_STATUS_LOSS;
input->input_source_ethernet =
Expand Down Expand Up @@ -330,6 +337,15 @@ static void fsm_update(synapse_pb_Status *status, const struct status_input *inp
synapse_pb_Status_Mode_MODE_VELOCITY, // post
status->status_message, sizeof(status->status_message), // status
&status->request_seq, &status->request_rejected, // request
1, input->cmd_vel_timeout, "cmd_vel not received"); // guards

transition(&status->mode, // state
input->cmd_vel_timeout, // request
"actuator fallback, cmd_vel not received", // label
synapse_pb_Status_Mode_MODE_VELOCITY, // pre
synapse_pb_Status_Mode_MODE_ACTUATORS, // post
status->status_message, sizeof(status->status_message), // status
&status->request_seq, &status->request_rejected, // request
0); // guards

transition(&status->mode, // state
Expand Down Expand Up @@ -370,7 +386,28 @@ static void fsm_update(synapse_pb_Status *status, const struct status_input *inp
status->status_message, sizeof(status->status_message), // status
&status->request_seq, &status->request_rejected, // request
// guards
1, input->topic_status_loss, "no topic data");
1, input->input_status_loss, "no topic data");

// input source transitions
transition(&status->input_source, // state
input->req.input_source_radio_control, // request
"request input source radio control", // label
STATE_ANY, // pre
synapse_pb_Status_InputSource_INPUT_SOURCE_RADIO_CONTROL, // post
status->status_message, sizeof(status->status_message), // status
&status->request_seq, &status->request_rejected, // request
// guards
1, input->input_status_loss, "no RC input data");

transition(&status->input_source, // state
input->req.input_source_ethernet, // request
"request input source ethernet", // label
STATE_ANY, // pre
synapse_pb_Status_InputSource_INPUT_SOURCE_ETHERNET, // post
status->status_message, sizeof(status->status_message), // status
&status->request_seq, &status->request_rejected, // request
// guards
1, input->input_status_loss, "no enet input data");

// set timestamp
stamp_msg(&status->stamp, k_uptime_ticks());
Expand Down Expand Up @@ -459,6 +496,7 @@ static void b3rb_fsm_run(void *p0, void *p1, void *p2)

if (in->update_cmd_vel_ethernet) {
zros_sub_update(&ctx->sub_cmd_vel_ethernet);
ctx->cmd_vel_last_ticks = ctx->now_ticks;
}

// update input from correct source
Expand Down
12 changes: 12 additions & 0 deletions app/b3rb/src/input_mapping.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -51,4 +51,16 @@ void input_request_compute(struct input_request *req, const synapse_pb_Input *in
LOG_INF("request lights off");
req->lights_on = false;
}
if (input->channel[CH_VRB_CCW] > 0.5f && !req->input_source_radio_control) {
req->input_source_radio_control = true;
req->input_source_ethernet = false;
LOG_INF("request rc input");
} else if (input->channel[CH_VRB_CCW] < -0.5f && !req->input_source_ethernet) {
LOG_INF("request ethernet input");
req->input_source_ethernet = true;
req->input_source_radio_control = false;
} else {
req->input_source_ethernet = false;
req->input_source_radio_control = false;
}
}
2 changes: 2 additions & 0 deletions app/b3rb/src/input_mapping.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,8 @@ int three_position_switch(float val);
struct input_request {
bool arm;
bool disarm;
bool input_source_radio_control;
bool input_source_ethernet;
bool topic_source_input;
bool topic_source_ethernet;
bool mode_actuators;
Expand Down
Loading