From 0a8322d22e4d3de214975580d43e05dc434c9e3f Mon Sep 17 00:00:00 2001 From: philipp schoenberger Date: Thu, 4 Oct 2018 16:44:12 +0200 Subject: [PATCH] go for esp32: - remove freq offset overwrite - remove delayMilliseconds - print if bind phase is over - add prescale factor (try with leonardo) - substract execution time from wait -> works only for esp32 as it seems - add analog input pins for joysticks + arm - add analog reads within input update + scale it to channels --- Multiprotocol/CC2500_SPI.ino | 2 +- Multiprotocol/Common.ino | 2 +- Multiprotocol/FrSkyD_cc2500.ino | 5 + Multiprotocol/Multiprotocol.h | 5 +- Multiprotocol/Multiprotocol.ino | 222 ++++++++++++++------------------ Multiprotocol/Pins.h | 8 +- Multiprotocol/inputs.ino | 54 ++++++-- Multiprotocol/mi.ino | 41 +----- 8 files changed, 159 insertions(+), 180 deletions(-) diff --git a/Multiprotocol/CC2500_SPI.ino b/Multiprotocol/CC2500_SPI.ino index f703c5a..ad37973 100644 --- a/Multiprotocol/CC2500_SPI.ino +++ b/Multiprotocol/CC2500_SPI.ino @@ -119,7 +119,7 @@ void CC2500_SetTxRxMode(uint8_t mode) uint8_t CC2500_Reset() { CC2500_Strobe(CC2500_SRES); - delayMilliseconds(1); + delay(1); CC2500_SetTxRxMode(TXRX_OFF); return CC2500_ReadReg(CC2500_0E_FREQ1) == 0xC4;//check if reset } diff --git a/Multiprotocol/Common.ino b/Multiprotocol/Common.ino index d2329b0..ccb94db 100644 --- a/Multiprotocol/Common.ino +++ b/Multiprotocol/Common.ino @@ -135,7 +135,7 @@ uint16_t convert_channel_frsky(uint8_t num) #if defined(FRSKYD_CC2500_INO) || defined(FRSKYX_CC2500_INO) enum { FRSKY_BIND = 0, - FRSKY_BIND_DONE = 300, + FRSKY_BIND_DONE = 1000, FRSKY_DATA1, FRSKY_DATA2, FRSKY_DATA3, diff --git a/Multiprotocol/FrSkyD_cc2500.ino b/Multiprotocol/FrSkyD_cc2500.ino index bf15dbc..1127ec7 100644 --- a/Multiprotocol/FrSkyD_cc2500.ino +++ b/Multiprotocol/FrSkyD_cc2500.ino @@ -116,6 +116,7 @@ uint16_t initFrSky_2way() uint16_t ReadFrSky_2way() { + //debugln("%s",__func__); if (state < FRSKY_BIND_DONE) { @@ -130,6 +131,10 @@ uint16_t ReadFrSky_2way() debugln("%s bind done",__func__); } else state++; + + if(state == FRSKY_BIND_DONE) { + debugln("%s bind done fr",__func__); + } return 9000; } if (state == FRSKY_BIND_DONE) diff --git a/Multiprotocol/Multiprotocol.h b/Multiprotocol/Multiprotocol.h index dfe7f8f..e43326a 100644 --- a/Multiprotocol/Multiprotocol.h +++ b/Multiprotocol/Multiprotocol.h @@ -358,8 +358,9 @@ enum MultiPacketTypes //** Debug messages ** //******************** uint16_t debug_time=0; - #define debug(msg, ...) {char buf[64]; sprintf(buf, msg, ##__VA_ARGS__); Serial.println(buf);} - #define debugln(msg, ...) {char buf[64]; sprintf(buf, msg "\r\n", ##__VA_ARGS__); Serial.println(buf);} + char buf[128]; + #define debug(msg, ...) { sprintf(buf, msg, ##__VA_ARGS__); Serial.println(buf);} + #define debugln(msg, ...) { sprintf(buf, msg "\r\n", ##__VA_ARGS__); Serial.println(buf);} #define debug_time(msg) { uint16_t debug_time_TCNT1=TCNT1; debug_time=debug_time_TCNT1-debug_time; debugln(msg "%u", debug_time); debug_time=debug_time_TCNT1; } //******************** //*** Blink timing *** diff --git a/Multiprotocol/Multiprotocol.ino b/Multiprotocol/Multiprotocol.ino index 5c2d225..5986c03 100644 --- a/Multiprotocol/Multiprotocol.ino +++ b/Multiprotocol/Multiprotocol.ino @@ -127,7 +127,7 @@ volatile uint8_t discard_frame = 0; // Telemetry #define MAX_PKT 29 uint8_t pkt[MAX_PKT];//telemetry receiving packets - +float TIMER_PRESCALE = 5.82; // Callback typedef uint16_t (*void_function_t) (void);//pointer to a function with no parameters which return an uint16_t integer void_function_t remote_callback = 0; @@ -164,8 +164,10 @@ void setup() #endif // Timer1 config + #ifdef TCCR1A TCCR1A = 0; TCCR1B = (1 << CS11); //prescaler8, set timer1 to increment every 0.5us(16Mhz) and start timer + #endif // Random //random_init(); @@ -191,7 +193,7 @@ void setup() #endif //Wait for every component to start - delayMilliseconds(100); + delay(100); // Read status of bind button if( /*IS_BIND_BUTTON_on */ true) @@ -284,42 +286,10 @@ void setup() break; } debugln("sub_protocol: %d", sub_protocol); - - //Forced frequency tuning values for CC2500 protocols - #if defined(FORCE_FRSKYD_TUNING) && defined(FRSKYD_CC2500_INO) - if(protocol==PROTO_FRSKYD) - option = FORCE_FRSKYD_TUNING; // Use config-defined tuning value for FrSkyD - else - #endif - #if defined(FORCE_FRSKYV_TUNING) && defined(FRSKYV_CC2500_INO) - if(protocol==PROTO_FRSKYV) - option = FORCE_FRSKYV_TUNING; // Use config-defined tuning value for FrSkyV - else - #endif - #if defined(FORCE_FRSKYX_TUNING) && defined(FRSKYX_CC2500_INO) - if(protocol==PROTO_FRSKYX) - option = FORCE_FRSKYX_TUNING; // Use config-defined tuning value for FrSkyX - else - #endif - #if defined(FORCE_SFHSS_TUNING) && defined(SFHSS_CC2500_INO) - if (protocol==PROTO_SFHSS) - option = FORCE_SFHSS_TUNING; // Use config-defined tuning value for SFHSS - else - #endif - #if defined(FORCE_CORONA_TUNING) && defined(CORONA_CC2500_INO) - if (protocol==PROTO_CORONA) - option = FORCE_CORONA_TUNING; // Use config-defined tuning value for CORONA - else - #endif - #if defined(FORCE_HITEC_TUNING) && defined(HITEC_CC2500_INO) - if (protocol==PROTO_HITEC) - option = FORCE_HITEC_TUNING; // Use config-defined tuning value for HITEC - else - #endif - option = PPM_prot[line].option; // Use radio-defined option value - - debugln("freq offset: %d", option); - if(PPM_prot[line].power) POWER_FLAG_on; + option = PPM_prot[line].option; // Use radio-defined option value + debugln("freq offset: %d", option); + if(PPM_prot[line].power) + POWER_FLAG_on; if(PPM_prot[line].autobind) { AUTOBIND_FLAG_on; @@ -329,125 +299,128 @@ void setup() protocol_init(); - #ifndef STM32_BOARD - //Configure PPM interrupt - #else - attachInterrupt(PPM_pin,PPM_decode,FALLING); - #endif - #if defined(TELEMETRY) PPM_Telemetry_serial_init();// Configure serial for telemetry #endif } else #endif //ENABLE_PPM - { // Serial - #ifdef ENABLE_SERIAL - for(uint8_t i=0;i<3;i++) - cur_protocol[i]=0; - protocol=0; - #ifdef CHECK_FOR_BOOTLOADER - Mprotocol_serial_init(1); // Configure serial and enable RX interrupt - #else - Mprotocol_serial_init(); // Configure serial and enable RX interrupt - #endif - #endif //ENABLE_SERIAL - } debugln("Init complete"); + update_inputs(); + + debugln("time now %lu ", micros()); + delay(1000); + debugln("time after 1 sec %lu", micros()); + + + #ifdef TCCR1A + while((TIFR1 & OCF1A_bm) == 0); // wait + unsigned long s = micros(); + cli(); // Disable global int due to RW of 16 bits registers + OCR1A += 10000 * TIMER_PRESCALE; // set compare A for callback + TIFR1=OCF1A_bm; // clear compare A=callback flag + sei(); // enable global int + while((TIFR1 & OCF1A_bm) == 0); // wait 2ms... + unsigned long e = micros(); + double a = e; + a -= s; + a = 10000/a; + debugln("time now %lu", s); + debugln("time after 1 sec %lu", e); + debugln("scalor %d", a); + TIMER_PRESCALE *= a; + #endif } // Main // Protocol scheduler void loop() { - uint16_t next_callback,diff=0xFFFF; - + uint32_t next_callback; + uint16_t diff=0xFFFF; + + uint32_t end__ = micros(); + uint32_t start = micros(); + if(remote_callback==0 || IS_WAIT_BIND_on || diff>2*200) + { + do + { + Update_All(); + } + while(remote_callback==0 || IS_WAIT_BIND_on); + } while(1) { - if(remote_callback==0 || IS_WAIT_BIND_on || diff>2*200) - { - do - { - Update_All(); - } - while(remote_callback==0 || IS_WAIT_BIND_on); - } - #ifndef STM32_BOARD - if( (TIFR1 & OCF1A_bm) != 0) - { - cli(); // Disable global int due to RW of 16 bits registers - OCR1A=TCNT1; // Callback should already have been called... Use "now" as new sync point. - sei(); // Enable global int - } - else - while((TIFR1 & OCF1A_bm) == 0); // Wait before callback - #else - if((TIMER2_BASE->SR & TIMER_SR_CC1IF)!=0) - { - debugln("Callback miss"); - cli(); - OCR1A = TCNT1; - sei(); - } - else - while((TIMER2_BASE->SR & TIMER_SR_CC1IF )==0); // Wait before callback + #if 0 + if( (TIFR1 & OCF1A_bm) != 0) + { + cli(); // Disable global int due to RW of 16 bits registers + OCR1A=TCNT1; // Callback should already have been called... Use "now" as new sync point. + sei(); // Enable global int + } + else + while((TIFR1 & OCF1A_bm) == 0); // Wait before callback + #endif + + + end__ = micros(); do { - TX_MAIN_PAUSE_on; - tx_pause(); - if(IS_INPUT_SIGNAL_on && remote_callback!=0) - next_callback=remote_callback(); - else - next_callback=2000; // No PPM/serial signal check again in 2ms... + //TX_MAIN_PAUSE_on; + //tx_pause(); + start = end__; + next_callback=remote_callback(); #if 0 - // 15 < 250 - // 20 250 250 - if (next_callback > 4000) { - next_callback-=2000; - Update_All(); - delayMicroseconds(2000); - } - delayMicroseconds(next_callback*2); -#else TX_MAIN_PAUSE_off; tx_resume(); while(next_callback>4000) { // start to wait here as much as we can... next_callback-=2000; // We will wait below for 2ms cli(); // Disable global int due to RW of 16 bits registers - OCR1A += 2000 *2; // set compare A for callback - #ifndef STM32_BOARD - TIFR1=OCF1A_bm; // clear compare A=callback flag - #else - TIMER2_BASE->SR = 0x1E5F & ~TIMER_SR_CC1IF; // Clear Timer2/Comp1 interrupt flag - #endif + OCR1A += 2000 * TIMER_PRESCALE; // set compare A for callback + TIFR1=OCF1A_bm; // clear compare A=callback flag sei(); // enable global int if(Update_All()) // Protocol changed? { next_callback=0; // Launch new protocol ASAP break; } - #ifndef STM32_BOARD - while((TIFR1 & OCF1A_bm) == 0); // wait 2ms... - #else - while((TIMER2_BASE->SR & TIMER_SR_CC1IF)==0);//2ms wait - #endif + while((TIFR1 & OCF1A_bm) == 0); // wait 2ms... } // at this point we have a maximum of 4ms in next_callback - next_callback *= 2 ; cli(); // Disable global int due to RW of 16 bits registers - OCR1A+= next_callback ; // set compare A for callback - #ifndef STM32_BOARD - TIFR1=OCF1A_bm; // clear compare A=callback flag - #else - TIMER2_BASE->SR = 0x1E5F & ~TIMER_SR_CC1IF; // Clear Timer2/Comp1 interrupt flag - #endif + OCR1A+= next_callback*TIMER_PRESCALE ; // set compare A for callback + TIFR1=OCF1A_bm; // clear compare A=callback flag diff=OCR1A-TCNT1; // compare timer and comparator sei(); // enable global int +#else + + if (next_callback > 4000) + update_inputs(); + uint32_t wait_until = start + next_callback; + end__ = micros(); + if (end__-start < next_callback) { + uint32_t wait = next_callback; + wait -= ((end__-start)); + //wait -= 1000; + delayMicroseconds(wait); + //debugln("%lu vs %lu",wait,next_callback); + + /* + debugln("took %lu (%lu - %lu) vs %lu wait %lu wait until %lu", + end__-start, start, end__, + next_callback, + wait, wait_until); + */ + end__ += wait; + } #endif + end__ = micros(); + //debugln("%lu vs %lu",end__-start,next_callback); //debugln("next %d diff %lu (%lu to %lu) diff %x ",org_next_call, end__ - start_, end__, start_, diff); + } while(diff&0x8000); // Callback did not took more than requested time for next callback // so we can launch Update_All before next callback @@ -471,8 +444,6 @@ uint8_t Update_All() } #endif //ENABLE_SERIAL #ifdef ENABLE_PPM - //INPUT_SIGNAL_on; - update_inputs(); if(mode_select!=MODE_SERIAL && IS_PPM_FLAG_on) // PPM mode and a full frame has been received { debugln("%s:%d",__func__, __LINE__); @@ -517,6 +488,10 @@ uint8_t Update_All() bind_counter=2; } #endif //ENABLE_BIND_CH + + + update_inputs(); + if(IS_CHANGE_PROTOCOL_FLAG_on) { @@ -718,6 +693,7 @@ static void protocol_init() WAIT_BIND_off; CHANGE_PROTOCOL_FLAG_off; +#if 0 if(next_callback>32000) { // next_callback should not be more than 32767 so we will wait here... uint16_t temp=(next_callback>>10)-2; @@ -733,7 +709,9 @@ static void protocol_init() #endif sei(); // enable global int BIND_BUTTON_FLAG_off; // do not bind/reset id anymore even if protocol change - +#else + delayMicroseconds(next_callback); +#endif debugln("%s BIND_BUTTON_FLAG_off",__func__); } @@ -891,7 +869,7 @@ void modules_reset() #endif //Wait for every component to reset - delayMilliseconds(100); + delay(100); prev_power=0xFD; // unused power value } diff --git a/Multiprotocol/Pins.h b/Multiprotocol/Pins.h index c0200a4..34f6aab 100644 --- a/Multiprotocol/Pins.h +++ b/Multiprotocol/Pins.h @@ -18,7 +18,7 @@ #define LED_output #define IS_LED_on false #define LED_toggle -#if 1 +#if 0 // SDIO MOSI #define SDI_pin 16 @@ -99,6 +99,12 @@ #define CC25_CSN_0 (digitalRead(CC25_CSN_pin) == LOW) #define CC25_CSN_input pinMode(CC25_CSN_pin, INPUT); #define CC25_CSN_output pinMode(CC25_CSN_pin, OUTPUT); + + #define Throttle_pin 34 + #define Yaw_pin 35 + #define Roll_pin 12 + #define Pitch_pin 13 + #define Aux2_pin 27 #endif //******************* //*** EEPROM *** diff --git a/Multiprotocol/inputs.ino b/Multiprotocol/inputs.ino index e04ffff..223b703 100644 --- a/Multiprotocol/inputs.ino +++ b/Multiprotocol/inputs.ino @@ -1,18 +1,46 @@ void update_inputs(void) { - //analogRead() - static int inc = 1; - if (inc > 0) { - if (Channel_data[THROTTLE] < CHANNEL_MIN_125 + 100) { + + INPUT_SIGNAL_on; + + if (state < FRSKY_BIND_DONE) + return; + + #ifdef Throttle_pin + uint16_t throttle = analogRead(Throttle_pin); + uint16_t yaw = analogRead(Yaw_pin); + uint16_t roll = analogRead(Roll_pin); + uint16_t pitch = analogRead(Pitch_pin); + uint16_t aux1 = 0;//analogRead(Aux1_pin); + uint16_t aux2 = analogRead(Aux2_pin); + + uint16_t ch_min=CHANNEL_MIN_100; + uint16_t ch_max=CHANNEL_MAX_100; + + + Channel_data[THROTTLE] += map(throttle, 0, 3500, ch_min, ch_max); + Channel_data[RUDDER] += map(yaw, 0, 3500, ch_min, ch_max); + Channel_data[AILERON] += map(roll, 0, 3500, ch_min, ch_max); + Channel_data[ELEVATOR] += map(pitch, 0, 3500, ch_min, ch_max); + //Channel_data[CH5] += map(aux1, 0, 3500, ch_min, ch_max); + Channel_data[CH6] += map(aux2, 0, 3500, ch_min, ch_max); + + + Channel_data[THROTTLE] /= 2; + Channel_data[RUDDER] /= 2; + Channel_data[AILERON] /= 2; + Channel_data[ELEVATOR] /= 2; + Channel_data[CH5] /= 2; + Channel_data[CH6] /= 2; + + //debugln("T %d y %d r %d p %d a1 %d a2 %d",throttle,yaw,roll,pitch,aux1,aux2); + //debugln("T %d y %d r %d p %d a1 %d a2 %d",Channel_data[THROTTLE],Channel_data[RUDDER],Channel_data[AILERON],Channel_data[ELEVATOR],Channel_data[CH5],Channel_data[CH6]); + #else + //analogRead() + if (Channel_data[THROTTLE] < CHANNEL_MAX_100) { Channel_data[THROTTLE] += 1; - }else { - inc = -1; - } - }else { - if (Channel_data[THROTTLE] > CHANNEL_MIN_125) { - Channel_data[THROTTLE] -= 1; - }else { - inc = +1; + } else { + Channel_data[THROTTLE] = CHANNEL_MIN_100; } - } + #endif } diff --git a/Multiprotocol/mi.ino b/Multiprotocol/mi.ino index d0df521..ffeb6f7 100644 --- a/Multiprotocol/mi.ino +++ b/Multiprotocol/mi.ino @@ -27,43 +27,4 @@ int16_t map16b( int16_t x, int16_t in_min, int16_t in_max, int16_t out_min, int1 x = y / (in_max - in_min) ; return x + out_min ; } - -int16_t map( int16_t x, int16_t in_min, int16_t in_max, int16_t out_min, int16_t out_max) -{ -// return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; - long y ; - x -= in_min ; - y = out_max - out_min ; - y *= x ; - x = y / (in_max - in_min) ; - return x + out_min ; -} - -// replacement millis() and micros() -// These work polled, no interrupts -// micros() MUST be called at least once every 32 milliseconds -uint16_t MillisPrecount ; -uint16_t lastTimerValue ; -uint32_t TotalMicros ; -uint32_t TotalMillis ; -uint8_t Correction ; - -void delayMilliseconds(unsigned long ms) -{ - #if 0 - uint16_t start = (uint16_t)micros(); - uint16_t lms = ms ; - - while (lms > 0) { - if (((uint16_t)micros() - start) >= 1000) { - lms--; - start += 1000; - } - } - #else - - -delay(ms); -#endif - -} +#define delayMilliseconds(x) delay(x)