mirror of
https://github.com/OSURoboticsClub/Rover_2017_2018.git
synced 2025-11-09 02:31:14 +00:00
Added non-final versions of iris and tower firmware.
This commit is contained in:
249
software/firmware/libraries/SBUS/SBUS.cpp
Normal file
249
software/firmware/libraries/SBUS/SBUS.cpp
Normal file
@@ -0,0 +1,249 @@
|
||||
/*
|
||||
SBUS.cpp
|
||||
Brian R Taylor
|
||||
brian.taylor@bolderflight.com
|
||||
2017-01-13
|
||||
|
||||
Copyright (c) 2016 Bolder Flight Systems
|
||||
|
||||
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.
|
||||
*/
|
||||
|
||||
// Teensy 3.0 || Teensy 3.1/3.2 || Teensy 3.5 || Teensy 3.6 || Teensy LC
|
||||
#if defined(__MK20DX128__) || defined(__MK20DX256__) || defined(__MK64FX512__) || \
|
||||
defined(__MK66FX1M0__) || defined(__MKL26Z64__)
|
||||
|
||||
#include "Arduino.h"
|
||||
#include "SBUS.h"
|
||||
|
||||
#if defined(__MK20DX128__) || defined(__MK20DX256__)
|
||||
// globals needed for emulating two stop bytes on Teensy 3.0 and 3.1/3.2
|
||||
IntervalTimer serialTimer;
|
||||
HardwareSerial* SERIALPORT;
|
||||
uint8_t PACKET[25];
|
||||
volatile int SENDINDEX;
|
||||
void sendByte();
|
||||
#endif
|
||||
|
||||
/* SBUS object, input the serial bus */
|
||||
SBUS::SBUS(HardwareSerial& bus){
|
||||
_bus = &bus;
|
||||
}
|
||||
|
||||
/* starts the serial communication */
|
||||
void SBUS::begin(){
|
||||
// initialize parsing state
|
||||
_fpos = 0;
|
||||
#if defined(__MK20DX128__) || defined(__MK20DX256__) // Teensy 3.0 || Teensy 3.1/3.2
|
||||
// begin the serial port for SBUS
|
||||
_bus->begin(100000,SERIAL_8E1_RXINV_TXINV);
|
||||
SERIALPORT = _bus;
|
||||
#endif
|
||||
|
||||
#if defined(__MK64FX512__) || defined(__MK66FX1M0__) || defined(__MKL26Z64__) // Teensy 3.5 || Teensy 3.6 || Teensy LC
|
||||
// begin the serial port for SBUS
|
||||
_bus->begin(100000,SERIAL_8E2_RXINV_TXINV);
|
||||
#endif
|
||||
}
|
||||
|
||||
/* read the SBUS data and calibrate it to +/- 1 */
|
||||
bool SBUS::readCal(float* calChannels, uint8_t* failsafe, uint16_t* lostFrames){
|
||||
uint16_t channels[16];
|
||||
|
||||
// read the SBUS data
|
||||
if(read(&channels[0],failsafe,lostFrames)){
|
||||
|
||||
// linear calibration
|
||||
for(uint8_t i = 0; i < 16; i++){
|
||||
calChannels[i] = channels[i] * _sbusScale + _sbusBias;
|
||||
}
|
||||
|
||||
// return true on receiving a full packet
|
||||
return true;
|
||||
}
|
||||
else{
|
||||
|
||||
// return false if a full packet is not received
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
/* read the SBUS data */
|
||||
bool SBUS::read(uint16_t* channels, uint8_t* failsafe, uint16_t* lostFrames){
|
||||
|
||||
// parse the SBUS packet
|
||||
if(parse()){
|
||||
|
||||
// 16 channels of 11 bit data
|
||||
channels[0] = (int16_t) ((_payload[0] |_payload[1]<<8) & 0x07FF);
|
||||
channels[1] = (int16_t) ((_payload[1]>>3 |_payload[2]<<5) & 0x07FF);
|
||||
channels[2] = (int16_t) ((_payload[2]>>6 |_payload[3]<<2 |_payload[4]<<10) & 0x07FF);
|
||||
channels[3] = (int16_t) ((_payload[4]>>1 |_payload[5]<<7) & 0x07FF);
|
||||
channels[4] = (int16_t) ((_payload[5]>>4 |_payload[6]<<4) & 0x07FF);
|
||||
channels[5] = (int16_t) ((_payload[6]>>7 |_payload[7]<<1 |_payload[8]<<9) & 0x07FF);
|
||||
channels[6] = (int16_t) ((_payload[8]>>2 |_payload[9]<<6) & 0x07FF);
|
||||
channels[7] = (int16_t) ((_payload[9]>>5 |_payload[10]<<3) & 0x07FF);
|
||||
channels[8] = (int16_t) ((_payload[11] |_payload[12]<<8) & 0x07FF);
|
||||
channels[9] = (int16_t) ((_payload[12]>>3|_payload[13]<<5) & 0x07FF);
|
||||
channels[10] = (int16_t) ((_payload[13]>>6|_payload[14]<<2|_payload[15]<<10) & 0x07FF);
|
||||
channels[11] = (int16_t) ((_payload[15]>>1|_payload[16]<<7) & 0x07FF);
|
||||
channels[12] = (int16_t) ((_payload[16]>>4|_payload[17]<<4) & 0x07FF);
|
||||
channels[13] = (int16_t) ((_payload[17]>>7|_payload[18]<<1|_payload[19]<<9) & 0x07FF);
|
||||
channels[14] = (int16_t) ((_payload[19]>>2|_payload[20]<<6) & 0x07FF);
|
||||
channels[15] = (int16_t) ((_payload[20]>>5|_payload[21]<<3) & 0x07FF);
|
||||
|
||||
// count lost frames
|
||||
if (_payload[22] & _sbusLostFrame) {
|
||||
*lostFrames = *lostFrames + 1;
|
||||
}
|
||||
|
||||
// failsafe state
|
||||
if (_payload[22] & _sbusFailSafe) {
|
||||
*failsafe = 1;
|
||||
}
|
||||
else{
|
||||
*failsafe = 0;
|
||||
}
|
||||
|
||||
// return true on receiving a full packet
|
||||
return true;
|
||||
}
|
||||
else{
|
||||
|
||||
// return false if a full packet is not received
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
/* parse the SBUS data */
|
||||
bool SBUS::parse(){
|
||||
static elapsedMicros sbusTime = 0;
|
||||
|
||||
if(sbusTime > SBUS_TIMEOUT){_fpos = 0;}
|
||||
|
||||
// see if serial data is available
|
||||
while(_bus->available() > 0){
|
||||
sbusTime = 0;
|
||||
static uint8_t c;
|
||||
static uint8_t b;
|
||||
c = _bus->read();
|
||||
|
||||
// find the header
|
||||
if(_fpos == 0){
|
||||
if((c == _sbusHeader)&&((b == _sbusFooter)||((b & 0x0F) == _sbus2Footer))){
|
||||
_fpos++;
|
||||
}
|
||||
else{
|
||||
_fpos = 0;
|
||||
}
|
||||
}
|
||||
else{
|
||||
|
||||
// strip off the data
|
||||
if((_fpos-1) < _payloadSize){
|
||||
_payload[_fpos-1] = c;
|
||||
_fpos++;
|
||||
}
|
||||
|
||||
// check the end byte
|
||||
if((_fpos-1) == _payloadSize){
|
||||
if((c == _sbusFooter)||((c & 0x0F) == _sbus2Footer)) {
|
||||
_fpos = 0;
|
||||
return true;
|
||||
}
|
||||
else{
|
||||
_fpos = 0;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
b = c;
|
||||
}
|
||||
// return false if a partial packet
|
||||
return false;
|
||||
}
|
||||
|
||||
/* write SBUS packets */
|
||||
void SBUS::write(uint16_t* channels){
|
||||
uint8_t packet[25];
|
||||
|
||||
/* assemble the SBUS packet */
|
||||
|
||||
// SBUS header
|
||||
packet[0] = _sbusHeader;
|
||||
|
||||
// 16 channels of 11 bit data
|
||||
packet[1] = (uint8_t) ((channels[0] & 0x07FF));
|
||||
packet[2] = (uint8_t) ((channels[0] & 0x07FF)>>8 | (channels[1] & 0x07FF)<<3);
|
||||
packet[3] = (uint8_t) ((channels[1] & 0x07FF)>>5 | (channels[2] & 0x07FF)<<6);
|
||||
packet[4] = (uint8_t) ((channels[2] & 0x07FF)>>2);
|
||||
packet[5] = (uint8_t) ((channels[2] & 0x07FF)>>10 | (channels[3] & 0x07FF)<<1);
|
||||
packet[6] = (uint8_t) ((channels[3] & 0x07FF)>>7 | (channels[4] & 0x07FF)<<4);
|
||||
packet[7] = (uint8_t) ((channels[4] & 0x07FF)>>4 | (channels[5] & 0x07FF)<<7);
|
||||
packet[8] = (uint8_t) ((channels[5] & 0x07FF)>>1);
|
||||
packet[9] = (uint8_t) ((channels[5] & 0x07FF)>>9 | (channels[6] & 0x07FF)<<2);
|
||||
packet[10] = (uint8_t) ((channels[6] & 0x07FF)>>6 | (channels[7] & 0x07FF)<<5);
|
||||
packet[11] = (uint8_t) ((channels[7] & 0x07FF)>>3);
|
||||
packet[12] = (uint8_t) ((channels[8] & 0x07FF));
|
||||
packet[13] = (uint8_t) ((channels[8] & 0x07FF)>>8 | (channels[9] & 0x07FF)<<3);
|
||||
packet[14] = (uint8_t) ((channels[9] & 0x07FF)>>5 | (channels[10] & 0x07FF)<<6);
|
||||
packet[15] = (uint8_t) ((channels[10] & 0x07FF)>>2);
|
||||
packet[16] = (uint8_t) ((channels[10] & 0x07FF)>>10 | (channels[11] & 0x07FF)<<1);
|
||||
packet[17] = (uint8_t) ((channels[11] & 0x07FF)>>7 | (channels[12] & 0x07FF)<<4);
|
||||
packet[18] = (uint8_t) ((channels[12] & 0x07FF)>>4 | (channels[13] & 0x07FF)<<7);
|
||||
packet[19] = (uint8_t) ((channels[13] & 0x07FF)>>1);
|
||||
packet[20] = (uint8_t) ((channels[13] & 0x07FF)>>9 | (channels[14] & 0x07FF)<<2);
|
||||
packet[21] = (uint8_t) ((channels[14] & 0x07FF)>>6 | (channels[15] & 0x07FF)<<5);
|
||||
packet[22] = (uint8_t) ((channels[15] & 0x07FF)>>3);
|
||||
|
||||
// flags
|
||||
packet[23] = 0x00;
|
||||
|
||||
// footer
|
||||
packet[24] = _sbusFooter;
|
||||
|
||||
#if defined(__MK20DX128__) || defined(__MK20DX256__) // Teensy 3.0 || Teensy 3.1/3.2
|
||||
// use ISR to send byte at a time,
|
||||
// 130 us between bytes to emulate 2 stop bits
|
||||
noInterrupts();
|
||||
memcpy(&PACKET,&packet,sizeof(packet));
|
||||
interrupts();
|
||||
serialTimer.priority(255);
|
||||
serialTimer.begin(sendByte,130);
|
||||
#endif
|
||||
|
||||
#if defined(__MK64FX512__) || defined(__MK66FX1M0__) || defined(__MKL26Z64__) // Teensy 3.5 || Teensy 3.6 || Teensy LC
|
||||
// write packet
|
||||
_bus->write(packet,25);
|
||||
#endif
|
||||
}
|
||||
|
||||
// function to send byte at a time with
|
||||
// ISR to emulate 2 stop bits on Teensy 3.0 and 3.1/3.2
|
||||
#if defined(__MK20DX128__) || defined(__MK20DX256__) // Teensy 3.0 || Teensy 3.1/3.2
|
||||
void sendByte(){
|
||||
if(SENDINDEX < 25) {
|
||||
SERIALPORT->write(PACKET[SENDINDEX]);
|
||||
SENDINDEX++;
|
||||
}
|
||||
else{
|
||||
serialTimer.end();
|
||||
SENDINDEX = 0;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif
|
||||
Reference in New Issue
Block a user