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
118
119
120
121
122
123
124
125
126
127
128
129

extern crate hackflight;

use std::net::UdpSocket;

use hackflight::Demands;
use hackflight::Motors;
use hackflight::VehicleState;
use hackflight::pids;
use hackflight::step;
use hackflight::mixers::quadxbf;
use hackflight::utils::rescale;
use hackflight::utils::rad2deg;

const RATE_KP  : f32 = 1.441305;
const RATE_KI  : f32 = 48.8762;
const RATE_KD  : f32 = 0.021160;
const RATE_KF  : f32 = 0.0165048;
const LEVEL_KP : f32 = 0.0;

const ALT_HOLD_KP : f32 = 7.5e-2;
const ALT_HOLD_KI : f32 = 1.5e-1;

fn main() -> std::io::Result<()> {

    const IN_BUF_SIZE:usize  = 17*8; // 17 doubles in
    const OUT_BUF_SIZE:usize = 4*8;  // 4 doubles out

    fn read_float(buf:[u8; IN_BUF_SIZE], idx:usize) -> f32 {
        let mut dst = [0u8; 8];
        let beg = 8 * idx;
        let end = beg + 8;
        dst.clone_from_slice(&buf[beg..end]);
        f64::from_le_bytes(dst) as f32
    }

    fn read_degrees(buf:[u8; IN_BUF_SIZE], idx:usize) -> f32 {
        rad2deg(read_float(buf, idx))
    }

    fn state_from_telemetry(buf:[u8; IN_BUF_SIZE]) -> VehicleState {
        VehicleState {
            x:read_float(buf, 1),       
            dx:read_float(buf, 2),
            y:read_float(buf, 3),
            dy:read_float(buf, 4),
            z:-read_float(buf, 5),         // NED => ENU
            dz:-read_float(buf, 6),        // NED => ENU
            phi:read_degrees(buf, 7),
            dphi:read_degrees(buf, 8),
            theta:-read_degrees(buf, 9),   // note sign reversal
            dtheta:-read_degrees(buf, 10), // note sign reversal
            psi:read_degrees(buf, 11),
            dpsi:read_degrees(buf, 12)
        }
    }

    fn demands_from_telemetry(buf:[u8; IN_BUF_SIZE]) -> Demands {
        Demands {
            throttle:read_float(buf, 13),
            roll:read_float(buf, 14),
            pitch:read_float(buf, 15),
            yaw:read_float(buf, 16)
        }
    }

    fn write_motors(motors:Motors) -> [u8; OUT_BUF_SIZE] {
        let mut buf = [0u8; OUT_BUF_SIZE]; 
        let motorvals = [motors.m1, motors.m2, motors.m3, motors.m4];
        for j in 0..4 {
            let bytes = (motorvals[j] as f64).to_le_bytes();
            for k in 0..8 {
                buf[j*8+k] = bytes[k];
            }
        }
        buf
    }

    // We have to bind client socket to some address
    let motor_client_socket = UdpSocket::bind("0.0.0.0:0")?;

    // Bind server socket to address,port that client will connect to
    let telemetry_server_socket = UdpSocket::bind("127.0.0.1:5001")?;

    println!("Hit the Play button ...");

    let alt_hold_pid = pids::make_alt_hold(ALT_HOLD_KP, ALT_HOLD_KI);

    let angle_pid = pids::make_angle(RATE_KP, RATE_KI, RATE_KD, RATE_KF, LEVEL_KP);

    let mixer = quadxbf::QuadXbf { };

    let mut pids: [pids::Controller; 2] = [angle_pid, alt_hold_pid];

    // Loop forever, waiting for client
    loop {

        // Get incoming telemetry values
        let mut in_buf = [0; IN_BUF_SIZE]; 
        telemetry_server_socket.recv_from(&mut in_buf)?;

        // Sim sends negative time value on halt
        let time = read_float(in_buf, 0);
        if time < 0.0 { 
            break Ok(()); 
        }

        // Convert simulator time to microseconds
        let usec = (time * 1e6) as u32;

        // Build vehicle state 
        let vstate = state_from_telemetry(in_buf);

        // Get incoming stick demands
        let mut stick_demands = demands_from_telemetry(in_buf);

        // Reset PID controllers on zero throttle
        let pid_reset = stick_demands.throttle < 0.05;

        // Rescale throttle [-1,+1] => [0,1]
        stick_demands.throttle = rescale(stick_demands.throttle, -1.0, 1.0, 0.0, 1.0);

        // let motors = Motors {m1: 0.0, m2: 0.0, m3:0.0, m4:0.0};
        let motors = step(&stick_demands, &vstate, &mut pids, &pid_reset, &usec, &mixer);

        let out_buf = write_motors(motors);

        motor_client_socket.send_to(&out_buf, "127.0.0.1:5000")?;
    }
}