Thomas/add hardware support

copter/Cargo.toml

@@ -14,6 +14,7 @@ i2cdev-bmp280 = { path = "./local/i2cdev-sensors/i2cdev-bmp280" }
 i2cdev-l3gd20 = { path = "./local/i2cdev-sensors/i2cdev-l3gd20" }
 i2cdev-lsm303d = { path = "./local/i2cdev-sensors/i2cdev-lsm303d" }
 i2cdev-lsm9ds0 = { path = "./local/i2cdev-sensors/i2cdev-lsm9ds0" }
+i2cdev-lsm303dlhc = { path = "./local/i2cdev-sensors/i2cdev-lsm303dlhc" }
 pca9685 = { path = "./local/rust-pca9685" }
 ads111x = { path = "./local/rust-ads1115" }
 unbounded-gpsd = "0.4.2"

copter/configurations/src/calibrations.rs

@@ -8,46 +8,46 @@ use na::{Matrix3, Vector3};
 
 #[derive(Debug, Deserialize, Serialize)]
 pub struct Simple {
-    pub offsets: Vec<f64>,
+    pub offsets: Vec<f32>
 }
 
 impl Simple {
-    pub fn new(offsets: Vector3<f64>) -> Simple {
+    pub fn new(offsets: Vector3<f32>) -> Simple {
         Simple {
             offsets: offsets.as_slice().to_vec(),
         }
     }
 
-    pub fn get_offsets(&self) -> Vector3<f64> {
+    pub fn get_offsets(&self) -> Vector3<f32> {
         Vector3::from_column_slice(&self.offsets)
     }
 }
 
 #[derive(Debug, Deserialize, Serialize)]
 pub struct Ellipsoid {
-    pub offsets: Vec<f64>,
-    pub rotation: Vec<f64>,
-    pub gains: Vec<f64>,
+    pub offsets: Vec<f32>,
+    pub rotation: Vec<f32>,
+    pub gains: Vec<f32>
 }
 
 impl Ellipsoid {
-    pub fn new(offsets: Vector3<f64>, rotation: Matrix3<f64>, gains: Vector3<f64>) -> Ellipsoid {
+    pub fn new(offsets: Vector3<f32>, rotation: Matrix3<f32>, gains: Vector3<f32>) -> Ellipsoid {
         Ellipsoid {
             offsets: offsets.as_slice().to_vec(),
             rotation: rotation.as_slice().to_vec(),
-            gains: gains.as_slice().to_vec(),
+            gains: gains.as_slice().to_vec()
         }
     }
 
-    pub fn get_offsets(&self) -> Vector3<f64> {
+    pub fn get_offsets(&self) -> Vector3<f32> {
         Vector3::from_column_slice(&self.offsets)
     }
 
-    pub fn get_rotation(&self) -> Matrix3<f64> {
+    pub fn get_rotation(&self) -> Matrix3<f32> {
         Matrix3::from_column_slice(&self.rotation)
     }
 
-    pub fn get_gains(&self) -> Vector3<f64> {
+    pub fn get_gains(&self) -> Vector3<f32> {
         Vector3::from_column_slice(&self.gains)
     }
 }

copter/ekf_matlab/ekf_derivation

@@ -0,0 +1 @@
+% SafeFlight EKF derivation

copter/ekf_matlab/ekf_derivation.m~

@@ -0,0 +1,137 @@
+% SafeFlight EKF derivation
+% Written by Martin Deegan
+clc
+
+helper_functions;
+
+% 19 state EKF
+% Position: latitude, longitude, altitude
+% Velocity: North, East, Vertical
+% Attitude: quaternion representation
+% Biases: Gyroscope, Accelerometer
+% Magnetic Field
+syms lat lon alt vn ve vz qx qy qz qw gbx gby gbz abx aby abz mx my mz
+syms dlat dlon dalt dvn dve dvz dtx dty dtz dgbx dgby dgbz dabx daby dabz dmx dmy dmz
+syms dt
+syms x
+syms dx
+x = [lat; lon; alt; vn; ve; vz; qx; qy; qz; qw; gbx; gby; gbz; abx; aby; abz; mx; my; mz];
+dx = [lat lon alt vn ve vz qx qy qz qw gbx gby gbz abx aby abz mx my mz];
+
+% Control Variables
+syms gxp gyp gzp axp ayp azp
+syms gx gy gz ax ay az
+syms up un
+up = [gxp; gyp; gzp; axp; ayp; azp];
+un = [gx; gy; gz; ax; ay; az];
+
+% transition functions
+syms remove_bias(r,b)
+remove_bias(r, b) = r-b;
+syms vec_mean(v1, v2)
+vec_mean(v1, v2) = (v1+v2)/2;
+syms predict_state(p, v, q, gb, ab, gp, ap, g, a, dt)
+syms predict_state_corrected(p, v, q, gp, g, a, dt)
+syms predict_state_velocity(qx, qy, qz, qw, vn, ve, vz, ax, ay, az, dt)
+syms predict_state_attitude(qx, qy, qz, qw, gxp, gyp, gzp, gx, gy, gz, dt)
+syms predict_state_position(lat, lon, alt, vn, ve, vz, dt)
+
+dXddx = jacobian(add_e_state([lat; lon; alt; vn; ve; vz; qx; qy; qz; qw;...
+                                gbx; gby; gbz; abx; aby; abz; mx; my; mz], ...
+                                [dlat; dlon; dalt; dvn; dve; dvz; dtx; dty; dtz; ...
+                                dgbx; dgby; dgbz; dabx; daby; dabz; dmx; dmy; dmz]), ...
+                                [dlat dlon dalt dvn dve dvz dtx dty dtz dgbx dgby dgbz dabx daby dabz dmx dmy dmz]);
+
+A = jacobian(predict_state(x, up, un, dt), dx);
+A(7:10,7:13) = 0;
+
+syms fx fy fz
+H_field = jacobian(rotate_field(x, [fx; fy; fz]), dx);
+V_field = jacobian(rotate_field
+
+disp("Jacobian of the state with respect to the error state (X_dx):");
+
+
+disp("Linearization of the state transition function (A):");
+% disp(dXddx);
+disp(H_field);
+
+% --------- Prediction Step ----------------
+function integrate_gyroscope_f = integrate_gyroscope(q, gp, gn, dt)
+    w = (gp + gn)/2;
+    omega_mean = omega_matrix(w);
+    omega_prev = omega_matrix(gp);
+    omega_next = omega_matrix(gn);
+    expon = expm(omega_mean);
+    second_term = (1/48)*(omega_next*omega_prev-omega_prev*omega_next)*dt^2;
+    integrate_gyroscope_f = (expon + second_term)*q;
+end
+
+function predict_velocity_f = predict_velocity(v, a, q, dt)
+    rot = rot_matrix(q);
+    a_w = rot*a;
+    v_p = v + a_w * dt;
+    predict_velocity_f = v_p;
+end
+
+function predict_position_f = predict_position(p, v, dt)
+    p_p = p + v*dt;
+    predict_position_f = p_p;
+end
+
+function predict_state_f = predict_state(x, up, un, dt)
+    un = un - x(11:16);
+    q = integrate_gyroscope(x(7:10), up(1:3), un(1:3), dt);
+    v = predict_velocity(x(4:6), un(4:6), x(7:10), dt);
+    p = predict_position(x(1:3), x(4:6), dt);
+    predict_state_f = [p; v; q; x(11:19)];
+end
+
+% -------------------- Updates -------------------------
+
+function rotate_field_f = rotate_field(x, f) 
+    rot = rot_matrix(x(7:10));
+    rotate_field_f = rot * f;
+end
+
+
+% -------------------------------Helper functions and variables --------------------------------------
+
+
+% Helper Matrices
+
+function rot_matrix_f = rot_matrix(q)
+rot_matrix_f = [1-2*q(2)^2-2*q(3)^2 2*q(1)*q(2)-2*q(3)*q(4) 2*q(1)*q(3)+2*q(2)*q(4);
+               2*q(1)*q(2)+2*q(3)*q(4) 1-2*q(1)^2-2*q(3)^2 2*q(2)*q(3)-2*q(1)*q(4);
+               2*q(1)*q(3)-2*q(2)*q(4) 2*q(2)*q(3)+2*q(1)*q(4) 1-2*q(1)^2-2*q(2)^2];
+end
+
+function q_prod_l_f = q_prod_l(q)
+    q_prod_l_f = [q(4) q(3) -q(2) q(1); -q(3) q(4) q(1) q(2); q(2) -q(1) q(4) q(3); -q(1) -q(2) -q(3) q(4)];
+end
+
+function q_prod_r_f = q_prod_r(q)
+    q_prod_r_f = [q(4) -q(3) q(2) q(1); q(3) q(4) -q(1) q(2); -q(2) q(1) q(4) q(3); -q(1) -q(2) -q(3) q(4)];
+end
+
+function skew_matrix_f = skew_matrix(w)
+    skew_matrix_f = [0 -w(3) w(2); w(3) 0 -w(1); -w(2) w(1) 0];
+end
+
+function omega_matrix_f = omega_matrix(w)
+    omega_matrix_f = [0 w(3) -w(2) w(1); -w(3) 0 w(1) w(2); w(2) -w(1) 0 w(3); -w(1) -w(2) -w(3) 0];
+end
+
+function small_e_to_dq_f = small_e_to_dq(theta)
+    small_e_to_dq_f = [0.5*theta(1); 0.5*theta(2); 0.5*theta(3); 1];
+end
+
+function add_e_state_f = add_e_state(x, dx)
+    p_prod = q_prod_r(x(7:10));
+    dq = small_e_to_dq(dx(7:9));
+    q_fixed = p_prod * dq;
+    dx_dim = [dx(1:6); 0; 0; 0; 0; dx(10:18)];
+    x_fixed = x + dx_dim;
+    x_fixed(7:10) = q_fixed;
+    add_e_state_f = x_fixed;
+end

copter/ekf_matlab/helper_functions.m

@@ -0,0 +1,2 @@
+
+

copter/ekf_matlab/helper_functions.m~

@@ -0,0 +1,49 @@
+% Helper functions
+clc
+% Quaternion Helpers
+syms quaternion_to_rotation_matrix(qx, qy, qz, qw)
+quaternion_to_rotation_matrix(qx, qy, qz, qw) = [1-2*qy^2-2*qz^2 2*qx*qy-2*qz*qw 2*qx*qz+2*qy*qw;
+                                                 2*qx*qy+2*qz*qw 1-2*qx^2-2*qz^2 2*qy*qz-2*qx*qw;
+                                                 2*qx*qz-2*qy*qw 2*qy*qz+2*qx*qw 1-2*qx^2-2*qy^2];
+
+syms left_product_matrix(qx, qy, qz, qw)
+left_product_matrix(qx, qy, qz, qw) = [qw qz -qy qx; -qz qw qx qy; qy -qx qw qz; -qx -qy -qz qw];
+syms right_product_matrix(q)
+right_product_matrix(q) = [q(4) -q(3) q(2) q(1); q(3) q(4) -q(1) q(2); -q(2) q(1) q(4) q(3); -q(1) -q(2) -q(3) q(4)];
+
+syms sqew_matrix(x, y, z)
+sqew_matrix(x, y, z) = [0 -z y; z 0 -x; -y x 0];
+
+syms omega_matrix(x, y, z)
+omega_matrix(qx, qy, qz, qw) = [0 z -y x; -z 0 x y; y -x 0 z; -x -y -z 0];
+
+syms small_error_to_error_quaternion(theta)
+small_error_to_error_quaternion(theta) = [0.5*theta(1); 0.5*theta(2); 0.5*theta(3); 1];
+
+syms add_error_state(lat, lon, alt, vn, ve, vz, qx, qy, qz, qw, gbx, gby, gbz, abx, aby, abz, mx, my, mz, dlat, dlon, dalt, dvn, dve, dvz, dtx, dty, dtz, dgbx, dgby, dgbz, dabx, daby, dabz, dmx, dmy, dmz)
+add_error_state(lat, lon, alt, vn, ve, vz, qx, qy, qz, qw, gbx, gby, gbz, abx, aby, abz, mx, my, mz, ...
+                dlat, dlon, dalt, dvn, dve, dvz, dtx, dty, dtz, dgbx, dgby, dgbz, dabx, daby, dabz, dmx, dmy, dmz) = ...
+                    [lat+dlat; lon+dlon; alt+dalt; vn+dvn; ve+dve; vz+dvz; ...
+                        right_product_matrix(qx, qy, qz, qw)*small_error_to_error_quaternion(dtx, dty, dtz); ...
+                            gbx+dgbx; gby+dgby; gbz+dgbz; abx+dabx; aby+daby; abz+dabz; mx+dmx; my+dmy; mz+dmz];
+
+dXddx = jacobian(add_e_state_f([lat; lon; alt; vn; ve; vz; qx; qy; qz; qw; gbx; gby; gbz;...
+                                abx; aby; abz; mx; my; mz; dlat, dlon, dalt, dvn, dve, dvz, dtx, dty, dtz, dgbx, dgby, dgbz, dabx, daby, dabz, dmx, dmy; dmz]), [dx])
+% Helper Matrices
+I3 = eye(3);
+I4 = eye(4);
+
+syms func(x, dx)
+%func = [x(1)+2*dx(1)+3*dx(2); x(2)-dx(3); x(3)*dx(1); x(4)-dx(1)+dx(2)];
+
+function add_e_state_f = add_e_state(x, dx)
+    p_prod = right_product_matrix(x(7:10));
+    dq = small_error_to_error_quaternion(dx(7:9));
+    q_fixed = p_prod * dq;
+    dx_dim = [dx(1:6); 0; 0; 0; 0; dx(10:)];
+    x_fixed = x + dx_dim;
+    x_fixed(7:10) = q_fixed;
+    add_e_state_f = x_fixed;
+end
+
+

copter/src/flight/altitude.rs

@@ -0,0 +1,46 @@
+use configurations::Config;
+
+const MAX_DESCEND_RATE: f32 = -0.03;
+const MAX_ASCEND_RATE: f32 = 0.25;
+
+pub struct Altitude {
+    alt_kp: f32,
+    alt_ki: f32,
+    alt_kd: f32,
+    last_altitude: f32,
+    integral: f32,
+    pub last_mid_level: f32
+}
+
+impl Altitude {
+    pub fn new(altitude: f32) -> Altitude {
+        let config = Config::new();
+
+        Altitude {
+            alt_kp: config.alt_kp,
+            alt_ki: config.alt_ki,
+            alt_kd: config.alt_kd,
+            last_altitude: altitude,
+            integral: 0.0,
+            last_mid_level: 0.0
+        }
+    }
+
+    pub fn get_mid_level(&mut self, current_altitude: f32, desired_altitude: f32, climb: Option<f32>, dt: f32) -> f32 {
+        let mut proportional = desired_altitude - current_altitude;
+        proportional  = proportional * proportional * proportional;
+        self.integral += proportional * dt;
+        let derivative = (self.last_altitude - current_altitude) / dt;
+
+        let error = proportional * self.alt_kp + self.integral * self.alt_ki + derivative * self.alt_kd;
+
+        if error < MAX_DESCEND_RATE {
+            self.last_mid_level += MAX_DESCEND_RATE;
+            self.integral -= proportional * dt;
+        } else {
+            self.last_mid_level += error;
+        }
+
+        self.last_mid_level
+    }
+}