/*************************************************************************** YAPP - Yet Another PIC Program YAPP.c Copyright © Martin Newell, 2005,2006 San Jose, California Permission granted for personal use only Use for commercial purposes prohibited without permission ** Updated Dec 24, 2005 * Option to support servo-style PCM on throttle to drive an electronic speed controller, ESC, instead of the pulse-coded brushed motor output. This was added to be able to drive a 3-phase brushless motor controller. This form of motor output is not controlled by the stand-by procesdure. Behavior on loss of signal is determined by the external electronic speed control. See #define MOTOR_PWM * New output pin option, see #define RUDDER_RUDDER Outputs rudder on pairs of pins to give 50mA drive off the chip which can drive >=80 ohm actuator directly. Uses pins 2,3 connected and 6,7 connected. Motor is output on normal pin 5. Intended for an add-on chip to add 4th channel rudder control. Can be used in combination with MOTOR_PWM (see above) to give 3 actuators and PWM motor output. ** Original Version June 17, 2005 * For PIC12F629/675 and PIC12F635/683 * With PIC12F629 (4MHz), input is read to resolution of 10 for left, right, up, down and 20 for throttle With PIC12F635 (8MHz), input is read to resolution of 20 for left, right, up, down and 40 for throttle but the throttle reading is divided by 2 since output resolution is 20 in all cases Output resolution of 20 for aileron and elevator is used even for 4MHz to give finer resolution in expo table. Input resolution for 4MHz could be doubled by using the GPIORead and OCHalfTimer variables, but currently not * Uses Pulse Frequency Modulation, PFM Output frequency for PIC12F629(4MHz) is 5.26 KHz to 26.3 Khz at mid range Output frequency for PIC12F635(8MHz) is 5.26 KHz to 52.6 Khz at mid range * Decodes 4 channels 4th channel could be used in separate chip - see RUDDER_ELEVATOR and RUDDER_RUDDER below * Adapts to positive- or negative-going input * Adapts to transmitter channel configuration. Expects aileron, (which is typically used to control the rudder), elevator and rudder to be on increasing channels. with throttle channel inserted anywhere. Throttle taken to be channel furthest from center on start up. * Supports Rudder/Aileron-Elevator and Elevon modes. Attach left elevon to aileron outputs with up for left, and right elevon to elevator outputs with up for up. See "Startup Configuration" below to activate Elevon mode. * Uses "Startup Configuration" period while throttle stick above zero. Motor will not start. Start Rx with Throttle at about 3/4 max Startup Configurations include: 1. Move Aileron/Elevator stick to top right corner for 1/2 second => sets Elevon mode 2. Move Aileron/Elevator stick to top left corner for 1/2 second => un-sets Elevon mode 3. Move Aileron/Elevator stick to bottom right corner for 1/2 second => sets negative Expo mode 4. Move Aileron/Elevator stick to bottom left corner for 1/2 second => un-sets negative Expo mode 5. Move Throttle stick to max for 1/2 second => callibrates Rx to neutral at current Aileron, Elevator and Rudder stick positions 6. Move Throttle stick to minimum for 1/2 second => Enables motor and all configurations are locked. Control surfaces will twitch when a mode is changed, for confirmation. Configuration is remembered in EEPROM and is restored on next power up, except for Center Callibration. Normal operation, after configuring: Start Rx with Throttle at minimum => Saved modes are restored and throttle is enabled, or, Start Rx with Throttle at maximum => Rx callibrates center to Tx sticks. Move throttle to minimum to fly. * Goes through progressive standby if signal lost, until found again This requires finding inter-frame gap and some number of valid frames 1. After 1.5 seconds - Aileron goes 1/5 left, Elevator to neutral 2. After another 1.5 seconds - Throttle multiplied by 3/4 3. Repeat 2. until Throttle zero * Uses polling, no interrupts * This file reads best with 4 character tabs, fixed pitch font ****************************************************************************/ // Enable only one of the following #define PIC12F629 // 8 pin,, 4MHz, OSCCAL, EEDATA //#define PIC12F635 // 8 pin, 8MHz, OSCTUNE, EEDAT //#define PIC12F675 // 8 pin,, 4MHz, OSCCAL, EEDATA, A/D //#define PIC12F683 // 8 pin,, 8MHz, OSCTUNE, EEDAT, A/D /************************** Common Declarations ****************************/ #ifdef PIC12F629 #define CLOCK_FREQ 4 // MHz #define TICK 38 // One tick of OCTimer - units of uS #pragma config = 0x3F84 #endif #ifdef PIC12F635 #define CLOCK_FREQ 8 // MHz #define TICK 19 // One tick of OCTimer - units of uS #pragma config = 0x33C4 #pragma bit IRCF0 @ OSCCON.4 // definitions missing from PIC12F635.h #pragma bit IRCF1 @ OSCCON.5 #pragma bit IRCF2 @ OSCCON.6 #endif #ifdef PIC12F675 #define CLOCK_FREQ 4 // MHz #define TICK 38 // One tick of OCTimer - units of uS #define HAS_AD #pragma config = 0x3F84 #endif #ifdef PIC12F683 #define CLOCK_FREQ 8 // MHz #define TICK 19 // One tick of OCTimer - units of uS #define HAS_AD #pragma config = 0x33C4 #pragma bit IRCF0 @ OSCCON.4 // definitions missing from PIC12F683.h #pragma bit IRCF1 @ OSCCON.5 #pragma bit IRCF2 @ OSCCON.6 #endif //#define SIMULATOR // Comment out for release code #pragma update_RP 0 // Disable setting RP0 except where need in initialization // Chip Configuration parameters // Oscillator Internal // Watchdog Off // Power Up Timer On // MCLR Internal // Brown Out Off // Code Protect Off // Data EE Read Protect Off //#pragma optimize 1 char pulse; // Sense of timing pulses. Set to INPUT_PIN or 0 for +ve or -ve going pulses #define RESOLUTION 20 // Defines number of steps from 0 to max for each channel #define INPUT_PIN 8 // Used instead of bit addressing for better code #define INPUT_ON ((GPIO & INPUT_PIN) == pulse) // value of 'pulse' defines whether On = high or low #define INPUT_OFF ((GPIO & INPUT_PIN) != pulse) // value of 'pulse' defines whether Off = low or high char MotorChannel; // The motor channel [0..3] //#define MOTOR_PWM // Generate servo-style PWM on motor pin instead of PFM for direct motor drive // Define bits in GPIO // Exactly one of the following 4 blocks must be enabled by un-commenting the initial #define #define AILERON_ELEVATOR // Conventional Aileron-Elevator (e.g. Leichty) // _______ // Vcc -|1 8|- Ground // Elevator -|2 7|- Aileron // Elevator -|3 6|- Aileron // Input -|4_____5|- Motor // #ifdef AILERON_ELEVATOR #define AILERON_LEFT 1 // GP0 Aileron Left #define AILERON_RIGHT 2 // GP1 Aileron Right #define MOTOR 4 // GP2 Motor #define ELEVATOR_DOWN 0x10 // GP4 Elevator Down #define ELEVATOR_UP 0x20 // GP5 Elevator Up #define RUDDER_LEFT 0 // Not output Rudder Left #define RUDDER_RIGHT 0 // Not output Rudder Right #define INVERT_OUTPUT_MASK 0 // No inversions #endif //#define RUDDER_ELEVATOR // For auxiliary 4th channel chip // _______ // Vcc -|1 8|- Ground // Elevator -|2 7|- Rudder // Elevator -|3 6|- Rudder // Input -|4_____5|- Motor // #ifdef RUDDER_ELEVATOR #define RUDDER_LEFT 1 // GP0 Rudder Left #define RUDDER_RIGHT 2 // GP1 Rudder Right #define MOTOR 4 // GP2 Motor #define ELEVATOR_DOWN 0x10 // GP4 Elevator Down #define ELEVATOR_UP 0x20 // GP5 Elevator Up #define AILERON_LEFT 0 // Not output Aileron Left #define AILERON_RIGHT 0 // Not output Aileron Right #define INVERT_OUTPUT_MASK 0 // No inversions #endif //#define RUDDER_RUDDER // For auxiliary 4th channel chip // Connect pins 2&3 and 6&7 to give double current drive // _______ // Vcc -|1 8|- Ground // Rudder R -|2 7|- Rudder L // Rudder R -|3 6|- Rudder L // Input -|4_____5|- Motor // #ifdef RUDDER_RUDDER #define RUDDER_LEFT 3 // GP0,GP1 Rudder Left #define RUDDER_RIGHT 0x30 // GP4,GP5 Rudder Right #define MOTOR 4 // GP2 Motor #define ELEVATOR_DOWN 0 // Not output Elevator Down #define ELEVATOR_UP 0 // Not output Elevator Up #define AILERON_LEFT 0 // Not output Aileron Left #define AILERON_RIGHT 0 // Not output Aileron Right #define INVERT_OUTPUT_MASK 0 // No inversions #endif //#define RFFS_100 // _______ // Vcc -|1 8|- Ground // Motor -|2 7|- Aileron // Elevator -|3 6|- Aileron // Input -|4_____5|- Elevator // #ifdef RFFS_100 #define ELEVATOR_UP 1 // GP0 Elevator Up #define ELEVATOR_DOWN 2 // GP1 Elevator Down #define AILERON_RIGHT 4 // GP2 Aileron Right #define AILERON_LEFT 0x10 // GP4 Aileron Left #define MOTOR 0x20 // GP5 Motor #define RUDDER_LEFT 0 // Not output Rudder Left #define RUDDER_RIGHT 0 // Not output Rudder Right #define INVERT_OUTPUT_MASK 0x1F // Invert all outputs except Motor #endif // Values of control channels signed char count[4]; // counts for the 4 channels char countPrev[4]; // previous counts for the 4 channels char GP[4]; // GPIO bit for each channel signed char counti; // temp count[i] char count2; // Saves motor count[2] in case corrupted while decoding input char motorESC; // Mask for motor output to electronic speed control // Startup Configuration counters #define CENTER_CALIBRATE // Enables the center callibration function in StartUp char Param; // Parameter flags saved in EEPROM char MotorEnable; // >=0 until throttle has been zero for 1/2 second. i.e.bit 7 used as a flag char ElevonCounter; // >=0 until right stick in corner for 1/2 second while MotorEnable off. bit 7 used as a flag char ExpoCounter; // >=0 until right stick in corner for 1/2 second while MotorEnable off. bit 7 used as a flag #ifdef CENTER_CALIBRATE char CenterCounter; // >=0 until throttle stick at max for 1/2 second while MotorEnable off. bit 7 used as a flag #endif #define ELEVON_BIT 1 // Bit set in Param for Elevon mode #define EXPO_BIT 2 // Bit set in Param for Expo mode #define CONFIG_COUNTER 28 // Units are frame times, ~18mS char GetOsccal(); // Defined at end of file // Timing parameters // My Tower transmitter: // Frame time: 18,120 uS, constant // Channel time: 6,060 - 9,700 uS // Interframe: 8,420 - 12,060 uS // Ch1(Ail): Left: 1078, Center: 1510, Right: 1930 uS // Ch2(Ele): Down: 1078, Center: 1510, Up: 1930 uS // Ch3(Thr): Min: 1048, Max: 1940 uS // Ch4(Rud): Left: 1124, Center: 1520, Right: 1940 uS // Ch5(Dum): Cons: 970 uS // Timing pulse width: 300 uS, but signal from Micro Leichty front end is 800-900 nS wide when slightly swamped // Use 1500 +- 380 (1120 to 1880) for aileron, elevator and rudder channels // Use 1140 to 1900 for motor channel #define MIN_PULSE_WIDTH (100/TICK) // minimum width of timing pulse - units of ticks #define MAX_PULSE_WIDTH (1000/TICK) // maximum width of timing pulse - units of ticks #define MIN_PERIOD (950/TICK) // minimum channel period - units of ticks #define MAX_PERIOD (2050/TICK) // maximum channel period - units of ticks #define CENTER_PERIOD (1500/TICK) // default center stick channel period - units of ticks #define MIN_MOTOR_PERIOD (1140/TICK) // minimum motor period - units of ticks #define FULL_THROW (380/TICK) // aileron & elevator counts at full throw #ifdef CENTER_CALIBRATE char Center_Aileron; // Measured Aileron at center stick char Center_Elevator; // Measured Elevator at center stick char Center_Rudder; // Measured Rudder at center stick #endif /********************************* Expo *******************************************/ // Lookup and return value from "exponential" array // Based on (t^2+1)*t/2 // Done this way to avoid using data space // No bounds checking - argument must be in range [0..20] // To fit in 8 cycles, must be called as: // OC(); W = x; y = Expo(); OC(); // Do not redefine as char Expo(char x) because that incurs an extra cycle // This procedure needs to be in low memory char Expo() { #asm ADDWF PC,1 ; jump to appropriate place in data table RETLW .0 // 0 RETLW .0 // 1 RETLW .1 // 2 RETLW .1 // 3 RETLW .2 // 4 RETLW .2 // 5 RETLW .3 // 6 RETLW .3 // 7 RETLW .4 // 8 RETLW .5 // 9 RETLW .6 // 10 RETLW .7 // 11 RETLW .8 // 12 RETLW .9 // 13 RETLW .10 // 14 RETLW .11 // 15 RETLW .12 // 16 RETLW .14 // 17 RETLW .16 // 18 RETLW .18 // 19 RETLW .20 // 20 #endasm } /********************************* OutputCycle ********************************* SaveFSR points to even numbered entries. Buffer size must be even. For initialization, can set GPIONext to 0. buf:GPIO-0 <- SaveFSR - (value will be copied to GPIOCurr during this entry for output at end) GPIO-1 - (value will be copied to GPIONext ready for next entry) GPIO-2 GPIO-3 - value was copied to GPIONext on previous entry for output at beginning GPIO-4 <- SaveFSR - (value will be copied to GPIOCurr during this entry for output at end) GPIO-5 - (value will be copied to GPIONext ready for next entry) GPIO-6 . . . GPIO-16 GPIO-17 - value was copied to GPIONext on previous entry for output at beginning GPIO-18 <- SaveFSR - (value will be copied to GPIOCurr during this entry for output at end) GPIO-19 - (value will be copied to GPIONext ready for next entry) ********************************************************************************/ // Doubled OutputCycle with lookahead // 30 instructions for 2 outputs // Allows 8 cycles in main for each entry // Total cycle time = 30+8 = 38 // Synchronizes exactly to 38 cycles, provided it is called in time // Outputs 2 half cycles to GPIO per entry // 38/2 = 19 => 19*20 = 380 cycles => 2.63KHz-26.3KHz @ 4MHz clock, 5.26KHz-52.6KHz @ 8MHz clock // Also records GPIO in GPIORead halfway through for input timing // Also increments OCTimer once per entry // Touch this procedure at your own risk #define TMR0_PERIOD 38 // This is the period of OC() allowing 8 cycles outside #define OutputBuffer_Address 0x60-RESOLUTION // Carefully chosen so bit 5 signals overflow char OutputBuffer[RESOLUTION] @ OutputBuffer_Address; // Single GPIO output buffer - same on 12F629 and 635 char *SaveFSR; // Points to next GPIO value to be read char GPIORead; // Value of GPIO read in OutputCycle2() for external use char OCTimer; // Incremented once per cycle char GPIOCurr; // Used internally in OutputCycle2() for value output at end char GPIONext; // Used internally in OutputCycle2() for value output at beginning of next cycle void OC() { #asm // On entry: // GPIONext is next GPIO value to be output // SaveFSR points to value after GPIONext MOVLW .15 ; PC += TMR0 & 0xF; // Synchronize BCF 0x03,RP0 ; not strictly needed since #pragma update_RP 0 ANDWF TMR0,W ; limit jump to 15 ADDWF PC,1 ; jump to appropriate place in delay sequence NOP ; nop();0 delay sequence to get exact synchronization NOP ; nop();1 NOP ; nop();2 NOP ; nop();3 NOP ; nop();4 NOP ; nop();5 NOP ; nop();6 <- set breakpoint here to check for calling in time NOP ; nop();7 <- should never be before here NOP ; nop();8 NOP ; nop();9 NOP ; nop();10 NOP ; nop();11 NOP ; nop();12 NOP ; nop();13 NOP ; nop();14 ; +15 is next instruction MOVF GPIONext,W ; 1st GPIO output as soon as possible MOVWF GPIO MOVLW 5+3-TMR0_PERIOD ; TMR0 = 5+3-TMR0_PERIOD; // 5 is value TMR0 should have on getting to next instruction // 3 is for delay in TMR0 restarting MOVWF TMR0 MOVF SaveFSR,W ; FSR = SaveFSR; MOVWF FSR // MOVF GPIO,W ; GPIORead = GPIO; // Read GPIO for input timing - currently not used // MOVWF GPIORead ; This is half way through testing loop INCF OCTimer ; OCTimer++; // Increment timer for input timing MOVF motorESC,W ; GPIOCurr = INDF + motorESC; ADDWF INDF,W MOVWF GPIOCurr ; will be output at end INCF FSR,1 ; FSR++; MOVF motorESC,W ; GPIONext = INDF + motorESC; ADDWF INDF,W ; ready for next cycle MOVWF GPIONext INCF FSR,1 ; FSR++; MOVLW OutputBuffer_Address ; if (FSR.5) SaveFSR = OutputBuffer_Address; //wraps BTFSS FSR,5 ; else SaveFSR = FSR MOVF FSR,W ; Wraps when bit 5 set in FSR MOVWF SaveFSR MOVF GPIOCurr,W MOVWF GPIO ; 2nd GPIO output, exactly 19 cycles after 1st one RETURN #endasm } // Initialize OC() void OCInit() { char i; for (i = 0; i < RESOLUTION; i++) OutputBuffer[i] = INVERT_OUTPUT_MASK; // Everything off SaveFSR = OutputBuffer; // Points to next GPIO value to be read GPIONext = INVERT_OUTPUT_MASK; // dummy for first entry TMR0 = -4; // must stay "<0" when first used in OC() OC(); } /********************************* Chip Init *******************************************/ // Initialize Chip State void ChipInit() { #pragma update_RP 1 // Allow setting RP0 for initialization #if CLOCK_FREQ == 8 IRCF0 = IRCF1 = IRCF2 = 1; // Select 8MHz OSCTUNE = GetOsccal(); // Set specified oscillator calibration #else OSCCAL = GetOsccal(); // Set built-in oscillator calibration #endif // PIC12F635 TRISIO = 8; // Input on GP3. 0 = Output, 1 = Input TMR1ON = 0; // TMR1 off TMR1IF = 0; // TMR1 interrupt flag cleared OPTION = 8; // Assign prescaler to WDT, so TMR0 clock is 1:1 T1CKPS0 = 1; T1CKPS1 = 1; // 8:1 prescale on TMR1 TMR1ON = 1; // TMR1 on - never turned off again CM2 = 1; CM1 = 1; CM0 = 1; // Digital I/O #ifdef HAS_AD ANSEL = 0; //Turn off ADC on the 12F675 #endif GPIO = INVERT_OUTPUT_MASK; // Zero coding all outputs #pragma update_RP 0 // Don't set RP0 any more - needed to be able to call OC() fast enough } /********************************* Jingle *******************************************/ #define on ((AILERON_LEFT + ELEVATOR_DOWN + RUDDER_LEFT) ^ INVERT_OUTPUT_MASK) #define off INVERT_OUTPUT_MASK #define PER (8/CLOCK_FREQ) #define BEAT 200/PER // Re-use GP[0] for variables #define lastt GP[0] // last value read from timer for counting ms #define ms GP[1] // millisecond timer #define endt GP[2] // time sought #define t GP[3] // current time // Wait n * 4 uS // Increment ms every 1 mS void Wait (char n) { t = TMR1L; if (t < lastt) ms++; lastt = t; endt = t + n; //let it overflow while (TMR1L != endt); } // Play a note for one beat (half period in 4mS units) void Note(char tone) { for (ms = 0; ms < BEAT;) { GPIO = on; Wait(tone); GPIO = 0; Wait(tone); } } // Play nothing for one beat void Gap() { for (ms = 0; ms < BEAT;) Wait(10); } // Play a jingle void Jingle() { lastt = 0; ms = 0; TMR1L = 0; Note(174/PER); Note(207/PER); Gap(); Gap(); Gap(); Note(207/PER); Note(195/PER); Note(174/PER); Note(103/PER); Gap(); Note(103/PER); Gap(); Note(130/PER); Gap(); Gap(); Gap(); } /********************************* EEPROM Read and Write *******************************************/ #define MAGIC 42 // In word 0 of EEPROM says it is initialized // Write data to the EEPROM // Beware - it takes about 4 mS for the write to complete #pragma update_RP 1 // Allow setting RP0 for initialization void WriteEEPROM(char addr, char data) { EEIF = 0; // clear done flag EEADR = addr; // set up address #if CLOCK_FREQ == 8 EEDAT = data; // set up data to write #else EEDATA = data; // set up data to write #endif WREN = 1; // enable writing EECON2 = 0x55; // required sequence EECON2 = 0xAA; WR = 1; // do the write WREN = 0; // disable other writes while (EEIF == 0) {}; // wait for write to complete - about 4mS EEIF = 0; // clear done flag #pragma update_RP 0 // Don't set RP0 any more - needed to be able to call OC() fast enough RP0 = 0; // assumed in main() #if CLOCK_FREQ == 8 RP1 = 0; // assumed in main() #endif } #pragma update_RP 1 // Allow setting RP0 for initialization char ReadEEPROM(char addr) { char dat; EEADR = addr; RD = 1; // do the read #if CLOCK_FREQ == 8 dat = EEDAT; #else dat = EEDATA; #endif #pragma update_RP 0 // Don't set RP0 any more - needed to be able to call OC() fast enough RP0 = 0; #if CLOCK_FREQ == 8 RP1 = 0; // assumed in main() #endif return dat; // result immediately available } void ParamInit() { // Get parameters from EEPROM, if set // EEPROM word zero must be 42 // EEPROM word 1 stores Param Param = ReadEEPROM(0); if (Param == MAGIC) { // 42 used as keyword to say EEPROM has been written Param = ReadEEPROM(1); } else { WriteEEPROM(0, MAGIC); // Do this here because it takes 4mS to settle Param = 0; // initialize all parameter flags to 0 } } /********************************* StartupConfig *******************************************/ // Set various parameters while in Startup (before MotorEnable) // Right stick Right-Top (+,-): Elevon mode Elevon Off ---- Elevon On // Right stick Left-Top (-,-): Aileron-Elevator | | // Right stick Right-Bottom (-,+): Expo | | // Right stick Left-Bottom (+,+): Linear Expo Off ------- Expo on // Throttle at max - Center Calibrate Aileron, Elevator and Rudder void StartupInit() { // Initialize Startup Configuration counters MotorEnable = CONFIG_COUNTER; ElevonCounter = CONFIG_COUNTER; ExpoCounter = CONFIG_COUNTER; #ifdef CENTER_CALIBRATE CenterCounter = CONFIG_COUNTER; Center_Aileron = CENTER_PERIOD; Center_Elevator = CENTER_PERIOD; Center_Rudder = CENTER_PERIOD; #endif } void StartupConfig() { if (MotorEnable.7 == 0) { OC(); // Don't start motor output until throttle stick has stayed at zero for 1/2 second if (count[2]) MotorEnable = CONFIG_COUNTER; // Didn't stay zero long enough, start again MotorEnable--; OC(); // decrement towards -1 if (MotorEnable.7) { // Here exactly once, when MotorEnable first goes negative // Record parameters in EEPROM // Should be easily done by the time we // get here since StartupConfig() called about every 18mS WriteEEPROM(1, Param); OC(); // Will get one glitch in output, but everything should be zero anyway count[0] = RESOLUTION/2; // Momentarily move surfaces as signal that mode is set count[1] = RESOLUTION/2; } // Check for Elevon mode - Full Down, Left-Right for Off-On if (ElevonCounter.7 == 0) { OC(); if (count[1] > -FULL_THROW) ElevonCounter = CONFIG_COUNTER; OC(); // Didn't stay at top long enough, start again if (count[0] < FULL_THROW) { OC(); // Check full left or full right if (count[0] > -FULL_THROW) ElevonCounter = CONFIG_COUNTER; // Didn't stay extreme, start again } OC(); ElevonCounter--; // decrement towards -1 } else { OC(); // Here whenever ElevonCounter makes it to -1 if (count[0] >= FULL_THROW) { Param |= ELEVON_BIT; OC(); // Set elevon mode on } else Param &= ~ELEVON_BIT; OC(); // Set elevon mode off ElevonCounter = CONFIG_COUNTER; // Reset counter count[0] = RESOLUTION/2; // Momentarily move surfaces as signal that mode is set count[1] = RESOLUTION/2; } OC(); // Check for Expo mode - Full Up, Left-Right for Off-On if (ExpoCounter.7 == 0) { OC(); if (count[1] < FULL_THROW-1) ExpoCounter = CONFIG_COUNTER; OC(); // Didn't stay at top long enough, start again if (count[0] < FULL_THROW-1) { OC(); // Check full left or full right if (count[0] > -FULL_THROW-1) ExpoCounter = CONFIG_COUNTER; // Didn't stay extreme, start again } OC(); ExpoCounter--; // decrement towards -1 } else { OC(); // Here whenever ExpoCounter makes it to -1 if (count[0] >= FULL_THROW-1) { Param |= EXPO_BIT; OC(); // Set Expo mode on } else Param &= ~EXPO_BIT; OC(); // Set Expo mode off ExpoCounter = CONFIG_COUNTER; // Reset counter count[0] = 0; // Momentarily move surfaces as signal that mode is set count[1] = 0; } OC(); #ifdef CENTER_CALIBRATE // Check for Center Calibrate - Throttle full On if (CenterCounter.7 == 0) { OC(); if (count[2] < FULL_THROW+FULL_THROW) CenterCounter = CONFIG_COUNTER; OC(); // Didn't stay at top long enough, start again CenterCounter--; // decrement towards -1 } else { OC(); // Here whenever CenterCounter makes it to -1 Center_Aileron += count[0]; Center_Elevator += count[1]; Center_Rudder += count[3]; CenterCounter = CONFIG_COUNTER; // Reset counter count[0] = RESOLUTION/2; // Momentarily move surfaces as signal that mode is set count[1] = RESOLUTION/2; } OC(); #endif // CENTER_CALIBRATE count[2] = 0; // Zero throttle anyway } OC(); } /********************************* SynchUp *******************************************/ // Seeks INPUT_ON // Returns 0 if OK, else 1 // Fail if wait more than 14.442mS // If successful, OCTimer will have been set to zero at the transition // Also OCHalfTimer will be 1 if transition found earlier during OutputCycle else 0 // Timing of synch transition is critical - need to examine assembler code // Important to have only 1 call to OC() for each test of GPIO.3 // Note - value of input signal corresponding to INPUT_ON and INPUT_OFF depends on value of pulse char OCHalfTimer; char SynchUp() { OC(); OCHalfTimer = 0; OCTimer = 0; do { OC(); if (INPUT_ON) goto found; } while (OCTimer.7 == 0); // here if GPIO still Off when OCTimer reaches 128, in 4.814mS (2.407 for 8MHz) OCTimer = 0; do { OC(); if (INPUT_ON) goto found; } while (OCTimer.7 == 0); // here if GPIO still Off when OCTimer reaches 128, in 4.814mS (2.407 for 8MHz) OCTimer = 0; do { OC(); if (INPUT_ON) goto found; } while (OCTimer.7 == 0); // here if GPIO still Off when OCTimer reaches 128, in 4.814mS (2.407 for 8MHz) #if CLOCK_FREQ == 8 OCTimer = 0; do { OC(); if (INPUT_ON) goto found; } while (OCTimer.7 == 0); // here if GPIO still Off when OCTimer reaches 128, in 4.814mS (2.407 for 8MHz) OCTimer = 0; do { OC(); if (INPUT_ON) goto found; } while (OCTimer.7 == 0); // here if GPIO still Off when OCTimer reaches 128, in 4.814mS (2.407 for 8MHz) OCTimer = 0; do { OC(); if (INPUT_ON) goto found; } while (OCTimer.7 == 0); // here if GPIO still Off when OCTimer reaches 128, in 4.814mS (2.407 for 8MHz) #endif // CLOCK_FREQ == 8 // Timed out OC(); return 1; found: OCTimer = 0; OC(); if ((GPIORead & INPUT_PIN) == pulse) OCHalfTimer++; OC(); return 0; } /********************************* DecodePulses *******************************************/ // Decode one set of input timing pulses // return 1 if fails, else 0 // Assumes OCTimer was re-started at beginning of first pulse // Note - value of input signal corresponding to INPUT_ON and INPUT_OFF depends on value of pulse char DecodePulses() { OC(); char i,j; for (i = 0; i < 4; i++) { OC(); // for each channel // Check pulse remains for at least MIN_PULSE_WIDTH OCTimer -= MAX_PULSE_WIDTH; // as if set to -MAX_PULSE_WIDTH at beginning of period do { OC(); if (INPUT_OFF) goto endPulseFound; } while (OCTimer.7); OC(); // Here if timed out return 1; endPulseFound: OC(); // End of pulse found, now wait for beginning of next pulse. Timing critical counti = OCTimer + MAX_PULSE_WIDTH; OC(); // Check if pulse was long enough if (counti < MIN_PULSE_WIDTH) return 1; // Can't be since so many calls to OC() before first test OCTimer += MAX_PULSE_WIDTH - MAX_PERIOD; // as if set to -MAX_PERIOD at beginning of period // Must have only one call to OC() in following loop do { OC(); if (INPUT_ON) goto endPeriodFound; } while (OCTimer.7); OC(); // Here if timed out return 1; endPeriodFound: OC(); //+1 // Capture timing first counti = OCTimer + MAX_PERIOD -1; OC(); // -1 because of OC() since transition count[i] = counti; OCTimer = 2; OC(); // now counting from this transition // 2 because of 2 OC()s since transition // Check if period was long enough if (counti < MIN_PERIOD) return 1; OC(); } // for (i = 0; i < 4; i++) { // for each channel OC(); // Succeeded // Shuffle to get motor channel in count[2] #define swap(a,b) counti=count[a];OC();count[a]=count[b];count[b]=counti if (MotorChannel == 3) { // swap with count[2] swap(2,3); OC(); return 0; } OC(); if (MotorChannel == 1) { // swap with count[2] swap(1,2); OC(); return 0; } OC(); if (MotorChannel == 0) { // cycle count[0..2] swap(0,1); swap(1,2); } OC(); return 0; } /********************************* ProcessCounts *******************************************/ // Process raw count[] array into output values void ProcessCounts() { OC(); char i; // Remove expected 1 LSB jitter due to asynchronous sampling by averaging with previous counts count[0] = countPrev[0] = (count[0] + countPrev[0]) >> 1; OC(); count[1] = countPrev[1] = (count[1] + countPrev[1]) >> 1; OC(); count[2] = countPrev[2] = (count[2] + countPrev[2]) >> 1; OC(); count[3] = countPrev[3] = (count[3] + countPrev[3]) >> 1; OC(); #ifdef MOTOR_PWM // Cause OC() to output a PWM motor pulse // Done here to get smoothed count // Note, spacing between pulses is timed by input frame rate motorESC = MOTOR; for (i = count[2]; i; i--) OC(); motorESC = 0; OC(); #endif // Normalize counts #ifdef CENTER_CALIBRATE count[0] -= Center_Aileron; count[1] -= Center_Elevator; count[2] -= MIN_MOTOR_PERIOD; count[3] -= Center_Rudder; OC(); #else count[0] -= CENTER_PERIOD; count[1] -= CENTER_PERIOD; count[2] -= MIN_MOTOR_PERIOD; count[3] -= CENTER_PERIOD; OC(); #endif // Motor if (count[2].7) count[2] = 0; // Clip to 0 before StartupConfig // Check Startup Configurations // Done here because we want actual stick positions StartupConfig(); OC(); // Elevon mixing - here because need signed, unscaled quantities if (Param & ELEVON_BIT) { counti = count[0]; count[0] -= count[1]; count[1] += counti; } OC(); // Map count[] values in place and set GPIO bits // Aileron if (count[0].7) { // i.e. if negative count[0] = -count[0]; GP[0] = AILERON_LEFT; } else { GP[0] = AILERON_RIGHT; } OC(); // Elevator if (count[1].7) { // i.e. if negative count[1] = -count[1]; GP[1] = ELEVATOR_DOWN; } else { GP[1] = ELEVATOR_UP; } OC(); // Channel 3 if (count[3].7) { // i.e. if negative count[3] = -count[3]; GP[3] = RUDDER_LEFT; } else { GP[3] = RUDDER_RIGHT; } OC(); #if CLOCK_FREQ == 4 count[0] += count[0]; // Double value to get range [0..20] count[1] += count[1]; //count[2] += count[2]; // Motor already full resolution count[3] += count[3]; OC(); #else // for CLOCK_FREQ == 8 count[2] = count[2] >> 1; OC(); // Motor has double needed resolution #endif // Expo must go here - count[]s are positive but not yet scaled if (count[0] > RESOLUTION) count[0] = RESOLUTION; OC(); if (count[1] > RESOLUTION) count[1] = RESOLUTION; OC(); if (count[2] > RESOLUTION) count[2] = RESOLUTION; OC(); if (count[3] > RESOLUTION) count[3] = RESOLUTION; OC(); if (Param & EXPO_BIT) { OC(); W = count[0]; count[0] = Expo(); OC(); W = count[1]; count[1] = Expo(); OC(); // count[2], motor, left linear W = count[3]; count[3] = Expo(); } OC(); // Scale - no scaling needed } /********************************* BuildGPIOArray *******************************************/ // Build the GPIO array // Use double buffering if appropriate and enough space // Expects count[] and GP[] to be set up // For inverted outputs enable the following statement //#define INVERT_OUTPUT // or rely on void BuildGPIOArray() { char GPVal; // Build GPIO value char i,j; // Loop indices signed char y[4]; // Accumulators in PFM //#define PWM #ifdef PWM // Pulse Width Modulation output - needs double buffering // No need to sort, just cycle through each entry setting bits for (i = 0; i < RESOLUTION; i++) { OC(); GPVal = 0; for (j = 0; j < 4; j++) { OC(); // For each channel if (count[j] > i) { OC(); GPVal += GP[j]; } OC(); } OC(); buf[i] = GPVal; OC(); } #else //PFM Pulse Frequency Modulation output - overwrite buffer in place y[0] = 0; y[1] = 0; y[2] = 0; y[3] = 0; OC(); for (i = 0; i < RESOLUTION; i++) { OC(); GPVal = 0; // loop unwound for speed (far fewer OC()) (it's also less code) y[0] -= count[0]; // Channel 0 if (y[0] < 0) { OC(); y[0] += RESOLUTION; GPVal += GP[0]; } OC(); y[1] -= count[1]; // Channel 1 if (y[1] < 0) { OC(); y[1] += RESOLUTION; GPVal += GP[1]; } OC(); #ifndef MOTOR_PWM // Include normal throttle pulsed output y[2] -= count[2]; // Channel 2 if (y[2] < 0) { OC(); y[2] += RESOLUTION; GPVal += GP[2]; } OC(); #endif y[3] -= count[3]; // Channel 3 if (y[3] < 0) { OC(); y[3] += RESOLUTION; GPVal += GP[3]; } OC(); OutputBuffer[i] = GPVal ^ INVERT_OUTPUT_MASK; OC(); // Invert required outputs } OC(); #endif //PWM } /********************************* StandBy *******************************************/ // Progressively shut down controls // After 1.5 secs: Aileron to 1/5 left, Elevator to neutral, Motor unchanged // After 3.0 secs: Aileron at 1/5 left, Elevator at neutral, Motor reduced by 1 unit or zero // Every 1.5 secs: ditto #define STANDBY_PERIOD (6*CLOCK_FREQ/4) // ~1.5 sec char StandByCount; #define STANDBY_CYCLES 4 // Number of complete correct consecutive frames needed to assume good signal char StandByTMR1H; #define TMR1_REACHED(T) (((TMR1H - T) & 0x80) == 0) #define TMR1_NOT_REACHED(T) (((TMR1H - T) & 0x80) != 0) void StandByInit() { // Initialize motor for StandBy() during initial synching count[2] = 0; count2 = 0; GP[2] = MOTOR; } void StandByTurnOn() { OC(); // Turn on StandBy StandByCount = STANDBY_PERIOD; StandByTMR1H = TMR1H + 128; } void StandBy() { OC(); if (TMR1_REACHED(StandByTMR1H)) { OC(); StandByTMR1H = StandByTMR1H + 128; // another half period of TMR1H (262 mS or 131 mS) if (StandByCount == 0) { OC(); StandByCount = STANDBY_PERIOD; count[0] = FULL_THROW/5; GP[0] = AILERON_LEFT; OC(); // Aileron 1/5 left, or Left Elevon if (Param & ELEVON_BIT) { count[1] = FULL_THROW/5; // Right Elevon 1/5 down } else { count[1] = 0; // Elevator neutral } OC(); GP[1] = ELEVATOR_DOWN; count[2] = count2; // motor count[3] = FULL_THROW/5; // Rudder 1/5 left GP[3] = RUDDER_LEFT; OC(); BuildGPIOArray(); OC(); counti = count[2] >> 1; // multiply motor speed by 3/4 counti = count[2] + counti; OC(); count[2] = counti >> 1; OC(); count2 = count[2]; // save motor count because count[2] may get random number in a corrupt frame } StandByCount--; } OC(); } /********************************* FindMinInterFrame ******************************/ // Seek period of no transitions for 4mS // Returns 0 if successful, 1 if not found in time // Sets 'pulse' depending on positive- or negative-going pulses // Calls Standby() // Note - value of input signal corresponding to INPUT_ON and INPUT_OFF depends on value of pulse #define MIN_INTERFRAME_TIME (2*CLOCK_FREQ/4) // ~4 mS char MinInterFrameTMR1H; // Times 4 mS minimum gap #define BAD_INTERFRAME_TIME (6*CLOCK_FREQ/4) // ~12mS Ticks of TMR1H after which declare bad interframe - 6mS char BadInterFrameTMR1H; // Times too long until start of interframe gap found char FindMinInterFrame() { OC(); BadInterFrameTMR1H = TMR1H + BAD_INTERFRAME_TIME; // Time when waited too long MinInterFrameTMR1H = TMR1H + MIN_INTERFRAME_TIME; OC(); while(TMR1_NOT_REACHED(MinInterFrameTMR1H)) { OC(); if (INPUT_ON) { OC(); // went On too soon MinInterFrameTMR1H = TMR1H + MIN_INTERFRAME_TIME; // Reset timer. pulse = INPUT_PIN - pulse; OC(); // toggle pulse and try again // with redefined pulse, input just went Off if (TMR1_REACHED(BadInterFrameTMR1H)) { OC(); // Failed - been too long to start again looking for gap return 1; } } OC(); } OC(); // Here if stayed off long enough - gap found return 0; } /********************************* SetMotorChannel ******************************/ // Set MotorChannel to be the channel furthest from CENTER_PERIOD // Other channels are assumed in the order Aileron, Elevator, Rudder #define max GP[0] // Borrow GP[0] void SetMotorChannel() { OC(); char i; MotorChannel = 2; // Loop until got a good frame while (1) { OC(); while (FindMinInterFrame()) OC(); // input stayed Off long enough - try to Synch if (SynchUp()) continue; OC(); // Here when found first pulse transition after inter-frame period if (DecodePulses() == 0) break; OC(); } // Find channel furthest from midpoint max = 0; OC(); for (i = 0; i < 4; i++) { OC(); counti = count[i]; OC(); counti -= CENTER_PERIOD; if (counti < 0) counti = -counti; OC(); if (counti >= max) { max = counti; MotorChannel = i; } OC(); } OC(); } /**********************************************************************************/ /********************************* MAIN *******************************************/ /**********************************************************************************/ void main() { char onStandBy; // non-zero if need to call StandBy char i; ChipInit(); #ifndef SIMULATOR Jingle(); #endif ParamInit(); StandByInit(); onStandBy = 0; StartupInit(); pulse = 0; // sense of input timing pulses - confirmed or changed during Startup // Takes on values [0,INPUT_PIN] // Initialize countPrev[] for (i = 0; i < 4; i++) countPrev[i] = CENTER_PERIOD; // Throttle in center for Startup Configuration motorESC = 0; OCInit(); SetMotorChannel(); // Loop for a restart while (1) { if (onStandBy == 0) StandByTurnOn(); OC(); onStandBy = STANDBY_CYCLES; OC(); // Loop while staying in synch while (1) { OC(); // motorESC = MOTOR; OC(); motorESC = 0; OC(); //Pulse on motor pin for oscilloscope if (onStandBy) StandBy(); OC(); if (FindMinInterFrame()) break; OC(); // input stayed Off long enough - try to Synch if (SynchUp()) break; OC(); // Here when found first pulse transition after inter-frame period if (DecodePulses()) break; OC(); // Raw periods are now in count[] ProcessCounts(); OC(); // Processed control data now in count[] if (onStandBy) { onStandBy--; OC(); } else { OC(); BuildGPIOArray(); OC(); count2 = count[2]; // save good motor count in case corrupted in DecodePulses() - used in Standby() } } OC(); } } // main #ifdef PIC12F683 #pragma origin 0x7FF #else #pragma origin 0x3FF #endif // This is to enable us to create a call to the oscillator calibration value in 12F629 // On 12F629 this is a dummy - actual procedure is preset in chip and will override // On 12F635 this procedure runs and returns value shown - need to calibrate manually // A change of 1 causes about 1% change in frequency // No need to recompile to change value - can set the value in WinPic's buffer and rewrite the PIC char GetOsccal() {return 0x0;} // Initialize EEPROM data #define EEPROM_START 0x2100 #pragma cdata[EEPROM_START] #pragma cdata[] = MAGIC, 0x00 // AILERON-ELEVATOR mode, EXPO off