1use crate::drawable::{Align, Drawable, NumOfLine};
2use crate::symbol::{Symbol, SymbolIndex, SymbolTable};
3use crate::telemetry::Telemetry;
4use crate::{AspectRatio, PixelRatio};
5#[allow(unused_imports)] use micromath::F32Ext;
7
8pub struct Pitchladder {
9 horizental_symbols: [SymbolIndex; 7],
10 vertical_symbols: [SymbolIndex; 5],
11 char_pixel_ratio: PixelRatio,
12 fov_height: isize,
13}
14
15type Point = (isize, isize);
16
17impl Pitchladder {
18 pub fn new(
19 symbol_table: &SymbolTable,
20 fov: u8,
21 char_pixel_ratio: PixelRatio,
22 aspect_ratio: AspectRatio,
23 ) -> Self {
24 let mut ladder = Self {
25 horizental_symbols: [0; 7],
26 vertical_symbols: [0; 5],
27 char_pixel_ratio,
28 fov_height: aspect_ratio.diagonal_to_height(fov.into()) as isize,
29 };
30 let slice = &symbol_table.as_slice();
31 let symbols = &slice[Symbol::LineTop as usize..Symbol::LineBottom as usize + 1];
32 ladder.horizental_symbols.copy_from_slice(&symbols);
33 let symbols = &slice[Symbol::LineLeft as usize..Symbol::LineRight as usize + 1];
34 ladder.vertical_symbols.copy_from_slice(&symbols);
35 ladder
36 }
37
38 fn draw_line<F: FnMut(isize, isize)>(&self, p0: Point, p1: Point, mut callback: F) {
39 let (x0, y0) = p0;
40 let (x1, y1) = p1;
41 let dx = (x1 - x0).abs();
42 let sx = if x0 < x1 { 1 } else { -1 };
43 let dy = -(y1 - y0).abs();
44 let sy = if y0 < y1 { 1 } else { -1 };
45 let mut err = dx + dy;
46 let (mut x, mut y) = (x0, y0);
47 loop {
48 callback(x, y);
49 if x == x1 && y == y1 {
50 break;
51 }
52 let err2 = err * 2;
53 if err2 >= dy {
54 err += dy;
55 x += sx;
56 }
57 if err2 <= dx {
58 err += dx;
59 y += sy;
60 }
61 }
62 }
63}
64
65impl<T: AsMut<[u8]>> Drawable<T> for Pitchladder {
66 fn align(&self) -> Align {
67 Align::Center
68 }
69
70 fn draw(&self, telemetry: &Telemetry, output: &mut [T]) -> NumOfLine {
71 let height = output.len() as isize;
72 let width = output[0].as_mut().len() as isize;
73
74 let roll = match telemetry.attitude.roll {
75 -180..=-91 => 180 + telemetry.attitude.roll,
76 -90..=90 => telemetry.attitude.roll,
77 91..=180 => telemetry.attitude.roll - 180,
78 _ => 0,
79 };
80 let pitch = -telemetry.attitude.pitch as isize;
81
82 let ratio = self.char_pixel_ratio;
83 let ratio = (ratio.0 as isize * width) as f32 / (ratio.1 as isize * height) as f32;
84 let k1000 = ((roll as f32).to_radians().tan() * ratio * 1000.0) as isize; if -70 <= roll && roll <= 70 {
87 let symbols = &self.horizental_symbols;
88 let callback = |x, y| {
89 let y_index = y / symbols.len() as isize;
90 if 0 <= y_index && y_index < height && 0 <= x && x < width {
91 let symbol = symbols[y as usize % symbols.len()];
92 output[y_index as usize].as_mut()[x as usize] = symbol;
93 }
94 };
95 let num_symbols = symbols.len() as isize;
96 let y_offset = pitch * height * num_symbols / self.fov_height + num_symbols / 2;
97 let y_center = ((width / 2 * height * num_symbols / width) * k1000 / 1000) as isize;
98 let y0 = -y_center + (height / 2) * num_symbols + y_offset;
99 let y1 = y_center + (height / 2) * num_symbols + y_offset;
100 self.draw_line((0, y0), (width, y1), callback);
101 } else {
102 let symbols = &self.vertical_symbols;
103 let num_symbols = symbols.len() as isize;
104 let y_offset = pitch * height / self.fov_height;
105 let x_offset = width / 2 * num_symbols + num_symbols / 2;
106 for y in 0..height {
107 let x = (y - (height / 2) - y_offset) * num_symbols * 1000 / k1000 + x_offset;
108 let x_index = x / num_symbols as isize;
109 if 0 <= x_index && x_index < width {
110 let symbol = symbols[x as usize % symbols.len()];
111 output[y as usize].as_mut()[x_index as usize] = symbol;
112 }
113 }
114 }
115 0
116 }
117}
118
119#[cfg(test)]
120mod test {
121 use crate::drawable::Drawable;
122 use crate::symbol::default_symbol_table;
123 use crate::telemetry::Telemetry;
124 use crate::test_utils::{fill_edge, to_utf8_string};
125 use crate::{AspectRatio, PixelRatio};
126
127 use super::Pitchladder;
128
129 const PX_RATIO: PixelRatio = pixel_ratio!(10:22);
130 const ASPECT_RATIO: AspectRatio = aspect_ratio!(16:10);
131
132 #[test]
133 fn test_horizental() {
134 let mut buffer = [[0u8; 32]; 9];
135 let pitch_ladder = Pitchladder::new(&default_symbol_table(), 150, PX_RATIO, ASPECT_RATIO);
136 let telemetry = Telemetry::default();
137 pitch_ladder.draw(&telemetry, &mut buffer);
138 fill_edge(&mut buffer);
139 let expected = ". .\
140 . .\
141 . .\
142 . .\
143 ────────────────────────────────\
144 . .\
145 . .\
146 . .\
147 . .";
148 assert_eq!(expected, to_utf8_string(&buffer));
149 }
150
151 #[test]
152 fn test_pitch() {
153 let mut buffer = [[0u8; 32]; 9];
154 let pitch_ladder = Pitchladder::new(&default_symbol_table(), 150, PX_RATIO, ASPECT_RATIO);
155 let mut telemetry = Telemetry::default();
156 telemetry.attitude.pitch = 7;
157 pitch_ladder.draw(&telemetry, &mut buffer);
158 fill_edge(&mut buffer);
159 let expected = ". .\
160 . .\
161 . .\
162 ▁▁▁▁▁▁▁▁▁▁▁▁▁▁▁▁▁▁▁▁▁▁▁▁▁▁▁▁▁▁▁▁\
163 . .\
164 . .\
165 . .\
166 . .\
167 . .";
168 assert_eq!(expected, to_utf8_string(&buffer));
169 }
170
171 #[test]
172 fn test_shallow_roll_left() {
173 let mut buffer = [[0u8; 32]; 9];
174 let pitch_ladder = Pitchladder::new(&default_symbol_table(), 18, PX_RATIO, ASPECT_RATIO);
175 let mut telemetry = Telemetry::default();
176 telemetry.attitude.roll = -15;
177 pitch_ladder.draw(&telemetry, &mut buffer);
178 fill_edge(&mut buffer);
179 let expected = ". .\
180 . .\
181 . ▁▁⎽\
182 . ▁⎽⎼──⎻⎺▔ .\
183 . ▁⎽⎽⎼─⎻⎺⎺▔ .\
184 . ▁⎽⎼─⎻⎻⎺▔ .\
185 ⎻⎺▔▔ .\
186 . .\
187 . .";
188 assert_eq!(expected, to_utf8_string(&buffer));
189 }
190
191 #[test]
192 fn test_shallow_roll_right() {
193 let mut buffer = [[0u8; 32]; 9];
194 let pitch_ladder = Pitchladder::new(&default_symbol_table(), 18, PX_RATIO, ASPECT_RATIO);
195 let mut telemetry = Telemetry::default();
196 telemetry.attitude.roll = 15;
197 pitch_ladder.draw(&telemetry, &mut buffer);
198 fill_edge(&mut buffer);
199 let expected = ". .\
200 . .\
201 ⎼⎽▁▁ .\
202 . ▔⎺⎻─⎼⎼⎽▁ .\
203 . ▔⎺⎺⎻─⎼⎽⎽▁ .\
204 . ▔⎺⎻──⎼⎽▁ .\
205 . ▔▔⎺\
206 . .\
207 . .";
208 assert_eq!(expected, to_utf8_string(&buffer));
209 }
210
211 #[test]
212 fn test_roll_left() {
213 let mut buffer = [[0u8; 32]; 9];
214 let pitch_ladder = Pitchladder::new(&default_symbol_table(), 18, PX_RATIO, ASPECT_RATIO);
215 let mut telemetry = Telemetry::default();
216 telemetry.attitude.roll = -30;
217 pitch_ladder.draw(&telemetry, &mut buffer);
218 fill_edge(&mut buffer);
219 let expected = ". ▁⎼\
220 . ▁⎼─⎺▔.\
221 . ▁⎼─⎺▔ .\
222 . ▁⎼─⎺▔ .\
223 . ▁⎼─⎺▔ .\
224 . ▁⎼─⎺▔ .\
225 . ▁⎼─⎺▔ .\
226 . ▁⎼─⎺▔ .\
227 ─⎺▔ .";
228 assert_eq!(expected, to_utf8_string(&buffer));
229 }
230
231 #[test]
232 fn test_roll_right() {
233 let mut buffer = [[0u8; 32]; 9];
234 let pitch_ladder = Pitchladder::new(&default_symbol_table(), 18, PX_RATIO, ASPECT_RATIO);
235 let mut telemetry = Telemetry::default();
236 telemetry.attitude.roll = 45;
237 pitch_ladder.draw(&telemetry, &mut buffer);
238 fill_edge(&mut buffer);
239 let expected = ". ⎼▔⎺⎼▁ .\
240 . ▔─▁ .\
241 . ⎻▁ .\
242 . ⎻⎽▁ .\
243 . ⎺⎼▁ .\
244 . ▔─▁ .\
245 . ─▁ .\
246 . ⎻⎽▁ .\
247 . ⎺⎼▁ .";
248 assert_eq!(expected, to_utf8_string(&buffer));
249 }
250
251 #[test]
252 fn test_deep_roll_left() {
253 let mut buffer = [[0u8; 32]; 9];
254 let pitch_ladder = Pitchladder::new(&default_symbol_table(), 18, PX_RATIO, ASPECT_RATIO);
255 let mut telemetry = Telemetry::default();
256 telemetry.attitude.roll = -80;
257 pitch_ladder.draw(&telemetry, &mut buffer);
258 fill_edge(&mut buffer);
259 let expected = ". ⎪ .\
260 . ⎪ .\
261 . ⎪ .\
262 . | .\
263 . | .\
264 . | .\
265 . ▏ .\
266 . ▏ .\
267 . ▏ .";
268 assert_eq!(expected, to_utf8_string(&buffer));
269 }
270
271 #[test]
272 fn test_roll_more_than_70_with_pitch() {
273 let mut buffer = [[0u8; 32]; 9];
274 let pitch_ladder = Pitchladder::new(&default_symbol_table(), 18, PX_RATIO, ASPECT_RATIO);
275 let mut telemetry = Telemetry::default();
276 telemetry.attitude.roll = 71;
277 telemetry.attitude.pitch = 10;
278 pitch_ladder.draw(&telemetry, &mut buffer);
279 fill_edge(&mut buffer);
280 let expected = ". ▏ .\
281 . ▏ .\
282 . | .\
283 . ⎪ .\
284 . ⎪ .\
285 . ▏ .\
286 . ▏ .\
287 . | .\
288 . ⎪ .";
289 assert_eq!(expected, to_utf8_string(&buffer));
290 }
291
292 #[test]
293 fn test_vertical() {
294 let mut buffer = [[0u8; 32]; 9];
295 let pitch_ladder = Pitchladder::new(&default_symbol_table(), 18, PX_RATIO, ASPECT_RATIO);
296 let mut telemetry = Telemetry::default();
297 telemetry.attitude.roll = 90;
298 pitch_ladder.draw(&telemetry, &mut buffer);
299 fill_edge(&mut buffer);
300 let expected = ". | .\
301 . | .\
302 . | .\
303 . | .\
304 . | .\
305 . | .\
306 . | .\
307 . | .\
308 . | .";
309 assert_eq!(expected, to_utf8_string(&buffer));
310 }
311
312 #[test]
313 fn test_ranges() {
314 let mut telemetry = Telemetry::default();
315 let mut buffer = [[0u8; 32]; 9];
316 let pitch_ladder = Pitchladder::new(&default_symbol_table(), 18, PX_RATIO, ASPECT_RATIO);
317 for i in 0..180 {
318 telemetry.attitude.roll = i as i16;
319 pitch_ladder.draw(&telemetry, &mut buffer);
320 }
321 }
322}