From 03b2d5b97acd0b5daa4bd70e05a8449d4ac8ee30 Mon Sep 17 00:00:00 2001 From: Angus Ainslie Date: Sat, 2 Mar 2013 14:43:31 -0700 Subject: [PATCH 1/5] Add SAVE_CENTER conditonal option. It enbles code to save the tx stick center for less trimming --- config.h | 5 ++++- kk.c | 2 +- receiver.c | 55 ++++++++++++++++++++++++++++++++++++++++++++++++++---- receiver.h | 5 ++++- settings.c | 11 ++++++++--- settings.h | 11 ++++++++++- 6 files changed, 78 insertions(+), 11 deletions(-) diff --git a/config.h b/config.h index e02b5c8..0b4ae8e 100644 --- a/config.h +++ b/config.h @@ -9,6 +9,9 @@ #include #include "typedefs.h" + +#define SAVE_CENTER + #include "io_cfg.h" /* Multicopter Type */ @@ -22,4 +25,4 @@ //#define HEX_COPTER //#define Y6_COPTER -#endif \ No newline at end of file +#endif diff --git a/kk.c b/kk.c index 40288d8..d8fda55 100644 --- a/kk.c +++ b/kk.c @@ -17,10 +17,10 @@ static void setup() { MCUCR = _BV(PUD); // Disable hardware pull-up + settingsSetup(); receiverSetup(); gyrosSetup(); motorsSetup(); - settingsSetup(); LED_DIR = OUTPUT; LED = 0; diff --git a/receiver.c b/receiver.c index 512934d..236bc2f 100644 --- a/receiver.c +++ b/receiver.c @@ -1,4 +1,5 @@ #include "receiver.h" +#include "settings.h" /*** BEGIN VARIABLES ***/ int16_t RxInRoll; @@ -16,6 +17,11 @@ uint16_t RxChannel2; uint16_t RxChannel3; uint16_t RxChannel4; +uint16_t CenterRollValue; +uint16_t CenterPitchValue; +uint16_t CenterCollValue; +uint16_t CenterYawValue; + #ifdef TWIN_COPTER int16_t RxInOrgPitch; #endif @@ -126,6 +132,11 @@ void receiverSetup() RX_COLL = 0; RX_YAW = 0; + CenterRollValue = Config.CenterRollValue; + CenterPitchValue = Config.CenterPitchValue; + CenterCollValue = Config.CenterCollValue; + CenterYawValue = Config.CenterYawValue; + /* * timer1 (16bit) - run at 8MHz, used to measure Rx pulses * and to control ESC/servo pulse @@ -171,16 +182,17 @@ void RxGetChannels() uint8_t t = 0xff; do { asm volatile("mov %0, %1":"=r" (i_sreg),"=r" (t)::"memory"); - RxInRoll = fastdiv8(RxChannel1 - 1520 * 8); - RxInPitch = fastdiv8(RxChannel2 - 1520 * 8); + RxInRoll = fastdiv8(RxChannel1 - CenterRollValue * 8); + RxInPitch = fastdiv8(RxChannel2 - CenterPitchValue * 8); RxInCollective = fastdiv8(RxChannel3 - 1120 * 8); - RxInYaw = fastdiv8(RxChannel4 - 1520 * 8); + RxInYaw = fastdiv8(RxChannel4 - CenterYawValue * 8); } while(i_sreg != t); #ifdef TWIN_COPTER RxInOrgPitch = RxInPitch; #endif } +#ifndef SAVE_CENTER void receiverStickCenter() { uint8_t i; @@ -193,4 +205,39 @@ void receiverStickCenter() i--; } } -} \ No newline at end of file +} +#else +void receiverStickCenter() +{ + uint8_t i; + int16_t RawRoll = 0; + int16_t RawColl = 0; + int16_t RawYaw = 0; + int16_t RawPitch = 0; + + LED = 1; + _delay_ms( 500 ); + + for( i=0; i<8; i++ ) + { + RxGetChannels(); + LED ^= 1; + RawRoll += RxChannel1/8; + RawPitch += RxChannel2/8; + RawColl += RxChannel3/8; + RawYaw += RxChannel4/8; + _delay_ms(100); + } + + Config.CenterRollValue = RawRoll/8; + Config.CenterPitchValue = RawPitch/8; + Config.CenterCollValue = RawColl/8; + Config.CenterYawValue = RawYaw/8; + Save_Config_to_EEPROM(); + + while(1) { + LED ^= 1; + _delay_ms( 1000 ); + } +} +#endif diff --git a/receiver.h b/receiver.h index 0d94d6e..e414de0 100644 --- a/receiver.h +++ b/receiver.h @@ -14,6 +14,9 @@ // limits the maximum stick collective (range 80->100 100=Off) // this allows gyros to stabilise better when full throttle applied #define MAX_COLLECTIVE 1000 // 95 + +#define DEFAULT_CENTER 1520 + /*** END DEFINES ***/ @@ -52,4 +55,4 @@ void RxGetChannels(void); void receiverStickCenter(void); /*** END PROTOTYPES ***/ -#endif \ No newline at end of file +#endif diff --git a/settings.c b/settings.c index 39bd877..861de44 100644 --- a/settings.c +++ b/settings.c @@ -2,6 +2,7 @@ #include #include "gyros.h" +#include "receiver.h" struct config Config; @@ -34,13 +35,17 @@ void Set_EEPROM_Default_Config() Config.RollGyroDirection = GYRO_REVERSED; Config.PitchGyroDirection = GYRO_REVERSED; Config.YawGyroDirection = GYRO_NORMAL; + Config.CenterRollValue = DEFAULT_CENTER; + Config.CenterPitchValue = DEFAULT_CENTER; + Config.CenterCollValue = DEFAULT_CENTER; + Config.CenterYawValue = DEFAULT_CENTER; } void Initial_EEPROM_Config_Load() { // load up last settings from EEPROM - if(eeprom_read_byte((uint8_t *)EEPROM_DATA_START_POS) != 42) { - Config.setup = 42; + if(eeprom_read_byte((uint8_t *)EEPROM_DATA_START_POS) != CONFIG_VERSION) { + Config.setup = CONFIG_VERSION; Set_EEPROM_Default_Config(); // write to eeProm Save_Config_to_EEPROM(); @@ -67,4 +72,4 @@ void settingsClearAll() Set_EEPROM_Default_Config(); while(1) ; -} \ No newline at end of file +} diff --git a/settings.h b/settings.h index 5634949..30204b3 100644 --- a/settings.h +++ b/settings.h @@ -5,6 +5,11 @@ /*** BEGIN DEFINITIONS ***/ #define EEPROM_DATA_START_POS 0 // Settings save offset in eeprom +#ifdef SAVE_CENTER +#define CONFIG_VERSION 43 +#else +#define CONFIG_VERSION 42 +#endif /*** END DEFINITIONS ***/ /*** BEGIN TYPES ***/ @@ -14,6 +19,10 @@ struct config { uint8_t RollGyroDirection; uint8_t PitchGyroDirection; uint8_t YawGyroDirection; + uint16_t CenterRollValue; + uint16_t CenterPitchValue; + uint16_t CenterCollValue; + uint16_t CenterYawValue; }; /*** END TYPES ***/ @@ -31,4 +40,4 @@ void settingsSetup(void); void settingsClearAll(void); /*** END PROTOTYPES ***/ -#endif \ No newline at end of file +#endif From 6fb97cb433478e7df251d8e5d2a68fb1dca1ed2b Mon Sep 17 00:00:00 2001 From: Angus Ainslie Date: Sat, 2 Mar 2013 14:40:56 -0700 Subject: [PATCH 2/5] re-write the arduino SoftSerial.cpp to be used for debugging --- soft_serial.c | 524 ++++++++++++++++++++++++++++++++++++++++++++++++++ soft_serial.h | 95 +++++++++ 2 files changed, 619 insertions(+) create mode 100644 soft_serial.c create mode 100644 soft_serial.h diff --git a/soft_serial.c b/soft_serial.c new file mode 100644 index 0000000..a5178c4 --- /dev/null +++ b/soft_serial.c @@ -0,0 +1,524 @@ +/* +soft_serial.c (formerly SoftwareSerial.cpp (formerly NewSoftSerial.cpp)) - +software serial library for AVR microcontrollers +-- Interrupt-driven receive and other improvements by ladyada + (http://ladyada.net) +-- Tuning, circular buffer, + porting to 8MHz processors, + various optimizations, PROGMEM delay tables, inverse logic and + direct port writing by Mikal Hart (http://www.arduiniana.org) +-- Pin change interrupt macros by Paul Stoffregen (http://www.pjrc.com) +-- 20MHz processor support by Garrett Mace (http://www.macetech.com) +-- ATmega1280/2560 support by Brett Hagman (http://www.roguerobotics.com/) + +This library is free software; you can redistribute it and/or +modify it under the terms of the GNU Lesser General Public +License as published by the Free Software Foundation; either +version 2.1 of the License, or (at your option) any later version. + +This library is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +Lesser General Public License for more details. + +You should have received a copy of the GNU Lesser General Public +License along with this library; if not, write to the Free Software +Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +The latest version of this library can always be found at +http://arduiniana.org. +*/ + +// When set, _DEBUG co-opts pins 11 and 13 for debugging with an +// oscilloscope or logic analyzer. Beware: it also slightly modifies +// the bit times, so don't rely on it too much at high baud rates +#define _DEBUG 0 +#define _DEBUG_PIN1 11 +#define _DEBUG_PIN2 13 +// +// Includes +// +#include +#include +#include "soft_serial.h" + +#include "config.h" +#include "io_cfg.h" + +#ifdef SERIAL_PORT + +#define ECHO + +// static data +//static uint8_t ss_receivePin; +//static uint8_t ss_receiveBitMask; +//static uint8_t ss_transmitBitMask; + +static uint16_t ss_rx_delay_centering; +static uint16_t ss_rx_delay_intrabit; +static uint16_t ss_rx_delay_stopbit; +static uint16_t ss_tx_delay; + +static uint8_t ss_buffer_overflow; +static uint8_t ss_inverse_logic; +static uint8_t ss_initialized = FALSE; + +static char ss_receive_buffer[SS_MAX_RX_BUFF]; +static volatile uint8_t ss_receive_buffer_tail; +static volatile uint8_t ss_receive_buffer_head; + +#define digitalPinToPCICR(p) (((p) >= 0 && (p) <= 21) ? (&PCICR) : ((uint8_t *)0)) +#define digitalPinToPCICRbit(p) (((p) <= 7) ? 2 : (((p) <= 13) ? 0 : 1)) +#define digitalPinToPCMSK(p) (((p) <= 7) ? (&PCMSK2) : (((p) <= 13) ? (&PCMSK0) : (((p) <= 21) ? (&PCMSK1) : ((uint8_t *)0)))) +#define digitalPinToPCMSKbit(p) (((p) <= 7) ? (p) : (((p) <= 13) ? ((p) - 8) : ((p) - 14))) +// +// Lookup table +// +typedef struct _DELAY_TABLE +{ + long baud; + int16_t rx_delay_centering; + int16_t rx_delay_intrabit; + int16_t rx_delay_stopbit; + int16_t tx_delay; +} DELAY_TABLE; + +#if F_CPU == 16000000 + +static const DELAY_TABLE PROGMEM table[] = +{ + // baud rxcenter rxintra rxstop tx + { 115200, 1, 17, 17, 12, }, + { 57600, 10, 37, 37, 33, }, + { 38400, 25, 57, 57, 54, }, + { 31250, 31, 70, 70, 68, }, + { 28800, 34, 77, 77, 74, }, + { 19200, 54, 117, 117, 114, }, + { 14400, 74, 156, 156, 153, }, + { 9600, 114, 236, 236, 233, }, + { 4800, 233, 474, 474, 471, }, + { 2400, 471, 950, 950, 947, }, + { 1200, 947, 1902, 1902, 1899, }, + { 300, 3804, 7617, 7617, 7614, }, +}; + +const int XMIT_START_ADJUSTMENT = 5; + +#elif F_CPU == 8000000 + +static const DELAY_TABLE table[] PROGMEM = +{ + // baud rxcenter rxintra rxstop tx + { 115200, 1, 5, 5, 3, }, + { 57600, 1, 15, 15, 13, }, + { 38400, 2, 25, 26, 23, }, + { 31250, 7, 32, 33, 29, }, + { 28800, 11, 35, 35, 32, }, + { 19200, 20, 55, 55, 52, }, + { 14400, 30, 75, 75, 72, }, + { 9600, 50, 114, 114, 112, }, + { 4800, 110, 233, 233, 230, }, + { 2400, 229, 472, 472, 469, }, + { 1200, 467, 948, 948, 945, }, + { 300, 1895, 3805, 3805, 3802, }, +}; + +const int XMIT_START_ADJUSTMENT = 4; + +#elif F_CPU == 20000000 + +// 20MHz support courtesy of the good people at macegr.com. +// Thanks, Garrett! + +static const DELAY_TABLE PROGMEM table[] = +{ + // baud rxcenter rxintra rxstop tx + { 115200, 3, 21, 21, 18, }, + { 57600, 20, 43, 43, 41, }, + { 38400, 37, 73, 73, 70, }, + { 31250, 45, 89, 89, 88, }, + { 28800, 46, 98, 98, 95, }, + { 19200, 71, 148, 148, 145, }, + { 14400, 96, 197, 197, 194, }, + { 9600, 146, 297, 297, 294, }, + { 4800, 296, 595, 595, 592, }, + { 2400, 592, 1189, 1189, 1186, }, + { 1200, 1187, 2379, 2379, 2376, }, + { 300, 4759, 9523, 9523, 9520, }, +}; + +const int XMIT_START_ADJUSTMENT = 6; + +#else + +#error This version of SoftwareSerial supports only 20, 16 and 8MHz processors + +#endif + +// +// Statics +// +static char ss_receive_buffer[SS_MAX_RX_BUFF]; +static volatile uint8_t ss_receive_buffer_tail = 0; +static volatile uint8_t ss_receive_buffer_head = 0; + +// +// Debugging +// +// This function generates a brief pulse +// for debugging or measuring on an oscilloscope. +inline void DebugPulse(uint8_t pin, uint8_t count) +{ +#if _DEBUG + volatile uint8_t *pport = portOutputRegister(digitalPinToPort(pin)); + + uint8_t val = *pport; + while (count--) + { + *pport = val | digitalPinToBitMask(pin); + *pport = val; + } +#endif +} + +/* static */ +inline void ss_tunedDelay(uint16_t delay) { + uint8_t tmp=0; + + asm volatile("sbiw %0, 0x01 \n\t" + "ldi %1, 0xFF \n\t" + "cpi %A0, 0xFF \n\t" + "cpc %B0, %1 \n\t" + "brne .-10 \n\t" + : "+r" (delay), "+a" (tmp) + : "0" (delay) + ); +} + +// This function sets the current object as the "listening" +// one and returns true if it replaces another +uint8_t ss_listen() +{ + uint8_t oldSREG = SREG; + ss_buffer_overflow = false; + cli(); + ss_receive_buffer_head = ss_receive_buffer_tail = 0; + SREG = oldSREG; + + ss_initialized = TRUE; + + return true; +} + +uint8_t ss_isListening(void) +{ + return ss_initialized; +} + +// +// The receive routine called by the interrupt handler +// +void ss_recv() +{ + +#if GCC_VERSION < 40302 +// Work-around for avr-gcc 4.3.0 OSX version bug +// Preserve the registers that the compiler misses +// (courtesy of Arduino forum user *etracer*) + asm volatile( + "push r18 \n\t" + "push r19 \n\t" + "push r20 \n\t" + "push r21 \n\t" + "push r22 \n\t" + "push r23 \n\t" + "push r26 \n\t" + "push r27 \n\t" + ::); +#endif + + uint8_t d = 0; + + // If RX line is high, then we don't see any start bit + // so interrupt is probably not for us + if (ss_inverse_logic ? ss_rx_pin_read() : !ss_rx_pin_read()) + { + // Wait approximately 1/2 of a bit width to "center" the sample + ss_tunedDelay(ss_rx_delay_centering); + DebugPulse(_DEBUG_PIN2, 1); + + // Read each of the 8 bits + for (uint8_t i=0x1; i; i <<= 1) + { + ss_tunedDelay(ss_rx_delay_intrabit); + DebugPulse(_DEBUG_PIN2, 1); + uint8_t noti = ~i; + if (ss_rx_pin_read()) + d |= i; + else // else clause added to ensure function timing is ~balanced + d &= noti; + } + + // skip the stop bit + ss_tunedDelay(ss_rx_delay_stopbit); + DebugPulse(_DEBUG_PIN2, 1); + + if (ss_inverse_logic) + d = ~d; + + // if buffer full, set the overflow flag and return + if ((ss_receive_buffer_tail + 1) % SS_MAX_RX_BUFF != ss_receive_buffer_head) + { + // save new data in buffer: tail points to where byte goes + ss_receive_buffer[ss_receive_buffer_tail] = d; // save new byte + ss_receive_buffer_tail = (ss_receive_buffer_tail + 1) % SS_MAX_RX_BUFF; + } + else + { +#if _DEBUG // for scope: pulse pin as overflow indictator + DebugPulse(_DEBUG_PIN1, 1); +#endif + ss_buffer_overflow = true; + } + +#ifdef ECHO + ss_write( d ); + +#endif + } + +#if GCC_VERSION < 40302 +// Work-around for avr-gcc 4.3.0 OSX version bug +// Restore the registers that the compiler misses + asm volatile( + "pop r27 \n\t" + "pop r26 \n\t" + "pop r23 \n\t" + "pop r22 \n\t" + "pop r21 \n\t" + "pop r20 \n\t" + "pop r19 \n\t" + "pop r18 \n\t" + ::); +#endif +} + +void ss_tx_pin_write(uint8_t pin_state) +{ + SER_TX = pin_state; +#ifdef XXXX + if (pin_state == LOW) + SER_TX &= ~ss_transmitBitMask; + else + SER_TX |= ss_transmitBitMask; +#endif +} + +uint8_t ss_rx_pin_read() +{ + return SER_RX; + //return SER_RX & ss_receiveBitMask; +} + +// +// Interrupt handling +// +//ISR(PCINT2_vect) +//{ +// ss_recv(); +//} + +void soft_serial(uint8_t rxReg, uint8_t rxBit, uint8_t txReg, uint8_t txBit, uint8_t inverse_logic ) +{ + uint8_t vect; + + ss_rx_delay_centering = 0; + ss_rx_delay_intrabit = 0; + ss_rx_delay_stopbit = 0; + ss_tx_delay = 0; + ss_buffer_overflow = false; + ss_inverse_logic = inverse_logic; + + #if defined(SERIAL_RX_M4) + vect = PCINT23; + #elif defined(SERIAL_RX_M5) + vect = PCINT22; + #elif defined(SERIAL_RX_M6) + vect = PCINT21; + #endif + + PCMSK2 |= _BV(vect); // PD1 + + ss_setTX(txReg, txBit); + ss_setRX(rxReg, rxBit); +} + +void ss_setTX(uint8_t reg, uint8_t bit) +{ + SER_TX_DIR = OUTPUT; + SER_TX = HIGH; + //ss_transmitBitMask = ~bit & 0xff; + //_transmitPortRegister = reg; +} + +void ss_setRX(uint8_t reg, uint8_t bit) +{ + SER_RX_DIR = INPUT; + if (!ss_inverse_logic) + SER_RX= HIGH; // pullup for normal logic! + //ss_receivePin = SER_RX; + //ss_receiveBitMask = ~bit & 0xFF; + //_receivePortRegister = reg; +} + +void ss_begin(long speed) +{ + ss_rx_delay_centering = ss_rx_delay_intrabit = ss_rx_delay_stopbit = ss_tx_delay = 0; + + for (unsigned i=0; i + +/****************************************************************************** +* Definitions +******************************************************************************/ + +#define SS_MAX_RX_BUFF 64 // RX buffer size + +#ifndef GCC_VERSION +#define GCC_VERSION (__GNUC__ * 10000 + __GNUC_MINOR__ * 100 + __GNUC_PATCHLEVEL__) +#endif + +uint8_t ss_listen(void); +void ss_recv(void); +uint8_t ss_rx_pin_read(void); +void ss_setTX(uint8_t reg, uint8_t bit); +void ss_setRX(uint8_t reg, uint8_t bit); +void ss_end(void); +int ss_read(void); +uint8_t ss_isListening(void); +void ss_tunedDelay(uint16_t delay); +int ss_available(void); +void soft_serial(uint8_t rxReg, uint8_t rxBit, uint8_t txReg, uint8_t txBit, uint8_t inverse_logic); +void ss_begin(long speed); +size_t ss_write_str(char *buf); +size_t ss_write(uint8_t b); +int ss_peek(void); +void ss_flush(void); + +#ifdef XXXX +class SoftwareSerial +{ +private: + static SoftwareSerial *active_object; + + // private methods + void tx_pin_write(uint8_t pin_state); + + // private static method for timing + +public: + // public methods + ~SoftwareSerial(); + void end(); + bool overflow() { bool ret = _buffer_overflow; _buffer_overflow = false; return ret; } + + virtual size_t write(uint8_t byte); + virtual int available(); + virtual void flush(); + + //using Print::write; + + // public only for easy access by interrupt handlers +}; +#endif + +#define HIGH 1 +#define LOW 0 + + +#endif From 443e446e1ad977048fd89e156a42e30716403d07 Mon Sep 17 00:00:00 2001 From: Angus Ainslie Date: Sat, 2 Mar 2013 15:18:00 -0700 Subject: [PATCH 3/5] soft serial send is working. Add the rest of the soft serial changes --- Makefile | 7 ++-- io_cfg.h | 15 ++++++++ kk.c | 64 +++++++++++++++++++++++++++++----- motors.c | 29 +++++++++++++++- soft_serial.c | 95 ++++++++++++++++++++++++++++++++++----------------- 5 files changed, 166 insertions(+), 44 deletions(-) diff --git a/Makefile b/Makefile index 636108c..e410b66 100644 --- a/Makefile +++ b/Makefile @@ -80,8 +80,7 @@ OBJDIR = ./.obj/ # List C source files here. (C dependencies are automatically generated.) -SRC = $(TARGET).c motors.c gyros.c receiver.c settings.c pid.c - +SRC = $(TARGET).c motors.c gyros.c receiver.c settings.c pid.c soft_serial.c # List C++ source files here. (C dependencies are automatically generated.) CPPSRC = @@ -273,10 +272,10 @@ LDFLAGS += $(PRINTF_LIB) $(SCANF_LIB) $(MATH_LIB) # Type: avrdude -c ? # to get a full listing. # -AVRDUDE_PROGRAMMER = usbasp-clone +AVRDUDE_PROGRAMMER = usbasp # com1 = serial port. Use lpt1 to connect to parallel port. -AVRDUDE_PORT = usb # programmer connected to serial device +AVRDUDE_PORT = /dev/ttyUSB0 # programmer connected to serial device AVRDUDE_WRITE_FLASH = -U flash:w:$(TARGET).hex #AVRDUDE_WRITE_EEPROM = -U eeprom:w:$(TARGET).eep diff --git a/io_cfg.h b/io_cfg.h index 3634d8f..ea436ca 100644 --- a/io_cfg.h +++ b/io_cfg.h @@ -35,16 +35,31 @@ #define M1 REGISTER_BIT(PORTB,2) #define M2 REGISTER_BIT(PORTB,1) +#ifdef SERIAL_PORT +#define M3 REGISTER_BIT(PORTD,6) +#define M4 REGISTER_BIT(PORTD,5) +#define SER_TX REGISTER_BIT(PORTB,0) +#define SER_RX REGISTER_BIT(PORTD,7) +#define SERIAL_RX_M4 +#else #define M3 REGISTER_BIT(PORTB,0) #define M4 REGISTER_BIT(PORTD,7) #define M5 REGISTER_BIT(PORTD,6) #define M6 REGISTER_BIT(PORTD,5) +#endif #define M1_DIR REGISTER_BIT(DDRB,2) #define M2_DIR REGISTER_BIT(DDRB,1) +#ifdef SERIAL_PORT +#define M3_DIR REGISTER_BIT(DDRD,6) +#define M4_DIR REGISTER_BIT(DDRD,5) +#define SER_TX_DIR REGISTER_BIT(DDRB,0) +#define SER_RX_DIR REGISTER_BIT(DDRD,5) +#else #define M3_DIR REGISTER_BIT(DDRB,0) #define M4_DIR REGISTER_BIT(DDRD,7) #define M5_DIR REGISTER_BIT(DDRD,6) #define M6_DIR REGISTER_BIT(DDRD,5) +#endif #define LED REGISTER_BIT(PORTB,6) #define LED_DIR REGISTER_BIT(DDRB,6) diff --git a/kk.c b/kk.c index d8fda55..f51b2c8 100644 --- a/kk.c +++ b/kk.c @@ -3,6 +3,7 @@ #include "settings.h" #include "receiver.h" #include "motors.h" +#include "soft_serial.h" bool Armed; @@ -21,6 +22,26 @@ static void setup() receiverSetup(); gyrosSetup(); motorsSetup(); +#ifdef SERIAL_PORT + soft_serial( PORTD, 7, PORTB, 0, false ); + ss_begin(57600); + + ss_write_str( "\r\n\r\nConfig.RollGyroDirection = " ); + ss_write_num( Config.RollGyroDirection ); + ss_write_str( "\r\nConfig.PitchGyroDirection = " ); + ss_write_num( Config.PitchGyroDirection ); + ss_write_str( "\r\nConfig.YawGyroDirection = " ); + ss_write_num( Config.YawGyroDirection ); + ss_write_str( "\r\nConfig.CenterRollValue = " ); + ss_write_num( Config.CenterRollValue ); + ss_write_str( "\r\nConfig.CenterPitchValue = " ); + ss_write_num( Config.CenterPitchValue ); + ss_write_str( "\r\nConfig.CenterCollValue = " ); + ss_write_num( Config.CenterCollValue ); + ss_write_str( "\r\nConfig.CenterYawValue = " ); + ss_write_num( Config.CenterYawValue ); + ss_write( '\r\n' ); +#endif LED_DIR = OUTPUT; LED = 0; @@ -43,12 +64,12 @@ static void setup() * Flash the LED once at power on */ LED = 1; - _delay_ms(150); + _delay_ms(250); LED = 0; sei(); - _delay_ms(1500); + _delay_ms(2000); ReadGainPots(); ReadGainPots(); @@ -56,13 +77,38 @@ static void setup() bool rollMin = (GainInADC[ROLL] < (ADC_MAX * 5) / 100); // 5% threshold bool yawMin = (GainInADC[YAW] < (ADC_MAX * 5) / 100); // 5% threshold - if(pitchMin && rollMin && yawMin) { settingsClearAll(); } // Clear config - else if(pitchMin && yawMin) { motorsIdentify(); } // Motor identification + if(pitchMin && rollMin && yawMin) { + #ifdef SERIAL_PORT + ss_write_str( "\r\nClearing settings" ); + #endif + settingsClearAll(); + } // Clear config + else if(pitchMin && yawMin) { + #ifdef SERIAL_PORT + ss_write_str( "\r\nMotor Identify" ); + #endif + motorsIdentify(); + } // Motor identification // else if(pitchMin && rollMin) { } // Future use // else if(rollMin && yawMin) { } // Future use - else if(pitchMin) { receiverStickCenter(); } // Stick Centering Test - else if(rollMin) { gyrosReverse(); } // Gyro direction reversing - else if(yawMin) { motorsThrottleCalibration(); } // ESC throttle calibration + else if(pitchMin) { + #ifdef SERIAL_PORT + ss_write_str( "\r\nStick Center" ); + #endif + receiverStickCenter(); + } // Stick Centering Test + else if(rollMin) { + #ifdef SERIAL_PORT + ss_write_str( "\r\nGyro reverse" ); + #endif + gyrosReverse(); + } // Gyro direction reversing + else if(yawMin) { + #ifdef SERIAL_PORT + ss_write_str( "\r\nThrottle Calibrate" ); + #endif + motorsThrottleCalibration(); + } // ESC throttle calibration } static inline void loop() @@ -446,6 +492,7 @@ static inline void loop() MotorOut6 = 500; } #elif defined(TRI_COPTER) + MotorOut1 = 0; MotorOut2 = 0; MotorOut3 = 0; @@ -468,6 +515,7 @@ static inline void loop() LED = 0; output_motor_ppm(); + } int main() @@ -475,6 +523,6 @@ int main() setup(); while(1) - loop(); + loop(); return 1; } diff --git a/motors.c b/motors.c index 5f1e03a..20c8f2a 100644 --- a/motors.c +++ b/motors.c @@ -20,9 +20,14 @@ void motorsSetup() M2_DIR = OUTPUT; M3_DIR = OUTPUT; M4_DIR = OUTPUT; +#ifdef SERIAL_PORT + SER_TX_DIR = OUTPUT; + SER_RX_DIR = INPUT; +#else M5_DIR = OUTPUT; M6_DIR = OUTPUT; - +#endif + /* * timer0 (8bit) - run at 8MHz, used to control ESC pulses * We use 8Mhz instead of 1MHz (1 usec) to avoid alignment jitter. @@ -75,6 +80,7 @@ void output_motor_ppm() MotorOut4 = 0; else if(MotorOut4 > t) MotorOut4 = t; +#ifndef SERIAL_PORT if(MotorOut5 < 0) MotorOut5 = 0; else if(MotorOut5 > t) @@ -83,6 +89,7 @@ void output_motor_ppm() MotorOut6 = 0; else if(MotorOut6 > t) MotorOut6 = t; +#endif /* SERIAL_PORT */ t = 1000; MotorOut1+= t; @@ -90,26 +97,32 @@ void output_motor_ppm() MotorOut2+= t; MotorOut3+= t; MotorOut4+= t; +#ifndef SERIAL_PORT MotorOut5+= t; MotorOut6+= t; +#endif /* SERIAL_PORT */ #endif MotorOut1<<= 3; MotorOut2<<= 3; MotorOut3<<= 3; MotorOut4<<= 3; +#ifndef SERIAL_PORT MotorOut5<<= 3; MotorOut6<<= 3; +#endif /* SERIAL_PORT */ /* * Mirror M3, M4 to M5, M6, when possible, for hardware PPM * support. The compiler will throw away the above operations on * M5 and M6 when it sees these. */ +#ifndef SERIAL_PORT #if defined(DUAL_COPTER) || defined(TRI_COPTER) || defined(QUAD_COPTER) || defined(QUAD_X_COPTER) || defined(Y4_COPTER) MotorOut5 = MotorOut3; MotorOut6 = MotorOut4; #endif +#endif /* SERIAL_PORT */ /* * We can use timer compare output mode to provide jitter-free @@ -165,14 +178,25 @@ void output_motor_ppm() * * We hope that TCNT0 and TCNT1 are always synchronized. */ +#ifdef SERIAL_PORT + OCR0A = MotorStartTCNT1 + MotorOut3; + OCR0B = MotorStartTCNT1 + MotorOut4; +#else OCR0A = MotorStartTCNT1 + MotorOut5; OCR0B = MotorStartTCNT1 + MotorOut6; +#endif do { cli(); t = TCNT1; sei(); t-= MotorStartTCNT1; +#ifdef SERIAL_PORT + if(t + 0xff >= MotorOut3) + TCCR0A&= ~_BV(COM0A0); /* Clear pin on match */ + if(t + 0xff >= MotorOut4) + TCCR0A&= ~_BV(COM0B0); /* Clear pin on match */ +#else if(t >= MotorOut3) M3 = 0; if(t >= MotorOut4) @@ -181,6 +205,7 @@ void output_motor_ppm() TCCR0A&= ~_BV(COM0A0); /* Clear pin on match */ if(t + 0xff >= MotorOut6) TCCR0A&= ~_BV(COM0B0); /* Clear pin on match */ +#endif t-= ((2000 + PWM_LOW_PULSE_US) << 3) - 0xff; } while(t < 0); @@ -259,6 +284,7 @@ void output_motor_ppm() // TCCR0B = _BV(CS00) | _BV(FOC0A) | _BV(FOC0B); #endif +#ifndef SERIAL_PORT /* * Wait for the on time so we can turn on the software pins. */ @@ -293,6 +319,7 @@ void output_motor_ppm() #elif defined(QUAD_COPTER) || defined(QUAD_X_COPTER) || defined(Y4_COPTER) || defined(HEX_COPTER) || defined(Y6_COPTER) M3 = 1; M4 = 1; +#endif #endif /* * We leave with the output pins ON. diff --git a/soft_serial.c b/soft_serial.c index a5178c4..2815c43 100644 --- a/soft_serial.c +++ b/soft_serial.c @@ -35,6 +35,8 @@ The latest version of this library can always be found at #define _DEBUG 0 #define _DEBUG_PIN1 11 #define _DEBUG_PIN2 13 +#define __DELAY_BACKWARD_COMPATIBLE__ + // // Includes // @@ -47,13 +49,6 @@ The latest version of this library can always be found at #ifdef SERIAL_PORT -#define ECHO - -// static data -//static uint8_t ss_receivePin; -//static uint8_t ss_receiveBitMask; -//static uint8_t ss_transmitBitMask; - static uint16_t ss_rx_delay_centering; static uint16_t ss_rx_delay_intrabit; static uint16_t ss_rx_delay_stopbit; @@ -71,6 +66,7 @@ static volatile uint8_t ss_receive_buffer_head; #define digitalPinToPCICRbit(p) (((p) <= 7) ? 2 : (((p) <= 13) ? 0 : 1)) #define digitalPinToPCMSK(p) (((p) <= 7) ? (&PCMSK2) : (((p) <= 13) ? (&PCMSK0) : (((p) <= 21) ? (&PCMSK1) : ((uint8_t *)0)))) #define digitalPinToPCMSKbit(p) (((p) <= 7) ? (p) : (((p) <= 13) ? ((p) - 8) : ((p) - 14))) + // // Lookup table // @@ -83,6 +79,8 @@ typedef struct _DELAY_TABLE int16_t tx_delay; } DELAY_TABLE; +#define TUNED_DELAY +#ifdef TUNED_DELAY #if F_CPU == 16000000 static const DELAY_TABLE PROGMEM table[] = @@ -109,8 +107,8 @@ const int XMIT_START_ADJUSTMENT = 5; static const DELAY_TABLE table[] PROGMEM = { // baud rxcenter rxintra rxstop tx - { 115200, 1, 5, 5, 3, }, - { 57600, 1, 15, 15, 13, }, + { 115200, 1, 5, 5, 6, }, + { 57600, 1, 15, 15, 15, }, { 38400, 2, 25, 26, 23, }, { 31250, 7, 32, 33, 29, }, { 28800, 11, 35, 35, 32, }, @@ -153,6 +151,27 @@ const int XMIT_START_ADJUSTMENT = 6; #error This version of SoftwareSerial supports only 20, 16 and 8MHz processors +#endif +#else //TUNED_DELAY +static const DELAY_TABLE PROGMEM table[] = +{ + // baud rxcenter rxintra rxstop tx + { 115200, 3, 21, 21, 1.0/115200*1000*1000, }, + { 57600, 20, 43, 43, 1.0/57600*1000*1000, }, + { 38400, 37, 73, 73, 1.0/38400*1000*1000, }, + { 31250, 45, 89, 89, 1.0/31250*1000*1000, }, + { 28800, 46, 98, 98, 1.0/28800*1000*1000, }, + { 19200, 71, 148, 148, 1.0/19200*1000*1000, }, + { 14400, 96, 197, 197, 1.0/14400*1000*1000, }, + { 9600, 146, 297, 297, 1.0/9600*1000*1000, }, + { 4800, 296, 595, 595, 1.0/4800*1000*1000, }, + { 2400, 592, 1189, 1189, 1.0/2400*1000*1000, }, + { 1200, 1187, 2379, 2379, 1.0/1200*1000*1000, }, + { 300, 4759, 9523, 9523, 1.0/300*1000*1000, }, +}; + +const int XMIT_START_ADJUSTMENT = 4; + #endif // @@ -181,6 +200,7 @@ inline void DebugPulse(uint8_t pin, uint8_t count) #endif } +#ifdef TUNED_DELAY /* static */ inline void ss_tunedDelay(uint16_t delay) { uint8_t tmp=0; @@ -194,9 +214,12 @@ inline void ss_tunedDelay(uint16_t delay) { : "0" (delay) ); } +#else +inline void ss_tunedDelay(uint16_t delay) { + _delay_us( delay ); +} +#endif -// This function sets the current object as the "listening" -// one and returns true if it replaces another uint8_t ss_listen() { uint8_t oldSREG = SREG; @@ -280,11 +303,6 @@ void ss_recv() #endif ss_buffer_overflow = true; } - -#ifdef ECHO - ss_write( d ); - -#endif } #if GCC_VERSION < 40302 @@ -357,8 +375,6 @@ void ss_setTX(uint8_t reg, uint8_t bit) { SER_TX_DIR = OUTPUT; SER_TX = HIGH; - //ss_transmitBitMask = ~bit & 0xff; - //_transmitPortRegister = reg; } void ss_setRX(uint8_t reg, uint8_t bit) @@ -366,9 +382,6 @@ void ss_setRX(uint8_t reg, uint8_t bit) SER_RX_DIR = INPUT; if (!ss_inverse_logic) SER_RX= HIGH; // pullup for normal logic! - //ss_receivePin = SER_RX; - //ss_receiveBitMask = ~bit & 0xFF; - //_receivePortRegister = reg; } void ss_begin(long speed) @@ -391,13 +404,11 @@ void ss_begin(long speed) // Set up RX interrupts, but only if we have a valid RX baud rate if (ss_rx_delay_stopbit) { -#ifdef XXXX if (digitalPinToPCICR(ss_receivePin)) { - //*digitalPinToPCICR(ss_receivePin) |= _BV(digitalPinToPCICRbit(ss_receivePin)); - //*digitalPinToPCMSK(ss_receivePin) |= _BV(digitalPinToPCMSKbit(ss_receivePin)); + *digitalPinToPCICR(ss_receivePin) |= _BV(digitalPinToPCICRbit(ss_receivePin)); + *digitalPinToPCMSK(ss_receivePin) |= _BV(digitalPinToPCMSKbit(ss_receivePin)); } -#endif ss_tunedDelay(ss_tx_delay); // if we were low this establishes the end } @@ -411,8 +422,8 @@ void ss_begin(long speed) void ss_end() { - //if (digitalPinToPCMSK(ss_receivePin)) - //*digitalPinToPCMSK(ss_receivePin) &= ~_BV(digitalPinToPCMSKbit(ss_receivePin)); + if (digitalPinToPCMSK(ss_receivePin)) + *digitalPinToPCMSK(ss_receivePin) &= ~_BV(digitalPinToPCMSKbit(ss_receivePin)); } // Read data from buffer @@ -441,19 +452,42 @@ int ss_available() size_t ss_write_str( char *buf ) { - while( buf != 0 ) { + while( *buf != 0 ) { ss_write( *buf++ ); + ss_tunedDelay(ss_tx_delay<<4); } } +size_t ss_write_num( int n ) +{ + char buf[11]; + int i; + + buf[10] = 0; + + if( n != 0 ) { + for( i=9; i>0 && n>0; i-- ) + { + buf[i] = n%10 + '0'; + n = n/10; + } + } else { + buf[9] = '0'; + i=8; + } + + /* any good C book will tell you not to use the */ + /* value of a for loop variable after a loop. That's */ + /* really good advice however it's ignored here */ + ss_write_str( &buf[++i] ); +} + size_t ss_write(uint8_t b) { if (ss_tx_delay == 0) { return 0; } - LED ^= 1; - uint8_t oldSREG = SREG; cli(); // turn off interrupts for a clean txmit @@ -484,7 +518,6 @@ size_t ss_write(uint8_t b) ss_tx_pin_write(HIGH); // send 1 else ss_tx_pin_write(LOW); // send 0 - ss_tunedDelay(ss_tx_delay); } From 2fab28506f86e0a54f9fe223d89f22e7751cbe33 Mon Sep 17 00:00:00 2001 From: Angus Ainslie Date: Sat, 2 Mar 2013 15:18:51 -0700 Subject: [PATCH 4/5] clean out typedefs cruft --- typedefs.h | 34 ---------------------------------- 1 file changed, 34 deletions(-) diff --git a/typedefs.h b/typedefs.h index b3d5688..864f439 100644 --- a/typedefs.h +++ b/typedefs.h @@ -3,11 +3,6 @@ #include -//set bit or PORTB |= (1<<3); -//#define set_bit(port, bit) ((port) |= (uint8_t)(1 << bit)) -//clear bit -//#define clr_bit(port, bit) ((port) &= (uint8_t)~(1 << bit)) - #define INPUT 0 #define OUTPUT 1 @@ -30,33 +25,4 @@ typedef struct #define REGISTER_BIT(rg,bt) ((volatile _io_reg*)&rg)->bit##bt - -/* Example: - -#define BUTTON_PIN REGISTER_BIT(PINB,3) -#define LED_PORT REGISTER_BIT(PORTB,4) - -#define BUTTON_DIR REGISTER_BIT(DDRB,3) -#define LED_DIR REGISTER_BIT(DDRB,4) - -main() -{ - uint8_t is_button = BUTTON_PIN; - // this actually is expanded by the C preprocessor to: - // uint8_t is_button = ((volatile _io_reg*)&PINB)->bit3; - - LED_DIR = 1; - // which after the preprocessor looks like: - // ((volatile _io_reg*)&DDRB)->bit4 = 1; - - BUTTON_DIR = 0; - - while (1) { - LED_PORT = BUTTON_PIN; - } -} - -*/ - - #endif From 6ac1273ec77e0ad49e8893cc10a7a45438c61f7a Mon Sep 17 00:00:00 2001 From: Angus Ainslie Date: Sat, 16 Mar 2013 19:58:29 -0600 Subject: [PATCH 5/5] Fix up the soft serial RX interrupts handling Add menu for dumping some debug values wAdd Linvor rate setting --- config.h | 2 + io_cfg.h | 2 +- kk.c | 33 +++++- receiver.c | 73 ++++++++---- soft_serial.c | 307 +++++++++++++++++++++++++++++++++++++------------- soft_serial.h | 38 ++----- 6 files changed, 322 insertions(+), 133 deletions(-) diff --git a/config.h b/config.h index 0b4ae8e..f95df33 100644 --- a/config.h +++ b/config.h @@ -11,6 +11,8 @@ #include "typedefs.h" #define SAVE_CENTER +#define SERIAL_PORT +#define BT_SERIAL_PORT #include "io_cfg.h" diff --git a/io_cfg.h b/io_cfg.h index ea436ca..e897e0c 100644 --- a/io_cfg.h +++ b/io_cfg.h @@ -53,7 +53,7 @@ #define M3_DIR REGISTER_BIT(DDRD,6) #define M4_DIR REGISTER_BIT(DDRD,5) #define SER_TX_DIR REGISTER_BIT(DDRB,0) -#define SER_RX_DIR REGISTER_BIT(DDRD,5) +#define SER_RX_DIR REGISTER_BIT(DDRD,7) #else #define M3_DIR REGISTER_BIT(DDRB,0) #define M4_DIR REGISTER_BIT(DDRD,7) diff --git a/kk.c b/kk.c index f51b2c8..79f556d 100644 --- a/kk.c +++ b/kk.c @@ -23,8 +23,8 @@ static void setup() gyrosSetup(); motorsSetup(); #ifdef SERIAL_PORT - soft_serial( PORTD, 7, PORTB, 0, false ); - ss_begin(57600); + soft_serial( false ); + ss_begin(9600); ss_write_str( "\r\n\r\nConfig.RollGyroDirection = " ); ss_write_num( Config.RollGyroDirection ); @@ -73,6 +73,23 @@ static void setup() ReadGainPots(); ReadGainPots(); +#ifdef SERIAL_PORT + ReadGyros(); + ss_write_str( "\r\nPitch gain = " ); + ss_write_num( GainInADC[PITCH] ); + ss_write_str( "\r\nRoll gain = " ); + ss_write_num( GainInADC[ROLL] ); + ss_write_str( "\r\nYaw gain = " ); + ss_write_num( GainInADC[YAW] ); + ss_write_str( "\r\nPitch gyro = " ); + ss_write_num( gyroADC[PITCH] ); + ss_write_str( "\r\nRoll gyro = " ); + ss_write_num( gyroADC[ROLL] ); + ss_write_str( "\r\nYaw gyro = " ); + ss_write_num( gyroADC[YAW] ); + ss_write( '\r\n' ); +#endif + bool pitchMin = (GainInADC[PITCH] < (ADC_MAX * 5) / 100); // 5% threshold bool rollMin = (GainInADC[ROLL] < (ADC_MAX * 5) / 100); // 5% threshold bool yawMin = (GainInADC[YAW] < (ADC_MAX * 5) / 100); // 5% threshold @@ -518,11 +535,19 @@ static inline void loop() } + int main() { setup(); +#ifdef BT_SERIAL_PORT + setLinvorRate( 115200 ); +#endif - while(1) - loop(); + while(1) { + loop(); +#ifdef SERIAL_PORT + menu(); +#endif + } return 1; } diff --git a/receiver.c b/receiver.c index 236bc2f..cf33c80 100644 --- a/receiver.c +++ b/receiver.c @@ -1,6 +1,8 @@ #include "receiver.h" #include "settings.h" +extern bool Armed; + /*** BEGIN VARIABLES ***/ int16_t RxInRoll; int16_t RxInPitch; @@ -22,6 +24,8 @@ uint16_t CenterPitchValue; uint16_t CenterCollValue; uint16_t CenterYawValue; +uint16_t lastRoll = 0; + #ifdef TWIN_COPTER int16_t RxInOrgPitch; #endif @@ -34,25 +38,46 @@ int16_t RxInOrgPitch; * such as completely unused movs that clobber other registers. */ -ISR(PCINT2_vect, ISR_NAKED) +ISR(PCINT2_vect) { - if(RX_ROLL) { // rising - asm volatile("lds %A0, %1" : "=r" (RxChannel1Start) : "i" (&TCNT1L)); - asm volatile("lds %B0, %1" : "=r" (RxChannel1Start) : "i" (&TCNT1H)); - asm volatile("reti"); - } else { // falling - asm volatile( - "lds %A0, %3\n" - "lds %B0, %4\n" - "in %1, __SREG__\n" - "sub %A0, %A2\n" - "sbc %B0, %B2\n" - "out __SREG__, %1\n" - : "+r" (i_tmp), "+r" (i_sreg), "+r" (RxChannel1Start) - : "i" (&TCNT1L), "i" (&TCNT1H)); - RxChannel1 = i_tmp; + uint16_t roll = RX_ROLL; + uint8_t oldSREG = SREG; + cli(); + +#if defined(SERIAL_RX_M4) || defined(SERIAL_RX_M5) || defined(SERIAL_RX_M6) + if( !Armed ) + { + //LED ^= 0; + ss_recv(); + } - asm volatile ("reti"); +#endif + if( roll != lastRoll ) { + lastRoll = roll; + /* pin 17 */ + if(roll) { // rising + asm volatile("lds %A0, %1" : "=r" (RxChannel1Start) : "i" (&TCNT1L)); + asm volatile("lds %B0, %1" : "=r" (RxChannel1Start) : "i" (&TCNT1H)); + //asm volatile("reti"); + } else { // falling + asm volatile( + "lds %A0, %3\n" + "lds %B0, %4\n" + "in %1, __SREG__\n" + "sub %A0, %A2\n" + "sbc %B0, %B2\n" + "out __SREG__, %1\n" + : "+r" (i_tmp), "+r" (i_sreg), "+r" (RxChannel1Start) + : "i" (&TCNT1L), "i" (&TCNT1H)); + RxChannel1 = i_tmp; + } + } + + PCIFR |= 0x04; + SREG = oldSREG; // turn interrupts back on + +// asm volatile ("reti"); + } ISR(INT0_vect, ISR_NAKED) @@ -97,7 +122,7 @@ ISR(INT1_vect, ISR_NAKED) asm volatile ("reti"); } -ISR(PCINT0_vect, ISR_NAKED) +ISR(PCINT0_vect) { if(RX_YAW) { // rising asm volatile("lds %A0, %1" : "=r" (RxChannel4Start) : "i" (&TCNT1L)); @@ -115,6 +140,14 @@ ISR(PCINT0_vect, ISR_NAKED) : "i" (&TCNT1L), "i" (&TCNT1H)); RxChannel4 = i_tmp; } +#if defined( SERIAL_RX_M3 ) + if( !Armed ) + { + ss_recv(); + } + PCIFR |= 0x01; +#endif + asm volatile ("reti"); } /*** END RECEIVER INTERRUPTS ***/ @@ -147,8 +180,8 @@ void receiverSetup() * Enable Rx pin interrupts */ PCICR = _BV(PCIE0) | _BV(PCIE2); // PCINT0..7, PCINT16..23 enable - PCMSK0 = _BV(PCINT7); // PB7 - PCMSK2 = _BV(PCINT17); // PD1 + PCMSK0 |= _BV(PCINT7); // PB7 + PCMSK2 |= _BV(PCINT17); // PD1 EICRA = _BV(ISC00) | _BV(ISC10); // Any change INT0, INT1 EIMSK = _BV(INT0) | _BV(INT1); // External Interrupt Mask Register diff --git a/soft_serial.c b/soft_serial.c index 2815c43..4f4ba60 100644 --- a/soft_serial.c +++ b/soft_serial.c @@ -33,19 +33,21 @@ The latest version of this library can always be found at // oscilloscope or logic analyzer. Beware: it also slightly modifies // the bit times, so don't rely on it too much at high baud rates #define _DEBUG 0 -#define _DEBUG_PIN1 11 -#define _DEBUG_PIN2 13 -#define __DELAY_BACKWARD_COMPATIBLE__ +#define _DEBUG_PIN1 REGISTER_BIT(PORTB,3) +#define _DEBUG_PIN2 REGISTER_BIT(PORTB,5) // // Includes // #include #include +#include #include "soft_serial.h" #include "config.h" #include "io_cfg.h" +#include "settings.h" +#include "gyros.h" #ifdef SERIAL_PORT @@ -62,11 +64,6 @@ static char ss_receive_buffer[SS_MAX_RX_BUFF]; static volatile uint8_t ss_receive_buffer_tail; static volatile uint8_t ss_receive_buffer_head; -#define digitalPinToPCICR(p) (((p) >= 0 && (p) <= 21) ? (&PCICR) : ((uint8_t *)0)) -#define digitalPinToPCICRbit(p) (((p) <= 7) ? 2 : (((p) <= 13) ? 0 : 1)) -#define digitalPinToPCMSK(p) (((p) <= 7) ? (&PCMSK2) : (((p) <= 13) ? (&PCMSK0) : (((p) <= 21) ? (&PCMSK1) : ((uint8_t *)0)))) -#define digitalPinToPCMSKbit(p) (((p) <= 7) ? (p) : (((p) <= 13) ? ((p) - 8) : ((p) - 14))) - // // Lookup table // @@ -79,8 +76,6 @@ typedef struct _DELAY_TABLE int16_t tx_delay; } DELAY_TABLE; -#define TUNED_DELAY -#ifdef TUNED_DELAY #if F_CPU == 16000000 static const DELAY_TABLE PROGMEM table[] = @@ -151,27 +146,6 @@ const int XMIT_START_ADJUSTMENT = 6; #error This version of SoftwareSerial supports only 20, 16 and 8MHz processors -#endif -#else //TUNED_DELAY -static const DELAY_TABLE PROGMEM table[] = -{ - // baud rxcenter rxintra rxstop tx - { 115200, 3, 21, 21, 1.0/115200*1000*1000, }, - { 57600, 20, 43, 43, 1.0/57600*1000*1000, }, - { 38400, 37, 73, 73, 1.0/38400*1000*1000, }, - { 31250, 45, 89, 89, 1.0/31250*1000*1000, }, - { 28800, 46, 98, 98, 1.0/28800*1000*1000, }, - { 19200, 71, 148, 148, 1.0/19200*1000*1000, }, - { 14400, 96, 197, 197, 1.0/14400*1000*1000, }, - { 9600, 146, 297, 297, 1.0/9600*1000*1000, }, - { 4800, 296, 595, 595, 1.0/4800*1000*1000, }, - { 2400, 592, 1189, 1189, 1.0/2400*1000*1000, }, - { 1200, 1187, 2379, 2379, 1.0/1200*1000*1000, }, - { 300, 4759, 9523, 9523, 1.0/300*1000*1000, }, -}; - -const int XMIT_START_ADJUSTMENT = 4; - #endif // @@ -200,7 +174,6 @@ inline void DebugPulse(uint8_t pin, uint8_t count) #endif } -#ifdef TUNED_DELAY /* static */ inline void ss_tunedDelay(uint16_t delay) { uint8_t tmp=0; @@ -214,11 +187,6 @@ inline void ss_tunedDelay(uint16_t delay) { : "0" (delay) ); } -#else -inline void ss_tunedDelay(uint16_t delay) { - _delay_us( delay ); -} -#endif uint8_t ss_listen() { @@ -261,10 +229,19 @@ void ss_recv() #endif uint8_t d = 0; - + uint8_t val = 0; // If RX line is high, then we don't see any start bit // so interrupt is probably not for us - if (ss_inverse_logic ? ss_rx_pin_read() : !ss_rx_pin_read()) + #if defined(SERIAL_RX_M3) + val = REGISTER_BIT( PINB, 0 ); + #elif defined(SERIAL_RX_M4) + val = REGISTER_BIT( PIND, 7 ); + #elif defined(SERIAL_RX_M5) + val = REGISTER_BIT( PIND, 6 ); + #elif defined(SERIAL_RX_M6) + val = REGISTER_BIT( PIND, 5 ); + #endif + if (ss_inverse_logic ? val : !val ) { // Wait approximately 1/2 of a bit width to "center" the sample ss_tunedDelay(ss_rx_delay_centering); @@ -276,16 +253,27 @@ void ss_recv() ss_tunedDelay(ss_rx_delay_intrabit); DebugPulse(_DEBUG_PIN2, 1); uint8_t noti = ~i; - if (ss_rx_pin_read()) + #if defined(SERIAL_RX_M3) + val = REGISTER_BIT( PINB, 0 ); + #elif defined(SERIAL_RX_M4) + val = REGISTER_BIT( PIND, 7 ); + #elif defined(SERIAL_RX_M5) + val = REGISTER_BIT( PIND, 6 ); + #elif defined(SERIAL_RX_M6) + val = REGISTER_BIT( PIND, 5 ); + #endif + + if ( val ) d |= i; else // else clause added to ensure function timing is ~balanced d &= noti; + } // skip the stop bit ss_tunedDelay(ss_rx_delay_stopbit); DebugPulse(_DEBUG_PIN2, 1); - + if (ss_inverse_logic) d = ~d; @@ -300,9 +288,18 @@ void ss_recv() { #if _DEBUG // for scope: pulse pin as overflow indictator DebugPulse(_DEBUG_PIN1, 1); + _DEBUG_PIN1 ^= 1; #endif ss_buffer_overflow = true; } + + #if _DEBUG + ss_write( (char)d ); + ss_write( ':' ); + ss_write_hex( (int)d ); + ss_write( '\n' ); + ss_write( '\r' ); + #endif } #if GCC_VERSION < 40302 @@ -324,32 +321,15 @@ void ss_recv() void ss_tx_pin_write(uint8_t pin_state) { SER_TX = pin_state; -#ifdef XXXX - if (pin_state == LOW) - SER_TX &= ~ss_transmitBitMask; - else - SER_TX |= ss_transmitBitMask; -#endif } uint8_t ss_rx_pin_read() { return SER_RX; - //return SER_RX & ss_receiveBitMask; } -// -// Interrupt handling -// -//ISR(PCINT2_vect) -//{ -// ss_recv(); -//} - -void soft_serial(uint8_t rxReg, uint8_t rxBit, uint8_t txReg, uint8_t txBit, uint8_t inverse_logic ) +void soft_serial( uint8_t inverse_logic ) { - uint8_t vect; - ss_rx_delay_centering = 0; ss_rx_delay_intrabit = 0; ss_rx_delay_stopbit = 0; @@ -357,31 +337,35 @@ void soft_serial(uint8_t rxReg, uint8_t rxBit, uint8_t txReg, uint8_t txBit, uin ss_buffer_overflow = false; ss_inverse_logic = inverse_logic; - #if defined(SERIAL_RX_M4) - vect = PCINT23; + #if defined(SERIAL_RX_M3) + PCICR |= _BV(PCIE0); + PCMSK0 |= _BV(PCINT0); // PB0 + #elif defined(SERIAL_RX_M4) + PCICR |= _BV(PCIE2); + PCMSK2 |= _BV(PCINT23); // PD7 #elif defined(SERIAL_RX_M5) - vect = PCINT22; + PCICR |= _BV(PCIE2); + PCMSK2 |= _BV(PCINT22); // PD6 #elif defined(SERIAL_RX_M6) - vect = PCINT21; + PCICR |= _BV(PCIE2); + PCMSK2 |= _BV(PCINT21); // PD5 #endif - PCMSK2 |= _BV(vect); // PD1 - - ss_setTX(txReg, txBit); - ss_setRX(rxReg, rxBit); + ss_setTX(); + ss_setRX(); } -void ss_setTX(uint8_t reg, uint8_t bit) +void ss_setTX() { SER_TX_DIR = OUTPUT; SER_TX = HIGH; } -void ss_setRX(uint8_t reg, uint8_t bit) +void ss_setRX() { SER_RX_DIR = INPUT; if (!ss_inverse_logic) - SER_RX= HIGH; // pullup for normal logic! + SER_RX = HIGH; // pullup for normal logic! } void ss_begin(long speed) @@ -404,14 +388,22 @@ void ss_begin(long speed) // Set up RX interrupts, but only if we have a valid RX baud rate if (ss_rx_delay_stopbit) { - if (digitalPinToPCICR(ss_receivePin)) - { - *digitalPinToPCICR(ss_receivePin) |= _BV(digitalPinToPCICRbit(ss_receivePin)); - *digitalPinToPCMSK(ss_receivePin) |= _BV(digitalPinToPCMSKbit(ss_receivePin)); - } +#ifdef SERIAL_RX_M4 + PCMSK2 = _BV( PCINT23 ); +#elif defined( SERIAL_RX_M5 ) + PCMSK2 |= _BV( PCINT22 ); +#elif defined( SERIAL_RX_M6 ) + PCMSK2 |= _BV( PCINT21 ); +#elif defined( SERIAL_RX_M3 ) + PCMSK0 |= _BV( PCINT0 ); +#endif + ss_tunedDelay(ss_tx_delay); // if we were low this establishes the end } + REGISTER_BIT(DDRB,3) = OUTPUT; + REGISTER_BIT(DDRB,5) = OUTPUT; + #if _DEBUG pinMode(_DEBUG_PIN1, OUTPUT); pinMode(_DEBUG_PIN2, OUTPUT); @@ -422,8 +414,15 @@ void ss_begin(long speed) void ss_end() { - if (digitalPinToPCMSK(ss_receivePin)) - *digitalPinToPCMSK(ss_receivePin) &= ~_BV(digitalPinToPCMSKbit(ss_receivePin)); +#ifdef SERIAL_RX_M4 + PCMSK2 &= ~_BV( PCINT23 ); +#elif defined( SERIAL_RX_M5 ) + PCMSK2 &= ~_BV( PCINT22 ); +#elif defined( SERIAL_RX_M6 ) + PCMSK2 &= ~_BV( PCINT21 ); +#elif defined( SERIAL_RX_M3 ) + PCMSK0 &= ~_BV( PCINT0 ); +#endif } // Read data from buffer @@ -480,6 +479,39 @@ size_t ss_write_num( int n ) /* value of a for loop variable after a loop. That's */ /* really good advice however it's ignored here */ ss_write_str( &buf[++i] ); + + return 9 - i; +} + +size_t ss_write_hex( int n ) +{ + char buf[9]; + int i, rem; + + buf[8] = 0; + + if( n != 0 ) { + for( i=7; i>0 && n>0; i-- ) + { + rem = n%16; + if( rem < 10 ) + buf[i] = rem + '0'; + else + buf[i] = rem - 10 + 'A'; + + n = n/16; + } + } else { + buf[7] = '0'; + i=6; + } + + /* any good C book will tell you not to use the */ + /* value of a for loop variable after a loop. That's */ + /* really good advice however it's ignored here */ + ss_write_str( &buf[++i] ); + + return 9 - i; } size_t ss_write(uint8_t b) @@ -554,4 +586,125 @@ int ss_peek() return ss_receive_buffer[ss_receive_buffer_head]; } +void setLinvorRate( int32_t rate ) +{ + char buffer[16], rate_str[16]; + int i = 0; + int c = 0; + + ss_write_str( "AT\r\n" ); + _delay_ms(1000); + do { + c = ss_read(); + if( c != -1 && i < 16 ) + buffer[i++] = c; + } while( c != -1 ); + + switch( rate ) { + case 1200: + strcpy( rate_str, "AT+BAUD1\r\n" ); + break; + case 2400: + strcpy( rate_str, "AT+BAUD2\r\n" ); + break; + case 4800: + strcpy( rate_str, "AT+BAUD3\r\n" ); + break; + case 9600: + strcpy( rate_str, "AT+BAUD4\r\n" ); + break; + case 19200: + strcpy( rate_str, "AT+BAUD5\r\n" ); + break; + case 38400: + strcpy( rate_str, "AT+BAUD6\r\n" ); + break; + case 57600: + strcpy( rate_str, "AT+BAUD7\r\n" ); + break; + case 115200L: + strcpy( rate_str, "AT+BAUD8\r\n" ); + break; +#ifdef UART_SERIAL + case 230400L: + strcpy( rate_str, "AT+BAUD9\r\n" ); + break; + case 460800L: + strcpy( rate_str, "AT+BAUDA\r\n" ); + break; + case 921600L: + strcpy( rate_str, "AT+BAUDB\r\n" ); + break; + case 1382400L: + strcpy( rate_str, "AT+BAUDC\r\n" ); + break; +#endif + } + + if( !strncmp( buffer, "OK", 2 )) + { + i = 0; + ss_write_str( rate_str ); + _delay_ms(1000); + do { + c = ss_read(); + if( c != -1 && i < 16 ) + buffer[i++] = c; + } while( c != -1 ); + } + else + { + ss_write_str( "Baud rate setting failed" ); + } + + if( !strncmp( buffer, "OK", 2 )) + soft_serial( 115200L ); +} + +void menu( void ) +{ + int16_t c; + + c = ss_read(); + if( c != -1 ) { + switch( c ) { + case 'g': + ReadGainPots(); + ss_write_str( "\r\nPitch gain = " ); + ss_write_num( GainInADC[PITCH] ); + ss_write_str( "\r\nRoll gain = " ); + ss_write_num( GainInADC[ROLL] ); + ss_write_str( "\r\nYaw gain = " ); + ss_write_num( GainInADC[YAW] ); + ss_write( '\r\n' ); + break; + case 'G': + ReadGyros(); + ss_write_str( "\r\nPitch gyro = " ); + ss_write_num( gyroADC[PITCH] ); + ss_write_str( "\r\nRoll gyro = " ); + ss_write_num( gyroADC[ROLL] ); + ss_write_str( "\r\nYaw gyro = " ); + ss_write_num( gyroADC[YAW] ); + ss_write( '\r\n' ); + break; +#ifdef SAVE_CENTER + case 'S': + // this doesn't return + receiverStickCenter(); + break; + case 's': + ss_write_str( "\r\nRoll Center = " ); + ss_write_num( Config.CenterRollValue ); + ss_write_str( "\r\nPitch Center = " ); + ss_write_num( Config.CenterPitchValue ); + ss_write_str( "\r\nCollector Center = " ); + ss_write_num( Config.CenterCollValue ); + ss_write_str( "\r\nYaw Center = " ); + ss_write_num( Config.CenterYawValue ); + break; +#endif + } + } +} #endif diff --git a/soft_serial.h b/soft_serial.h index 0de865e..e94bbac 100644 --- a/soft_serial.h +++ b/soft_serial.h @@ -47,49 +47,25 @@ The latest version of this library can always be found at uint8_t ss_listen(void); void ss_recv(void); uint8_t ss_rx_pin_read(void); -void ss_setTX(uint8_t reg, uint8_t bit); -void ss_setRX(uint8_t reg, uint8_t bit); +void ss_setTX(void); +void ss_setRX(void); void ss_end(void); int ss_read(void); uint8_t ss_isListening(void); void ss_tunedDelay(uint16_t delay); int ss_available(void); -void soft_serial(uint8_t rxReg, uint8_t rxBit, uint8_t txReg, uint8_t txBit, uint8_t inverse_logic); +void soft_serial(uint8_t inverse_logic); void ss_begin(long speed); size_t ss_write_str(char *buf); size_t ss_write(uint8_t b); +size_t ss_write_num( int n ); +size_t ss_write_hex( int n ); int ss_peek(void); void ss_flush(void); - -#ifdef XXXX -class SoftwareSerial -{ -private: - static SoftwareSerial *active_object; - - // private methods - void tx_pin_write(uint8_t pin_state); - - // private static method for timing - -public: - // public methods - ~SoftwareSerial(); - void end(); - bool overflow() { bool ret = _buffer_overflow; _buffer_overflow = false; return ret; } - - virtual size_t write(uint8_t byte); - virtual int available(); - virtual void flush(); - - //using Print::write; - - // public only for easy access by interrupt handlers -}; -#endif +void setLinvorRate( int32_t rate ); +void menus( void ); #define HIGH 1 #define LOW 0 - #endif