/* * Authors (alphabetical order) * - Bertrand Songis * - Bryan J. Rentoul (Gruvin) * - Cameron Weeks * - Erez Raviv * - Jean-Pierre Parisy * - Karl Szmutny * - Michael Blandford * - Michal Hlavinka * - Pat Mackenzie * - Philip Moss * - Rob Thomson * - Romolo Manfredini * - Thomas Husterer * * open9x is based on code named * gruvin9x by Bryan J. Rentoul: http://code.google.com/p/gruvin9x/, * er9x by Erez Raviv: http://code.google.com/p/er9x/, * and the original (and ongoing) project by * Thomas Husterer, th9x: http://code.google.com/p/th9x/ * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License version 2 as * published by the Free Software Foundation. * * This program 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 General Public License for more details. * */ #include "open9x.h" #ifdef SPLASH const pm_uchar splashdata[] PROGMEM = { 'S','P','S',0, #include "s9xsplash.lbm" 'S','P','E',0}; const pm_uchar * s9xsplash = splashdata+4; #endif #include "menus.h" // MM/SD card Disk IO Support #if defined (PCBV4) gtime_t g_unixTime; // Global date/time register, incremented each second in per10ms() #endif EEGeneral g_eeGeneral; ModelData g_model; #if !defined(PCBARM) uint8_t g_tmr1Latency_max; uint8_t g_tmr1Latency_min; #endif uint16_t g_timeMain; #ifdef DEBUG uint16_t g_time_per10; #endif #ifdef AUDIO audioQueue audio; #endif uint8_t heartbeat; uint8_t stickMode; int8_t safetyCh[NUM_CHNOUT]; union ReusableBuffer reusableBuffer; const pm_char s_charTab[] PROGMEM = "_-.,"; const pm_uint8_t bchout_ar[] PROGMEM = { 0x1B, 0x1E, 0x27, 0x2D, 0x36, 0x39, 0x4B, 0x4E, 0x63, 0x6C, 0x72, 0x78, 0x87, 0x8D, 0x93, 0x9C, 0xB1, 0xB4, 0xC6, 0xC9, 0xD2, 0xD8, 0xE1, 0xE4 }; uint8_t channel_order(uint8_t x) { return ( ((pgm_read_byte(bchout_ar + g_eeGeneral.templateSetup) >> (6-(x-1) * 2)) & 3 ) + 1 ); } /* mode1 rud ele thr ail mode2 rud thr ele ail mode3 ail ele thr rud mode4 ail thr ele rud */ const pm_uint8_t modn12x3[] PROGMEM = { 1, 2, 3, 4, 1, 3, 2, 4, 4, 2, 3, 1, 4, 3, 2, 1 }; char idx2char(int8_t idx) { if (idx == 0) return ' '; if (idx < 0) { if (idx > -27) return 'a' - idx - 1; idx = -idx; } if (idx < 27) return 'A' + idx - 1; if (idx < 37) return '0' + idx - 27; if (idx <= 40) return pgm_read_byte(s_charTab+idx-37); if (idx <= ZCHAR_MAX) return 'z' + 5 + idx - 40; return ' '; } PhaseData *phaseaddress(uint8_t idx) { return &g_model.phaseData[idx]; } ExpoData *expoaddress(uint8_t idx ) { return &g_model.expoData[idx]; } MixData *mixaddress(uint8_t idx) { return &g_model.mixData[idx]; } LimitData *limitaddress(uint8_t idx) { return &g_model.limitData[idx]; } void generalDefault() { memset(&g_eeGeneral, 0, sizeof(g_eeGeneral)); g_eeGeneral.lightSw = SWITCH_ON; g_eeGeneral.myVers = EEPROM_VER; g_eeGeneral.contrast = 25; g_eeGeneral.vBatWarn = 90; for (int i = 0; i < 7; ++i) { g_eeGeneral.calibMid[i] = 0x200; g_eeGeneral.calibSpanNeg[i] = 0x180; g_eeGeneral.calibSpanPos[i] = 0x180; } g_eeGeneral.chkSum = (0x200 * 7) + (0x180 * 5); } uint16_t evalChkSum() { uint16_t sum=0; for (int i=0; i<12;i++) sum += g_eeGeneral.calibMid[i]; return sum; } #ifndef TEMPLATES inline void applyDefaultTemplate() { for (int i=0; idestCh = i; md->weight = 100; md->srcRaw = channel_order(i+1); } STORE_MODELVARS; } #endif void modelDefault(uint8_t id) { memset(&g_model, 0, sizeof(g_model)); applyDefaultTemplate(); } void resetProto() { #if defined(DSM2_SERIAL) if (g_model.protocol == PROTO_DSM2) { cli(); #if defined(FRSKY) DSM2_Init(); #endif sei(); } else { cli(); #if defined(FRSKY) FRSKY_Init(); #else DSM2_Done(); #endif sei(); #if defined(FRSKY) FRSKY_setModelAlarms(); #endif } #elif defined(FRSKY) FRSKY_setModelAlarms(); #endif } int16_t intpol(int16_t x, uint8_t idx) // -100, -75, -50, -25, 0 ,25 ,50, 75, 100 { #define D9 (RESX * 2 / 8) #define D5 (RESX * 2 / 4) bool cv9 = idx >= MAX_CURVE5; int8_t *crv = cv9 ? g_model.curves9[idx-MAX_CURVE5] : g_model.curves5[idx]; int16_t erg; x+=RESXu; if(x < 0) { erg = (int16_t)crv[0] * (RESX/4); } else if(x >= (RESX*2)) { erg = (int16_t)crv[(cv9 ? 8 : 4)] * (RESX/4); } else { int16_t a,dx; if(cv9){ a = (uint16_t)x / D9; dx =((uint16_t)x % D9) * 2; } else { a = (uint16_t)x / D5; dx = (uint16_t)x % D5; } erg = (int16_t)crv[a]*((D5-dx)/2) + (int16_t)crv[a+1]*(dx/2); } return erg / 25; // 100*D5/RESX; } int16_t applyCurve(int16_t x, int8_t idx) { /* already tried to have only one return at the end */ switch(idx) { case 0: return x; case 1: if (x < 0) x = 0; //x|x>0 return x; case 2: if (x > 0) x = 0; //x|x<0 return x; case 3: // x|abs(x) return abs(x); case 4: //f|f>0 return x > 0 ? RESX : 0; case 5: //f|f<0 return x < 0 ? -RESX : 0; case 6: //f|abs(f) return x > 0 ? RESX : -RESX; } if (idx < 0) { x = -x; idx = -idx + 6; } return intpol(x, idx - 7); } // expo-funktion: // --------------- // kmplot // f(x,k)=exp(ln(x)*k/10) ;P[0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20] // f(x,k)=x*x*x*k/10 + x*(1-k/10) ;P[0,1,2,3,4,5,6,7,8,9,10] // f(x,k)=x*x*k/10 + x*(1-k/10) ;P[0,1,2,3,4,5,6,7,8,9,10] // f(x,k)=1+(x-1)*(x-1)*(x-1)*k/10 + (x-1)*(1-k/10) ;P[0,1,2,3,4,5,6,7,8,9,10] uint16_t expou(uint16_t x, uint16_t k) { // k*x*x*x + (1-k)*x return ((unsigned long)x*x*x/0x10000*k/(RESXul*RESXul/0x10000) + (RESKul-k)*x+RESKul/2)/RESKul; } int16_t expo(int16_t x, int16_t k) { if(k == 0) return x; int16_t y; bool neg = x < 0; if(neg) x = -x; if(k<0){ y = RESXu-expou(RESXu-x,-k); }else{ y = expou(x,k); } return neg? -y:y; } #ifdef EXTENDED_EXPO /// expo with y-offset class Expo { uint16_t c; int16_t d,drx; public: void init(uint8_t k, int8_t yo); static int16_t expou(uint16_t x,uint16_t c, int16_t d); int16_t expo(int16_t x); }; void Expo::init(uint8_t k, int8_t yo) { c = (uint16_t) k * 256 / 100; d = (int16_t) yo * 256 / 100; drx = d * ((uint16_t)RESXu/256); } int16_t Expo::expou(uint16_t x,uint16_t c, int16_t d) { uint16_t a = 256 - c - d; if( (int16_t)a < 0 ) a = 0; // a x^3 + c x + d // 9 18 27 11 20 18 uint32_t res = ((uint32_t)x * x * x / 0x10000 * a / (RESXul*RESXul/0x10000) + (uint32_t)x * c ) / 256; return (int16_t)res; } int16_t Expo::expo(int16_t x) { if(c==256 && d==0) return x; if(x>=0) return expou(x,c,d) + drx; return -expou(-x,c,-d) + drx; } #endif void applyExpos(int16_t *anas, uint8_t phase) { static int16_t anas2[NUM_STICKS]; // values before expo, to ensure same expo base when multiple expo lines are used memcpy(anas2, anas, sizeof(anas2)); if (phase == 255) phase = getFlightPhase(); phase++; int8_t cur_chn = -1; for (uint8_t i=0; i=0 && ed.mode&2)) { cur_chn = ed.chn; int16_t k = ed.expo; v = expo(v, k); uint8_t ed_curve = ed.curve; if (ed_curve) v = applyCurve(v, ed_curve > 10 ? ed_curve + 4 : ed_curve); v = ((int32_t)v * ed.weight) / 100; anas[cur_chn] = v; } } } } int16_t applyLimits(uint8_t channel, int32_t value) { int16_t ofs = g_model.limitData[channel].offset; int16_t lim_p = 10 * (g_model.limitData[channel].max + 100); int16_t lim_n = 10 * (g_model.limitData[channel].min - 100); //multiply by 10 to get same range as ofs (-1000..1000) if (ofs > lim_p) ofs = lim_p; if (ofs < lim_n) ofs = lim_n; if (value) value = (value > 0) ? value * ((int32_t) lim_p - ofs) / 100000 : -value * ((int32_t) lim_n - ofs) / 100000; //div by 100000 -> output = -1024..1024 value += calc1000toRESX(ofs); lim_p = calc1000toRESX(lim_p); lim_n = calc1000toRESX(lim_n); if (value > lim_p) value = lim_p; if (value < lim_n) value = lim_n; ofs = value; // we convert value to a 16bit value and reuse ofs if (g_model.limitData[channel].revert) ofs = -ofs; // finally do the reverse. if (safetyCh[channel] != -128) // if safety channel available for channel check ofs = calc100toRESX(safetyCh[channel]); return ofs; } int16_t ex_chans[NUM_CHNOUT] = {0}; // Outputs (before LIMITS) of the last perMain #ifdef HELI int16_t cyc_anas[3] = {0}; #endif int16_t getValue(uint8_t i) { /*srcRaw is shifted +1!*/ if(i CSW_CHOUT_BASE+NUM_CHNOUT) { y = convertTelemValue(cs.v1-(CSW_CHOUT_BASE+NUM_CHNOUT), 128+cs.v2); if (cs.v1 >= CSW_CHOUT_BASE+NUM_CHNOUT+TELEM_ALT) { // Fill the threshold array barsThresholds[cs.v1-CSW_CHOUT_BASE-NUM_CHNOUT-TELEM_ALT] = 128 + cs.v2; } } else #endif { y = calc100toRESX(cs.v2); } switch (cs.func) { case CS_VPOS: result = (x>y); break; case CS_VNEG: result = (xy); break; // case CS_ANEG: default: result = (abs(x)y); break; case CS_LESS: result = (x=y); break; // case CS_ELESS: default: result = (x<=y); break; } } } } return swtch > 0 ? result : !result; } bool getSwitch(int8_t swtch, bool nc) { s_last_switch_used = ((uint16_t)nc<<15); return __getSwitch(swtch); } #ifdef FLIGHT_PHASES uint8_t getFlightPhase() { for (uint8_t i=1; iswtch && getSwitch(phase->swtch, 0)) { return i; } } return 0; } #endif int16_t getRawTrimValue(uint8_t phase, uint8_t idx) { int16_t result; if (s_trimPtr[idx]) { result = *s_trimPtr[idx]; } else { PhaseData *p = phaseaddress(phase); #if defined(PCBSTD) result = (((int16_t)p->trim[idx]) << 2) + ((p->trim_ext >> (2*idx)) & 0x03); #else result = p->trim[idx]; #endif } return result; } int16_t getTrimValue(uint8_t phase, uint8_t idx) { return getRawTrimValue(getTrimFlightPhase(phase, idx), idx); } void setTrimValue(uint8_t phase, uint8_t idx, int16_t trim) { if (s_trimPtr[idx]) { *s_trimPtr[idx] = limit((int16_t)-125, trim, (int16_t)+125); } else { PhaseData *p = phaseaddress(phase); #if defined(PCBSTD) p->trim[idx] = (int8_t)(trim >> 2); p->trim_ext = (p->trim_ext & ~(0x03 << (2*idx))) + (((trim & 0x03) << (2*idx))); #else p->trim[idx] = trim; #endif } STORE_MODELVARS; } uint8_t getTrimFlightPhase(uint8_t phase, uint8_t idx) { for (uint8_t i=0; i= phase) result++; phase = result; } return 0; } #if defined(PCBV4) uint8_t s_perOut_flight_phase; uint8_t getRotaryEncoderFlightPhase(uint8_t idx) { uint8_t phase = s_perOut_flight_phase; for (uint8_t i=0; irotaryEncoders[idx]; if (value <= ROTARY_ENCODER_MAX) return phase; uint8_t result = value-ROTARY_ENCODER_MAX-1; if (result >= phase) result++; phase = result; } return 0; } int16_t getRotaryEncoder(uint8_t idx) { return phaseaddress(getRotaryEncoderFlightPhase(idx))->rotaryEncoders[idx]; } void incRotaryEncoder(uint8_t idx, int8_t inc) { g_rotenc[idx] += inc; int16_t *value = &(phaseaddress(getRotaryEncoderFlightPhase(idx))->rotaryEncoders[idx]); *value = limit((int16_t)-1024, (int16_t)(*value + (inc * 8)), (int16_t)+1024); eeDirty(EE_MODEL); } #endif #if defined(FRSKY) || defined(PCBARM) void putsTelemetryValue(uint8_t x, uint8_t y, int16_t val, uint8_t unit, uint8_t att) { #ifdef IMPERIAL_UNITS if (unit == UNIT_DEGREES) { val += 18 ; val *= 115 ; val >>= 6 ; } if (unit == UNIT_METERS) { // m to ft *105/32 val = val * 3 + ( val >> 2 ) + (val >> 5) ; } if (unit == UNIT_FEET) { unit = UNIT_METERS; } #else if (unit == UNIT_KTS) { // kts to km/h val = (val * 463) / 250; } #endif lcd_outdezAtt(x, (att & DBLSIZE ? y - FH : y), val, att & (~NO_UNIT)); // TODO we could add this test inside lcd_outdezAtt! if (~att & NO_UNIT && unit != UNIT_RAW) lcd_putsiAtt(lcd_lastPos/*+1*/, y, STR_VTELEMUNIT, unit, 0); } #endif void clearKeyEvents() { #ifdef SIMU while (keyDown() && main_thread_running) sleep(1/*ms*/); #else while (keyDown()); // loop until all keys are up #endif putEvent(0); } #define INAC_DEVISOR 256 // Bypass splash screen with stick movement uint16_t stickMoveValue() { uint16_t sum = 0; for (uint8_t i=0; i<4; i++) sum += anaIn(i)/INAC_DEVISOR; return sum; } void checkBacklight() { if (getSwitch(g_eeGeneral.lightSw, 0) || g_eeGeneral.lightAutoOff) BACKLIGHT_ON; else BACKLIGHT_OFF; } #ifdef SPLASH void doSplash() { if(!g_eeGeneral.disableSplashScreen) { checkBacklight() ; lcd_clear(); lcd_img(0, 0, s9xsplash,0,0); refreshDisplay(); lcdSetRefVolt(g_eeGeneral.contrast); clearKeyEvents(); #ifndef SIMU for(uint8_t i=0; i<32; i++) getADC_filt(); // init ADC array #endif uint16_t inacSum = stickMoveValue(); uint16_t tgtime = get_tmr10ms() + SPLASH_TIMEOUT; //2sec splash screen while (tgtime != get_tmr10ms()) { #ifdef SIMU if (!main_thread_running) return; sleep(1/*ms*/); #else getADC_filt(); #endif uint16_t tsum = stickMoveValue(); if(keyDown() || (tsum!=inacSum)) return; //wait for key release if (check_soft_power() > e_power_trainer) return; // Usb on or power off checkBacklight(); } } } #endif #if !defined(PCBARM) void checkLowEEPROM() { if(g_eeGeneral.disableMemoryWarning) return; if(EeFsGetFree() < 200) { alert(STR_EEPROMLOWMEM); } } #endif void checkTHR() { if(g_eeGeneral.disableThrottleWarning) return; int thrchn=(2-(stickMode&1));//stickMode=0123 -> thr=2121 int16_t lowLim = THRCHK_DEADBAND + g_eeGeneral.calibMid[thrchn] - g_eeGeneral.calibSpanNeg[thrchn]; #ifndef SIMU getADC_single(); // if thr is down - do not display warning at all #endif int16_t v = anaIn(thrchn); if (g_eeGeneral.throttleReversed) v = - v; if(v<=lowLim) return; // first - display warning message(STR_ALERT, STR_THROTTLENOTIDLE, STR_RESETTHROTTLE, STR_PRESSANYKEYTOSKIP); //loop until all switches are reset while (1) { #ifdef SIMU if (!main_thread_running) return; sleep(1/*ms*/); #else getADC_single(); #endif int16_t v = anaIn(thrchn); if (g_eeGeneral.throttleReversed) v = - v; if (check_soft_power() > e_power_trainer) return; // Usb on or power off if(v<=lowLim || keyDown()) { clearKeyEvents(); return; } checkBacklight(); } } void checkAlarm() // added by Gohst { if (g_eeGeneral.disableAlarmWarning) return; if (g_eeGeneral.beeperMode == e_mode_quiet) alert(STR_ALARMSDISABLED); } void checkSwitches() { if(!g_eeGeneral.switchWarning) return; // if warning is on bool state = (g_eeGeneral.switchWarning > 0); bool first = true; //loop until all switches are reset while (1) { uint8_t i; for(i=SW_BASE; i e_power_trainer) return; // Usb on or power off checkBacklight(); #ifdef SIMU if (!main_thread_running) return; sleep(1/*ms*/); #endif } } void alert(const pm_char * s) { message(STR_ALERT, s, 0, STR_PRESSANYKEY); while(1) { #ifdef SIMU if (!main_thread_running) return; sleep(1/*ms*/); #endif if (check_soft_power() > e_power_trainer) return; // Usb on or power off if (keyDown()) return; // wait for key release checkBacklight(); wdt_reset(); } } void message(const pm_char *title, const pm_char *s, const pm_char *t, const char *last) { lcd_clear(); lcd_putsAtt(0, 0, title, DBLSIZE); lcd_putsLeft(4*FH, s); if (t) lcd_putsLeft(5*FH, t); if (last) { lcd_putsLeft(7*FH, last); AUDIO_ERROR(); } refreshDisplay(); lcdSetRefVolt(g_eeGeneral.contrast); clearKeyEvents(); } int8_t *s_trimPtr[NUM_STICKS] = { NULL, NULL, NULL, NULL }; uint8_t checkTrim(uint8_t event) { int8_t k = (event & EVT_KEY_MASK) - TRM_BASE; int8_t s = g_model.trimInc; if (k>=0 && k<8) { // && (event & _MSK_KEY_REPT)) //LH_DWN LH_UP LV_DWN LV_UP RV_DWN RV_UP RH_DWN RH_UP uint8_t idx = CONVERT_MODE(1+k/2) - 1; uint8_t phase = getTrimFlightPhase(getFlightPhase(), idx); int16_t before = getRawTrimValue(phase, idx); int8_t v = (s==0) ? min(32, abs(before)/4+1) : 1 << (s-1); // 1=>1 2=>2 3=>4 4=>8 bool thro = (idx==THR_STICK && g_model.thrTrim); if (thro) v = 4; // if throttle trim and trim trottle then step=4 int16_t after = (k&1) ? before + v : before - v; // positive = k&1 bool beepTrim = false; for (int16_t mark=TRIM_MIN; mark<=TRIM_MAX; mark+=TRIM_MAX) { if ((mark!=0 || !thro) && ((mark!=TRIM_MIN && after>=mark && beforemark))) { after = mark; beepTrim = true; } } if ((beforeTRIM_MAX) || (before>after && after TRIM_EXTENDED_MAX) { after = TRIM_EXTENDED_MAX; } setTrimValue(phase, idx, after); #if defined (AUDIO) // toneFreq higher/lower according to trim position // limit the frequency, range -125 to 125 = toneFreq: 19 to 101 if (after > TRIM_MAX) after = TRIM_MAX; if (after < TRIM_MIN) after = TRIM_MIN; after /= 4; after += 60; #endif if (beepTrim) { killEvents(event); AUDIO_TRIM_MIDDLE(); } else { #if defined (AUDIO) audio.event(AU_TRIM_MOVE, after); #else if (event & _MSK_KEY_REPT) warble = true; AUDIO_TRIM(); #endif } return 0; } return event; } #ifdef SIMU uint16_t BandGap = 225; #else // #define STARTADCONV (ADCSRA = (1<> 2 ) ; t_ana[1][x] = ( t_ana[1][x] + t_ana[0][x] ) >> 1 ; t_ana[0][x] = ( t_ana[0][x] + Analog_values[x] ) >> 1 ; } } #else void getADC_filt() { static uint16_t t_ana[2][8]; for (uint8_t adc_input=0; adc_input<8; adc_input++) { ADMUX=adc_input|ADC_VREF_TYPE; // Start the AD conversion ADCSRA|=0x40; // Do this while waiting s_anaFilt[adc_input] = (s_anaFilt[adc_input]/2 + t_ana[1][adc_input]) & 0xFFFE; //gain of 2 on last conversion - clear last bit t_ana[1][adc_input] = (t_ana[1][adc_input] + t_ana[0][adc_input]) >> 1; // Wait for the AD conversion to complete while ((ADCSRA & 0x10)==0); ADCSRA|=0x10; t_ana[0][adc_input] = (t_ana[0][adc_input] + ADCW) >> 1; } } #endif #if defined(PCBARM) void getADC_osmp() { register uint32_t x; register uint32_t y; uint16_t temp[NUMBER_ANALOG]; for( x = 0; x < NUMBER_ANALOG; x += 1 ) { temp[x] = 0; } for( y = 0; y < 4; y += 1 ) { read_9_adc(); for( x = 0; x < NUMBER_ANALOG; x += 1 ) { temp[x] += Analog_values[x]; } } for( x = 0; x < NUMBER_ANALOG; x += 1 ) { s_anaFilt[x] = temp[x] >> 3; } } #else void getADC_osmp() { uint16_t temp_ana; for (uint8_t adc_input=0; adc_input<8; adc_input++) { temp_ana = 0; ADMUX = adc_input|ADC_VREF_TYPE; for (uint8_t i=0; i<4;i++) { // Going from 10bits to 11 bits. Addition = n. Loop 4^n times // Start the AD conversion ADCSRA|=0x40; // Wait for the AD conversion to complete while ((ADCSRA & 0x10)==0); ADCSRA|=0x10; temp_ana += ADCW; } s_anaFilt[adc_input] = temp_ana / 2; // divide by 2^n to normalize result. } } #endif #if defined(PCBARM) void getADC_single() { register uint32_t x ; read_9_adc() ; for( x = 0 ; x < NUMBER_ANALOG ; x += 1 ) { s_anaFilt[x] = Analog_values[x] >> 1 ; } } #else void getADC_single() { for (uint8_t adc_input=0; adc_input<8; adc_input++) { ADMUX=adc_input|ADC_VREF_TYPE; // Start the AD conversion ADCSRA|=0x40; // Wait for the AD conversion to complete while ((ADCSRA & 0x10)==0); ADCSRA|=0x10; s_anaFilt[adc_input]= ADCW * 2; // use 11 bit numbers } } #endif #if not defined(PCBARM) void getADC_bandgap() { #if defined (PCBV4) // For times over-sample with no divide, x2 to end at a half averaged, x8. DON'T ASK mmmkay? :P This is how I want it. ADCSRA|=0x40; while ((ADCSRA & 0x10)==0); ADCSRA|=0x10; BandGap=ADCW; ADCSRA|=0x40; while ((ADCSRA & 0x10)==0); ADCSRA|=0x10; BandGap+=ADCW; ADCSRA|=0x40; while ((ADCSRA & 0x10)==0); ADCSRA|=0x10; BandGap+=ADCW; ADCSRA|=0x40; while ((ADCSRA & 0x10)==0); ADCSRA|=0x10; BandGap+=ADCW; BandGap *= 2; ADCSRB |= (1< only phone jack = slave mode #if defined(PCBV4) return SLAVE_MODE; #else static bool lastSlaveMode = false; static uint8_t checkDelay = 0; if (IS_AUDIO_BUSY()) { checkDelay = 20; } else if (checkDelay) { --checkDelay; } else { lastSlaveMode = SLAVE_MODE; } return lastSlaveMode; #endif } #endif uint16_t s_timeCumTot; uint16_t s_timeCumThr; // THR in 1/16 sec uint16_t s_timeCum16ThrP; // THR% in 1/16 sec uint8_t s_timerState[2]; int16_t s_timerVal[2]; uint8_t s_timerVal_10ms[2] = {0, 0}; uint8_t trimsCheckTimer = 0; void resetTimer(uint8_t idx) { s_timerState[idx] = TMR_OFF; // is changed to RUNNING dep from mode s_timerVal[idx] = g_model.timers[idx].val; s_timerVal_10ms[idx] = 0 ; } void resetAll() { // TODO s_traceCnt to be reset? resetTimer(0); resetTimer(1); #ifdef FRSKY resetTelemetry(); #endif } static uint8_t lastSwPos[2] = {0, 0}; static uint16_t s_cnt[2] = {0, 0}; static uint16_t s_sum[2] = {0, 0}; static uint8_t sw_toggled[2] = {false, false}; static uint16_t s_time_cum_16[2] = {0, 0}; uint8_t s_traceBuf[MAXTRACE]; uint8_t s_traceWr; int8_t s_traceCnt; #if defined(HELI) || defined(FRSKY_HUB) uint16_t isqrt32(uint32_t n) { uint16_t c = 0x8000; uint16_t g = 0x8000; for(;;) { if((uint32_t)g*g > n) g ^= c; c >>= 1; if(c == 0) return g; g |= c; } } #endif // static variables used in perOut - moved here so they don't interfere with the stack // It's also easier to initialize them here. int16_t anas [NUM_STICKS] = {0}; int16_t trims[NUM_STICKS] = {0}; int32_t chans[NUM_CHNOUT] = {0}; uint32_t inacCounter = 0; uint16_t inacSum = 0; BeepANACenter bpanaCenter = 0; int16_t sDelay[MAX_MIXERS] = {0}; int32_t act [MAX_MIXERS] = {0}; uint8_t swOn [MAX_MIXERS] = {0}; uint8_t mixWarning; FORCEINLINE void evalTrims(uint8_t phase) { for (uint8_t i=0; i throttle trim if applicable // TODO avoid int32_t vv int32_t vv = 2*RESX; int16_t trim = getTrimValue(phase, i); if (i==THR_STICK && g_model.thrTrim) { if (g_eeGeneral.throttleReversed) trim = -trim; int16_t v = anas[i]; vv = ((int32_t)trim-TRIM_MIN)*(RESX-v)/(2*RESX); } else if (trimsCheckTimer > 0) { trim = 0; } trims[i] = (vv==2*RESX) ? trim*2 : (int16_t)vv*2; // if throttle trim -> trim low end } } enum PerOutMode { e_perout_mode_normal = 0, e_perout_mode_trims, e_perout_mode_zeros, e_instant_trim }; uint8_t s_perout_mode = e_perout_mode_normal; BeepANACenter evalSticks(uint8_t phase) { BeepANACenter anaCenter = 0; #ifdef HELI uint16_t d = 0; if (g_model.swashR.value) { uint32_t v = (int32_t(calibratedStick[ELE_STICK])*calibratedStick[ELE_STICK] + int32_t(calibratedStick[AIL_STICK])*calibratedStick[AIL_STICK]); uint32_t q = int32_t(RESX)*g_model.swashR.value/100; q *= q; if (v>q) d = isqrt32(v); } #endif for (uint8_t i=0; i [-1024..1024] uint8_t ch = (i < NUM_STICKS ? CONVERT_MODE(i+1) - 1 : i); #if defined(PCBV4) int16_t v = ((i < NUM_STICKS+NUM_POTS) ? anaIn(i) : getRotaryEncoder(i-(NUM_STICKS+NUM_POTS))); #else int16_t v = anaIn(i); #endif #ifndef SIMU if(i < NUM_STICKS+NUM_POTS){ v -= g_eeGeneral.calibMid[i]; v = v * (int32_t)RESX / (max((int16_t)100,(v>0 ? g_eeGeneral.calibSpanPos[i] : g_eeGeneral.calibSpanNeg[i]))); } #endif if(v < -RESX) v = -RESX; if(v > RESX) v = RESX; if (g_eeGeneral.throttleReversed && ch==THR_STICK) v = -v; if (i < NUM_STICKS+NUM_POTS) calibratedStick[ch] = v; //for show in expo uint8_t tmp = (uint16_t)abs(v) / 16; if (tmp <= 1) anaCenter |= (tmp==0 ? (BeepANACenter)1<mode) { uint8_t chStud = td->srcChn; int32_t vStud = (g_ppmIns[chStud]- g_eeGeneral.trainer.calib[chStud]); vStud *= td->studWeight; vStud /= 50; switch (td->mode) { case 1: v += vStud; break; // add-mode case 2: v = vStud; break; // subst-mode } } } #ifdef HELI if(d && (ch==ELE_STICK || ch==AIL_STICK)) v = int32_t(v)*g_model.swashR.value*RESX/(int32_t(d)*100); #endif anas[ch] = v; //set values for mixer } } /* EXPOs */ applyExpos(anas, phase); /* TRIMs */ evalTrims(phase); return anaCenter; } #ifdef DEBUG /* * This is a test function for debugging purpose, you may insert there your code and compile with the option DEBUG=YES */ void testFunc() { // printf("testFunc\n"); fflush(stdout); } #endif uint16_t active_functions = 0; // current max = 16 functions void evalFunctions() { assert((int)(sizeof(active_functions)*8) > (int)(FUNC_MAX-NUM_CHNOUT)); for (uint8_t i=0; iswtch) { uint16_t mask = (sd->func >= FUNC_TRAINER ? (1 << (sd->func-FUNC_TRAINER)) : 0); if (getSwitch(sd->swtch, 0)) { if (sd->func < FUNC_TRAINER && (g_menuStack[g_menuStackPtr] != menuProcFunctionSwitches || m_posVert != (i+1) || m_posHorz > 1)) { safetyCh[sd->func] = (int8_t)sd->param; } if (~active_functions & mask) { if (sd->func == FUNC_INSTANT_TRIM) { if (g_menuStack[0] == menuMainView #if defined(FRSKY) || g_menuStack[0] == menuProcFrsky #endif ) instantTrim(); } #if defined(SOMO) if (sd->func == FUNC_PLAY_SOMO) { somoPushPrompt(sd->param); } #endif #if defined(DEBUG) if (sd->func == FUNC_TEST) { testFunc(); } #endif if (sd->func == FUNC_RESET) { switch (sd->param) { case 0: case 1: resetTimer(sd->param); break; case 2: resetAll(); break; #ifdef FRSKY case 3: resetTelemetry(); break; #endif } } } if (sd->func == FUNC_PLAY_SOUND) { #if defined(AUDIO) audioDefevent(AU_FRSKY_FIRST+sd->param); #else beep(3); #endif } #if defined(HAPTIC) if (sd->func == FUNC_HAPTIC) { haptic.event(AU_FRSKY_LAST+sd->param); } #endif active_functions |= mask; } else { active_functions &= (~mask); } } } } void perOut(uint8_t phase) { #if defined(PCBV4) s_perOut_flight_phase = phase; #endif BeepANACenter anaCenter = evalSticks(phase); if (s_perout_mode == e_perout_mode_normal) { //===========BEEP CENTER================ anaCenter &= g_model.beepANACenter; if(((bpanaCenter ^ anaCenter) & anaCenter)) AUDIO_POT_STICK_MIDDLE(); bpanaCenter = anaCenter; #ifdef HELI if(g_model.swashR.value) { uint32_t v = ((int32_t)anas[ELE_STICK]*anas[ELE_STICK] + (int32_t)anas[AIL_STICK]*anas[AIL_STICK]); uint32_t q = (int32_t)RESX*g_model.swashR.value/100; q *= q; if(v>q) { uint16_t d = isqrt32(v); anas[ELE_STICK] = (int32_t)anas[ELE_STICK]*g_model.swashR.value*RESX/((int32_t)d*100); anas[AIL_STICK] = (int32_t)anas[AIL_STICK]*g_model.swashR.value*RESX/((int32_t)d*100); } } #define REZ_SWASH_X(x) ((x) - (x)/8 - (x)/128 - (x)/512) // 1024*sin(60) ~= 886 #define REZ_SWASH_Y(x) ((x)) // 1024 => 1024 if(g_model.swashR.type) { int16_t vp = anas[ELE_STICK]+trims[ELE_STICK]; int16_t vr = anas[AIL_STICK]+trims[AIL_STICK]; int16_t vc = 0; if (g_model.swashR.collectiveSource) vc = getValue(g_model.swashR.collectiveSource-1); if(g_model.swashR.invertELE) vp = -vp; if(g_model.swashR.invertAIL) vr = -vr; if(g_model.swashR.invertCOL) vc = -vc; switch (g_model.swashR.type) { case (SWASH_TYPE_120): vp = REZ_SWASH_Y(vp); vr = REZ_SWASH_X(vr); cyc_anas[0] = vc - vp; cyc_anas[1] = vc + vp/2 + vr; cyc_anas[2] = vc + vp/2 - vr; break; case (SWASH_TYPE_120X): vp = REZ_SWASH_X(vp); vr = REZ_SWASH_Y(vr); cyc_anas[0] = vc - vr; cyc_anas[1] = vc + vr/2 + vp; cyc_anas[2] = vc + vr/2 - vp; break; case (SWASH_TYPE_140): vp = REZ_SWASH_Y(vp); vr = REZ_SWASH_Y(vr); cyc_anas[0] = vc - vp; cyc_anas[1] = vc + vp + vr; cyc_anas[2] = vc + vp - vr; break; case (SWASH_TYPE_90): vp = REZ_SWASH_Y(vp); vr = REZ_SWASH_Y(vr); cyc_anas[0] = vc - vp; cyc_anas[1] = vc + vr; cyc_anas[2] = vc - vr; break; default: break; } } #endif s_trimPtr[0] = NULL; s_trimPtr[1] = NULL; s_trimPtr[2] = NULL; s_trimPtr[3] = NULL; } memset(chans, 0, sizeof(chans)); // All outputs to 0 //========== MIXER LOOP =============== mixWarning = 0; // TODO should be in a local variable on stack for (uint8_t i=0; isrcRaw==0) break; if (md->phase != 0) { if (md->phase > 0) { if (phase+1 != md->phase) continue; } else { if (phase+1 == -md->phase) continue; } } //========== SWITCH =============== bool sw = getSwitch(md->swtch, 1); //========== VALUE =============== //Notice 0 = NC switch means not used -> always on line uint8_t k = md->srcRaw-1; int16_t v = 0; if (s_perout_mode != e_perout_mode_normal) { if (!sw || k >= NUM_STICKS || (k == THR_STICK && g_model.thrTrim)) continue; } else { if (k < NUM_STICKS) v = anas[k]; //Switch is on. MAX=FULL=512 or value. else if (k>=MIXSRC_CH1-1 && k<=MIXSRC_CH16-1 && k-MIXSRC_CH1+1destCh) // if we've already calculated the value - take it instead v = chans[k-MIXSRC_CH1+1] / 100; else if (k>=MIXSRC_THR-1 && k<=MIXSRC_SWC-1) { v = getSwitch(k-MIXSRC_THR+1+1, 0) ? +1024 : -1024; if (v<0 && !md->swtch) sw = false; } else { v = getValue(k <= MIXSRC_3POS ? k : k-MAX_SWITCH); } } //========== DELAYS =============== uint8_t swTog; bool apply_offset = true; if (sw) { // switch on? (if no switch selected => on) swTog = !swOn[i]; if (s_perout_mode == e_perout_mode_normal) { swOn[i] = true; if (md->delayUp) { if (swTog) { sDelay[i] = md->delayUp * 100; } if (sDelay[i]) { // perform delay if(tick10ms) sDelay[i]--; if (!md->swtch) { v = -1024; } else { continue; } } } if (md->mixWarn) mixWarning |= 1<<(md->mixWarn-1); // Mix warning } } else { bool has_delay = false; swTog = swOn[i]; swOn[i] = false; if (md->delayDown) { if (swTog) { sDelay[i] = md->delayDown * 100; } if (sDelay[i]) { // perform delay if(tick10ms) sDelay[i]--; if (!md->swtch) v = +1024; has_delay = true; } else if (!md->swtch) { v = -1024; } } if (!has_delay) { if (md->speedDown) { if (md->mltpx==MLTPX_REP) continue; if (md->swtch) { v = 0; apply_offset = false; } } else if (md->swtch) { continue; } } } //========== OFFSET =============== if (apply_offset && md->sOffset) v += calc100toRESX(md->sOffset); //========== SPEED =============== if (s_perout_mode==e_perout_mode_normal && (md->speedUp || md->speedDown)) // there are delay values { #define DEL_MULT 256 int16_t diff = v-act[i]/DEL_MULT; if (diff) { //rate = steps/sec => 32*1024/100*md->speedUp/Down //act[i] += diff>0 ? (32768)/((int16_t)100*md->speedUp) : -(32768)/((int16_t)100*md->speedDown); //-100..100 => 32768 -> 100*83886/256 = 32768, For MAX we divide by 2 since it's asymmetrical if (tick10ms) { int32_t rate = (int32_t)DEL_MULT*2048*100; if(md->weight) rate /= abs(md->weight); // TODO port optim er9x by Mike act[i] = (diff>0) ? ((md->speedUp>0) ? act[i]+(rate)/((int16_t)100*md->speedUp) : (int32_t)v*DEL_MULT) : ((md->speedDown>0) ? act[i]-(rate)/((int16_t)100*md->speedDown) : (int32_t)v*DEL_MULT) ; } if(((diff>0) && (v<(act[i]/DEL_MULT))) || ((diff<0) && (v>(act[i]/DEL_MULT)))) act[i]=(int32_t)v*DEL_MULT; //deal with overflow v = act[i]/DEL_MULT; } } //========== CURVES =============== if (md->curve) v = applyCurve(v, md->curve); //========== TRIMS =============== if (k < NUM_STICKS) { if (s_perout_mode < e_perout_mode_zeros && md->carryTrim == TRIM_ON) { v += trims[k]; } if (s_perout_mode == e_perout_mode_normal && md->carryTrim == TRIM_OFFSET) { v = md->sOffset; v = calc1000toRESX(v << 3); s_trimPtr[k] = &md->sOffset; // use the value stored here for the trim } } //========== MULTIPLEX =============== int32_t dv = (int32_t)v*md->weight; int8_t differential = md->differential; if (differential>0 && dv<0) dv = (dv * (50-differential)) / 50; else if (differential<0 && dv>0) dv = (dv * (50+differential)) / 50; int32_t *ptr = &chans[md->destCh]; // Save calculating address several times switch(md->mltpx){ case MLTPX_REP: *ptr = dv; break; case MLTPX_MUL: dv /= 100; dv *= *ptr; dv /= RESXl; *ptr = dv; break; default: // MLTPX_ADD *ptr += dv; //Mixer output add up to the line (dv + (dv>0 ? 100/2 : -100/2))/(100); break; } } } #ifdef DISPLAY_USER_DATA char userDataDisplayBuf[TELEM_SCREEN_BUFFER_SIZE]; #endif #if (defined(PCBARM) && !defined(REVA) && !defined(SIMU)) || (defined(PCBV4) && !defined(REV0) && !defined(SIMU)) #define TIME_TO_WRITE (s_eeDirtyMsk && (get_tmr10ms() - s_eeDirtyTime10ms) >= WRITE_DELAY_10MS) #else #define TIME_TO_WRITE s_eeDirtyMsk #endif int32_t sum_chans512[NUM_CHNOUT] = {0}; void perMain() { static uint16_t lastTMR; uint16_t tmr10ms = get_tmr10ms(); tick10ms = (tmr10ms != lastTMR); lastTMR = tmr10ms; #define MAX_ACT 0xffff static uint16_t fp_act[MAX_PHASES] = {0}; static uint16_t delta = 0; static uint8_t s_fade_flight_phases = 0; static uint8_t s_last_phase = 255; uint8_t phase = getFlightPhase(); int32_t weight = 0; if (s_last_phase != phase) { if (s_last_phase == 255) { fp_act[phase] = MAX_ACT; } else { uint8_t fadeTime = max(g_model.phaseData[s_last_phase].fadeOut, g_model.phaseData[phase].fadeIn); if (fadeTime) { s_fade_flight_phases |= (1< 1024*100 // later we multiply by the limit (up to 100) and then we need to normalize // at the end chans[i] = chans[i]/100 => -1024..1024 // interpolate value with min/max so we get smooth motion from center to stop // this limits based on v original values and min=-1024, max=1024 RESX=1024 //printf("chans%d=%d\n", i, chans[i]);fflush(stdout); int32_t q = (s_fade_flight_phases ? (sum_chans512[i] / weight) * 16 : chans[i]); ex_chans[i] = q / 100; // for the next perMain int16_t value = applyLimits(i, q); cli(); g_chans512[i] = value; // copy consistent word to int-level sei(); } // TODO same code here + integrate the timer which could be common #if defined(PCBARM) if (Tenms) { Tenms = 0 ; if (Eeprom32_process_state != E32_IDLE) ee32_process(); else if (TIME_TO_WRITE) eeCheck(); } #else if (!eeprom_buffer_size) { if (theFile.isWriting()) theFile.nextWriteStep(); else if (TIME_TO_WRITE) eeCheck(); } #endif if (!tick10ms) return; //make sure the rest happen only every 10ms. int16_t val; if (g_model.thrTraceSrc == 0) { val = calibratedStick[THR_STICK]; // get throttle channel value } else if (g_model.thrTraceSrc > NUM_POTS) { val = g_chans512[g_model.thrTraceSrc-NUM_POTS-1]; } else { val = calibratedStick[g_model.thrTraceSrc+NUM_STICKS-1]; } val += RESX; val /= (RESX/16); // calibrate it // Throttle trace start static uint16_t s_time_tot; static uint16_t s_time_trace; static uint8_t s_cnt_1s; static uint16_t s_sum_1s; static uint16_t s_cnt_10s; static uint16_t s_sum_10s; s_cnt_1s++; s_sum_1s += val; if ((uint16_t)(tmr10ms - s_time_tot) >= 100) { // 1sec s_time_tot += 100; s_timeCumTot += 1; val = s_sum_1s / s_cnt_1s; s_timeCum16ThrP += val / 2; if (val) s_timeCumThr += 1; s_cnt_10s += s_cnt_1s; s_sum_10s += s_sum_1s; s_cnt_1s = 0; s_sum_1s = 0; if ((uint16_t)(tmr10ms - s_time_trace) >= 1000) {// 10sec s_time_trace += 1000; val = s_sum_10s / s_cnt_10s; s_sum_10s = 0; s_cnt_10s = 0; s_traceBuf[s_traceWr++] = val; if (s_traceWr >= MAXTRACE) s_traceWr = 0; if (s_traceCnt >= 0) s_traceCnt++; // TODO to be checked } } // Throttle trace end // Timers start for (uint8_t i=0; i<2; i++) { int8_t tm = g_model.timers[i].mode; uint16_t tv = g_model.timers[i].val; if (tm) { if (s_timerState[i] == TMR_OFF) { s_timerState[i] = TMR_RUNNING; s_cnt[i] = 0; s_sum[i] = 0; s_time_cum_16[i] = 0; } uint8_t atm = (tm >= 0 ? tm : TMR_VAROFS-tm-1); // value for time described in timer->mode // OFFABSTHsTH%THt if (atm == TMRMODE_THR_REL) { s_cnt[i]++; s_sum[i]+=val; } if (atm>=(TMR_VAROFS+MAX_SWITCH)){ // toggeled switch if(!(sw_toggled[i] | s_sum[i] | s_cnt[i] | lastSwPos[i])) lastSwPos[i] = tm < 0; // if initializing then init the lastSwPos uint8_t swPos = getSwitch(tm>0 ? tm-(TMR_VAROFS+MAX_SWITCH-1) : tm+(TMR_VAROFS+MAX_SWITCH-1), 0); if (swPos && !lastSwPos[i]) sw_toggled[i] = !sw_toggled[i]; // if switch is flipped first time -> change counter state lastSwPos[i] = swPos; } if ( (s_timerVal_10ms[i] += 1 ) >= 100 ) { s_timerVal_10ms[i] -= 100 ; if (tv) s_timerVal[i] = tv - s_timerVal[i]; if (atm==TMRMODE_ABS) { s_timerVal[i]++; } else if (atm==TMRMODE_THR) { if (val) s_timerVal[i]++; } else if (atm==TMRMODE_THR_REL) { if (s_cnt[i]) { val = s_sum[i]/s_cnt[i]; s_sum[i] -= val*s_cnt[i]; //rest s_cnt[i] = 0; s_time_cum_16[i] += val/2; if (s_time_cum_16[i] >= 16) { s_timerVal[i] ++; s_time_cum_16[i] -= 16; } } } else if (atm==TMRMODE_THR_TRG) { if (val || s_timerVal[i] > 0) s_timerVal[i]++; } else { if (atm<(TMR_VAROFS+MAX_SWITCH)) sw_toggled[i] = tm>0 ? getSwitch(tm-(TMR_VAROFS-1), 0) : !getSwitch(-tm, 0); // normal switch if (sw_toggled[i]) s_timerVal[i]++; } switch(s_timerState[i]) { case TMR_RUNNING: if (tv && s_timerVal[i]>=(int16_t)tv) s_timerState[i]=TMR_BEEPING; break; case TMR_BEEPING: if (s_timerVal[i] >= (int16_t)tv + MAX_ALERT_TIME) s_timerState[i]=TMR_STOPPED; break; } if (tv) s_timerVal[i] = tv - s_timerVal[i]; //if counting backwards - display backwards } } }; static int16_t last_tmr; if (last_tmr != s_timerVal[0]) { // beep only if seconds advance if (s_timerState[0] == TMR_RUNNING) { if (g_eeGeneral.preBeep && g_model.timers[0].val) { // beep when 30, 15, 10, 5,4,3,2,1 seconds remaining if(s_timerVal[0]==30) AUDIO_TIMER_30(); //beep three times if(s_timerVal[0]==20) AUDIO_TIMER_20(); //beep two times if(s_timerVal[0]==10) AUDIO_TIMER_10(); if(s_timerVal[0]<= 3) AUDIO_TIMER_LT3(); } if (g_eeGeneral.minuteBeep && (((g_model.timers[0].val ? g_model.timers[0].val-s_timerVal[0] : s_timerVal[0])%60)==0)) { // short beep every minute AUDIO_MINUTE_BEEP(); } } else if(s_timerState[0] == TMR_BEEPING) { AUDIO_WARNING1(); } last_tmr = s_timerVal[0]; } // Timers end if (s_fade_flight_phases) { for (uint8_t p=0; p delta) fp_act[p] += delta; else { fp_act[p] = MAX_ACT; s_fade_flight_phases -= (1< delta) fp_act[p] -= delta; else { fp_act[p] = 0; s_fade_flight_phases -= (1<50) { inacCounter++; uint16_t tsum = 0; for(uint8_t i=0;i<4;i++) tsum += anaIn(i)/64; // reduce sensitivity if(tsum!=inacSum){ inacSum = tsum; inacCounter=0; } if(inacCounter>((uint32_t)g_eeGeneral.inactivityTimer*100*60)) if((inacCounter&0x3F)==10) AUDIO_INACTIVITY(); } #if defined(SDCARD) writeLogs(); #endif #if defined(FRSKY) && defined(DISPLAY_USER_DATA) char userDataRxBuffer[21]; // Temp buffer used to collect fr-sky user data // retrieve bytes from user data buffer and insert into display string, // scrolling at the 21 character mark (edge of screen) uint8_t numbytes = frskyGetUserData(userDataRxBuffer, 21); // Get as many bytes as we can static uint8_t displayBufferIndex; for (uint8_t byt=0; byt < numbytes; byt++) { displayBufferIndex++; if (displayBufferIndex > 20) { for (int xx=0; xx<20; xx++) // scroll one char left userDataDisplayBuf[xx] = userDataDisplayBuf[xx+1]; displayBufferIndex = 20; } userDataDisplayBuf[displayBufferIndex] = userDataRxBuffer[byt]; // Write the raw byte out to log file, if open if (testLogOpen && (g_oLogFile.fs != 0)) f_putc(userDataRxBuffer[byt], &g_oLogFile); } #endif lcd_clear(); uint8_t evt = getEvent(); evt = checkTrim(evt); // TODO port lightOnStickMove from er9x + flash saving, call checkBacklight() if(g_LightOffCounter) g_LightOffCounter--; if(evt) g_LightOffCounter = g_eeGeneral.lightAutoOff*500; // on keypress turn the light on 5*100 if (getSwitch(g_eeGeneral.lightSw,0) || g_LightOffCounter) BACKLIGHT_ON; else BACKLIGHT_OFF; #if defined(PCBARM) && defined(FRSKY) check_frsky(); #endif g_menuStack[g_menuStackPtr](evt); refreshDisplay(); #if defined(PCBARM) if ( check_soft_power() == e_power_trainer ) { // On trainer power PIOC->PIO_PDR = PIO_PC22 ; // Disable bit C22 Assign to peripheral } else { PIOC->PIO_PER = PIO_PC22 ; // Enable bit C22 as input } #elif defined(PCBV4) // PPM signal on phono-jack. In or out? ... if(checkSlaveMode()) { PORTG |= (1<50) { AUDIO_ERROR(); // TODO AUDIO_TX_BATTERY_LOW() } } break; } #if defined(PCBARM) HAPTIC_HEARTBEAT(); // AUDIO_HEARTBEAT(); // the queue processing #endif } int16_t g_ppmIns[8]; uint8_t ppmInState = 0; //0=unsync 1..8= wait for value i-1 #if !defined(SIMU) && !defined(PCBARM) volatile uint8_t g_tmr16KHz; //continuous timer 16ms (16MHz/1024/256) -- 8-bit counter overflow #if defined (PCBV4) ISR(TIMER2_OVF_vect) #else ISR(TIMER0_OVF_vect) // TODO now NOBLOCK in er9x #endif { g_tmr16KHz++; // gruvin: Not 16KHz. Overflows occur at 61.035Hz (1/256th of 15.625KHz) // to give *16.384ms* intervals. Kind of matters for accuracy elsewhere. ;) // g_tmr16KHz is used to software-construct a 16-bit timer // from TIMER-0 (8-bit). See getTmr16KHz, below. } uint16_t getTmr16KHz() { while(1){ uint8_t hb = g_tmr16KHz; #if defined (PCBV4) uint8_t lb = TCNT2; #else uint8_t lb = TCNT0; #endif if(hb-g_tmr16KHz==0) return (hb<<8)|lb; } } #if defined (PCBV4) ISR(TIMER2_COMPA_vect, ISR_NOBLOCK) //10ms timer #else ISR(TIMER0_COMP_vect, ISR_NOBLOCK) //10ms timer #endif { cli(); #if defined (PCBV4) static uint8_t accuracyWarble = 4; // becasue 16M / 1024 / 100 = 156.25. So bump every 4. uint8_t bump = (!(accuracyWarble++ & 0x03)) ? 157 : 156; TIMSK2 &= ~(1<4000 && val < 16000) // G: Prioritize reset pulse. (Needed when less than 8 incoming pulses) ppmInState = 1; // triggered else { if (ppmInState && ppmInState<=8) { if (val>800 && val<2200) // if valid pulse-width range { g_ppmIns[ppmInState++ - 1] = (int16_t)(val - 1500)*(g_eeGeneral.PPM_Multiplier+10)/10; //+-500 != 512, but close enough. } else ppmInState = 0; // not triggered } } lastCapt = capture; cli(); // disable other interrupts for stack pops before this function's RETI #if defined (PCBV4) TIMSK3 |= (1< 0) { UDR0 = frskyTxBuffer[--frskyTxBufferCount]; } else { UCSR0B &= ~(1 << UDRIE0); // disable UDRE0 interrupt } } #endif #if defined (DSM2_SERIAL) && !defined(PCBARM) FORCEINLINE void DSM2_USART0_vect() { UDR0 = *((uint16_t*)pulses2MHzRPtr); pulses2MHzRPtr += sizeof(uint16_t); if (pulses2MHzRPtr == pulses2MHzWPtr) { UCSR0B &= ~(1 << UDRIE0); // disable UDRE0 interrupt } } #endif #if !defined(SIMU) && !defined(PCBARM) #if defined (FRSKY) or defined(DSM2_SERIAL) ISR(USART0_UDRE_vect) { #if defined (FRSKY) and defined (DSM2_SERIAL) if (g_model.protocol == PROTO_DSM2) { DSM2_USART0_vect(); } else { FRSKY_USART0_vect(); } #elif defined (FRSKY) FRSKY_USART0_vect(); #else DSM2_USART0_vect(); #endif } #endif #endif #if defined (PCBV4) /*---------------------------------------------------------*/ /* User Provided Date/Time Function for FatFs module */ /*---------------------------------------------------------*/ /* This is a real time clock service to be called from */ /* FatFs module. Any valid time must be returned even if */ /* the system does not support a real time clock. */ /* This is not required in read-only configuration. */ uint32_t o9x_get_fattime(void) // TODO why not in ff.cpp? { struct gtm t; filltm(&g_unixTime, &t); // create a struct tm date/time structure from global unix time stamp /* Pack date and time into a DWORD variable */ return ((DWORD)(t.tm_year - 80) << 25) | ((uint32_t)(t.tm_mon+1) << 21) | ((uint32_t)t.tm_mday << 16) | ((uint32_t)t.tm_hour << 11) | ((uint32_t)t.tm_min << 5) | ((uint32_t)t.tm_sec >> 1); } #endif void instantTrim() { uint8_t phase = getFlightPhase(); for (uint8_t i=0; i TRIM_EXTENDED_MAX) { trim = TRIM_EXTENDED_MAX; } setTrimValue(trim_phase, i, trim); } } STORE_MODELVARS; AUDIO_WARNING2(); } void moveTrimsToOffsets() // copy state of 3 primary to subtrim { int16_t zeros[NUM_CHNOUT]; uint8_t phase = getFlightPhase(); s_perout_mode = e_perout_mode_zeros; perOut(phase); // do output loop - zero input sticks and trims for (uint8_t i=0; i q ) { *p-- = 0x55 ; } } #endif g_menuStack[0] = menuMainView; g_menuStack[1] = menuProcModelSelect; lcdSetRefVolt(25); sei(); // interrupts needed for FRSKY_Init and eeReadAll. #if defined (FRSKY) and !defined (DSM2_SERIAL) FRSKY_Init(); #endif #if defined (DSM2_SERIAL) and !defined (FRSKY) DSM2_Init(); #endif #ifdef JETI JETI_Init(); #endif #ifdef ARDUPILOT ARDUPILOT_Init(); #endif #ifdef NMEA NMEA_Init(); #endif #ifdef MAVLINK MAVLINK_Init(); #endif #ifdef MENU_ROTARY_SW init_rotary_sw(); #endif eeReadAll(); uint8_t cModel = g_eeGeneral.currModel; #if defined(PCBARM) set_volume(g_eeGeneral.speakerVolume); PWM->PWM_CH_NUM[0].PWM_CDTYUPD = g_eeGeneral.backlightBright; #endif #if defined(PCBV4) if (MCUSR != (1 << PORF) && !g_eeGeneral.unexpectedShutdown) #elif defined(PCBSTD) if (MCUCSR != (1 << PORF)) #else if (!g_eeGeneral.unexpectedShutdown) #endif { #ifdef SPLASH // TODO rather use another Model Parameter if (g_model.protocol != PROTO_DSM2) doSplash(); #endif #if !defined(PCBARM) checkLowEEPROM(); #endif getADC_single(); checkTHR(); checkSwitches(); checkAlarm(); } #if defined(PCBARM) || defined(PCBV4) if (!g_eeGeneral.unexpectedShutdown) { g_eeGeneral.unexpectedShutdown = 1; eeDirty(EE_GENERAL); } #endif clearKeyEvents(); //make sure no keys are down before proceeding lcdSetRefVolt(g_eeGeneral.contrast); g_LightOffCounter = g_eeGeneral.lightAutoOff*500; //turn on light for x seconds - no need to press key Issue 152 if(cModel!=g_eeGeneral.currModel) eeDirty(EE_GENERAL); // if model was quick-selected, make sure it sticks #if defined(PCBARM) start_ppm_capture(); // TODO inside startPulses? #endif startPulses(); if (check_soft_power() <= e_power_trainer) { wdt_enable(WDTO_500MS); } #if defined(PCBARM) register uint32_t shutdown_state = 0; #elif defined(PCBV4) uint8_t shutdown_state = 0; #endif while(1) { #if defined(PCBARM) || defined(PCBV4) if ((shutdown_state=check_soft_power()) > e_power_trainer) break; #endif #if defined(PCBARM) uint16_t t0 = getTmr2MHz(); #else uint16_t t0 = getTmr16KHz(); #endif if (g_eeGeneral.filterInput == 1) { getADC_filt() ; } else if ( g_eeGeneral.filterInput == 2) { getADC_osmp() ; } else { getADC_single() ; } #if defined(PCBARM) && defined(REVB) Current_analogue = ( Current_analogue * 31 + s_anaFilt[8] ) >> 5 ; #elif defined(PCBV4) // For PCB V4, use our own 1.2V, external reference (connected to ADC3) ADCSRB &= ~(1< g_timeMain) g_timeMain = t0 ; } #if defined(PCBARM) || defined(PCBV4) // Time to switch off lcd_clear() ; displayPopup(STR_SHUTDOWN); g_eeGeneral.unexpectedShutdown=0; eeDirty(EE_GENERAL); eeCheck(true); lcd_clear() ; refreshDisplay() ; soft_power_off(); // Only turn power off if necessary #endif #if defined(PCBARM) if (shutdown_state == e_power_usb) { lcd_putcAtt( 48, 24, 'U', DBLSIZE ) ; lcd_putcAtt( 60, 24, 'S', DBLSIZE ) ; lcd_putcAtt( 72, 24, 'B', DBLSIZE ) ; refreshDisplay() ; usb_mode(); } #endif #if defined(PCBARM) || defined(PCBV4) lcdSetRefVolt(0); // TODO before soft_power_off? #endif #if defined(PCBV4) //never return from main() - there is no code to return back, if any daelays occurs in physical power it does dead loop. wdt_disable(); for(;;){} #endif } #endif