* WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
-use ringbuf::Ringbuf;
+use fixed15_49;
use systick;
+type Fixed = fixed15_49::Fixed15_49;
+
enum ParseState {
Start,
InPacket,
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.
}
impl TimeAndPos {
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),
}
}
-}
-fn to_lower(c: u8) -> u8 {
- c | 0x20
-}
+ pub fn distance_cm(&self, other: &TimeAndPos) -> i64 {
+ let a_lat = self.latitude_rad;
+ let a_lon = self.longitude_rad;
-fn try_read() -> Option<u8> {
- extern {
- static mut uart0_rx_buf: Ringbuf;
- }
+ let b_lat = other.latitude_rad;
+ let b_lon = other.longitude_rad;
- unsafe {
- if uart0_rx_buf.is_empty() {
- None
- } else {
- Some(uart0_rx_buf.read())
- }
+ 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 {
+ c | 0x20
+}
+
fn parse_coordinate(s: &[u8]) -> i32 {
// Find the position of the decimal separator for the minutes.
let dot_position = s.iter().enumerate().find(|(_, &c)| {
minutes
}
+fn parse_coordinate_q(s: &[u8]) -> Fixed {
+ // Find the position of the decimal separator for the minutes.
+ let dot_position_o = s.iter().enumerate().find(|(_, &c)| {
+ c == b'.'
+ }).and_then(|(i, _)| {
+ Some(i)
+ });
+
+ if dot_position_o.is_none() {
+ return Fixed::from_i64(0);
+ }
+
+ let dot_position = dot_position_o.unwrap();
+
+ // Minutes take two digits before the decimal separator.
+ let num_degree_digits = dot_position - 2;
+
+ let degrees = s[0..num_degree_digits].iter().fold(0, |d, c| {
+ (d * 10) + (c - b'0') as i32
+ });
+
+ let minutes = s[num_degree_digits..dot_position].iter().fold(0, |d, c| {
+ (d * 10) + (c - b'0') as i32
+ });
+
+ let decimal_minutes = s[dot_position + 1..].iter().fold(0, |d, c| {
+ (d * 10) + (c - b'0') as i32
+ });
+
+ let mut result = Fixed::from_i64(decimal_minutes.into());
+ result /= Fixed::from_i64(10000);
+
+ result += Fixed::from_i64(minutes.into());
+ result /= Fixed::from_i64(60);
+
+ result += Fixed::from_i64(degrees.into());
+
+ return result;
+}
+
// Only works for 2016 onwards.
fn is_leap_year(year: u32) -> bool {
(year & 3) == 0
}
}
- pub fn update(&mut self, tap: &mut TimeAndPos) -> bool {
+ pub fn update<F>(&mut self, tap: &mut TimeAndPos, mut read_func: F) -> bool
+ where F: FnMut() -> Option<u8>
+ {
let hexdigits = b"0123456789abcdef";
- while let Some(received) = try_read() {
+ while let Some(received) = read_func() {
if received == b'$' {
self.state = ParseState::InPacket;
self.offset = 0;
return false;
}
- if pos_fix_indicator != b"1" {
- return false;
+ match pos_fix_indicator {
+ b"1" => {
+ // Valid standard GPS fix (low resolution).
+ },
+ b"2" => {
+ // Valid differential GPS fix (high resolution).
+ },
+ _ => {
+ return false;
+ }
}
if utc_time.len() < 6 {
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;
}
true