Line data Source code
1 : #pragma once
2 :
3 : #include "AP_Frsky_SPort.h"
4 :
5 : #if AP_FRSKY_SPORT_PASSTHROUGH_ENABLED
6 :
7 : #include <AP_RCTelemetry/AP_RCTelemetry.h>
8 : #include <AP_Mission/AP_Mission_config.h>
9 :
10 : #include "AP_Frsky_SPortParser.h"
11 : #include "AP_Frsky_MAVlite.h"
12 :
13 : #include "AP_Frsky_Telem.h"
14 :
15 : #if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
16 : #include "AP_Frsky_MAVlite_SPortToMAVlite.h"
17 : #include "AP_Frsky_MAVlite_MAVliteToSPort.h"
18 : #include "AP_Frsky_MAVliteMsgHandler.h"
19 :
20 : #define SPORT_TX_PACKET_DUPLICATES 1 // number of duplicates packets we send (fport only)
21 : #endif
22 :
23 : class AP_Frsky_SPort_Passthrough : public AP_Frsky_SPort, public AP_RCTelemetry
24 : {
25 : public:
26 :
27 3 : AP_Frsky_SPort_Passthrough(AP_HAL::UARTDriver *port, bool use_external_data, AP_Frsky_Parameters *&frsky_parameters) :
28 : AP_Frsky_SPort(port),
29 : AP_RCTelemetry(WFQ_LAST_ITEM),
30 3 : _frsky_parameters(frsky_parameters),
31 3 : _use_external_data(use_external_data)
32 : {
33 3 : singleton = this;
34 3 : }
35 :
36 0 : static AP_Frsky_SPort_Passthrough *get_singleton(void) {
37 0 : return singleton;
38 : }
39 :
40 : bool init() override;
41 : bool init_serial_port() override;
42 :
43 : // passthrough WFQ scheduler
44 : bool is_packet_ready(uint8_t idx, bool queue_empty) override;
45 : void process_packet(uint8_t idx) override;
46 : void adjust_packet_weight(bool queue_empty) override;
47 : // setup ready for passthrough operation
48 : void setup_wfq_scheduler(void) override;
49 :
50 : bool get_next_msg_chunk(void) override;
51 :
52 : bool get_telem_data(sport_packet_t* packet_array, uint8_t &packet_count, const uint8_t max_size) override;
53 : #if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
54 : bool set_telem_data(const uint8_t frame, const uint16_t appid, const uint32_t data) override;
55 : #endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
56 :
57 102 : void queue_text_message(MAV_SEVERITY severity, const char *text) override
58 : {
59 102 : AP_RCTelemetry::queue_message(severity, text);
60 102 : }
61 :
62 : enum PassthroughPacketType : uint8_t {
63 : TEXT = 0, // 0x5000 status text (dynamic)
64 : ATTITUDE = 1, // 0x5006 Attitude and range (dynamic)
65 : GPS_LAT = 2, // 0x800 GPS lat
66 : GPS_LON = 3, // 0x800 GPS lon
67 : VEL_YAW = 4, // 0x5005 Vel and Yaw
68 : AP_STATUS = 5, // 0x5001 AP status
69 : GPS_STATUS = 6, // 0x5002 GPS status
70 : HOME = 7, // 0x5004 Home
71 : BATT_2 = 8, // 0x5008 Battery 2 status
72 : BATT_1 = 9, // 0x5008 Battery 1 status
73 : PARAM = 10, // 0x5007 parameters
74 : RPM = 11, // 0x500A rpm sensors 1 and 2
75 : UDATA = 12, // user data
76 : #if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
77 : MAV = 13, // mavlite
78 : #endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
79 : TERRAIN = 14, // 0x500B terrain data
80 : WIND = 15, // 0x500C wind data
81 : #if AP_MISSION_ENABLED
82 : WAYPOINT = 16, // 0x500D waypoint data
83 : #endif // AP_MISSION_ENABLED
84 : WFQ_LAST_ITEM // must be last
85 : };
86 :
87 : protected:
88 :
89 : void send() override;
90 :
91 : private:
92 :
93 : AP_Frsky_Parameters *&_frsky_parameters;
94 :
95 : enum PassthroughParam : uint8_t {
96 : NONE = 0,
97 : FRAME_TYPE = 1,
98 : BATT_FS_VOLTAGE = 2,
99 : BATT_FS_CAPACITY = 3,
100 : BATT_CAPACITY_1 = 4,
101 : BATT_CAPACITY_2 = 5,
102 : TELEMETRY_FEATURES = 6
103 : };
104 :
105 : enum PassthroughFeatures : uint8_t {
106 : BIDIR = 0,
107 : SCRIPTING = 1,
108 : };
109 :
110 : // methods to convert flight controller data to FrSky SPort Passthrough (OpenTX) format
111 : uint32_t calc_param(void);
112 : uint32_t calc_batt(uint8_t instance);
113 : uint32_t calc_ap_status(void);
114 : uint32_t calc_home(void);
115 : uint32_t calc_velandyaw(void);
116 : uint32_t calc_attiandrng(void);
117 : uint32_t calc_rpm(void);
118 : uint32_t calc_terrain(void);
119 : uint32_t calc_wind(void);
120 : #if AP_MISSION_ENABLED
121 : uint32_t calc_waypoint(void);
122 : #endif // AP_MISSION_ENABLED
123 :
124 : // use_external_data is set when this library will
125 : // be providing data to another transport, such as FPort
126 : bool _use_external_data;
127 :
128 : struct {
129 : sport_packet_t packet;
130 : bool pending;
131 : } external_data;
132 :
133 : struct {
134 : uint32_t chunk; // a "chunk" (four characters/bytes) at a time of the queued message to be sent
135 : uint8_t repeats; // send each message "chunk" 3 times to make sure the entire messsage gets through without getting cut
136 : uint8_t char_index; // index of which character to get in the message
137 : } _msg_chunk;
138 :
139 : // passthrough default sensor id
140 : uint8_t downlink_sensor_id = 0x1B;
141 :
142 : #if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
143 : // bidirectional sport telemetry
144 : struct {
145 : uint8_t uplink_sensor_id = 0x0D;
146 : uint8_t downlink1_sensor_id = 0x34;
147 : uint8_t downlink2_sensor_id = 0x67;
148 : uint8_t tx_packet_duplicates;
149 : ObjectBuffer_TS<AP_Frsky_SPort::sport_packet_t> rx_packet_queue{SPORT_PACKET_QUEUE_LENGTH};
150 : ObjectBuffer_TS<AP_Frsky_SPort::sport_packet_t> tx_packet_queue{SPORT_PACKET_QUEUE_LENGTH};
151 : } _SPort_bidir;
152 :
153 : AP_Frsky_SPortParser _sport_handler;
154 : AP_Frsky_MAVlite_SPortToMAVlite sport_to_mavlite;
155 : AP_Frsky_MAVlite_MAVliteToSPort mavlite_to_sport;
156 :
157 : // tx/rx sport packet processing
158 : void queue_rx_packet(const AP_Frsky_SPort::sport_packet_t sp);
159 : void process_rx_queue(void);
160 : void process_tx_queue(void);
161 :
162 : // create an object to handle incoming mavlite messages; a
163 : // callback method is provided to allow the handler to send responses
164 : bool send_message(const AP_Frsky_MAVlite_Message &txmsg);
165 : AP_Frsky_MAVliteMsgHandler mavlite{FUNCTOR_BIND_MEMBER(&AP_Frsky_SPort_Passthrough::send_message, bool, const AP_Frsky_MAVlite_Message &)};
166 : #endif
167 : void set_sensor_id(AP_Int8 idx, uint8_t &sensor);
168 : void send_sport_frame(uint8_t frame, uint16_t appid, uint32_t data);
169 :
170 : // true if we need to respond to the last polling byte
171 : bool is_passthrough_byte(const uint8_t byte) const;
172 :
173 : uint8_t _paramID;
174 :
175 : uint32_t calc_gps_status(void);
176 :
177 : static AP_Frsky_SPort_Passthrough *singleton;
178 : };
179 :
180 : namespace AP {
181 : AP_Frsky_SPort_Passthrough *frsky_passthrough_telem();
182 : };
183 :
184 :
185 : #endif // AP_FRSKY_SPORT_PASSTHROUGH_ENABLED
|
