mirror of https://github.com/OpenIPC/wiki.git
292 lines
10 KiB
C++
292 lines
10 KiB
C++
/*
|
|
* Brian R Taylor
|
|
* brian.taylor@bolderflight.com
|
|
*
|
|
* Copyright (c) 2022 Bolder Flight Systems Inc
|
|
*
|
|
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
* of this software and associated documentation files (the “Software”), to
|
|
* deal in the Software without restriction, including without limitation the
|
|
* rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
|
|
* sell copies of the Software, and to permit persons to whom the Software is
|
|
* furnished to do so, subject to the following conditions:
|
|
*
|
|
* The above copyright notice and this permission notice shall be included in
|
|
* all copies or substantial portions of the Software.
|
|
*
|
|
* THE SOFTWARE IS PROVIDED “AS IS”, WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
|
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
|
|
* FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
|
|
* IN THE SOFTWARE.
|
|
*/
|
|
|
|
#include "sbus.h" // NOLINT
|
|
#if defined(ARDUINO)
|
|
#include <Arduino.h>
|
|
#else
|
|
#include <cstddef>
|
|
#include <cstdint>
|
|
#include "core/core.h"
|
|
#endif
|
|
|
|
namespace bfs {
|
|
|
|
void SbusRx::Begin() {
|
|
if (fast_) {
|
|
baud_ = 200000;
|
|
} else {
|
|
baud_ = 100000;
|
|
}
|
|
/* Start the bus */
|
|
/* Teensy 3.0 || Teensy 3.1/3.2 */
|
|
#if defined(__MK20DX128__) || defined(__MK20DX256__)
|
|
if (inv_) {
|
|
uart_->begin(baud_, SERIAL_8E1_RXINV_TXINV);
|
|
} else {
|
|
uart_->begin(baud_, SERIAL_8E1);
|
|
}
|
|
/*
|
|
* Teensy 3.5 || Teensy 3.6 ||
|
|
* Teensy LC || Teensy 4.0/4.1 ||
|
|
* Teensy 4.0 Beta
|
|
*/
|
|
#elif defined(__MK64FX512__) || defined(__MK66FX1M0__) || \
|
|
defined(__MKL26Z64__) || defined(__IMXRT1062__) || \
|
|
defined(__IMXRT1052__)
|
|
if (inv_){
|
|
uart_->begin(baud_, SERIAL_8E2_RXINV_TXINV);
|
|
} else {
|
|
uart_->begin(baud_, SERIAL_8E2);
|
|
}
|
|
/* STM32L4 */
|
|
#elif defined(STM32L496xx) || defined(STM32L476xx) || \
|
|
defined(STM32L433xx) || defined(STM32L432xx)
|
|
if (inv_) {
|
|
uart_->begin(baud_, SERIAL_8E2 | 0xC000ul);
|
|
} else {
|
|
uart_->begin(baud_, SERIAL_8E2);
|
|
}
|
|
/* ESP32 */
|
|
#elif defined(ESP32)
|
|
uart_->begin(baud_, SERIAL_8E2, rxpin_, txpin_, inv_);
|
|
/* Everything else, with a hardware inverter */
|
|
#else
|
|
uart_->begin(baud_, SERIAL_8E2);
|
|
#endif
|
|
/* flush the bus */
|
|
uart_->flush();
|
|
}
|
|
|
|
bool SbusRx::Read() {
|
|
/* Read through all available packets to get the newest */
|
|
new_data_ = false;
|
|
do {
|
|
if (Parse()) {
|
|
new_data_ = true;
|
|
}
|
|
} while (uart_->available());
|
|
return new_data_;
|
|
}
|
|
|
|
bool SbusRx::Parse() {
|
|
/* Parse messages */
|
|
while (uart_->available()) {
|
|
cur_byte_ = uart_->read();
|
|
if (state_ == 0) {
|
|
if ((cur_byte_ == HEADER_) && ((prev_byte_ == FOOTER_) ||
|
|
((prev_byte_ & 0x0F) == FOOTER2_))) {
|
|
buf_[state_++] = cur_byte_;
|
|
} else {
|
|
state_ = 0;
|
|
}
|
|
} else if (state_ < PAYLOAD_LEN_ + HEADER_LEN_) {
|
|
buf_[state_++] = cur_byte_;
|
|
} else if (state_ < PAYLOAD_LEN_ + HEADER_LEN_ + FOOTER_LEN_) {
|
|
state_ = 0;
|
|
prev_byte_ = cur_byte_;
|
|
if ((cur_byte_ == FOOTER_) || ((cur_byte_ & 0x0F) == FOOTER2_)) {
|
|
/* Grab the channel data */
|
|
data_.ch[0] = static_cast<int16_t>(buf_[1] |
|
|
((buf_[2] << 8) & 0x07FF));
|
|
data_.ch[1] = static_cast<int16_t>((buf_[2] >> 3) |
|
|
((buf_[3] << 5) & 0x07FF));
|
|
data_.ch[2] = static_cast<int16_t>((buf_[3] >> 6) |
|
|
(buf_[4] << 2) |
|
|
((buf_[5] << 10) & 0x07FF));
|
|
data_.ch[3] = static_cast<int16_t>((buf_[5] >> 1) |
|
|
((buf_[6] << 7) & 0x07FF));
|
|
data_.ch[4] = static_cast<int16_t>((buf_[6] >> 4) |
|
|
((buf_[7] << 4) & 0x07FF));
|
|
data_.ch[5] = static_cast<int16_t>((buf_[7] >> 7) |
|
|
(buf_[8] << 1) |
|
|
((buf_[9] << 9) & 0x07FF));
|
|
data_.ch[6] = static_cast<int16_t>((buf_[9] >> 2) |
|
|
((buf_[10] << 6) & 0x07FF));
|
|
data_.ch[7] = static_cast<int16_t>((buf_[10] >> 5) |
|
|
((buf_[11] << 3) & 0x07FF));
|
|
data_.ch[8] = static_cast<int16_t>(buf_[12] |
|
|
((buf_[13] << 8) & 0x07FF));
|
|
data_.ch[9] = static_cast<int16_t>((buf_[13] >> 3) |
|
|
((buf_[14] << 5) & 0x07FF));
|
|
data_.ch[10] = static_cast<int16_t>((buf_[14] >> 6) |
|
|
(buf_[15] << 2) |
|
|
((buf_[16] << 10) & 0x07FF));
|
|
data_.ch[11] = static_cast<int16_t>((buf_[16] >> 1) |
|
|
((buf_[17] << 7) & 0x07FF));
|
|
data_.ch[12] = static_cast<int16_t>((buf_[17] >> 4) |
|
|
((buf_[18] << 4) & 0x07FF));
|
|
data_.ch[13] = static_cast<int16_t>((buf_[18] >> 7) |
|
|
(buf_[19] << 1) |
|
|
((buf_[20] << 9) & 0x07FF));
|
|
data_.ch[14] = static_cast<int16_t>((buf_[20] >> 2) |
|
|
((buf_[21] << 6) & 0x07FF));
|
|
data_.ch[15] = static_cast<int16_t>((buf_[21] >> 5) |
|
|
((buf_[22] << 3) & 0x07FF));
|
|
/* CH 17 */
|
|
data_.ch17 = buf_[23] & CH17_MASK_;
|
|
/* CH 18 */
|
|
data_.ch18 = buf_[23] & CH18_MASK_;
|
|
/* Grab the lost frame */
|
|
data_.lost_frame = buf_[23] & LOST_FRAME_MASK_;
|
|
/* Grab the failsafe */
|
|
data_.failsafe = buf_[23] & FAILSAFE_MASK_;
|
|
return true;
|
|
} else {
|
|
return false;
|
|
}
|
|
} else {
|
|
state_ = 0;
|
|
}
|
|
prev_byte_ = cur_byte_;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
/* Needed for emulating two stop bytes on Teensy 3.0 and 3.1/3.2 */
|
|
#if defined(__MK20DX128__) || defined(__MK20DX256__)
|
|
namespace {
|
|
IntervalTimer serial_timer;
|
|
HardwareSerial *sbus_bus;
|
|
uint8_t sbus_packet[25];
|
|
volatile int send_index = 0;
|
|
void SendByte() {
|
|
if (send_index < 25) {
|
|
sbus_bus->write(sbus_packet[send_index]);
|
|
send_index++;
|
|
} else {
|
|
serial_timer.end();
|
|
send_index = 0;
|
|
}
|
|
}
|
|
} // namespace
|
|
#endif
|
|
|
|
void SbusTx::Begin() {
|
|
if (fast_) {
|
|
baud_ = 200000;
|
|
} else {
|
|
baud_ = 100000;
|
|
}
|
|
/* Start the bus */
|
|
/* Teensy 3.0 || Teensy 3.1/3.2 */
|
|
#if defined(__MK20DX128__) || defined(__MK20DX256__)
|
|
sbus_bus = uart_;
|
|
if (inv_) {
|
|
uart_->begin(baud_, SERIAL_8E1_RXINV_TXINV);
|
|
} else {
|
|
uart_->begin(baud_, SERIAL_8E1);
|
|
}
|
|
/*
|
|
* Teensy 3.5 || Teensy 3.6 ||
|
|
* Teensy LC || Teensy 4.0/4.1 ||
|
|
* Teensy 4.0 Beta
|
|
*/
|
|
#elif defined(__MK64FX512__) || defined(__MK66FX1M0__) || \
|
|
defined(__MKL26Z64__) || defined(__IMXRT1062__) || \
|
|
defined(__IMXRT1052__)
|
|
if (inv_){
|
|
uart_->begin(baud_, SERIAL_8E2_RXINV_TXINV);
|
|
} else {
|
|
uart_->begin(baud_, SERIAL_8E2);
|
|
}
|
|
/* STM32L4 */
|
|
#elif defined(STM32L496xx) || defined(STM32L476xx) || \
|
|
defined(STM32L433xx) || defined(STM32L432xx)
|
|
if (inv_) {
|
|
uart_->begin(baud_, SERIAL_8E2 | 0xC000ul);
|
|
} else {
|
|
uart_->begin(baud_, SERIAL_8E2);
|
|
}
|
|
/* ESP32 */
|
|
#elif defined(ESP32)
|
|
uart_->begin(baud_, SERIAL_8E2, rxpin_, txpin_, inv_);
|
|
/* Everything else, with a hardware inverter */
|
|
#else
|
|
uart_->begin(baud_, SERIAL_8E2);
|
|
#endif
|
|
}
|
|
|
|
void SbusTx::Write() {
|
|
/* Assemble packet */
|
|
buf_[0] = HEADER_;
|
|
buf_[1] = static_cast<uint8_t>((data_.ch[0] & 0x07FF));
|
|
buf_[2] = static_cast<uint8_t>((data_.ch[0] & 0x07FF) >> 8 |
|
|
(data_.ch[1] & 0x07FF) << 3);
|
|
buf_[3] = static_cast<uint8_t>((data_.ch[1] & 0x07FF) >> 5 |
|
|
(data_.ch[2] & 0x07FF) << 6);
|
|
buf_[4] = static_cast<uint8_t>((data_.ch[2] & 0x07FF) >> 2);
|
|
buf_[5] = static_cast<uint8_t>((data_.ch[2] & 0x07FF) >> 10 |
|
|
(data_.ch[3] & 0x07FF) << 1);
|
|
buf_[6] = static_cast<uint8_t>((data_.ch[3] & 0x07FF) >> 7 |
|
|
(data_.ch[4] & 0x07FF) << 4);
|
|
buf_[7] = static_cast<uint8_t>((data_.ch[4] & 0x07FF) >> 4 |
|
|
(data_.ch[5] & 0x07FF) << 7);
|
|
buf_[8] = static_cast<uint8_t>((data_.ch[5] & 0x07FF) >> 1);
|
|
buf_[9] = static_cast<uint8_t>((data_.ch[5] & 0x07FF) >> 9 |
|
|
(data_.ch[6] & 0x07FF) << 2);
|
|
buf_[10] = static_cast<uint8_t>((data_.ch[6] & 0x07FF) >> 6 |
|
|
(data_.ch[7] & 0x07FF) << 5);
|
|
buf_[11] = static_cast<uint8_t>((data_.ch[7] & 0x07FF) >> 3);
|
|
buf_[12] = static_cast<uint8_t>((data_.ch[8] & 0x07FF));
|
|
buf_[13] = static_cast<uint8_t>((data_.ch[8] & 0x07FF) >> 8 |
|
|
(data_.ch[9] & 0x07FF) << 3);
|
|
buf_[14] = static_cast<uint8_t>((data_.ch[9] & 0x07FF) >> 5 |
|
|
(data_.ch[10] & 0x07FF) << 6);
|
|
buf_[15] = static_cast<uint8_t>((data_.ch[10] & 0x07FF) >> 2);
|
|
buf_[16] = static_cast<uint8_t>((data_.ch[10] & 0x07FF) >> 10 |
|
|
(data_.ch[11] & 0x07FF) << 1);
|
|
buf_[17] = static_cast<uint8_t>((data_.ch[11] & 0x07FF) >> 7 |
|
|
(data_.ch[12] & 0x07FF) << 4);
|
|
buf_[18] = static_cast<uint8_t>((data_.ch[12] & 0x07FF) >> 4 |
|
|
(data_.ch[13] & 0x07FF) << 7);
|
|
buf_[19] = static_cast<uint8_t>((data_.ch[13] & 0x07FF) >> 1);
|
|
buf_[20] = static_cast<uint8_t>((data_.ch[13] & 0x07FF) >> 9 |
|
|
(data_.ch[14] & 0x07FF) << 2);
|
|
buf_[21] = static_cast<uint8_t>((data_.ch[14] & 0x07FF) >> 6 |
|
|
(data_.ch[15] & 0x07FF) << 5);
|
|
buf_[22] = static_cast<uint8_t>((data_.ch[15] & 0x07FF) >> 3);
|
|
buf_[23] = 0x00 | (data_.ch17 * CH17_MASK_) | (data_.ch18 * CH18_MASK_) |
|
|
(data_.failsafe * FAILSAFE_MASK_) |
|
|
(data_.lost_frame * LOST_FRAME_MASK_);
|
|
buf_[24] = FOOTER_;
|
|
/* Send packet to servos */
|
|
#if defined(__MK20DX128__) || defined(__MK20DX256__)
|
|
/*
|
|
* Use ISR to send byte at a time,
|
|
* 130 us between bytes to emulate 2 stop bits
|
|
*/
|
|
__disable_irq();
|
|
memcpy(sbus_packet, buf_, sizeof(buf_));
|
|
__enable_irq();
|
|
serial_timer.priority(255);
|
|
serial_timer.begin(SendByte, 130);
|
|
#else
|
|
uart_->write(buf_, sizeof(buf_));
|
|
#endif
|
|
}
|
|
|
|
} // namespace bfs
|