hackflight 0.1.7

Simple flight-control software for makers in Rust
Documentation 
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

/*
   Filters module

   Copyright (C) 2022 Simon D. Levy

   MIT License
 */

use std::f32::consts::PI;

use crate::clock::DT;

// Pt1 --------------------------------------------------------------------

#[derive(Clone,Copy)]
pub struct Pt1 {

    state: f32,
    k: f32
}

pub fn apply_pt1(mut filter: Pt1, input: f32) -> f32 {

    filter.state = filter.state + filter.k * (input - filter.state);

    filter.state
}

pub fn make_pt1(f_cut: f32) -> Pt1 {

    let k = compute_pt1_gain(f_cut);

    Pt1 {state: 0.0, k: k }
}

pub fn adjust_pt1_gain(mut filter: Pt1, f_cut: f32)
{
    filter.k = compute_pt1_gain(f_cut);
}


fn compute_pt1_gain(f_cut:f32) -> f32 {

    compute_gain(1.0, f_cut)
}

// Pt2 --------------------------------------------------------------------

#[derive(Clone,Copy)]
pub struct Pt2 {

    state: f32,
    state1: f32,
    k: f32
}

pub fn apply_pt2(mut filter: Pt2, input: f32) -> f32 {

    let state1 = filter.state1 + filter.k * (input - filter.state1);

    filter.state = filter.state + filter.k * (state1 - filter.state);

    filter.state
}

pub fn make_pt2(f_cut: f32) -> Pt2 {

    let k = compute_gain_with_order(2.0, f_cut);

    Pt2 {state: 0.0, state1: 0.0, k: k }
}

// Pt3 --------------------------------------------------------------------

#[derive(Clone,Copy)]
pub struct Pt3 {

    state: f32,
    state1: f32,
    state2: f32,
    k: f32
}

pub fn apply_pt3(mut filter: Pt3, input: f32) -> f32 {

    let state1 = filter.state1 + filter.k * (input - filter.state1);
    let state2 = filter.state2 + filter.k * (state1 - filter.state2);

    filter.state = filter.state + filter.k * (state2 - filter.state);

    filter.state
}

pub fn make_pt3(f_cut: f32) -> Pt3 {

    let k = compute_gain_with_order(3.0, f_cut);

    Pt3 {state: 0.0, state1: 0.0, state2: 0.0, k: k }
}


// Helpers --------------------------------------------------------------------

fn compute_gain_with_order(order: f32, f_cut: f32) -> f32 {

    let two: f32 = 2.0;
    let order_cutoff_correction = 1.0 / (two.powf(1.0 / order) - 1.0).sqrt();

    compute_gain(order_cutoff_correction, f_cut)
}

fn compute_gain(order_cutoff_correction: f32, f_cut: f32) -> f32 {

    let rc = 1.0 / (2.0 * order_cutoff_correction * PI * f_cut);

    DT / (rc + DT)
}