diff --git a/.gitignore b/.gitignore index c507849..7cb988a 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,3 @@ target .idea +Cargo.lock \ No newline at end of file diff --git a/Cargo.lock b/Cargo.lock deleted file mode 100644 index 632fbab..0000000 --- a/Cargo.lock +++ /dev/null @@ -1,23 +0,0 @@ -[root] -name = "navigation" -version = "0.1.6" -dependencies = [ - "rand 0.3.14 (registry+https://github.com/rust-lang/crates.io-index)", -] - -[[package]] -name = "libc" -version = "0.2.6" -source = "registry+https://github.com/rust-lang/crates.io-index" - -[[package]] -name = "rand" -version = "0.3.14" -source = "registry+https://github.com/rust-lang/crates.io-index" -dependencies = [ - "libc 0.2.6 (registry+https://github.com/rust-lang/crates.io-index)", -] - -[metadata] -"checksum libc 0.2.6 (registry+https://github.com/rust-lang/crates.io-index)" = "b608bf5e09bb38b075938d5d261682511bae283ef4549cc24fa66b1b8050de7b" -"checksum rand 0.3.14 (registry+https://github.com/rust-lang/crates.io-index)" = "2791d88c6defac799c3f20d74f094ca33b9332612d9aef9078519c82e4fe04a5" diff --git a/Cargo.toml b/Cargo.toml index d2ddf6b..55803c4 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,12 +1,13 @@ [package] name = "navigation" description = "Provides basic navigation between GPS waypoints" -version = "0.1.7" -authors = ["Andy Grove "] -homepage = "https://github.com/andygrove/rust-navigation" -documentation = "https://github.com/andygrove/rust-navigation" -repository = "https://github.com/andygrove/rust-navigation" +version = "0.2.0" +authors = ["Andy Grove ", "Christopher Moran "] +homepage = "https://github.com/ucsb-coast-lab/rust-navigation" +documentation = "https://github.com/ucsb-coast-lab/rust-navigation" +repository = "https://github.com/ucsb-coast-lab/rust-navigation" license = "MIT/Apache-2.0" +edition = "2018" [dependencies] -rand = "0.3.13" +rand = "0.8" diff --git a/README.md b/README.md index bc4ac07..3ee6465 100644 --- a/README.md +++ b/README.md @@ -36,3 +36,33 @@ let bearing = boulder.estimate_bearing_to(&dia); // results in 110.44 (40.091311, -105.185128) -> (40.090946, -105.184925): bearing=157.0 estimate=156.9 diff=0.1 [OK] (40.091150, -105.185586) -> (40.091221, -105.185793): bearing=294.2 estimate=294.1 diff=0.1 [OK] ``` + +## AlvinXY submodule + +The `alvinxy` submodule adds the ability to switch between a local and global coordinate frame according to the +``` +Murphy, Chris & Singh, Hanumant. (2010). Rectilinear coordinate frames for Deep sea navigation. 2010 IEEE/OES Autonomous Underwater Vehicles, AUV 2010. 1 - 10. 10.1109/AUV.2010.5779654.` +``` + +### AlvinXY mission planning example + +A mission planned for a vehicle using this coordinate plan might look like the following, where the final GPS coordinates could be exported to a mission file or use directly by a control system. This implementation has been used for mission planning of an autonomous underwater vehicle in a freshwater reservoir. + +``` +fn main() { + let origin = Location::new(34.589,-119.96472); + // Writing the vehicle coordinate sequence and push them to vector + let mut wt_list: Vec = Vec::new(); + let mut b = LocalCoor::new(-900.0,0.0); + let mut soff = LocalCoor::new(-900.,-100.); + let mut son = LocalCoor::new(-900.,100.); + wt_list.push(b); + wt_list.push(soff); + wt_list.push(son); + + for wpt in &mut wt_list { + wpt.rotate_local(15.0); + println!("List of waypoints after rotation and conversion: {:?}",xy2latlon(*wpt,origin)); + } +} +``` diff --git a/src/alvinxy.rs b/src/alvinxy.rs new file mode 100644 index 0000000..c3cc4f1 --- /dev/null +++ b/src/alvinxy.rs @@ -0,0 +1,81 @@ +//! This is module converts between GPS coordinates in decimal form to a local coordinate structure per the AlvinXY algorithm as laid out in the paper below. +//! +//! `Murphy, Chris & Singh, Hanumant. (2010). Rectilinear coordinate frames for Deep sea navigation. 2010 IEEE/OES Autonomous Underwater Vehicles, AUV 2010. 1 - 10. 10.1109/AUV.2010.5779654.` +//! +//! This implementation has been used to generate waypoints for an autonomous underwater vehicle mission in a freshwater resevoir + +use std::f64; + +#[derive(Clone, Copy, Debug)] +pub struct LocalCoor { + pub x: f64, + pub y: f64, +} + +use crate::Location; + +impl LocalCoor { + /// Create a new coordinate in the local reference frame + pub fn new(x: f64, y: f64) -> Self { + LocalCoor { x, y } + } + + /// Rotate a coordinate around the origin by an angle supplied in degrees + pub fn rotate_local(&mut self, theta: f64) { + let theta = theta.to_radians(); + self.x = (self.x * theta.cos()) - (self.y * theta.sin()); + self.y = (self.x * theta.sin()) + (self.y * theta.cos()); + } +} + +/// Convert a GPS `Location` coordinate to a local `LocalCoor` coordinate frame +pub fn latlon2xy(coordinate: Location, origin: Location) -> LocalCoor { + let x = (coordinate.lon - origin.lon) * mdeglon(origin.lat); + let y = (coordinate.lat - origin.lat) * mdeglat(origin.lat); + LocalCoor::new(x, y) +} + +/// Converts from the local `LocalCoor` xy to global `Location` Coordinate frame +pub fn xy2latlon(coordinate: LocalCoor, origin: Location) -> Location { + let lon = coordinate.x / mdeglon(origin.lat) + origin.lon; + let lat = coordinate.y / mdeglat(origin.lat) + origin.lat; + Location { lat, lon } +} + +#[inline] +fn mdeglon(lat0: f64) -> f64 { + let lat0rad = lat0.to_radians(); + 111415.13 * lat0rad.cos() - (94.55 * (3.0 * lat0rad).cos()) - (0.12 * (5.0 * lat0rad).cos()) +} + +#[inline] +fn mdeglat(lat0: f64) -> f64 { + let lat0rad = lat0.to_radians(); + 111132.09 - (566.05 * (2.0 * lat0rad).cos()) + (1.20 * (4.0 * lat0rad).cos()) + - (0.002 * (6.0 * lat0rad).cos()) +} + +// This test was used to verify the coordinate offsets for an AUV mission in a Californian freshwater resevoir +#[test] +fn reservoir_coordinates() { + let origin = Location::new(34.589000, -119.966000); + let o = latlon2xy(Location::new(34.589000, -119.966000), origin); // Making sure the origin in (0.0,0.0) + println!("The origin is at: ({},{})", o.x, o.y); + assert!((o.x == 0.0) && (o.y == 0.0)); + // Directly above/North + let n = latlon2xy(Location::new(34.59000, -119.966000), origin); + println!("nx,ny = ({},{})", n.x, n.y); + assert!(n.y > o.y); + // Directly below/South + let s = latlon2xy(Location::new(34.58800, -119.966000), origin); + println!("nx,ny = ({},{})", s.x, s.y); + assert!(s.y < o.y); + // Directly left/West (longitude DECREASES going West) + let w = latlon2xy(Location::new(34.589000, -119.967000), origin); + println!("wx,wy = ({},{})", w.x, w.y); + assert!(w.x < o.x); + // Directly right/East (latitude INCREASES going East) + let e = latlon2xy(Location::new(34.589000, -119.96500), origin); + println!("ex,ey = ({},{})", e.x, e.y); + assert!(e.x > o.x); +} diff --git a/src/lib.rs b/src/lib.rs index f262aa5..9a0cc0c 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -1,6 +1,7 @@ extern crate rand; +pub mod alvinxy; -const PI: f64 = 3.141592; +use std::f64::consts::PI; /// parses an NMEA string (degrees decimal minutes) such as "3953.4210" into (39, 53.4210) and /// then to 39 + 53.4210/60 @@ -20,7 +21,7 @@ pub enum Direction { } // represents a location in decimal degrees format -#[derive(Debug)] +#[derive(Debug, Clone, Copy)] pub struct Location { pub lat: f64, pub lon: f64, @@ -28,27 +29,23 @@ pub struct Location { impl Location { pub fn parse_nmea(lat: &str, lat_dir: &str, lon: &str, lon_dir: &str) -> Result { - - let lat_dir = try!(Location::parse_direction(lat_dir)); - let lat_mult = try!(match lat_dir { + let lat_dir = Location::parse_direction(lat_dir)?; + let lat_mult = match lat_dir { Direction::North => Ok(1_f64), Direction::South => Ok(-1_f64), _ => Err(format!("Invalid latitude direction {:?}", lat_dir)), - }); - let lat = try!(parse_nmea(lat)) * lat_mult; + }?; + let lat = parse_nmea(lat)? * lat_mult; - let lon_dir = try!(Location::parse_direction(lon_dir)); - let lon_mult = try!(match lon_dir { + let lon_dir = Location::parse_direction(lon_dir)?; + let lon_mult = match lon_dir { Direction::East => Ok(1_f64), Direction::West => Ok(-1_f64), _ => Err(format!("Invalid longitude direction {:?}", lon_dir)), - }); - let lon = try!(parse_nmea(lon)) * lon_mult; + }?; + let lon = parse_nmea(lon)? * lon_mult; - Ok(Location { - lat: lat, - lon: lon, - }) + Ok(Location { lat, lon }) } pub fn parse_direction(d: &str) -> Result { @@ -62,7 +59,6 @@ impl Location { } } - // Degrees, Minutes, Seconds pub struct DMS { d: i32, @@ -110,18 +106,18 @@ impl Location { let dest_long = radians(dest.lon); let mut delta_long = dest_long - start_long; - let delta_phi = ((dest_lat / 2.0 + PI / 4.0).tan() / (start_lat / 2.0 + PI / 4.0).tan()) - .ln(); + let delta_phi = + ((dest_lat / 2.0 + PI / 4.0).tan() / (start_lat / 2.0 + PI / 4.0).tan()).ln(); if delta_long.abs() > PI { if delta_long > 0.0 { delta_long = -(2.0 * PI - delta_long); } else { - delta_long = 2.0 * PI + delta_long; + delta_long += 2.0 * PI; } } - return (degrees(delta_long.atan2(delta_phi)) + 360.0) % 360.0; + (degrees(delta_long.atan2(delta_phi)) + 360.0) % 360.0 } /** experimental cheaper method of calculating bearing */ @@ -134,20 +130,28 @@ impl Location { let ax = lon_delta.abs(); let ay = lat_delta.abs(); - let angle: f64 = 180.0 / 3.141592 * - if ax > ay { - (ay / ax).atan() - } else { - (ax / ay).atan() - }; + let angle: f64 = 180.0 / PI + * if ax > ay { + (ay / ax).atan() + } else { + (ax / ay).atan() + }; // println!("angle = {}", angle); let bearing: f64 = if lon_delta > 0.0 { if lat_delta > 0.0 { - if ax > ay { 90.0 - angle } else { angle } + if ax > ay { + 90.0 - angle + } else { + angle + } } else { - if ax > ay { 90.0 + angle } else { 180.0 - angle } + if ax > ay { + 90.0 + angle + } else { + 180.0 - angle + } } } else { if lat_delta > 0.0 { @@ -165,7 +169,7 @@ impl Location { } }; - return bearing; + bearing } } @@ -185,7 +189,6 @@ fn degrees(n: f64) -> f64 { #[test] fn test_estimation_accuracy() { - let lon_min = -105.18591; let lon_max = -105.18467; let lat_min = 40.09027; @@ -194,28 +197,33 @@ fn test_estimation_accuracy() { let mut ok = true; for _ in 0..100000 { - // create two random points on map - let l1 = Location::new(lat_min + rand::random::() * (lat_max - lat_min), - lon_min + rand::random::() * (lon_max - lon_min)); + let l1 = Location::new( + lat_min + rand::random::() * (lat_max - lat_min), + lon_min + rand::random::() * (lon_max - lon_min), + ); - let l2 = Location::new(lat_min + rand::random::() * (lat_max - lat_min), - lon_min + rand::random::() * (lon_max - lon_min)); + let l2 = Location::new( + lat_min + rand::random::() * (lat_max - lat_min), + lon_min + rand::random::() * (lon_max - lon_min), + ); let bearing = l1.calc_bearing_to(&l2); // let estimate = l1.estimate_bearing_to(&l2, 1.0, 1.0); let estimate = l1.estimate_bearing_to(&l2, 69.0, 53.0); let diff = (bearing - estimate).abs(); - println!("({}, {}) -> ({}, {}): bearing={} estimate={} diff={} [{}]", - format!("{:.*}", 6, l1.lat), - format!("{:.*}", 6, l1.lon), - format!("{:.*}", 6, l2.lat), - format!("{:.*}", 6, l2.lon), - format!("{:.*}", 1, bearing), - format!("{:.*}", 1, estimate), - format!("{:.*}", 1, diff), - if diff < 1.0 { "OK" } else { "FAIL" }); + println!( + "({}, {}) -> ({}, {}): bearing={} estimate={} diff={} [{}]", + format!("{:.*}", 6, l1.lat), + format!("{:.*}", 6, l1.lon), + format!("{:.*}", 6, l2.lat), + format!("{:.*}", 6, l2.lon), + format!("{:.*}", 1, bearing), + format!("{:.*}", 1, estimate), + format!("{:.*}", 1, diff), + if diff < 1.0 { "OK" } else { "FAIL" } + ); if diff > 1.0 { ok = false; @@ -224,13 +232,10 @@ fn test_estimation_accuracy() { } assert!(ok); - } - #[test] fn calc_bearing_boulder_to_dia() { - // 39.8617° N, 104.6731° W let dia = Location::new(39.8617, -104.6731); @@ -238,60 +243,67 @@ fn calc_bearing_boulder_to_dia() { let boulder = Location::new(40.0274, -105.2519); assert_eq!("110.48", format!("{:.*}", 2, boulder.calc_bearing_to(&dia))); - assert_eq!("110.44", - format!("{:.*}", 2, boulder.estimate_bearing_to(&dia, 69.0, 53.0))); - + assert_eq!( + "110.44", + format!("{:.*}", 2, boulder.estimate_bearing_to(&dia, 69.0, 53.0)) + ); } #[test] fn convert_dms_to_decimal() { - let dia = Location::new(DMS { - d: 39, - m: 51, - s: 42, - } - .to_decimal(), - DMS { - d: -104, - m: 40, - s: 22, - } - .to_decimal()); + let dia = Location::new( + DMS { + d: 39, + m: 51, + s: 42, + } + .to_decimal(), + DMS { + d: -104, + m: 40, + s: 22, + } + .to_decimal(), + ); assert_eq!("39.861666666666665, -104.67277777777778", dia.to_string()); } #[test] fn test_sparkfun_route() { - let mut route: Vec = Vec::new(); route.push(Location::new(40.0906963, -105.185844)); route.push(Location::new(40.0908317, -105.185734)); route.push(Location::new(40.0910061, -105.1855154)); // TODO: need to confirm that these bearings are actually correct - assert_eq!("31.86", - format!("{:.*}", 2, &route[0].calc_bearing_to(&route[1]))); - assert_eq!("43.80", - format!("{:.*}", 2, &route[1].calc_bearing_to(&route[2]))); - + assert_eq!( + "31.86", + format!("{:.*}", 2, &route[0].calc_bearing_to(&route[1])) + ); + assert_eq!( + "43.80", + format!("{:.*}", 2, &route[1].calc_bearing_to(&route[2])) + ); } #[test] fn test_sparkfun_route_2() { - let mut route: Vec = Vec::new(); route.push(Location::new(40.09069, -105.18585)); route.push(Location::new(40.09128, -105.18517)); // TODO: need to confirm that these bearings are actually correct - assert_eq!("41.40", - format!("{:.*}", 2, &route[0].calc_bearing_to(&route[1]))); - + assert_eq!( + "41.40", + format!("{:.*}", 2, &route[0].calc_bearing_to(&route[1])) + ); } #[test] fn test_parse_nmea() { - assert_eq!("101.6971", - format!("{:.*}", 4, parse_nmea("10141.82531").unwrap())); + assert_eq!( + "101.6971", + format!("{:.*}", 4, parse_nmea("10141.82531").unwrap()) + ); }