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

close #19 close #13

Browse Source
This commit is contained in:
Pawel Spychalski (DzikuVx)
2017-10-24 20:07:23 +02:00
parent 474d2a9c84
commit 2a47aac573
4 changed files with 91 additions and 63 deletions
Download Patch File Download Diff File Expand all files Collapse all files

82
crossbow.ino
View File

@@ -12,6 +12,7 @@
#include <LoRa.h>
// #include <PinChangeInterrupt.h>
#include "variables.h"
#include "sbus.h"
#include "qsp.h"
// LoRa32u4 ports
@@ -19,7 +20,7 @@
#define LORA32U4_RST_PIN 4
#define LORA32U4_DI0_PIN 7
int ppm[PPM_OUTPUT_CHANNEL_COUNT] = {0};
int ppm[16] = {0};
/*
* Main defines for device working in TX mode
@@ -40,7 +41,8 @@ PPMReader ppmReader(PPM_INPUT_PIN, PPM_INPUT_INTERRUPT, true);
* Main defines for device working in RX mode
*/
#ifdef DEVICE_MODE_RX
uint32_t sbusTime = 0;
uint8_t sbusPacket[SBUS_PACKET_LENGTH] = {0};
#endif
/*
@@ -136,6 +138,10 @@ display.display();
void setup(void)
{
#ifdef DEBUG_SERIAL
Serial.begin(115200);
#endif
qsp.hardwareWriteFunction = writeToRadio;
#ifdef DEVICE_MODE_RX
@@ -149,7 +155,6 @@ void setup(void)
#endif
#ifdef LORA_HARDWARE_SPI
Serial.begin(115200);
#ifdef WAIT_FOR_SERIAL
while (!Serial) {
@@ -193,28 +198,16 @@ void setup(void)
// display.display();
//initiallize default ppm values
for (int i = 0; i < PPM_OUTPUT_CHANNEL_COUNT; i++)
for (int i = 0; i < 16; i++)
{
ppm[i] = PPM_CHANNEL_DEFAULT_VALUE;
}
pinMode(PPM_OUTPUT_PIN, OUTPUT);
digitalWrite(PPM_OUTPUT_PIN, !PPM_SIGNAL_POSITIVE_STATE); //set the PPM signal pin to the default state (off)
cli();
TCCR1A = 0; // set entire TCCR1 register to 0
TCCR1B = 0;
OCR1A = 100; // compare match register, change this
TCCR1B |= (1 << WGM12); // turn on CTC mode
TCCR1B |= (1 << CS11); // 8 prescaler: 0,5 microseconds at 16mhz
TIMSK1 |= (1 << OCIE1A); // enable timer compare interrupt
sei();
pinMode(RX_ADC_PIN_1, INPUT);
pinMode(RX_ADC_PIN_2, INPUT);
pinMode(RX_ADC_PIN_3, INPUT);
Serial1.begin(100000, SERIAL_8E2);
#endif
#ifdef DEVICE_MODE_TX
@@ -238,54 +231,11 @@ void setup(void)
#ifdef DEBUG_LED
qsp.debugConfig |= DEBUG_FLAG_LED;
#endif
}
#ifdef DEVICE_MODE_RX
void writePpmOutput(uint8_t val) {
if (qsp.deviceState == DEVICE_STATE_OK) {
digitalWrite(PPM_OUTPUT_PIN, val);
} else {
//This is failsafe state, we pull output low so FC can decide about failsafe
digitalWrite(PPM_OUTPUT_PIN, LOW);
}
}
ISR(TIMER1_COMPA_vect) { //leave this alone
static boolean state = true;
TCNT1 = 0;
if (state)
{ //start pulse
writePpmOutput(PPM_SIGNAL_POSITIVE_STATE);
OCR1A = PPM_PULSE_LENGTH * PPM_OUTPUT_MULTIPLIER;
state = false;
}
else
{ //end pulse and calculate when to start the next pulse
static byte cur_chan_numb;
static unsigned int calc_rest;
writePpmOutput(!PPM_SIGNAL_POSITIVE_STATE);
state = true;
if (cur_chan_numb >= PPM_OUTPUT_CHANNEL_COUNT)
{
cur_chan_numb = 0;
calc_rest = calc_rest + PPM_PULSE_LENGTH; //
OCR1A = (PPM_FRAME_LENGTH - calc_rest) * PPM_OUTPUT_MULTIPLIER;
calc_rest = 0;
}
else
{
OCR1A = (ppm[cur_chan_numb] - PPM_PULSE_LENGTH) * PPM_OUTPUT_MULTIPLIER;
calc_rest = calc_rest + ppm[cur_chan_numb];
cur_chan_numb++;
}
}
}
void updateRxDeviceState(RxDeviceState_t *rxDeviceState) {
rxDeviceState->rxVoltage = map(analogRead(RX_ADC_PIN_1), 0, 1024, 0, 255);
rxDeviceState->a1Voltage = map(analogRead(RX_ADC_PIN_2), 0, 1024, 0, 255);
@@ -306,7 +256,7 @@ void loop(void)
*/
if (
qsp.protocolState != QSP_STATE_IDLE &&
abs(millis() - qsp.frameDecodingStartedAt) > QSP_MAX_FRAME_DECODE_TIME
abs(currentMillis - qsp.frameDecodingStartedAt) > QSP_MAX_FRAME_DECODE_TIME
) {
qsp.protocolState = QSP_STATE_IDLE;
}
@@ -365,6 +315,14 @@ void loop(void)
#endif
#ifdef DEVICE_MODE_RX
if (currentMillis > sbusTime) {
sbusPreparePacket(sbusPacket, ppm, false, (qsp.deviceState == DEVICE_STATE_FAILSAFE));
Serial1.write(sbusPacket, SBUS_PACKET_LENGTH);
sbusTime = currentMillis + SBUS_UPDATE_RATE;
}
/*
* RX_HEALTH QSP frame
*/

61
sbus.cpp Normal file
View File

@@ -0,0 +1,61 @@
#include "Arduino.h"
#include "variables.h"
#define SBUS_MIN_OFFSET 173
#define SBUS_MID_OFFSET 992
#define SBUS_MAX_OFFSET 1811
#define SBUS_CHANNEL_NUMBER 16
#define SBUS_PACKET_LENGTH 25
#define SBUS_FRAME_HEADER 0x0f
#define SBUS_FRAME_FOOTER 0x00
#define SBUS_FRAME_FOOTER_V2 0x04
#define SBUS_STATE_FAILSAFE 0x08
#define SBUS_STATE_SIGNALLOSS 0x04
void sbusPreparePacket(uint8_t packet[], int channels[], bool isSignalLoss, bool isFailsafe){
static int output[SBUS_CHANNEL_NUMBER] = {0};
/*
* 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] = map(channels[i], RC_CHANNEL_MIN, RC_CHANNEL_MAX, SBUS_MIN_OFFSET, SBUS_MAX_OFFSET);
}
uint8_t stateByte = 0x00;
if (isSignalLoss) {
stateByte |= SBUS_STATE_SIGNALLOSS;
}
if (isFailsafe) {
stateByte |= SBUS_STATE_FAILSAFE;
}
packet[0] = SBUS_FRAME_HEADER; //Header
packet[1] = (uint8_t) (output[0] & 0x07FF);
packet[2] = (uint8_t) ((output[0] & 0x07FF)>>8 | (output[1] & 0x07FF)<<3);
packet[3] = (uint8_t) ((output[1] & 0x07FF)>>5 | (output[2] & 0x07FF)<<6);
packet[4] = (uint8_t) ((output[2] & 0x07FF)>>2);
packet[5] = (uint8_t) ((output[2] & 0x07FF)>>10 | (output[3] & 0x07FF)<<1);
packet[6] = (uint8_t) ((output[3] & 0x07FF)>>7 | (output[4] & 0x07FF)<<4);
packet[7] = (uint8_t) ((output[4] & 0x07FF)>>4 | (output[5] & 0x07FF)<<7);
packet[8] = (uint8_t) ((output[5] & 0x07FF)>>1);
packet[9] = (uint8_t) ((output[5] & 0x07FF)>>9 | (output[6] & 0x07FF)<<2);
packet[10] = (uint8_t) ((output[6] & 0x07FF)>>6 | (output[7] & 0x07FF)<<5);
packet[11] = (uint8_t) ((output[7] & 0x07FF)>>3);
packet[12] = (uint8_t) ((output[8] & 0x07FF));
packet[13] = (uint8_t) ((output[8] & 0x07FF)>>8 | (output[9] & 0x07FF)<<3);
packet[14] = (uint8_t) ((output[9] & 0x07FF)>>5 | (output[10] & 0x07FF)<<6);
packet[15] = (uint8_t) ((output[10] & 0x07FF)>>2);
packet[16] = (uint8_t) ((output[10] & 0x07FF)>>10 | (output[11] & 0x07FF)<<1);
packet[17] = (uint8_t) ((output[11] & 0x07FF)>>7 | (output[12] & 0x07FF)<<4);
packet[18] = (uint8_t) ((output[12] & 0x07FF)>>4 | (output[13] & 0x07FF)<<7);
packet[19] = (uint8_t) ((output[13] & 0x07FF)>>1);
packet[20] = (uint8_t) ((output[13] & 0x07FF)>>9 | (output[14] & 0x07FF)<<2);
packet[21] = (uint8_t) ((output[14] & 0x07FF)>>6 | (output[15] & 0x07FF)<<5);
packet[22] = (uint8_t) ((output[15] & 0x07FF)>>3);
packet[23] = stateByte; //Flags byte
packet[24] = SBUS_FRAME_FOOTER; //Footer
}

3
sbus.h Normal file
View File

@@ -0,0 +1,3 @@
#include "Arduino.h"
void sbusPreparePacket(uint8_t packet[], int channels[], bool isSignalLoss, bool isFailsafe);

8
variables.h
View File

@@ -1,11 +1,17 @@
#pragma once
#define SBUS_UPDATE_RATE 12 //ms
#define SBUS_PACKET_LENGTH 25
#define RC_CHANNEL_MIN 990
#define RC_CHANNEL_MAX 2010
//Only for UART connected radio modules
#define UART_SPEED 57600
#define E45_TTL_100_UART_DOWNTIME 30
#define RX_RX_HEALTH_FRAME_RATE 1000
#define TX_RC_FRAME_RATE 50 //ms
#define TX_RC_FRAME_RATE 500 //ms
#define RX_FAILSAFE_DELAY (TX_RC_FRAME_RATE * 8)
#define TX_PING_RATE 2000