1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
use std::error::Error;
use std::ops::{Add, Sub, Mul, Div};
#[derive(Debug)]
pub struct Vec3 {
pub x: f32,
pub y: f32,
pub z: f32
}
impl Vec3 {
pub fn zeros() -> Vec3 {
Vec3 { x: 0.0, y: 0.0, z: 0.0 }
}
}
impl Add for Vec3 {
type Output = Self;
fn add(self, other: Vec3) -> Self {
Vec3 {
x: self.x + other.x,
y: self.y + other.y,
z: self.z + other.z
}
}
}
impl Sub for Vec3 {
type Output = Self;
fn sub(self, other: Vec3) -> Self {
Vec3 {
x: self.x - other.x,
y: self.y - other.y,
z: self.z - other.z
}
}
}
impl Mul<f32> for Vec3 {
type Output = Self;
fn mul(self, other: f32) -> Self {
Vec3 {
x: self.x * other,
y: self.y * other,
z: self.z * other
}
}
}
impl Div<f32> for Vec3 {
type Output = Self;
fn div(self, other: f32) -> Self {
Vec3 {
x: self.x / other,
y: self.y / other,
z: self.z / other
}
}
}
pub trait Accelerometer {
type Error: Error;
fn acceleration_reading(&mut self) -> Result<Vec3, Self::Error>;
}
pub trait Gyroscope {
type Error: Error;
fn angular_rate_reading(&mut self) -> Result<Vec3, Self::Error>;
}
pub trait Magnetometer {
type Error: Error;
fn magnetic_reading(&mut self) -> Result<Vec3, Self::Error>;
}
pub trait Thermometer {
type Error: Error;
fn temperature_celsius(&mut self) -> Result<f32, Self::Error>;
}
pub trait Barometer {
type Error: Error;
fn pressure_kpa(&mut self) -> Result<f32, Self::Error>;
}
pub trait Altimeter {
type Error: Error;
fn altitude_meters(&mut self, sea_level_kpa: f32) -> Result<f32, Self::Error>;
}
impl<T> Altimeter for T
where T: Barometer
{
type Error = <Self as Barometer>::Error;
fn altitude_meters(&mut self, sea_level_kpa: f32) -> Result<f32, Self::Error> {
let pressure = try!(self.pressure_kpa()) * 1000.;
let sea_level_pa = sea_level_kpa * 1000.;
let altitude = 44330. * (1. - (pressure / sea_level_pa).powf(0.1903));
Ok(altitude)
}
}