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
142
143
144
145
146

// Copyright 2015, Paul Osborne <osbpau@gmail.com>
//
// Licensed under the Apache License, Version 2.0 <LICENSE-APACHE or
// http://www.apache.org/license/LICENSE-2.0> or the MIT license
// <LICENSE-MIT or http://opensource.org/licenses/MIT>, at your
// option.  This file may not be copied, modified, or distributed
// except according to those terms.

use std::error::Error;
use std::ops::{Add, Sub, Mul, Div};

#[derive(Debug, Copy, Clone)]
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
        }
    }
}

/// Trait for sensors that provide access to accelerometer readings (3-axis)
pub trait Accelerometer {
    type Error: Error;

    /// Returns 'Ok(accel)' if available, otherwise returns 'Err(Self::Error)'
    fn acceleration_reading(&mut self) -> Result<Vec3, Self::Error>;
}

pub trait Gyroscope {
    type Error: Error;

    /// Returns 'Ok(ang_rate)' if available, otherwise returns 'Err(Self::Error)'
    fn angular_rate_reading(&mut self) -> Result<Vec3, Self::Error>;
}

pub trait Magnetometer {
    type Error: Error;

    /// Returns 'Ok(mag)' if available, otherwise returns 'Err(Self::Error)'
    fn magnetic_reading(&mut self) -> Result<Vec3, Self::Error>;
}

/// Trait for sensors that provide access to temperature readings
pub trait Thermometer {
    type Error: Error;

    /// Get a temperature from the sensor in degrees celsius
    ///
    /// Returns `Ok(temperature)` if available, otherwise returns `Err(Self::Error)`
    fn temperature_celsius(&mut self) -> Result<f32, Self::Error>;
}

/// Trait for sensors that provide access to pressure readings
pub trait Barometer {
    type Error: Error;

    /// Get a pressure reading from the sensor in kPa
    ///
    /// Returns `Ok(temperature)` if avialable, otherwise returns `Err(Self::Error)`
    fn pressure_kpa(&mut self) -> Result<f32, Self::Error>;
}

/// Trait for sensors that provide access to altitude readings
pub trait Altimeter {
    type Error: Error;

    /// Get an altitude reading from the sensor in meters, relative to the pressure in kPa at
    /// sea level
    ///
    /// Returns `Ok(altitude)` if available, otherwise returns `Err(Self::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)
    }
}

/// Trait for sensors that provide access to humidity readings
pub trait Hygrometer {
    type Error: Error;

    /// Read the relative humidity from the sensor in percent
    fn relative_humidity(&mut self) -> Result<f32, Self::Error>;
}