Skip to content

Commit

Permalink
updated some displays
Browse files Browse the repository at this point in the history
  • Loading branch information
@DanHert
DanHert committed Nov 27, 2023
1 parent 7589af6 commit 19e3bac
Show file tree
Hide file tree
Showing 2 changed files with 105 additions and 51 deletions.
53 changes: 33 additions & 20 deletions src/data_acquisition.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -895,28 +895,34 @@ void Acquisition::prefillUavStatus() {
uav_status_.odom_hdg = 0.0;
uav_status_.odom_frame = "N/A";
uav_status_.odom_estimators.clear();
uav_status_.max_flight_z = 0.0;
uav_status_.cpu_load = 0.0;
uav_status_.cpu_ghz = 0.0;
uav_status_.cpu_temperature = 0.0;
uav_status_.free_ram = 0.0;
uav_status_.free_hdd = 0.0;
uav_status_.hw_api_hz = 0.0;
uav_status_.hw_api_armed = false;
uav_status_.hw_api_mode = "N/A";
uav_status_.hw_api_gnss_ok = false;
uav_status_.hw_api_gnss_qual = 0.0;
uav_status_.battery_volt = 0.0;
uav_status_.battery_curr = 0.0;
uav_status_.thrust = 0.0;
uav_status_.mass_estimate = 0.0;
uav_status_.mass_set = 0.0;
uav_status_.horizontal_estimator = "N/A";
uav_status_.vertical_estimator = "N/A";
uav_status_.heading_estimator = "N/A";
uav_status_.agl_estimator = "N/A";
uav_status_.max_flight_z = 0.0;
uav_status_.cpu_load = 0.0;
uav_status_.cpu_ghz = 0.0;
uav_status_.cpu_temperature = 0.0;
uav_status_.free_ram = 0.0;
uav_status_.free_hdd = 0.0;
uav_status_.hw_api_hz = 0.0;
uav_status_.hw_api_armed = false;
uav_status_.hw_api_mode = "N/A";
uav_status_.hw_api_gnss_ok = false;
uav_status_.hw_api_gnss_qual = 0.0;
uav_status_.battery_volt = 0.0;
uav_status_.battery_curr = 0.0;
uav_status_.thrust = 0.0;
uav_status_.mass_estimate = 0.0;
uav_status_.mass_set = 0.0;
uav_status_.custom_topics.clear();
uav_status_.custom_string_outputs.clear();
uav_status_.flying_normally = false;
uav_status_.null_tracker = true;
uav_status_.have_goal = false;
uav_status_.callbacks_enabled = false;
uav_status_.flying_normally = false;
uav_status_.null_tracker = true;
uav_status_.have_goal = false;
uav_status_.rc_mode = false;
uav_status_.tracking_trajectory = false;
uav_status_.callbacks_enabled = false;
}

//}
Expand Down Expand Up @@ -1239,6 +1245,11 @@ void Acquisition::estimationDiagCallback(const mrs_msgs::EstimationDiagnosticsCo
std::swap(uav_status_.odom_estimators[0], uav_status_.odom_estimators[i]);
}
}

uav_status_.horizontal_estimator = msg->estimator_horizontal;
uav_status_.vertical_estimator = msg->estimator_vertical;
uav_status_.heading_estimator = msg->estimator_heading;
uav_status_.agl_estimator = msg->estimator_agl_height;
}
}

Expand Down Expand Up @@ -1452,6 +1463,8 @@ void Acquisition::controlManagerCallback(const mrs_msgs::ControlManagerDiagnosti

uav_status_.flying_normally = msg->flying_normally;
uav_status_.have_goal = msg->tracker_status.have_goal;
uav_status_.rc_mode = msg->rc_mode;
uav_status_.tracking_trajectory = msg->tracker_status.tracking_trajectory;
uav_status_.callbacks_enabled = msg->tracker_status.callbacks_enabled;

if (msg->active_tracker == "NullTracker") {
Expand Down
103 changes: 72 additions & 31 deletions src/status.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -96,6 +96,9 @@ class Status {
double general_info_window_rate_ = 1;
double generic_topic_window_rate_ = 1;

bool increment_counter_ = false;
int estimator_display_counter_ = 0;

int _service_num_calls_ = 20;
double _service_delay_ = 0.1;

Expand Down Expand Up @@ -507,6 +510,10 @@ void Status::statusTimerFast([[maybe_unused]] const ros::TimerEvent& event) {
}
}

if (estimator_display_counter_ == 3) {
estimator_display_counter_ = 0;
}

{
mrs_lib::Routine profiler_routine = profiler_.createRoutine("uavStateHandler");
uavStateHandler(uav_state_window_);
Expand Down Expand Up @@ -666,6 +673,9 @@ void Status::statusTimerSlow([[maybe_unused]] const ros::TimerEvent& event) {
return;
}

increment_counter_ = !increment_counter_;
estimator_display_counter_ += int(increment_counter_);

{
mrs_lib::Routine profiler_routine = profiler_.createRoutine("hwApiStateHander");
hwApiStateHander(hw_api_state_window_);
Expand Down Expand Up @@ -1910,7 +1920,11 @@ void Status::uavStateHandler(WINDOW* win) {
double cmd_hdg;

std::string odom_frame;
std::string curr_estimator;
std::string main_estimator;
std::string horizontal_estimator;
std::string vertical_estimator;
std::string heading_estimator;
std::string agl_estimator;
double max_flight_z;

bool null_tracker;
Expand All @@ -1930,7 +1944,12 @@ void Status::uavStateHandler(WINDOW* win) {
cmd_z = uav_status_.cmd_z;
cmd_hdg = uav_status_.cmd_hdg;

uav_status_.odom_estimators.empty() ? curr_estimator = "NONE" : curr_estimator = uav_status_.odom_estimators[0];
uav_status_.odom_estimators.empty() ? main_estimator = "NONE" : main_estimator = uav_status_.odom_estimators[0];

horizontal_estimator = uav_status_.horizontal_estimator;
vertical_estimator = uav_status_.vertical_estimator;
heading_estimator = uav_status_.heading_estimator;
agl_estimator = uav_status_.agl_estimator;

max_flight_z = uav_status_.max_flight_z;

Expand Down Expand Up @@ -1970,7 +1989,7 @@ void Status::uavStateHandler(WINDOW* win) {
printLimitedDouble(win, 3, 1, "%4.0f", state_z, 1000);
printLimitedDouble(win, 4, 1, "%4.1f", heading, 1000);

printLimitedString(win, 1, 6, curr_estimator, 2);
printLimitedString(win, 1, 6, main_estimator, 2);
}
}

Expand Down Expand Up @@ -2037,9 +2056,29 @@ void Status::uavStateHandler(WINDOW* win) {
wattron(win, COLOR_PAIR(color));
}

int pos = odom_frame.find("/") + 1;
printLimitedString(win, 1, 11, odom_frame.substr(pos, odom_frame.length()), 14);
printLimitedString(win, 4, 11, curr_estimator, 14);
/* horizontal_estimator = uav_status_.horizontal_estimator; */
/* vertical_estimator = uav_status_.vertical_estimator; */
/* heading_estimator = uav_status_.heading_estimator; */
/* agl_estimator = uav_status_.agl_estimator; */
printLimitedString(win, 1, 11, main_estimator, 14);

switch (estimator_display_counter_) {
case 0: {
printLimitedString(win, 2, 11, "hor: " + horizontal_estimator, 14);
break;
}
case 1: {
printLimitedString(win, 2, 11, "ver: " + vertical_estimator, 14);
break;
}
case 2: {
printLimitedString(win, 2, 11, "hdg: " + heading_estimator, 14);
break;
}
}


printLimitedString(win, 4, 11, "ag: " + agl_estimator, 14);

double dist_to_max_z = max_flight_z - state_z;
if (dist_to_max_z < 0.0) {
Expand Down Expand Up @@ -2082,7 +2121,9 @@ void Status::controlManagerHandler(WINDOW* win) {
string curr_gains;
string curr_constraints;
bool callbacks_enabled;
bool has_goal;
bool rc_mode;
bool have_goal;
bool tracking_trajectory;

{
std::scoped_lock lock(mutex_status_msg_);
Expand All @@ -2094,9 +2135,11 @@ void Status::controlManagerHandler(WINDOW* win) {
uav_status_.gains.empty() ? curr_gains = "NONE" : curr_gains = uav_status_.gains[0];
uav_status_.constraints.empty() ? curr_constraints = "NONE" : curr_constraints = uav_status_.constraints[0];

callbacks_enabled = uav_status_.callbacks_enabled;
has_goal = uav_status_.have_goal;
null_tracker = uav_status_.null_tracker;
callbacks_enabled = uav_status_.callbacks_enabled;
rc_mode = uav_status_.rc_mode;
have_goal = uav_status_.have_goal;
tracking_trajectory = uav_status_.tracking_trajectory;
null_tracker = uav_status_.null_tracker;
}

werase(win);
Expand Down Expand Up @@ -2207,38 +2250,34 @@ void Status::controlManagerHandler(WINDOW* win) {
wattron(win, COLOR_PAIR(color));
}

if (!callbacks_enabled) {
if (rc_mode) {
wattron(win, A_BLINK);
wattron(win, COLOR_PAIR(RED));
mvwprintw(win, 1, 18, "RC_MODE");
wattroff(win, COLOR_PAIR(RED));
wattroff(win, A_BLINK);

} else if (!callbacks_enabled) {
wattron(win, COLOR_PAIR(RED));
mvwprintw(win, 1, 20, "NO_CB");
wattroff(win, COLOR_PAIR(RED));
}

if (tracking_trajectory) {
wattron(win, COLOR_PAIR(GREEN));
mvwprintw(win, 2, 21, "TRAJ");
wattroff(win, COLOR_PAIR(GREEN));

if (has_goal) {

} else if (have_goal) {
wattron(win, COLOR_PAIR(GREEN));
mvwprintw(win, 2, 22, "FLY");
mvwprintw(win, 2, 21, "GOTO");
wattroff(win, COLOR_PAIR(GREEN));

} else {

wattron(win, COLOR_PAIR(YELLOW));
mvwprintw(win, 2, 21, "IDLE");
wattroff(win, COLOR_PAIR(YELLOW));
}

/* if (rate == 0) { */
/* printNoData(win, 1, 2 + curr_controller.length()); */
/* } else { */
/* printLimitedString(win, 1, 2 + curr_controller.length(), curr_gains, 10); */
/* } */

/* if (rate == 0) { */
/* printNoData(win, 2, 2 + curr_tracker.length()); */
/* } else { */
/* printLimitedString(win, 2, 2 + curr_tracker.length(), curr_constraints, 10); */
/* } */
}

//}
Expand Down Expand Up @@ -2841,10 +2880,12 @@ void Status::prefillUavStatus() {
uav_status_.mass_set = 0.0;
uav_status_.custom_topics.clear();
uav_status_.custom_string_outputs.clear();
uav_status_.flying_normally = false;
uav_status_.null_tracker = true;
uav_status_.have_goal = false;
uav_status_.callbacks_enabled = false;
uav_status_.flying_normally = false;
uav_status_.null_tracker = true;
uav_status_.have_goal = false;
uav_status_.rc_mode = false;
uav_status_.tracking_trajectory = false;
uav_status_.callbacks_enabled = false;
}

//}
Expand Down

0 comments on commit 19e3bac

Please sign in to comment.