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 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