david.tahir/QuadMeUp_Crossbow
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

platform node

Browse Source
This commit is contained in:
Pawel Spychalski
2018-05-17 14:13:41 +02:00
parent e915f30f1f
commit 5ff36034b8
10 changed files with 84 additions and 75 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
.vscode/settings.json vendored
View File

@@ -8,7 +8,8 @@
"iterator": "cpp",
"rope": "c",
"__locale": "c",
"string": "c"
"string": "c",
"ios": "cpp"
},
"files.exclude": {
"**/build": true

15
crossbow/crossbow.ino
View File

@@ -14,6 +14,7 @@ Copyright (c) 20xx, MPL Contributor1 contrib1@example.net
#include "main_variables.h"
#include "qsp.h"
#include "sbus.h"
#include "platform_node.h"
#ifdef ARDUINO_AVR_FEATHER32U4
#define LORA_SS_PIN 8
@@ -34,6 +35,7 @@ Copyright (c) 20xx, MPL Contributor1 contrib1@example.net
#endif
RadioNode radioNode;
PlatformNode platformNode;
/*
* Main defines for device working in TX mode
@@ -52,8 +54,6 @@ RadioNode radioNode;
#error please select tx input source
#endif
volatile int16_t TxInput::channels[TX_INPUT_CHANNEL_COUNT];
#include "txbuzzer.h"
BuzzerState_t buzzer;
@@ -163,11 +163,6 @@ void setup(void)
radioNode.init(LORA_SS_PIN, LORA_RST_PIN, LORA_DI0_PIN, onReceive);
#ifdef DEVICE_MODE_RX
//initiallize default ppm values
for (int i = 0; i < 16; i++)
{
rxDeviceState.channels[i] = PPM_CHANNEL_DEFAULT_VALUE;
}
pinMode(RX_ADC_PIN_1, INPUT);
pinMode(RX_ADC_PIN_2, INPUT);
@@ -373,7 +368,7 @@ void loop(void)
break;
case QSP_FRAME_RC_DATA:
encodeRcDataPayload(&qsp, txInput.channels, PPM_INPUT_CHANNEL_COUNT);
encodeRcDataPayload(&qsp, PLATFORM_CHANNEL_COUNT);
break;
}
@@ -434,9 +429,9 @@ void loop(void)
}
if (currentMillis > sbusTime) {
rxDeviceState.channels[RSSI_CHANNEL - 1] = rxDeviceState.indicatedRssi;
platformNode.setRcChannel(RSSI_CHANNEL - 1, rxDeviceState.indicatedRssi, 0);
sbusPreparePacket(sbusPacket, rxDeviceState.channels, false, (qsp.deviceState == DEVICE_STATE_FAILSAFE));
sbusPreparePacket(sbusPacket, false, (qsp.deviceState == DEVICE_STATE_FAILSAFE));
Serial1.write(sbusPacket, SBUS_PACKET_LENGTH);
sbusTime = currentMillis + SBUS_UPDATE_RATE;
}

21
crossbow/platform_node.cpp Normal file
View File

@@ -0,0 +1,21 @@
#include "platform_node.h"
PlatformNode::PlatformNode(void) {
for (uint8_t i = 0; i < PLATFORM_TOTAL_CHANNEL_COUNT; i++) {
_channels[i] = PLATFORM_DEFAULT_CHANNEL_VALUE;
}
}
int PlatformNode::getRcChannel(uint8_t channel) {
if (channel < PLATFORM_TOTAL_CHANNEL_COUNT) {
return _channels[channel];
} else {
return PLATFORM_DEFAULT_CHANNEL_VALUE;
}
}
void PlatformNode::setRcChannel(uint8_t channel, int value, int offset) {
if (channel < PLATFORM_TOTAL_CHANNEL_COUNT) {
_channels[channel] = value + offset;
}
}

22
crossbow/platform_node.h Normal file
View File

@@ -0,0 +1,22 @@
#pragma once
#include "Arduino.h"
#ifndef PLATFORM_NODE_H
#define PLATFORM_NODE_H
#define PLATFORM_TOTAL_CHANNEL_COUNT 11 //Including RSSI channel and other
#define PLATFORM_CHANNEL_COUNT 10
#define PLATFORM_DEFAULT_CHANNEL_VALUE 1000
class PlatformNode {
public:
PlatformNode(void);
int getRcChannel(uint8_t channel);
void setRcChannel(uint8_t channel, int value, int offset);
private:
volatile int _channels[PLATFORM_TOTAL_CHANNEL_COUNT];
};
#endif

45
crossbow/qsp.cpp
View File

@@ -2,38 +2,17 @@
#include "variables.h"
void qspDecodeRcDataFrame(QspConfiguration_t *qsp, RxDeviceState_t *rxDeviceSate) {
int temporaryPpmOutput[PPM_OUTPUT_CHANNEL_COUNT] = {0};
//TODO fix it, baby :)
temporaryPpmOutput[0] = (uint16_t) (((uint16_t) qsp->payload[0] << 2) & 0x3fc) | ((qsp->payload[1] >> 6) & 0x03);
temporaryPpmOutput[1] = (uint16_t) (((uint16_t) qsp->payload[1] << 4) & 0x3f0) | ((qsp->payload[2] >> 4) & 0x0F);
temporaryPpmOutput[2] = (uint16_t) (((uint16_t) qsp->payload[2] << 6) & 0x3c0) | ((qsp->payload[3] >> 2) & 0x3F);
temporaryPpmOutput[3] = (uint16_t) (((uint16_t) qsp->payload[3] << 8) & 0x300) | ((qsp->payload[4]) & 0xFF);
temporaryPpmOutput[4] = qsp->payload[5];
temporaryPpmOutput[5] = qsp->payload[6];
temporaryPpmOutput[6] = (qsp->payload[7] >> 4) & 0b00001111;
temporaryPpmOutput[7] = qsp->payload[7] & 0b00001111;
temporaryPpmOutput[8] = (qsp->payload[8] >> 4) & 0b00001111;
temporaryPpmOutput[9] = qsp->payload[8] & 0b00001111;
//10bit channels are passed as is
//8bit channels needs to be shifted left 2 places
temporaryPpmOutput[4] = temporaryPpmOutput[4] << 2;
temporaryPpmOutput[5] = temporaryPpmOutput[5] << 2;
//4bit channels needs to be shifted left 6 places
temporaryPpmOutput[6] = temporaryPpmOutput[6] << 6;
temporaryPpmOutput[7] = temporaryPpmOutput[7] << 6;
temporaryPpmOutput[8] = temporaryPpmOutput[8] << 6;
temporaryPpmOutput[9] = temporaryPpmOutput[9] << 6;
/*
* Copy tremporary to real output and add missing 1000
*/
for (uint8_t i = 0; i < PPM_OUTPUT_CHANNEL_COUNT; i++) {
rxDeviceSate->channels[i] = temporaryPpmOutput[i] + 1000;
}
platformNode.setRcChannel(0, (uint16_t) (((uint16_t) qsp->payload[0] << 2) & 0x3fc) | ((qsp->payload[1] >> 6) & 0x03), 1000);
platformNode.setRcChannel(1, (uint16_t) (((uint16_t) qsp->payload[1] << 4) & 0x3f0) | ((qsp->payload[2] >> 4) & 0x0F), 1000);
platformNode.setRcChannel(2, (uint16_t) (((uint16_t) qsp->payload[2] << 6) & 0x3c0) | ((qsp->payload[3] >> 2) & 0x3F), 1000);
platformNode.setRcChannel(3, (uint16_t) (((uint16_t) qsp->payload[3] << 8) & 0x300) | ((qsp->payload[4]) & 0xFF), 1000);
platformNode.setRcChannel(4, ((int) qsp->payload[5]) << 2, 1000);
platformNode.setRcChannel(5, ((int) qsp->payload[6]) << 2, 1000);
platformNode.setRcChannel(6, ((int) ((qsp->payload[7] >> 4) & 0b00001111)) << 6, 1000);
platformNode.setRcChannel(7, ((int) (qsp->payload[7] & 0b00001111)) << 6, 1000);
platformNode.setRcChannel(8, ((int) ((qsp->payload[8] >> 4) & 0b00001111)) << 6, 1000);
platformNode.setRcChannel(9, ((int) (qsp->payload[8] & 0b00001111)) << 6, 1000);
}
uint8_t get10bitHighShift(uint8_t channel) {
@@ -92,11 +71,11 @@ void decodeRxHealthPayload(QspConfiguration_t *qsp, RxDeviceState_t *rxDeviceSta
/**
* Encode 10 RC channels
*/
void encodeRcDataPayload(QspConfiguration_t *qsp, volatile int16_t channels[], uint8_t noOfChannels)
void encodeRcDataPayload(QspConfiguration_t *qsp, uint8_t noOfChannels)
{
for (uint8_t i = 0; i < noOfChannels; i++)
{
int cV = constrain(channels[i], 1000, 2000) - 1000;
int cV = constrain(platformNode.getRcChannel(i), 1000, 2000) - 1000;
uint16_t channelValue10 = cV & 0x03ff;
uint8_t channelValue8 = (cV >> 2) & 0xff;

5
crossbow/qsp.h
View File

@@ -1,6 +1,9 @@
#include "Arduino.h"
#include "variables.h"
#include "radio_node.h"
#include "platform_node.h"
extern PlatformNode platformNode;
void qspDecodeRcDataFrame(QspConfiguration_t *qsp, RxDeviceState_t *rxDeviceSate);
void decodeRxHealthPayload(QspConfiguration_t *qsp, RxDeviceState_t *rxDeviceState);
@@ -9,7 +12,7 @@ uint8_t get10bitHighShift(uint8_t channel);
uint8_t get10bitLowShift(uint8_t channel);
void qspComputeCrc(QspConfiguration_t *qsp, uint8_t dataByte);
void encodeRxHealthPayload(QspConfiguration_t *qsp, RxDeviceState_t *rxDeviceState, uint8_t rssi, uint8_t snr);
void encodeRcDataPayload(QspConfiguration_t *qsp, volatile int16_t channels[], uint8_t noOfChannels);
void encodeRcDataPayload(QspConfiguration_t *qsp, uint8_t noOfChannels);
void qspDecodeIncomingFrame(
QspConfiguration_t *qsp,
uint8_t incomingByte,

42
crossbow/sbus.cpp
View File

@@ -31,16 +31,16 @@ int mapSbusToChannel(int in) {
return (((long) in - 173l) * 1020l / 1638l) + 990;
}
void sbusPreparePacket(uint8_t packet[], int16_t channels[], bool isSignalLoss, bool isFailsafe){
void sbusPreparePacket(uint8_t packet[], bool isSignalLoss, bool isFailsafe){
static int output[SBUS_CHANNEL_NUMBER] = {0};
int output[SBUS_CHANNEL_NUMBER];
/*
* Map 1000-2000 with middle at 1500 chanel values to
* 173-1811 with middle at 992 S.BUS protocol requires
*/
for (uint8_t i = 0; i < SBUS_CHANNEL_NUMBER; i++) {
output[i] = mapChannelToSbus(channels[i]);
output[i] = mapChannelToSbus(platformNode.getRcChannel(i));
}
uint8_t stateByte = 0x00;
@@ -83,7 +83,6 @@ SbusInput::SbusInput(HardwareSerial &serial) : _serial(serial)
{
}
void SbusInput::loop(void)
{
if (_serial.available()) {
@@ -116,27 +115,20 @@ void SbusInput::recoverStuckFrames(void)
}
}
void sbusToChannels(volatile int16_t channels[], byte buffer[]) {
channels[0] = ((buffer[1] |buffer[2]<<8) & 0x07FF);
channels[1] = ((buffer[2]>>3 |buffer[3]<<5) & 0x07FF);
channels[2] = ((buffer[3]>>6 |buffer[4]<<2 |buffer[5]<<10) & 0x07FF);
channels[3] = ((buffer[5]>>1 |buffer[6]<<7) & 0x07FF);
channels[4] = ((buffer[6]>>4 |buffer[7]<<4) & 0x07FF);
channels[5] = ((buffer[7]>>7 |buffer[8]<<1 |buffer[9]<<9) & 0x07FF);
channels[6] = ((buffer[9]>>2 |buffer[10]<<6) & 0x07FF);
channels[7] = ((buffer[10]>>5|buffer[11]<<3) & 0x07FF);
channels[8] = ((buffer[12] |buffer[13]<<8) & 0x07FF);
channels[9] = ((buffer[13]>>3|buffer[14]<<5) & 0x07FF);
channels[10] = ((buffer[14]>>6|buffer[15]<<2|buffer[16]<<10) & 0x07FF);
channels[11] = ((buffer[16]>>1|buffer[17]<<7) & 0x07FF);
channels[12] = ((buffer[17]>>4|buffer[18]<<4) & 0x07FF);
channels[13] = ((buffer[18]>>7|buffer[19]<<1|buffer[20]<<9) & 0x07FF);
channels[14] = ((buffer[20]>>2|buffer[21]<<6) & 0x07FF);
channels[15] = ((buffer[21]>>5|buffer[22]<<3) & 0x07FF);
void sbusToChannels(byte buffer[]) {
for (uint8_t channelIndex = 0; channelIndex < SBUS_CHANNEL_NUMBER; channelIndex++) {
channels[channelIndex] = mapSbusToChannel(channels[channelIndex]);
}
platformNode.setRcChannel(0, mapSbusToChannel((buffer[1] | buffer[2]<<8) & 0x07FF), 0);
platformNode.setRcChannel(1, mapSbusToChannel((buffer[2]>>3 | buffer[3]<<5) & 0x07FF), 0);
platformNode.setRcChannel(2, mapSbusToChannel((buffer[3]>>6 | buffer[4]<<2 | buffer[5]<<10) & 0x07FF), 0);
platformNode.setRcChannel(3, mapSbusToChannel((buffer[5]>>1 | buffer[6]<<7) & 0x07FF), 0);
platformNode.setRcChannel(4, mapSbusToChannel((buffer[6]>>4 | buffer[7]<<4) & 0x07FF), 0);
platformNode.setRcChannel(5, mapSbusToChannel((buffer[7]>>7 | buffer[8]<<1 |buffer[9]<<9) & 0x07FF), 0);
platformNode.setRcChannel(6, mapSbusToChannel((buffer[9]>>2 | buffer[10]<<6) & 0x07FF), 0);
platformNode.setRcChannel(7, mapSbusToChannel((buffer[10]>>5 | buffer[11]<<3) & 0x07FF), 0);
platformNode.setRcChannel(8, mapSbusToChannel((buffer[12] | buffer[13]<<8) & 0x07FF), 0);
platformNode.setRcChannel(9, mapSbusToChannel((buffer[13]>>3 | buffer[14]<<5) & 0x07FF), 0);
//We use only 10 channels, so the reset can be just ignored
}
void SbusInput::sbusRead() {
@@ -169,7 +161,7 @@ void SbusInput::sbusRead() {
) {
//We have full frame now
_frameDecodingEndedAt = millis();
sbusToChannels(channels, buffer);
sbusToChannels(buffer);
_protocolState = SBUS_DECODING_STATE_IDLE;
}
}

2
crossbow/sbus.h
View File

@@ -27,7 +27,7 @@ class SbusInput : public TxInput
void sbusRead(void);
};
void sbusPreparePacket(uint8_t packet[], int16_t channels[], bool isSignalLoss, bool isFailsafe);
void sbusPreparePacket(uint8_t packet[], bool isSignalLoss, bool isFailsafe);
#endif

2
crossbow/tx_input.h
View File

@@ -10,12 +10,10 @@ class TxInput
{
public:
virtual ~TxInput() {}
int get(uint8_t channel) { return channels[channel]; };
virtual void start(void) {};
virtual void stop(void) {};
virtual bool isReceiving(void) { return false; };
virtual void loop(void) {};
volatile static int16_t channels[TX_INPUT_CHANNEL_COUNT];
};
#endif

2
crossbow/variables.h
View File

@@ -69,7 +69,6 @@ enum deviceStates {
#define PPM_INPUT_PIN 0 // Has to be one of Interrupt pins
#define PPM_INPUT_CHANNEL_COUNT 10
#define PPM_OUTPUT_CHANNEL_COUNT 10
#define TX_BUZZER_PIN A5
@@ -99,7 +98,6 @@ struct RxDeviceState_t {
uint8_t a1Voltage = 0;
uint8_t a2Voltage = 0;
uint8_t flags = 0;
int16_t channels[16] = {};
int16_t indicatedRssi = 0;
};