Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Frsky_Telem: use ringbuffer for Frsky message queueing

This commit is contained in:
floaledm 2016-09-20 21:40:17 -05:00 committed by Tom Pittenger
parent d58c193c90
commit ff0d0b1aa5
2 changed files with 26 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp
View File

@ -23,6 +23,8 @@
#include "AP_Frsky_Telem.h"
extern const AP_HAL::HAL& hal;
ObjectArray<mavlink_statustext_t> AP_Frsky_Telem::_statustext_queue(FRSKY_TELEM_PAYLOAD_STATUS_CAPACITY);
//constructor
AP_Frsky_Telem::AP_Frsky_Telem(AP_AHRS &ahrs, const AP_BattMonitor &battery, const RangeFinder &rng) :
_ahrs(ahrs),
@ -95,7 +97,7 @@ void AP_Frsky_Telem::send_SPort_Passthrough(void)
// build mavlink message queue for sensor_status_flags
check_sensor_status_flags();
// if there's any message in the queue, start sending them chunk by chunk; three times each chunk
if (_msg.sent_idx != _msg.queued_idx) {
if (!_statustext_queue.empty()) {
send_uint32(DIY_FIRST_ID, get_next_msg_chunk());
return;
}
@ -417,16 +419,16 @@ uint32_t AP_Frsky_Telem::get_next_msg_chunk(void)
{
if (_msg_chunk.repeats == 0) {
_msg_chunk.chunk = 0;
uint8_t character = _msg.data[_msg.sent_idx].text[_msg_chunk.char_index++];
uint8_t character = _statustext_queue[0]->text[_msg_chunk.char_index++];
if (character) {
_msg_chunk.chunk |= character<<24;
character = _msg.data[_msg.sent_idx].text[_msg_chunk.char_index++];
character = _statustext_queue[0]->text[_msg_chunk.char_index++];
if (character) {
_msg_chunk.chunk |= character<<16;
character = _msg.data[_msg.sent_idx].text[_msg_chunk.char_index++];
character = _statustext_queue[0]->text[_msg_chunk.char_index++];
if (character) {
_msg_chunk.chunk |= character<<8;
character = _msg.data[_msg.sent_idx].text[_msg_chunk.char_index++];
character = _statustext_queue[0]->text[_msg_chunk.char_index++];
if (character) {
_msg_chunk.chunk |= character;
}
@ -436,16 +438,16 @@ uint32_t AP_Frsky_Telem::get_next_msg_chunk(void)
if (!character) { // we've reached the end of the message (string terminated by '\0')
_msg_chunk.char_index = 0;
// add severity which is sent as the MSB of the last three bytes of the last chunk (bits 24, 16, and 8) since a character is on 7 bits
_msg_chunk.chunk |= (_msg.data[_msg.sent_idx].severity & 0x4)<<21;
_msg_chunk.chunk |= (_msg.data[_msg.sent_idx].severity & 0x2)<<14;
_msg_chunk.chunk |= (_msg.data[_msg.sent_idx].severity & 0x1)<<7;
_msg_chunk.chunk |= (_statustext_queue[0]->severity & 0x4)<<21;
_msg_chunk.chunk |= (_statustext_queue[0]->severity & 0x2)<<14;
_msg_chunk.chunk |= (_statustext_queue[0]->severity & 0x1)<<7;
}
}
_msg_chunk.repeats++;
if (_msg_chunk.repeats > 2) { // repeat each message chunk 3 times to ensure transmission
_msg_chunk.repeats = 0;
if (_msg_chunk.char_index == 0) { // if we're ready for the next message
_msg.sent_idx = (_msg.sent_idx + 1) % MSG_BUFFER_LENGTH; // flag the current message as sent
_statustext_queue.remove(0);
}
}
return _msg_chunk.chunk;
@ -457,9 +459,15 @@ uint32_t AP_Frsky_Telem::get_next_msg_chunk(void)
*/
void AP_Frsky_Telem::queue_message(MAV_SEVERITY severity, const char *text)
{
_msg.data[_msg.queued_idx].severity = severity;
_msg.data[_msg.queued_idx].text = text;
_msg.queued_idx = (_msg.queued_idx + 1) % MSG_BUFFER_LENGTH;
mavlink_statustext_t statustext{};
statustext.severity = severity;
strncpy(statustext.text, text, sizeof(statustext.text));
// The force push will ensure comm links do not block other comm links forever if they fail.
// If we push to a full buffer then we overwrite the oldest entry, effectively removing the
// block but not until the buffer fills up.
_statustext_queue.push_force(statustext);
}
/*

20
libraries/AP_Frsky_Telem/AP_Frsky_Telem.h
View File

@ -21,8 +21,9 @@
#include <AP_Notify/AP_Notify.h>
#include <AP_RangeFinder/AP_RangeFinder.h>
#include <AP_SerialManager/AP_SerialManager.h>
#include <AP_HAL/utility/RingBuffer.h>
#define MSG_BUFFER_LENGTH 5 // size of the message buffer queue (number of messages waiting to be sent)
#define FRSKY_TELEM_PAYLOAD_STATUS_CAPACITY 5 // size of the message buffer queue (max number of messages waiting to be sent)
/*
for FrSky D protocol (D-receivers)
@ -119,7 +120,7 @@ public:
// init - perform required initialisation
void init(const AP_SerialManager &serial_manager, const char *firmware_str, const uint8_t mav_type, AP_Float *fs_batt_voltage = nullptr, AP_Float *fs_batt_mah = nullptr, uint32_t *ap_value = nullptr);
// add statustext message to FrSky lib queue.
// add statustext message to FrSky lib message queue
void queue_message(MAV_SEVERITY severity, const char *text);
// update flight control mode. The control mode is vehicle type specific
@ -132,17 +133,10 @@ public:
// MAV_SYS_STATUS_* values from mavlink. If a bit is set then it
// indicates that the sensor or subsystem is present but not
// functioning correctly
void update_sensor_status_flags(uint32_t error_mask) { _ap.sensor_status_flags = error_mask; }
struct msg_t
{
struct {
const char *text;
uint8_t severity;
} data[MSG_BUFFER_LENGTH];
uint8_t queued_idx;
uint8_t sent_idx;
};
static ObjectArray<mavlink_statustext_t> _statustext_queue;
private:
AP_AHRS &_ahrs;
@ -224,8 +218,6 @@ private:
uint8_t repeats; // send each message "chunk" 3 times to make sure the entire messsage gets through without getting cut
uint8_t char_index; // index of which character to get in the message
} _msg_chunk;
msg_t _msg;
// main transmission function when protocol is FrSky SPort Passthrough (OpenTX)
void send_SPort_Passthrough(void);