Skip to content

Commit

Permalink
[CPP-41]Refactor velocity subtab with new paradigm fix bug in deque. (#…
Browse files Browse the repository at this point in the history 
…27)

* Adding some tracking tab unittests.

* [CPP-41]Refactor velocity subtab with new paradigm fix bug in deque.

* Switch ProtoMsgSender -> MessageSender

* Create type for arc mutex.

* Respond to review requests.
  • Loading branch information
@john-michaelburke
john-michaelburke authored Apr 2, 2021
1 parent 4db5fed commit 50ff730
Show file tree
Hide file tree
Showing 18 changed files with 1,318 additions and 442 deletions.
13 changes: 9 additions & 4 deletions console_backend/benches/cpu_benches.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,10 @@ use std::{
};

extern crate console_backend;
use console_backend::{process_messages, types::SharedState};
use console_backend::{
process_messages,
types::{ClientSender, SharedState},
};

const BENCH_FILEPATH: &str = "./tests/data/piksi-relay.sbp";
const BENCHMARK_TIME_LIMIT: u64 = 10000;
Expand Down Expand Up @@ -45,7 +48,7 @@ fn run_process_messages(file_in_name: &str, failure: bool) {
assert!(iter_count > 0);
});
{
let (client_send, client_recv) = mpsc::channel::<Vec<u8>>();
let (client_send_, client_recv) = mpsc::channel::<Vec<u8>>();
client_recv_tx
.send(client_recv)
.expect("sending client recv handle should succeed");
Expand All @@ -54,8 +57,10 @@ fn run_process_messages(file_in_name: &str, failure: bool) {
}
let messages = sbp::iter_messages(Box::new(fs::File::open(file_in_name).unwrap()));
let shared_state = SharedState::new();
let shared_state = Arc::new(Mutex::new(shared_state));
process_messages::process_messages(messages, &shared_state, client_send);
let client_send = ClientSender {
inner: client_send_,
};
process_messages::process_messages(messages, shared_state, client_send);
}
recv_thread.join().expect("join should succeed");
}
Expand Down
13 changes: 12 additions & 1 deletion console_backend/src/constants.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,8 +6,10 @@ use std::{

use crate::types::Error;

// Tracking Tab constants.
// Common constants.
pub const NUM_POINTS: usize = 200;

// Tracking Signals Tab constants.
pub const NUM_SATELLITES: usize = 60;
pub const TRK_RATE: f64 = 2.0;
pub const GLO_SLOT_SAT_MAX: u8 = 90;
Expand All @@ -30,6 +32,15 @@ pub const QZS_L2C_M: &str = "QZS L2C M";
pub const SBAS_L1: &str = "SBAS L1";
pub const SHOW_LEGEND: &str = "Show Legend";

// Solution Velocity Tab constants.
pub const HORIZONTAL_COLOR: &str = "#E41A1C";
pub const VERTICAL_COLOR: &str = "#377EB8";
pub const MPS: &str = "m/s";
pub const MPH: &str = "mph";
pub const KPH: &str = "kph";
pub const MPS2MPH: f64 = 2.236934;
pub const MPS2KPH: f64 = 3.600000;

#[repr(u8)]
#[derive(Debug, Clone, Copy, Eq, Hash, PartialEq)]
pub enum SignalCodes {
Expand Down
4 changes: 3 additions & 1 deletion console_backend/src/lib.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,9 @@ pub mod console_backend_capnp {
include!(concat!(env!("OUT_DIR"), "/console_backend_capnp.rs"));
}
pub mod constants;
pub mod main_tab;
pub mod process_messages;
pub mod server;
pub mod tracking_tab;
pub mod solution_velocity_tab;
pub mod tracking_signals_tab;
pub mod types;
19 changes: 19 additions & 0 deletions console_backend/src/main_tab.rs
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,19 @@
// use std::sync::{Arc, Mutex};

use crate::solution_velocity_tab::SolutionVelocityTab;
use crate::tracking_signals_tab::TrackingSignalsTab;
use crate::types::*;

pub struct MainTab<'a> {
pub tracking_signals_tab: TrackingSignalsTab,
pub solution_velocity_tab: SolutionVelocityTab<'a>,
}

impl<'a> MainTab<'a> {
pub fn new(shared_state: SharedState) -> MainTab<'a> {
MainTab {
tracking_signals_tab: TrackingSignalsTab::new(shared_state.clone()),
solution_velocity_tab: SolutionVelocityTab::new(shared_state),
}
}
}
132 changes: 21 additions & 111 deletions console_backend/src/process_messages.rs
View file
Original file line number Diff line number Diff line change
@@ -1,143 +1,53 @@
use capnp::message::Builder;
use capnp::serialize;
use ordered_float::OrderedFloat;
use sbp::messages::SBP;
use std::sync::{mpsc, Arc, Mutex};

use crate::console_backend_capnp as m;

use crate::tracking_tab::*;
use crate::types::SharedState;
use crate::main_tab::*;
use crate::types::*;

pub fn process_messages(
messages: impl Iterator<Item = sbp::Result<SBP>>,
shared_state: &Arc<Mutex<SharedState>>,
client_send_clone: mpsc::Sender<Vec<u8>>,
shared_state: SharedState,
client_send_clone: ClientSender,
) {
let mut hpoints: Vec<(f64, OrderedFloat<f64>)> = vec![];
let mut vpoints: Vec<(f64, OrderedFloat<f64>)> = vec![];
let mut tow: f64;
let shared_state_clone = Arc::clone(&shared_state);
let mut tracking_signals = TrackingSignalsTab::new(&shared_state_clone);

let mut main = MainTab::new(shared_state);
for message in messages {
match message {
Ok(SBP::MsgTrackingState(msg)) => {
tracking_signals
.handle_msg_tracking_state(msg.states.clone(), client_send_clone.clone());
main.tracking_signals_tab
.handle_msg_tracking_state(msg.states.clone(), &mut client_send_clone.clone());
}
Ok(SBP::MsgObs(msg)) => {
tracking_signals.handle_obs(
main.tracking_signals_tab.handle_obs(
ObservationMsg::MsgObs(msg.clone()),
client_send_clone.clone(),
&mut client_send_clone.clone(),
);
}
Ok(SBP::MsgMeasurementState(msg)) => {
tracking_signals
.handle_msg_measurement_state(msg.states.clone(), client_send_clone.clone());
main.tracking_signals_tab.handle_msg_measurement_state(
msg.states.clone(),
&mut client_send_clone.clone(),
);
}
Ok(SBP::MsgObsDepA(_msg)) => {
//CPP-85 Unhandled for tracking signals plot tab.
println!("The message type, MsgObsDepA, is not handled in the Tracking->SignalsPlot tab.");
}
Ok(SBP::MsgObsDepB(msg)) => {
tracking_signals.handle_obs(
main.tracking_signals_tab.handle_obs(
ObservationMsg::MsgObsDepB(msg.clone()),
client_send_clone.clone(),
&mut client_send_clone.clone(),
);
}
Ok(SBP::MsgObsDepC(msg)) => {
tracking_signals.handle_obs(
main.tracking_signals_tab.handle_obs(
ObservationMsg::MsgObsDepC(msg.clone()),
client_send_clone.clone(),
&mut client_send_clone.clone(),
);
}

Ok(SBP::MsgVelNED(velocity_ned)) => {
let n = velocity_ned.n as f64;
let e = velocity_ned.e as f64;
let d = velocity_ned.d as f64;

let h_vel = f64::sqrt(f64::powi(n, 2) + f64::powi(e, 2)) / 1000.0;
let v_vel = (-1.0 * d) / 1000.0;

tow = velocity_ned.tow as f64 / 1000.0;

let mut _min = 0.0;
let mut _max = 1.0;
{
let vmin = vpoints
.iter()
.min_by_key(|i| i.1)
.unwrap_or(&(0.0, OrderedFloat(0.0)));
let vmax = vpoints
.iter()
.max_by_key(|i| i.1)
.unwrap_or(&(1.0, OrderedFloat(0.0)));
let hmin = hpoints
.iter()
.min_by_key(|i| i.1)
.unwrap_or(&(0.0, OrderedFloat(0.0)));
let hmax = hpoints
.iter()
.max_by_key(|i| i.1)
.unwrap_or(&(1.0, OrderedFloat(0.0)));

if vmin.1.into_inner() < hmin.1.into_inner() {
_min = vmin.1.into_inner();
} else {
_min = hmin.1.into_inner();
}
if vmax.1.into_inner() > hmax.1.into_inner() {
_max = vmax.1.into_inner();
} else {
_max = hmax.1.into_inner();
}
}

if hpoints.len() >= 200 {
hpoints.remove(0);
}
if vpoints.len() >= 200 {
vpoints.remove(0);
}
hpoints.push((tow, OrderedFloat(h_vel)));
vpoints.push((tow, OrderedFloat(v_vel)));

let mut builder = Builder::new_default();
let msg = builder.init_root::<m::message::Builder>();

let mut velocity_status = msg.init_velocity_status();

velocity_status.set_min(_min);
velocity_status.set_max(_max);

{
let mut hvel_points = velocity_status
.reborrow()
.init_hpoints(hpoints.len() as u32);
for (i, (x, OrderedFloat(y))) in hpoints.iter().enumerate() {
let mut point_val = hvel_points.reborrow().get(i as u32);
point_val.set_x(*x);
point_val.set_y(*y);
}
}
{
let mut vvel_points = velocity_status
.reborrow()
.init_vpoints(vpoints.len() as u32);
for (i, (x, OrderedFloat(y))) in vpoints.iter().enumerate() {
let mut point_val = vvel_points.reborrow().get(i as u32);
point_val.set_x(*x);
point_val.set_y(*y);
}
}

let mut msg_bytes: Vec<u8> = vec![];
serialize::write_message(&mut msg_bytes, &builder).unwrap();

client_send_clone.send(msg_bytes).unwrap();
Ok(SBP::MsgVelNED(msg)) => {
main.solution_velocity_tab
.handle_vel_ned(msg.clone(), &mut client_send_clone.clone());
}

_ => {
// no-op
}
Expand Down
Loading

0 comments on commit 50ff730

Please sign in to comment.