mirror of https://github.com/ArduPilot/ardupilot
AP_Frsky_Telem: fix for conditional compilation of bidir support
This commit is contained in:
parent
6dfa42c958
commit
e044cdfde7
|
@ -3,6 +3,10 @@
|
||||||
#include <AP_HAL/AP_HAL.h>
|
#include <AP_HAL/AP_HAL.h>
|
||||||
#include <GCS_MAVLink/GCS_MAVLink.h>
|
#include <GCS_MAVLink/GCS_MAVLink.h>
|
||||||
|
|
||||||
|
#ifndef HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
|
||||||
|
#define HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL 1
|
||||||
|
#endif
|
||||||
|
|
||||||
class AP_Frsky_Backend
|
class AP_Frsky_Backend
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
|
@ -26,10 +30,13 @@ public:
|
||||||
{
|
{
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
|
||||||
virtual bool set_telem_data(const uint8_t frame, const uint16_t appid, const uint32_t data)
|
virtual bool set_telem_data(const uint8_t frame, const uint16_t appid, const uint32_t data)
|
||||||
{
|
{
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
virtual void queue_text_message(MAV_SEVERITY severity, const char *text) { }
|
virtual void queue_text_message(MAV_SEVERITY severity, const char *text) { }
|
||||||
|
|
||||||
|
|
|
@ -1,4 +1,5 @@
|
||||||
#pragma once
|
#pragma once
|
||||||
|
#include "AP_Frsky_Backend.h"
|
||||||
|
|
||||||
/*
|
/*
|
||||||
|
|
||||||
|
@ -26,7 +27,6 @@
|
||||||
|
|
||||||
*/
|
*/
|
||||||
|
|
||||||
|
|
||||||
#define MAVLITE_MAX_PAYLOAD_LEN 31 // 7 float params + cmd_id + options
|
#define MAVLITE_MAX_PAYLOAD_LEN 31 // 7 float params + cmd_id + options
|
||||||
#define MAVLITE_MSG_SPORT_PACKETS_COUNT(LEN) static_cast<uint8_t>(1 + ceilf((LEN-2)/5.0f)) // number of sport packets required to transport a message with LEN payload
|
#define MAVLITE_MSG_SPORT_PACKETS_COUNT(LEN) static_cast<uint8_t>(1 + ceilf((LEN-2)/5.0f)) // number of sport packets required to transport a message with LEN payload
|
||||||
#define SPORT_PACKET_QUEUE_LENGTH static_cast<uint8_t>(30U*MAVLITE_MSG_SPORT_PACKETS_COUNT(MAVLITE_MAX_PAYLOAD_LEN))
|
#define SPORT_PACKET_QUEUE_LENGTH static_cast<uint8_t>(30U*MAVLITE_MSG_SPORT_PACKETS_COUNT(MAVLITE_MAX_PAYLOAD_LEN))
|
||||||
|
|
|
@ -5,6 +5,7 @@
|
||||||
|
|
||||||
extern const AP_HAL::HAL& hal;
|
extern const AP_HAL::HAL& hal;
|
||||||
|
|
||||||
|
#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
|
||||||
/*
|
/*
|
||||||
* Handle the COMMAND_LONG mavlite message
|
* Handle the COMMAND_LONG mavlite message
|
||||||
* for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
|
* for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
|
||||||
|
@ -284,3 +285,4 @@ bool AP_Frsky_MAVliteMsgHandler::send_message(AP_Frsky_MAVlite_Message &txmsg)
|
||||||
{
|
{
|
||||||
return _send_fn(txmsg);
|
return _send_fn(txmsg);
|
||||||
}
|
}
|
||||||
|
#endif
|
|
@ -4,6 +4,7 @@
|
||||||
|
|
||||||
#include "AP_Frsky_SPort.h"
|
#include "AP_Frsky_SPort.h"
|
||||||
|
|
||||||
|
#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
|
||||||
void AP_Frsky_MAVlite_MAVliteToSPort::reset()
|
void AP_Frsky_MAVlite_MAVliteToSPort::reset()
|
||||||
{
|
{
|
||||||
checksum = 0;
|
checksum = 0;
|
||||||
|
@ -96,3 +97,4 @@ void AP_Frsky_MAVlite_MAVliteToSPort::process_byte(const uint8_t b, ObjectBuffer
|
||||||
packet_offs = 2; // skip setting sensorid and frame
|
packet_offs = 2; // skip setting sensorid and frame
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
#endif
|
|
@ -7,6 +7,7 @@
|
||||||
|
|
||||||
#include <stdint.h>
|
#include <stdint.h>
|
||||||
|
|
||||||
|
#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
|
||||||
/*
|
/*
|
||||||
* An instance of this class encodes a MAVlite message into several
|
* An instance of this class encodes a MAVlite message into several
|
||||||
* SPort packets, and pushes them onto the supplied queue.
|
* SPort packets, and pushes them onto the supplied queue.
|
||||||
|
@ -50,3 +51,4 @@ private:
|
||||||
int16_t checksum; // sent at end of packet
|
int16_t checksum; // sent at end of packet
|
||||||
void update_checksum(const uint8_t c);
|
void update_checksum(const uint8_t c);
|
||||||
};
|
};
|
||||||
|
#endif
|
|
@ -2,6 +2,7 @@
|
||||||
|
|
||||||
#include <AP_Math/AP_Math.h>
|
#include <AP_Math/AP_Math.h>
|
||||||
|
|
||||||
|
#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
|
||||||
bool AP_Frsky_MAVlite_Message::get_bytes(uint8_t *bytes, const uint8_t offset, const uint8_t count) const
|
bool AP_Frsky_MAVlite_Message::get_bytes(uint8_t *bytes, const uint8_t offset, const uint8_t count) const
|
||||||
{
|
{
|
||||||
if (offset + count > MAVLITE_MAX_PAYLOAD_LEN) {
|
if (offset + count > MAVLITE_MAX_PAYLOAD_LEN) {
|
||||||
|
@ -53,3 +54,4 @@ void AP_Frsky_MAVlite_Message::bit8_pack(uint8_t &value, const uint8_t bit_value
|
||||||
}
|
}
|
||||||
value |= (bit_value<<bit_offset) & mask;
|
value |= (bit_value<<bit_offset) & mask;
|
||||||
}
|
}
|
||||||
|
#endif
|
|
@ -6,6 +6,7 @@
|
||||||
|
|
||||||
#include <stdint.h>
|
#include <stdint.h>
|
||||||
|
|
||||||
|
#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
|
||||||
class AP_Frsky_MAVlite_Message {
|
class AP_Frsky_MAVlite_Message {
|
||||||
public:
|
public:
|
||||||
// helpers
|
// helpers
|
||||||
|
@ -44,3 +45,4 @@ private:
|
||||||
bool get_bytes(uint8_t *bytes, const uint8_t offset, const uint8_t count) const WARN_IF_UNUSED;
|
bool get_bytes(uint8_t *bytes, const uint8_t offset, const uint8_t count) const WARN_IF_UNUSED;
|
||||||
bool set_bytes(const uint8_t *bytes, const uint8_t offset, const uint8_t count) WARN_IF_UNUSED;
|
bool set_bytes(const uint8_t *bytes, const uint8_t offset, const uint8_t count) WARN_IF_UNUSED;
|
||||||
};
|
};
|
||||||
|
#endif
|
|
@ -2,6 +2,7 @@
|
||||||
|
|
||||||
#include "AP_Frsky_MAVlite.h"
|
#include "AP_Frsky_MAVlite.h"
|
||||||
|
|
||||||
|
#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
|
||||||
void AP_Frsky_MAVlite_SPortToMAVlite::reset(void)
|
void AP_Frsky_MAVlite_SPortToMAVlite::reset(void)
|
||||||
{
|
{
|
||||||
checksum = 0;
|
checksum = 0;
|
||||||
|
@ -102,3 +103,4 @@ void AP_Frsky_MAVlite_SPortToMAVlite::parse(uint8_t byte)
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
#endif
|
|
@ -5,6 +5,7 @@
|
||||||
|
|
||||||
#include <stdint.h>
|
#include <stdint.h>
|
||||||
|
|
||||||
|
#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
|
||||||
/*
|
/*
|
||||||
* An instance of this class decodes a stream of SPort packets into a
|
* An instance of this class decodes a stream of SPort packets into a
|
||||||
* MAVlite message (see AP_Frsky_MAVlite_Message.h). It is expected
|
* MAVlite message (see AP_Frsky_MAVlite_Message.h). It is expected
|
||||||
|
@ -46,3 +47,4 @@ private:
|
||||||
int16_t checksum; // sent at end of packet
|
int16_t checksum; // sent at end of packet
|
||||||
void update_checksum(const uint8_t c);
|
void update_checksum(const uint8_t c);
|
||||||
};
|
};
|
||||||
|
#endif
|
|
@ -545,27 +545,6 @@ uint32_t AP_Frsky_SPort_Passthrough::calc_attiandrng(void)
|
||||||
return attiandrng;
|
return attiandrng;
|
||||||
}
|
}
|
||||||
|
|
||||||
#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
|
|
||||||
/*
|
|
||||||
allow external transports (e.g. FPort), to supply telemetry data
|
|
||||||
*/
|
|
||||||
bool AP_Frsky_SPort_Passthrough::set_telem_data(const uint8_t frame, const uint16_t appid, const uint32_t data)
|
|
||||||
{
|
|
||||||
// queue only Uplink packets
|
|
||||||
if (frame == SPORT_UPLINK_FRAME || frame == SPORT_UPLINK_FRAME_RW) {
|
|
||||||
const AP_Frsky_SPort::sport_packet_t sp {
|
|
||||||
0x00, // this is ignored by process_sport_rx_queue() so no need for a real sensor ID
|
|
||||||
frame,
|
|
||||||
appid,
|
|
||||||
data
|
|
||||||
};
|
|
||||||
|
|
||||||
_SPort_bidir.rx_packet_queue.push_force(sp);
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
/*
|
/*
|
||||||
fetch Sport data for an external transport, such as FPort
|
fetch Sport data for an external transport, such as FPort
|
||||||
*/
|
*/
|
||||||
|
@ -647,8 +626,26 @@ uint16_t AP_Frsky_SPort_Passthrough::prep_number(int32_t number, uint8_t digits,
|
||||||
return res;
|
return res;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
|
||||||
|
/*
|
||||||
|
allow external transports (e.g. FPort), to supply telemetry data
|
||||||
|
*/
|
||||||
|
bool AP_Frsky_SPort_Passthrough::set_telem_data(const uint8_t frame, const uint16_t appid, const uint32_t data)
|
||||||
|
{
|
||||||
|
// queue only Uplink packets
|
||||||
|
if (frame == SPORT_UPLINK_FRAME || frame == SPORT_UPLINK_FRAME_RW) {
|
||||||
|
const AP_Frsky_SPort::sport_packet_t sp {
|
||||||
|
0x00, // this is ignored by process_sport_rx_queue() so no need for a real sensor ID
|
||||||
|
frame,
|
||||||
|
appid,
|
||||||
|
data
|
||||||
|
};
|
||||||
|
|
||||||
|
_SPort_bidir.rx_packet_queue.push_force(sp);
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* Queue uplink packets in the sport rx queue
|
* Queue uplink packets in the sport rx queue
|
||||||
|
|
|
@ -43,7 +43,9 @@ public:
|
||||||
bool get_next_msg_chunk(void) override;
|
bool get_next_msg_chunk(void) override;
|
||||||
|
|
||||||
bool get_telem_data(uint8_t &frame, uint16_t &appid, uint32_t &data) override;
|
bool get_telem_data(uint8_t &frame, uint16_t &appid, uint32_t &data) override;
|
||||||
|
#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
|
||||||
bool set_telem_data(const uint8_t frame, const uint16_t appid, const uint32_t data) override;
|
bool set_telem_data(const uint8_t frame, const uint16_t appid, const uint32_t data) override;
|
||||||
|
#endif
|
||||||
|
|
||||||
void queue_text_message(MAV_SEVERITY severity, const char *text) override
|
void queue_text_message(MAV_SEVERITY severity, const char *text) override
|
||||||
{
|
{
|
||||||
|
|
|
@ -89,6 +89,7 @@ bool AP_Frsky_Telem::_get_telem_data(uint8_t &frame, uint16_t &appid, uint32_t &
|
||||||
return _backend->get_telem_data(frame, appid, data);
|
return _backend->get_telem_data(frame, appid, data);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
|
||||||
bool AP_Frsky_Telem::_set_telem_data(uint8_t frame, uint16_t appid, uint32_t data)
|
bool AP_Frsky_Telem::_set_telem_data(uint8_t frame, uint16_t appid, uint32_t data)
|
||||||
{
|
{
|
||||||
if (_backend == nullptr) {
|
if (_backend == nullptr) {
|
||||||
|
@ -96,6 +97,7 @@ bool AP_Frsky_Telem::_set_telem_data(uint8_t frame, uint16_t appid, uint32_t dat
|
||||||
}
|
}
|
||||||
return _backend->set_telem_data(frame, appid, data);
|
return _backend->set_telem_data(frame, appid, data);
|
||||||
}
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
/*
|
/*
|
||||||
fetch Sport data for an external transport, such as FPort
|
fetch Sport data for an external transport, such as FPort
|
||||||
|
@ -117,6 +119,7 @@ bool AP_Frsky_Telem::get_telem_data(uint8_t &frame, uint16_t &appid, uint32_t &d
|
||||||
return singleton->_get_telem_data(frame, appid, data);
|
return singleton->_get_telem_data(frame, appid, data);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
|
||||||
/*
|
/*
|
||||||
allow external transports (e.g. FPort), to supply telemetry data
|
allow external transports (e.g. FPort), to supply telemetry data
|
||||||
*/
|
*/
|
||||||
|
@ -135,6 +138,7 @@ bool AP_Frsky_Telem::set_telem_data(const uint8_t frame, const uint16_t appid, c
|
||||||
}
|
}
|
||||||
return singleton->_set_telem_data(frame, appid, data);
|
return singleton->_set_telem_data(frame, appid, data);
|
||||||
}
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
namespace AP
|
namespace AP
|
||||||
{
|
{
|
||||||
|
|
|
@ -14,10 +14,6 @@
|
||||||
*/
|
*/
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#ifndef HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
|
|
||||||
#define HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL 1
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#include "AP_Frsky_Backend.h"
|
#include "AP_Frsky_Backend.h"
|
||||||
|
|
||||||
class AP_Frsky_Parameters;
|
class AP_Frsky_Parameters;
|
||||||
|
@ -57,12 +53,11 @@ public:
|
||||||
private:
|
private:
|
||||||
|
|
||||||
AP_Frsky_Backend *_backend;
|
AP_Frsky_Backend *_backend;
|
||||||
|
|
||||||
#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
|
|
||||||
AP_Frsky_Parameters* _frsky_parameters;
|
AP_Frsky_Parameters* _frsky_parameters;
|
||||||
|
|
||||||
// get next telemetry data for external consumers of SPort data (internal function)
|
// get next telemetry data for external consumers of SPort data (internal function)
|
||||||
bool _get_telem_data(uint8_t &frame, uint16_t &appid, uint32_t &data);
|
bool _get_telem_data(uint8_t &frame, uint16_t &appid, uint32_t &data);
|
||||||
|
#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
|
||||||
// set telemetry data from external producer of SPort data (internal function)
|
// set telemetry data from external producer of SPort data (internal function)
|
||||||
bool _set_telem_data(const uint8_t frame, const uint16_t appid, const uint32_t data);
|
bool _set_telem_data(const uint8_t frame, const uint16_t appid, const uint32_t data);
|
||||||
#endif
|
#endif
|
||||||
|
|
Loading…
Reference in New Issue