Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 :
16 : /* Notify display driver for 128 x 64 pixel displays */
17 : #include "AP_Notify_config.h"
18 :
19 : #if HAL_DISPLAY_ENABLED
20 :
21 : #include "Display.h"
22 :
23 : #include "Display_SH1106_I2C.h"
24 : #include "Display_SSD1306_I2C.h"
25 : #include "Display_SITL.h"
26 :
27 : #include "AP_Notify.h"
28 :
29 : #include <stdio.h>
30 : #include <AP_GPS/AP_GPS.h>
31 : #include <AP_BattMonitor/AP_BattMonitor.h>
32 :
33 : extern const AP_HAL::HAL& hal;
34 :
35 : // Bit Map
36 : // Rotate right 90 degrees
37 : // Ex: 0x7C, 0x12, 0x11, 0x12, 0x7C, // 'A'
38 : // 0XXXXX00
39 : // 000X00X0
40 : // 000X000X
41 : // 000X00X0
42 : // 0XXXXX00
43 : static const uint8_t _font[] = {
44 : #ifdef AP_NOTIFY_DISPLAY_USE_EMOJI
45 : // 0x Group
46 : 0x00, 0x00, 0x00, 0x00, 0x00, // 00
47 : 0x3E, 0x5B, 0x4F, 0x5B, 0x3E, // 01
48 : 0x3E, 0x6B, 0x4F, 0x6B, 0x3E, // 02
49 : 0x1C, 0x3E, 0x7C, 0x3E, 0x1C, // 03
50 : 0x18, 0x3C, 0x7E, 0x3C, 0x18, // 04
51 : 0x1C, 0x57, 0x7D, 0x57, 0x1C, // 05
52 : 0x1C, 0x5E, 0x7F, 0x5E, 0x1C, // 06
53 : 0x00, 0x18, 0x3C, 0x18, 0x00, // 07
54 : 0xFF, 0xE7, 0xC3, 0xE7, 0xFF, // 08
55 : 0x00, 0x18, 0x24, 0x18, 0x00, // 09
56 : 0xFF, 0xE7, 0xDB, 0xE7, 0xFF, // 0A
57 : 0x30, 0x48, 0x3A, 0x06, 0x0E, // 0B
58 : 0x26, 0x29, 0x79, 0x29, 0x26, // 0C
59 : 0x40, 0x7F, 0x05, 0x05, 0x07, // 0D
60 : 0x40, 0x7F, 0x05, 0x25, 0x3F, // 0E
61 : 0x5A, 0x3C, 0xE7, 0x3C, 0x5A, // 0F
62 : // 1x Group
63 : 0x7F, 0x3E, 0x1C, 0x1C, 0x08, // 10
64 : 0x08, 0x1C, 0x1C, 0x3E, 0x7F, // 11
65 : 0x14, 0x22, 0x7F, 0x22, 0x14, // 12
66 : 0x5F, 0x5F, 0x00, 0x5F, 0x5F, // 13
67 : 0x06, 0x09, 0x7F, 0x01, 0x7F, // 14
68 : 0x00, 0x66, 0x89, 0x95, 0x6A, // 15
69 : 0x60, 0x60, 0x60, 0x60, 0x60, // 16
70 : 0x94, 0xA2, 0xFF, 0xA2, 0x94, // 17
71 : 0x08, 0x04, 0x7E, 0x04, 0x08, // 18
72 : 0x10, 0x20, 0x7E, 0x20, 0x10, // 19
73 : 0x08, 0x08, 0x2A, 0x1C, 0x08, // 1A
74 : 0x08, 0x1C, 0x2A, 0x08, 0x08, // 1B
75 : 0x1E, 0x10, 0x10, 0x10, 0x10, // 1C
76 : 0x0C, 0x1E, 0x0C, 0x1E, 0x0C, // 1D
77 : 0x30, 0x38, 0x3E, 0x38, 0x30, // 1E
78 : 0x06, 0x0E, 0x3E, 0x0E, 0x06, // 1F
79 : #endif
80 : // 2x Group
81 : 0x00, 0x00, 0x00, 0x00, 0x00, // ' '
82 : 0x00, 0x00, 0x5F, 0x00, 0x00, // '!'
83 : 0x00, 0x07, 0x00, 0x07, 0x00, // '"'
84 : 0x14, 0x7F, 0x14, 0x7F, 0x14, // '#'
85 : 0x24, 0x2A, 0x7F, 0x2A, 0x12, // '$'
86 : 0x23, 0x13, 0x08, 0x64, 0x62, // '%'
87 : 0x36, 0x49, 0x56, 0x20, 0x50, // '&'
88 : 0x00, 0x08, 0x07, 0x03, 0x00, // '''
89 : 0x00, 0x1C, 0x22, 0x41, 0x00, // '('
90 : 0x00, 0x41, 0x22, 0x1C, 0x00, // ')'
91 : 0x2A, 0x1C, 0x7F, 0x1C, 0x2A, // '*'
92 : 0x08, 0x08, 0x3E, 0x08, 0x08, // '+'
93 : 0x00, 0x80, 0x70, 0x30, 0x00, // ','
94 : 0x08, 0x08, 0x08, 0x08, 0x08, // '-'
95 : 0x00, 0x00, 0x60, 0x60, 0x00, // '.'
96 : 0x20, 0x10, 0x08, 0x04, 0x02, // '/'
97 : // 3x Group
98 : 0x3E, 0x51, 0x49, 0x45, 0x3E, // '0'
99 : 0x00, 0x42, 0x7F, 0x40, 0x00, // '1'
100 : 0x72, 0x49, 0x49, 0x49, 0x46, // '2'
101 : 0x21, 0x41, 0x49, 0x4D, 0x33, // '3'
102 : 0x18, 0x14, 0x12, 0x7F, 0x10, // '4'
103 : 0x27, 0x45, 0x45, 0x45, 0x39, // '5'
104 : 0x3C, 0x4A, 0x49, 0x49, 0x31, // '6'
105 : 0x41, 0x21, 0x11, 0x09, 0x07, // '7'
106 : 0x36, 0x49, 0x49, 0x49, 0x36, // '8'
107 : 0x46, 0x49, 0x49, 0x29, 0x1E, // '9'
108 : 0x00, 0x00, 0x14, 0x00, 0x00, // ':'
109 : 0x00, 0x40, 0x34, 0x00, 0x00, // ';'
110 : 0x00, 0x08, 0x14, 0x22, 0x41, // '<'
111 : 0x14, 0x14, 0x14, 0x14, 0x14, // '='
112 : 0x00, 0x41, 0x22, 0x14, 0x08, // '>'
113 : 0x02, 0x01, 0x59, 0x09, 0x06, // '?'
114 : // 4x Group
115 : 0x3E, 0x41, 0x5D, 0x59, 0x4E, // '@'
116 : 0x7C, 0x12, 0x11, 0x12, 0x7C, // 'A'
117 : 0x7F, 0x49, 0x49, 0x49, 0x36, // 'B'
118 : 0x3E, 0x41, 0x41, 0x41, 0x22, // 'C'
119 : 0x7F, 0x41, 0x41, 0x41, 0x3E, // 'D'
120 : 0x7F, 0x49, 0x49, 0x49, 0x41, // 'E'
121 : 0x7F, 0x09, 0x09, 0x09, 0x01, // 'F'
122 : 0x3E, 0x41, 0x41, 0x51, 0x73, // 'G'
123 : 0x7F, 0x08, 0x08, 0x08, 0x7F, // 'H'
124 : 0x00, 0x41, 0x7F, 0x41, 0x00, // 'I'
125 : 0x20, 0x40, 0x41, 0x3F, 0x01, // 'J'
126 : 0x7F, 0x08, 0x14, 0x22, 0x41, // 'K'
127 : 0x7F, 0x40, 0x40, 0x40, 0x40, // 'L'
128 : 0x7F, 0x02, 0x1C, 0x02, 0x7F, // 'M'
129 : 0x7F, 0x04, 0x08, 0x10, 0x7F, // 'N'
130 : 0x3E, 0x41, 0x41, 0x41, 0x3E, // 'O'
131 : // 5x Group
132 : 0x7F, 0x09, 0x09, 0x09, 0x06, // 'P'
133 : 0x3E, 0x41, 0x51, 0x21, 0x5E, // 'Q'
134 : 0x7F, 0x09, 0x19, 0x29, 0x46, // 'R'
135 : 0x26, 0x49, 0x49, 0x49, 0x32, // 'S'
136 : 0x03, 0x01, 0x7F, 0x01, 0x03, // 'T'
137 : 0x3F, 0x40, 0x40, 0x40, 0x3F, // 'U'
138 : 0x1F, 0x20, 0x40, 0x20, 0x1F, // 'V'
139 : 0x3F, 0x40, 0x38, 0x40, 0x3F, // 'W'
140 : 0x63, 0x14, 0x08, 0x14, 0x63, // 'X'
141 : 0x03, 0x04, 0x78, 0x04, 0x03, // 'Y'
142 : 0x61, 0x59, 0x49, 0x4D, 0x43, // 'Z'
143 : 0x00, 0x7F, 0x41, 0x41, 0x41, // '['
144 : 0x02, 0x04, 0x08, 0x10, 0x20, // '\'
145 : 0x00, 0x41, 0x41, 0x41, 0x7F, // ']'
146 : 0x04, 0x02, 0x01, 0x02, 0x04, // '^'
147 : 0x40, 0x40, 0x40, 0x40, 0x40, // '_'
148 : // 6x Group
149 : 0x00, 0x03, 0x07, 0x08, 0x00, // '`'
150 : 0x20, 0x54, 0x54, 0x78, 0x40, // 'a'
151 : 0x7F, 0x28, 0x44, 0x44, 0x38, // 'b'
152 : 0x38, 0x44, 0x44, 0x44, 0x28, // 'c'
153 : 0x38, 0x44, 0x44, 0x28, 0x7F, // 'd'
154 : 0x38, 0x54, 0x54, 0x54, 0x18, // 'e'
155 : 0x00, 0x08, 0x7E, 0x09, 0x02, // 'f'
156 : 0x18, 0xA4, 0xA4, 0x9C, 0x78, // 'g'
157 : 0x7F, 0x08, 0x04, 0x04, 0x78, // 'h'
158 : 0x00, 0x44, 0x7D, 0x40, 0x00, // 'i'
159 : 0x20, 0x40, 0x40, 0x3D, 0x00, // 'j'
160 : 0x7F, 0x10, 0x28, 0x44, 0x00, // 'k'
161 : 0x00, 0x41, 0x7F, 0x40, 0x00, // 'l'
162 : 0x7C, 0x04, 0x78, 0x04, 0x78, // 'm'
163 : 0x7C, 0x08, 0x04, 0x04, 0x78, // 'n'
164 : 0x38, 0x44, 0x44, 0x44, 0x38, // 'o'
165 : // 7x Group
166 : 0xFC, 0x18, 0x24, 0x24, 0x18, // 'p'
167 : 0x18, 0x24, 0x24, 0x18, 0xFC, // 'q'
168 : 0x7C, 0x08, 0x04, 0x04, 0x08, // 'r'
169 : 0x48, 0x54, 0x54, 0x54, 0x24, // 's'
170 : 0x04, 0x04, 0x3F, 0x44, 0x24, // 't'
171 : 0x3C, 0x40, 0x40, 0x20, 0x7C, // 'u'
172 : 0x1C, 0x20, 0x40, 0x20, 0x1C, // 'v'
173 : 0x3C, 0x40, 0x30, 0x40, 0x3C, // 'w'
174 : 0x44, 0x28, 0x10, 0x28, 0x44, // 'x'
175 : 0x4C, 0x90, 0x90, 0x90, 0x7C, // 'y'
176 : 0x44, 0x64, 0x54, 0x4C, 0x44, // 'z'
177 : 0x00, 0x08, 0x36, 0x41, 0x00, // '{'
178 : 0x00, 0x00, 0x77, 0x00, 0x00, // '|'
179 : 0x00, 0x41, 0x36, 0x08, 0x00, // '}'
180 : 0x02, 0x01, 0x02, 0x04, 0x02, // '~'
181 : #ifdef AP_NOTIFY_DISPLAY_USE_EMOJI
182 : 0x3C, 0x26, 0x23, 0x26, 0x3C, // 7F
183 : // 8x Group
184 : 0x1E, 0xA1, 0xA1, 0x61, 0x12, // 80
185 : 0x3A, 0x40, 0x40, 0x20, 0x7A, // 81
186 : 0x38, 0x54, 0x54, 0x55, 0x59, // 82
187 : 0x21, 0x55, 0x55, 0x79, 0x41, // 83
188 : 0x22, 0x54, 0x54, 0x78, 0x42, // 84
189 : 0x21, 0x55, 0x54, 0x78, 0x40, // 85
190 : 0x20, 0x54, 0x55, 0x79, 0x40, // 86
191 : 0x0C, 0x1E, 0x52, 0x72, 0x12, // 87
192 : 0x39, 0x55, 0x55, 0x55, 0x59, // 88
193 : 0x39, 0x54, 0x54, 0x54, 0x59, // 89
194 : 0x39, 0x55, 0x54, 0x54, 0x58, // 8A
195 : 0x00, 0x00, 0x45, 0x7C, 0x41, // 8B
196 : 0x00, 0x02, 0x45, 0x7D, 0x42, // 8C
197 : 0x00, 0x01, 0x45, 0x7C, 0x40, // 8D
198 : 0x7D, 0x12, 0x11, 0x12, 0x7D, // 8E
199 : 0xF0, 0x28, 0x25, 0x28, 0xF0, // 8F
200 : // 9x Group
201 : 0x7C, 0x54, 0x55, 0x45, 0x00, // 90
202 : 0x20, 0x54, 0x54, 0x7C, 0x54, // 91
203 : 0x7C, 0x0A, 0x09, 0x7F, 0x49, // 92
204 : 0x32, 0x49, 0x49, 0x49, 0x32, // 93
205 : 0x3A, 0x44, 0x44, 0x44, 0x3A, // 94
206 : 0x32, 0x4A, 0x48, 0x48, 0x30, // 95
207 : 0x3A, 0x41, 0x41, 0x21, 0x7A, // 96
208 : 0x3A, 0x42, 0x40, 0x20, 0x78, // 97
209 : 0x00, 0x9D, 0xA0, 0xA0, 0x7D, // 98
210 : 0x3D, 0x42, 0x42, 0x42, 0x3D, // 99
211 : 0x3D, 0x40, 0x40, 0x40, 0x3D, // 9A
212 : 0x3C, 0x24, 0xFF, 0x24, 0x24, // 9B
213 : 0x48, 0x7E, 0x49, 0x43, 0x66, // 9C
214 : 0x2B, 0x2F, 0xFC, 0x2F, 0x2B, // 9D
215 : 0xFF, 0x09, 0x29, 0xF6, 0x20, // 9E
216 : 0xC0, 0x88, 0x7E, 0x09, 0x03, // 9F
217 : // Ax Group
218 : 0x20, 0x54, 0x54, 0x79, 0x41, // A0
219 : 0x00, 0x00, 0x44, 0x7D, 0x41, // A1
220 : 0x30, 0x48, 0x48, 0x4A, 0x32, // A2
221 : 0x38, 0x40, 0x40, 0x22, 0x7A, // A3
222 : 0x00, 0x7A, 0x0A, 0x0A, 0x72, // A4
223 : 0x7D, 0x0D, 0x19, 0x31, 0x7D, // A5
224 : 0x26, 0x29, 0x29, 0x2F, 0x28, // A6
225 : 0x26, 0x29, 0x29, 0x29, 0x26, // A7
226 : 0x30, 0x48, 0x4D, 0x40, 0x20, // A8
227 : 0x38, 0x08, 0x08, 0x08, 0x08, // A9
228 : 0x08, 0x08, 0x08, 0x08, 0x38, // AA
229 : 0x2F, 0x10, 0xC8, 0xAC, 0xBA, // AB
230 : 0x2F, 0x10, 0x28, 0x34, 0xFA, // AC
231 : 0x00, 0x00, 0x7B, 0x00, 0x00, // AD
232 : 0x08, 0x14, 0x2A, 0x14, 0x22, // AE
233 : 0x22, 0x14, 0x2A, 0x14, 0x08, // AF
234 : // Bx Group
235 : 0x55, 0x00, 0x55, 0x00, 0x55, // B0
236 : 0xAA, 0x55, 0xAA, 0x55, 0xAA, // B1
237 : 0xFF, 0x55, 0xFF, 0x55, 0xFF, // B2
238 : 0x00, 0x00, 0x00, 0xFF, 0x00, // B3
239 : 0x10, 0x10, 0x10, 0xFF, 0x00, // B4
240 : 0x14, 0x14, 0x14, 0xFF, 0x00, // B5
241 : 0x10, 0x10, 0xFF, 0x00, 0xFF, // B6
242 : 0x10, 0x10, 0xF0, 0x10, 0xF0, // B7
243 : 0x14, 0x14, 0x14, 0xFC, 0x00, // B8
244 : 0x14, 0x14, 0xF7, 0x00, 0xFF, // B9
245 : 0x00, 0x00, 0xFF, 0x00, 0xFF, // BA
246 : 0x14, 0x14, 0xF4, 0x04, 0xFC, // BB
247 : 0x14, 0x14, 0x17, 0x10, 0x1F, // BC
248 : 0x10, 0x10, 0x1F, 0x10, 0x1F, // BD
249 : 0x14, 0x14, 0x14, 0x1F, 0x00, // BE
250 : 0x10, 0x10, 0x10, 0xF0, 0x00, // BF
251 : // Cx Group
252 : 0x00, 0x00, 0x00, 0x1F, 0x10, // C0
253 : 0x10, 0x10, 0x10, 0x1F, 0x10, // C1
254 : 0x10, 0x10, 0x10, 0xF0, 0x10, // C2
255 : 0x00, 0x00, 0x00, 0xFF, 0x10, // C3
256 : 0x10, 0x10, 0x10, 0x10, 0x10, // C4
257 : 0x10, 0x10, 0x10, 0xFF, 0x10, // C5
258 : 0x00, 0x00, 0x00, 0xFF, 0x14, // C6
259 : 0x00, 0x00, 0xFF, 0x00, 0xFF, // C7
260 : 0x00, 0x00, 0x1F, 0x10, 0x17, // C8
261 : 0x00, 0x00, 0xFC, 0x04, 0xF4, // C9
262 : 0x14, 0x14, 0x17, 0x10, 0x17, // CA
263 : 0x14, 0x14, 0xF4, 0x04, 0xF4, // CB
264 : 0x00, 0x00, 0xFF, 0x00, 0xF7, // CC
265 : 0x14, 0x14, 0x14, 0x14, 0x14, // CD
266 : 0x14, 0x14, 0xF7, 0x00, 0xF7, // CE
267 : 0x14, 0x14, 0x14, 0x17, 0x14, // CF
268 : // Dx Group
269 : 0x10, 0x10, 0x1F, 0x10, 0x1F, // D0
270 : 0x14, 0x14, 0x14, 0xF4, 0x14, // D1
271 : 0x10, 0x10, 0xF0, 0x10, 0xF0, // D2
272 : 0x00, 0x00, 0x1F, 0x10, 0x1F, // D3
273 : 0x00, 0x00, 0x00, 0x1F, 0x14, // D4
274 : 0x00, 0x00, 0x00, 0xFC, 0x14, // D5
275 : 0x00, 0x00, 0xF0, 0x10, 0xF0, // D6
276 : 0x10, 0x10, 0xFF, 0x10, 0xFF, // D7
277 : 0x14, 0x14, 0x14, 0xFF, 0x14, // D8
278 : 0x10, 0x10, 0x10, 0x1F, 0x00, // D9
279 : 0x00, 0x00, 0x00, 0xF0, 0x10, // DA
280 : 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, // DB
281 : 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, // DC
282 : 0xFF, 0xFF, 0xFF, 0x00, 0x00, // DD
283 : 0x00, 0x00, 0x00, 0xFF, 0xFF, // DE
284 : 0x0F, 0x0F, 0x0F, 0x0F, 0x0F, // DF
285 : // Ex Group
286 : 0x38, 0x44, 0x44, 0x38, 0x44, // E0
287 : 0xFC, 0x4A, 0x4A, 0x4A, 0x34, // E1
288 : 0x7E, 0x02, 0x02, 0x06, 0x06, // E2
289 : 0x02, 0x7E, 0x02, 0x7E, 0x02, // E3
290 : 0x63, 0x55, 0x49, 0x41, 0x63, // E4
291 : 0x38, 0x44, 0x44, 0x3C, 0x04, // E5
292 : 0x40, 0x7E, 0x20, 0x1E, 0x20, // E6
293 : 0x06, 0x02, 0x7E, 0x02, 0x02, // E7
294 : 0x99, 0xA5, 0xE7, 0xA5, 0x99, // E8
295 : 0x1C, 0x2A, 0x49, 0x2A, 0x1C, // E9
296 : 0x4C, 0x72, 0x01, 0x72, 0x4C, // EA
297 : 0x30, 0x4A, 0x4D, 0x4D, 0x30, // EB
298 : 0x30, 0x48, 0x78, 0x48, 0x30, // EC
299 : 0xBC, 0x62, 0x5A, 0x46, 0x3D, // ED
300 : 0x3E, 0x49, 0x49, 0x49, 0x00, // EE
301 : 0x7E, 0x01, 0x01, 0x01, 0x7E, // EF
302 : // Fx Group
303 : 0x2A, 0x2A, 0x2A, 0x2A, 0x2A, // F0
304 : 0x44, 0x44, 0x5F, 0x44, 0x44, // F1
305 : 0x40, 0x51, 0x4A, 0x44, 0x40, // F2
306 : 0x40, 0x44, 0x4A, 0x51, 0x40, // F3
307 : 0x00, 0x00, 0xFF, 0x01, 0x03, // F4
308 : 0xE0, 0x80, 0xFF, 0x00, 0x00, // F5
309 : 0x08, 0x08, 0x6B, 0x6B, 0x08, // F6
310 : 0x36, 0x12, 0x36, 0x24, 0x36, // F7
311 : 0x06, 0x0F, 0x09, 0x0F, 0x06, // F8
312 : 0x00, 0x00, 0x18, 0x18, 0x00, // F9
313 : 0x00, 0x00, 0x10, 0x10, 0x00, // FA
314 : 0x30, 0x40, 0xFF, 0x01, 0x01, // FB
315 : 0x00, 0x1F, 0x01, 0x01, 0x1E, // FC
316 : 0x00, 0x19, 0x1D, 0x17, 0x12, // FD
317 : 0x00, 0x3C, 0x3C, 0x3C, 0x3C, // FE
318 : 0x00, 0x00, 0x00, 0x00, 0x00 // FF
319 : #endif
320 : };
321 :
322 : #ifdef AP_NOTIFY_DISPLAY_USE_EMOJI
323 : static_assert(ARRAY_SIZE(_font) == 1280, "_font is correct size");
324 : #else
325 : static_assert(ARRAY_SIZE(_font) == 475, "_font is correct size");
326 : #endif
327 :
328 0 : Display_Backend *Display::probe_i2c_display(uint8_t bus, probe_i2c_display_probefn_t probe)
329 : {
330 :
331 0 : const auto dev_ptr = hal.i2c_mgr->get_device_ptr(bus, NOTIFY_DISPLAY_I2C_ADDR);
332 0 : if (dev_ptr == nullptr) {
333 0 : return nullptr;
334 : }
335 0 : Display_Backend *ret = probe(*dev_ptr);
336 0 : if (ret == nullptr) {
337 0 : delete dev_ptr;
338 : }
339 0 : return ret;
340 : }
341 :
342 2094 : bool Display::init(void)
343 : {
344 : // exit immediately if already initialised
345 2094 : if (_driver != nullptr) {
346 0 : return true;
347 : }
348 :
349 : // initialise driver
350 69102 : FOREACH_I2C(i) {
351 8376 : switch (pNotify->_display_type) {
352 0 : case DISPLAY_SSD1306: {
353 0 : _driver = probe_i2c_display(i, Display_SSD1306_I2C::probe);
354 0 : break;
355 : }
356 0 : case DISPLAY_SH1106: {
357 0 : _driver = probe_i2c_display(i, Display_SH1106_I2C::probe);
358 0 : break;
359 : }
360 0 : case DISPLAY_SITL: {
361 : #ifdef WITH_SITL_OSD
362 : _driver = Display_SITL::probe(); // never fails
363 : #elif CONFIG_HAL_BOARD == HAL_BOARD_SITL
364 0 : ::fprintf(stderr, "SITL Display ineffective without --osd\n");
365 : #endif
366 0 : break;
367 : }
368 8376 : case DISPLAY_OFF:
369 : default:
370 8376 : break;
371 : }
372 8376 : if (_driver != nullptr) {
373 0 : break;
374 : }
375 : }
376 :
377 2094 : if (_driver == nullptr) {
378 2094 : return false;
379 : }
380 :
381 : // update all on display
382 0 : update_all();
383 0 : _driver->hw_update();
384 :
385 0 : return true;
386 : }
387 :
388 0 : void Display::update()
389 : {
390 : // max update frequency 2Hz
391 : static uint8_t timer = 0;
392 0 : if (timer++ < 25) {
393 0 : return;
394 : }
395 0 : timer = 0;
396 :
397 0 : if (AP_Notify::flags.armed) {
398 0 : if (_screenpage != 1) {
399 0 : _driver->clear_screen();
400 0 : update_arm(3);
401 0 : _screenpage = 1;
402 0 : _driver->hw_update(); //update hw once , do not transmition to display in fly
403 : }
404 0 : return;
405 : }
406 :
407 0 : if (_screenpage != 2) {
408 0 : _driver->clear_screen(); //once clear screen when page changed
409 0 : _screenpage = 2;
410 : }
411 :
412 0 : update_all();
413 0 : _driver->hw_update(); //update at 2 Hz in disarmed mode
414 :
415 : }
416 :
417 0 : void Display::update_all()
418 : {
419 0 : if(!BIT_IS_SET(_send_text_scr_override, 0)) {
420 0 : update_text(0);
421 : }
422 :
423 0 : if(!BIT_IS_SET(_send_text_scr_override, 1)) {
424 0 : update_mode(1);
425 : }
426 :
427 : #if AP_BATTERY_ENABLED
428 0 : if(!BIT_IS_SET(_send_text_scr_override, 2)) {
429 0 : update_battery(2);
430 : }
431 : #endif
432 : #if AP_GPS_ENABLED
433 0 : if(!BIT_IS_SET(_send_text_scr_override, 3)) {
434 0 : update_gps(3);
435 : }
436 : #endif
437 :
438 0 : if(!BIT_IS_SET(_send_text_scr_override, 4)) {
439 : //update_gps_sats(4);
440 0 : update_prearm(4);
441 : }
442 :
443 0 : if(!BIT_IS_SET(_send_text_scr_override, 5)) {
444 0 : update_ekf(5);
445 : }
446 0 : }
447 :
448 0 : void Display::send_text_blocking(const char *text, uint8_t r)
449 : {
450 0 : if (text == nullptr) {
451 0 : return;
452 : }
453 0 : if (r >= DISPLAY_TEXT_NUM_ROWS) {
454 0 : return;
455 : }
456 0 : BIT_SET(_send_text_scr_override, r);
457 0 : char txt [DISPLAY_MESSAGE_SIZE] = {};
458 0 : memset(txt, ' ', DISPLAY_MESSAGE_SIZE);
459 0 : memcpy(txt, text, strnlen(text, DISPLAY_MESSAGE_SIZE));
460 0 : draw_text(COLUMN(0), ROW(r), txt);
461 : }
462 :
463 0 : void Display::release_text(uint8_t r)
464 : {
465 0 : BIT_CLEAR(_send_text_scr_override, r);
466 0 : }
467 :
468 0 : void Display::draw_text(uint16_t x, uint16_t y, const char* c)
469 : {
470 0 : if (nullptr == c) {
471 0 : return;
472 : }
473 0 : while (*c != 0) {
474 : #ifndef AP_NOTIFY_DISPLAY_USE_EMOJI
475 0 : if (*c >= ' ' && *c <= '~') {
476 0 : draw_char(x, y, *c - ' ');
477 : } else {
478 : // convert oob characters to spaces
479 0 : draw_char(x, y, 0);
480 : }
481 : #else
482 : draw_char(x, y, *c);
483 : #endif
484 0 : x += 7;
485 0 : c++;
486 : }
487 : }
488 :
489 0 : void Display::draw_char(uint16_t x, uint16_t y, const char c)
490 : {
491 : uint8_t line;
492 :
493 : // draw char to pixel
494 0 : for (uint8_t i = 0; i < 6; i++) {
495 0 : if (i == 5) {
496 0 : line = 0;
497 : } else {
498 0 : line = _font[(c * 5) + i];
499 : }
500 :
501 0 : for (uint8_t j = 0; j < 8; j++) {
502 0 : if (line & 1) {
503 0 : _driver->set_pixel(x + i, y + j);
504 : } else {
505 0 : _driver->clear_pixel(x + i, y + j);
506 : }
507 0 : line >>= 1;
508 : }
509 : }
510 0 : }
511 :
512 0 : void Display::update_arm(uint8_t r)
513 : {
514 0 : if (AP_Notify::flags.armed) {
515 0 : draw_text(COLUMN(0), ROW(r), ">>>>> ARMED! <<<<<");
516 : } else {
517 0 : draw_text(COLUMN(0), ROW(r), " disarmed ");
518 : }
519 0 : }
520 :
521 0 : void Display::update_prearm(uint8_t r)
522 : {
523 0 : if (AP_Notify::flags.pre_arm_check) {
524 0 : draw_text(COLUMN(0), ROW(r), "Prearm: passed ");
525 : } else {
526 0 : draw_text(COLUMN(0), ROW(r), "Prearm: failed ");
527 : }
528 0 : }
529 :
530 : #if AP_GPS_ENABLED
531 0 : void Display::update_gps(uint8_t r)
532 : {
533 : static const char * gpsfixname[] = {"Other", "NoGPS","NoFix","2D","3D","DGPS", "RTK f", "RTK F"};
534 : char msg [DISPLAY_MESSAGE_SIZE];
535 : const char * fixname;
536 0 : switch (AP_Notify::flags.gps_status) {
537 0 : case AP_GPS_FixType::NO_GPS:
538 0 : fixname = gpsfixname[1];
539 0 : break;
540 0 : case AP_GPS_FixType::NONE:
541 0 : fixname = gpsfixname[2];
542 0 : break;
543 0 : case AP_GPS_FixType::FIX_2D:
544 0 : fixname = gpsfixname[3];
545 0 : break;
546 0 : case AP_GPS_FixType::FIX_3D:
547 0 : fixname = gpsfixname[4];
548 0 : break;
549 0 : case AP_GPS_FixType::DGPS:
550 0 : fixname = gpsfixname[5];
551 0 : break;
552 0 : case AP_GPS_FixType::RTK_FLOAT:
553 0 : fixname = gpsfixname[6];
554 0 : break;
555 0 : case AP_GPS_FixType::RTK_FIXED:
556 0 : fixname = gpsfixname[7];
557 0 : break;
558 0 : default:
559 0 : fixname = gpsfixname[0];
560 0 : break;
561 : }
562 0 : snprintf(msg, DISPLAY_MESSAGE_SIZE, "GPS:%-5s Sats:%2u", fixname, (unsigned)AP_Notify::flags.gps_num_sats) ;
563 0 : draw_text(COLUMN(0), ROW(r), msg);
564 0 : }
565 :
566 0 : void Display::update_gps_sats(uint8_t r)
567 : {
568 0 : draw_text(COLUMN(0), ROW(r), "Sats:");
569 0 : draw_char(COLUMN(8), ROW(r), (AP_Notify::flags.gps_num_sats / 10) + '0');
570 0 : draw_char(COLUMN(9), ROW(r), (AP_Notify::flags.gps_num_sats % 10) + '0');
571 0 : }
572 : #endif
573 :
574 0 : void Display::update_ekf(uint8_t r)
575 : {
576 0 : if (AP_Notify::flags.ekf_bad) {
577 0 : draw_text(COLUMN(0), ROW(r), "EKF: fail");
578 : } else {
579 0 : draw_text(COLUMN(0), ROW(r), "EKF: ok ");
580 : }
581 0 : }
582 :
583 : #if AP_BATTERY_ENABLED
584 0 : void Display::update_battery(uint8_t r)
585 : {
586 : char msg [DISPLAY_MESSAGE_SIZE];
587 0 : AP_BattMonitor &battery = AP::battery();
588 : uint8_t pct;
589 0 : if (battery.capacity_remaining_pct(pct)) {
590 0 : snprintf(msg, DISPLAY_MESSAGE_SIZE, "BAT:%4.2fV %2d%% ", (double)battery.voltage(), pct) ;
591 : } else {
592 0 : snprintf(msg, DISPLAY_MESSAGE_SIZE, "BAT:%4.2fV --%% ", (double)battery.voltage()) ;
593 : }
594 0 : draw_text(COLUMN(0), ROW(r), msg);
595 0 : }
596 : #endif // AP_BATTERY_ENABLED
597 :
598 0 : void Display::update_mode(uint8_t r)
599 : {
600 : char msg [DISPLAY_MESSAGE_SIZE];
601 0 : if (pNotify->get_flight_mode_str()) {
602 0 : snprintf(msg, DISPLAY_MESSAGE_SIZE, "Mode: %s", pNotify->get_flight_mode_str()) ;
603 0 : draw_text(COLUMN(0), ROW(r), msg);
604 : }
605 0 : }
606 :
607 0 : void Display::update_text_empty(uint8_t r)
608 : {
609 0 : char msg [DISPLAY_MESSAGE_SIZE] = {};
610 0 : memset(msg, ' ', sizeof(msg)-1);
611 0 : _movedelay = 0;
612 0 : _mstartpos = 0;
613 0 : draw_text(COLUMN(0), ROW(r), msg);
614 0 : }
615 :
616 0 : void Display::update_text(uint8_t r)
617 : {
618 0 : char msg [DISPLAY_MESSAGE_SIZE] = {};
619 0 : char txt [NOTIFY_TEXT_BUFFER_SIZE] = {};
620 :
621 0 : const bool text_is_valid = AP_HAL::millis() - pNotify->_send_text_updated_millis < _send_text_valid_millis;
622 0 : if (!text_is_valid) {
623 0 : update_text_empty(r);
624 0 : return;
625 : }
626 :
627 0 : if (_movedelay > 0) {
628 0 : _movedelay--;
629 0 : return;
630 : }
631 :
632 0 : snprintf(txt, sizeof(txt), "%s", pNotify->get_text());
633 :
634 0 : memset(msg, ' ', sizeof(msg)-1); // leave null termination
635 0 : const uint8_t len = strlen(&txt[_mstartpos]);
636 0 : const uint8_t to_copy = (len < sizeof(msg)-1) ? len : (sizeof(msg)-1);
637 0 : memcpy(msg, &txt[_mstartpos], to_copy);
638 :
639 0 : if (len <= sizeof(msg)-1) {
640 : // end-of-message reached; pause scrolling a while
641 0 : _movedelay = 4;
642 : // reset startpos so we start scrolling from the start again:
643 0 : _mstartpos = 0;
644 : } else {
645 0 : _mstartpos++;
646 : }
647 :
648 0 : draw_text(COLUMN(0), ROW(0), msg);
649 : }
650 :
651 : #endif // HAL_DISPLAY_ENABLED
|
