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 : MSP telemetry library backend base class
16 : */
17 : #pragma once
18 :
19 : #include "AP_MSP_config.h"
20 :
21 : #if HAL_MSP_ENABLED
22 :
23 : #include <AP_RCTelemetry/AP_RCTelemetry.h>
24 : #include <AP_SerialManager/AP_SerialManager.h>
25 : #include <AP_OSD/AP_OSD.h>
26 : #if AP_MSP_VIDEOTX_ENABLED
27 : #include <AP_VideoTX/AP_VideoTX.h>
28 : #endif
29 :
30 : #include "msp.h"
31 :
32 : #include <time.h>
33 :
34 : #if AP_MSP_VIDEOTX_ENABLED
35 : #define MSP_TIME_SLOT_MAX 13
36 : #else
37 : #define MSP_TIME_SLOT_MAX 12
38 : #endif
39 : #define CELLFULL 4.35
40 : #define MSP_TXT_BUFFER_SIZE 15U // 11 + 3 utf8 chars + terminator
41 : #define MSP_TXT_VISIBLE_CHARS 11U
42 :
43 : class AP_MSP;
44 :
45 : class AP_MSP_Telem_Backend : AP_RCTelemetry
46 : {
47 : friend AP_MSP;
48 : public:
49 : AP_MSP_Telem_Backend(AP_HAL::UARTDriver *uart);
50 :
51 : typedef struct battery_state_s {
52 : float batt_current_a;
53 : float batt_consumed_mah;
54 : float batt_voltage_v;
55 : int32_t batt_capacity_mah;
56 : uint8_t batt_cellcount;
57 : MSP::battery_state_e batt_state;
58 : } battery_state_t;
59 :
60 : typedef struct PACKED gps_state_s {
61 : uint8_t fix_type;
62 : uint8_t num_sats;
63 : int32_t lat;
64 : int32_t lon;
65 : uint16_t alt_m;
66 : uint16_t speed_cms;
67 : uint16_t ground_course_dd;
68 : } gps_state_t;
69 :
70 : typedef struct airspeed_state_s {
71 : float airspeed_estimate_ms;
72 : bool airspeed_have_estimate;
73 : } airspeed_state_t;
74 :
75 : typedef struct home_state_s {
76 : bool home_is_set;
77 : float home_bearing_cd;
78 : uint32_t home_distance_m;
79 : int32_t rel_altitude_cm;
80 : } home_state_t;
81 :
82 : // init - perform required initialisation
83 : virtual bool init() override;
84 : virtual bool init_uart();
85 : virtual void enable_warnings();
86 : virtual void hide_osd_items(void);
87 :
88 : // MSP tx/rx processors
89 : void process_incoming_data(); // incoming data
90 : void process_outgoing_data(); // push outgoing data
91 :
92 : #if HAL_WITH_MSP_DISPLAYPORT
93 : // displayport commands
94 : // betaflight/src/main/io/displayport_msp.c
95 : virtual void msp_displayport_heartbeat();
96 : virtual void msp_displayport_grab();
97 : virtual void msp_displayport_release();
98 : virtual void msp_displayport_clear_screen();
99 : virtual void msp_displayport_draw_screen();
100 : virtual void msp_displayport_write_string(uint8_t col, uint8_t row, bool blink, const char *string, const uint8_t font_table);
101 : virtual void msp_displayport_set_options(const uint8_t font_index, const uint8_t screen_resolution);
102 : #endif
103 : protected:
104 : enum msp_packet_type : uint8_t {
105 : EMPTY_SLOT = 0,
106 : NAME,
107 : STATUS,
108 : CONFIG,
109 : RAW_GPS,
110 : COMP_GPS,
111 : ATTITUDE,
112 : ALTITUDE,
113 : ANALOG,
114 : BATTERY_STATE,
115 : #if HAL_WITH_ESC_TELEM
116 : ESC_SENSOR_DATA,
117 : #endif
118 : RTC_DATETIME,
119 : #if AP_MSP_VIDEOTX_ENABLED
120 : VTX_PARAMETERS,
121 : #endif
122 : };
123 :
124 : const uint16_t msp_packet_type_map[MSP_TIME_SLOT_MAX] = {
125 : 0,
126 : MSP_NAME,
127 : MSP_STATUS,
128 : MSP_OSD_CONFIG,
129 : MSP_RAW_GPS,
130 : MSP_COMP_GPS,
131 : MSP_ATTITUDE,
132 : MSP_ALTITUDE,
133 : MSP_ANALOG,
134 : MSP_BATTERY_STATE,
135 : #if HAL_WITH_ESC_TELEM
136 : MSP_ESC_SENSOR_DATA,
137 : #endif
138 : MSP_RTC,
139 : #if AP_MSP_VIDEOTX_ENABLED
140 : MSP_VTX_CONFIG,
141 : #endif
142 : };
143 :
144 : /* UTF-8 encodings
145 : U+2191 ↑ e2 86 91 UPWARDS ARROW
146 : U+2197 ↗ e2 86 97 NORTH EAST ARROW
147 : U+2192 → e2 86 92 RIGHTWARDS ARROW
148 : U+2198 ↘ e2 86 98 SOUTH EAST ARROW
149 : U+2193 ↓ e2 86 93 DOWNWARDS ARROW
150 : U+2199 ↙ e2 86 99 SOUTH WEST ARROW
151 : U+2190 ← e2 86 90 LEFTWARDS ARROW
152 : U+2196 ↖ e2 86 96 NORTH WEST ARROW
153 : */
154 : static constexpr uint8_t arrows[8] = {0x91, 0x97, 0x92, 0x98, 0x93, 0x99, 0x90, 0x96};
155 :
156 : static const uint8_t message_scroll_time_ms = 200;
157 : static const uint8_t message_scroll_delay = 5;
158 :
159 : // each backend can hide/unhide items dynamically
160 : uint64_t osd_hidden_items_bitmask;
161 :
162 : // MSP decoder status
163 : MSP::msp_port_t _msp_port;
164 :
165 : #if AP_MSP_VIDEOTX_ENABLED
166 : // set once the VTX has uploaded its own config (MSP_SET_VTX_CONFIG),
167 : // completing the betaflight-style boot handshake; gates deviceIsReady
168 : bool _vtx_config_received;
169 : // push the VTX config on change, repeated this many times for delivery
170 : // robustness; the air unit does not report its state back so we cannot
171 : // confirm receipt the way the Tramp backend does
172 : static const uint8_t MSP_VTX_CONFIG_PUSH_COUNT = 3;
173 : uint8_t _vtx_config_push_remaining;
174 : // last configured VTX settings pushed to the air unit, to detect changes
175 : struct {
176 : uint8_t band;
177 : uint8_t channel;
178 : uint16_t freq_mhz;
179 : uint16_t power_mw;
180 : bool pitmode;
181 : bool valid;
182 : } _vtx_pushed;
183 : #endif // AP_MSP_VIDEOTX_ENABLED
184 :
185 : // passthrough WFQ scheduler
186 : bool is_packet_ready(uint8_t idx, bool queue_empty) override;
187 : void process_packet(uint8_t idx) override;
188 2373 : void adjust_packet_weight(bool queue_empty) override {}
189 : void setup_wfq_scheduler(void) override;
190 0 : bool get_next_msg_chunk(void) override
191 : {
192 0 : return true;
193 : }
194 :
195 : // telemetry helpers
196 : uint8_t calc_cell_count(float battery_voltage);
197 : virtual float get_vspeed_ms(void) const;
198 : virtual bool get_rssi(float &rssi) const;
199 : virtual void update_home_pos(home_state_t &home_state);
200 : virtual void update_battery_state(battery_state_t &_battery_state);
201 : virtual void update_gps_state(gps_state_t &gps_state);
202 : virtual void update_airspeed(airspeed_state_t &airspeed_state);
203 : virtual void update_flight_mode_str(char *flight_mode_str, uint8_t size, bool wind_enabled);
204 :
205 : // MSP parsing
206 : void msp_process_received_command();
207 : MSP::MSPCommandResult msp_process_command(MSP::msp_packet_t *cmd, MSP::msp_packet_t *reply);
208 : MSP::MSPCommandResult msp_process_sensor_command(uint16_t cmd_msp, MSP::sbuf_t *src);
209 : MSP::MSPCommandResult msp_process_out_command(uint16_t cmd_msp, MSP::sbuf_t *src, MSP::sbuf_t *dst);
210 :
211 : // MSP send
212 : void msp_send_packet(uint16_t cmd, MSP::msp_version_e msp_version, const void *p, uint16_t size, bool is_request);
213 :
214 : // MSP sensor command processing
215 : void msp_handle_opflow(const MSP::msp_opflow_data_message_t &pkt);
216 : void msp_handle_rangefinder(const MSP::msp_rangefinder_data_message_t &pkt);
217 : void msp_handle_gps(const MSP::msp_gps_data_message_t &pkt);
218 : void msp_handle_compass(const MSP::msp_compass_data_message_t &pkt);
219 : void msp_handle_baro(const MSP::msp_baro_data_message_t &pkt);
220 : void msp_handle_airspeed(const MSP::msp_airspeed_data_message_t &pkt);
221 :
222 : // implementation specific helpers
223 : // we only set arming status
224 : // custom masks are needed for vendor specific settings
225 : virtual uint32_t get_osd_flight_mode_bitmask(void);
226 :
227 : virtual bool is_scheduler_enabled() const = 0; // only osd backends should allow a push type telemetry
228 8618 : virtual bool use_msp_thread() const {return true;}; // is this backend hanlded by the MSP thread?
229 : virtual AP_SerialManager::SerialProtocol get_serial_protocol() const = 0;
230 : virtual bool displaying_stats_screen() const;
231 :
232 : // implementation specific MSP out command processing
233 : virtual MSP::MSPCommandResult msp_process_out_api_version(MSP::sbuf_t *dst);
234 : virtual MSP::MSPCommandResult msp_process_out_fc_version(MSP::sbuf_t *dst);
235 : virtual MSP::MSPCommandResult msp_process_out_fc_variant(MSP::sbuf_t *dst);
236 : virtual MSP::MSPCommandResult msp_process_out_uid(MSP::sbuf_t *dst);
237 : virtual MSP::MSPCommandResult msp_process_out_board_info(MSP::sbuf_t *dst);
238 : virtual MSP::MSPCommandResult msp_process_out_build_info(MSP::sbuf_t *dst);
239 : virtual MSP::MSPCommandResult msp_process_out_name(MSP::sbuf_t *dst);
240 : virtual MSP::MSPCommandResult msp_process_out_status(MSP::sbuf_t *dst);
241 : virtual MSP::MSPCommandResult msp_process_out_osd_config(MSP::sbuf_t *dst);
242 : virtual MSP::MSPCommandResult msp_process_out_osd_canvas(MSP::sbuf_t *dst);
243 : virtual MSP::MSPCommandResult msp_process_out_raw_gps(MSP::sbuf_t *dst);
244 : virtual MSP::MSPCommandResult msp_process_out_comp_gps(MSP::sbuf_t *dst);
245 : virtual MSP::MSPCommandResult msp_process_out_attitude(MSP::sbuf_t *dst);
246 : virtual MSP::MSPCommandResult msp_process_out_altitude(MSP::sbuf_t *dst);
247 : virtual MSP::MSPCommandResult msp_process_out_analog(MSP::sbuf_t *dst);
248 : virtual MSP::MSPCommandResult msp_process_out_battery_state(MSP::sbuf_t *dst);
249 : virtual MSP::MSPCommandResult msp_process_out_esc_sensor_data(MSP::sbuf_t *dst);
250 : virtual MSP::MSPCommandResult msp_process_out_rtc(MSP::sbuf_t *dst);
251 : virtual MSP::MSPCommandResult msp_process_out_rc(MSP::sbuf_t *dst);
252 : #if AP_MSP_VIDEOTX_ENABLED
253 : virtual MSP::MSPCommandResult msp_process_out_vtx_config(MSP::sbuf_t *src, MSP::sbuf_t *dst);
254 : virtual MSP::MSPCommandResult msp_process_in_vtx_config(MSP::sbuf_t *src, MSP::sbuf_t *dst);
255 : virtual MSP::MSPCommandResult msp_process_in_vtxtable_powerlevel(MSP::sbuf_t *src, MSP::sbuf_t *dst);
256 : // true when a VTX config push is due this scheduler tick (push on change,
257 : // then repeat MSP_VTX_CONFIG_PUSH_COUNT times)
258 : bool vtx_should_push_config();
259 : #endif // AP_MSP_VIDEOTX_ENABLED
260 :
261 : private:
262 : #if AP_MSP_VIDEOTX_ENABLED
263 : // helpers operating on the AP::vtx() singleton, applying a betaflight-style
264 : // MSP_SET_VTX_CONFIG / MSP_SET_VTXTABLE_POWERLEVEL onto the configured VTX
265 : void msp_vtx_set_band_and_channel(uint8_t band, uint8_t channel);
266 : void msp_vtx_set_frequency(uint16_t freq_mhz);
267 : void msp_vtx_set_power_index(uint8_t index);
268 : uint8_t msp_vtx_get_power_index() const;
269 : void msp_vtx_set_pitmode(bool pitmode);
270 : #endif // AP_MSP_VIDEOTX_ENABLED
271 : };
272 :
273 : #endif //HAL_MSP_ENABLED
|
