pub struct TimeAndPos {
pub system_time: u32,
pub unix_time: u32,
- pub latitude: i32, // Positive means north, negative means south.
- pub longitude: i32, // Positive means east, negative means west.
+ pub latitude_deg: i32, // Positive means north, negative means south.
+ pub longitude_deg: i32, // Positive means east, negative means west.
pub latitude_rad: Fixed, // Positive means north, negative means south.
pub longitude_rad: Fixed, // Positive means east, negative means west.
}
TimeAndPos {
system_time: 0,
unix_time: 0,
- latitude: 0,
- longitude: 0,
+ latitude_deg: 0,
+ longitude_deg: 0,
latitude_rad: Fixed::from_i64(0),
longitude_rad: Fixed::from_i64(0),
}
}
+
+ pub fn distance_cm(&self, other: &TimeAndPos) -> i64 {
+ let a_lat = self.latitude_rad;
+ let a_lon = self.longitude_rad;
+
+ let b_lat = other.latitude_rad;
+ let b_lon = other.longitude_rad;
+
+ let x = (b_lon - a_lon) * ((b_lat + a_lat) / Fixed::from_i64(2)).cos();
+ let y = b_lat - a_lat;
+
+ let d_km = (x * x + y * y).sqrt() * Fixed::from_i64(6371);
+
+ let hundred = Fixed::from_i64(100);
+ let thousand = Fixed::from_i64(1000);
+
+ // Convert from km to cm.
+ (d_km * thousand * hundred).to_i64()
+ }
}
fn to_lower(c: u8) -> u8 {
tap.system_time = systick::now();
tap.unix_time = unix_time;
- tap.latitude = parse_coordinate(latitude);
- tap.longitude = parse_coordinate(longitude);
+ tap.latitude_deg = parse_coordinate(latitude);
+ tap.longitude_deg = parse_coordinate(longitude);
tap.latitude_rad = parse_coordinate_q(latitude).to_radians();
tap.longitude_rad = parse_coordinate_q(longitude).to_radians();
if north_south == b"S" {
- tap.latitude = -tap.latitude;
+ tap.latitude_deg = -tap.latitude_deg;
tap.latitude_rad = -tap.latitude_rad;
}
if east_west == b"W" {
- tap.longitude = -tap.longitude;
+ tap.longitude_deg = -tap.longitude_deg;
tap.longitude_rad = -tap.longitude_rad;
}