pub fn rescale(
val: f32,
oldmin: f32,
oldmax: f32,
newmin: f32,
newmax: f32,
) -> f32Examples found in repository?
examples/multisim.rs (line 120)
23fn main() -> std::io::Result<()> {
24
25 const IN_BUF_SIZE:usize = 17*8; // 17 doubles in
26 const OUT_BUF_SIZE:usize = 4*8; // 4 doubles out
27
28 fn read_float(buf:[u8; IN_BUF_SIZE], idx:usize) -> f32 {
29 let mut dst = [0u8; 8];
30 let beg = 8 * idx;
31 let end = beg + 8;
32 dst.clone_from_slice(&buf[beg..end]);
33 f64::from_le_bytes(dst) as f32
34 }
35
36 fn read_degrees(buf:[u8; IN_BUF_SIZE], idx:usize) -> f32 {
37 rad2deg(read_float(buf, idx))
38 }
39
40 fn state_from_telemetry(buf:[u8; IN_BUF_SIZE]) -> VehicleState {
41 VehicleState {
42 x:read_float(buf, 1),
43 dx:read_float(buf, 2),
44 y:read_float(buf, 3),
45 dy:read_float(buf, 4),
46 z:-read_float(buf, 5), // NED => ENU
47 dz:-read_float(buf, 6), // NED => ENU
48 phi:read_degrees(buf, 7),
49 dphi:read_degrees(buf, 8),
50 theta:-read_degrees(buf, 9), // note sign reversal
51 dtheta:-read_degrees(buf, 10), // note sign reversal
52 psi:read_degrees(buf, 11),
53 dpsi:read_degrees(buf, 12)
54 }
55 }
56
57 fn demands_from_telemetry(buf:[u8; IN_BUF_SIZE]) -> Demands {
58 Demands {
59 throttle:read_float(buf, 13),
60 roll:read_float(buf, 14),
61 pitch:read_float(buf, 15),
62 yaw:read_float(buf, 16)
63 }
64 }
65
66 fn write_motors(motors:Motors) -> [u8; OUT_BUF_SIZE] {
67 let mut buf = [0u8; OUT_BUF_SIZE];
68 let motorvals = [motors.m1, motors.m2, motors.m3, motors.m4];
69 for j in 0..4 {
70 let bytes = (motorvals[j] as f64).to_le_bytes();
71 for k in 0..8 {
72 buf[j*8+k] = bytes[k];
73 }
74 }
75 buf
76 }
77
78 // We have to bind client socket to some address
79 let motor_client_socket = UdpSocket::bind("0.0.0.0:0")?;
80
81 // Bind server socket to address,port that client will connect to
82 let telemetry_server_socket = UdpSocket::bind("127.0.0.1:5001")?;
83
84 println!("Hit the Play button ...");
85
86 let alt_hold_pid = pids::make_alt_hold(ALT_HOLD_KP, ALT_HOLD_KI);
87
88 let angle_pid = pids::make_angle(RATE_KP, RATE_KI, RATE_KD, RATE_KF, LEVEL_KP);
89
90 let mixer = quadxbf::QuadXbf { };
91
92 let mut pids: [pids::Controller; 2] = [angle_pid, alt_hold_pid];
93
94 // Loop forever, waiting for client
95 loop {
96
97 // Get incoming telemetry values
98 let mut in_buf = [0; IN_BUF_SIZE];
99 telemetry_server_socket.recv_from(&mut in_buf)?;
100
101 // Sim sends negative time value on halt
102 let time = read_float(in_buf, 0);
103 if time < 0.0 {
104 break Ok(());
105 }
106
107 // Convert simulator time to microseconds
108 let usec = (time * 1e6) as u32;
109
110 // Build vehicle state
111 let vstate = state_from_telemetry(in_buf);
112
113 // Get incoming stick demands
114 let mut stick_demands = demands_from_telemetry(in_buf);
115
116 // Reset PID controllers on zero throttle
117 let pid_reset = stick_demands.throttle < 0.05;
118
119 // Rescale throttle [-1,+1] => [0,1]
120 stick_demands.throttle = rescale(stick_demands.throttle, -1.0, 1.0, 0.0, 1.0);
121
122 // let motors = Motors {m1: 0.0, m2: 0.0, m3:0.0, m4:0.0};
123 let motors = step(&stick_demands, &vstate, &mut pids, &pid_reset, &usec, &mixer);
124
125 let out_buf = write_motors(motors);
126
127 motor_client_socket.send_to(&out_buf, "127.0.0.1:5000")?;
128 }
129}