mirror of https://github.com/ArduPilot/ardupilot
AP_Frsky_Telem: fix for HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL = 0
This commit is contained in:
parent
e86d5e113f
commit
5b7aaae008
|
@ -14,9 +14,9 @@
|
||||||
*/
|
*/
|
||||||
#include "AP_Frsky_Parameters.h"
|
#include "AP_Frsky_Parameters.h"
|
||||||
|
|
||||||
#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
|
|
||||||
|
|
||||||
const AP_Param::GroupInfo AP_Frsky_Parameters::var_info[] = {
|
const AP_Param::GroupInfo AP_Frsky_Parameters::var_info[] = {
|
||||||
|
#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
|
||||||
// @Param: UPLINK_ID
|
// @Param: UPLINK_ID
|
||||||
// @DisplayName: Uplink sensor id
|
// @DisplayName: Uplink sensor id
|
||||||
// @Description: Change the uplink sensor id (SPort only)
|
// @Description: Change the uplink sensor id (SPort only)
|
||||||
|
@ -37,6 +37,7 @@ const AP_Param::GroupInfo AP_Frsky_Parameters::var_info[] = {
|
||||||
// @Values: -1:Disable,7:7,8:8,9:9,10:10,11:11,12:12,13:13,14:14,15:15,16:16,17:17,18:18,19:19,20:20,21:21,22:22,23:23,24:24,25:25,26:26
|
// @Values: -1:Disable,7:7,8:8,9:9,10:10,11:11,12:12,13:13,14:14,15:15,16:16,17:17,18:18,19:19,20:20,21:21,22:22,23:23,24:24,25:25,26:26
|
||||||
// @User: Advanced
|
// @User: Advanced
|
||||||
AP_GROUPINFO("DNLINK2_ID", 3, AP_Frsky_Parameters, _dnlink2_id, 7),
|
AP_GROUPINFO("DNLINK2_ID", 3, AP_Frsky_Parameters, _dnlink2_id, 7),
|
||||||
|
#endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
|
||||||
|
|
||||||
// @Param: DNLINK_ID
|
// @Param: DNLINK_ID
|
||||||
// @DisplayName: Default downlink sensor id
|
// @DisplayName: Default downlink sensor id
|
||||||
|
@ -58,5 +59,3 @@ AP_Frsky_Parameters::AP_Frsky_Parameters()
|
||||||
{
|
{
|
||||||
AP_Param::setup_object_defaults(this, var_info);
|
AP_Param::setup_object_defaults(this, var_info);
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
|
|
||||||
|
|
|
@ -16,7 +16,6 @@
|
||||||
|
|
||||||
#include "AP_Frsky_Telem.h"
|
#include "AP_Frsky_Telem.h"
|
||||||
|
|
||||||
#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
|
|
||||||
#include <AP_HAL/AP_HAL.h>
|
#include <AP_HAL/AP_HAL.h>
|
||||||
#include <AP_Param/AP_Param.h>
|
#include <AP_Param/AP_Param.h>
|
||||||
|
|
||||||
|
@ -33,11 +32,11 @@ public:
|
||||||
|
|
||||||
private:
|
private:
|
||||||
// settable parameters
|
// settable parameters
|
||||||
AP_Int8 _uplink_id;
|
|
||||||
AP_Int8 _dnlink_id;
|
AP_Int8 _dnlink_id;
|
||||||
|
#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
|
||||||
|
AP_Int8 _uplink_id;
|
||||||
AP_Int8 _dnlink1_id;
|
AP_Int8 _dnlink1_id;
|
||||||
AP_Int8 _dnlink2_id;
|
AP_Int8 _dnlink2_id;
|
||||||
|
#endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
|
||||||
AP_Int8 _options;
|
AP_Int8 _options;
|
||||||
};
|
};
|
||||||
|
|
||||||
#endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
|
|
||||||
|
|
|
@ -20,8 +20,8 @@
|
||||||
|
|
||||||
#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
|
#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
|
||||||
#include "AP_Frsky_MAVlite.h"
|
#include "AP_Frsky_MAVlite.h"
|
||||||
#include "AP_Frsky_Parameters.h"
|
|
||||||
#endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
|
#endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
|
||||||
|
#include "AP_Frsky_Parameters.h"
|
||||||
|
|
||||||
/*
|
/*
|
||||||
for FrSky SPort Passthrough
|
for FrSky SPort Passthrough
|
||||||
|
@ -940,6 +940,16 @@ void AP_Frsky_SPort_Passthrough::process_tx_queue()
|
||||||
send_sport_frame(SPORT_DOWNLINK_FRAME, packet.appid, packet.data);
|
send_sport_frame(SPORT_DOWNLINK_FRAME, packet.appid, packet.data);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Send a mavlite message
|
||||||
|
* Message is chunked in sport packets pushed in the tx queue
|
||||||
|
* for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
|
||||||
|
*/
|
||||||
|
bool AP_Frsky_SPort_Passthrough::send_message(const AP_Frsky_MAVlite_Message &txmsg)
|
||||||
|
{
|
||||||
|
return mavlite_to_sport.process(_SPort_bidir.tx_packet_queue, txmsg);
|
||||||
|
}
|
||||||
|
#endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
|
||||||
/*
|
/*
|
||||||
* Utility method to apply constraints in changing sensor id values
|
* Utility method to apply constraints in changing sensor id values
|
||||||
* for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
|
* for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
|
||||||
|
@ -956,17 +966,6 @@ void AP_Frsky_SPort_Passthrough::set_sensor_id(AP_Int8 param_idx, uint8_t &senso
|
||||||
sensor = calc_sensor_id(idx);
|
sensor = calc_sensor_id(idx);
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
|
||||||
* Send a mavlite message
|
|
||||||
* Message is chunked in sport packets pushed in the tx queue
|
|
||||||
* for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
|
|
||||||
*/
|
|
||||||
bool AP_Frsky_SPort_Passthrough::send_message(const AP_Frsky_MAVlite_Message &txmsg)
|
|
||||||
{
|
|
||||||
return mavlite_to_sport.process(_SPort_bidir.tx_packet_queue, txmsg);
|
|
||||||
}
|
|
||||||
#endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
|
|
||||||
|
|
||||||
namespace AP
|
namespace AP
|
||||||
{
|
{
|
||||||
AP_Frsky_SPort_Passthrough *frsky_passthrough_telem()
|
AP_Frsky_SPort_Passthrough *frsky_passthrough_telem()
|
||||||
|
|
|
@ -149,7 +149,6 @@ private:
|
||||||
AP_Frsky_MAVlite_SPortToMAVlite sport_to_mavlite;
|
AP_Frsky_MAVlite_SPortToMAVlite sport_to_mavlite;
|
||||||
AP_Frsky_MAVlite_MAVliteToSPort mavlite_to_sport;
|
AP_Frsky_MAVlite_MAVliteToSPort mavlite_to_sport;
|
||||||
|
|
||||||
void set_sensor_id(AP_Int8 idx, uint8_t &sensor);
|
|
||||||
// tx/rx sport packet processing
|
// tx/rx sport packet processing
|
||||||
void queue_rx_packet(const AP_Frsky_SPort::sport_packet_t sp);
|
void queue_rx_packet(const AP_Frsky_SPort::sport_packet_t sp);
|
||||||
void process_rx_queue(void);
|
void process_rx_queue(void);
|
||||||
|
@ -160,7 +159,7 @@ private:
|
||||||
bool send_message(const AP_Frsky_MAVlite_Message &txmsg);
|
bool send_message(const AP_Frsky_MAVlite_Message &txmsg);
|
||||||
AP_Frsky_MAVliteMsgHandler mavlite{FUNCTOR_BIND_MEMBER(&AP_Frsky_SPort_Passthrough::send_message, bool, const AP_Frsky_MAVlite_Message &)};
|
AP_Frsky_MAVliteMsgHandler mavlite{FUNCTOR_BIND_MEMBER(&AP_Frsky_SPort_Passthrough::send_message, bool, const AP_Frsky_MAVlite_Message &)};
|
||||||
#endif
|
#endif
|
||||||
|
void set_sensor_id(AP_Int8 idx, uint8_t &sensor);
|
||||||
void send_sport_frame(uint8_t frame, uint16_t appid, uint32_t data);
|
void send_sport_frame(uint8_t frame, uint16_t appid, uint32_t data);
|
||||||
|
|
||||||
// true if we need to respond to the last polling byte
|
// true if we need to respond to the last polling byte
|
||||||
|
|
Loading…
Reference in New Issue